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