From 6fbab7eb4f942eea26206e4570d4073e71cf7205 Mon Sep 17 00:00:00 2001 From: manu Date: Thu, 23 May 2019 19:26:50 +0000 Subject: [PATCH] loader: Add pnp functions for autoloading modules based on linker.hints This adds some new commands to loader : - pnpmatch This takes a pnpinfo string as argument and tries to find a kernel module associated with it. -v and -d option are available and are the same as in devmatch (v is verbose, d dumps the hints). - pnpload This takes a pnpinfo string as argument and tries to load a kernel module associated with it. - pnpautoload This will attempt to load every kernel module for each buses. Each buses are probed, the probe function will generate pnpinfo string and load kernel module associated with it if it exists. Only simplebus for FDT system is implemented for now. Since we need the dtb and overlays to be applied before searching the tree fdt_devmatch_next will load and apply the dtb + overlays. All the pnp parsing code comes from devmatch and is the same at 99%. Reviewed by: imp, kevans Differential Revision: https://reviews.freebsd.org/D19498 --- stand/common/module.c | 507 ++++++++++++++++++++++++++++++++++++- stand/fdt/fdt_loader_cmd.c | 68 +++++ stand/fdt/fdt_platform.h | 3 + 3 files changed, 566 insertions(+), 12 deletions(-) diff --git a/stand/common/module.c b/stand/common/module.c index 970f2dbc27f..b4193c20380 100644 --- a/stand/common/module.c +++ b/stand/common/module.c @@ -39,6 +39,10 @@ __FBSDID("$FreeBSD$"); #include #include +#if defined(LOADER_FDT_SUPPORT) +#include +#endif + #include "bootstrap.h" #define MDIR_REMOVED 0x0001 @@ -52,18 +56,18 @@ struct moduledir { STAILQ_ENTRY(moduledir) d_link; }; -static int file_load(char *, vm_offset_t, struct preloaded_file **); -static int file_load_dependencies(struct preloaded_file *); -static char * file_search(const char *, char **); -static struct kernel_module *file_findmodule(struct preloaded_file *, char *, - struct mod_depend *); -static int file_havepath(const char *); -static char *mod_searchmodule(char *, struct mod_depend *); -static void file_insert_tail(struct preloaded_file *); -static void file_remove(struct preloaded_file *); -struct file_metadata *metadata_next(struct file_metadata *, int); -static void moduledir_readhints(struct moduledir *); -static void moduledir_rebuild(void); +static int file_load(char *filename, vm_offset_t dest, struct preloaded_file **result); +static int file_load_dependencies(struct preloaded_file *base_mod); +static char * file_search(const char *name, char **extlist); +static struct kernel_module * file_findmodule(struct preloaded_file *fp, char *modname, struct mod_depend *verinfo); +static int file_havepath(const char *name); +static char *mod_searchmodule(char *name, struct mod_depend *verinfo); +static char * mod_searchmodule_pnpinfo(const char *bus, const char *pnpinfo); +static void file_insert_tail(struct preloaded_file *mp); +static void file_remove(struct preloaded_file *fp); +struct file_metadata* metadata_next(struct file_metadata *base_mp, int type); +static void moduledir_readhints(struct moduledir *mdp); +static void moduledir_rebuild(void); /* load address should be tweaked by first module loaded (kernel) */ static vm_offset_t loadaddr = 0; @@ -344,6 +348,187 @@ command_lsmod(int argc, char *argv[]) return(CMD_OK); } +COMMAND_SET(pnpmatch, "pnpmatch", "list matched modules based on pnpinfo", command_pnpmatch); + +static int pnp_all_flag = 0; +static int pnp_dump_flag = 0; +static int pnp_unbound_flag = 0; +static int pnp_verbose_flag = 0; + +static int +command_pnpmatch(int argc, char *argv[]) +{ + char *module; + int ch; + + pnp_verbose_flag = 0; + pnp_dump_flag = 0; + optind = 1; + optreset = 1; + while ((ch = getopt(argc, argv, "vd")) != -1) { + switch(ch) { + case 'v': + pnp_verbose_flag = 1; + break; + case 'd': + pnp_dump_flag = 1; + break; + case '?': + default: + /* getopt has already reported an error */ + return(CMD_OK); + } + } + argv += (optind - 1); + argc -= (optind - 1); + + module = mod_searchmodule_pnpinfo(argv[1], argv[2]); + if (module) + printf("Matched module: %s\n", module); + else if(argv[1]) + printf("No module matches %s\n", argv[1]); + + return (CMD_OK); +} + +COMMAND_SET(pnpload, "pnpload", "load matched modules based on pnpinfo", command_pnpload); + +static int +command_pnpload(int argc, char *argv[]) +{ + char *module; + int ch, error; + + pnp_verbose_flag = 0; + pnp_dump_flag = 0; + optind = 1; + optreset = 1; + while ((ch = getopt(argc, argv, "vd")) != -1) { + switch(ch) { + case 'v': + pnp_verbose_flag = 1; + break; + case 'd': + pnp_dump_flag = 1; + break; + case '?': + default: + /* getopt has already reported an error */ + return(CMD_OK); + } + } + argv += (optind - 1); + argc -= (optind - 1); + + if (argc != 2) + return (CMD_ERROR); + + module = mod_searchmodule_pnpinfo(argv[1], argv[2]); + + error = mod_load(module, NULL, 0, NULL); + if (error == EEXIST) { + snprintf(command_errbuf, sizeof(command_errbuf), + "warning: module '%s' already loaded", argv[1]); + return (CMD_WARN); + } + + return (error == 0 ? CMD_OK : CMD_CRIT); +} + +#if defined(LOADER_FDT_SUPPORT) +static void +pnpautoload_simplebus(void) { + const char *pnpstring; + const char *compatstr; + char *pnpinfo = NULL; + char *module; + int tag = 0, len, pnplen; + int error; + + while (1) { + pnpstring = fdt_devmatch_next(&tag, &len); + if (pnpstring == NULL) + return; + + compatstr = pnpstring; + for (pnplen = 0; pnplen != len; compatstr = pnpstring + pnplen) { + pnplen += strlen(compatstr) + 1; + asprintf(&pnpinfo, "compat=%s", compatstr); + + module = mod_searchmodule_pnpinfo("simplebus", pnpinfo); + if (module) { + error = mod_loadkld(module, 0, NULL); + if (error) + printf("Cannot load module %s\n", module); + break; + } + } + free(pnpinfo); + free(module); + } +} +#endif + +struct pnp_bus { + const char *name; + void (*load)(void); +}; + +struct pnp_bus pnp_buses[] = { +#if defined(LOADER_FDT_SUPPORT) + {"simplebus", pnpautoload_simplebus}, +#endif +}; + +COMMAND_SET(pnpautoload, "pnpautoload", "auto load modules based on pnpinfo", command_pnpautoload); + +static int +command_pnpautoload(int argc, char *argv[]) +{ + int i; + int verbose; + int ch, error, match; + + pnp_verbose_flag = 0; + pnp_dump_flag = 0; + verbose = 0; + optind = 1; + optreset = 1; + match = 0; + while ((ch = getopt(argc, argv, "v")) != -1) { + switch(ch) { + case 'v': + verbose = 1; + break; + case '?': + default: + /* getopt has already reported an error */ + return(CMD_OK); + } + } + argv += (optind - 1); + argc -= (optind - 1); + + if (argc > 2) + return (CMD_ERROR); + + for (i = 0; i < nitems(pnp_buses); i++) { + if (argc == 2 && strcmp(argv[1], pnp_buses[i].name) != 0) { + if (verbose) + printf("Skipping bus %s\n", pnp_buses[i].name); + continue; + } + if (verbose) + printf("Autoloading modules for simplebus\n"); + pnp_buses[i].load(); + match = 1; + } + if (match == 0) + printf("Unsupported bus %s\n", argv[1]); + + return (CMD_OK); +} + /* * File level interface, functions file_* */ @@ -908,6 +1093,284 @@ mod_search_hints(struct moduledir *mdp, const char *modname, return result; } +static int +getint(void **ptr) +{ + int *p = *ptr; + int rv; + + p = (int *)roundup2((intptr_t)p, sizeof(int)); + rv = *p++; + *ptr = p; + return rv; +} + +static void +getstr(void **ptr, char *val) +{ + int *p = *ptr; + char *c = (char *)p; + int len = *(uint8_t *)c; + + memcpy(val, c + 1, len); + val[len] = 0; + c += len + 1; + *ptr = (void *)c; +} + +static int +pnpval_as_int(const char *val, const char *pnpinfo) +{ + int rv; + char key[256]; + char *cp; + + if (pnpinfo == NULL) + return -1; + + cp = strchr(val, ';'); + key[0] = ' '; + if (cp == NULL) + strlcpy(key + 1, val, sizeof(key) - 1); + else { + memcpy(key + 1, val, cp - val); + key[cp - val + 1] = '\0'; + } + strlcat(key, "=", sizeof(key)); + if (strncmp(key + 1, pnpinfo, strlen(key + 1)) == 0) + rv = strtol(pnpinfo + strlen(key + 1), NULL, 0); + else { + cp = strstr(pnpinfo, key); + if (cp == NULL) + rv = -1; + else + rv = strtol(cp + strlen(key), NULL, 0); + } + return rv; +} + +static void +quoted_strcpy(char *dst, const char *src) +{ + char q = ' '; + + if (*src == '\'' || *src == '"') + q = *src++; + while (*src && *src != q) + *dst++ = *src++; // XXX backtick quoting + *dst++ = '\0'; + // XXX overflow +} + +static char * +pnpval_as_str(const char *val, const char *pnpinfo) +{ + static char retval[256]; + char key[256]; + char *cp; + + if (pnpinfo == NULL) { + *retval = '\0'; + return retval; + } + + cp = strchr(val, ';'); + key[0] = ' '; + if (cp == NULL) + strlcpy(key + 1, val, sizeof(key) - 1); + else { + memcpy(key + 1, val, cp - val); + key[cp - val + 1] = '\0'; + } + strlcat(key, "=", sizeof(key)); + if (strncmp(key + 1, pnpinfo, strlen(key + 1)) == 0) + quoted_strcpy(retval, pnpinfo + strlen(key + 1)); + else { + cp = strstr(pnpinfo, key); + if (cp == NULL) + strcpy(retval, "MISSING"); + else + quoted_strcpy(retval, cp + strlen(key)); + } + return retval; +} + +static char * +devmatch_search_hints(struct moduledir *mdp, const char *bus, const char *dev, const char *pnpinfo) +{ + char val1[256], val2[256]; + int ival, len, ents, i, notme, mask, bit, v, found; + void *ptr, *walker, *hints_end; + char *lastmod = NULL, *cp, *s; + + moduledir_readhints(mdp); + found = 0; + if (mdp->d_hints == NULL) + goto bad; + walker = mdp->d_hints; + hints_end = walker + mdp->d_hintsz; + while (walker < hints_end && !found) { + len = getint(&walker); + ival = getint(&walker); + ptr = walker; + switch (ival) { + case MDT_VERSION: + getstr(&ptr, val1); + ival = getint(&ptr); + getstr(&ptr, val2); + if (pnp_dump_flag || pnp_verbose_flag) + printf("Version: if %s.%d kmod %s\n", val1, ival, val2); + break; + case MDT_MODULE: + getstr(&ptr, val1); + getstr(&ptr, val2); + if (lastmod) + free(lastmod); + lastmod = strdup(val2); + if (pnp_dump_flag || pnp_verbose_flag) + printf("module %s in %s\n", val1, val1); + break; + case MDT_PNP_INFO: + if (!pnp_dump_flag && !pnp_unbound_flag && lastmod && strcmp(lastmod, "kernel") == 0) + break; + getstr(&ptr, val1); + getstr(&ptr, val2); + ents = getint(&ptr); + if (pnp_dump_flag || pnp_verbose_flag) + printf("PNP info for bus %s format %s %d entries (%s)\n", + val1, val2, ents, lastmod); + if (strcmp(val1, "usb") == 0) { + if (pnp_verbose_flag) + printf("Treating usb as uhub -- bug in source table still?\n"); + strcpy(val1, "uhub"); + } + if (bus && strcmp(val1, bus) != 0) { + if (pnp_verbose_flag) + printf("Skipped because table for bus %s, looking for %s\n", + val1, bus); + break; + } + for (i = 0; i < ents; i++) { + if (pnp_verbose_flag) + printf("---------- Entry %d ----------\n", i); + if (pnp_dump_flag) + printf(" "); + cp = val2; + notme = 0; + mask = -1; + bit = -1; + do { + switch (*cp) { + /* All integer fields */ + case 'I': + case 'J': + case 'G': + case 'L': + case 'M': + ival = getint(&ptr); + if (pnp_dump_flag) { + printf("%#x:", ival); + break; + } + if (bit >= 0 && ((1 << bit) & mask) == 0) + break; + v = pnpval_as_int(cp + 2, pnpinfo); + if (pnp_verbose_flag) + printf("Matching %s (%c) table=%#x tomatch=%#x\n", + cp + 2, *cp, v, ival); + switch (*cp) { + case 'J': + if (ival == -1) + break; + /*FALLTHROUGH*/ + case 'I': + if (v != ival) + notme++; + break; + case 'G': + if (v < ival) + notme++; + break; + case 'L': + if (v > ival) + notme++; + break; + case 'M': + mask = ival; + break; + } + break; + /* String fields */ + case 'D': + case 'Z': + getstr(&ptr, val1); + if (pnp_dump_flag) { + printf("'%s':", val1); + break; + } + if (*cp == 'D') + break; + s = pnpval_as_str(cp + 2, pnpinfo); + if (strcmp(s, val1) != 0) + notme++; + break; + /* Key override fields, required to be last in the string */ + case 'T': + /* + * This is imperfect and only does one key and will be redone + * to be more general for multiple keys. Currently, nothing + * does that. + */ + if (pnp_dump_flag) /* No per-row data stored */ + break; + if (cp[strlen(cp) - 1] == ';') /* Skip required ; at end */ + cp[strlen(cp) - 1] = '\0'; /* in case it's not there */ + if ((s = strstr(pnpinfo, cp + 2)) == NULL) + notme++; + else if (s > pnpinfo && s[-1] != ' ') + notme++; + break; + default: + printf("Unknown field type %c\n:", *cp); + break; + } + bit++; + cp = strchr(cp, ';'); + if (cp) + cp++; + } while (cp && *cp); + if (pnp_dump_flag) + printf("\n"); + else if (!notme) { + if (!pnp_unbound_flag) { + if (pnp_verbose_flag) + printf("Matches --- %s ---\n", lastmod); + } + found++; + } + } + break; + default: + break; + } + walker = (void *)(len - sizeof(int) + (intptr_t)walker); + } + if (pnp_unbound_flag && found == 0 && *pnpinfo) { + if (pnp_verbose_flag) + printf("------------------------- "); + printf("%s on %s pnpinfo %s", *dev ? dev : "unattached", bus, pnpinfo); + if (pnp_verbose_flag) + printf(" -------------------------"); + printf("\n"); + } + if (found != 0) + return (lastmod); + free(lastmod); + +bad: + return (NULL); +} + /* * Attempt to locate the file containing the module (name) */ @@ -931,6 +1394,26 @@ mod_searchmodule(char *name, struct mod_depend *verinfo) return(result); } +static char * +mod_searchmodule_pnpinfo(const char *bus, const char *pnpinfo) +{ + struct moduledir *mdp; + char *result; + + moduledir_rebuild(); + /* + * Now we ready to lookup module in the given directories + */ + result = NULL; + STAILQ_FOREACH(mdp, &moduledir_list, d_link) { + result = devmatch_search_hints(mdp, bus, NULL, pnpinfo); + if (result) + break; + } + + return(result); +} + int file_addmodule(struct preloaded_file *fp, char *modname, int version, struct kernel_module **newmp) diff --git a/stand/fdt/fdt_loader_cmd.c b/stand/fdt/fdt_loader_cmd.c index ef6e59be6af..6253195b889 100644 --- a/stand/fdt/fdt_loader_cmd.c +++ b/stand/fdt/fdt_loader_cmd.c @@ -67,6 +67,8 @@ static struct fdt_header *fdt_to_load = NULL; static struct fdt_header *fdtp = NULL; /* Size of FDT blob */ static size_t fdtp_size = 0; +/* Have we loaded all the needed overlays */ +static int fdt_overlays_applied = 0; static int fdt_load_dtb(vm_offset_t va); static void fdt_print_overlay_load_error(int err, const char *filename); @@ -439,6 +441,9 @@ fdt_apply_overlays() if ((fdtp == NULL) || (fdtp_size == 0)) return; + if (fdt_overlays_applied) + return; + max_overlay_size = 0; for (fp = file_findfile(NULL, "dtbo"); fp != NULL; fp = fp->f_next) { if (max_overlay_size < fp->f_size) @@ -507,6 +512,7 @@ fdt_apply_overlays() fdtp_size = current_fdtp_size; } free(overlay); + fdt_overlays_applied = 1; } int @@ -1859,3 +1865,65 @@ fdt_cmd_nyi(int argc, char *argv[]) printf("command not yet implemented\n"); return (CMD_ERROR); } + +const char * +fdt_devmatch_next(int *tag, int *compatlen) +{ + const struct fdt_property *p; + const struct fdt_property *status; + int o, len = -1; + static int depth = 0; + + if (fdtp == NULL) { + fdt_setup_fdtp(); + fdt_apply_overlays(); + } + + if (*tag != 0) { + o = *tag; + /* We are at the end of the DTB */ + if (o < 0) + return (NULL); + } else { + o = fdt_path_offset(fdtp, "/"); + if (o < 0) { + printf("Can't find dtb\n"); + return (NULL); + } + depth = 0; + } + + /* Find the next node with a compatible property */ + while (1) { + p = NULL; + if (o >= 0 && depth >= 0) { + /* skip disabled nodes */ + status = fdt_get_property(fdtp, o, "status", &len); + if (len > 0) { + if (strcmp(status->data, "disabled") == 0) { + o = fdt_next_node(fdtp, o, &depth); + if (o < 0) /* End of tree */ + return (NULL); + continue; + } + } + + p = fdt_get_property(fdtp, o, "compatible", &len); + } + if (p) + break; + o = fdt_next_node(fdtp, o, &depth); + if (o < 0) /* End of tree */ + return (NULL); + } + + /* Prepare next node for next call */ + o = fdt_next_node(fdtp, o, &depth); + *tag = o; + + if (len >= 0) { + *compatlen = len; + return (p->data); + } + return (NULL); +} diff --git a/stand/fdt/fdt_platform.h b/stand/fdt/fdt_platform.h index bbc2fe0549c..439ebe13c2b 100644 --- a/stand/fdt/fdt_platform.h +++ b/stand/fdt/fdt_platform.h @@ -55,4 +55,7 @@ int fdt_platform_load_dtb(void); void fdt_platform_load_overlays(void); void fdt_platform_fixups(void); +/* Devmatch/pnp function */ +const char *fdt_devmatch_next(int *tag, int *compatlen); + #endif /* FDT_PLATFORM_H */ -- 2.45.0