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