]>
Commit | Line | Data |
---|---|---|
230c77a5 GR |
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> | |
dd7c983e | 6 | #include <linux/stop_machine.h> |
28bb030f GR |
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); | |
89a3927a GR |
130 | if (!ret) |
131 | ret = ftrace_modify_code((unsigned long)&ftrace_regs_call, | |
132 | (unsigned long)func, true, true); | |
28bb030f GR |
133 | return ret; |
134 | } | |
135 | ||
136 | int __init ftrace_dyn_arch_init(void) | |
137 | { | |
138 | return 0; | |
139 | } | |
140 | #endif /* CONFIG_DYNAMIC_FTRACE */ | |
230c77a5 | 141 | |
89a3927a GR |
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 | ||
d7950be1 GR |
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; | |
230c77a5 | 156 | |
d7950be1 GR |
157 | if (unlikely(atomic_read(¤t->tracing_graph_pause))) |
158 | return; | |
230c77a5 | 159 | |
d7950be1 | 160 | old = *parent; |
230c77a5 | 161 | |
d7950be1 GR |
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 | } | |
230c77a5 | 188 | } |
28bb030f GR |
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 */ | |
230c77a5 | 204 | |
a13d5887 | 205 | #ifdef CONFIG_DYNAMIC_FTRACE |
dd7c983e GR |
206 | #ifndef CONFIG_CPU_HAS_ICACHE_INS |
207 | struct ftrace_modify_param { | |
208 | int command; | |
209 | atomic_t cpu_count; | |
210 | }; | |
211 | ||
212 | static int __ftrace_modify_code(void *data) | |
213 | { | |
214 | struct ftrace_modify_param *param = data; | |
215 | ||
216 | if (atomic_inc_return(¶m->cpu_count) == 1) { | |
217 | ftrace_modify_all_code(param->command); | |
218 | atomic_inc(¶m->cpu_count); | |
219 | } else { | |
220 | while (atomic_read(¶m->cpu_count) <= num_online_cpus()) | |
221 | cpu_relax(); | |
222 | local_icache_inv_all(NULL); | |
223 | } | |
224 | ||
225 | return 0; | |
226 | } | |
227 | ||
228 | void arch_ftrace_update_code(int command) | |
229 | { | |
230 | struct ftrace_modify_param param = { command, ATOMIC_INIT(0) }; | |
231 | ||
232 | stop_machine(__ftrace_modify_code, ¶m, cpu_online_mask); | |
233 | } | |
234 | #endif | |
a13d5887 | 235 | #endif /* CONFIG_DYNAMIC_FTRACE */ |
dd7c983e | 236 | |
230c77a5 GR |
237 | /* _mcount is defined in abi's mcount.S */ |
238 | EXPORT_SYMBOL(_mcount); |