drivers/dpll/dpll_netlink.c

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