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