kmodloader: modprobe: skip possible command line arguments
[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 }