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