1 #include <linux/spinlock.h>
2 #include <linux/errno.h>
3 #include <linux/module.h>
8 #include <linux/device.h>
9 #include <linux/platform_device.h>
12 #include <asm/system.h>
13 #include <asm/signal.h>
14 #include <asm/ptrace.h>
15 #include <mach/hw_watchpoint.h>
17 #define DBGWVR_BASE 0xF0170180
18 #define DBGWCR_BASE 0xF01701C0
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
28 #define UNLOCK_KEY 0xC5ACCE55
29 #define HDBGEN (1 << 14)
30 #define MDBGEN (1 << 15)
31 #define DBGWCR_VAL 0x000001E7
33 #define WP_EN (1 << 0)
34 #define LSC_LDR (1 << 3)
35 #define LSC_STR (2 << 3)
36 #define LSC_ALL (3 << 3)
38 struct wp_event wp_event
;
40 volatile int my_watch_data
;
41 volatile int my_watch_data2
;
42 volatile int my_watch_data3
;
44 int my_wp_handler1(unsigned int addr
)
46 printk("Access my data from an instruction at 0x%x\n", addr
);
50 int my_wp_handler2(unsigned int addr
)
52 printk("Access my data from an instruction at 0x%x\n", addr
);
53 /* trigger exception */
60 init_wp_event(&wp_event
, &my_watch_data
, &my_watch_data
, WP_EVENT_TYPE_ALL
, my_wp_handler1
);
62 err
= add_hw_watchpoint(&wp_event
);
64 printk("add hw watch point failed...\n");
65 /* fail to add watchpoing */
67 /* the memory address is under watching */
68 printk("add hw watch point success...\n");
70 //del_hw_watchpoint(&wp_event);
79 init_wp_event(&wp_event
, &my_watch_data2
, &my_watch_data2
, WP_EVENT_TYPE_ALL
, my_wp_handler1
);
81 err
= add_hw_watchpoint(&wp_event
);
83 printk("add hw watch point failed...\n");
84 /* fail to add watchpoing */
86 /* the memory address is under watching */
87 printk("add hw watch point success...\n");
89 //del_hw_watchpoint(&wp_event);
98 init_wp_event(&wp_event
, &my_watch_data3
, &my_watch_data3
, WP_EVENT_TYPE_ALL
, my_wp_handler1
);
100 err
= add_hw_watchpoint(&wp_event
);
102 printk("add hw watch point failed...\n");
103 /* fail to add watchpoing */
105 /* the memory address is under watching */
106 printk("add hw watch point success...\n");
108 //del_hw_watchpoint(&wp_event);
110 /* test watchpoint */
116 static struct wp_event wp_events
[MAX_NR_WATCH_POINT
];
117 static spinlock_t wp_lock
;
120 * enable_hw_watchpoint: Enable the H/W watchpoint.
123 int enable_hw_watchpoint(void)
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");
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));
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
;
150 * add_hw_watchpoint: add a watch point.
151 * @wp_event: pointer to the struct wp_event.
154 int add_hw_watchpoint(struct wp_event
*wp_event
)
163 if (!(wp_event
->handler
)) {
167 ret
= enable_hw_watchpoint();
173 if (wp_event
->type
== WP_EVENT_TYPE_READ
) {
175 } else if (wp_event
->type
== WP_EVENT_TYPE_WRITE
) {
177 } else if (wp_event
->type
== WP_EVENT_TYPE_ALL
) {
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;
190 spin_unlock_irqrestore(&wp_lock
, flags
);
192 if (i
== MAX_NR_WATCH_POINT
) {
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
;
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
;
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
));
217 * del_hw_watchpoint: delete a watch point.
218 * @wp_event: pointer to the struct wp_event.
221 int del_hw_watchpoint(struct wp_event
*wp_event
)
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
;
244 spin_unlock_irqrestore(&wp_lock
, flags
);
246 if (i
== MAX_NR_WATCH_POINT
) {
253 int watchpoint_handler(unsigned long addr
, unsigned int fsr
, struct pt_regs
*regs
)
255 unsigned int wfar
, daddr
, iaddr
;
257 #if defined CONFIG_ARCH_MT8127
259 *v7 Debug the address of instruction that triggered the watchpoint is in DBGWFAR
260 *v7.1 Debug the address is in DFAR
263 "MRC p15, 0, %0, c6, c0, 0\n"
269 wfar
= *(volatile unsigned int*)(DBGWFAR
+ raw_smp_processor_id() * 0x2000);
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
);
277 /* update PC to avoid re-execution of the instruction under watching */
278 regs
->ARM_pc
+= thumb_mode(regs
)? 2: 4;
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
;
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
;
297 printk(KERN_ALERT
"No watchpoint handler. Ignore.\n");
307 static struct device_driver hw_drv
=
309 .name
= "hw_watchpoint",
310 .bus
= &platform_bus_type
,
314 static ssize_t
watchpoint_test_show(struct device_driver
*driver
, char *buf
)
316 return snprintf(buf
, PAGE_SIZE
, "currently run on cpu: %d\n", smp_processor_id());
319 static ssize_t
watchpoint_test_store(struct device_driver
*driver
,
320 const char *buf
, size_t count
)
326 DRIVER_ATTR(watchpoint_test
, 0644, watchpoint_test_show
, watchpoint_test_store
);
329 static int __init
hw_watchpoint_init(void)
332 spin_lock_init(&wp_lock
);
335 ret
= driver_register(&hw_drv
);
337 pr_err("Fail to register mt_hw_drv");
340 ret
= driver_create_file(&hw_drv
, &driver_attr_watchpoint_test
);
342 pr_err("Fail to create mt_hw_drv sysfs files");
345 driver_create_file(&hw_drv
, &driver_attr_watchpoint_test
);
348 hook_fault_code(2, watchpoint_handler
, SIGTRAP
, 0, "watchpoint debug exception");
349 printk(KERN_ALERT
"watchpoint handler init. \n");
356 //EXPORT_SYMBOL(add_hw_watchpoint);
358 arch_initcall(hw_watchpoint_init
);