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