drivers/base/swnode.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Software nodes for the firmware node framework.
   4  *
   5  * Copyright (C) 2018, Intel Corporation
   6  * Author: Heikki Krogerus <heikki.krogerus@linux.intel.com>
   7  */
   8
   9 #include <linux/device.h>
  10 #include <linux/kernel.h>
  11 #include <linux/property.h>
  12 #include <linux/slab.h>
  13
  14 struct software_node {
  15         int id;
  16         struct kobject kobj;
  17         struct fwnode_handle fwnode;
  18
  19         /* hierarchy */
  20         struct ida child_ids;
  21         struct list_head entry;
  22         struct list_head children;
  23         struct software_node *parent;
  24
  25         /* properties */
  26         const struct property_entry *properties;
  27 };
  28
  29 static DEFINE_IDA(swnode_root_ids);
  30 static struct kset *swnode_kset;
  31
  32 #define kobj_to_swnode(_kobj_) container_of(_kobj_, struct software_node, kobj)
  33
  34 static const struct fwnode_operations software_node_ops;
  35
  36 bool 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 /* property_entry processing */
  53
  54 static const struct property_entry *
  55 property_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
  67 static void
  68 property_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
 106 static 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
 134 static 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
 151 static 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
 166 static 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
 181 static 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
 196 static 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
 211 static int
 212 property_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
 224 static 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
 246 static 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         /* Find out the array length. */
 255         prop = property_entry_get(props, propname);
 256         if (!prop)
 257                 return -EINVAL;
 258
 259         if (prop->is_array)
 260                 /* Find the length of an array. */
 261                 array_len = property_entry_count_elems_of_size(props, propname,
 262                                                           sizeof(const char *));
 263         else
 264                 /* The array length for a non-array string property is 1. */
 265                 array_len = 1;
 266
 267         /* Return how many there are if strings is NULL. */
 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
 283 static 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
 301 static 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
 326 static 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
 367 out_free_data:
 368         property_entry_free_data(dst);
 369         return -ENOMEM;
 370 }
 371
 372 /**
 373  * property_entries_dup - duplicate array of properties
 374  * @properties: array of properties to copy
 375  *
 376  * This function creates a deep copy of the given NULL-terminated array
 377  * of property entries.
 378  */
 379 struct property_entry *
 380 property_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 }
 405 EXPORT_SYMBOL_GPL(property_entries_dup);
 406
 407 /**
 408  * property_entries_free - free previously allocated array of properties
 409  * @properties: array of properties to destroy
 410  *
 411  * This function frees given NULL-terminated array of property entries,
 412  * along with their data.
 413  */
 414 void 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 }
 426 EXPORT_SYMBOL_GPL(property_entries_free);
 427
 428 /* -------------------------------------------------------------------------- */
 429 /* fwnode operations */
 430
 431 static 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
 440 static 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
 447 static 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
 454 static 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
 465 static 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
 475 struct fwnode_handle *
 476 software_node_get_parent(const struct fwnode_handle *fwnode)
 477 {
 478         struct software_node *swnode = to_software_node(fwnode);
 479
 480         return swnode->parent ? &swnode->parent->fwnode : NULL;
 481 }
 482
 483 struct fwnode_handle *
 484 software_node_get_next_child(const struct fwnode_handle *fwnode,
 485                              struct fwnode_handle *child)
 486 {
 487         struct software_node *p = to_software_node(fwnode);
 488         struct software_node *c = to_software_node(child);
 489
 490         if (list_empty(&p->children) ||
 491             (c && list_is_last(&c->entry, &p->children)))
 492                 return NULL;
 493
 494         if (c)
 495                 c = list_next_entry(c, entry);
 496         else
 497                 c = list_first_entry(&p->children, struct software_node, entry);
 498         return &c->fwnode;
 499 }
 500
 501
 502 static const struct fwnode_operations software_node_ops = {
 503         .get = software_node_get,
 504         .put = software_node_put,
 505         .property_present = software_node_property_present,
 506         .property_read_int_array = software_node_read_int_array,
 507         .property_read_string_array = software_node_read_string_array,
 508         .get_parent = software_node_get_parent,
 509         .get_next_child_node = software_node_get_next_child,
 510 };
 511
 512 /* -------------------------------------------------------------------------- */
 513
 514 static int
 515 software_node_register_properties(struct software_node *swnode,
 516                                   const struct property_entry *properties)
 517 {
 518         struct property_entry *props;
 519
 520         props = property_entries_dup(properties);
 521         if (IS_ERR(props))
 522                 return PTR_ERR(props);
 523
 524         swnode->properties = props;
 525
 526         return 0;
 527 }
 528
 529 static void software_node_release(struct kobject *kobj)
 530 {
 531         struct software_node *swnode = kobj_to_swnode(kobj);
 532
 533         if (swnode->parent) {
 534                 ida_simple_remove(&swnode->parent->child_ids, swnode->id);
 535                 list_del(&swnode->entry);
 536         } else {
 537                 ida_simple_remove(&swnode_root_ids, swnode->id);
 538         }
 539
 540         ida_destroy(&swnode->child_ids);
 541         property_entries_free(swnode->properties);
 542         kfree(swnode);
 543 }
 544
 545 static struct kobj_type software_node_type = {
 546         .release = software_node_release,
 547         .sysfs_ops = &kobj_sysfs_ops,
 548 };
 549
 550 struct fwnode_handle *
 551 fwnode_create_software_node(const struct property_entry *properties,
 552                             const struct fwnode_handle *parent)
 553 {
 554         struct software_node *p = NULL;
 555         struct software_node *swnode;
 556         int ret;
 557
 558         if (parent) {
 559                 if (IS_ERR(parent))
 560                         return ERR_CAST(parent);
 561                 if (!is_software_node(parent))
 562                         return ERR_PTR(-EINVAL);
 563                 p = to_software_node(parent);
 564         }
 565
 566         swnode = kzalloc(sizeof(*swnode), GFP_KERNEL);
 567         if (!swnode)
 568                 return ERR_PTR(-ENOMEM);
 569
 570         ret = ida_simple_get(p ? &p->child_ids : &swnode_root_ids, 0, 0,
 571                              GFP_KERNEL);
 572         if (ret < 0) {
 573                 kfree(swnode);
 574                 return ERR_PTR(ret);
 575         }
 576
 577         swnode->id = ret;
 578         swnode->kobj.kset = swnode_kset;
 579         swnode->fwnode.ops = &software_node_ops;
 580
 581         ida_init(&swnode->child_ids);
 582         INIT_LIST_HEAD(&swnode->entry);
 583         INIT_LIST_HEAD(&swnode->children);
 584         swnode->parent = p;
 585
 586         if (p)
 587                 list_add_tail(&swnode->entry, &p->children);
 588
 589         ret = kobject_init_and_add(&swnode->kobj, &software_node_type,
 590                                    p ? &p->kobj : NULL, "node%d", swnode->id);
 591         if (ret) {
 592                 kobject_put(&swnode->kobj);
 593                 return ERR_PTR(ret);
 594         }
 595
 596         ret = software_node_register_properties(swnode, properties);
 597         if (ret) {
 598                 kobject_put(&swnode->kobj);
 599                 return ERR_PTR(ret);
 600         }
 601
 602         kobject_uevent(&swnode->kobj, KOBJ_ADD);
 603         return &swnode->fwnode;
 604 }
 605 EXPORT_SYMBOL_GPL(fwnode_create_software_node);
 606
 607 void fwnode_remove_software_node(struct fwnode_handle *fwnode)
 608 {
 609         struct software_node *swnode = to_software_node(fwnode);
 610
 611         if (!swnode)
 612                 return;
 613
 614         kobject_put(&swnode->kobj);
 615 }
 616 EXPORT_SYMBOL_GPL(fwnode_remove_software_node);
 617
 618 int software_node_notify(struct device *dev, unsigned long action)
 619 {
 620         struct fwnode_handle *fwnode = dev_fwnode(dev);
 621         struct software_node *swnode;
 622         int ret;
 623
 624         if (!fwnode)
 625                 return 0;
 626
 627         if (!is_software_node(fwnode))
 628                 fwnode = fwnode->secondary;
 629         if (!is_software_node(fwnode))
 630                 return 0;
 631
 632         swnode = to_software_node(fwnode);
 633
 634         switch (action) {
 635         case KOBJ_ADD:
 636                 ret = sysfs_create_link(&dev->kobj, &swnode->kobj,
 637                                         "software_node");
 638                 if (ret)
 639                         break;
 640
 641                 ret = sysfs_create_link(&swnode->kobj, &dev->kobj,
 642                                         dev_name(dev));
 643                 if (ret) {
 644                         sysfs_remove_link(&dev->kobj, "software_node");
 645                         break;
 646                 }
 647                 kobject_get(&swnode->kobj);
 648                 break;
 649         case KOBJ_REMOVE:
 650                 sysfs_remove_link(&swnode->kobj, dev_name(dev));
 651                 sysfs_remove_link(&dev->kobj, "software_node");
 652                 kobject_put(&swnode->kobj);
 653                 break;
 654         default:
 655                 break;
 656         }
 657
 658         return 0;
 659 }
 660
 661 static int __init software_node_init(void)
 662 {
 663         swnode_kset = kset_create_and_add("software_nodes", NULL, kernel_kobj);
 664         if (!swnode_kset)
 665                 return -ENOMEM;
 666         return 0;
 667 }
 668 postcore_initcall(software_node_init);
 669
 670 static void __exit software_node_exit(void)
 671 {
 672         ida_destroy(&swnode_root_ids);
 673         kset_unregister(swnode_kset);
 674 }
 675 __exitcall(software_node_exit);