block: properly handle vlist_add() for label, uuid and device cases
[project/ubox.git] / kmodloader.c
1 /*
2  * Copyright (C) 2013 Felix Fietkau <nbd@openwrt.org>
3  * Copyright (C) 2013 John Crispin <blogic@openwrt.org>
4  *
5  * This program is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU Lesser General Public License version 2.1
7  * as published by the Free Software Foundation
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  */
14
15 #define _GNU_SOURCE
16 #include <sys/syscall.h>
17 #include <sys/mman.h>
18 #include <sys/utsname.h>
19
20 #include <stdlib.h>
21 #include <unistd.h>
22 #include <sys/syscall.h>
23 #include <sys/types.h>
24 #include <values.h>
25 #include <errno.h>
26 #include <stdio.h>
27 #include <string.h>
28 #include <sys/stat.h>
29 #include <fcntl.h>
30 #include <syslog.h>
31 #include <libgen.h>
32 #include <glob.h>
33 #include <elf.h>
34
35 #include <libubox/avl.h>
36 #include <libubox/avl-cmp.h>
37 #include <libubox/utils.h>
38
39 #define DEF_MOD_PATH "/lib/modules/%s/"
40
41 #define LOG(fmt, ...) do { \
42         syslog(LOG_INFO, fmt, ## __VA_ARGS__); \
43         printf("kmod: "fmt, ## __VA_ARGS__); \
44         } while (0)
45
46
47 enum {
48         SCANNED,
49         PROBE,
50         LOADED,
51 };
52
53 struct module {
54         struct avl_node avl;
55
56         char *name;
57         char *depends;
58
59         int size;
60         int usage;
61         int state;
62         int error;
63 };
64
65 static struct avl_tree modules;
66 static char *prefix = "";
67
68 static struct module *find_module(const char *name)
69 {
70         struct module *m;
71         return avl_find_element(&modules, name, m, avl);
72 }
73
74 static void free_modules(void)
75 {
76         struct module *m, *tmp;
77
78         avl_remove_all_elements(&modules, m, avl, tmp)
79                 free(m);
80 }
81
82 static char* get_module_path(char *name)
83 {
84         static char path[256];
85         struct utsname ver;
86         struct stat s;
87         char *t;
88
89         if (!stat(name, &s))
90                 return name;
91
92         uname(&ver);
93         snprintf(path, 256, "%s" DEF_MOD_PATH "%s.ko", prefix, ver.release, name);
94
95         if (!stat(path, &s))
96                 return path;
97
98         t = name;
99         while (t && *t) {
100                 if (*t == '_')
101                         *t = '-';
102                 t++;
103         }
104
105         snprintf(path, 256, "%s" DEF_MOD_PATH "%s.ko", prefix, ver.release, name);
106
107         if (!stat(path, &s))
108                 return path;
109
110         return NULL;
111 }
112
113 static char* get_module_name(char *path)
114 {
115         static char name[32];
116         char *t;
117
118         strncpy(name, basename(path), sizeof(name));
119
120         t = strstr(name, ".ko");
121         if (t)
122                 *t = '\0';
123         t = name;
124         while (t && *t) {
125                 if (*t == '-')
126                         *t = '_';
127                 t++;
128         }
129
130         return name;
131 }
132
133 #if __WORDSIZE == 64
134 static int elf_find_section(char *map, const char *section, unsigned int *offset, unsigned int *size)
135 {
136         const char *secnames;
137         Elf64_Ehdr *e;
138         Elf64_Shdr *sh;
139         int i;
140
141         e = (Elf64_Ehdr *) map;
142         sh = (Elf64_Shdr *) (map + e->e_shoff);
143
144         secnames = map + sh[e->e_shstrndx].sh_offset;
145         for (i = 0; i < e->e_shnum; i++) {
146                 if (!strcmp(section, secnames + sh[i].sh_name)) {
147                         *size = sh[i].sh_size;
148                         *offset = sh[i].sh_offset;
149                         return 0;
150                 }
151         }
152
153         return -1;
154 }
155 #else
156 static int elf_find_section(char *map, const char *section, unsigned int *offset, unsigned int *size)
157 {
158         const char *secnames;
159         Elf32_Ehdr *e;
160         Elf32_Shdr *sh;
161         int i;
162
163         e = (Elf32_Ehdr *) map;
164         sh = (Elf32_Shdr *) (map + e->e_shoff);
165
166         secnames = map + sh[e->e_shstrndx].sh_offset;
167         for (i = 0; i < e->e_shnum; i++) {
168                 if (!strcmp(section, secnames + sh[i].sh_name)) {
169                         *size = sh[i].sh_size;
170                         *offset = sh[i].sh_offset;
171                         return 0;
172                 }
173         }
174
175         return -1;
176 }
177 #endif
178
179 static struct module *
180 alloc_module(const char *name, const char *depends, int size)
181 {
182         struct module *m;
183         char *_name, *_dep;
184
185         m = calloc_a(sizeof(*m),
186                 &_name, strlen(name) + 1,
187                 &_dep, depends ? strlen(depends) + 1 : 0);
188         if (!m)
189                 return NULL;
190
191         m->avl.key = m->name = strcpy(_name, name);
192
193         if (depends)
194                 m->depends = strcpy(_dep, depends);
195
196         m->size = size;
197         avl_insert(&modules, &m->avl);
198         return m;
199 }
200
201 static int scan_loaded_modules(void)
202 {
203         size_t buf_len = 0;
204         char *buf = NULL;
205         FILE *fp;
206
207         fp = fopen("/proc/modules", "r");
208         if (!fp) {
209                 LOG("failed to open /proc/modules\n");
210                 return -1;
211         }
212
213         while (getline(&buf, &buf_len, fp) > 0) {
214                 struct module m;
215                 struct module *n;
216
217                 m.name = strtok(buf, " ");
218                 m.size = atoi(strtok(NULL, " "));
219                 m.usage = atoi(strtok(NULL, " "));
220                 m.depends = strtok(NULL, " ");
221
222                 if (!m.name || !m.depends)
223                         continue;
224
225                 n = alloc_module(m.name, m.depends, m.size);
226                 n->usage = m.usage;
227                 n->state = LOADED;
228         }
229         free(buf);
230         fclose(fp);
231
232         return 0;
233 }
234
235 static struct module* get_module_info(const char *module, const char *name)
236 {
237         int fd = open(module, O_RDONLY);
238         unsigned int offset, size;
239         char *map, *strings, *dep = NULL;
240         struct module *m;
241         struct stat s;
242
243         if (!fd) {
244                 LOG("failed to open %s\n", module);
245                 return NULL;
246         }
247
248         if (fstat(fd, &s) == -1) {
249                 LOG("failed to stat %s\n", module);
250                 return NULL;
251         }
252
253         map = mmap(NULL, s.st_size, PROT_READ, MAP_PRIVATE, fd, 0);
254         if (map == MAP_FAILED) {
255                 LOG("failed to mmap %s\n", module);
256                 return NULL;
257         }
258
259         if (elf_find_section(map, ".modinfo", &offset, &size)) {
260                 LOG("failed to load the .modinfo section from %s\n", module);
261                 return NULL;
262         }
263
264         strings = map + offset;
265         while (strings && (strings < map + offset + size)) {
266                 char *sep;
267                 int len;
268
269                 while (!strings[0])
270                         strings++;
271                 sep = strstr(strings, "=");
272                 if (!sep)
273                         break;
274                 len = sep - strings;
275                 sep++;
276                 if (!strncmp(strings, "depends=", len + 1))
277                         dep = sep;
278                 strings = &sep[strlen(sep)];
279         }
280
281         m = alloc_module(name, dep, s.st_size);
282         if (!m)
283                 return NULL;
284
285         m->state = SCANNED;
286
287         return m;
288 }
289
290 static int scan_module_folder(void)
291 {
292         int gl_flags = GLOB_NOESCAPE | GLOB_MARK;
293         struct utsname ver;
294         char *path;
295         glob_t gl;
296         int j;
297
298         uname(&ver);
299         path = alloca(sizeof(DEF_MOD_PATH "*.ko") + strlen(ver.release) + 1);
300         sprintf(path, DEF_MOD_PATH "*.ko", ver.release);
301
302         if (glob(path, gl_flags, NULL, &gl) < 0)
303                 return -1;
304
305         for (j = 0; j < gl.gl_pathc; j++) {
306                 char *name = get_module_name(gl.gl_pathv[j]);
307                 struct module *m;
308
309                 if (!name)
310                         continue;
311
312                 m = find_module(name);
313                 if (!m)
314                         get_module_info(gl.gl_pathv[j], name);
315         }
316
317         globfree(&gl);
318         free(path);
319
320         return 0;
321 }
322
323 static int print_modinfo(char *module)
324 {
325         int fd = open(module, O_RDONLY);
326         unsigned int offset, size;
327         struct stat s;
328         char *map, *strings;
329
330         if (!fd) {
331                 LOG("failed to open %s\n", module);
332                 return -1;
333         }
334
335         if (fstat(fd, &s) == -1) {
336                 LOG("failed to stat %s\n", module);
337                 return -1;
338         }
339
340         map = mmap(NULL, s.st_size, PROT_READ, MAP_PRIVATE, fd, 0);
341         if (map == MAP_FAILED) {
342                 LOG("failed to mmap %s\n", module);
343                 return -1;
344         }
345
346         if (elf_find_section(map, ".modinfo", &offset, &size)) {
347                 LOG("failed to load the .modinfo section from %s\n", module);
348                 return -1;
349         }
350
351         strings = map + offset;
352         printf("module:\t\t%s\n", module);
353         while (strings && (strings < map + offset + size)) {
354                 char *dup = NULL;
355                 char *sep;
356
357                 while (!strings[0])
358                         strings++;
359                 sep = strstr(strings, "=");
360                 if (!sep)
361                         break;
362                 dup = strndup(strings, sep - strings);
363                 sep++;
364                 if (strncmp(strings, "parm", 4)) {
365                         if (strlen(dup) < 7)
366                                 printf("%s:\t\t%s\n",  dup, sep);
367                         else
368                                 printf("%s:\t%s\n",  dup, sep);
369                 }
370                 strings = &sep[strlen(sep)];
371                 if (dup)
372                         free(dup);
373         }
374
375         return 0;
376 }
377
378 static int deps_available(struct module *m, int verbose)
379 {
380         char *deps, *_deps;
381         char *comma;
382         int err = 0;
383
384         if (!strcmp(m->depends, "-") || !strcmp(m->depends, ""))
385                 return 0;
386
387         _deps = deps = strdup(m->depends);
388
389         do {
390                 comma = strstr(deps, ",");
391                 if (comma)
392                         *comma = '\0';
393
394                 m = find_module(deps);
395
396                 if (verbose && !m)
397                         LOG("missing dependency %s\n", deps);
398                 if (verbose && m && (m->state != LOADED))
399                         LOG("dependency not loaded %s\n", deps);
400                 if (!m || (m->state != LOADED))
401                         err++;
402                 if (comma)
403                         deps = ++comma;
404         } while (comma);
405
406         free(_deps);
407
408         return err;
409 }
410
411 static int insert_module(char *path, const char *options)
412 {
413         void *data = 0;
414         struct stat s;
415         int fd, ret = -1;
416
417         if (stat(path, &s)) {
418                 LOG("missing module %s\n", path);
419                 return ret;
420         }
421
422         fd = open(path, O_RDONLY);
423         if (!fd) {
424                 LOG("cannot open %s\n", path);
425                 return ret;
426         }
427
428         data = malloc(s.st_size);
429         if (read(fd, data, s.st_size) == s.st_size)
430                 ret = syscall(__NR_init_module, data, s.st_size, options);
431         else
432                 LOG("failed to read full module %s\n", path);
433
434         close(fd);
435         free(data);
436
437         return ret;
438 }
439
440 static void load_moddeps(struct module *_m)
441 {
442         char *deps, *_deps;
443         struct module *m;
444         char *comma;
445
446         if (!strcmp(_m->depends, "-") || !strcmp(_m->depends, ""))
447                 return;
448
449         _deps = deps = strdup(_m->depends);
450
451         do {
452                 char *t = deps;
453
454                 comma = strstr(deps, ",");
455                 if (comma)
456                         *comma = '\0';
457
458                 m = find_module(deps);
459
460                 if (!m) {
461                         while (t && *t) {
462                                 if (*t == '-')
463                                         *t = '_';
464                                 t++;
465                         }
466                         m = find_module(deps);
467                 }
468
469                 if (!m)
470                         LOG("failed to find dependency %s\n", deps);
471                 if (m && (m->state != LOADED)) {
472                         m->state = PROBE;
473                         load_moddeps(m);
474                 }
475
476                 if (comma)
477                         deps = ++comma;
478         } while (comma);
479
480         free(_deps);
481 }
482
483 static int load_modprobe(void)
484 {
485         int loaded, todo;
486         struct module *m;
487
488         avl_for_each_element(&modules, m, avl)
489                 if (m->state == PROBE)
490                         load_moddeps(m);
491
492         do {
493                 loaded = 0;
494                 todo = 0;
495                 avl_for_each_element(&modules, m, avl) {
496                         if ((m->state == PROBE) && (!deps_available(m, 0))) {
497                                 if (!insert_module(get_module_path(m->name), "")) {
498                                         m->state = LOADED;
499                                         m->error = 0;
500                                         loaded++;
501                                         continue;
502                                 }
503                                 m->error = 1;
504                         }
505
506                         if ((m->state == PROBE) || m->error)
507                                 todo++;
508                 }
509         } while (loaded);
510
511         return todo;
512 }
513
514 static int print_insmod_usage(void)
515 {
516         LOG("Usage:\n\tinsmod filename [args]\n");
517
518         return -1;
519 }
520
521 static int print_usage(char *arg)
522 {
523         LOG("Usage:\n\t%s module\n", arg);
524
525         return -1;
526 }
527
528 static int main_insmod(int argc, char **argv)
529 {
530         char *name, *cur, *options;
531         int i, ret, len;
532
533         if (argc < 2)
534                 return print_insmod_usage();
535
536         name = get_module_name(argv[1]);
537         if (!name) {
538                 LOG("cannot find module - %s\n", argv[1]);
539                 return -1;
540         }
541
542         if (scan_loaded_modules())
543                 return -1;
544
545         if (find_module(name)) {
546                 LOG("module is already loaded - %s\n", name);
547                 return -1;
548
549         }
550
551         free_modules();
552
553         for (len = 0, i = 2; i < argc; i++)
554                 len += strlen(argv[i]) + 1;
555
556         options = malloc(len);
557         options[0] = 0;
558         cur = options;
559         for (i = 2; i < argc; i++) {
560                 if (options[0]) {
561                         *cur = ' ';
562                         cur++;
563                 }
564                 cur += sprintf(cur, "%s", argv[i]);
565         }
566
567         ret = insert_module(get_module_path(name), options);
568         free(options);
569
570         if (ret)
571                 LOG("failed to insert %s\n", get_module_path(name));
572
573         return ret;
574 }
575
576 static int main_rmmod(int argc, char **argv)
577 {
578         struct module *m;
579         char *name;
580         int ret;
581
582         if (argc != 2)
583                 return print_usage("rmmod");
584
585         if (scan_loaded_modules())
586                 return -1;
587
588         name = get_module_name(argv[1]);
589         m = find_module(name);
590         if (!m) {
591                 LOG("module is not loaded\n");
592                 return -1;
593         }
594         free_modules();
595
596         ret = syscall(__NR_delete_module, name, 0);
597
598         if (ret)
599                 LOG("unloading the module failed\n");
600
601         return ret;
602 }
603
604 static int main_lsmod(int argc, char **argv)
605 {
606         struct module *m;
607
608         if (scan_loaded_modules())
609                 return -1;
610
611         avl_for_each_element(&modules, m, avl)
612                 if (m->state == LOADED)
613                         printf("%-20s%8d%3d %s\n",
614                                 m->name, m->size, m->usage,
615                                 (*m->depends == '-') ? ("") : (m->depends));
616
617         free_modules();
618
619         return 0;
620 }
621
622 static int main_modinfo(int argc, char **argv)
623 {
624         char *module;
625
626         if (argc != 2)
627                 return print_usage("modinfo");
628
629         module = get_module_path(argv[1]);
630         if (!module) {
631                 LOG("cannot find module - %s\n", argv[1]);
632                 return -1;
633         }
634
635         print_modinfo(module);
636
637         return 0;
638 }
639
640 static int main_modprobe(int argc, char **argv)
641 {
642         struct module *m;
643         char *name;
644
645         if (argc != 2)
646                 return print_usage("modprobe");
647
648         if (scan_loaded_modules())
649                 return -1;
650
651         if (scan_module_folder())
652                 return -1;
653
654         name = get_module_name(argv[1]);
655         m = find_module(name);
656         if (m && m->state == LOADED) {
657                 LOG("%s is already loaded\n", name);
658                 return -1;
659         } else if (!m) {
660                 LOG("failed to find a module named %s\n", name);
661         } else {
662                 int fail;
663
664                 m->state = PROBE;
665
666                 fail = load_modprobe();
667
668                 if (fail) {
669                         LOG("%d module%s could not be probed\n",
670                                         fail, (fail == 1) ? ("") : ("s"));
671
672                         avl_for_each_element(&modules, m, avl)
673                                 if ((m->state == PROBE) || m->error)
674                                         LOG("- %s\n", m->name);
675                 }
676         }
677
678         free_modules();
679
680         return 0;
681 }
682
683 static int main_loader(int argc, char **argv)
684 {
685         int gl_flags = GLOB_NOESCAPE | GLOB_MARK;
686         char *dir = "/etc/modules.d/*";
687         struct module *m;
688         glob_t gl;
689         char *path;
690         int fail, j;
691
692         if (argc > 1)
693                 dir = argv[1];
694
695         if (argc > 2)
696                 prefix = argv[2];
697
698         path = malloc(strlen(dir) + 2);
699         strcpy(path, dir);
700         strcat(path, "*");
701
702         if (scan_loaded_modules())
703                 return -1;
704
705         if (scan_module_folder())
706                 return -1;
707
708         syslog(0, "kmodloader: loading kernel modules from %s\n", path);
709
710         if (glob(path, gl_flags, NULL, &gl) < 0)
711                 goto out;
712
713         for (j = 0; j < gl.gl_pathc; j++) {
714                 FILE *fp = fopen(gl.gl_pathv[j], "r");
715                 size_t mod_len = 0;
716                 char *mod = NULL;
717
718                 if (!fp) {
719                         LOG("failed to open %s\n", gl.gl_pathv[j]);
720                         continue;
721                 }
722
723                 while (getline(&mod, &mod_len, fp) > 0) {
724                         char *nl = strchr(mod, '\n');
725                         struct module *m;
726                         char *opts;
727
728                         if (nl)
729                                 *nl = '\0';
730
731                         opts = strchr(mod, ' ');
732                         if (opts)
733                                 *opts++ = '\0';
734
735                         m = find_module(get_module_name(mod));
736                         if (!m || (m->state == LOADED))
737                                 continue;
738
739                         m->state = PROBE;
740                         if (basename(gl.gl_pathv[j])[0] - '0' <= 9)
741                                 load_modprobe();
742
743                 }
744                 free(mod);
745                 fclose(fp);
746         }
747
748         fail = load_modprobe();
749
750         if (fail) {
751                 LOG("%d module%s could not be probed\n",
752                                 fail, (fail == 1) ? ("") : ("s"));
753
754                 avl_for_each_element(&modules, m, avl)
755                         if ((m->state == PROBE) || (m->error))
756                                 LOG("- %s - %d\n", m->name, deps_available(m, 1));
757         }
758
759 out:
760         globfree(&gl);
761         free(path);
762
763         return 0;
764 }
765
766 int main(int argc, char **argv)
767 {
768         char *exec = basename(*argv);
769
770         avl_init(&modules, avl_strcmp, false, NULL);
771         if (!strcmp(exec, "insmod"))
772                 return main_insmod(argc, argv);
773
774         if (!strcmp(exec, "rmmod"))
775                 return main_rmmod(argc, argv);
776
777         if (!strcmp(exec, "lsmod"))
778                 return main_lsmod(argc, argv);
779
780         if (!strcmp(exec, "modinfo"))
781                 return main_modinfo(argc, argv);
782
783         if (!strcmp(exec, "modprobe"))
784                 return main_modprobe(argc, argv);
785
786         return main_loader(argc, argv);
787 }