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