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