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 <glob.h>
  32 #include <elf.h>
  33
  34 #include <libubox/list.h>
  35
  36 #define DEF_MOD_PATH "/lib/modules/%s/"
  37
  38 enum {
  39         SCANNED,
  40         PROBE,
  41         LOADED,
  42         FAILED,
  43 };
  44
  45 struct module {
  46         struct list_head list;
  47
  48         char *name;
  49         char *depends;
  50
  51         int size;
  52         int usage;
  53         int state;
  54 };
  55
  56 static LIST_HEAD(modules);
  57
  58 static struct module* find_module(char *name)
  59 {
  60         struct module *m = NULL;
  61
  62         list_for_each_entry(m, &modules, list)
  63                 if (!strcmp(m->name, name))
  64                         return m;
  65         return NULL;
  66 }
  67
  68 static void free_module(struct module *m)
  69 {
  70         if (m->name)
  71                 free(m->name);
  72         if (m->depends)
  73                 free(m->depends);
  74         free(m);
  75 }
  76
  77 static void free_modules(void)
  78 {
  79         struct list_head *p, *n;
  80
  81         list_for_each_safe(p, n, &modules) {
  82                 struct module *m = container_of(p, struct module, list);
  83                 list_del(p);
  84                 free_module(m);
  85         }
  86 }
  87
  88 static char* get_module_path(char *name)
  89 {
  90         static char path[256];
  91         struct utsname ver;
  92         struct stat s;
  93         char *t;
  94
  95         if (!stat(name, &s))
  96                 return name;
  97
  98         uname(&ver);
  99         snprintf(path, 256, DEF_MOD_PATH "%s.ko", ver.release, name);
 100
 101         if (!stat(path, &s))
 102                 return path;
 103
 104         t = name;
 105         while (t && *t) {
 106                 if (*t == '_')
 107                         *t = '-';
 108                 t++;
 109         }
 110
 111         snprintf(path, 256, DEF_MOD_PATH "%s.ko", ver.release, name);
 112
 113         if (!stat(path, &s))
 114                 return path;
 115
 116         return NULL;
 117 }
 118
 119 static char* get_module_name(char *path)
 120 {
 121         static char name[32];
 122         char *t;
 123
 124         strncpy(name, basename(path), sizeof(name));
 125
 126         t = strstr(name, ".ko");
 127         if (t)
 128                 *t = '\0';
 129         t = name;
 130         while (t && *t) {
 131                 if (*t == '-')
 132                         *t = '_';
 133                 t++;
 134         }
 135
 136         return name;
 137 }
 138
 139 #if __WORDSIZE == 64
 140 static int elf_find_section(char *map, const char *section, unsigned int *offset, unsigned int *size)
 141 {
 142         const char *secnames;
 143         Elf64_Ehdr *e;
 144         Elf64_Shdr *sh;
 145         int i;
 146
 147         e = (Elf64_Ehdr *) map;
 148         sh = (Elf64_Shdr *) (map + e->e_shoff);
 149
 150         secnames = map + sh[e->e_shstrndx].sh_offset;
 151         for (i = 0; i < e->e_shnum; i++) {
 152                 if (!strcmp(section, secnames + sh[i].sh_name)) {
 153                         *size = sh[i].sh_size;
 154                         *offset = sh[i].sh_offset;
 155                         return 0;
 156                 }
 157         }
 158
 159         return -1;
 160 }
 161 #else
 162 static int elf_find_section(char *map, const char *section, unsigned int *offset, unsigned int *size)
 163 {
 164         const char *secnames;
 165         Elf32_Ehdr *e;
 166         Elf32_Shdr *sh;
 167         int i;
 168
 169         e = (Elf32_Ehdr *) map;
 170         sh = (Elf32_Shdr *) (map + e->e_shoff);
 171
 172         secnames = map + sh[e->e_shstrndx].sh_offset;
 173         for (i = 0; i < e->e_shnum; i++) {
 174                 if (!strcmp(section, secnames + sh[i].sh_name)) {
 175                         *size = sh[i].sh_size;
 176                         *offset = sh[i].sh_offset;
 177                         return 0;
 178                 }
 179         }
 180
 181         return -1;
 182 }
 183 #endif
 184
 185 static int scan_loaded_modules(void)
 186 {
 187         FILE *fp = fopen("/proc/modules", "r");
 188         char buf[256];
 189
 190         if (!fp) {
 191                 fprintf(stderr, "failed to open /proc/modules\n");
 192                 return -1;
 193         }
 194
 195         while (fgets(buf, sizeof(buf), fp)) {
 196                 struct module m;
 197                 struct module *n;
 198
 199                 m.name = strtok(buf, " ");
 200                 m.size = atoi(strtok(NULL, " "));
 201                 m.usage = atoi(strtok(NULL, " "));
 202                 m.depends = strtok(NULL, " ");
 203
 204                 if (!m.name || !m.depends)
 205                         continue;
 206
 207                 n = malloc(sizeof(struct module));
 208                 if (!n)
 209                         continue;
 210
 211                 n->name = strdup(m.name);
 212                 n->depends = strdup(m.depends);
 213                 n->size = m.size;
 214                 n->usage = m.usage;
 215                 n->state = LOADED;
 216
 217                 list_add_tail(&n->list, &modules);
 218         }
 219
 220         return 0;
 221 }
 222
 223 static struct module* get_module_info(char *module)
 224 {
 225         int fd = open(module, O_RDONLY);
 226         unsigned int offset, size;
 227         char *map, *strings;
 228         struct module *m;
 229         struct stat s;
 230
 231         if (!fd) {
 232                 fprintf(stderr, "failed to open %s\n", module);
 233                 return NULL;
 234         }
 235
 236         if (fstat(fd, &s) == -1) {
 237                 fprintf(stderr, "failed to stat %s\n", module);
 238                 return NULL;
 239         }
 240
 241         map = mmap(NULL, s.st_size, PROT_READ, MAP_PRIVATE, fd, 0);
 242         if (map == MAP_FAILED) {
 243                 fprintf(stderr, "failed to mmap %s\n", module);
 244                 return NULL;
 245         }
 246
 247         if (elf_find_section(map, ".modinfo", &offset, &size)) {
 248                 fprintf(stderr, "failed to load the .modinfo section from %s\n", module);
 249                 return NULL;
 250         }
 251
 252         strings = map + offset;
 253         m = malloc(sizeof(struct module));
 254         if (!m)
 255                 return NULL;
 256
 257         memset(m, 0, sizeof(struct module));
 258         m->size = s.st_size;
 259         while (strings && (strings < map + offset + size)) {
 260                 char *sep;
 261                 int len;
 262
 263                 while (!strings[0])
 264                         strings++;
 265                 sep = strstr(strings, "=");
 266                 if (!sep)
 267                         break;
 268                 len = sep - strings;
 269                 sep++;
 270                 if (!strncmp(strings, "depends=", len + 1))
 271                         m->depends = strdup(sep);
 272                 strings = &sep[strlen(sep)];
 273         }
 274
 275         m->state = SCANNED;
 276
 277         return m;
 278 }
 279
 280 static int scan_module_folder(char *dir)
 281 {
 282         int gl_flags = GLOB_NOESCAPE | GLOB_MARK;
 283         int j;
 284         glob_t gl;
 285
 286         if (glob(dir, gl_flags, NULL, &gl) < 0)
 287                 return -1;
 288
 289         for (j = 0; j < gl.gl_pathc; j++) {
 290                 char *name = get_module_name(gl.gl_pathv[j]);
 291                 struct module *m;
 292
 293                 if (!name)
 294                         continue;
 295
 296                 m = find_module(name);
 297                 if (!m) {
 298                         m = get_module_info(gl.gl_pathv[j]);
 299                         m->name = strdup(name);
 300                         list_add_tail(&m->list, &modules);
 301                 }
 302         }
 303
 304         globfree(&gl);
 305
 306         return 0;
 307 }
 308
 309 static int print_modinfo(char *module)
 310 {
 311         int fd = open(module, O_RDONLY);
 312         unsigned int offset, size;
 313         struct stat s;
 314         char *map, *strings;
 315
 316         if (!fd) {
 317                 fprintf(stderr, "failed to open %s\n", module);
 318                 return -1;
 319         }
 320
 321         if (fstat(fd, &s) == -1) {
 322                 fprintf(stderr, "failed to stat %s\n", module);
 323                 return -1;
 324         }
 325
 326         map = mmap(NULL, s.st_size, PROT_READ, MAP_PRIVATE, fd, 0);
 327         if (map == MAP_FAILED) {
 328                 fprintf(stderr, "failed to mmap %s\n", module);
 329                 return -1;
 330         }
 331
 332         if (elf_find_section(map, ".modinfo", &offset, &size)) {
 333                 fprintf(stderr, "failed to load the .modinfo section from %s\n", module);
 334                 return -1;
 335         }
 336
 337         strings = map + offset;
 338         printf("module:\t\t%s\n", module);
 339         while (strings && (strings < map + offset + size)) {
 340                 char *dup = NULL;
 341                 char *sep;
 342
 343                 while (!strings[0])
 344                         strings++;
 345                 sep = strstr(strings, "=");
 346                 if (!sep)
 347                         break;
 348                 dup = strndup(strings, sep - strings);
 349                 sep++;
 350                 if (strncmp(strings, "parm", 4)) {
 351                         if (strlen(dup) < 7)
 352                                 printf("%s:\t\t%s\n",  dup, sep);
 353                         else
 354                                 printf("%s:\t%s\n",  dup, sep);
 355                 }
 356                 strings = &sep[strlen(sep)];
 357                 if (dup)
 358                         free(dup);
 359         }
 360
 361         return 0;
 362 }
 363
 364 static int insert_module(char *path, const char *options)
 365 {
 366         void *data = 0;
 367         struct stat s;
 368         int fd, ret = -1;
 369
 370         if (stat(path, &s)) {
 371                 fprintf(stderr, "missing module %s\n", path);
 372                 return ret;
 373         }
 374
 375         fd = open(path, O_RDONLY);
 376         if (!fd) {
 377                 fprintf(stderr, "cannot open %s\n", path);
 378                 return ret;
 379         }
 380
 381         data = malloc(s.st_size);
 382         if (read(fd, data, s.st_size) == s.st_size) {
 383                 ret = syscall(__NR_init_module, data, s.st_size, options);
 384                 if (ret)
 385                         fprintf(stderr, "failed to insert %s\n", path);
 386         } else {
 387                 fprintf(stderr, "failed to read full module %s\n", path);
 388         }
 389
 390         close(fd);
 391         free(data);
 392
 393         return ret;
 394 }
 395
 396 static int deps_available(struct module *m)
 397 {
 398         char *deps = m->depends;
 399         char *comma;
 400
 401         if (!strcmp(deps, "-"))
 402                 return 0;
 403         while (*deps && (NULL != ((comma = strstr(deps, ","))))) {
 404                 *comma = '\0';
 405
 406                 m = find_module(deps);
 407
 408                 if (!m || (m->state != LOADED))
 409                         return -1;
 410
 411                 deps = ++comma;
 412         }
 413
 414         return 0;
 415 }
 416
 417 static int load_depmod(void)
 418 {
 419         int loaded, todo;
 420         struct module *m;
 421
 422         do {
 423                 loaded = 0;
 424                 todo = 0;
 425                 list_for_each_entry(m, &modules, list) {
 426                         if ((m->state == PROBE) && (!deps_available(m))) {
 427                                 if (!insert_module(get_module_path(m->name), "")) {
 428                                         m->state = LOADED;
 429                                         loaded++;
 430                                         continue;
 431                                 }
 432                                 m->state = FAILED;
 433                         } else if (m->state == PROBE) {
 434                                 todo++;
 435                         }
 436                 }
 437 //              printf("loaded %d modules this pass\n", loaded);
 438         } while (loaded);
 439
 440 //      printf("missing todos %d\n", todo);
 441
 442         return -todo;
 443 }
 444
 445 static int print_insmod_usage(void)
 446 {
 447         fprintf(stderr, "Usage:\n\tinsmod filename [args]\n");
 448
 449         return -1;
 450 }
 451
 452 static int print_usage(char *arg)
 453 {
 454         fprintf(stderr, "Usage:\n\t%s module\n", arg);
 455
 456         return -1;
 457 }
 458
 459 static int main_insmod(int argc, char **argv)
 460 {
 461         char options[256] = "";
 462         char *name;
 463         int i;
 464
 465         if (argc < 2)
 466                 return print_insmod_usage();
 467
 468         name = get_module_name(argv[1]);
 469         if (!name) {
 470                 fprintf(stderr, "cannot find module - %s\n", argv[1]);
 471                 return -1;
 472         }
 473
 474         if (scan_loaded_modules())
 475                 return -1;
 476
 477         if (find_module(name)) {
 478                 fprintf(stderr, "module is already loaded - %s\n", name);
 479                 return -1;
 480
 481         }
 482
 483         free_modules();
 484
 485         for (i = 2; i < argc; i++)
 486                 if (snprintf(options, sizeof(options), "%s %s", options, argv[i]) >= sizeof(options)) {
 487                         fprintf(stderr, "argument line too long - %s\n", options);
 488                         return -1;
 489                 }
 490
 491         return insert_module(get_module_path(name), options);
 492 }
 493
 494 static int main_rmmod(int argc, char **argv)
 495 {
 496         struct module *m;
 497         char *name;
 498         int ret;
 499
 500         if (argc != 2)
 501                 return print_usage("rmmod");
 502
 503         if (scan_loaded_modules())
 504                 return -1;
 505
 506         name = get_module_name(argv[1]);
 507         m = find_module(name);
 508         if (!m) {
 509                 fprintf(stderr, "module is not loaded\n");
 510                 return -1;
 511         }
 512         free_modules();
 513
 514         ret = syscall(__NR_delete_module, name, 0);
 515
 516         if (ret)
 517                 fprintf(stderr, "unloading the module failed\n");
 518
 519         return ret;
 520 }
 521
 522 static int main_lsmod(int argc, char **argv)
 523 {
 524         struct module *m;
 525
 526         if (scan_loaded_modules())
 527                 return -1;
 528
 529         list_for_each_entry(m, &modules, list)
 530                 if (m->state == LOADED)
 531                         printf("%-20s%8d%3d %s\n",
 532                                 m->name, m->size, m->usage,
 533                                 (*m->depends == '-') ? ("") : (m->depends));
 534
 535         free_modules();
 536
 537         return 0;
 538 }
 539
 540 static int main_modinfo(int argc, char **argv)
 541 {
 542         char *module;
 543
 544         if (argc != 2)
 545                 return print_usage("modinfo");
 546
 547         module = get_module_path(argv[1]);
 548         if (!module) {
 549                 fprintf(stderr, "cannot find module - %s\n", argv[1]);
 550                 return -1;
 551         }
 552
 553         print_modinfo(module);
 554
 555         return 0;
 556 }
 557
 558 static int main_depmod(int argc, char **argv)
 559 {
 560         struct utsname ver;
 561         struct module *m;
 562         char path[128];
 563         char *name;
 564
 565         if (argc != 2)
 566                 return print_usage("depmod");
 567
 568         if (scan_loaded_modules())
 569                 return -1;
 570
 571         uname(&ver);
 572         snprintf(path, sizeof(path), DEF_MOD_PATH "*.ko", ver.release);
 573
 574         scan_module_folder(path);
 575
 576         name = get_module_name(argv[1]);
 577         m = find_module(name);
 578         if (m && m->state == LOADED) {
 579                 fprintf(stderr, "%s is already loaded\n", name);
 580                 return -1;
 581         } else if (!m) {
 582                 fprintf(stderr, "failed to find a module named %s\n", name);
 583         } else {
 584                 m->state = PROBE;
 585                 load_depmod();
 586         }
 587
 588         free_modules();
 589
 590         return 0;
 591 }
 592
 593 static int main_loader(int argc, char **argv)
 594 {
 595         int gl_flags = GLOB_NOESCAPE | GLOB_MARK;
 596         char *dir = "/etc/modules.d/*";
 597         glob_t gl;
 598         char *path;
 599
 600         if (argc > 1)
 601                 dir = argv[1];
 602
 603         path = malloc(strlen(dir) + 2);
 604         strcpy(path, dir);
 605         strcat(path, "*");
 606
 607         syslog(0, "kmodloader: loading kernel modules from %s\n", path);
 608
 609         if (glob(path, gl_flags, NULL, &gl) >= 0) {
 610                 int j;
 611
 612                 for (j = 0; j < gl.gl_pathc; j++) {
 613                         FILE *fp = fopen(gl.gl_pathv[j], "r");
 614
 615                         if (!fp) {
 616                                 fprintf(stderr, "failed to open %s\n", gl.gl_pathv[j]);
 617                         } else {
 618                                 char mod[256];
 619
 620                                 while (fgets(mod, sizeof(mod), fp)) {
 621                                         char *nl = strchr(mod, '\n');
 622                                         char *opts;
 623
 624                                         if (nl)
 625                                                 *nl = '\0';
 626
 627                                         opts = strchr(mod, ' ');
 628                                         if (opts)
 629                                                 *opts++ = '\0';
 630
 631                                         insert_module(get_module_path(mod), (opts) ? (opts) : (""));
 632                                 }
 633                                 fclose(fp);
 634                         }
 635                 }
 636         }
 637
 638         globfree(&gl);
 639         free(path);
 640
 641         return 0;
 642 }
 643
 644 int main(int argc, char **argv)
 645 {
 646         char *exec = basename(*argv);
 647
 648         if (!strcmp(exec, "insmod"))
 649                 return main_insmod(argc, argv);
 650
 651         if (!strcmp(exec, "rmmod"))
 652                 return main_rmmod(argc, argv);
 653
 654         if (!strcmp(exec, "lsmod"))
 655                 return main_lsmod(argc, argv);
 656
 657         if (!strcmp(exec, "modinfo"))
 658                 return main_modinfo(argc, argv);
 659
 660         if (!strcmp(exec, "depmod"))
 661                 return main_depmod(argc, argv);
 662
 663         return main_loader(argc, argv);
 664 }