Add Z/z protocal support for breakpoint set/remove.

Signed-off-by: Peng Fan <peng....@freescale.com>
---
 common/kgdb.c  | 273 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 include/kgdb.h |  35 ++++++++
 2 files changed, 308 insertions(+)

diff --git a/common/kgdb.c b/common/kgdb.c
index d357463..fd83ccd 100644
--- a/common/kgdb.c
+++ b/common/kgdb.c
@@ -92,6 +92,8 @@
 #include <kgdb.h>
 #include <command.h>
 
+#include <asm-generic/errno.h>
+
 #undef KGDB_DEBUG
 
 /*
@@ -111,6 +113,17 @@ static int longjmp_on_fault = 0;
 static int kdebug = 1;
 #endif
 
+struct kgdb_bkpt kgdb_break[KGDB_MAX_BREAKPOINTS] = {
+       [0 ... KGDB_MAX_BREAKPOINTS-1] = { .state = BP_UNDEFINED }
+};
+
+#ifdef CONFIG_ARM
+unsigned char gdb_bpt_instr[4] = {0xfe, 0xde, 0xff, 0xe7};
+#else
+#error "Please implement gdb_bpt_instr!"
+#endif
+
+
 static const char hexchars[]="0123456789abcdef";
 
 /* Convert ch from a hex digit to an int */
@@ -309,6 +322,200 @@ putpacket(unsigned char *buffer)
        } while ((recv & 0x7f) != '+');
 }
 
+int kgdb_validate_break_address(unsigned addr)
+{
+       /* Need More */
+       return 0;
+}
+
+static int probe_kernel_read(unsigned char *dst, void *src, size_t size)
+{
+       int i;
+       unsigned char *dst_ptr = dst;
+       unsigned char *src_ptr = src;
+
+       for (i = 0; i < size; i++)
+               *dst_ptr++ = *src_ptr++;
+
+       return 0;
+}
+
+static int probe_kernel_write(char *dst, void *src, size_t size)
+{
+       int i;
+       char *dst_ptr = dst;
+       char *src_ptr = src;
+
+       for (i = 0; i < size; i++)
+               *dst_ptr++ = *src_ptr++;
+
+       return 0;
+}
+
+__weak int kgdb_arch_set_breakpoint(struct kgdb_bkpt *bpt)
+{
+       int err;
+
+       err = probe_kernel_read(bpt->saved_instr, (char *)bpt->bpt_addr,
+                               BREAK_INSTR_SIZE);
+       if (err)
+               return err;
+
+       err = probe_kernel_write((char *)bpt->bpt_addr, gdb_bpt_instr,
+                                BREAK_INSTR_SIZE);
+
+       return err;
+}
+
+/*
+ * Set the breakpoints whose state is BP_SET to BP_ACTIVE
+ */
+int kgdb_active_sw_breakpoints(void)
+{
+       int err;
+       int ret = 0;
+       int i;
+
+       for (i = 0; i < KGDB_MAX_BREAKPOINTS; i++) {
+               if (kgdb_break[i].state != BP_SET)
+                       continue;
+
+               err = kgdb_arch_set_breakpoint(&kgdb_break[i]);
+               if (err) {
+                       ret = err;
+                       printf("KGDB: BP install failed: %lx\n",
+                              kgdb_break[i].bpt_addr);
+                       continue;
+               }
+
+               kgdb_break[i].state = BP_ACTIVE;
+
+               /*
+                * kgdb_arch_set_breakpoint touched dcache and memory.
+                * cache should be flushed to let icache can see the updated
+                * inst.
+                */
+               /* flush work is done in do_exit */
+               kgdb_flush_cache_all();
+       }
+
+       return ret;
+}
+
+/*
+ * Set state from BP_SET to BP_REMOVED
+ */
+int kgdb_remove_sw_breakpoint(unsigned int addr)
+{
+       int i;
+
+       for (i = 0; i < KGDB_MAX_BREAKPOINTS; i++) {
+               if ((kgdb_break[i].state == BP_SET) &&
+                   (kgdb_break[i].bpt_addr == addr)) {
+                       kgdb_break[i].state = BP_REMOVED;
+                       return 0;
+               }
+       }
+
+       return -ENOENT;
+}
+
+int kgdb_set_sw_breakpoint(unsigned int addr)
+{
+       int err = kgdb_validate_break_address(addr);
+       int breakno = -1;
+       int i;
+
+       if (err)
+               return err;
+
+       for (i = 0; i < KGDB_MAX_BREAKPOINTS; i++) {
+               if ((kgdb_break[i].state == BP_SET) &&
+                   (kgdb_break[i].bpt_addr == addr))
+                       return -EEXIST;
+       }
+
+       for (i = 0; i < KGDB_MAX_BREAKPOINTS; i++) {
+               /* removed or unused, use it */
+               if ((kgdb_break[i].state == BP_REMOVED) ||
+                   (kgdb_break[i].state == BP_UNDEFINED)) {
+                       breakno = i;
+                       break;
+               }
+       }
+
+       if (breakno == -1)
+               return -E2BIG;
+
+       kgdb_break[breakno].state = BP_SET;
+       kgdb_break[breakno].type = BP_BREAKPOINT;
+       kgdb_break[breakno].bpt_addr = addr;
+
+       return 0;
+}
+
+__weak int kgdb_arch_remove_breakpoint(struct kgdb_bkpt *bpt)
+{
+       return probe_kernel_write((char *)bpt->bpt_addr,
+                                 (char *)bpt->saved_instr, BREAK_INSTR_SIZE);
+}
+
+/*
+ * set breakpoints whose state is BP_ACTIVE to BP_SET
+ */
+int kgdb_deactivate_sw_breakpoints(void)
+{
+       int err;
+       int ret = 0;
+       int i;
+
+       for (i = 0; i < KGDB_MAX_BREAKPOINTS; i++) {
+               if (kgdb_break[i].state != BP_ACTIVE)
+                       continue;
+
+               err = kgdb_arch_remove_breakpoint(&kgdb_break[i]);
+               if (err) {
+                       printf("KGDB: BP remove failed: %lx\n",
+                              kgdb_break[i].bpt_addr);
+                       ret = err;
+               }
+
+               kgdb_break[i].state = BP_SET;
+
+               /*
+                * kgdb_arch_remove_breakpoint touched dcache and memory.
+                * cache should be flushed to let icache can see the updated
+                * inst.
+                */
+               kgdb_flush_cache_all();
+       }
+
+       return ret;
+}
+
+static int kgdb_remove_all_break(void)
+{
+       int err;
+       int i;
+
+       /* Clear memory breakpoints. */
+       for (i = 0; i < KGDB_MAX_BREAKPOINTS; i++) {
+               if (kgdb_break[i].state != BP_ACTIVE)
+                       goto setundefined;
+               err = kgdb_arch_remove_breakpoint(&kgdb_break[i]);
+               if (err)
+                       printf("KGDB: breakpoint remove failed: %lx\n",
+                              kgdb_break[i].bpt_addr);
+setundefined:
+               kgdb_break[i].state = BP_UNDEFINED;
+       }
+
+       /* clear hardware breakpoints. */
+       /* ToDo in future. */
+
+       return 0;
+}
+
 /*
  * This function does all command processing for interfacing to gdb.
  */
@@ -318,6 +525,7 @@ handle_exception (struct pt_regs *regs)
        int addr;
        int length;
        char *ptr;
+       char *bpt_type;
        kgdb_data kd;
        int i;
 
@@ -376,6 +584,12 @@ handle_exception (struct pt_regs *regs)
 
        putpacket((unsigned char *)&remcomOutBuffer);
 
+       /*
+        * Each time trigger a kgdb break, first deactive all active
+        * breakpoints.
+        */
+       kgdb_deactivate_sw_breakpoints();
+
        while (1) {
                volatile int errnum;
 
@@ -394,6 +608,8 @@ handle_exception (struct pt_regs *regs)
                if (errnum == 0) switch (remcomInBuffer[0]) {
 
                case '?':               /* report most recent signal */
+                       kgdb_remove_all_break();
+
                        remcomOutBuffer[0] = 'S';
                        remcomOutBuffer[1] = hexchars[kd.sigval >> 4];
                        remcomOutBuffer[2] = hexchars[kd.sigval & 0xf];
@@ -465,6 +681,8 @@ handle_exception (struct pt_regs *regs)
                                kd.extype |= KGDBEXIT_WITHADDR;
                        }
 
+                       kgdb_active_sw_breakpoints();
+
                        goto doexit;
 
                case 'S':    /* SSS  single step with signal SS */
@@ -479,6 +697,8 @@ handle_exception (struct pt_regs *regs)
                                kd.extype |= KGDBEXIT_WITHADDR;
                        }
 
+                       kgdb_active_sw_breakpoints();
+
                doexit:
 /* Need to flush the instruction cache here, as we may have deposited a
  * breakpoint, and the icache probably has no way of knowing that a data ref to
@@ -506,6 +726,59 @@ handle_exception (struct pt_regs *regs)
                                kgdb_error(KGDBERR_BADPARAMS);
                        }
                        break;
+               case 'z':
+                       /*
+                        * Break point remove
+                        * packet: zt,addr,length
+                        */
+               case 'Z':
+                       /*
+                        * Break point set
+                        * packet: Zt,addr,length
+                        *
+                        * t is type: `0' - software breakpoint,
+                        * `1' - hardware breakpoint, `2' - write watchpoint,
+                        * `3' - read watchpoint, `4' - access watchpoint;
+                        * addr is address; length is in bytes. For a software
+                        * breakpoint, length specifies the size of the
+                        * instruction to be patched. For hardware breakpoints
+                        * and watchpoints length specifies the memory region
+                        * to be monitored. To avoid potential problems with
+                        * duplicate packets, the operations should be
+                        * implemented in an idempotent way.
+                        */
+                       bpt_type = &remcomInBuffer[1];
+                       ptr = &remcomInBuffer[2];
+
+                       if (*(ptr++) != ',') {
+                               errnum = -EINVAL;
+                               break;
+                       }
+
+                       if (!hexToInt(&ptr, &addr)) {
+                               errnum = -EINVAL;
+                               break;
+                       }
+
+                       if (*ptr++ != ',') {
+                               errnum = -EINVAL;
+                               break;
+                       }
+
+                       /* only software breakpoint is implemented */
+                       if ((remcomInBuffer[0] == 'Z') && (*bpt_type == '0')) {
+                               errnum = kgdb_set_sw_breakpoint(addr);
+                       } else if ((remcomInBuffer[0] == 'z') &&
+                                (*bpt_type == '0')) {
+                               errnum = kgdb_remove_sw_breakpoint(addr);
+                       } else {
+                               /* Unsupported */
+                               errnum = -EINVAL;
+                       }
+
+                       if (errnum == 0)
+                               strcpy(remcomOutBuffer, "OK");
+                       break;
                }                       /* switch */
 
                if (errnum != 0)
diff --git a/include/kgdb.h b/include/kgdb.h
index b6ba742..f9152b5 100644
--- a/include/kgdb.h
+++ b/include/kgdb.h
@@ -20,6 +20,12 @@
 
 #define KGDBEXIT_WITHADDR      0x100
 
+#if defined(CONFIG_ARM)
+#define BREAK_INSTR_SIZE       4
+#else
+#error "BREAK_INSTR_SIZE not set"
+#endif
+
 typedef
        struct {
                int num;
@@ -38,6 +44,29 @@ typedef
        }
 kgdb_data;
 
+enum kgdb_bptype {
+       BP_BREAKPOINT = 0,
+       BP_HARDWARE_BREAKPOINT,
+       BP_WRITE_WATCHPOINT,
+       BP_READ_WATCHPOINT,
+       BP_ACCESS_WATCHPOINT,
+       BP_POKE_BREAKPOINT,
+};
+
+enum kgdb_bpstate {
+       BP_UNDEFINED = 0,
+       BP_REMOVED,
+       BP_SET,
+       BP_ACTIVE
+};
+
+struct kgdb_bkpt {
+       unsigned long           bpt_addr;
+       unsigned char           saved_instr[BREAK_INSTR_SIZE];
+       enum kgdb_bptype        type;
+       enum kgdb_bpstate       state;
+};
+
 /* these functions are provided by the generic kgdb support */
 extern void kgdb_init(void);
 extern void kgdb_error(int);
@@ -67,4 +96,10 @@ extern void kgdb_interruptible(int);
 /* this is referenced in the trap handler for the platform */
 extern int (*debugger_exception_handler)(struct pt_regs *);
 
+int kgdb_set_sw_break(unsigned int addr);
+int kgdb_remove_sw_break(unsigned int addr);
+int kgdb_validate_break_address(unsigned int addr);
+
+#define KGDB_MAX_BREAKPOINTS   32
+
 #endif /* __KGDB_H__ */
-- 
1.8.4.5

_______________________________________________
U-Boot mailing list
U-Boot@lists.denx.de
http://lists.denx.de/mailman/listinfo/u-boot

Reply via email to