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