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 (probe_kernel_read((void *)olds, (void *)hook_pos, sizeof(nops))) 76 return -EFAULT; 77 78 if (memcmp((void *)nops, (void *)olds, sizeof(nops))) { 79 pr_err("%p: nop but get (%04x %04x %04x %04x %04x %04x %04x)\n", 80 (void *)hook_pos, 81 olds[0], olds[1], olds[2], olds[3], olds[4], olds[5], 82 olds[6]); 83 84 return -EINVAL; 85 } 86 87 return 0; 88 } 89 90 static int ftrace_modify_code(unsigned long hook, unsigned long target, 91 bool enable, bool nolr) 92 { 93 uint16_t call[7]; 94 95 unsigned long hook_pos = hook - 2; 96 int ret = 0; 97 98 make_jbsr(target, hook, call, nolr); 99 100 ret = probe_kernel_write((void *)hook_pos, enable ? call : nops, 101 sizeof(nops)); 102 if (ret) 103 return -EPERM; 104 105 flush_icache_range(hook_pos, hook_pos + MCOUNT_INSN_SIZE); 106 107 return 0; 108 } 109 110 int ftrace_make_call(struct dyn_ftrace *rec, unsigned long addr) 111 { 112 int ret = ftrace_check_current_nop(rec->ip); 113 114 if (ret) 115 return ret; 116 117 return ftrace_modify_code(rec->ip, addr, true, false); 118 } 119 120 int ftrace_make_nop(struct module *mod, struct dyn_ftrace *rec, 121 unsigned long addr) 122 { 123 return ftrace_modify_code(rec->ip, addr, false, false); 124 } 125 126 int ftrace_update_ftrace_func(ftrace_func_t func) 127 { 128 int ret = ftrace_modify_code((unsigned long)&ftrace_call, 129 (unsigned long)func, true, true); 130 if (!ret) 131 ret = ftrace_modify_code((unsigned long)&ftrace_regs_call, 132 (unsigned long)func, true, true); 133 return ret; 134 } 135 136 int __init ftrace_dyn_arch_init(void) 137 { 138 return 0; 139 } 140 #endif /* CONFIG_DYNAMIC_FTRACE */ 141 142 #ifdef CONFIG_DYNAMIC_FTRACE_WITH_REGS 143 int ftrace_modify_call(struct dyn_ftrace *rec, unsigned long old_addr, 144 unsigned long addr) 145 { 146 return ftrace_modify_code(rec->ip, addr, true, true); 147 } 148 #endif 149 150 #ifdef CONFIG_FUNCTION_GRAPH_TRACER 151 void prepare_ftrace_return(unsigned long *parent, unsigned long self_addr, 152 unsigned long frame_pointer) 153 { 154 unsigned long return_hooker = (unsigned long)&return_to_handler; 155 unsigned long old; 156 157 if (unlikely(atomic_read(¤t->tracing_graph_pause))) 158 return; 159 160 old = *parent; 161 162 if (!function_graph_enter(old, self_addr, 163 *(unsigned long *)frame_pointer, parent)) { 164 /* 165 * For csky-gcc function has sub-call: 166 * subi sp, sp, 8 167 * stw r8, (sp, 0) 168 * mov r8, sp 169 * st.w r15, (sp, 0x4) 170 * push r15 171 * jl _mcount 172 * We only need set *parent for resume 173 * 174 * For csky-gcc function has no sub-call: 175 * subi sp, sp, 4 176 * stw r8, (sp, 0) 177 * mov r8, sp 178 * push r15 179 * jl _mcount 180 * We need set *parent and *(frame_pointer + 4) for resume, 181 * because lr is resumed twice. 182 */ 183 *parent = return_hooker; 184 frame_pointer += 4; 185 if (*(unsigned long *)frame_pointer == old) 186 *(unsigned long *)frame_pointer = return_hooker; 187 } 188 } 189 190 #ifdef CONFIG_DYNAMIC_FTRACE 191 int ftrace_enable_ftrace_graph_caller(void) 192 { 193 return ftrace_modify_code((unsigned long)&ftrace_graph_call, 194 (unsigned long)&ftrace_graph_caller, true, true); 195 } 196 197 int ftrace_disable_ftrace_graph_caller(void) 198 { 199 return ftrace_modify_code((unsigned long)&ftrace_graph_call, 200 (unsigned long)&ftrace_graph_caller, false, true); 201 } 202 #endif /* CONFIG_DYNAMIC_FTRACE */ 203 #endif /* CONFIG_FUNCTION_GRAPH_TRACER */ 204 205 #ifndef CONFIG_CPU_HAS_ICACHE_INS 206 struct ftrace_modify_param { 207 int command; 208 atomic_t cpu_count; 209 }; 210 211 static int __ftrace_modify_code(void *data) 212 { 213 struct ftrace_modify_param *param = data; 214 215 if (atomic_inc_return(¶m->cpu_count) == 1) { 216 ftrace_modify_all_code(param->command); 217 atomic_inc(¶m->cpu_count); 218 } else { 219 while (atomic_read(¶m->cpu_count) <= num_online_cpus()) 220 cpu_relax(); 221 local_icache_inv_all(NULL); 222 } 223 224 return 0; 225 } 226 227 void arch_ftrace_update_code(int command) 228 { 229 struct ftrace_modify_param param = { command, ATOMIC_INIT(0) }; 230 231 stop_machine(__ftrace_modify_code, ¶m, cpu_online_mask); 232 } 233 #endif 234 235 /* _mcount is defined in abi's mcount.S */ 236 EXPORT_SYMBOL(_mcount); 237