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 }