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