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