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 ibt;
  56     unsigned long padding[1];
  57 }ko_param;
  58
  59 #pragma pack()
  60
  61 static printk_pf kprintf = NULL;
  62 static set_memory_attr_pf set_mem_ro = NULL;
  63 static set_memory_attr_pf set_mem_rw = NULL;
  64 static kprobe_reg_pf reg_kprobe = NULL;
  65 static kprobe_unreg_pf unreg_kprobe = NULL;
  66
  67 static volatile ko_param g_ko_param =
  68 {
  69     { magic_sig },
  70     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
  71 };
  72
  73 #if defined(CONFIG_X86_64)
  74 #define PATCH_OP_POS1    3
  75 #define CODE_MATCH1(code, i) \
  76     (code[i] == 0x40 && code[i + 1] == 0x80 && code[i + 2] == 0xce && code[i + 3] == 0x80)
  77
  78 #define PATCH_OP_POS2    1
  79 #define CODE_MATCH2(code, i) \
  80     (code[i] == 0x0C && code[i + 1] == 0x80 && code[i + 2] == 0x89 && code[i + 3] == 0xC6)
  81
  82 #define PATCH_OP_POS3    4
  83 #define CODE_MATCH3(code, i) \
  84     (code[i] == 0x44 && code[i + 1] == 0x89 && code[i + 2] == 0xe8 && code[i + 3] == 0x0c && code[i + 4] == 0x80)
  85
  86
  87
  88
  89
  90 #elif defined(CONFIG_X86_32)
  91 #define PATCH_OP_POS1    2
  92 #define CODE_MATCH1(code, i) \
  93     (code[i] == 0x80 && code[i + 1] == 0xca && code[i + 2] == 0x80 && code[i + 3] == 0xe8)
  94
  95 #define PATCH_OP_POS2    PATCH_OP_POS1
  96 #define CODE_MATCH2      CODE_MATCH1
  97 #define PATCH_OP_POS3    PATCH_OP_POS1
  98 #define CODE_MATCH3      CODE_MATCH1
  99
 100
 101 #else
 102 #error "unsupported arch"
 103 #endif
 104
 105 #ifdef VTOY_IBT
 106 #ifdef CONFIG_X86_64
 107 /* Using 64-bit values saves one instruction clearing the high half of low */
 108 #define DECLARE_ARGS(val, low, high)    unsigned long low, high
 109 #define EAX_EDX_VAL(val, low, high)     ((low) | (high) << 32)
 110 #define EAX_EDX_RET(val, low, high)     "=a" (low), "=d" (high)
 111 #else
 112 #define DECLARE_ARGS(val, low, high)    unsigned long long val
 113 #define EAX_EDX_VAL(val, low, high)     (val)
 114 #define EAX_EDX_RET(val, low, high)     "=A" (val)
 115 #endif
 116
 117 #define EX_TYPE_WRMSR                    8
 118 #define EX_TYPE_RDMSR                    9
 119 #define MSR_IA32_S_CET                  0x000006a2 /* kernel mode cet */
 120 #define CET_ENDBR_EN                    (1ULL << 2)
 121
 122 /* Exception table entry */
 123 #ifdef __ASSEMBLY__
 124
 125 #define _ASM_EXTABLE_TYPE(from, to, type)                       \
 126         .pushsection "__ex_table","a" ;                         \
 127         .balign 4 ;                                             \
 128         .long (from) - . ;                                      \
 129         .long (to) - . ;                                        \
 130         .long type ;                                            \
 131         .popsection
 132
 133 #else /* ! __ASSEMBLY__ */
 134
 135 #define _ASM_EXTABLE_TYPE(from, to, type)                       \
 136         " .pushsection \"__ex_table\",\"a\"\n"                  \
 137         " .balign 4\n"                                          \
 138         " .long (" #from ") - .\n"                              \
 139         " .long (" #to ") - .\n"                                \
 140         " .long " __stringify(type) " \n"                       \
 141         " .popsection\n"
 142
 143 #endif /* __ASSEMBLY__ */
 144 #endif /* VTOY_IBT */
 145
 146
 147
 148
 149
 150
 151 #define vdebug(fmt, args...) if(kprintf) kprintf(KERN_ERR fmt, ##args)
 152
 153 static unsigned char *g_get_patch[MAX_PATCH] = { NULL };
 154 static unsigned char *g_put_patch[MAX_PATCH] = { NULL };
 155
 156 static void notrace dmpatch_restore_code(unsigned char *opCode)
 157 {
 158     unsigned long align;
 159
 160     if (opCode)
 161     {
 162         align = (unsigned long)opCode / g_ko_param.pgsize * g_ko_param.pgsize;
 163         set_mem_rw(align, 1);
 164         *opCode = 0x80;
 165         set_mem_ro(align, 1);
 166     }
 167 }
 168
 169 static int notrace dmpatch_replace_code
 170 (
 171     int style,
 172     unsigned long addr,
 173     unsigned long size,
 174     int expect,
 175     const char *desc,
 176     unsigned char **patch
 177 )
 178 {
 179     int i = 0;
 180     int cnt = 0;
 181     unsigned long align;
 182     unsigned char *opCode = (unsigned char *)addr;
 183
 184     vdebug("patch for %s style[%d] 0x%lx %d\n", desc, style, addr, (int)size);
 185
 186     for (i = 0; i < (int)size - 8; i++)
 187     {
 188         if (style == 1)
 189         {
 190             if (CODE_MATCH1(opCode, i) && cnt < MAX_PATCH)
 191             {
 192                 patch[cnt] = opCode + i + PATCH_OP_POS1;
 193                 cnt++;
 194             }
 195         }
 196         else if (style == 2)
 197         {
 198             if (CODE_MATCH2(opCode, i) && cnt < MAX_PATCH)
 199             {
 200                 patch[cnt] = opCode + i + PATCH_OP_POS2;
 201                 cnt++;
 202             }
 203         }
 204         else if (style == 3)
 205         {
 206             if (CODE_MATCH3(opCode, i) && cnt < MAX_PATCH)
 207             {
 208                 patch[cnt] = opCode + i + PATCH_OP_POS3;
 209                 cnt++;
 210             }
 211         }
 212     }
 213
 214
 215
 216
 217
 218     if (cnt != expect || cnt >= MAX_PATCH)
 219     {
 220         vdebug("patch error: cnt=%d expect=%d\n", cnt, expect);
 221         return 1;
 222     }
 223
 224
 225     for (i = 0; i < cnt; i++)
 226     {
 227         opCode = patch[i];
 228         align = (unsigned long)opCode / g_ko_param.pgsize * g_ko_param.pgsize;
 229
 230         set_mem_rw(align, 1);
 231         *opCode = 0;
 232         set_mem_ro(align, 1);
 233     }
 234
 235     return 0;
 236 }
 237
 238 #ifdef VTOY_IBT
 239 static __always_inline unsigned long long dmpatch_rdmsr(unsigned int msr)
 240 {
 241         DECLARE_ARGS(val, low, high);
 242
 243         asm volatile("1: rdmsr\n"
 244                      "2:\n"
 245                      _ASM_EXTABLE_TYPE(1b, 2b, EX_TYPE_RDMSR)
 246                      : EAX_EDX_RET(val, low, high) : "c" (msr));
 247
 248         return EAX_EDX_VAL(val, low, high);
 249 }
 250
 251 static __always_inline void dmpatch_wrmsr(unsigned int msr, u32 low, u32 high)
 252 {
 253         asm volatile("1: wrmsr\n"
 254                      "2:\n"
 255                      _ASM_EXTABLE_TYPE(1b, 2b, EX_TYPE_WRMSR)
 256                      : : "c" (msr), "a"(low), "d" (high) : "memory");
 257 }
 258
 259 static u64 dmpatch_ibt_save(void)
 260 {
 261     u64 msr = 0;
 262     u64 val = 0;
 263
 264     msr = dmpatch_rdmsr(MSR_IA32_S_CET);
 265     val = msr & ~CET_ENDBR_EN;
 266     dmpatch_wrmsr(MSR_IA32_S_CET, (u32)(val & 0xffffffffULL), (u32)(val >> 32));
 267
 268     return msr;
 269 }
 270
 271 static void dmpatch_ibt_restore(u64 save)
 272 {
 273         u64 msr;
 274
 275     msr = dmpatch_rdmsr(MSR_IA32_S_CET);
 276
 277         msr &= ~CET_ENDBR_EN;
 278         msr |= (save & CET_ENDBR_EN);
 279
 280     dmpatch_wrmsr(MSR_IA32_S_CET, (u32)(msr & 0xffffffffULL), (u32)(msr >> 32));
 281 }
 282 #else
 283 static u64 dmpatch_ibt_save(void) { return 0; }
 284 static void dmpatch_ibt_restore(u64 save) { (void)save; }
 285 #endif
 286
 287 static int notrace dmpatch_init(void)
 288 {
 289     int r = 0;
 290     int rc = 0;
 291     u64 msr = 0;
 292
 293     if (g_ko_param.ibt == 0x8888)
 294     {
 295         msr = dmpatch_ibt_save();
 296     }
 297
 298     kprintf = (printk_pf)(g_ko_param.printk_addr);
 299
 300     vdebug("dmpatch_init start pagesize=%lu ...\n", g_ko_param.pgsize);
 301
 302     if (g_ko_param.struct_size != sizeof(ko_param))
 303     {
 304         vdebug("Invalid struct size %d %d\n", (int)g_ko_param.struct_size, (int)sizeof(ko_param));
 305         return -EINVAL;
 306     }
 307
 308     if (g_ko_param.sym_get_addr == 0 || g_ko_param.sym_put_addr == 0 ||
 309         g_ko_param.ro_addr == 0 || g_ko_param.rw_addr == 0)
 310     {
 311         return -EINVAL;
 312     }
 313
 314     set_mem_ro = (set_memory_attr_pf)(g_ko_param.ro_addr);
 315     set_mem_rw = (set_memory_attr_pf)(g_ko_param.rw_addr);
 316     reg_kprobe = (kprobe_reg_pf)g_ko_param.reg_kprobe_addr;
 317     unreg_kprobe = (kprobe_unreg_pf)g_ko_param.unreg_kprobe_addr;
 318
 319     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);
 320     if (r && g_ko_param.kv_major >= 5)
 321     {
 322         vdebug("new2 patch dm_get_table_device...\n");
 323         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);
 324     }
 325
 326     if (r && g_ko_param.kv_major >= 5)
 327     {
 328         vdebug("new3 patch dm_get_table_device...\n");
 329         r = dmpatch_replace_code(3, g_ko_param.sym_get_addr, g_ko_param.sym_get_size, 1, "dm_get_table_device", g_get_patch);
 330     }
 331
 332
 333     if (r)
 334     {
 335         rc = -EINVAL;
 336         goto out;
 337     }
 338     vdebug("patch dm_get_table_device success\n");
 339
 340     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);
 341     if (r)
 342     {
 343         rc = -EINVAL;
 344         goto out;
 345     }
 346     vdebug("patch dm_put_table_device success\n");
 347
 348     vdebug("#####################################\n");
 349     vdebug("######## dm patch success ###########\n");
 350     vdebug("#####################################\n");
 351
 352     if (g_ko_param.ibt == 0x8888)
 353     {
 354         dmpatch_ibt_restore(msr);
 355     }
 356
 357 out:
 358
 359         return rc;
 360 }
 361
 362 static void notrace dmpatch_exit(void)
 363 {
 364     int i = 0;
 365     u64 msr;
 366
 367     if (g_ko_param.ibt == 0x8888)
 368     {
 369         msr = dmpatch_ibt_save();
 370     }
 371
 372     for (i = 0; i < MAX_PATCH; i++)
 373     {
 374         dmpatch_restore_code(g_get_patch[i]);
 375         dmpatch_restore_code(g_put_patch[i]);
 376     }
 377
 378     vdebug("dmpatch_exit success\n");
 379
 380     if (g_ko_param.ibt == 0x8888)
 381     {
 382         dmpatch_ibt_restore(msr);
 383     }
 384 }
 385
 386 module_init(dmpatch_init);
 387 module_exit(dmpatch_exit);
 388
 389
 390 MODULE_DESCRIPTION("dmpatch driver");
 391 MODULE_AUTHOR("longpanda <admin@ventoy.net>");
 392 MODULE_LICENSE("GPL");
 393