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