vtoykmod.c

/******************************************************************************
 * vtoykmod.c  ---- ventoy kmod
 *
 * Copyright (c) 2021, longpanda <admin@ventoy.net>
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 3 of the
 * License, or (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <errno.h>
#include <unistd.h>
#ifdef VTOY_X86_64
#include <cpuid.h>
#endif

#define _ull unsigned long long

#define magic_sig 0xB0, 0xB1, 0xB2, 0xB3, 0xB4, 0xB5, 0xB6, 0xB7, 0xB8, 0xB9, 0xBA, 0xBB, 0xBC, 0xBD, 0xBE, 0xBF

#define EI_NIDENT (16)

#define	EI_MAG0		0		/* e_ident[] indexes */
#define	EI_MAG1		1
#define	EI_MAG2		2
#define	EI_MAG3		3
#define	EI_CLASS	4
#define	EI_DATA		5
#define	EI_VERSION	6
#define	EI_OSABI	7
#define	EI_PAD		8

#define	ELFMAG0		0x7f		/* EI_MAG */
#define	ELFMAG1		'E'
#define	ELFMAG2		'L'
#define	ELFMAG3		'F'
#define	ELFMAG		"\177ELF"
#define	SELFMAG		4

#define	ELFCLASSNONE	0		/* EI_CLASS */
#define	ELFCLASS32	1
#define	ELFCLASS64	2
#define	ELFCLASSNUM	3

#define ELFDATANONE	0		/* e_ident[EI_DATA] */
#define ELFDATA2LSB	1
#define ELFDATA2MSB	2

#define EV_NONE		0		/* e_version, EI_VERSION */
#define EV_CURRENT	1
#define EV_NUM		2

#define ELFOSABI_NONE	0
#define ELFOSABI_LINUX	3

#define SHT_STRTAB	3

#pragma pack(1)


typedef struct
{
    unsigned char	e_ident[EI_NIDENT];	/* Magic number and other info */
    uint16_t	e_type;			/* Object file type */
    uint16_t	e_machine;		/* Architecture */
    uint32_t	e_version;		/* Object file version */
    uint32_t	e_entry;		/* Entry point virtual address */
    uint32_t	e_phoff;		/* Program header table file offset */
    uint32_t	e_shoff;		/* Section header table file offset */
    uint32_t	e_flags;		/* Processor-specific flags */
    uint16_t	e_ehsize;		/* ELF header size in bytes */
    uint16_t	e_phentsize;	/* Program header table entry size */
    uint16_t	e_phnum;		/* Program header table entry count */
    uint16_t	e_shentsize;	/* Section header table entry size */
    uint16_t	e_shnum;		/* Section header table entry count */
    uint16_t	e_shstrndx;		/* Section header string table index */
} Elf32_Ehdr;

typedef struct
{
    unsigned char	e_ident[EI_NIDENT];	/* Magic number and other info */
    uint16_t	e_type;			/* Object file type */
    uint16_t	e_machine;		/* Architecture */
    uint32_t	e_version;		/* Object file version */
    uint64_t	e_entry;		/* Entry point virtual address */
    uint64_t	e_phoff;		/* Program header table file offset */
    uint64_t	e_shoff;		/* Section header table file offset */
    uint32_t	e_flags;		/* Processor-specific flags */
    uint16_t	e_ehsize;		/* ELF header size in bytes */
    uint16_t	e_phentsize;	/* Program header table entry size */
    uint16_t	e_phnum;		/* Program header table entry count */
    uint16_t	e_shentsize;	/* Section header table entry size */
    uint16_t	e_shnum;		/* Section header table entry count */
    uint16_t	e_shstrndx;		/* Section header string table index */
} Elf64_Ehdr;

typedef struct
{
    uint32_t	sh_name;		/* Section name (string tbl index) */
    uint32_t	sh_type;		/* Section type */
    uint32_t	sh_flags;		/* Section flags */
    uint32_t	sh_addr;		/* Section virtual addr at execution */
    uint32_t	sh_offset;		/* Section file offset */
    uint32_t	sh_size;		/* Section size in bytes */
    uint32_t	sh_link;		/* Link to another section */
    uint32_t	sh_info;		/* Additional section information */
    uint32_t	sh_addralign;	/* Section alignment */
    uint32_t	sh_entsize;		/* Entry size if section holds table */
} Elf32_Shdr;

typedef struct
{
    uint32_t	sh_name;		/* Section name (string tbl index) */
    uint32_t	sh_type;		/* Section type */
    uint64_t	sh_flags;		/* Section flags */
    uint64_t	sh_addr;		/* Section virtual addr at execution */
    uint64_t	sh_offset;		/* Section file offset */
    uint64_t	sh_size;		/* Section size in bytes */
    uint32_t	sh_link;		/* Link to another section */
    uint32_t	sh_info;		/* Additional section information */
    uint64_t	sh_addralign;	/* Section alignment */
    uint64_t	sh_entsize;		/* Entry size if section holds table */
} Elf64_Shdr;

typedef struct elf32_rel {
  uint32_t	r_offset;
  uint32_t	r_info;
} Elf32_Rel;

typedef struct elf64_rel {
  uint64_t r_offset;	/* Location at which to apply the action */
  uint64_t r_info;	    /* index and type of relocation */
} Elf64_Rel;

typedef struct elf32_rela{
  uint32_t	r_offset;
  uint32_t	r_info;
  int32_t	r_addend;
} Elf32_Rela;

typedef struct elf64_rela {
  uint64_t r_offset;	/* Location at which to apply the action */
  uint64_t r_info;	    /* index and type of relocation */
  int64_t  r_addend;	/* Constant addend used to compute value */
} Elf64_Rela;


struct modversion_info {
	unsigned long crc;
	char name[64 - sizeof(unsigned long)];
};

struct modversion_info2 {
	/* Offset of the next modversion entry in relation to this one. */
	uint32_t next;
	uint32_t crc;
	char name[0];
};


typedef struct ko_param
{
    unsigned char magic[16];
    unsigned long struct_size;
    unsigned long pgsize;
    unsigned long printk_addr;
    unsigned long ro_addr;
    unsigned long rw_addr;
    unsigned long reg_kprobe_addr;
    unsigned long unreg_kprobe_addr;
    unsigned long sym_get_addr;
    unsigned long sym_get_size;
    unsigned long sym_put_addr;
    unsigned long sym_put_size;
    unsigned long kv_major;
    unsigned long ibt;
    unsigned long kv_minor;
    unsigned long blkdev_get_addr;
    unsigned long blkdev_put_addr;
    unsigned long bdev_open_addr;
    unsigned long kv_subminor;
    unsigned long padding[1];
}ko_param;

#pragma pack()

static int verbose = 0;
#define debug(fmt, ...) if(verbose) printf(fmt, ##__VA_ARGS__)

static int vtoykmod_write_file(char *name, void *buf, int size)
{
    FILE *fp;

    fp = fopen(name, "wb+");
    if (!fp)
    {
        return -1;
    }
    
    fwrite(buf, 1, size, fp);
    fclose(fp);

    return 0;
}

static int vtoykmod_read_file(char *name, char **buf)
{
    int size;
    FILE *fp;
    char *databuf;

    fp = fopen(name, "rb");
    if (!fp)
    {
        debug("failed to open %s %d\n", name, errno);
        return -1;
    }
    
    fseek(fp, 0, SEEK_END);
    size = (int)ftell(fp);
    fseek(fp, 0, SEEK_SET);

    databuf = malloc(size);
    if (!databuf)
    {
        debug("failed to open malloc %d\n", size);
        return -1;
    }
    
    fread(databuf, 1, size, fp);
    fclose(fp);

    *buf = databuf;
    return size;
}

static int vtoykmod_find_section64(char *buf, char *section, int *offset, int *len, Elf64_Shdr **shdr)
{
    uint16_t i;
    int cmplen;
    char *name = NULL;
    char *strtbl = NULL;
    Elf64_Ehdr *elf = NULL;
    Elf64_Shdr *sec = NULL;

    cmplen = (int)strlen(section);

    elf = (Elf64_Ehdr *)buf;
    sec = (Elf64_Shdr *)(buf + elf->e_shoff);
    strtbl = buf + sec[elf->e_shstrndx].sh_offset;

    for (i = 0; i < elf->e_shnum; i++)
    {
        name = strtbl + sec[i].sh_name;
        if (name && strncmp(name, section, cmplen) == 0)
        {
            *offset = (int)(sec[i].sh_offset);
            *len = (int)(sec[i].sh_size);
            if (shdr)
            {
                *shdr = sec + i;
            }
            return 0;
        }
    }

    return 1;
}

static int vtoykmod_find_section32(char *buf, char *section, int *offset, int *len, Elf32_Shdr **shdr)
{
    uint16_t i;
    int cmplen;
    char *name = NULL;
    char *strtbl = NULL;
    Elf32_Ehdr *elf = NULL;
    Elf32_Shdr *sec = NULL;

    cmplen = (int)strlen(section);

    elf = (Elf32_Ehdr *)buf;
    sec = (Elf32_Shdr *)(buf + elf->e_shoff);
    strtbl = buf + sec[elf->e_shstrndx].sh_offset;

    for (i = 0; i < elf->e_shnum; i++)
    {
        name = strtbl + sec[i].sh_name;
        if (name && strncmp(name, section, cmplen) == 0)
        {
            *offset = (int)(sec[i].sh_offset);
            *len = (int)(sec[i].sh_size);
            if (shdr)
            {
                *shdr = sec + i;
            }
            return 0;
        }
    }

    return 1;
}

static int vtoykmod_update_modcrc1(char *oldmodver, int oldcnt, char *newmodver, int newcnt)
{
    int i, j;
    struct modversion_info *pold, *pnew;

    pold = (struct modversion_info *)oldmodver;
    pnew = (struct modversion_info *)newmodver;

    debug("module update modver format 1\n");
    for (i = 0; i < oldcnt; i++)
    {
        for (j = 0; j < newcnt; j++)
        {
            if (strcmp(pold[i].name, pnew[j].name) == 0)
            {
                debug("CRC  0x%08lx --> 0x%08lx  %s\n", pold[i].crc, pnew[i].crc, pold[i].name);
                pold[i].crc = pnew[j].crc;
                break;
            }
        }
    }

    return 0;
}


static int vtoykmod_update_modcrc2(char *oldmodver, int oldlen, char *newmodver, int newlen)
{
    struct modversion_info2 *pold, *pnew, *pnewend;

    pold = (struct modversion_info2 *)oldmodver;
    pnew = (struct modversion_info2 *)newmodver;
    pnewend = (struct modversion_info2 *)(newmodver + newlen);

    debug("module update modver format 2\n");
    /* here we think that there is only module_layout in oldmodver */

    for (; pnew < pnewend && pnew->next; pnew = (struct modversion_info2 *)((char *)pnew + pnew->next))
    {
	    if (strcmp(pnew->name, "module_layout") == 0)
        {
            debug("CRC  0x%08x --> 0x%08x  %s\n", pold->crc, pnew->crc, pnew->name);
            memset(pold, 0, oldlen);
            pold->next = 0x18;  /* 8 + module_layout align 8 */
            pold->crc = pnew->crc;
            strcpy(pold->name, pnew->name);
            break;
        }
    }

    return 0;
}


static int vtoykmod_update_modcrc(char *oldmodver, int oldlen, char *newmodver, int newlen)
{
    uint32_t uiCrc = 0;

    memcpy(&uiCrc, newmodver + 4, 4);

    if (uiCrc > 0)
    {
        return vtoykmod_update_modcrc2(oldmodver, oldlen, newmodver, newlen);
    }
    else
    {
        return vtoykmod_update_modcrc1(oldmodver, oldlen / 64, newmodver, newlen / 64);
    }
}

static int vtoykmod_update_vermagic(char *oldbuf, int oldsize, char *newbuf, int newsize, int *modver)
{
    int i = 0;
    char *oldver = NULL;
    char *newver = NULL;

    *modver = 0;

    for (i = 0; i < oldsize - 9; i++)
    {
        if (strncmp(oldbuf + i, "vermagic=", 9) == 0)
        {
            oldver = oldbuf + i + 9;
            debug("Find old vermagic at %d <%s>\n", i, oldver);
            break;
        }
    }
    
    for (i = 0; i < newsize - 9; i++)
    {
        if (strncmp(newbuf + i, "vermagic=", 9) == 0)
        {
            newver = newbuf + i + 9;
            debug("Find new vermagic at %d <%s>\n", i, newver);
            break;
        }
    }

    if (oldver && newver)
    {
        memcpy(oldver, newver, strlen(newver) + 1);
        //if (strstr(newver, "modversions"))
        {
            *modver = 1;
        }
    }

    return 0;
}

int vtoykmod_update(int kvMajor, int kvMinor, char *oldko, char *newko)
{
    int rc = 0;
    int modver = 0;
    int oldoff, oldlen;
    int newoff, newlen;
    int oldsize, newsize;
    char *newbuf, *oldbuf;
    Elf64_Shdr *sec = NULL;

    oldsize = vtoykmod_read_file(oldko, &oldbuf);
    newsize = vtoykmod_read_file(newko, &newbuf);
    if (oldsize < 0 || newsize < 0)
    {
        return 1;
    }

    /* 1: update vermagic */
    vtoykmod_update_vermagic(oldbuf, oldsize, newbuf, newsize, &modver);

    /* 2: update modversion crc */
    if (modver)
    {
        if (oldbuf[EI_CLASS] == ELFCLASS64)
        {
            rc  = vtoykmod_find_section64(oldbuf, "__versions", &oldoff, &oldlen, NULL);
            rc += vtoykmod_find_section64(newbuf, "__versions", &newoff, &newlen, NULL);            
        }
        else
        {
            rc  = vtoykmod_find_section32(oldbuf, "__versions", &oldoff, &oldlen, NULL);
            rc += vtoykmod_find_section32(newbuf, "__versions", &newoff, &newlen, NULL);
        }

        if (rc == 0)
        {
            vtoykmod_update_modcrc(oldbuf + oldoff, oldlen, newbuf + newoff, newlen);
        }
    }
    else
    {
        debug("no need to proc modversions\n");
    }
    
    /* 3: update relocate address */
    if (oldbuf[EI_CLASS] == ELFCLASS64)
    {
        Elf64_Rela *oldRela, *newRela;
        
        rc  = vtoykmod_find_section64(oldbuf, ".rela.gnu.linkonce.this_module", &oldoff, &oldlen, NULL);
        rc += vtoykmod_find_section64(newbuf, ".rela.gnu.linkonce.this_module", &newoff, &newlen, NULL);
        if (rc == 0)
        {
            oldRela = (Elf64_Rela *)(oldbuf + oldoff);
            newRela = (Elf64_Rela *)(newbuf + newoff);
            
            debug("init_module rela: 0x%llx --> 0x%llx\n", (_ull)(oldRela[0].r_offset), (_ull)(newRela[0].r_offset));
            oldRela[0].r_offset = newRela[0].r_offset;
            oldRela[0].r_addend = newRela[0].r_addend;
            
            debug("cleanup_module rela: 0x%llx --> 0x%llx\n", (_ull)(oldRela[1].r_offset), (_ull)(newRela[1].r_offset));
            oldRela[1].r_offset = newRela[1].r_offset;
            oldRela[1].r_addend = newRela[1].r_addend;
        }
        else
        {
            debug("section .rela.gnu.linkonce.this_module not found\n");
        }

        if (kvMajor > 6 || (kvMajor == 6 && kvMinor >= 3))
        {
            rc  = vtoykmod_find_section64(oldbuf, ".gnu.linkonce.this_module", &oldoff, &oldlen, &sec);
            rc += vtoykmod_find_section64(newbuf, ".gnu.linkonce.this_module", &newoff, &newlen, NULL);
            if (rc == 0)
            {
                debug("section .gnu.linkonce.this_module change oldlen:0x%x to newlen:0x%x\n", oldlen, newlen);
                if (sec)
                {
                    sec->sh_size = newlen;                
                }
            }
            else
            {
                debug("section .gnu.linkonce.this_module not found\n");
            }
        }
    }
    else
    {
        Elf32_Rel *oldRel, *newRel;
        
        rc  = vtoykmod_find_section32(oldbuf, ".rel.gnu.linkonce.this_module", &oldoff, &oldlen, NULL);
        rc += vtoykmod_find_section32(newbuf, ".rel.gnu.linkonce.this_module", &newoff, &newlen, NULL);
        if (rc == 0)
        {
            oldRel = (Elf32_Rel *)(oldbuf + oldoff);
            newRel = (Elf32_Rel *)(newbuf + newoff);

            debug("init_module rel: 0x%x --> 0x%x\n", oldRel[0].r_offset, newRel[0].r_offset);
            oldRel[0].r_offset = newRel[0].r_offset;

            debug("cleanup_module rel: 0x%x --> 0x%x\n", oldRel[0].r_offset, newRel[0].r_offset);
            oldRel[1].r_offset = newRel[1].r_offset;
        }
        else
        {
            debug("section .rel.gnu.linkonce.this_module not found\n");
        }
    }

    vtoykmod_write_file(oldko, oldbuf, oldsize);

    free(oldbuf);
    free(newbuf);

    return 0;
}

int vtoykmod_fill_param(char **argv)
{
    int i;
    int size;
    char *buf = NULL;
    ko_param *param;
    unsigned char magic[16] = { magic_sig };
    
    size = vtoykmod_read_file(argv[0], &buf);
    if (size < 0)
    {
        return 1;
    }

    for (i = 0; i < size; i++)
    {
        if (memcmp(buf + i, magic, 16) == 0)
        {
            debug("Find param magic at %d\n", i);
            param = (ko_param *)(buf + i);
            
            param->struct_size = (unsigned long)sizeof(ko_param);
            param->pgsize = strtoul(argv[1], NULL, 10);
            param->printk_addr = strtoul(argv[2], NULL, 16);
            param->ro_addr = strtoul(argv[3], NULL, 16);
            param->rw_addr = strtoul(argv[4], NULL, 16);
            param->sym_get_addr = strtoul(argv[5], NULL, 16);
            param->sym_get_size = strtoul(argv[6], NULL, 10);
            param->sym_put_addr = strtoul(argv[7], NULL, 16);
            param->sym_put_size = strtoul(argv[8], NULL, 10);
            param->reg_kprobe_addr = strtoul(argv[9], NULL, 16);
            param->unreg_kprobe_addr = strtoul(argv[10], NULL, 16);
            param->kv_major = strtoul(argv[11], NULL, 10);
            param->ibt = strtoul(argv[12], NULL, 16);;
            param->kv_minor = strtoul(argv[13], NULL, 10);
            param->blkdev_get_addr = strtoul(argv[14], NULL, 16);
            param->blkdev_put_addr = strtoul(argv[15], NULL, 16);
            param->kv_subminor = strtoul(argv[16], NULL, 10);
            param->bdev_open_addr = strtoul(argv[17], NULL, 16);

            debug("pgsize=%lu (%s)\n", param->pgsize, argv[1]);
            debug("printk_addr=0x%lx (%s)\n", param->printk_addr, argv[2]);
            debug("ro_addr=0x%lx (%s)\n", param->ro_addr, argv[3]);
            debug("rw_addr=0x%lx (%s)\n", param->rw_addr, argv[4]);
            debug("sym_get_addr=0x%lx (%s)\n", param->sym_get_addr, argv[5]);
            debug("sym_get_size=%lu (%s)\n", param->sym_get_size, argv[6]);
            debug("sym_put_addr=0x%lx (%s)\n", param->sym_put_addr, argv[7]);
            debug("sym_put_size=%lu (%s)\n", param->sym_put_size, argv[8]);
            debug("reg_kprobe_addr=0x%lx (%s)\n", param->reg_kprobe_addr, argv[9]);
            debug("unreg_kprobe_addr=0x%lx (%s)\n", param->unreg_kprobe_addr, argv[10]);
            debug("kv_major=%lu (%s)\n", param->kv_major, argv[11]);
            debug("ibt=0x%lx (%s)\n", param->ibt, argv[12]);
            debug("kv_minor=%lu (%s)\n", param->kv_minor, argv[13]);
            debug("blkdev_get_addr=0x%lx (%s)\n", param->blkdev_get_addr, argv[14]);
            debug("blkdev_put_addr=0x%lx (%s)\n", param->blkdev_put_addr, argv[15]);
            debug("kv_subminor=%lu (%s)\n", param->kv_subminor, argv[16]);
            debug("bdev_open_addr=0x%lx (%s)\n", param->bdev_open_addr, argv[17]);
            
            break;
        }
    }

    if (i >= size)
    {
        debug("### param magic not found \n");
    }
    
    vtoykmod_write_file(argv[0], buf, size);

    free(buf);
    return 0;
}

#ifdef VTOY_X86_64
static int vtoykmod_check_ibt(void)
{
    uint32_t eax = 0, ebx = 0, ecx = 0, edx = 0;
    
    __cpuid_count(7, 0, eax, ebx, ecx, edx);
    
    if (edx & (1 << 20))
    {
        return 0;
    }
    return 1;
}
#else
static int vtoykmod_check_ibt(void)
{
    return 1;
}
#endif

int vtoykmod_main(int argc, char **argv)
{
    int i;
    int kvMajor = 0;
    int kvMinor = 0;

    for (i = 0; i < argc; i++)
    {
        if (argv[i][0] == '-' && argv[i][1] == 'v')
        {
            verbose = 1;
            break;
        }
    }

    if (verbose)
    {
        printf("==== Dump Argv ====\n");
        for (i = 0; i < argc; i++)
        {
            printf("<%s> ", argv[i]);
        }
        printf("\n");
    }

    if (argv[1][0] == '-' && argv[1][1] == 'f')
    {
        return vtoykmod_fill_param(argv + 2);
    }
    else if (argv[1][0] == '-' && argv[1][1] == 'u')
    {
        kvMajor = (int)strtol(argv[2], NULL, 10);
        kvMinor = (int)strtol(argv[3], NULL, 10);
        return vtoykmod_update(kvMajor, kvMinor, argv[4], argv[5]);
    }
    else if (argv[1][0] == '-' && argv[1][1] == 'I')
    {
        return vtoykmod_check_ibt();
    }

    return 0;
}

// wrapper main
#ifndef BUILD_VTOY_TOOL
int main(int argc, char **argv)
{
    return vtoykmod_main(argc, argv);
}
#endif