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 /* Pin may have been unregistered from this dpll already */ 571 if (!ref_sync_pin_priv) 572 continue; 573 if (WARN_ON(!ops->ref_sync_get)) 574 return -EOPNOTSUPP; 575 ret = ops->ref_sync_get(pin, pin_priv, ref_sync_pin, 576 ref_sync_pin_priv, &state, extack); 577 if (ret) 578 return ret; 579 nest = nla_nest_start(msg, DPLL_A_PIN_REFERENCE_SYNC); 580 if (!nest) 581 return -EMSGSIZE; 582 if (nla_put_s32(msg, DPLL_A_PIN_ID, ref_sync_pin->id)) 583 goto nest_cancel; 584 if (nla_put_s32(msg, DPLL_A_PIN_STATE, state)) 585 goto nest_cancel; 586 nla_nest_end(msg, nest); 587 } 588 return 0; 589 590 nest_cancel: 591 nla_nest_cancel(msg, nest); 592 return -EMSGSIZE; 593 } 594 595 static bool dpll_pin_is_freq_supported(struct dpll_pin *pin, u32 freq) 596 { 597 int fs; 598 599 for (fs = 0; fs < pin->prop.freq_supported_num; fs++) 600 if (freq >= pin->prop.freq_supported[fs].min && 601 freq <= pin->prop.freq_supported[fs].max) 602 return true; 603 return false; 604 } 605 606 static int 607 dpll_msg_add_pin_parents(struct sk_buff *msg, struct dpll_pin *pin, 608 struct dpll_pin_ref *dpll_ref, 609 struct netlink_ext_ack *extack) 610 { 611 enum dpll_pin_state state; 612 struct dpll_pin_ref *ref; 613 struct dpll_pin *ppin; 614 struct nlattr *nest; 615 unsigned long index; 616 int ret; 617 618 xa_for_each(&pin->parent_refs, index, ref) { 619 const struct dpll_pin_ops *ops = dpll_pin_ops(ref); 620 void *parent_priv; 621 622 ppin = ref->pin; 623 parent_priv = dpll_pin_on_dpll_priv(dpll_ref->dpll, ppin); 624 ret = ops->state_on_pin_get(pin, 625 dpll_pin_on_pin_priv(ppin, pin), 626 ppin, parent_priv, &state, extack); 627 if (ret) 628 return ret; 629 nest = nla_nest_start(msg, DPLL_A_PIN_PARENT_PIN); 630 if (!nest) 631 return -EMSGSIZE; 632 ret = dpll_msg_add_dev_parent_handle(msg, ppin->id); 633 if (ret) 634 goto nest_cancel; 635 if (nla_put_u32(msg, DPLL_A_PIN_STATE, state)) { 636 ret = -EMSGSIZE; 637 goto nest_cancel; 638 } 639 nla_nest_end(msg, nest); 640 } 641 642 return 0; 643 644 nest_cancel: 645 nla_nest_cancel(msg, nest); 646 return ret; 647 } 648 649 static int 650 dpll_msg_add_pin_dplls(struct sk_buff *msg, struct dpll_pin *pin, 651 struct netlink_ext_ack *extack) 652 { 653 struct dpll_pin_ref *ref; 654 struct nlattr *attr; 655 unsigned long index; 656 int ret; 657 658 xa_for_each(&pin->dpll_refs, index, ref) { 659 attr = nla_nest_start(msg, DPLL_A_PIN_PARENT_DEVICE); 660 if (!attr) 661 return -EMSGSIZE; 662 ret = dpll_msg_add_dev_parent_handle(msg, ref->dpll->id); 663 if (ret) 664 goto nest_cancel; 665 ret = dpll_msg_add_pin_on_dpll_state(msg, pin, ref, extack); 666 if (ret) 667 goto nest_cancel; 668 ret = dpll_msg_add_pin_operstate(msg, pin, ref, extack); 669 if (ret) 670 goto nest_cancel; 671 ret = dpll_msg_add_pin_prio(msg, pin, ref, extack); 672 if (ret) 673 goto nest_cancel; 674 ret = dpll_msg_add_pin_direction(msg, pin, ref, extack); 675 if (ret) 676 goto nest_cancel; 677 ret = dpll_msg_add_phase_offset(msg, pin, ref, extack); 678 if (ret) 679 goto nest_cancel; 680 ret = dpll_msg_add_ffo(msg, pin, ref, 681 DPLL_FFO_PIN_DEVICE, extack); 682 if (ret) 683 goto nest_cancel; 684 nla_nest_end(msg, attr); 685 } 686 687 return 0; 688 689 nest_cancel: 690 nla_nest_end(msg, attr); 691 return ret; 692 } 693 694 static int 695 dpll_cmd_pin_get_one(struct sk_buff *msg, struct dpll_pin *pin, 696 struct netlink_ext_ack *extack) 697 { 698 const struct dpll_pin_properties *prop = &pin->prop; 699 struct dpll_pin_ref *ref; 700 int ret; 701 702 ref = dpll_xa_ref_dpll_first(&pin->dpll_refs); 703 ASSERT_NOT_NULL(ref); 704 705 ret = dpll_msg_add_pin_handle(msg, pin); 706 if (ret) 707 return ret; 708 if (nla_put_string(msg, DPLL_A_PIN_MODULE_NAME, 709 pin->module_name)) 710 return -EMSGSIZE; 711 if (nla_put_64bit(msg, DPLL_A_PIN_CLOCK_ID, sizeof(pin->clock_id), 712 &pin->clock_id, DPLL_A_PIN_PAD)) 713 return -EMSGSIZE; 714 if (prop->board_label && 715 nla_put_string(msg, DPLL_A_PIN_BOARD_LABEL, prop->board_label)) 716 return -EMSGSIZE; 717 if (prop->panel_label && 718 nla_put_string(msg, DPLL_A_PIN_PANEL_LABEL, prop->panel_label)) 719 return -EMSGSIZE; 720 if (prop->package_label && 721 nla_put_string(msg, DPLL_A_PIN_PACKAGE_LABEL, 722 prop->package_label)) 723 return -EMSGSIZE; 724 if (nla_put_u32(msg, DPLL_A_PIN_TYPE, prop->type)) 725 return -EMSGSIZE; 726 if (nla_put_u32(msg, DPLL_A_PIN_CAPABILITIES, prop->capabilities)) 727 return -EMSGSIZE; 728 ret = dpll_msg_add_pin_freq(msg, pin, ref, extack); 729 if (ret) 730 return ret; 731 if (prop->phase_gran && 732 nla_put_u32(msg, DPLL_A_PIN_PHASE_ADJUST_GRAN, 733 prop->phase_gran)) 734 return -EMSGSIZE; 735 if (nla_put_s32(msg, DPLL_A_PIN_PHASE_ADJUST_MIN, 736 prop->phase_range.min)) 737 return -EMSGSIZE; 738 if (nla_put_s32(msg, DPLL_A_PIN_PHASE_ADJUST_MAX, 739 prop->phase_range.max)) 740 return -EMSGSIZE; 741 ret = dpll_msg_add_pin_phase_adjust(msg, pin, ref, extack); 742 if (ret) 743 return ret; 744 ret = dpll_msg_add_ffo(msg, pin, ref, 745 DPLL_FFO_PORT_RXTX_RATE, extack); 746 if (ret) 747 return ret; 748 ret = dpll_msg_add_measured_freq(msg, pin, ref, extack); 749 if (ret) 750 return ret; 751 ret = dpll_msg_add_pin_esync(msg, pin, ref, extack); 752 if (ret) 753 return ret; 754 if (!xa_empty(&pin->ref_sync_pins)) 755 ret = dpll_msg_add_pin_ref_sync(msg, pin, ref, extack); 756 if (ret) 757 return ret; 758 if (xa_empty(&pin->parent_refs)) 759 ret = dpll_msg_add_pin_dplls(msg, pin, extack); 760 else 761 ret = dpll_msg_add_pin_parents(msg, pin, ref, extack); 762 763 return ret; 764 } 765 766 static int 767 dpll_device_get_one(struct dpll_device *dpll, struct sk_buff *msg, 768 struct netlink_ext_ack *extack) 769 { 770 int ret; 771 772 ret = dpll_msg_add_dev_handle(msg, dpll); 773 if (ret) 774 return ret; 775 if (nla_put_string(msg, DPLL_A_MODULE_NAME, module_name(dpll->module))) 776 return -EMSGSIZE; 777 if (nla_put_64bit(msg, DPLL_A_CLOCK_ID, sizeof(dpll->clock_id), 778 &dpll->clock_id, DPLL_A_PAD)) 779 return -EMSGSIZE; 780 ret = dpll_msg_add_temp(msg, dpll, extack); 781 if (ret) 782 return ret; 783 ret = dpll_msg_add_lock_status(msg, dpll, extack); 784 if (ret) 785 return ret; 786 ret = dpll_msg_add_clock_quality_level(msg, dpll, extack); 787 if (ret) 788 return ret; 789 ret = dpll_msg_add_mode(msg, dpll, extack); 790 if (ret) 791 return ret; 792 ret = dpll_msg_add_mode_supported(msg, dpll, extack); 793 if (ret) 794 return ret; 795 if (nla_put_u32(msg, DPLL_A_TYPE, dpll->type)) 796 return -EMSGSIZE; 797 ret = dpll_msg_add_phase_offset_monitor(msg, dpll, extack); 798 if (ret) 799 return ret; 800 ret = dpll_msg_add_phase_offset_avg_factor(msg, dpll, extack); 801 if (ret) 802 return ret; 803 ret = dpll_msg_add_freq_monitor(msg, dpll, extack); 804 if (ret) 805 return ret; 806 807 return 0; 808 } 809 810 static int 811 dpll_device_event_send(enum dpll_cmd event, struct dpll_device *dpll) 812 { 813 struct sk_buff *msg; 814 int ret = -ENOMEM; 815 void *hdr; 816 817 if (WARN_ON(!xa_get_mark(&dpll_device_xa, dpll->id, DPLL_REGISTERED))) 818 return -ENODEV; 819 msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 820 if (!msg) 821 return -ENOMEM; 822 hdr = genlmsg_put(msg, 0, 0, &dpll_nl_family, 0, event); 823 if (!hdr) 824 goto err_free_msg; 825 ret = dpll_device_get_one(dpll, msg, NULL); 826 if (ret) 827 goto err_cancel_msg; 828 genlmsg_end(msg, hdr); 829 genlmsg_multicast(&dpll_nl_family, msg, 0, 0, GFP_KERNEL); 830 831 return 0; 832 833 err_cancel_msg: 834 genlmsg_cancel(msg, hdr); 835 err_free_msg: 836 nlmsg_free(msg); 837 838 return ret; 839 } 840 841 int dpll_device_create_ntf(struct dpll_device *dpll) 842 { 843 dpll_device_notify(dpll, DPLL_DEVICE_CREATED); 844 return dpll_device_event_send(DPLL_CMD_DEVICE_CREATE_NTF, dpll); 845 } 846 847 int dpll_device_delete_ntf(struct dpll_device *dpll) 848 { 849 dpll_device_notify(dpll, DPLL_DEVICE_DELETED); 850 return dpll_device_event_send(DPLL_CMD_DEVICE_DELETE_NTF, dpll); 851 } 852 853 /** 854 * __dpll_device_change_ntf - notify that the dpll device has been changed 855 * @dpll: registered dpll pointer 856 * 857 * Context: caller must hold dpll_lock. Suitable for use inside device 858 * callbacks which are already invoked under dpll_lock. 859 * Return: 0 if succeeds, error code otherwise. 860 */ 861 int __dpll_device_change_ntf(struct dpll_device *dpll) 862 { 863 lockdep_assert_held(&dpll_lock); 864 dpll_device_notify(dpll, DPLL_DEVICE_CHANGED); 865 return dpll_device_event_send(DPLL_CMD_DEVICE_CHANGE_NTF, dpll); 866 } 867 EXPORT_SYMBOL_GPL(__dpll_device_change_ntf); 868 869 /** 870 * dpll_device_change_ntf - notify that the dpll device has been changed 871 * @dpll: registered dpll pointer 872 * 873 * Context: acquires and holds a dpll_lock. 874 * Return: 0 if succeeds, error code otherwise. 875 */ 876 int dpll_device_change_ntf(struct dpll_device *dpll) 877 { 878 int ret; 879 880 mutex_lock(&dpll_lock); 881 ret = __dpll_device_change_ntf(dpll); 882 mutex_unlock(&dpll_lock); 883 884 return ret; 885 } 886 EXPORT_SYMBOL_GPL(dpll_device_change_ntf); 887 888 static int 889 dpll_pin_event_send(enum dpll_cmd event, struct dpll_pin *pin) 890 { 891 struct sk_buff *msg; 892 int ret = -ENOMEM; 893 void *hdr; 894 895 if (!dpll_pin_available(pin)) 896 return -ENODEV; 897 898 msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 899 if (!msg) 900 return -ENOMEM; 901 902 hdr = genlmsg_put(msg, 0, 0, &dpll_nl_family, 0, event); 903 if (!hdr) 904 goto err_free_msg; 905 ret = dpll_cmd_pin_get_one(msg, pin, NULL); 906 if (ret) 907 goto err_cancel_msg; 908 genlmsg_end(msg, hdr); 909 genlmsg_multicast(&dpll_nl_family, msg, 0, 0, GFP_KERNEL); 910 911 return 0; 912 913 err_cancel_msg: 914 genlmsg_cancel(msg, hdr); 915 err_free_msg: 916 nlmsg_free(msg); 917 918 return ret; 919 } 920 921 int dpll_pin_create_ntf(struct dpll_pin *pin, u64 src_clock_id) 922 { 923 dpll_pin_notify(pin, src_clock_id, DPLL_PIN_CREATED); 924 return dpll_pin_event_send(DPLL_CMD_PIN_CREATE_NTF, pin); 925 } 926 927 int dpll_pin_delete_ntf(struct dpll_pin *pin, u64 src_clock_id) 928 { 929 dpll_pin_notify(pin, src_clock_id, DPLL_PIN_DELETED); 930 return dpll_pin_event_send(DPLL_CMD_PIN_DELETE_NTF, pin); 931 } 932 933 /** 934 * __dpll_pin_change_ntf - notify that the pin has been changed 935 * @pin: registered pin pointer 936 * 937 * Context: caller must hold dpll_lock. Suitable for use inside pin 938 * callbacks which are already invoked under dpll_lock. 939 * Return: 0 if succeeds, error code otherwise. 940 */ 941 int __dpll_pin_change_ntf(struct dpll_pin *pin) 942 { 943 lockdep_assert_held(&dpll_lock); 944 dpll_pin_notify(pin, pin->clock_id, DPLL_PIN_CHANGED); 945 return dpll_pin_event_send(DPLL_CMD_PIN_CHANGE_NTF, pin); 946 } 947 EXPORT_SYMBOL_GPL(__dpll_pin_change_ntf); 948 949 /** 950 * dpll_pin_change_ntf - notify that the pin has been changed 951 * @pin: registered pin pointer 952 * 953 * Context: acquires and holds a dpll_lock. 954 * Return: 0 if succeeds, error code otherwise. 955 */ 956 int dpll_pin_change_ntf(struct dpll_pin *pin) 957 { 958 int ret; 959 960 mutex_lock(&dpll_lock); 961 ret = __dpll_pin_change_ntf(pin); 962 mutex_unlock(&dpll_lock); 963 964 return ret; 965 } 966 EXPORT_SYMBOL_GPL(dpll_pin_change_ntf); 967 968 static int 969 dpll_mode_set(struct dpll_device *dpll, struct nlattr *a, 970 struct netlink_ext_ack *extack) 971 { 972 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 973 DECLARE_BITMAP(modes, DPLL_MODE_MAX + 1) = { 0 }; 974 enum dpll_mode mode = nla_get_u32(a), old_mode; 975 int ret; 976 977 if (!(ops->mode_set && ops->supported_modes_get)) { 978 NL_SET_ERR_MSG_ATTR(extack, a, 979 "dpll device does not support mode switch"); 980 return -EOPNOTSUPP; 981 } 982 983 ret = ops->mode_get(dpll, dpll_priv(dpll), &old_mode, extack); 984 if (ret) { 985 NL_SET_ERR_MSG(extack, "unable to get current mode"); 986 return ret; 987 } 988 989 if (mode == old_mode) 990 return 0; 991 992 ret = ops->supported_modes_get(dpll, dpll_priv(dpll), modes, extack); 993 if (ret) { 994 NL_SET_ERR_MSG(extack, "unable to get supported modes"); 995 return ret; 996 } 997 998 if (!test_bit(mode, modes)) { 999 NL_SET_ERR_MSG(extack, 1000 "dpll device does not support requested mode"); 1001 return -EINVAL; 1002 } 1003 1004 return ops->mode_set(dpll, dpll_priv(dpll), mode, extack); 1005 } 1006 1007 static int 1008 dpll_phase_offset_monitor_set(struct dpll_device *dpll, struct nlattr *a, 1009 struct netlink_ext_ack *extack) 1010 { 1011 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 1012 enum dpll_feature_state state = nla_get_u32(a), old_state; 1013 int ret; 1014 1015 if (!(ops->phase_offset_monitor_set && ops->phase_offset_monitor_get)) { 1016 NL_SET_ERR_MSG_ATTR(extack, a, "dpll device not capable of phase offset monitor"); 1017 return -EOPNOTSUPP; 1018 } 1019 ret = ops->phase_offset_monitor_get(dpll, dpll_priv(dpll), &old_state, 1020 extack); 1021 if (ret) { 1022 NL_SET_ERR_MSG(extack, "unable to get current state of phase offset monitor"); 1023 return ret; 1024 } 1025 if (state == old_state) 1026 return 0; 1027 1028 return ops->phase_offset_monitor_set(dpll, dpll_priv(dpll), state, 1029 extack); 1030 } 1031 1032 static int 1033 dpll_phase_offset_avg_factor_set(struct dpll_device *dpll, struct nlattr *a, 1034 struct netlink_ext_ack *extack) 1035 { 1036 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 1037 u32 factor = nla_get_u32(a); 1038 1039 if (!ops->phase_offset_avg_factor_set) { 1040 NL_SET_ERR_MSG_ATTR(extack, a, 1041 "device not capable of changing phase offset average factor"); 1042 return -EOPNOTSUPP; 1043 } 1044 1045 return ops->phase_offset_avg_factor_set(dpll, dpll_priv(dpll), factor, 1046 extack); 1047 } 1048 1049 static int 1050 dpll_freq_monitor_set(struct dpll_device *dpll, struct nlattr *a, 1051 struct netlink_ext_ack *extack) 1052 { 1053 const struct dpll_device_ops *ops = dpll_device_ops(dpll); 1054 enum dpll_feature_state state = nla_get_u32(a), old_state; 1055 int ret; 1056 1057 if (!(ops->freq_monitor_set && ops->freq_monitor_get)) { 1058 NL_SET_ERR_MSG_ATTR(extack, a, 1059 "dpll device not capable of frequency monitor"); 1060 return -EOPNOTSUPP; 1061 } 1062 ret = ops->freq_monitor_get(dpll, dpll_priv(dpll), &old_state, 1063 extack); 1064 if (ret) { 1065 NL_SET_ERR_MSG(extack, 1066 "unable to get current state of frequency monitor"); 1067 return ret; 1068 } 1069 if (state == old_state) 1070 return 0; 1071 1072 return ops->freq_monitor_set(dpll, dpll_priv(dpll), state, extack); 1073 } 1074 1075 static int 1076 dpll_pin_freq_set(struct dpll_pin *pin, struct nlattr *a, 1077 struct netlink_ext_ack *extack) 1078 { 1079 u64 freq = nla_get_u64(a), old_freq; 1080 struct dpll_pin_ref *ref, *failed; 1081 const struct dpll_pin_ops *ops; 1082 struct dpll_device *dpll; 1083 unsigned long i; 1084 int ret; 1085 1086 if (!dpll_pin_is_freq_supported(pin, freq)) { 1087 NL_SET_ERR_MSG_ATTR(extack, a, "frequency is not supported by the device"); 1088 return -EINVAL; 1089 } 1090 1091 xa_for_each(&pin->dpll_refs, i, ref) { 1092 ops = dpll_pin_ops(ref); 1093 if (!ops->frequency_set || !ops->frequency_get) { 1094 NL_SET_ERR_MSG(extack, "frequency set not supported by the device"); 1095 return -EOPNOTSUPP; 1096 } 1097 } 1098 ref = dpll_xa_ref_dpll_first(&pin->dpll_refs); 1099 ops = dpll_pin_ops(ref); 1100 dpll = ref->dpll; 1101 ret = ops->frequency_get(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll, 1102 dpll_priv(dpll), &old_freq, extack); 1103 if (ret) { 1104 NL_SET_ERR_MSG(extack, "unable to get old frequency value"); 1105 return ret; 1106 } 1107 if (freq == old_freq) 1108 return 0; 1109 1110 xa_for_each(&pin->dpll_refs, i, ref) { 1111 ops = dpll_pin_ops(ref); 1112 dpll = ref->dpll; 1113 ret = ops->frequency_set(pin, dpll_pin_on_dpll_priv(dpll, pin), 1114 dpll, dpll_priv(dpll), freq, extack); 1115 if (ret) { 1116 failed = ref; 1117 NL_SET_ERR_MSG_FMT(extack, "frequency set failed for dpll_id:%u", 1118 dpll->id); 1119 goto rollback; 1120 } 1121 } 1122 __dpll_pin_change_ntf(pin); 1123 1124 return 0; 1125 1126 rollback: 1127 xa_for_each(&pin->dpll_refs, i, ref) { 1128 if (ref == failed) 1129 break; 1130 ops = dpll_pin_ops(ref); 1131 dpll = ref->dpll; 1132 if (ops->frequency_set(pin, dpll_pin_on_dpll_priv(dpll, pin), 1133 dpll, dpll_priv(dpll), old_freq, extack)) 1134 NL_SET_ERR_MSG(extack, "set frequency rollback failed"); 1135 } 1136 return ret; 1137 } 1138 1139 static int 1140 dpll_pin_esync_set(struct dpll_pin *pin, struct nlattr *a, 1141 struct netlink_ext_ack *extack) 1142 { 1143 struct dpll_pin_ref *ref, *failed; 1144 const struct dpll_pin_ops *ops; 1145 struct dpll_pin_esync esync; 1146 u64 freq = nla_get_u64(a); 1147 struct dpll_device *dpll; 1148 bool supported = false; 1149 unsigned long i; 1150 int ret; 1151 1152 xa_for_each(&pin->dpll_refs, i, ref) { 1153 ops = dpll_pin_ops(ref); 1154 if (!ops->esync_set || !ops->esync_get) { 1155 NL_SET_ERR_MSG(extack, 1156 "embedded sync feature is not supported by this device"); 1157 return -EOPNOTSUPP; 1158 } 1159 } 1160 ref = dpll_xa_ref_dpll_first(&pin->dpll_refs); 1161 ops = dpll_pin_ops(ref); 1162 dpll = ref->dpll; 1163 ret = ops->esync_get(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll, 1164 dpll_priv(dpll), &esync, extack); 1165 if (ret) { 1166 NL_SET_ERR_MSG(extack, "unable to get current embedded sync frequency value"); 1167 return ret; 1168 } 1169 if (freq == esync.freq) 1170 return 0; 1171 for (i = 0; i < esync.range_num; i++) 1172 if (freq <= esync.range[i].max && freq >= esync.range[i].min) 1173 supported = true; 1174 if (!supported) { 1175 NL_SET_ERR_MSG_ATTR(extack, a, 1176 "requested embedded sync frequency value is not supported by this device"); 1177 return -EINVAL; 1178 } 1179 1180 xa_for_each(&pin->dpll_refs, i, ref) { 1181 void *pin_dpll_priv; 1182 1183 ops = dpll_pin_ops(ref); 1184 dpll = ref->dpll; 1185 pin_dpll_priv = dpll_pin_on_dpll_priv(dpll, pin); 1186 ret = ops->esync_set(pin, pin_dpll_priv, dpll, dpll_priv(dpll), 1187 freq, extack); 1188 if (ret) { 1189 failed = ref; 1190 NL_SET_ERR_MSG_FMT(extack, 1191 "embedded sync frequency set failed for dpll_id: %u", 1192 dpll->id); 1193 goto rollback; 1194 } 1195 } 1196 __dpll_pin_change_ntf(pin); 1197 1198 return 0; 1199 1200 rollback: 1201 xa_for_each(&pin->dpll_refs, i, ref) { 1202 void *pin_dpll_priv; 1203 1204 if (ref == failed) 1205 break; 1206 ops = dpll_pin_ops(ref); 1207 dpll = ref->dpll; 1208 pin_dpll_priv = dpll_pin_on_dpll_priv(dpll, pin); 1209 if (ops->esync_set(pin, pin_dpll_priv, dpll, dpll_priv(dpll), 1210 esync.freq, extack)) 1211 NL_SET_ERR_MSG(extack, "set embedded sync frequency rollback failed"); 1212 } 1213 return ret; 1214 } 1215 1216 static int 1217 dpll_pin_ref_sync_state_set(struct dpll_pin *pin, 1218 unsigned long ref_sync_pin_idx, 1219 const enum dpll_pin_state state, 1220 struct netlink_ext_ack *extack) 1221 1222 { 1223 struct dpll_pin_ref *ref, *failed; 1224 const struct dpll_pin_ops *ops; 1225 enum dpll_pin_state old_state; 1226 struct dpll_pin *ref_sync_pin; 1227 struct dpll_device *dpll; 1228 unsigned long i; 1229 int ret; 1230 1231 ref_sync_pin = xa_find(&pin->ref_sync_pins, &ref_sync_pin_idx, 1232 ULONG_MAX, XA_PRESENT); 1233 if (!ref_sync_pin) { 1234 NL_SET_ERR_MSG(extack, "reference sync pin not found"); 1235 return -EINVAL; 1236 } 1237 if (!dpll_pin_available(ref_sync_pin)) { 1238 NL_SET_ERR_MSG(extack, "reference sync pin not available"); 1239 return -EINVAL; 1240 } 1241 ref = dpll_xa_ref_dpll_first(&pin->dpll_refs); 1242 ASSERT_NOT_NULL(ref); 1243 ops = dpll_pin_ops(ref); 1244 if (!ops->ref_sync_set || !ops->ref_sync_get) { 1245 NL_SET_ERR_MSG(extack, "reference sync not supported by this pin"); 1246 return -EOPNOTSUPP; 1247 } 1248 dpll = ref->dpll; 1249 ret = ops->ref_sync_get(pin, dpll_pin_on_dpll_priv(dpll, pin), 1250 ref_sync_pin, 1251 dpll_pin_on_dpll_priv(dpll, ref_sync_pin), 1252 &old_state, extack); 1253 if (ret) { 1254 NL_SET_ERR_MSG(extack, "unable to get old reference sync state"); 1255 return ret; 1256 } 1257 if (state == old_state) 1258 return 0; 1259 xa_for_each(&pin->dpll_refs, i, ref) { 1260 ops = dpll_pin_ops(ref); 1261 dpll = ref->dpll; 1262 ret = ops->ref_sync_set(pin, dpll_pin_on_dpll_priv(dpll, pin), 1263 ref_sync_pin, 1264 dpll_pin_on_dpll_priv(dpll, 1265 ref_sync_pin), 1266 state, extack); 1267 if (ret) { 1268 failed = ref; 1269 NL_SET_ERR_MSG_FMT(extack, "reference sync set failed for dpll_id:%u", 1270 dpll->id); 1271 goto rollback; 1272 } 1273 } 1274 __dpll_pin_change_ntf(pin); 1275 1276 return 0; 1277 1278 rollback: 1279 xa_for_each(&pin->dpll_refs, i, ref) { 1280 if (ref == failed) 1281 break; 1282 ops = dpll_pin_ops(ref); 1283 dpll = ref->dpll; 1284 if (ops->ref_sync_set(pin, dpll_pin_on_dpll_priv(dpll, pin), 1285 ref_sync_pin, 1286 dpll_pin_on_dpll_priv(dpll, ref_sync_pin), 1287 old_state, extack)) 1288 NL_SET_ERR_MSG(extack, "set reference sync rollback failed"); 1289 } 1290 return ret; 1291 } 1292 1293 static int 1294 dpll_pin_ref_sync_set(struct dpll_pin *pin, struct nlattr *nest, 1295 struct netlink_ext_ack *extack) 1296 { 1297 struct nlattr *tb[DPLL_A_PIN_MAX + 1]; 1298 enum dpll_pin_state state; 1299 u32 sync_pin_id; 1300 1301 nla_parse_nested(tb, DPLL_A_PIN_MAX, nest, 1302 dpll_reference_sync_nl_policy, extack); 1303 if (!tb[DPLL_A_PIN_ID]) { 1304 NL_SET_ERR_MSG(extack, "sync pin id expected"); 1305 return -EINVAL; 1306 } 1307 sync_pin_id = nla_get_u32(tb[DPLL_A_PIN_ID]); 1308 1309 if (!tb[DPLL_A_PIN_STATE]) { 1310 NL_SET_ERR_MSG(extack, "sync pin state expected"); 1311 return -EINVAL; 1312 } 1313 state = nla_get_u32(tb[DPLL_A_PIN_STATE]); 1314 1315 return dpll_pin_ref_sync_state_set(pin, sync_pin_id, state, extack); 1316 } 1317 1318 static int 1319 dpll_pin_on_pin_state_set(struct dpll_pin *pin, u32 parent_idx, 1320 enum dpll_pin_state state, 1321 struct netlink_ext_ack *extack) 1322 { 1323 struct dpll_pin_ref *parent_ref; 1324 const struct dpll_pin_ops *ops; 1325 struct dpll_pin_ref *dpll_ref; 1326 void *pin_priv, *parent_priv; 1327 struct dpll_pin *parent; 1328 unsigned long i; 1329 int ret; 1330 1331 /* fwnode pins may not set the capability bit upfront; let the ops 1332 * layer return -EOPNOTSUPP if the operation is unsupported. 1333 */ 1334 if (!(DPLL_PIN_CAPABILITIES_STATE_CAN_CHANGE & 1335 pin->prop.capabilities) && !pin->fwnode) { 1336 NL_SET_ERR_MSG(extack, "state changing is not allowed"); 1337 return -EOPNOTSUPP; 1338 } 1339 parent = xa_load(&dpll_pin_xa, parent_idx); 1340 if (!parent) 1341 return -EINVAL; 1342 parent_ref = xa_load(&pin->parent_refs, parent->pin_idx); 1343 if (!parent_ref) 1344 return -EINVAL; 1345 xa_for_each(&parent->dpll_refs, i, dpll_ref) { 1346 ops = dpll_pin_ops(parent_ref); 1347 if (!ops->state_on_pin_set) 1348 return -EOPNOTSUPP; 1349 pin_priv = dpll_pin_on_pin_priv(parent, pin); 1350 parent_priv = dpll_pin_on_dpll_priv(dpll_ref->dpll, parent); 1351 ret = ops->state_on_pin_set(pin, pin_priv, parent, parent_priv, 1352 state, extack); 1353 if (ret) 1354 return ret; 1355 } 1356 __dpll_pin_change_ntf(pin); 1357 1358 return 0; 1359 } 1360 1361 static int 1362 dpll_pin_state_set(struct dpll_device *dpll, struct dpll_pin *pin, 1363 enum dpll_pin_state state, 1364 struct netlink_ext_ack *extack) 1365 { 1366 const struct dpll_pin_ops *ops; 1367 struct dpll_pin_ref *ref; 1368 int ret; 1369 1370 /* fwnode pins may not set the capability bit upfront; let the ops 1371 * layer return -EOPNOTSUPP if the operation is unsupported. 1372 */ 1373 if (!(DPLL_PIN_CAPABILITIES_STATE_CAN_CHANGE & 1374 pin->prop.capabilities) && !pin->fwnode) { 1375 NL_SET_ERR_MSG(extack, "state changing is not allowed"); 1376 return -EOPNOTSUPP; 1377 } 1378 ref = xa_load(&pin->dpll_refs, dpll->id); 1379 ASSERT_NOT_NULL(ref); 1380 ops = dpll_pin_ops(ref); 1381 if (!ops->state_on_dpll_set) 1382 return -EOPNOTSUPP; 1383 ret = ops->state_on_dpll_set(pin, dpll_pin_on_dpll_priv(dpll, pin), 1384 dpll, dpll_priv(dpll), state, extack); 1385 if (ret) 1386 return ret; 1387 __dpll_pin_change_ntf(pin); 1388 1389 return 0; 1390 } 1391 1392 static int 1393 dpll_pin_prio_set(struct dpll_device *dpll, struct dpll_pin *pin, 1394 u32 prio, struct netlink_ext_ack *extack) 1395 { 1396 const struct dpll_pin_ops *ops; 1397 struct dpll_pin_ref *ref; 1398 int ret; 1399 1400 if (!(DPLL_PIN_CAPABILITIES_PRIORITY_CAN_CHANGE & 1401 pin->prop.capabilities)) { 1402 NL_SET_ERR_MSG(extack, "prio changing is not allowed"); 1403 return -EOPNOTSUPP; 1404 } 1405 ref = xa_load(&pin->dpll_refs, dpll->id); 1406 ASSERT_NOT_NULL(ref); 1407 ops = dpll_pin_ops(ref); 1408 if (!ops->prio_set) 1409 return -EOPNOTSUPP; 1410 ret = ops->prio_set(pin, dpll_pin_on_dpll_priv(dpll, pin), dpll, 1411 dpll_priv(dpll), prio, extack); 1412 if (ret) 1413 return ret; 1414 __dpll_pin_change_ntf(pin); 1415 1416 return 0; 1417 } 1418 1419 static int 1420 dpll_pin_direction_set(struct dpll_pin *pin, struct dpll_device *dpll, 1421 enum dpll_pin_direction direction, 1422 struct netlink_ext_ack *extack) 1423 { 1424 const struct dpll_pin_ops *ops; 1425 struct dpll_pin_ref *ref; 1426 int ret; 1427 1428 if (!(DPLL_PIN_CAPABILITIES_DIRECTION_CAN_CHANGE & 1429 pin->prop.capabilities)) { 1430 NL_SET_ERR_MSG(extack, "direction changing is not allowed"); 1431 return -EOPNOTSUPP; 1432 } 1433 ref = xa_load(&pin->dpll_refs, dpll->id); 1434 ASSERT_NOT_NULL(ref); 1435 ops = dpll_pin_ops(ref); 1436 if (!ops->direction_set) 1437 return -EOPNOTSUPP; 1438 ret = ops->direction_set(pin, dpll_pin_on_dpll_priv(dpll, pin), 1439 dpll, dpll_priv(dpll), direction, extack); 1440 if (ret) 1441 return ret; 1442 __dpll_pin_change_ntf(pin); 1443 1444 return 0; 1445 } 1446 1447 static int 1448 dpll_pin_phase_adj_set(struct dpll_pin *pin, struct nlattr *phase_adj_attr, 1449 struct netlink_ext_ack *extack) 1450 { 1451 struct dpll_pin_ref *ref, *failed; 1452 const struct dpll_pin_ops *ops; 1453 s32 phase_adj, old_phase_adj; 1454 struct dpll_device *dpll; 1455 unsigned long i; 1456 int ret; 1457 1458 phase_adj = nla_get_s32(phase_adj_attr); 1459 if (phase_adj > pin->prop.phase_range.max || 1460 phase_adj < pin->prop.phase_range.min) { 1461 NL_SET_ERR_MSG_ATTR(extack, phase_adj_attr, 1462 "phase adjust value of out range"); 1463 return -EINVAL; 1464 } 1465 if (pin->prop.phase_gran && phase_adj % (s32)pin->prop.phase_gran) { 1466 NL_SET_ERR_MSG_ATTR_FMT(extack, phase_adj_attr, 1467 "phase adjust value not multiple of %u", 1468 pin->prop.phase_gran); 1469 return -EINVAL; 1470 } 1471 1472 xa_for_each(&pin->dpll_refs, i, ref) { 1473 ops = dpll_pin_ops(ref); 1474 if (!ops->phase_adjust_set || !ops->phase_adjust_get) { 1475 NL_SET_ERR_MSG(extack, "phase adjust not supported"); 1476 return -EOPNOTSUPP; 1477 } 1478 } 1479 ref = dpll_xa_ref_dpll_first(&pin->dpll_refs); 1480 ops = dpll_pin_ops(ref); 1481 dpll = ref->dpll; 1482 ret = ops->phase_adjust_get(pin, dpll_pin_on_dpll_priv(dpll, pin), 1483 dpll, dpll_priv(dpll), &old_phase_adj, 1484 extack); 1485 if (ret) { 1486 NL_SET_ERR_MSG(extack, "unable to get old phase adjust value"); 1487 return ret; 1488 } 1489 if (phase_adj == old_phase_adj) 1490 return 0; 1491 1492 xa_for_each(&pin->dpll_refs, i, ref) { 1493 ops = dpll_pin_ops(ref); 1494 dpll = ref->dpll; 1495 ret = ops->phase_adjust_set(pin, 1496 dpll_pin_on_dpll_priv(dpll, pin), 1497 dpll, dpll_priv(dpll), phase_adj, 1498 extack); 1499 if (ret) { 1500 failed = ref; 1501 NL_SET_ERR_MSG_FMT(extack, 1502 "phase adjust set failed for dpll_id:%u", 1503 dpll->id); 1504 goto rollback; 1505 } 1506 } 1507 __dpll_pin_change_ntf(pin); 1508 1509 return 0; 1510 1511 rollback: 1512 xa_for_each(&pin->dpll_refs, i, ref) { 1513 if (ref == failed) 1514 break; 1515 ops = dpll_pin_ops(ref); 1516 dpll = ref->dpll; 1517 if (ops->phase_adjust_set(pin, dpll_pin_on_dpll_priv(dpll, pin), 1518 dpll, dpll_priv(dpll), old_phase_adj, 1519 extack)) 1520 NL_SET_ERR_MSG(extack, "set phase adjust rollback failed"); 1521 } 1522 return ret; 1523 } 1524 1525 static int 1526 dpll_pin_parent_device_set(struct dpll_pin *pin, struct nlattr *parent_nest, 1527 struct netlink_ext_ack *extack) 1528 { 1529 struct nlattr *tb[DPLL_A_PIN_MAX + 1]; 1530 enum dpll_pin_direction direction; 1531 enum dpll_pin_state state; 1532 struct dpll_pin_ref *ref; 1533 struct dpll_device *dpll; 1534 u32 pdpll_idx, prio; 1535 int ret; 1536 1537 nla_parse_nested(tb, DPLL_A_PIN_MAX, parent_nest, 1538 dpll_pin_parent_device_nl_policy, extack); 1539 if (!tb[DPLL_A_PIN_PARENT_ID]) { 1540 NL_SET_ERR_MSG(extack, "device parent id expected"); 1541 return -EINVAL; 1542 } 1543 pdpll_idx = nla_get_u32(tb[DPLL_A_PIN_PARENT_ID]); 1544 dpll = xa_load(&dpll_device_xa, pdpll_idx); 1545 if (!dpll) { 1546 NL_SET_ERR_MSG(extack, "parent device not found"); 1547 return -EINVAL; 1548 } 1549 ref = xa_load(&pin->dpll_refs, dpll->id); 1550 if (!ref) { 1551 NL_SET_ERR_MSG(extack, "pin not connected to given parent device"); 1552 return -EINVAL; 1553 } 1554 if (tb[DPLL_A_PIN_STATE]) { 1555 state = nla_get_u32(tb[DPLL_A_PIN_STATE]); 1556 ret = dpll_pin_state_set(dpll, pin, state, extack); 1557 if (ret) 1558 return ret; 1559 } 1560 if (tb[DPLL_A_PIN_PRIO]) { 1561 prio = nla_get_u32(tb[DPLL_A_PIN_PRIO]); 1562 ret = dpll_pin_prio_set(dpll, pin, prio, extack); 1563 if (ret) 1564 return ret; 1565 } 1566 if (tb[DPLL_A_PIN_DIRECTION]) { 1567 direction = nla_get_u32(tb[DPLL_A_PIN_DIRECTION]); 1568 ret = dpll_pin_direction_set(pin, dpll, direction, extack); 1569 if (ret) 1570 return ret; 1571 } 1572 return 0; 1573 } 1574 1575 static int 1576 dpll_pin_parent_pin_set(struct dpll_pin *pin, struct nlattr *parent_nest, 1577 struct netlink_ext_ack *extack) 1578 { 1579 struct nlattr *tb[DPLL_A_PIN_MAX + 1]; 1580 u32 ppin_idx; 1581 int ret; 1582 1583 nla_parse_nested(tb, DPLL_A_PIN_MAX, parent_nest, 1584 dpll_pin_parent_pin_nl_policy, extack); 1585 if (!tb[DPLL_A_PIN_PARENT_ID]) { 1586 NL_SET_ERR_MSG(extack, "device parent id expected"); 1587 return -EINVAL; 1588 } 1589 ppin_idx = nla_get_u32(tb[DPLL_A_PIN_PARENT_ID]); 1590 1591 if (tb[DPLL_A_PIN_STATE]) { 1592 enum dpll_pin_state state = nla_get_u32(tb[DPLL_A_PIN_STATE]); 1593 1594 ret = dpll_pin_on_pin_state_set(pin, ppin_idx, state, extack); 1595 if (ret) 1596 return ret; 1597 } 1598 1599 return 0; 1600 } 1601 1602 static int 1603 dpll_pin_set_from_nlattr(struct dpll_pin *pin, struct genl_info *info) 1604 { 1605 struct nlattr *a; 1606 int rem, ret; 1607 1608 nla_for_each_attr(a, genlmsg_data(info->genlhdr), 1609 genlmsg_len(info->genlhdr), rem) { 1610 switch (nla_type(a)) { 1611 case DPLL_A_PIN_FREQUENCY: 1612 ret = dpll_pin_freq_set(pin, a, info->extack); 1613 if (ret) 1614 return ret; 1615 break; 1616 case DPLL_A_PIN_PHASE_ADJUST: 1617 ret = dpll_pin_phase_adj_set(pin, a, info->extack); 1618 if (ret) 1619 return ret; 1620 break; 1621 case DPLL_A_PIN_PARENT_DEVICE: 1622 ret = dpll_pin_parent_device_set(pin, a, info->extack); 1623 if (ret) 1624 return ret; 1625 break; 1626 case DPLL_A_PIN_PARENT_PIN: 1627 ret = dpll_pin_parent_pin_set(pin, a, info->extack); 1628 if (ret) 1629 return ret; 1630 break; 1631 case DPLL_A_PIN_ESYNC_FREQUENCY: 1632 ret = dpll_pin_esync_set(pin, a, info->extack); 1633 if (ret) 1634 return ret; 1635 break; 1636 case DPLL_A_PIN_REFERENCE_SYNC: 1637 ret = dpll_pin_ref_sync_set(pin, a, info->extack); 1638 if (ret) 1639 return ret; 1640 break; 1641 } 1642 } 1643 1644 return 0; 1645 } 1646 1647 static struct dpll_pin * 1648 dpll_pin_find(u64 clock_id, struct nlattr *mod_name_attr, 1649 enum dpll_pin_type type, struct nlattr *board_label, 1650 struct nlattr *panel_label, struct nlattr *package_label, 1651 struct netlink_ext_ack *extack) 1652 { 1653 bool board_match, panel_match, package_match; 1654 struct dpll_pin *pin_match = NULL, *pin; 1655 const struct dpll_pin_properties *prop; 1656 bool cid_match, mod_match, type_match; 1657 unsigned long i; 1658 1659 xa_for_each_marked(&dpll_pin_xa, i, pin, DPLL_REGISTERED) { 1660 prop = &pin->prop; 1661 cid_match = clock_id ? pin->clock_id == clock_id : true; 1662 mod_match = mod_name_attr && pin->module_name[0] ? 1663 !nla_strcmp(mod_name_attr, 1664 pin->module_name) : true; 1665 type_match = type ? prop->type == type : true; 1666 board_match = board_label ? (prop->board_label ? 1667 !nla_strcmp(board_label, prop->board_label) : false) : 1668 true; 1669 panel_match = panel_label ? (prop->panel_label ? 1670 !nla_strcmp(panel_label, prop->panel_label) : false) : 1671 true; 1672 package_match = package_label ? (prop->package_label ? 1673 !nla_strcmp(package_label, prop->package_label) : 1674 false) : true; 1675 if (cid_match && mod_match && type_match && board_match && 1676 panel_match && package_match) { 1677 if (pin_match) { 1678 NL_SET_ERR_MSG(extack, "multiple matches"); 1679 return ERR_PTR(-EINVAL); 1680 } 1681 pin_match = pin; 1682 } 1683 } 1684 if (!pin_match) { 1685 NL_SET_ERR_MSG(extack, "not found"); 1686 return ERR_PTR(-ENODEV); 1687 } 1688 return pin_match; 1689 } 1690 1691 static struct dpll_pin *dpll_pin_find_from_nlattr(struct genl_info *info) 1692 { 1693 struct nlattr *attr, *mod_name_attr = NULL, *board_label_attr = NULL, 1694 *panel_label_attr = NULL, *package_label_attr = NULL; 1695 enum dpll_pin_type type = 0; 1696 u64 clock_id = 0; 1697 int rem = 0; 1698 1699 nla_for_each_attr(attr, genlmsg_data(info->genlhdr), 1700 genlmsg_len(info->genlhdr), rem) { 1701 switch (nla_type(attr)) { 1702 case DPLL_A_PIN_CLOCK_ID: 1703 if (clock_id) 1704 goto duplicated_attr; 1705 clock_id = nla_get_u64(attr); 1706 break; 1707 case DPLL_A_PIN_MODULE_NAME: 1708 if (mod_name_attr) 1709 goto duplicated_attr; 1710 mod_name_attr = attr; 1711 break; 1712 case DPLL_A_PIN_TYPE: 1713 if (type) 1714 goto duplicated_attr; 1715 type = nla_get_u32(attr); 1716 break; 1717 case DPLL_A_PIN_BOARD_LABEL: 1718 if (board_label_attr) 1719 goto duplicated_attr; 1720 board_label_attr = attr; 1721 break; 1722 case DPLL_A_PIN_PANEL_LABEL: 1723 if (panel_label_attr) 1724 goto duplicated_attr; 1725 panel_label_attr = attr; 1726 break; 1727 case DPLL_A_PIN_PACKAGE_LABEL: 1728 if (package_label_attr) 1729 goto duplicated_attr; 1730 package_label_attr = attr; 1731 break; 1732 default: 1733 break; 1734 } 1735 } 1736 if (!(clock_id || mod_name_attr || board_label_attr || 1737 panel_label_attr || package_label_attr)) { 1738 NL_SET_ERR_MSG(info->extack, "missing attributes"); 1739 return ERR_PTR(-EINVAL); 1740 } 1741 return dpll_pin_find(clock_id, mod_name_attr, type, board_label_attr, 1742 panel_label_attr, package_label_attr, 1743 info->extack); 1744 duplicated_attr: 1745 NL_SET_ERR_MSG(info->extack, "duplicated attribute"); 1746 return ERR_PTR(-EINVAL); 1747 } 1748 1749 int dpll_nl_pin_id_get_doit(struct sk_buff *skb, struct genl_info *info) 1750 { 1751 struct dpll_pin *pin; 1752 struct sk_buff *msg; 1753 struct nlattr *hdr; 1754 int ret; 1755 1756 msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 1757 if (!msg) 1758 return -ENOMEM; 1759 hdr = genlmsg_put_reply(msg, info, &dpll_nl_family, 0, 1760 DPLL_CMD_PIN_ID_GET); 1761 if (!hdr) { 1762 nlmsg_free(msg); 1763 return -EMSGSIZE; 1764 } 1765 pin = dpll_pin_find_from_nlattr(info); 1766 if (IS_ERR(pin)) { 1767 nlmsg_free(msg); 1768 return PTR_ERR(pin); 1769 } 1770 if (!dpll_pin_available(pin)) { 1771 nlmsg_free(msg); 1772 return -ENODEV; 1773 } 1774 ret = dpll_msg_add_pin_handle(msg, pin); 1775 if (ret) { 1776 nlmsg_free(msg); 1777 return ret; 1778 } 1779 genlmsg_end(msg, hdr); 1780 1781 return genlmsg_reply(msg, info); 1782 } 1783 1784 int dpll_nl_pin_get_doit(struct sk_buff *skb, struct genl_info *info) 1785 { 1786 struct dpll_pin *pin = info->user_ptr[0]; 1787 struct sk_buff *msg; 1788 struct nlattr *hdr; 1789 int ret; 1790 1791 if (!pin) 1792 return -ENODEV; 1793 msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 1794 if (!msg) 1795 return -ENOMEM; 1796 hdr = genlmsg_put_reply(msg, info, &dpll_nl_family, 0, 1797 DPLL_CMD_PIN_GET); 1798 if (!hdr) { 1799 nlmsg_free(msg); 1800 return -EMSGSIZE; 1801 } 1802 ret = dpll_cmd_pin_get_one(msg, pin, info->extack); 1803 if (ret) { 1804 nlmsg_free(msg); 1805 return ret; 1806 } 1807 genlmsg_end(msg, hdr); 1808 1809 return genlmsg_reply(msg, info); 1810 } 1811 1812 int dpll_nl_pin_get_dumpit(struct sk_buff *skb, struct netlink_callback *cb) 1813 { 1814 struct dpll_dump_ctx *ctx = dpll_dump_context(cb); 1815 struct dpll_pin *pin; 1816 struct nlattr *hdr; 1817 unsigned long i; 1818 int ret = 0; 1819 1820 mutex_lock(&dpll_lock); 1821 xa_for_each_marked_start(&dpll_pin_xa, i, pin, DPLL_REGISTERED, 1822 ctx->idx) { 1823 if (!dpll_pin_available(pin)) 1824 continue; 1825 hdr = genlmsg_put(skb, NETLINK_CB(cb->skb).portid, 1826 cb->nlh->nlmsg_seq, 1827 &dpll_nl_family, NLM_F_MULTI, 1828 DPLL_CMD_PIN_GET); 1829 if (!hdr) { 1830 ret = -EMSGSIZE; 1831 break; 1832 } 1833 ret = dpll_cmd_pin_get_one(skb, pin, cb->extack); 1834 if (ret) { 1835 genlmsg_cancel(skb, hdr); 1836 break; 1837 } 1838 genlmsg_end(skb, hdr); 1839 } 1840 mutex_unlock(&dpll_lock); 1841 1842 if (ret == -EMSGSIZE) { 1843 ctx->idx = i; 1844 return skb->len; 1845 } 1846 return ret; 1847 } 1848 1849 int dpll_nl_pin_set_doit(struct sk_buff *skb, struct genl_info *info) 1850 { 1851 struct dpll_pin *pin = info->user_ptr[0]; 1852 1853 return dpll_pin_set_from_nlattr(pin, info); 1854 } 1855 1856 static struct dpll_device * 1857 dpll_device_find(u64 clock_id, struct nlattr *mod_name_attr, 1858 enum dpll_type type, struct netlink_ext_ack *extack) 1859 { 1860 struct dpll_device *dpll_match = NULL, *dpll; 1861 bool cid_match, mod_match, type_match; 1862 unsigned long i; 1863 1864 xa_for_each_marked(&dpll_device_xa, i, dpll, DPLL_REGISTERED) { 1865 cid_match = clock_id ? dpll->clock_id == clock_id : true; 1866 mod_match = mod_name_attr ? (module_name(dpll->module) ? 1867 !nla_strcmp(mod_name_attr, 1868 module_name(dpll->module)) : false) : true; 1869 type_match = type ? dpll->type == type : true; 1870 if (cid_match && mod_match && type_match) { 1871 if (dpll_match) { 1872 NL_SET_ERR_MSG(extack, "multiple matches"); 1873 return ERR_PTR(-EINVAL); 1874 } 1875 dpll_match = dpll; 1876 } 1877 } 1878 if (!dpll_match) { 1879 NL_SET_ERR_MSG(extack, "not found"); 1880 return ERR_PTR(-ENODEV); 1881 } 1882 1883 return dpll_match; 1884 } 1885 1886 static struct dpll_device * 1887 dpll_device_find_from_nlattr(struct genl_info *info) 1888 { 1889 struct nlattr *attr, *mod_name_attr = NULL; 1890 enum dpll_type type = 0; 1891 u64 clock_id = 0; 1892 int rem = 0; 1893 1894 nla_for_each_attr(attr, genlmsg_data(info->genlhdr), 1895 genlmsg_len(info->genlhdr), rem) { 1896 switch (nla_type(attr)) { 1897 case DPLL_A_CLOCK_ID: 1898 if (clock_id) 1899 goto duplicated_attr; 1900 clock_id = nla_get_u64(attr); 1901 break; 1902 case DPLL_A_MODULE_NAME: 1903 if (mod_name_attr) 1904 goto duplicated_attr; 1905 mod_name_attr = attr; 1906 break; 1907 case DPLL_A_TYPE: 1908 if (type) 1909 goto duplicated_attr; 1910 type = nla_get_u32(attr); 1911 break; 1912 default: 1913 break; 1914 } 1915 } 1916 if (!clock_id && !mod_name_attr && !type) { 1917 NL_SET_ERR_MSG(info->extack, "missing attributes"); 1918 return ERR_PTR(-EINVAL); 1919 } 1920 return dpll_device_find(clock_id, mod_name_attr, type, info->extack); 1921 duplicated_attr: 1922 NL_SET_ERR_MSG(info->extack, "duplicated attribute"); 1923 return ERR_PTR(-EINVAL); 1924 } 1925 1926 int dpll_nl_device_id_get_doit(struct sk_buff *skb, struct genl_info *info) 1927 { 1928 struct dpll_device *dpll; 1929 struct sk_buff *msg; 1930 struct nlattr *hdr; 1931 int ret; 1932 1933 msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 1934 if (!msg) 1935 return -ENOMEM; 1936 hdr = genlmsg_put_reply(msg, info, &dpll_nl_family, 0, 1937 DPLL_CMD_DEVICE_ID_GET); 1938 if (!hdr) { 1939 nlmsg_free(msg); 1940 return -EMSGSIZE; 1941 } 1942 1943 dpll = dpll_device_find_from_nlattr(info); 1944 if (IS_ERR(dpll)) { 1945 nlmsg_free(msg); 1946 return PTR_ERR(dpll); 1947 } 1948 ret = dpll_msg_add_dev_handle(msg, dpll); 1949 if (ret) { 1950 nlmsg_free(msg); 1951 return ret; 1952 } 1953 genlmsg_end(msg, hdr); 1954 1955 return genlmsg_reply(msg, info); 1956 } 1957 1958 int dpll_nl_device_get_doit(struct sk_buff *skb, struct genl_info *info) 1959 { 1960 struct dpll_device *dpll = info->user_ptr[0]; 1961 struct sk_buff *msg; 1962 struct nlattr *hdr; 1963 int ret; 1964 1965 msg = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); 1966 if (!msg) 1967 return -ENOMEM; 1968 hdr = genlmsg_put_reply(msg, info, &dpll_nl_family, 0, 1969 DPLL_CMD_DEVICE_GET); 1970 if (!hdr) { 1971 nlmsg_free(msg); 1972 return -EMSGSIZE; 1973 } 1974 1975 ret = dpll_device_get_one(dpll, msg, info->extack); 1976 if (ret) { 1977 nlmsg_free(msg); 1978 return ret; 1979 } 1980 genlmsg_end(msg, hdr); 1981 1982 return genlmsg_reply(msg, info); 1983 } 1984 1985 static int 1986 dpll_set_from_nlattr(struct dpll_device *dpll, struct genl_info *info) 1987 { 1988 struct nlattr *a; 1989 int rem, ret; 1990 1991 nla_for_each_attr(a, genlmsg_data(info->genlhdr), 1992 genlmsg_len(info->genlhdr), rem) { 1993 switch (nla_type(a)) { 1994 case DPLL_A_MODE: 1995 ret = dpll_mode_set(dpll, a, info->extack); 1996 if (ret) 1997 return ret; 1998 break; 1999 case DPLL_A_PHASE_OFFSET_MONITOR: 2000 ret = dpll_phase_offset_monitor_set(dpll, a, 2001 info->extack); 2002 if (ret) 2003 return ret; 2004 break; 2005 case DPLL_A_PHASE_OFFSET_AVG_FACTOR: 2006 ret = dpll_phase_offset_avg_factor_set(dpll, a, 2007 info->extack); 2008 if (ret) 2009 return ret; 2010 break; 2011 case DPLL_A_FREQUENCY_MONITOR: 2012 ret = dpll_freq_monitor_set(dpll, a, 2013 info->extack); 2014 if (ret) 2015 return ret; 2016 break; 2017 } 2018 } 2019 2020 return 0; 2021 } 2022 2023 int dpll_nl_device_set_doit(struct sk_buff *skb, struct genl_info *info) 2024 { 2025 struct dpll_device *dpll = info->user_ptr[0]; 2026 2027 return dpll_set_from_nlattr(dpll, info); 2028 } 2029 2030 int dpll_nl_device_get_dumpit(struct sk_buff *skb, struct netlink_callback *cb) 2031 { 2032 struct dpll_dump_ctx *ctx = dpll_dump_context(cb); 2033 struct dpll_device *dpll; 2034 struct nlattr *hdr; 2035 unsigned long i; 2036 int ret = 0; 2037 2038 mutex_lock(&dpll_lock); 2039 xa_for_each_marked_start(&dpll_device_xa, i, dpll, DPLL_REGISTERED, 2040 ctx->idx) { 2041 hdr = genlmsg_put(skb, NETLINK_CB(cb->skb).portid, 2042 cb->nlh->nlmsg_seq, &dpll_nl_family, 2043 NLM_F_MULTI, DPLL_CMD_DEVICE_GET); 2044 if (!hdr) { 2045 ret = -EMSGSIZE; 2046 break; 2047 } 2048 ret = dpll_device_get_one(dpll, skb, cb->extack); 2049 if (ret) { 2050 genlmsg_cancel(skb, hdr); 2051 break; 2052 } 2053 genlmsg_end(skb, hdr); 2054 } 2055 mutex_unlock(&dpll_lock); 2056 2057 if (ret == -EMSGSIZE) { 2058 ctx->idx = i; 2059 return skb->len; 2060 } 2061 return ret; 2062 } 2063 2064 int dpll_pre_doit(const struct genl_split_ops *ops, struct sk_buff *skb, 2065 struct genl_info *info) 2066 { 2067 u32 id; 2068 2069 if (GENL_REQ_ATTR_CHECK(info, DPLL_A_ID)) 2070 return -EINVAL; 2071 2072 mutex_lock(&dpll_lock); 2073 id = nla_get_u32(info->attrs[DPLL_A_ID]); 2074 info->user_ptr[0] = dpll_device_get_by_id(id); 2075 if (!info->user_ptr[0]) { 2076 NL_SET_ERR_MSG(info->extack, "device not found"); 2077 goto unlock; 2078 } 2079 return 0; 2080 unlock: 2081 mutex_unlock(&dpll_lock); 2082 return -ENODEV; 2083 } 2084 2085 void dpll_post_doit(const struct genl_split_ops *ops, struct sk_buff *skb, 2086 struct genl_info *info) 2087 { 2088 mutex_unlock(&dpll_lock); 2089 } 2090 2091 int 2092 dpll_lock_doit(const struct genl_split_ops *ops, struct sk_buff *skb, 2093 struct genl_info *info) 2094 { 2095 mutex_lock(&dpll_lock); 2096 2097 return 0; 2098 } 2099 2100 void 2101 dpll_unlock_doit(const struct genl_split_ops *ops, struct sk_buff *skb, 2102 struct genl_info *info) 2103 { 2104 mutex_unlock(&dpll_lock); 2105 } 2106 2107 int dpll_pin_pre_doit(const struct genl_split_ops *ops, struct sk_buff *skb, 2108 struct genl_info *info) 2109 { 2110 int ret; 2111 2112 mutex_lock(&dpll_lock); 2113 if (GENL_REQ_ATTR_CHECK(info, DPLL_A_PIN_ID)) { 2114 ret = -EINVAL; 2115 goto unlock_dev; 2116 } 2117 info->user_ptr[0] = xa_load(&dpll_pin_xa, 2118 nla_get_u32(info->attrs[DPLL_A_PIN_ID])); 2119 if (!info->user_ptr[0] || 2120 !dpll_pin_available(info->user_ptr[0])) { 2121 NL_SET_ERR_MSG(info->extack, "pin not found"); 2122 ret = -ENODEV; 2123 goto unlock_dev; 2124 } 2125 2126 return 0; 2127 2128 unlock_dev: 2129 mutex_unlock(&dpll_lock); 2130 return ret; 2131 } 2132 2133 void dpll_pin_post_doit(const struct genl_split_ops *ops, struct sk_buff *skb, 2134 struct genl_info *info) 2135 { 2136 mutex_unlock(&dpll_lock); 2137 } 2138