1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * Generic netlink for DPLL management framework 4 * 5 * Copyright (c) 2023 Meta Platforms, Inc. and affiliates 6 * Copyright (c) 2023 Intel and affiliates 7 * 8 */ 9 #include <linux/module.h> 10 #include <linux/kernel.h> 11 #include <linux/netdevice.h> 12 #include <net/genetlink.h> 13 #include "dpll_core.h" 14 #include "dpll_netlink.h" 15 #include "dpll_nl.h" 16 #include <uapi/linux/dpll.h> 17 18 #define ASSERT_NOT_NULL(ptr) (WARN_ON(!ptr)) 19 20 #define xa_for_each_marked_start(xa, index, entry, filter, start) \ 21 for (index = start, entry = xa_find(xa, &index, ULONG_MAX, filter); \ 22 entry; entry = xa_find_after(xa, &index, ULONG_MAX, filter)) 23 24 struct dpll_dump_ctx { 25 unsigned long idx; 26 }; 27 28 static struct dpll_dump_ctx *dpll_dump_context(struct netlink_callback *cb) 29 { 30 return (struct dpll_dump_ctx *)cb->ctx; 31 } 32 33 static int 34 dpll_msg_add_dev_handle(struct sk_buff *msg, struct dpll_device *dpll) 35 { 36 if (nla_put_u32(msg, DPLL_A_ID, dpll->id)) 37 return -EMSGSIZE; 38 39 return 0; 40 } 41 42 static int 43 dpll_msg_add_dev_parent_handle(struct sk_buff *msg, u32 id) 44 { 45 if (nla_put_u32(msg, DPLL_A_PIN_PARENT_ID, id)) 46 return -EMSGSIZE; 47 48 return 0; 49 } 50 51 /** 52 * dpll_msg_add_pin_handle - attach pin handle attribute to a given message 53 * @msg: pointer to sk_buff message to attach a pin handle 54 * @pin: pin pointer 55 * 56 * Return: 57 * * 0 - success 58 * * -EMSGSIZE - no space in message to attach pin handle 59 */ 60 static int dpll_msg_add_pin_handle(struct sk_buff *msg, struct dpll_pin *pin) 61 { 62 if (!pin) 63 return 0; 64 if (nla_put_u32(msg, DPLL_A_PIN_ID, pin->id)) 65 return -EMSGSIZE; 66 return 0; 67 } 68 69 static struct dpll_pin *dpll_netdev_pin(const struct net_device *dev) 70 { 71 return rcu_dereference_rtnl(dev->dpll_pin); 72 } 73 74 /** 75 * dpll_netdev_pin_handle_size - get size of pin handle attribute of a netdev 76 * @dev: netdev from which to get the pin 77 * 78 * Return: byte size of pin handle attribute, or 0 if @dev has no pin. 79 */ 80 size_t dpll_netdev_pin_handle_size(const struct net_device *dev) 81 { 82 return dpll_netdev_pin(dev) ? nla_total_size(4) : 0; /* DPLL_A_PIN_ID */ 83 } 84 85 int dpll_netdev_add_pin_handle(struct sk_buff *msg, 86 const struct net_device *dev) 87 { 88 return dpll_msg_add_pin_handle(msg, dpll_netdev_pin(dev)); 89 } 90 91 static int 92 dpll_msg_add_mode(struct sk_buff *msg, struct dpll_device *dpll, 93 struct netlink_ext_ack *extack) 94 { 95 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 96 enum dpll_mode mode; 97 int ret; 98 99 ret = ops->mode_get(dpll, dpll_priv(dpll), &mode, extack); 100 if (ret) 101 return ret; 102 if (nla_put_u32(msg, DPLL_A_MODE, mode)) 103 return -EMSGSIZE; 104 105 return 0; 106 } 107 108 static int 109 dpll_msg_add_mode_supported(struct sk_buff *msg, struct dpll_device *dpll, 110 struct netlink_ext_ack *extack) 111 { 112 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 113 enum dpll_mode mode; 114 int ret; 115 116 /* No mode change is supported now, so the only supported mode is the 117 * one obtained by mode_get(). 118 */ 119 120 ret = ops->mode_get(dpll, dpll_priv(dpll), &mode, extack); 121 if (ret) 122 return ret; 123 if (nla_put_u32(msg, DPLL_A_MODE_SUPPORTED, mode)) 124 return -EMSGSIZE; 125 126 return 0; 127 } 128 129 static int 130 dpll_msg_add_phase_offset_monitor(struct sk_buff *msg, struct dpll_device *dpll, 131 struct netlink_ext_ack *extack) 132 { 133 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 134 enum dpll_feature_state state; 135 int ret; 136 137 if (ops->phase_offset_monitor_set && ops->phase_offset_monitor_get) { 138 ret = ops->phase_offset_monitor_get(dpll, dpll_priv(dpll), 139 &state, extack); 140 if (ret) 141 return ret; 142 if (nla_put_u32(msg, DPLL_A_PHASE_OFFSET_MONITOR, state)) 143 return -EMSGSIZE; 144 } 145 146 return 0; 147 } 148 149 static int 150 dpll_msg_add_lock_status(struct sk_buff *msg, struct dpll_device *dpll, 151 struct netlink_ext_ack *extack) 152 { 153 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 154 enum dpll_lock_status_error status_error = 0; 155 enum dpll_lock_status status; 156 int ret; 157 158 ret = ops->lock_status_get(dpll, dpll_priv(dpll), &status, 159 &status_error, extack); 160 if (ret) 161 return ret; 162 if (nla_put_u32(msg, DPLL_A_LOCK_STATUS, status)) 163 return -EMSGSIZE; 164 if (status_error && 165 (status == DPLL_LOCK_STATUS_UNLOCKED || 166 status == DPLL_LOCK_STATUS_HOLDOVER) && 167 nla_put_u32(msg, DPLL_A_LOCK_STATUS_ERROR, status_error)) 168 return -EMSGSIZE; 169 170 return 0; 171 } 172 173 static int 174 dpll_msg_add_temp(struct sk_buff *msg, struct dpll_device *dpll, 175 struct netlink_ext_ack *extack) 176 { 177 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 178 s32 temp; 179 int ret; 180 181 if (!ops->temp_get) 182 return 0; 183 ret = ops->temp_get(dpll, dpll_priv(dpll), &temp, extack); 184 if (ret) 185 return ret; 186 if (nla_put_s32(msg, DPLL_A_TEMP, temp)) 187 return -EMSGSIZE; 188 189 return 0; 190 } 191 192 static int 193 dpll_msg_add_clock_quality_level(struct sk_buff *msg, struct dpll_device *dpll, 194 struct netlink_ext_ack *extack) 195 { 196 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 197 DECLARE_BITMAP(qls, DPLL_CLOCK_QUALITY_LEVEL_MAX) = { 0 }; 198 enum dpll_clock_quality_level ql; 199 int ret; 200 201 if (!ops->clock_quality_level_get) 202 return 0; 203 ret = ops->clock_quality_level_get(dpll, dpll_priv(dpll), qls, extack); 204 if (ret) 205 return ret; 206 for_each_set_bit(ql, qls, DPLL_CLOCK_QUALITY_LEVEL_MAX) 207 if (nla_put_u32(msg, DPLL_A_CLOCK_QUALITY_LEVEL, ql)) 208 return -EMSGSIZE; 209 210 return 0; 211 } 212 213 static int 214 dpll_msg_add_pin_prio(struct sk_buff *msg, struct dpll_pin *pin, 215 struct dpll_pin_ref *ref, 216 struct netlink_ext_ack *extack) 217 { 218 const struct dpll_pin_ops *ops = dpll_pin_ops(ref); 219 struct dpll_device *dpll = ref->dpll; 220 u32 prio; 221 int ret; 222 223 if (!ops->prio_get) 224 return 0; 225 ret = ops->prio_get(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll, 226 dpll_priv(dpll), &prio, extack); 227 if (ret) 228 return ret; 229 if (nla_put_u32(msg, DPLL_A_PIN_PRIO, prio)) 230 return -EMSGSIZE; 231 232 return 0; 233 } 234 235 static int 236 dpll_msg_add_pin_on_dpll_state(struct sk_buff *msg, struct dpll_pin *pin, 237 struct dpll_pin_ref *ref, 238 struct netlink_ext_ack *extack) 239 { 240 const struct dpll_pin_ops *ops = dpll_pin_ops(ref); 241 struct dpll_device *dpll = ref->dpll; 242 enum dpll_pin_state state; 243 int ret; 244 245 if (!ops->state_on_dpll_get) 246 return 0; 247 ret = ops->state_on_dpll_get(pin, dpll_pin_on_dpll_priv(dpll, pin), 248 dpll, dpll_priv(dpll), &state, extack); 249 if (ret) 250 return ret; 251 if (nla_put_u32(msg, DPLL_A_PIN_STATE, state)) 252 return -EMSGSIZE; 253 254 return 0; 255 } 256 257 static int 258 dpll_msg_add_pin_direction(struct sk_buff *msg, struct dpll_pin *pin, 259 struct dpll_pin_ref *ref, 260 struct netlink_ext_ack *extack) 261 { 262 const struct dpll_pin_ops *ops = dpll_pin_ops(ref); 263 struct dpll_device *dpll = ref->dpll; 264 enum dpll_pin_direction direction; 265 int ret; 266 267 ret = ops->direction_get(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll, 268 dpll_priv(dpll), &direction, extack); 269 if (ret) 270 return ret; 271 if (nla_put_u32(msg, DPLL_A_PIN_DIRECTION, direction)) 272 return -EMSGSIZE; 273 274 return 0; 275 } 276 277 static int 278 dpll_msg_add_pin_phase_adjust(struct sk_buff *msg, struct dpll_pin *pin, 279 struct dpll_pin_ref *ref, 280 struct netlink_ext_ack *extack) 281 { 282 const struct dpll_pin_ops *ops = dpll_pin_ops(ref); 283 struct dpll_device *dpll = ref->dpll; 284 s32 phase_adjust; 285 int ret; 286 287 if (!ops->phase_adjust_get) 288 return 0; 289 ret = ops->phase_adjust_get(pin, dpll_pin_on_dpll_priv(dpll, pin), 290 dpll, dpll_priv(dpll), 291 &phase_adjust, extack); 292 if (ret) 293 return ret; 294 if (nla_put_s32(msg, DPLL_A_PIN_PHASE_ADJUST, phase_adjust)) 295 return -EMSGSIZE; 296 297 return 0; 298 } 299 300 static int 301 dpll_msg_add_phase_offset(struct sk_buff *msg, struct dpll_pin *pin, 302 struct dpll_pin_ref *ref, 303 struct netlink_ext_ack *extack) 304 { 305 const struct dpll_pin_ops *ops = dpll_pin_ops(ref); 306 struct dpll_device *dpll = ref->dpll; 307 s64 phase_offset; 308 int ret; 309 310 if (!ops->phase_offset_get) 311 return 0; 312 ret = ops->phase_offset_get(pin, dpll_pin_on_dpll_priv(dpll, pin), 313 dpll, dpll_priv(dpll), &phase_offset, 314 extack); 315 if (ret) 316 return ret; 317 if (nla_put_64bit(msg, DPLL_A_PIN_PHASE_OFFSET, sizeof(phase_offset), 318 &phase_offset, DPLL_A_PIN_PAD)) 319 return -EMSGSIZE; 320 321 return 0; 322 } 323 324 static int dpll_msg_add_ffo(struct sk_buff *msg, struct dpll_pin *pin, 325 struct dpll_pin_ref *ref, 326 struct netlink_ext_ack *extack) 327 { 328 const struct dpll_pin_ops *ops = dpll_pin_ops(ref); 329 struct dpll_device *dpll = ref->dpll; 330 s64 ffo; 331 int ret; 332 333 if (!ops->ffo_get) 334 return 0; 335 ret = ops->ffo_get(pin, dpll_pin_on_dpll_priv(dpll, pin), 336 dpll, dpll_priv(dpll), &ffo, extack); 337 if (ret) { 338 if (ret == -ENODATA) 339 return 0; 340 return ret; 341 } 342 return nla_put_sint(msg, DPLL_A_PIN_FRACTIONAL_FREQUENCY_OFFSET, ffo); 343 } 344 345 static int 346 dpll_msg_add_pin_freq(struct sk_buff *msg, struct dpll_pin *pin, 347 struct dpll_pin_ref *ref, struct netlink_ext_ack *extack) 348 { 349 const struct dpll_pin_ops *ops = dpll_pin_ops(ref); 350 struct dpll_device *dpll = ref->dpll; 351 struct nlattr *nest; 352 int fs, ret; 353 u64 freq; 354 355 if (!ops->frequency_get) 356 return 0; 357 ret = ops->frequency_get(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll, 358 dpll_priv(dpll), &freq, extack); 359 if (ret) 360 return ret; 361 if (nla_put_64bit(msg, DPLL_A_PIN_FREQUENCY, sizeof(freq), &freq, 362 DPLL_A_PIN_PAD)) 363 return -EMSGSIZE; 364 for (fs = 0; fs < pin->prop.freq_supported_num; fs++) { 365 nest = nla_nest_start(msg, DPLL_A_PIN_FREQUENCY_SUPPORTED); 366 if (!nest) 367 return -EMSGSIZE; 368 freq = pin->prop.freq_supported[fs].min; 369 if (nla_put_64bit(msg, DPLL_A_PIN_FREQUENCY_MIN, sizeof(freq), 370 &freq, DPLL_A_PIN_PAD)) { 371 nla_nest_cancel(msg, nest); 372 return -EMSGSIZE; 373 } 374 freq = pin->prop.freq_supported[fs].max; 375 if (nla_put_64bit(msg, DPLL_A_PIN_FREQUENCY_MAX, sizeof(freq), 376 &freq, DPLL_A_PIN_PAD)) { 377 nla_nest_cancel(msg, nest); 378 return -EMSGSIZE; 379 } 380 nla_nest_end(msg, nest); 381 } 382 383 return 0; 384 } 385 386 static int 387 dpll_msg_add_pin_esync(struct sk_buff *msg, struct dpll_pin *pin, 388 struct dpll_pin_ref *ref, struct netlink_ext_ack *extack) 389 { 390 const struct dpll_pin_ops *ops = dpll_pin_ops(ref); 391 struct dpll_device *dpll = ref->dpll; 392 struct dpll_pin_esync esync; 393 struct nlattr *nest; 394 int ret, i; 395 396 if (!ops->esync_get) 397 return 0; 398 ret = ops->esync_get(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll, 399 dpll_priv(dpll), &esync, extack); 400 if (ret == -EOPNOTSUPP) 401 return 0; 402 else if (ret) 403 return ret; 404 if (nla_put_64bit(msg, DPLL_A_PIN_ESYNC_FREQUENCY, sizeof(esync.freq), 405 &esync.freq, DPLL_A_PIN_PAD)) 406 return -EMSGSIZE; 407 if (nla_put_u32(msg, DPLL_A_PIN_ESYNC_PULSE, esync.pulse)) 408 return -EMSGSIZE; 409 for (i = 0; i < esync.range_num; i++) { 410 nest = nla_nest_start(msg, 411 DPLL_A_PIN_ESYNC_FREQUENCY_SUPPORTED); 412 if (!nest) 413 return -EMSGSIZE; 414 if (nla_put_64bit(msg, DPLL_A_PIN_FREQUENCY_MIN, 415 sizeof(esync.range[i].min), 416 &esync.range[i].min, DPLL_A_PIN_PAD)) 417 goto nest_cancel; 418 if (nla_put_64bit(msg, DPLL_A_PIN_FREQUENCY_MAX, 419 sizeof(esync.range[i].max), 420 &esync.range[i].max, DPLL_A_PIN_PAD)) 421 goto nest_cancel; 422 nla_nest_end(msg, nest); 423 } 424 return 0; 425 426 nest_cancel: 427 nla_nest_cancel(msg, nest); 428 return -EMSGSIZE; 429 } 430 431 static bool dpll_pin_is_freq_supported(struct dpll_pin *pin, u32 freq) 432 { 433 int fs; 434 435 for (fs = 0; fs < pin->prop.freq_supported_num; fs++) 436 if (freq >= pin->prop.freq_supported[fs].min && 437 freq <= pin->prop.freq_supported[fs].max) 438 return true; 439 return false; 440 } 441 442 static int 443 dpll_msg_add_pin_parents(struct sk_buff *msg, struct dpll_pin *pin, 444 struct dpll_pin_ref *dpll_ref, 445 struct netlink_ext_ack *extack) 446 { 447 enum dpll_pin_state state; 448 struct dpll_pin_ref *ref; 449 struct dpll_pin *ppin; 450 struct nlattr *nest; 451 unsigned long index; 452 int ret; 453 454 xa_for_each(&pin->parent_refs, index, ref) { 455 const struct dpll_pin_ops *ops = dpll_pin_ops(ref); 456 void *parent_priv; 457 458 ppin = ref->pin; 459 parent_priv = dpll_pin_on_dpll_priv(dpll_ref->dpll, ppin); 460 ret = ops->state_on_pin_get(pin, 461 dpll_pin_on_pin_priv(ppin, pin), 462 ppin, parent_priv, &state, extack); 463 if (ret) 464 return ret; 465 nest = nla_nest_start(msg, DPLL_A_PIN_PARENT_PIN); 466 if (!nest) 467 return -EMSGSIZE; 468 ret = dpll_msg_add_dev_parent_handle(msg, ppin->id); 469 if (ret) 470 goto nest_cancel; 471 if (nla_put_u32(msg, DPLL_A_PIN_STATE, state)) { 472 ret = -EMSGSIZE; 473 goto nest_cancel; 474 } 475 nla_nest_end(msg, nest); 476 } 477 478 return 0; 479 480 nest_cancel: 481 nla_nest_cancel(msg, nest); 482 return ret; 483 } 484 485 static int 486 dpll_msg_add_pin_dplls(struct sk_buff *msg, struct dpll_pin *pin, 487 struct netlink_ext_ack *extack) 488 { 489 struct dpll_pin_ref *ref; 490 struct nlattr *attr; 491 unsigned long index; 492 int ret; 493 494 xa_for_each(&pin->dpll_refs, index, ref) { 495 attr = nla_nest_start(msg, DPLL_A_PIN_PARENT_DEVICE); 496 if (!attr) 497 return -EMSGSIZE; 498 ret = dpll_msg_add_dev_parent_handle(msg, ref->dpll->id); 499 if (ret) 500 goto nest_cancel; 501 ret = dpll_msg_add_pin_on_dpll_state(msg, pin, ref, extack); 502 if (ret) 503 goto nest_cancel; 504 ret = dpll_msg_add_pin_prio(msg, pin, ref, extack); 505 if (ret) 506 goto nest_cancel; 507 ret = dpll_msg_add_pin_direction(msg, pin, ref, extack); 508 if (ret) 509 goto nest_cancel; 510 ret = dpll_msg_add_phase_offset(msg, pin, ref, extack); 511 if (ret) 512 goto nest_cancel; 513 nla_nest_end(msg, attr); 514 } 515 516 return 0; 517 518 nest_cancel: 519 nla_nest_end(msg, attr); 520 return ret; 521 } 522 523 static int 524 dpll_cmd_pin_get_one(struct sk_buff *msg, struct dpll_pin *pin, 525 struct netlink_ext_ack *extack) 526 { 527 const struct dpll_pin_properties *prop = &pin->prop; 528 struct dpll_pin_ref *ref; 529 int ret; 530 531 ref = dpll_xa_ref_dpll_first(&pin->dpll_refs); 532 ASSERT_NOT_NULL(ref); 533 534 ret = dpll_msg_add_pin_handle(msg, pin); 535 if (ret) 536 return ret; 537 if (nla_put_string(msg, DPLL_A_PIN_MODULE_NAME, 538 module_name(pin->module))) 539 return -EMSGSIZE; 540 if (nla_put_64bit(msg, DPLL_A_PIN_CLOCK_ID, sizeof(pin->clock_id), 541 &pin->clock_id, DPLL_A_PIN_PAD)) 542 return -EMSGSIZE; 543 if (prop->board_label && 544 nla_put_string(msg, DPLL_A_PIN_BOARD_LABEL, prop->board_label)) 545 return -EMSGSIZE; 546 if (prop->panel_label && 547 nla_put_string(msg, DPLL_A_PIN_PANEL_LABEL, prop->panel_label)) 548 return -EMSGSIZE; 549 if (prop->package_label && 550 nla_put_string(msg, DPLL_A_PIN_PACKAGE_LABEL, 551 prop->package_label)) 552 return -EMSGSIZE; 553 if (nla_put_u32(msg, DPLL_A_PIN_TYPE, prop->type)) 554 return -EMSGSIZE; 555 if (nla_put_u32(msg, DPLL_A_PIN_CAPABILITIES, prop->capabilities)) 556 return -EMSGSIZE; 557 ret = dpll_msg_add_pin_freq(msg, pin, ref, extack); 558 if (ret) 559 return ret; 560 if (nla_put_s32(msg, DPLL_A_PIN_PHASE_ADJUST_MIN, 561 prop->phase_range.min)) 562 return -EMSGSIZE; 563 if (nla_put_s32(msg, DPLL_A_PIN_PHASE_ADJUST_MAX, 564 prop->phase_range.max)) 565 return -EMSGSIZE; 566 ret = dpll_msg_add_pin_phase_adjust(msg, pin, ref, extack); 567 if (ret) 568 return ret; 569 ret = dpll_msg_add_ffo(msg, pin, ref, extack); 570 if (ret) 571 return ret; 572 ret = dpll_msg_add_pin_esync(msg, pin, ref, extack); 573 if (ret) 574 return ret; 575 if (xa_empty(&pin->parent_refs)) 576 ret = dpll_msg_add_pin_dplls(msg, pin, extack); 577 else 578 ret = dpll_msg_add_pin_parents(msg, pin, ref, extack); 579 580 return ret; 581 } 582 583 static int 584 dpll_device_get_one(struct dpll_device *dpll, struct sk_buff *msg, 585 struct netlink_ext_ack *extack) 586 { 587 int ret; 588 589 ret = dpll_msg_add_dev_handle(msg, dpll); 590 if (ret) 591 return ret; 592 if (nla_put_string(msg, DPLL_A_MODULE_NAME, module_name(dpll->module))) 593 return -EMSGSIZE; 594 if (nla_put_64bit(msg, DPLL_A_CLOCK_ID, sizeof(dpll->clock_id), 595 &dpll->clock_id, DPLL_A_PAD)) 596 return -EMSGSIZE; 597 ret = dpll_msg_add_temp(msg, dpll, extack); 598 if (ret) 599 return ret; 600 ret = dpll_msg_add_lock_status(msg, dpll, extack); 601 if (ret) 602 return ret; 603 ret = dpll_msg_add_clock_quality_level(msg, dpll, extack); 604 if (ret) 605 return ret; 606 ret = dpll_msg_add_mode(msg, dpll, extack); 607 if (ret) 608 return ret; 609 ret = dpll_msg_add_mode_supported(msg, dpll, extack); 610 if (ret) 611 return ret; 612 if (nla_put_u32(msg, DPLL_A_TYPE, dpll->type)) 613 return -EMSGSIZE; 614 ret = dpll_msg_add_phase_offset_monitor(msg, dpll, extack); 615 if (ret) 616 return ret; 617 618 return 0; 619 } 620 621 static int 622 dpll_device_event_send(enum dpll_cmd event, struct dpll_device *dpll) 623 { 624 struct sk_buff *msg; 625 int ret = -ENOMEM; 626 void *hdr; 627 628 if (WARN_ON(!xa_get_mark(&dpll_device_xa, dpll->id, DPLL_REGISTERED))) 629 return -ENODEV; 630 msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 631 if (!msg) 632 return -ENOMEM; 633 hdr = genlmsg_put(msg, 0, 0, &dpll_nl_family, 0, event); 634 if (!hdr) 635 goto err_free_msg; 636 ret = dpll_device_get_one(dpll, msg, NULL); 637 if (ret) 638 goto err_cancel_msg; 639 genlmsg_end(msg, hdr); 640 genlmsg_multicast(&dpll_nl_family, msg, 0, 0, GFP_KERNEL); 641 642 return 0; 643 644 err_cancel_msg: 645 genlmsg_cancel(msg, hdr); 646 err_free_msg: 647 nlmsg_free(msg); 648 649 return ret; 650 } 651 652 int dpll_device_create_ntf(struct dpll_device *dpll) 653 { 654 return dpll_device_event_send(DPLL_CMD_DEVICE_CREATE_NTF, dpll); 655 } 656 657 int dpll_device_delete_ntf(struct dpll_device *dpll) 658 { 659 return dpll_device_event_send(DPLL_CMD_DEVICE_DELETE_NTF, dpll); 660 } 661 662 static int 663 __dpll_device_change_ntf(struct dpll_device *dpll) 664 { 665 return dpll_device_event_send(DPLL_CMD_DEVICE_CHANGE_NTF, dpll); 666 } 667 668 static bool dpll_pin_available(struct dpll_pin *pin) 669 { 670 struct dpll_pin_ref *par_ref; 671 unsigned long i; 672 673 if (!xa_get_mark(&dpll_pin_xa, pin->id, DPLL_REGISTERED)) 674 return false; 675 xa_for_each(&pin->parent_refs, i, par_ref) 676 if (xa_get_mark(&dpll_pin_xa, par_ref->pin->id, 677 DPLL_REGISTERED)) 678 return true; 679 xa_for_each(&pin->dpll_refs, i, par_ref) 680 if (xa_get_mark(&dpll_device_xa, par_ref->dpll->id, 681 DPLL_REGISTERED)) 682 return true; 683 return false; 684 } 685 686 /** 687 * dpll_device_change_ntf - notify that the dpll device has been changed 688 * @dpll: registered dpll pointer 689 * 690 * Context: acquires and holds a dpll_lock. 691 * Return: 0 if succeeds, error code otherwise. 692 */ 693 int dpll_device_change_ntf(struct dpll_device *dpll) 694 { 695 int ret; 696 697 mutex_lock(&dpll_lock); 698 ret = __dpll_device_change_ntf(dpll); 699 mutex_unlock(&dpll_lock); 700 701 return ret; 702 } 703 EXPORT_SYMBOL_GPL(dpll_device_change_ntf); 704 705 static int 706 dpll_pin_event_send(enum dpll_cmd event, struct dpll_pin *pin) 707 { 708 struct sk_buff *msg; 709 int ret = -ENOMEM; 710 void *hdr; 711 712 if (!dpll_pin_available(pin)) 713 return -ENODEV; 714 715 msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 716 if (!msg) 717 return -ENOMEM; 718 719 hdr = genlmsg_put(msg, 0, 0, &dpll_nl_family, 0, event); 720 if (!hdr) 721 goto err_free_msg; 722 ret = dpll_cmd_pin_get_one(msg, pin, NULL); 723 if (ret) 724 goto err_cancel_msg; 725 genlmsg_end(msg, hdr); 726 genlmsg_multicast(&dpll_nl_family, msg, 0, 0, GFP_KERNEL); 727 728 return 0; 729 730 err_cancel_msg: 731 genlmsg_cancel(msg, hdr); 732 err_free_msg: 733 nlmsg_free(msg); 734 735 return ret; 736 } 737 738 int dpll_pin_create_ntf(struct dpll_pin *pin) 739 { 740 return dpll_pin_event_send(DPLL_CMD_PIN_CREATE_NTF, pin); 741 } 742 743 int dpll_pin_delete_ntf(struct dpll_pin *pin) 744 { 745 return dpll_pin_event_send(DPLL_CMD_PIN_DELETE_NTF, pin); 746 } 747 748 static int __dpll_pin_change_ntf(struct dpll_pin *pin) 749 { 750 return dpll_pin_event_send(DPLL_CMD_PIN_CHANGE_NTF, pin); 751 } 752 753 /** 754 * dpll_pin_change_ntf - notify that the pin has been changed 755 * @pin: registered pin pointer 756 * 757 * Context: acquires and holds a dpll_lock. 758 * Return: 0 if succeeds, error code otherwise. 759 */ 760 int dpll_pin_change_ntf(struct dpll_pin *pin) 761 { 762 int ret; 763 764 mutex_lock(&dpll_lock); 765 ret = __dpll_pin_change_ntf(pin); 766 mutex_unlock(&dpll_lock); 767 768 return ret; 769 } 770 EXPORT_SYMBOL_GPL(dpll_pin_change_ntf); 771 772 static int 773 dpll_phase_offset_monitor_set(struct dpll_device *dpll, struct nlattr *a, 774 struct netlink_ext_ack *extack) 775 { 776 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 777 enum dpll_feature_state state = nla_get_u32(a), old_state; 778 int ret; 779 780 if (!(ops->phase_offset_monitor_set && ops->phase_offset_monitor_get)) { 781 NL_SET_ERR_MSG_ATTR(extack, a, "dpll device not capable of phase offset monitor"); 782 return -EOPNOTSUPP; 783 } 784 ret = ops->phase_offset_monitor_get(dpll, dpll_priv(dpll), &old_state, 785 extack); 786 if (ret) { 787 NL_SET_ERR_MSG(extack, "unable to get current state of phase offset monitor"); 788 return ret; 789 } 790 if (state == old_state) 791 return 0; 792 793 return ops->phase_offset_monitor_set(dpll, dpll_priv(dpll), state, 794 extack); 795 } 796 797 static int 798 dpll_pin_freq_set(struct dpll_pin *pin, struct nlattr *a, 799 struct netlink_ext_ack *extack) 800 { 801 u64 freq = nla_get_u64(a), old_freq; 802 struct dpll_pin_ref *ref, *failed; 803 const struct dpll_pin_ops *ops; 804 struct dpll_device *dpll; 805 unsigned long i; 806 int ret; 807 808 if (!dpll_pin_is_freq_supported(pin, freq)) { 809 NL_SET_ERR_MSG_ATTR(extack, a, "frequency is not supported by the device"); 810 return -EINVAL; 811 } 812 813 xa_for_each(&pin->dpll_refs, i, ref) { 814 ops = dpll_pin_ops(ref); 815 if (!ops->frequency_set || !ops->frequency_get) { 816 NL_SET_ERR_MSG(extack, "frequency set not supported by the device"); 817 return -EOPNOTSUPP; 818 } 819 } 820 ref = dpll_xa_ref_dpll_first(&pin->dpll_refs); 821 ops = dpll_pin_ops(ref); 822 dpll = ref->dpll; 823 ret = ops->frequency_get(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll, 824 dpll_priv(dpll), &old_freq, extack); 825 if (ret) { 826 NL_SET_ERR_MSG(extack, "unable to get old frequency value"); 827 return ret; 828 } 829 if (freq == old_freq) 830 return 0; 831 832 xa_for_each(&pin->dpll_refs, i, ref) { 833 ops = dpll_pin_ops(ref); 834 dpll = ref->dpll; 835 ret = ops->frequency_set(pin, dpll_pin_on_dpll_priv(dpll, pin), 836 dpll, dpll_priv(dpll), freq, extack); 837 if (ret) { 838 failed = ref; 839 NL_SET_ERR_MSG_FMT(extack, "frequency set failed for dpll_id:%u", 840 dpll->id); 841 goto rollback; 842 } 843 } 844 __dpll_pin_change_ntf(pin); 845 846 return 0; 847 848 rollback: 849 xa_for_each(&pin->dpll_refs, i, ref) { 850 if (ref == failed) 851 break; 852 ops = dpll_pin_ops(ref); 853 dpll = ref->dpll; 854 if (ops->frequency_set(pin, dpll_pin_on_dpll_priv(dpll, pin), 855 dpll, dpll_priv(dpll), old_freq, extack)) 856 NL_SET_ERR_MSG(extack, "set frequency rollback failed"); 857 } 858 return ret; 859 } 860 861 static int 862 dpll_pin_esync_set(struct dpll_pin *pin, struct nlattr *a, 863 struct netlink_ext_ack *extack) 864 { 865 struct dpll_pin_ref *ref, *failed; 866 const struct dpll_pin_ops *ops; 867 struct dpll_pin_esync esync; 868 u64 freq = nla_get_u64(a); 869 struct dpll_device *dpll; 870 bool supported = false; 871 unsigned long i; 872 int ret; 873 874 xa_for_each(&pin->dpll_refs, i, ref) { 875 ops = dpll_pin_ops(ref); 876 if (!ops->esync_set || !ops->esync_get) { 877 NL_SET_ERR_MSG(extack, 878 "embedded sync feature is not supported by this device"); 879 return -EOPNOTSUPP; 880 } 881 } 882 ref = dpll_xa_ref_dpll_first(&pin->dpll_refs); 883 ops = dpll_pin_ops(ref); 884 dpll = ref->dpll; 885 ret = ops->esync_get(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll, 886 dpll_priv(dpll), &esync, extack); 887 if (ret) { 888 NL_SET_ERR_MSG(extack, "unable to get current embedded sync frequency value"); 889 return ret; 890 } 891 if (freq == esync.freq) 892 return 0; 893 for (i = 0; i < esync.range_num; i++) 894 if (freq <= esync.range[i].max && freq >= esync.range[i].min) 895 supported = true; 896 if (!supported) { 897 NL_SET_ERR_MSG_ATTR(extack, a, 898 "requested embedded sync frequency value is not supported by this device"); 899 return -EINVAL; 900 } 901 902 xa_for_each(&pin->dpll_refs, i, ref) { 903 void *pin_dpll_priv; 904 905 ops = dpll_pin_ops(ref); 906 dpll = ref->dpll; 907 pin_dpll_priv = dpll_pin_on_dpll_priv(dpll, pin); 908 ret = ops->esync_set(pin, pin_dpll_priv, dpll, dpll_priv(dpll), 909 freq, extack); 910 if (ret) { 911 failed = ref; 912 NL_SET_ERR_MSG_FMT(extack, 913 "embedded sync frequency set failed for dpll_id: %u", 914 dpll->id); 915 goto rollback; 916 } 917 } 918 __dpll_pin_change_ntf(pin); 919 920 return 0; 921 922 rollback: 923 xa_for_each(&pin->dpll_refs, i, ref) { 924 void *pin_dpll_priv; 925 926 if (ref == failed) 927 break; 928 ops = dpll_pin_ops(ref); 929 dpll = ref->dpll; 930 pin_dpll_priv = dpll_pin_on_dpll_priv(dpll, pin); 931 if (ops->esync_set(pin, pin_dpll_priv, dpll, dpll_priv(dpll), 932 esync.freq, extack)) 933 NL_SET_ERR_MSG(extack, "set embedded sync frequency rollback failed"); 934 } 935 return ret; 936 } 937 938 static int 939 dpll_pin_on_pin_state_set(struct dpll_pin *pin, u32 parent_idx, 940 enum dpll_pin_state state, 941 struct netlink_ext_ack *extack) 942 { 943 struct dpll_pin_ref *parent_ref; 944 const struct dpll_pin_ops *ops; 945 struct dpll_pin_ref *dpll_ref; 946 void *pin_priv, *parent_priv; 947 struct dpll_pin *parent; 948 unsigned long i; 949 int ret; 950 951 if (!(DPLL_PIN_CAPABILITIES_STATE_CAN_CHANGE & 952 pin->prop.capabilities)) { 953 NL_SET_ERR_MSG(extack, "state changing is not allowed"); 954 return -EOPNOTSUPP; 955 } 956 parent = xa_load(&dpll_pin_xa, parent_idx); 957 if (!parent) 958 return -EINVAL; 959 parent_ref = xa_load(&pin->parent_refs, parent->pin_idx); 960 if (!parent_ref) 961 return -EINVAL; 962 xa_for_each(&parent->dpll_refs, i, dpll_ref) { 963 ops = dpll_pin_ops(parent_ref); 964 if (!ops->state_on_pin_set) 965 return -EOPNOTSUPP; 966 pin_priv = dpll_pin_on_pin_priv(parent, pin); 967 parent_priv = dpll_pin_on_dpll_priv(dpll_ref->dpll, parent); 968 ret = ops->state_on_pin_set(pin, pin_priv, parent, parent_priv, 969 state, extack); 970 if (ret) 971 return ret; 972 } 973 __dpll_pin_change_ntf(pin); 974 975 return 0; 976 } 977 978 static int 979 dpll_pin_state_set(struct dpll_device *dpll, struct dpll_pin *pin, 980 enum dpll_pin_state state, 981 struct netlink_ext_ack *extack) 982 { 983 const struct dpll_pin_ops *ops; 984 struct dpll_pin_ref *ref; 985 int ret; 986 987 if (!(DPLL_PIN_CAPABILITIES_STATE_CAN_CHANGE & 988 pin->prop.capabilities)) { 989 NL_SET_ERR_MSG(extack, "state changing is not allowed"); 990 return -EOPNOTSUPP; 991 } 992 ref = xa_load(&pin->dpll_refs, dpll->id); 993 ASSERT_NOT_NULL(ref); 994 ops = dpll_pin_ops(ref); 995 if (!ops->state_on_dpll_set) 996 return -EOPNOTSUPP; 997 ret = ops->state_on_dpll_set(pin, dpll_pin_on_dpll_priv(dpll, pin), 998 dpll, dpll_priv(dpll), state, extack); 999 if (ret) 1000 return ret; 1001 __dpll_pin_change_ntf(pin); 1002 1003 return 0; 1004 } 1005 1006 static int 1007 dpll_pin_prio_set(struct dpll_device *dpll, struct dpll_pin *pin, 1008 u32 prio, struct netlink_ext_ack *extack) 1009 { 1010 const struct dpll_pin_ops *ops; 1011 struct dpll_pin_ref *ref; 1012 int ret; 1013 1014 if (!(DPLL_PIN_CAPABILITIES_PRIORITY_CAN_CHANGE & 1015 pin->prop.capabilities)) { 1016 NL_SET_ERR_MSG(extack, "prio changing is not allowed"); 1017 return -EOPNOTSUPP; 1018 } 1019 ref = xa_load(&pin->dpll_refs, dpll->id); 1020 ASSERT_NOT_NULL(ref); 1021 ops = dpll_pin_ops(ref); 1022 if (!ops->prio_set) 1023 return -EOPNOTSUPP; 1024 ret = ops->prio_set(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll, 1025 dpll_priv(dpll), prio, extack); 1026 if (ret) 1027 return ret; 1028 __dpll_pin_change_ntf(pin); 1029 1030 return 0; 1031 } 1032 1033 static int 1034 dpll_pin_direction_set(struct dpll_pin *pin, struct dpll_device *dpll, 1035 enum dpll_pin_direction direction, 1036 struct netlink_ext_ack *extack) 1037 { 1038 const struct dpll_pin_ops *ops; 1039 struct dpll_pin_ref *ref; 1040 int ret; 1041 1042 if (!(DPLL_PIN_CAPABILITIES_DIRECTION_CAN_CHANGE & 1043 pin->prop.capabilities)) { 1044 NL_SET_ERR_MSG(extack, "direction changing is not allowed"); 1045 return -EOPNOTSUPP; 1046 } 1047 ref = xa_load(&pin->dpll_refs, dpll->id); 1048 ASSERT_NOT_NULL(ref); 1049 ops = dpll_pin_ops(ref); 1050 if (!ops->direction_set) 1051 return -EOPNOTSUPP; 1052 ret = ops->direction_set(pin, dpll_pin_on_dpll_priv(dpll, pin), 1053 dpll, dpll_priv(dpll), direction, extack); 1054 if (ret) 1055 return ret; 1056 __dpll_pin_change_ntf(pin); 1057 1058 return 0; 1059 } 1060 1061 static int 1062 dpll_pin_phase_adj_set(struct dpll_pin *pin, struct nlattr *phase_adj_attr, 1063 struct netlink_ext_ack *extack) 1064 { 1065 struct dpll_pin_ref *ref, *failed; 1066 const struct dpll_pin_ops *ops; 1067 s32 phase_adj, old_phase_adj; 1068 struct dpll_device *dpll; 1069 unsigned long i; 1070 int ret; 1071 1072 phase_adj = nla_get_s32(phase_adj_attr); 1073 if (phase_adj > pin->prop.phase_range.max || 1074 phase_adj < pin->prop.phase_range.min) { 1075 NL_SET_ERR_MSG_ATTR(extack, phase_adj_attr, 1076 "phase adjust value not supported"); 1077 return -EINVAL; 1078 } 1079 1080 xa_for_each(&pin->dpll_refs, i, ref) { 1081 ops = dpll_pin_ops(ref); 1082 if (!ops->phase_adjust_set || !ops->phase_adjust_get) { 1083 NL_SET_ERR_MSG(extack, "phase adjust not supported"); 1084 return -EOPNOTSUPP; 1085 } 1086 } 1087 ref = dpll_xa_ref_dpll_first(&pin->dpll_refs); 1088 ops = dpll_pin_ops(ref); 1089 dpll = ref->dpll; 1090 ret = ops->phase_adjust_get(pin, dpll_pin_on_dpll_priv(dpll, pin), 1091 dpll, dpll_priv(dpll), &old_phase_adj, 1092 extack); 1093 if (ret) { 1094 NL_SET_ERR_MSG(extack, "unable to get old phase adjust value"); 1095 return ret; 1096 } 1097 if (phase_adj == old_phase_adj) 1098 return 0; 1099 1100 xa_for_each(&pin->dpll_refs, i, ref) { 1101 ops = dpll_pin_ops(ref); 1102 dpll = ref->dpll; 1103 ret = ops->phase_adjust_set(pin, 1104 dpll_pin_on_dpll_priv(dpll, pin), 1105 dpll, dpll_priv(dpll), phase_adj, 1106 extack); 1107 if (ret) { 1108 failed = ref; 1109 NL_SET_ERR_MSG_FMT(extack, 1110 "phase adjust set failed for dpll_id:%u", 1111 dpll->id); 1112 goto rollback; 1113 } 1114 } 1115 __dpll_pin_change_ntf(pin); 1116 1117 return 0; 1118 1119 rollback: 1120 xa_for_each(&pin->dpll_refs, i, ref) { 1121 if (ref == failed) 1122 break; 1123 ops = dpll_pin_ops(ref); 1124 dpll = ref->dpll; 1125 if (ops->phase_adjust_set(pin, dpll_pin_on_dpll_priv(dpll, pin), 1126 dpll, dpll_priv(dpll), old_phase_adj, 1127 extack)) 1128 NL_SET_ERR_MSG(extack, "set phase adjust rollback failed"); 1129 } 1130 return ret; 1131 } 1132 1133 static int 1134 dpll_pin_parent_device_set(struct dpll_pin *pin, struct nlattr *parent_nest, 1135 struct netlink_ext_ack *extack) 1136 { 1137 struct nlattr *tb[DPLL_A_PIN_MAX + 1]; 1138 enum dpll_pin_direction direction; 1139 enum dpll_pin_state state; 1140 struct dpll_pin_ref *ref; 1141 struct dpll_device *dpll; 1142 u32 pdpll_idx, prio; 1143 int ret; 1144 1145 nla_parse_nested(tb, DPLL_A_PIN_MAX, parent_nest, 1146 dpll_pin_parent_device_nl_policy, extack); 1147 if (!tb[DPLL_A_PIN_PARENT_ID]) { 1148 NL_SET_ERR_MSG(extack, "device parent id expected"); 1149 return -EINVAL; 1150 } 1151 pdpll_idx = nla_get_u32(tb[DPLL_A_PIN_PARENT_ID]); 1152 dpll = xa_load(&dpll_device_xa, pdpll_idx); 1153 if (!dpll) { 1154 NL_SET_ERR_MSG(extack, "parent device not found"); 1155 return -EINVAL; 1156 } 1157 ref = xa_load(&pin->dpll_refs, dpll->id); 1158 if (!ref) { 1159 NL_SET_ERR_MSG(extack, "pin not connected to given parent device"); 1160 return -EINVAL; 1161 } 1162 if (tb[DPLL_A_PIN_STATE]) { 1163 state = nla_get_u32(tb[DPLL_A_PIN_STATE]); 1164 ret = dpll_pin_state_set(dpll, pin, state, extack); 1165 if (ret) 1166 return ret; 1167 } 1168 if (tb[DPLL_A_PIN_PRIO]) { 1169 prio = nla_get_u32(tb[DPLL_A_PIN_PRIO]); 1170 ret = dpll_pin_prio_set(dpll, pin, prio, extack); 1171 if (ret) 1172 return ret; 1173 } 1174 if (tb[DPLL_A_PIN_DIRECTION]) { 1175 direction = nla_get_u32(tb[DPLL_A_PIN_DIRECTION]); 1176 ret = dpll_pin_direction_set(pin, dpll, direction, extack); 1177 if (ret) 1178 return ret; 1179 } 1180 return 0; 1181 } 1182 1183 static int 1184 dpll_pin_parent_pin_set(struct dpll_pin *pin, struct nlattr *parent_nest, 1185 struct netlink_ext_ack *extack) 1186 { 1187 struct nlattr *tb[DPLL_A_PIN_MAX + 1]; 1188 u32 ppin_idx; 1189 int ret; 1190 1191 nla_parse_nested(tb, DPLL_A_PIN_MAX, parent_nest, 1192 dpll_pin_parent_pin_nl_policy, extack); 1193 if (!tb[DPLL_A_PIN_PARENT_ID]) { 1194 NL_SET_ERR_MSG(extack, "device parent id expected"); 1195 return -EINVAL; 1196 } 1197 ppin_idx = nla_get_u32(tb[DPLL_A_PIN_PARENT_ID]); 1198 1199 if (tb[DPLL_A_PIN_STATE]) { 1200 enum dpll_pin_state state = nla_get_u32(tb[DPLL_A_PIN_STATE]); 1201 1202 ret = dpll_pin_on_pin_state_set(pin, ppin_idx, state, extack); 1203 if (ret) 1204 return ret; 1205 } 1206 1207 return 0; 1208 } 1209 1210 static int 1211 dpll_pin_set_from_nlattr(struct dpll_pin *pin, struct genl_info *info) 1212 { 1213 struct nlattr *a; 1214 int rem, ret; 1215 1216 nla_for_each_attr(a, genlmsg_data(info->genlhdr), 1217 genlmsg_len(info->genlhdr), rem) { 1218 switch (nla_type(a)) { 1219 case DPLL_A_PIN_FREQUENCY: 1220 ret = dpll_pin_freq_set(pin, a, info->extack); 1221 if (ret) 1222 return ret; 1223 break; 1224 case DPLL_A_PIN_PHASE_ADJUST: 1225 ret = dpll_pin_phase_adj_set(pin, a, info->extack); 1226 if (ret) 1227 return ret; 1228 break; 1229 case DPLL_A_PIN_PARENT_DEVICE: 1230 ret = dpll_pin_parent_device_set(pin, a, info->extack); 1231 if (ret) 1232 return ret; 1233 break; 1234 case DPLL_A_PIN_PARENT_PIN: 1235 ret = dpll_pin_parent_pin_set(pin, a, info->extack); 1236 if (ret) 1237 return ret; 1238 break; 1239 case DPLL_A_PIN_ESYNC_FREQUENCY: 1240 ret = dpll_pin_esync_set(pin, a, info->extack); 1241 if (ret) 1242 return ret; 1243 break; 1244 } 1245 } 1246 1247 return 0; 1248 } 1249 1250 static struct dpll_pin * 1251 dpll_pin_find(u64 clock_id, struct nlattr *mod_name_attr, 1252 enum dpll_pin_type type, struct nlattr *board_label, 1253 struct nlattr *panel_label, struct nlattr *package_label, 1254 struct netlink_ext_ack *extack) 1255 { 1256 bool board_match, panel_match, package_match; 1257 struct dpll_pin *pin_match = NULL, *pin; 1258 const struct dpll_pin_properties *prop; 1259 bool cid_match, mod_match, type_match; 1260 unsigned long i; 1261 1262 xa_for_each_marked(&dpll_pin_xa, i, pin, DPLL_REGISTERED) { 1263 prop = &pin->prop; 1264 cid_match = clock_id ? pin->clock_id == clock_id : true; 1265 mod_match = mod_name_attr && module_name(pin->module) ? 1266 !nla_strcmp(mod_name_attr, 1267 module_name(pin->module)) : true; 1268 type_match = type ? prop->type == type : true; 1269 board_match = board_label ? (prop->board_label ? 1270 !nla_strcmp(board_label, prop->board_label) : false) : 1271 true; 1272 panel_match = panel_label ? (prop->panel_label ? 1273 !nla_strcmp(panel_label, prop->panel_label) : false) : 1274 true; 1275 package_match = package_label ? (prop->package_label ? 1276 !nla_strcmp(package_label, prop->package_label) : 1277 false) : true; 1278 if (cid_match && mod_match && type_match && board_match && 1279 panel_match && package_match) { 1280 if (pin_match) { 1281 NL_SET_ERR_MSG(extack, "multiple matches"); 1282 return ERR_PTR(-EINVAL); 1283 } 1284 pin_match = pin; 1285 } 1286 } 1287 if (!pin_match) { 1288 NL_SET_ERR_MSG(extack, "not found"); 1289 return ERR_PTR(-ENODEV); 1290 } 1291 return pin_match; 1292 } 1293 1294 static struct dpll_pin *dpll_pin_find_from_nlattr(struct genl_info *info) 1295 { 1296 struct nlattr *attr, *mod_name_attr = NULL, *board_label_attr = NULL, 1297 *panel_label_attr = NULL, *package_label_attr = NULL; 1298 enum dpll_pin_type type = 0; 1299 u64 clock_id = 0; 1300 int rem = 0; 1301 1302 nla_for_each_attr(attr, genlmsg_data(info->genlhdr), 1303 genlmsg_len(info->genlhdr), rem) { 1304 switch (nla_type(attr)) { 1305 case DPLL_A_PIN_CLOCK_ID: 1306 if (clock_id) 1307 goto duplicated_attr; 1308 clock_id = nla_get_u64(attr); 1309 break; 1310 case DPLL_A_PIN_MODULE_NAME: 1311 if (mod_name_attr) 1312 goto duplicated_attr; 1313 mod_name_attr = attr; 1314 break; 1315 case DPLL_A_PIN_TYPE: 1316 if (type) 1317 goto duplicated_attr; 1318 type = nla_get_u32(attr); 1319 break; 1320 case DPLL_A_PIN_BOARD_LABEL: 1321 if (board_label_attr) 1322 goto duplicated_attr; 1323 board_label_attr = attr; 1324 break; 1325 case DPLL_A_PIN_PANEL_LABEL: 1326 if (panel_label_attr) 1327 goto duplicated_attr; 1328 panel_label_attr = attr; 1329 break; 1330 case DPLL_A_PIN_PACKAGE_LABEL: 1331 if (package_label_attr) 1332 goto duplicated_attr; 1333 package_label_attr = attr; 1334 break; 1335 default: 1336 break; 1337 } 1338 } 1339 if (!(clock_id || mod_name_attr || board_label_attr || 1340 panel_label_attr || package_label_attr)) { 1341 NL_SET_ERR_MSG(info->extack, "missing attributes"); 1342 return ERR_PTR(-EINVAL); 1343 } 1344 return dpll_pin_find(clock_id, mod_name_attr, type, board_label_attr, 1345 panel_label_attr, package_label_attr, 1346 info->extack); 1347 duplicated_attr: 1348 NL_SET_ERR_MSG(info->extack, "duplicated attribute"); 1349 return ERR_PTR(-EINVAL); 1350 } 1351 1352 int dpll_nl_pin_id_get_doit(struct sk_buff *skb, struct genl_info *info) 1353 { 1354 struct dpll_pin *pin; 1355 struct sk_buff *msg; 1356 struct nlattr *hdr; 1357 int ret; 1358 1359 msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 1360 if (!msg) 1361 return -ENOMEM; 1362 hdr = genlmsg_put_reply(msg, info, &dpll_nl_family, 0, 1363 DPLL_CMD_PIN_ID_GET); 1364 if (!hdr) { 1365 nlmsg_free(msg); 1366 return -EMSGSIZE; 1367 } 1368 pin = dpll_pin_find_from_nlattr(info); 1369 if (!IS_ERR(pin)) { 1370 if (!dpll_pin_available(pin)) { 1371 nlmsg_free(msg); 1372 return -ENODEV; 1373 } 1374 ret = dpll_msg_add_pin_handle(msg, pin); 1375 if (ret) { 1376 nlmsg_free(msg); 1377 return ret; 1378 } 1379 } 1380 genlmsg_end(msg, hdr); 1381 1382 return genlmsg_reply(msg, info); 1383 } 1384 1385 int dpll_nl_pin_get_doit(struct sk_buff *skb, struct genl_info *info) 1386 { 1387 struct dpll_pin *pin = info->user_ptr[0]; 1388 struct sk_buff *msg; 1389 struct nlattr *hdr; 1390 int ret; 1391 1392 if (!pin) 1393 return -ENODEV; 1394 msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 1395 if (!msg) 1396 return -ENOMEM; 1397 hdr = genlmsg_put_reply(msg, info, &dpll_nl_family, 0, 1398 DPLL_CMD_PIN_GET); 1399 if (!hdr) { 1400 nlmsg_free(msg); 1401 return -EMSGSIZE; 1402 } 1403 ret = dpll_cmd_pin_get_one(msg, pin, info->extack); 1404 if (ret) { 1405 nlmsg_free(msg); 1406 return ret; 1407 } 1408 genlmsg_end(msg, hdr); 1409 1410 return genlmsg_reply(msg, info); 1411 } 1412 1413 int dpll_nl_pin_get_dumpit(struct sk_buff *skb, struct netlink_callback *cb) 1414 { 1415 struct dpll_dump_ctx *ctx = dpll_dump_context(cb); 1416 struct dpll_pin *pin; 1417 struct nlattr *hdr; 1418 unsigned long i; 1419 int ret = 0; 1420 1421 mutex_lock(&dpll_lock); 1422 xa_for_each_marked_start(&dpll_pin_xa, i, pin, DPLL_REGISTERED, 1423 ctx->idx) { 1424 if (!dpll_pin_available(pin)) 1425 continue; 1426 hdr = genlmsg_put(skb, NETLINK_CB(cb->skb).portid, 1427 cb->nlh->nlmsg_seq, 1428 &dpll_nl_family, NLM_F_MULTI, 1429 DPLL_CMD_PIN_GET); 1430 if (!hdr) { 1431 ret = -EMSGSIZE; 1432 break; 1433 } 1434 ret = dpll_cmd_pin_get_one(skb, pin, cb->extack); 1435 if (ret) { 1436 genlmsg_cancel(skb, hdr); 1437 break; 1438 } 1439 genlmsg_end(skb, hdr); 1440 } 1441 mutex_unlock(&dpll_lock); 1442 1443 if (ret == -EMSGSIZE) { 1444 ctx->idx = i; 1445 return skb->len; 1446 } 1447 return ret; 1448 } 1449 1450 int dpll_nl_pin_set_doit(struct sk_buff *skb, struct genl_info *info) 1451 { 1452 struct dpll_pin *pin = info->user_ptr[0]; 1453 1454 return dpll_pin_set_from_nlattr(pin, info); 1455 } 1456 1457 static struct dpll_device * 1458 dpll_device_find(u64 clock_id, struct nlattr *mod_name_attr, 1459 enum dpll_type type, struct netlink_ext_ack *extack) 1460 { 1461 struct dpll_device *dpll_match = NULL, *dpll; 1462 bool cid_match, mod_match, type_match; 1463 unsigned long i; 1464 1465 xa_for_each_marked(&dpll_device_xa, i, dpll, DPLL_REGISTERED) { 1466 cid_match = clock_id ? dpll->clock_id == clock_id : true; 1467 mod_match = mod_name_attr ? (module_name(dpll->module) ? 1468 !nla_strcmp(mod_name_attr, 1469 module_name(dpll->module)) : false) : true; 1470 type_match = type ? dpll->type == type : true; 1471 if (cid_match && mod_match && type_match) { 1472 if (dpll_match) { 1473 NL_SET_ERR_MSG(extack, "multiple matches"); 1474 return ERR_PTR(-EINVAL); 1475 } 1476 dpll_match = dpll; 1477 } 1478 } 1479 if (!dpll_match) { 1480 NL_SET_ERR_MSG(extack, "not found"); 1481 return ERR_PTR(-ENODEV); 1482 } 1483 1484 return dpll_match; 1485 } 1486 1487 static struct dpll_device * 1488 dpll_device_find_from_nlattr(struct genl_info *info) 1489 { 1490 struct nlattr *attr, *mod_name_attr = NULL; 1491 enum dpll_type type = 0; 1492 u64 clock_id = 0; 1493 int rem = 0; 1494 1495 nla_for_each_attr(attr, genlmsg_data(info->genlhdr), 1496 genlmsg_len(info->genlhdr), rem) { 1497 switch (nla_type(attr)) { 1498 case DPLL_A_CLOCK_ID: 1499 if (clock_id) 1500 goto duplicated_attr; 1501 clock_id = nla_get_u64(attr); 1502 break; 1503 case DPLL_A_MODULE_NAME: 1504 if (mod_name_attr) 1505 goto duplicated_attr; 1506 mod_name_attr = attr; 1507 break; 1508 case DPLL_A_TYPE: 1509 if (type) 1510 goto duplicated_attr; 1511 type = nla_get_u32(attr); 1512 break; 1513 default: 1514 break; 1515 } 1516 } 1517 if (!clock_id && !mod_name_attr && !type) { 1518 NL_SET_ERR_MSG(info->extack, "missing attributes"); 1519 return ERR_PTR(-EINVAL); 1520 } 1521 return dpll_device_find(clock_id, mod_name_attr, type, info->extack); 1522 duplicated_attr: 1523 NL_SET_ERR_MSG(info->extack, "duplicated attribute"); 1524 return ERR_PTR(-EINVAL); 1525 } 1526 1527 int dpll_nl_device_id_get_doit(struct sk_buff *skb, struct genl_info *info) 1528 { 1529 struct dpll_device *dpll; 1530 struct sk_buff *msg; 1531 struct nlattr *hdr; 1532 int ret; 1533 1534 msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 1535 if (!msg) 1536 return -ENOMEM; 1537 hdr = genlmsg_put_reply(msg, info, &dpll_nl_family, 0, 1538 DPLL_CMD_DEVICE_ID_GET); 1539 if (!hdr) { 1540 nlmsg_free(msg); 1541 return -EMSGSIZE; 1542 } 1543 1544 dpll = dpll_device_find_from_nlattr(info); 1545 if (!IS_ERR(dpll)) { 1546 ret = dpll_msg_add_dev_handle(msg, dpll); 1547 if (ret) { 1548 nlmsg_free(msg); 1549 return ret; 1550 } 1551 } 1552 genlmsg_end(msg, hdr); 1553 1554 return genlmsg_reply(msg, info); 1555 } 1556 1557 int dpll_nl_device_get_doit(struct sk_buff *skb, struct genl_info *info) 1558 { 1559 struct dpll_device *dpll = info->user_ptr[0]; 1560 struct sk_buff *msg; 1561 struct nlattr *hdr; 1562 int ret; 1563 1564 msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 1565 if (!msg) 1566 return -ENOMEM; 1567 hdr = genlmsg_put_reply(msg, info, &dpll_nl_family, 0, 1568 DPLL_CMD_DEVICE_GET); 1569 if (!hdr) { 1570 nlmsg_free(msg); 1571 return -EMSGSIZE; 1572 } 1573 1574 ret = dpll_device_get_one(dpll, msg, info->extack); 1575 if (ret) { 1576 nlmsg_free(msg); 1577 return ret; 1578 } 1579 genlmsg_end(msg, hdr); 1580 1581 return genlmsg_reply(msg, info); 1582 } 1583 1584 static int 1585 dpll_set_from_nlattr(struct dpll_device *dpll, struct genl_info *info) 1586 { 1587 int ret; 1588 1589 if (info->attrs[DPLL_A_PHASE_OFFSET_MONITOR]) { 1590 struct nlattr *a = info->attrs[DPLL_A_PHASE_OFFSET_MONITOR]; 1591 1592 ret = dpll_phase_offset_monitor_set(dpll, a, info->extack); 1593 if (ret) 1594 return ret; 1595 } 1596 1597 return 0; 1598 } 1599 1600 int dpll_nl_device_set_doit(struct sk_buff *skb, struct genl_info *info) 1601 { 1602 struct dpll_device *dpll = info->user_ptr[0]; 1603 1604 return dpll_set_from_nlattr(dpll, info); 1605 } 1606 1607 int dpll_nl_device_get_dumpit(struct sk_buff *skb, struct netlink_callback *cb) 1608 { 1609 struct dpll_dump_ctx *ctx = dpll_dump_context(cb); 1610 struct dpll_device *dpll; 1611 struct nlattr *hdr; 1612 unsigned long i; 1613 int ret = 0; 1614 1615 mutex_lock(&dpll_lock); 1616 xa_for_each_marked_start(&dpll_device_xa, i, dpll, DPLL_REGISTERED, 1617 ctx->idx) { 1618 hdr = genlmsg_put(skb, NETLINK_CB(cb->skb).portid, 1619 cb->nlh->nlmsg_seq, &dpll_nl_family, 1620 NLM_F_MULTI, DPLL_CMD_DEVICE_GET); 1621 if (!hdr) { 1622 ret = -EMSGSIZE; 1623 break; 1624 } 1625 ret = dpll_device_get_one(dpll, skb, cb->extack); 1626 if (ret) { 1627 genlmsg_cancel(skb, hdr); 1628 break; 1629 } 1630 genlmsg_end(skb, hdr); 1631 } 1632 mutex_unlock(&dpll_lock); 1633 1634 if (ret == -EMSGSIZE) { 1635 ctx->idx = i; 1636 return skb->len; 1637 } 1638 return ret; 1639 } 1640 1641 int dpll_pre_doit(const struct genl_split_ops *ops, struct sk_buff *skb, 1642 struct genl_info *info) 1643 { 1644 u32 id; 1645 1646 if (GENL_REQ_ATTR_CHECK(info, DPLL_A_ID)) 1647 return -EINVAL; 1648 1649 mutex_lock(&dpll_lock); 1650 id = nla_get_u32(info->attrs[DPLL_A_ID]); 1651 info->user_ptr[0] = dpll_device_get_by_id(id); 1652 if (!info->user_ptr[0]) { 1653 NL_SET_ERR_MSG(info->extack, "device not found"); 1654 goto unlock; 1655 } 1656 return 0; 1657 unlock: 1658 mutex_unlock(&dpll_lock); 1659 return -ENODEV; 1660 } 1661 1662 void dpll_post_doit(const struct genl_split_ops *ops, struct sk_buff *skb, 1663 struct genl_info *info) 1664 { 1665 mutex_unlock(&dpll_lock); 1666 } 1667 1668 int 1669 dpll_lock_doit(const struct genl_split_ops *ops, struct sk_buff *skb, 1670 struct genl_info *info) 1671 { 1672 mutex_lock(&dpll_lock); 1673 1674 return 0; 1675 } 1676 1677 void 1678 dpll_unlock_doit(const struct genl_split_ops *ops, struct sk_buff *skb, 1679 struct genl_info *info) 1680 { 1681 mutex_unlock(&dpll_lock); 1682 } 1683 1684 int dpll_pin_pre_doit(const struct genl_split_ops *ops, struct sk_buff *skb, 1685 struct genl_info *info) 1686 { 1687 int ret; 1688 1689 mutex_lock(&dpll_lock); 1690 if (GENL_REQ_ATTR_CHECK(info, DPLL_A_PIN_ID)) { 1691 ret = -EINVAL; 1692 goto unlock_dev; 1693 } 1694 info->user_ptr[0] = xa_load(&dpll_pin_xa, 1695 nla_get_u32(info->attrs[DPLL_A_PIN_ID])); 1696 if (!info->user_ptr[0] || 1697 !dpll_pin_available(info->user_ptr[0])) { 1698 NL_SET_ERR_MSG(info->extack, "pin not found"); 1699 ret = -ENODEV; 1700 goto unlock_dev; 1701 } 1702 1703 return 0; 1704 1705 unlock_dev: 1706 mutex_unlock(&dpll_lock); 1707 return ret; 1708 } 1709 1710 void dpll_pin_post_doit(const struct genl_split_ops *ops, struct sk_buff *skb, 1711 struct genl_info *info) 1712 { 1713 mutex_unlock(&dpll_lock); 1714 } 1715