VtoyTool/vtoykmod.c

   1 /******************************************************************************
   2  * vtoykmod.c  ---- ventoy kmod
   3  *
   4  * Copyright (c) 2021, longpanda <admin@ventoy.net>
   5  *
   6  * This program is free software; you can redistribute it and/or
   7  * modify it under the terms of the GNU General Public License as
   8  * published by the Free Software Foundation; either version 3 of the
   9  * License, or (at your option) any later version.
  10  *
  11  * This program is distributed in the hope that it will be useful, but
  12  * WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  14  * General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License
  17  * along with this program; if not, see <http://www.gnu.org/licenses/>.
  18  *
  19  */
  20
  21 #include <stdio.h>
  22 #include <stdlib.h>
  23 #include <stdint.h>
  24 #include <string.h>
  25 #include <errno.h>
  26 #include <unistd.h>
  27 #ifdef VTOY_X86_64
  28 #include <cpuid.h>
  29 #endif
  30
  31 #define _ull unsigned long long
  32
  33 #define magic_sig 0xB0, 0xB1, 0xB2, 0xB3, 0xB4, 0xB5, 0xB6, 0xB7, 0xB8, 0xB9, 0xBA, 0xBB, 0xBC, 0xBD, 0xBE, 0xBF
  34
  35 #define EI_NIDENT (16)
  36
  37 #define EI_MAG0         0               /* e_ident[] indexes */
  38 #define EI_MAG1         1
  39 #define EI_MAG2         2
  40 #define EI_MAG3         3
  41 #define EI_CLASS        4
  42 #define EI_DATA         5
  43 #define EI_VERSION      6
  44 #define EI_OSABI        7
  45 #define EI_PAD          8
  46
  47 #define ELFMAG0         0x7f            /* EI_MAG */
  48 #define ELFMAG1         'E'
  49 #define ELFMAG2         'L'
  50 #define ELFMAG3         'F'
  51 #define ELFMAG          "\177ELF"
  52 #define SELFMAG         4
  53
  54 #define ELFCLASSNONE    0               /* EI_CLASS */
  55 #define ELFCLASS32      1
  56 #define ELFCLASS64      2
  57 #define ELFCLASSNUM     3
  58
  59 #define ELFDATANONE     0               /* e_ident[EI_DATA] */
  60 #define ELFDATA2LSB     1
  61 #define ELFDATA2MSB     2
  62
  63 #define EV_NONE         0               /* e_version, EI_VERSION */
  64 #define EV_CURRENT      1
  65 #define EV_NUM          2
  66
  67 #define ELFOSABI_NONE   0
  68 #define ELFOSABI_LINUX  3
  69
  70 #define SHT_STRTAB      3
  71
  72 #pragma pack(1)
  73
  74
  75 typedef struct
  76 {
  77     unsigned char       e_ident[EI_NIDENT];     /* Magic number and other info */
  78     uint16_t    e_type;                 /* Object file type */
  79     uint16_t    e_machine;              /* Architecture */
  80     uint32_t    e_version;              /* Object file version */
  81     uint32_t    e_entry;                /* Entry point virtual address */
  82     uint32_t    e_phoff;                /* Program header table file offset */
  83     uint32_t    e_shoff;                /* Section header table file offset */
  84     uint32_t    e_flags;                /* Processor-specific flags */
  85     uint16_t    e_ehsize;               /* ELF header size in bytes */
  86     uint16_t    e_phentsize;    /* Program header table entry size */
  87     uint16_t    e_phnum;                /* Program header table entry count */
  88     uint16_t    e_shentsize;    /* Section header table entry size */
  89     uint16_t    e_shnum;                /* Section header table entry count */
  90     uint16_t    e_shstrndx;             /* Section header string table index */
  91 } Elf32_Ehdr;
  92
  93 typedef struct
  94 {
  95     unsigned char       e_ident[EI_NIDENT];     /* Magic number and other info */
  96     uint16_t    e_type;                 /* Object file type */
  97     uint16_t    e_machine;              /* Architecture */
  98     uint32_t    e_version;              /* Object file version */
  99     uint64_t    e_entry;                /* Entry point virtual address */
 100     uint64_t    e_phoff;                /* Program header table file offset */
 101     uint64_t    e_shoff;                /* Section header table file offset */
 102     uint32_t    e_flags;                /* Processor-specific flags */
 103     uint16_t    e_ehsize;               /* ELF header size in bytes */
 104     uint16_t    e_phentsize;    /* Program header table entry size */
 105     uint16_t    e_phnum;                /* Program header table entry count */
 106     uint16_t    e_shentsize;    /* Section header table entry size */
 107     uint16_t    e_shnum;                /* Section header table entry count */
 108     uint16_t    e_shstrndx;             /* Section header string table index */
 109 } Elf64_Ehdr;
 110
 111 typedef struct
 112 {
 113     uint32_t    sh_name;                /* Section name (string tbl index) */
 114     uint32_t    sh_type;                /* Section type */
 115     uint32_t    sh_flags;               /* Section flags */
 116     uint32_t    sh_addr;                /* Section virtual addr at execution */
 117     uint32_t    sh_offset;              /* Section file offset */
 118     uint32_t    sh_size;                /* Section size in bytes */
 119     uint32_t    sh_link;                /* Link to another section */
 120     uint32_t    sh_info;                /* Additional section information */
 121     uint32_t    sh_addralign;   /* Section alignment */
 122     uint32_t    sh_entsize;             /* Entry size if section holds table */
 123 } Elf32_Shdr;
 124
 125 typedef struct
 126 {
 127     uint32_t    sh_name;                /* Section name (string tbl index) */
 128     uint32_t    sh_type;                /* Section type */
 129     uint64_t    sh_flags;               /* Section flags */
 130     uint64_t    sh_addr;                /* Section virtual addr at execution */
 131     uint64_t    sh_offset;              /* Section file offset */
 132     uint64_t    sh_size;                /* Section size in bytes */
 133     uint32_t    sh_link;                /* Link to another section */
 134     uint32_t    sh_info;                /* Additional section information */
 135     uint64_t    sh_addralign;   /* Section alignment */
 136     uint64_t    sh_entsize;             /* Entry size if section holds table */
 137 } Elf64_Shdr;
 138
 139 typedef struct elf32_rel {
 140   uint32_t      r_offset;
 141   uint32_t      r_info;
 142 } Elf32_Rel;
 143
 144 typedef struct elf64_rel {
 145   uint64_t r_offset;    /* Location at which to apply the action */
 146   uint64_t r_info;          /* index and type of relocation */
 147 } Elf64_Rel;
 148
 149 typedef struct elf32_rela{
 150   uint32_t      r_offset;
 151   uint32_t      r_info;
 152   int32_t       r_addend;
 153 } Elf32_Rela;
 154
 155 typedef struct elf64_rela {
 156   uint64_t r_offset;    /* Location at which to apply the action */
 157   uint64_t r_info;          /* index and type of relocation */
 158   int64_t  r_addend;    /* Constant addend used to compute value */
 159 } Elf64_Rela;
 160
 161
 162 struct modversion_info {
 163         unsigned long crc;
 164         char name[64 - sizeof(unsigned long)];
 165 };
 166
 167
 168 typedef struct ko_param
 169 {
 170     unsigned char magic[16];
 171     unsigned long struct_size;
 172     unsigned long pgsize;
 173     unsigned long printk_addr;
 174     unsigned long ro_addr;
 175     unsigned long rw_addr;
 176     unsigned long reg_kprobe_addr;
 177     unsigned long unreg_kprobe_addr;
 178     unsigned long sym_get_addr;
 179     unsigned long sym_get_size;
 180     unsigned long sym_put_addr;
 181     unsigned long sym_put_size;
 182     unsigned long kv_major;
 183     unsigned long ibt;
 184     unsigned long padding[1];
 185 }ko_param;
 186
 187 #pragma pack()
 188
 189 static int verbose = 0;
 190 #define debug(fmt, ...) if(verbose) printf(fmt, ##__VA_ARGS__)
 191
 192 static int vtoykmod_write_file(char *name, void *buf, int size)
 193 {
 194     FILE *fp;
 195
 196     fp = fopen(name, "wb+");
 197     if (!fp)
 198     {
 199         return -1;
 200     }
 201
 202     fwrite(buf, 1, size, fp);
 203     fclose(fp);
 204
 205     return 0;
 206 }
 207
 208 static int vtoykmod_read_file(char *name, char **buf)
 209 {
 210     int size;
 211     FILE *fp;
 212     char *databuf;
 213
 214     fp = fopen(name, "rb");
 215     if (!fp)
 216     {
 217         debug("failed to open %s %d\n", name, errno);
 218         return -1;
 219     }
 220
 221     fseek(fp, 0, SEEK_END);
 222     size = (int)ftell(fp);
 223     fseek(fp, 0, SEEK_SET);
 224
 225     databuf = malloc(size);
 226     if (!databuf)
 227     {
 228         debug("failed to open malloc %d\n", size);
 229         return -1;
 230     }
 231
 232     fread(databuf, 1, size, fp);
 233     fclose(fp);
 234
 235     *buf = databuf;
 236     return size;
 237 }
 238
 239 static int vtoykmod_find_section64(char *buf, char *section, int *offset, int *len)
 240 {
 241     uint16_t i;
 242     int cmplen;
 243     char *name = NULL;
 244     char *strtbl = NULL;
 245     Elf64_Ehdr *elf = NULL;
 246     Elf64_Shdr *sec = NULL;
 247
 248     cmplen = (int)strlen(section);
 249
 250     elf = (Elf64_Ehdr *)buf;
 251     sec = (Elf64_Shdr *)(buf + elf->e_shoff);
 252     strtbl = buf + sec[elf->e_shstrndx].sh_offset;
 253
 254     for (i = 0; i < elf->e_shnum; i++)
 255     {
 256         name = strtbl + sec[i].sh_name;
 257         if (name && strncmp(name, section, cmplen) == 0)
 258         {
 259             *offset = (int)(sec[i].sh_offset);
 260             *len = (int)(sec[i].sh_size);
 261             return 0;
 262         }
 263     }
 264
 265     return 1;
 266 }
 267
 268 static int vtoykmod_find_section32(char *buf, char *section, int *offset, int *len)
 269 {
 270     uint16_t i;
 271     int cmplen;
 272     char *name = NULL;
 273     char *strtbl = NULL;
 274     Elf32_Ehdr *elf = NULL;
 275     Elf32_Shdr *sec = NULL;
 276
 277     cmplen = (int)strlen(section);
 278
 279     elf = (Elf32_Ehdr *)buf;
 280     sec = (Elf32_Shdr *)(buf + elf->e_shoff);
 281     strtbl = buf + sec[elf->e_shstrndx].sh_offset;
 282
 283     for (i = 0; i < elf->e_shnum; i++)
 284     {
 285         name = strtbl + sec[i].sh_name;
 286         if (name && strncmp(name, section, cmplen) == 0)
 287         {
 288             *offset = (int)(sec[i].sh_offset);
 289             *len = (int)(sec[i].sh_size);
 290             return 0;
 291         }
 292     }
 293
 294     return 1;
 295 }
 296
 297 static int vtoykmod_update_modcrc(char *oldmodver, int oldcnt, char *newmodver, int newcnt)
 298 {
 299     int i, j;
 300     struct modversion_info *pold, *pnew;
 301
 302     pold = (struct modversion_info *)oldmodver;
 303     pnew = (struct modversion_info *)newmodver;
 304
 305     for (i = 0; i < oldcnt; i++)
 306     {
 307         for (j = 0; j < newcnt; j++)
 308         {
 309             if (strcmp(pold[i].name, pnew[j].name) == 0)
 310             {
 311                 debug("CRC  0x%08lx --> 0x%08lx  %s\n", pold[i].crc, pnew[i].crc, pold[i].name);
 312                 pold[i].crc = pnew[j].crc;
 313                 break;
 314             }
 315         }
 316     }
 317
 318     return 0;
 319 }
 320
 321 static int vtoykmod_update_vermagic(char *oldbuf, int oldsize, char *newbuf, int newsize, int *modver)
 322 {
 323     int i = 0;
 324     char *oldver = NULL;
 325     char *newver = NULL;
 326
 327     *modver = 0;
 328
 329     for (i = 0; i < oldsize - 9; i++)
 330     {
 331         if (strncmp(oldbuf + i, "vermagic=", 9) == 0)
 332         {
 333             oldver = oldbuf + i + 9;
 334             debug("Find old vermagic at %d <%s>\n", i, oldver);
 335             break;
 336         }
 337     }
 338
 339     for (i = 0; i < newsize - 9; i++)
 340     {
 341         if (strncmp(newbuf + i, "vermagic=", 9) == 0)
 342         {
 343             newver = newbuf + i + 9;
 344             debug("Find new vermagic at %d <%s>\n", i, newver);
 345             break;
 346         }
 347     }
 348
 349     if (oldver && newver)
 350     {
 351         memcpy(oldver, newver, strlen(newver) + 1);
 352         //if (strstr(newver, "modversions"))
 353         {
 354             *modver = 1;
 355         }
 356     }
 357
 358     return 0;
 359 }
 360
 361 int vtoykmod_update(char *oldko, char *newko)
 362 {
 363     int rc = 0;
 364     int modver = 0;
 365     int oldoff, oldlen;
 366     int newoff, newlen;
 367     int oldsize, newsize;
 368     char *newbuf, *oldbuf;
 369
 370     oldsize = vtoykmod_read_file(oldko, &oldbuf);
 371     newsize = vtoykmod_read_file(newko, &newbuf);
 372     if (oldsize < 0 || newsize < 0)
 373     {
 374         return 1;
 375     }
 376
 377     /* 1: update vermagic */
 378     vtoykmod_update_vermagic(oldbuf, oldsize, newbuf, newsize, &modver);
 379
 380     /* 2: update modversion crc */
 381     if (modver)
 382     {
 383         if (oldbuf[EI_CLASS] == ELFCLASS64)
 384         {
 385             rc  = vtoykmod_find_section64(oldbuf, "__versions", &oldoff, &oldlen);
 386             rc += vtoykmod_find_section64(newbuf, "__versions", &newoff, &newlen);
 387         }
 388         else
 389         {
 390             rc  = vtoykmod_find_section32(oldbuf, "__versions", &oldoff, &oldlen);
 391             rc += vtoykmod_find_section32(newbuf, "__versions", &newoff, &newlen);
 392         }
 393
 394         if (rc == 0)
 395         {
 396             vtoykmod_update_modcrc(oldbuf + oldoff, oldlen / 64, newbuf + newoff, newlen / 64);
 397         }
 398     }
 399     else
 400     {
 401         debug("no need to proc modversions\n");
 402     }
 403
 404     /* 3: update relocate address */
 405     if (oldbuf[EI_CLASS] == ELFCLASS64)
 406     {
 407         Elf64_Rela *oldRela, *newRela;
 408
 409         rc  = vtoykmod_find_section64(oldbuf, ".rela.gnu.linkonce.this_module", &oldoff, &oldlen);
 410         rc += vtoykmod_find_section64(newbuf, ".rela.gnu.linkonce.this_module", &newoff, &newlen);
 411         if (rc == 0)
 412         {
 413             oldRela = (Elf64_Rela *)(oldbuf + oldoff);
 414             newRela = (Elf64_Rela *)(newbuf + newoff);
 415
 416             debug("init_module rela: 0x%llx --> 0x%llx\n", (_ull)(oldRela[0].r_offset), (_ull)(newRela[0].r_offset));
 417             oldRela[0].r_offset = newRela[0].r_offset;
 418             oldRela[0].r_addend = newRela[0].r_addend;
 419
 420             debug("cleanup_module rela: 0x%llx --> 0x%llx\n", (_ull)(oldRela[1].r_offset), (_ull)(newRela[1].r_offset));
 421             oldRela[1].r_offset = newRela[1].r_offset;
 422             oldRela[1].r_addend = newRela[1].r_addend;
 423         }
 424         else
 425         {
 426             debug("section .rela.gnu.linkonce.this_module not found\n");
 427         }
 428     }
 429     else
 430     {
 431         Elf32_Rel *oldRel, *newRel;
 432
 433         rc  = vtoykmod_find_section32(oldbuf, ".rel.gnu.linkonce.this_module", &oldoff, &oldlen);
 434         rc += vtoykmod_find_section32(newbuf, ".rel.gnu.linkonce.this_module", &newoff, &newlen);
 435         if (rc == 0)
 436         {
 437             oldRel = (Elf32_Rel *)(oldbuf + oldoff);
 438             newRel = (Elf32_Rel *)(newbuf + newoff);
 439
 440             debug("init_module rel: 0x%x --> 0x%x\n", oldRel[0].r_offset, newRel[0].r_offset);
 441             oldRel[0].r_offset = newRel[0].r_offset;
 442
 443             debug("cleanup_module rel: 0x%x --> 0x%x\n", oldRel[0].r_offset, newRel[0].r_offset);
 444             oldRel[1].r_offset = newRel[1].r_offset;
 445         }
 446         else
 447         {
 448             debug("section .rel.gnu.linkonce.this_module not found\n");
 449         }
 450     }
 451
 452     vtoykmod_write_file(oldko, oldbuf, oldsize);
 453
 454     free(oldbuf);
 455     free(newbuf);
 456
 457     return 0;
 458 }
 459
 460 int vtoykmod_fill_param(char **argv)
 461 {
 462     int i;
 463     int size;
 464     char *buf = NULL;
 465     ko_param *param;
 466     unsigned char magic[16] = { magic_sig };
 467
 468     size = vtoykmod_read_file(argv[0], &buf);
 469     if (size < 0)
 470     {
 471         return 1;
 472     }
 473
 474     for (i = 0; i < size; i++)
 475     {
 476         if (memcmp(buf + i, magic, 16) == 0)
 477         {
 478             debug("Find param magic at %d\n", i);
 479             param = (ko_param *)(buf + i);
 480
 481             param->struct_size = (unsigned long)sizeof(ko_param);
 482             param->pgsize = strtoul(argv[1], NULL, 10);
 483             param->printk_addr = strtoul(argv[2], NULL, 16);
 484             param->ro_addr = strtoul(argv[3], NULL, 16);
 485             param->rw_addr = strtoul(argv[4], NULL, 16);
 486             param->sym_get_addr = strtoul(argv[5], NULL, 16);
 487             param->sym_get_size = strtoul(argv[6], NULL, 10);
 488             param->sym_put_addr = strtoul(argv[7], NULL, 16);
 489             param->sym_put_size = strtoul(argv[8], NULL, 10);
 490             param->reg_kprobe_addr = strtoul(argv[9], NULL, 16);
 491             param->unreg_kprobe_addr = strtoul(argv[10], NULL, 16);
 492             param->kv_major = (unsigned long)(argv[11][0] - '0');
 493             param->ibt = strtoul(argv[12], NULL, 16);;
 494
 495             debug("pgsize=%lu (%s)\n", param->pgsize, argv[1]);
 496             debug("printk_addr=0x%lx (%s)\n", param->printk_addr, argv[2]);
 497             debug("ro_addr=0x%lx (%s)\n", param->ro_addr, argv[3]);
 498             debug("rw_addr=0x%lx (%s)\n", param->rw_addr, argv[4]);
 499             debug("sym_get_addr=0x%lx (%s)\n", param->sym_get_addr, argv[5]);
 500             debug("sym_get_size=%lu (%s)\n", param->sym_get_size, argv[6]);
 501             debug("sym_put_addr=0x%lx (%s)\n", param->sym_put_addr, argv[7]);
 502             debug("sym_put_size=%lu (%s)\n", param->sym_put_size, argv[8]);
 503             debug("reg_kprobe_addr=0x%lx (%s)\n", param->reg_kprobe_addr, argv[9]);
 504             debug("unreg_kprobe_addr=0x%lx (%s)\n", param->unreg_kprobe_addr, argv[10]);
 505             debug("kv_major=%lu (%s)\n", param->kv_major, argv[11]);
 506             debug("ibt=0x%lx (%s)\n", param->ibt, argv[12]);
 507
 508             break;
 509         }
 510     }
 511
 512     if (i >= size)
 513     {
 514         debug("### param magic not found \n");
 515     }
 516
 517     vtoykmod_write_file(argv[0], buf, size);
 518
 519     free(buf);
 520     return 0;
 521 }
 522
 523 #ifdef VTOY_X86_64
 524 static int vtoykmod_check_ibt(void)
 525 {
 526     uint32_t eax = 0, ebx = 0, ecx = 0, edx = 0;
 527
 528     __cpuid_count(7, 0, eax, ebx, ecx, edx);
 529
 530     if (edx & (1 << 20))
 531     {
 532         return 0;
 533     }
 534     return 1;
 535 }
 536 #else
 537 static int vtoykmod_check_ibt(void)
 538 {
 539     return 1;
 540 }
 541 #endif
 542
 543 int vtoykmod_main(int argc, char **argv)
 544 {
 545     int i;
 546
 547     for (i = 0; i < argc; i++)
 548     {
 549         if (argv[i][0] == '-' && argv[i][1] == 'v')
 550         {
 551             verbose = 1;
 552             break;
 553         }
 554     }
 555
 556     if (argv[1][0] == '-' && argv[1][1] == 'f')
 557     {
 558         return vtoykmod_fill_param(argv + 2);
 559     }
 560     else if (argv[1][0] == '-' && argv[1][1] == 'u')
 561     {
 562         return vtoykmod_update(argv[2], argv[3]);
 563     }
 564     else if (argv[1][0] == '-' && argv[1][1] == 'I')
 565     {
 566         return vtoykmod_check_ibt();
 567     }
 568
 569     return 0;
 570 }
 571
 572 // wrapper main
 573 #ifndef BUILD_VTOY_TOOL
 574 int main(int argc, char **argv)
 575 {
 576     return vtoykmod_main(argc, argv);
 577 }
 578 #endif
 579