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