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 static bool dpll_pin_available(struct dpll_pin *pin) 52 { 53 struct dpll_pin_ref *par_ref; 54 unsigned long i; 55 56 if (!xa_get_mark(&dpll_pin_xa, pin->id, DPLL_REGISTERED)) 57 return false; 58 xa_for_each(&pin->parent_refs, i, par_ref) 59 if (xa_get_mark(&dpll_pin_xa, par_ref->pin->id, 60 DPLL_REGISTERED)) 61 return true; 62 xa_for_each(&pin->dpll_refs, i, par_ref) 63 if (xa_get_mark(&dpll_device_xa, par_ref->dpll->id, 64 DPLL_REGISTERED)) 65 return true; 66 return false; 67 } 68 69 /** 70 * dpll_msg_add_pin_handle - attach pin handle attribute to a given message 71 * @msg: pointer to sk_buff message to attach a pin handle 72 * @pin: pin pointer 73 * 74 * Return: 75 * * 0 - success 76 * * -EMSGSIZE - no space in message to attach pin handle 77 */ 78 static int dpll_msg_add_pin_handle(struct sk_buff *msg, struct dpll_pin *pin) 79 { 80 if (!pin) 81 return 0; 82 if (nla_put_u32(msg, DPLL_A_PIN_ID, pin->id)) 83 return -EMSGSIZE; 84 return 0; 85 } 86 87 static struct dpll_pin *dpll_netdev_pin(const struct net_device *dev) 88 { 89 return rcu_dereference_rtnl(dev->dpll_pin); 90 } 91 92 int dpll_netdev_add_pin_handle(struct sk_buff *msg, 93 const struct net_device *dev) 94 { 95 return dpll_msg_add_pin_handle(msg, dpll_netdev_pin(dev)); 96 } 97 98 static int 99 dpll_msg_add_mode(struct sk_buff *msg, struct dpll_device *dpll, 100 struct netlink_ext_ack *extack) 101 { 102 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 103 enum dpll_mode mode; 104 int ret; 105 106 ret = ops->mode_get(dpll, dpll_priv(dpll), &mode, extack); 107 if (ret) 108 return ret; 109 if (nla_put_u32(msg, DPLL_A_MODE, mode)) 110 return -EMSGSIZE; 111 112 return 0; 113 } 114 115 static int 116 dpll_msg_add_mode_supported(struct sk_buff *msg, struct dpll_device *dpll, 117 struct netlink_ext_ack *extack) 118 { 119 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 120 DECLARE_BITMAP(modes, DPLL_MODE_MAX + 1) = { 0 }; 121 enum dpll_mode mode; 122 int ret; 123 124 if (ops->supported_modes_get) { 125 ret = ops->supported_modes_get(dpll, dpll_priv(dpll), modes, 126 extack); 127 if (ret) 128 return ret; 129 } else { 130 /* If the supported modes are not reported by the driver, the 131 * only supported mode is the one obtained by mode_get(). 132 */ 133 ret = ops->mode_get(dpll, dpll_priv(dpll), &mode, extack); 134 if (ret) 135 return ret; 136 137 __set_bit(mode, modes); 138 } 139 140 for_each_set_bit(mode, modes, DPLL_MODE_MAX + 1) 141 if (nla_put_u32(msg, DPLL_A_MODE_SUPPORTED, mode)) 142 return -EMSGSIZE; 143 144 return 0; 145 } 146 147 static int 148 dpll_msg_add_phase_offset_monitor(struct sk_buff *msg, struct dpll_device *dpll, 149 struct netlink_ext_ack *extack) 150 { 151 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 152 enum dpll_feature_state state; 153 int ret; 154 155 if (ops->phase_offset_monitor_set && ops->phase_offset_monitor_get) { 156 ret = ops->phase_offset_monitor_get(dpll, dpll_priv(dpll), 157 &state, extack); 158 if (ret) 159 return ret; 160 if (nla_put_u32(msg, DPLL_A_PHASE_OFFSET_MONITOR, state)) 161 return -EMSGSIZE; 162 } 163 164 return 0; 165 } 166 167 static int 168 dpll_msg_add_freq_monitor(struct sk_buff *msg, struct dpll_device *dpll, 169 struct netlink_ext_ack *extack) 170 { 171 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 172 enum dpll_feature_state state; 173 int ret; 174 175 if (ops->freq_monitor_set && ops->freq_monitor_get) { 176 ret = ops->freq_monitor_get(dpll, dpll_priv(dpll), 177 &state, extack); 178 if (ret) 179 return ret; 180 if (nla_put_u32(msg, DPLL_A_FREQUENCY_MONITOR, state)) 181 return -EMSGSIZE; 182 } 183 184 return 0; 185 } 186 187 static int 188 dpll_msg_add_phase_offset_avg_factor(struct sk_buff *msg, 189 struct dpll_device *dpll, 190 struct netlink_ext_ack *extack) 191 { 192 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 193 u32 factor; 194 int ret; 195 196 if (ops->phase_offset_avg_factor_get) { 197 ret = ops->phase_offset_avg_factor_get(dpll, dpll_priv(dpll), 198 &factor, extack); 199 if (ret) 200 return ret; 201 if (nla_put_u32(msg, DPLL_A_PHASE_OFFSET_AVG_FACTOR, factor)) 202 return -EMSGSIZE; 203 } 204 205 return 0; 206 } 207 208 static int 209 dpll_msg_add_lock_status(struct sk_buff *msg, struct dpll_device *dpll, 210 struct netlink_ext_ack *extack) 211 { 212 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 213 enum dpll_lock_status_error status_error = 0; 214 enum dpll_lock_status status; 215 int ret; 216 217 ret = ops->lock_status_get(dpll, dpll_priv(dpll), &status, 218 &status_error, extack); 219 if (ret) 220 return ret; 221 if (nla_put_u32(msg, DPLL_A_LOCK_STATUS, status)) 222 return -EMSGSIZE; 223 if (status_error && 224 (status == DPLL_LOCK_STATUS_UNLOCKED || 225 status == DPLL_LOCK_STATUS_HOLDOVER) && 226 nla_put_u32(msg, DPLL_A_LOCK_STATUS_ERROR, status_error)) 227 return -EMSGSIZE; 228 229 return 0; 230 } 231 232 static int 233 dpll_msg_add_temp(struct sk_buff *msg, struct dpll_device *dpll, 234 struct netlink_ext_ack *extack) 235 { 236 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 237 s32 temp; 238 int ret; 239 240 if (!ops->temp_get) 241 return 0; 242 ret = ops->temp_get(dpll, dpll_priv(dpll), &temp, extack); 243 if (ret) 244 return ret; 245 if (nla_put_s32(msg, DPLL_A_TEMP, temp)) 246 return -EMSGSIZE; 247 248 return 0; 249 } 250 251 static int 252 dpll_msg_add_clock_quality_level(struct sk_buff *msg, struct dpll_device *dpll, 253 struct netlink_ext_ack *extack) 254 { 255 DECLARE_BITMAP(qls, DPLL_CLOCK_QUALITY_LEVEL_MAX + 1) = { 0 }; 256 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 257 enum dpll_clock_quality_level ql; 258 int ret; 259 260 if (!ops->clock_quality_level_get) 261 return 0; 262 ret = ops->clock_quality_level_get(dpll, dpll_priv(dpll), qls, extack); 263 if (ret) 264 return ret; 265 for_each_set_bit(ql, qls, DPLL_CLOCK_QUALITY_LEVEL_MAX + 1) 266 if (nla_put_u32(msg, DPLL_A_CLOCK_QUALITY_LEVEL, ql)) 267 return -EMSGSIZE; 268 269 return 0; 270 } 271 272 static int 273 dpll_msg_add_pin_prio(struct sk_buff *msg, struct dpll_pin *pin, 274 struct dpll_pin_ref *ref, 275 struct netlink_ext_ack *extack) 276 { 277 const struct dpll_pin_ops *ops = dpll_pin_ops(ref); 278 struct dpll_device *dpll = ref->dpll; 279 u32 prio; 280 int ret; 281 282 if (!ops->prio_get) 283 return 0; 284 ret = ops->prio_get(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll, 285 dpll_priv(dpll), &prio, extack); 286 if (ret) 287 return ret; 288 if (nla_put_u32(msg, DPLL_A_PIN_PRIO, prio)) 289 return -EMSGSIZE; 290 291 return 0; 292 } 293 294 static int 295 dpll_msg_add_pin_on_dpll_state(struct sk_buff *msg, struct dpll_pin *pin, 296 struct dpll_pin_ref *ref, 297 struct netlink_ext_ack *extack) 298 { 299 const struct dpll_pin_ops *ops = dpll_pin_ops(ref); 300 struct dpll_device *dpll = ref->dpll; 301 enum dpll_pin_state state; 302 int ret; 303 304 if (!ops->state_on_dpll_get) 305 return 0; 306 ret = ops->state_on_dpll_get(pin, dpll_pin_on_dpll_priv(dpll, pin), 307 dpll, dpll_priv(dpll), &state, extack); 308 if (ret) 309 return ret; 310 if (nla_put_u32(msg, DPLL_A_PIN_STATE, state)) 311 return -EMSGSIZE; 312 313 return 0; 314 } 315 316 static int 317 dpll_msg_add_pin_operstate(struct sk_buff *msg, struct dpll_pin *pin, 318 struct dpll_pin_ref *ref, 319 struct netlink_ext_ack *extack) 320 { 321 const struct dpll_pin_ops *ops = dpll_pin_ops(ref); 322 struct dpll_device *dpll = ref->dpll; 323 enum dpll_pin_operstate operstate; 324 int ret; 325 326 if (!ops->operstate_on_dpll_get) 327 return 0; 328 ret = ops->operstate_on_dpll_get(pin, 329 dpll_pin_on_dpll_priv(dpll, pin), 330 dpll, dpll_priv(dpll), 331 &operstate, extack); 332 if (ret) 333 return ret; 334 if (nla_put_u32(msg, DPLL_A_PIN_OPERSTATE, operstate)) 335 return -EMSGSIZE; 336 337 return 0; 338 } 339 340 static int 341 dpll_msg_add_pin_direction(struct sk_buff *msg, struct dpll_pin *pin, 342 struct dpll_pin_ref *ref, 343 struct netlink_ext_ack *extack) 344 { 345 const struct dpll_pin_ops *ops = dpll_pin_ops(ref); 346 struct dpll_device *dpll = ref->dpll; 347 enum dpll_pin_direction direction; 348 int ret; 349 350 ret = ops->direction_get(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll, 351 dpll_priv(dpll), &direction, extack); 352 if (ret) 353 return ret; 354 if (nla_put_u32(msg, DPLL_A_PIN_DIRECTION, direction)) 355 return -EMSGSIZE; 356 357 return 0; 358 } 359 360 static int 361 dpll_msg_add_pin_phase_adjust(struct sk_buff *msg, struct dpll_pin *pin, 362 struct dpll_pin_ref *ref, 363 struct netlink_ext_ack *extack) 364 { 365 const struct dpll_pin_ops *ops = dpll_pin_ops(ref); 366 struct dpll_device *dpll = ref->dpll; 367 s32 phase_adjust; 368 int ret; 369 370 if (!ops->phase_adjust_get) 371 return 0; 372 ret = ops->phase_adjust_get(pin, dpll_pin_on_dpll_priv(dpll, pin), 373 dpll, dpll_priv(dpll), 374 &phase_adjust, extack); 375 if (ret) 376 return ret; 377 if (nla_put_s32(msg, DPLL_A_PIN_PHASE_ADJUST, phase_adjust)) 378 return -EMSGSIZE; 379 380 return 0; 381 } 382 383 static int 384 dpll_msg_add_phase_offset(struct sk_buff *msg, struct dpll_pin *pin, 385 struct dpll_pin_ref *ref, 386 struct netlink_ext_ack *extack) 387 { 388 const struct dpll_pin_ops *ops = dpll_pin_ops(ref); 389 struct dpll_device *dpll = ref->dpll; 390 s64 phase_offset; 391 int ret; 392 393 if (!ops->phase_offset_get) 394 return 0; 395 ret = ops->phase_offset_get(pin, dpll_pin_on_dpll_priv(dpll, pin), 396 dpll, dpll_priv(dpll), &phase_offset, 397 extack); 398 if (ret) 399 return ret; 400 if (nla_put_64bit(msg, DPLL_A_PIN_PHASE_OFFSET, sizeof(phase_offset), 401 &phase_offset, DPLL_A_PIN_PAD)) 402 return -EMSGSIZE; 403 404 return 0; 405 } 406 407 static int dpll_msg_add_ffo(struct sk_buff *msg, struct dpll_pin *pin, 408 struct dpll_pin_ref *ref, 409 enum dpll_ffo_type type, 410 struct netlink_ext_ack *extack) 411 { 412 const struct dpll_pin_ops *ops = dpll_pin_ops(ref); 413 struct dpll_ffo_param ffo = { .type = type }; 414 int ret; 415 416 if (!ops->ffo_get || !(ops->supported_ffo & BIT(type))) 417 return 0; 418 ret = ops->ffo_get(pin, dpll_pin_on_dpll_priv(ref->dpll, pin), 419 ref->dpll, dpll_priv(ref->dpll), &ffo, extack); 420 if (ret) { 421 if (ret == -ENODATA) 422 return 0; 423 return ret; 424 } 425 if (nla_put_sint(msg, DPLL_A_PIN_FRACTIONAL_FREQUENCY_OFFSET, 426 div_s64(ffo.ffo, 1000000))) 427 return -EMSGSIZE; 428 return nla_put_sint(msg, 429 DPLL_A_PIN_FRACTIONAL_FREQUENCY_OFFSET_PPT, 430 ffo.ffo); 431 } 432 433 static int dpll_msg_add_measured_freq(struct sk_buff *msg, struct dpll_pin *pin, 434 struct dpll_pin_ref *ref, 435 struct netlink_ext_ack *extack) 436 { 437 const struct dpll_device_ops *dev_ops = dpll_device_ops(ref->dpll); 438 const struct dpll_pin_ops *ops = dpll_pin_ops(ref); 439 struct dpll_device *dpll = ref->dpll; 440 enum dpll_feature_state state; 441 u64 measured_freq; 442 int ret; 443 444 if (!ops->measured_freq_get) 445 return 0; 446 ret = dev_ops->freq_monitor_get(dpll, dpll_priv(dpll), 447 &state, extack); 448 if (ret) 449 return ret; 450 if (state == DPLL_FEATURE_STATE_DISABLE) 451 return 0; 452 ret = ops->measured_freq_get(pin, dpll_pin_on_dpll_priv(dpll, pin), 453 dpll, dpll_priv(dpll), &measured_freq, 454 extack); 455 if (ret) 456 return ret; 457 if (nla_put_64bit(msg, DPLL_A_PIN_MEASURED_FREQUENCY, 458 sizeof(measured_freq), &measured_freq, 459 DPLL_A_PIN_PAD)) 460 return -EMSGSIZE; 461 462 return 0; 463 } 464 465 static int 466 dpll_msg_add_pin_freq(struct sk_buff *msg, struct dpll_pin *pin, 467 struct dpll_pin_ref *ref, struct netlink_ext_ack *extack) 468 { 469 const struct dpll_pin_ops *ops = dpll_pin_ops(ref); 470 struct dpll_device *dpll = ref->dpll; 471 struct nlattr *nest; 472 int fs, ret; 473 u64 freq; 474 475 if (!ops->frequency_get) 476 return 0; 477 ret = ops->frequency_get(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll, 478 dpll_priv(dpll), &freq, extack); 479 if (ret) 480 return ret; 481 if (nla_put_64bit(msg, DPLL_A_PIN_FREQUENCY, sizeof(freq), &freq, 482 DPLL_A_PIN_PAD)) 483 return -EMSGSIZE; 484 for (fs = 0; fs < pin->prop.freq_supported_num; fs++) { 485 nest = nla_nest_start(msg, DPLL_A_PIN_FREQUENCY_SUPPORTED); 486 if (!nest) 487 return -EMSGSIZE; 488 freq = pin->prop.freq_supported[fs].min; 489 if (nla_put_64bit(msg, DPLL_A_PIN_FREQUENCY_MIN, sizeof(freq), 490 &freq, DPLL_A_PIN_PAD)) { 491 nla_nest_cancel(msg, nest); 492 return -EMSGSIZE; 493 } 494 freq = pin->prop.freq_supported[fs].max; 495 if (nla_put_64bit(msg, DPLL_A_PIN_FREQUENCY_MAX, sizeof(freq), 496 &freq, DPLL_A_PIN_PAD)) { 497 nla_nest_cancel(msg, nest); 498 return -EMSGSIZE; 499 } 500 nla_nest_end(msg, nest); 501 } 502 503 return 0; 504 } 505 506 static int 507 dpll_msg_add_pin_esync(struct sk_buff *msg, struct dpll_pin *pin, 508 struct dpll_pin_ref *ref, struct netlink_ext_ack *extack) 509 { 510 const struct dpll_pin_ops *ops = dpll_pin_ops(ref); 511 struct dpll_device *dpll = ref->dpll; 512 struct dpll_pin_esync esync; 513 struct nlattr *nest; 514 int ret, i; 515 516 if (!ops->esync_get) 517 return 0; 518 ret = ops->esync_get(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll, 519 dpll_priv(dpll), &esync, extack); 520 if (ret == -EOPNOTSUPP) 521 return 0; 522 else if (ret) 523 return ret; 524 if (nla_put_64bit(msg, DPLL_A_PIN_ESYNC_FREQUENCY, sizeof(esync.freq), 525 &esync.freq, DPLL_A_PIN_PAD)) 526 return -EMSGSIZE; 527 if (nla_put_u32(msg, DPLL_A_PIN_ESYNC_PULSE, esync.pulse)) 528 return -EMSGSIZE; 529 for (i = 0; i < esync.range_num; i++) { 530 nest = nla_nest_start(msg, 531 DPLL_A_PIN_ESYNC_FREQUENCY_SUPPORTED); 532 if (!nest) 533 return -EMSGSIZE; 534 if (nla_put_64bit(msg, DPLL_A_PIN_FREQUENCY_MIN, 535 sizeof(esync.range[i].min), 536 &esync.range[i].min, DPLL_A_PIN_PAD)) 537 goto nest_cancel; 538 if (nla_put_64bit(msg, DPLL_A_PIN_FREQUENCY_MAX, 539 sizeof(esync.range[i].max), 540 &esync.range[i].max, DPLL_A_PIN_PAD)) 541 goto nest_cancel; 542 nla_nest_end(msg, nest); 543 } 544 return 0; 545 546 nest_cancel: 547 nla_nest_cancel(msg, nest); 548 return -EMSGSIZE; 549 } 550 551 static int 552 dpll_msg_add_pin_ref_sync(struct sk_buff *msg, struct dpll_pin *pin, 553 struct dpll_pin_ref *ref, 554 struct netlink_ext_ack *extack) 555 { 556 const struct dpll_pin_ops *ops = dpll_pin_ops(ref); 557 struct dpll_device *dpll = ref->dpll; 558 void *pin_priv, *ref_sync_pin_priv; 559 struct dpll_pin *ref_sync_pin; 560 enum dpll_pin_state state; 561 struct nlattr *nest; 562 unsigned long index; 563 int ret; 564 565 pin_priv = dpll_pin_on_dpll_priv(dpll, pin); 566 xa_for_each(&pin->ref_sync_pins, index, ref_sync_pin) { 567 if (!dpll_pin_available(ref_sync_pin)) 568 continue; 569 ref_sync_pin_priv = dpll_pin_on_dpll_priv(dpll, ref_sync_pin); 570 if (WARN_ON(!ops->ref_sync_get)) 571 return -EOPNOTSUPP; 572 ret = ops->ref_sync_get(pin, pin_priv, ref_sync_pin, 573 ref_sync_pin_priv, &state, extack); 574 if (ret) 575 return ret; 576 nest = nla_nest_start(msg, DPLL_A_PIN_REFERENCE_SYNC); 577 if (!nest) 578 return -EMSGSIZE; 579 if (nla_put_s32(msg, DPLL_A_PIN_ID, ref_sync_pin->id)) 580 goto nest_cancel; 581 if (nla_put_s32(msg, DPLL_A_PIN_STATE, state)) 582 goto nest_cancel; 583 nla_nest_end(msg, nest); 584 } 585 return 0; 586 587 nest_cancel: 588 nla_nest_cancel(msg, nest); 589 return -EMSGSIZE; 590 } 591 592 static bool dpll_pin_is_freq_supported(struct dpll_pin *pin, u32 freq) 593 { 594 int fs; 595 596 for (fs = 0; fs < pin->prop.freq_supported_num; fs++) 597 if (freq >= pin->prop.freq_supported[fs].min && 598 freq <= pin->prop.freq_supported[fs].max) 599 return true; 600 return false; 601 } 602 603 static int 604 dpll_msg_add_pin_parents(struct sk_buff *msg, struct dpll_pin *pin, 605 struct dpll_pin_ref *dpll_ref, 606 struct netlink_ext_ack *extack) 607 { 608 enum dpll_pin_state state; 609 struct dpll_pin_ref *ref; 610 struct dpll_pin *ppin; 611 struct nlattr *nest; 612 unsigned long index; 613 int ret; 614 615 xa_for_each(&pin->parent_refs, index, ref) { 616 const struct dpll_pin_ops *ops = dpll_pin_ops(ref); 617 void *parent_priv; 618 619 ppin = ref->pin; 620 parent_priv = dpll_pin_on_dpll_priv(dpll_ref->dpll, ppin); 621 ret = ops->state_on_pin_get(pin, 622 dpll_pin_on_pin_priv(ppin, pin), 623 ppin, parent_priv, &state, extack); 624 if (ret) 625 return ret; 626 nest = nla_nest_start(msg, DPLL_A_PIN_PARENT_PIN); 627 if (!nest) 628 return -EMSGSIZE; 629 ret = dpll_msg_add_dev_parent_handle(msg, ppin->id); 630 if (ret) 631 goto nest_cancel; 632 if (nla_put_u32(msg, DPLL_A_PIN_STATE, state)) { 633 ret = -EMSGSIZE; 634 goto nest_cancel; 635 } 636 nla_nest_end(msg, nest); 637 } 638 639 return 0; 640 641 nest_cancel: 642 nla_nest_cancel(msg, nest); 643 return ret; 644 } 645 646 static int 647 dpll_msg_add_pin_dplls(struct sk_buff *msg, struct dpll_pin *pin, 648 struct netlink_ext_ack *extack) 649 { 650 struct dpll_pin_ref *ref; 651 struct nlattr *attr; 652 unsigned long index; 653 int ret; 654 655 xa_for_each(&pin->dpll_refs, index, ref) { 656 attr = nla_nest_start(msg, DPLL_A_PIN_PARENT_DEVICE); 657 if (!attr) 658 return -EMSGSIZE; 659 ret = dpll_msg_add_dev_parent_handle(msg, ref->dpll->id); 660 if (ret) 661 goto nest_cancel; 662 ret = dpll_msg_add_pin_on_dpll_state(msg, pin, ref, extack); 663 if (ret) 664 goto nest_cancel; 665 ret = dpll_msg_add_pin_operstate(msg, pin, ref, extack); 666 if (ret) 667 goto nest_cancel; 668 ret = dpll_msg_add_pin_prio(msg, pin, ref, extack); 669 if (ret) 670 goto nest_cancel; 671 ret = dpll_msg_add_pin_direction(msg, pin, ref, extack); 672 if (ret) 673 goto nest_cancel; 674 ret = dpll_msg_add_phase_offset(msg, pin, ref, extack); 675 if (ret) 676 goto nest_cancel; 677 ret = dpll_msg_add_ffo(msg, pin, ref, 678 DPLL_FFO_PIN_DEVICE, extack); 679 if (ret) 680 goto nest_cancel; 681 nla_nest_end(msg, attr); 682 } 683 684 return 0; 685 686 nest_cancel: 687 nla_nest_end(msg, attr); 688 return ret; 689 } 690 691 static int 692 dpll_cmd_pin_get_one(struct sk_buff *msg, struct dpll_pin *pin, 693 struct netlink_ext_ack *extack) 694 { 695 const struct dpll_pin_properties *prop = &pin->prop; 696 struct dpll_pin_ref *ref; 697 int ret; 698 699 ref = dpll_xa_ref_dpll_first(&pin->dpll_refs); 700 ASSERT_NOT_NULL(ref); 701 702 ret = dpll_msg_add_pin_handle(msg, pin); 703 if (ret) 704 return ret; 705 if (nla_put_string(msg, DPLL_A_PIN_MODULE_NAME, 706 pin->module_name)) 707 return -EMSGSIZE; 708 if (nla_put_64bit(msg, DPLL_A_PIN_CLOCK_ID, sizeof(pin->clock_id), 709 &pin->clock_id, DPLL_A_PIN_PAD)) 710 return -EMSGSIZE; 711 if (prop->board_label && 712 nla_put_string(msg, DPLL_A_PIN_BOARD_LABEL, prop->board_label)) 713 return -EMSGSIZE; 714 if (prop->panel_label && 715 nla_put_string(msg, DPLL_A_PIN_PANEL_LABEL, prop->panel_label)) 716 return -EMSGSIZE; 717 if (prop->package_label && 718 nla_put_string(msg, DPLL_A_PIN_PACKAGE_LABEL, 719 prop->package_label)) 720 return -EMSGSIZE; 721 if (nla_put_u32(msg, DPLL_A_PIN_TYPE, prop->type)) 722 return -EMSGSIZE; 723 if (nla_put_u32(msg, DPLL_A_PIN_CAPABILITIES, prop->capabilities)) 724 return -EMSGSIZE; 725 ret = dpll_msg_add_pin_freq(msg, pin, ref, extack); 726 if (ret) 727 return ret; 728 if (prop->phase_gran && 729 nla_put_u32(msg, DPLL_A_PIN_PHASE_ADJUST_GRAN, 730 prop->phase_gran)) 731 return -EMSGSIZE; 732 if (nla_put_s32(msg, DPLL_A_PIN_PHASE_ADJUST_MIN, 733 prop->phase_range.min)) 734 return -EMSGSIZE; 735 if (nla_put_s32(msg, DPLL_A_PIN_PHASE_ADJUST_MAX, 736 prop->phase_range.max)) 737 return -EMSGSIZE; 738 ret = dpll_msg_add_pin_phase_adjust(msg, pin, ref, extack); 739 if (ret) 740 return ret; 741 ret = dpll_msg_add_ffo(msg, pin, ref, 742 DPLL_FFO_PORT_RXTX_RATE, extack); 743 if (ret) 744 return ret; 745 ret = dpll_msg_add_measured_freq(msg, pin, ref, extack); 746 if (ret) 747 return ret; 748 ret = dpll_msg_add_pin_esync(msg, pin, ref, extack); 749 if (ret) 750 return ret; 751 if (!xa_empty(&pin->ref_sync_pins)) 752 ret = dpll_msg_add_pin_ref_sync(msg, pin, ref, extack); 753 if (ret) 754 return ret; 755 if (xa_empty(&pin->parent_refs)) 756 ret = dpll_msg_add_pin_dplls(msg, pin, extack); 757 else 758 ret = dpll_msg_add_pin_parents(msg, pin, ref, extack); 759 760 return ret; 761 } 762 763 static int 764 dpll_device_get_one(struct dpll_device *dpll, struct sk_buff *msg, 765 struct netlink_ext_ack *extack) 766 { 767 int ret; 768 769 ret = dpll_msg_add_dev_handle(msg, dpll); 770 if (ret) 771 return ret; 772 if (nla_put_string(msg, DPLL_A_MODULE_NAME, module_name(dpll->module))) 773 return -EMSGSIZE; 774 if (nla_put_64bit(msg, DPLL_A_CLOCK_ID, sizeof(dpll->clock_id), 775 &dpll->clock_id, DPLL_A_PAD)) 776 return -EMSGSIZE; 777 ret = dpll_msg_add_temp(msg, dpll, extack); 778 if (ret) 779 return ret; 780 ret = dpll_msg_add_lock_status(msg, dpll, extack); 781 if (ret) 782 return ret; 783 ret = dpll_msg_add_clock_quality_level(msg, dpll, extack); 784 if (ret) 785 return ret; 786 ret = dpll_msg_add_mode(msg, dpll, extack); 787 if (ret) 788 return ret; 789 ret = dpll_msg_add_mode_supported(msg, dpll, extack); 790 if (ret) 791 return ret; 792 if (nla_put_u32(msg, DPLL_A_TYPE, dpll->type)) 793 return -EMSGSIZE; 794 ret = dpll_msg_add_phase_offset_monitor(msg, dpll, extack); 795 if (ret) 796 return ret; 797 ret = dpll_msg_add_phase_offset_avg_factor(msg, dpll, extack); 798 if (ret) 799 return ret; 800 ret = dpll_msg_add_freq_monitor(msg, dpll, extack); 801 if (ret) 802 return ret; 803 804 return 0; 805 } 806 807 static int 808 dpll_device_event_send(enum dpll_cmd event, struct dpll_device *dpll) 809 { 810 struct sk_buff *msg; 811 int ret = -ENOMEM; 812 void *hdr; 813 814 if (WARN_ON(!xa_get_mark(&dpll_device_xa, dpll->id, DPLL_REGISTERED))) 815 return -ENODEV; 816 msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 817 if (!msg) 818 return -ENOMEM; 819 hdr = genlmsg_put(msg, 0, 0, &dpll_nl_family, 0, event); 820 if (!hdr) 821 goto err_free_msg; 822 ret = dpll_device_get_one(dpll, msg, NULL); 823 if (ret) 824 goto err_cancel_msg; 825 genlmsg_end(msg, hdr); 826 genlmsg_multicast(&dpll_nl_family, msg, 0, 0, GFP_KERNEL); 827 828 return 0; 829 830 err_cancel_msg: 831 genlmsg_cancel(msg, hdr); 832 err_free_msg: 833 nlmsg_free(msg); 834 835 return ret; 836 } 837 838 int dpll_device_create_ntf(struct dpll_device *dpll) 839 { 840 dpll_device_notify(dpll, DPLL_DEVICE_CREATED); 841 return dpll_device_event_send(DPLL_CMD_DEVICE_CREATE_NTF, dpll); 842 } 843 844 int dpll_device_delete_ntf(struct dpll_device *dpll) 845 { 846 dpll_device_notify(dpll, DPLL_DEVICE_DELETED); 847 return dpll_device_event_send(DPLL_CMD_DEVICE_DELETE_NTF, dpll); 848 } 849 850 /** 851 * __dpll_device_change_ntf - notify that the dpll device has been changed 852 * @dpll: registered dpll pointer 853 * 854 * Context: caller must hold dpll_lock. Suitable for use inside device 855 * callbacks which are already invoked under dpll_lock. 856 * Return: 0 if succeeds, error code otherwise. 857 */ 858 int __dpll_device_change_ntf(struct dpll_device *dpll) 859 { 860 lockdep_assert_held(&dpll_lock); 861 dpll_device_notify(dpll, DPLL_DEVICE_CHANGED); 862 return dpll_device_event_send(DPLL_CMD_DEVICE_CHANGE_NTF, dpll); 863 } 864 EXPORT_SYMBOL_GPL(__dpll_device_change_ntf); 865 866 /** 867 * dpll_device_change_ntf - notify that the dpll device has been changed 868 * @dpll: registered dpll pointer 869 * 870 * Context: acquires and holds a dpll_lock. 871 * Return: 0 if succeeds, error code otherwise. 872 */ 873 int dpll_device_change_ntf(struct dpll_device *dpll) 874 { 875 int ret; 876 877 mutex_lock(&dpll_lock); 878 ret = __dpll_device_change_ntf(dpll); 879 mutex_unlock(&dpll_lock); 880 881 return ret; 882 } 883 EXPORT_SYMBOL_GPL(dpll_device_change_ntf); 884 885 static int 886 dpll_pin_event_send(enum dpll_cmd event, struct dpll_pin *pin) 887 { 888 struct sk_buff *msg; 889 int ret = -ENOMEM; 890 void *hdr; 891 892 if (!dpll_pin_available(pin)) 893 return -ENODEV; 894 895 msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 896 if (!msg) 897 return -ENOMEM; 898 899 hdr = genlmsg_put(msg, 0, 0, &dpll_nl_family, 0, event); 900 if (!hdr) 901 goto err_free_msg; 902 ret = dpll_cmd_pin_get_one(msg, pin, NULL); 903 if (ret) 904 goto err_cancel_msg; 905 genlmsg_end(msg, hdr); 906 genlmsg_multicast(&dpll_nl_family, msg, 0, 0, GFP_KERNEL); 907 908 return 0; 909 910 err_cancel_msg: 911 genlmsg_cancel(msg, hdr); 912 err_free_msg: 913 nlmsg_free(msg); 914 915 return ret; 916 } 917 918 int dpll_pin_create_ntf(struct dpll_pin *pin, u64 src_clock_id) 919 { 920 dpll_pin_notify(pin, src_clock_id, DPLL_PIN_CREATED); 921 return dpll_pin_event_send(DPLL_CMD_PIN_CREATE_NTF, pin); 922 } 923 924 int dpll_pin_delete_ntf(struct dpll_pin *pin, u64 src_clock_id) 925 { 926 dpll_pin_notify(pin, src_clock_id, DPLL_PIN_DELETED); 927 return dpll_pin_event_send(DPLL_CMD_PIN_DELETE_NTF, pin); 928 } 929 930 /** 931 * __dpll_pin_change_ntf - notify that the pin has been changed 932 * @pin: registered pin pointer 933 * 934 * Context: caller must hold dpll_lock. Suitable for use inside pin 935 * callbacks which are already invoked under dpll_lock. 936 * Return: 0 if succeeds, error code otherwise. 937 */ 938 int __dpll_pin_change_ntf(struct dpll_pin *pin) 939 { 940 lockdep_assert_held(&dpll_lock); 941 dpll_pin_notify(pin, pin->clock_id, DPLL_PIN_CHANGED); 942 return dpll_pin_event_send(DPLL_CMD_PIN_CHANGE_NTF, pin); 943 } 944 EXPORT_SYMBOL_GPL(__dpll_pin_change_ntf); 945 946 /** 947 * dpll_pin_change_ntf - notify that the pin has been changed 948 * @pin: registered pin pointer 949 * 950 * Context: acquires and holds a dpll_lock. 951 * Return: 0 if succeeds, error code otherwise. 952 */ 953 int dpll_pin_change_ntf(struct dpll_pin *pin) 954 { 955 int ret; 956 957 mutex_lock(&dpll_lock); 958 ret = __dpll_pin_change_ntf(pin); 959 mutex_unlock(&dpll_lock); 960 961 return ret; 962 } 963 EXPORT_SYMBOL_GPL(dpll_pin_change_ntf); 964 965 static int 966 dpll_mode_set(struct dpll_device *dpll, struct nlattr *a, 967 struct netlink_ext_ack *extack) 968 { 969 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 970 DECLARE_BITMAP(modes, DPLL_MODE_MAX + 1) = { 0 }; 971 enum dpll_mode mode = nla_get_u32(a), old_mode; 972 int ret; 973 974 if (!(ops->mode_set && ops->supported_modes_get)) { 975 NL_SET_ERR_MSG_ATTR(extack, a, 976 "dpll device does not support mode switch"); 977 return -EOPNOTSUPP; 978 } 979 980 ret = ops->mode_get(dpll, dpll_priv(dpll), &old_mode, extack); 981 if (ret) { 982 NL_SET_ERR_MSG(extack, "unable to get current mode"); 983 return ret; 984 } 985 986 if (mode == old_mode) 987 return 0; 988 989 ret = ops->supported_modes_get(dpll, dpll_priv(dpll), modes, extack); 990 if (ret) { 991 NL_SET_ERR_MSG(extack, "unable to get supported modes"); 992 return ret; 993 } 994 995 if (!test_bit(mode, modes)) { 996 NL_SET_ERR_MSG(extack, 997 "dpll device does not support requested mode"); 998 return -EINVAL; 999 } 1000 1001 return ops->mode_set(dpll, dpll_priv(dpll), mode, extack); 1002 } 1003 1004 static int 1005 dpll_phase_offset_monitor_set(struct dpll_device *dpll, struct nlattr *a, 1006 struct netlink_ext_ack *extack) 1007 { 1008 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 1009 enum dpll_feature_state state = nla_get_u32(a), old_state; 1010 int ret; 1011 1012 if (!(ops->phase_offset_monitor_set && ops->phase_offset_monitor_get)) { 1013 NL_SET_ERR_MSG_ATTR(extack, a, "dpll device not capable of phase offset monitor"); 1014 return -EOPNOTSUPP; 1015 } 1016 ret = ops->phase_offset_monitor_get(dpll, dpll_priv(dpll), &old_state, 1017 extack); 1018 if (ret) { 1019 NL_SET_ERR_MSG(extack, "unable to get current state of phase offset monitor"); 1020 return ret; 1021 } 1022 if (state == old_state) 1023 return 0; 1024 1025 return ops->phase_offset_monitor_set(dpll, dpll_priv(dpll), state, 1026 extack); 1027 } 1028 1029 static int 1030 dpll_phase_offset_avg_factor_set(struct dpll_device *dpll, struct nlattr *a, 1031 struct netlink_ext_ack *extack) 1032 { 1033 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 1034 u32 factor = nla_get_u32(a); 1035 1036 if (!ops->phase_offset_avg_factor_set) { 1037 NL_SET_ERR_MSG_ATTR(extack, a, 1038 "device not capable of changing phase offset average factor"); 1039 return -EOPNOTSUPP; 1040 } 1041 1042 return ops->phase_offset_avg_factor_set(dpll, dpll_priv(dpll), factor, 1043 extack); 1044 } 1045 1046 static int 1047 dpll_freq_monitor_set(struct dpll_device *dpll, struct nlattr *a, 1048 struct netlink_ext_ack *extack) 1049 { 1050 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 1051 enum dpll_feature_state state = nla_get_u32(a), old_state; 1052 int ret; 1053 1054 if (!(ops->freq_monitor_set && ops->freq_monitor_get)) { 1055 NL_SET_ERR_MSG_ATTR(extack, a, 1056 "dpll device not capable of frequency monitor"); 1057 return -EOPNOTSUPP; 1058 } 1059 ret = ops->freq_monitor_get(dpll, dpll_priv(dpll), &old_state, 1060 extack); 1061 if (ret) { 1062 NL_SET_ERR_MSG(extack, 1063 "unable to get current state of frequency monitor"); 1064 return ret; 1065 } 1066 if (state == old_state) 1067 return 0; 1068 1069 return ops->freq_monitor_set(dpll, dpll_priv(dpll), state, extack); 1070 } 1071 1072 static int 1073 dpll_pin_freq_set(struct dpll_pin *pin, struct nlattr *a, 1074 struct netlink_ext_ack *extack) 1075 { 1076 u64 freq = nla_get_u64(a), old_freq; 1077 struct dpll_pin_ref *ref, *failed; 1078 const struct dpll_pin_ops *ops; 1079 struct dpll_device *dpll; 1080 unsigned long i; 1081 int ret; 1082 1083 if (!dpll_pin_is_freq_supported(pin, freq)) { 1084 NL_SET_ERR_MSG_ATTR(extack, a, "frequency is not supported by the device"); 1085 return -EINVAL; 1086 } 1087 1088 xa_for_each(&pin->dpll_refs, i, ref) { 1089 ops = dpll_pin_ops(ref); 1090 if (!ops->frequency_set || !ops->frequency_get) { 1091 NL_SET_ERR_MSG(extack, "frequency set not supported by the device"); 1092 return -EOPNOTSUPP; 1093 } 1094 } 1095 ref = dpll_xa_ref_dpll_first(&pin->dpll_refs); 1096 ops = dpll_pin_ops(ref); 1097 dpll = ref->dpll; 1098 ret = ops->frequency_get(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll, 1099 dpll_priv(dpll), &old_freq, extack); 1100 if (ret) { 1101 NL_SET_ERR_MSG(extack, "unable to get old frequency value"); 1102 return ret; 1103 } 1104 if (freq == old_freq) 1105 return 0; 1106 1107 xa_for_each(&pin->dpll_refs, i, ref) { 1108 ops = dpll_pin_ops(ref); 1109 dpll = ref->dpll; 1110 ret = ops->frequency_set(pin, dpll_pin_on_dpll_priv(dpll, pin), 1111 dpll, dpll_priv(dpll), freq, extack); 1112 if (ret) { 1113 failed = ref; 1114 NL_SET_ERR_MSG_FMT(extack, "frequency set failed for dpll_id:%u", 1115 dpll->id); 1116 goto rollback; 1117 } 1118 } 1119 __dpll_pin_change_ntf(pin); 1120 1121 return 0; 1122 1123 rollback: 1124 xa_for_each(&pin->dpll_refs, i, ref) { 1125 if (ref == failed) 1126 break; 1127 ops = dpll_pin_ops(ref); 1128 dpll = ref->dpll; 1129 if (ops->frequency_set(pin, dpll_pin_on_dpll_priv(dpll, pin), 1130 dpll, dpll_priv(dpll), old_freq, extack)) 1131 NL_SET_ERR_MSG(extack, "set frequency rollback failed"); 1132 } 1133 return ret; 1134 } 1135 1136 static int 1137 dpll_pin_esync_set(struct dpll_pin *pin, struct nlattr *a, 1138 struct netlink_ext_ack *extack) 1139 { 1140 struct dpll_pin_ref *ref, *failed; 1141 const struct dpll_pin_ops *ops; 1142 struct dpll_pin_esync esync; 1143 u64 freq = nla_get_u64(a); 1144 struct dpll_device *dpll; 1145 bool supported = false; 1146 unsigned long i; 1147 int ret; 1148 1149 xa_for_each(&pin->dpll_refs, i, ref) { 1150 ops = dpll_pin_ops(ref); 1151 if (!ops->esync_set || !ops->esync_get) { 1152 NL_SET_ERR_MSG(extack, 1153 "embedded sync feature is not supported by this device"); 1154 return -EOPNOTSUPP; 1155 } 1156 } 1157 ref = dpll_xa_ref_dpll_first(&pin->dpll_refs); 1158 ops = dpll_pin_ops(ref); 1159 dpll = ref->dpll; 1160 ret = ops->esync_get(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll, 1161 dpll_priv(dpll), &esync, extack); 1162 if (ret) { 1163 NL_SET_ERR_MSG(extack, "unable to get current embedded sync frequency value"); 1164 return ret; 1165 } 1166 if (freq == esync.freq) 1167 return 0; 1168 for (i = 0; i < esync.range_num; i++) 1169 if (freq <= esync.range[i].max && freq >= esync.range[i].min) 1170 supported = true; 1171 if (!supported) { 1172 NL_SET_ERR_MSG_ATTR(extack, a, 1173 "requested embedded sync frequency value is not supported by this device"); 1174 return -EINVAL; 1175 } 1176 1177 xa_for_each(&pin->dpll_refs, i, ref) { 1178 void *pin_dpll_priv; 1179 1180 ops = dpll_pin_ops(ref); 1181 dpll = ref->dpll; 1182 pin_dpll_priv = dpll_pin_on_dpll_priv(dpll, pin); 1183 ret = ops->esync_set(pin, pin_dpll_priv, dpll, dpll_priv(dpll), 1184 freq, extack); 1185 if (ret) { 1186 failed = ref; 1187 NL_SET_ERR_MSG_FMT(extack, 1188 "embedded sync frequency set failed for dpll_id: %u", 1189 dpll->id); 1190 goto rollback; 1191 } 1192 } 1193 __dpll_pin_change_ntf(pin); 1194 1195 return 0; 1196 1197 rollback: 1198 xa_for_each(&pin->dpll_refs, i, ref) { 1199 void *pin_dpll_priv; 1200 1201 if (ref == failed) 1202 break; 1203 ops = dpll_pin_ops(ref); 1204 dpll = ref->dpll; 1205 pin_dpll_priv = dpll_pin_on_dpll_priv(dpll, pin); 1206 if (ops->esync_set(pin, pin_dpll_priv, dpll, dpll_priv(dpll), 1207 esync.freq, extack)) 1208 NL_SET_ERR_MSG(extack, "set embedded sync frequency rollback failed"); 1209 } 1210 return ret; 1211 } 1212 1213 static int 1214 dpll_pin_ref_sync_state_set(struct dpll_pin *pin, 1215 unsigned long ref_sync_pin_idx, 1216 const enum dpll_pin_state state, 1217 struct netlink_ext_ack *extack) 1218 1219 { 1220 struct dpll_pin_ref *ref, *failed; 1221 const struct dpll_pin_ops *ops; 1222 enum dpll_pin_state old_state; 1223 struct dpll_pin *ref_sync_pin; 1224 struct dpll_device *dpll; 1225 unsigned long i; 1226 int ret; 1227 1228 ref_sync_pin = xa_find(&pin->ref_sync_pins, &ref_sync_pin_idx, 1229 ULONG_MAX, XA_PRESENT); 1230 if (!ref_sync_pin) { 1231 NL_SET_ERR_MSG(extack, "reference sync pin not found"); 1232 return -EINVAL; 1233 } 1234 if (!dpll_pin_available(ref_sync_pin)) { 1235 NL_SET_ERR_MSG(extack, "reference sync pin not available"); 1236 return -EINVAL; 1237 } 1238 ref = dpll_xa_ref_dpll_first(&pin->dpll_refs); 1239 ASSERT_NOT_NULL(ref); 1240 ops = dpll_pin_ops(ref); 1241 if (!ops->ref_sync_set || !ops->ref_sync_get) { 1242 NL_SET_ERR_MSG(extack, "reference sync not supported by this pin"); 1243 return -EOPNOTSUPP; 1244 } 1245 dpll = ref->dpll; 1246 ret = ops->ref_sync_get(pin, dpll_pin_on_dpll_priv(dpll, pin), 1247 ref_sync_pin, 1248 dpll_pin_on_dpll_priv(dpll, ref_sync_pin), 1249 &old_state, extack); 1250 if (ret) { 1251 NL_SET_ERR_MSG(extack, "unable to get old reference sync state"); 1252 return ret; 1253 } 1254 if (state == old_state) 1255 return 0; 1256 xa_for_each(&pin->dpll_refs, i, ref) { 1257 ops = dpll_pin_ops(ref); 1258 dpll = ref->dpll; 1259 ret = ops->ref_sync_set(pin, dpll_pin_on_dpll_priv(dpll, pin), 1260 ref_sync_pin, 1261 dpll_pin_on_dpll_priv(dpll, 1262 ref_sync_pin), 1263 state, extack); 1264 if (ret) { 1265 failed = ref; 1266 NL_SET_ERR_MSG_FMT(extack, "reference sync set failed for dpll_id:%u", 1267 dpll->id); 1268 goto rollback; 1269 } 1270 } 1271 __dpll_pin_change_ntf(pin); 1272 1273 return 0; 1274 1275 rollback: 1276 xa_for_each(&pin->dpll_refs, i, ref) { 1277 if (ref == failed) 1278 break; 1279 ops = dpll_pin_ops(ref); 1280 dpll = ref->dpll; 1281 if (ops->ref_sync_set(pin, dpll_pin_on_dpll_priv(dpll, pin), 1282 ref_sync_pin, 1283 dpll_pin_on_dpll_priv(dpll, ref_sync_pin), 1284 old_state, extack)) 1285 NL_SET_ERR_MSG(extack, "set reference sync rollback failed"); 1286 } 1287 return ret; 1288 } 1289 1290 static int 1291 dpll_pin_ref_sync_set(struct dpll_pin *pin, struct nlattr *nest, 1292 struct netlink_ext_ack *extack) 1293 { 1294 struct nlattr *tb[DPLL_A_PIN_MAX + 1]; 1295 enum dpll_pin_state state; 1296 u32 sync_pin_id; 1297 1298 nla_parse_nested(tb, DPLL_A_PIN_MAX, nest, 1299 dpll_reference_sync_nl_policy, extack); 1300 if (!tb[DPLL_A_PIN_ID]) { 1301 NL_SET_ERR_MSG(extack, "sync pin id expected"); 1302 return -EINVAL; 1303 } 1304 sync_pin_id = nla_get_u32(tb[DPLL_A_PIN_ID]); 1305 1306 if (!tb[DPLL_A_PIN_STATE]) { 1307 NL_SET_ERR_MSG(extack, "sync pin state expected"); 1308 return -EINVAL; 1309 } 1310 state = nla_get_u32(tb[DPLL_A_PIN_STATE]); 1311 1312 return dpll_pin_ref_sync_state_set(pin, sync_pin_id, state, extack); 1313 } 1314 1315 static int 1316 dpll_pin_on_pin_state_set(struct dpll_pin *pin, u32 parent_idx, 1317 enum dpll_pin_state state, 1318 struct netlink_ext_ack *extack) 1319 { 1320 struct dpll_pin_ref *parent_ref; 1321 const struct dpll_pin_ops *ops; 1322 struct dpll_pin_ref *dpll_ref; 1323 void *pin_priv, *parent_priv; 1324 struct dpll_pin *parent; 1325 unsigned long i; 1326 int ret; 1327 1328 /* fwnode pins may not set the capability bit upfront; let the ops 1329 * layer return -EOPNOTSUPP if the operation is unsupported. 1330 */ 1331 if (!(DPLL_PIN_CAPABILITIES_STATE_CAN_CHANGE & 1332 pin->prop.capabilities) && !pin->fwnode) { 1333 NL_SET_ERR_MSG(extack, "state changing is not allowed"); 1334 return -EOPNOTSUPP; 1335 } 1336 parent = xa_load(&dpll_pin_xa, parent_idx); 1337 if (!parent) 1338 return -EINVAL; 1339 parent_ref = xa_load(&pin->parent_refs, parent->pin_idx); 1340 if (!parent_ref) 1341 return -EINVAL; 1342 xa_for_each(&parent->dpll_refs, i, dpll_ref) { 1343 ops = dpll_pin_ops(parent_ref); 1344 if (!ops->state_on_pin_set) 1345 return -EOPNOTSUPP; 1346 pin_priv = dpll_pin_on_pin_priv(parent, pin); 1347 parent_priv = dpll_pin_on_dpll_priv(dpll_ref->dpll, parent); 1348 ret = ops->state_on_pin_set(pin, pin_priv, parent, parent_priv, 1349 state, extack); 1350 if (ret) 1351 return ret; 1352 } 1353 __dpll_pin_change_ntf(pin); 1354 1355 return 0; 1356 } 1357 1358 static int 1359 dpll_pin_state_set(struct dpll_device *dpll, struct dpll_pin *pin, 1360 enum dpll_pin_state state, 1361 struct netlink_ext_ack *extack) 1362 { 1363 const struct dpll_pin_ops *ops; 1364 struct dpll_pin_ref *ref; 1365 int ret; 1366 1367 /* fwnode pins may not set the capability bit upfront; let the ops 1368 * layer return -EOPNOTSUPP if the operation is unsupported. 1369 */ 1370 if (!(DPLL_PIN_CAPABILITIES_STATE_CAN_CHANGE & 1371 pin->prop.capabilities) && !pin->fwnode) { 1372 NL_SET_ERR_MSG(extack, "state changing is not allowed"); 1373 return -EOPNOTSUPP; 1374 } 1375 ref = xa_load(&pin->dpll_refs, dpll->id); 1376 ASSERT_NOT_NULL(ref); 1377 ops = dpll_pin_ops(ref); 1378 if (!ops->state_on_dpll_set) 1379 return -EOPNOTSUPP; 1380 ret = ops->state_on_dpll_set(pin, dpll_pin_on_dpll_priv(dpll, pin), 1381 dpll, dpll_priv(dpll), state, extack); 1382 if (ret) 1383 return ret; 1384 __dpll_pin_change_ntf(pin); 1385 1386 return 0; 1387 } 1388 1389 static int 1390 dpll_pin_prio_set(struct dpll_device *dpll, struct dpll_pin *pin, 1391 u32 prio, struct netlink_ext_ack *extack) 1392 { 1393 const struct dpll_pin_ops *ops; 1394 struct dpll_pin_ref *ref; 1395 int ret; 1396 1397 if (!(DPLL_PIN_CAPABILITIES_PRIORITY_CAN_CHANGE & 1398 pin->prop.capabilities)) { 1399 NL_SET_ERR_MSG(extack, "prio changing is not allowed"); 1400 return -EOPNOTSUPP; 1401 } 1402 ref = xa_load(&pin->dpll_refs, dpll->id); 1403 ASSERT_NOT_NULL(ref); 1404 ops = dpll_pin_ops(ref); 1405 if (!ops->prio_set) 1406 return -EOPNOTSUPP; 1407 ret = ops->prio_set(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll, 1408 dpll_priv(dpll), prio, extack); 1409 if (ret) 1410 return ret; 1411 __dpll_pin_change_ntf(pin); 1412 1413 return 0; 1414 } 1415 1416 static int 1417 dpll_pin_direction_set(struct dpll_pin *pin, struct dpll_device *dpll, 1418 enum dpll_pin_direction direction, 1419 struct netlink_ext_ack *extack) 1420 { 1421 const struct dpll_pin_ops *ops; 1422 struct dpll_pin_ref *ref; 1423 int ret; 1424 1425 if (!(DPLL_PIN_CAPABILITIES_DIRECTION_CAN_CHANGE & 1426 pin->prop.capabilities)) { 1427 NL_SET_ERR_MSG(extack, "direction changing is not allowed"); 1428 return -EOPNOTSUPP; 1429 } 1430 ref = xa_load(&pin->dpll_refs, dpll->id); 1431 ASSERT_NOT_NULL(ref); 1432 ops = dpll_pin_ops(ref); 1433 if (!ops->direction_set) 1434 return -EOPNOTSUPP; 1435 ret = ops->direction_set(pin, dpll_pin_on_dpll_priv(dpll, pin), 1436 dpll, dpll_priv(dpll), direction, extack); 1437 if (ret) 1438 return ret; 1439 __dpll_pin_change_ntf(pin); 1440 1441 return 0; 1442 } 1443 1444 static int 1445 dpll_pin_phase_adj_set(struct dpll_pin *pin, struct nlattr *phase_adj_attr, 1446 struct netlink_ext_ack *extack) 1447 { 1448 struct dpll_pin_ref *ref, *failed; 1449 const struct dpll_pin_ops *ops; 1450 s32 phase_adj, old_phase_adj; 1451 struct dpll_device *dpll; 1452 unsigned long i; 1453 int ret; 1454 1455 phase_adj = nla_get_s32(phase_adj_attr); 1456 if (phase_adj > pin->prop.phase_range.max || 1457 phase_adj < pin->prop.phase_range.min) { 1458 NL_SET_ERR_MSG_ATTR(extack, phase_adj_attr, 1459 "phase adjust value of out range"); 1460 return -EINVAL; 1461 } 1462 if (pin->prop.phase_gran && phase_adj % (s32)pin->prop.phase_gran) { 1463 NL_SET_ERR_MSG_ATTR_FMT(extack, phase_adj_attr, 1464 "phase adjust value not multiple of %u", 1465 pin->prop.phase_gran); 1466 return -EINVAL; 1467 } 1468 1469 xa_for_each(&pin->dpll_refs, i, ref) { 1470 ops = dpll_pin_ops(ref); 1471 if (!ops->phase_adjust_set || !ops->phase_adjust_get) { 1472 NL_SET_ERR_MSG(extack, "phase adjust not supported"); 1473 return -EOPNOTSUPP; 1474 } 1475 } 1476 ref = dpll_xa_ref_dpll_first(&pin->dpll_refs); 1477 ops = dpll_pin_ops(ref); 1478 dpll = ref->dpll; 1479 ret = ops->phase_adjust_get(pin, dpll_pin_on_dpll_priv(dpll, pin), 1480 dpll, dpll_priv(dpll), &old_phase_adj, 1481 extack); 1482 if (ret) { 1483 NL_SET_ERR_MSG(extack, "unable to get old phase adjust value"); 1484 return ret; 1485 } 1486 if (phase_adj == old_phase_adj) 1487 return 0; 1488 1489 xa_for_each(&pin->dpll_refs, i, ref) { 1490 ops = dpll_pin_ops(ref); 1491 dpll = ref->dpll; 1492 ret = ops->phase_adjust_set(pin, 1493 dpll_pin_on_dpll_priv(dpll, pin), 1494 dpll, dpll_priv(dpll), phase_adj, 1495 extack); 1496 if (ret) { 1497 failed = ref; 1498 NL_SET_ERR_MSG_FMT(extack, 1499 "phase adjust set failed for dpll_id:%u", 1500 dpll->id); 1501 goto rollback; 1502 } 1503 } 1504 __dpll_pin_change_ntf(pin); 1505 1506 return 0; 1507 1508 rollback: 1509 xa_for_each(&pin->dpll_refs, i, ref) { 1510 if (ref == failed) 1511 break; 1512 ops = dpll_pin_ops(ref); 1513 dpll = ref->dpll; 1514 if (ops->phase_adjust_set(pin, dpll_pin_on_dpll_priv(dpll, pin), 1515 dpll, dpll_priv(dpll), old_phase_adj, 1516 extack)) 1517 NL_SET_ERR_MSG(extack, "set phase adjust rollback failed"); 1518 } 1519 return ret; 1520 } 1521 1522 static int 1523 dpll_pin_parent_device_set(struct dpll_pin *pin, struct nlattr *parent_nest, 1524 struct netlink_ext_ack *extack) 1525 { 1526 struct nlattr *tb[DPLL_A_PIN_MAX + 1]; 1527 enum dpll_pin_direction direction; 1528 enum dpll_pin_state state; 1529 struct dpll_pin_ref *ref; 1530 struct dpll_device *dpll; 1531 u32 pdpll_idx, prio; 1532 int ret; 1533 1534 nla_parse_nested(tb, DPLL_A_PIN_MAX, parent_nest, 1535 dpll_pin_parent_device_nl_policy, extack); 1536 if (!tb[DPLL_A_PIN_PARENT_ID]) { 1537 NL_SET_ERR_MSG(extack, "device parent id expected"); 1538 return -EINVAL; 1539 } 1540 pdpll_idx = nla_get_u32(tb[DPLL_A_PIN_PARENT_ID]); 1541 dpll = xa_load(&dpll_device_xa, pdpll_idx); 1542 if (!dpll) { 1543 NL_SET_ERR_MSG(extack, "parent device not found"); 1544 return -EINVAL; 1545 } 1546 ref = xa_load(&pin->dpll_refs, dpll->id); 1547 if (!ref) { 1548 NL_SET_ERR_MSG(extack, "pin not connected to given parent device"); 1549 return -EINVAL; 1550 } 1551 if (tb[DPLL_A_PIN_STATE]) { 1552 state = nla_get_u32(tb[DPLL_A_PIN_STATE]); 1553 ret = dpll_pin_state_set(dpll, pin, state, extack); 1554 if (ret) 1555 return ret; 1556 } 1557 if (tb[DPLL_A_PIN_PRIO]) { 1558 prio = nla_get_u32(tb[DPLL_A_PIN_PRIO]); 1559 ret = dpll_pin_prio_set(dpll, pin, prio, extack); 1560 if (ret) 1561 return ret; 1562 } 1563 if (tb[DPLL_A_PIN_DIRECTION]) { 1564 direction = nla_get_u32(tb[DPLL_A_PIN_DIRECTION]); 1565 ret = dpll_pin_direction_set(pin, dpll, direction, extack); 1566 if (ret) 1567 return ret; 1568 } 1569 return 0; 1570 } 1571 1572 static int 1573 dpll_pin_parent_pin_set(struct dpll_pin *pin, struct nlattr *parent_nest, 1574 struct netlink_ext_ack *extack) 1575 { 1576 struct nlattr *tb[DPLL_A_PIN_MAX + 1]; 1577 u32 ppin_idx; 1578 int ret; 1579 1580 nla_parse_nested(tb, DPLL_A_PIN_MAX, parent_nest, 1581 dpll_pin_parent_pin_nl_policy, extack); 1582 if (!tb[DPLL_A_PIN_PARENT_ID]) { 1583 NL_SET_ERR_MSG(extack, "device parent id expected"); 1584 return -EINVAL; 1585 } 1586 ppin_idx = nla_get_u32(tb[DPLL_A_PIN_PARENT_ID]); 1587 1588 if (tb[DPLL_A_PIN_STATE]) { 1589 enum dpll_pin_state state = nla_get_u32(tb[DPLL_A_PIN_STATE]); 1590 1591 ret = dpll_pin_on_pin_state_set(pin, ppin_idx, state, extack); 1592 if (ret) 1593 return ret; 1594 } 1595 1596 return 0; 1597 } 1598 1599 static int 1600 dpll_pin_set_from_nlattr(struct dpll_pin *pin, struct genl_info *info) 1601 { 1602 struct nlattr *a; 1603 int rem, ret; 1604 1605 nla_for_each_attr(a, genlmsg_data(info->genlhdr), 1606 genlmsg_len(info->genlhdr), rem) { 1607 switch (nla_type(a)) { 1608 case DPLL_A_PIN_FREQUENCY: 1609 ret = dpll_pin_freq_set(pin, a, info->extack); 1610 if (ret) 1611 return ret; 1612 break; 1613 case DPLL_A_PIN_PHASE_ADJUST: 1614 ret = dpll_pin_phase_adj_set(pin, a, info->extack); 1615 if (ret) 1616 return ret; 1617 break; 1618 case DPLL_A_PIN_PARENT_DEVICE: 1619 ret = dpll_pin_parent_device_set(pin, a, info->extack); 1620 if (ret) 1621 return ret; 1622 break; 1623 case DPLL_A_PIN_PARENT_PIN: 1624 ret = dpll_pin_parent_pin_set(pin, a, info->extack); 1625 if (ret) 1626 return ret; 1627 break; 1628 case DPLL_A_PIN_ESYNC_FREQUENCY: 1629 ret = dpll_pin_esync_set(pin, a, info->extack); 1630 if (ret) 1631 return ret; 1632 break; 1633 case DPLL_A_PIN_REFERENCE_SYNC: 1634 ret = dpll_pin_ref_sync_set(pin, a, info->extack); 1635 if (ret) 1636 return ret; 1637 break; 1638 } 1639 } 1640 1641 return 0; 1642 } 1643 1644 static struct dpll_pin * 1645 dpll_pin_find(u64 clock_id, struct nlattr *mod_name_attr, 1646 enum dpll_pin_type type, struct nlattr *board_label, 1647 struct nlattr *panel_label, struct nlattr *package_label, 1648 struct netlink_ext_ack *extack) 1649 { 1650 bool board_match, panel_match, package_match; 1651 struct dpll_pin *pin_match = NULL, *pin; 1652 const struct dpll_pin_properties *prop; 1653 bool cid_match, mod_match, type_match; 1654 unsigned long i; 1655 1656 xa_for_each_marked(&dpll_pin_xa, i, pin, DPLL_REGISTERED) { 1657 prop = &pin->prop; 1658 cid_match = clock_id ? pin->clock_id == clock_id : true; 1659 mod_match = mod_name_attr && pin->module_name[0] ? 1660 !nla_strcmp(mod_name_attr, 1661 pin->module_name) : true; 1662 type_match = type ? prop->type == type : true; 1663 board_match = board_label ? (prop->board_label ? 1664 !nla_strcmp(board_label, prop->board_label) : false) : 1665 true; 1666 panel_match = panel_label ? (prop->panel_label ? 1667 !nla_strcmp(panel_label, prop->panel_label) : false) : 1668 true; 1669 package_match = package_label ? (prop->package_label ? 1670 !nla_strcmp(package_label, prop->package_label) : 1671 false) : true; 1672 if (cid_match && mod_match && type_match && board_match && 1673 panel_match && package_match) { 1674 if (pin_match) { 1675 NL_SET_ERR_MSG(extack, "multiple matches"); 1676 return ERR_PTR(-EINVAL); 1677 } 1678 pin_match = pin; 1679 } 1680 } 1681 if (!pin_match) { 1682 NL_SET_ERR_MSG(extack, "not found"); 1683 return ERR_PTR(-ENODEV); 1684 } 1685 return pin_match; 1686 } 1687 1688 static struct dpll_pin *dpll_pin_find_from_nlattr(struct genl_info *info) 1689 { 1690 struct nlattr *attr, *mod_name_attr = NULL, *board_label_attr = NULL, 1691 *panel_label_attr = NULL, *package_label_attr = NULL; 1692 enum dpll_pin_type type = 0; 1693 u64 clock_id = 0; 1694 int rem = 0; 1695 1696 nla_for_each_attr(attr, genlmsg_data(info->genlhdr), 1697 genlmsg_len(info->genlhdr), rem) { 1698 switch (nla_type(attr)) { 1699 case DPLL_A_PIN_CLOCK_ID: 1700 if (clock_id) 1701 goto duplicated_attr; 1702 clock_id = nla_get_u64(attr); 1703 break; 1704 case DPLL_A_PIN_MODULE_NAME: 1705 if (mod_name_attr) 1706 goto duplicated_attr; 1707 mod_name_attr = attr; 1708 break; 1709 case DPLL_A_PIN_TYPE: 1710 if (type) 1711 goto duplicated_attr; 1712 type = nla_get_u32(attr); 1713 break; 1714 case DPLL_A_PIN_BOARD_LABEL: 1715 if (board_label_attr) 1716 goto duplicated_attr; 1717 board_label_attr = attr; 1718 break; 1719 case DPLL_A_PIN_PANEL_LABEL: 1720 if (panel_label_attr) 1721 goto duplicated_attr; 1722 panel_label_attr = attr; 1723 break; 1724 case DPLL_A_PIN_PACKAGE_LABEL: 1725 if (package_label_attr) 1726 goto duplicated_attr; 1727 package_label_attr = attr; 1728 break; 1729 default: 1730 break; 1731 } 1732 } 1733 if (!(clock_id || mod_name_attr || board_label_attr || 1734 panel_label_attr || package_label_attr)) { 1735 NL_SET_ERR_MSG(info->extack, "missing attributes"); 1736 return ERR_PTR(-EINVAL); 1737 } 1738 return dpll_pin_find(clock_id, mod_name_attr, type, board_label_attr, 1739 panel_label_attr, package_label_attr, 1740 info->extack); 1741 duplicated_attr: 1742 NL_SET_ERR_MSG(info->extack, "duplicated attribute"); 1743 return ERR_PTR(-EINVAL); 1744 } 1745 1746 int dpll_nl_pin_id_get_doit(struct sk_buff *skb, struct genl_info *info) 1747 { 1748 struct dpll_pin *pin; 1749 struct sk_buff *msg; 1750 struct nlattr *hdr; 1751 int ret; 1752 1753 msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 1754 if (!msg) 1755 return -ENOMEM; 1756 hdr = genlmsg_put_reply(msg, info, &dpll_nl_family, 0, 1757 DPLL_CMD_PIN_ID_GET); 1758 if (!hdr) { 1759 nlmsg_free(msg); 1760 return -EMSGSIZE; 1761 } 1762 pin = dpll_pin_find_from_nlattr(info); 1763 if (IS_ERR(pin)) { 1764 nlmsg_free(msg); 1765 return PTR_ERR(pin); 1766 } 1767 if (!dpll_pin_available(pin)) { 1768 nlmsg_free(msg); 1769 return -ENODEV; 1770 } 1771 ret = dpll_msg_add_pin_handle(msg, pin); 1772 if (ret) { 1773 nlmsg_free(msg); 1774 return ret; 1775 } 1776 genlmsg_end(msg, hdr); 1777 1778 return genlmsg_reply(msg, info); 1779 } 1780 1781 int dpll_nl_pin_get_doit(struct sk_buff *skb, struct genl_info *info) 1782 { 1783 struct dpll_pin *pin = info->user_ptr[0]; 1784 struct sk_buff *msg; 1785 struct nlattr *hdr; 1786 int ret; 1787 1788 if (!pin) 1789 return -ENODEV; 1790 msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 1791 if (!msg) 1792 return -ENOMEM; 1793 hdr = genlmsg_put_reply(msg, info, &dpll_nl_family, 0, 1794 DPLL_CMD_PIN_GET); 1795 if (!hdr) { 1796 nlmsg_free(msg); 1797 return -EMSGSIZE; 1798 } 1799 ret = dpll_cmd_pin_get_one(msg, pin, info->extack); 1800 if (ret) { 1801 nlmsg_free(msg); 1802 return ret; 1803 } 1804 genlmsg_end(msg, hdr); 1805 1806 return genlmsg_reply(msg, info); 1807 } 1808 1809 int dpll_nl_pin_get_dumpit(struct sk_buff *skb, struct netlink_callback *cb) 1810 { 1811 struct dpll_dump_ctx *ctx = dpll_dump_context(cb); 1812 struct dpll_pin *pin; 1813 struct nlattr *hdr; 1814 unsigned long i; 1815 int ret = 0; 1816 1817 mutex_lock(&dpll_lock); 1818 xa_for_each_marked_start(&dpll_pin_xa, i, pin, DPLL_REGISTERED, 1819 ctx->idx) { 1820 if (!dpll_pin_available(pin)) 1821 continue; 1822 hdr = genlmsg_put(skb, NETLINK_CB(cb->skb).portid, 1823 cb->nlh->nlmsg_seq, 1824 &dpll_nl_family, NLM_F_MULTI, 1825 DPLL_CMD_PIN_GET); 1826 if (!hdr) { 1827 ret = -EMSGSIZE; 1828 break; 1829 } 1830 ret = dpll_cmd_pin_get_one(skb, pin, cb->extack); 1831 if (ret) { 1832 genlmsg_cancel(skb, hdr); 1833 break; 1834 } 1835 genlmsg_end(skb, hdr); 1836 } 1837 mutex_unlock(&dpll_lock); 1838 1839 if (ret == -EMSGSIZE) { 1840 ctx->idx = i; 1841 return skb->len; 1842 } 1843 return ret; 1844 } 1845 1846 int dpll_nl_pin_set_doit(struct sk_buff *skb, struct genl_info *info) 1847 { 1848 struct dpll_pin *pin = info->user_ptr[0]; 1849 1850 return dpll_pin_set_from_nlattr(pin, info); 1851 } 1852 1853 static struct dpll_device * 1854 dpll_device_find(u64 clock_id, struct nlattr *mod_name_attr, 1855 enum dpll_type type, struct netlink_ext_ack *extack) 1856 { 1857 struct dpll_device *dpll_match = NULL, *dpll; 1858 bool cid_match, mod_match, type_match; 1859 unsigned long i; 1860 1861 xa_for_each_marked(&dpll_device_xa, i, dpll, DPLL_REGISTERED) { 1862 cid_match = clock_id ? dpll->clock_id == clock_id : true; 1863 mod_match = mod_name_attr ? (module_name(dpll->module) ? 1864 !nla_strcmp(mod_name_attr, 1865 module_name(dpll->module)) : false) : true; 1866 type_match = type ? dpll->type == type : true; 1867 if (cid_match && mod_match && type_match) { 1868 if (dpll_match) { 1869 NL_SET_ERR_MSG(extack, "multiple matches"); 1870 return ERR_PTR(-EINVAL); 1871 } 1872 dpll_match = dpll; 1873 } 1874 } 1875 if (!dpll_match) { 1876 NL_SET_ERR_MSG(extack, "not found"); 1877 return ERR_PTR(-ENODEV); 1878 } 1879 1880 return dpll_match; 1881 } 1882 1883 static struct dpll_device * 1884 dpll_device_find_from_nlattr(struct genl_info *info) 1885 { 1886 struct nlattr *attr, *mod_name_attr = NULL; 1887 enum dpll_type type = 0; 1888 u64 clock_id = 0; 1889 int rem = 0; 1890 1891 nla_for_each_attr(attr, genlmsg_data(info->genlhdr), 1892 genlmsg_len(info->genlhdr), rem) { 1893 switch (nla_type(attr)) { 1894 case DPLL_A_CLOCK_ID: 1895 if (clock_id) 1896 goto duplicated_attr; 1897 clock_id = nla_get_u64(attr); 1898 break; 1899 case DPLL_A_MODULE_NAME: 1900 if (mod_name_attr) 1901 goto duplicated_attr; 1902 mod_name_attr = attr; 1903 break; 1904 case DPLL_A_TYPE: 1905 if (type) 1906 goto duplicated_attr; 1907 type = nla_get_u32(attr); 1908 break; 1909 default: 1910 break; 1911 } 1912 } 1913 if (!clock_id && !mod_name_attr && !type) { 1914 NL_SET_ERR_MSG(info->extack, "missing attributes"); 1915 return ERR_PTR(-EINVAL); 1916 } 1917 return dpll_device_find(clock_id, mod_name_attr, type, info->extack); 1918 duplicated_attr: 1919 NL_SET_ERR_MSG(info->extack, "duplicated attribute"); 1920 return ERR_PTR(-EINVAL); 1921 } 1922 1923 int dpll_nl_device_id_get_doit(struct sk_buff *skb, struct genl_info *info) 1924 { 1925 struct dpll_device *dpll; 1926 struct sk_buff *msg; 1927 struct nlattr *hdr; 1928 int ret; 1929 1930 msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 1931 if (!msg) 1932 return -ENOMEM; 1933 hdr = genlmsg_put_reply(msg, info, &dpll_nl_family, 0, 1934 DPLL_CMD_DEVICE_ID_GET); 1935 if (!hdr) { 1936 nlmsg_free(msg); 1937 return -EMSGSIZE; 1938 } 1939 1940 dpll = dpll_device_find_from_nlattr(info); 1941 if (IS_ERR(dpll)) { 1942 nlmsg_free(msg); 1943 return PTR_ERR(dpll); 1944 } 1945 ret = dpll_msg_add_dev_handle(msg, dpll); 1946 if (ret) { 1947 nlmsg_free(msg); 1948 return ret; 1949 } 1950 genlmsg_end(msg, hdr); 1951 1952 return genlmsg_reply(msg, info); 1953 } 1954 1955 int dpll_nl_device_get_doit(struct sk_buff *skb, struct genl_info *info) 1956 { 1957 struct dpll_device *dpll = info->user_ptr[0]; 1958 struct sk_buff *msg; 1959 struct nlattr *hdr; 1960 int ret; 1961 1962 msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 1963 if (!msg) 1964 return -ENOMEM; 1965 hdr = genlmsg_put_reply(msg, info, &dpll_nl_family, 0, 1966 DPLL_CMD_DEVICE_GET); 1967 if (!hdr) { 1968 nlmsg_free(msg); 1969 return -EMSGSIZE; 1970 } 1971 1972 ret = dpll_device_get_one(dpll, msg, info->extack); 1973 if (ret) { 1974 nlmsg_free(msg); 1975 return ret; 1976 } 1977 genlmsg_end(msg, hdr); 1978 1979 return genlmsg_reply(msg, info); 1980 } 1981 1982 static int 1983 dpll_set_from_nlattr(struct dpll_device *dpll, struct genl_info *info) 1984 { 1985 struct nlattr *a; 1986 int rem, ret; 1987 1988 nla_for_each_attr(a, genlmsg_data(info->genlhdr), 1989 genlmsg_len(info->genlhdr), rem) { 1990 switch (nla_type(a)) { 1991 case DPLL_A_MODE: 1992 ret = dpll_mode_set(dpll, a, info->extack); 1993 if (ret) 1994 return ret; 1995 break; 1996 case DPLL_A_PHASE_OFFSET_MONITOR: 1997 ret = dpll_phase_offset_monitor_set(dpll, a, 1998 info->extack); 1999 if (ret) 2000 return ret; 2001 break; 2002 case DPLL_A_PHASE_OFFSET_AVG_FACTOR: 2003 ret = dpll_phase_offset_avg_factor_set(dpll, a, 2004 info->extack); 2005 if (ret) 2006 return ret; 2007 break; 2008 case DPLL_A_FREQUENCY_MONITOR: 2009 ret = dpll_freq_monitor_set(dpll, a, 2010 info->extack); 2011 if (ret) 2012 return ret; 2013 break; 2014 } 2015 } 2016 2017 return 0; 2018 } 2019 2020 int dpll_nl_device_set_doit(struct sk_buff *skb, struct genl_info *info) 2021 { 2022 struct dpll_device *dpll = info->user_ptr[0]; 2023 2024 return dpll_set_from_nlattr(dpll, info); 2025 } 2026 2027 int dpll_nl_device_get_dumpit(struct sk_buff *skb, struct netlink_callback *cb) 2028 { 2029 struct dpll_dump_ctx *ctx = dpll_dump_context(cb); 2030 struct dpll_device *dpll; 2031 struct nlattr *hdr; 2032 unsigned long i; 2033 int ret = 0; 2034 2035 mutex_lock(&dpll_lock); 2036 xa_for_each_marked_start(&dpll_device_xa, i, dpll, DPLL_REGISTERED, 2037 ctx->idx) { 2038 hdr = genlmsg_put(skb, NETLINK_CB(cb->skb).portid, 2039 cb->nlh->nlmsg_seq, &dpll_nl_family, 2040 NLM_F_MULTI, DPLL_CMD_DEVICE_GET); 2041 if (!hdr) { 2042 ret = -EMSGSIZE; 2043 break; 2044 } 2045 ret = dpll_device_get_one(dpll, skb, cb->extack); 2046 if (ret) { 2047 genlmsg_cancel(skb, hdr); 2048 break; 2049 } 2050 genlmsg_end(skb, hdr); 2051 } 2052 mutex_unlock(&dpll_lock); 2053 2054 if (ret == -EMSGSIZE) { 2055 ctx->idx = i; 2056 return skb->len; 2057 } 2058 return ret; 2059 } 2060 2061 int dpll_pre_doit(const struct genl_split_ops *ops, struct sk_buff *skb, 2062 struct genl_info *info) 2063 { 2064 u32 id; 2065 2066 if (GENL_REQ_ATTR_CHECK(info, DPLL_A_ID)) 2067 return -EINVAL; 2068 2069 mutex_lock(&dpll_lock); 2070 id = nla_get_u32(info->attrs[DPLL_A_ID]); 2071 info->user_ptr[0] = dpll_device_get_by_id(id); 2072 if (!info->user_ptr[0]) { 2073 NL_SET_ERR_MSG(info->extack, "device not found"); 2074 goto unlock; 2075 } 2076 return 0; 2077 unlock: 2078 mutex_unlock(&dpll_lock); 2079 return -ENODEV; 2080 } 2081 2082 void dpll_post_doit(const struct genl_split_ops *ops, struct sk_buff *skb, 2083 struct genl_info *info) 2084 { 2085 mutex_unlock(&dpll_lock); 2086 } 2087 2088 int 2089 dpll_lock_doit(const struct genl_split_ops *ops, struct sk_buff *skb, 2090 struct genl_info *info) 2091 { 2092 mutex_lock(&dpll_lock); 2093 2094 return 0; 2095 } 2096 2097 void 2098 dpll_unlock_doit(const struct genl_split_ops *ops, struct sk_buff *skb, 2099 struct genl_info *info) 2100 { 2101 mutex_unlock(&dpll_lock); 2102 } 2103 2104 int dpll_pin_pre_doit(const struct genl_split_ops *ops, struct sk_buff *skb, 2105 struct genl_info *info) 2106 { 2107 int ret; 2108 2109 mutex_lock(&dpll_lock); 2110 if (GENL_REQ_ATTR_CHECK(info, DPLL_A_PIN_ID)) { 2111 ret = -EINVAL; 2112 goto unlock_dev; 2113 } 2114 info->user_ptr[0] = xa_load(&dpll_pin_xa, 2115 nla_get_u32(info->attrs[DPLL_A_PIN_ID])); 2116 if (!info->user_ptr[0] || 2117 !dpll_pin_available(info->user_ptr[0])) { 2118 NL_SET_ERR_MSG(info->extack, "pin not found"); 2119 ret = -ENODEV; 2120 goto unlock_dev; 2121 } 2122 2123 return 0; 2124 2125 unlock_dev: 2126 mutex_unlock(&dpll_lock); 2127 return ret; 2128 } 2129 2130 void dpll_pin_post_doit(const struct genl_split_ops *ops, struct sk_buff *skb, 2131 struct genl_info *info) 2132 { 2133 mutex_unlock(&dpll_lock); 2134 } 2135