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