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