Redesign KProbe module (separating core and arch parts).
[kernel/swap-modules.git] / kprobe / arch / asm-arm / dbi_kprobes.c
1 /*
2  *  Dynamic Binary Instrumentation Module based on KProbes
3  *  modules/kprobe/arch/asm-arm/dbi_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) Samsung Electronics, 2006-2010
20  *
21  * 2006-2007    Ekaterina Gorelkina <e.gorelkina@samsung.com>: initial implementation for ARM/MIPS
22  * 2008-2009    Alexey Gerenkov <a.gerenkov@samsung.com> User-Space
23  *              Probes initial implementation; Support x86.
24  * 2010         Ekaterina Gorelkina <e.gorelkina@samsung.com>: redesign module for separating core and arch parts 
25  *
26
27  */
28
29 #include "dbi_kprobes.h"
30 #include "../dbi_kprobes.h"
31
32
33 #include "../../dbi_kdebug.h"
34 #include "../../dbi_insn_slots.h"
35 #include "../../dbi_kprobes_deps.h"
36 #include "../../dbi_uprobes.h"
37
38 #include <asm/cacheflush.h>
39
40 unsigned int *arr_traps_original;
41
42 extern unsigned int *sched_addr;
43 extern unsigned int *fork_addr;
44
45 extern struct kprobe * per_cpu__current_kprobe;
46 extern spinlock_t kretprobe_lock;
47 extern struct kretprobe *sched_rp;
48
49 extern struct hlist_head kprobe_insn_pages;
50 extern struct hlist_head uprobe_insn_pages;
51
52 extern unsigned long (*kallsyms_search) (const char *name);
53
54 extern struct kprobe *kprobe_running (void);
55 extern struct kprobe_ctlblk *get_kprobe_ctlblk (void);
56 extern void reset_current_kprobe (void);
57
58 unsigned int arr_traps_template[] = {   0xe1a0c00d,    // mov          ip, sp
59         0xe92dd800,    // stmdb        sp!, {fp, ip, lr, pc}
60              0xe24cb004,    // sub          fp, ip, #4      ; 0x4
61              0x00000000,    // b                                    
62              0xe3500000,    // cmp          r0, #0  ; 0x0   
63              0xe89da800,    // ldmia        sp, {fp, sp, pc}
64              0x00000000,    // nop
65              0xffffffff     // end
66 };
67
68
69 /*
70  * Function return probe trampoline:
71  *      - init_kprobes() establishes a probepoint here
72  *      - When the probed function returns, this probe
73  *              causes the handlers to fire
74  */
75 void kretprobe_trampoline_holder (void)
76 {
77         asm volatile (".global kretprobe_trampoline\n"
78                         "kretprobe_trampoline:\n"
79                         "nop\n"
80                         "nop\n" 
81                         "mov pc, r14\n");
82 }
83
84
85 struct kprobe trampoline_p =
86 {
87         .addr = (kprobe_opcode_t *) & kretprobe_trampoline,
88         .pre_handler = trampoline_probe_handler
89 };
90
91
92 void gen_insn_execbuf_holder (void)
93 {
94         asm volatile (".global gen_insn_execbuf\n" 
95                         "gen_insn_execbuf:\n" 
96                         "nop\n" 
97                         "nop\n" 
98                         "nop\n"                 // original instruction
99                         "nop\n" 
100                         "ldr    pc, [pc, #4]\n" //ssbreak 
101                         "nop\n"                   //retbreak
102                         "nop\n" 
103                         "nop\n");                 //stored PC-4(next insn addr)
104 }
105
106
107 /*
108  * 0. push Rx on stack
109  * 1. load address to Rx
110  * 2. do insn using Rx
111  * 3. pop Rx from stack
112  * 4. BREAK1
113  * 5. BREAK2
114  * 6. stored PC
115  * 7. stored PC-4(next insn addr)
116  */
117 void pc_dep_insn_execbuf_holder (void)
118 {
119         asm volatile (".global pc_dep_insn_execbuf\n" 
120                         "pc_dep_insn_execbuf:\n"
121                         "str    r0, [sp, #-4]\n" 
122                         "ldr    r0, [pc, #12]\n" 
123                         "nop\n" // instruction with replaced PC
124                         "ldr    r0, [sp, #-4]\n"
125                         "ldr    pc, [pc, #4]\n" //ssbreak
126                         "nop\n" // retbreak
127                         "nop\n" // stored PC
128                         "nop\n");// stored PC-4 (next insn addr)
129 }
130
131 int prep_pc_dep_insn_execbuf (kprobe_opcode_t * insns, kprobe_opcode_t insn, int uregs)
132 {
133         int i;
134
135         if (uregs & 0x10)
136         {
137                 int reg_mask = 0x1;
138                 //search in reg list
139                 for (i = 0; i < 13; i++, reg_mask <<= 1)
140                 {
141                         if (!(insn & reg_mask))
142                                 break;
143                 }
144         }
145         else
146         {
147                 for (i = 0; i < 13; i++)
148                 {
149                         //              DBPRINTF("prep_pc_dep_insn_execbuf: check R%d/%d, changing regs %x in %x", 
150                         //                              i, ARM_INSN_REG_RN(insn), uregs, insn);
151                         if ((uregs & 0x1) && (ARM_INSN_REG_RN (insn) == i))
152                                 continue;
153                         if ((uregs & 0x2) && (ARM_INSN_REG_RD (insn) == i))
154                                 continue;
155                         if ((uregs & 0x4) && (ARM_INSN_REG_RS (insn) == i))
156                                 continue;
157                         if ((uregs & 0x8) && (ARM_INSN_REG_RM (insn) == i))
158                                 continue;
159                         break;
160                 }
161         }
162         if (i == 13)
163         {
164                 DBPRINTF ("there are no free register %x in insn %lx!", uregs, insn);
165                 return -EINVAL;
166         }
167         DBPRINTF ("prep_pc_dep_insn_execbuf: using R%d, changing regs %x", i, uregs);
168
169         // set register to save
170         ARM_INSN_REG_SET_RD (insns[0], i);
171         // set register to load address to
172         ARM_INSN_REG_SET_RD (insns[1], i);
173         // set instruction to execute and patch it 
174         if (uregs & 0x10)
175         {
176                 ARM_INSN_REG_CLEAR_MR (insn, 15);
177                 ARM_INSN_REG_SET_MR (insn, i);
178         }
179         else
180         {
181                 if ((uregs & 0x1) && (ARM_INSN_REG_RN (insn) == 15))
182                         ARM_INSN_REG_SET_RN (insn, i);
183                 if ((uregs & 0x2) && (ARM_INSN_REG_RD (insn) == 15))
184                         ARM_INSN_REG_SET_RD (insn, i);
185                 if ((uregs & 0x4) && (ARM_INSN_REG_RS (insn) == 15))
186                         ARM_INSN_REG_SET_RS (insn, i);
187                 if ((uregs & 0x8) && (ARM_INSN_REG_RM (insn) == 15))
188                         ARM_INSN_REG_SET_RM (insn, i);
189         }
190         insns[UPROBES_TRAMP_INSN_IDX] = insn;
191         // set register to restore
192         ARM_INSN_REG_SET_RD (insns[3], i);
193         return 0;
194 }
195
196
197 int arch_check_insn (struct arch_specific_insn *ainsn)
198 {
199         int ret = 0;
200         // check instructions that can change PC by nature 
201         if (ARM_INSN_MATCH (UNDEF, ainsn->insn[0]) ||
202                         ARM_INSN_MATCH (AUNDEF, ainsn->insn[0]) ||
203                         ARM_INSN_MATCH (SWI, ainsn->insn[0]) ||
204                         ARM_INSN_MATCH (BREAK, ainsn->insn[0]) ||
205                         ARM_INSN_MATCH (B, ainsn->insn[0]) ||
206                         ARM_INSN_MATCH (BL, ainsn->insn[0]) ||
207                         ARM_INSN_MATCH (BLX1, ainsn->insn[0]) || 
208                         ARM_INSN_MATCH (BLX2, ainsn->insn[0]) || 
209                         ARM_INSN_MATCH (BX, ainsn->insn[0]) || 
210                         ARM_INSN_MATCH (BXJ, ainsn->insn[0]))
211         {
212                 DBPRINTF ("arch_check_insn: %lx\n", ainsn->insn[0]);
213                 ret = -EFAULT;
214         }
215 #ifndef CONFIG_CPU_V7
216         // check instructions that can write result to PC
217         else if ((ARM_INSN_MATCH (DPIS, ainsn->insn[0]) ||
218                                 ARM_INSN_MATCH (DPRS, ainsn->insn[0]) ||
219                                 ARM_INSN_MATCH (DPI, ainsn->insn[0]) || 
220                                 ARM_INSN_MATCH (LIO, ainsn->insn[0]) || 
221                                 ARM_INSN_MATCH (LRO, ainsn->insn[0])) && 
222                         (ARM_INSN_REG_RD (ainsn->insn[0]) == 15))
223         {
224                 DBPRINTF ("arch_check_insn: %lx\n", ainsn->insn[0]);
225                 ret = -EFAULT;
226         }
227 #endif // CONFIG_CPU_V7
228         // check special instruction loads store multiple registers
229         else if ((ARM_INSN_MATCH (LM, ainsn->insn[0]) || ARM_INSN_MATCH (SM, ainsn->insn[0])) &&
230                         // store pc or load to pc
231                         (ARM_INSN_REG_MR (ainsn->insn[0], 15) ||
232                          // store/load with pc update
233                          ((ARM_INSN_REG_RN (ainsn->insn[0]) == 15) && (ainsn->insn[0] & 0x200000))))
234         {
235                 DBPRINTF ("arch_check_insn: %lx\n", ainsn->insn[0]);
236                 ret = -EFAULT;
237         }
238         return ret;
239 }
240
241 int arch_prepare_kretprobe (struct kretprobe *p)
242 {
243         DBPRINTF("Warrning: arch_prepare_kretprobe is not implemented\n");
244         return 0;
245 }
246
247 int arch_prepare_kprobe (struct kprobe *p)
248 {
249         kprobe_opcode_t insns[KPROBES_TRAMP_LEN];
250         int uregs, pc_dep;
251         int ret = 0;
252
253         if (!ret)
254         {
255                 kprobe_opcode_t insn[MAX_INSN_SIZE];
256                 struct arch_specific_insn ainsn;
257                 /* insn: must be on special executable page on i386. */
258                 p->ainsn.insn = get_insn_slot (NULL, 0);
259                 if (!p->ainsn.insn)
260                         return -ENOMEM;
261                 memcpy (insn, p->addr, MAX_INSN_SIZE * sizeof (kprobe_opcode_t));
262                 ainsn.insn = insn;
263                 ret = arch_check_insn (&ainsn);
264                 if (!ret)
265                 {
266                         p->opcode = *p->addr;
267
268                         p->ainsn.boostable = 1;
269                         uregs = pc_dep = 0;
270                         // Rn, Rm ,Rd
271                         if (ARM_INSN_MATCH (DPIS, insn[0]) || ARM_INSN_MATCH (LRO, insn[0]) || 
272                                         ARM_INSN_MATCH (SRO, insn[0]))
273                         {
274
275                                 uregs = 0xb;
276                                 if ((ARM_INSN_REG_RN (insn[0]) == 15) || (ARM_INSN_REG_RM (insn[0]) == 15) || 
277                                                 (ARM_INSN_MATCH (SRO, insn[0]) && (ARM_INSN_REG_RD (insn[0]) == 15)))
278                                 {
279
280                                         DBPRINTF ("Unboostable insn %lx, DPIS/LRO/SRO\n", insn[0]);
281                                         pc_dep = 1;
282                                 }
283                         }
284                         // Rn ,Rd
285                         else if (ARM_INSN_MATCH (DPI, insn[0]) || ARM_INSN_MATCH (LIO, insn[0]) || 
286                                         ARM_INSN_MATCH (SIO, insn[0]))
287                         {
288
289                                 uregs = 0x3;
290                                 if ((ARM_INSN_REG_RN (insn[0]) == 15) || (ARM_INSN_MATCH (SIO, insn[0]) && 
291                                                         (ARM_INSN_REG_RD (insn[0]) == 15)))
292                                 {
293
294                                         pc_dep = 1;
295                                         DBPRINTF ("Unboostable insn %lx/%p/%d, DPI/LIO/SIO\n", insn[0], p, p->ainsn.boostable);
296                                 }
297                         }
298                         // Rn, Rm, Rs                                   
299                         else if (ARM_INSN_MATCH (DPRS, insn[0]))
300                         {
301
302                                 uregs = 0xd;
303                                 if ((ARM_INSN_REG_RN (insn[0]) == 15) || (ARM_INSN_REG_RM (insn[0]) == 15) || 
304                                                 (ARM_INSN_REG_RS (insn[0]) == 15))
305                                 {
306
307                                         pc_dep = 1;
308                                         DBPRINTF ("Unboostable insn %lx, DPRS\n", insn[0]);
309                                 }
310                         }
311                         // register list
312                         else if (ARM_INSN_MATCH (SM, insn[0]))
313                         {
314
315                                 uregs = 0x10;
316                                 if (ARM_INSN_REG_MR (insn[0], 15))
317                                 {
318
319                                         DBPRINTF ("Unboostable insn %lx, SM\n", insn[0]);
320                                         pc_dep = 1;
321                                 }
322                         }
323                         // check instructions that can write result to SP andu uses PC
324                         if (pc_dep  && (ARM_INSN_REG_RD (ainsn.insn[0]) == 13))
325                         {
326                                 static int count;
327                                 count++;
328                                 //printk ("insn writes result to SP and uses PC: %lx/%d\n", ainsn.insn[0], count);
329                                 free_insn_slot (&kprobe_insn_pages, NULL, p->ainsn.insn, 0);
330                                 ret = -EFAULT;
331                         }
332                         else {
333                                 if (uregs && pc_dep)
334                                 {
335                                         memcpy (insns, pc_dep_insn_execbuf, sizeof (insns));
336                                         if (prep_pc_dep_insn_execbuf (insns, insn[0], uregs) != 0)
337                                         {
338                                                 DBPRINTF ("failed to prepare exec buffer for insn %lx!", insn[0]);
339                                                 free_insn_slot (&kprobe_insn_pages, NULL, p->ainsn.insn, 0);
340                                                 return -EINVAL;
341                                         }
342                                         //insns[KPROBES_TRAMP_SS_BREAK_IDX] = BREAKPOINT_INSTRUCTION;
343                                         insns[6] = (kprobe_opcode_t) (p->addr + 2);
344                                 }
345                                 else
346                                 {
347                                         memcpy (insns, gen_insn_execbuf, sizeof (insns));
348                                         insns[KPROBES_TRAMP_INSN_IDX] = insn[0];
349                                 }                       
350                                 //insns[KPROBES_TRAMP_RET_BREAK_IDX] = UNDEF_INSTRUCTION;
351                                 insns[7] = (kprobe_opcode_t) (p->addr + 1);
352                                 DBPRINTF ("arch_prepare_kprobe: insn %lx", insn[0]);
353                                 DBPRINTF ("arch_prepare_kprobe: to %p - %lx %lx %lx %lx %lx %lx %lx %lx %lx", 
354                                                 p->ainsn.insn, insns[0], insns[1], insns[2], insns[3], insns[4], 
355                                                 insns[5], insns[6], insns[7], insns[8]);
356                                 memcpy (p->ainsn.insn, insns, sizeof(insns));
357                         }
358                 }
359                 else
360                 {
361                         free_insn_slot (&kprobe_insn_pages, NULL, p->ainsn.insn, 0);
362                 }
363         }
364
365         return ret;
366 }
367
368 static unsigned int arch_construct_brunch (unsigned int base, unsigned int addr, int link)
369 {
370         kprobe_opcode_t insn;
371         unsigned int bpi = (unsigned int) base - (unsigned int) addr - 8;
372         insn = bpi >> 2;
373         DBPRINTF ("base=%x addr=%x base-addr-8=%x\n", base, addr, bpi);
374         if (abs (insn & 0xffffff) > 0xffffff)
375         {
376                 DBPRINTF ("ERROR: kprobe address out of range\n");
377                 BUG ();
378         }
379         insn = insn & 0xffffff;
380         insn = insn | ((link != 0) ? 0xeb000000 : 0xea000000);
381         DBPRINTF ("insn=%lX\n", insn);
382         return (unsigned int) insn;
383 }
384
385 int arch_prepare_uprobe (struct kprobe *p, struct task_struct *task, int atomic)
386 {
387         int ret = 0;
388         kprobe_opcode_t insns[UPROBES_TRAMP_LEN];
389
390         int uregs, pc_dep;
391
392         if ((unsigned long) p->addr & 0x01)
393         {
394                 DBPRINTF ("Attempt to register kprobe at an unaligned address");
395                 ret = -EINVAL;
396         }
397
398         if (!ret)
399         {
400                 kprobe_opcode_t insn[MAX_INSN_SIZE];
401                 struct arch_specific_insn ainsn;
402
403                 if (!read_proc_vm_atomic (task, (unsigned long) p->addr, &insn, MAX_INSN_SIZE * sizeof(kprobe_opcode_t)))
404                         panic ("failed to read memory %p!\n", p->addr);
405                 ainsn.insn = insn;
406                 ret = arch_check_insn (&ainsn);
407                 if (!ret)
408                 {
409                         p->opcode = insn[0];
410                         p->ainsn.insn = get_insn_slot(task, atomic);
411                         if (!p->ainsn.insn)
412                                 return -ENOMEM;
413
414                         p->ainsn.boostable = 1;
415                         uregs = pc_dep = 0;
416                         // Rn, Rm ,Rd
417                         if (ARM_INSN_MATCH (DPIS, insn[0]) || ARM_INSN_MATCH (LRO, insn[0]) || 
418                                         ARM_INSN_MATCH (SRO, insn[0]))
419                         {
420
421                                 uregs = 0xb;
422                                 if ((ARM_INSN_REG_RN (insn[0]) == 15) || (ARM_INSN_REG_RM (insn[0]) == 15) || 
423                                                 (ARM_INSN_MATCH (SRO, insn[0]) && (ARM_INSN_REG_RD (insn[0]) == 15)))
424                                 {
425
426                                         DBPRINTF ("Unboostable insn %lx, DPIS/LRO/SRO\n", insn[0]);
427                                         pc_dep = 1;
428                                 }
429                         }
430                         // Rn ,Rd
431                         else if (ARM_INSN_MATCH (DPI, insn[0]) || ARM_INSN_MATCH (LIO, insn[0]) || 
432                                         ARM_INSN_MATCH (SIO, insn[0]))
433                         {
434
435                                 uregs = 0x3;
436                                 if ((ARM_INSN_REG_RN (insn[0]) == 15) || (ARM_INSN_MATCH (SIO, insn[0]) && 
437                                                         (ARM_INSN_REG_RD (insn[0]) == 15)))
438                                 {
439
440                                         pc_dep = 1;
441                                         DBPRINTF ("Unboostable insn %lx/%p/%d, DPI/LIO/SIO\n", insn[0], p, p->ainsn.boostable);
442                                 }
443                         }
444                         // Rn, Rm, Rs                                   
445                         else if (ARM_INSN_MATCH (DPRS, insn[0]))
446                         {
447
448                                 uregs = 0xd;
449                                 if ((ARM_INSN_REG_RN (insn[0]) == 15) || (ARM_INSN_REG_RM (insn[0]) == 15) || 
450                                                 (ARM_INSN_REG_RS (insn[0]) == 15))
451                                 {
452
453                                         pc_dep = 1;
454                                         DBPRINTF ("Unboostable insn %lx, DPRS\n", insn[0]);
455                                 }
456                         }
457                         // register list
458                         else if (ARM_INSN_MATCH (SM, insn[0]))
459                         {
460
461                                 uregs = 0x10;
462                                 if (ARM_INSN_REG_MR (insn[0], 15))
463                                 {
464
465                                         DBPRINTF ("Unboostable insn %lx, SM\n", insn[0]);
466                                         pc_dep = 1;
467                                 }
468                         }
469                         // check instructions that can write result to SP andu uses PC
470                         if (pc_dep  && (ARM_INSN_REG_RD (ainsn.insn[0]) == 13))
471                         {
472                                 static int count;
473                                 count++;
474                                 //printk ("insn writes result to SP and uses PC: %lx/%d\n", ainsn.insn[0], count);
475                                 free_insn_slot (&uprobe_insn_pages, task, p->ainsn.insn, 0);
476                                 ret = -EFAULT;
477                         }
478                         else {
479                                 if (uregs && pc_dep)
480                                 {
481                                         memcpy (insns, pc_dep_insn_execbuf, sizeof (insns));
482                                         if (prep_pc_dep_insn_execbuf (insns, insn[0], uregs) != 0)
483                                         {
484                                                 DBPRINTF ("failed to prepare exec buffer for insn %lx!", insn[0]);
485                                                 free_insn_slot (&uprobe_insn_pages, task, p->ainsn.insn, 0);
486                                                 return -EINVAL;
487                                         }
488                                         //insns[UPROBES_TRAMP_SS_BREAK_IDX] = BREAKPOINT_INSTRUCTION;
489                                         insns[6] = (kprobe_opcode_t) (p->addr + 2);
490                                 }
491                                 else
492                                 {
493                                         memcpy (insns, gen_insn_execbuf, sizeof (insns));
494                                         insns[UPROBES_TRAMP_INSN_IDX] = insn[0];
495                                 }                       
496                                 insns[UPROBES_TRAMP_RET_BREAK_IDX] = UNDEF_INSTRUCTION;
497                                 insns[7] = (kprobe_opcode_t) (p->addr + 1);
498                                 DBPRINTF ("arch_prepare_uprobe: to %p - %lx %lx %lx %lx %lx %lx %lx %lx %lx", 
499                                                 p->ainsn.insn, insns[0], insns[1], insns[2], insns[3], insns[4], 
500                                                 insns[5], insns[6], insns[7], insns[8]);
501                         }
502
503                         if (!write_proc_vm_atomic (task, (unsigned long) p->ainsn.insn, insns, sizeof (insns)))
504                         {
505                                 panic("failed to write memory %p!\n", p->ainsn.insn);
506                                 DBPRINTF ("failed to write insn slot to process memory: insn %p, addr %p, probe %p!", insn, p->ainsn.insn, p->addr);
507                                 /*printk ("failed to write insn slot to process memory: %p/%d insn %lx, addr %p, probe %p!\n", 
508                                   task, task->pid, insn, p->ainsn.insn, p->addr);*/
509                                 free_insn_slot (&uprobe_insn_pages, task, p->ainsn.insn, 0);
510                                 return -EINVAL;
511                         }
512                 }
513         }
514
515         return ret;
516
517 }
518
519 int arch_prepare_uretprobe (struct kretprobe *p, struct task_struct *task)
520 {
521         DBPRINTF("Warrning: arch_prepare_uretprobe is not implemented\n");
522         return 0;
523 }
524
525 void prepare_singlestep (struct kprobe *p, struct pt_regs *regs)
526 {
527         if(p->ss_addr)
528         {
529                 regs->uregs[15] = (unsigned long) p->ss_addr;
530                 p->ss_addr = NULL;
531         }
532         else
533                 regs->uregs[15] = (unsigned long) p->ainsn.insn;
534 }
535
536 void save_previous_kprobe (struct kprobe_ctlblk *kcb, struct kprobe *cur_p)
537 {
538         if (kcb->prev_kprobe.kp != NULL)
539         {
540                 DBPRINTF ("no space to save new probe[]: task = %d/%s", current->pid, current->comm);
541         }
542
543         kcb->prev_kprobe.kp = kprobe_running ();
544         kcb->prev_kprobe.status = kcb->kprobe_status;
545 }
546
547 void restore_previous_kprobe (struct kprobe_ctlblk *kcb)
548 {
549         __get_cpu_var (current_kprobe) = kcb->prev_kprobe.kp;
550         kcb->kprobe_status = kcb->prev_kprobe.status;
551         kcb->prev_kprobe.kp = NULL;
552         kcb->prev_kprobe.status = 0;
553 }
554
555 void set_current_kprobe (struct kprobe *p, struct pt_regs *regs, struct kprobe_ctlblk *kcb)
556 {
557         __get_cpu_var (current_kprobe) = p;
558         DBPRINTF ("set_current_kprobe: p=%p addr=%p\n", p, p->addr);
559 }
560
561
562 int kprobe_handler (struct pt_regs *regs)
563 {
564         struct kprobe *p = 0;
565         int ret = 0, pid = 0, retprobe = 0, reenter = 0;
566         kprobe_opcode_t *addr = NULL, *ssaddr = 0;
567         struct kprobe_ctlblk *kcb;
568
569         /* We're in an interrupt, but this is clear and BUG()-safe. */
570
571         addr = (kprobe_opcode_t *) (regs->uregs[15] - 4);
572         DBPRINTF ("KPROBE: regs->uregs[15] = 0x%lx addr = 0x%p\n", regs->uregs[15], addr);
573         regs->uregs[15] -= 4;
574         //DBPRINTF("regs->uregs[14] = 0x%lx\n", regs->uregs[14]);
575
576         preempt_disable ();
577
578         kcb = get_kprobe_ctlblk ();
579
580         if (user_mode (regs))
581         {
582                 //DBPRINTF("exception[%lu] from user mode %s/%u addr %p (%lx).", nCount, current->comm, current->pid, addr, regs->uregs[14]);
583                 pid = current->tgid;
584         }
585
586         /* Check we're not actually recursing */
587         if (kprobe_running ())
588         {
589                 DBPRINTF ("lock???");
590                 p = get_kprobe (addr, pid, current);
591                 if (p)
592                 {
593                         if(!pid && (addr == (kprobe_opcode_t *)kretprobe_trampoline)){
594                                 save_previous_kprobe (kcb, p);
595                                 kcb->kprobe_status = KPROBE_REENTER;
596                                 reenter = 1;
597                         }
598                         else {
599                                 /* We have reentered the kprobe_handler(), since
600                                  * another probe was hit while within the handler.
601                                  * We here save the original kprobes variables and
602                                  * just single step on the instruction of the new probe
603                                  * without calling any user handlers.
604                                  */
605                                 if(!p->ainsn.boostable){
606                                         save_previous_kprobe (kcb, p);
607                                         set_current_kprobe (p, regs, kcb);
608                                 }
609                                 kprobes_inc_nmissed_count (p);
610                                 prepare_singlestep (p, regs);
611                                 if(!p->ainsn.boostable)
612                                         kcb->kprobe_status = KPROBE_REENTER;
613                                 preempt_enable_no_resched ();
614                                 return 1;
615                         }
616                 }
617                 else
618                 {
619                         if(pid) { //we can reenter probe upon uretprobe exception   
620                                 DBPRINTF ("check for UNDEF_INSTRUCTION %p\n", addr);
621                                 // UNDEF_INSTRUCTION from user space
622                                 p = get_kprobe_by_insn_slot (addr-UPROBES_TRAMP_RET_BREAK_IDX, pid, current);
623                                 if (p) {
624                                         save_previous_kprobe (kcb, p);
625                                         kcb->kprobe_status = KPROBE_REENTER;
626                                         reenter = 1;
627                                         retprobe = 1;
628                                         DBPRINTF ("uretprobe %p\n", addr);
629                                 }
630                         }
631                         if(!p) {
632                                 p = __get_cpu_var (current_kprobe);
633                                 DBPRINTF ("kprobe_running !!! p = 0x%p p->break_handler = 0x%p", p, p->break_handler);
634                                 /*if (p->break_handler && p->break_handler(p, regs)) {
635                                   DBPRINTF("kprobe_running !!! goto ss");
636                                   goto ss_probe;
637                                   } */                  
638                                 DBPRINTF ("unknown uprobe at %p cur at %p/%p\n", addr, p->addr, p->ainsn.insn);
639                                 if(pid)
640                                         ssaddr = p->ainsn.insn + UPROBES_TRAMP_SS_BREAK_IDX;
641                                 else
642                                         ssaddr = p->ainsn.insn + KPROBES_TRAMP_SS_BREAK_IDX;                            
643                                 if (addr == ssaddr)
644                                 {
645                                         regs->uregs[15] = (unsigned long) (p->addr + 1);
646                                         DBPRINTF ("finish step at %p cur at %p/%p, redirect to %lx\n", addr, p->addr, p->ainsn.insn, regs->uregs[15]);
647                                         if (kcb->kprobe_status == KPROBE_REENTER) {
648                                                 restore_previous_kprobe (kcb);
649                                         }
650                                         else {
651                                                 reset_current_kprobe ();
652                                         }
653                                 }
654                                 DBPRINTF ("kprobe_running !!! goto no");
655                                 ret = 1;
656                                 /* If it's not ours, can't be delete race, (we hold lock). */
657                                 DBPRINTF ("no_kprobe");
658                                 goto no_kprobe;
659                         }
660                 }
661         }
662
663         //if(einsn != UNDEF_INSTRUCTION) {
664         DBPRINTF ("get_kprobe %p-%d", addr, pid);
665         if (!p)
666                 p = get_kprobe (addr, pid, current);
667         if (!p)
668         {
669                 if(pid) {
670                         DBPRINTF ("search UNDEF_INSTRUCTION %p\n", addr);
671                         // UNDEF_INSTRUCTION from user space
672                         p = get_kprobe_by_insn_slot (addr-UPROBES_TRAMP_RET_BREAK_IDX, pid, current);
673                         if (!p) {
674                                 /* Not one of ours: let kernel handle it */
675                                 DBPRINTF ("no_kprobe");
676                                 //printk("no_kprobe2 ret = %d\n", ret);
677                                 goto no_kprobe;
678                         }
679                         retprobe = 1;
680                         DBPRINTF ("uretprobe %p\n", addr);
681                 }
682                 else {
683                         /* Not one of ours: let kernel handle it */
684                         DBPRINTF ("no_kprobe");
685                         //printk("no_kprobe2 ret = %d\n", ret);
686                         goto no_kprobe;
687                 }
688         }
689
690         set_current_kprobe (p, regs, kcb);
691         if(!reenter)
692                 kcb->kprobe_status = KPROBE_HIT_ACTIVE;
693
694         if (retprobe)           //(einsn == UNDEF_INSTRUCTION)
695                 ret = trampoline_probe_handler (p, regs);
696         else if (p->pre_handler)
697         {
698                 ret = p->pre_handler (p, regs);
699                 if(!p->ainsn.boostable)
700                         kcb->kprobe_status = KPROBE_HIT_SS;
701                 else if(p->pre_handler != trampoline_probe_handler)
702                         reset_current_kprobe ();                        
703         }
704
705         if (ret)
706         {
707                 DBPRINTF ("p->pre_handler 1");
708                 /* handler has already set things up, so skip ss setup */
709                 return 1;
710         }
711         DBPRINTF ("p->pre_handler 0");
712
713 no_kprobe:
714         preempt_enable_no_resched ();
715         return ret;
716 }
717
718
719 void patch_suspended_task_ret_addr(struct task_struct *p, struct kretprobe *rp)
720 {
721         struct kretprobe_instance *ri = NULL;
722         struct hlist_node *node, *tmp; 
723         struct hlist_head *head;
724         unsigned long flags;
725         int found = 0;
726
727         spin_lock_irqsave (&kretprobe_lock, flags); 
728         head = kretprobe_inst_table_head (p);
729         hlist_for_each_entry_safe (ri, node, tmp, head, hlist){
730                 if ((ri->rp == rp) && (p == ri->task)){
731                         found = 1;
732                         break; 
733                 }
734         }
735         spin_unlock_irqrestore (&kretprobe_lock, flags); 
736
737 #ifndef task_thread_info
738 #define task_thread_info(task) (task)->thread_info
739 #endif // task_thread_info
740
741         if (found){
742                 // update PC
743                 if(thread_saved_pc(p) != (unsigned long)&kretprobe_trampoline){
744                         ri->ret_addr = (kprobe_opcode_t *)thread_saved_pc(p);
745                         task_thread_info(p)->cpu_context.pc = (unsigned long) &kretprobe_trampoline;
746                 }
747                 return; 
748         }
749
750         if ((ri = get_free_rp_inst(rp)) != NULL)
751         {
752                 ri->rp = rp; 
753                 ri->rp2 = NULL; 
754                 ri->task = p;
755                 ri->ret_addr = (kprobe_opcode_t *)thread_saved_pc(p);
756                 task_thread_info(p)->cpu_context.pc = (unsigned long) &kretprobe_trampoline;
757                 add_rp_inst (ri);
758                 //              printk("change2 saved pc %p->%p for %d/%d/%p\n", ri->ret_addr, &kretprobe_trampoline, p->tgid, p->pid, p);
759         }
760         else{
761                 printk("no ri for %d\n", p->pid);
762                 BUG();                          
763         }
764 }
765
766 int setjmp_pre_handler (struct kprobe *p, struct pt_regs *regs)
767 {
768         struct jprobe *jp = container_of (p, struct jprobe, kp);
769         kprobe_pre_entry_handler_t pre_entry;
770         entry_point_t entry;
771
772 # ifdef REENTER
773         p = __get_cpu_var (current_kprobe);
774 # endif
775
776         DBPRINTF ("pjp = 0x%p jp->entry = 0x%p", jp, jp->entry);
777         entry = (entry_point_t) jp->entry;
778         pre_entry = (kprobe_pre_entry_handler_t) jp->pre_entry;
779         //if(!entry)
780         //      DIE("entry NULL", regs)
781         DBPRINTF ("entry = 0x%p jp->entry = 0x%p", entry, jp->entry);
782
783         //call handler for all kernel probes and user space ones which belong to current tgid
784         if (!p->tgid || (p->tgid == current->tgid))
785         {               
786                 if(!p->tgid && ((unsigned int)p->addr == sched_addr) && sched_rp){
787                         struct task_struct *p, *g;
788                         rcu_read_lock();
789                         //swapper task
790                         if(current != &init_task)
791                                 patch_suspended_task_ret_addr(&init_task, sched_rp);
792                         // other tasks
793                         do_each_thread(g, p){
794                                 if(p == current)
795                                         continue;                                                                       
796                                 patch_suspended_task_ret_addr(p, sched_rp);
797                         } while_each_thread(g, p);
798                         rcu_read_unlock();
799                 }
800                 if (pre_entry)
801                         p->ss_addr = (void *)pre_entry (jp->priv_arg, regs);
802                 if (entry){
803                         entry (regs->ARM_r0, regs->ARM_r1, regs->ARM_r2, regs->ARM_r3, regs->ARM_r4, regs->ARM_r5);
804                 }
805                 else {
806                         if (p->tgid)
807                                 arch_uprobe_return ();
808                         else
809                                 jprobe_return ();
810                 }
811         }
812         else if (p->tgid)
813                 arch_uprobe_return ();
814
815         prepare_singlestep (p, regs);
816
817         return 1;       
818 }
819
820 void jprobe_return (void)
821 {
822         preempt_enable_no_resched();
823 }
824
825 void arch_uprobe_return (void)
826 {
827         preempt_enable_no_resched();
828 }
829
830 int longjmp_break_handler (struct kprobe *p, struct pt_regs *regs)
831 {
832 # ifndef REENTER
833         //kprobe_opcode_t insn = BREAKPOINT_INSTRUCTION;
834         kprobe_opcode_t insns[2];
835
836         if (p->pid)
837         {
838                 insns[0] = BREAKPOINT_INSTRUCTION;
839                 insns[1] = p->opcode;
840                 //p->opcode = *p->addr;
841                 if (read_proc_vm_atomic (current, (unsigned long) (p->addr), &(p->opcode), sizeof (p->opcode)) < sizeof (p->opcode))
842                 {
843                         printk ("ERROR[%lu]: failed to read vm of proc %s/%u addr %p.", nCount, current->comm, current->pid, p->addr);
844                         return -1;
845                 }
846                 //*p->addr = BREAKPOINT_INSTRUCTION;
847                 //*(p->addr+1) = p->opcode;             
848                 if (write_proc_vm_atomic (current, (unsigned long) (p->addr), insns, sizeof (insns)) < sizeof (insns))
849                 {
850                         printk ("ERROR[%lu]: failed to write vm of proc %s/%u addr %p.", nCount, current->comm, current->pid, p->addr);
851                         return -1;
852                 }
853         }
854         else
855         {
856                 DBPRINTF ("p->opcode = 0x%lx *p->addr = 0x%lx p->addr = 0x%p\n", p->opcode, *p->addr, p->addr);
857                 *(p->addr + 1) = p->opcode;
858                 p->opcode = *p->addr;
859                 *p->addr = BREAKPOINT_INSTRUCTION;
860                 flush_icache_range ((unsigned int) p->addr, (unsigned int) (((unsigned int) p->addr) + (sizeof (kprobe_opcode_t) * 2)));
861         }
862
863         reset_current_kprobe ();
864
865 #endif //REENTER 
866
867         return 0;
868 }
869
870
871 void arch_arm_kprobe (struct kprobe *p)
872 {
873         *p->addr = BREAKPOINT_INSTRUCTION;
874         flush_icache_range ((unsigned long) p->addr, (unsigned long) p->addr + sizeof (kprobe_opcode_t));
875 }
876
877 void arch_disarm_kprobe (struct kprobe *p)
878 {
879         *p->addr = p->opcode;
880         flush_icache_range ((unsigned long) p->addr, (unsigned long) p->addr + sizeof (kprobe_opcode_t));
881 }
882
883
884 int trampoline_probe_handler (struct kprobe *p, struct pt_regs *regs)
885 {
886
887         struct kretprobe_instance *ri = NULL; 
888         struct hlist_head *head, empty_rp; 
889         struct hlist_node *node, *tmp; 
890         unsigned long flags, orig_ret_address = 0;
891         unsigned long trampoline_address = (unsigned long) &kretprobe_trampoline;
892         struct kretprobe *crp = NULL; 
893         struct kprobe_ctlblk *kcb = get_kprobe_ctlblk ();
894
895         DBPRINTF ("start");
896
897         if (p && p->tgid){
898                 // in case of user space retprobe trampoline is at the Nth instruction of US tramp 
899                 trampoline_address = (unsigned long)(p->ainsn.insn + UPROBES_TRAMP_RET_BREAK_IDX);
900         }
901
902         INIT_HLIST_HEAD (&empty_rp); 
903         spin_lock_irqsave (&kretprobe_lock, flags); 
904         head = kretprobe_inst_table_head (current);
905
906         /*
907          * It is possible to have multiple instances associated with a given
908          * task either because an multiple functions in the call path
909          * have a return probe installed on them, and/or more then one 
910          * return probe was registered for a target function.
911          *
912          * We can handle this because:
913          *     - instances are always inserted at the head of the list
914          *     - when multiple return probes are registered for the same
915          *       function, the first instance's ret_addr will point to the
916          *       real return address, and all the rest will point to
917          *       kretprobe_trampoline
918          */
919         hlist_for_each_entry_safe (ri, node, tmp, head, hlist)
920         {
921                 if (ri->task != current)
922                         /* another task is sharing our hash bucket */
923                         continue; 
924                 if (ri->rp && ri->rp->handler){
925                         ri->rp->handler (ri, regs, ri->rp->priv_arg);
926                 }
927
928                 orig_ret_address = (unsigned long) ri->ret_addr; 
929                 recycle_rp_inst (ri, &empty_rp); 
930                 if (orig_ret_address != trampoline_address)
931                         /*
932                          * This is the real return address. Any other
933                          * instances associated with this task are for
934                          * other calls deeper on the call stack
935                          */
936                         break;
937         }
938         kretprobe_assert (ri, orig_ret_address, trampoline_address);
939         //BUG_ON(!orig_ret_address || (orig_ret_address == trampoline_address));
940         if (trampoline_address != (unsigned long) &kretprobe_trampoline){
941                 if (ri->rp2) BUG_ON (ri->rp2->kp.tgid == 0);
942                 if (ri->rp) BUG_ON (ri->rp->kp.tgid == 0);
943                 else if (ri->rp2) BUG_ON (ri->rp2->kp.tgid == 0);
944         }
945         if ((ri->rp && ri->rp->kp.tgid) || (ri->rp2 && ri->rp2->kp.tgid)) 
946                 BUG_ON (trampoline_address == (unsigned long) &kretprobe_trampoline);
947
948         regs->uregs[14] = orig_ret_address; 
949         DBPRINTF ("regs->uregs[14] = 0x%lx\n", regs->uregs[14]);
950         DBPRINTF ("regs->uregs[15] = 0x%lx\n", regs->uregs[15]); 
951         if (trampoline_address != (unsigned long) &kretprobe_trampoline) 
952                 regs->uregs[15] = orig_ret_address;
953         else
954                 regs->uregs[15] += 4;
955         DBPRINTF ("regs->uregs[15] = 0x%lx\n", regs->uregs[15]);
956
957         if(p){ // ARM, MIPS, X86 user space
958                 if (kcb->kprobe_status == KPROBE_REENTER)
959                         restore_previous_kprobe (kcb);
960                 else
961                         reset_current_kprobe ();
962
963                 //TODO: test - enter function, delete us retprobe, exit function 
964                 // for user space retprobes only - deferred deletion
965                 if (trampoline_address != (unsigned long) &kretprobe_trampoline)
966                 {
967                         // if we are not at the end of the list and current retprobe should be disarmed 
968                         if (node && ri->rp2)
969                         {
970                                 crp = ri->rp2;
971                                 /*sprintf(die_msg, "deferred disarm p->addr = %p [%lx %lx %lx]\n", 
972                                   crp->kp.addr, *kaddrs[0], *kaddrs[1], *kaddrs[2]);
973                                   DIE(die_msg, regs); */
974                                 // look for other instances for the same retprobe
975                                 hlist_for_each_entry_continue (ri, node, hlist)
976                                 {
977                                         if (ri->task != current) 
978                                                 continue;       /* another task is sharing our hash bucket */
979                                         if (ri->rp2 == crp)     //if instance belong to the same retprobe
980                                                 break;
981                                 }
982                                 if (!node)
983                                 {       // if there are no more instances for this retprobe
984                                         // delete retprobe
985                                         DBPRINTF ("defered retprobe deletion p->addr = %p", crp->kp.addr);
986                                         unregister_uprobe (&crp->kp, current, 1);
987                                         kfree (crp);
988                                 }
989                         }
990                 }
991         }
992
993         spin_unlock_irqrestore (&kretprobe_lock, flags); 
994         hlist_for_each_entry_safe (ri, node, tmp, &empty_rp, hlist)
995         {
996                 hlist_del (&ri->hlist); 
997                 kfree (ri);
998         }
999
1000         preempt_enable_no_resched ();
1001         /*
1002          * By returning a non-zero value, we are telling
1003          * kprobe_handler() that we don't want the post_handler
1004          * to run (and have re-enabled preemption)
1005          */
1006         return 1;
1007 }
1008
1009 void  __arch_prepare_kretprobe (struct kretprobe *rp, struct pt_regs *regs)
1010 {
1011
1012         struct kretprobe_instance *ri;
1013
1014         DBPRINTF ("start\n");
1015         //TODO: test - remove retprobe after func entry but before its exit
1016         if ((ri = get_free_rp_inst (rp)) != NULL)
1017         {
1018                 ri->rp = rp; 
1019                 ri->rp2 = NULL; 
1020                 ri->task = current;
1021                 ri->ret_addr = (kprobe_opcode_t *) regs->uregs[14];
1022                 if (rp->kp.tgid)
1023                         regs->uregs[14] = (unsigned long) (rp->kp.ainsn.insn + UPROBES_TRAMP_RET_BREAK_IDX);
1024                 else    /* Replace the return addr with trampoline addr */
1025                         regs->uregs[14] = (unsigned long) &kretprobe_trampoline; 
1026                 DBPRINTF ("ret addr set to %p->%lx\n", ri->ret_addr, regs->uregs[14]);
1027                 add_rp_inst (ri);
1028         }
1029         else {
1030                 DBPRINTF ("WARNING: missed retprobe %p\n", rp->kp.addr);
1031                 rp->nmissed++;
1032         }
1033
1034 }
1035
1036 int asm_init_module_dependencies()
1037 {
1038         //No module dependencies 
1039         return 0;
1040 }
1041
1042 int __init arch_init_kprobes (void)
1043 {
1044
1045         unsigned int do_bp_handler; 
1046         unsigned int kprobe_handler_addr;
1047
1048         unsigned int insns_num = 0;
1049         unsigned int code_size = 0;
1050
1051         int ret = 0;
1052
1053         if (arch_init_module_dependencies())
1054         {
1055                 DBPRINTF ("Unable to init module dependencies\n"); 
1056                 return -1;
1057         }
1058
1059         do_bp_handler = (unsigned int) kallsyms_search ("do_undefinstr");
1060
1061         kprobe_handler_addr = (unsigned int) &kprobe_handler;
1062         insns_num = sizeof (arr_traps_template) / sizeof (arr_traps_template[0]);
1063         code_size = insns_num * sizeof (unsigned int); 
1064         DBPRINTF ("insns_num = %d\n", insns_num);
1065         // Save original code
1066         arr_traps_original = kmalloc (code_size, GFP_KERNEL);
1067         if (!arr_traps_original)
1068         {
1069                 DBPRINTF ("Unable to allocate space for original code of <do_bp>!\n"); 
1070                 return -1;
1071         }
1072         memcpy (arr_traps_original, (void *) do_bp_handler, code_size);
1073
1074         arr_traps_template[NOTIFIER_CALL_CHAIN_INDEX] = arch_construct_brunch ((unsigned int)kprobe_handler, do_bp_handler + NOTIFIER_CALL_CHAIN_INDEX * 4, 1);
1075
1076         // Insert new code
1077         memcpy ((void *) do_bp_handler, arr_traps_template, code_size); 
1078         flush_icache_range (do_bp_handler, do_bp_handler + code_size); 
1079         if((ret = register_kprobe (&trampoline_p, 0)) != 0){
1080                 //unregister_jprobe(&do_exit_p, 0);
1081                 return ret;
1082         }
1083
1084         return ret;     
1085 }
1086
1087 void __exit arch_exit_kprobes (void)
1088 {
1089         unsigned int do_bp_handler;
1090
1091         unsigned int insns_num = 0;
1092         unsigned int code_size = 0;
1093
1094         // Get instruction address  
1095         do_bp_handler = (unsigned int) kallsyms_search ("do_undefinstr");
1096
1097         //unregister_jprobe(&do_exit_p, 0);
1098
1099         // Replace back the original code
1100
1101         insns_num = sizeof (arr_traps_template) / sizeof (arr_traps_template[0]);
1102         code_size = insns_num * sizeof (unsigned int); 
1103         memcpy ((void *) do_bp_handler, arr_traps_original, code_size); 
1104         flush_icache_range (do_bp_handler, do_bp_handler + code_size); 
1105         kfree (arr_traps_original); 
1106         arr_traps_original = NULL;
1107 }
1108
1109
1110 EXPORT_SYMBOL_GPL (arch_uprobe_return);
1111 EXPORT_SYMBOL_GPL (arch_exit_kprobes);
1112