kmodloader.c

   1 /*
   2  * Copyright (C) 2013 Felix Fietkau <nbd@openwrt.org>
   3  * Copyright (C) 2013 John Crispin <blogic@openwrt.org>
   4  *
   5  * This program is free software; you can redistribute it and/or modify
   6  * it under the terms of the GNU Lesser General Public License version 2.1
   7  * as published by the Free Software Foundation
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  */
  14
  15 #define _GNU_SOURCE
  16 #include <sys/syscall.h>
  17 #include <sys/mman.h>
  18 #include <sys/utsname.h>
  19
  20 #include <stdlib.h>
  21 #include <unistd.h>
  22 #include <sys/syscall.h>
  23 #include <sys/types.h>
  24 #include <values.h>
  25 #include <errno.h>
  26 #include <stdio.h>
  27 #include <string.h>
  28 #include <sys/stat.h>
  29 #include <fcntl.h>
  30 #include <syslog.h>
  31 #include <libgen.h>
  32 #include <glob.h>
  33 #include <elf.h>
  34
  35 #include <libubox/avl.h>
  36 #include <libubox/avl-cmp.h>
  37 #include <libubox/utils.h>
  38
  39 #define DEF_MOD_PATH "/lib/modules/%s/"
  40
  41 #define LOG(fmt, ...) do { \
  42         syslog(LOG_INFO, fmt, ## __VA_ARGS__); \
  43         printf("kmod: "fmt, ## __VA_ARGS__); \
  44         } while (0)
  45
  46
  47 enum {
  48         SCANNED,
  49         PROBE,
  50         LOADED,
  51 };
  52
  53 struct module {
  54         struct avl_node avl;
  55
  56         char *name;
  57         char *depends;
  58         char *opts;
  59
  60         int size;
  61         int usage;
  62         int state;
  63         int error;
  64 };
  65
  66 static struct avl_tree modules;
  67 static char *prefix = "";
  68
  69 static struct module *find_module(const char *name)
  70 {
  71         struct module *m;
  72         return avl_find_element(&modules, name, m, avl);
  73 }
  74
  75 static void free_modules(void)
  76 {
  77         struct module *m, *tmp;
  78
  79         avl_remove_all_elements(&modules, m, avl, tmp)
  80                 free(m);
  81 }
  82
  83 static char* get_module_path(char *name)
  84 {
  85         static char path[256];
  86         struct utsname ver;
  87         struct stat s;
  88
  89         if (!stat(name, &s))
  90                 return name;
  91
  92         uname(&ver);
  93         snprintf(path, 256, "%s" DEF_MOD_PATH "%s.ko", prefix, ver.release, name);
  94
  95         if (!stat(path, &s))
  96                 return path;
  97
  98         return NULL;
  99 }
 100
 101 static char* get_module_name(char *path)
 102 {
 103         static char name[32];
 104         char *t;
 105
 106         strncpy(name, basename(path), sizeof(name));
 107
 108         t = strstr(name, ".ko");
 109         if (t)
 110                 *t = '\0';
 111
 112         return name;
 113 }
 114
 115 static int elf64_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
 137 static int elf32_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
 159 static int elf_find_section(char *map, const char *section, unsigned int *offset, unsigned int *size)
 160 {
 161         int clazz = map[EI_CLASS];
 162
 163         if (clazz == ELFCLASS32)
 164                 return elf32_find_section(map, section, offset, size);
 165         else if (clazz == ELFCLASS64)
 166                 return elf64_find_section(map, section, offset, size);
 167
 168         LOG("unknown elf format %d\n", clazz);
 169
 170         return -1;
 171 }
 172
 173 static struct module *
 174 alloc_module(const char *name, const char *depends, int size)
 175 {
 176         struct module *m;
 177         char *_name, *_dep;
 178
 179         m = calloc_a(sizeof(*m),
 180                 &_name, strlen(name) + 1,
 181                 &_dep, depends ? strlen(depends) + 2 : 0);
 182         if (!m)
 183                 return NULL;
 184
 185         m->avl.key = m->name = strcpy(_name, name);
 186         m->opts = 0;
 187
 188         if (depends) {
 189                 m->depends = strcpy(_dep, depends);
 190                 while (*_dep) {
 191                         if (*_dep == ',')
 192                                 *_dep = '\0';
 193                         _dep++;
 194                 }
 195         }
 196
 197         m->size = size;
 198         avl_insert(&modules, &m->avl);
 199
 200         return m;
 201 }
 202
 203 static int scan_loaded_modules(void)
 204 {
 205         size_t buf_len = 0;
 206         char *buf = NULL;
 207         FILE *fp;
 208
 209         fp = fopen("/proc/modules", "r");
 210         if (!fp) {
 211                 LOG("failed to open /proc/modules\n");
 212                 return -1;
 213         }
 214
 215         while (getline(&buf, &buf_len, fp) > 0) {
 216                 struct module m;
 217                 struct module *n;
 218
 219                 m.name = strtok(buf, " ");
 220                 m.size = atoi(strtok(NULL, " "));
 221                 m.usage = atoi(strtok(NULL, " "));
 222                 m.depends = strtok(NULL, " ");
 223
 224                 if (!m.name || !m.depends)
 225                         continue;
 226
 227                 n = alloc_module(m.name, m.depends, m.size);
 228                 n->usage = m.usage;
 229                 n->state = LOADED;
 230         }
 231         free(buf);
 232         fclose(fp);
 233
 234         return 0;
 235 }
 236
 237 static struct module* get_module_info(const char *module, const char *name)
 238 {
 239         int fd = open(module, O_RDONLY);
 240         unsigned int offset, size;
 241         char *map, *strings, *dep = NULL;
 242         struct module *m;
 243         struct stat s;
 244
 245         if (!fd) {
 246                 LOG("failed to open %s\n", module);
 247                 return NULL;
 248         }
 249
 250         if (fstat(fd, &s) == -1) {
 251                 LOG("failed to stat %s\n", module);
 252                 return NULL;
 253         }
 254
 255         map = mmap(NULL, s.st_size, PROT_READ, MAP_PRIVATE, fd, 0);
 256         if (map == MAP_FAILED) {
 257                 LOG("failed to mmap %s\n", module);
 258                 return NULL;
 259         }
 260
 261         if (elf_find_section(map, ".modinfo", &offset, &size)) {
 262                 LOG("failed to load the .modinfo section from %s\n", module);
 263                 return NULL;
 264         }
 265
 266         strings = map + offset;
 267         while (strings && (strings < map + offset + size)) {
 268                 char *sep;
 269                 int len;
 270
 271                 while (!strings[0])
 272                         strings++;
 273                 sep = strstr(strings, "=");
 274                 if (!sep)
 275                         break;
 276                 len = sep - strings;
 277                 sep++;
 278                 if (!strncmp(strings, "depends=", len + 1))
 279                         dep = sep;
 280                 strings = &sep[strlen(sep)];
 281         }
 282
 283         m = alloc_module(name, dep, s.st_size);
 284         if (!m)
 285                 return NULL;
 286
 287         m->state = SCANNED;
 288
 289         return m;
 290 }
 291
 292 static int scan_module_folder(void)
 293 {
 294         int gl_flags = GLOB_NOESCAPE | GLOB_MARK;
 295         struct utsname ver;
 296         char *path;
 297         glob_t gl;
 298         int j;
 299
 300         uname(&ver);
 301         path = alloca(sizeof(DEF_MOD_PATH "*.ko") + strlen(prefix) + strlen(ver.release) + 1);
 302         sprintf(path, "%s" DEF_MOD_PATH "*.ko", prefix, ver.release);
 303
 304         if (glob(path, gl_flags, NULL, &gl) < 0)
 305                 return -1;
 306
 307         for (j = 0; j < gl.gl_pathc; j++) {
 308                 char *name = get_module_name(gl.gl_pathv[j]);
 309                 struct module *m;
 310
 311                 if (!name)
 312                         continue;
 313
 314                 m = find_module(name);
 315                 if (!m)
 316                         get_module_info(gl.gl_pathv[j], name);
 317         }
 318
 319         globfree(&gl);
 320
 321         return 0;
 322 }
 323
 324 static int print_modinfo(char *module)
 325 {
 326         int fd = open(module, O_RDONLY);
 327         unsigned int offset, size;
 328         struct stat s;
 329         char *map, *strings;
 330
 331         if (!fd) {
 332                 LOG("failed to open %s\n", module);
 333                 return -1;
 334         }
 335
 336         if (fstat(fd, &s) == -1) {
 337                 LOG("failed to stat %s\n", module);
 338                 return -1;
 339         }
 340
 341         map = mmap(NULL, s.st_size, PROT_READ, MAP_PRIVATE, fd, 0);
 342         if (map == MAP_FAILED) {
 343                 LOG("failed to mmap %s\n", module);
 344                 return -1;
 345         }
 346
 347         if (elf_find_section(map, ".modinfo", &offset, &size)) {
 348                 LOG("failed to load the .modinfo section from %s\n", module);
 349                 return -1;
 350         }
 351
 352         strings = map + offset;
 353         printf("module:\t\t%s\n", module);
 354         while (strings && (strings < map + offset + size)) {
 355                 char *dup = NULL;
 356                 char *sep;
 357
 358                 while (!strings[0])
 359                         strings++;
 360                 sep = strstr(strings, "=");
 361                 if (!sep)
 362                         break;
 363                 dup = strndup(strings, sep - strings);
 364                 sep++;
 365                 if (strncmp(strings, "parm", 4)) {
 366                         if (strlen(dup) < 7)
 367                                 printf("%s:\t\t%s\n",  dup, sep);
 368                         else
 369                                 printf("%s:\t%s\n",  dup, sep);
 370                 }
 371                 strings = &sep[strlen(sep)];
 372                 if (dup)
 373                         free(dup);
 374         }
 375
 376         return 0;
 377 }
 378
 379 static int deps_available(struct module *m, int verbose)
 380 {
 381         char *dep;
 382         int err = 0;
 383
 384         if (!strcmp(m->depends, "-") || !strcmp(m->depends, ""))
 385                 return 0;
 386
 387         dep = m->depends;
 388
 389         while (*dep) {
 390                 m = find_module(dep);
 391
 392                 if (verbose && !m)
 393                         LOG("missing dependency %s\n", dep);
 394                 if (verbose && m && (m->state != LOADED))
 395                         LOG("dependency not loaded %s\n", dep);
 396                 if (!m || (m->state != LOADED))
 397                         err++;
 398                 dep += strlen(dep) + 1;
 399         }
 400
 401         return err;
 402 }
 403
 404 static int insert_module(char *path, const char *options)
 405 {
 406         void *data = 0;
 407         struct stat s;
 408         int fd, ret = -1;
 409
 410         if (stat(path, &s)) {
 411                 LOG("missing module %s\n", path);
 412                 return ret;
 413         }
 414
 415         fd = open(path, O_RDONLY);
 416         if (!fd) {
 417                 LOG("cannot open %s\n", path);
 418                 return ret;
 419         }
 420
 421         data = malloc(s.st_size);
 422         if (read(fd, data, s.st_size) == s.st_size)
 423                 ret = syscall(__NR_init_module, data, s.st_size, options);
 424         else
 425                 LOG("failed to read full module %s\n", path);
 426
 427         close(fd);
 428         free(data);
 429
 430         return ret;
 431 }
 432
 433 static void load_moddeps(struct module *_m)
 434 {
 435         char *dep;
 436         struct module *m;
 437
 438         if (!strcmp(_m->depends, "-") || !strcmp(_m->depends, ""))
 439                 return;
 440
 441         dep = _m->depends;
 442
 443         while (*dep) {
 444                 m = find_module(dep);
 445
 446                 if (!m)
 447                         LOG("failed to find dependency %s\n", dep);
 448                 if (m && (m->state != LOADED)) {
 449                         m->state = PROBE;
 450                         load_moddeps(m);
 451                 }
 452
 453                 dep = dep + strlen(dep) + 1;
 454         }
 455 }
 456
 457 static int iterations = 0;
 458 static int load_modprobe(void)
 459 {
 460         int loaded, todo;
 461         struct module *m;
 462
 463         avl_for_each_element(&modules, m, avl)
 464                 if (m->state == PROBE)
 465                         load_moddeps(m);
 466
 467         do {
 468                 loaded = 0;
 469                 todo = 0;
 470                 avl_for_each_element(&modules, m, avl) {
 471                         if ((m->state == PROBE) && (!deps_available(m, 0))) {
 472                                 if (!insert_module(get_module_path(m->name), (m->opts) ? (m->opts) : (""))) {
 473                                         m->state = LOADED;
 474                                         m->error = 0;
 475                                         loaded++;
 476                                         continue;
 477                                 }
 478                                 m->error = 1;
 479                         }
 480
 481                         if ((m->state == PROBE) || m->error)
 482                                 todo++;
 483                 }
 484                 iterations++;
 485         } while (loaded);
 486
 487         return todo;
 488 }
 489
 490 static int print_insmod_usage(void)
 491 {
 492         LOG("Usage:\n\tinsmod filename [args]\n");
 493
 494         return -1;
 495 }
 496
 497 static int print_usage(char *arg)
 498 {
 499         LOG("Usage:\n\t%s module\n", arg);
 500
 501         return -1;
 502 }
 503
 504 static int main_insmod(int argc, char **argv)
 505 {
 506         char *name, *cur, *options;
 507         int i, ret, len;
 508
 509         if (argc < 2)
 510                 return print_insmod_usage();
 511
 512         name = get_module_name(argv[1]);
 513         if (!name) {
 514                 LOG("cannot find module - %s\n", argv[1]);
 515                 return -1;
 516         }
 517
 518         if (scan_loaded_modules())
 519                 return -1;
 520
 521         if (find_module(name)) {
 522                 LOG("module is already loaded - %s\n", name);
 523                 return -1;
 524
 525         }
 526
 527         free_modules();
 528
 529         for (len = 0, i = 2; i < argc; i++)
 530                 len += strlen(argv[i]) + 1;
 531
 532         options = malloc(len);
 533         options[0] = 0;
 534         cur = options;
 535         for (i = 2; i < argc; i++) {
 536                 if (options[0]) {
 537                         *cur = ' ';
 538                         cur++;
 539                 }
 540                 cur += sprintf(cur, "%s", argv[i]);
 541         }
 542
 543         if (!get_module_path(name)) {
 544                 fprintf(stderr, "Failed to find %s. Maybe it is a built in module ?\n", name);
 545                 return -1;
 546         }
 547
 548         ret = insert_module(get_module_path(name), options);
 549         free(options);
 550
 551         if (ret)
 552                 LOG("failed to insert %s\n", get_module_path(name));
 553
 554         return ret;
 555 }
 556
 557 static int main_rmmod(int argc, char **argv)
 558 {
 559         struct module *m;
 560         char *name;
 561         int ret;
 562
 563         if (argc != 2)
 564                 return print_usage("rmmod");
 565
 566         if (scan_loaded_modules())
 567                 return -1;
 568
 569         name = get_module_name(argv[1]);
 570         m = find_module(name);
 571         if (!m) {
 572                 LOG("module is not loaded\n");
 573                 return -1;
 574         }
 575         ret = syscall(__NR_delete_module, m->name, 0);
 576
 577         if (ret)
 578                 LOG("unloading the module failed\n");
 579
 580         free_modules();
 581
 582         return ret;
 583 }
 584
 585 static int main_lsmod(int argc, char **argv)
 586 {
 587         struct module *m;
 588
 589         if (scan_loaded_modules())
 590                 return -1;
 591
 592         avl_for_each_element(&modules, m, avl)
 593                 if (m->state == LOADED)
 594                         printf("%-20s%8d%3d %s\n",
 595                                 m->name, m->size, m->usage,
 596                                 (*m->depends == '-') ? ("") : (m->depends));
 597
 598         free_modules();
 599
 600         return 0;
 601 }
 602
 603 static int main_modinfo(int argc, char **argv)
 604 {
 605         struct module *m;
 606         char *name;
 607
 608         if (argc != 2)
 609                 return print_usage("modinfo");
 610
 611         if (scan_module_folder())
 612                 return -1;
 613
 614         name = get_module_name(argv[1]);
 615         m = find_module(name);
 616         if (!m) {
 617                 LOG("cannot find module - %s\n", argv[1]);
 618                 return -1;
 619         }
 620
 621         name = get_module_path(m->name);
 622         if (!name) {
 623                 LOG("cannot find path of module - %s\n", m->name);
 624                 return -1;
 625         }
 626
 627         print_modinfo(name);
 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                         if (opts)
 732                                 m->opts = strdup(opts);
 733                         m->state = PROBE;
 734                         if (basename(gl.gl_pathv[j])[0] - '0' <= 9)
 735                                 load_modprobe();
 736
 737                 }
 738                 free(mod);
 739                 fclose(fp);
 740         }
 741
 742         fail = load_modprobe();
 743         LOG("ran %d iterations\n", iterations);
 744
 745         if (fail) {
 746                 LOG("%d module%s could not be probed\n",
 747                                 fail, (fail == 1) ? ("") : ("s"));
 748
 749                 avl_for_each_element(&modules, m, avl)
 750                         if ((m->state == PROBE) || (m->error))
 751                                 LOG("- %s - %d\n", m->name, deps_available(m, 1));
 752         }
 753
 754 out:
 755         globfree(&gl);
 756         free(path);
 757
 758         return 0;
 759 }
 760
 761 static int avl_modcmp(const void *k1, const void *k2, void *ptr)
 762 {
 763         const char *s1 = k1;
 764         const char *s2 = k2;
 765
 766         while (*s1 && ((*s1 == *s2) ||
 767                        ((*s1 == '_') && (*s2 == '-')) ||
 768                        ((*s1 == '-') && (*s2 == '_'))))
 769         {
 770                 s1++;
 771                 s2++;
 772         }
 773
 774         return *(const unsigned char *)s1 - *(const unsigned char *)s2;
 775 }
 776
 777 int main(int argc, char **argv)
 778 {
 779         char *exec = basename(*argv);
 780
 781         avl_init(&modules, avl_modcmp, false, NULL);
 782         if (!strcmp(exec, "insmod"))
 783                 return main_insmod(argc, argv);
 784
 785         if (!strcmp(exec, "rmmod"))
 786                 return main_rmmod(argc, argv);
 787
 788         if (!strcmp(exec, "lsmod"))
 789                 return main_lsmod(argc, argv);
 790
 791         if (!strcmp(exec, "modinfo"))
 792                 return main_modinfo(argc, argv);
 793
 794         if (!strcmp(exec, "modprobe"))
 795                 return main_modprobe(argc, argv);
 796
 797         return main_loader(argc, argv);
 798 }