/* * Copyright (c) 2015-2016, NVIDIA CORPORATION. All rights reserved. * * Author: Laxman Dewangan * * This program is free software; you can redistribute it and/or modify it * under the terms and conditions of the GNU General Public License, * version 2, as published by the Free Software Foundation. * * This program is distributed in the hope it will be useful, but WITHOUT * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for * more details. */ #include #include #include #include #include #include #include #include "of_private.h" #define MAXIMUM_FNAME_LENGTH 300 enum plugin_manager_match_type { PLUGIN_MANAGER_MATCH_EXACT, PLUGIN_MANAGER_MATCH_PARTIAL, PLUGIN_MANAGER_MATCH_GE, PLUGIN_MANAGER_MATCH_LT, }; struct connection_info { int level; const char *uid_str; struct device_node *node; struct device_node *org_pm; struct device_node *copy_node; struct device_node *parent_conn_node; struct device_node *child_conn_node; }; static int link_connection_to_plugin_modules(struct device_node *plmroot, struct device_node *plcroot, struct device_node *ds, struct device_node *connector); static struct property *__of_copy_property(const struct property *prop, void *new_value, int val_len, gfp_t flags) { struct property *propn; int nlen; void *nval; propn = kzalloc(sizeof(*propn), flags); if (!propn) return NULL; propn->name = kstrdup(prop->name, flags); if (!propn->name) goto err_fail_name; nlen = (new_value) ? val_len : prop->length; nval = (new_value) ? new_value : prop->value; if (nlen > 0) { propn->value = kzalloc(nlen, flags); if (!propn->value) goto err_fail_value; memcpy(propn->value, nval, nlen); propn->length = nlen; } return propn; err_fail_value: kfree(propn->name); err_fail_name: kfree(propn); return NULL; } static struct property *__of_create_property_by_name(const char *name, void *new_value, int val_len) { struct property *propn; if (!name) return NULL; propn = kzalloc(sizeof(*propn), GFP_KERNEL); if (!propn) return NULL; propn->name = kstrdup(name, GFP_KERNEL); if (!propn->name) goto err_fail_name; if ((val_len > 0) || !new_value) { propn->value = kzalloc(val_len, GFP_KERNEL); if (!propn->value) goto err_fail_value; memcpy(propn->value, new_value, val_len); propn->length = val_len; } return propn; err_fail_value: kfree(propn->name); err_fail_name: kfree(propn); return NULL; } static void free_property(struct property *pp) { if (!pp) return; kfree(pp->name); kfree(pp->value); kfree(pp); } static struct device_node *of_get_child_by_last_name(struct device_node *node, const char *name) { struct device_node *child; for_each_child_of_node(node, child) { const char *lname = strrchr(child->full_name, '/'); if (!strcmp(lname + 1, name)) return child; } return NULL; } static struct device_node *of_get_nested_child_by_name(struct device_node *node, const char *name) { struct device_node *cnode = node; const char *cur_name = name; char child_name[100]; int nlen; int len = strlen(name); if (!len) return NULL; while (len) { nlen = strcspn(cur_name, "/"); if (!nlen) return NULL; memcpy(child_name, cur_name, nlen); child_name[nlen] = '\0'; cnode = of_get_child_by_last_name(cnode, child_name); if (!cnode) return NULL; /* '/' adjustment. */ nlen++; cur_name += nlen; if (len <= nlen) break; len -= nlen; } return cnode; } static void of_add_node_to_parent(struct device_node *parent, struct device_node *child) { struct device_node *last_sibling; child->sibling = NULL; child->parent = parent; if (!parent->child) { parent->child = child; return; } last_sibling = parent->child; while (last_sibling->sibling) last_sibling = last_sibling->sibling; last_sibling->sibling = child; } static int plugin_module_get_uid(void) { static atomic_t pm_uid = ATOMIC_INIT(-1); return atomic_inc_return(&pm_uid); } struct device_node *create_simple_device_node(const char *path, const char *add_name, size_t data_size) { struct device_node *new_np; new_np = kzalloc(sizeof(*new_np), GFP_KERNEL); if (!new_np) return NULL; new_np->full_name = kasprintf(GFP_KERNEL, "%s/%s", path, add_name); new_np->name = kasprintf(GFP_KERNEL, "%s", add_name); if (data_size) { new_np->data = kzalloc(data_size, GFP_KERNEL); if (!new_np->data) goto clean; } return new_np; clean: kfree(new_np->full_name); kfree(new_np->name); kfree(new_np); return NULL; } static void free_simple_device_node(struct device_node *np) { if (!np) return; kfree(np->full_name); kfree(np->name); kfree(np->data); kfree(np); } struct device_node *duplicate_single_node(struct device_node *np, const char *base_dir, const char *path, const char *new_name) { struct device_node *dup; struct property *pp, *new_pp; int ret; const char *add_name; char fname[MAXIMUM_FNAME_LENGTH + 1] = {}; dup = kzalloc(sizeof(*dup), GFP_KERNEL); if (!dup) return NULL; if (new_name) { add_name = new_name; } else { add_name = strrchr(np->full_name, '/'); add_name++; } if (path) { strncpy(fname, path, MAXIMUM_FNAME_LENGTH); } else { const char *lname = strrchr(np->full_name, '/'); int llen = strlen(np->full_name) - strlen(lname); strncpy(fname, np->full_name, MAXIMUM_FNAME_LENGTH); fname[llen] = '\0'; } if (base_dir) dup->full_name = kasprintf(GFP_KERNEL, "%s%s/%s", base_dir, fname, add_name); else dup->full_name = kasprintf(GFP_KERNEL, "%s/%s", fname, add_name); of_node_init(dup); for_each_property_of_node(np, pp) { if (!strcmp(pp->name, "name")) new_pp = __of_copy_property(pp, (void *)add_name, strlen(add_name), GFP_KERNEL); else new_pp = __of_copy_property(pp, NULL, 0, GFP_KERNEL); if (!new_pp) { kfree(dup->full_name); kfree(dup); return NULL; } ret = of_add_property(dup, new_pp); if (ret < 0) { pr_err("Prop %s can not be added on node %s\n", new_pp->name, dup->full_name); free_property(new_pp); kfree(dup->full_name); kfree(dup); return NULL; } } dup->name = __of_get_property(dup, "name", NULL) ? : ""; dup->type = __of_get_property(dup, "device_type", NULL) ? : ""; return dup; } struct device_node *get_copy_of_node(struct device_node *np, const char *base_dir, const char *path, const char *new_name) { struct device_node *dup; struct device_node *child, *child_dup; struct device_node *prev_child = NULL; dup = duplicate_single_node(np, base_dir, path, new_name); if (!dup) return NULL; for_each_child_of_node(np, child) { child_dup = get_copy_of_node(child, NULL, dup->full_name, NULL); if (!child_dup) { kfree(dup); return NULL; } child_dup->parent = dup; child_dup->sibling = NULL; if (!prev_child) dup->child = child_dup; else prev_child->sibling = child_dup; prev_child = child_dup; } return dup; } static struct device_node *add_module_connection(struct device_node *parent, struct device_node *pm_node, const char *child_name) { struct device_node *child; struct connection_info *cinfo; child = create_simple_device_node(parent->full_name, child_name, sizeof(*cinfo)); if (!child) { pr_info("Can not create device node %s\n", child_name); return NULL; } cinfo = child->data; cinfo->org_pm = pm_node; cinfo->node = child; of_add_node_to_parent(parent, child); return child; } static int of_get_next_phandle(void) { static phandle curr_handle; static bool first_time = true; struct device_node *np; phandle next_handle; unsigned long flags; raw_spin_lock_irqsave(&devtree_lock, flags); if (first_time) { for_each_of_allnodes(np) { if (np->phandle > curr_handle) curr_handle = np->phandle; } first_time = false; } next_handle = curr_handle++; raw_spin_unlock_irqrestore(&devtree_lock, flags); return next_handle; } static struct property *__of_string_append(struct device_node *target, struct property *prop) { struct property *new_prop, *tprop; const char *tprop_name, *curr_str; int slen, tlen, lenp; tprop_name = of_prop_next_string(prop, NULL); if (!tprop_name) return NULL; new_prop = kzalloc(sizeof(*new_prop), GFP_KERNEL); if (!new_prop) return NULL; new_prop->name = kstrdup(tprop_name, GFP_KERNEL); if (!new_prop->name) goto err_fail_name; curr_str = of_prop_next_string(prop, tprop_name); for (slen = 0; curr_str; curr_str = of_prop_next_string(prop, curr_str)) slen += strlen(curr_str); tprop = of_find_property(target, tprop_name, &lenp); tlen = (tprop) ? tprop->length : 0; new_prop->value = kmalloc(slen + tlen, GFP_KERNEL); if (!new_prop->value) goto err_fail_value; if (tlen) memcpy(new_prop->value, tprop->value, tlen); if (slen) { curr_str = of_prop_next_string(prop, tprop_name); memcpy(new_prop->value + tlen, curr_str, slen); } new_prop->length = slen + tlen; return new_prop; err_fail_value: kfree(new_prop->name); err_fail_name: kfree(new_prop); return NULL; } static int do_property_override_from_overlay(struct device_node *target, struct device_node *overlay) { struct property *prop; struct property *tprop; struct property *new_prop; const char *pval; int lenp = 0; int ret; pr_debug("Update properties from %s to %s\n", overlay->full_name, target->full_name); for_each_property_of_node(overlay, prop) { /* Skip those we do not want to proceed */ if (!strcmp(prop->name, "name") || !strcmp(prop->name, "phandle") || !strcmp(prop->name, "linux,phandle")) continue; if (!strcmp(prop->name, "delete-target-property")) { if (prop->length <= 0) continue; pval = (const char *)prop->value; pr_info("Removing Prop %s from target %s\n", pval, target->full_name); tprop = of_find_property(target, pval, &lenp); if (tprop) of_remove_property(target, tprop); continue; } if (!strcmp(prop->name, "append-string-property")) { if (prop->length <= 0) continue; new_prop = __of_string_append(target, prop); if (!new_prop) { pr_err("Prop %s can not be appended\n", of_prop_next_string(prop, NULL)); return -EINVAL; } goto add_prop; } new_prop = __of_copy_property(prop, NULL, 0, GFP_KERNEL); if (!new_prop) { pr_err("Prop %s can not be duplicated\n", prop->name); return -EINVAL; } add_prop: tprop = of_find_property(target, new_prop->name, &lenp); if (!tprop) { ret = of_add_property(target, new_prop); if (ret < 0) { pr_err("Prop %s can not be added on node %s\n", new_prop->name, target->full_name); goto cleanup; } } else { ret = of_update_property(target, new_prop); if (ret < 0) { pr_err("Prop %s can not be updated on node %s\n", new_prop->name, target->full_name); goto cleanup; } } } return 0; cleanup: free_property(new_prop); return ret; } static int plugin_manager_get_fabid(const char *id_str) { int fabid = 0; int id; int i; if (strlen(id_str) < 13) return -EINVAL; for (i = 0; i < 3; ++i) { id = id_str[10 + i]; switch (id) { case 48 ... 57: /* 0 to 9 */ id = id - 48; break; case 65 ... 90: /* A to Z */ id = id - 65 + 10; break; case 97 ... 122: /* a to z */ id = id - 97 + 10; break; default: return -EINVAL; } /* Make digit position to 100x to avoid carry */ fabid = fabid * 100 + id; } return fabid; } static bool plugin_manager_match_id(struct device_node *np, const char *id_name) { struct property *prop; const char *in_str = id_name; int match_type = PLUGIN_MANAGER_MATCH_EXACT; int valid_str_len = strlen(id_name); int fabid = 0, prop_fabid; int i; if ((valid_str_len > 2) && (in_str[0] == '>') && (in_str[1] == '=')) { in_str += 2; valid_str_len -= 2; match_type = PLUGIN_MANAGER_MATCH_GE; goto match_type_done; } if ((valid_str_len > 1) && (in_str[0] == '<')) { in_str += 1; valid_str_len -= 1; match_type = PLUGIN_MANAGER_MATCH_LT; goto match_type_done; } if ((valid_str_len > 1) && (in_str[0] == '^')) { in_str += 1; valid_str_len -= 1; match_type = PLUGIN_MANAGER_MATCH_PARTIAL; goto match_type_done; } for (i = 0; i < valid_str_len; ++i) { if (in_str[i] == '*') { valid_str_len = i; match_type = PLUGIN_MANAGER_MATCH_PARTIAL; break; } } match_type_done: if ((match_type == PLUGIN_MANAGER_MATCH_GE) || (match_type == PLUGIN_MANAGER_MATCH_LT)) { fabid = plugin_manager_get_fabid(in_str); if (fabid < 0) return false; } for_each_property_of_node(np, prop) { /* Skip those we do not want to proceed */ if (!strcmp(prop->name, "name") || !strcmp(prop->name, "phandle") || !strcmp(prop->name, "linux,phandle")) continue; switch (match_type) { case PLUGIN_MANAGER_MATCH_EXACT: if (strlen(prop->name) != valid_str_len) break; if (!memcmp(in_str, prop->name, valid_str_len)) return true; break; case PLUGIN_MANAGER_MATCH_PARTIAL: if (strlen(prop->name) < valid_str_len) break; if (!memcmp(in_str, prop->name, valid_str_len)) return true; break; case PLUGIN_MANAGER_MATCH_GE: case PLUGIN_MANAGER_MATCH_LT: if (strlen(prop->name) < 13) break; if (memcmp(in_str, prop->name, 10)) break; prop_fabid = plugin_manager_get_fabid(prop->name); if (prop_fabid < 0) break; if (prop_fabid >= fabid && match_type == PLUGIN_MANAGER_MATCH_GE) return true; if (prop_fabid < fabid && match_type == PLUGIN_MANAGER_MATCH_LT) return true; break; default: break; } } return false; } static int do_property_overrides(struct device_node *target, struct device_node *overlay) { struct device_node *tchild, *ochild; const char *address_name; int ret; ret = do_property_override_from_overlay(target, overlay); if (ret < 0) { pr_err("Target %s update with overlay %s failed: %d\n", target->name, overlay->name, ret); return ret; } for_each_child_of_node(overlay, ochild) { address_name = strrchr(ochild->full_name, '/'); tchild = of_get_child_by_last_name(target, address_name + 1); if (!tchild) { pr_err("Overlay node %s not found in target node %s\n", ochild->full_name, target->full_name); continue; } ret = do_property_overrides(tchild, ochild); if (ret < 0) { pr_err("Target %s update with overlay %s failed: %d\n", tchild->name, ochild->name, ret); return ret; } } return 0; } static int handle_properties_overrides(struct device_node *np, struct device_node *target) { struct device_node *overlay; int ret; if (!target) { target = of_parse_phandle(np, "target", 0); if (!target) { pr_err("Node %s does not have targer node\n", np->name); return -EINVAL; } } overlay = of_get_child_by_name(np, "_overlay_"); if (!overlay) { pr_err("Node %s does not have Overlay\n", np->name); return -EINVAL; } ret = do_property_overrides(target, overlay); if (ret < 0) { pr_err("Target %s update with overlay %s failed: %d\n", target->name, overlay->name, ret); return -EINVAL; } return 0; } static int __init plugin_manager(struct device_node *np) { struct device_node *board_np, *nct_np, *odm_np, *cnp; struct device_node *config_np, *chip_np; const char *bname; struct property *prop; int board_count; int odm_count, nct_count, chip_id_count; int cname_count, cval_count; int nchild; bool found = false; bool override_on_all_match; int ret; override_on_all_match = of_property_read_bool(np, "enable-override-on-all-matches"); cname_count = of_property_count_strings(np, "config-names"); cval_count = of_property_count_u32_elems(np, "configs"); if (cname_count != cval_count) { pr_err("Node %s does not have config-names and configs\n", np->name); return -EINVAL; } board_count = of_property_count_strings(np, "ids"); odm_count = of_property_count_strings(np, "odm-data"); nct_count = of_property_count_strings(np, "nct-data"); chip_id_count = of_property_count_strings(np, "chip-id"); if ((board_count <= 0) && (odm_count <= 0) && (cname_count <= 0) && (nct_count <= 0) && (chip_id_count <= 0)) { pr_err("Node %s does not have property ids, nct and odm data\n", np->name); return -EINVAL; } nchild = of_get_child_count(np); if (!nchild) { pr_err("Node %s does not have Overlay child\n", np->name); return -EINVAL; } /* Match the IDs or odm data */ board_np = of_find_node_by_path("/chosen/plugin-manager/ids"); odm_np = of_find_node_by_path("/chosen/plugin-manager/odm-data"); nct_np = of_find_node_by_path("/chosen/plugin-manager/nct-data"); chip_np = of_find_node_by_path("/chosen/plugin-manager/chip-id"); config_np = of_find_node_by_path("/chosen/plugin-manager/configs"); if (!board_np && !odm_np && !config_np && !nct_np && !chip_np) { pr_err("chosen/plugin-manager does'nt have ids, nct and odm-data\n"); return -EINVAL; } if ((board_count > 0) && board_np) { of_property_for_each_string(np, "ids", prop, bname) { found = plugin_manager_match_id(board_np, bname); if (found) { pr_info("node %s match with board %s\n", np->full_name, bname); if (override_on_all_match) break; goto search_done; } } if (override_on_all_match && !found) return 0; } if ((odm_count > 0) && odm_np) { bool is_anded_odm_overrides; is_anded_odm_overrides = of_property_read_bool(np, "odm-anded-override"); of_property_for_each_string(np, "odm-data", prop, bname) { found = of_property_read_bool(odm_np, bname); if (found) { pr_info("node %s match with odm-data %s\n", np->full_name, bname); if (is_anded_odm_overrides) continue; if (override_on_all_match) break; goto search_done; } else { if (is_anded_odm_overrides) return 0; } } if (override_on_all_match && !found) return 0; if (!override_on_all_match) goto search_done; } if ((nct_count > 0) && nct_np) { of_property_for_each_string(np, "nct-data", prop, bname) { found = of_property_read_bool(nct_np, bname); if (found) { pr_info("node %s match with nct-data %s\n", np->full_name, bname); if (override_on_all_match) break; goto search_done; } } if (override_on_all_match && !found) return 0; } if ((chip_id_count > 0) && chip_np) { of_property_for_each_string(np, "chip-id", prop, bname) { found = of_property_read_bool(chip_np, bname); if (found) { pr_info("node %s match with chip-id %s\n", np->full_name, bname); if (override_on_all_match) break; goto search_done; } } if (override_on_all_match && !found) return 0; } if ((cname_count > 0) && config_np) { int index = 0; u32 pval = 0, pmv = 0, mask, value; of_property_for_each_string(np, "config-names", prop, bname) { ret = of_property_read_u32_index(np, "configs", index, &pmv); if (ret < 0) { pr_info("node %s do not have proper configs\n", np->name); return ret; } index++; ret = of_property_read_u32(config_np, bname, &pval); if (ret < 0) continue; mask = (pmv >> 8) & 0xFF; value = pmv & 0xFF; pval &= 0xFF; found = ((pval & mask) == value); if (found) { pr_info("node %s match with config %s\n", np->full_name, bname); if (override_on_all_match) break; goto search_done; } } if (override_on_all_match && !found) return 0; } search_done: if (!found) return 0; for_each_child_of_node(np, cnp) handle_properties_overrides(cnp, NULL); return 0; } static int get_node_address(const struct device_node *np) { char *name = strrchr(np->full_name, '@'); int addr = 0; if (!name) return 0; name++; if (*name == '0') addr = memparse(name, &name); return addr; } static struct device_node *plugin_module_get_node_by_path( struct device_node *pm_node, const char *rpath) { struct connection_info *cinfo = pm_node->data; const char *end_str, *pdev_str, *dev_path; struct device_node *cnode = pm_node; char child_name[100]; char *nfpath; int nlen, ret; bool uuid; if (!((rpath[0] == '.' && rpath[1] == '/'))) return NULL; pdev_str = rpath + 2; end_str = pdev_str + strlen(pdev_str); while (pdev_str < end_str) { uuid = false; nlen = strcspn(pdev_str, "/"); if (!nlen) return NULL; if (of_property_read_bool(cnode, "make-unique-node-name")) { snprintf(child_name, 100, "%s_%s", cinfo->uid_str, pdev_str); child_name[strlen(cinfo->uid_str) + nlen + 1] = '\0'; uuid = true; } else { memcpy(child_name, pdev_str, nlen); child_name[nlen] = '\0'; } pdev_str += nlen + 1; cnode = of_get_child_by_name(cnode, child_name); if (!cnode) break; if (!of_property_read_bool(cnode, "PM-RELOCATED")) continue; if (of_property_read_bool(cnode, "make-unique-node-name")) uuid = true; ret = of_property_read_string(cnode, "device-path", &dev_path); if (ret < 0) return NULL; if (uuid) nfpath = kasprintf(GFP_KERNEL, "%s/%s_%s", dev_path, cinfo->uid_str, pdev_str); else nfpath = kasprintf(GFP_KERNEL, "%s/%s", dev_path, pdev_str); if (!nfpath) return NULL; cnode = of_find_node_by_path(nfpath); kfree(nfpath); break; } return cnode; } static struct property *plugin_module_get_property_by_path_name( struct device_node *pm_node, const char *pname) { struct property *prop = NULL; struct device_node *pnode; char *path, *prop_name; int len; path = kasprintf(GFP_KERNEL, "%s", pname); if (!path) return NULL; prop_name = strrchr(path, '/'); len = strlen(path) - strlen(prop_name); path[len] = '\0'; prop_name++; pnode = plugin_module_get_node_by_path(pm_node, path); if (!pnode) goto end; prop = of_find_property(pnode, prop_name, NULL); end: kfree(path); return prop; } static struct property *plugin_module_create_property_to_path( struct device_node *pm_node, struct property *ref_prop, const char *pname) { struct property *prop = NULL; struct device_node *pnode; char *path, *prop_name; void *prop_value = (ref_prop) ? ref_prop->value : NULL; int prop_len = (ref_prop) ? ref_prop->length : 0; int len; int ret; path = kasprintf(GFP_KERNEL, "%s", pname); if (!path) return NULL; prop_name = strrchr(path, '/'); len = strlen(path) - strlen(prop_name); path[len] = '\0'; prop_name++; pnode = plugin_module_get_node_by_path(pm_node, path); if (!pnode) goto end; prop = __of_create_property_by_name(prop_name, prop_value, prop_len); if (!prop) goto end; ret = of_add_property(pnode, prop); if (ret < 0) { pr_err("Prop %s can not be added on node %s\n", prop->name, pnode->full_name); free_property(prop); prop = NULL; } end: kfree(path); return prop; } static void plugin_module_resolve_uid(struct device_node *cnp) { struct connection_info *cinfo = cnp->data; struct device_node *np = cinfo->copy_node; struct device_node *funcs, *sub_funcs, *child; struct property *name_prop; char *lname, *nname, *fname, *nfname; const void *ovalue; int len; int uid; funcs = of_get_child_by_name(np, "functions"); if (!funcs) return; uid = plugin_module_get_uid(); cinfo->uid_str = kasprintf(GFP_KERNEL, "PM%03d", uid); if (!cinfo->uid_str) return; fname = kzalloc(MAXIMUM_FNAME_LENGTH + 1, GFP_KERNEL); if (!fname) return; for_each_child_of_node(funcs, sub_funcs) { if (!of_property_read_bool(sub_funcs, "make-unique-node-name")) continue; for_each_child_of_node(sub_funcs, child) { ovalue = child->full_name; snprintf(fname, MAXIMUM_FNAME_LENGTH, "%s", child->full_name); lname = strrchr(fname, '/'); len = strlen(fname) - strlen(lname); fname[len] = '\0'; nfname = kasprintf(GFP_KERNEL, "%s/%s_%s", fname, cinfo->uid_str, lname + 1); if (!nfname) { pr_err("Failed to resolve full name\n"); continue; } child->full_name = nfname; kfree(ovalue); name_prop = of_find_property(child, "name", NULL); if (!name_prop) { child->name = ""; continue; } ovalue = name_prop->value; len = strlen(cinfo->uid_str) + strlen(child->name) + 2; nname = kzalloc(len, GFP_KERNEL); if (!nname) { pr_err("Failed to create UID name\n"); continue; } snprintf(nname, len, "%s_%s", cinfo->uid_str, child->name); name_prop->value = nname; name_prop->length = len; kfree(ovalue); child->name = nname; pr_debug("Resolve the unique ID of node %s\n", child->full_name); } } kfree(fname); } static int create_dup_nodes_for_connected_plm(struct device_node *cnp) { struct connection_info *cinfo = cnp->data; struct device_node *child; struct device_node *np = NULL; int ret; if (cinfo) np = cinfo->org_pm; if (np) { cinfo->copy_node = get_copy_of_node(np, NULL, NULL, NULL); if (!cinfo->copy_node) { pr_err("Failed to copy node %s\n", np->full_name); return -ENOMEM; } /* Resolve the UID */ plugin_module_resolve_uid(cnp); } for_each_child_of_node(cnp, child) { ret = create_dup_nodes_for_connected_plm(child); if (ret < 0) return ret; } return 0; } static int link_module_node_to_connector_node(struct device_node *cnp) { struct connection_info *cinfo = cnp->data; struct device_node *connector, *module; struct device_node *ds, *us, *ds_con; struct connection_info *cdata, *mdata; if (!cinfo->copy_node) return 0; ds = of_get_child_by_name(cinfo->copy_node, "downstream"); if (!ds) { pr_debug("There is no do downstream connectors: %s\n", cinfo->copy_node->full_name); return 0; } for_each_child_of_node(cnp, connector) { ds_con = of_get_child_by_name(ds, connector->name); if (!ds_con) { pr_err("Node %s does not have connector %s\n", ds->full_name, connector->name); continue; } module = connector->child; mdata = module->data; cdata = connector->data; if (!mdata->copy_node) { pr_debug("Module %s does not have plug-in node\n", module->name); continue; } us = of_get_child_by_name(mdata->copy_node, "upstream"); if (!us) { pr_err("Module %s does not have upstream node\n", module->name); continue; } mdata->parent_conn_node = ds_con; cdata->child_conn_node = us; link_module_node_to_connector_node(module); } return 0; } static void update_new_path_node_and_children(struct device_node *np, const char *new_path) { struct device_node *child; const char *new_fname, *lname; lname = strrchr(np->full_name, '/'); new_fname = kasprintf(GFP_KERNEL, "%s%s", new_path, lname); kfree(np->full_name); np->full_name = new_fname; __of_attach_node_sysfs(np); for_each_child_of_node(np, child) update_new_path_node_and_children(child, np->full_name); } static int process_sub_devs_copy(struct device_node *pm_node, struct device_node *sub_dev) { struct device_node *us_dev_np, *bus = NULL; struct device_node *dev_child, *child, *next_child; struct property *nprop; const char *dev_str; int ret; char path[200]; char prop_name[50]; const char *sp; const char *us_dev; unsigned long flags; int llen; ret = of_property_read_string(sub_dev, "device-path", &dev_str); if (ret < 0) { pr_err("device-path property not found in node %s\n", sub_dev->full_name); return ret; } memset(path, 0, 200); memcpy(path, dev_str, strlen(dev_str)); if ((dev_str[0] == '.' && dev_str[1] == '/')) { sp = strrchr(dev_str, '/'); memset(prop_name, 0, 50); memcpy(prop_name, sp + 1, strlen(sp)); llen = strlen(dev_str) - strlen(sp); path[llen] = '\0'; us_dev_np = of_get_nested_child_by_name(pm_node, path + 2); if (!us_dev_np) { pr_err("Node %s not found at %s\n", path + 2, pm_node->full_name); return -EINVAL; } ret = of_property_read_string(us_dev_np, prop_name, &us_dev); if (ret < 0) { pr_err("Node %s does not have prop %s\n\n", us_dev_np->full_name, prop_name); return ret; } bus = of_find_node_by_path(us_dev); } else if (dev_str[0] == '/') { bus = of_find_node_by_path(dev_str); } else { pr_err("device-path %s (%d) not processed: %s\n", dev_str, (int)strlen(dev_str), sub_dev->full_name); return 0; } if (!bus) { pr_err("Node path %s not found\n", us_dev); return ret; } dev_child = sub_dev->child; mutex_lock(&of_mutex); raw_spin_lock_irqsave(&devtree_lock, flags); child = sub_dev->child; while (child) { next_child = child->sibling; of_add_node_to_parent(bus, child); pr_debug("Added child %s to bus %s\n", child->full_name, bus->full_name); child = next_child; } raw_spin_unlock_irqrestore(&devtree_lock, flags); while (dev_child) { update_new_path_node_and_children(dev_child, bus->full_name); dev_child = dev_child->sibling; } mutex_unlock(&of_mutex); nprop = __of_create_property_by_name("PM-RELOCATED", NULL, 0); if (!nprop) return -ENOMEM; ret = of_add_property(sub_dev, nprop); if (ret < 0) { pr_err("Prop %s can not be added on node %s\n", nprop->name, sub_dev->full_name); return ret; } return 0; } static int ops_copy_property_value(struct device_node *np, struct device_node *ops) { const char *sprop_str, *tprop_str; struct property *sprop, *tprop; int ret; ret = of_property_read_string(ops, "source-property", &sprop_str); if (ret < 0) { pr_err("Failed to read source-property @%s\n", ops->full_name); return ret; } ret = of_property_read_string(ops, "target-property", &tprop_str); if (ret < 0) { pr_err("Failed to read target-property @%s\n", ops->full_name); return ret; } sprop = plugin_module_get_property_by_path_name(np, sprop_str); if (!sprop) { pr_err("Failed to get property %s:%s\n", sprop_str, ops->full_name); return -EINVAL; } tprop = plugin_module_get_property_by_path_name(np, tprop_str); if (!tprop) { plugin_module_create_property_to_path(np, sprop, tprop_str); return 0; } if (!sprop->length || !sprop->value) { pr_err("No value for source %s\n", sprop_str); return -EINVAL; } if (sprop->length == tprop->length) { memcpy(tprop->value, sprop->value, tprop->length); } else { void *oval = tprop->value; void *nval = kzalloc(sprop->length, GFP_KERNEL); if (!nval) return -ENOMEM; tprop->value = nval; memcpy(tprop->value, sprop->value, sprop->length); tprop->length = sprop->length; kfree(oval); } return 0; } static int ops_resolve_property_value(struct device_node *np, struct device_node *ops) { struct device_node *ref_node; const char *tprop_str; struct property *sprop, *tprop; void *oval, *nval; int ret, len; ret = of_property_read_string(ops, "target-property", &tprop_str); if (ret < 0) { pr_err("Failed to read target-property @%s\n", ops->full_name); return ret; } tprop = plugin_module_get_property_by_path_name(np, tprop_str); if (!tprop) { pr_err("Failed to get Property %s at %s\n", tprop_str, np->full_name); return -EINVAL; } ref_node = plugin_module_get_node_by_path(np, tprop->value); if (!ref_node) { pr_debug("Failed to get node %s @%s\n", (char *)tprop->value, ops->full_name); goto try_for_property; } len = strlen(ref_node->full_name); nval = kzalloc(len + 2, GFP_KERNEL); if (!nval) { pr_err("Memory allocation failed %s\n", ops->full_name); return -ENOMEM; } memcpy(nval, ref_node->full_name, len); tprop->length = len + 2; goto copy_to_target; try_for_property: sprop = plugin_module_get_property_by_path_name(np, tprop->value); if (!sprop) { pr_err("Failed to get Property %s at %s\n", (char *)tprop->value, np->full_name); return -EINVAL; } if (sprop->length == tprop->length) { memcpy(tprop->value, sprop->value, sprop->length); return 0; } nval = kzalloc(sprop->length, GFP_KERNEL); if (!nval) return -ENOMEM; memcpy(nval, sprop->value, sprop->length); tprop->length = sprop->length; copy_to_target: oval = tprop->value; tprop->value = nval; kfree(oval); return 0; } static int ops_resolve_property_handle(struct device_node *np, struct device_node *ops) { const char *rnod_str, *tprop_str; struct property *tprop; struct device_node *ref_node; int ret; ret = of_property_read_string(ops, "target-property", &tprop_str); if (ret < 0) { pr_err("Failed to read target-property @%s\n", ops->full_name); return ret; } ret = of_property_read_string(ops, "reference-node", &rnod_str); if (ret < 0) { pr_err("Failed to read refernce-node @%s\n", ops->full_name); return ret; } ref_node = plugin_module_get_node_by_path(np, rnod_str); if (!ref_node) { pr_err("Failed to get node %s @%s\n", rnod_str, ops->full_name); return ret; } if (!ref_node->phandle) ref_node->phandle = of_get_next_phandle(); tprop = plugin_module_get_property_by_path_name(np, tprop_str); if (!tprop) { tprop = plugin_module_create_property_to_path(np, NULL, tprop_str); if (!tprop) { pr_err("Failed to create property %s\n", tprop_str); return -EINVAL; } tprop->value = kzalloc(8, GFP_KERNEL); if (!tprop->value) return -ENOMEM; tprop->length = 8; } *(uint32_t *)tprop->value = cpu_to_be32(ref_node->phandle); pr_debug("%s: %s and inode %s handle %u\n", __func__, ops->full_name, ref_node->full_name, ref_node->phandle); return 0; } static int process_sub_functons(struct device_node *pm_node, struct device_node *funcs) { struct device_node *ops; for_each_child_of_node(funcs, ops) { if (of_property_read_bool(ops, "resolve-property-value")) ops_resolve_property_value(pm_node, ops); else if (of_property_read_bool(ops, "copy-property-value")) ops_copy_property_value(pm_node, ops); else if (of_property_read_bool(ops, "resolve-property-handle")) ops_resolve_property_handle(pm_node, ops); else pr_err("Unidentified ops at %s\n", ops->full_name); } return 0; } static int copy_all_node_properties(struct device_node *dest, struct device_node *src) { struct property *sprop, *dprop; int ret; for_each_property_of_node(src, sprop) { if (!strcmp(sprop->name, "name") || !strcmp(sprop->name, "phandle") || !strcmp(sprop->name, "linux,phandle")) continue; dprop = of_find_property(dest, sprop->name, NULL); /* New property */ if (!dprop) { dprop = __of_copy_property(sprop, NULL, 0, GFP_KERNEL); if (!dprop) { pr_err("Prop %s can not be duplicated\n", sprop->name); continue; } ret = of_add_property(dest, dprop); if (ret < 0) { pr_err("Prop %s can not be added on node %s\n", dprop->name, dest->full_name); free_property(dprop); } continue; } /* Boolean property */ if (!sprop->length) { if (dprop->length) { kfree(dprop->value); dprop->value = NULL; dprop->length = 0; } continue; } /* Different length property */ if (dprop->length != sprop->length) { void *old_val = dprop->value; void *new_val = kzalloc(sprop->length, GFP_KERNEL); memcpy(new_val, sprop->value, sprop->length); dprop->value = new_val; dprop->length = sprop->length; kfree(old_val); continue; } /* Same length */ memcpy(dprop->value, sprop->value, sprop->length); } return 0; } static void map_module_connector_to_parent(struct device_node *module_us_np, struct device_node *ds_con_np) { struct device_node *ds_pins, *module_pins; const char *lname; for_each_child_of_node(ds_con_np, ds_pins) { lname = strrchr(ds_pins->full_name, '/'); lname++; module_pins = of_get_child_by_last_name(module_us_np, lname); if (!module_pins) { module_pins = get_copy_of_node(ds_pins, NULL, module_us_np->full_name, lname); of_add_node_to_parent(module_us_np, module_pins); continue; } copy_all_node_properties(module_pins, ds_pins); } } static void process_plugin_module_connections(struct device_node *cnp) { struct connection_info *cinfo = cnp->data; struct connection_info *cdata, *mdata; struct device_node *connector, *module; struct device_node *funcs, *sub_func; if (!cinfo->copy_node) return; if (!cinfo->copy_node->data) cinfo->copy_node->data = cinfo; /* Perform functons */ funcs = of_get_child_by_name(cinfo->copy_node, "functions"); if (!funcs) goto handle_ds_connection; for_each_child_of_node(funcs, sub_func) { if (of_property_read_bool(sub_func, "copy-subdevices")) process_sub_devs_copy(cinfo->copy_node, sub_func); else process_sub_functons(cinfo->copy_node, sub_func); } handle_ds_connection: /* Copy Downstream connectors to connected module's upstream */ for_each_child_of_node(cnp, connector) { module = connector->child; if (!module) continue; mdata = module->data; cdata = connector->data; if (mdata->parent_conn_node) { pr_debug("Copying node %s to %s\n", mdata->parent_conn_node->full_name, cdata->child_conn_node->full_name); map_module_connector_to_parent( cdata->child_conn_node, mdata->parent_conn_node); } process_plugin_module_connections(module); } } static struct device_node *connect_all_child_modules( struct device_node *plmroot, struct device_node *plcroot, struct device_node *module) { struct connection_info *cinfo; struct device_node *nplcroot = plcroot; struct device_node *child, *ds; struct property *prop; const char *module_id = NULL; const char *s; bool found; for_each_property_of_node(module, prop) { /* Skip those we do not want to proceed */ if (!strcmp(prop->name, "name") || !strcmp(prop->name, "phandle") || !strcmp(prop->name, "linux,phandle")) continue; /* Other property is module ID: Support only one module */ nplcroot = add_module_connection(plcroot, NULL, prop->name); if (!nplcroot) { pr_info("Not able to create %s\n", prop->name); return NULL; } module_id = prop->name; break; } if (!module_id) { pr_info("Module name not found at %s, parent %s\n", module->full_name, nplcroot->full_name); return NULL; } /* Get the plugin-module node whose asset-id match with module ID */ found = false; for_each_child_of_node(plmroot, child) { of_property_for_each_string(child, "asset-id", prop, s) { if (!strcmp(module_id, s)) { pr_debug("Asset matched ID %s:%s\n", module_id, child->full_name); cinfo = nplcroot->data; cinfo->org_pm = child; found = true; break; } } if (found) break; } if (!found) { pr_debug("Plugin module %s not found\n", module->full_name); return NULL; } /* Process for down stream connectors for this module */ ds = of_get_child_by_name(child, "downstream"); if (ds) { link_connection_to_plugin_modules(plmroot, nplcroot, ds, module); return nplcroot; } for_each_child_of_node(module, child) { nplcroot = connect_all_child_modules(plmroot, nplcroot, child); if (!nplcroot) { pr_info("Deadend found at %s\n", child->full_name); return NULL; } }; return nplcroot; } static int link_connection_to_plugin_modules(struct device_node *plmroot, struct device_node *plcroot, struct device_node *ds, struct device_node *connector) { struct device_node *ds_con, *module, *new_connector; struct device_node *nplcroot; const char *s, *con_name; int ret, naddr, i; int mod_add, add; bool found; for_each_child_of_node(ds, ds_con) { /* Match the connection bus with plug-in-module */ ret = of_property_read_string(ds_con, "identification-bus", &s); if (ret) continue; new_connector = NULL; if (*s == '/') { con_name = strrchr(connector->full_name, '/'); if (!strcmp(con_name, s)) new_connector = connector; } else if ((*s == '.') && (*(s + 1) == '/')) { new_connector = of_get_nested_child_by_name(connector, s + 2); } if (!new_connector) continue; naddr = of_property_count_u32_elems(ds_con, "identification-slave-add"); if (naddr <= 0) { nplcroot = add_module_connection(plcroot, NULL, ds_con->name); if (!nplcroot) return 0; for_each_child_of_node(new_connector, module) connect_all_child_modules(plmroot, nplcroot, module); continue; } nplcroot = NULL; for_each_child_of_node(new_connector, module) { found = false; mod_add = get_node_address(module); if (mod_add <= 0) continue; for (i = 0; i < naddr; ++i) { u32 pval; ret = of_property_read_u32_index( ds_con, "identification-slave-add", i, &pval); if (ret < 0) continue; add = pval; if (add == mod_add) found = true; } if (found) { if (!nplcroot) nplcroot = add_module_connection( plcroot, NULL, ds_con->name); if (!nplcroot) return 0; connect_all_child_modules(plmroot, nplcroot, module); } } } return 0; } static void connection_manager(void) { struct device_node *plmroot, *conroot; struct device_node *plmmods, *ds, *connector; struct device_node *plcroot; struct connection_info *cinfo; plmroot = of_find_node_by_path("/plugin-modules"); if (!plmroot) { pr_info("Plugin module not found\n"); return; } conroot = of_find_node_by_path("/chosen/plugin-manager/ids/connection"); if (!conroot) { pr_info("chosen/conenction not found\n"); return; } plcroot = create_simple_device_node("/", "plugin-connection", sizeof(*cinfo)); if (!plcroot) { pr_info("Failed to create node /plugin-connection\n"); return; } for_each_child_of_node(plmroot, plmmods) { if (of_property_read_bool(plmmods, "the-root-base-board")) { cinfo = plcroot->data; cinfo->node = plcroot; cinfo->org_pm = plmmods; break; } } /* Create connection tree based on module connected */ for_each_child_of_node(conroot, connector) { for_each_child_of_node(plmroot, plmmods) { ds = of_get_child_by_name(plmmods, "downstream"); if (!ds) continue; link_connection_to_plugin_modules(plmroot, plcroot, ds, connector); } } /* Duplicate plug-in module nodes to each of modules for override */ create_dup_nodes_for_connected_plm(plcroot); /* Link modules node to connector node */ link_module_node_to_connector_node(plcroot); /* Process upstream, device and downstream nodes */ process_plugin_module_connections(plcroot); free_simple_device_node(plcroot); } static int __init plugin_manager_init(void) { struct device_node *pm_node; struct device_node *child; int ret; pr_info("Initializing plugin-manager\n"); connection_manager(); pm_node = of_find_node_by_path("/plugin-manager"); if (!pm_node) { pr_info("Plugin-manager not available\n"); return 0; } if (!of_device_is_available(pm_node)) { pr_info("Plugin-manager status disabled\n"); return 0; } for_each_available_child_of_node(pm_node, child) { ret = plugin_manager(child); if (ret < 0) pr_err("Error in parsing node %s: %d\n", child->full_name, ret); } return 0; } core_initcall(plugin_manager_init);