arch/arm/mach-mt8127/hw_watchpoint.c

   1 #include <linux/spinlock.h>
   2 #include <linux/errno.h>
   3 #include <linux/module.h>
   4
   5 //#define WATCH_DBG
   6 #ifdef WATCH_DBG
   7 // Test for online cpu
   8 #include <linux/device.h>
   9 #include <linux/platform_device.h>
  10 #endif
  11
  12 #include <asm/system.h>
  13 #include <asm/signal.h>
  14 #include <asm/ptrace.h>
  15 #include <mach/hw_watchpoint.h>
  16
  17 #define DBGWVR_BASE 0xF0170180
  18 #define DBGWCR_BASE 0xF01701C0
  19
  20 #define DBGLAR 0xF0170FB0
  21 #define DBGDSCR 0xF0170088
  22 #define DBGWFAR 0xF0170018
  23 #define DBGOSLAR 0xF0170300
  24 #define DBGOSSAR 0xF0170304
  25 #define MAX_NR_WATCH_POINT 4
  26 #define NUM_CPU    4
  27
  28 #define UNLOCK_KEY 0xC5ACCE55
  29 #define HDBGEN (1 << 14)
  30 #define MDBGEN (1 << 15)
  31 #define DBGWCR_VAL 0x000001E7
  32 /**/
  33 #define WP_EN (1 << 0)
  34 #define LSC_LDR (1 << 3)
  35 #define LSC_STR (2 << 3)
  36 #define LSC_ALL (3 << 3)
  37 #if 0
  38  struct wp_event wp_event;
  39  int err;
  40  volatile int my_watch_data;
  41  volatile int my_watch_data2;
  42  volatile int my_watch_data3;
  43
  44  int my_wp_handler1(unsigned int addr)
  45  {
  46      printk("Access my data from an instruction at 0x%x\n", addr);
  47      return 0;
  48  }
  49
  50  int my_wp_handler2(unsigned int addr)
  51  {
  52      printk("Access my data from an instruction at 0x%x\n", addr);
  53      /* trigger exception */
  54      return 1;
  55  }
  56
  57  void foo(void)
  58  {
  59      int test;
  60      init_wp_event(&wp_event, &my_watch_data, &my_watch_data, WP_EVENT_TYPE_ALL, my_wp_handler1);
  61
  62      err = add_hw_watchpoint(&wp_event);
  63      if (err) {
  64          printk("add hw watch point failed...\n");
  65          /* fail to add watchpoing */
  66      } else {
  67          /* the memory address is under watching */
  68          printk("add hw watch point success...\n");
  69
  70          //del_hw_watchpoint(&wp_event);
  71      }
  72      /* test watchpoint */
  73      my_watch_data = 1;
  74  }
  75
  76  void foo2(void)
  77  {
  78      int test;
  79      init_wp_event(&wp_event, &my_watch_data2, &my_watch_data2, WP_EVENT_TYPE_ALL, my_wp_handler1);
  80
  81      err = add_hw_watchpoint(&wp_event);
  82      if (err) {
  83          printk("add hw watch point failed...\n");
  84          /* fail to add watchpoing */
  85      } else {
  86          /* the memory address is under watching */
  87          printk("add hw watch point success...\n");
  88
  89          //del_hw_watchpoint(&wp_event);
  90      }
  91      /* test watchpoint */
  92      my_watch_data2 = 2;
  93  }
  94
  95 void foo3(void)
  96 {
  97      int test;
  98      init_wp_event(&wp_event, &my_watch_data3, &my_watch_data3, WP_EVENT_TYPE_ALL, my_wp_handler1);
  99
 100      err = add_hw_watchpoint(&wp_event);
 101      if (err) {
 102          printk("add hw watch point failed...\n");
 103          /* fail to add watchpoing */
 104      } else {
 105          /* the memory address is under watching */
 106          printk("add hw watch point success...\n");
 107
 108          //del_hw_watchpoint(&wp_event);
 109      }
 110      /* test watchpoint */
 111      my_watch_data3 = 3;
 112 }
 113
 114 #endif
 115
 116 static struct wp_event wp_events[MAX_NR_WATCH_POINT];
 117 static spinlock_t wp_lock;
 118
 119 /*
 120  * enable_hw_watchpoint: Enable the H/W watchpoint.
 121  * Return error code.
 122  */
 123 int enable_hw_watchpoint(void)
 124 {
 125     int i;
 126
 127     //for(i = 0 ; i < NUM_CPU; i++) {
 128     for_each_online_cpu(i) {
 129         if (*(volatile unsigned int *)(DBGDSCR + i * 0x2000) & HDBGEN) {
 130             printk(KERN_ALERT "halting debug mode enabled. Unable to access hardware resources.\n");
 131             return -EPERM;
 132         }
 133
 134         if (*(volatile unsigned int *)(DBGDSCR + i * 0x2000) & MDBGEN) {
 135             /* already enabled */
 136             printk(KERN_ALERT "already enabled, DBGDSCR = 0x%x\n", *(volatile unsigned int *)(DBGDSCR + i * 0x2000));
 137             //return 0;
 138         }
 139
 140         *(volatile unsigned int *)(DBGLAR + i * 0x2000) = UNLOCK_KEY;
 141         //printk(KERN_ALERT "DBGLAR + 4 = 0x%x\n", *(volatile unsigned int *)(DBGDSCR + i * 0x2000));
 142         *(volatile unsigned int *)(DBGOSLAR + i * 0x2000) = ~UNLOCK_KEY;
 143         *(volatile unsigned int *)(DBGDSCR + i * 0x2000) |= MDBGEN;
 144
 145     }
 146     return 0;
 147 }
 148
 149 /*
 150  * add_hw_watchpoint: add a watch point.
 151  * @wp_event: pointer to the struct wp_event.
 152  * Return error code.
 153  */
 154 int add_hw_watchpoint(struct wp_event *wp_event)
 155 {
 156     int ret, i, j;
 157     unsigned long flags;
 158     unsigned int ctl;
 159
 160     if (!wp_event) {
 161         return -EINVAL;
 162     }
 163     if (!(wp_event->handler)) {
 164         return -EINVAL;
 165     }
 166
 167     ret = enable_hw_watchpoint();
 168     if (ret) {
 169         return ret;
 170     }
 171
 172     ctl = DBGWCR_VAL;
 173     if (wp_event->type == WP_EVENT_TYPE_READ) {
 174         ctl |= LSC_LDR;
 175     } else if (wp_event->type == WP_EVENT_TYPE_WRITE) {
 176         ctl |= LSC_STR;
 177     } else if (wp_event->type == WP_EVENT_TYPE_ALL) {
 178         ctl |= LSC_ALL;
 179     } else {
 180         return -EINVAL;
 181     }
 182
 183     spin_lock_irqsave(&wp_lock, flags);
 184     for (i = 0; i < MAX_NR_WATCH_POINT; i++) {
 185         if (!wp_events[i].in_use) {
 186             wp_events[i].in_use = 1;
 187             break;
 188         }
 189     }
 190     spin_unlock_irqrestore(&wp_lock, flags);
 191
 192     if (i == MAX_NR_WATCH_POINT) {
 193         return -EAGAIN;
 194     }
 195
 196     wp_events[i].virt = wp_event->virt & ~3;    /* enforce word-aligned */
 197     wp_events[i].phys = wp_event->phys; /* no use currently */
 198     wp_events[i].type = wp_event->type;
 199     wp_events[i].handler = wp_event->handler;
 200     wp_events[i].auto_disable = wp_event->auto_disable;
 201
 202
 203     printk(KERN_ALERT "Add watchpoint %d at address 0x%x\n", i, wp_events[i].virt);
 204     //for(j = 0; j < NUM_CPU; j++) {
 205     for_each_online_cpu(j) {
 206         printk("cpu %d is online %d\n", smp_processor_id(), j);
 207         *(((volatile unsigned int *)(DBGWVR_BASE + 0x2000 * j)) + i) = wp_events[i].virt;
 208         *(((volatile unsigned int *)(DBGWCR_BASE + 0x2000 * j)) + i) = ctl;
 209
 210         printk(KERN_ALERT "*(((volatile unsigned int *)(DBGWVR_BASE + 0x%4lx)) + %d) = 0x%x\n",(unsigned long)(0x2000 * j), i, *(((volatile unsigned int *)(DBGWVR_BASE + 0x2000 * j)) + i));
 211         printk(KERN_ALERT "*(((volatile unsigned int *)(DBGWCR_BASE + 0x%4lx)) + %d) = 0x%x\n", (unsigned long)(0x2000 * j), i, *(((volatile unsigned int *)(DBGWCR_BASE + 0x2000 * j)) + i));
 212     }
 213     return 0;
 214 }
 215
 216 /*
 217  * del_hw_watchpoint: delete a watch point.
 218  * @wp_event: pointer to the struct wp_event.
 219  * Return error code.
 220  */
 221 int del_hw_watchpoint(struct wp_event *wp_event)
 222 {
 223     unsigned long flags;
 224     int i, j;
 225
 226     if (!wp_event) {
 227         return -EINVAL;
 228     }
 229
 230     spin_lock_irqsave(&wp_lock, flags);
 231     for (i = 0; i < MAX_NR_WATCH_POINT; i++) {
 232         if (wp_events[i].in_use && (wp_events[i].virt == wp_event->virt)) {
 233             wp_events[i].virt = 0;
 234             wp_events[i].phys = 0;
 235             wp_events[i].type = 0;
 236             wp_events[i].handler = NULL;
 237             wp_events[i].in_use = 0;
 238             //for(j = 0; j < NUM_CPU; j++)
 239             for_each_online_cpu(j)
 240                 *(((volatile unsigned int *)(DBGWCR_BASE + j * 0x2000)) + i) &= ~WP_EN;
 241             break;
 242         }
 243     }
 244     spin_unlock_irqrestore(&wp_lock, flags);
 245
 246     if (i == MAX_NR_WATCH_POINT) {
 247         return -EINVAL;
 248     } else {
 249         return 0;
 250     }
 251 }
 252
 253 int watchpoint_handler(unsigned long addr, unsigned int fsr, struct pt_regs *regs)
 254 {
 255     unsigned int wfar, daddr, iaddr;
 256     int i, ret, j;
 257 #if defined CONFIG_ARCH_MT8127
 258 /* Notes
 259  *v7 Debug the address of instruction that triggered the watchpoint is in DBGWFAR
 260  *v7.1 Debug the address is in DFAR
 261 */
 262     asm volatile(
 263     "MRC p15, 0, %0, c6, c0, 0\n"
 264     : "=r" (wfar)
 265     :
 266     : "cc"
 267     );
 268 #else
 269     wfar = *(volatile unsigned int*)(DBGWFAR +  raw_smp_processor_id() * 0x2000);
 270 #endif
 271     daddr = addr & ~3;
 272     iaddr = regs->ARM_pc;
 273     printk(KERN_ALERT "addr = 0x%x, DBGWFAR/DFAR = 0x%x\n", (unsigned int)addr, wfar);
 274     printk(KERN_ALERT "daddr = 0x%x, iaddr = 0x%x\n", daddr, iaddr);
 275
 276
 277     /* update PC to avoid re-execution of the instruction under watching */
 278     regs->ARM_pc += thumb_mode(regs)? 2: 4;
 279
 280     for (i = 0; i < MAX_NR_WATCH_POINT; i++) {
 281         if (wp_events[i].in_use && wp_events[i].virt == (daddr)) {
 282             printk(KERN_ALERT "Watchpoint %d triggers.\n", i);
 283             if (wp_events[i].handler) {
 284                 if (wp_events[i].auto_disable) {
 285                     //for(j = 0; j < NUM_CPU; j++)
 286                     for_each_online_cpu(j)
 287                         *(((volatile unsigned int *)(DBGWCR_BASE + j * 0x2000)) + i) &= ~WP_EN;
 288                 }
 289                 ret = wp_events[i].handler(iaddr);
 290                 if (wp_events[i].auto_disable) {
 291                     //for(j = 0; j < NUM_CPU; j++)
 292                     for_each_online_cpu(j)
 293                         *(((volatile unsigned int *)(DBGWCR_BASE + j * 0x2000)) + i) |= WP_EN;
 294                 }
 295                 return ret;
 296             } else {
 297                 printk(KERN_ALERT "No watchpoint handler. Ignore.\n");
 298                 return 0;
 299             }
 300         }
 301     }
 302
 303     return 0;
 304 }
 305
 306 #ifdef WATCH_BUG
 307 static struct device_driver hw_drv =
 308 {
 309     .name       = "hw_watchpoint",
 310     .bus        = &platform_bus_type,
 311     .owner      = THIS_MODULE
 312 };
 313
 314 static ssize_t watchpoint_test_show(struct device_driver *driver, char *buf)
 315 {
 316     return snprintf(buf, PAGE_SIZE, "currently run on cpu: %d\n", smp_processor_id());
 317 }
 318
 319 static ssize_t watchpoint_test_store(struct device_driver *driver,
 320                      const char *buf, size_t count)
 321 {
 322     foo3();
 323     return count;
 324 }
 325
 326 DRIVER_ATTR(watchpoint_test, 0644, watchpoint_test_show, watchpoint_test_store);
 327 #endif
 328
 329 static int __init hw_watchpoint_init(void)
 330 {
 331     int ret;
 332     spin_lock_init(&wp_lock);
 333
 334     #ifdef WATCH_DBG
 335     ret = driver_register(&hw_drv);
 336     if (ret) {
 337         pr_err("Fail to register mt_hw_drv");
 338     }
 339
 340     ret = driver_create_file(&hw_drv, &driver_attr_watchpoint_test);
 341     if (ret) {
 342         pr_err("Fail to create mt_hw_drv sysfs files");
 343     }
 344
 345     driver_create_file(&hw_drv, &driver_attr_watchpoint_test);
 346     #endif
 347
 348     hook_fault_code(2, watchpoint_handler, SIGTRAP, 0, "watchpoint debug exception");
 349     printk(KERN_ALERT "watchpoint handler init. \n");
 350     //foo();
 351     //foo2();
 352
 353     return 0;
 354 }
 355
 356 //EXPORT_SYMBOL(add_hw_watchpoint);
 357
 358 arch_initcall(hw_watchpoint_init);