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