major rework
[mw/openocd-lm32.git] / src / target / lm32.c
1 /***************************************************************************
2  *   Copyright (C) 2010 by Michael Walle                                   *
3  *   michael@walle.cc                                                      *
4  *                                                                         *
5  *   This program is free software; you can redistribute it and/or modify  *
6  *   it under the terms of the GNU General Public License as published by  *
7  *   the Free Software Foundation; either version 2 of the License, or     *
8  *   (at your option) any later version.                                   *
9  *                                                                         *
10  *   This program is distributed in the hope that it will be useful,       *
11  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
12  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
13  *   GNU General Public License for more details.                          *
14  *                                                                         *
15  *   You should have received a copy of the GNU General Public License     *
16  *   along with this program; if not, write to the                         *
17  *   Free Software Foundation, Inc.,                                       *
18  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
19  ***************************************************************************/
20 #ifdef HAVE_CONFIG_H
21 #include "config.h"
22 #endif
23
24 #include "breakpoints.h"
25 #include "register.h"
26 #include <helper/time_support.h>
27 #include <helper/jim.h>
28 #include <jtag/jtag.h>
29 #include "target.h"
30 #include "target_type.h"
31
32 /* TODO
33  *  - error handling
34  *  - disable hw breakpoints upon reset (better do a jtag core reset?)
35  */
36
37 /*
38  * The LM32 debug core is accessed through a JTAG data register. It has the
39  * following layout:
40  *
41  *  shift in:                 shift out:
42  *   1                         1
43  *   0 9 8 7 6 5 4 3 2 1 0     0 9 8 7 6 5 4 3 2 1 0
44  *  +---------------+-----+   +---------------+-+-+-+
45  *  |      DATA     | ADR |   |      DATA     |P|T|R|
46  *  +---------------+-----+   +---------------+-+-+-+
47  *   ADR                       R = JTAG RX ready
48  *    0 = debug protocol       T = JTAG TX full
49  *    1 = JTAG uart TX         P = DP command in progress
50  *    2-= JTAG uart RX
51  *
52  * If adr is LM32_ADDR_DP (0), the command will be transferred in data[7:4].
53  * If adr is LM32_ADDR_TX (1) or LM32_ADDR_RX (2), data[7:0] will be the
54  * character that should be transferred.
55  *
56  */
57
58
59 #define LM32_INST_BREAK              0xac000002UL
60
61 struct lm32
62 {
63         int lm32_magic;
64         struct reg_cache *reg_cache;
65         uint32_t reg_base;
66
67         /* current state of the debug handler */
68         int handler_running;
69
70         int num_bps;
71         int num_wps;
72         /* bitfield, bit 0 corresponds to bp0 */
73         uint32_t bps_used;
74         uint32_t wps_used;
75
76         uint32_t dc_csr;
77
78         uint32_t ir_insn;
79
80         bool dp_enabled;
81         bool ignore_ack;
82 };
83
84 struct lm32_reg
85 {
86         uint32_t num;
87         struct target *target;
88 };
89
90 static char* lm32_reg_list[] =
91 {
92         "r0", "r1", "r2", "r3", "r4", "r5", "r6", "r7", "r8", "r9", "r10", "r11",
93         "r12", "r13", "r14", "r15", "r16", "r17", "r18", "r19", "r20", "r21",
94         "r22", "r23", "r24", "r25", "gp", "fp", "sp", "ra", "ea", "ba", "pc",
95         "eid", "eba", "deba"
96 };
97
98 enum
99 {
100         LM32_R0 = 0, LM32_R1, LM32_R2, LM32_R3, LM32_R4, LM32_R5, LM32_R6, LM32_R7, LM32_R8,
101         LM32_R9, LM32_R10, LM32_R11, LM32_R12, LM32_R13, LM32_R14, LM32_R15, LM32_R16,
102         LM32_R17, LM32_R18, LM32_R19, LM32_R20, LM32_R21, LM32_R22, LM32_R23, LM32_R24,
103         LM32_R25, LM32_GP, LM32_FP, LM32_SP, LM32_RA, LM32_EA, LM32_BA, LM32_PC, LM32_EID,
104         LM32_EBA, LM32_DEBA, LM32_NUM_REGS
105 };
106
107 /* exception ids */
108 enum {
109         LM32_EID_RESET = 0,
110         LM32_EID_BREAKPOINT,
111         LM32_EID_IBUS_ERROR,
112         LM32_EID_WATCHPOINT,
113         LM32_EID_DBUS_ERROR,
114         LM32_EID_DIVIDE_BY_ZERO,
115         LM32_EID_INTERRUPT,
116         LM32_EID_SYSTEMCALL,
117 };
118
119 static const struct lm32_reg lm32_reg_arch_info[] =
120 {
121         { LM32_R0, NULL }, { LM32_R1, NULL }, { LM32_R2, NULL }, { LM32_R3, NULL },
122         { LM32_R4, NULL }, { LM32_R5, NULL }, { LM32_R6, NULL }, { LM32_R7, NULL },
123         { LM32_R8, NULL }, { LM32_R9, NULL }, { LM32_R10, NULL }, { LM32_R11, NULL },
124         { LM32_R12, NULL }, { LM32_R13, NULL }, { LM32_R14, NULL }, { LM32_R15, NULL },
125         { LM32_R16, NULL }, { LM32_R17, NULL }, { LM32_R18, NULL }, { LM32_R19, NULL },
126         { LM32_R20, NULL }, { LM32_R21, NULL }, { LM32_R22, NULL }, { LM32_R23, NULL },
127         { LM32_R24, NULL }, { LM32_R25, NULL }, { LM32_GP, NULL }, { LM32_FP, NULL },
128         { LM32_SP, NULL }, { LM32_RA, NULL }, { LM32_EA, NULL }, { LM32_BA, NULL },
129         { LM32_PC, NULL }, { LM32_EID, NULL }, { LM32_EBA, NULL }, { LM32_DEBA, NULL },
130 };
131
132 /* jtag 'address' */
133 enum {
134         LM32_ADDR_DP = 0,
135         LM32_ADDR_TX,
136         LM32_ADDR_RX,
137 };
138
139 /* debug protocol commands */
140 enum
141 {
142         LM32_DP_NOP = 0,
143         LM32_DP_READ_MEMORY,
144         LM32_DP_WRITE_MEMORY,
145         LM32_DP_READ_SEQUENTIAL,
146         LM32_DP_WRITE_SEQUENTIAL,
147         LM32_DP_WRITE_CSR,
148         LM32_DP_BREAK,
149         LM32_DP_RESET,
150 };
151
152 /* status bits */
153 enum {
154         LM32_STAT_RX_READY   = (1 << 0),
155         LM32_STAT_TX_FULL    = (1 << 1),
156         LM32_STAT_PROCESSING = (1 << 2),
157 };
158
159 /* monitor commands */
160 enum {
161         LM32_MONITOR_CMD_VERSION = 0,
162         LM32_MONITOR_CMD_REG_ADDR,
163         LM32_MONITOR_CMD_READ_CSR,
164         LM32_MONITOR_CMD_WRITE_CSR,
165         LM32_MONITOR_CMD_READ_MEM,
166         LM32_MONITOR_CMD_WRITE_MEM,
167         LM32_MONITOR_CMD_STORE_HALFWORD,
168         LM32_MONITOR_CMD_STORE_WORD,
169         LM32_MONITOR_CMD_LOAD_HALFWORD,
170         LM32_MONITOR_CMD_LOAD_WORD,
171         LM32_MONITOR_CMD_CONTINUE,
172 };
173
174 enum {
175         LM32_CSR_IE = 0,
176         LM32_CSR_IM,
177         LM32_CSR_IP,
178         LM32_CSR_ICC,
179         LM32_CSR_DCC,
180         LM32_CSR_CC,
181         LM32_CSR_CFG,
182         LM32_CSR_EBA,
183         LM32_CSR_DC,
184         LM32_CSR_DEBA,
185         LM32_CSR_JTX,
186         LM32_CSR_JRX,
187         LM32_CSR_BP0,
188         LM32_CSR_BP1,
189         LM32_CSR_BP2,
190         LM32_CSR_BP3,
191         LM32_CSR_WP0,
192         LM32_CSR_WP1,
193         LM32_CSR_WP2,
194         LM32_CSR_WP3,
195 };
196
197 enum {
198         LM32_CSR_DC_SS = (1 << 0),
199         LM32_CSR_DC_RE = (1 << 1),
200         LM32_CSR_DC_C0 = (1 << 2),
201         LM32_CSR_DC_C1 = (1 << 3),
202         LM32_CSR_DC_C2 = (1 << 4),
203         LM32_CSR_DC_C3 = (1 << 5),
204 };
205
206 enum {
207         LM32_CSR_CFG_M  = (1 << 0),
208         LM32_CSR_CFG_D  = (1 << 1),
209         LM32_CSR_CFG_S  = (1 << 2),
210         LM32_CSR_CFG_U  = (1 << 3),
211         LM32_CSR_CFG_X  = (1 << 4),
212         LM32_CSR_CFG_CC = (1 << 5),
213         LM32_CSR_CFG_IC = (1 << 6),
214         LM32_CSR_CFG_DC = (1 << 7),
215         LM32_CSR_CFG_G  = (1 << 8),
216         LM32_CSR_CFG_H  = (1 << 9),
217         LM32_CSR_CFG_R  = (1 << 10),
218         LM32_CSR_CFG_J  = (1 << 11),
219 };
220
221 #define LM32_CSR_CFG_INT(cfg) ((cfg >> 12) & 0x3f)
222 #define LM32_CSR_CFG_BP(cfg)  ((cfg >> 18) & 0x0f)
223 #define LM32_CSR_CFG_WP(cfg)  ((cfg >> 22) & 0x0f)
224 #define LM32_CSR_CFG_REV(cfg) ((cfg >> 26) & 0x3f)
225
226 /*
227  * helper
228  */
229 static inline struct lm32* target_to_lm32(struct target *target)
230 {
231         return target->arch_info;
232 }
233
234 /*
235  * jtag functions
236  */
237
238 static int
239 lm32_jtag_set_instr(struct jtag_tap *tap, uint32_t new_instr, tap_state_t end_state)
240 {
241         assert(tap);
242
243         if (buf_get_u32(tap->cur_instr, 0, tap->ir_length) != new_instr)
244         {
245                 struct scan_field field;
246                 uint8_t scratch[4];
247
248                 memset(&field, 0, sizeof(field));
249                 field.num_bits = tap->ir_length;
250                 field.out_value = scratch;
251                 buf_set_u32(scratch, 0, field.num_bits, new_instr);
252
253                 jtag_add_ir_scan(tap, &field, end_state);
254         }
255
256         return ERROR_OK;
257 }
258
259 static int
260 lm32_jtag_datar(struct target *target, uint32_t data_in, uint32_t *data_out)
261 {
262         int ret;
263         uint8_t out_value[4];
264         struct scan_field field;
265
266         buf_set_u32(out_value, 0, 11, data_in);
267
268         memset(&field, 0, sizeof(field));
269
270         field.num_bits = 11;
271         field.out_value = out_value;
272         field.in_value = (void*)data_out;
273
274         jtag_add_dr_scan(target->tap, 1, &field, TAP_IDLE);
275
276         if (data_out)
277         {
278                 if ((ret = jtag_execute_queue()) != ERROR_OK)
279                 {
280                         LOG_ERROR("register read failed");
281                         return ret;
282                 }
283         }
284         
285         return ERROR_OK;
286 }
287
288 static int
289 lm32_get_status(struct target *target, uint8_t *status)
290 {
291         uint32_t tmp;
292         int ret;
293
294         ret = lm32_jtag_datar(target, LM32_ADDR_DP | (LM32_DP_NOP << 7), &tmp);
295         if (ret != ERROR_OK)
296                 return ret;
297
298         *status = tmp & 0x7;
299
300         return ERROR_OK;
301 }
302
303 static int
304 lm32_wait_for_status(struct target *target, uint8_t mask, uint8_t value)
305 {
306         struct timeval timeout, now;
307         uint8_t status;
308         int ret;
309         int cnt = 0;
310
311 #define DEFAULT_TIMEOUT 1
312
313         gettimeofday(&timeout, NULL);
314         timeval_add_time(&timeout, DEFAULT_TIMEOUT, 0);
315
316         while (true)
317         {
318                 if ((ret = lm32_get_status(target, &status)) != ERROR_OK) {
319                         LOG_ERROR("error reading jtag status");
320                         return ret;
321                 }
322
323                 if ((status & mask) == value)
324                         break;
325
326                 gettimeofday(&now, NULL);
327                 if (timercmp(&now, &timeout, >))
328                         return ERROR_TARGET_TIMEOUT;
329
330                 if (debug_level >= 3)
331                 {
332                         LOG_DEBUG("sleeping 100ms");
333                         alive_sleep(100);
334                 } else {
335                         keep_alive();
336                 }
337                 cnt++;
338         }
339
340         if (debug_level >= 4)
341                 LOG_DEBUG("waited for %d loops", cnt);
342
343         return ERROR_OK;
344 }
345
346 static int
347 lm32_read_jrx(struct target *target, uint8_t *data)
348 {
349         int ret;
350         uint32_t tmp_data;
351
352         ret = lm32_wait_for_status(target, LM32_STAT_RX_READY, LM32_STAT_RX_READY);
353         if (ret != ERROR_OK)
354                 return ret;
355
356         ret = lm32_jtag_datar(target, LM32_ADDR_RX, NULL);
357         if (ret != ERROR_OK)
358                 return ret;
359
360         ret = lm32_jtag_datar(target, LM32_ADDR_DP | (LM32_DP_NOP << 7), &tmp_data);
361         if (ret != ERROR_OK)
362                 return ret;
363
364         ret = jtag_execute_queue();
365         if (ret != ERROR_OK)
366                 return ret;
367
368         *data = (tmp_data >> 3) & 0xff;
369
370         if (debug_level >= 4)
371                 LOG_DEBUG("read %02x", *data);
372
373         return ERROR_OK;
374 }
375
376 static int
377 lm32_write_jtx(struct target *target, uint8_t data)
378 {
379         int ret;
380
381         if (debug_level >= 4)
382                 LOG_DEBUG("sending %02x", data);
383
384         ret = lm32_wait_for_status(target, LM32_STAT_TX_FULL, 0);
385         if (ret != ERROR_OK)
386                 return ret;
387
388         ret = lm32_jtag_datar(target, (data << 3) | LM32_ADDR_TX, NULL);
389         if (ret != ERROR_OK)
390                 return ret;
391
392         return jtag_execute_queue();
393 }
394
395 static int
396 lm32_dp_send_data(struct target *target, uint8_t data, int flush)
397 {
398         if (lm32_jtag_datar(target, (data << 3) | LM32_ADDR_DP, NULL) != ERROR_OK)
399                 return ERROR_FAIL;
400
401         if (flush)
402                 if (jtag_execute_queue() != ERROR_OK)
403                         return ERROR_FAIL;
404
405         return ERROR_OK;
406 }
407
408 static int
409 lm32_dp_receive_data(struct target *target, uint8_t *data)
410 {
411         uint32_t tmp=0;
412         int ret;
413
414         ret = lm32_jtag_datar(target, (LM32_DP_NOP << 7) | LM32_ADDR_DP, &tmp);
415         if (ret != ERROR_OK)
416                 return ret;
417
418         *data = (tmp >> 3) & 0xff;
419
420         return ERROR_OK;
421 }
422
423 static int
424 lm32_dp_send_command(struct target *target, uint8_t cmd, int flush)
425 {
426         return lm32_dp_send_data(target, cmd << 4, flush);
427 }
428
429 /*
430  * helper functions
431  */
432
433 static int
434 lm32_mon_send_u16(struct target *target, uint16_t data)
435 {
436         int ret;
437
438         ret = lm32_write_jtx(target, (data >> 8) & 0xff);
439         if (ret != ERROR_OK)
440                 return ret;
441
442         ret = lm32_write_jtx(target, data & 0xff);
443         if (ret != ERROR_OK)
444                 return ret;
445
446         return ERROR_OK;
447 }
448
449 static int
450 lm32_mon_send_u32(struct target *target, uint32_t data)
451 {
452         int ret;
453
454         ret = lm32_write_jtx(target, data & 0xff);
455         if (ret != ERROR_OK)
456                 return ret;
457
458         ret = lm32_write_jtx(target, (data >> 8) & 0xff);
459         if (ret != ERROR_OK)
460                 return ret;
461
462         ret = lm32_write_jtx(target, (data >> 16) & 0xff);
463         if (ret != ERROR_OK)
464                 return ret;
465
466         ret = lm32_write_jtx(target, (data >> 24) & 0xff);
467         if (ret != ERROR_OK)
468                 return ret;
469
470         return ERROR_OK;
471 }
472
473 static int
474 lm32_mon_receive_u16(struct target *target, uint16_t *data)
475 {
476         int ret;
477         uint8_t byte;
478
479         *data = 0;
480
481         ret = lm32_read_jrx(target, &byte);
482         if (ret != ERROR_OK)
483                 return ret;
484         *data |= byte;
485
486         ret = lm32_read_jrx(target, &byte);
487         if (ret != ERROR_OK)
488                 return ret;
489         *data |= byte << 8;
490
491         return ERROR_OK;
492 }
493
494 static int
495 lm32_mon_receive_u32(struct target *target, uint32_t *data)
496 {
497         int ret;
498         uint8_t byte;
499
500         *data = 0;
501
502         ret = lm32_read_jrx(target, &byte);
503         if (ret != ERROR_OK)
504                 return ret;
505         *data |= byte;
506
507         ret = lm32_read_jrx(target, &byte);
508         if (ret != ERROR_OK)
509                 return ret;
510         *data |= byte << 8;
511
512         ret = lm32_read_jrx(target, &byte);
513         if (ret != ERROR_OK)
514                 return ret;
515         *data |= byte << 16;
516
517         ret = lm32_read_jrx(target, &byte);
518         if (ret != ERROR_OK)
519                 return ret;
520         *data |= byte << 24;
521
522         return ERROR_OK;
523 }
524
525 static int
526 lm32_dp_send_u32(struct target *target, uint32_t data, int flush)
527 {
528         int i;
529         int ret;
530
531         for (i = 3; i >= 0; i--)
532         {
533                 ret = lm32_dp_send_data(target, (data >> (8*i)) & 0xff, (i==0) ? flush : 0);
534                 if (ret != ERROR_OK)
535                         return ret;
536         }
537
538         return ERROR_OK;
539 }
540
541 /*
542  * monitor commands
543  */
544
545 static int
546 lm32_mon_version(struct target *target, uint8_t *version)
547 {
548         int ret;
549
550         ret = lm32_write_jtx(target, LM32_MONITOR_CMD_VERSION);
551         if (ret != ERROR_OK)
552                 return ret;
553
554         return lm32_read_jrx(target, version);
555 }
556
557 static int
558 lm32_mon_write_csr(struct target *target, uint8_t csr, uint32_t value)
559 {
560         int ret;
561
562         static const uint8_t mon_csr_map[] = {
563                 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00,
564                 0x01, 0x02, 0xff, 0xff, 0x03, 0x04, 0x05, 0x06,
565                 0x07, 0x08, 0x09, 0x0a
566         };
567         assert(csr < ARRAY_SIZE(mon_csr_map));
568         assert(mon_csr_map[csr] != 0xff);
569
570         ret = lm32_write_jtx(target, LM32_MONITOR_CMD_WRITE_CSR);
571         if (ret != ERROR_OK)
572                 return ret;
573
574         ret = lm32_write_jtx(target, mon_csr_map[csr]);
575         if (ret != ERROR_OK)
576                 return ret;
577
578         return lm32_mon_send_u32(target, value);
579 }
580
581 static int
582 lm32_mon_read_csr(struct target *target, uint8_t csr, uint32_t *value)
583 {
584         int ret;
585
586         static const uint8_t mon_csr_map[] = {
587                 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0xff,
588                 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
589                 0xff, 0xff, 0xff, 0xff
590         };
591         assert(csr < ARRAY_SIZE(mon_csr_map));
592         assert(mon_csr_map[csr] != 0xff);
593
594         ret = lm32_write_jtx(target, LM32_MONITOR_CMD_READ_CSR);
595         if (ret != ERROR_OK)
596                 return ret;
597
598         ret = lm32_write_jtx(target, mon_csr_map[csr]);
599         if (ret != ERROR_OK)
600                 return ret;
601
602         return lm32_mon_receive_u32(target, value);
603 }
604
605 static int
606 lm32_mon_read_memory(struct target *target, uint32_t dest, uint32_t len, uint8_t *buf)
607 {
608         int ret;
609         uint32_t address;
610         uint32_t end_address = dest + len;
611
612         /* send memory read request */
613         if ((ret = lm32_write_jtx(target, LM32_MONITOR_CMD_READ_MEM)) != ERROR_OK)
614                 return ret;
615         
616         /* send base address */
617         if ((ret = lm32_mon_send_u32(target, dest)) != ERROR_OK)
618                 return ret;
619
620         /* send length */
621         if ((ret = lm32_mon_send_u32(target, len)) != ERROR_OK)
622                 return ret;
623
624         /* receive data */
625         for (address = dest; address < end_address; address++)
626         {
627                 ret = lm32_read_jrx(target, buf++);
628                 if (ret != ERROR_OK)
629                         return ret;
630         }
631
632         return ERROR_OK;
633 }
634
635 static int
636 lm32_mon_write_memory(struct target *target, uint32_t dest, uint32_t len, uint8_t *buf)
637 {
638         int ret;
639         uint32_t address;
640         uint32_t end_address = dest + len;
641
642         /* send memory write request */
643         if ((ret = lm32_write_jtx(target, LM32_MONITOR_CMD_WRITE_MEM)) != ERROR_OK)
644                 return ret;
645         
646         /* send base address */
647         if ((ret = lm32_mon_send_u32(target, dest)) != ERROR_OK)
648                 return ret;
649
650         /* send length */
651         if ((ret = lm32_mon_send_u32(target, len)) != ERROR_OK)
652                 return ret;
653
654         /* send data */
655         for (address = dest; address < end_address; address++)
656         {
657                 ret = lm32_write_jtx(target, *(buf++));
658                 if (ret != ERROR_OK)
659                         return ret;
660         }
661
662         return ERROR_OK;
663 }
664
665 static int
666 lm32_mon_store_u16(struct target *target, uint32_t dest, uint16_t data)
667 {
668         int ret;
669
670         LOG_DEBUG("dest=0x%08x data=%04x", dest, data);
671
672         ret = lm32_write_jtx(target, LM32_MONITOR_CMD_STORE_HALFWORD);
673         if (ret != ERROR_OK)
674                 return ret;
675
676         ret = lm32_mon_send_u32(target, dest);
677         if (ret != ERROR_OK)
678                 return ret;
679
680         ret = lm32_mon_send_u16(target, data);
681         if (ret != ERROR_OK)
682                 return ret;
683
684         return ERROR_OK;
685 }
686
687 static int
688 lm32_mon_store_u32(struct target *target, uint32_t dest, uint32_t data)
689 {
690         int ret;
691
692         LOG_DEBUG("dest=0x%08x data=%08x", dest, data);
693
694         ret = lm32_write_jtx(target, LM32_MONITOR_CMD_STORE_WORD);
695         if (ret != ERROR_OK)
696                 return ret;
697
698         ret = lm32_mon_send_u32(target, dest);
699         if (ret != ERROR_OK)
700                 return ret;
701
702         ret = lm32_mon_send_u32(target, data);
703         if (ret != ERROR_OK)
704                 return ret;
705
706         return ERROR_OK;
707 }
708
709 static int
710 lm32_mon_load_u16(struct target *target, uint32_t src, uint16_t *data)
711 {
712         int ret;
713
714         ret = lm32_write_jtx(target, LM32_MONITOR_CMD_LOAD_HALFWORD);
715         if (ret != ERROR_OK)
716                 return ret;
717
718         ret = lm32_mon_send_u32(target, src);
719         if (ret != ERROR_OK)
720                 return ret;
721
722         ret = lm32_mon_receive_u16(target, data);
723         if (ret != ERROR_OK)
724                 return ret;
725
726         LOG_DEBUG("src=0x%08x data=%04x", src, *data);
727
728         return ERROR_OK;
729 }
730
731 static int
732 lm32_mon_load_u32(struct target *target, uint32_t src, uint32_t *data)
733 {
734         int ret;
735
736         ret = lm32_write_jtx(target, LM32_MONITOR_CMD_LOAD_WORD);
737         if (ret != ERROR_OK)
738                 return ret;
739
740         ret = lm32_mon_send_u32(target, src);
741         if (ret != ERROR_OK)
742                 return ret;
743
744         ret = lm32_mon_receive_u32(target, data);
745         if (ret != ERROR_OK)
746                 return ret;
747
748         LOG_DEBUG("src=0x%08x data=%08x", src, *data);
749
750         return ERROR_OK;
751 }
752
753 static int
754 lm32_mon_continue(struct target *target)
755 {
756         return lm32_write_jtx(target, LM32_MONITOR_CMD_CONTINUE);
757 }
758
759 static int
760 lm32_mon_registers_address(struct target *target, uint32_t *addr)
761 {
762         int ret;
763
764         ret = lm32_write_jtx(target, LM32_MONITOR_CMD_REG_ADDR);
765         if (ret != ERROR_OK)
766                 return ret;
767
768         ret = lm32_mon_receive_u32(target, addr);
769         if (ret != ERROR_OK)
770                 return ret;
771
772         return ERROR_OK;
773 }
774
775 /*
776  * debug protocol commands
777  */
778 static int
779 lm32_dp_write_csr(struct target *target, uint8_t csr, uint32_t value)
780 {
781         int ret;
782
783         ret = lm32_dp_send_command(target, LM32_DP_WRITE_CSR, 0);
784         if (ret != ERROR_OK)
785                 return ret;
786
787         ret = lm32_dp_send_u32(target, value, 0);
788         if (ret != ERROR_OK)
789                 return ret;
790         
791         ret = lm32_dp_send_data(target, csr, 1);
792         if (ret != ERROR_OK)
793                 return ret;
794
795         return ERROR_OK;
796 }
797
798 static int
799 lm32_dp_read_memory_addr(struct target *target, uint32_t address, uint8_t *data)
800 {
801         int ret;
802
803         ret = lm32_dp_send_command(target, LM32_DP_READ_MEMORY, 0);
804         if (ret != ERROR_OK)
805                 return ret;
806         
807         /* now send address */
808         ret = lm32_dp_send_u32(target, address, 1);
809         if (ret != ERROR_OK)
810                 return ret;
811
812         /* wait for completion */
813         ret = lm32_wait_for_status(target, LM32_STAT_PROCESSING, 0);
814         if (ret != ERROR_OK)
815                 return ret;
816
817         /* read byte */
818         return lm32_dp_receive_data(target, data);
819 }
820
821 static int
822 lm32_dp_read_sequential(struct target *target, uint8_t *data, int wait)
823 {
824         int ret;
825
826         ret = lm32_dp_send_command(target, LM32_DP_READ_SEQUENTIAL, 1);
827         if (ret != ERROR_OK)
828                 return ret;
829         
830         if (wait)
831         {
832                 /* wait for completion */
833                 ret = lm32_wait_for_status(target, LM32_STAT_PROCESSING, 0);
834                 if (ret != ERROR_OK)
835                         return ret;
836         }
837
838         /* read byte */
839         return lm32_dp_receive_data(target, data);
840 }
841
842 static int
843 lm32_dp_write_memory_addr(struct target *target, uint32_t address, uint8_t data)
844 {
845         int ret;
846
847         if (debug_level >= 4)
848                 LOG_DEBUG("address=%02x data=0x%02x", address, data);
849
850         ret = lm32_dp_send_command(target, LM32_DP_WRITE_MEMORY, 0);
851         if (ret != ERROR_OK)
852                 return ret;
853         
854         /* now send address */
855         ret = lm32_dp_send_u32(target, address, 0);
856         if (ret != ERROR_OK)
857                 return ret;
858
859         /* and the byte to write */
860         ret = lm32_dp_send_data(target, data, 1);
861         if (ret != ERROR_OK)
862                 return ret;
863
864         return lm32_wait_for_status(target, LM32_STAT_PROCESSING, 0);
865 }
866
867 static int
868 lm32_dp_write_sequential(struct target *target, uint8_t data, bool wait)
869 {
870         int ret;
871
872         if (debug_level >= 4)
873                 LOG_DEBUG("data=0x%02x", data);
874
875         ret = lm32_dp_send_command(target, LM32_DP_WRITE_SEQUENTIAL, 0);
876         if (ret != ERROR_OK)
877                 return ret;
878         
879         ret = lm32_dp_send_data(target, data, 0);
880         if (ret != ERROR_OK)
881                 return ret;
882
883         if (wait)
884                 return lm32_wait_for_status(target, LM32_STAT_PROCESSING, 0);
885         else
886                 return ERROR_OK;
887 }
888
889 static int
890 lm32_dp_read_memory(struct target *target, uint32_t dest, uint32_t len, uint8_t *buf)
891 {
892         struct lm32 *lm32 = target_to_lm32(target);
893         int ret;
894         uint32_t address;
895         uint32_t end_address = dest + len;
896
897         for (address = dest; address < end_address; address++)
898         {
899                 /* send address if this is the first call or if we cross a 64k boundary */
900                 if ((address == dest) || ((address & 0xffff) == 0))
901                 {
902                         ret = lm32_dp_read_memory_addr(target, address, buf++);
903                         if (ret != ERROR_OK)
904                                 return ret;
905                 } else {
906                         ret = lm32_dp_read_sequential(target, buf++, (lm32->ignore_ack) ? 0 : 1);
907                         if (ret != ERROR_OK)
908                                 return ret;
909                 }
910         }
911
912         /* flush the JTAG buffer, in case we haven't done it yet */
913         ret = jtag_execute_queue();
914         if (ret != ERROR_OK)
915                 return ret;
916
917         return ERROR_OK;
918 }
919
920 static int
921 lm32_dp_write_memory(struct target *target, uint32_t dest, uint32_t len, uint8_t *buf)
922 {
923         struct lm32 *lm32 = target_to_lm32(target);
924         int ret;
925         uint32_t address;
926         uint32_t end_address = dest + len;
927
928         for (address = dest; address < end_address; address++)
929         {
930                 /* send address if this is the first call or if we cross a 64k boundary */
931                 if ((address == dest) || ((address & 0xffff) == 0))
932                 {
933                         ret = lm32_dp_write_memory_addr(target, address, *(buf++));
934                         if (ret != ERROR_OK)
935                                 return ret;
936                 } else {
937                         ret = lm32_dp_write_sequential(target, *(buf++), (lm32->ignore_ack) ? 0 : 1);
938                         if (ret != ERROR_OK)
939                                 return ret;
940                 }
941         }
942
943         /* flush the JTAG buffer, in case we haven't done it yet */
944         ret = jtag_execute_queue();
945         if (ret != ERROR_OK)
946                 return ret;
947
948         return ERROR_OK;
949 }
950
951
952 /*
953  * wrapper functions
954  */
955
956 static int
957 lm32_write_csr(struct target *target, uint8_t csr, uint32_t value)
958 {
959         struct lm32 *lm32 = target_to_lm32(target);
960
961         LOG_DEBUG("setting csr %d to %08x", csr, value);
962
963         if (lm32->dp_enabled)
964                 return lm32_dp_write_csr(target, csr, value);
965         else
966                 return lm32_mon_write_csr(target, csr, value);
967 }
968
969 static int
970 lm32_read_memory(struct target *target, uint32_t dest, uint32_t size, uint32_t count, uint8_t *buf)
971 {
972         struct lm32 *lm32 = target_to_lm32(target);
973         unsigned int u;
974         int ret;
975
976         LOG_DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", dest, size, count);
977
978         if (target->state != TARGET_HALTED)
979         {
980                 LOG_WARNING("target not halted");
981                 return ERROR_TARGET_NOT_HALTED;
982         }
983
984         /* sanitize arguments */
985         if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buf))
986                 return ERROR_INVALID_ARGUMENTS;
987         
988         if (((size == 4) && (dest & 0x3)) || ((size == 2) && (dest & 0x1)))
989                 return ERROR_TARGET_UNALIGNED_ACCESS;
990         
991         switch (size)
992         {
993                 case 4:
994                         for (u = 0; u < count; u++)
995                         {
996                                 uint32_t data;
997                                 ret = lm32_mon_load_u32(target, dest, &data);
998                                 if (ret != ERROR_OK)
999                                         return ret;
1000
1001                                 target_buffer_set_u32(target, buf, data);
1002                                 dest += 4;
1003                                 buf += 4;
1004                         }
1005                         break;
1006                 case 2:
1007                         for (u = 0; u < count; u++)
1008                         {
1009                                 uint16_t data;
1010                                 ret = lm32_mon_load_u16(target, dest, &data);
1011                                 if (ret != ERROR_OK)
1012                                         return ret;
1013
1014                                 target_buffer_set_u16(target, buf, data);
1015                                 dest += 2;
1016                                 buf += 2;
1017                         }
1018                         break;
1019                 case 1:
1020                         if (lm32->dp_enabled)
1021                                 return lm32_dp_read_memory(target, dest, count, buf);
1022                         else
1023                                 return lm32_mon_read_memory(target, dest, count, buf);
1024                 default:
1025                         return ERROR_INVALID_ARGUMENTS;
1026         }
1027
1028         return ERROR_OK;
1029 }
1030
1031 static int
1032 lm32_write_memory(struct target *target, uint32_t dest, uint32_t size, uint32_t count, uint8_t *buf)
1033 {
1034         struct lm32 *lm32 = target_to_lm32(target);
1035         unsigned int u;
1036         int ret;
1037
1038         LOG_DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", dest, size, count);
1039
1040         if (target->state != TARGET_HALTED)
1041         {
1042                 LOG_WARNING("target not halted");
1043                 return ERROR_TARGET_NOT_HALTED;
1044         }
1045
1046         /* sanitize arguments */
1047         if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buf))
1048                 return ERROR_INVALID_ARGUMENTS;
1049
1050         if (((size == 4) && (dest & 0x3)) || ((size == 2) && (dest & 0x1)))
1051                 return ERROR_TARGET_UNALIGNED_ACCESS;
1052
1053         switch (size)
1054         {
1055                 case 4:
1056                         for (u = 0; u < count; u++)
1057                         {
1058                                 ret = lm32_mon_store_u32(target, dest, target_buffer_get_u32(target, buf));
1059                                 if (ret != ERROR_OK)
1060                                         return ret;
1061
1062                                 dest += 4;
1063                                 buf += 4;
1064                         }
1065                         break;
1066                 case 2:
1067                         for (u = 0; u < count; u++)
1068                         {
1069                                 ret = lm32_mon_store_u16(target, dest, target_buffer_get_u16(target, buf));
1070                                 if (ret != ERROR_OK)
1071                                         return ret;
1072
1073                                 dest += 2;
1074                                 buf += 2;
1075                         }
1076                         break;
1077                 case 1:
1078                         if (lm32->dp_enabled)
1079                                 return lm32_dp_write_memory(target, dest, count, buf);
1080                         else
1081                                 return lm32_mon_write_memory(target, dest, count, buf);
1082                 default:
1083                         return ERROR_INVALID_ARGUMENTS;
1084         }
1085
1086         return ERROR_OK;
1087 }
1088
1089 /*
1090  * target functions
1091  */
1092
1093 static void
1094 lm32_invalidate_regs(struct target *target)
1095 {
1096         struct lm32 *lm32 = target_to_lm32(target);
1097         int i;
1098
1099         for (i = 0; i < LM32_NUM_REGS; i++)
1100         {
1101                 lm32->reg_cache->reg_list[i].valid = 0;
1102                 lm32->reg_cache->reg_list[i].dirty = 0;
1103         }
1104 }
1105
1106 static int
1107 lm32_clear_hw_breakpoints(struct target *target)
1108 {
1109         struct lm32 *lm32 = target_to_lm32(target);
1110         int ret;
1111
1112         LOG_DEBUG("-");
1113
1114         switch (lm32->num_bps)
1115         {
1116                 default:
1117                 case 4:
1118                         ret = lm32_write_csr(target, LM32_CSR_BP3, 0);
1119                         if (ret != ERROR_OK)
1120                                 return ret;
1121                         lm32->bps_used &= ~(1 << 3);
1122                 case 3:
1123                         ret = lm32_write_csr(target, LM32_CSR_BP2, 0);
1124                         if (ret != ERROR_OK)
1125                                 return ret;
1126                         lm32->bps_used &= ~(1 << 2);
1127                 case 2:
1128                         ret = lm32_write_csr(target, LM32_CSR_BP1, 0);
1129                         if (ret != ERROR_OK)
1130                                 return ret;
1131                         lm32->bps_used &= ~(1 << 1);
1132                 case 1:
1133                         ret = lm32_write_csr(target, LM32_CSR_BP0, 0);
1134                         if (ret != ERROR_OK)
1135                                 return ret;
1136                         lm32->bps_used &= ~(1 << 0);
1137                 case 0:
1138                         break;
1139         }
1140
1141         return ERROR_OK;
1142 }
1143
1144 static int
1145 lm32_clear_watchpoints(struct target *target)
1146 {
1147         struct lm32 *lm32 = target_to_lm32(target);
1148         int ret;
1149
1150         LOG_DEBUG("-");
1151
1152         switch (lm32->num_wps)
1153         {
1154                 default:
1155                 case 4:
1156                         ret = lm32_write_csr(target, LM32_CSR_WP3, 0);
1157                         if (ret != ERROR_OK)
1158                                 return ret;
1159                         lm32->wps_used &= ~(1 << 3);
1160                 case 3:
1161                         ret = lm32_write_csr(target, LM32_CSR_WP2, 0);
1162                         if (ret != ERROR_OK)
1163                                 return ret;
1164                         lm32->wps_used &= ~(1 << 2);
1165                 case 2:
1166                         ret = lm32_write_csr(target, LM32_CSR_WP1, 0);
1167                         if (ret != ERROR_OK)
1168                                 return ret;
1169                         lm32->wps_used &= ~(1 << 1);
1170                 case 1:
1171                         ret = lm32_write_csr(target, LM32_CSR_WP0, 0);
1172                         if (ret != ERROR_OK)
1173                                 return ret;
1174                         lm32->wps_used &= ~(1 << 0);
1175                 case 0:
1176                         break;
1177         }
1178
1179         return ERROR_OK;
1180 }
1181
1182 static int
1183 lm32_debug_entry(struct target *target)
1184 {
1185         struct lm32 *lm32 = target_to_lm32(target);
1186
1187         int i;
1188         int ret;
1189         uint8_t eid;
1190         uint8_t buf[LM32_NUM_REGS * sizeof(uint32_t)];
1191
1192         LOG_DEBUG("-");
1193
1194         /* read registers base address */
1195         ret = lm32_mon_registers_address(target, &lm32->reg_base);
1196         if (ret != ERROR_OK)
1197         {
1198                 LOG_ERROR("Could not read registers base address");
1199                 return ERROR_TARGET_FAILURE;
1200         }
1201
1202         LOG_DEBUG("registers base address: %08x", lm32->reg_base);
1203
1204         /* read registers content */
1205         ret = target_read_memory(target, lm32->reg_base, 1, LM32_NUM_REGS * 4, buf);
1206         if (ret != ERROR_OK)
1207         {
1208                 LOG_ERROR("Could not read registers content");
1209                 return ERROR_TARGET_FAILURE;
1210         }
1211
1212         for (i = 0; i < LM32_NUM_REGS; i++)
1213         {
1214                 uint32_t data = buf[i*4] << 24 | buf[i*4+1] << 16 | buf[i*4+2] << 8 | buf[i*4+3];
1215                 buf_set_u32(lm32->reg_cache->reg_list[i].value, 0, 32, data);
1216
1217                 lm32->reg_cache->reg_list[i].dirty = 0;
1218                 lm32->reg_cache->reg_list[i].valid = 1;
1219
1220                 LOG_DEBUG("reg %i -> %02x", i, buf_get_u32(buf+(i*4), 0, 32));
1221         }
1222
1223         LOG_DEBUG("pc=0x%08x", buf_get_u32(lm32->reg_cache->reg_list[LM32_PC].value, 0, 32));
1224
1225         eid = buf_get_u32(lm32->reg_cache->reg_list[LM32_EID].value, 0, 32) & 0xff;
1226         switch (eid)
1227         {
1228                 case LM32_EID_RESET:
1229                         target->debug_reason = DBG_REASON_DBGRQ;
1230                         break;
1231                 case LM32_EID_BREAKPOINT:
1232                         if (target->halt_issued)
1233                                 target->debug_reason = DBG_REASON_DBGRQ;
1234                         else
1235                                 target->debug_reason = DBG_REASON_BREAKPOINT;
1236                         break;
1237                 case LM32_EID_WATCHPOINT:
1238                         target->debug_reason = DBG_REASON_WATCHPOINT;
1239                         break;
1240                 case LM32_EID_IBUS_ERROR:
1241                 case LM32_EID_DBUS_ERROR:
1242                 case LM32_EID_DIVIDE_BY_ZERO:
1243                 case LM32_EID_INTERRUPT:
1244                 case LM32_EID_SYSTEMCALL:
1245                 default:
1246                         target->debug_reason = DBG_REASON_UNDEFINED;
1247                         break;
1248         }
1249
1250         LOG_DEBUG("eid=%d debug_reason=%d", eid, target->debug_reason);
1251
1252         return ERROR_OK;
1253 }
1254
1255 static int
1256 lm32_enter_debug(struct target *target)
1257 {
1258         int ret;
1259         uint8_t data;
1260
1261         ret = lm32_read_jrx(target, &data);
1262         if (ret != ERROR_OK || data != 'T')
1263         {
1264                 target->state = TARGET_UNKNOWN;
1265                 return ret;
1266         }
1267
1268         target->state = TARGET_HALTED;
1269         lm32_debug_entry(target);
1270
1271         return ERROR_OK;
1272 }
1273
1274 static int
1275 lm32_restore_context(struct target *target)
1276 {
1277         struct lm32 *lm32 = target_to_lm32(target);
1278
1279         int i;
1280         int ret;
1281         int dirty;
1282         uint8_t buf[LM32_NUM_REGS * sizeof(uint32_t)];
1283
1284         LOG_DEBUG("-");
1285
1286         if (target->state != TARGET_HALTED)
1287         {
1288                 LOG_WARNING("target not halted");
1289                 return ERROR_TARGET_NOT_HALTED;
1290         }
1291
1292         dirty = 0;
1293         for (i = 0; i < LM32_NUM_REGS; i++)
1294         {
1295                 uint8_t* v = lm32->reg_cache->reg_list[i].value;
1296                 uint32_t data = v[0] << 24 | v[1] << 16 | v[2] << 8 | v[3];
1297                 buf_set_u32(buf+(i*4), 0, 32, data);
1298                 if (lm32->reg_cache->reg_list[i].dirty)
1299                 {
1300                         dirty = 1;
1301                 }
1302         }
1303
1304         /* write registers content back */
1305         if (dirty)
1306         {
1307                 ret = lm32_write_memory(target, lm32->reg_base, 4, LM32_NUM_REGS, buf);
1308                 if (ret != ERROR_OK)
1309                 {
1310                         LOG_ERROR("error writing monitor command");
1311                         return ERROR_TARGET_FAILURE;
1312                 }
1313         }
1314
1315         return ERROR_OK;
1316 }
1317
1318 static int
1319 lm32_arch_state(struct target *target)
1320 {
1321         return ERROR_OK;
1322 }
1323
1324 static int
1325 lm32_poll(struct target *target)
1326 {
1327         struct lm32 *lm32 = target_to_lm32(target);
1328         int ret = ERROR_OK;
1329         uint8_t status;
1330
1331         if (target->state == TARGET_RUNNING)
1332         {
1333                 enum target_state previous_state = target->state;
1334
1335                 if (lm32_get_status(target, &status) != ERROR_OK)
1336                 {
1337                         LOG_ERROR("Could not read TAP state. Reset SoC.");
1338                         return ERROR_TARGET_FAILURE;
1339                 }
1340
1341                 if (!(status & LM32_STAT_RX_READY))
1342                         /* rx not ready */
1343                         return ERROR_OK;
1344
1345                 ret = lm32_enter_debug(target);
1346                 if (ret != ERROR_OK)
1347                 {
1348                         LOG_ERROR("Error while polling target state. Reset SoC.");
1349                         return ERROR_TARGET_FAILURE;
1350                 }
1351
1352                 /* remove remapped exceptions in case we issued a 'reset halt' */
1353                 if (lm32->dc_csr & LM32_CSR_DC_RE)
1354                 {
1355                         lm32->dc_csr &= ~LM32_CSR_DC_RE;
1356                         ret = lm32_write_csr(target, LM32_CSR_DC, lm32->dc_csr);
1357                         if (ret != ERROR_OK)
1358                         {
1359                                 LOG_ERROR("Could not write CSR. Reset SoC.");
1360                                 return ERROR_TARGET_FAILURE;
1361                         }
1362                 }
1363
1364                 /* if target was running, signal that we halted
1365                  * otherwise we reentered from debug execution */
1366                 if (previous_state == TARGET_RUNNING)
1367                         target_call_event_callbacks(target, TARGET_EVENT_HALTED);
1368         }
1369
1370         return ret;
1371 }
1372
1373 static int
1374 lm32_halt(struct target *target)
1375 {
1376         int ret;
1377
1378         LOG_DEBUG("target->state: %s", target_state_name(target));
1379         
1380         if (target->state == TARGET_HALTED)
1381         {
1382                 LOG_DEBUG("target was already halted");
1383                 return ERROR_OK;
1384         }
1385         else if (target->state == TARGET_UNKNOWN)
1386         {
1387                 LOG_ERROR("target was in unknown state when halt was requested");
1388                 return ERROR_TARGET_INVALID;
1389         }
1390         else if (target->state == TARGET_RESET)
1391         {
1392                 LOG_ERROR("target was in reset state when halt was requested");
1393                 return ERROR_TARGET_INVALID;
1394         }
1395
1396         ret = lm32_dp_send_command(target, LM32_DP_BREAK, 1);
1397         if (ret != ERROR_OK)
1398         {
1399                 LOG_ERROR("could not send BREAK command");
1400                 return ret;
1401         }
1402
1403         lm32_invalidate_regs(target);
1404
1405         return ERROR_OK;
1406 }
1407
1408 static int
1409 lm32_assert_reset(struct target *target)
1410 {
1411         /* do nothing here */
1412         return ERROR_OK;
1413 }
1414
1415 static int
1416 lm32_deassert_reset(struct target *target)
1417 {
1418         struct lm32 *lm32 = target_to_lm32(target);
1419         int ret;
1420         uint8_t data;
1421
1422         ret = lm32_dp_send_command(target, LM32_DP_RESET, 0);
1423         if (ret != ERROR_OK)
1424                 goto err;
1425
1426         ret = lm32_dp_send_command(target, LM32_DP_BREAK, 0);
1427         if (ret != ERROR_OK)
1428                 goto err;
1429
1430         ret = lm32_read_jrx(target, &data);
1431         if (ret != ERROR_OK)
1432                 goto err;
1433
1434         if (data != 'T')
1435         {
1436                 LOG_ERROR("Error while accessing debug monitor. Reset SoC.");
1437                 return ERROR_TARGET_FAILURE;
1438         }
1439
1440         target->state = TARGET_HALTED;
1441
1442         ret = lm32_debug_entry(target);
1443         if (ret != ERROR_OK)
1444                 return ret;
1445
1446         ret = lm32_clear_hw_breakpoints(target);
1447         if (ret != ERROR_OK)
1448                 LOG_WARNING("Could not clear all hw breakpoints");
1449         
1450         ret = lm32_clear_watchpoints(target);
1451         if (ret != ERROR_OK)
1452                 LOG_WARNING("Could not clear all watchpoints");
1453
1454         if (target->reset_halt)
1455         {
1456                 lm32->dc_csr |= LM32_CSR_DC_RE;
1457                 ret = lm32_write_csr(target, LM32_CSR_DC, lm32->dc_csr);
1458                 if (ret != ERROR_OK)
1459                         goto err;
1460         }
1461
1462         ret = lm32_dp_send_command(target, LM32_DP_RESET, 1);
1463         if (ret != ERROR_OK)
1464                 goto err;
1465
1466         target->state = TARGET_RUNNING;
1467
1468         return ERROR_OK;
1469
1470 err:
1471         target->state = TARGET_UNKNOWN;
1472         return ret;
1473 }
1474
1475 static int
1476 lm32_resume(struct target *target, int current, uint32_t address, int handle_breakpoints, int debug_execution)
1477 {
1478         struct lm32 *lm32 = target_to_lm32(target);
1479         struct breakpoint *breakpoint;
1480
1481         int ret;
1482
1483         if (target->state != TARGET_HALTED)
1484         {
1485                 LOG_WARNING("target not halted");
1486                 return ERROR_TARGET_NOT_HALTED;
1487         }
1488
1489         if (!debug_execution)
1490         {
1491                 target_free_all_working_areas(target);
1492         }
1493
1494         if (!current)
1495         {
1496                 buf_set_u32(lm32->reg_cache->reg_list[LM32_PC].value, 0, 32, address);
1497                 lm32->reg_cache->reg_list[LM32_PC].dirty = 1;
1498                 lm32->reg_cache->reg_list[LM32_PC].valid = 1;
1499         }
1500         
1501         /* the front-end may request us not to handle breakpoints */
1502         if (handle_breakpoints)
1503         {
1504                 breakpoint = breakpoint_find(target,
1505                                 buf_get_u32(lm32->reg_cache->reg_list[LM32_PC].value, 0, 32));
1506
1507                 if (breakpoint != NULL)
1508                 {
1509                         /* TODO
1510                          * (1) unset breakpoint
1511                          * (2) single step
1512                          * (3) set breakpoint
1513                          */
1514                         ;
1515                 }
1516         }
1517
1518         if ((ret = lm32_restore_context(target)) != ERROR_OK)
1519         {
1520                 LOG_ERROR("error restoring context");
1521                 return ret;
1522         }
1523
1524         ret = lm32_mon_continue(target);
1525         if (ret != ERROR_OK)
1526                 return ret;
1527
1528         target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
1529         target->state = TARGET_RUNNING;
1530
1531         return ERROR_OK;
1532 }
1533
1534 static int
1535 lm32_step(struct target *target, int current, uint32_t address, int handle_breakpoints)
1536 {
1537         LOG_WARNING("not implemented");
1538
1539         return ERROR_OK;
1540 }
1541
1542 static int
1543 lm32_get_gdb_reg_list(struct target *target, struct reg **reg_list[], int *reg_list_size)
1544 {
1545         struct lm32 *lm32 = target_to_lm32(target);
1546         int i;
1547
1548         *reg_list_size = LM32_NUM_REGS;
1549         *reg_list = calloc(sizeof(struct reg*), LM32_NUM_REGS);
1550
1551         for (i = 0; i < LM32_NUM_REGS; i++)
1552         {
1553                 (*reg_list)[i] = &(lm32->reg_cache->reg_list[i]);
1554         }
1555
1556         return ERROR_OK;
1557 }
1558
1559 static int
1560 lm32_set_breakpoint(struct target *target, struct breakpoint *breakpoint)
1561 {
1562         struct lm32 *lm32 = target_to_lm32(target);
1563
1564         int ret;
1565
1566         if (breakpoint->set)
1567         {
1568                 LOG_WARNING("breakpoint already set");
1569                 return ERROR_OK;
1570         }
1571
1572         switch (breakpoint->type)
1573         {
1574                 case BKPT_HARD:
1575                 {
1576                         int bp_num;
1577                         int csr;
1578
1579                         /* find free hw bp */
1580                         for (bp_num = 0; bp_num < lm32->num_bps; bp_num++)
1581                         {
1582                                 if (!(lm32->bps_used & (1 << bp_num)))
1583                                         break;
1584                         }
1585                         assert (bp_num < lm32->num_bps);
1586
1587                         switch (bp_num)
1588                         {
1589                                 case 0: csr = LM32_CSR_BP0; break;
1590                                 case 1: csr = LM32_CSR_BP1; break;
1591                                 case 2: csr = LM32_CSR_BP2; break;
1592                                 case 3: csr = LM32_CSR_BP3; break;
1593                                 default:
1594                                         LOG_ERROR("BUG: bp# %i not supported", bp_num);
1595                                         exit(1);
1596                         }
1597
1598                         ret = lm32_write_csr(target, csr, breakpoint->address | 1);
1599
1600                         /* mark bp as used */
1601                         lm32->bps_used |= (1 << bp_num);
1602
1603                         /* remember bp_num */
1604                         breakpoint->set = bp_num + 1;
1605                         LOG_DEBUG("bp_num %i bp_value 0x%x", bp_num, breakpoint->address);
1606                 } break;
1607                 case BKPT_SOFT:
1608                 {
1609                         uint32_t verify = 0;
1610                         uint32_t orig_instr;
1611
1612                         ret = target_read_u32(target, breakpoint->address, &orig_instr);
1613                         if (ret != ERROR_OK)
1614                         {
1615                                 LOG_DEBUG("error while reading original instruction");
1616                                 return ret;
1617                         }
1618                         buf_set_u32(breakpoint->orig_instr, 0, 32, orig_instr);
1619
1620                         ret = target_write_u32(target, breakpoint->address, LM32_INST_BREAK);
1621                         if (ret != ERROR_OK)
1622                         {
1623                                 LOG_DEBUG("error while writing software breakpoint");
1624                                 return ret;
1625                         }
1626
1627                         ret = target_read_u32(target, breakpoint->address, &verify);
1628                         if (ret != ERROR_OK)
1629                         {
1630                                 LOG_DEBUG("error reading back software breakpoint");
1631                                 return ret;
1632                         }
1633
1634                         if (verify != LM32_INST_BREAK)
1635                         {
1636                                 LOG_ERROR("Unable to set 32bit breakpoint at address %08x - check that memory is read/writable", breakpoint->address);
1637                                 return ERROR_OK;
1638                         }
1639
1640                         breakpoint->set = 1;
1641                 } break;
1642                 default:
1643                 {
1644                         LOG_ERROR("BUG: unknown breakpoint type");
1645                         exit(1);
1646                 } break;
1647         }
1648
1649         return ERROR_OK;
1650 }
1651
1652 static int
1653 lm32_unset_breakpoint(struct target *target, struct breakpoint *breakpoint)
1654 {
1655         struct lm32 *lm32 = target_to_lm32(target);
1656         int ret;
1657
1658         if (!breakpoint->set)
1659         {
1660                 LOG_WARNING("breakpoint not set");
1661                 return ERROR_OK;
1662         }
1663
1664         switch (breakpoint->type)
1665         {
1666                 case BKPT_HARD:
1667                 {
1668                         int bp_num = breakpoint->set - 1;
1669                         int csr = 0;
1670
1671                         assert(bp_num < lm32->num_bps);
1672                         switch (bp_num)
1673                         {
1674                                 case 0: csr = LM32_CSR_BP0; break;
1675                                 case 1: csr = LM32_CSR_BP1; break;
1676                                 case 2: csr = LM32_CSR_BP2; break;
1677                                 case 3: csr = LM32_CSR_BP3; break;
1678                                 default:
1679                                         LOG_ERROR("BUG: bp# %i not supported", bp_num);
1680                                         exit(1);
1681                         }
1682
1683                         /* clear used flag */
1684                         lm32->bps_used &= ~(1 << bp_num);
1685
1686                         ret = lm32_write_csr(target, csr, 0);
1687                         if (ret != ERROR_OK)
1688                                 return ret;
1689                 } break;
1690                 case BKPT_SOFT:
1691                 {
1692                         uint32_t current_instr;
1693
1694                         /* check that user program has not modified breakpoint instruction */
1695                         ret = target_read_u32(target, breakpoint->address, &current_instr);
1696                         if (ret != ERROR_OK)
1697                         {
1698                                 LOG_DEBUG("error while reading memory");
1699                                 return ret;
1700                         }
1701                         if (current_instr == LM32_INST_BREAK)
1702                         {
1703                                 ret = lm32_write_memory(target, breakpoint->address, 4, 1, breakpoint->orig_instr);
1704                                 if (ret != ERROR_OK)
1705                                         return ret;
1706                         }
1707                         else
1708                         {
1709                                 LOG_WARNING("breakpoint at %08x has been overwritten by user", breakpoint->address);
1710                         }
1711                 } break;
1712                 default:
1713                 {
1714                         LOG_ERROR("BUG: unknown breakpoint type");
1715                         exit(1);
1716                 } break;
1717         }
1718
1719         breakpoint->set = 0;
1720
1721         return ERROR_OK;
1722 }
1723
1724 static int
1725 lm32_add_breakpoint(struct target *target, struct breakpoint *breakpoint)
1726 {
1727         struct lm32 *lm32 = target_to_lm32(target);
1728
1729         if (target->state != TARGET_HALTED)
1730         {
1731                 LOG_WARNING("target not halted");
1732                 return ERROR_TARGET_NOT_HALTED;
1733         }
1734
1735         if (breakpoint->length != 4)
1736                 return ERROR_INVALID_ARGUMENTS;
1737
1738         if (breakpoint->type == BKPT_HARD)
1739         {
1740                 if ((lm32->bps_used ^ ((1 << lm32->num_bps) - 1)) == 0)
1741                 {
1742                         LOG_INFO("no more hardware breakpoints available");
1743                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1744                 }
1745         }
1746
1747         return lm32_set_breakpoint(target, breakpoint);
1748 }
1749
1750 static int
1751 lm32_remove_breakpoint(struct target *target, struct breakpoint *breakpoint)
1752 {
1753         if (target->state != TARGET_HALTED)
1754         {
1755                 LOG_WARNING("target not halted");
1756                 return ERROR_TARGET_NOT_HALTED;
1757         }
1758
1759         if (breakpoint->length != 4)
1760                 return ERROR_INVALID_ARGUMENTS;
1761
1762         if (breakpoint->set)
1763         {
1764                 lm32_unset_breakpoint(target, breakpoint);
1765         }
1766
1767         return ERROR_OK;
1768 }
1769
1770 #if 0
1771 static int
1772 lm32_set_watchpoint(struct target *target, struct watchpoint *watchpoint)
1773 {
1774         struct lm32 *lm32 = target_to_lm32(target);
1775         int enable;
1776
1777         if (target->state != TARGET_HALTED)
1778         {
1779                 LOG_ERROR("target not halted");
1780                 return ERROR_TARGET_NOT_HALTED;
1781         }
1782
1783         switch(watchpoint->rw)
1784         {
1785                 case WPT_READ:
1786                         enable = 0x1;
1787                         break;
1788                 case WPT_WRITE:
1789                         enable = 0x2;
1790                         break;
1791                 default:
1792                         LOG_ERROR("BUG: watchpoint neither read nor write");
1793                         exit(1);
1794         }
1795 }
1796 #endif
1797
1798 static int
1799 lm32_add_watchpoint(struct target *target, struct watchpoint *watchpoint)
1800 {
1801         LOG_WARNING("not implemented");
1802
1803         return ERROR_OK;
1804 }
1805
1806 static int
1807 lm32_remove_watchpoint(struct target *target, struct watchpoint *watchpoint)
1808 {
1809         LOG_WARNING("not implemented");
1810
1811         return ERROR_OK;
1812 }
1813
1814 static int
1815 lm32_get_reg(struct reg *reg)
1816 {
1817         struct lm32_reg *arch_info = reg->arch_info;
1818         struct target *target = arch_info->target;
1819         struct lm32 *lm32 = target_to_lm32(target);
1820         int num = arch_info->num;
1821         uint8_t buf[4];
1822         uint32_t data;
1823         int ret;
1824
1825         LOG_DEBUG("name=%s", reg->name);
1826
1827         if (target->state != TARGET_HALTED)
1828         {
1829                 return ERROR_TARGET_NOT_HALTED;
1830         }
1831
1832         ret = target_read_memory(target, lm32->reg_base + (4*num), 4, 1, buf);
1833         if (ret != ERROR_OK)
1834                 return ret;
1835
1836         data = buf[0] << 24 | buf[1] << 16 | buf[2] << 8 | buf[3];
1837         buf_set_u32(lm32->reg_cache->reg_list[num].value, 0, 32, data);
1838         
1839         reg->dirty = 0;
1840         reg->valid = 1;
1841
1842         return ERROR_OK;
1843 }
1844
1845 static int
1846 lm32_set_reg(struct reg *reg, uint8_t *buf)
1847 {
1848         struct lm32_reg *arch_info = reg->arch_info;
1849         struct target *target = arch_info->target;
1850         struct lm32 *lm32 = target_to_lm32(target);
1851         int num = arch_info->num;
1852         uint32_t value = buf_get_u32(buf, 0, 32);
1853         uint8_t tmp_buf[4];
1854         int ret;
1855
1856         LOG_DEBUG("reg=%s value=%08x", reg->name, value);
1857
1858         if (target->state != TARGET_HALTED)
1859         {
1860                 return ERROR_TARGET_NOT_HALTED;
1861         }
1862
1863         tmp_buf[0] = (value >> 24) & 0xff;
1864         tmp_buf[1] = (value >> 16) & 0xff;
1865         tmp_buf[2] = (value >> 8) & 0xff;
1866         tmp_buf[3] = value & 0xff;
1867
1868         ret = lm32_write_memory(target, lm32->reg_base + (4 * num), 4, 1, tmp_buf);
1869         if (ret != ERROR_OK)
1870                 return ERROR_FAIL;
1871
1872         reg->dirty = 0;
1873
1874         return ERROR_OK;
1875 }
1876
1877 static const struct reg_arch_type lm32_reg_type =
1878 {
1879         .get = lm32_get_reg,
1880         .set = lm32_set_reg,
1881 };
1882
1883 static void
1884 lm32_build_reg_cache(struct target *target)
1885 {
1886         struct lm32 *lm32 = target_to_lm32(target);
1887         struct reg_cache **cache_p = register_get_last_cache_p(&target->reg_cache);
1888         struct lm32_reg *arch_info = malloc(sizeof(lm32_reg_arch_info));
1889         int i;
1890         int num_regs = ARRAY_SIZE(lm32_reg_arch_info);
1891
1892         struct reg_cache *cache = malloc(sizeof(struct reg_cache));
1893         struct reg *reg_list = malloc(num_regs * sizeof(struct reg));
1894
1895         cache->name = "LM32 registers";
1896         cache->next = NULL;
1897         cache->reg_list = reg_list;
1898         cache->num_regs = num_regs;
1899
1900         for (i = 0; i < num_regs; i++)
1901         {
1902                 reg_list[i].name = lm32_reg_list[i];
1903                 reg_list[i].value = calloc(4, 1);
1904                 reg_list[i].dirty = 0;
1905                 reg_list[i].valid = 0;
1906                 reg_list[i].size = 32;
1907                 reg_list[i].arch_info = &arch_info[i];
1908                 reg_list[i].type = &lm32_reg_type;
1909                 arch_info[i] = lm32_reg_arch_info[i];
1910                 arch_info[i].target = target;
1911         }
1912
1913         *cache_p = cache;
1914         lm32->reg_cache = cache;
1915 }
1916
1917 static int
1918 lm32_init_arch_info(struct target *target, struct lm32 *lm32, struct jtag_tap *tap, const char *variant)
1919 {
1920         target->arch_info = lm32;
1921
1922         if (!variant || !strlen(variant)) {
1923                 LOG_ERROR("variant not defined");
1924                 return ERROR_FAIL;
1925         }
1926
1927         if (strcmp(variant, "xc6s") == 0)
1928         {
1929 #define XC6S_USER1 0x2
1930                 lm32->ir_insn = XC6S_USER1;
1931         }
1932         else
1933         {
1934                 LOG_ERROR("%s: unrecognized variant %s", tap->dotted_name, variant);
1935                 return ERROR_FAIL;
1936         }
1937
1938         return ERROR_OK;
1939 }
1940
1941 static int
1942 lm32_init_target(struct command_context *cmd_ctx, struct target *target)
1943 {
1944         lm32_build_reg_cache(target);
1945
1946         return ERROR_OK;
1947 }
1948
1949 static int
1950 lm32_examine(struct target *target)
1951 {
1952         struct lm32 *lm32 = target_to_lm32(target);
1953
1954         /* set instruction register */
1955         LOG_DEBUG("setting IR to 0x%04x", lm32->ir_insn);
1956         lm32_jtag_set_instr(target->tap, lm32->ir_insn, TAP_IDLE);
1957
1958         target_set_examined(target);
1959
1960         return ERROR_OK;
1961 }
1962
1963 static int
1964 lm32_target_create(struct target *target, Jim_Interp *interp)
1965 {
1966         struct lm32 *lm32 = calloc(1, sizeof(struct lm32));
1967
1968         if (!lm32)
1969                 return ERROR_FAIL;
1970
1971         lm32->dp_enabled = false;
1972         lm32->ignore_ack = true;
1973         lm32->num_bps = 4;
1974         lm32->num_wps = 4;
1975         lm32->bps_used = 0;
1976         lm32->wps_used = 0;
1977
1978         return lm32_init_arch_info(target, lm32, target->tap,
1979                         target->variant);
1980 }
1981
1982 static int
1983 lm32_bulk_write_memory(struct target *target, uint32_t address, uint32_t count, uint8_t *buffer)
1984 {
1985         /* XXX endianess ? */
1986         return lm32_write_memory(target, address, 1, 4*count, buffer);
1987 }
1988
1989 static int
1990 lm32_checksum_memory(struct target *target, uint32_t address, uint32_t size, uint32_t *checksum)
1991 {
1992         return ERROR_FAIL; /* use bulk read method */
1993 }
1994
1995 /*
1996  * command handlers
1997  */
1998 COMMAND_HANDLER(lm32_handle_break_command)
1999 {
2000         struct target *target = get_current_target(CMD_CTX);
2001         return lm32_dp_send_command(target, LM32_DP_BREAK, 1);
2002 }
2003
2004 COMMAND_HANDLER(lm32_handle_reset_command)
2005 {
2006         struct target *target = get_current_target(CMD_CTX);
2007         target->state = TARGET_RUNNING;
2008         return lm32_dp_send_command(target, LM32_DP_RESET, 1);
2009 }
2010
2011 COMMAND_HANDLER(lm32_handle_status_command)
2012 {
2013         struct target *target = get_current_target(CMD_CTX);
2014         uint8_t status;
2015         int ret;
2016
2017         ret = lm32_get_status(target, &status);
2018         if (ret != ERROR_OK)
2019                 return ret;
2020
2021         command_print(CMD_CTX, "rx_ready=%i tx_full=%i processing=%i",
2022                 (status & LM32_STAT_RX_READY) ? 1 : 0,
2023                 (status & LM32_STAT_TX_FULL) ? 1 : 0,
2024                 (status & LM32_STAT_PROCESSING) ? 1 : 0
2025         );
2026
2027         return ERROR_OK;
2028 }
2029
2030 COMMAND_HANDLER(lm32_handle_cfg_command)
2031 {
2032         struct target *target = get_current_target(CMD_CTX);
2033         uint32_t cfg;
2034         int ret;
2035
2036         ret = lm32_mon_read_csr(target, LM32_CSR_CFG, &cfg);
2037         if (ret != ERROR_OK)
2038                 return ret;
2039
2040         static const char* on_off[2] = { "enabled", "disabled" };
2041
2042         command_print(CMD_CTX, "CFG=0x%08x", cfg);
2043         command_print(CMD_CTX, "  Multiply:      %s",
2044                         on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
2045         command_print(CMD_CTX, "  Divide:        %s",
2046                         on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
2047         command_print(CMD_CTX, "  Barrel shift:  %s",
2048                         on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
2049         command_print(CMD_CTX, "  Sign extend:   %s",
2050                         on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
2051         command_print(CMD_CTX, "  User:          %s",
2052                         on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
2053         command_print(CMD_CTX, "  Cycle counter: %s",
2054                         on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
2055         command_print(CMD_CTX, "  D-Cache:       %s",
2056                         on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
2057         command_print(CMD_CTX, "  I-Cache:       %s",
2058                         on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
2059         command_print(CMD_CTX, "  Debug:         %s",
2060                         on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
2061         command_print(CMD_CTX, "  H/W debug:     %s",
2062                         on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
2063         command_print(CMD_CTX, "  ROM debug:     %s",
2064                         on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
2065         command_print(CMD_CTX, "  JTAG UART:     %s",
2066                         on_off[(cfg & LM32_CSR_CFG_M) ? 0 : 1]);
2067         command_print(CMD_CTX, "  Interrupts:    %d",
2068                         LM32_CSR_CFG_INT(cfg));
2069         command_print(CMD_CTX, "  Breakpoints:   %d",
2070                         LM32_CSR_CFG_BP(cfg));
2071         command_print(CMD_CTX, "  Watchpoints:   %d",
2072                         LM32_CSR_CFG_WP(cfg));
2073         command_print(CMD_CTX, "  Revision:      %d",
2074                         LM32_CSR_CFG_REV(cfg));
2075
2076         return ERROR_OK;
2077 }
2078
2079 COMMAND_HANDLER(lm32_handle_juart_read_command)
2080 {
2081         struct target *target = get_current_target(CMD_CTX);
2082         uint8_t data;
2083         int ret;
2084
2085         if ((ret = lm32_read_jrx(target, &data)) != ERROR_OK)
2086         {
2087                 LOG_ERROR("error reading rx");
2088                 return ret;
2089         }
2090
2091         command_print(CMD_CTX, "rx=%02x", data);
2092
2093         return ERROR_OK;
2094 }
2095
2096 COMMAND_HANDLER(lm32_handle_juart_write_command)
2097 {
2098         struct target *target = get_current_target(CMD_CTX);
2099         uint8_t byte;
2100
2101         if (CMD_ARGC < 1)
2102         {
2103                 LOG_ERROR("'lm32 juart write <byte>' command takes one operands");
2104                 return ERROR_OK;
2105         }
2106         
2107         COMMAND_PARSE_NUMBER(u8, CMD_ARGV[0], byte);
2108
2109         return lm32_write_jtx(target, byte);
2110 }
2111
2112 COMMAND_HANDLER(lm32_handle_monitor_version_command)
2113 {
2114         struct target *target = get_current_target(CMD_CTX);
2115         int ret;
2116         uint8_t version;
2117
2118         ret = lm32_mon_version(target, &version);
2119         if (ret != ERROR_OK)
2120         {
2121                 LOG_ERROR("error reading version");
2122                 return ret;
2123         }
2124
2125         command_print(CMD_CTX, "monitor version is %d", version);
2126
2127         return ERROR_OK;
2128 }
2129
2130 COMMAND_HANDLER(lm32_handle_monitor_sh_command)
2131 {
2132         struct target *target = get_current_target(CMD_CTX);
2133         int ret;
2134         uint32_t addr;
2135         uint16_t data;
2136
2137         if (CMD_ARGC < 2)
2138         {
2139                 LOG_ERROR("'lm32 monitor sh <addr> <data>' command takes two operands");
2140                 return ERROR_OK;
2141         }
2142         
2143         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
2144         COMMAND_PARSE_NUMBER(u16, CMD_ARGV[1], data);
2145
2146         ret = lm32_mon_store_u16(target, addr, data);
2147         if (ret != ERROR_OK)
2148         {
2149                 LOG_ERROR("error storing halfword");
2150                 return ret;
2151         }
2152
2153         return ERROR_OK;
2154 }
2155
2156 COMMAND_HANDLER(lm32_handle_monitor_sw_command)
2157 {
2158         struct target *target = get_current_target(CMD_CTX);
2159         int ret;
2160         uint32_t addr;
2161         uint32_t data;
2162
2163         if (CMD_ARGC < 2)
2164         {
2165                 LOG_ERROR("'lm32 monitor sw <addr> <data>' command takes two operands");
2166                 return ERROR_OK;
2167         }
2168         
2169         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
2170         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], data);
2171
2172         ret = lm32_mon_store_u32(target, addr, data);
2173         if (ret != ERROR_OK)
2174         {
2175                 LOG_ERROR("error storing word");
2176                 return ret;
2177         }
2178
2179         return ERROR_OK;
2180 }
2181
2182 COMMAND_HANDLER(lm32_handle_monitor_lh_command)
2183 {
2184         struct target *target = get_current_target(CMD_CTX);
2185         int ret;
2186         uint32_t addr;
2187         uint16_t data;
2188
2189         if (CMD_ARGC < 1)
2190         {
2191                 LOG_ERROR("'lm32 monitor lh <addr>' command takes one operand");
2192                 return ERROR_OK;
2193         }
2194         
2195         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
2196
2197         ret = lm32_mon_load_u16(target, addr, &data);
2198         if (ret != ERROR_OK)
2199         {
2200                 LOG_ERROR("error loading halfword");
2201                 return ret;
2202         }
2203
2204         command_print(CMD_CTX, "0x%04x", data);
2205
2206         return ERROR_OK;
2207 }
2208
2209 COMMAND_HANDLER(lm32_handle_monitor_lw_command)
2210 {
2211         struct target *target = get_current_target(CMD_CTX);
2212         int ret;
2213         uint32_t addr;
2214         uint32_t data;
2215
2216         if (CMD_ARGC < 1)
2217         {
2218                 LOG_ERROR("'lm32 monitor lw <addr>' command takes one operand");
2219                 return ERROR_OK;
2220         }
2221         
2222         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
2223
2224         ret = lm32_mon_load_u32(target, addr, &data);
2225         if (ret != ERROR_OK)
2226         {
2227                 LOG_ERROR("error loading halfword");
2228                 return ret;
2229         }
2230
2231         command_print(CMD_CTX, "0x%08x", data);
2232
2233         return ERROR_OK;
2234 }
2235
2236 COMMAND_HANDLER(lm32_handle_dp_read_command)
2237 {
2238         struct target *target = get_current_target(CMD_CTX);
2239         uint32_t addr;
2240         uint8_t data;
2241         int ret;
2242
2243         if (CMD_ARGC < 1)
2244         {
2245                 LOG_ERROR("'lm32 dp read <addr>' command takes one operand");
2246                 return ERROR_OK;
2247         }
2248         
2249         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
2250
2251         ret = lm32_dp_read_memory_addr(target, addr, &data);
2252         if (ret != ERROR_OK)
2253                 return ret;
2254
2255         command_print(CMD_CTX, "%08x=%02x", addr, data);
2256
2257         return ERROR_OK;
2258 }
2259
2260 COMMAND_HANDLER(lm32_handle_dp_read_sequential_command)
2261 {
2262         struct target *target = get_current_target(CMD_CTX);
2263         uint8_t data;
2264         int ret;
2265
2266         ret = lm32_dp_read_sequential(target, &data, 1);
2267         if (ret != ERROR_OK)
2268                 return ret;
2269
2270         command_print(CMD_CTX, "%02x", data);
2271
2272         return ERROR_OK;
2273 }
2274
2275 COMMAND_HANDLER(lm32_handle_dp_write_command)
2276 {
2277         struct target *target = get_current_target(CMD_CTX);
2278         uint32_t addr;
2279         uint8_t data;
2280         int ret;
2281
2282         if (CMD_ARGC < 2)
2283         {
2284                 LOG_ERROR("'lm32 dp write <addr> <byte>' command takes two operands");
2285                 return ERROR_OK;
2286         }
2287         
2288         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
2289         COMMAND_PARSE_NUMBER(u8, CMD_ARGV[1], data);
2290
2291         ret = lm32_dp_write_memory_addr(target, addr, data);
2292         if (ret != ERROR_OK)
2293                 return ret;
2294
2295         return ERROR_OK;
2296 }
2297
2298 COMMAND_HANDLER(lm32_handle_dp_write_sequential_command)
2299 {
2300         struct target *target = get_current_target(CMD_CTX);
2301         uint8_t data;
2302         int ret;
2303
2304         if (CMD_ARGC < 1)
2305         {
2306                 LOG_ERROR("'lm32 dp write_seq <byte>' command takes one operand");
2307                 return ERROR_OK;
2308         }
2309         
2310         COMMAND_PARSE_NUMBER(u8, CMD_ARGV[0], data);
2311
2312         ret = lm32_dp_write_sequential(target, data, 1);
2313         if (ret != ERROR_OK)
2314                 return ret;
2315
2316         return ERROR_OK;
2317 }
2318
2319 COMMAND_HANDLER(lm32_handle_debug_protocol_command)
2320 {
2321         struct target *target = get_current_target(CMD_CTX);
2322         struct lm32 *lm32 = target_to_lm32(target);
2323
2324         if (CMD_ARGC > 0)
2325                 COMMAND_PARSE_ENABLE(CMD_ARGV[0], lm32->dp_enabled);
2326
2327         command_print(CMD_CTX, "debug protocol is %s", (lm32->dp_enabled) ? "enabled" : "disabled");
2328
2329         return ERROR_OK;
2330 }
2331
2332 COMMAND_HANDLER(lm32_handle_ignore_ack_command)
2333 {
2334         struct target *target = get_current_target(CMD_CTX);
2335         struct lm32 *lm32 = target_to_lm32(target);
2336
2337         if (CMD_ARGC > 0)
2338                 COMMAND_PARSE_ON_OFF(CMD_ARGV[0], lm32->ignore_ack);
2339
2340         command_print(CMD_CTX, "ignore ack is %s", (lm32->ignore_ack) ? "on" : "off");
2341
2342         return ERROR_OK;
2343 }
2344
2345 static const struct command_registration lm32_dp_command_handlers[] =
2346 {
2347         {
2348                 .name = "read",
2349                 .handler = lm32_handle_dp_read_command,
2350                 .mode = COMMAND_EXEC,
2351                 .help = "Execute debug protocol read command",
2352                 .usage = "addr",
2353         },
2354         {
2355                 .name = "read_seq",
2356                 .handler = lm32_handle_dp_read_sequential_command,
2357                 .mode = COMMAND_EXEC,
2358                 .help = "Execute debug protocol read sequential command",
2359                 .usage = "",
2360         },
2361         {
2362                 .name = "write",
2363                 .handler = lm32_handle_dp_write_command,
2364                 .mode = COMMAND_EXEC,
2365                 .help = "Execute debug protocol write command",
2366                 .usage = "addr byte",
2367         },
2368         {
2369                 .name = "write_seq",
2370                 .handler = lm32_handle_dp_write_sequential_command,
2371                 .mode = COMMAND_EXEC,
2372                 .help = "Execute debug protocol write sequential command",
2373                 .usage = "byte",
2374         },
2375         COMMAND_REGISTRATION_DONE
2376 };
2377
2378 static const struct command_registration lm32_monitor_command_handlers[] =
2379 {
2380         {
2381                 .name = "version",
2382                 .handler = lm32_handle_monitor_version_command,
2383                 .mode = COMMAND_EXEC,
2384                 .help = "Retrieve monitor version",
2385                 .usage = "",
2386         },
2387         {
2388                 .name = "sh",
2389                 .handler = lm32_handle_monitor_sh_command,
2390                 .mode = COMMAND_EXEC,
2391                 .help = "Store halfword",
2392                 .usage = "addr data",
2393         },
2394         {
2395                 .name = "sw",
2396                 .handler = lm32_handle_monitor_sw_command,
2397                 .mode = COMMAND_EXEC,
2398                 .help = "Store word",
2399                 .usage = "addr data",
2400         },
2401         {
2402                 .name = "lh",
2403                 .handler = lm32_handle_monitor_lh_command,
2404                 .mode = COMMAND_EXEC,
2405                 .help = "Load halfword",
2406                 .usage = "addr",
2407         },
2408         {
2409                 .name = "lw",
2410                 .handler = lm32_handle_monitor_lw_command,
2411                 .mode = COMMAND_EXEC,
2412                 .help = "Load word",
2413                 .usage = "addr",
2414         },
2415         COMMAND_REGISTRATION_DONE
2416 };
2417
2418 static const struct command_registration lm32_juart_command_handlers[] =
2419 {
2420         {
2421                 .name = "read",
2422                 .handler = lm32_handle_juart_read_command,
2423                 .mode = COMMAND_EXEC,
2424                 .help = "Read from the JTAG uart.",
2425                 .usage = "",
2426         },
2427         {
2428                 .name = "write",
2429                 .handler = lm32_handle_juart_write_command,
2430                 .mode = COMMAND_EXEC,
2431                 .help = "Write to the JTAG uart.",
2432                 .usage = "byte",
2433         },
2434         COMMAND_REGISTRATION_DONE
2435 };
2436
2437 static const struct command_registration lm32_any_command_handlers[] =
2438 {
2439         {
2440                 .name = "reset",
2441                 .handler = lm32_handle_reset_command,
2442                 .mode = COMMAND_ANY,
2443                 .help = "Set PC to reset vector.",
2444                 .usage = "",
2445         },
2446         {
2447                 .name = "break",
2448                 .handler = lm32_handle_break_command,
2449                 .mode = COMMAND_ANY,
2450                 .help = "Set PC to debug vector.",
2451                 .usage = "",
2452         },
2453         {
2454                 .name = "status",
2455                 .handler = lm32_handle_status_command,
2456                 .mode = COMMAND_EXEC,
2457                 .help = "Show status flags.",
2458                 .usage = "",
2459         },
2460         {
2461                 .name = "cfg",
2462                 .handler = lm32_handle_cfg_command,
2463                 .mode = COMMAND_EXEC,
2464                 .help = "Display processor CFG register.",
2465                 .usage = "",
2466         },
2467         {
2468                 .name = "debug_protocol",
2469                 .handler = lm32_handle_debug_protocol_command,
2470                 .mode = COMMAND_EXEC,
2471                 .help = "Use the hw-assisted debug protocol to do memory transfers.",
2472                 .usage = "['enable'|'disable']",
2473         },
2474         {
2475                 .name = "ignore_ack",
2476                 .handler = lm32_handle_ignore_ack_command,
2477                 .mode = COMMAND_EXEC,
2478                 .help = "Don't wait for an ACK before transferring the next data byte.",
2479                 .usage = "['on'|'off']",
2480         },
2481         {
2482                 .name = "dp",
2483                 .mode = COMMAND_ANY,
2484                 .help = "JTAG debug protocol commands",
2485                 .chain = lm32_dp_command_handlers,
2486         },
2487         {
2488                 .name = "monitor",
2489                 .mode = COMMAND_ANY,
2490                 .help = "JTAG monitor commands",
2491                 .chain = lm32_monitor_command_handlers,
2492         },
2493         {
2494                 .name = "juart",
2495                 .mode = COMMAND_ANY,
2496                 .help = "JTAG uart commands",
2497                 .chain = lm32_juart_command_handlers,
2498         },
2499         COMMAND_REGISTRATION_DONE
2500 };
2501
2502 static const struct command_registration lm32_command_handlers[] =
2503 {
2504         {
2505                 .name = "lm32",
2506                 .mode = COMMAND_ANY,
2507                 .help = "lm32 command group",
2508                 .chain = lm32_any_command_handlers,
2509         },
2510         COMMAND_REGISTRATION_DONE
2511 };
2512
2513 struct target_type lm32_target =
2514 {
2515         .name = "lm32",
2516
2517         .poll = lm32_poll,
2518         .arch_state = lm32_arch_state,
2519
2520         .target_request_data = NULL,
2521
2522         .halt = lm32_halt,
2523         .resume = lm32_resume,
2524         .step = lm32_step,
2525
2526         .assert_reset = lm32_assert_reset,
2527         .deassert_reset = lm32_deassert_reset,
2528         .soft_reset_halt = NULL,
2529
2530         .get_gdb_reg_list = lm32_get_gdb_reg_list,
2531
2532         .read_memory = lm32_read_memory,
2533         .read_phys_memory = NULL,
2534         .write_memory = lm32_write_memory,
2535         .write_phys_memory = NULL,
2536         .bulk_write_memory = lm32_bulk_write_memory,
2537
2538         .checksum_memory = lm32_checksum_memory,
2539         .blank_check_memory = NULL,
2540
2541         .run_algorithm = NULL,
2542
2543         .add_breakpoint = lm32_add_breakpoint,
2544         .remove_breakpoint = lm32_remove_breakpoint,
2545         .add_watchpoint = lm32_add_watchpoint,
2546         .remove_watchpoint = lm32_remove_watchpoint,
2547
2548         .commands = lm32_command_handlers,
2549         .target_create = lm32_target_create,
2550         .init_target = lm32_init_target,
2551         .examine = lm32_examine,
2552
2553         .virt2phys = NULL,
2554         .mmu = NULL,
2555 };
2556