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