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