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