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