52ced9af7f49557f58c4b5687b794cd7d6845354
[kernel/swap-modules.git] / kprobe / arch / asm-mips / dbi_kprobes.c
1 /*
2  *  Dynamic Binary Instrumentation Module based on KProbes
3  *  modules/kprobe/arch/asm-mips/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/ARM/MIPS for both user-space and kernel space.
24  * 2010         Ekaterina Gorelkina <e.gorelkina@samsung.com>: redesign module for separating core and arch parts
25  * 2012         Stanislav Andreev <s.andreev@samsung.com>: added time debug profiling support; BUG() message fix
26  */
27
28 #include "dbi_kprobes.h"
29 #include "../dbi_kprobes.h"
30 #include "../../dbi_kprobes.h"
31
32 #include "../../dbi_kdebug.h"
33 #include "../../dbi_insn_slots.h"
34 #include "../../dbi_kprobes_deps.h"
35 #include "../../dbi_uprobes.h"
36
37 #ifdef OVERHEAD_DEBUG
38 #include <linux/time.h>
39 #endif
40
41 #define SUPRESS_BUG_MESSAGES
42
43 unsigned int *arr_traps_original;
44
45 extern unsigned long sched_addr;
46
47 extern struct kprobe * per_cpu__current_kprobe;
48 extern spinlock_t kretprobe_lock;
49 extern struct kretprobe *sched_rp;
50
51 extern struct hlist_head kprobe_insn_pages;
52 extern struct hlist_head uprobe_insn_pages;
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 extern unsigned long (*kallsyms_search) (const char *name);
59
60 #ifdef OVERHEAD_DEBUG
61 unsigned long swap_sum_time = 0;
62 unsigned long swap_sum_hit = 0;
63 EXPORT_SYMBOL_GPL (swap_sum_time);
64 EXPORT_SYMBOL_GPL (swap_sum_hit);
65 #endif
66
67 unsigned int arr_traps_template[] = {  0x3c010000,   // lui  a1       [0]
68         0x24210000,   // addiu a1, a1  [1]
69         0x00200008,   // jr a1         [2]
70         0x00000000,   // nop
71         0xffffffff    // end
72 };
73
74 /*
75  * Function return probe trampoline:
76  *      - init_kprobes() establishes a probepoint here
77  *      - When the probed function returns, this probe
78  *              causes the handlers to fire
79  */
80 void kretprobe_trampoline_holder (void)
81 {
82         asm volatile (".global kretprobe_trampoline\n"
83                         "kretprobe_trampoline:\n"
84                         "nop\n"
85                         "nop\n");
86 }
87
88
89 struct kprobe trampoline_p =
90 {
91         .addr = (kprobe_opcode_t *) & kretprobe_trampoline,
92         .pre_handler = trampoline_probe_handler
93 };
94
95 void gen_insn_execbuf(void);
96
97 void gen_insn_execbuf_holder (void)
98 {
99         asm volatile (".global gen_insn_execbuf\n"
100                         "gen_insn_execbuf:\n"
101                         "nop\n"                 // original instruction
102                         "nop\n"                 //ssbreak
103                         "nop\n");               //retbreak
104 }
105
106
107 int arch_check_insn (struct arch_specific_insn *ainsn)
108 {
109         int ret = 0;
110
111         switch (MIPS_INSN_OPCODE (ainsn->insn[0]))
112         {
113                 case MIPS_BEQ_OPCODE:   //B, BEQ
114                 case MIPS_BEQL_OPCODE:  //BEQL
115                 case MIPS_BNE_OPCODE:   //BNE
116                 case MIPS_BNEL_OPCODE:  //BNEL
117                 case MIPS_BGTZ_OPCODE:  //BGTZ
118                 case MIPS_BGTZL_OPCODE: //BGTZL
119                 case MIPS_BLEZ_OPCODE:  //BLEZ
120                 case MIPS_BLEZL_OPCODE: //BLEZL
121                 case MIPS_J_OPCODE:     //J
122                 case MIPS_JAL_OPCODE:   //JAL
123                         DBPRINTF ("arch_check_insn: opcode");
124                         ret = -EFAULT;
125                         break;
126                 case MIPS_REGIMM_OPCODE:
127                         //BAL, BGEZ, BGEZAL, BGEZALL, BGEZL, BLTZ, BLTZAL, BLTZALL, BLTZL
128                         switch (MIPS_INSN_RT (ainsn->insn[0]))
129                         {
130                                 case MIPS_BLTZ_RT:
131                                 case MIPS_BGEZ_RT:
132                                 case MIPS_BLTZL_RT:
133                                 case MIPS_BGEZL_RT:
134                                 case MIPS_BLTZAL_RT:
135                                 case MIPS_BGEZAL_RT:
136                                 case MIPS_BLTZALL_RT:
137                                 case MIPS_BGEZALL_RT:
138                                         DBPRINTF ("arch_check_insn: REGIMM opcode\n");
139                                         ret = -EFAULT;
140                                         break;
141                         }
142                         break;
143                         //BC1F, BC1FL, BC1T, BC1TL
144                 case MIPS_COP1_OPCODE:
145                         //BC2F, BC2FL, BC2T, BC2TL
146                 case MIPS_COP2_OPCODE:
147                         if (MIPS_INSN_RS (ainsn->insn[0]) == MIPS_BC_RS)
148                         {
149                                 DBPRINTF ("arch_check_insn: COP1 opcode\n");
150                                 ret = -EFAULT;
151                         }
152                         break;
153                 case MIPS_SPECIAL_OPCODE:
154                         //BREAK, JALR, JALR.HB, JR, JR.HB
155                         switch (MIPS_INSN_FUNC (ainsn->insn[0]))
156                         {
157                                 case MIPS_JR_FUNC:
158                                 case MIPS_JALR_FUNC:
159                                 case MIPS_BREAK_FUNC:
160                                 case MIPS_SYSCALL_FUNC:
161                                         DBPRINTF ("arch_check_insn: SPECIAL opcode\n");
162                                         ret = -EFAULT;
163                                         break;
164                         }
165                         break;
166         }
167         return ret;
168 }
169
170 int arch_prepare_kretprobe (struct kretprobe *p)
171 {
172         DBPRINTF("Warrning: arch_prepare_kretprobe is not implemented\n");
173         return 0;
174 }
175
176 int arch_prepare_kprobe (struct kprobe *p)
177 {
178         kprobe_opcode_t insns[KPROBES_TRAMP_LEN];
179
180         int ret = 0;
181         if (!ret)
182         {
183                 kprobe_opcode_t insn[MAX_INSN_SIZE];
184                 struct arch_specific_insn ainsn;
185                 /* insn: must be on special executable page on i386. */
186                 p->ainsn.insn = get_insn_slot (NULL, 0);
187                 if (!p->ainsn.insn)
188                         return -ENOMEM;
189                 memcpy (insn, p->addr, MAX_INSN_SIZE * sizeof (kprobe_opcode_t));
190                 ainsn.insn = insn;
191                 ret = arch_check_insn (&ainsn);
192                 if (!ret)
193                 {
194                         p->opcode = *p->addr;
195                         p->ainsn.boostable = 0;
196                         memcpy (insns, gen_insn_execbuf, sizeof (insns));
197                         insns[KPROBES_TRAMP_INSN_IDX] = insn[0];
198                         insns[KPROBES_TRAMP_SS_BREAK_IDX] = BREAKPOINT_INSTRUCTION;
199                         insns[KPROBES_TRAMP_RET_BREAK_IDX] = UNDEF_INSTRUCTION;
200                         DBPRINTF ("arch_prepare_kprobe: insn %lx", insn[0]);
201                         DBPRINTF ("arch_prepare_kprobe: to %p - %lx %lx %lx",
202                                         p->ainsn.insn, insns[0], insns[1], insns[2]);
203                         memcpy (p->ainsn.insn, insns, sizeof(insns));
204                 }
205                 else
206                 {
207                         free_insn_slot(&kprobe_insn_pages, NULL, p->ainsn.insn);
208                 }
209         }
210
211         return ret;
212 }
213
214 int arch_prepare_uprobe (struct kprobe *p, struct task_struct *task, int atomic)
215 {
216         int ret = 0;
217         kprobe_opcode_t insns[UPROBES_TRAMP_LEN];
218
219         if ((unsigned long) p->addr & 0x01)
220         {
221                 DBPRINTF ("Attempt to register kprobe at an unaligned address");
222                 ret = -EINVAL;
223         }
224
225         if (!ret)
226         {
227                 kprobe_opcode_t insn[MAX_INSN_SIZE];
228                 struct arch_specific_insn ainsn;
229
230                 if (!read_proc_vm_atomic (task, (unsigned long) p->addr, &insn, MAX_INSN_SIZE * sizeof(kprobe_opcode_t)))
231                         panic ("failed to read memory %p!\n", p->addr);
232                 ainsn.insn = insn;
233                 ret = arch_check_insn (&ainsn);
234                 if (!ret)
235                 {
236                         p->opcode = insn[0];
237                         p->ainsn.insn = get_insn_slot(task, atomic);
238                         if (!p->ainsn.insn)
239                                 return -ENOMEM;
240                         p->ainsn.boostable = 0;
241                         memcpy (insns, gen_insn_execbuf, sizeof (insns));
242                         insns[UPROBES_TRAMP_INSN_IDX] = insn[0];
243                         insns[UPROBES_TRAMP_SS_BREAK_IDX] = BREAKPOINT_INSTRUCTION;
244                         insns[UPROBES_TRAMP_RET_BREAK_IDX] = UNDEF_INSTRUCTION;
245                         DBPRINTF ("arch_prepare_uprobe: insn %lx", insn[0]);
246                         DBPRINTF ("arch_prepare_uprobe: to %p - %lx %lx %lx",
247                                         p->ainsn.insn, insns[0], insns[1], insns[2]);
248
249                         if (!write_proc_vm_atomic (task, (unsigned long) p->ainsn.insn, insns, sizeof (insns)))
250                         {
251                                 panic("failed to write memory %p!\n", p->ainsn.insn);
252                                 DBPRINTF ("failed to write insn slot to process memory: insn %p, addr %p, probe %p!", insn, p->ainsn.insn, p->addr);
253                                 /*printk ("failed to write insn slot to process memory: %p/%d insn %lx, addr %p, probe %p!\n",
254                                   task, task->pid, insn, p->ainsn.insn, p->addr);*/
255                                 free_insn_slot(&uprobe_insn_pages, task, p->ainsn.insn);
256                                 return -EINVAL;
257                         }
258                 }
259         }
260
261         return ret;
262 }
263
264 int arch_prepare_uretprobe (struct kretprobe *p, struct task_struct *task)
265 {
266         DBPRINTF("Warrning: arch_prepare_uretprobe is not implemented\n");
267         return 0;
268 }
269
270 void prepare_singlestep (struct kprobe *p, struct pt_regs *regs)
271 {
272         if(p->ss_addr)
273         {
274                 regs->cp0_epc = (unsigned long) p->ss_addr;
275                 p->ss_addr = NULL;
276         }
277         else
278                 regs->cp0_epc = (unsigned long) p->ainsn.insn;
279 }
280
281
282 void save_previous_kprobe (struct kprobe_ctlblk *kcb, struct kprobe *cur_p)
283 {
284         if (kcb->prev_kprobe.kp != NULL)
285         {
286                 panic ("no space to save new probe[]: task = %d/%s, prev %d/%p, current %d/%p, new %d/%p,",
287                                 current->pid, current->comm, kcb->prev_kprobe.kp->tgid, kcb->prev_kprobe.kp->addr,
288                                 kprobe_running()->tgid, kprobe_running()->addr, cur_p->tgid, cur_p->addr);
289         }
290
291         kcb->prev_kprobe.kp = kprobe_running ();
292         kcb->prev_kprobe.status = kcb->kprobe_status;
293 }
294
295 void restore_previous_kprobe (struct kprobe_ctlblk *kcb)
296 {
297         __get_cpu_var (current_kprobe) = kcb->prev_kprobe.kp;
298         kcb->kprobe_status = kcb->prev_kprobe.status;
299         kcb->prev_kprobe.kp = NULL;
300         kcb->prev_kprobe.status = 0;
301 }
302
303 void set_current_kprobe (struct kprobe *p, struct pt_regs *regs, struct kprobe_ctlblk *kcb)
304 {
305         __get_cpu_var (current_kprobe) = p;
306         DBPRINTF ("set_current_kprobe[]: p=%p addr=%p\n", p, p->addr);
307 }
308
309 int kprobe_handler (struct pt_regs *regs)
310 {
311         struct kprobe *p = 0;
312         int ret = 0, pid = 0, retprobe = 0, reenter = 0;
313         kprobe_opcode_t *addr = NULL, *ssaddr = 0;
314         struct kprobe_ctlblk *kcb;
315 #ifdef OVERHEAD_DEBUG
316         struct timeval swap_tv1;
317         struct timeval swap_tv2;
318 #endif
319 #ifdef SUPRESS_BUG_MESSAGES
320         int swap_oops_in_progress;
321 #endif
322
323         /* We're in an interrupt, but this is clear and BUG()-safe. */
324
325         addr = (kprobe_opcode_t *) regs->cp0_epc;
326         DBPRINTF ("regs->regs[ 31 ] = 0x%lx\n", regs->regs[31]);
327
328 #ifdef SUPRESS_BUG_MESSAGES
329         // oops_in_progress used to avoid BUG() messages that slow down kprobe_handler() execution
330         swap_oops_in_progress = oops_in_progress;
331         oops_in_progress = 1;
332 #endif
333 #ifdef OVERHEAD_DEBUG
334 #define USEC_IN_SEC_NUM                         1000000
335         do_gettimeofday(&swap_tv1);
336 #endif
337         preempt_disable ();
338
339         kcb = get_kprobe_ctlblk ();
340
341         if (user_mode (regs))
342         {
343                 //DBPRINTF("exception[%lu] from user mode %s/%u addr %p (%lx).", nCount, current->comm, current->pid, addr, regs->uregs[14]);
344                 pid = current->tgid;
345         }
346
347         /* Check we're not actually recursing */
348         if (kprobe_running ())
349         {
350                 DBPRINTF ("lock???");
351                 p = get_kprobe(addr, pid);
352                 if (p)
353                 {
354                         if(!pid && (addr == (kprobe_opcode_t *)kretprobe_trampoline)){
355                                 save_previous_kprobe (kcb, p);
356                                 kcb->kprobe_status = KPROBE_REENTER;
357                                 reenter = 1;
358                         }
359                         else {
360                                 /* We have reentered the kprobe_handler(), since
361                                  * another probe was hit while within the handler.
362                                  * We here save the original kprobes variables and
363                                  * just single step on the instruction of the new probe
364                                  * without calling any user handlers.
365                                  */
366                                 if(!p->ainsn.boostable){
367                                         save_previous_kprobe (kcb, p);
368                                         set_current_kprobe (p, regs, kcb);
369                                 }
370                                 kprobes_inc_nmissed_count (p);
371                                 prepare_singlestep (p, regs);
372                                 if(!p->ainsn.boostable)
373                                         kcb->kprobe_status = KPROBE_REENTER;
374                                 preempt_enable_no_resched ();
375 #ifdef OVERHEAD_DEBUG
376                                 do_gettimeofday(&swap_tv2);
377                                 swap_sum_hit++;
378                                 swap_sum_time += ((swap_tv2.tv_sec - swap_tv1.tv_sec) * USEC_IN_SEC_NUM +
379                                         (swap_tv2.tv_usec - swap_tv1.tv_usec));
380 #endif
381 #ifdef SUPRESS_BUG_MESSAGES
382                                 oops_in_progress = swap_oops_in_progress;
383 #endif
384                                 return 1;
385                         }
386                 }
387                 else
388                 {
389                         if(pid) { //we can reenter probe upon uretprobe exception
390                                 DBPRINTF ("check for UNDEF_INSTRUCTION %p\n", addr);
391                                 // UNDEF_INSTRUCTION from user space
392                                 p = get_kprobe_by_insn_slot (addr-UPROBES_TRAMP_RET_BREAK_IDX, pid, current);
393                                 if (p) {
394                                         save_previous_kprobe (kcb, p);
395                                         kcb->kprobe_status = KPROBE_REENTER;
396                                         reenter = 1;
397                                         retprobe = 1;
398                                         DBPRINTF ("uretprobe %p\n", addr);
399                                 }
400                         }
401                         if(!p) {
402                                 p = __get_cpu_var (current_kprobe);
403                                 DBPRINTF ("kprobe_running !!! p = 0x%p p->break_handler = 0x%p", p, p->break_handler);
404                                 /*if (p->break_handler && p->break_handler(p, regs)) {
405                                   DBPRINTF("kprobe_running !!! goto ss");
406                                   goto ss_probe;
407                                   } */
408                                 DBPRINTF ("unknown uprobe at %p cur at %p/%p\n", addr, p->addr, p->ainsn.insn);
409                                 if(pid)
410                                         ssaddr = p->ainsn.insn + UPROBES_TRAMP_SS_BREAK_IDX;
411                                 else
412                                         ssaddr = p->ainsn.insn + KPROBES_TRAMP_SS_BREAK_IDX;
413                                 if (addr == ssaddr)
414                                 {
415                                         regs->cp0_epc = (unsigned long) (p->addr + 1);
416                                         DBPRINTF ("finish step at %p cur at %p/%p, redirect to %lx\n", addr, p->addr, p->ainsn.insn, regs->cp0_epc);
417
418                                         if (kcb->kprobe_status == KPROBE_REENTER) {
419                                                 restore_previous_kprobe (kcb);
420                                         }
421                                         else {
422                                                 reset_current_kprobe ();
423                                         }
424                                 }
425                                 DBPRINTF ("kprobe_running !!! goto no");
426                                 ret = 1;
427                                 /* If it's not ours, can't be delete race, (we hold lock). */
428                                 DBPRINTF ("no_kprobe");
429                                 goto no_kprobe;
430                         }
431                 }
432         }
433
434         //if(einsn != UNDEF_INSTRUCTION) {
435         DBPRINTF ("get_kprobe %p-%d", addr, pid);
436         if (!p)
437                 p = get_kprobe(addr, pid);
438         if (!p)
439         {
440                 if(pid) {
441                         DBPRINTF ("search UNDEF_INSTRUCTION %p\n", addr);
442                         // UNDEF_INSTRUCTION from user space
443                         p = get_kprobe_by_insn_slot (addr-UPROBES_TRAMP_RET_BREAK_IDX, pid, current);
444                         if (!p) {
445                                 /* Not one of ours: let kernel handle it */
446                                 DBPRINTF ("no_kprobe");
447                                 //printk("no_kprobe2 ret = %d\n", ret);
448                                 goto no_kprobe;
449                         }
450                         retprobe = 1;
451                         DBPRINTF ("uretprobe %p\n", addr);
452                 }
453                 else {
454                         /* Not one of ours: let kernel handle it */
455                         DBPRINTF ("no_kprobe");
456                         //printk("no_kprobe2 ret = %d\n", ret);
457                         goto no_kprobe;
458                 }
459         }
460
461         set_current_kprobe (p, regs, kcb);
462         if(!reenter)
463                 kcb->kprobe_status = KPROBE_HIT_ACTIVE;
464
465         if (retprobe)           //(einsn == UNDEF_INSTRUCTION)
466                 ret = trampoline_probe_handler (p, regs);
467         else if (p->pre_handler)
468         {
469                 ret = p->pre_handler (p, regs);
470                 if(!p->ainsn.boostable)
471                         kcb->kprobe_status = KPROBE_HIT_SS;
472                 else if(p->pre_handler != trampoline_probe_handler) {
473 #ifdef SUPRESS_BUG_MESSAGES
474                         preempt_disable();
475 #endif
476                         reset_current_kprobe ();
477 #ifdef SUPRESS_BUG_MESSAGES
478                         preempt_enable_no_resched();
479 #endif
480                 }
481         }
482
483         if (ret)
484         {
485                 DBPRINTF ("p->pre_handler[] 1");
486 #ifdef OVERHEAD_DEBUG
487                 do_gettimeofday(&swap_tv2);
488                 swap_sum_hit++;
489                 swap_sum_time += ((swap_tv2.tv_sec - swap_tv1.tv_sec) * USEC_IN_SEC_NUM +
490                         (swap_tv2.tv_usec - swap_tv1.tv_usec));
491 #endif
492 #ifdef SUPRESS_BUG_MESSAGES
493                 oops_in_progress = swap_oops_in_progress;
494 #endif
495                 /* handler has already set things up, so skip ss setup */
496                 return 1;
497         }
498         DBPRINTF ("p->pre_handler 0");
499
500 no_kprobe:
501         preempt_enable_no_resched ();
502 #ifdef OVERHEAD_DEBUG
503         do_gettimeofday(&swap_tv2);
504         swap_sum_hit++;
505         swap_sum_time += ((swap_tv2.tv_sec - swap_tv1.tv_sec) * USEC_IN_SEC_NUM +
506                 (swap_tv2.tv_usec - swap_tv1.tv_usec));
507 #endif
508 #ifdef SUPRESS_BUG_MESSAGES
509         oops_in_progress = swap_oops_in_progress;
510 #endif
511         return ret;
512 }
513
514 void patch_suspended_task_ret_addr(struct task_struct *p, struct kretprobe *rp)
515 {
516         DBPRINTF("patch_suspended_task_ret_addr is not implemented");
517 }
518
519 int setjmp_pre_handler (struct kprobe *p, struct pt_regs *regs)
520 {
521         struct jprobe *jp = container_of (p, struct jprobe, kp);
522         kprobe_pre_entry_handler_t pre_entry;
523         entry_point_t entry;
524
525 # ifdef REENTER
526         p = __get_cpu_var (current_kprobe);
527 # endif
528
529         DBPRINTF ("pjp = 0x%p jp->entry = 0x%p", jp, jp->entry);
530         entry = (entry_point_t) jp->entry;
531         pre_entry = (kprobe_pre_entry_handler_t) jp->pre_entry;
532         //if(!entry)
533         //      DIE("entry NULL", regs)
534         DBPRINTF ("entry = 0x%p jp->entry = 0x%p", entry, jp->entry);
535
536         //call handler for all kernel probes and user space ones which belong to current tgid
537         if (!p->tgid || (p->tgid == current->tgid))
538         {
539                 if(!p->tgid && (p->addr == sched_addr) && sched_rp){
540                         struct task_struct *p, *g;
541                         rcu_read_lock();
542                         //swapper task
543                         if(current != &init_task)
544                                 patch_suspended_task_ret_addr(&init_task, sched_rp);
545                         // other tasks
546                         do_each_thread(g, p){
547                                 if(p == current)
548                                         continue;
549                                 patch_suspended_task_ret_addr(p, sched_rp);
550                         } while_each_thread(g, p);
551                         rcu_read_unlock();
552                 }
553                 if (pre_entry)
554                         p->ss_addr = (void *)pre_entry (jp->priv_arg, regs);
555                 if (entry){
556                         entry (regs->regs[4], regs->regs[5], regs->regs[6], regs->regs[7], regs->regs[8], regs->regs[9]);
557                 }
558                 else {
559                         if (p->tgid)
560                                 dbi_arch_uprobe_return ();
561                         else
562                                 dbi_jprobe_return ();
563                 }
564         }
565         else if (p->tgid)
566                 dbi_arch_uprobe_return ();
567
568         prepare_singlestep (p, regs);
569
570         return 1;
571 }
572
573
574 void dbi_jprobe_return (void)
575 {
576         preempt_enable_no_resched();
577 }
578
579 void dbi_arch_uprobe_return (void)
580 {
581         preempt_enable_no_resched();
582 }
583
584 int longjmp_break_handler (struct kprobe *p, struct pt_regs *regs)
585 {
586 #ifndef REENTER
587         //kprobe_opcode_t insn = BREAKPOINT_INSTRUCTION;
588         kprobe_opcode_t insns[2];
589
590         if (p->pid)
591         {
592                 insns[0] = BREAKPOINT_INSTRUCTION;
593                 insns[1] = p->opcode;
594                 //p->opcode = *p->addr;
595                 if (read_proc_vm_atomic (current, (unsigned long) (p->addr), &(p->opcode), sizeof (p->opcode)) < sizeof (p->opcode))
596                 {
597                         printk ("ERROR[]: failed to read vm of proc %s/%u addr %p.", current->comm, current->pid, p->addr);
598                         return -1;
599                 }
600                 //*p->addr = BREAKPOINT_INSTRUCTION;
601                 //*(p->addr+1) = p->opcode;
602                 if (write_proc_vm_atomic (current, (unsigned long) (p->addr), insns, sizeof (insns)) < sizeof (insns))
603                 {
604                         printk ("ERROR[]: failed to write vm of proc %s/%u addr %p.", current->comm, current->pid, p->addr);
605                         return -1;
606                 }
607         }
608         else
609         {
610                 DBPRINTF ("p->opcode = 0x%lx *p->addr = 0x%lx p->addr = 0x%p\n", p->opcode, *p->addr, p->addr);
611                 *(p->addr + 1) = p->opcode;
612                 p->opcode = *p->addr;
613                 *p->addr = BREAKPOINT_INSTRUCTION;
614                 flush_icache_range ((unsigned int) p->addr, (unsigned int) (((unsigned int) p->addr) + (sizeof (kprobe_opcode_t) * 2)));
615         }
616
617         reset_current_kprobe ();
618 #endif
619         return 0;
620 }
621
622 void arch_arm_kprobe (struct kprobe *p)
623 {
624         *p->addr = BREAKPOINT_INSTRUCTION;
625         flush_icache_range ((unsigned long) p->addr, (unsigned long) p->addr + sizeof (kprobe_opcode_t));
626 }
627
628 void arch_disarm_kprobe (struct kprobe *p)
629 {
630         *p->addr = p->opcode;
631         flush_icache_range ((unsigned long) p->addr, (unsigned long) p->addr + sizeof (kprobe_opcode_t));
632 }
633
634 int trampoline_probe_handler (struct kprobe *p, struct pt_regs *regs)
635 {
636         struct kretprobe_instance *ri = NULL;
637         struct hlist_head *head, empty_rp;
638         struct hlist_node *node, *tmp;
639         unsigned long flags, orig_ret_address = 0;
640         unsigned long trampoline_address = (unsigned long) &kretprobe_trampoline;
641         struct kretprobe *crp = NULL;
642         struct kprobe_ctlblk *kcb = get_kprobe_ctlblk ();
643
644         DBPRINTF ("start");
645
646         if (p && p->tgid){
647                 // in case of user space retprobe trampoline is at the Nth instruction of US tramp
648                 trampoline_address = (unsigned long)(p->ainsn.insn + UPROBES_TRAMP_RET_BREAK_IDX);
649         }
650
651         INIT_HLIST_HEAD (&empty_rp);
652         spin_lock_irqsave (&kretprobe_lock, flags);
653         head = kretprobe_inst_table_head (current);
654
655         /*
656          * It is possible to have multiple instances associated with a given
657          * task either because an multiple functions in the call path
658          * have a return probe installed on them, and/or more then one
659          * return probe was registered for a target function.
660          *
661          * We can handle this because:
662          *     - instances are always inserted at the head of the list
663          *     - when multiple return probes are registered for the same
664          *       function, the first instance's ret_addr will point to the
665          *       real return address, and all the rest will point to
666          *       kretprobe_trampoline
667          */
668         hlist_for_each_entry_safe (ri, node, tmp, head, hlist)
669         {
670                 if (ri->task != current)
671                         /* another task is sharing our hash bucket */
672                         continue;
673                 if (ri->rp && ri->rp->handler){
674                         ri->rp->handler (ri, regs, ri->rp->priv_arg);
675
676                 }
677
678                 orig_ret_address = (unsigned long) ri->ret_addr;
679                 recycle_rp_inst (ri);
680                 if (orig_ret_address != trampoline_address)
681                         /*
682                          * This is the real return address. Any other
683                          * instances associated with this task are for
684                          * other calls deeper on the call stack
685                          */
686                         break;
687         }
688         kretprobe_assert (ri, orig_ret_address, trampoline_address);
689         //BUG_ON(!orig_ret_address || (orig_ret_address == trampoline_address));
690         if (trampoline_address != (unsigned long) &kretprobe_trampoline){
691                 if (ri->rp2) BUG_ON (ri->rp2->kp.tgid == 0);
692                 if (ri->rp) BUG_ON (ri->rp->kp.tgid == 0);
693                 else if (ri->rp2) BUG_ON (ri->rp2->kp.tgid == 0);
694         }
695         if ((ri->rp && ri->rp->kp.tgid) || (ri->rp2 && ri->rp2->kp.tgid))
696                 BUG_ON (trampoline_address == (unsigned long) &kretprobe_trampoline);
697
698         regs->regs[31] = orig_ret_address;
699         DBPRINTF ("regs->cp0_epc = 0x%lx", regs->cp0_epc);
700         if (trampoline_address != (unsigned long) &kretprobe_trampoline)
701                 regs->cp0_epc = orig_ret_address;
702         else
703                 regs->cp0_epc = regs->cp0_epc + 4;
704         DBPRINTF ("regs->cp0_epc = 0x%lx", regs->cp0_epc);
705         DBPRINTF ("regs->cp0_status = 0x%lx", regs->cp0_status);
706
707         if(p){ // ARM, MIPS, X86 user space
708                 if (kcb->kprobe_status == KPROBE_REENTER)
709                         restore_previous_kprobe (kcb);
710                 else
711                         reset_current_kprobe ();
712
713                 //TODO: test - enter function, delete us retprobe, exit function
714                 // for user space retprobes only - deferred deletion
715                 if (trampoline_address != (unsigned long) &kretprobe_trampoline)
716                 {
717                         // if we are not at the end of the list and current retprobe should be disarmed
718                         if (node && ri->rp2)
719                         {
720                                 crp = ri->rp2;
721                                 /*sprintf(die_msg, "deferred disarm p->addr = %p [%lx %lx %lx]\n",
722                                   crp->kp.addr, *kaddrs[0], *kaddrs[1], *kaddrs[2]);
723                                   DIE(die_msg, regs); */
724                                 // look for other instances for the same retprobe
725                                 hlist_for_each_entry_continue (ri, node, hlist)
726                                 {
727                                         if (ri->task != current)
728                                                 continue;       /* another task is sharing our hash bucket */
729                                         if (ri->rp2 == crp)     //if instance belong to the same retprobe
730                                                 break;
731                                 }
732                                 if (!node)
733                                 {       // if there are no more instances for this retprobe
734                                         // delete retprobe
735                                         DBPRINTF ("defered retprobe deletion p->addr = %p", crp->kp.addr);
736                                         unregister_uprobe (&crp->kp, current, 1);
737                                         kfree (crp);
738                                 }
739                         }
740                 }
741         }
742
743         spin_unlock_irqrestore (&kretprobe_lock, flags);
744         hlist_for_each_entry_safe (ri, node, tmp, &empty_rp, hlist)
745         {
746                 hlist_del (&ri->hlist);
747                 kfree (ri);
748         }
749         preempt_enable_no_resched ();
750         /*
751          * By returning a non-zero value, we are telling
752          * kprobe_handler() that we don't want the post_handler
753          * to run (and have re-enabled preemption)
754          */
755         return 1;
756 }
757
758 void __arch_prepare_kretprobe (struct kretprobe *rp, struct pt_regs *regs)
759 {
760
761         struct kretprobe_instance *ri;
762
763         DBPRINTF ("start\n");
764         //TODO: test - remove retprobe after func entry but before its exit
765         if ((ri = get_free_rp_inst (rp)) != NULL)
766         {
767                 ri->rp = rp;
768                 ri->rp2 = NULL;
769                 ri->task = current;
770                 ri->ret_addr = (kprobe_opcode_t *) regs->regs[31];
771                 if (rp->kp.tgid)
772                         regs->regs[31] = (unsigned long) (rp->kp.ainsn.insn + UPROBES_TRAMP_RET_BREAK_IDX);
773                 else    /* Replace the return addr with trampoline addr */
774                         regs->regs[31] = (unsigned long) &kretprobe_trampoline;
775                 add_rp_inst (ri);
776         }
777         else {
778                 DBPRINTF ("WARNING: missed retprobe %p\n", rp->kp.addr);
779                 rp->nmissed++;
780         }
781 }
782
783 DECLARE_MOD_CB_DEP(flush_icache_range, \
784                 void, unsigned long __user start, unsigned long __user end);
785 DECLARE_MOD_CB_DEP(flush_icache_page, \
786                 void, struct vm_area_struct * vma, struct page * page);
787 DECLARE_MOD_CB_DEP(flush_cache_page, \
788                 void, struct vm_area_struct * vma, unsigned long page);
789
790 int asm_init_module_dependencies()
791 {
792         INIT_MOD_DEP_CB(flush_icache_range, r4k_flush_icache_range);
793         INIT_MOD_DEP_CB(flush_icache_page, r4k_flush_icache_page);
794         INIT_MOD_DEP_CB(flush_cache_page, r4k_flush_cache_page);
795
796         return 0;
797 }
798
799
800 int __init arch_init_kprobes (void)
801 {
802         unsigned int do_bp_handler;
803         unsigned int kprobe_handler_addr;
804
805         unsigned int insns_num = 0;
806         unsigned int code_size = 0;
807
808         unsigned int reg_hi;
809         unsigned int reg_lo;
810
811         int ret;
812
813         if (arch_init_module_dependencies())
814         {
815                 DBPRINTF ("Unable to init module dependencies\n");
816                 return -1;
817         }
818
819         do_bp_handler = (unsigned int) kallsyms_search ("do_bp");
820
821         kprobe_handler_addr = (unsigned int) &kprobe_handler;
822         insns_num = sizeof (arr_traps_template) / sizeof (arr_traps_template[0]);
823         code_size = insns_num * sizeof (unsigned int);
824         DBPRINTF ("insns_num = %d\n", insns_num);
825         // Save original code
826         arr_traps_original = kmalloc (code_size, GFP_KERNEL);
827         if (!arr_traps_original)
828         {
829                 DBPRINTF ("Unable to allocate space for original code of <do_bp>!\n");
830                 return -1;
831         }
832         memcpy (arr_traps_original, (void *) do_bp_handler, code_size);
833
834         reg_hi = HIWORD (kprobe_handler_addr);
835         reg_lo = LOWORD (kprobe_handler_addr);
836         if (reg_lo >= 0x8000)
837                 reg_hi += 0x0001;
838         arr_traps_template[REG_HI_INDEX] |= reg_hi;
839         arr_traps_template[REG_LO_INDEX] |= reg_lo;
840
841         // Insert new code
842         memcpy ((void *) do_bp_handler, arr_traps_template, code_size);
843         flush_icache_range (do_bp_handler, do_bp_handler + code_size);
844         if((ret = dbi_register_kprobe (&trampoline_p)) != 0){
845                 //dbi_unregister_jprobe(&do_exit_p, 0);
846                 return ret;
847         }
848 }
849
850 void __exit dbi_arch_exit_kprobes (void)
851 {
852         unsigned int do_bp_handler;
853
854         unsigned int insns_num = 0;
855         unsigned int code_size = 0;
856
857         // Get instruction address
858         do_bp_handler = (unsigned int) kallsyms_search ("do_undefinstr");
859
860         //dbi_unregister_jprobe(&do_exit_p, 0);
861
862         // Replace back the original code
863
864         insns_num = sizeof (arr_traps_template) / sizeof (arr_traps_template[0]);
865         code_size = insns_num * sizeof (unsigned int);
866         memcpy ((void *) do_bp_handler, arr_traps_original, code_size);
867         flush_icache_range (do_bp_handler, do_bp_handler + code_size);
868         kfree (arr_traps_original);
869         arr_traps_original = NULL;
870 }
871
872 //EXPORT_SYMBOL_GPL (dbi_arch_uprobe_return);
873 //EXPORT_SYMBOL_GPL (dbi_arch_exit_kprobes);
874