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 kv_minor;
  57     unsigned long blkdev_get_addr;
  58     unsigned long blkdev_put_addr;
  59     unsigned long padding[1];
  60 }ko_param;
  61
  62 #pragma pack()
  63
  64 static printk_pf kprintf = NULL;
  65 static set_memory_attr_pf set_mem_ro = NULL;
  66 static set_memory_attr_pf set_mem_rw = NULL;
  67 static kprobe_reg_pf reg_kprobe = NULL;
  68 static kprobe_unreg_pf unreg_kprobe = NULL;
  69
  70 static volatile ko_param g_ko_param =
  71 {
  72     { magic_sig },
  73     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
  74 };
  75
  76 #if defined(CONFIG_X86_64)
  77 #define PATCH_OP_POS1    3
  78 #define CODE_MATCH1(code, i) \
  79     (code[i] == 0x40 && code[i + 1] == 0x80 && code[i + 2] == 0xce && code[i + 3] == 0x80)
  80
  81 #define PATCH_OP_POS2    1
  82 #define CODE_MATCH2(code, i) \
  83     (code[i] == 0x0C && code[i + 1] == 0x80 && code[i + 2] == 0x89 && code[i + 3] == 0xC6)
  84
  85 #define PATCH_OP_POS3    4
  86 #define CODE_MATCH3(code, i) \
  87     (code[i] == 0x44 && code[i + 1] == 0x89 && code[i + 2] == 0xe8 && code[i + 3] == 0x0c && code[i + 4] == 0x80)
  88
  89
  90
  91
  92
  93 #elif defined(CONFIG_X86_32)
  94 #define PATCH_OP_POS1    2
  95 #define CODE_MATCH1(code, i) \
  96     (code[i] == 0x80 && code[i + 1] == 0xca && code[i + 2] == 0x80 && code[i + 3] == 0xe8)
  97
  98 #define PATCH_OP_POS2    PATCH_OP_POS1
  99 #define CODE_MATCH2      CODE_MATCH1
 100 #define PATCH_OP_POS3    PATCH_OP_POS1
 101 #define CODE_MATCH3      CODE_MATCH1
 102
 103
 104 #else
 105 #error "unsupported arch"
 106 #endif
 107
 108 #ifdef VTOY_IBT
 109 #ifdef CONFIG_X86_64
 110 /* Using 64-bit values saves one instruction clearing the high half of low */
 111 #define DECLARE_ARGS(val, low, high)    unsigned long low, high
 112 #define EAX_EDX_VAL(val, low, high)     ((low) | (high) << 32)
 113 #define EAX_EDX_RET(val, low, high)     "=a" (low), "=d" (high)
 114 #else
 115 #define DECLARE_ARGS(val, low, high)    unsigned long long val
 116 #define EAX_EDX_VAL(val, low, high)     (val)
 117 #define EAX_EDX_RET(val, low, high)     "=A" (val)
 118 #endif
 119
 120 #define EX_TYPE_WRMSR                    8
 121 #define EX_TYPE_RDMSR                    9
 122 #define MSR_IA32_S_CET                  0x000006a2 /* kernel mode cet */
 123 #define CET_ENDBR_EN                    (1ULL << 2)
 124
 125 /* Exception table entry */
 126 #ifdef __ASSEMBLY__
 127
 128 #define _ASM_EXTABLE_TYPE(from, to, type)                       \
 129         .pushsection "__ex_table","a" ;                         \
 130         .balign 4 ;                                             \
 131         .long (from) - . ;                                      \
 132         .long (to) - . ;                                        \
 133         .long type ;                                            \
 134         .popsection
 135
 136 #else /* ! __ASSEMBLY__ */
 137
 138 #define _ASM_EXTABLE_TYPE(from, to, type)                       \
 139         " .pushsection \"__ex_table\",\"a\"\n"                  \
 140         " .balign 4\n"                                          \
 141         " .long (" #from ") - .\n"                              \
 142         " .long (" #to ") - .\n"                                \
 143         " .long " __stringify(type) " \n"                       \
 144         " .popsection\n"
 145
 146 #endif /* __ASSEMBLY__ */
 147 #endif /* VTOY_IBT */
 148
 149
 150
 151
 152
 153
 154 #define vdebug(fmt, args...) if(kprintf) kprintf(KERN_ERR fmt, ##args)
 155
 156 static unsigned int g_claim_ptr = 0;
 157 static unsigned char *g_get_patch[MAX_PATCH] = { NULL };
 158 static unsigned char *g_put_patch[MAX_PATCH] = { NULL };
 159
 160 static void notrace dmpatch_restore_code(int bytes, unsigned char *opCode, unsigned int code)
 161 {
 162     unsigned long align;
 163
 164     if (opCode)
 165     {
 166         align = (unsigned long)opCode / g_ko_param.pgsize * g_ko_param.pgsize;
 167         set_mem_rw(align, 1);
 168         if (bytes == 1)
 169         {
 170             *opCode = (unsigned char)code;
 171         }
 172         else
 173         {
 174             *(unsigned int *)opCode = code;
 175         }
 176         set_mem_ro(align, 1);
 177     }
 178 }
 179
 180 static int notrace dmpatch_replace_code
 181 (
 182     int style,
 183     unsigned long addr,
 184     unsigned long size,
 185     int expect,
 186     const char *desc,
 187     unsigned char **patch
 188 )
 189 {
 190     int i = 0;
 191     int cnt = 0;
 192     unsigned long align;
 193     unsigned char *opCode = (unsigned char *)addr;
 194
 195     vdebug("patch for %s style[%d] 0x%lx %d\n", desc, style, addr, (int)size);
 196
 197     for (i = 0; i < (int)size - 8; i++)
 198     {
 199         if (style == 1)
 200         {
 201             if (CODE_MATCH1(opCode, i) && cnt < MAX_PATCH)
 202             {
 203                 patch[cnt] = opCode + i + PATCH_OP_POS1;
 204                 cnt++;
 205             }
 206         }
 207         else if (style == 2)
 208         {
 209             if (CODE_MATCH2(opCode, i) && cnt < MAX_PATCH)
 210             {
 211                 patch[cnt] = opCode + i + PATCH_OP_POS2;
 212                 cnt++;
 213             }
 214         }
 215         else if (style == 3)
 216         {
 217             if (CODE_MATCH3(opCode, i) && cnt < MAX_PATCH)
 218             {
 219                 patch[cnt] = opCode + i + PATCH_OP_POS3;
 220                 cnt++;
 221             }
 222         }
 223     }
 224
 225
 226
 227
 228
 229     if (cnt != expect || cnt >= MAX_PATCH)
 230     {
 231         vdebug("patch error: cnt=%d expect=%d\n", cnt, expect);
 232         return 1;
 233     }
 234
 235
 236     for (i = 0; i < cnt; i++)
 237     {
 238         opCode = patch[i];
 239         align = (unsigned long)opCode / g_ko_param.pgsize * g_ko_param.pgsize;
 240
 241         set_mem_rw(align, 1);
 242         *opCode = 0;
 243         set_mem_ro(align, 1);
 244     }
 245
 246     return 0;
 247 }
 248
 249 static unsigned long dmpatch_find_call_offset(unsigned long addr, unsigned long size, unsigned long func)
 250 {
 251     unsigned long i = 0;
 252     unsigned long dest;
 253     unsigned char *opCode = NULL;
 254     unsigned char aucOffset[8] = { 0, 0, 0, 0, 0xFF, 0xFF, 0xFF, 0xFF };
 255
 256     opCode = (unsigned char *)addr;
 257
 258     for (i = 0; i + 4 < size; i++)
 259     {
 260         if (opCode[i] == 0xE8)
 261         {
 262             aucOffset[0] = opCode[i + 1];
 263             aucOffset[1] = opCode[i + 2];
 264             aucOffset[2] = opCode[i + 3];
 265             aucOffset[3] = opCode[i + 4];
 266
 267             dest = addr + i + 5 + *(unsigned long *)aucOffset;
 268             if (dest == func)
 269             {
 270                 return i;
 271             }
 272         }
 273     }
 274
 275     return 0;
 276 }
 277
 278 static unsigned int dmpatch_patch_claim_ptr(void)
 279 {
 280     unsigned long i = 0;
 281     unsigned long t = 0;
 282     unsigned long offset1;
 283     unsigned long offset2;
 284     unsigned long align;
 285     unsigned char *opCode = NULL;
 286
 287     vdebug("Get addr: 0x%lx %lu 0x%lx\n", g_ko_param.sym_get_addr, g_ko_param.sym_get_size, g_ko_param.blkdev_get_addr);
 288     vdebug("Put addr: 0x%lx %lu 0x%lx\n", g_ko_param.sym_put_addr, g_ko_param.sym_put_size, g_ko_param.blkdev_put_addr);
 289
 290     opCode = (unsigned char *)g_ko_param.sym_get_addr;
 291     for (i = 0; i < 4; i++)
 292     {
 293         vdebug("%02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X\n",
 294             opCode[i + 0], opCode[i + 1], opCode[i + 2], opCode[i + 3],
 295             opCode[i + 4], opCode[i + 5], opCode[i + 6], opCode[i + 7],
 296             opCode[i + 8], opCode[i + 9], opCode[i + 10], opCode[i + 11],
 297             opCode[i + 12], opCode[i + 13], opCode[i + 14], opCode[i + 15]);
 298     }
 299
 300     offset1 = dmpatch_find_call_offset(g_ko_param.sym_get_addr, g_ko_param.sym_get_size, g_ko_param.blkdev_get_addr);
 301     offset2 = dmpatch_find_call_offset(g_ko_param.sym_put_addr, g_ko_param.sym_put_size, g_ko_param.blkdev_put_addr);
 302     if (offset1 == 0 || offset2 == 0)
 303     {
 304         vdebug("call blkdev_get or blkdev_put Not found, %lu %lu\n", offset1, offset2);
 305         return 1;
 306     }
 307     vdebug("call addr1:0x%lx  call addr2:0x%lx\n",
 308         g_ko_param.sym_get_addr + offset1,
 309         g_ko_param.sym_put_addr + offset2);
 310
 311     opCode = (unsigned char *)g_ko_param.sym_get_addr;
 312     for (i = offset1 - 1, t = 0; (i > 0) && (t < 24); i--, t++)
 313     {
 314         /* rdx */
 315         if (opCode[i] == 0x48 && opCode[i + 1] == 0xc7 && opCode[i + 2] == 0xc2)
 316         {
 317             g_claim_ptr = *(unsigned int *)(opCode + i + 3);
 318             g_get_patch[0] = opCode + i + 3;
 319             vdebug("claim_ptr(%08X) found at get addr 0x%lx\n", g_claim_ptr, g_ko_param.sym_get_addr + i + 3);
 320             break;
 321         }
 322     }
 323
 324     if (g_claim_ptr == 0)
 325     {
 326         vdebug("Claim_ptr not found in get\n");
 327         return 1;
 328     }
 329
 330     opCode = (unsigned char *)g_ko_param.sym_put_addr;
 331     for (i = offset2 - 1, t = 0; (i > 0) && (t < 24); i--, t++)
 332     {
 333         /* rsi */
 334         if (opCode[i] == 0x48 && opCode[i + 1] == 0xc7 && opCode[i + 2] == 0xc6)
 335         {
 336             if (*(unsigned int *)(opCode + i + 3) == g_claim_ptr)
 337             {
 338                 vdebug("claim_ptr found at put addr 0x%lx\n", g_ko_param.sym_put_addr + i + 3);
 339                 g_put_patch[0] = opCode + i + 3;
 340                 break;
 341             }
 342         }
 343     }
 344
 345     if (g_put_patch[0] == 0)
 346     {
 347         vdebug("Claim_ptr not found in put\n");
 348         return 1;
 349     }
 350
 351     align = (unsigned long)g_get_patch[0] / g_ko_param.pgsize * g_ko_param.pgsize;
 352     set_mem_rw(align, 1);
 353     *(unsigned int *)(g_get_patch[0]) = 0;
 354     set_mem_ro(align, 1);
 355
 356     align = (unsigned long)g_put_patch[0] / g_ko_param.pgsize * g_ko_param.pgsize;
 357     set_mem_rw(align, 1);
 358     *(unsigned int *)(g_put_patch[0]) = 0;
 359     set_mem_ro(align, 1);
 360
 361     return 0;
 362 }
 363
 364 #ifdef VTOY_IBT
 365 static __always_inline unsigned long long dmpatch_rdmsr(unsigned int msr)
 366 {
 367         DECLARE_ARGS(val, low, high);
 368
 369         asm volatile("1: rdmsr\n"
 370                      "2:\n"
 371                      _ASM_EXTABLE_TYPE(1b, 2b, EX_TYPE_RDMSR)
 372                      : EAX_EDX_RET(val, low, high) : "c" (msr));
 373
 374         return EAX_EDX_VAL(val, low, high);
 375 }
 376
 377 static __always_inline void dmpatch_wrmsr(unsigned int msr, u32 low, u32 high)
 378 {
 379         asm volatile("1: wrmsr\n"
 380                      "2:\n"
 381                      _ASM_EXTABLE_TYPE(1b, 2b, EX_TYPE_WRMSR)
 382                      : : "c" (msr), "a"(low), "d" (high) : "memory");
 383 }
 384
 385 static u64 dmpatch_ibt_save(void)
 386 {
 387     u64 msr = 0;
 388     u64 val = 0;
 389
 390     msr = dmpatch_rdmsr(MSR_IA32_S_CET);
 391     val = msr & ~CET_ENDBR_EN;
 392     dmpatch_wrmsr(MSR_IA32_S_CET, (u32)(val & 0xffffffffULL), (u32)(val >> 32));
 393
 394     return msr;
 395 }
 396
 397 static void dmpatch_ibt_restore(u64 save)
 398 {
 399         u64 msr;
 400
 401     msr = dmpatch_rdmsr(MSR_IA32_S_CET);
 402
 403         msr &= ~CET_ENDBR_EN;
 404         msr |= (save & CET_ENDBR_EN);
 405
 406     dmpatch_wrmsr(MSR_IA32_S_CET, (u32)(msr & 0xffffffffULL), (u32)(msr >> 32));
 407 }
 408 #else
 409 static u64 dmpatch_ibt_save(void) { return 0; }
 410 static void dmpatch_ibt_restore(u64 save) { (void)save; }
 411 #endif
 412
 413 static int notrace dmpatch_init(void)
 414 {
 415     int r = 0;
 416     int rc = 0;
 417     u64 msr = 0;
 418
 419     if (g_ko_param.ibt == 0x8888)
 420     {
 421         msr = dmpatch_ibt_save();
 422     }
 423
 424     kprintf = (printk_pf)(g_ko_param.printk_addr);
 425
 426     vdebug("dmpatch_init start pagesize=%lu kernel=%lu.%lu ...\n",
 427         g_ko_param.pgsize, g_ko_param.kv_major, g_ko_param.kv_minor);
 428
 429     if (g_ko_param.struct_size != sizeof(ko_param))
 430     {
 431         vdebug("Invalid struct size %d %d\n", (int)g_ko_param.struct_size, (int)sizeof(ko_param));
 432         return -EINVAL;
 433     }
 434
 435     if (g_ko_param.sym_get_addr == 0 || g_ko_param.sym_put_addr == 0 ||
 436         g_ko_param.ro_addr == 0 || g_ko_param.rw_addr == 0)
 437     {
 438         return -EINVAL;
 439     }
 440
 441     set_mem_ro = (set_memory_attr_pf)(g_ko_param.ro_addr);
 442     set_mem_rw = (set_memory_attr_pf)(g_ko_param.rw_addr);
 443     reg_kprobe = (kprobe_reg_pf)g_ko_param.reg_kprobe_addr;
 444     unreg_kprobe = (kprobe_unreg_pf)g_ko_param.unreg_kprobe_addr;
 445
 446     if (g_ko_param.kv_major > 6 || (g_ko_param.kv_major == 6 && g_ko_param.kv_minor >= 5))
 447     {
 448         vdebug("new interface patch dm_get_table_device...\n");
 449         r = dmpatch_patch_claim_ptr();
 450     }
 451     else
 452     {
 453         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);
 454         if (r && g_ko_param.kv_major >= 5)
 455         {
 456             vdebug("new2 patch dm_get_table_device...\n");
 457             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);
 458         }
 459
 460         if (r && g_ko_param.kv_major >= 5)
 461         {
 462             vdebug("new3 patch dm_get_table_device...\n");
 463             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);
 464         }
 465     }
 466
 467     if (r)
 468     {
 469         rc = -EINVAL;
 470         goto out;
 471     }
 472     vdebug("patch dm_get_table_device success\n");
 473
 474     if (g_ko_param.kv_major >= 6 && g_ko_param.kv_minor >= 5)
 475     {
 476         r = 0;
 477     }
 478     else
 479     {
 480         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);
 481     }
 482     if (r)
 483     {
 484         rc = -EINVAL;
 485         goto out;
 486     }
 487     vdebug("patch dm_put_table_device success\n");
 488
 489     vdebug("#####################################\n");
 490     vdebug("######## dm patch success ###########\n");
 491     vdebug("#####################################\n");
 492
 493     if (g_ko_param.ibt == 0x8888)
 494     {
 495         dmpatch_ibt_restore(msr);
 496     }
 497
 498 out:
 499
 500         return rc;
 501 }
 502
 503 static void notrace dmpatch_exit(void)
 504 {
 505     int i = 0;
 506     u64 msr;
 507
 508     if (g_ko_param.ibt == 0x8888)
 509     {
 510         msr = dmpatch_ibt_save();
 511     }
 512
 513     if (g_claim_ptr)
 514     {
 515         dmpatch_restore_code(4, g_get_patch[0], g_claim_ptr);
 516         dmpatch_restore_code(4, g_put_patch[0], g_claim_ptr);
 517     }
 518     else
 519     {
 520         for (i = 0; i < MAX_PATCH; i++)
 521         {
 522             dmpatch_restore_code(1, g_get_patch[i], 0x80);
 523             dmpatch_restore_code(1, g_put_patch[i], 0x80);
 524         }
 525     }
 526
 527     vdebug("dmpatch_exit success\n");
 528
 529     if (g_ko_param.ibt == 0x8888)
 530     {
 531         dmpatch_ibt_restore(msr);
 532     }
 533 }
 534
 535 module_init(dmpatch_init);
 536 module_exit(dmpatch_exit);
 537
 538
 539 MODULE_DESCRIPTION("dmpatch driver");
 540 MODULE_AUTHOR("longpanda <admin@ventoy.net>");
 541 MODULE_LICENSE("GPL");
 542