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 struct modversion_info2 {
 168         /* Offset of the next modversion entry in relation to this one. */
 169         uint32_t next;
 170         uint32_t crc;
 171         char name[0];
 172 };
 173
 174
 175 typedef struct ko_param
 176 {
 177     unsigned char magic[16];
 178     unsigned long struct_size;
 179     unsigned long pgsize;
 180     unsigned long printk_addr;
 181     unsigned long ro_addr;
 182     unsigned long rw_addr;
 183     unsigned long reg_kprobe_addr;
 184     unsigned long unreg_kprobe_addr;
 185     unsigned long sym_get_addr;
 186     unsigned long sym_get_size;
 187     unsigned long sym_put_addr;
 188     unsigned long sym_put_size;
 189     unsigned long kv_major;
 190     unsigned long ibt;
 191     unsigned long padding[1];
 192 }ko_param;
 193
 194 #pragma pack()
 195
 196 static int verbose = 0;
 197 #define debug(fmt, ...) if(verbose) printf(fmt, ##__VA_ARGS__)
 198
 199 static int vtoykmod_write_file(char *name, void *buf, int size)
 200 {
 201     FILE *fp;
 202
 203     fp = fopen(name, "wb+");
 204     if (!fp)
 205     {
 206         return -1;
 207     }
 208
 209     fwrite(buf, 1, size, fp);
 210     fclose(fp);
 211
 212     return 0;
 213 }
 214
 215 static int vtoykmod_read_file(char *name, char **buf)
 216 {
 217     int size;
 218     FILE *fp;
 219     char *databuf;
 220
 221     fp = fopen(name, "rb");
 222     if (!fp)
 223     {
 224         debug("failed to open %s %d\n", name, errno);
 225         return -1;
 226     }
 227
 228     fseek(fp, 0, SEEK_END);
 229     size = (int)ftell(fp);
 230     fseek(fp, 0, SEEK_SET);
 231
 232     databuf = malloc(size);
 233     if (!databuf)
 234     {
 235         debug("failed to open malloc %d\n", size);
 236         return -1;
 237     }
 238
 239     fread(databuf, 1, size, fp);
 240     fclose(fp);
 241
 242     *buf = databuf;
 243     return size;
 244 }
 245
 246 static int vtoykmod_find_section64(char *buf, char *section, int *offset, int *len)
 247 {
 248     uint16_t i;
 249     int cmplen;
 250     char *name = NULL;
 251     char *strtbl = NULL;
 252     Elf64_Ehdr *elf = NULL;
 253     Elf64_Shdr *sec = NULL;
 254
 255     cmplen = (int)strlen(section);
 256
 257     elf = (Elf64_Ehdr *)buf;
 258     sec = (Elf64_Shdr *)(buf + elf->e_shoff);
 259     strtbl = buf + sec[elf->e_shstrndx].sh_offset;
 260
 261     for (i = 0; i < elf->e_shnum; i++)
 262     {
 263         name = strtbl + sec[i].sh_name;
 264         if (name && strncmp(name, section, cmplen) == 0)
 265         {
 266             *offset = (int)(sec[i].sh_offset);
 267             *len = (int)(sec[i].sh_size);
 268             return 0;
 269         }
 270     }
 271
 272     return 1;
 273 }
 274
 275 static int vtoykmod_find_section32(char *buf, char *section, int *offset, int *len)
 276 {
 277     uint16_t i;
 278     int cmplen;
 279     char *name = NULL;
 280     char *strtbl = NULL;
 281     Elf32_Ehdr *elf = NULL;
 282     Elf32_Shdr *sec = NULL;
 283
 284     cmplen = (int)strlen(section);
 285
 286     elf = (Elf32_Ehdr *)buf;
 287     sec = (Elf32_Shdr *)(buf + elf->e_shoff);
 288     strtbl = buf + sec[elf->e_shstrndx].sh_offset;
 289
 290     for (i = 0; i < elf->e_shnum; i++)
 291     {
 292         name = strtbl + sec[i].sh_name;
 293         if (name && strncmp(name, section, cmplen) == 0)
 294         {
 295             *offset = (int)(sec[i].sh_offset);
 296             *len = (int)(sec[i].sh_size);
 297             return 0;
 298         }
 299     }
 300
 301     return 1;
 302 }
 303
 304 static int vtoykmod_update_modcrc1(char *oldmodver, int oldcnt, char *newmodver, int newcnt)
 305 {
 306     int i, j;
 307     struct modversion_info *pold, *pnew;
 308
 309     pold = (struct modversion_info *)oldmodver;
 310     pnew = (struct modversion_info *)newmodver;
 311
 312     debug("module update modver format 1\n");
 313     for (i = 0; i < oldcnt; i++)
 314     {
 315         for (j = 0; j < newcnt; j++)
 316         {
 317             if (strcmp(pold[i].name, pnew[j].name) == 0)
 318             {
 319                 debug("CRC  0x%08lx --> 0x%08lx  %s\n", pold[i].crc, pnew[i].crc, pold[i].name);
 320                 pold[i].crc = pnew[j].crc;
 321                 break;
 322             }
 323         }
 324     }
 325
 326     return 0;
 327 }
 328
 329
 330 static int vtoykmod_update_modcrc2(char *oldmodver, int oldlen, char *newmodver, int newlen)
 331 {
 332     struct modversion_info2 *pold, *pnew, *pnewend;
 333
 334     pold = (struct modversion_info2 *)oldmodver;
 335     pnew = (struct modversion_info2 *)newmodver;
 336     pnewend = (struct modversion_info2 *)(newmodver + newlen);
 337
 338     debug("module update modver format 2\n");
 339     /* here we think that there is only module_layout in oldmodver */
 340
 341     for (; pnew < pnewend && pnew->next; pnew = (struct modversion_info2 *)((char *)pnew + pnew->next))
 342     {
 343             if (strcmp(pnew->name, "module_layout") == 0)
 344         {
 345             debug("CRC  0x%08x --> 0x%08x  %s\n", pold->crc, pnew->crc, pnew->name);
 346             memset(pold, 0, oldlen);
 347             pold->next = 0x18;  /* 8 + module_layout align 8 */
 348             pold->crc = pnew->crc;
 349             strcpy(pold->name, pnew->name);
 350             break;
 351         }
 352     }
 353
 354     return 0;
 355 }
 356
 357
 358 static int vtoykmod_update_modcrc(char *oldmodver, int oldlen, char *newmodver, int newlen)
 359 {
 360     uint32_t uiCrc = 0;
 361
 362     memcpy(&uiCrc, newmodver + 4, 4);
 363
 364     if (uiCrc > 0)
 365     {
 366         return vtoykmod_update_modcrc2(oldmodver, oldlen, newmodver, newlen);
 367     }
 368     else
 369     {
 370         return vtoykmod_update_modcrc1(oldmodver, oldlen / 64, newmodver, newlen / 64);
 371     }
 372 }
 373
 374 static int vtoykmod_update_vermagic(char *oldbuf, int oldsize, char *newbuf, int newsize, int *modver)
 375 {
 376     int i = 0;
 377     char *oldver = NULL;
 378     char *newver = NULL;
 379
 380     *modver = 0;
 381
 382     for (i = 0; i < oldsize - 9; i++)
 383     {
 384         if (strncmp(oldbuf + i, "vermagic=", 9) == 0)
 385         {
 386             oldver = oldbuf + i + 9;
 387             debug("Find old vermagic at %d <%s>\n", i, oldver);
 388             break;
 389         }
 390     }
 391
 392     for (i = 0; i < newsize - 9; i++)
 393     {
 394         if (strncmp(newbuf + i, "vermagic=", 9) == 0)
 395         {
 396             newver = newbuf + i + 9;
 397             debug("Find new vermagic at %d <%s>\n", i, newver);
 398             break;
 399         }
 400     }
 401
 402     if (oldver && newver)
 403     {
 404         memcpy(oldver, newver, strlen(newver) + 1);
 405         //if (strstr(newver, "modversions"))
 406         {
 407             *modver = 1;
 408         }
 409     }
 410
 411     return 0;
 412 }
 413
 414 int vtoykmod_update(char *oldko, char *newko)
 415 {
 416     int rc = 0;
 417     int modver = 0;
 418     int oldoff, oldlen;
 419     int newoff, newlen;
 420     int oldsize, newsize;
 421     char *newbuf, *oldbuf;
 422
 423     oldsize = vtoykmod_read_file(oldko, &oldbuf);
 424     newsize = vtoykmod_read_file(newko, &newbuf);
 425     if (oldsize < 0 || newsize < 0)
 426     {
 427         return 1;
 428     }
 429
 430     /* 1: update vermagic */
 431     vtoykmod_update_vermagic(oldbuf, oldsize, newbuf, newsize, &modver);
 432
 433     /* 2: update modversion crc */
 434     if (modver)
 435     {
 436         if (oldbuf[EI_CLASS] == ELFCLASS64)
 437         {
 438             rc  = vtoykmod_find_section64(oldbuf, "__versions", &oldoff, &oldlen);
 439             rc += vtoykmod_find_section64(newbuf, "__versions", &newoff, &newlen);
 440         }
 441         else
 442         {
 443             rc  = vtoykmod_find_section32(oldbuf, "__versions", &oldoff, &oldlen);
 444             rc += vtoykmod_find_section32(newbuf, "__versions", &newoff, &newlen);
 445         }
 446
 447         if (rc == 0)
 448         {
 449             vtoykmod_update_modcrc(oldbuf + oldoff, oldlen, newbuf + newoff, newlen);
 450         }
 451     }
 452     else
 453     {
 454         debug("no need to proc modversions\n");
 455     }
 456
 457     /* 3: update relocate address */
 458     if (oldbuf[EI_CLASS] == ELFCLASS64)
 459     {
 460         Elf64_Rela *oldRela, *newRela;
 461
 462         rc  = vtoykmod_find_section64(oldbuf, ".rela.gnu.linkonce.this_module", &oldoff, &oldlen);
 463         rc += vtoykmod_find_section64(newbuf, ".rela.gnu.linkonce.this_module", &newoff, &newlen);
 464         if (rc == 0)
 465         {
 466             oldRela = (Elf64_Rela *)(oldbuf + oldoff);
 467             newRela = (Elf64_Rela *)(newbuf + newoff);
 468
 469             debug("init_module rela: 0x%llx --> 0x%llx\n", (_ull)(oldRela[0].r_offset), (_ull)(newRela[0].r_offset));
 470             oldRela[0].r_offset = newRela[0].r_offset;
 471             oldRela[0].r_addend = newRela[0].r_addend;
 472
 473             debug("cleanup_module rela: 0x%llx --> 0x%llx\n", (_ull)(oldRela[1].r_offset), (_ull)(newRela[1].r_offset));
 474             oldRela[1].r_offset = newRela[1].r_offset;
 475             oldRela[1].r_addend = newRela[1].r_addend;
 476         }
 477         else
 478         {
 479             debug("section .rela.gnu.linkonce.this_module not found\n");
 480         }
 481     }
 482     else
 483     {
 484         Elf32_Rel *oldRel, *newRel;
 485
 486         rc  = vtoykmod_find_section32(oldbuf, ".rel.gnu.linkonce.this_module", &oldoff, &oldlen);
 487         rc += vtoykmod_find_section32(newbuf, ".rel.gnu.linkonce.this_module", &newoff, &newlen);
 488         if (rc == 0)
 489         {
 490             oldRel = (Elf32_Rel *)(oldbuf + oldoff);
 491             newRel = (Elf32_Rel *)(newbuf + newoff);
 492
 493             debug("init_module rel: 0x%x --> 0x%x\n", oldRel[0].r_offset, newRel[0].r_offset);
 494             oldRel[0].r_offset = newRel[0].r_offset;
 495
 496             debug("cleanup_module rel: 0x%x --> 0x%x\n", oldRel[0].r_offset, newRel[0].r_offset);
 497             oldRel[1].r_offset = newRel[1].r_offset;
 498         }
 499         else
 500         {
 501             debug("section .rel.gnu.linkonce.this_module not found\n");
 502         }
 503     }
 504
 505     vtoykmod_write_file(oldko, oldbuf, oldsize);
 506
 507     free(oldbuf);
 508     free(newbuf);
 509
 510     return 0;
 511 }
 512
 513 int vtoykmod_fill_param(char **argv)
 514 {
 515     int i;
 516     int size;
 517     char *buf = NULL;
 518     ko_param *param;
 519     unsigned char magic[16] = { magic_sig };
 520
 521     size = vtoykmod_read_file(argv[0], &buf);
 522     if (size < 0)
 523     {
 524         return 1;
 525     }
 526
 527     for (i = 0; i < size; i++)
 528     {
 529         if (memcmp(buf + i, magic, 16) == 0)
 530         {
 531             debug("Find param magic at %d\n", i);
 532             param = (ko_param *)(buf + i);
 533
 534             param->struct_size = (unsigned long)sizeof(ko_param);
 535             param->pgsize = strtoul(argv[1], NULL, 10);
 536             param->printk_addr = strtoul(argv[2], NULL, 16);
 537             param->ro_addr = strtoul(argv[3], NULL, 16);
 538             param->rw_addr = strtoul(argv[4], NULL, 16);
 539             param->sym_get_addr = strtoul(argv[5], NULL, 16);
 540             param->sym_get_size = strtoul(argv[6], NULL, 10);
 541             param->sym_put_addr = strtoul(argv[7], NULL, 16);
 542             param->sym_put_size = strtoul(argv[8], NULL, 10);
 543             param->reg_kprobe_addr = strtoul(argv[9], NULL, 16);
 544             param->unreg_kprobe_addr = strtoul(argv[10], NULL, 16);
 545             param->kv_major = (unsigned long)(argv[11][0] - '0');
 546             param->ibt = strtoul(argv[12], NULL, 16);;
 547
 548             debug("pgsize=%lu (%s)\n", param->pgsize, argv[1]);
 549             debug("printk_addr=0x%lx (%s)\n", param->printk_addr, argv[2]);
 550             debug("ro_addr=0x%lx (%s)\n", param->ro_addr, argv[3]);
 551             debug("rw_addr=0x%lx (%s)\n", param->rw_addr, argv[4]);
 552             debug("sym_get_addr=0x%lx (%s)\n", param->sym_get_addr, argv[5]);
 553             debug("sym_get_size=%lu (%s)\n", param->sym_get_size, argv[6]);
 554             debug("sym_put_addr=0x%lx (%s)\n", param->sym_put_addr, argv[7]);
 555             debug("sym_put_size=%lu (%s)\n", param->sym_put_size, argv[8]);
 556             debug("reg_kprobe_addr=0x%lx (%s)\n", param->reg_kprobe_addr, argv[9]);
 557             debug("unreg_kprobe_addr=0x%lx (%s)\n", param->unreg_kprobe_addr, argv[10]);
 558             debug("kv_major=%lu (%s)\n", param->kv_major, argv[11]);
 559             debug("ibt=0x%lx (%s)\n", param->ibt, argv[12]);
 560
 561             break;
 562         }
 563     }
 564
 565     if (i >= size)
 566     {
 567         debug("### param magic not found \n");
 568     }
 569
 570     vtoykmod_write_file(argv[0], buf, size);
 571
 572     free(buf);
 573     return 0;
 574 }
 575
 576 #ifdef VTOY_X86_64
 577 static int vtoykmod_check_ibt(void)
 578 {
 579     uint32_t eax = 0, ebx = 0, ecx = 0, edx = 0;
 580
 581     __cpuid_count(7, 0, eax, ebx, ecx, edx);
 582
 583     if (edx & (1 << 20))
 584     {
 585         return 0;
 586     }
 587     return 1;
 588 }
 589 #else
 590 static int vtoykmod_check_ibt(void)
 591 {
 592     return 1;
 593 }
 594 #endif
 595
 596 int vtoykmod_main(int argc, char **argv)
 597 {
 598     int i;
 599
 600     for (i = 0; i < argc; i++)
 601     {
 602         if (argv[i][0] == '-' && argv[i][1] == 'v')
 603         {
 604             verbose = 1;
 605             break;
 606         }
 607     }
 608
 609     if (argv[1][0] == '-' && argv[1][1] == 'f')
 610     {
 611         return vtoykmod_fill_param(argv + 2);
 612     }
 613     else if (argv[1][0] == '-' && argv[1][1] == 'u')
 614     {
 615         return vtoykmod_update(argv[2], argv[3]);
 616     }
 617     else if (argv[1][0] == '-' && argv[1][1] == 'I')
 618     {
 619         return vtoykmod_check_ibt();
 620     }
 621
 622     return 0;
 623 }
 624
 625 // wrapper main
 626 #ifndef BUILD_VTOY_TOOL
 627 int main(int argc, char **argv)
 628 {
 629     return vtoykmod_main(argc, argv);
 630 }
 631 #endif
 632