preload/preload_threads.c

   1 #include <linux/kernel.h>
   2 #include <linux/sched.h>
   3 #include <linux/slab.h>
   4 #include <linux/spinlock.h>
   5 #include <linux/mm.h>
   6 #include <linux/mman.h>
   7 #include <linux/list.h>
   8 #include <kprobe/swap_ktd.h>
   9 #include "preload.h"
  10 #include "preload_threads.h"
  11 #include "preload_debugfs.h"
  12 #include "preload_pd.h"
  13
  14 struct preload_td {
  15         struct list_head slots;
  16         unsigned long flags;
  17 };
  18
  19 struct thread_slot {
  20         struct list_head list;
  21         struct list_head disabled_addrs;
  22
  23         unsigned long caller;
  24         unsigned char call_type;
  25         bool drop;   /* TODO Workaround, remove when will be possible to install
  26                       * several us probes at the same addr. */
  27 };
  28
  29 struct disabled_addr {
  30         struct list_head list;
  31         unsigned long addr;
  32 };
  33
  34 static void preload_ktd_init(struct task_struct *task, void *data)
  35 {
  36         struct preload_td *td = (struct preload_td *)data;
  37
  38         INIT_LIST_HEAD(&td->slots);
  39         td->flags = 0;
  40 }
  41
  42 static void preload_ktd_exit(struct task_struct *task, void *data)
  43 {
  44         /* TODO: to be implement */
  45 }
  46
  47 static struct ktask_data preload_ktd = {
  48         .init = preload_ktd_init,
  49         .exit = preload_ktd_exit,
  50         .size = sizeof(struct preload_td),
  51 };
  52
  53
  54 static inline struct preload_td *get_preload_td(struct task_struct *task)
  55 {
  56         return (struct preload_td *)swap_ktd(&preload_ktd, task);
  57 }
  58
  59 unsigned long get_preload_flags(struct task_struct *task)
  60 {
  61         return get_preload_td(task)->flags;
  62 }
  63
  64 void set_preload_flags(struct task_struct *task,
  65                        unsigned long flags)
  66 {
  67         get_preload_td(task)->flags = flags;
  68 }
  69
  70
  71 static inline bool __is_addr_found(struct disabled_addr *da,
  72                                    unsigned long addr)
  73 {
  74         if (da->addr == addr)
  75                 return true;
  76
  77         return false;
  78 }
  79
  80 static inline void __remove_from_disable_list(struct disabled_addr *da)
  81 {
  82         list_del(&da->list);
  83         kfree(da);
  84 }
  85
  86 static inline void __remove_whole_disable_list(struct thread_slot *slot)
  87 {
  88         struct disabled_addr *da, *n;
  89
  90         list_for_each_entry_safe(da, n, &slot->disabled_addrs, list)
  91                 __remove_from_disable_list(da);
  92 }
  93
  94 static inline void __init_slot(struct thread_slot *slot)
  95 {
  96         slot->caller = 0;
  97         slot->call_type = 0;
  98         slot->drop = false;
  99         INIT_LIST_HEAD(&slot->disabled_addrs);
 100 }
 101
 102 static inline void __reinit_slot(struct thread_slot *slot)
 103 {
 104         __remove_whole_disable_list(slot);
 105         __init_slot(slot);
 106 }
 107
 108 static inline void __set_slot(struct thread_slot *slot,
 109                               struct task_struct *task, unsigned long caller,
 110                               unsigned char call_type, bool drop)
 111 {
 112         slot->caller = caller;
 113         slot->call_type = call_type;
 114         slot->drop = drop;
 115 }
 116
 117 static inline int __add_to_disable_list(struct thread_slot *slot,
 118                                         unsigned long disable_addr)
 119 {
 120         struct disabled_addr *da = kmalloc(sizeof(*da), GFP_ATOMIC);
 121
 122         if (da == NULL)
 123                 return -ENOMEM;
 124
 125         INIT_LIST_HEAD(&da->list);
 126         da->addr = disable_addr;
 127         list_add_tail(&da->list, &slot->disabled_addrs);
 128
 129         return 0;
 130 }
 131
 132 static inline struct disabled_addr *__find_disabled_addr(struct thread_slot *slot,
 133                                                          unsigned long addr)
 134 {
 135         struct disabled_addr *da;
 136
 137         list_for_each_entry(da, &slot->disabled_addrs, list)
 138                 if (__is_addr_found(da, addr))
 139                         return da;
 140
 141         return NULL;
 142 }
 143
 144 /* Adds a new slot */
 145 static inline struct thread_slot *__grow_slot(void)
 146 {
 147         struct thread_slot *tmp = kmalloc(sizeof(*tmp), GFP_ATOMIC);
 148
 149         if (tmp == NULL)
 150                 return NULL;
 151
 152         INIT_LIST_HEAD(&tmp->list);
 153         __init_slot(tmp);
 154
 155         return tmp;
 156 }
 157
 158 /* Free slot */
 159 static void __clean_slot(struct thread_slot *slot)
 160 {
 161         list_del(&slot->list);
 162         kfree(slot);
 163 }
 164
 165 /* There is no list_last_entry in Linux 3.10 */
 166 #ifndef list_last_entry
 167 #define list_last_entry(ptr, type, member) \
 168         list_entry((ptr)->prev, type, member)
 169 #endif /* list_last_entry */
 170
 171 static inline struct thread_slot *__get_task_slot(struct task_struct *task)
 172 {
 173         struct preload_td *td = get_preload_td(task);
 174
 175         return list_empty(&td->slots) ? NULL :
 176                 list_last_entry(&td->slots, struct thread_slot, list);
 177 }
 178
 179
 180
 181
 182 int preload_threads_set_data(struct task_struct *task, unsigned long caller,
 183                              unsigned char call_type,
 184                              unsigned long disable_addr, bool drop)
 185 {
 186         struct preload_td *td = get_preload_td(task);
 187         struct thread_slot *slot;
 188         int ret = 0;
 189
 190         slot = __grow_slot();
 191         if (slot == NULL) {
 192                 ret = -ENOMEM;
 193                 goto set_data_done;
 194         }
 195
 196         if ((disable_addr != 0) &&
 197             (__add_to_disable_list(slot, disable_addr) != 0)) {
 198                 printk(KERN_ERR PRELOAD_PREFIX "Cannot alloc memory!\n");
 199                 ret = -ENOMEM;
 200                 goto set_data_done;
 201         }
 202
 203         __set_slot(slot, task, caller, call_type, drop);
 204         list_add_tail(&slot->list, &td->slots);
 205
 206 set_data_done:
 207         return ret;
 208 }
 209
 210 int preload_threads_get_caller(struct task_struct *task, unsigned long *caller)
 211 {
 212         struct thread_slot *slot;
 213         int ret = 0;
 214
 215         slot = __get_task_slot(task);
 216         if (slot != NULL) {
 217                 *caller = slot->caller;
 218                 goto get_caller_done;
 219         }
 220
 221         /* If we're here - slot was not found */
 222         ret = -EINVAL;
 223
 224 get_caller_done:
 225         return ret;
 226 }
 227
 228 int preload_threads_get_call_type(struct task_struct *task,
 229                                   unsigned char *call_type)
 230 {
 231         struct thread_slot *slot;
 232         int ret = 0;
 233
 234         slot = __get_task_slot(task);
 235         if (slot != NULL) {
 236                 *call_type = slot->call_type;
 237                 goto get_call_type_done;
 238         }
 239
 240         /* If we're here - slot was not found */
 241         ret = -EINVAL;
 242
 243 get_call_type_done:
 244         return ret;
 245 }
 246
 247 int preload_threads_get_drop(struct task_struct *task)
 248 {
 249         struct thread_slot *slot;
 250         int ret = 0;
 251
 252         slot = __get_task_slot(task);
 253         if (slot != NULL) {
 254                 ret = (int) slot->drop;
 255                 goto get_drop_done;
 256         }
 257
 258         /* If we're here - slot was not found */
 259         ret = -EINVAL;
 260
 261 get_drop_done:
 262         return ret;
 263 }
 264
 265 bool preload_threads_check_disabled_probe(struct task_struct *task,
 266                                           unsigned long addr)
 267 {
 268         struct thread_slot *slot;
 269         bool ret = false;
 270
 271         slot = __get_task_slot(task);
 272         if (slot != NULL)
 273                 ret = __find_disabled_addr(slot, addr) == NULL ? false : true;
 274
 275         return ret;
 276 }
 277
 278 void preload_threads_enable_probe(struct task_struct *task, unsigned long addr)
 279 {
 280         struct thread_slot *slot;
 281         struct disabled_addr *da;
 282
 283         slot = __get_task_slot(task);
 284         if (slot == NULL) {
 285                 printk(KERN_ERR PRELOAD_PREFIX "Error! Slot not found!\n");
 286                 goto enable_probe_failed;
 287         }
 288
 289         da = __find_disabled_addr(slot, addr);
 290         if (da != NULL)
 291                 __remove_from_disable_list(da);
 292
 293 enable_probe_failed:
 294         return; /* make gcc happy: cannot place label right before '}' */
 295 }
 296
 297 int preload_threads_put_data(struct task_struct *task)
 298 {
 299         struct thread_slot *slot;
 300         int ret = 0;
 301
 302         slot = __get_task_slot(task);
 303         if (slot != NULL) {
 304                 __reinit_slot(slot);
 305                 __clean_slot(slot); /* remove from list */
 306                 goto put_data_done;
 307         }
 308
 309 put_data_done:
 310         return ret;
 311 }
 312
 313 int preload_threads_init(void)
 314 {
 315         return swap_ktd_reg(&preload_ktd);
 316 }
 317
 318 void preload_threads_exit(void)
 319 {
 320         swap_ktd_unreg(&preload_ktd);
 321 }