add support for module handling
[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 syslog(0, "kmodloader: loading kernel modules from %s\n", path);
608
609 if (glob(path, gl_flags, NULL, &gl) >= 0) {
610 int j;
611
612 for (j = 0; j < gl.gl_pathc; j++) {
613 FILE *fp = fopen(gl.gl_pathv[j], "r");
614
615 if (!fp) {
616 fprintf(stderr, "failed to open %s\n", gl.gl_pathv[j]);
617 } else {
618 char mod[256];
619
620 while (fgets(mod, sizeof(mod), fp)) {
621 char *nl = strchr(mod, '\n');
622 char *opts;
623
624 if (nl)
625 *nl = '\0';
626
627 opts = strchr(mod, ' ');
628 if (opts)
629 *opts++ = '\0';
630
631 insert_module(get_module_path(mod), (opts) ? (opts) : (""));
632 }
633 fclose(fp);
634 }
635 }
636 }
637
638 globfree(&gl);
639 free(path);
640
641 return 0;
642 }
643
644 int main(int argc, char **argv)
645 {
646 char *exec = basename(*argv);
647
648 if (!strcmp(exec, "insmod"))
649 return main_insmod(argc, argv);
650
651 if (!strcmp(exec, "rmmod"))
652 return main_rmmod(argc, argv);
653
654 if (!strcmp(exec, "lsmod"))
655 return main_lsmod(argc, argv);
656
657 if (!strcmp(exec, "modinfo"))
658 return main_modinfo(argc, argv);
659
660 if (!strcmp(exec, "depmod"))
661 return main_depmod(argc, argv);
662
663 return main_loader(argc, argv);
664 }