1
2
3
4
5
6
7
8
9#include <linux/device.h>
10#include <linux/kernel.h>
11#include <linux/property.h>
12#include <linux/slab.h>
13
14struct software_node {
15 int id;
16 struct kobject kobj;
17 struct fwnode_handle fwnode;
18
19
20 struct ida child_ids;
21 struct list_head entry;
22 struct list_head children;
23 struct software_node *parent;
24
25
26 const struct property_entry *properties;
27};
28
29static DEFINE_IDA(swnode_root_ids);
30static struct kset *swnode_kset;
31
32#define kobj_to_swnode(_kobj_) container_of(_kobj_, struct software_node, kobj)
33
34static const struct fwnode_operations software_node_ops;
35
36bool is_software_node(const struct fwnode_handle *fwnode)
37{
38 return !IS_ERR_OR_NULL(fwnode) && fwnode->ops == &software_node_ops;
39}
40
41#define to_software_node(__fwnode) \
42 ({ \
43 typeof(__fwnode) __to_software_node_fwnode = __fwnode; \
44 \
45 is_software_node(__to_software_node_fwnode) ? \
46 container_of(__to_software_node_fwnode, \
47 struct software_node, fwnode) : \
48 NULL; \
49 })
50
51
52
53
54static const struct property_entry *
55property_entry_get(const struct property_entry *prop, const char *name)
56{
57 if (!prop)
58 return NULL;
59
60 for (; prop->name; prop++)
61 if (!strcmp(name, prop->name))
62 return prop;
63
64 return NULL;
65}
66
67static void
68property_set_pointer(struct property_entry *prop, const void *pointer)
69{
70 switch (prop->type) {
71 case DEV_PROP_U8:
72 if (prop->is_array)
73 prop->pointer.u8_data = pointer;
74 else
75 prop->value.u8_data = *((u8 *)pointer);
76 break;
77 case DEV_PROP_U16:
78 if (prop->is_array)
79 prop->pointer.u16_data = pointer;
80 else
81 prop->value.u16_data = *((u16 *)pointer);
82 break;
83 case DEV_PROP_U32:
84 if (prop->is_array)
85 prop->pointer.u32_data = pointer;
86 else
87 prop->value.u32_data = *((u32 *)pointer);
88 break;
89 case DEV_PROP_U64:
90 if (prop->is_array)
91 prop->pointer.u64_data = pointer;
92 else
93 prop->value.u64_data = *((u64 *)pointer);
94 break;
95 case DEV_PROP_STRING:
96 if (prop->is_array)
97 prop->pointer.str = pointer;
98 else
99 prop->value.str = pointer;
100 break;
101 default:
102 break;
103 }
104}
105
106static const void *property_get_pointer(const struct property_entry *prop)
107{
108 switch (prop->type) {
109 case DEV_PROP_U8:
110 if (prop->is_array)
111 return prop->pointer.u8_data;
112 return &prop->value.u8_data;
113 case DEV_PROP_U16:
114 if (prop->is_array)
115 return prop->pointer.u16_data;
116 return &prop->value.u16_data;
117 case DEV_PROP_U32:
118 if (prop->is_array)
119 return prop->pointer.u32_data;
120 return &prop->value.u32_data;
121 case DEV_PROP_U64:
122 if (prop->is_array)
123 return prop->pointer.u64_data;
124 return &prop->value.u64_data;
125 case DEV_PROP_STRING:
126 if (prop->is_array)
127 return prop->pointer.str;
128 return &prop->value.str;
129 default:
130 return NULL;
131 }
132}
133
134static const void *property_entry_find(const struct property_entry *props,
135 const char *propname, size_t length)
136{
137 const struct property_entry *prop;
138 const void *pointer;
139
140 prop = property_entry_get(props, propname);
141 if (!prop)
142 return ERR_PTR(-EINVAL);
143 pointer = property_get_pointer(prop);
144 if (!pointer)
145 return ERR_PTR(-ENODATA);
146 if (length > prop->length)
147 return ERR_PTR(-EOVERFLOW);
148 return pointer;
149}
150
151static int property_entry_read_u8_array(const struct property_entry *props,
152 const char *propname,
153 u8 *values, size_t nval)
154{
155 const void *pointer;
156 size_t length = nval * sizeof(*values);
157
158 pointer = property_entry_find(props, propname, length);
159 if (IS_ERR(pointer))
160 return PTR_ERR(pointer);
161
162 memcpy(values, pointer, length);
163 return 0;
164}
165
166static int property_entry_read_u16_array(const struct property_entry *props,
167 const char *propname,
168 u16 *values, size_t nval)
169{
170 const void *pointer;
171 size_t length = nval * sizeof(*values);
172
173 pointer = property_entry_find(props, propname, length);
174 if (IS_ERR(pointer))
175 return PTR_ERR(pointer);
176
177 memcpy(values, pointer, length);
178 return 0;
179}
180
181static int property_entry_read_u32_array(const struct property_entry *props,
182 const char *propname,
183 u32 *values, size_t nval)
184{
185 const void *pointer;
186 size_t length = nval * sizeof(*values);
187
188 pointer = property_entry_find(props, propname, length);
189 if (IS_ERR(pointer))
190 return PTR_ERR(pointer);
191
192 memcpy(values, pointer, length);
193 return 0;
194}
195
196static int property_entry_read_u64_array(const struct property_entry *props,
197 const char *propname,
198 u64 *values, size_t nval)
199{
200 const void *pointer;
201 size_t length = nval * sizeof(*values);
202
203 pointer = property_entry_find(props, propname, length);
204 if (IS_ERR(pointer))
205 return PTR_ERR(pointer);
206
207 memcpy(values, pointer, length);
208 return 0;
209}
210
211static int
212property_entry_count_elems_of_size(const struct property_entry *props,
213 const char *propname, size_t length)
214{
215 const struct property_entry *prop;
216
217 prop = property_entry_get(props, propname);
218 if (!prop)
219 return -EINVAL;
220
221 return prop->length / length;
222}
223
224static int property_entry_read_int_array(const struct property_entry *props,
225 const char *name,
226 unsigned int elem_size, void *val,
227 size_t nval)
228{
229 if (!val)
230 return property_entry_count_elems_of_size(props, name,
231 elem_size);
232 switch (elem_size) {
233 case sizeof(u8):
234 return property_entry_read_u8_array(props, name, val, nval);
235 case sizeof(u16):
236 return property_entry_read_u16_array(props, name, val, nval);
237 case sizeof(u32):
238 return property_entry_read_u32_array(props, name, val, nval);
239 case sizeof(u64):
240 return property_entry_read_u64_array(props, name, val, nval);
241 }
242
243 return -ENXIO;
244}
245
246static int property_entry_read_string_array(const struct property_entry *props,
247 const char *propname,
248 const char **strings, size_t nval)
249{
250 const struct property_entry *prop;
251 const void *pointer;
252 size_t array_len, length;
253
254
255 prop = property_entry_get(props, propname);
256 if (!prop)
257 return -EINVAL;
258
259 if (prop->is_array)
260
261 array_len = property_entry_count_elems_of_size(props, propname,
262 sizeof(const char *));
263 else
264
265 array_len = 1;
266
267
268 if (!strings)
269 return array_len;
270
271 array_len = min(nval, array_len);
272 length = array_len * sizeof(*strings);
273
274 pointer = property_entry_find(props, propname, length);
275 if (IS_ERR(pointer))
276 return PTR_ERR(pointer);
277
278 memcpy(strings, pointer, length);
279
280 return array_len;
281}
282
283static void property_entry_free_data(const struct property_entry *p)
284{
285 const void *pointer = property_get_pointer(p);
286 size_t i, nval;
287
288 if (p->is_array) {
289 if (p->type == DEV_PROP_STRING && p->pointer.str) {
290 nval = p->length / sizeof(const char *);
291 for (i = 0; i < nval; i++)
292 kfree(p->pointer.str[i]);
293 }
294 kfree(pointer);
295 } else if (p->type == DEV_PROP_STRING) {
296 kfree(p->value.str);
297 }
298 kfree(p->name);
299}
300
301static int property_copy_string_array(struct property_entry *dst,
302 const struct property_entry *src)
303{
304 const char **d;
305 size_t nval = src->length / sizeof(*d);
306 int i;
307
308 d = kcalloc(nval, sizeof(*d), GFP_KERNEL);
309 if (!d)
310 return -ENOMEM;
311
312 for (i = 0; i < nval; i++) {
313 d[i] = kstrdup(src->pointer.str[i], GFP_KERNEL);
314 if (!d[i] && src->pointer.str[i]) {
315 while (--i >= 0)
316 kfree(d[i]);
317 kfree(d);
318 return -ENOMEM;
319 }
320 }
321
322 dst->pointer.str = d;
323 return 0;
324}
325
326static int property_entry_copy_data(struct property_entry *dst,
327 const struct property_entry *src)
328{
329 const void *pointer = property_get_pointer(src);
330 const void *new;
331 int error;
332
333 if (src->is_array) {
334 if (!src->length)
335 return -ENODATA;
336
337 if (src->type == DEV_PROP_STRING) {
338 error = property_copy_string_array(dst, src);
339 if (error)
340 return error;
341 new = dst->pointer.str;
342 } else {
343 new = kmemdup(pointer, src->length, GFP_KERNEL);
344 if (!new)
345 return -ENOMEM;
346 }
347 } else if (src->type == DEV_PROP_STRING) {
348 new = kstrdup(src->value.str, GFP_KERNEL);
349 if (!new && src->value.str)
350 return -ENOMEM;
351 } else {
352 new = pointer;
353 }
354
355 dst->length = src->length;
356 dst->is_array = src->is_array;
357 dst->type = src->type;
358
359 property_set_pointer(dst, new);
360
361 dst->name = kstrdup(src->name, GFP_KERNEL);
362 if (!dst->name)
363 goto out_free_data;
364
365 return 0;
366
367out_free_data:
368 property_entry_free_data(dst);
369 return -ENOMEM;
370}
371
372
373
374
375
376
377
378
379struct property_entry *
380property_entries_dup(const struct property_entry *properties)
381{
382 struct property_entry *p;
383 int i, n = 0;
384 int ret;
385
386 while (properties[n].name)
387 n++;
388
389 p = kcalloc(n + 1, sizeof(*p), GFP_KERNEL);
390 if (!p)
391 return ERR_PTR(-ENOMEM);
392
393 for (i = 0; i < n; i++) {
394 ret = property_entry_copy_data(&p[i], &properties[i]);
395 if (ret) {
396 while (--i >= 0)
397 property_entry_free_data(&p[i]);
398 kfree(p);
399 return ERR_PTR(ret);
400 }
401 }
402
403 return p;
404}
405EXPORT_SYMBOL_GPL(property_entries_dup);
406
407
408
409
410
411
412
413
414void property_entries_free(const struct property_entry *properties)
415{
416 const struct property_entry *p;
417
418 if (!properties)
419 return;
420
421 for (p = properties; p->name; p++)
422 property_entry_free_data(p);
423
424 kfree(properties);
425}
426EXPORT_SYMBOL_GPL(property_entries_free);
427
428
429
430
431static struct fwnode_handle *software_node_get(struct fwnode_handle *fwnode)
432{
433 struct software_node *swnode = to_software_node(fwnode);
434
435 kobject_get(&swnode->kobj);
436
437 return &swnode->fwnode;
438}
439
440static void software_node_put(struct fwnode_handle *fwnode)
441{
442 struct software_node *swnode = to_software_node(fwnode);
443
444 kobject_put(&swnode->kobj);
445}
446
447static bool software_node_property_present(const struct fwnode_handle *fwnode,
448 const char *propname)
449{
450 return !!property_entry_get(to_software_node(fwnode)->properties,
451 propname);
452}
453
454static int software_node_read_int_array(const struct fwnode_handle *fwnode,
455 const char *propname,
456 unsigned int elem_size, void *val,
457 size_t nval)
458{
459 struct software_node *swnode = to_software_node(fwnode);
460
461 return property_entry_read_int_array(swnode->properties, propname,
462 elem_size, val, nval);
463}
464
465static int software_node_read_string_array(const struct fwnode_handle *fwnode,
466 const char *propname,
467 const char **val, size_t nval)
468{
469 struct software_node *swnode = to_software_node(fwnode);
470
471 return property_entry_read_string_array(swnode->properties, propname,
472 val, nval);
473}
474
475static struct fwnode_handle *
476software_node_get_parent(const struct fwnode_handle *fwnode)
477{
478 struct software_node *swnode = to_software_node(fwnode);
479
480 return swnode ? (swnode->parent ? &swnode->parent->fwnode : NULL) :
481 NULL;
482}
483
484static struct fwnode_handle *
485software_node_get_next_child(const struct fwnode_handle *fwnode,
486 struct fwnode_handle *child)
487{
488 struct software_node *p = to_software_node(fwnode);
489 struct software_node *c = to_software_node(child);
490
491 if (!p || list_empty(&p->children) ||
492 (c && list_is_last(&c->entry, &p->children)))
493 return NULL;
494
495 if (c)
496 c = list_next_entry(c, entry);
497 else
498 c = list_first_entry(&p->children, struct software_node, entry);
499 return &c->fwnode;
500}
501
502static struct fwnode_handle *
503software_node_get_named_child_node(const struct fwnode_handle *fwnode,
504 const char *childname)
505{
506 struct software_node *swnode = to_software_node(fwnode);
507 const struct property_entry *prop;
508 struct software_node *child;
509
510 if (!swnode || list_empty(&swnode->children))
511 return NULL;
512
513 list_for_each_entry(child, &swnode->children, entry) {
514 prop = property_entry_get(child->properties, "name");
515 if (!prop)
516 continue;
517 if (!strcmp(childname, prop->value.str)) {
518 kobject_get(&child->kobj);
519 return &child->fwnode;
520 }
521 }
522 return NULL;
523}
524
525static const struct fwnode_operations software_node_ops = {
526 .get = software_node_get,
527 .put = software_node_put,
528 .property_present = software_node_property_present,
529 .property_read_int_array = software_node_read_int_array,
530 .property_read_string_array = software_node_read_string_array,
531 .get_parent = software_node_get_parent,
532 .get_next_child_node = software_node_get_next_child,
533 .get_named_child_node = software_node_get_named_child_node,
534};
535
536
537
538static int
539software_node_register_properties(struct software_node *swnode,
540 const struct property_entry *properties)
541{
542 struct property_entry *props;
543
544 props = property_entries_dup(properties);
545 if (IS_ERR(props))
546 return PTR_ERR(props);
547
548 swnode->properties = props;
549
550 return 0;
551}
552
553static void software_node_release(struct kobject *kobj)
554{
555 struct software_node *swnode = kobj_to_swnode(kobj);
556
557 if (swnode->parent) {
558 ida_simple_remove(&swnode->parent->child_ids, swnode->id);
559 list_del(&swnode->entry);
560 } else {
561 ida_simple_remove(&swnode_root_ids, swnode->id);
562 }
563
564 ida_destroy(&swnode->child_ids);
565 property_entries_free(swnode->properties);
566 kfree(swnode);
567}
568
569static struct kobj_type software_node_type = {
570 .release = software_node_release,
571 .sysfs_ops = &kobj_sysfs_ops,
572};
573
574struct fwnode_handle *
575fwnode_create_software_node(const struct property_entry *properties,
576 const struct fwnode_handle *parent)
577{
578 struct software_node *p = NULL;
579 struct software_node *swnode;
580 int ret;
581
582 if (parent) {
583 if (IS_ERR(parent))
584 return ERR_CAST(parent);
585 if (!is_software_node(parent))
586 return ERR_PTR(-EINVAL);
587 p = to_software_node(parent);
588 }
589
590 swnode = kzalloc(sizeof(*swnode), GFP_KERNEL);
591 if (!swnode)
592 return ERR_PTR(-ENOMEM);
593
594 ret = ida_simple_get(p ? &p->child_ids : &swnode_root_ids, 0, 0,
595 GFP_KERNEL);
596 if (ret < 0) {
597 kfree(swnode);
598 return ERR_PTR(ret);
599 }
600
601 swnode->id = ret;
602 swnode->kobj.kset = swnode_kset;
603 swnode->fwnode.ops = &software_node_ops;
604
605 ida_init(&swnode->child_ids);
606 INIT_LIST_HEAD(&swnode->entry);
607 INIT_LIST_HEAD(&swnode->children);
608 swnode->parent = p;
609
610 if (p)
611 list_add_tail(&swnode->entry, &p->children);
612
613 ret = kobject_init_and_add(&swnode->kobj, &software_node_type,
614 p ? &p->kobj : NULL, "node%d", swnode->id);
615 if (ret) {
616 kobject_put(&swnode->kobj);
617 return ERR_PTR(ret);
618 }
619
620 ret = software_node_register_properties(swnode, properties);
621 if (ret) {
622 kobject_put(&swnode->kobj);
623 return ERR_PTR(ret);
624 }
625
626 kobject_uevent(&swnode->kobj, KOBJ_ADD);
627 return &swnode->fwnode;
628}
629EXPORT_SYMBOL_GPL(fwnode_create_software_node);
630
631void fwnode_remove_software_node(struct fwnode_handle *fwnode)
632{
633 struct software_node *swnode = to_software_node(fwnode);
634
635 if (!swnode)
636 return;
637
638 kobject_put(&swnode->kobj);
639}
640EXPORT_SYMBOL_GPL(fwnode_remove_software_node);
641
642int software_node_notify(struct device *dev, unsigned long action)
643{
644 struct fwnode_handle *fwnode = dev_fwnode(dev);
645 struct software_node *swnode;
646 int ret;
647
648 if (!fwnode)
649 return 0;
650
651 if (!is_software_node(fwnode))
652 fwnode = fwnode->secondary;
653 if (!is_software_node(fwnode))
654 return 0;
655
656 swnode = to_software_node(fwnode);
657
658 switch (action) {
659 case KOBJ_ADD:
660 ret = sysfs_create_link(&dev->kobj, &swnode->kobj,
661 "software_node");
662 if (ret)
663 break;
664
665 ret = sysfs_create_link(&swnode->kobj, &dev->kobj,
666 dev_name(dev));
667 if (ret) {
668 sysfs_remove_link(&dev->kobj, "software_node");
669 break;
670 }
671 kobject_get(&swnode->kobj);
672 break;
673 case KOBJ_REMOVE:
674 sysfs_remove_link(&swnode->kobj, dev_name(dev));
675 sysfs_remove_link(&dev->kobj, "software_node");
676 kobject_put(&swnode->kobj);
677 break;
678 default:
679 break;
680 }
681
682 return 0;
683}
684
685static int __init software_node_init(void)
686{
687 swnode_kset = kset_create_and_add("software_nodes", NULL, kernel_kobj);
688 if (!swnode_kset)
689 return -ENOMEM;
690 return 0;
691}
692postcore_initcall(software_node_init);
693
694static void __exit software_node_exit(void)
695{
696 ida_destroy(&swnode_root_ids);
697 kset_unregister(swnode_kset);
698}
699__exitcall(software_node_exit);
700
