driver/probes_manager.c

   1 ////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //      FILE:           probes_manager.c
   4 //
   5 //      DESCRIPTION:
   6 //      This file is C source for SWAP driver.
   7 //
   8 //      SEE ALSO:       probes_manager.h
   9 //      AUTHOR:         L.Komkov, A.Gerenkov
  10 //      COMPANY NAME:   Samsung Research Center in Moscow
  11 //      DEPT NAME:      Advanced Software Group
  12 //      CREATED:        2008.02.15
  13 //      VERSION:        1.0
  14 //      REVISION DATE:  2008.12.03
  15 //
  16 ////////////////////////////////////////////////////////////////////////////////////
  17
  18 #include <linux/percpu.h>
  19 #include <ksyms.h>
  20 #include <dbi_kprobes_deps.h>
  21 #include "module.h"
  22 #include "probes_manager.h"
  23
  24 /* list of selected non-predefined kernel probes */
  25 static HLIST_HEAD(kernel_probes);
  26
  27 static int register_kernel_probe(kernel_probe_t *p)
  28 {
  29         int ret;
  30
  31         /* register jprobe */
  32         ret = dbi_register_jprobe(&p->jprobe);
  33         if (ret) {
  34                 EPRINTF("dbi_register_jprobe(0x%lx) failure %d", p->addr, ret);
  35                 return ret;
  36         };
  37
  38         /* register kretprobe */
  39         ret = dbi_register_kretprobe(&p->retprobe);
  40         if (ret) {
  41                 EPRINTF("dbi_register_kretprobe(0x%lx) failure %d",
  42                         p->addr, ret);
  43
  44                 dbi_unregister_jprobe(&p->jprobe);
  45                 return ret;
  46         }
  47
  48         return 0;
  49 }
  50
  51 static int unregister_kernel_probe(kernel_probe_t *p)
  52 {
  53         dbi_unregister_kretprobe(&p->retprobe);
  54         dbi_unregister_jprobe(&p->jprobe);
  55
  56         return 0;
  57 }
  58
  59 int set_kernel_probes(void)
  60 {
  61         int ret = 0;
  62         kernel_probe_t *p;
  63         struct hlist_node *node;
  64
  65         swap_hlist_for_each_entry_rcu(p, node, &kernel_probes, hlist) {
  66                 ret = register_kernel_probe(p);
  67                 if (ret) {
  68                         /* return into safe state */
  69                         /* FIXME: unset for installed probes */
  70                         unset_kernel_probes();
  71                         break;
  72                 }
  73         }
  74
  75         return ret;
  76 }
  77
  78 int unset_kernel_probes(void)
  79 {
  80         kernel_probe_t *p;
  81         struct hlist_node *node;
  82
  83         swap_hlist_for_each_entry_rcu(p, node, &kernel_probes, hlist)
  84                 unregister_kernel_probe(p);
  85
  86         return 0;
  87 }
  88
  89 static kernel_probe_t *create_kern_probe(unsigned long addr)
  90 {
  91         kernel_probe_t *probe = kmalloc(sizeof(*probe), GFP_KERNEL);
  92         if (!probe) {
  93                 EPRINTF("no memory for new probe!");
  94                 return NULL;
  95         }
  96
  97         memset(probe, 0, sizeof(*probe));
  98         probe->addr = addr;
  99         probe->jprobe.kp.addr = probe->retprobe.kp.addr = (kprobe_opcode_t *)addr;
 100         probe->jprobe.priv_arg = probe->retprobe.priv_arg = probe;
 101
 102         INIT_HLIST_NODE(&probe->hlist);
 103
 104         return probe;
 105 }
 106
 107 static void free_kern_probe(kernel_probe_t *p)
 108 {
 109         kfree(p);
 110 }
 111
 112 /* Searches non-predefined kernel probe in the list. */
 113 static kernel_probe_t* find_probe(unsigned long addr)
 114 {
 115         kernel_probe_t *p;
 116         struct hlist_node *node;
 117
 118         /* check if such probe does exist */
 119         swap_hlist_for_each_entry_rcu(p, node, &kernel_probes, hlist)
 120                 if (p->addr == addr)
 121                         return p;
 122
 123         return NULL;
 124 }
 125
 126 /* Adds non-predefined kernel probe to the list. */
 127 static void add_probe_to_list(kernel_probe_t *p)
 128 {
 129         hlist_add_head_rcu(&p->hlist, &kernel_probes);
 130 }
 131
 132 int add_probe(unsigned long addr,
 133               unsigned long pre_handler,
 134               unsigned long jp_handler,
 135               unsigned long rp_handler)
 136 {
 137         kernel_probe_t *p;
 138
 139         /* check if such probe does already exist */
 140         p = find_probe(addr);
 141         if (p)
 142                 /* It is not a problem if we have already registered
 143                    this probe before */
 144                 return -EINVAL;
 145
 146
 147         p = create_kern_probe(addr);
 148         if (!p)
 149                 return -ENOMEM;
 150
 151         p->jprobe.pre_entry = (kprobe_pre_entry_handler_t)pre_handler;
 152         p->jprobe.entry = (kprobe_opcode_t *)jp_handler;
 153         p->retprobe.handler = (kretprobe_handler_t)rp_handler;
 154
 155         add_probe_to_list(p);
 156
 157         return 0;
 158 }
 159
 160 int reset_probes(void)
 161 {
 162         struct hlist_node *node, *tnode;
 163         kernel_probe_t *p;
 164
 165         swap_hlist_for_each_entry_safe (p, node, tnode, &kernel_probes, hlist) {
 166                 hlist_del(node);
 167                 free_kern_probe(p);
 168         }
 169
 170         return 0;
 171 }
 172
 173 static DEFINE_PER_CPU(kernel_probe_t *, gpKernProbe) = NULL;
 174
 175 unsigned long
 176 def_jprobe_event_pre_handler (kernel_probe_t * probe, struct pt_regs *regs)
 177 {
 178         __get_cpu_var (gpKernProbe) = probe;
 179
 180         return 0;
 181 }
 182
 183 void
 184 def_jprobe_event_handler (unsigned long arg1, unsigned long arg2, unsigned long arg3, unsigned long arg4, unsigned long arg5, unsigned long arg6)
 185 {
 186         //static int nCount;
 187         kernel_probe_t *probe = __get_cpu_var(gpKernProbe);
 188
 189         pack_event_info(KS_PROBE_ID, RECORD_ENTRY, "pxxxxxx", probe->addr, arg1, arg2, arg3, arg4, arg5, arg6);
 190         dbi_jprobe_return ();
 191 }
 192
 193 int
 194 def_retprobe_event_handler (struct kretprobe_instance *pi, struct pt_regs *regs, kernel_probe_t * probe)
 195 {
 196         int ret_val;
 197
 198         ret_val = regs_return_value(regs);
 199         pack_event_info(KS_PROBE_ID, RECORD_RET, "pd", probe->addr, ret_val);
 200
 201         return 0;
 202 }
 203
 204 int install_kern_otg_probe(unsigned long addr,
 205                            unsigned long pre_handler,
 206                            unsigned long jp_handler,
 207                            unsigned long rp_handler)
 208 {
 209         kernel_probe_t *p;
 210         int ret = 0;
 211
 212         p = find_probe(addr);
 213         if (p) {
 214                 return -EINVAL;;
 215         }
 216
 217         p = create_kern_probe(addr);
 218         if (!p)
 219                 return -ENOMEM;
 220
 221         p->jprobe.pre_entry = (kprobe_pre_entry_handler_t)pre_handler;
 222         p->jprobe.entry = (kprobe_opcode_t *)jp_handler;
 223         p->retprobe.handler = (kretprobe_handler_t)rp_handler;
 224
 225         add_probe_to_list(p);
 226
 227         ret = register_kernel_probe(p);
 228         if (ret) {
 229                 EPRINTF("Cannot set kernel probe at addr %lx", addr);
 230                 return ret;
 231         }
 232
 233         return 0;
 234 }
 235 EXPORT_SYMBOL_GPL(install_kern_otg_probe);