summaryrefslogtreecommitdiff
path: root/Debugger/debug_comm.S
blob: 6a7aa11933dde2cfefaf69bd234c17176c762a3f (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
/** @file debug_comm.S
 *  @brief GDB Server communications support routines
 *
 */

/* Copyright (C) 2007-2011 the NxOS developers
 *
 * Module Developed by: TC Wan <tcwan@cs.usm.my>
 *
 * See AUTHORS for a full list of the developers.
 *
 * See COPYING for redistribution license
 *
 */


/* #define __NXOS__		*/	/* Defined externally in SConstruct */

#define __ASSEMBLY__
#include "debug_macros.h"
#include "debug_stub.h"
#include "debug_comm.h"

.bss
.align 4

debug_InUSBBuf:
	.space	USB_BUFSIZE,0
debug_OutUSBBuf:
	.space	USB_BUFSIZE,0

debug_msgRxBufPtr:
	.word	0x0
debug_msgTxBufPtr:
	.word	0x0

debug_msgRxBuf_AppendPtr:
    .word   0x0
debug_msgTxBuf_AppendPtr:
    .word   0x0

    .equ    RXAPPENDPTR_OFFSET, (debug_msgRxBuf_AppendPtr - debug_msgRxBufPtr)
    .equ    TXAPPENDPTR_OFFSET, (debug_msgTxBuf_AppendPtr - debug_msgTxBufPtr)

debug_segmentRxNum:				/* Current Rx Segment Number */
	.word	0x0

#ifndef __NXOS__
debug_nxtMsgLength:            /* NXT Firmware Received USB Message Length */
    .word   0x0
#endif

.data
.align 4

nxt_usbcmd_header:
	.byte	USB_NXT_TELEGRAMTYPE, 0x00, 0x00		/* padded to 3 bytes */

hex2char_lut:
	.ascii	"0123456789ABCDEF"

/* Macros
 */


/* _hex2char_lut
 *		Internal routine to intialize the LUT address pointer
 */
	.macro _hex2char_lut	addrptr
	ldr		\addrptr, =hex2char_lut
	.endm

/* _hex2char_cont
 *		Internal routine that assumes that the LUT has been loaded.
 *		This macro accepts a byte sized hex value as a parameter register(7:0) and returns the
 *		ASCII equivalent in in the same register(7:0)
 *		The second parameter is the LUT address pointer register to use (assumed to be initialized)
 *		WARNING: Assumes that the value in register is sanity checked before invoking macro
 */
	.macro	_hex2char_cont	reg, addrptr
	ldrb	\reg, [\addrptr, \reg]
	.endm

/* _hex2char
 *		This macro accepts a byte sized hex value as a parameter register(7:0) and returns the
 *		ASCII equivalent in in the same register(7:0)
 *		The second parameter is the LUT address pointer register to use (register content is destroyed)
 *		WARNING: Assumes that the value in register is sanity checked before invoking macro
 */
	.macro	_hex2char	reg, addrptr
	_hex2char_lut	\addrptr
	_hex2char_cont	\reg, \addrptr
	.endm

/* _char2hex
 *		This macro accepts an ASCII char as a parameter register(7:0) and returns the
 *		equivalent byte sized hex value in in the same register(7:0)
 *		WARNING: Assumes that the char in register is a valid hex char before invoking macro
 */
	.macro	_char2hex	reg
	cmp	\reg, #'A'									/* If Alpha */
	bichs	\reg, \reg, #ASCII_LOWER2UPPER_MASK		/* Convert to Uppercase */
	subhs	\reg, \reg, #7							/* Adjustment to allow for subtraction with 0x30 */
	sub		\reg, \reg, #0x30						/* get final hex value */
	.endm


.code 32
.text
.align 	4


/* Utility Routines
 */

/* hex2char
 *		This routine accepts a byte sized hex value in R0(7:0) and returns the
 *		ASCII equivalent in R0(7:0)
 */
	.global	hex2char

hex2char:
	stmfd	sp!, {r1,lr}
	and		r0, #NIBBLE0			/* make sure that input is sane */
	_hex2char	r0, r1
	ldmfd	sp!, {r1,pc}

/* char2hex
 *		This routine accepts an ASCII character in R0(7:0) and returns the
 *		equivalent byte sized hex value in R0(7:0)
 */
	.global	char2hex

char2hex:
	and		r0, #BYTE0			/* make sure that input is sane */
	cmp		r0, #'0'
	blo		exit_char2hex
	cmp		r0, #'F'
	bhi		exit_char2hex
	_char2hex	r0
exit_char2hex:
	bx		lr

/* byte2ascii_cont
 *      (Shared routine, does not perform sanity checks)
 *      On entry:
 *          R0: ASCII buffer pointer
 *          R1[7:0]: byte value
 *      On exit:
 *          R0: Address of next empty char slot in buffer
 *          R1: Destroyed
 *
 *      This routine accepts an ASCII buffer pointer in R0 and a byte value in R1,
 *		and stores the ASCII equivalent byte value in the buffer pointed to by R0.
 *		Note: On return, R0 points to next empty char slot in buffer
 */
byte2ascii_cont:
	stmfd	sp!, {r2,r3,r4, lr}
	lsl		r2, r1, #24			    /* Keep copy of input byte value R1[7:0], shifted to MSB R2[31:24] */
	mov		r4, #2					/* Loop counter */
	_hex2char_lut	r3				/* initialize LUT pointer */
1:	ror		r2, r2, #28 			/* Rotate MSNibble R2[31:28] into LSNibble position R2[3:0] */
	and		r1, r2, #NIBBLE0		/* Mask out everything else, store Nibble in R1 */
	_hex2char_cont	r1, r3			/* Convert nibble to ASCII char */
	strb	r1, [r0], #1
	subs	r4, r4, #1				/* decrement loop counter */
	bne		1b
	ldmfd	sp!, {r2,r3,r4, pc}

/* byte2ascii
 *      On entry:
 *          R0: ASCII buffer pointer
 *          R1[7:0]: Byte value
 *      On exit:
 *          R0: Address of next empty char slot in buffer
 *          R1: Original Address of Buffer
 *
 *		This routine accepts an ASCII buffer pointer in R0 and a byte value in R1,
 *		and stores the ASCII equivalent byte value in the buffer pointed to by R0.
 *		Note: On return, R0 points to the next empty char slot in buffer
 */
	.global	byte2ascii

byte2ascii:
	stmfd	sp!, {r0, lr}			/* Keep ASCII buffer pointer */
	and		r1, #BYTE0				/* sanitize input */
	bl		byte2ascii_cont
	ldmfd	sp!, {r1, pc}			/* return original string pointer in R1 */

/* halfword2ascii
 *      On entry:
 *          R0: ASCII buffer pointer
 *          R1[15:0]: Halfword value
 *      On exit:
 *          R0: Address of next empty char slot in buffer
 *          R1: Original Address of Buffer
 *
 *      This routine accepts an ASCII buffer pointer in R0 and a halfword value in R1,
 *      and stores the ASCII equivalent halfword value in the buffer pointed to by R0.
 *      Note: On return, R0 points to the next empty char slot in buffer
 */
	.global	halfword2ascii
halfword2ascii:
	stmfd	sp!, {r0,r2,r3, lr}		/* Keep ASCII buffer pointer */
	mov		r2, r1, lsl #16			/* copy of input halfword value R1[15:0], shifted to MSH R2[31:16] */
	mov		r3, #2					/* Loop Counter */
	b		_conv_byte2ascii		/* goto Byte conversion loop */

/* word2ascii
 *      On entry:
 *          R0: ASCII buffer pointer
 *          R1[31:0]: Word value
 *      On exit:
 *          R0: Address of next empty char slot in buffer
 *          R1: Original Address of Buffer
 *
 *      This routine accepts an ASCII buffer pointer in R0 and a word value in R1,
 *      and stores the ASCII equivalent word value in the buffer pointed to by R0.
 *      Note: On return, R0 points to the next empty char slot in buffer
 */
	.global	word2ascii
word2ascii:
	stmfd	sp!, {r0,r2,r3, lr}		/* Keep ASCII buffer pointer */
	mov		r2, r1					/* copy of input word value R1[31:0] */
	mov		r3, #4					/* Loop Counter */

	/* Fall through to byte coversion loop */

_conv_byte2ascii:
	ror		r2, r2, #24			    /* Rotate MSB R2[31:24] into LSB position R2[7:0] */
	and		r1, r2, #BYTE0			/* Copy byte value in R2[7:0] into R1 */
	bl		byte2ascii_cont         /* R0: next ASCII buffer location pointer, R1: destroyed */
	subs	r3, r3, #1
	bne		_conv_byte2ascii
	ldmfd	sp!, {r1,r2,r3, pc}


/* ascii2byte
 *      On entry:
 *          R0: ASCII buffer pointer
 *      On exit:
 *          R0[7:0]: Byte value
 *          R1: Address of next char slot in buffer
 *
 *		This routine accepts an ASCII buffer pointer in R0,
 *		and returns the byte value in R0[7:0].
 *		Note: On return, R1 points to the ASCII buffer location after the current 2 chars.
 *		WARNING: This routine assumes that the input buffer was sanitized and contains valid Hex chars,
 *				 otherwise it will return invalid results.
 */
	.global	ascii2byte

ascii2byte:
	stmfd	sp!, {r2,r3, lr}
	mov		r3, #2					/* Loop counter */
	b		_conv_ascii2byte

/* ascii2halfword
 *      On entry:
 *          R0: ASCII buffer pointer
 *      On exit:
 *          R0[15:0]: Halfword value
 *          R1: Address of next char slot in buffer
 *
 *		This routine accepts an ASCII buffer pointer in R0,
 *		and returns the Halfword value in R0[15:0].
 *		Note: On return, R1 points to the ASCII buffer location after the current 4 chars.
 *		WARNING: This routine assumes that the input buffer was sanitized and contains valid Hex chars,
 *				 otherwise it will return invalid results.
 */
	.global	ascii2halfword

ascii2halfword:
	stmfd	sp!, {r2,r3, lr}
	mov		r3, #4					/* Loop counter */
	b		_conv_ascii2byte


/* ascii2word
 *      On entry:
 *          R0: ASCII buffer pointer
 *      On exit:
 *          R0[31:0]: Word value
 *          R1: Address of next char slot in buffer
 *
 *		This routine accepts an ASCII buffer pointer in R0,
 *		and returns the word value in R0[31:0].
 *		Note: On return, R1 points to the ASCII buffer location after the current 8 chars.
 *		WARNING: This routine assumes that the input buffer was sanitized and contains valid Hex chars,
 *				 otherwise it will return invalid results.
 */
	.global	ascii2word

ascii2word:
	stmfd	sp!, {r2,r3, lr}
	mov		r3, #8					/* Loop counter */

	/* Fall through to byte coversion loop */

_conv_ascii2byte:
	teq		r0, #0
	beq		_exit_conv_ascii2byte	/* exit if NULL pointer in R0 */
	mov		r1, r0					/* Copy of ASCII buffer pointer */
	mov		r2, #0					/* Initialize results */
2:	ldrb	r0, [r1], #1			/* Load ASCII char */
	bl		char2hex				/* on return, hex value in R0 */
	orr		r2, r0, r2, lsl #4		/* merge Nibble into results */
	subs	r3, r3, #1
	bne		2b
	mov		r0, r2					/* Copy it to R0 as return value */
_exit_conv_ascii2byte:
	ldmfd	sp!, {r2,r3, pc}		/* return hex value in R0 */




/* Debugger Communications Routines
 *		It does not make sense to pass information from the Debugger Module to the Comm. link one character
 *		at a time, especially if we're not using a native serial interface (e.g., EIA-232). Consequently
 *		a Message interface has been defined. This can still call getChar() and putChar() subroutines
 *		if so desired, but it'll be a purely internal matter.
 *
 * Message Format
 *		Since we need to use EP1&2 (Bulk channels) to communicate with the PC Host, the messages should
 *		follow the NXT Direct Commands message structure (this will allow for interoperability with NXT Firmware
 *		in addition to NxOS). The maximum length of any USB communications via the Bulk channel is 64 bytes.
 *		There is a one byte Telegram Type field which identifies the type of telegram, followed by the
 *		Telegram header and actual message.
 *
 *		The LEGO Mindstorms Communications Protocol Direct Commands GDB Message format (including all headers)
 *		is as follows:
 *
 *		GDB Command
 *		===========
 *			Byte 0:	Telegram Type Field (0x8d Direct Command, No response required)	| USB Channel Header
 *			Byte 1:	Segment No (1-255, 0: Last Segment; limit is MSG_NUMSEGMENTS)	|
 *			Byte 2:	Telegram Size (Len of USB Buffer - 3, max is MSG_SEGMENTSIZE)	|
 *			Byte 3-N: Message data													| GDB Command
 *
 *		The GDB Command (of size M) has the following format:
 *				Offset 0: '$'
 *				Offset 1: GDB Command char
 *				Offset 2 - (M-4): Command packet info
 *				Offset M-3: '#'
 *				Offset M-2: MSB of Checksum
 *				Offset M-1: LSB of Checksum
 *
 *		The maximum size of a GDB Command packet is MSGBUF_SIZE - 4 ('$', '#', 2 byte checksum)
 *
 *		GDB Response
 *		============
 *			Byte 0:	Telegram Type Field (0x8d Direct Command, No response required)	| USB Channel Header
 *			Byte 1:	Segment No (1-255, 0: Last Segment; limit is MSG_NUMSEGMENTS)	|
 *			Byte 2:	Telegram Size (Len of USB Buffer - 3, max is MSG_SEGMENTSIZE)	|
 *			Byte 3-N: Message data													| GDB Response
 *
 *		The GDB Response (of size M) has the following format:
 *				Offset 0: '+' or '-' Command Received Status
 *				Offset 1: '$'
 *				Offset 2 - (M-4): Response packet info
 *				Offset M-3: '#'
 *				Offset M-2: MSB of Checksum
 *				Offset M-1: LSB of Checksum
 *
 *		The maximum size of a GDB Response packet is MSGBUF_SIZE - 5 ('-'/'+', '$', '#', 2 byte checksum)
 *
 *		Note: The Telegram Size is the actual size of the Message Data portion
 *			  (i.e., excludes the three header bytes, includes the GDB Command/Response Packet checksum bytes
 *					 in the last segment)
 */

 	.global dbg__comm_init
/* dbg__comm_init
 *      Initialize communications channel.
 *      On Entry:
 *          R0: MSG Rx Buf Pointer
 *          R1: MSG Tx Buf Pointer
 */

dbg__comm_init:
	stmfd	sp!, {lr}
	ldr     r2, =debug_msgRxBufPtr
	stmia   r2!, {r0, r1}          /* debug_msgRxBufPtr and debug_msgTxBufPtr */
	stmia   r2!, {r0, r1}          /* debug_msgRxBuf_AppendPtr and debug_msgTxBuf_AppendPtr */
	bl		_dbg__usb_readbuf_reset
	ldmfd	sp!, {pc}

_dbg__usb_readbuf_reset:
#ifdef __NXOS__
	stmfd	sp!, {lr}
	ldr		r0, =debug_InUSBBuf
	mov		r1, #USB_BUFSIZE
	ldr		r2, =nx_usb_read
	mov		lr, pc
	bx		r2
	ldmfd	sp!, {pc}
#else
    ldr     r1, =debug_nxtMsgLength
    mov     r0, #0
    str     r0, [r1]                        /* Clear NXT USB Received Message Length */
	bx		lr

    .global dbg__copyNxtDebugMsg
/* dbg__copyNxtDebugMsg
 *      Copy NXT Firmware Debug Message to our own Buffers, indicate Msg Received status.
 *      On Entry:
 *          R0: NXT USB Input Buf Pointer
 *          R1: USB Command Bit
 *          R2: USB Raw Message Length
 *      On Exit:
 *          R0-R3: Destroyed
 */

dbg__copyNxtDebugMsg:
    ldr     r1, =debug_nxtMsgLength
    str     r2, [r1]
    ldr     r1, =debug_InUSBBuf
    _dbg_memcpy r1, r0, r2, r3                  /* r3: scratch register */
    bx      lr
#endif

/* _dbg_reset_msgTxBuf_AppendPtr
 *      Internal variable to reset pointers.
 *      On Exit:
 *          R0: debug_msgTxBuf_AppendPtr
 *          R1: destroyed
 */
_dbg_reset_msgTxBuf_AppendPtr:
    ldr     r1, =debug_msgTxBufPtr              /* Should not be modified */
    ldr     r0, [r1]
    str     r0, [r1, #TXAPPENDPTR_OFFSET]
    mov     pc, lr

/* _dbg__usbHasMsg
 *		Internal Segment Reassembly Routine.
 *		On exit:
 *			r0: !0: (Availale Telegram Message Size), 0: no incoming message/segment
 *			r1: message segment number
 */
_dbg__usbHasMsg:
	stmfd	sp!, {lr}
#ifdef __NXOS__
	ldr		r2, =nx_usb_data_read
	mov		lr,pc
	bx		r2					    /* Number of bytes read in R0 */
	/* Note: The return value in R0 is the USB Buffer Size, includes NXT Direct Command Header */
#else
    ldr     r0, =debug_nxtMsgLength
    ldr     r0, [r0]                /* R0 contains the USB Buffer Size, including the NXT Direct Command Header */
#endif

	ldr		r2, =debug_InUSBBuf
	ldrb    r1, [r2, #USB_NXT_TELEGRAMTYPE_OFFSET]
	cmp     r1, #USB_NXT_TELEGRAMTYPE
	bne     invalid_USBMsg          /* Invalid telegram type, ignore */

	ldrb	r1, [r2, #USB_NXT_TELEGRAMSIZE_OFFSET]
	sub     r0, r0, r1              /* USB Buffer Size - Telegram Size = 3 (header size) */
	cmp     r0, #USB_GDBMSG_START   /* Start offset is equal to header size */
    bne     invalid_USBMsg          /* Invalid Message Length, ignore */

    mov     r0, r1                  /* Telegram Message Size */
	ldrb	r1, [r2, #USB_NXT_SEGNUM_OFFSET]
    b       _exit_dbg__usbHasMsg

invalid_USBMsg:
    mov   r0, #0
_exit_dbg__usbHasMsg:
    ldmfd   sp!, {pc}

/* _copy_msg_from_usbbuf
 *      Internal USB buffer copy routine, handles segment reassembly.
 *      On entry:
 *          r0: number of bytes to copy
 *          r1: segment number
 *      On exit:
 *          r0: cummulative message length
 *          r1: segment number
 *          r2, r3: Destroyed
 */
_copy_msg_from_usbbuf:
    stmfd   sp!, {r1,r4,r5,r6,lr}
    movs    r4, r0
    beq     _exit_copy_msg_from_usbbuf

    ldr     r6, =debug_msgRxBufPtr              /* Address of Pointers */
    ldr     r5, [r6]                            /* Rx buffer Start Address */
    ldr     r2, [r6, #RXAPPENDPTR_OFFSET]       /* Append Pointer */

    sub     r3, r2, r5                          /* r3: current length of message */
    add     r3, r3, r4                          /* new cummulative length of message */
    cmp     r3, #MSGBUF_SIZE
    movhi   r4, #0                              /* Buffer overflow! */
    strhi   r5, [r6, #RXAPPENDPTR_OFFSET]       /* Reset AppendPtr to beginning of Rx Buffer */
    bhi     _exit_copy_msg_from_usbbuf

    ldr     r3, =debug_InUSBBuf
    add     r3, r3, #USB_GDBMSG_START
    _dbg_memcpy r2, r3, r4, r0                  /* r2 updated to point to next empty char slot in Rx buffer */
    sub     r4, r2, r5                          /* r4: cummulative length of message */

    /* Update debug_msgRxBuf_AppendPtr */
    teq     r1, #0                              /* Check if this is last segment (segment 0) */
    streq   r5, [r6, #RXAPPENDPTR_OFFSET]       /* Reset AppendPtr to beginning of Rx Buffer if so */
    strne   r2, [r6, #RXAPPENDPTR_OFFSET]       /* Otherwise, update Append Pointer to receive next segment */

_exit_copy_msg_from_usbbuf:
    bl      _dbg__usb_readbuf_reset             /* Next USB telegram transaction */
    mov     r0, r4                              /* Return cummulative message length in R0 */
    ldmfd   sp!, {r1,r4,r5,r6,pc}               /* Return segment number in R1 */


/* _msgbuf_checksum
 *      Internal routine to calculate checksum character buffer.
 *      On entry:
 *          r0: pointer to character buffer to checksum (assume ASCIIZ terminated)
 *      On exit:
 *          r0: pointer to character buffer after ASCIIZ
 *          r1: checksum (8-bit binary)
 *          r2: message length
 *          r3: destroyed
 */
_msgbuf_checksum:
    mov     r1, #0                              /* clear checksum */
    mov     r2, #0                              /* clear length */
1:  ldrb    r3, [r0], #1                        /* Iterate through buffer */
    add     r1, r1, r3                          /* cummulative sum of char */
    teq     r3, #0
    addne   r2, r2, #1                          /* increment message length */
    bne     1b                                  /* until ASCIIZ found */
    and     r1, #BYTE0                          /* Modulo 256 */
    mov     pc, lr

    .global dbg__getDebugMsg
/* dbg__getDebugMsg
 *      Retrieve pending Debugger Message if available (Non-Blocking).
 *      On entry:
 *          No parameters (assume pointers were initialized previously using dbg__comm_init)
 *      On exit:
 *          r0: >0 = Valid GDB Message Length (incl '$', excluding '#' and checksum),
 *              0 = no valid message (yet), -1 = error
 *          r1: GDB Message Buffer Pointer  (incl '$', excluding '#' and checksum)
 *          r2, r3: Destroyed
 *      Note: If GDB Message were returned, it is ASCIIZ terminated, does not include '#' and checksum
 */
dbg__getDebugMsg:
    stmfd   sp!, {r4,r5,lr}
    bl      _dbg__usbHasMsg                     /* r0: message length, r1: segment number */
    teq     r0, #0
    beq     exit_dbg__getDebugMsg               /* no new message, exit with R0 = 0 */

    ldr     r4, =debug_segmentRxNum
    ldr     r2, [r4]                            /* Get current Segment Number */
    add     r2, r2, #1                          /* Expected Segment Number for comparison */
    teq     r1, #0
    streq   r1, [r4]                            /* Update current Segment Number with 0 since it is the last segment */
    beq     _hasMsg2Copy
    cmp     r1, #MSG_NUMSEGMENTS                /* Segment Number < MSG_NUMSEGMENTS? */
    bhs     _invalid_segment
    teq     r1, r2                              /* Valid Segment Number, check against Expected Segment Number */
    beq     _hasMsg2Copy                        /* Segment Number matches Expected Segment Number, update buffers */

_invalid_segment:
    bl      _dbg__usb_readbuf_reset             /* Invalid, Next USB telegram transaction */
    mov     r0, #0                              /* Reset Segment Number */
    str     r0, [r4]                            /* Update current Segment Number with 0 to prepare for new message */
    b       exit_dbg__getMsgError               /* Exit with error */

_hasMsg2Copy:
    str     r1, [r4]                            /* Update current Segment Number */
    bl      _copy_msg_from_usbbuf               /* r0: cummulative message length, r1: segment number */
    teq     r1, #0
    movne   r0, #0                              /* Incomplete message, ignore for now */
    bne     exit_dbg__getDebugMsg               /* Message not complete yet, exit */

    /* Check for valid GDB message */
    mov     r4, r0                              /* keep message length in R4, assume to be within MSGBUF_SIZE */
    ldr     r5, =debug_msgRxBufPtr
    ldr     r5, [r5]                            /* Rx buffer Start Address */

#ifdef CHECK_GDBSTARTCHAR
    /* Checked in dbg__bkpt_waitCMD */
    ldrb    r0, [r5]
    teq     r0, #MSGBUF_STARTCHAR               /* Look for '$' */
    bne     exit_dbg__getMsgError               /* No start char '$', exit with error */
#endif

    sub     r2, r4, #MSGBUF_CHKSUMOFFSET        /* Look for '#': Message Length - 3 = '#' offset */
    ldrb    r0, [r5, r2]
    teq     r0, #MSGBUF_CHKSUMCHAR
    bne     exit_dbg__getMsgError               /* No checksum char '#', exit with error */

    mov     r1, #0
    strb    r1, [r5, r2]                        /* Zero out '#' char for checksum calc later */

    add     r0, r5, #1                          /* Checksum packet data (excl '$') */
    bl      _msgbuf_checksum                    /* R2: length (excl '$'), R1: calculated checksum, R0: pointer to checksum in receive buffer */
    mov     r3, r1                              /* Keep calculated checksum in R3 (R1 destroyed by ascii2byte) */
    bl      ascii2byte                          /* R0: received checksum, R1: address of next buffer location */
    teq     r0, r3                              /* Compare calculated checksum in R3 against received checksum in R0 */
    bne     exit_dbg__getMsgError               /* Checksums do not match, exit with error */

    subeq   r0, r4, #MSGBUF_CHKSUMOFFSET        /* Update message length (incl '$') as return parameter */
    add     r2, r2, #1                          /* expected message length (from _msgbuf_checksum) */
    teq     r0, r2
    beq     exit_dbg__getDebugMsg               /* Valid length, return */

exit_dbg__getMsgError:
    mov     r0, #MSGBUF_MSGERROR
exit_dbg__getDebugMsg:
    mov     r1, r5                              /* Return GDB Message Buffer Pointer in R1 */
	ldmfd	sp!, {r4,r5,pc}

/* _dbg__sendUSBMsg
 *      Internal USB send routine (interfaces with drivers).
 *      On entry:
 *          R0: Total USB Message Buffer length
 *      On exit:
            R0: Tx Status (TRUE if data sent)
 */
_dbg__sendUSBMsg:
    stmfd   sp!, {r4, lr}
    mov     r4, r0                              /* Keep USB Buffer length in R4 */
#ifdef __NXOS__
    /* Check USB bus status, transmit message if possible */
    bl      nx_usb_is_connected                 /* R0 = True if USB is ready */
    cmp     r0, #TRUE
    bne     _exit_dbg__sendUSBMsg

    /* Actual transmission (blocking) */
    ldr     r0, =debug_OutUSBBuf                /* data pointer parameter */
    mov     r1, r4                              /* USB buffer length */
    bl      nx_usb_write

1:  bl      nx_usb_data_written                 /* R0 = True if data has been sent */
    cmp     r0, #TRUE
    /* FIXME: implement timeout */
    bne     1b                                  /* Busy Wait Loop */
#else
    /* Check USB bus status, transmit message if possible */
    bl      dUsbIsConfigured                    /* R0: UByte status, TRUE / FALSE */
    teq     r0, #nxt_UBYTE_TRUE
    movne   r0, #FALSE
    bne     _exit_dbg__sendUSBMsg

    /* Actual transmission (blocking) */
    ldr     r0, =debug_OutUSBBuf                /* data pointer parameter */
    mov     r1, r4                              /* USB buffer length */
    bl      dUsbWrite                           /* call NXT Firmware USB driver, return 0: done, !0: remaining chars */
    teq     r0, #0                              /* Tx done if returned length is 0 */
    movne   r0, #FALSE
    moveq   r0, #TRUE                           /* Convert NXT firmware return value to our Status (TRUE/FALSE) */
#endif

_exit_dbg__sendUSBMsg:
    ldmfd   sp!, {r4, pc}


/* _copy_msg_to_usbbuf
 *      Internal USB buffer copy routine, handles segment fragmentation.
 *      On entry:
 *          r0: number of bytes to copy
 *          r1: segment number
 *      On exit:
 *          r0: cummulative message length
 *          r1: segment number
 *          r2, r3: Destroyed
 */
_copy_msg_to_usbbuf:
    stmfd   sp!, {r1,r4,r5,r6,lr}
    ldr     r6, =debug_msgTxBufPtr              /* Address of Pointers */
    ldr     r5, [r6, #TXAPPENDPTR_OFFSET]       /* Retrieve Tx Append Pointer */

    movs    r4, r0
    beq     _exit_copy_msg_to_usbbuf

#ifdef CHECK_TXLEN
    add     r0, r4, #USB_GDBMSG_START   /* offset = header size */
    cmp     r0, #USB_BUFSIZE
    bhi     _exit_copy_msg_to_usbbuf    /* We let calling routine detect problem (segment number will increment) */
#endif

    /* Fill in USB Message Header */
    ldr     r3, =debug_OutUSBBuf
    mov     r2, #USB_NXT_TELEGRAMTYPE
    strb    r2, [r3], #1            /* Telegram type */
    strb    r1, [r3], #1            /* Segment Number */
    strb    r0, [r3], #1            /* Message Length */

    mov     r2, r5                              /* Copy to R2 for updating */
    mov     r1, r4                              /* actual GDB message fragment length (exclude USB header) */
    _dbg_memcpy r3, r2, r1, r0                  /* This copies over the message fragment, r3, r2 updated */
    mov     r5, r2                              /* Updated Tx Append Pointer, keep in R5 for now */

    add     r0, r4, #USB_GDBMSG_START           /* Total USB Buffer Size for Tx (USB_GDBMSG_START offset = header size) */
    bl      _dbg__sendUSBMsg                    /* Common interface routine to USB drivers */
    cmp     r0, #TRUE
    ldrne   r5, [r6, #TXAPPENDPTR_OFFSET]       /* Tx failed, Retrieve Original Tx Append Pointer */
    streq   r5, [r6, #TXAPPENDPTR_OFFSET]       /* Tx succeeded, Update Tx Append Pointer to new position */

_exit_copy_msg_to_usbbuf:
    ldr     r6, [r6]                            /* Retrieve Tx Buffer Start Address */
    sub     r0, r5, r6                          /* Return calculated cummulative message length (R0) */
    ldmfd   sp!, {r1,r4,r5,r6,pc}               /* Return segment number in R1 */

	.global dbg__putDebugMsg
/* dbg__putDebugMsg
 *		Sends Debugger Message from calling routine after appending checksum (Blocking) .
 *		On entry:
 *			No parameters (assume pointers were initialized previously using dbg__comm_init)
 *		On exit:
 *			r0: status (0: success, -1: error)
 *      Note: GDB Message to be sent must be ASCIIZ terminated, does not include '#' and checksum
 *            Response packets start with '+'/'-' followed by '$' (2 bytes prefix)
 */
dbg__putDebugMsg:
	stmfd	sp!, {r4,r5,lr}
	/* Perform Checksum Calculation */
    ldr     r5, =debug_msgTxBufPtr              /* R5: data structure base pointer */
    ldr     r4, [r5]                            /* Tx buffer Start Address */
    str     r4, [r5, #TXAPPENDPTR_OFFSET]       /* Reset Tx buffer Append Pointer */
    add     r0, r4, #2                          /* skip '+'/'-' and '$' */
    bl      _msgbuf_checksum                    /* R2: length (excl '+'/'-' and '$'), R1: calculated checksum, R0: pointer to checksum in tx buffer */

#ifdef CHECK_TXLEN
    add     r2, r2, #2                          /* r2: returned length from _msgbuf_checksum, added with prefix length */
    sub     r3, r0, r4                          /* r3: calculated length from pointers (incl. prefix length) */
    teq     r2, r3
    bne     exit_dbg__putMsgError
#endif

    mov     r3, #MSGBUF_CHKSUMCHAR
    strb    r3, [r0, #-1]                       /* Insert '#' */
    bl      byte2ascii                          /* On return, R0 points to location after checksum bytes, R1 is original pointer to checksum */
    sub     r4, r0, r4                          /* R4 = Calculated total message length (incl '+'/'-' and '$', '#' and checksum bytes */
    cmp     r4, #MSG_SEGMENTSIZE                /* If total message length > MSG_SEGMENTSIZE */
    mov     r1, #0                              /* Initialize Segment Number = 0 (last segment) first */
    mov     r0, #0                              /* Initial cummulative message length */
    mov     r5, #0                              /* Previous cummulative message length */

    /* We assume unsigned message lengths, so the arithmetic MUST NOT result in negative values */
_cont_putMsg:
    cmp     r4, r0
    movls   r0, #0                              /* R0: Exit status (success) */
    bls     exit_dbg__putDebugMsg               /* If Total message length (r4) <= Cummulative message length (r0), we're done */
    add     r2, r0, #MSG_SEGMENTSIZE            /* R2: calculate new Max cummulative message length */
    cmp     r4, r2                              /* Check total message length (R4) against new Max cummulative message length (R2) */
    subls   r0, r4, r0                          /* if total message length (R4) <= new Max cummulative message length (R2), send remainder */
    movls   r1, #0                              /* Flag as last segment (Segment Number = 0) */
    movhi   r0, #MSG_SEGMENTSIZE                /* else send MSG_SEGMENTSIZE bytes */
    addhi   r1, r1, #1                          /* Increment Segment Number */
    cmp     r1, #MSG_NUMSEGMENTS
    bhs     exit_dbg__putMsgError               /* If Segment Number >= MSG_NUMSEGMENTS, flag error */
    bl      _copy_msg_to_usbbuf                 /* R0: cummulative message length, R1: segment number */
    teq     r5, r0                              /* Check if we managed to transmit the previous message */
    beq     exit_dbg__putMsgError               /* No, flag error */
    movne   r5, r0                              /* Update previous cummulative message length */
    b       _cont_putMsg

exit_dbg__putMsgError:
    mov     r0, #MSGBUF_MSGERROR
exit_dbg__putDebugMsg:
	ldmfd	sp!, {r4,r5,pc}