major rework lm32-target
authorMichael Walle <michael@walle.cc>
Sun, 7 Nov 2010 21:31:15 +0000 (22:31 +0100)
committerMichael Walle <michael@walle.cc>
Sun, 7 Nov 2010 21:31:15 +0000 (22:31 +0100)
src/target/lm32.c

index 9ed5683..8eb1415 100644 (file)
 #include "target.h"
 #include "target_type.h"
 
+/* TODO
+ *  - error handling
+ *  - disable hw breakpoints upon reset (better do a jtag core reset?)
+ */
+
 /*
  * The LM32 debug core is accessed through a JTAG data register. It has the
  * following layout:
@@ -62,11 +67,18 @@ struct lm32
        /* current state of the debug handler */
        int handler_running;
 
-       int num_bps_avail;
+       int num_bps;
+       int num_wps;
        /* bitfield, bit 0 corresponds to bp0 */
        uint32_t bps_used;
+       uint32_t wps_used;
+
+       uint32_t dc_csr;
 
        uint32_t ir_insn;
+
+       bool dp_enabled;
+       bool ignore_ack;
 };
 
 struct lm32_reg
@@ -92,6 +104,18 @@ enum
        LM32_EBA, LM32_DEBA, LM32_NUM_REGS
 };
 
+/* exception ids */
+enum {
+       LM32_EID_RESET = 0,
+       LM32_EID_BREAKPOINT,
+       LM32_EID_IBUS_ERROR,
+       LM32_EID_WATCHPOINT,
+       LM32_EID_DBUS_ERROR,
+       LM32_EID_DIVIDE_BY_ZERO,
+       LM32_EID_INTERRUPT,
+       LM32_EID_SYSTEMCALL,
+};
+
 static const struct lm32_reg lm32_reg_arch_info[] =
 {
        { LM32_R0, NULL }, { LM32_R1, NULL }, { LM32_R2, NULL }, { LM32_R3, NULL },
@@ -127,39 +151,86 @@ enum
 
 /* status bits */
 enum {
-       LM32_STAT_RX_READY   = 1 << 0,
-       LM32_STAT_TX_FULL    = 1 << 1,
-       LM32_STAT_PROCESSING = 1 << 2,
+       LM32_STAT_RX_READY   = (1 << 0),
+       LM32_STAT_TX_FULL    = (1 << 1),
+       LM32_STAT_PROCESSING = (1 << 2),
 };
 
 /* monitor commands */
 enum {
-       LM32_MONITOR_CMD_REG_ADDR = 0,
+       LM32_MONITOR_CMD_VERSION = 0,
+       LM32_MONITOR_CMD_REG_ADDR,
        LM32_MONITOR_CMD_READ_CSR,
        LM32_MONITOR_CMD_WRITE_CSR,
        LM32_MONITOR_CMD_READ_MEM,
        LM32_MONITOR_CMD_WRITE_MEM,
+       LM32_MONITOR_CMD_STORE_HALFWORD,
+       LM32_MONITOR_CMD_STORE_WORD,
+       LM32_MONITOR_CMD_LOAD_HALFWORD,
+       LM32_MONITOR_CMD_LOAD_WORD,
        LM32_MONITOR_CMD_CONTINUE,
 };
 
 enum {
-       LM32_MONITOR_READ_CSR_CFG = 0,
+       LM32_CSR_IE = 0,
+       LM32_CSR_IM,
+       LM32_CSR_IP,
+       LM32_CSR_ICC,
+       LM32_CSR_DCC,
+       LM32_CSR_CC,
+       LM32_CSR_CFG,
+       LM32_CSR_EBA,
+       LM32_CSR_DC,
+       LM32_CSR_DEBA,
+       LM32_CSR_JTX,
+       LM32_CSR_JRX,
+       LM32_CSR_BP0,
+       LM32_CSR_BP1,
+       LM32_CSR_BP2,
+       LM32_CSR_BP3,
+       LM32_CSR_WP0,
+       LM32_CSR_WP1,
+       LM32_CSR_WP2,
+       LM32_CSR_WP3,
+};
+
+enum {
+       LM32_CSR_DC_SS = (1 << 0),
+       LM32_CSR_DC_RE = (1 << 1),
+       LM32_CSR_DC_C0 = (1 << 2),
+       LM32_CSR_DC_C1 = (1 << 3),
+       LM32_CSR_DC_C2 = (1 << 4),
+       LM32_CSR_DC_C3 = (1 << 5),
 };
 
 enum {
-       LM32_MONITOR_WRITE_CSR_EBA = 0,
-       LM32_MONITOR_WRITE_CSR_DC,
-       LM32_MONITOR_WRITE_CSR_DEBA,
-       LM32_MONITOR_WRITE_CSR_BP0,
-       LM32_MONITOR_WRITE_CSR_BP1,
-       LM32_MONITOR_WRITE_CSR_BP2,
-       LM32_MONITOR_WRITE_CSR_BP3,
-       LM32_MONITOR_WRITE_CSR_WP0,
-       LM32_MONITOR_WRITE_CSR_WP1,
-       LM32_MONITOR_WRITE_CSR_WP2,
-       LM32_MONITOR_WRITE_CSR_WP3,
+       LM32_CSR_CFG_M  = (1 << 0),
+       LM32_CSR_CFG_D  = (1 << 1),
+       LM32_CSR_CFG_S  = (1 << 2),
+       LM32_CSR_CFG_U  = (1 << 3),
+       LM32_CSR_CFG_X  = (1 << 4),
+       LM32_CSR_CFG_CC = (1 << 5),
+       LM32_CSR_CFG_IC = (1 << 6),
+       LM32_CSR_CFG_DC = (1 << 7),
+       LM32_CSR_CFG_G  = (1 << 8),
+       LM32_CSR_CFG_H  = (1 << 9),
+       LM32_CSR_CFG_R  = (1 << 10),
+       LM32_CSR_CFG_J  = (1 << 11),
 };
 
+#define LM32_CSR_CFG_INT(cfg) ((cfg >> 12) & 0x3f)
+#define LM32_CSR_CFG_BP(cfg)  ((cfg >> 18) & 0x0f)
+#define LM32_CSR_CFG_WP(cfg)  ((cfg >> 22) & 0x0f)
+#define LM32_CSR_CFG_REV(cfg) ((cfg >> 26) & 0x3f)
+
+/*
+ * helper
+ */
+static inline struct lm32* target_to_lm32(struct target *target)
+{
+       return target->arch_info;
+}
+
 /*
  * jtag functions
  */
@@ -189,7 +260,7 @@ static int
 lm32_jtag_datar(struct target *target, uint32_t data_in, uint32_t *data_out)
 {
        int ret;
-       uint8_t out_value[2];
+       uint8_t out_value[4];
        struct scan_field field;
 
        buf_set_u32(out_value, 0, 11, data_in);
@@ -214,46 +285,6 @@ lm32_jtag_datar(struct target *target, uint32_t data_in, uint32_t *data_out)
        return ERROR_OK;
 }
 
-/*
- * helper functions
- */
-
-static int
-lm32_dp_send_data(struct target *target, uint8_t data, int flush)
-{
-       if (lm32_jtag_datar(target, (data << 3) | LM32_ADDR_DP, NULL) != ERROR_OK)
-               return ERROR_FAIL;
-
-       if (flush)
-               if (jtag_execute_queue() != ERROR_OK)
-                       return ERROR_FAIL;
-
-       return ERROR_OK;
-}
-
-static int
-lm32_dp_receive_data(struct target *target, uint8_t *data)
-{
-       uint32_t tmp=0;
-       int ret;
-
-       ret = lm32_jtag_datar(target, (LM32_DP_NOP << 7) | LM32_ADDR_DP, &tmp);
-       if (ret != ERROR_OK)
-               return ret;
-
-       LOG_DEBUG("tmp=%03x", tmp);
-
-       *data = (tmp >> 3) & 0xff;
-
-       return ERROR_OK;
-}
-
-static int
-lm32_dp_send_command(struct target *target, uint8_t cmd, int flush)
-{
-       return lm32_dp_send_data(target, cmd << 4, flush);
-}
-
 static int
 lm32_get_status(struct target *target, uint8_t *status)
 {
@@ -275,12 +306,14 @@ lm32_wait_for_status(struct target *target, uint8_t mask, uint8_t value)
        struct timeval timeout, now;
        uint8_t status;
        int ret;
+       int cnt = 0;
 
 #define DEFAULT_TIMEOUT 1
 
        gettimeofday(&timeout, NULL);
        timeval_add_time(&timeout, DEFAULT_TIMEOUT, 0);
-       for (;;)
+
+       while (true)
        {
                if ((ret = lm32_get_status(target, &status)) != ERROR_OK) {
                        LOG_ERROR("error reading jtag status");
@@ -301,13 +334,17 @@ lm32_wait_for_status(struct target *target, uint8_t mask, uint8_t value)
                } else {
                        keep_alive();
                }
+               cnt++;
        }
 
+       if (debug_level >= 4)
+               LOG_DEBUG("waited for %d loops", cnt);
+
        return ERROR_OK;
 }
 
 static int
-lm32_read_rx(struct target *target, uint8_t *data)
+lm32_read_jrx(struct target *target, uint8_t *data)
 {
        int ret;
        uint32_t tmp_data;
@@ -337,7 +374,7 @@ lm32_read_rx(struct target *target, uint8_t *data)
 }
 
 static int
-lm32_write_tx(struct target *target, uint8_t data)
+lm32_write_jtx(struct target *target, uint8_t data)
 {
        int ret;
 
@@ -356,31 +393,239 @@ lm32_write_tx(struct target *target, uint8_t data)
 }
 
 static int
-lm32_send_u32(struct target *target, uint32_t data)
+lm32_dp_send_data(struct target *target, uint8_t data, int flush)
+{
+       if (lm32_jtag_datar(target, (data << 3) | LM32_ADDR_DP, NULL) != ERROR_OK)
+               return ERROR_FAIL;
+
+       if (flush)
+               if (jtag_execute_queue() != ERROR_OK)
+                       return ERROR_FAIL;
+
+       return ERROR_OK;
+}
+
+static int
+lm32_dp_receive_data(struct target *target, uint8_t *data)
+{
+       uint32_t tmp=0;
+       int ret;
+
+       ret = lm32_jtag_datar(target, (LM32_DP_NOP << 7) | LM32_ADDR_DP, &tmp);
+       if (ret != ERROR_OK)
+               return ret;
+
+       *data = (tmp >> 3) & 0xff;
+
+       return ERROR_OK;
+}
+
+static int
+lm32_dp_send_command(struct target *target, uint8_t cmd, int flush)
+{
+       return lm32_dp_send_data(target, cmd << 4, flush);
+}
+
+/*
+ * helper functions
+ */
+
+static int
+lm32_mon_send_u16(struct target *target, uint16_t data)
+{
+       int ret;
+
+       ret = lm32_write_jtx(target, (data >> 8) & 0xff);
+       if (ret != ERROR_OK)
+               return ret;
+
+       ret = lm32_write_jtx(target, data & 0xff);
+       if (ret != ERROR_OK)
+               return ret;
+
+       return ERROR_OK;
+}
+
+static int
+lm32_mon_send_u32(struct target *target, uint32_t data)
+{
+       int ret;
+
+       ret = lm32_write_jtx(target, data & 0xff);
+       if (ret != ERROR_OK)
+               return ret;
+
+       ret = lm32_write_jtx(target, (data >> 8) & 0xff);
+       if (ret != ERROR_OK)
+               return ret;
+
+       ret = lm32_write_jtx(target, (data >> 16) & 0xff);
+       if (ret != ERROR_OK)
+               return ret;
+
+       ret = lm32_write_jtx(target, (data >> 24) & 0xff);
+       if (ret != ERROR_OK)
+               return ret;
+
+       return ERROR_OK;
+}
+
+static int
+lm32_mon_receive_u16(struct target *target, uint16_t *data)
+{
+       int ret;
+       uint8_t byte;
+
+       *data = 0;
+
+       ret = lm32_read_jrx(target, &byte);
+       if (ret != ERROR_OK)
+               return ret;
+       *data |= byte;
+
+       ret = lm32_read_jrx(target, &byte);
+       if (ret != ERROR_OK)
+               return ret;
+       *data |= byte << 8;
+
+       return ERROR_OK;
+}
+
+static int
+lm32_mon_receive_u32(struct target *target, uint32_t *data)
+{
+       int ret;
+       uint8_t byte;
+
+       *data = 0;
+
+       ret = lm32_read_jrx(target, &byte);
+       if (ret != ERROR_OK)
+               return ret;
+       *data |= byte;
+
+       ret = lm32_read_jrx(target, &byte);
+       if (ret != ERROR_OK)
+               return ret;
+       *data |= byte << 8;
+
+       ret = lm32_read_jrx(target, &byte);
+       if (ret != ERROR_OK)
+               return ret;
+       *data |= byte << 16;
+
+       ret = lm32_read_jrx(target, &byte);
+       if (ret != ERROR_OK)
+               return ret;
+       *data |= byte << 24;
+
+       return ERROR_OK;
+}
+
+static int
+lm32_dp_send_u32(struct target *target, uint32_t data, int flush)
 {
-       /* XXX endianness */
-       int i, ret;
-       uint8_t *buf = (uint8_t*)(&data);
+       int i;
+       int ret;
 
-       for (i = 0; i < 4; i++)
+       for (i = 3; i >= 0; i--)
        {
-               if ((ret = lm32_write_tx(target, buf[i])) != ERROR_OK)
+               ret = lm32_dp_send_data(target, (data >> (8*i)) & 0xff, (i==0) ? flush : 0);
+               if (ret != ERROR_OK)
                        return ret;
        }
 
        return ERROR_OK;
 }
 
+/*
+ * monitor commands
+ */
+
+static int
+lm32_mon_version(struct target *target, uint8_t *version)
+{
+       int ret;
+
+       ret = lm32_write_jtx(target, LM32_MONITOR_CMD_VERSION);
+       if (ret != ERROR_OK)
+               return ret;
+
+       return lm32_read_jrx(target, version);
+}
+
+static int
+lm32_mon_write_csr(struct target *target, uint8_t csr, uint32_t value)
+{
+       int ret;
+
+       static const uint8_t mon_csr_map[] = {
+               0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00,
+               0x01, 0x02, 0xff, 0xff, 0x03, 0x04, 0x05, 0x06,
+               0x07, 0x08, 0x09, 0x0a
+       };
+       assert(csr < ARRAY_SIZE(mon_csr_map));
+       assert(mon_csr_map[csr] != 0xff);
+
+       ret = lm32_write_jtx(target, LM32_MONITOR_CMD_WRITE_CSR);
+       if (ret != ERROR_OK)
+               return ret;
+
+       ret = lm32_write_jtx(target, mon_csr_map[csr]);
+       if (ret != ERROR_OK)
+               return ret;
+
+       return lm32_mon_send_u32(target, value);
+}
+
 static int
-lm32_recv_u32(struct target *target, uint32_t *data)
+lm32_mon_read_csr(struct target *target, uint8_t csr, uint32_t *value)
 {
-       /* XXX endianness */
-       int i, ret;
-       uint8_t *buf = (uint8_t*)data;
+       int ret;
 
-       for (i = 0; i < 4; i++)
+       static const uint8_t mon_csr_map[] = {
+               0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0xff,
+               0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+               0xff, 0xff, 0xff, 0xff
+       };
+       assert(csr < ARRAY_SIZE(mon_csr_map));
+       assert(mon_csr_map[csr] != 0xff);
+
+       ret = lm32_write_jtx(target, LM32_MONITOR_CMD_READ_CSR);
+       if (ret != ERROR_OK)
+               return ret;
+
+       ret = lm32_write_jtx(target, mon_csr_map[csr]);
+       if (ret != ERROR_OK)
+               return ret;
+
+       return lm32_mon_receive_u32(target, value);
+}
+
+static int
+lm32_mon_read_memory(struct target *target, uint32_t dest, uint32_t len, uint8_t *buf)
+{
+       int ret;
+       uint32_t address;
+       uint32_t end_address = dest + len;
+
+       /* send memory read request */
+       if ((ret = lm32_write_jtx(target, LM32_MONITOR_CMD_READ_MEM)) != ERROR_OK)
+               return ret;
+       
+       /* send base address */
+       if ((ret = lm32_mon_send_u32(target, dest)) != ERROR_OK)
+               return ret;
+
+       /* send length */
+       if ((ret = lm32_mon_send_u32(target, len)) != ERROR_OK)
+               return ret;
+
+       /* receive data */
+       for (address = dest; address < end_address; address++)
        {
-               if ((ret = lm32_read_rx(target, buf+i)) != ERROR_OK)
+               ret = lm32_read_jrx(target, buf++);
+               if (ret != ERROR_OK)
                        return ret;
        }
 
@@ -388,41 +633,182 @@ lm32_recv_u32(struct target *target, uint32_t *data)
 }
 
 static int
-lm32_dp_read_memory(struct target *target, uint32_t address, uint8_t *data)
+lm32_mon_write_memory(struct target *target, uint32_t dest, uint32_t len, uint8_t *buf)
 {
        int ret;
-       int i;
+       uint32_t address;
+       uint32_t end_address = dest + len;
 
-       ret = lm32_dp_send_command(target, LM32_DP_READ_MEMORY, 0);
-       if (ret != ERROR_OK)
+       /* send memory write request */
+       if ((ret = lm32_write_jtx(target, LM32_MONITOR_CMD_WRITE_MEM)) != ERROR_OK)
                return ret;
        
-       /* now send address */
-       for (i = 3; i >= 0; i--)
+       /* send base address */
+       if ((ret = lm32_mon_send_u32(target, dest)) != ERROR_OK)
+               return ret;
+
+       /* send length */
+       if ((ret = lm32_mon_send_u32(target, len)) != ERROR_OK)
+               return ret;
+
+       /* send data */
+       for (address = dest; address < end_address; address++)
        {
-               ret = lm32_dp_send_data(target, (address >> (8*i)) & 0xff, (i==0) ? 1 : 0);
+               ret = lm32_write_jtx(target, *(buf++));
                if (ret != ERROR_OK)
                        return ret;
        }
 
-       /* wait for completion */
-       ret = lm32_wait_for_status(target, LM32_STAT_PROCESSING, 0);
+       return ERROR_OK;
+}
+
+static int
+lm32_mon_store_u16(struct target *target, uint32_t dest, uint16_t data)
+{
+       int ret;
+
+       LOG_DEBUG("dest=0x%08x data=%04x", dest, data);
+
+       ret = lm32_write_jtx(target, LM32_MONITOR_CMD_STORE_HALFWORD);
        if (ret != ERROR_OK)
                return ret;
 
-       /* read byte */
-       return lm32_dp_receive_data(target, data);
+       ret = lm32_mon_send_u32(target, dest);
+       if (ret != ERROR_OK)
+               return ret;
+
+       ret = lm32_mon_send_u16(target, data);
+       if (ret != ERROR_OK)
+               return ret;
+
+       return ERROR_OK;
 }
 
 static int
-lm32_dp_read_sequential(struct target *target, uint8_t *data)
+lm32_mon_store_u32(struct target *target, uint32_t dest, uint32_t data)
 {
        int ret;
 
-       ret = lm32_dp_send_command(target, LM32_DP_READ_SEQUENTIAL, 1);
+       LOG_DEBUG("dest=0x%08x data=%08x", dest, data);
+
+       ret = lm32_write_jtx(target, LM32_MONITOR_CMD_STORE_WORD);
+       if (ret != ERROR_OK)
+               return ret;
+
+       ret = lm32_mon_send_u32(target, dest);
+       if (ret != ERROR_OK)
+               return ret;
+
+       ret = lm32_mon_send_u32(target, data);
+       if (ret != ERROR_OK)
+               return ret;
+
+       return ERROR_OK;
+}
+
+static int
+lm32_mon_load_u16(struct target *target, uint32_t src, uint16_t *data)
+{
+       int ret;
+
+       ret = lm32_write_jtx(target, LM32_MONITOR_CMD_LOAD_HALFWORD);
+       if (ret != ERROR_OK)
+               return ret;
+
+       ret = lm32_mon_send_u32(target, src);
+       if (ret != ERROR_OK)
+               return ret;
+
+       ret = lm32_mon_receive_u16(target, data);
+       if (ret != ERROR_OK)
+               return ret;
+
+       LOG_DEBUG("src=0x%08x data=%04x", src, *data);
+
+       return ERROR_OK;
+}
+
+static int
+lm32_mon_load_u32(struct target *target, uint32_t src, uint32_t *data)
+{
+       int ret;
+
+       ret = lm32_write_jtx(target, LM32_MONITOR_CMD_LOAD_WORD);
+       if (ret != ERROR_OK)
+               return ret;
+
+       ret = lm32_mon_send_u32(target, src);
+       if (ret != ERROR_OK)
+               return ret;
+
+       ret = lm32_mon_receive_u32(target, data);
+       if (ret != ERROR_OK)
+               return ret;
+
+       LOG_DEBUG("src=0x%08x data=%08x", src, *data);
+
+       return ERROR_OK;
+}
+
+static int
+lm32_mon_continue(struct target *target)
+{
+       return lm32_write_jtx(target, LM32_MONITOR_CMD_CONTINUE);
+}
+
+static int
+lm32_mon_registers_address(struct target *target, uint32_t *addr)
+{
+       int ret;
+
+       ret = lm32_write_jtx(target, LM32_MONITOR_CMD_REG_ADDR);
+       if (ret != ERROR_OK)
+               return ret;
+
+       ret = lm32_mon_receive_u32(target, addr);
+       if (ret != ERROR_OK)
+               return ret;
+
+       return ERROR_OK;
+}
+
+/*
+ * debug protocol commands
+ */
+static int
+lm32_dp_write_csr(struct target *target, uint8_t csr, uint32_t value)
+{
+       int ret;
+
+       ret = lm32_dp_send_command(target, LM32_DP_WRITE_CSR, 0);
+       if (ret != ERROR_OK)
+               return ret;
+
+       ret = lm32_dp_send_u32(target, value, 0);
        if (ret != ERROR_OK)
                return ret;
        
+       ret = lm32_dp_send_data(target, csr, 1);
+       if (ret != ERROR_OK)
+               return ret;
+
+       return ERROR_OK;
+}
+
+static int
+lm32_dp_read_memory_addr(struct target *target, uint32_t address, uint8_t *data)
+{
+       int ret;
+
+       ret = lm32_dp_send_command(target, LM32_DP_READ_MEMORY, 0);
+       if (ret != ERROR_OK)
+               return ret;
+       
+       /* now send address */
+       ret = lm32_dp_send_u32(target, address, 1);
+       if (ret != ERROR_OK)
+               return ret;
+
        /* wait for completion */
        ret = lm32_wait_for_status(target, LM32_STAT_PROCESSING, 0);
        if (ret != ERROR_OK)
@@ -433,56 +819,161 @@ lm32_dp_read_sequential(struct target *target, uint8_t *data)
 }
 
 static int
-lm32_dp_write_memory(struct target *target, uint32_t address, uint8_t data)
+lm32_dp_read_sequential(struct target *target, uint8_t *data, int wait)
 {
        int ret;
-       int i;
 
-       ret = lm32_dp_send_command(target, LM32_DP_WRITE_MEMORY, 0);
+       ret = lm32_dp_send_command(target, LM32_DP_READ_SEQUENTIAL, 1);
        if (ret != ERROR_OK)
                return ret;
        
-       /* now send address */
-       for (i = 3; i >= 0; i--)
+       if (wait)
        {
-               ret = lm32_dp_send_data(target, (address >> (8*i)) & 0xff, 0);
+               /* wait for completion */
+               ret = lm32_wait_for_status(target, LM32_STAT_PROCESSING, 0);
                if (ret != ERROR_OK)
                        return ret;
        }
 
+       /* read byte */
+       return lm32_dp_receive_data(target, data);
+}
+
+static int
+lm32_dp_write_memory_addr(struct target *target, uint32_t address, uint8_t data)
+{
+       int ret;
+
+       if (debug_level >= 4)
+               LOG_DEBUG("address=%02x data=0x%02x", address, data);
+
+       ret = lm32_dp_send_command(target, LM32_DP_WRITE_MEMORY, 0);
+       if (ret != ERROR_OK)
+               return ret;
+       
+       /* now send address */
+       ret = lm32_dp_send_u32(target, address, 0);
+       if (ret != ERROR_OK)
+               return ret;
+
        /* and the byte to write */
        ret = lm32_dp_send_data(target, data, 1);
        if (ret != ERROR_OK)
                return ret;
 
-       /* wait for completion */
        return lm32_wait_for_status(target, LM32_STAT_PROCESSING, 0);
 }
 
 static int
-lm32_dp_write_sequential(struct target *target, uint8_t data)
+lm32_dp_write_sequential(struct target *target, uint8_t data, bool wait)
 {
        int ret;
 
+       if (debug_level >= 4)
+               LOG_DEBUG("data=0x%02x", data);
+
        ret = lm32_dp_send_command(target, LM32_DP_WRITE_SEQUENTIAL, 0);
        if (ret != ERROR_OK)
                return ret;
        
-       ret = lm32_dp_send_data(target, data, 1);
+       ret = lm32_dp_send_data(target, data, 0);
        if (ret != ERROR_OK)
                return ret;
 
-       /* wait for completion */
-       return lm32_wait_for_status(target, LM32_STAT_PROCESSING, 0);
+       if (wait)
+               return lm32_wait_for_status(target, LM32_STAT_PROCESSING, 0);
+       else
+               return ERROR_OK;
+}
+
+static int
+lm32_dp_read_memory(struct target *target, uint32_t dest, uint32_t len, uint8_t *buf)
+{
+       struct lm32 *lm32 = target_to_lm32(target);
+       int ret;
+       uint32_t address;
+       uint32_t end_address = dest + len;
+
+       for (address = dest; address < end_address; address++)
+       {
+               /* send address if this is the first call or if we cross a 64k boundary */
+               if ((address == dest) || ((address & 0xffff) == 0))
+               {
+                       ret = lm32_dp_read_memory_addr(target, address, buf++);
+                       if (ret != ERROR_OK)
+                               return ret;
+               } else {
+                       ret = lm32_dp_read_sequential(target, buf++, (lm32->ignore_ack) ? 0 : 1);
+                       if (ret != ERROR_OK)
+                               return ret;
+               }
+       }
+
+       /* flush the JTAG buffer, in case we haven't done it yet */
+       ret = jtag_execute_queue();
+       if (ret != ERROR_OK)
+               return ret;
+
+       return ERROR_OK;
+}
+
+static int
+lm32_dp_write_memory(struct target *target, uint32_t dest, uint32_t len, uint8_t *buf)
+{
+       struct lm32 *lm32 = target_to_lm32(target);
+       int ret;
+       uint32_t address;
+       uint32_t end_address = dest + len;
+
+       for (address = dest; address < end_address; address++)
+       {
+               /* send address if this is the first call or if we cross a 64k boundary */
+               if ((address == dest) || ((address & 0xffff) == 0))
+               {
+                       ret = lm32_dp_write_memory_addr(target, address, *(buf++));
+                       if (ret != ERROR_OK)
+                               return ret;
+               } else {
+                       ret = lm32_dp_write_sequential(target, *(buf++), (lm32->ignore_ack) ? 0 : 1);
+                       if (ret != ERROR_OK)
+                               return ret;
+               }
+       }
+
+       /* flush the JTAG buffer, in case we haven't done it yet */
+       ret = jtag_execute_queue();
+       if (ret != ERROR_OK)
+               return ret;
+
+       return ERROR_OK;
+}
+
+
+/*
+ * wrapper functions
+ */
+
+static int
+lm32_write_csr(struct target *target, uint8_t csr, uint32_t value)
+{
+       struct lm32 *lm32 = target_to_lm32(target);
+
+       LOG_DEBUG("setting csr %d to %08x", csr, value);
+
+       if (lm32->dp_enabled)
+               return lm32_dp_write_csr(target, csr, value);
+       else
+               return lm32_mon_write_csr(target, csr, value);
 }
 
 static int
-lm32_read_memory(struct target *target, uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer)
+lm32_read_memory(struct target *target, uint32_t dest, uint32_t size, uint32_t count, uint8_t *buf)
 {
-       uint32_t u;
+       struct lm32 *lm32 = target_to_lm32(target);
+       unsigned int u;
        int ret;
 
-       LOG_DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
+       LOG_DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", dest, size, count);
 
        if (target->state != TARGET_HALTED)
        {
@@ -491,42 +982,60 @@ lm32_read_memory(struct target *target, uint32_t address, uint32_t size, uint32_
        }
 
        /* sanitize arguments */
-       if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
+       if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buf))
                return ERROR_INVALID_ARGUMENTS;
        
-       if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
+       if (((size == 4) && (dest & 0x3)) || ((size == 2) && (dest & 0x1)))
                return ERROR_TARGET_UNALIGNED_ACCESS;
        
-       /* send memory read request */
-       if ((ret = lm32_write_tx(target, LM32_MONITOR_CMD_READ_MEM)) != ERROR_OK)
-               return ret;
-       
-       /* send base address */
-       if ((ret = lm32_send_u32(target, address)) != ERROR_OK)
-               return ret;
-
-       /* send length */
-       if ((ret = lm32_send_u32(target, count*size)) != ERROR_OK)
-               return ret;
-
-       /* receive data */
-       /* XXX endianess */
-       for (u = 0; u < count*size; u++)
+       switch (size)
        {
-               if ((ret = lm32_read_rx(target, buffer+u)) != ERROR_OK)
-                       return ret;
+               case 4:
+                       for (u = 0; u < count; u++)
+                       {
+                               uint32_t data;
+                               ret = lm32_mon_load_u32(target, dest, &data);
+                               if (ret != ERROR_OK)
+                                       return ret;
+
+                               target_buffer_set_u32(target, buf, data);
+                               dest += 4;
+                               buf += 4;
+                       }
+                       break;
+               case 2:
+                       for (u = 0; u < count; u++)
+                       {
+                               uint16_t data;
+                               ret = lm32_mon_load_u16(target, dest, &data);
+                               if (ret != ERROR_OK)
+                                       return ret;
+
+                               target_buffer_set_u16(target, buf, data);
+                               dest += 2;
+                               buf += 2;
+                       }
+                       break;
+               case 1:
+                       if (lm32->dp_enabled)
+                               return lm32_dp_read_memory(target, dest, count, buf);
+                       else
+                               return lm32_mon_read_memory(target, dest, count, buf);
+               default:
+                       return ERROR_INVALID_ARGUMENTS;
        }
 
        return ERROR_OK;
 }
 
 static int
-lm32_write_memory(struct target *target, uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer)
+lm32_write_memory(struct target *target, uint32_t dest, uint32_t size, uint32_t count, uint8_t *buf)
 {
-       uint32_t u;
+       struct lm32 *lm32 = target_to_lm32(target);
+       unsigned int u;
        int ret;
 
-       LOG_DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
+       LOG_DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", dest, size, count);
 
        if (target->state != TARGET_HALTED)
        {
@@ -535,44 +1044,56 @@ lm32_write_memory(struct target *target, uint32_t address, uint32_t size, uint32
        }
 
        /* sanitize arguments */
-       if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
+       if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buf))
                return ERROR_INVALID_ARGUMENTS;
 
-       if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
+       if (((size == 4) && (dest & 0x3)) || ((size == 2) && (dest & 0x1)))
                return ERROR_TARGET_UNALIGNED_ACCESS;
 
-       /* send memory write request */
-       if ((ret = lm32_write_tx(target, LM32_MONITOR_CMD_WRITE_MEM)) != ERROR_OK)
-               return ret;
-       
-       /* send base address */
-       if ((ret = lm32_send_u32(target, address)) != ERROR_OK)
-               return ret;
+       switch (size)
+       {
+               case 4:
+                       for (u = 0; u < count; u++)
+                       {
+                               ret = lm32_mon_store_u32(target, dest, target_buffer_get_u32(target, buf));
+                               if (ret != ERROR_OK)
+                                       return ret;
 
-       /* send length */
-       if ((ret = lm32_send_u32(target, count*size)) != ERROR_OK)
-               return ret;
+                               dest += 4;
+                               buf += 4;
+                       }
+                       break;
+               case 2:
+                       for (u = 0; u < count; u++)
+                       {
+                               ret = lm32_mon_store_u16(target, dest, target_buffer_get_u16(target, buf));
+                               if (ret != ERROR_OK)
+                                       return ret;
 
-       /* send data */
-       /* XXX endianess */
-       for (u = 0; u < count*size; u++)
-       {
-               if ((ret = lm32_write_tx(target, buffer[u])) != ERROR_OK)
-                       return ret;
-#if 0
-#define KEEPALIVE_COUNT 200
-               if (u % KEEPALIVE_COUNT == 0)
-                       keep_alive();
-#endif
+                               dest += 2;
+                               buf += 2;
+                       }
+                       break;
+               case 1:
+                       if (lm32->dp_enabled)
+                               return lm32_dp_write_memory(target, dest, count, buf);
+                       else
+                               return lm32_mon_write_memory(target, dest, count, buf);
+               default:
+                       return ERROR_INVALID_ARGUMENTS;
        }
 
        return ERROR_OK;
 }
 
-static int
+/*
+ * target functions
+ */
+
+static void
 lm32_invalidate_regs(struct target *target)
 {
-       struct lm32 *lm32 = target->arch_info;
+       struct lm32 *lm32 = target_to_lm32(target);
        int i;
 
        for (i = 0; i < LM32_NUM_REGS; i++)
@@ -580,6 +1101,80 @@ lm32_invalidate_regs(struct target *target)
                lm32->reg_cache->reg_list[i].valid = 0;
                lm32->reg_cache->reg_list[i].dirty = 0;
        }
+}
+
+static int
+lm32_clear_hw_breakpoints(struct target *target)
+{
+       struct lm32 *lm32 = target_to_lm32(target);
+       int ret;
+
+       LOG_DEBUG("-");
+
+       switch (lm32->num_bps)
+       {
+               default:
+               case 4:
+                       ret = lm32_write_csr(target, LM32_CSR_BP3, 0);
+                       if (ret != ERROR_OK)
+                               return ret;
+                       lm32->bps_used &= ~(1 << 3);
+               case 3:
+                       ret = lm32_write_csr(target, LM32_CSR_BP2, 0);
+                       if (ret != ERROR_OK)
+                               return ret;
+                       lm32->bps_used &= ~(1 << 2);
+               case 2:
+                       ret = lm32_write_csr(target, LM32_CSR_BP1, 0);
+                       if (ret != ERROR_OK)
+                               return ret;
+                       lm32->bps_used &= ~(1 << 1);
+               case 1:
+                       ret = lm32_write_csr(target, LM32_CSR_BP0, 0);
+                       if (ret != ERROR_OK)
+                               return ret;
+                       lm32->bps_used &= ~(1 << 0);
+               case 0:
+                       break;
+       }
+
+       return ERROR_OK;
+}
+
+static int
+lm32_clear_watchpoints(struct target *target)
+{
+       struct lm32 *lm32 = target_to_lm32(target);
+       int ret;
+
+       LOG_DEBUG("-");
+
+       switch (lm32->num_wps)
+       {
+               default:
+               case 4:
+                       ret = lm32_write_csr(target, LM32_CSR_WP3, 0);
+                       if (ret != ERROR_OK)
+                               return ret;
+                       lm32->wps_used &= ~(1 << 3);
+               case 3:
+                       ret = lm32_write_csr(target, LM32_CSR_WP2, 0);
+                       if (ret != ERROR_OK)
+                               return ret;
+                       lm32->wps_used &= ~(1 << 2);
+               case 2:
+                       ret = lm32_write_csr(target, LM32_CSR_WP1, 0);
+                       if (ret != ERROR_OK)
+                               return ret;
+                       lm32->wps_used &= ~(1 << 1);
+               case 1:
+                       ret = lm32_write_csr(target, LM32_CSR_WP0, 0);
+                       if (ret != ERROR_OK)
+                               return ret;
+                       lm32->wps_used &= ~(1 << 0);
+               case 0:
+                       break;
+       }
 
        return ERROR_OK;
 }
@@ -587,43 +1182,35 @@ lm32_invalidate_regs(struct target *target)
 static int
 lm32_debug_entry(struct target *target)
 {
-       struct lm32 *lm32 = target->arch_info;
+       struct lm32 *lm32 = target_to_lm32(target);
 
        int i;
-       uint8_t *buf;
+       int ret;
+       uint8_t eid;
+       uint8_t buf[LM32_NUM_REGS * sizeof(uint32_t)];
 
        LOG_DEBUG("-");
 
        /* read registers base address */
-       if (lm32_write_tx(target, LM32_MONITOR_CMD_REG_ADDR) != ERROR_OK)
-       {
-               LOG_ERROR("error writing monitor command");
-               return ERROR_TARGET_FAILURE;
-       }
-
-       if (lm32_recv_u32(target, &(lm32->reg_base)) != ERROR_OK)
+       ret = lm32_mon_registers_address(target, &lm32->reg_base);
+       if (ret != ERROR_OK)
        {
-               LOG_ERROR("error reading base address");
+               LOG_ERROR("Could not read registers base address");
                return ERROR_TARGET_FAILURE;
        }
 
        LOG_DEBUG("registers base address: %08x", lm32->reg_base);
 
-       /* allocate buffer */
-       buf = calloc(LM32_NUM_REGS, 4);
-
        /* read registers content */
-       if (lm32_read_memory(target, lm32->reg_base, 4, LM32_NUM_REGS, buf)
-               != ERROR_OK)
+       ret = target_read_memory(target, lm32->reg_base, 1, LM32_NUM_REGS * 4, buf);
+       if (ret != ERROR_OK)
        {
-               LOG_ERROR("error reading memory");
-               free(buf);
+               LOG_ERROR("Could not read registers content");
                return ERROR_TARGET_FAILURE;
        }
 
        for (i = 0; i < LM32_NUM_REGS; i++)
        {
-               /* XXX endianess */
                uint32_t data = buf[i*4] << 24 | buf[i*4+1] << 16 | buf[i*4+2] << 8 | buf[i*4+3];
                buf_set_u32(lm32->reg_cache->reg_list[i].value, 0, 32, data);
 
@@ -633,11 +1220,34 @@ lm32_debug_entry(struct target *target)
                LOG_DEBUG("reg %i -> %02x", i, buf_get_u32(buf+(i*4), 0, 32));
        }
 
-       free(buf);
+       LOG_DEBUG("pc=0x%08x", buf_get_u32(lm32->reg_cache->reg_list[LM32_PC].value, 0, 32));
 
-       LOG_DEBUG("pc: %08x", buf_get_u32(lm32->reg_cache->reg_list[LM32_PC].value, 0, 32));
+       eid = buf_get_u32(lm32->reg_cache->reg_list[LM32_EID].value, 0, 32) & 0xff;
+       switch (eid)
+       {
+               case LM32_EID_RESET:
+                       target->debug_reason = DBG_REASON_DBGRQ;
+                       break;
+               case LM32_EID_BREAKPOINT:
+                       if (target->halt_issued)
+                               target->debug_reason = DBG_REASON_DBGRQ;
+                       else
+                               target->debug_reason = DBG_REASON_BREAKPOINT;
+                       break;
+               case LM32_EID_WATCHPOINT:
+                       target->debug_reason = DBG_REASON_WATCHPOINT;
+                       break;
+               case LM32_EID_IBUS_ERROR:
+               case LM32_EID_DBUS_ERROR:
+               case LM32_EID_DIVIDE_BY_ZERO:
+               case LM32_EID_INTERRUPT:
+               case LM32_EID_SYSTEMCALL:
+               default:
+                       target->debug_reason = DBG_REASON_UNDEFINED;
+                       break;
+       }
 
-       target->debug_reason = DBG_REASON_DBGRQ;
+       LOG_DEBUG("eid=%d debug_reason=%d", eid, target->debug_reason);
 
        return ERROR_OK;
 }
@@ -648,27 +1258,28 @@ lm32_enter_debug(struct target *target)
        int ret;
        uint8_t data;
 
-       ret = lm32_read_rx(target, &data);
+       ret = lm32_read_jrx(target, &data);
        if (ret != ERROR_OK || data != 'T')
-               goto err;
+       {
+               target->state = TARGET_UNKNOWN;
+               return ret;
+       }
 
        target->state = TARGET_HALTED;
        lm32_debug_entry(target);
 
        return ERROR_OK;
-err:
-       target->state = TARGET_UNKNOWN;
-       return ret;
 }
 
 static int
 lm32_restore_context(struct target *target)
 {
-       struct lm32 *lm32 = target->arch_info;
+       struct lm32 *lm32 = target_to_lm32(target);
 
        int i;
+       int ret;
        int dirty;
-       uint8_t *buf;
+       uint8_t buf[LM32_NUM_REGS * sizeof(uint32_t)];
 
        LOG_DEBUG("-");
 
@@ -678,15 +1289,11 @@ lm32_restore_context(struct target *target)
                return ERROR_TARGET_NOT_HALTED;
        }
 
-       /* allocate buffer */
-       buf = calloc(LM32_NUM_REGS, 4);
-
        dirty = 0;
        for (i = 0; i < LM32_NUM_REGS; i++)
        {
-               /* XXX endianess */
-               uint8_t* tmp_buf = lm32->reg_cache->reg_list[i].value;
-               uint32_t data = tmp_buf[0] << 24 | tmp_buf[1] << 16 | tmp_buf[2] << 8 | tmp_buf[3];
+               uint8_t* v = lm32->reg_cache->reg_list[i].value;
+               uint32_t data = v[0] << 24 | v[1] << 16 | v[2] << 8 | v[3];
                buf_set_u32(buf+(i*4), 0, 32, data);
                if (lm32->reg_cache->reg_list[i].dirty)
                {
@@ -697,23 +1304,17 @@ lm32_restore_context(struct target *target)
        /* write registers content back */
        if (dirty)
        {
-               if (lm32_write_memory(target, lm32->reg_base, 4, LM32_NUM_REGS, buf) != ERROR_OK)
+               ret = lm32_write_memory(target, lm32->reg_base, 4, LM32_NUM_REGS, buf);
+               if (ret != ERROR_OK)
                {
                        LOG_ERROR("error writing monitor command");
-                       free(buf);
                        return ERROR_TARGET_FAILURE;
                }
        }
 
-       free(buf);
-
        return ERROR_OK;
 }
 
-/*
- * target functions
- */
-
 static int
 lm32_arch_state(struct target *target)
 {
@@ -723,6 +1324,7 @@ lm32_arch_state(struct target *target)
 static int
 lm32_poll(struct target *target)
 {
+       struct lm32 *lm32 = target_to_lm32(target);
        int ret = ERROR_OK;
        uint8_t status;
 
@@ -747,6 +1349,18 @@ lm32_poll(struct target *target)
                        return ERROR_TARGET_FAILURE;
                }
 
+               /* remove remapped exceptions in case we issued a 'reset halt' */
+               if (lm32->dc_csr & LM32_CSR_DC_RE)
+               {
+                       lm32->dc_csr &= ~LM32_CSR_DC_RE;
+                       ret = lm32_write_csr(target, LM32_CSR_DC, lm32->dc_csr);
+                       if (ret != ERROR_OK)
+                       {
+                               LOG_ERROR("Could not write CSR. Reset SoC.");
+                               return ERROR_TARGET_FAILURE;
+                       }
+               }
+
                /* if target was running, signal that we halted
                 * otherwise we reentered from debug execution */
                if (previous_state == TARGET_RUNNING)
@@ -779,7 +1393,8 @@ lm32_halt(struct target *target)
                return ERROR_TARGET_INVALID;
        }
 
-       if ((ret = lm32_dp_send_command(target, LM32_DP_BREAK, 1)) != ERROR_OK)
+       ret = lm32_dp_send_command(target, LM32_DP_BREAK, 1);
+       if (ret != ERROR_OK)
        {
                LOG_ERROR("could not send BREAK command");
                return ret;
@@ -800,6 +1415,7 @@ lm32_assert_reset(struct target *target)
 static int
 lm32_deassert_reset(struct target *target)
 {
+       struct lm32 *lm32 = target_to_lm32(target);
        int ret;
        uint8_t data;
 
@@ -811,7 +1427,7 @@ lm32_deassert_reset(struct target *target)
        if (ret != ERROR_OK)
                goto err;
 
-       ret = lm32_read_rx(target, &data);
+       ret = lm32_read_jrx(target, &data);
        if (ret != ERROR_OK)
                goto err;
 
@@ -827,21 +1443,26 @@ lm32_deassert_reset(struct target *target)
        if (ret != ERROR_OK)
                return ret;
 
-       ret = lm32_write_tx(target, LM32_MONITOR_CMD_WRITE_CSR);
+       ret = lm32_clear_hw_breakpoints(target);
        if (ret != ERROR_OK)
-               return ret;
-
-       ret = lm32_write_tx(target, LM32_MONITOR_WRITE_CSR_DC);
+               LOG_WARNING("Could not clear all hw breakpoints");
+       
+       ret = lm32_clear_watchpoints(target);
        if (ret != ERROR_OK)
-               return ret;
+               LOG_WARNING("Could not clear all watchpoints");
 
-       /* Cn = disabled, RE = remap all exception, SS = single step disabled */
        if (target->reset_halt)
-               ret = lm32_send_u32(target, 0x00000002);
-       else
-               ret = lm32_send_u32(target, 0x00000000);
+       {
+               lm32->dc_csr |= LM32_CSR_DC_RE;
+               ret = lm32_write_csr(target, LM32_CSR_DC, lm32->dc_csr);
+               if (ret != ERROR_OK)
+                       goto err;
+       }
+
+       ret = lm32_dp_send_command(target, LM32_DP_RESET, 1);
+       if (ret != ERROR_OK)
+               goto err;
 
-       lm32_dp_send_command(target, LM32_DP_RESET, 1);
        target->state = TARGET_RUNNING;
 
        return ERROR_OK;
@@ -854,7 +1475,8 @@ err:
 static int
 lm32_resume(struct target *target, int current, uint32_t address, int handle_breakpoints, int debug_execution)
 {
-       struct lm32 *lm32 = target->arch_info;
+       struct lm32 *lm32 = target_to_lm32(target);
+       struct breakpoint *breakpoint;
 
        int ret;
 
@@ -864,12 +1486,10 @@ lm32_resume(struct target *target, int current, uint32_t address, int handle_bre
                return ERROR_TARGET_NOT_HALTED;
        }
 
-#if 0
        if (!debug_execution)
        {
                target_free_all_working_areas(target);
        }
-#endif
 
        if (!current)
        {
@@ -878,9 +1498,21 @@ lm32_resume(struct target *target, int current, uint32_t address, int handle_bre
                lm32->reg_cache->reg_list[LM32_PC].valid = 1;
        }
        
+       /* the front-end may request us not to handle breakpoints */
        if (handle_breakpoints)
        {
-               /* XXX */
+               breakpoint = breakpoint_find(target,
+                               buf_get_u32(lm32->reg_cache->reg_list[LM32_PC].value, 0, 32));
+
+               if (breakpoint != NULL)
+               {
+                       /* TODO
+                        * (1) unset breakpoint
+                        * (2) single step
+                        * (3) set breakpoint
+                        */
+                       ;
+               }
        }
 
        if ((ret = lm32_restore_context(target)) != ERROR_OK)
@@ -889,11 +1521,9 @@ lm32_resume(struct target *target, int current, uint32_t address, int handle_bre
                return ret;
        }
 
-       if ((ret = lm32_write_tx(target, LM32_MONITOR_CMD_CONTINUE)) != ERROR_OK)
-       {
-               LOG_ERROR("error writing jtag uart tx register");
+       ret = lm32_mon_continue(target);
+       if (ret != ERROR_OK)
                return ret;
-       }
 
        target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
        target->state = TARGET_RUNNING;
@@ -912,7 +1542,7 @@ lm32_step(struct target *target, int current, uint32_t address, int handle_break
 static int
 lm32_get_gdb_reg_list(struct target *target, struct reg **reg_list[], int *reg_list_size)
 {
-       struct lm32 *lm32 = target->arch_info;
+       struct lm32 *lm32 = target_to_lm32(target);
        int i;
 
        *reg_list_size = LM32_NUM_REGS;
@@ -929,9 +1559,9 @@ lm32_get_gdb_reg_list(struct target *target, struct reg **reg_list[], int *reg_l
 static int
 lm32_set_breakpoint(struct target *target, struct breakpoint *breakpoint)
 {
-       struct lm32 *lm32 = target->arch_info;
+       struct lm32 *lm32 = target_to_lm32(target);
 
-       int retval;
+       int ret;
 
        if (breakpoint->set)
        {
@@ -943,54 +1573,62 @@ lm32_set_breakpoint(struct target *target, struct breakpoint *breakpoint)
        {
                case BKPT_HARD:
                {
-                       int bp_num = 0;
-                       int csr = 0;
+                       int bp_num;
+                       int csr;
 
                        /* find free hw bp */
-                       for (bp_num = 0; !((1 << bp_num) & lm32->bps_used); bp_num++);
+                       for (bp_num = 0; bp_num < lm32->num_bps; bp_num++)
+                       {
+                               if (!(lm32->bps_used & (1 << bp_num)))
+                                       break;
+                       }
+                       assert (bp_num < lm32->num_bps);
 
                        switch (bp_num)
                        {
-                               case 0: csr = LM32_MONITOR_WRITE_CSR_BP0; break;
-                               case 1: csr = LM32_MONITOR_WRITE_CSR_BP1; break;
-                               case 2: csr = LM32_MONITOR_WRITE_CSR_BP2; break;
-                               case 3: csr = LM32_MONITOR_WRITE_CSR_BP3; break;
+                               case 0: csr = LM32_CSR_BP0; break;
+                               case 1: csr = LM32_CSR_BP1; break;
+                               case 2: csr = LM32_CSR_BP2; break;
+                               case 3: csr = LM32_CSR_BP3; break;
                                default:
                                        LOG_ERROR("BUG: bp# %i not supported", bp_num);
-                                       exit(-1);
-                                       break;
+                                       exit(1);
                        }
 
-                       lm32_write_tx(target, LM32_MONITOR_CMD_WRITE_CSR);
-                       lm32_write_tx(target, csr);
-                       lm32_send_u32(target, breakpoint->address + 1);
+                       ret = lm32_write_csr(target, csr, breakpoint->address | 1);
 
                        /* mark bp as used */
                        lm32->bps_used |= (1 << bp_num);
+
                        /* remember bp_num */
                        breakpoint->set = bp_num + 1;
                        LOG_DEBUG("bp_num %i bp_value 0x%x", bp_num, breakpoint->address);
                } break;
                case BKPT_SOFT:
                {
-                       uint32_t verify = 0xffffffff;
+                       uint32_t verify = 0;
+                       uint32_t orig_instr;
 
-                       if((retval = lm32_read_memory(target, breakpoint->address, breakpoint->length, 1, breakpoint->orig_instr)) != ERROR_OK)
+                       ret = target_read_u32(target, breakpoint->address, &orig_instr);
+                       if (ret != ERROR_OK)
                        {
-                               LOG_DEBUG("error reading memory");
-                               return retval;
+                               LOG_DEBUG("error while reading original instruction");
+                               return ret;
                        }
+                       buf_set_u32(breakpoint->orig_instr, 0, 32, orig_instr);
 
-                       if ((retval = target_write_u32(target, breakpoint->address, LM32_INST_BREAK)) != ERROR_OK)
+                       ret = target_write_u32(target, breakpoint->address, LM32_INST_BREAK);
+                       if (ret != ERROR_OK)
                        {
-                               LOG_DEBUG("error writing software breakpoint");
-                               return retval;
+                               LOG_DEBUG("error while writing software breakpoint");
+                               return ret;
                        }
 
-                       if ((retval = target_read_u32(target, breakpoint->address, &verify)) != ERROR_OK)
+                       ret = target_read_u32(target, breakpoint->address, &verify);
+                       if (ret != ERROR_OK)
                        {
                                LOG_DEBUG("error reading back software breakpoint");
-                               return retval;
+                               return ret;
                        }
 
                        if (verify != LM32_INST_BREAK)
@@ -1004,7 +1642,7 @@ lm32_set_breakpoint(struct target *target, struct breakpoint *breakpoint)
                default:
                {
                        LOG_ERROR("BUG: unknown breakpoint type");
-                       exit(-1);
+                       exit(1);
                } break;
        }
 
@@ -1014,8 +1652,8 @@ lm32_set_breakpoint(struct target *target, struct breakpoint *breakpoint)
 static int
 lm32_unset_breakpoint(struct target *target, struct breakpoint *breakpoint)
 {
-       struct lm32 *lm32 = target->arch_info;
-       int retval;
+       struct lm32 *lm32 = target_to_lm32(target);
+       int ret;
 
        if (!breakpoint->set)
        {
@@ -1030,46 +1668,51 @@ lm32_unset_breakpoint(struct target *target, struct breakpoint *breakpoint)
                        int bp_num = breakpoint->set - 1;
                        int csr = 0;
 
+                       assert(bp_num < lm32->num_bps);
                        switch (bp_num)
                        {
-                               case 0: csr = LM32_MONITOR_WRITE_CSR_BP0; break;
-                               case 1: csr = LM32_MONITOR_WRITE_CSR_BP1; break;
-                               case 2: csr = LM32_MONITOR_WRITE_CSR_BP2; break;
-                               case 3: csr = LM32_MONITOR_WRITE_CSR_BP3; break;
+                               case 0: csr = LM32_CSR_BP0; break;
+                               case 1: csr = LM32_CSR_BP1; break;
+                               case 2: csr = LM32_CSR_BP2; break;
+                               case 3: csr = LM32_CSR_BP3; break;
                                default:
                                        LOG_ERROR("BUG: bp# %i not supported", bp_num);
-                                       exit(-1);
-                                       break;
+                                       exit(1);
                        }
 
                        /* clear used flag */
                        lm32->bps_used &= ~(1 << bp_num);
 
-                       lm32_write_tx(target, LM32_MONITOR_CMD_WRITE_CSR);
-                       lm32_write_tx(target, csr);
-                       lm32_send_u32(target, 0);
+                       ret = lm32_write_csr(target, csr, 0);
+                       if (ret != ERROR_OK)
+                               return ret;
                } break;
                case BKPT_SOFT:
                {
                        uint32_t current_instr;
 
                        /* check that user program has not modified breakpoint instruction */
-                       if ((retval = lm32_read_memory(target, breakpoint->address, 4, 1, (uint8_t*)&current_instr)) != ERROR_OK)
+                       ret = target_read_u32(target, breakpoint->address, &current_instr);
+                       if (ret != ERROR_OK)
                        {
-                               return retval;
+                               LOG_DEBUG("error while reading memory");
+                               return ret;
                        }
                        if (current_instr == LM32_INST_BREAK)
                        {
-                               if ((retval = lm32_write_memory(target, breakpoint->address, 4, 1, breakpoint->orig_instr)) != ERROR_OK)
-                               {
-                                       return retval;
-                               }
+                               ret = lm32_write_memory(target, breakpoint->address, 4, 1, breakpoint->orig_instr);
+                               if (ret != ERROR_OK)
+                                       return ret;
+                       }
+                       else
+                       {
+                               LOG_WARNING("breakpoint at %08x has been overwritten by user", breakpoint->address);
                        }
                } break;
                default:
                {
                        LOG_ERROR("BUG: unknown breakpoint type");
-                       exit(-1);
+                       exit(1);
                } break;
        }
 
@@ -1081,7 +1724,7 @@ lm32_unset_breakpoint(struct target *target, struct breakpoint *breakpoint)
 static int
 lm32_add_breakpoint(struct target *target, struct breakpoint *breakpoint)
 {
-       struct lm32 *lm32 = target->arch_info;
+       struct lm32 *lm32 = target_to_lm32(target);
 
        if (target->state != TARGET_HALTED)
        {
@@ -1094,25 +1737,19 @@ lm32_add_breakpoint(struct target *target, struct breakpoint *breakpoint)
 
        if (breakpoint->type == BKPT_HARD)
        {
-               if (lm32->num_bps_avail < 1)
+               if ((lm32->bps_used ^ ((1 << lm32->num_bps) - 1)) == 0)
                {
-                       LOG_INFO("no hardware breakpoint available");
+                       LOG_INFO("no more hardware breakpoints available");
                        return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
                }
-
-               lm32->num_bps_avail--;
        }
 
-       lm32_set_breakpoint(target, breakpoint);
-
-       return ERROR_OK;
+       return lm32_set_breakpoint(target, breakpoint);
 }
 
 static int
 lm32_remove_breakpoint(struct target *target, struct breakpoint *breakpoint)
 {
-       struct lm32 *lm32 = target->arch_info;
-
        if (target->state != TARGET_HALTED)
        {
                LOG_WARNING("target not halted");
@@ -1127,12 +1764,37 @@ lm32_remove_breakpoint(struct target *target, struct breakpoint *breakpoint)
                lm32_unset_breakpoint(target, breakpoint);
        }
 
-       if (breakpoint->type == BKPT_HARD)
-               lm32->num_bps_avail++;
-
        return ERROR_OK;
 }
 
+#if 0
+static int
+lm32_set_watchpoint(struct target *target, struct watchpoint *watchpoint)
+{
+       struct lm32 *lm32 = target_to_lm32(target);
+       int enable;
+
+       if (target->state != TARGET_HALTED)
+       {
+               LOG_ERROR("target not halted");
+               return ERROR_TARGET_NOT_HALTED;
+       }
+
+       switch(watchpoint->rw)
+       {
+               case WPT_READ:
+                       enable = 0x1;
+                       break;
+               case WPT_WRITE:
+                       enable = 0x2;
+                       break;
+               default:
+                       LOG_ERROR("BUG: watchpoint neither read nor write");
+                       exit(1);
+       }
+}
+#endif
+
 static int
 lm32_add_watchpoint(struct target *target, struct watchpoint *watchpoint)
 {
@@ -1154,10 +1816,11 @@ lm32_get_reg(struct reg *reg)
 {
        struct lm32_reg *arch_info = reg->arch_info;
        struct target *target = arch_info->target;
-       struct lm32 *lm32 = target->arch_info;
+       struct lm32 *lm32 = target_to_lm32(target);
        int num = arch_info->num;
        uint8_t buf[4];
        uint32_t data;
+       int ret;
 
        LOG_DEBUG("name=%s", reg->name);
 
@@ -1166,12 +1829,9 @@ lm32_get_reg(struct reg *reg)
                return ERROR_TARGET_NOT_HALTED;
        }
 
-       /* XXX endianess */
-       if (lm32_read_memory(target, lm32->reg_base + (4*num), 4, 1, buf) != ERROR_OK)
-       {
-               LOG_DEBUG("error reading memory");
-               return ERROR_FAIL;
-       }
+       ret = target_read_memory(target, lm32->reg_base + (4*num), 4, 1, buf);
+       if (ret != ERROR_OK)
+               return ret;
 
        data = buf[0] << 24 | buf[1] << 16 | buf[2] << 8 | buf[3];
        buf_set_u32(lm32->reg_cache->reg_list[num].value, 0, 32, data);
@@ -1187,9 +1847,11 @@ lm32_set_reg(struct reg *reg, uint8_t *buf)
 {
        struct lm32_reg *arch_info = reg->arch_info;
        struct target *target = arch_info->target;
-       struct lm32 *lm32 = target->arch_info;
+       struct lm32 *lm32 = target_to_lm32(target);
        int num = arch_info->num;
        uint32_t value = buf_get_u32(buf, 0, 32);
+       uint8_t tmp_buf[4];
+       int ret;
 
        LOG_DEBUG("reg=%s value=%08x", reg->name, value);
 
@@ -1197,12 +1859,15 @@ lm32_set_reg(struct reg *reg, uint8_t *buf)
        {
                return ERROR_TARGET_NOT_HALTED;
        }
-               
-       if (lm32_write_memory(target, lm32->reg_base + (4*num), 4, 1, buf) != ERROR_OK)
-       {
-               LOG_DEBUG("error reading memory");
+
+       tmp_buf[0] = (value >> 24) & 0xff;
+       tmp_buf[1] = (value >> 16) & 0xff;
+       tmp_buf[2] = (value >> 8) & 0xff;
+       tmp_buf[3] = value & 0xff;
+
+       ret = lm32_write_memory(target, lm32->reg_base + (4 * num), 4, 1, tmp_buf);
+       if (ret != ERROR_OK)
                return ERROR_FAIL;
-       }
 
        reg->dirty = 0;
 
@@ -1218,7 +1883,7 @@ static const struct reg_arch_type lm32_reg_type =
 static void
 lm32_build_reg_cache(struct target *target)
 {
-       struct lm32 *lm32 = target->arch_info;
+       struct lm32 *lm32 = target_to_lm32(target);
        struct reg_cache **cache_p = register_get_last_cache_p(&target->reg_cache);
        struct lm32_reg *arch_info = malloc(sizeof(lm32_reg_arch_info));
        int i;
@@ -1284,7 +1949,7 @@ lm32_init_target(struct command_context *cmd_ctx, struct target *target)
 static int
 lm32_examine(struct target *target)
 {
-       struct lm32 *lm32 = target->arch_info;
+       struct lm32 *lm32 = target_to_lm32(target);
 
        /* set instruction register */
        LOG_DEBUG("setting IR to 0x%04x", lm32->ir_insn);
@@ -1303,6 +1968,13 @@ lm32_target_create(struct target *target, Jim_Interp *interp)
        if (!lm32)
                return ERROR_FAIL;
 
+       lm32->dp_enabled = false;
+       lm32->ignore_ack = true;
+       lm32->num_bps = 4;
+       lm32->num_wps = 4;
+       lm32->bps_used = 0;
+       lm32->wps_used = 0;
+
        return lm32_init_arch_info(target, lm32, target->tap,
                        target->variant);
 }
@@ -1310,7 +1982,8 @@ lm32_target_create(struct target *target, Jim_Interp *interp)
 static int
 lm32_bulk_write_memory(struct target *target, uint32_t address, uint32_t count, uint8_t *buffer)
 {
-       return lm32_write_memory(target, address, 4, count, buffer);
+       /* XXX endianess ? */
+       return lm32_write_memory(target, address, 1, 4*count, buffer);
 }
 
 static int
@@ -1331,7 +2004,6 @@ COMMAND_HANDLER(lm32_handle_break_command)
 COMMAND_HANDLER(lm32_handle_reset_command)
 {
        struct target *target = get_current_target(CMD_CTX);
-       /* XXX */
        target->state = TARGET_RUNNING;
        return lm32_dp_send_command(target, LM32_DP_RESET, 1);
 }
@@ -1355,13 +2027,62 @@ COMMAND_HANDLER(lm32_handle_status_command)
        return ERROR_OK;
 }
 
+COMMAND_HANDLER(lm32_handle_cfg_command)
+{
+       struct target *target = get_current_target(CMD_CTX);
+       uint32_t cfg;
+       int ret;
+
+       ret = lm32_mon_read_csr(target, LM32_CSR_CFG, &cfg);
+       if (ret != ERROR_OK)
+               return ret;
+
+       static const char* on_off[2] = { "enabled", "disabled" };
+
+       command_print(CMD_CTX, "CFG=0x%08x", cfg);
+       command_print(CMD_CTX, "  Multiply:      %s",
+                       on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
+       command_print(CMD_CTX, "  Divide:        %s",
+                       on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
+       command_print(CMD_CTX, "  Barrel shift:  %s",
+                       on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
+       command_print(CMD_CTX, "  Sign extend:   %s",
+                       on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
+       command_print(CMD_CTX, "  User:          %s",
+                       on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
+       command_print(CMD_CTX, "  Cycle counter: %s",
+                       on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
+       command_print(CMD_CTX, "  D-Cache:       %s",
+                       on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
+       command_print(CMD_CTX, "  I-Cache:       %s",
+                       on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
+       command_print(CMD_CTX, "  Debug:         %s",
+                       on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
+       command_print(CMD_CTX, "  H/W debug:     %s",
+                       on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
+       command_print(CMD_CTX, "  ROM debug:     %s",
+                       on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
+       command_print(CMD_CTX, "  JTAG UART:     %s",
+                       on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
+       command_print(CMD_CTX, "  Interrupts:    %d",
+                       LM32_CSR_CFG_INT(cfg));
+       command_print(CMD_CTX, "  Breakpoints:   %d",
+                       LM32_CSR_CFG_BP(cfg));
+       command_print(CMD_CTX, "  Watchpoints:   %d",
+                       LM32_CSR_CFG_WP(cfg));
+       command_print(CMD_CTX, "  Revision:      %d",
+                       LM32_CSR_CFG_REV(cfg));
+
+       return ERROR_OK;
+}
+
 COMMAND_HANDLER(lm32_handle_juart_read_command)
 {
        struct target *target = get_current_target(CMD_CTX);
        uint8_t data;
        int ret;
 
-       if ((ret = lm32_read_rx(target, &data)) != ERROR_OK)
+       if ((ret = lm32_read_jrx(target, &data)) != ERROR_OK)
        {
                LOG_ERROR("error reading rx");
                return ret;
@@ -1385,7 +2106,131 @@ COMMAND_HANDLER(lm32_handle_juart_write_command)
        
        COMMAND_PARSE_NUMBER(u8, CMD_ARGV[0], byte);
 
-       return lm32_write_tx(target, byte);
+       return lm32_write_jtx(target, byte);
+}
+
+COMMAND_HANDLER(lm32_handle_monitor_version_command)
+{
+       struct target *target = get_current_target(CMD_CTX);
+       int ret;
+       uint8_t version;
+
+       ret = lm32_mon_version(target, &version);
+       if (ret != ERROR_OK)
+       {
+               LOG_ERROR("error reading version");
+               return ret;
+       }
+
+       command_print(CMD_CTX, "monitor version is %d", version);
+
+       return ERROR_OK;
+}
+
+COMMAND_HANDLER(lm32_handle_monitor_sh_command)
+{
+       struct target *target = get_current_target(CMD_CTX);
+       int ret;
+       uint32_t addr;
+       uint16_t data;
+
+       if (CMD_ARGC < 2)
+       {
+               LOG_ERROR("'lm32 monitor sh <addr> <data>' command takes two operands");
+               return ERROR_OK;
+       }
+       
+       COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
+       COMMAND_PARSE_NUMBER(u16, CMD_ARGV[1], data);
+
+       ret = lm32_mon_store_u16(target, addr, data);
+       if (ret != ERROR_OK)
+       {
+               LOG_ERROR("error storing halfword");
+               return ret;
+       }
+
+       return ERROR_OK;
+}
+
+COMMAND_HANDLER(lm32_handle_monitor_sw_command)
+{
+       struct target *target = get_current_target(CMD_CTX);
+       int ret;
+       uint32_t addr;
+       uint32_t data;
+
+       if (CMD_ARGC < 2)
+       {
+               LOG_ERROR("'lm32 monitor sw <addr> <data>' command takes two operands");
+               return ERROR_OK;
+       }
+       
+       COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
+       COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], data);
+
+       ret = lm32_mon_store_u32(target, addr, data);
+       if (ret != ERROR_OK)
+       {
+               LOG_ERROR("error storing word");
+               return ret;
+       }
+
+       return ERROR_OK;
+}
+
+COMMAND_HANDLER(lm32_handle_monitor_lh_command)
+{
+       struct target *target = get_current_target(CMD_CTX);
+       int ret;
+       uint32_t addr;
+       uint16_t data;
+
+       if (CMD_ARGC < 1)
+       {
+               LOG_ERROR("'lm32 monitor lh <addr>' command takes one operand");
+               return ERROR_OK;
+       }
+       
+       COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
+
+       ret = lm32_mon_load_u16(target, addr, &data);
+       if (ret != ERROR_OK)
+       {
+               LOG_ERROR("error loading halfword");
+               return ret;
+       }
+
+       command_print(CMD_CTX, "0x%04x", data);
+
+       return ERROR_OK;
+}
+
+COMMAND_HANDLER(lm32_handle_monitor_lw_command)
+{
+       struct target *target = get_current_target(CMD_CTX);
+       int ret;
+       uint32_t addr;
+       uint32_t data;
+
+       if (CMD_ARGC < 1)
+       {
+               LOG_ERROR("'lm32 monitor lw <addr>' command takes one operand");
+               return ERROR_OK;
+       }
+       
+       COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
+
+       ret = lm32_mon_load_u32(target, addr, &data);
+       if (ret != ERROR_OK)
+       {
+               LOG_ERROR("error loading halfword");
+               return ret;
+       }
+
+       command_print(CMD_CTX, "0x%08x", data);
+
+       return ERROR_OK;
 }
 
 COMMAND_HANDLER(lm32_handle_dp_read_command)
@@ -1403,7 +2248,7 @@ COMMAND_HANDLER(lm32_handle_dp_read_command)
        
        COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
 
-       ret = lm32_dp_read_memory(target, addr, &data);
+       ret = lm32_dp_read_memory_addr(target, addr, &data);
        if (ret != ERROR_OK)
                return ret;
 
@@ -1418,7 +2263,7 @@ COMMAND_HANDLER(lm32_handle_dp_read_sequential_command)
        uint8_t data;
        int ret;
 
-       ret = lm32_dp_read_sequential(target, &data);
+       ret = lm32_dp_read_sequential(target, &data, 1);
        if (ret != ERROR_OK)
                return ret;
 
@@ -1443,7 +2288,7 @@ COMMAND_HANDLER(lm32_handle_dp_write_command)
        COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
        COMMAND_PARSE_NUMBER(u8, CMD_ARGV[1], data);
 
-       ret = lm32_dp_write_memory(target, addr, data);
+       ret = lm32_dp_write_memory_addr(target, addr, data);
        if (ret != ERROR_OK)
                return ret;
 
@@ -1464,13 +2309,39 @@ COMMAND_HANDLER(lm32_handle_dp_write_sequential_command)
        
        COMMAND_PARSE_NUMBER(u8, CMD_ARGV[0], data);
 
-       ret = lm32_dp_write_sequential(target, data);
+       ret = lm32_dp_write_sequential(target, data, 1);
        if (ret != ERROR_OK)
                return ret;
 
        return ERROR_OK;
 }
 
+COMMAND_HANDLER(lm32_handle_debug_protocol_command)
+{
+       struct target *target = get_current_target(CMD_CTX);
+       struct lm32 *lm32 = target_to_lm32(target);
+
+       if (CMD_ARGC > 0)
+               COMMAND_PARSE_ENABLE(CMD_ARGV[0], lm32->dp_enabled);
+
+       command_print(CMD_CTX, "debug protocol is %s", (lm32->dp_enabled) ? "enabled" : "disabled");
+
+       return ERROR_OK;
+}
+
+COMMAND_HANDLER(lm32_handle_ignore_ack_command)
+{
+       struct target *target = get_current_target(CMD_CTX);
+       struct lm32 *lm32 = target_to_lm32(target);
+
+       if (CMD_ARGC > 0)
+               COMMAND_PARSE_ON_OFF(CMD_ARGV[0], lm32->ignore_ack);
+
+       command_print(CMD_CTX, "ignore ack is %s", (lm32->ignore_ack) ? "on" : "off");
+
+       return ERROR_OK;
+}
+
 static const struct command_registration lm32_dp_command_handlers[] =
 {
        {
@@ -1501,6 +2372,47 @@ static const struct command_registration lm32_dp_command_handlers[] =
                .help = "Execute debug protocol write sequential command",
                .usage = "byte",
        },
+       COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration lm32_monitor_command_handlers[] =
+{
+       {
+               .name = "version",
+               .handler = lm32_handle_monitor_version_command,
+               .mode = COMMAND_EXEC,
+               .help = "Retrieve monitor version",
+               .usage = "",
+       },
+       {
+               .name = "sh",
+               .handler = lm32_handle_monitor_sh_command,
+               .mode = COMMAND_EXEC,
+               .help = "Store halfword",
+               .usage = "addr data",
+       },
+       {
+               .name = "sw",
+               .handler = lm32_handle_monitor_sw_command,
+               .mode = COMMAND_EXEC,
+               .help = "Store word",
+               .usage = "addr data",
+       },
+       {
+               .name = "lh",
+               .handler = lm32_handle_monitor_lh_command,
+               .mode = COMMAND_EXEC,
+               .help = "Load halfword",
+               .usage = "addr",
+       },
+       {
+               .name = "lw",
+               .handler = lm32_handle_monitor_lw_command,
+               .mode = COMMAND_EXEC,
+               .help = "Load word",
+               .usage = "addr",
+       },
+       COMMAND_REGISTRATION_DONE
 };
 
 static const struct command_registration lm32_juart_command_handlers[] =
@@ -1545,12 +2457,39 @@ static const struct command_registration lm32_any_command_handlers[] =
                .help = "Show status flags.",
                .usage = "",
        },
+       {
+               .name = "cfg",
+               .handler = lm32_handle_cfg_command,
+               .mode = COMMAND_EXEC,
+               .help = "Display processor CFG register.",
+               .usage = "",
+       },
+       {
+               .name = "debug_protocol",
+               .handler = lm32_handle_debug_protocol_command,
+               .mode = COMMAND_EXEC,
+               .help = "Use the hw-assisted debug protocol to do memory transfers.",
+               .usage = "['enable'|'disable']",
+       },
+       {
+               .name = "ignore_ack",
+               .handler = lm32_handle_ignore_ack_command,
+               .mode = COMMAND_EXEC,
+               .help = "Don't wait for an ACK before transferring the next data byte.",
+               .usage = "['on'|'off']",
+       },
        {
                .name = "dp",
                .mode = COMMAND_ANY,
                .help = "JTAG debug protocol commands",
                .chain = lm32_dp_command_handlers,
        },
+       {
+               .name = "monitor",
+               .mode = COMMAND_ANY,
+               .help = "JTAG monitor commands",
+               .chain = lm32_monitor_command_handlers,
+       },
        {
                .name = "juart",
                .mode = COMMAND_ANY,