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