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