-
Notifications
You must be signed in to change notification settings - Fork 195
/
OpenOCD_asm.S
220 lines (178 loc) · 4.57 KB
/
OpenOCD_asm.S
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
;
; OpenOCD_asm.S
;
; Optimized function for the OpenOCD mode
;
; Written by Sylvain Munaut <tnt@246tNt.com>
;
;
; Published in the public domain.
; For details see: http://creativecommons.org/publicdomain/zero/1.0/.
;
; This program is distributed in the hope that it will be useful,
; but WITHOUT ANY WARRANTY; without even the implied warranty o
; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
;
.equ __24FJ64GA002, 1
.include "p24FJ64GA002.inc"
;
; Hardware configuration
;
; Do we support delay ?
#define HAS_DELAY 1
; Bus pirate v3 hardware
#define IOPOR PORTB
#define BP_MOSI_BIT RB9
#define BP_CLK_BIT RB8
#define BP_MISO_BIT RB7
#define BP_CS_BIT RB6
; Pin usage by OpenOCD
#define OOCD_TDO_BIT BP_MISO_BIT
#define OOCD_TMS_BIT BP_CS_BIT
#define OOCD_CLK_BIT BP_CLK_BIT
#define OOCD_TDI_BIT BP_MOSI_BIT
;
; void binOpenOCDTapShiftFast(void *in_buf, void *out_buf, unsigned int bits, unsigned int delay)
;
; Parameters:
; w0 : input buffer
; w1 : output buffer
; w2 : # of bits
; w3 : delay (depends on config)
;
; Register usage:
;
; w0 : #bytes processed in the inner loop (-1) / tmp register elsewhere
; w1 : bit loop counter in the inner loop / tmp register elsewhere
; w2 : TDI data out
; w3 : TMS data out
; w4 : TDO data in
;
; w5 : total bytes consumed
; w6 : #bits_left - 1
; w7 : src ptr
; w8 : dst ptr
;
; w9 : constant IOPR
; w10 : constant OOCD_TDI_BIT
; w11 : constant OOCD_TMS_BIT
; w12 : delay loop
;
.text
.global _binOpenOCDTapShiftFast
_binOpenOCDTapShiftFast:
; Save registers
push.d w8 ; save w8,w9
push.d w10 ; save w10,w11
#ifdef HAS_DELAY
push.w w12 ; save 12
#endif
; Init consumed byte counter
clr w5 ; w5 = 0
; Save parameters
dec.w w2, w6 ; w6 = w2 - 1;
mov.w w0, w7 ; w7 = w0;
mov.w w1, w8 ; w8 = w1;
#ifdef HAS_DELAY
mov.w w3, w12
#endif
; Constants
mov.w #IOPOR, w9 ; w9 = IOPOR;
mov.w #OOCD_TDI_BIT, w10 ; w10 = OOCD_TDI_BIT;
mov.w #OOCD_TMS_BIT, w11 ; w11 = OOCD_TMS_BIT;
; Outer (word) loop
__loop_word: ; do {
; Wait until there are enough bytes RXed
mov.w _UART1RXToRecv, w0 ; w0 = UART1RXToRecv;
add.w w5, #4, w5 ; w5 += 4;
cpsgt w5, w0 ; w0 = min(w0, w5)
mov.w w5, w0
1:
cp _UART1RXRecvd ; while (_UART1RXRecvd < w0);
bra lt, 1b
; Fetch and organize 2x16 bits
mov.w [w7++], w2 ; w2 = *((uint16_t*)w7++);
mov.w [w7++], w3 ; w3 = *((uint16_t*)w7++);
; If memory had bytes 0xAA 0xBB 0xCC 0xDD,
; we now have:
; w2 = 0xBBAA;
; w3 = 0xDDCC;
;
; and we want:
; w2 = 0xCCAA; /* TDI bits (lsb out first) */
; w3 = 0xDDBB; /* TMS bits (lsb out first) */
swap.w w2 ; w2 = swap_bytes(w2);
xor.w w2, w3, w4 ; w4 = w2 ^ w3;
and.w #0xff, w4 ; w4 &= 0xff;
xor.w w2, w4, w2 ; w2 = w2 ^ w4;
xor.w w3, w4, w3 ; w3 = w3 ^ w4;
swap.w w2 ; w2 = swap_bytes(w2);
; Compute how much bits to process
mov.w #15, w0 ; w0 = min(15, w6)
cpsgt.w w6, w0
mov.w w6, w0
; Inner (bit) loop
mov w0, w1 ; w1 = w0;
__loop_bit: ; do {
; Delay loop
#if HAS_DELAY
repeat w12
nop
#endif
; Clear TCK
bclr.w [w9], #OOCD_CLK_BIT
; Output TMS & TDI
rrc.w w2, w2
bsw.c [w9], w10
rrc.w w3, w3
bsw.c [w9], w11
; Delay loop
#if HAS_DELAY
repeat w12
nop
#else
nop ; /* still must pause a cycle because */
; /* we wrote [w9] last cycle ... */
#endif
; Set TCK
bset.w [w9], #OOCD_CLK_BIT
; Sample TDO
btst.c [w9], #OOCD_TDO_BIT
rrc.w w4, w4
; Inner (bit) loop branch condition
dec.w w1, w1 ; } while (--w1 >= 0);
bra c, __loop_bit
; Fix & Store the result
subr w0, #15, w1 ; w1 = 15 - w0;
lsr w4, w1, w4 ; w4 >>= w1;
mov.w w4, [w8++] ; *((uint16_t*)w8++) = w4;
; Increment available bytes
lsr.w w0, #3, w0 ; w0 = w0 >> 3;
inc.w w0, w0 ; w0 = w0 + 1;
add _UART1TXAvailable ; UART1TXAvailable += w0;
; Trigger TX if needed
; (we're in the loop, so use optimized version, also we can't
; loose register content -> no function call)
btst IEC0, #U1TXIE
bra z, 1f
mov _UART1TXSent, w0
cp _UART1TXAvailable
bra z, 1f
bclr IFS0, #U1TXIF
bset IEC0, #U1TXIE
add _UART1TXBuf, wreg
mov [w0], w0
mov w0, U1TXREG
1:
; Outer (word) loop branch condition
sub.w w6, #16, w6 ; w6 -= 16;
bra c, __loop_word ; } while (w6>=0);
; Restore registers
#ifdef HAS_DELAY
pop.w w12 ; restore w12
#endif
pop.d w10 ; restore w10,w11
pop.d w8 ; restore w8,w9
; If there was anything left
goto _UART1TXInt
; (we used a goto, no need for return :)