projects / project / ubox.git / blob
? search:


065ac82cccbc4a96fa5a744a42ff95de8c9c0d78
[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 <libgen.h>
31 #include <glob.h>
32 #include <elf.h>
33
34 #include <libubox/avl.h>
35 #include <libubox/avl-cmp.h>
36 #include <libubox/utils.h>
37 #include <libubox/ulog.h>
38
39 #define DEF_MOD_PATH "/modules/%s/"
40
41 enum {
42 SCANNED,
43 PROBE,
44 LOADED,
45 };
46
47 struct module {
48 struct avl_node avl;
49
50 char *name;
51 char *depends;
52 char *opts;
53
54 int size;
55 int usage;
56 int state;
57 int error;
58 };
59
60 static struct avl_tree modules;
61
62 static char **module_folders = NULL;
63
64 static int init_module_folders(void)
65 {
66 int n = 0;
67 struct stat st;
68 struct utsname ver;
69 char *s, *e, *p, path[256], ldpath[256];
70
71 e = ldpath;
72 s = getenv("LD_LIBRARY_PATH");
73
74 if (s)
75 e += snprintf(ldpath, sizeof(ldpath), "%s:", s);
76
77 e += snprintf(e, sizeof(ldpath) - (e - ldpath), "/lib");
78
79 uname(&ver);
80
81 for (s = p = ldpath; p <= e; p++) {
82 if (*p != ':' && *p != '\0')
83 continue;
84
85 *p = 0;
86 snprintf(path, sizeof(path), "%s" DEF_MOD_PATH, s, ver.release);
87
88 if (!stat(path, &st) && S_ISDIR(st.st_mode)) {
89 module_folders = realloc(module_folders, sizeof(p) * (n + 2));
90
91 if (!module_folders)
92 return -1;
93
94 module_folders[n++] = strdup(path);
95 }
96
97 s = p + 1;
98 }
99
100 if (!module_folders)
101 return -1;
102
103 module_folders[n] = NULL;
104 return 0;
105 }
106
107 static struct module *find_module(const char *name)
108 {
109 struct module *m;
110 return avl_find_element(&modules, name, m, avl);
111 }
112
113 static void free_modules(void)
114 {
115 struct module *m, *tmp;
116
117 avl_remove_all_elements(&modules, m, avl, tmp)
118 free(m);
119 }
120
121 static char* get_module_path(char *name)
122 {
123 char **p;
124 static char path[256];
125 struct stat s;
126
127 if (!stat(name, &s) && S_ISREG(s.st_mode))
128 return name;
129
130 for (p = module_folders; *p; p++) {
131 snprintf(path, sizeof(path), "%s%s.ko", *p, name);
132 if (!stat(path, &s) && S_ISREG(s.st_mode))
133 return path;
134 }
135
136 return NULL;
137 }
138
139 static char* get_module_name(char *path)
140 {
141 static char name[32];
142 char *t;
143
144 strncpy(name, basename(path), sizeof(name));
145
146 t = strstr(name, ".ko");
147 if (t)
148 *t = '\0';
149
150 return name;
151 }
152
153 static int elf64_find_section(char *map, const char *section, unsigned int *offset, unsigned int *size)
154 {
155 const char *secnames;
156 Elf64_Ehdr *e;
157 Elf64_Shdr *sh;
158 int i;
159
160 e = (Elf64_Ehdr *) map;
161 sh = (Elf64_Shdr *) (map + e->e_shoff);
162
163 secnames = map + sh[e->e_shstrndx].sh_offset;
164 for (i = 0; i < e->e_shnum; i++) {
165 if (!strcmp(section, secnames + sh[i].sh_name)) {
166 *size = sh[i].sh_size;
167 *offset = sh[i].sh_offset;
168 return 0;
169 }
170 }
171
172 return -1;
173 }
174
175 static int elf32_find_section(char *map, const char *section, unsigned int *offset, unsigned int *size)
176 {
177 const char *secnames;
178 Elf32_Ehdr *e;
179 Elf32_Shdr *sh;
180 int i;
181
182 e = (Elf32_Ehdr *) map;
183 sh = (Elf32_Shdr *) (map + e->e_shoff);
184
185 secnames = map + sh[e->e_shstrndx].sh_offset;
186 for (i = 0; i < e->e_shnum; i++) {
187 if (!strcmp(section, secnames + sh[i].sh_name)) {
188 *size = sh[i].sh_size;
189 *offset = sh[i].sh_offset;
190 return 0;
191 }
192 }
193
194 return -1;
195 }
196
197 static int elf_find_section(char *map, const char *section, unsigned int *offset, unsigned int *size)
198 {
199 int clazz = map[EI_CLASS];
200
201 if (clazz == ELFCLASS32)
202 return elf32_find_section(map, section, offset, size);
203 else if (clazz == ELFCLASS64)
204 return elf64_find_section(map, section, offset, size);
205
206 ULOG_ERR("unknown elf format %d\n", clazz);
207
208 return -1;
209 }
210
211 static struct module *
212 alloc_module(const char *name, const char *depends, int size)
213 {
214 struct module *m;
215 char *_name, *_dep;
216
217 m = calloc_a(sizeof(*m),
218 &_name, strlen(name) + 1,
219 &_dep, depends ? strlen(depends) + 2 : 0);
220 if (!m)
221 return NULL;
222
223 m->avl.key = m->name = strcpy(_name, name);
224 m->opts = 0;
225
226 if (depends) {
227 m->depends = strcpy(_dep, depends);
228 while (*_dep) {
229 if (*_dep == ',')
230 *_dep = '\0';
231 _dep++;
232 }
233 }
234
235 m->size = size;
236 avl_insert(&modules, &m->avl);
237
238 return m;
239 }
240
241 static int scan_loaded_modules(void)
242 {
243 size_t buf_len = 0;
244 char *buf = NULL;
245 FILE *fp;
246
247 fp = fopen("/proc/modules", "r");
248 if (!fp) {
249 ULOG_ERR("failed to open /proc/modules\n");
250 return -1;
251 }
252
253 while (getline(&buf, &buf_len, fp) > 0) {
254 struct module m;
255 struct module *n;
256
257 m.name = strtok(buf, " ");
258 m.size = atoi(strtok(NULL, " "));
259 m.usage = atoi(strtok(NULL, " "));
260 m.depends = strtok(NULL, " ");
261
262 if (!m.name || !m.depends)
263 continue;
264
265 n = alloc_module(m.name, m.depends, m.size);
266 n->usage = m.usage;
267 n->state = LOADED;
268 }
269 free(buf);
270 fclose(fp);
271
272 return 0;
273 }
274
275 static struct module* get_module_info(const char *module, const char *name)
276 {
277 int fd = open(module, O_RDONLY);
278 unsigned int offset, size;
279 char *map = MAP_FAILED, *strings, *dep = NULL;
280 struct module *m = NULL;
281 struct stat s;
282
283 if (fd < 0) {
284 ULOG_ERR("failed to open %s\n", module);
285 goto out;
286 }
287
288 if (fstat(fd, &s) == -1) {
289 ULOG_ERR("failed to stat %s\n", module);
290 goto out;
291 }
292
293 map = mmap(NULL, s.st_size, PROT_READ, MAP_PRIVATE, fd, 0);
294 if (map == MAP_FAILED) {
295 ULOG_ERR("failed to mmap %s\n", module);
296 goto out;
297 }
298
299 if (elf_find_section(map, ".modinfo", &offset, &size)) {
300 ULOG_ERR("failed to load the .modinfo section from %s\n", module);
301 goto out;
302 }
303
304 strings = map + offset;
305 while (strings && (strings < map + offset + size)) {
306 char *sep;
307 int len;
308
309 while (!strings[0])
310 strings++;
311 sep = strstr(strings, "=");
312 if (!sep)
313 break;
314 len = sep - strings;
315 sep++;
316 if (!strncmp(strings, "depends=", len + 1))
317 dep = sep;
318 strings = &sep[strlen(sep)];
319 }
320
321 m = alloc_module(name, dep, s.st_size);
322
323 if (m)
324 m->state = SCANNED;
325
326 out:
327 if (map != MAP_FAILED)
328 munmap(map, s.st_size);
329
330 if (fd >= 0)
331 close(fd);
332
333 return m;
334 }
335
336 static int scan_module_folder(const char *dir)
337 {
338 int gl_flags = GLOB_NOESCAPE | GLOB_MARK;
339 struct utsname ver;
340 char *path;
341 glob_t gl;
342 int j;
343
344 uname(&ver);
345 path = alloca(strlen(dir) + sizeof("*.ko") + 1);
346 sprintf(path, "%s*.ko", dir);
347
348 if (glob(path, gl_flags, NULL, &gl) < 0)
349 return -1;
350
351 for (j = 0; j < gl.gl_pathc; j++) {
352 char *name = get_module_name(gl.gl_pathv[j]);
353 struct module *m;
354
355 if (!name)
356 continue;
357
358 m = find_module(name);
359 if (!m)
360 get_module_info(gl.gl_pathv[j], name);
361 }
362
363 globfree(&gl);
364
365 return 0;
366 }
367
368 static int scan_module_folders(void)
369 {
370 int rv = 0;
371 char **p;
372
373 if (init_module_folders())
374 return -1;
375
376 for (p = module_folders; *p; p++)
377 rv |= scan_module_folder(*p);
378
379 return rv;
380 }
381
382 static int print_modinfo(char *module)
383 {
384 int fd = open(module, O_RDONLY);
385 unsigned int offset, size;
386 struct stat s;
387 char *map = MAP_FAILED, *strings;
388 int rv = -1;
389
390 if (fd < 0) {
391 ULOG_ERR("failed to open %s\n", module);
392 goto out;
393 }
394
395 if (fstat(fd, &s) == -1) {
396 ULOG_ERR("failed to stat %s\n", module);
397 goto out;
398 }
399
400 map = mmap(NULL, s.st_size, PROT_READ, MAP_PRIVATE, fd, 0);
401 if (map == MAP_FAILED) {
402 ULOG_ERR("failed to mmap %s\n", module);
403 goto out;
404 }
405
406 if (elf_find_section(map, ".modinfo", &offset, &size)) {
407 ULOG_ERR("failed to load the .modinfo section from %s\n", module);
408 goto out;
409 }
410
411 strings = map + offset;
412 printf("module:\t\t%s\n", module);
413 while (strings && (strings < map + offset + size)) {
414 char *dup = NULL;
415 char *sep;
416
417 while (!strings[0])
418 strings++;
419 sep = strstr(strings, "=");
420 if (!sep)
421 break;
422 dup = strndup(strings, sep - strings);
423 sep++;
424 if (strncmp(strings, "parm", 4)) {
425 if (strlen(dup) < 7)
426 printf("%s:\t\t%s\n", dup, sep);
427 else
428 printf("%s:\t%s\n", dup, sep);
429 }
430 strings = &sep[strlen(sep)];
431 if (dup)
432 free(dup);
433 }
434
435 rv = 0;
436
437 out:
438 if (map != MAP_FAILED)
439 munmap(map, s.st_size);
440
441 if (fd >= 0)
442 close(fd);
443
444 return rv;
445 }
446
447 static int deps_available(struct module *m, int verbose)
448 {
449 char *dep;
450 int err = 0;
451
452 if (!m->depends || !strcmp(m->depends, "-") || !strcmp(m->depends, ""))
453 return 0;
454
455 dep = m->depends;
456
457 while (*dep) {
458 m = find_module(dep);
459
460 if (verbose && !m)
461 ULOG_ERR("missing dependency %s\n", dep);
462 if (verbose && m && (m->state != LOADED))
463 ULOG_ERR("dependency not loaded %s\n", dep);
464 if (!m || (m->state != LOADED))
465 err++;
466 dep += strlen(dep) + 1;
467 }
468
469 return err;
470 }
471
472 static int insert_module(char *path, const char *options)
473 {
474 void *data = 0;
475 struct stat s;
476 int fd, ret = -1;
477
478 if (stat(path, &s)) {
479 ULOG_ERR("missing module %s\n", path);
480 return ret;
481 }
482
483 fd = open(path, O_RDONLY);
484 if (fd < 0) {
485 ULOG_ERR("cannot open %s\n", path);
486 return ret;
487 }
488
489 data = malloc(s.st_size);
490 if (read(fd, data, s.st_size) == s.st_size)
491 ret = syscall(__NR_init_module, data, (unsigned long) s.st_size, options);
492 else
493 ULOG_ERR("failed to read full module %s\n", path);
494
495 close(fd);
496 free(data);
497
498 return ret;
499 }
500
501 static void load_moddeps(struct module *_m)
502 {
503 char *dep;
504 struct module *m;
505
506 if (!strcmp(_m->depends, "-") || !strcmp(_m->depends, ""))
507 return;
508
509 dep = _m->depends;
510
511 while (*dep) {
512 m = find_module(dep);
513
514 if (!m)
515 ULOG_ERR("failed to find dependency %s\n", dep);
516 if (m && (m->state != LOADED)) {
517 m->state = PROBE;
518 load_moddeps(m);
519 }
520
521 dep = dep + strlen(dep) + 1;
522 }
523 }
524
525 static int iterations = 0;
526 static int load_modprobe(void)
527 {
528 int loaded, todo;
529 struct module *m;
530
531 avl_for_each_element(&modules, m, avl)
532 if (m->state == PROBE)
533 load_moddeps(m);
534
535 do {
536 loaded = 0;
537 todo = 0;
538 avl_for_each_element(&modules, m, avl) {
539 if ((m->state == PROBE) && (!deps_available(m, 0))) {
540 if (!insert_module(get_module_path(m->name), (m->opts) ? (m->opts) : (""))) {
541 m->state = LOADED;
542 m->error = 0;
543 loaded++;
544 continue;
545 }
546 m->error = 1;
547 }
548
549 if ((m->state == PROBE) || m->error)
550 todo++;
551 }
552 iterations++;
553 } while (loaded);
554
555 return todo;
556 }
557
558 static int print_insmod_usage(void)
559 {
560 ULOG_INFO("Usage:\n\tinsmod filename [args]\n");
561
562 return -1;
563 }
564
565 static int print_usage(char *arg)
566 {
567 ULOG_INFO("Usage:\n\t%s module\n", arg);
568
569 return -1;
570 }
571
572 static int main_insmod(int argc, char **argv)
573 {
574 char *name, *cur, *options;
575 int i, ret, len;
576
577 if (argc < 2)
578 return print_insmod_usage();
579
580 name = get_module_name(argv[1]);
581 if (!name) {
582 ULOG_ERR("cannot find module - %s\n", argv[1]);
583 return -1;
584 }
585
586 if (scan_loaded_modules())
587 return -1;
588
589 if (find_module(name)) {
590 ULOG_ERR("module is already loaded - %s\n", name);
591 return -1;
592
593 }
594
595 free_modules();
596
597 for (len = 0, i = 2; i < argc; i++)
598 len += strlen(argv[i]) + 1;
599
600 options = malloc(len);
601 options[0] = 0;
602 cur = options;
603 for (i = 2; i < argc; i++) {
604 if (options[0]) {
605 *cur = ' ';
606 cur++;
607 }
608 cur += sprintf(cur, "%s", argv[i]);
609 }
610
611 if (init_module_folders()) {
612 fprintf(stderr, "Failed to find the folder holding the modules\n");
613 return -1;
614 }
615
616 if (get_module_path(argv[1])) {
617 name = argv[1];
618 } else if (!get_module_path(name)) {
619 fprintf(stderr, "Failed to find %s. Maybe it is a built in module ?\n", name);
620 return -1;
621 }
622
623 ret = insert_module(get_module_path(name), options);
624 free(options);
625
626 if (ret)
627 ULOG_ERR("failed to insert %s\n", get_module_path(name));
628
629 return ret;
630 }
631
632 static int main_rmmod(int argc, char **argv)
633 {
634 struct module *m;
635 char *name;
636 int ret;
637
638 if (argc != 2)
639 return print_usage("rmmod");
640
641 if (scan_loaded_modules())
642 return -1;
643
644 name = get_module_name(argv[1]);
645 m = find_module(name);
646 if (!m) {
647 ULOG_ERR("module is not loaded\n");
648 return -1;
649 }
650 ret = syscall(__NR_delete_module, m->name, 0);
651
652 if (ret)
653 ULOG_ERR("unloading the module failed\n");
654
655 free_modules();
656
657 return ret;
658 }
659
660 static int main_lsmod(int argc, char **argv)
661 {
662 struct module *m;
663 char *dep;
664
665 if (scan_loaded_modules())
666 return -1;
667
668 avl_for_each_element(&modules, m, avl)
669 if (m->state == LOADED) {
670 printf("%-20s%8d%3d ",
671 m->name, m->size, m->usage);
672 if (m->depends && strcmp(m->depends, "-") && strcmp(m->depends, "")) {
673 dep = m->depends;
674 while (*dep) {
675 printf("%s", dep);
676 dep = dep + strlen(dep) + 1;
677 if (*dep)
678 printf(",");
679 }
680 }
681 printf("\n");
682 }
683
684 free_modules();
685
686 return 0;
687 }
688
689 static int main_modinfo(int argc, char **argv)
690 {
691 struct module *m;
692 char *name;
693
694 if (argc != 2)
695 return print_usage("modinfo");
696
697 if (scan_module_folders())
698 return -1;
699
700 name = get_module_name(argv[1]);
701 m = find_module(name);
702 if (!m) {
703 ULOG_ERR("cannot find module - %s\n", argv[1]);
704 return -1;
705 }
706
707 name = get_module_path(m->name);
708 if (!name) {
709 ULOG_ERR("cannot find path of module - %s\n", m->name);
710 return -1;
711 }
712
713 print_modinfo(name);
714
715 return 0;
716 }
717
718 static int main_modprobe(int argc, char **argv)
719 {
720 struct module *m;
721 char *name;
722 char *mod = NULL;
723 int i;
724
725 for (i = 1; i < argc; i++)
726 if (argv[i][0] != '-') {
727 mod = argv[i];
728 break;
729 }
730 if (!mod)
731 return print_usage("modprobe");
732
733 if (scan_loaded_modules())
734 return -1;
735
736 if (scan_module_folders())
737 return -1;
738
739 name = get_module_name(mod);
740 m = find_module(name);
741 if (m && m->state == LOADED) {
742 ULOG_ERR("%s is already loaded\n", name);
743 return -1;
744 } else if (!m) {
745 ULOG_ERR("failed to find a module named %s\n", name);
746 } else {
747 int fail;
748
749 m->state = PROBE;
750
751 fail = load_modprobe();
752
753 if (fail) {
754 ULOG_ERR("%d module%s could not be probed\n",
755 fail, (fail == 1) ? ("") : ("s"));
756
757 avl_for_each_element(&modules, m, avl)
758 if ((m->state == PROBE) || m->error)
759 ULOG_ERR("- %s\n", m->name);
760 }
761 }
762
763 free_modules();
764
765 return 0;
766 }
767
768 static int main_loader(int argc, char **argv)
769 {
770 int gl_flags = GLOB_NOESCAPE | GLOB_MARK;
771 char *dir = "/etc/modules.d/";
772 struct module *m;
773 glob_t gl;
774 char *path;
775 int fail, j;
776
777 if (argc > 1)
778 dir = argv[1];
779
780 path = malloc(strlen(dir) + 2);
781 strcpy(path, dir);
782 strcat(path, "*");
783
784 if (scan_loaded_modules()) {
785 free (path);
786 return -1;
787 }
788
789 if (scan_module_folders()) {
790 free (path);
791 return -1;
792 }
793
794 ULOG_INFO("loading kernel modules from %s\n", path);
795
796 if (glob(path, gl_flags, NULL, &gl) < 0)
797 goto out;
798
799 for (j = 0; j < gl.gl_pathc; j++) {
800 FILE *fp = fopen(gl.gl_pathv[j], "r");
801 size_t mod_len = 0;
802 char *mod = NULL;
803
804 if (!fp) {
805 ULOG_ERR("failed to open %s\n", gl.gl_pathv[j]);
806 continue;
807 }
808
809 while (getline(&mod, &mod_len, fp) > 0) {
810 char *nl = strchr(mod, '\n');
811 struct module *m;
812 char *opts;
813
814 if (nl)
815 *nl = '\0';
816
817 opts = strchr(mod, ' ');
818 if (opts)
819 *opts++ = '\0';
820
821 m = find_module(get_module_name(mod));
822 if (!m || (m->state == LOADED))
823 continue;
824
825 if (opts)
826 m->opts = strdup(opts);
827 m->state = PROBE;
828 if (basename(gl.gl_pathv[j])[0] - '0' <= 9)
829 load_modprobe();
830
831 }
832 free(mod);
833 fclose(fp);
834 }
835
836 fail = load_modprobe();
837
838 if (fail) {
839 ULOG_ERR("%d module%s could not be probed\n",
840 fail, (fail == 1) ? ("") : ("s"));
841
842 avl_for_each_element(&modules, m, avl)
843 if ((m->state == PROBE) || (m->error))
844 ULOG_ERR("- %s - %d\n", m->name, deps_available(m, 1));
845 } else {
846 ULOG_INFO("done loading kernel modules from %s\n", path);
847 }
848
849 out:
850 globfree(&gl);
851 free(path);
852
853 return 0;
854 }
855
856 static int avl_modcmp(const void *k1, const void *k2, void *ptr)
857 {
858 const char *s1 = k1;
859 const char *s2 = k2;
860
861 while (*s1 && ((*s1 == *s2) ||
862 ((*s1 == '_') && (*s2 == '-')) ||
863 ((*s1 == '-') && (*s2 == '_'))))
864 {
865 s1++;
866 s2++;
867 }
868
869 return *(const unsigned char *)s1 - *(const unsigned char *)s2;
870 }
871
872 int main(int argc, char **argv)
873 {
874 char *exec = basename(*argv);
875
876 avl_init(&modules, avl_modcmp, false, NULL);
877 if (!strcmp(exec, "insmod"))
878 return main_insmod(argc, argv);
879
880 if (!strcmp(exec, "rmmod"))
881 return main_rmmod(argc, argv);
882
883 if (!strcmp(exec, "lsmod"))
884 return main_lsmod(argc, argv);
885
886 if (!strcmp(exec, "modinfo"))
887 return main_modinfo(argc, argv);
888
889 if (!strcmp(exec, "modprobe"))
890 return main_modprobe(argc, argv);
891
892 ulog_open(ULOG_KMSG, LOG_USER, "kmodloader");
893 return main_loader(argc, argv);
894 }
OpenWrt core utilities