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