insmod should not insert of already inserted
[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 <glob.h>
32 #include <elf.h>
33
34 #include <libubox/list.h>
35
36 #define DEF_MOD_PATH "/lib/modules/%s/"
37
38 enum {
39         SCANNED,
40         PROBE,
41         LOADED,
42         FAILED,
43 };
44
45 struct module {
46         struct list_head list;
47
48         char *name;
49         char *depends;
50
51         int size;
52         int usage;
53         int state;
54 };
55
56 static LIST_HEAD(modules);
57
58 static struct module* find_module(char *name)
59 {
60         struct module *m = NULL;
61
62         list_for_each_entry(m, &modules, list)
63                 if (!strcmp(m->name, name))
64                         return m;
65         return NULL;
66 }
67
68 static void free_module(struct module *m)
69 {
70         if (m->name)
71                 free(m->name);
72         if (m->depends)
73                 free(m->depends);
74         free(m);
75 }
76
77 static void free_modules(void)
78 {
79         struct list_head *p, *n;
80
81         list_for_each_safe(p, n, &modules) {
82                 struct module *m = container_of(p, struct module, list);
83                 list_del(p);
84                 free_module(m);
85         }
86 }
87
88 static char* get_module_path(char *name)
89 {
90         static char path[256];
91         struct utsname ver;
92         struct stat s;
93         char *t;
94
95         if (!stat(name, &s))
96                 return name;
97
98         uname(&ver);
99         snprintf(path, 256, DEF_MOD_PATH "%s.ko", ver.release, name);
100
101         if (!stat(path, &s))
102                 return path;
103
104         t = name;
105         while (t && *t) {
106                 if (*t == '_')
107                         *t = '-';
108                 t++;
109         }
110
111         snprintf(path, 256, DEF_MOD_PATH "%s.ko", ver.release, name);
112
113         if (!stat(path, &s))
114                 return path;
115
116         return NULL;
117 }
118
119 static char* get_module_name(char *path)
120 {
121         static char name[32];
122         char *t;
123
124         strncpy(name, basename(path), sizeof(name));
125
126         t = strstr(name, ".ko");
127         if (t)
128                 *t = '\0';
129         t = name;
130         while (t && *t) {
131                 if (*t == '-')
132                         *t = '_';
133                 t++;
134         }
135
136         return name;
137 }
138
139 #if __WORDSIZE == 64
140 static int elf_find_section(char *map, const char *section, unsigned int *offset, unsigned int *size)
141 {
142         const char *secnames;
143         Elf64_Ehdr *e;
144         Elf64_Shdr *sh;
145         int i;
146
147         e = (Elf64_Ehdr *) map;
148         sh = (Elf64_Shdr *) (map + e->e_shoff);
149
150         secnames = map + sh[e->e_shstrndx].sh_offset;
151         for (i = 0; i < e->e_shnum; i++) {
152                 if (!strcmp(section, secnames + sh[i].sh_name)) {
153                         *size = sh[i].sh_size;
154                         *offset = sh[i].sh_offset;
155                         return 0;
156                 }
157         }
158
159         return -1;
160 }
161 #else
162 static int elf_find_section(char *map, const char *section, unsigned int *offset, unsigned int *size)
163 {
164         const char *secnames;
165         Elf32_Ehdr *e;
166         Elf32_Shdr *sh;
167         int i;
168
169         e = (Elf32_Ehdr *) map;
170         sh = (Elf32_Shdr *) (map + e->e_shoff);
171
172         secnames = map + sh[e->e_shstrndx].sh_offset;
173         for (i = 0; i < e->e_shnum; i++) {
174                 if (!strcmp(section, secnames + sh[i].sh_name)) {
175                         *size = sh[i].sh_size;
176                         *offset = sh[i].sh_offset;
177                         return 0;
178                 }
179         }
180
181         return -1;
182 }
183 #endif
184
185 static int scan_loaded_modules(void)
186 {
187         FILE *fp = fopen("/proc/modules", "r");
188         char buf[256];
189
190         if (!fp) {
191                 fprintf(stderr, "failed to open /proc/modules\n");
192                 return -1;
193         }
194
195         while (fgets(buf, sizeof(buf), fp)) {
196                 struct module m;
197                 struct module *n;
198
199                 m.name = strtok(buf, " ");
200                 m.size = atoi(strtok(NULL, " "));
201                 m.usage = atoi(strtok(NULL, " "));
202                 m.depends = strtok(NULL, " ");
203
204                 if (!m.name || !m.depends)
205                         continue;
206
207                 n = malloc(sizeof(struct module));
208                 if (!n)
209                         continue;
210
211                 n->name = strdup(m.name);
212                 n->depends = strdup(m.depends);
213                 n->size = m.size;
214                 n->usage = m.usage;
215                 n->state = LOADED;
216
217                 list_add_tail(&n->list, &modules);
218         }
219
220         return 0;
221 }
222
223 static struct module* get_module_info(char *module)
224 {
225         int fd = open(module, O_RDONLY);
226         unsigned int offset, size;
227         char *map, *strings;
228         struct module *m;
229         struct stat s;
230
231         if (!fd) {
232                 fprintf(stderr, "failed to open %s\n", module);
233                 return NULL;
234         }
235
236         if (fstat(fd, &s) == -1) {
237                 fprintf(stderr, "failed to stat %s\n", module);
238                 return NULL;
239         }
240
241         map = mmap(NULL, s.st_size, PROT_READ, MAP_PRIVATE, fd, 0);
242         if (map == MAP_FAILED) {
243                 fprintf(stderr, "failed to mmap %s\n", module);
244                 return NULL;
245         }
246
247         if (elf_find_section(map, ".modinfo", &offset, &size)) {
248                 fprintf(stderr, "failed to load the .modinfo section from %s\n", module);
249                 return NULL;
250         }
251
252         strings = map + offset;
253         m = malloc(sizeof(struct module));
254         if (!m)
255                 return NULL;
256
257         memset(m, 0, sizeof(struct module));
258         m->size = s.st_size;
259         while (strings && (strings < map + offset + size)) {
260                 char *sep;
261                 int len;
262
263                 while (!strings[0])
264                         strings++;
265                 sep = strstr(strings, "=");
266                 if (!sep)
267                         break;
268                 len = sep - strings;
269                 sep++;
270                 if (!strncmp(strings, "depends=", len + 1))
271                         m->depends = strdup(sep);
272                 strings = &sep[strlen(sep)];
273         }
274
275         m->state = SCANNED;
276
277         return m;
278 }
279
280 static int scan_module_folder(char *dir)
281 {
282         int gl_flags = GLOB_NOESCAPE | GLOB_MARK;
283         int j;
284         glob_t gl;
285
286         if (glob(dir, gl_flags, NULL, &gl) < 0)
287                 return -1;
288
289         for (j = 0; j < gl.gl_pathc; j++) {
290                 char *name = get_module_name(gl.gl_pathv[j]);
291                 struct module *m;
292
293                 if (!name)
294                         continue;
295
296                 m = find_module(name);
297                 if (!m) {
298                         m = get_module_info(gl.gl_pathv[j]);
299                         m->name = strdup(name);
300                         list_add_tail(&m->list, &modules);
301                 }
302         }
303
304         globfree(&gl);
305
306         return 0;
307 }
308
309 static int print_modinfo(char *module)
310 {
311         int fd = open(module, O_RDONLY);
312         unsigned int offset, size;
313         struct stat s;
314         char *map, *strings;
315
316         if (!fd) {
317                 fprintf(stderr, "failed to open %s\n", module);
318                 return -1;
319         }
320
321         if (fstat(fd, &s) == -1) {
322                 fprintf(stderr, "failed to stat %s\n", module);
323                 return -1;
324         }
325
326         map = mmap(NULL, s.st_size, PROT_READ, MAP_PRIVATE, fd, 0);
327         if (map == MAP_FAILED) {
328                 fprintf(stderr, "failed to mmap %s\n", module);
329                 return -1;
330         }
331
332         if (elf_find_section(map, ".modinfo", &offset, &size)) {
333                 fprintf(stderr, "failed to load the .modinfo section from %s\n", module);
334                 return -1;
335         }
336
337         strings = map + offset;
338         printf("module:\t\t%s\n", module);
339         while (strings && (strings < map + offset + size)) {
340                 char *dup = NULL;
341                 char *sep;
342
343                 while (!strings[0])
344                         strings++;
345                 sep = strstr(strings, "=");
346                 if (!sep)
347                         break;
348                 dup = strndup(strings, sep - strings);
349                 sep++;
350                 if (strncmp(strings, "parm", 4)) {
351                         if (strlen(dup) < 7)
352                                 printf("%s:\t\t%s\n",  dup, sep);
353                         else
354                                 printf("%s:\t%s\n",  dup, sep);
355                 }
356                 strings = &sep[strlen(sep)];
357                 if (dup)
358                         free(dup);
359         }
360
361         return 0;
362 }
363
364 static int insert_module(char *path, const char *options)
365 {
366         void *data = 0;
367         struct stat s;
368         int fd, ret = -1;
369
370         if (stat(path, &s)) {
371                 fprintf(stderr, "missing module %s\n", path);
372                 return ret;
373         }
374
375         fd = open(path, O_RDONLY);
376         if (!fd) {
377                 fprintf(stderr, "cannot open %s\n", path);
378                 return ret;
379         }
380
381         data = malloc(s.st_size);
382         if (read(fd, data, s.st_size) == s.st_size) {
383                 ret = syscall(__NR_init_module, data, s.st_size, options);
384                 if (ret)
385                         fprintf(stderr, "failed to insert %s\n", path);
386         } else {
387                 fprintf(stderr, "failed to read full module %s\n", path);
388         }
389
390         close(fd);
391         free(data);
392
393         return ret;
394 }
395
396 static int deps_available(struct module *m)
397 {
398         char *deps = m->depends;
399         char *comma;
400
401         if (!strcmp(deps, "-"))
402                 return 0;
403         while (*deps && (NULL != ((comma = strstr(deps, ","))))) {
404                 *comma = '\0';
405
406                 m = find_module(deps);
407
408                 if (!m || (m->state != LOADED))
409                         return -1;
410
411                 deps = ++comma;
412         }
413
414         return 0;
415 }
416
417 static int load_depmod(void)
418 {
419         int loaded, todo;
420         struct module *m;
421
422         do {
423                 loaded = 0;
424                 todo = 0;
425                 list_for_each_entry(m, &modules, list) {
426                         if ((m->state == PROBE) && (!deps_available(m))) {
427                                 if (!insert_module(get_module_path(m->name), "")) {
428                                         m->state = LOADED;
429                                         loaded++;
430                                         continue;
431                                 }
432                                 m->state = FAILED;
433                         } else if (m->state == PROBE) {
434                                 todo++;
435                         }
436                 }
437 //              printf("loaded %d modules this pass\n", loaded);
438         } while (loaded);
439
440 //      printf("missing todos %d\n", todo);
441
442         return -todo;
443 }
444
445 static int print_insmod_usage(void)
446 {
447         fprintf(stderr, "Usage:\n\tinsmod filename [args]\n");
448
449         return -1;
450 }
451
452 static int print_usage(char *arg)
453 {
454         fprintf(stderr, "Usage:\n\t%s module\n", arg);
455
456         return -1;
457 }
458
459 static int main_insmod(int argc, char **argv)
460 {
461         char options[256] = "";
462         char *name;
463         int i;
464
465         if (argc < 2)
466                 return print_insmod_usage();
467
468         name = get_module_name(argv[1]);
469         if (!name) {
470                 fprintf(stderr, "cannot find module - %s\n", argv[1]);
471                 return -1;
472         }
473
474         if (scan_loaded_modules())
475                 return -1;
476
477         if (find_module(name)) {
478                 fprintf(stderr, "module is already loaded - %s\n", name);
479                 return -1;
480
481         }
482
483         free_modules();
484
485         for (i = 2; i < argc; i++)
486                 if (snprintf(options, sizeof(options), "%s %s", options, argv[i]) >= sizeof(options)) {
487                         fprintf(stderr, "argument line too long - %s\n", options);
488                         return -1;
489                 }
490
491         return insert_module(get_module_path(name), options);
492 }
493
494 static int main_rmmod(int argc, char **argv)
495 {
496         struct module *m;
497         char *name;
498         int ret;
499
500         if (argc != 2)
501                 return print_usage("rmmod");
502
503         if (scan_loaded_modules())
504                 return -1;
505
506         name = get_module_name(argv[1]);
507         m = find_module(name);
508         if (!m) {
509                 fprintf(stderr, "module is not loaded\n");
510                 return -1;
511         }
512         free_modules();
513
514         ret = syscall(__NR_delete_module, name, 0);
515
516         if (ret)
517                 fprintf(stderr, "unloading the module failed\n");
518
519         return ret;
520 }
521
522 static int main_lsmod(int argc, char **argv)
523 {
524         struct module *m;
525
526         if (scan_loaded_modules())
527                 return -1;
528
529         list_for_each_entry(m, &modules, list)
530                 if (m->state == LOADED)
531                         printf("%-20s%8d%3d %s\n",
532                                 m->name, m->size, m->usage,
533                                 (*m->depends == '-') ? ("") : (m->depends));
534
535         free_modules();
536
537         return 0;
538 }
539
540 static int main_modinfo(int argc, char **argv)
541 {
542         char *module;
543
544         if (argc != 2)
545                 return print_usage("modinfo");
546
547         module = get_module_path(argv[1]);
548         if (!module) {
549                 fprintf(stderr, "cannot find module - %s\n", argv[1]);
550                 return -1;
551         }
552
553         print_modinfo(module);
554
555         return 0;
556 }
557
558 static int main_depmod(int argc, char **argv)
559 {
560         struct utsname ver;
561         struct module *m;
562         char path[128];
563         char *name;
564
565         if (argc != 2)
566                 return print_usage("depmod");
567
568         if (scan_loaded_modules())
569                 return -1;
570
571         uname(&ver);
572         snprintf(path, sizeof(path), DEF_MOD_PATH "*.ko", ver.release);
573
574         scan_module_folder(path);
575
576         name = get_module_name(argv[1]);
577         m = find_module(name);
578         if (m && m->state == LOADED) {
579                 fprintf(stderr, "%s is already loaded\n", name);
580                 return -1;
581         } else if (!m) {
582                 fprintf(stderr, "failed to find a module named %s\n", name);
583         } else {
584                 m->state = PROBE;
585                 load_depmod();
586         }
587
588         free_modules();
589
590         return 0;
591 }
592
593 static int main_loader(int argc, char **argv)
594 {
595         int gl_flags = GLOB_NOESCAPE | GLOB_MARK;
596         char *dir = "/etc/modules.d/*";
597         glob_t gl;
598         char *path;
599
600         if (argc > 1)
601                 dir = argv[1];
602
603         path = malloc(strlen(dir) + 2);
604         strcpy(path, dir);
605         strcat(path, "*");
606
607         scan_loaded_modules();
608
609         syslog(0, "kmodloader: loading kernel modules from %s\n", path);
610
611         if (glob(path, gl_flags, NULL, &gl) >= 0) {
612                 int j;
613
614                 for (j = 0; j < gl.gl_pathc; j++) {
615                         FILE *fp = fopen(gl.gl_pathv[j], "r");
616
617                         if (!fp) {
618                                 fprintf(stderr, "failed to open %s\n", gl.gl_pathv[j]);
619                         } else {
620                                 char mod[256];
621
622                                 while (fgets(mod, sizeof(mod), fp)) {
623                                         char *nl = strchr(mod, '\n');
624                                         struct module *m;
625                                         char *opts;
626
627                                         if (nl)
628                                                 *nl = '\0';
629
630                                         opts = strchr(mod, ' ');
631                                         if (opts)
632                                                 *opts++ = '\0';
633
634                                         m = find_module(get_module_name(mod));
635                                         if (m)
636                                                 continue;
637                                         insert_module(get_module_path(mod), (opts) ? (opts) : (""));
638                                 }
639                                 fclose(fp);
640                         }
641                 }
642         }
643
644         globfree(&gl);
645         free(path);
646
647         return 0;
648 }
649
650 int main(int argc, char **argv)
651 {
652         char *exec = basename(*argv);
653
654         if (!strcmp(exec, "insmod"))
655                 return main_insmod(argc, argv);
656
657         if (!strcmp(exec, "rmmod"))
658                 return main_rmmod(argc, argv);
659
660         if (!strcmp(exec, "lsmod"))
661                 return main_lsmod(argc, argv);
662
663         if (!strcmp(exec, "modinfo"))
664                 return main_modinfo(argc, argv);
665
666         if (!strcmp(exec, "depmod"))
667                 return main_depmod(argc, argv);
668
669         return main_loader(argc, argv);
670 }