Index: head/stand/common/module.c =================================================================== --- head/stand/common/module.c (revision 348193) +++ head/stand/common/module.c (revision 348194) @@ -1,1164 +1,1647 @@ /*- * Copyright (c) 1998 Michael Smith * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. */ #include __FBSDID("$FreeBSD$"); /* * file/module function dispatcher, support, etc. */ #include #include #include #include #include #include #include +#if defined(LOADER_FDT_SUPPORT) +#include +#endif + #include "bootstrap.h" #define MDIR_REMOVED 0x0001 #define MDIR_NOHINTS 0x0002 struct moduledir { char *d_path; /* path of modules directory */ u_char *d_hints; /* content of linker.hints file */ int d_hintsz; /* size of hints data */ int d_flags; 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; #if defined(LOADER_FDT_SUPPORT) static const char *default_searchpath = "/boot/kernel;/boot/modules;/boot/dtb"; #else static const char *default_searchpath = "/boot/kernel;/boot/modules"; #endif static STAILQ_HEAD(, moduledir) moduledir_list = STAILQ_HEAD_INITIALIZER(moduledir_list); struct preloaded_file *preloaded_files = NULL; static char *kld_ext_list[] = { ".ko", "", ".debug", NULL }; /* * load an object, either a disk file or code module. * * To load a file, the syntax is: * * load -t * * code modules are loaded as: * * load */ COMMAND_SET(load, "load", "load a kernel or module", command_load); static int command_load(int argc, char *argv[]) { struct preloaded_file *fp; char *typestr; char *prefix; char *skip; int dflag, dofile, dokld, ch, error; dflag = dokld = dofile = 0; optind = 1; optreset = 1; typestr = NULL; if (argc == 1) { command_errmsg = "no filename specified"; return (CMD_CRIT); } prefix = skip = NULL; while ((ch = getopt(argc, argv, "dkp:s:t:")) != -1) { switch(ch) { case 'd': dflag++; break; case 'k': dokld = 1; break; case 'p': prefix = optarg; break; case 's': skip = optarg; break; case 't': typestr = optarg; dofile = 1; break; case '?': default: /* getopt has already reported an error */ return (CMD_OK); } } argv += (optind - 1); argc -= (optind - 1); /* * Request to load a raw file? */ if (dofile) { if ((argc != 2) || (typestr == NULL) || (*typestr == 0)) { command_errmsg = "invalid load type"; return (CMD_CRIT); } #ifdef LOADER_VERIEXEC if (strncmp(typestr, "manifest", 8) == 0) { if (dflag > 0) ve_debug_set(dflag); return (load_manifest(argv[1], prefix, skip, NULL)); } #ifdef LOADER_VERIEXEC_PASS_MANIFEST if (strncmp(typestr, "pass_manifest", 13) == 0) { if (dflag > 0) ve_debug_set(dflag); return (pass_manifest(argv[1], prefix)); } #endif #endif fp = file_findfile(argv[1], typestr); if (fp) { snprintf(command_errbuf, sizeof(command_errbuf), "warning: file '%s' already loaded", argv[1]); return (CMD_WARN); } if (file_loadraw(argv[1], typestr, 1) != NULL) return (CMD_OK); /* Failing to load mfs_root is never going to end well! */ if (strcmp("mfs_root", typestr) == 0) return (CMD_FATAL); return (CMD_ERROR); } /* * Do we have explicit KLD load ? */ if (dokld || file_havepath(argv[1])) { error = mod_loadkld(argv[1], argc - 2, argv + 2); if (error == EEXIST) { snprintf(command_errbuf, sizeof(command_errbuf), "warning: KLD '%s' already loaded", argv[1]); return (CMD_WARN); } return (error == 0 ? CMD_OK : CMD_CRIT); } /* * Looks like a request for a module. */ error = mod_load(argv[1], NULL, argc - 2, argv + 2); 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); } #ifdef LOADER_GELI_SUPPORT COMMAND_SET(load_geli, "load_geli", "load a geli key", command_load_geli); static int command_load_geli(int argc, char *argv[]) { char typestr[80]; char *cp; int ch, num; if (argc < 3) { command_errmsg = "usage is [-n key#] "; return(CMD_ERROR); } num = 0; optind = 1; optreset = 1; while ((ch = getopt(argc, argv, "n:")) != -1) { switch(ch) { case 'n': num = strtol(optarg, &cp, 0); if (cp == optarg) { snprintf(command_errbuf, sizeof(command_errbuf), "bad key index '%s'", optarg); return(CMD_ERROR); } break; case '?': default: /* getopt has already reported an error */ return(CMD_OK); } } argv += (optind - 1); argc -= (optind - 1); sprintf(typestr, "%s:geli_keyfile%d", argv[1], num); return (file_loadraw(argv[2], typestr, 1) ? CMD_OK : CMD_ERROR); } #endif void unload(void) { struct preloaded_file *fp; while (preloaded_files != NULL) { fp = preloaded_files; preloaded_files = preloaded_files->f_next; file_discard(fp); } loadaddr = 0; unsetenv("kernelname"); } COMMAND_SET(unload, "unload", "unload all modules", command_unload); static int command_unload(int argc, char *argv[]) { unload(); return(CMD_OK); } COMMAND_SET(lsmod, "lsmod", "list loaded modules", command_lsmod); static int command_lsmod(int argc, char *argv[]) { struct preloaded_file *fp; struct kernel_module *mp; struct file_metadata *md; char lbuf[80]; int ch, verbose, ret = 0; verbose = 0; optind = 1; optreset = 1; 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); } } pager_open(); for (fp = preloaded_files; fp; fp = fp->f_next) { snprintf(lbuf, sizeof(lbuf), " %p: ", (void *) fp->f_addr); pager_output(lbuf); pager_output(fp->f_name); snprintf(lbuf, sizeof(lbuf), " (%s, 0x%lx)\n", fp->f_type, (long)fp->f_size); if (pager_output(lbuf)) break; if (fp->f_args != NULL) { pager_output(" args: "); pager_output(fp->f_args); if (pager_output("\n")) break; } if (fp->f_modules) { pager_output(" modules: "); for (mp = fp->f_modules; mp; mp = mp->m_next) { snprintf(lbuf, sizeof(lbuf), "%s.%d ", mp->m_name, mp->m_version); pager_output(lbuf); } if (pager_output("\n")) break; } if (verbose) { /* XXX could add some formatting smarts here to display some better */ for (md = fp->f_metadata; md != NULL; md = md->md_next) { snprintf(lbuf, sizeof(lbuf), " 0x%04x, 0x%lx\n", md->md_type, (long) md->md_size); if (pager_output(lbuf)) break; } } if (ret) break; } pager_close(); 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_* */ int file_load(char *filename, vm_offset_t dest, struct preloaded_file **result) { static int last_file_format = 0; struct preloaded_file *fp; int error; int i; if (archsw.arch_loadaddr != NULL) dest = archsw.arch_loadaddr(LOAD_RAW, filename, dest); error = EFTYPE; for (i = last_file_format, fp = NULL; file_formats[i] && fp == NULL; i++) { error = (file_formats[i]->l_load)(filename, dest, &fp); if (error == 0) { fp->f_loader = last_file_format = i; /* remember the loader */ *result = fp; break; } else if (last_file_format == i && i != 0) { /* Restart from the beginning */ i = -1; last_file_format = 0; fp = NULL; continue; } if (error == EFTYPE) continue; /* Unknown to this handler? */ if (error) { snprintf(command_errbuf, sizeof(command_errbuf), "can't load file '%s': %s", filename, strerror(error)); break; } } return (error); } static int file_load_dependencies(struct preloaded_file *base_file) { struct file_metadata *md; struct preloaded_file *fp; struct mod_depend *verinfo; struct kernel_module *mp; char *dmodname; int error; md = file_findmetadata(base_file, MODINFOMD_DEPLIST); if (md == NULL) return (0); error = 0; do { verinfo = (struct mod_depend*)md->md_data; dmodname = (char *)(verinfo + 1); if (file_findmodule(NULL, dmodname, verinfo) == NULL) { printf("loading required module '%s'\n", dmodname); error = mod_load(dmodname, verinfo, 0, NULL); if (error) break; /* * If module loaded via kld name which isn't listed * in the linker.hints file, we should check if it have * required version. */ mp = file_findmodule(NULL, dmodname, verinfo); if (mp == NULL) { snprintf(command_errbuf, sizeof(command_errbuf), "module '%s' exists but with wrong version", dmodname); error = ENOENT; break; } } md = metadata_next(md, MODINFOMD_DEPLIST); } while (md); if (!error) return (0); /* Load failed; discard everything */ while (base_file != NULL) { fp = base_file; base_file = base_file->f_next; file_discard(fp); } return (error); } /* * We've been asked to load (fname) as (type), so just suck it in, * no arguments or anything. */ struct preloaded_file * file_loadraw(const char *fname, char *type, int insert) { struct preloaded_file *fp; char *name; int fd, got; vm_offset_t laddr; /* We can't load first */ if ((file_findfile(NULL, NULL)) == NULL) { command_errmsg = "can't load file before kernel"; return(NULL); } /* locate the file on the load path */ name = file_search(fname, NULL); if (name == NULL) { snprintf(command_errbuf, sizeof(command_errbuf), "can't find '%s'", fname); return(NULL); } if ((fd = open(name, O_RDONLY)) < 0) { snprintf(command_errbuf, sizeof(command_errbuf), "can't open '%s': %s", name, strerror(errno)); free(name); return(NULL); } #ifdef LOADER_VERIEXEC if (verify_file(fd, name, 0, VE_MUST) < 0) { sprintf(command_errbuf, "can't verify '%s'", name); free(name); close(fd); return(NULL); } #endif if (archsw.arch_loadaddr != NULL) loadaddr = archsw.arch_loadaddr(LOAD_RAW, name, loadaddr); printf("%s ", name); laddr = loadaddr; for (;;) { /* read in 4k chunks; size is not really important */ got = archsw.arch_readin(fd, laddr, 4096); if (got == 0) /* end of file */ break; if (got < 0) { /* error */ snprintf(command_errbuf, sizeof(command_errbuf), "error reading '%s': %s", name, strerror(errno)); free(name); close(fd); return(NULL); } laddr += got; } printf("size=%#jx\n", (uintmax_t)(laddr - loadaddr)); /* Looks OK so far; create & populate control structure */ fp = file_alloc(); if (fp == NULL) { snprintf(command_errbuf, sizeof (command_errbuf), "no memory to load %s", name); free(name); close(fd); return (NULL); } fp->f_name = name; fp->f_type = strdup(type); fp->f_args = NULL; fp->f_metadata = NULL; fp->f_loader = -1; fp->f_addr = loadaddr; fp->f_size = laddr - loadaddr; if (fp->f_type == NULL) { snprintf(command_errbuf, sizeof (command_errbuf), "no memory to load %s", name); free(name); close(fd); return (NULL); } /* recognise space consumption */ loadaddr = laddr; /* Add to the list of loaded files */ if (insert != 0) file_insert_tail(fp); close(fd); return(fp); } /* * Load the module (name), pass it (argc),(argv), add container file * to the list of loaded files. * If module is already loaded just assign new argc/argv. */ int mod_load(char *modname, struct mod_depend *verinfo, int argc, char *argv[]) { struct kernel_module *mp; int err; char *filename; if (file_havepath(modname)) { printf("Warning: mod_load() called instead of mod_loadkld() for module '%s'\n", modname); return (mod_loadkld(modname, argc, argv)); } /* see if module is already loaded */ mp = file_findmodule(NULL, modname, verinfo); if (mp) { #ifdef moduleargs free(mp->m_args); mp->m_args = unargv(argc, argv); #endif snprintf(command_errbuf, sizeof(command_errbuf), "warning: module '%s' already loaded", mp->m_name); return (0); } /* locate file with the module on the search path */ filename = mod_searchmodule(modname, verinfo); if (filename == NULL) { snprintf(command_errbuf, sizeof(command_errbuf), "can't find '%s'", modname); return (ENOENT); } err = mod_loadkld(filename, argc, argv); free(filename); return (err); } /* * Load specified KLD. If path is omitted, then try to locate it via * search path. */ int mod_loadkld(const char *kldname, int argc, char *argv[]) { struct preloaded_file *fp; int err; char *filename; vm_offset_t loadaddr_saved; /* * Get fully qualified KLD name */ filename = file_search(kldname, kld_ext_list); if (filename == NULL) { snprintf(command_errbuf, sizeof(command_errbuf), "can't find '%s'", kldname); return (ENOENT); } /* * Check if KLD already loaded */ fp = file_findfile(filename, NULL); if (fp) { snprintf(command_errbuf, sizeof(command_errbuf), "warning: KLD '%s' already loaded", filename); free(filename); return (0); } do { err = file_load(filename, loadaddr, &fp); if (err) break; fp->f_args = unargv(argc, argv); loadaddr_saved = loadaddr; loadaddr = fp->f_addr + fp->f_size; file_insert_tail(fp); /* Add to the list of loaded files */ if (file_load_dependencies(fp) != 0) { err = ENOENT; file_remove(fp); loadaddr = loadaddr_saved; fp = NULL; break; } } while(0); if (err == EFTYPE) { snprintf(command_errbuf, sizeof(command_errbuf), "don't know how to load module '%s'", filename); } if (err) file_discard(fp); free(filename); return (err); } /* * Find a file matching (name) and (type). * NULL may be passed as a wildcard to either. */ struct preloaded_file * file_findfile(const char *name, const char *type) { struct preloaded_file *fp; for (fp = preloaded_files; fp != NULL; fp = fp->f_next) { if (((name == NULL) || !strcmp(name, fp->f_name)) && ((type == NULL) || !strcmp(type, fp->f_type))) break; } return (fp); } /* * Find a module matching (name) inside of given file. * NULL may be passed as a wildcard. */ struct kernel_module * file_findmodule(struct preloaded_file *fp, char *modname, struct mod_depend *verinfo) { struct kernel_module *mp, *best; int bestver, mver; if (fp == NULL) { for (fp = preloaded_files; fp; fp = fp->f_next) { mp = file_findmodule(fp, modname, verinfo); if (mp) return (mp); } return (NULL); } best = NULL; bestver = 0; for (mp = fp->f_modules; mp; mp = mp->m_next) { if (strcmp(modname, mp->m_name) == 0) { if (verinfo == NULL) return (mp); mver = mp->m_version; if (mver == verinfo->md_ver_preferred) return (mp); if (mver >= verinfo->md_ver_minimum && mver <= verinfo->md_ver_maximum && mver > bestver) { best = mp; bestver = mver; } } } return (best); } /* * Make a copy of (size) bytes of data from (p), and associate them as * metadata of (type) to the module (mp). */ void file_addmetadata(struct preloaded_file *fp, int type, size_t size, void *p) { struct file_metadata *md; md = malloc(sizeof(struct file_metadata) - sizeof(md->md_data) + size); if (md != NULL) { md->md_size = size; md->md_type = type; bcopy(p, md->md_data, size); md->md_next = fp->f_metadata; } fp->f_metadata = md; } /* * Find a metadata object of (type) associated with the file (fp) */ struct file_metadata * file_findmetadata(struct preloaded_file *fp, int type) { struct file_metadata *md; for (md = fp->f_metadata; md != NULL; md = md->md_next) if (md->md_type == type) break; return(md); } /* * Remove all metadata from the file. */ void file_removemetadata(struct preloaded_file *fp) { struct file_metadata *md, *next; for (md = fp->f_metadata; md != NULL; md = next) { next = md->md_next; free(md); } fp->f_metadata = NULL; } struct file_metadata * metadata_next(struct file_metadata *md, int type) { if (md == NULL) return (NULL); while((md = md->md_next) != NULL) if (md->md_type == type) break; return (md); } static char *emptyextlist[] = { "", NULL }; /* * Check if the given file is in place and return full path to it. */ static char * file_lookup(const char *path, const char *name, int namelen, char **extlist) { struct stat st; char *result, *cp, **cpp; int pathlen, extlen, len; pathlen = strlen(path); extlen = 0; if (extlist == NULL) extlist = emptyextlist; for (cpp = extlist; *cpp; cpp++) { len = strlen(*cpp); if (len > extlen) extlen = len; } result = malloc(pathlen + namelen + extlen + 2); if (result == NULL) return (NULL); bcopy(path, result, pathlen); if (pathlen > 0 && result[pathlen - 1] != '/') result[pathlen++] = '/'; cp = result + pathlen; bcopy(name, cp, namelen); cp += namelen; for (cpp = extlist; *cpp; cpp++) { strcpy(cp, *cpp); if (stat(result, &st) == 0 && S_ISREG(st.st_mode)) return result; } free(result); return NULL; } /* * Check if file name have any qualifiers */ static int file_havepath(const char *name) { const char *cp; archsw.arch_getdev(NULL, name, &cp); return (cp != name || strchr(name, '/') != NULL); } /* * Attempt to find the file (name) on the module searchpath. * If (name) is qualified in any way, we simply check it and * return it or NULL. If it is not qualified, then we attempt * to construct a path using entries in the environment variable * module_path. * * The path we return a pointer to need never be freed, as we manage * it internally. */ static char * file_search(const char *name, char **extlist) { struct moduledir *mdp; struct stat sb; char *result; int namelen; /* Don't look for nothing */ if (name == NULL) return(NULL); if (*name == 0) return(strdup(name)); if (file_havepath(name)) { /* Qualified, so just see if it exists */ if (stat(name, &sb) == 0) return(strdup(name)); return(NULL); } moduledir_rebuild(); result = NULL; namelen = strlen(name); STAILQ_FOREACH(mdp, &moduledir_list, d_link) { result = file_lookup(mdp->d_path, name, namelen, extlist); if (result) break; } return(result); } #define INT_ALIGN(base, ptr) ptr = \ (base) + roundup2((ptr) - (base), sizeof(int)) static char * mod_search_hints(struct moduledir *mdp, const char *modname, struct mod_depend *verinfo) { u_char *cp, *recptr, *bufend, *best; char *result; int *intp, bestver, blen, clen, found, ival, modnamelen, reclen; moduledir_readhints(mdp); modnamelen = strlen(modname); found = 0; result = NULL; bestver = 0; if (mdp->d_hints == NULL) goto bad; recptr = mdp->d_hints; bufend = recptr + mdp->d_hintsz; clen = blen = 0; best = cp = NULL; while (recptr < bufend && !found) { intp = (int*)recptr; reclen = *intp++; ival = *intp++; cp = (u_char*)intp; switch (ival) { case MDT_VERSION: clen = *cp++; if (clen != modnamelen || bcmp(cp, modname, clen) != 0) break; cp += clen; INT_ALIGN(mdp->d_hints, cp); ival = *(int*)cp; cp += sizeof(int); clen = *cp++; if (verinfo == NULL || ival == verinfo->md_ver_preferred) { found = 1; break; } if (ival >= verinfo->md_ver_minimum && ival <= verinfo->md_ver_maximum && ival > bestver) { bestver = ival; best = cp; blen = clen; } break; default: break; } recptr += reclen + sizeof(int); } /* * Finally check if KLD is in the place */ if (found) result = file_lookup(mdp->d_path, (const char *)cp, clen, NULL); else if (best) result = file_lookup(mdp->d_path, (const char *)best, blen, NULL); bad: /* * If nothing found or hints is absent - fallback to the old way * by using "kldname[.ko]" as module name. */ if (!found && !bestver && result == NULL) result = file_lookup(mdp->d_path, modname, modnamelen, kld_ext_list); 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) */ static char * mod_searchmodule(char *name, struct mod_depend *verinfo) { 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 = mod_search_hints(mdp, name, verinfo); + if (result) + break; + } + + 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) { struct kernel_module *mp; struct mod_depend mdepend; bzero(&mdepend, sizeof(mdepend)); mdepend.md_ver_preferred = version; mp = file_findmodule(fp, modname, &mdepend); if (mp) return (EEXIST); mp = calloc(1, sizeof(struct kernel_module)); if (mp == NULL) return (ENOMEM); mp->m_name = strdup(modname); if (mp->m_name == NULL) { free(mp); return (ENOMEM); } mp->m_version = version; mp->m_fp = fp; mp->m_next = fp->f_modules; fp->f_modules = mp; if (newmp) *newmp = mp; return (0); } /* * Throw a file away */ void file_discard(struct preloaded_file *fp) { struct file_metadata *md, *md1; struct kernel_module *mp, *mp1; if (fp == NULL) return; md = fp->f_metadata; while (md) { md1 = md; md = md->md_next; free(md1); } mp = fp->f_modules; while (mp) { free(mp->m_name); mp1 = mp; mp = mp->m_next; free(mp1); } free(fp->f_name); free(fp->f_type); free(fp->f_args); free(fp); } /* * Allocate a new file; must be used instead of malloc() * to ensure safe initialisation. */ struct preloaded_file * file_alloc(void) { return (calloc(1, sizeof(struct preloaded_file))); } /* * Add a module to the chain */ static void file_insert_tail(struct preloaded_file *fp) { struct preloaded_file *cm; /* Append to list of loaded file */ fp->f_next = NULL; if (preloaded_files == NULL) { preloaded_files = fp; } else { for (cm = preloaded_files; cm->f_next != NULL; cm = cm->f_next) ; cm->f_next = fp; } } /* * Remove module from the chain */ static void file_remove(struct preloaded_file *fp) { struct preloaded_file *cm; if (preloaded_files == NULL) return; if (preloaded_files == fp) { preloaded_files = fp->f_next; return; } for (cm = preloaded_files; cm->f_next != NULL; cm = cm->f_next) { if (cm->f_next == fp) { cm->f_next = fp->f_next; return; } } } static char * moduledir_fullpath(struct moduledir *mdp, const char *fname) { char *cp; cp = malloc(strlen(mdp->d_path) + strlen(fname) + 2); if (cp == NULL) return NULL; strcpy(cp, mdp->d_path); strcat(cp, "/"); strcat(cp, fname); return (cp); } /* * Read linker.hints file into memory performing some sanity checks. */ static void moduledir_readhints(struct moduledir *mdp) { struct stat st; char *path; int fd, size, version; if (mdp->d_hints != NULL || (mdp->d_flags & MDIR_NOHINTS)) return; path = moduledir_fullpath(mdp, "linker.hints"); if (stat(path, &st) != 0 || st.st_size < (ssize_t)(sizeof(version) + sizeof(int)) || st.st_size > LINKER_HINTS_MAX || (fd = open(path, O_RDONLY)) < 0) { free(path); mdp->d_flags |= MDIR_NOHINTS; return; } free(path); size = read(fd, &version, sizeof(version)); if (size != sizeof(version) || version != LINKER_HINTS_VERSION) goto bad; size = st.st_size - size; mdp->d_hints = malloc(size); if (mdp->d_hints == NULL) goto bad; if (read(fd, mdp->d_hints, size) != size) goto bad; mdp->d_hintsz = size; close(fd); return; bad: close(fd); free(mdp->d_hints); mdp->d_hints = NULL; mdp->d_flags |= MDIR_NOHINTS; return; } /* * Extract directories from the ';' separated list, remove duplicates. */ static void moduledir_rebuild(void) { struct moduledir *mdp, *mtmp; const char *path, *cp, *ep; size_t cplen; path = getenv("module_path"); if (path == NULL) path = default_searchpath; /* * Rebuild list of module directories if it changed */ STAILQ_FOREACH(mdp, &moduledir_list, d_link) mdp->d_flags |= MDIR_REMOVED; for (ep = path; *ep != 0; ep++) { cp = ep; for (; *ep != 0 && *ep != ';'; ep++) ; /* * Ignore trailing slashes */ for (cplen = ep - cp; cplen > 1 && cp[cplen - 1] == '/'; cplen--) ; STAILQ_FOREACH(mdp, &moduledir_list, d_link) { if (strlen(mdp->d_path) != cplen || bcmp(cp, mdp->d_path, cplen) != 0) continue; mdp->d_flags &= ~MDIR_REMOVED; break; } if (mdp == NULL) { mdp = malloc(sizeof(*mdp) + cplen + 1); if (mdp == NULL) return; mdp->d_path = (char*)(mdp + 1); bcopy(cp, mdp->d_path, cplen); mdp->d_path[cplen] = 0; mdp->d_hints = NULL; mdp->d_flags = 0; STAILQ_INSERT_TAIL(&moduledir_list, mdp, d_link); } if (*ep == 0) break; } /* * Delete unused directories if any */ mdp = STAILQ_FIRST(&moduledir_list); while (mdp) { if ((mdp->d_flags & MDIR_REMOVED) == 0) { mdp = STAILQ_NEXT(mdp, d_link); } else { free(mdp->d_hints); mtmp = mdp; mdp = STAILQ_NEXT(mdp, d_link); STAILQ_REMOVE(&moduledir_list, mtmp, moduledir, d_link); free(mtmp); } } return; } Index: head/stand/fdt/fdt_loader_cmd.c =================================================================== --- head/stand/fdt/fdt_loader_cmd.c (revision 348193) +++ head/stand/fdt/fdt_loader_cmd.c (revision 348194) @@ -1,1861 +1,1929 @@ /*- * Copyright (c) 2009-2010 The FreeBSD Foundation * All rights reserved. * * This software was developed by Semihalf under sponsorship from * the FreeBSD Foundation. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. */ #include __FBSDID("$FreeBSD$"); #include #include #include #include #include #include #include "bootstrap.h" #include "fdt_platform.h" #ifdef DEBUG #define debugf(fmt, args...) do { printf("%s(): ", __func__); \ printf(fmt,##args); } while (0) #else #define debugf(fmt, args...) #endif #define FDT_CWD_LEN 256 #define FDT_MAX_DEPTH 12 #define FDT_PROP_SEP " = " #define COPYOUT(s,d,l) archsw.arch_copyout(s, d, l) #define COPYIN(s,d,l) archsw.arch_copyin(s, d, l) #define FDT_STATIC_DTB_SYMBOL "fdt_static_dtb" #define CMD_REQUIRES_BLOB 0x01 /* Location of FDT yet to be loaded. */ /* This may be in read-only memory, so can't be manipulated directly. */ static struct fdt_header *fdt_to_load = NULL; /* Location of FDT on heap. */ /* This is the copy we actually manipulate. */ 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); static int fdt_check_overlay_compatible(void *base_fdt, void *overlay_fdt); static int fdt_cmd_nyi(int argc, char *argv[]); static int fdt_load_dtb_overlays_string(const char * filenames); static int fdt_cmd_addr(int argc, char *argv[]); static int fdt_cmd_mkprop(int argc, char *argv[]); static int fdt_cmd_cd(int argc, char *argv[]); static int fdt_cmd_hdr(int argc, char *argv[]); static int fdt_cmd_ls(int argc, char *argv[]); static int fdt_cmd_prop(int argc, char *argv[]); static int fdt_cmd_pwd(int argc, char *argv[]); static int fdt_cmd_rm(int argc, char *argv[]); static int fdt_cmd_mknode(int argc, char *argv[]); static int fdt_cmd_mres(int argc, char *argv[]); typedef int cmdf_t(int, char *[]); struct cmdtab { const char *name; cmdf_t *handler; int flags; }; static const struct cmdtab commands[] = { { "addr", &fdt_cmd_addr, 0 }, { "alias", &fdt_cmd_nyi, 0 }, { "cd", &fdt_cmd_cd, CMD_REQUIRES_BLOB }, { "header", &fdt_cmd_hdr, CMD_REQUIRES_BLOB }, { "ls", &fdt_cmd_ls, CMD_REQUIRES_BLOB }, { "mknode", &fdt_cmd_mknode, CMD_REQUIRES_BLOB }, { "mkprop", &fdt_cmd_mkprop, CMD_REQUIRES_BLOB }, { "mres", &fdt_cmd_mres, CMD_REQUIRES_BLOB }, { "prop", &fdt_cmd_prop, CMD_REQUIRES_BLOB }, { "pwd", &fdt_cmd_pwd, CMD_REQUIRES_BLOB }, { "rm", &fdt_cmd_rm, CMD_REQUIRES_BLOB }, { NULL, NULL } }; static char cwd[FDT_CWD_LEN] = "/"; static vm_offset_t fdt_find_static_dtb() { Elf_Ehdr *ehdr; Elf_Shdr *shdr; Elf_Sym sym; vm_offset_t strtab, symtab, fdt_start; uint64_t offs; struct preloaded_file *kfp; struct file_metadata *md; char *strp; int i, sym_count; debugf("fdt_find_static_dtb()\n"); sym_count = symtab = strtab = 0; strp = NULL; offs = __elfN(relocation_offset); kfp = file_findfile(NULL, NULL); if (kfp == NULL) return (0); /* Locate the dynamic symbols and strtab. */ md = file_findmetadata(kfp, MODINFOMD_ELFHDR); if (md == NULL) return (0); ehdr = (Elf_Ehdr *)md->md_data; md = file_findmetadata(kfp, MODINFOMD_SHDR); if (md == NULL) return (0); shdr = (Elf_Shdr *)md->md_data; for (i = 0; i < ehdr->e_shnum; ++i) { if (shdr[i].sh_type == SHT_DYNSYM && symtab == 0) { symtab = shdr[i].sh_addr + offs; sym_count = shdr[i].sh_size / sizeof(Elf_Sym); } else if (shdr[i].sh_type == SHT_STRTAB && strtab == 0) { strtab = shdr[i].sh_addr + offs; } } /* * The most efficient way to find a symbol would be to calculate a * hash, find proper bucket and chain, and thus find a symbol. * However, that would involve code duplication (e.g. for hash * function). So we're using simpler and a bit slower way: we're * iterating through symbols, searching for the one which name is * 'equal' to 'fdt_static_dtb'. To speed up the process a little bit, * we are eliminating symbols type of which is not STT_NOTYPE, or(and) * those which binding attribute is not STB_GLOBAL. */ fdt_start = 0; while (sym_count > 0 && fdt_start == 0) { COPYOUT(symtab, &sym, sizeof(sym)); symtab += sizeof(sym); --sym_count; if (ELF_ST_BIND(sym.st_info) != STB_GLOBAL || ELF_ST_TYPE(sym.st_info) != STT_NOTYPE) continue; strp = strdupout(strtab + sym.st_name); if (strcmp(strp, FDT_STATIC_DTB_SYMBOL) == 0) fdt_start = (vm_offset_t)sym.st_value + offs; free(strp); } return (fdt_start); } static int fdt_load_dtb(vm_offset_t va) { struct fdt_header header; int err; debugf("fdt_load_dtb(0x%08jx)\n", (uintmax_t)va); COPYOUT(va, &header, sizeof(header)); err = fdt_check_header(&header); if (err < 0) { if (err == -FDT_ERR_BADVERSION) { snprintf(command_errbuf, sizeof(command_errbuf), "incompatible blob version: %d, should be: %d", fdt_version(fdtp), FDT_LAST_SUPPORTED_VERSION); } else { snprintf(command_errbuf, sizeof(command_errbuf), "error validating blob: %s", fdt_strerror(err)); } return (1); } /* * Release previous blob */ if (fdtp) free(fdtp); fdtp_size = fdt_totalsize(&header); fdtp = malloc(fdtp_size); if (fdtp == NULL) { command_errmsg = "can't allocate memory for device tree copy"; return (1); } COPYOUT(va, fdtp, fdtp_size); debugf("DTB blob found at 0x%jx, size: 0x%jx\n", (uintmax_t)va, (uintmax_t)fdtp_size); return (0); } int fdt_load_dtb_addr(struct fdt_header *header) { int err; debugf("fdt_load_dtb_addr(%p)\n", header); fdtp_size = fdt_totalsize(header); err = fdt_check_header(header); if (err < 0) { snprintf(command_errbuf, sizeof(command_errbuf), "error validating blob: %s", fdt_strerror(err)); return (err); } free(fdtp); if ((fdtp = malloc(fdtp_size)) == NULL) { command_errmsg = "can't allocate memory for device tree copy"; return (1); } bcopy(header, fdtp, fdtp_size); return (0); } int fdt_load_dtb_file(const char * filename) { struct preloaded_file *bfp, *oldbfp; int err; debugf("fdt_load_dtb_file(%s)\n", filename); oldbfp = file_findfile(NULL, "dtb"); /* Attempt to load and validate a new dtb from a file. */ if ((bfp = file_loadraw(filename, "dtb", 1)) == NULL) { snprintf(command_errbuf, sizeof(command_errbuf), "failed to load file '%s'", filename); return (1); } if ((err = fdt_load_dtb(bfp->f_addr)) != 0) { file_discard(bfp); return (err); } /* A new dtb was validated, discard any previous file. */ if (oldbfp) file_discard(oldbfp); return (0); } static int fdt_load_dtb_overlay(const char * filename) { struct preloaded_file *bfp; struct fdt_header header; int err; debugf("fdt_load_dtb_overlay(%s)\n", filename); /* Attempt to load and validate a new dtb from a file. FDT_ERR_NOTFOUND * is normally a libfdt error code, but libfdt would actually return * -FDT_ERR_NOTFOUND. We re-purpose the error code here to convey a * similar meaning: the file itself was not found, which can still be * considered an error dealing with FDT pieces. */ if ((bfp = file_loadraw(filename, "dtbo", 1)) == NULL) return (FDT_ERR_NOTFOUND); COPYOUT(bfp->f_addr, &header, sizeof(header)); err = fdt_check_header(&header); if (err < 0) { file_discard(bfp); return (err); } return (0); } static void fdt_print_overlay_load_error(int err, const char *filename) { switch (err) { case FDT_ERR_NOTFOUND: printf("%s: failed to load file\n", filename); break; case -FDT_ERR_BADVERSION: printf("%s: incompatible blob version: %d, should be: %d\n", filename, fdt_version(fdtp), FDT_LAST_SUPPORTED_VERSION); break; default: /* libfdt errs are negative */ if (err < 0) printf("%s: error validating blob: %s\n", filename, fdt_strerror(err)); else printf("%s: unknown load error\n", filename); break; } } static int fdt_load_dtb_overlays_string(const char * filenames) { char *names; char *name, *name_ext; char *comaptr; int err, namesz; debugf("fdt_load_dtb_overlays_string(%s)\n", filenames); names = strdup(filenames); if (names == NULL) return (1); name = names; do { comaptr = strchr(name, ','); if (comaptr) *comaptr = '\0'; err = fdt_load_dtb_overlay(name); if (err == FDT_ERR_NOTFOUND) { /* Allocate enough to append ".dtbo" */ namesz = strlen(name) + 6; name_ext = malloc(namesz); if (name_ext == NULL) { fdt_print_overlay_load_error(err, name); name = comaptr + 1; continue; } snprintf(name_ext, namesz, "%s.dtbo", name); err = fdt_load_dtb_overlay(name_ext); free(name_ext); } /* Catch error with either initial load or fallback load */ if (err != 0) fdt_print_overlay_load_error(err, name); name = comaptr + 1; } while(comaptr); free(names); return (0); } /* * fdt_check_overlay_compatible - check that the overlay_fdt is compatible with * base_fdt before we attempt to apply it. It will need to re-calculate offsets * in the base every time, rather than trying to cache them earlier in the * process, because the overlay application process can/will invalidate a lot of * offsets. */ static int fdt_check_overlay_compatible(void *base_fdt, void *overlay_fdt) { const char *compat; int compat_len, ocompat_len; int oroot_offset, root_offset; int slidx, sllen; oroot_offset = fdt_path_offset(overlay_fdt, "/"); if (oroot_offset < 0) return (oroot_offset); /* * If /compatible in the overlay does not exist or if it is empty, then * we're automatically compatible. We do this for the sake of rapid * overlay development for overlays that aren't intended to be deployed. * The user assumes the risk of using an overlay without /compatible. */ if (fdt_get_property(overlay_fdt, oroot_offset, "compatible", &ocompat_len) == NULL || ocompat_len == 0) return (0); root_offset = fdt_path_offset(base_fdt, "/"); if (root_offset < 0) return (root_offset); /* * However, an empty or missing /compatible on the base is an error, * because allowing this offers no advantages. */ if (fdt_get_property(base_fdt, root_offset, "compatible", &compat_len) == NULL) return (compat_len); else if(compat_len == 0) return (1); slidx = 0; compat = fdt_stringlist_get(overlay_fdt, oroot_offset, "compatible", slidx, &sllen); while (compat != NULL) { if (fdt_stringlist_search(base_fdt, root_offset, "compatible", compat) >= 0) return (0); ++slidx; compat = fdt_stringlist_get(overlay_fdt, oroot_offset, "compatible", slidx, &sllen); }; /* We've exhausted the overlay's /compatible property... no match */ return (1); } void fdt_apply_overlays() { struct preloaded_file *fp; size_t max_overlay_size, next_fdtp_size; size_t current_fdtp_size; void *current_fdtp; void *next_fdtp; void *overlay; int rv; 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) max_overlay_size = fp->f_size; } /* Nothing to apply */ if (max_overlay_size == 0) return; overlay = malloc(max_overlay_size); if (overlay == NULL) { printf("failed to allocate memory for DTB blob with overlays\n"); return; } current_fdtp = fdtp; current_fdtp_size = fdtp_size; for (fp = file_findfile(NULL, "dtbo"); fp != NULL; fp = fp->f_next) { COPYOUT(fp->f_addr, overlay, fp->f_size); /* Check compatible first to avoid unnecessary allocation */ rv = fdt_check_overlay_compatible(current_fdtp, overlay); if (rv != 0) { printf("DTB overlay '%s' not compatible\n", fp->f_name); continue; } printf("applying DTB overlay '%s'\n", fp->f_name); next_fdtp_size = current_fdtp_size + fp->f_size; next_fdtp = malloc(next_fdtp_size); if (next_fdtp == NULL) { /* * Output warning, then move on to applying other * overlays in case this one is simply too large. */ printf("failed to allocate memory for overlay base\n"); continue; } rv = fdt_open_into(current_fdtp, next_fdtp, next_fdtp_size); if (rv != 0) { free(next_fdtp); printf("failed to open base dtb into overlay base\n"); continue; } /* Both overlay and next_fdtp may be modified in place */ rv = fdt_overlay_apply(next_fdtp, overlay); if (rv == 0) { /* Rotate next -> current */ if (current_fdtp != fdtp) free(current_fdtp); current_fdtp = next_fdtp; current_fdtp_size = next_fdtp_size; } else { /* * Assume here that the base we tried to apply on is * either trashed or in an inconsistent state. Trying to * load it might work, but it's better to discard it and * play it safe. */ free(next_fdtp); printf("failed to apply overlay: %s\n", fdt_strerror(rv)); } } /* We could have failed to apply all overlays; then we do nothing */ if (current_fdtp != fdtp) { free(fdtp); fdtp = current_fdtp; fdtp_size = current_fdtp_size; } free(overlay); + fdt_overlays_applied = 1; } int fdt_is_setup(void) { if (fdtp != NULL) return (1); return (0); } int fdt_setup_fdtp() { struct preloaded_file *bfp; vm_offset_t va; debugf("fdt_setup_fdtp()\n"); /* If we already loaded a file, use it. */ if ((bfp = file_findfile(NULL, "dtb")) != NULL) { if (fdt_load_dtb(bfp->f_addr) == 0) { printf("Using DTB from loaded file '%s'.\n", bfp->f_name); fdt_platform_load_overlays(); return (0); } } /* If we were given the address of a valid blob in memory, use it. */ if (fdt_to_load != NULL) { if (fdt_load_dtb_addr(fdt_to_load) == 0) { printf("Using DTB from memory address %p.\n", fdt_to_load); fdt_platform_load_overlays(); return (0); } } if (fdt_platform_load_dtb() == 0) { fdt_platform_load_overlays(); return (0); } /* If there is a dtb compiled into the kernel, use it. */ if ((va = fdt_find_static_dtb()) != 0) { if (fdt_load_dtb(va) == 0) { printf("Using DTB compiled into kernel.\n"); return (0); } } command_errmsg = "No device tree blob found!\n"; return (1); } #define fdt_strtovect(str, cellbuf, lim, cellsize) _fdt_strtovect((str), \ (cellbuf), (lim), (cellsize), 0); /* Force using base 16 */ #define fdt_strtovectx(str, cellbuf, lim, cellsize) _fdt_strtovect((str), \ (cellbuf), (lim), (cellsize), 16); static int _fdt_strtovect(const char *str, void *cellbuf, int lim, unsigned char cellsize, uint8_t base) { const char *buf = str; const char *end = str + strlen(str) - 2; uint32_t *u32buf = NULL; uint8_t *u8buf = NULL; int cnt = 0; if (cellsize == sizeof(uint32_t)) u32buf = (uint32_t *)cellbuf; else u8buf = (uint8_t *)cellbuf; if (lim == 0) return (0); while (buf < end) { /* Skip white whitespace(s)/separators */ while (!isxdigit(*buf) && buf < end) buf++; if (u32buf != NULL) u32buf[cnt] = cpu_to_fdt32((uint32_t)strtol(buf, NULL, base)); else u8buf[cnt] = (uint8_t)strtol(buf, NULL, base); if (cnt + 1 <= lim - 1) cnt++; else break; buf++; /* Find another number */ while ((isxdigit(*buf) || *buf == 'x') && buf < end) buf++; } return (cnt); } void fdt_fixup_ethernet(const char *str, char *ethstr, int len) { uint8_t tmp_addr[6]; /* Convert macaddr string into a vector of uints */ fdt_strtovectx(str, &tmp_addr, 6, sizeof(uint8_t)); /* Set actual property to a value from vect */ fdt_setprop(fdtp, fdt_path_offset(fdtp, ethstr), "local-mac-address", &tmp_addr, 6 * sizeof(uint8_t)); } void fdt_fixup_cpubusfreqs(unsigned long cpufreq, unsigned long busfreq) { int lo, o = 0, o2, maxo = 0, depth; const uint32_t zero = 0; /* We want to modify every subnode of /cpus */ o = fdt_path_offset(fdtp, "/cpus"); if (o < 0) return; /* maxo should contain offset of node next to /cpus */ depth = 0; maxo = o; while (depth != -1) maxo = fdt_next_node(fdtp, maxo, &depth); /* Find CPU frequency properties */ o = fdt_node_offset_by_prop_value(fdtp, o, "clock-frequency", &zero, sizeof(uint32_t)); o2 = fdt_node_offset_by_prop_value(fdtp, o, "bus-frequency", &zero, sizeof(uint32_t)); lo = MIN(o, o2); while (o != -FDT_ERR_NOTFOUND && o2 != -FDT_ERR_NOTFOUND) { o = fdt_node_offset_by_prop_value(fdtp, lo, "clock-frequency", &zero, sizeof(uint32_t)); o2 = fdt_node_offset_by_prop_value(fdtp, lo, "bus-frequency", &zero, sizeof(uint32_t)); /* We're only interested in /cpus subnode(s) */ if (lo > maxo) break; fdt_setprop_inplace_cell(fdtp, lo, "clock-frequency", (uint32_t)cpufreq); fdt_setprop_inplace_cell(fdtp, lo, "bus-frequency", (uint32_t)busfreq); lo = MIN(o, o2); } } #ifdef notyet static int fdt_reg_valid(uint32_t *reg, int len, int addr_cells, int size_cells) { int cells_in_tuple, i, tuples, tuple_size; uint32_t cur_start, cur_size; cells_in_tuple = (addr_cells + size_cells); tuple_size = cells_in_tuple * sizeof(uint32_t); tuples = len / tuple_size; if (tuples == 0) return (EINVAL); for (i = 0; i < tuples; i++) { if (addr_cells == 2) cur_start = fdt64_to_cpu(reg[i * cells_in_tuple]); else cur_start = fdt32_to_cpu(reg[i * cells_in_tuple]); if (size_cells == 2) cur_size = fdt64_to_cpu(reg[i * cells_in_tuple + 2]); else cur_size = fdt32_to_cpu(reg[i * cells_in_tuple + 1]); if (cur_size == 0) return (EINVAL); debugf(" reg#%d (start: 0x%0x size: 0x%0x) valid!\n", i, cur_start, cur_size); } return (0); } #endif void fdt_fixup_memory(struct fdt_mem_region *region, size_t num) { struct fdt_mem_region *curmr; uint32_t addr_cells, size_cells; uint32_t *addr_cellsp, *size_cellsp; int err, i, len, memory, root; size_t realmrno; uint8_t *buf, *sb; uint64_t rstart, rsize; int reserved; root = fdt_path_offset(fdtp, "/"); if (root < 0) { sprintf(command_errbuf, "Could not find root node !"); return; } memory = fdt_path_offset(fdtp, "/memory"); if (memory <= 0) { /* Create proper '/memory' node. */ memory = fdt_add_subnode(fdtp, root, "memory"); if (memory <= 0) { snprintf(command_errbuf, sizeof(command_errbuf), "Could not fixup '/memory' " "node, error code : %d!\n", memory); return; } err = fdt_setprop(fdtp, memory, "device_type", "memory", sizeof("memory")); if (err < 0) return; } addr_cellsp = (uint32_t *)fdt_getprop(fdtp, root, "#address-cells", NULL); size_cellsp = (uint32_t *)fdt_getprop(fdtp, root, "#size-cells", NULL); if (addr_cellsp == NULL || size_cellsp == NULL) { snprintf(command_errbuf, sizeof(command_errbuf), "Could not fixup '/memory' node : " "%s %s property not found in root node!\n", (!addr_cellsp) ? "#address-cells" : "", (!size_cellsp) ? "#size-cells" : ""); return; } addr_cells = fdt32_to_cpu(*addr_cellsp); size_cells = fdt32_to_cpu(*size_cellsp); /* * Convert memreserve data to memreserve property * Check if property already exists */ reserved = fdt_num_mem_rsv(fdtp); if (reserved && (fdt_getprop(fdtp, root, "memreserve", NULL) == NULL)) { len = (addr_cells + size_cells) * reserved * sizeof(uint32_t); sb = buf = (uint8_t *)malloc(len); if (!buf) return; bzero(buf, len); for (i = 0; i < reserved; i++) { if (fdt_get_mem_rsv(fdtp, i, &rstart, &rsize)) break; if (rsize) { /* Ensure endianness, and put cells into a buffer */ if (addr_cells == 2) *(uint64_t *)buf = cpu_to_fdt64(rstart); else *(uint32_t *)buf = cpu_to_fdt32(rstart); buf += sizeof(uint32_t) * addr_cells; if (size_cells == 2) *(uint64_t *)buf = cpu_to_fdt64(rsize); else *(uint32_t *)buf = cpu_to_fdt32(rsize); buf += sizeof(uint32_t) * size_cells; } } /* Set property */ if ((err = fdt_setprop(fdtp, root, "memreserve", sb, len)) < 0) printf("Could not fixup 'memreserve' property.\n"); free(sb); } /* Count valid memory regions entries in sysinfo. */ realmrno = num; for (i = 0; i < num; i++) if (region[i].start == 0 && region[i].size == 0) realmrno--; if (realmrno == 0) { sprintf(command_errbuf, "Could not fixup '/memory' node : " "sysinfo doesn't contain valid memory regions info!\n"); return; } len = (addr_cells + size_cells) * realmrno * sizeof(uint32_t); sb = buf = (uint8_t *)malloc(len); if (!buf) return; bzero(buf, len); for (i = 0; i < num; i++) { curmr = ®ion[i]; if (curmr->size != 0) { /* Ensure endianness, and put cells into a buffer */ if (addr_cells == 2) *(uint64_t *)buf = cpu_to_fdt64(curmr->start); else *(uint32_t *)buf = cpu_to_fdt32(curmr->start); buf += sizeof(uint32_t) * addr_cells; if (size_cells == 2) *(uint64_t *)buf = cpu_to_fdt64(curmr->size); else *(uint32_t *)buf = cpu_to_fdt32(curmr->size); buf += sizeof(uint32_t) * size_cells; } } /* Set property */ if ((err = fdt_setprop(fdtp, memory, "reg", sb, len)) < 0) sprintf(command_errbuf, "Could not fixup '/memory' node.\n"); free(sb); } void fdt_fixup_stdout(const char *str) { char *ptr; int len, no, sero; const struct fdt_property *prop; char *tmp[10]; ptr = (char *)str + strlen(str) - 1; while (ptr > str && isdigit(*(str - 1))) str--; if (ptr == str) return; no = fdt_path_offset(fdtp, "/chosen"); if (no < 0) return; prop = fdt_get_property(fdtp, no, "stdout", &len); /* If /chosen/stdout does not extist, create it */ if (prop == NULL || (prop != NULL && len == 0)) { bzero(tmp, 10 * sizeof(char)); strcpy((char *)&tmp, "serial"); if (strlen(ptr) > 3) /* Serial number too long */ return; strncpy((char *)tmp + 6, ptr, 3); sero = fdt_path_offset(fdtp, (const char *)tmp); if (sero < 0) /* * If serial device we're trying to assign * stdout to doesn't exist in DT -- return. */ return; fdt_setprop(fdtp, no, "stdout", &tmp, strlen((char *)&tmp) + 1); fdt_setprop(fdtp, no, "stdin", &tmp, strlen((char *)&tmp) + 1); } } void fdt_load_dtb_overlays(const char *extras) { const char *s; /* Any extra overlays supplied by pre-loader environment */ if (extras != NULL && *extras != '\0') { printf("Loading DTB overlays: '%s'\n", extras); fdt_load_dtb_overlays_string(extras); } /* Any overlays supplied by loader environment */ s = getenv("fdt_overlays"); if (s != NULL && *s != '\0') { printf("Loading DTB overlays: '%s'\n", s); fdt_load_dtb_overlays_string(s); } } /* * Locate the blob, fix it up and return its location. */ static int fdt_fixup(void) { int chosen; debugf("fdt_fixup()\n"); if (fdtp == NULL && fdt_setup_fdtp() != 0) return (0); /* Create /chosen node (if not exists) */ if ((chosen = fdt_subnode_offset(fdtp, 0, "chosen")) == -FDT_ERR_NOTFOUND) chosen = fdt_add_subnode(fdtp, 0, "chosen"); /* Value assigned to fixup-applied does not matter. */ if (fdt_getprop(fdtp, chosen, "fixup-applied", NULL)) return (1); fdt_platform_fixups(); /* * Re-fetch the /chosen subnode; our fixups may apply overlays or add * nodes/properties that invalidate the offset we grabbed or created * above, so we can no longer trust it. */ chosen = fdt_subnode_offset(fdtp, 0, "chosen"); fdt_setprop(fdtp, chosen, "fixup-applied", NULL, 0); return (1); } /* * Copy DTB blob to specified location and return size */ int fdt_copy(vm_offset_t va) { int err; debugf("fdt_copy va 0x%08x\n", va); if (fdtp == NULL) { err = fdt_setup_fdtp(); if (err) { printf("No valid device tree blob found!\n"); return (0); } } if (fdt_fixup() == 0) return (0); COPYIN(fdtp, va, fdtp_size); return (fdtp_size); } int command_fdt_internal(int argc, char *argv[]) { cmdf_t *cmdh; int flags; int i, err; if (argc < 2) { command_errmsg = "usage is 'fdt []"; return (CMD_ERROR); } /* * Validate fdt . */ i = 0; cmdh = NULL; while (!(commands[i].name == NULL)) { if (strcmp(argv[1], commands[i].name) == 0) { /* found it */ cmdh = commands[i].handler; flags = commands[i].flags; break; } i++; } if (cmdh == NULL) { command_errmsg = "unknown command"; return (CMD_ERROR); } if (flags & CMD_REQUIRES_BLOB) { /* * Check if uboot env vars were parsed already. If not, do it now. */ if (fdt_fixup() == 0) return (CMD_ERROR); } /* * Call command handler. */ err = (*cmdh)(argc, argv); return (err); } static int fdt_cmd_addr(int argc, char *argv[]) { struct preloaded_file *fp; struct fdt_header *hdr; const char *addr; char *cp; fdt_to_load = NULL; if (argc > 2) addr = argv[2]; else { sprintf(command_errbuf, "no address specified"); return (CMD_ERROR); } hdr = (struct fdt_header *)strtoul(addr, &cp, 16); if (cp == addr) { snprintf(command_errbuf, sizeof(command_errbuf), "Invalid address: %s", addr); return (CMD_ERROR); } while ((fp = file_findfile(NULL, "dtb")) != NULL) { file_discard(fp); } fdt_to_load = hdr; return (CMD_OK); } static int fdt_cmd_cd(int argc, char *argv[]) { char *path; char tmp[FDT_CWD_LEN]; int len, o; path = (argc > 2) ? argv[2] : "/"; if (path[0] == '/') { len = strlen(path); if (len >= FDT_CWD_LEN) goto fail; } else { /* Handle path specification relative to cwd */ len = strlen(cwd) + strlen(path) + 1; if (len >= FDT_CWD_LEN) goto fail; strcpy(tmp, cwd); strcat(tmp, "/"); strcat(tmp, path); path = tmp; } o = fdt_path_offset(fdtp, path); if (o < 0) { snprintf(command_errbuf, sizeof(command_errbuf), "could not find node: '%s'", path); return (CMD_ERROR); } strcpy(cwd, path); return (CMD_OK); fail: snprintf(command_errbuf, sizeof(command_errbuf), "path too long: %d, max allowed: %d", len, FDT_CWD_LEN - 1); return (CMD_ERROR); } static int fdt_cmd_hdr(int argc __unused, char *argv[] __unused) { char line[80]; int ver; if (fdtp == NULL) { command_errmsg = "no device tree blob pointer?!"; return (CMD_ERROR); } ver = fdt_version(fdtp); pager_open(); sprintf(line, "\nFlattened device tree header (%p):\n", fdtp); if (pager_output(line)) goto out; sprintf(line, " magic = 0x%08x\n", fdt_magic(fdtp)); if (pager_output(line)) goto out; sprintf(line, " size = %d\n", fdt_totalsize(fdtp)); if (pager_output(line)) goto out; sprintf(line, " off_dt_struct = 0x%08x\n", fdt_off_dt_struct(fdtp)); if (pager_output(line)) goto out; sprintf(line, " off_dt_strings = 0x%08x\n", fdt_off_dt_strings(fdtp)); if (pager_output(line)) goto out; sprintf(line, " off_mem_rsvmap = 0x%08x\n", fdt_off_mem_rsvmap(fdtp)); if (pager_output(line)) goto out; sprintf(line, " version = %d\n", ver); if (pager_output(line)) goto out; sprintf(line, " last compatible version = %d\n", fdt_last_comp_version(fdtp)); if (pager_output(line)) goto out; if (ver >= 2) { sprintf(line, " boot_cpuid = %d\n", fdt_boot_cpuid_phys(fdtp)); if (pager_output(line)) goto out; } if (ver >= 3) { sprintf(line, " size_dt_strings = %d\n", fdt_size_dt_strings(fdtp)); if (pager_output(line)) goto out; } if (ver >= 17) { sprintf(line, " size_dt_struct = %d\n", fdt_size_dt_struct(fdtp)); if (pager_output(line)) goto out; } out: pager_close(); return (CMD_OK); } static int fdt_cmd_ls(int argc, char *argv[]) { const char *prevname[FDT_MAX_DEPTH] = { NULL }; const char *name; char *path; int i, o, depth; path = (argc > 2) ? argv[2] : NULL; if (path == NULL) path = cwd; o = fdt_path_offset(fdtp, path); if (o < 0) { snprintf(command_errbuf, sizeof(command_errbuf), "could not find node: '%s'", path); return (CMD_ERROR); } for (depth = 0; (o >= 0) && (depth >= 0); o = fdt_next_node(fdtp, o, &depth)) { name = fdt_get_name(fdtp, o, NULL); if (depth > FDT_MAX_DEPTH) { printf("max depth exceeded: %d\n", depth); continue; } prevname[depth] = name; /* Skip root (i = 1) when printing devices */ for (i = 1; i <= depth; i++) { if (prevname[i] == NULL) break; if (strcmp(cwd, "/") == 0) printf("/"); printf("%s", prevname[i]); } printf("\n"); } return (CMD_OK); } static __inline int isprint(int c) { return (c >= ' ' && c <= 0x7e); } static int fdt_isprint(const void *data, int len, int *count) { const char *d; char ch; int yesno, i; if (len == 0) return (0); d = (const char *)data; if (d[len - 1] != '\0') return (0); *count = 0; yesno = 1; for (i = 0; i < len; i++) { ch = *(d + i); if (isprint(ch) || (ch == '\0' && i > 0)) { /* Count strings */ if (ch == '\0') (*count)++; continue; } yesno = 0; break; } return (yesno); } static int fdt_data_str(const void *data, int len, int count, char **buf) { char *b, *tmp; const char *d; int buf_len, i, l; /* * Calculate the length for the string and allocate memory. * * Note that 'len' already includes at least one terminator. */ buf_len = len; if (count > 1) { /* * Each token had already a terminator buried in 'len', but we * only need one eventually, don't count space for these. */ buf_len -= count - 1; /* Each consecutive token requires a ", " separator. */ buf_len += count * 2; } /* Add some space for surrounding double quotes. */ buf_len += count * 2; /* Note that string being put in 'tmp' may be as big as 'buf_len'. */ b = (char *)malloc(buf_len); tmp = (char *)malloc(buf_len); if (b == NULL) goto error; if (tmp == NULL) { free(b); goto error; } b[0] = '\0'; /* * Now that we have space, format the string. */ i = 0; do { d = (const char *)data + i; l = strlen(d) + 1; sprintf(tmp, "\"%s\"%s", d, (i + l) < len ? ", " : ""); strcat(b, tmp); i += l; } while (i < len); *buf = b; free(tmp); return (0); error: return (1); } static int fdt_data_cell(const void *data, int len, char **buf) { char *b, *tmp; const uint32_t *c; int count, i, l; /* Number of cells */ count = len / 4; /* * Calculate the length for the string and allocate memory. */ /* Each byte translates to 2 output characters */ l = len * 2; if (count > 1) { /* Each consecutive cell requires a " " separator. */ l += (count - 1) * 1; } /* Each cell will have a "0x" prefix */ l += count * 2; /* Space for surrounding <> and terminator */ l += 3; b = (char *)malloc(l); tmp = (char *)malloc(l); if (b == NULL) goto error; if (tmp == NULL) { free(b); goto error; } b[0] = '\0'; strcat(b, "<"); for (i = 0; i < len; i += 4) { c = (const uint32_t *)((const uint8_t *)data + i); sprintf(tmp, "0x%08x%s", fdt32_to_cpu(*c), i < (len - 4) ? " " : ""); strcat(b, tmp); } strcat(b, ">"); *buf = b; free(tmp); return (0); error: return (1); } static int fdt_data_bytes(const void *data, int len, char **buf) { char *b, *tmp; const char *d; int i, l; /* * Calculate the length for the string and allocate memory. */ /* Each byte translates to 2 output characters */ l = len * 2; if (len > 1) /* Each consecutive byte requires a " " separator. */ l += (len - 1) * 1; /* Each byte will have a "0x" prefix */ l += len * 2; /* Space for surrounding [] and terminator. */ l += 3; b = (char *)malloc(l); tmp = (char *)malloc(l); if (b == NULL) goto error; if (tmp == NULL) { free(b); goto error; } b[0] = '\0'; strcat(b, "["); for (i = 0, d = data; i < len; i++) { sprintf(tmp, "0x%02x%s", d[i], i < len - 1 ? " " : ""); strcat(b, tmp); } strcat(b, "]"); *buf = b; free(tmp); return (0); error: return (1); } static int fdt_data_fmt(const void *data, int len, char **buf) { int count; if (len == 0) { *buf = NULL; return (1); } if (fdt_isprint(data, len, &count)) return (fdt_data_str(data, len, count, buf)); else if ((len % 4) == 0) return (fdt_data_cell(data, len, buf)); else return (fdt_data_bytes(data, len, buf)); } static int fdt_prop(int offset) { char *line, *buf; const struct fdt_property *prop; const char *name; const void *data; int len, rv; line = NULL; prop = fdt_offset_ptr(fdtp, offset, sizeof(*prop)); if (prop == NULL) return (1); name = fdt_string(fdtp, fdt32_to_cpu(prop->nameoff)); len = fdt32_to_cpu(prop->len); rv = 0; buf = NULL; if (len == 0) { /* Property without value */ line = (char *)malloc(strlen(name) + 2); if (line == NULL) { rv = 2; goto out2; } sprintf(line, "%s\n", name); goto out1; } /* * Process property with value */ data = prop->data; if (fdt_data_fmt(data, len, &buf) != 0) { rv = 3; goto out2; } line = (char *)malloc(strlen(name) + strlen(FDT_PROP_SEP) + strlen(buf) + 2); if (line == NULL) { sprintf(command_errbuf, "could not allocate space for string"); rv = 4; goto out2; } sprintf(line, "%s" FDT_PROP_SEP "%s\n", name, buf); out1: pager_open(); pager_output(line); pager_close(); out2: if (buf) free(buf); if (line) free(line); return (rv); } static int fdt_modprop(int nodeoff, char *propname, void *value, char mode) { uint32_t cells[100]; const char *buf; int len, rv; const struct fdt_property *p; p = fdt_get_property(fdtp, nodeoff, propname, NULL); if (p != NULL) { if (mode == 1) { /* Adding inexistant value in mode 1 is forbidden */ sprintf(command_errbuf, "property already exists!"); return (CMD_ERROR); } } else if (mode == 0) { sprintf(command_errbuf, "property does not exist!"); return (CMD_ERROR); } rv = 0; buf = value; switch (*buf) { case '&': /* phandles */ break; case '<': /* Data cells */ len = fdt_strtovect(buf, (void *)&cells, 100, sizeof(uint32_t)); rv = fdt_setprop(fdtp, nodeoff, propname, &cells, len * sizeof(uint32_t)); break; case '[': /* Data bytes */ len = fdt_strtovect(buf, (void *)&cells, 100, sizeof(uint8_t)); rv = fdt_setprop(fdtp, nodeoff, propname, &cells, len * sizeof(uint8_t)); break; case '"': default: /* Default -- string */ rv = fdt_setprop_string(fdtp, nodeoff, propname, value); break; } if (rv != 0) { if (rv == -FDT_ERR_NOSPACE) sprintf(command_errbuf, "Device tree blob is too small!\n"); else sprintf(command_errbuf, "Could not add/modify property!\n"); } return (rv); } /* Merge strings from argv into a single string */ static int fdt_merge_strings(int argc, char *argv[], int start, char **buffer) { char *buf; int i, idx, sz; *buffer = NULL; sz = 0; for (i = start; i < argc; i++) sz += strlen(argv[i]); /* Additional bytes for whitespaces between args */ sz += argc - start; buf = (char *)malloc(sizeof(char) * sz); if (buf == NULL) { sprintf(command_errbuf, "could not allocate space " "for string"); return (1); } bzero(buf, sizeof(char) * sz); idx = 0; for (i = start, idx = 0; i < argc; i++) { strcpy(buf + idx, argv[i]); idx += strlen(argv[i]); buf[idx] = ' '; idx++; } buf[sz - 1] = '\0'; *buffer = buf; return (0); } /* Extract offset and name of node/property from a given path */ static int fdt_extract_nameloc(char **pathp, char **namep, int *nodeoff) { int o; char *path = *pathp, *name = NULL, *subpath = NULL; subpath = strrchr(path, '/'); if (subpath == NULL) { o = fdt_path_offset(fdtp, cwd); name = path; path = (char *)&cwd; } else { *subpath = '\0'; if (strlen(path) == 0) path = cwd; name = subpath + 1; o = fdt_path_offset(fdtp, path); } if (strlen(name) == 0) { sprintf(command_errbuf, "name not specified"); return (1); } if (o < 0) { snprintf(command_errbuf, sizeof(command_errbuf), "could not find node: '%s'", path); return (1); } *namep = name; *nodeoff = o; *pathp = path; return (0); } static int fdt_cmd_prop(int argc, char *argv[]) { char *path, *propname, *value; int o, next, depth, rv; uint32_t tag; path = (argc > 2) ? argv[2] : NULL; value = NULL; if (argc > 3) { /* Merge property value strings into one */ if (fdt_merge_strings(argc, argv, 3, &value) != 0) return (CMD_ERROR); } else value = NULL; if (path == NULL) path = cwd; rv = CMD_OK; if (value) { /* If value is specified -- try to modify prop. */ if (fdt_extract_nameloc(&path, &propname, &o) != 0) return (CMD_ERROR); rv = fdt_modprop(o, propname, value, 0); if (rv) return (CMD_ERROR); return (CMD_OK); } /* User wants to display properties */ o = fdt_path_offset(fdtp, path); if (o < 0) { snprintf(command_errbuf, sizeof(command_errbuf), "could not find node: '%s'", path); rv = CMD_ERROR; goto out; } depth = 0; while (depth >= 0) { tag = fdt_next_tag(fdtp, o, &next); switch (tag) { case FDT_NOP: break; case FDT_PROP: if (depth > 1) /* Don't process properties of nested nodes */ break; if (fdt_prop(o) != 0) { sprintf(command_errbuf, "could not process " "property"); rv = CMD_ERROR; goto out; } break; case FDT_BEGIN_NODE: depth++; if (depth > FDT_MAX_DEPTH) { printf("warning: nesting too deep: %d\n", depth); goto out; } break; case FDT_END_NODE: depth--; if (depth == 0) /* * This is the end of our starting node, force * the loop finish. */ depth--; break; } o = next; } out: return (rv); } static int fdt_cmd_mkprop(int argc, char *argv[]) { int o; char *path, *propname, *value; path = (argc > 2) ? argv[2] : NULL; value = NULL; if (argc > 3) { /* Merge property value strings into one */ if (fdt_merge_strings(argc, argv, 3, &value) != 0) return (CMD_ERROR); } else value = NULL; if (fdt_extract_nameloc(&path, &propname, &o) != 0) return (CMD_ERROR); if (fdt_modprop(o, propname, value, 1)) return (CMD_ERROR); return (CMD_OK); } static int fdt_cmd_rm(int argc, char *argv[]) { int o, rv; char *path = NULL, *propname; if (argc > 2) path = argv[2]; else { sprintf(command_errbuf, "no node/property name specified"); return (CMD_ERROR); } o = fdt_path_offset(fdtp, path); if (o < 0) { /* If node not found -- try to find & delete property */ if (fdt_extract_nameloc(&path, &propname, &o) != 0) return (CMD_ERROR); if ((rv = fdt_delprop(fdtp, o, propname)) != 0) { snprintf(command_errbuf, sizeof(command_errbuf), "could not delete %s\n", (rv == -FDT_ERR_NOTFOUND) ? "(property/node does not exist)" : ""); return (CMD_ERROR); } else return (CMD_OK); } /* If node exists -- remove node */ rv = fdt_del_node(fdtp, o); if (rv) { sprintf(command_errbuf, "could not delete node"); return (CMD_ERROR); } return (CMD_OK); } static int fdt_cmd_mknode(int argc, char *argv[]) { int o, rv; char *path = NULL, *nodename = NULL; if (argc > 2) path = argv[2]; else { sprintf(command_errbuf, "no node name specified"); return (CMD_ERROR); } if (fdt_extract_nameloc(&path, &nodename, &o) != 0) return (CMD_ERROR); rv = fdt_add_subnode(fdtp, o, nodename); if (rv < 0) { if (rv == -FDT_ERR_NOSPACE) sprintf(command_errbuf, "Device tree blob is too small!\n"); else sprintf(command_errbuf, "Could not add node!\n"); return (CMD_ERROR); } return (CMD_OK); } static int fdt_cmd_pwd(int argc, char *argv[]) { char line[FDT_CWD_LEN]; pager_open(); sprintf(line, "%s\n", cwd); pager_output(line); pager_close(); return (CMD_OK); } static int fdt_cmd_mres(int argc, char *argv[]) { uint64_t start, size; int i, total; char line[80]; pager_open(); total = fdt_num_mem_rsv(fdtp); if (total > 0) { if (pager_output("Reserved memory regions:\n")) goto out; for (i = 0; i < total; i++) { fdt_get_mem_rsv(fdtp, i, &start, &size); sprintf(line, "reg#%d: (start: 0x%jx, size: 0x%jx)\n", i, start, size); if (pager_output(line)) goto out; } } else pager_output("No reserved memory regions\n"); out: pager_close(); return (CMD_OK); } static int 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); } Index: head/stand/fdt/fdt_platform.h =================================================================== --- head/stand/fdt/fdt_platform.h (revision 348193) +++ head/stand/fdt/fdt_platform.h (revision 348194) @@ -1,58 +1,61 @@ /*- * Copyright (c) 2014 Andrew Turner * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. * * $FreeBSD$ */ #ifndef FDT_PLATFORM_H #define FDT_PLATFORM_H struct fdt_header; struct fdt_mem_region { unsigned long start; unsigned long size; }; #define TMP_MAX_ETH 8 int fdt_copy(vm_offset_t); void fdt_fixup_cpubusfreqs(unsigned long, unsigned long); void fdt_fixup_ethernet(const char *, char *, int); void fdt_fixup_memory(struct fdt_mem_region *, size_t); void fdt_fixup_stdout(const char *); void fdt_apply_overlays(void); int fdt_load_dtb_addr(struct fdt_header *); int fdt_load_dtb_file(const char *); void fdt_load_dtb_overlays(const char *); int fdt_setup_fdtp(void); int fdt_is_setup(void); /* The platform library needs to implement these functions */ 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 */