1 // SPDX-License-Identifier: GPL-2.0 2 // Copyright (C) 2018 Hangzhou C-SKY Microsystems co.,ltd. 3 4 #include <linux/ftrace.h> 5 #include <linux/uaccess.h> 6 #include <asm/cacheflush.h> 7 8 #ifdef CONFIG_DYNAMIC_FTRACE 9 10 #define NOP 0x4000 11 #define NOP32_HI 0xc400 12 #define NOP32_LO 0x4820 13 #define PUSH_LR 0x14d0 14 #define MOVIH_LINK 0xea3a 15 #define ORI_LINK 0xef5a 16 #define JSR_LINK 0xe8fa 17 #define BSR_LINK 0xe000 18 19 /* 20 * Gcc-csky with -pg will insert stub in function prologue: 21 * push lr 22 * jbsr _mcount 23 * nop32 24 * nop32 25 * 26 * If the (callee - current_pc) is less then 64MB, we'll use bsr: 27 * push lr 28 * bsr _mcount 29 * nop32 30 * nop32 31 * else we'll use (movih + ori + jsr): 32 * push lr 33 * movih r26, ... 34 * ori r26, ... 35 * jsr r26 36 * 37 * (r26 is our reserved link-reg) 38 * 39 */ 40 static inline void make_jbsr(unsigned long callee, unsigned long pc, 41 uint16_t *call, bool nolr) 42 { 43 long offset; 44 45 call[0] = nolr ? NOP : PUSH_LR; 46 47 offset = (long) callee - (long) pc; 48 49 if (unlikely(offset < -67108864 || offset > 67108864)) { 50 call[1] = MOVIH_LINK; 51 call[2] = callee >> 16; 52 call[3] = ORI_LINK; 53 call[4] = callee & 0xffff; 54 call[5] = JSR_LINK; 55 call[6] = 0; 56 } else { 57 offset = offset >> 1; 58 59 call[1] = BSR_LINK | 60 ((uint16_t)((unsigned long) offset >> 16) & 0x3ff); 61 call[2] = (uint16_t)((unsigned long) offset & 0xffff); 62 call[3] = call[5] = NOP32_HI; 63 call[4] = call[6] = NOP32_LO; 64 } 65 } 66 67 static uint16_t nops[7] = {NOP, NOP32_HI, NOP32_LO, NOP32_HI, NOP32_LO, 68 NOP32_HI, NOP32_LO}; 69 static int ftrace_check_current_nop(unsigned long hook) 70 { 71 uint16_t olds[7]; 72 unsigned long hook_pos = hook - 2; 73 74 if (probe_kernel_read((void *)olds, (void *)hook_pos, sizeof(nops))) 75 return -EFAULT; 76 77 if (memcmp((void *)nops, (void *)olds, sizeof(nops))) { 78 pr_err("%p: nop but get (%04x %04x %04x %04x %04x %04x %04x)\n", 79 (void *)hook_pos, 80 olds[0], olds[1], olds[2], olds[3], olds[4], olds[5], 81 olds[6]); 82 83 return -EINVAL; 84 } 85 86 return 0; 87 } 88 89 static int ftrace_modify_code(unsigned long hook, unsigned long target, 90 bool enable, bool nolr) 91 { 92 uint16_t call[7]; 93 94 unsigned long hook_pos = hook - 2; 95 int ret = 0; 96 97 make_jbsr(target, hook, call, nolr); 98 99 ret = probe_kernel_write((void *)hook_pos, enable ? call : nops, 100 sizeof(nops)); 101 if (ret) 102 return -EPERM; 103 104 flush_icache_range(hook_pos, hook_pos + MCOUNT_INSN_SIZE); 105 106 return 0; 107 } 108 109 int ftrace_make_call(struct dyn_ftrace *rec, unsigned long addr) 110 { 111 int ret = ftrace_check_current_nop(rec->ip); 112 113 if (ret) 114 return ret; 115 116 return ftrace_modify_code(rec->ip, addr, true, false); 117 } 118 119 int ftrace_make_nop(struct module *mod, struct dyn_ftrace *rec, 120 unsigned long addr) 121 { 122 return ftrace_modify_code(rec->ip, addr, false, false); 123 } 124 125 int ftrace_update_ftrace_func(ftrace_func_t func) 126 { 127 int ret = ftrace_modify_code((unsigned long)&ftrace_call, 128 (unsigned long)func, true, true); 129 return ret; 130 } 131 132 int __init ftrace_dyn_arch_init(void) 133 { 134 return 0; 135 } 136 #endif /* CONFIG_DYNAMIC_FTRACE */ 137 138 #ifdef CONFIG_FUNCTION_GRAPH_TRACER 139 void prepare_ftrace_return(unsigned long *parent, unsigned long self_addr, 140 unsigned long frame_pointer) 141 { 142 unsigned long return_hooker = (unsigned long)&return_to_handler; 143 unsigned long old; 144 145 if (unlikely(atomic_read(¤t->tracing_graph_pause))) 146 return; 147 148 old = *parent; 149 150 if (!function_graph_enter(old, self_addr, 151 *(unsigned long *)frame_pointer, parent)) { 152 /* 153 * For csky-gcc function has sub-call: 154 * subi sp, sp, 8 155 * stw r8, (sp, 0) 156 * mov r8, sp 157 * st.w r15, (sp, 0x4) 158 * push r15 159 * jl _mcount 160 * We only need set *parent for resume 161 * 162 * For csky-gcc function has no sub-call: 163 * subi sp, sp, 4 164 * stw r8, (sp, 0) 165 * mov r8, sp 166 * push r15 167 * jl _mcount 168 * We need set *parent and *(frame_pointer + 4) for resume, 169 * because lr is resumed twice. 170 */ 171 *parent = return_hooker; 172 frame_pointer += 4; 173 if (*(unsigned long *)frame_pointer == old) 174 *(unsigned long *)frame_pointer = return_hooker; 175 } 176 } 177 178 #ifdef CONFIG_DYNAMIC_FTRACE 179 int ftrace_enable_ftrace_graph_caller(void) 180 { 181 return ftrace_modify_code((unsigned long)&ftrace_graph_call, 182 (unsigned long)&ftrace_graph_caller, true, true); 183 } 184 185 int ftrace_disable_ftrace_graph_caller(void) 186 { 187 return ftrace_modify_code((unsigned long)&ftrace_graph_call, 188 (unsigned long)&ftrace_graph_caller, false, true); 189 } 190 #endif /* CONFIG_DYNAMIC_FTRACE */ 191 #endif /* CONFIG_FUNCTION_GRAPH_TRACER */ 192 193 /* _mcount is defined in abi's mcount.S */ 194 EXPORT_SYMBOL(_mcount); 195