|
| 1 | +#include <linux/module.h> |
| 2 | +#include <linux/kernel.h> |
| 3 | +#include <linux/slab.h> |
| 4 | +#include <linux/kallsyms.h> |
| 5 | + |
| 6 | +#define DEBUG(fmt, args...) printk(KERN_INFO KBUILD_MODNAME ": "fmt"\n", ##args) |
| 7 | + |
| 8 | +#define ASSERT(condition) do { \ |
| 9 | + if (!(condition)) { \ |
| 10 | + WARN(true, "Assertion \""#condition"\" failed"); \ |
| 11 | + return -EINVAL; \ |
| 12 | + } \ |
| 13 | +} while(0) |
| 14 | + |
| 15 | +typedef unsigned long word; |
| 16 | + |
| 17 | +#define FAR_JMP 0xe51ff004 // ldr pc, [pc, #-4] |
| 18 | + |
| 19 | +enum { |
| 20 | + TI_BRANCH_HOOK_1 = 0, // FAR_JMP |
| 21 | + TI_BRANCH_HOOK_2, // addr of hook |
| 22 | + TI_ORIG_INSN_1, // overwritten first word of f |
| 23 | + TI_ORIG_INSN_2, // overwritten second word of f |
| 24 | + TI_CALL_ORIG_1, // FAR_JMP |
| 25 | + TI_CALL_ORIG_2, // addr of f+8 |
| 26 | + TI_CALL_MAX, |
| 27 | +} trampoline_instruction_t; |
| 28 | + |
| 29 | +static void hook_fun(word *f, void *hook, word **orig_fun) { |
| 30 | + word *trampoline = kcalloc(TI_CALL_MAX, sizeof(word), GFP_KERNEL); |
| 31 | + |
| 32 | + trampoline[TI_BRANCH_HOOK_1] = FAR_JMP; |
| 33 | + trampoline[TI_BRANCH_HOOK_2] = (word)hook; |
| 34 | + trampoline[TI_ORIG_INSN_1] = f[0]; |
| 35 | + trampoline[TI_ORIG_INSN_2] = f[1]; |
| 36 | + trampoline[TI_CALL_ORIG_1] = FAR_JMP; |
| 37 | + trampoline[TI_CALL_ORIG_2] = (word)&f[2]; |
| 38 | + |
| 39 | + *orig_fun = &trampoline[TI_ORIG_INSN_1]; |
| 40 | + |
| 41 | + f[0] = FAR_JMP; |
| 42 | + f[1] = (word)trampoline; |
| 43 | + |
| 44 | + DEBUG("hook_fun(f=%pS (%px), hook=%pS (%px)), trampoline is at %px", f, f, hook, hook, trampoline); |
| 45 | + |
| 46 | + for (int i = 0; i < TI_CALL_MAX; i++) |
| 47 | + DEBUG(" trampoline[%d] = %lx", i, trampoline[i]); |
| 48 | + for (int i = 0; i < 2; i++) |
| 49 | + DEBUG(" set f[%d]: %lx", i, f[i]); |
| 50 | +} |
| 51 | + |
| 52 | +static int restore_fun(word **orig_fun) { |
| 53 | + word *trampoline = *orig_fun - TI_ORIG_INSN_1; |
| 54 | + ASSERT(trampoline[TI_BRANCH_HOOK_1] == FAR_JMP); |
| 55 | + ASSERT(trampoline[TI_CALL_ORIG_1] == FAR_JMP); |
| 56 | + |
| 57 | + word *f = ((word **)trampoline)[TI_CALL_ORIG_2] - 2; |
| 58 | + DEBUG("restore_fun(orig_fun=%pS (%px)), trampoline is at %px, f=%pS (%px)", orig_fun, orig_fun, trampoline, f, f); |
| 59 | + |
| 60 | + ASSERT(f[0] == FAR_JMP); |
| 61 | + ASSERT(f[1] == (word)trampoline); |
| 62 | + |
| 63 | + f[0] = trampoline[TI_ORIG_INSN_1]; |
| 64 | + f[1] = trampoline[TI_ORIG_INSN_2]; |
| 65 | + |
| 66 | + for (int i = 0; i < 2; i++) |
| 67 | + DEBUG(" reset f[%d]: %lx", i, f[i]); |
| 68 | + |
| 69 | + kfree(trampoline); |
| 70 | + *orig_fun = NULL; |
| 71 | + |
| 72 | + return 0; |
| 73 | +} |
| 74 | + |
| 75 | +int (*orig_MTAUD_SetChannelVolume)(int, int, int) = NULL; |
| 76 | + |
| 77 | +int my_MTAUD_SetChannelVolume(int decoder, int channel, int value) { |
| 78 | + DEBUG("my_MTAUD_SetChannelVolume(%d, %d, %d)", decoder, channel, value); |
| 79 | + |
| 80 | + if (channel == 0 || channel == 21) { |
| 81 | + int ret = orig_MTAUD_SetChannelVolume(0, 6, value); |
| 82 | + DEBUG(" also set channel 6, rc=%d", ret); |
| 83 | + } else if (channel == 1 || channel == 22) { |
| 84 | + int ret = orig_MTAUD_SetChannelVolume(0, 7, value); |
| 85 | + DEBUG(" also set channel 7, rc=%d", ret); |
| 86 | + } |
| 87 | + |
| 88 | + return orig_MTAUD_SetChannelVolume(decoder, channel, value); |
| 89 | +} |
| 90 | + |
| 91 | +int init_module(void) { |
| 92 | + word *p = (word *)kallsyms_lookup_name("_MTAUD_SetChannelVolume"); |
| 93 | + if (!p) { |
| 94 | + printk(KERN_ERR "can't find symbol\n"); |
| 95 | + return -ENOENT; |
| 96 | + } |
| 97 | + |
| 98 | + hook_fun((word *)p, my_MTAUD_SetChannelVolume, (word **)&orig_MTAUD_SetChannelVolume); |
| 99 | + DEBUG("loaded."); |
| 100 | + return 0; |
| 101 | +} |
| 102 | + |
| 103 | +void cleanup_module(void) { |
| 104 | + restore_fun((word **)&orig_MTAUD_SetChannelVolume); |
| 105 | + DEBUG("unloaded."); |
| 106 | +} |
| 107 | + |
| 108 | +MODULE_LICENSE("GPL"); |
0 commit comments