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