1
2
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
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41static 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
68static uint16_t nops[7] = {NOP, NOP32_HI, NOP32_LO, NOP32_HI, NOP32_LO,
69 NOP32_HI, NOP32_LO};
70static 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
90static 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
110int 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
120int 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
126int 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
136int __init ftrace_dyn_arch_init(void)
137{
138 return 0;
139}
140#endif
141
142#ifdef CONFIG_DYNAMIC_FTRACE_WITH_REGS
143int 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
151void 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
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
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
191int 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
197int 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
203#endif
204
205#ifdef CONFIG_DYNAMIC_FTRACE
206#ifndef CONFIG_CPU_HAS_ICACHE_INS
207struct ftrace_modify_param {
208 int command;
209 atomic_t cpu_count;
210};
211
212static 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
228void 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
235#endif
236
237
238EXPORT_SYMBOL(_mcount);
239