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