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