DMPATCH/dmpatch.c

   1 /******************************************************************************
   2  * dmpatch.c  ---- patch for device-mapper
   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 <linux/init.h>
  22 #include <linux/module.h>
  23 #include <linux/kallsyms.h>
  24 #include <linux/mutex.h>
  25 #include <linux/mempool.h>
  26 #include <linux/delay.h>
  27 #include <linux/wait.h>
  28 #include <linux/slab.h>
  29
  30 #define MAX_PATCH   4
  31
  32 #define magic_sig 0xB0, 0xB1, 0xB2, 0xB3, 0xB4, 0xB5, 0xB6, 0xB7, 0xB8, 0xB9, 0xBA, 0xBB, 0xBC, 0xBD, 0xBE, 0xBF
  33
  34 typedef int (*kprobe_reg_pf)(void *);
  35 typedef void (*kprobe_unreg_pf)(void *);
  36 typedef int (*printk_pf)(const char *fmt, ...);
  37 typedef int (*set_memory_attr_pf)(unsigned long addr, int numpages);
  38
  39 #pragma pack(1)
  40 typedef struct ko_param
  41 {
  42     unsigned char magic[16];
  43     unsigned long struct_size;
  44     unsigned long pgsize;
  45     unsigned long printk_addr;
  46     unsigned long ro_addr;
  47     unsigned long rw_addr;
  48     unsigned long reg_kprobe_addr;
  49     unsigned long unreg_kprobe_addr;
  50     unsigned long sym_get_addr;
  51     unsigned long sym_get_size;
  52     unsigned long sym_put_addr;
  53     unsigned long sym_put_size;
  54     unsigned long kv_major;
  55     unsigned long padding[2];
  56 }ko_param;
  57
  58 #pragma pack()
  59
  60 static printk_pf kprintf = NULL;
  61 static set_memory_attr_pf set_mem_ro = NULL;
  62 static set_memory_attr_pf set_mem_rw = NULL;
  63 static kprobe_reg_pf reg_kprobe = NULL;
  64 static kprobe_unreg_pf unreg_kprobe = NULL;
  65
  66 static volatile ko_param g_ko_param =
  67 {
  68     { magic_sig },
  69     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
  70 };
  71
  72 #if defined(CONFIG_X86_64)
  73 #define PATCH_OP_POS1    3
  74 #define CODE_MATCH1(code, i) \
  75     (code[i] == 0x40 && code[i + 1] == 0x80 && code[i + 2] == 0xce && code[i + 3] == 0x80)
  76
  77 #define PATCH_OP_POS2    1
  78 #define CODE_MATCH2(code, i) \
  79     (code[i] == 0x0C && code[i + 1] == 0x80 && code[i + 2] == 0x89 && code[i + 3] == 0xC6)
  80
  81 #elif defined(CONFIG_X86_32)
  82 #define PATCH_OP_POS1    2
  83 #define CODE_MATCH1(code, i) \
  84     (code[i] == 0x80 && code[i + 1] == 0xca && code[i + 2] == 0x80 && code[i + 3] == 0xe8)
  85
  86 #define PATCH_OP_POS2    2
  87 #define CODE_MATCH2(code, i) \
  88     (code[i] == 0x80 && code[i + 1] == 0xca && code[i + 2] == 0x80 && code[i + 3] == 0xe8)
  89
  90 #else
  91 #error "unsupported arch"
  92 #endif
  93
  94 #define vdebug(fmt, args...) if(kprintf) kprintf(KERN_ERR fmt, ##args)
  95
  96 static unsigned char *g_get_patch[MAX_PATCH] = { NULL };
  97 static unsigned char *g_put_patch[MAX_PATCH] = { NULL };
  98
  99 static void notrace dmpatch_restore_code(unsigned char *opCode)
 100 {
 101     unsigned long align;
 102
 103     if (opCode)
 104     {
 105         align = (unsigned long)opCode / g_ko_param.pgsize * g_ko_param.pgsize;
 106         set_mem_rw(align, 1);
 107         *opCode = 0x80;
 108         set_mem_ro(align, 1);
 109     }
 110 }
 111
 112 static int notrace dmpatch_replace_code
 113 (
 114     int style,
 115     unsigned long addr,
 116     unsigned long size,
 117     int expect,
 118     const char *desc,
 119     unsigned char **patch
 120 )
 121 {
 122     int i = 0;
 123     int cnt = 0;
 124     unsigned long align;
 125     unsigned char *opCode = (unsigned char *)addr;
 126
 127     vdebug("patch for %s style[%d] 0x%lx %d\n", desc, style, addr, (int)size);
 128
 129     for (i = 0; i < (int)size - 4; i++)
 130     {
 131         if (style == 1)
 132         {
 133             if (CODE_MATCH1(opCode, i) && cnt < MAX_PATCH)
 134             {
 135                 patch[cnt] = opCode + i + PATCH_OP_POS1;
 136                 cnt++;
 137             }
 138         }
 139         else
 140         {
 141             if (CODE_MATCH2(opCode, i) && cnt < MAX_PATCH)
 142             {
 143                 patch[cnt] = opCode + i + PATCH_OP_POS2;
 144                 cnt++;
 145             }
 146         }
 147     }
 148
 149     if (cnt != expect || cnt >= MAX_PATCH)
 150     {
 151         vdebug("patch error: cnt=%d expect=%d\n", cnt, expect);
 152         return 1;
 153     }
 154
 155
 156     for (i = 0; i < cnt; i++)
 157     {
 158         opCode = patch[i];
 159         align = (unsigned long)opCode / g_ko_param.pgsize * g_ko_param.pgsize;
 160
 161         set_mem_rw(align, 1);
 162         *opCode = 0;
 163         set_mem_ro(align, 1);
 164     }
 165
 166     return 0;
 167 }
 168
 169 static int notrace dmpatch_init(void)
 170 {
 171     int r = 0;
 172     int rc = 0;
 173
 174     kprintf = (printk_pf)(g_ko_param.printk_addr);
 175
 176     vdebug("dmpatch_init start pagesize=%lu ...\n", g_ko_param.pgsize);
 177
 178     if (g_ko_param.struct_size != sizeof(ko_param))
 179     {
 180         vdebug("Invalid struct size %d %d\n", (int)g_ko_param.struct_size, (int)sizeof(ko_param));
 181         return -EINVAL;
 182     }
 183
 184     if (g_ko_param.sym_get_addr == 0 || g_ko_param.sym_put_addr == 0 ||
 185         g_ko_param.ro_addr == 0 || g_ko_param.rw_addr == 0)
 186     {
 187         return -EINVAL;
 188     }
 189
 190     set_mem_ro = (set_memory_attr_pf)(g_ko_param.ro_addr);
 191     set_mem_rw = (set_memory_attr_pf)(g_ko_param.rw_addr);
 192     reg_kprobe = (kprobe_reg_pf)g_ko_param.reg_kprobe_addr;
 193     unreg_kprobe = (kprobe_unreg_pf)g_ko_param.unreg_kprobe_addr;
 194
 195     r = dmpatch_replace_code(1, g_ko_param.sym_get_addr, g_ko_param.sym_get_size, 2, "dm_get_table_device", g_get_patch);
 196     if (r && g_ko_param.kv_major >= 5)
 197     {
 198         vdebug("new patch dm_get_table_device...\n");
 199         r = dmpatch_replace_code(2, g_ko_param.sym_get_addr, g_ko_param.sym_get_size, 1, "dm_get_table_device", g_get_patch);
 200     }
 201
 202     if (r)
 203     {
 204         rc = -EINVAL;
 205         goto out;
 206     }
 207     vdebug("patch dm_get_table_device success\n");
 208
 209     r = dmpatch_replace_code(1, g_ko_param.sym_put_addr, g_ko_param.sym_put_size, 1, "dm_put_table_device", g_put_patch);
 210     if (r)
 211     {
 212         rc = -EINVAL;
 213         goto out;
 214     }
 215     vdebug("patch dm_put_table_device success\n");
 216
 217     vdebug("#####################################\n");
 218     vdebug("######## dm patch success ###########\n");
 219     vdebug("#####################################\n");
 220
 221 out:
 222
 223         return rc;
 224 }
 225
 226 static void notrace dmpatch_exit(void)
 227 {
 228     int i = 0;
 229
 230     for (i = 0; i < MAX_PATCH; i++)
 231     {
 232         dmpatch_restore_code(g_get_patch[i]);
 233         dmpatch_restore_code(g_put_patch[i]);
 234     }
 235
 236     vdebug("dmpatch_exit success\n");
 237 }
 238
 239 module_init(dmpatch_init);
 240 module_exit(dmpatch_exit);
 241
 242
 243 MODULE_DESCRIPTION("dmpatch driver");
 244 MODULE_AUTHOR("longpanda <admin@ventoy.net>");
 245 MODULE_LICENSE("GPL");
 246