[PATCH] Kprobes ia64 qp fix
[linux-2.6.git] / arch / ia64 / kernel / kprobes.c
1 /*
2  *  Kernel Probes (KProbes)
3  *  arch/ia64/kernel/kprobes.c
4  *
5  * This program is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software
17  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
18  *
19  * Copyright (C) IBM Corporation, 2002, 2004
20  * Copyright (C) Intel Corporation, 2005
21  *
22  * 2005-Apr     Rusty Lynch <rusty.lynch@intel.com> and Anil S Keshavamurthy
23  *              <anil.s.keshavamurthy@intel.com> adapted from i386
24  */
25
26 #include <linux/config.h>
27 #include <linux/kprobes.h>
28 #include <linux/ptrace.h>
29 #include <linux/spinlock.h>
30 #include <linux/string.h>
31 #include <linux/slab.h>
32 #include <linux/preempt.h>
33 #include <linux/moduleloader.h>
34
35 #include <asm/pgtable.h>
36 #include <asm/kdebug.h>
37
38 extern void jprobe_inst_return(void);
39
40 /* kprobe_status settings */
41 #define KPROBE_HIT_ACTIVE       0x00000001
42 #define KPROBE_HIT_SS           0x00000002
43
44 static struct kprobe *current_kprobe;
45 static unsigned long kprobe_status;
46 static struct pt_regs jprobe_saved_regs;
47
48 enum instruction_type {A, I, M, F, B, L, X, u};
49 static enum instruction_type bundle_encoding[32][3] = {
50   { M, I, I },                          /* 00 */
51   { M, I, I },                          /* 01 */
52   { M, I, I },                          /* 02 */
53   { M, I, I },                          /* 03 */
54   { M, L, X },                          /* 04 */
55   { M, L, X },                          /* 05 */
56   { u, u, u },                          /* 06 */
57   { u, u, u },                          /* 07 */
58   { M, M, I },                          /* 08 */
59   { M, M, I },                          /* 09 */
60   { M, M, I },                          /* 0A */
61   { M, M, I },                          /* 0B */
62   { M, F, I },                          /* 0C */
63   { M, F, I },                          /* 0D */
64   { M, M, F },                          /* 0E */
65   { M, M, F },                          /* 0F */
66   { M, I, B },                          /* 10 */
67   { M, I, B },                          /* 11 */
68   { M, B, B },                          /* 12 */
69   { M, B, B },                          /* 13 */
70   { u, u, u },                          /* 14 */
71   { u, u, u },                          /* 15 */
72   { B, B, B },                          /* 16 */
73   { B, B, B },                          /* 17 */
74   { M, M, B },                          /* 18 */
75   { M, M, B },                          /* 19 */
76   { u, u, u },                          /* 1A */
77   { u, u, u },                          /* 1B */
78   { M, F, B },                          /* 1C */
79   { M, F, B },                          /* 1D */
80   { u, u, u },                          /* 1E */
81   { u, u, u },                          /* 1F */
82 };
83
84 int arch_prepare_kprobe(struct kprobe *p)
85 {
86         unsigned long addr = (unsigned long) p->addr;
87         unsigned long *bundle_addr = (unsigned long *)(addr & ~0xFULL);
88         unsigned long slot = addr & 0xf;
89         unsigned long template;
90         unsigned long major_opcode = 0;
91         unsigned long lx_type_inst = 0;
92         unsigned long kprobe_inst = 0;
93         bundle_t *bundle = &p->ainsn.insn.bundle;
94
95         memcpy(&p->opcode.bundle, bundle_addr, sizeof(bundle_t));
96         memcpy(&p->ainsn.insn.bundle, bundle_addr, sizeof(bundle_t));
97
98         p->ainsn.inst_flag = 0;
99         p->ainsn.target_br_reg = 0;
100
101         template = bundle->quad0.template;
102
103         if (((bundle_encoding[template][1] == L) && slot > 1) || (slot > 2)) {
104                 printk(KERN_WARNING "Attempting to insert unaligned kprobe at 0x%lx\n",
105                                 addr);
106                 return -EINVAL;
107         }
108
109         if (slot == 1 && bundle_encoding[template][1] == L) {
110                 lx_type_inst = 1;
111                 slot = 2;
112         }
113
114         switch (slot) {
115         case 0:
116                 major_opcode = (bundle->quad0.slot0 >> SLOT0_OPCODE_SHIFT);
117                 kprobe_inst = bundle->quad0.slot0;
118                 bundle->quad0.slot0 = BREAK_INST | (0x3f & kprobe_inst);
119                 break;
120         case 1:
121                 major_opcode = (bundle->quad1.slot1_p1 >> SLOT1_p1_OPCODE_SHIFT);
122                 kprobe_inst = (bundle->quad0.slot1_p0 |
123                                 (bundle->quad1.slot1_p1 << (64-46)));
124                 bundle->quad0.slot1_p0 = BREAK_INST | (0x3f & kprobe_inst);
125                 bundle->quad1.slot1_p1 = (BREAK_INST >> (64-46));
126                 break;
127         case 2:
128                 major_opcode = (bundle->quad1.slot2 >> SLOT2_OPCODE_SHIFT);
129                 kprobe_inst = bundle->quad1.slot2;
130                 bundle->quad1.slot2 = BREAK_INST | (0x3f & kprobe_inst);
131                 break;
132         }
133
134         /*
135          * Look for IP relative Branches, IP relative call or
136          * IP relative predicate instructions
137          */
138         if (bundle_encoding[template][slot] == B) {
139                 switch (major_opcode) {
140                         case INDIRECT_CALL_OPCODE:
141                                 p->ainsn.inst_flag |= INST_FLAG_FIX_BRANCH_REG;
142                                 p->ainsn.target_br_reg = ((kprobe_inst >> 6) & 0x7);
143                                 break;
144                         case IP_RELATIVE_PREDICT_OPCODE:
145                         case IP_RELATIVE_BRANCH_OPCODE:
146                                 p->ainsn.inst_flag |= INST_FLAG_FIX_RELATIVE_IP_ADDR;
147                                 break;
148                         case IP_RELATIVE_CALL_OPCODE:
149                                 p->ainsn.inst_flag |= INST_FLAG_FIX_RELATIVE_IP_ADDR;
150                                 p->ainsn.inst_flag |= INST_FLAG_FIX_BRANCH_REG;
151                                 p->ainsn.target_br_reg = ((kprobe_inst >> 6) & 0x7);
152                                 break;
153                         default:
154                                 /* Do nothing */
155                                 break;
156                 }
157         } else if (lx_type_inst) {
158                 switch (major_opcode) {
159                         case LONG_CALL_OPCODE:
160                                 p->ainsn.inst_flag |= INST_FLAG_FIX_BRANCH_REG;
161                                 p->ainsn.target_br_reg = ((kprobe_inst >> 6) & 0x7);
162                                 break;
163                         default:
164                                 /* Do nothing */
165                                 break;
166                 }
167         }
168
169         return 0;
170 }
171
172 void arch_arm_kprobe(struct kprobe *p)
173 {
174         unsigned long addr = (unsigned long)p->addr;
175         unsigned long arm_addr = addr & ~0xFULL;
176
177         memcpy((char *)arm_addr, &p->ainsn.insn.bundle, sizeof(bundle_t));
178         flush_icache_range(arm_addr, arm_addr + sizeof(bundle_t));
179 }
180
181 void arch_disarm_kprobe(struct kprobe *p)
182 {
183         unsigned long addr = (unsigned long)p->addr;
184         unsigned long arm_addr = addr & ~0xFULL;
185
186         /* p->opcode contains the original unaltered bundle */
187         memcpy((char *) arm_addr, (char *) &p->opcode.bundle, sizeof(bundle_t));
188         flush_icache_range(arm_addr, arm_addr + sizeof(bundle_t));
189 }
190
191 void arch_remove_kprobe(struct kprobe *p)
192 {
193 }
194
195 /*
196  * We are resuming execution after a single step fault, so the pt_regs
197  * structure reflects the register state after we executed the instruction
198  * located in the kprobe (p->ainsn.insn.bundle).  We still need to adjust
199  * the ip to point back to the original stack address. To set the IP address
200  * to original stack address, handle the case where we need to fixup the
201  * relative IP address and/or fixup branch register.
202  */
203 static void resume_execution(struct kprobe *p, struct pt_regs *regs)
204 {
205         unsigned long bundle_addr = ((unsigned long) (&p->opcode.bundle)) & ~0xFULL;
206         unsigned long resume_addr = (unsigned long)p->addr & ~0xFULL;
207         unsigned long template;
208         int slot = ((unsigned long)p->addr & 0xf);
209
210         template = p->opcode.bundle.quad0.template;
211
212         if (slot == 1 && bundle_encoding[template][1] == L)
213                 slot = 2;
214
215         if (p->ainsn.inst_flag) {
216
217                 if (p->ainsn.inst_flag & INST_FLAG_FIX_RELATIVE_IP_ADDR) {
218                         /* Fix relative IP address */
219                         regs->cr_iip = (regs->cr_iip - bundle_addr) + resume_addr;
220                 }
221
222                 if (p->ainsn.inst_flag & INST_FLAG_FIX_BRANCH_REG) {
223                 /*
224                  * Fix target branch register, software convention is
225                  * to use either b0 or b6 or b7, so just checking
226                  * only those registers
227                  */
228                         switch (p->ainsn.target_br_reg) {
229                         case 0:
230                                 if ((regs->b0 == bundle_addr) ||
231                                         (regs->b0 == bundle_addr + 0x10)) {
232                                         regs->b0 = (regs->b0 - bundle_addr) +
233                                                 resume_addr;
234                                 }
235                                 break;
236                         case 6:
237                                 if ((regs->b6 == bundle_addr) ||
238                                         (regs->b6 == bundle_addr + 0x10)) {
239                                         regs->b6 = (regs->b6 - bundle_addr) +
240                                                 resume_addr;
241                                 }
242                                 break;
243                         case 7:
244                                 if ((regs->b7 == bundle_addr) ||
245                                         (regs->b7 == bundle_addr + 0x10)) {
246                                         regs->b7 = (regs->b7 - bundle_addr) +
247                                                 resume_addr;
248                                 }
249                                 break;
250                         } /* end switch */
251                 }
252                 goto turn_ss_off;
253         }
254
255         if (slot == 2) {
256                 if (regs->cr_iip == bundle_addr + 0x10) {
257                         regs->cr_iip = resume_addr + 0x10;
258                 }
259         } else {
260                 if (regs->cr_iip == bundle_addr) {
261                         regs->cr_iip = resume_addr;
262                 }
263         }
264
265 turn_ss_off:
266         /* Turn off Single Step bit */
267         ia64_psr(regs)->ss = 0;
268 }
269
270 static void prepare_ss(struct kprobe *p, struct pt_regs *regs)
271 {
272         unsigned long bundle_addr = (unsigned long) &p->opcode.bundle;
273         unsigned long slot = (unsigned long)p->addr & 0xf;
274
275         /* Update instruction pointer (IIP) and slot number (IPSR.ri) */
276         regs->cr_iip = bundle_addr & ~0xFULL;
277
278         if (slot > 2)
279                 slot = 0;
280
281         ia64_psr(regs)->ri = slot;
282
283         /* turn on single stepping */
284         ia64_psr(regs)->ss = 1;
285 }
286
287 static int pre_kprobes_handler(struct pt_regs *regs)
288 {
289         struct kprobe *p;
290         int ret = 0;
291         kprobe_opcode_t *addr = (kprobe_opcode_t *)instruction_pointer(regs);
292
293         preempt_disable();
294
295         /* Handle recursion cases */
296         if (kprobe_running()) {
297                 p = get_kprobe(addr);
298                 if (p) {
299                         if (kprobe_status == KPROBE_HIT_SS) {
300                                 unlock_kprobes();
301                                 goto no_kprobe;
302                         }
303                         arch_disarm_kprobe(p);
304                         ret = 1;
305                 } else {
306                         /*
307                          * jprobe instrumented function just completed
308                          */
309                         p = current_kprobe;
310                         if (p->break_handler && p->break_handler(p, regs)) {
311                                 goto ss_probe;
312                         }
313                 }
314         }
315
316         lock_kprobes();
317         p = get_kprobe(addr);
318         if (!p) {
319                 unlock_kprobes();
320                 goto no_kprobe;
321         }
322
323         kprobe_status = KPROBE_HIT_ACTIVE;
324         current_kprobe = p;
325
326         if (p->pre_handler && p->pre_handler(p, regs))
327                 /*
328                  * Our pre-handler is specifically requesting that we just
329                  * do a return.  This is handling the case where the
330                  * pre-handler is really our special jprobe pre-handler.
331                  */
332                 return 1;
333
334 ss_probe:
335         prepare_ss(p, regs);
336         kprobe_status = KPROBE_HIT_SS;
337         return 1;
338
339 no_kprobe:
340         preempt_enable_no_resched();
341         return ret;
342 }
343
344 static int post_kprobes_handler(struct pt_regs *regs)
345 {
346         if (!kprobe_running())
347                 return 0;
348
349         if (current_kprobe->post_handler)
350                 current_kprobe->post_handler(current_kprobe, regs, 0);
351
352         resume_execution(current_kprobe, regs);
353
354         unlock_kprobes();
355         preempt_enable_no_resched();
356         return 1;
357 }
358
359 static int kprobes_fault_handler(struct pt_regs *regs, int trapnr)
360 {
361         if (!kprobe_running())
362                 return 0;
363
364         if (current_kprobe->fault_handler &&
365             current_kprobe->fault_handler(current_kprobe, regs, trapnr))
366                 return 1;
367
368         if (kprobe_status & KPROBE_HIT_SS) {
369                 resume_execution(current_kprobe, regs);
370                 unlock_kprobes();
371                 preempt_enable_no_resched();
372         }
373
374         return 0;
375 }
376
377 int kprobe_exceptions_notify(struct notifier_block *self, unsigned long val,
378                              void *data)
379 {
380         struct die_args *args = (struct die_args *)data;
381         switch(val) {
382         case DIE_BREAK:
383                 if (pre_kprobes_handler(args->regs))
384                         return NOTIFY_STOP;
385                 break;
386         case DIE_SS:
387                 if (post_kprobes_handler(args->regs))
388                         return NOTIFY_STOP;
389                 break;
390         case DIE_PAGE_FAULT:
391                 if (kprobes_fault_handler(args->regs, args->trapnr))
392                         return NOTIFY_STOP;
393         default:
394                 break;
395         }
396         return NOTIFY_DONE;
397 }
398
399 int setjmp_pre_handler(struct kprobe *p, struct pt_regs *regs)
400 {
401         struct jprobe *jp = container_of(p, struct jprobe, kp);
402         unsigned long addr = ((struct fnptr *)(jp->entry))->ip;
403
404         /* save architectural state */
405         jprobe_saved_regs = *regs;
406
407         /* after rfi, execute the jprobe instrumented function */
408         regs->cr_iip = addr & ~0xFULL;
409         ia64_psr(regs)->ri = addr & 0xf;
410         regs->r1 = ((struct fnptr *)(jp->entry))->gp;
411
412         /*
413          * fix the return address to our jprobe_inst_return() function
414          * in the jprobes.S file
415          */
416         regs->b0 = ((struct fnptr *)(jprobe_inst_return))->ip;
417
418         return 1;
419 }
420
421 int longjmp_break_handler(struct kprobe *p, struct pt_regs *regs)
422 {
423         *regs = jprobe_saved_regs;
424         return 1;
425 }