1 #include <u.h>
2 #include <libc.h>
3
4 typedef struct Tap Tap;
5 typedef struct Dap Dap;
6
7 struct Tap
8 {
9 int off;
10 int len;
11 int delay;
12
13 u32int id;
14 u32int dapsel;
15 };
16
17 struct Dap
18 {
19 Tap *tap;
20
21 uint port;
22 u32int id;
23 };
24
25 int dfd = -1;
26 int lastbit = -1;
27
28 int irlen;
29
30 int ntaps;
31 Tap* taps;
32
33 int ndaps;
34 Dap* daps;
35
36 Dap* ahbap;
37 Dap* apbap;
38
39 /* MPSSE command bits */
40 enum {
41 FEW = 1<<0, /* -ve CLK on write */
42 BITS = 1<<1, /* bits or bytes */
43 FER = 1<<2, /* -ve CLK on read */
44 LSB = 1<<3, /* LSB first = 1 else MSB first */
45 TDI = 1<<4, /* do write TDI */
46 TDO = 1<<5, /* do read TDO */
47 TMS = 1<<6, /* do write TMS */
48 };
49
50 void
ioinit(char * dev)51 ioinit(char *dev)
52 {
53 uchar b[3];
54
55 dfd = open(dev, ORDWR);
56 if(dfd < 0)
57 sysfatal("open: %r");
58
59 b[0] = 0x80;
60 b[1] = 0x08;
61 b[2] = 0x0B;
62 write(dfd, b, 3);
63 }
64
65 void
io(int cmd,int len,uchar * dat)66 io(int cmd, int len, uchar *dat)
67 {
68 uchar buf[64];
69 uchar *p = buf;
70
71 *p++ = cmd;
72 *p++ = len-1;
73 if((cmd & BITS) != 0)
74 len = 1;
75 else
76 *p++ = (len-1)>>8;
77 if((cmd & (TDI|TMS)) != 0){
78 memmove(p, dat, len);
79 p += len;
80 }
81 if(write(dfd, buf, p - buf) != (p - buf))
82 sysfatal("io write: %r");
83 if((cmd & TDO) != 0)
84 if(readn(dfd, dat, len) != len)
85 sysfatal("io read: %r");
86 }
87
88 void
dstate(u32int s,int len)89 dstate(u32int s, int len)
90 {
91 uchar b[1];
92
93 assert(len < 8);
94 b[0] = s;
95 if(lastbit != -1){
96 b[0] |= lastbit << 7;
97 lastbit = -1;
98 }
99 io(TMS|LSB|BITS|FEW, len, b);
100 }
101 uvlong
dshift(uvlong w,int len)102 dshift(uvlong w, int len)
103 {
104 uchar b[8];
105 int c, s, n;
106
107 c = TDI|LSB|FEW;
108 if(len < 0){
109 len = -len;
110 c |= TDO;
111 }
112 s = 0;
113 n = len/8;
114 if(n > 0) {
115 switch(n){
116 case 8: b[7] = w >> 56;
117 case 7: b[6] = w >> 48;
118 case 6: b[5] = w >> 40;
119 case 5: b[4] = w >> 32;
120 case 4: b[3] = w >> 24;
121 case 3: b[2] = w >> 16;
122 case 2: b[1] = w >> 8;
123 case 1: b[0] = w >> 0;
124 }
125 io(c, n, b);
126 s = n*8;
127 if((c & TDO) != 0){
128 w &= ~((1ULL<<s)-1);
129 switch(n){
130 case 8: w |= (uvlong)b[7] << 56;
131 case 7: w |= (uvlong)b[6] << 48;
132 case 6: w |= (uvlong)b[5] << 40;
133 case 5: w |= (uvlong)b[4] << 32;
134 case 4: w |= (uvlong)b[3] << 24;
135 case 3: w |= (uvlong)b[2] << 16;
136 case 2: w |= (uvlong)b[1] << 8;
137 case 1: w |= (uvlong)b[0] << 0;
138 }
139 }
140 len -= s;
141 }
142 if(len > 0){
143 b[0] = w >> s;
144 c |= BITS;
145 io(c, len, b);
146 if((c & TDO) != 0){
147 w &= ~((uvlong)((1<<len)-1) << s);
148 w |= (uvlong)(b[0] >> 8-len) << s;
149 }
150 s += len;
151 }
152 return w & (1ULL<<s)-1;
153 }
154 void
dshiftones(int len)155 dshiftones(int len)
156 {
157 while(len >= 64){
158 dshift(~0ULL, 64);
159 len -= 64;
160 }
161 dshift(~0ULL, len);
162 }
163 int
dshiftdelay(void)164 dshiftdelay(void)
165 {
166 int i;
167
168 /* send ones */
169 dshiftones(512);
170 for(i=0; i<512; i++){
171 if(dshift(i != 0, -1) == 0)
172 return i;
173 }
174 return 0;
175 }
176
177 void
irw(Tap * tap,uvlong w)178 irw(Tap *tap, uvlong w)
179 {
180 /* 0011 -> Shift-IR */
181 dstate(0x3, 4);
182
183 dshiftones(tap->off);
184 if((tap->off + tap->len) == irlen){
185 dshift(w, tap->len-1);
186 lastbit = w >> (tap->len-1);
187 } else {
188 dshift(w, tap->len);
189 dshiftones(irlen - (tap->off + tap->len-1));
190 lastbit = 1;
191 }
192
193 /* 011 -> Idle */
194 dstate(0x3, 3);
195 }
196 uvlong
drr(Tap * tap,int len)197 drr(Tap *tap, int len)
198 {
199 uvlong w, d;
200
201 /* 001 -> Shift-DR */
202 dstate(0x1, 3);
203
204 d = dshift(0, -tap->delay);
205 w = dshift(0, -len);
206 dshift(d, tap->delay);
207 dshift(w, len-1);
208 lastbit = (w >> len-1) & 1;
209
210 /* 011 -> Idle */
211 dstate(0x3, 3);
212
213 return w;
214 }
215 void
drw(Tap * tap,uvlong w,int len)216 drw(Tap *tap, uvlong w, int len)
217 {
218 /* 001 -> Shift-DR */
219 dstate(0x1, 3);
220
221 dshift(0, tap->delay);
222 dshift(w, len-1);
223 lastbit = (w >> len-1) & 1;
224
225 /* 011 -> Idle */
226 dstate(0x3, 3);
227 }
228
229 enum {
230 ABORT = 0x8,
231 DPACC = 0xA,
232 APACC = 0xB,
233 CTRLSTAT = 0x4,
234 SELECT = 0x8,
235 RDBUF = 0xC,
236 };
237
238 u32int
dapr(Dap * dap,uchar r,uchar a)239 dapr(Dap *dap, uchar r, uchar a)
240 {
241 uvlong w;
242
243 irw(dap->tap, r);
244 w = 1 | (a >> 1) & 0x6;
245 drw(dap->tap, w, 35);
246 do {
247 w = drr(dap->tap, 35);
248 } while((w & 7) == 1);
249 return w >> 3;
250 }
251 void
dapw(Dap * dap,uchar r,uchar a,u32int v)252 dapw(Dap *dap, uchar r, uchar a, u32int v)
253 {
254 uvlong w;
255
256 irw(dap->tap, r);
257 w = (a >> 1) & 0x6;
258 w |= (uvlong)v << 3;
259 drw(dap->tap, w, 35);
260 }
261
262 void
app(Dap * dap)263 app(Dap *dap)
264 {
265 enum {
266 CSYSPWRUPACK = 1<<31,
267 CSYSPWRUPREQ = 1<<30,
268 CDBGPWRUPACK = 1<<29,
269 CDBGPWRUPREQ = 1<<28,
270 CDBGRSTACK = 1<<27,
271 CDBGRSTREQ = 1<<26,
272 };
273 u32int s;
274
275 for(;;){
276 s = dapr(dap, DPACC, CTRLSTAT);
277 if((s & (CDBGPWRUPACK|CSYSPWRUPACK)) == (CDBGPWRUPACK|CSYSPWRUPACK))
278 break;
279 s |= CSYSPWRUPREQ|CDBGPWRUPREQ;
280 dapw(dap, DPACC, CTRLSTAT, s);
281 }
282 }
283 void
apa(Dap * dap,uchar a)284 apa(Dap *dap, uchar a)
285 {
286 u32int s;
287
288 s = dap->port<<24 | a&0xf0;
289 if(s != dap->tap->dapsel){
290 dap->tap->dapsel = s;
291 dapw(dap, DPACC, SELECT, s);
292 app(dap);
293 }
294 }
295 u32int
apr(Dap * dap,uchar a)296 apr(Dap *dap, uchar a)
297 {
298 apa(dap, a);
299 return dapr(dap, APACC, a&0xC);
300 }
301 void
apw(Dap * dap,uchar a,u32int v)302 apw(Dap *dap, uchar a, u32int v)
303 {
304 apa(dap, a);
305 dapw(dap, APACC, a&0xC, v);
306 }
307 u32int
mmr(Dap * ap,u32int addr)308 mmr(Dap *ap, u32int addr)
309 {
310 apw(ap, 0x4, addr);
311 return apr(ap, 0xC);
312 }
313 void
mmw(Dap * ap,u32int addr,u32int val)314 mmw(Dap *ap, u32int addr, u32int val)
315 {
316 apw(ap, 0x4, addr);
317 apw(ap, 0xC, val);
318 }
319
320 void
tapreset(void)321 tapreset(void)
322 {
323 int i, j, o;
324
325 dstate(0x1F, 6); /* 011111 -> Reset->Idle */
326 dstate(0x3, 4); /* 0011 -> Shift-IR */
327
328 irlen = dshiftdelay();
329 lastbit = 1;
330
331 dstate(0x7, 5); /* 00111 -> Shift-IR->Shift-DR */
332
333 ntaps = dshiftdelay();
334
335 dstate(0x1F, 6); /* 011111 -> Reset->Idle */
336 dstate(0x1, 3); /* 001 -> Shift-DR */
337
338 taps = realloc(taps, sizeof(taps[0]) * ntaps);
339
340 o = 0;
341 for(i=ntaps-1; i>=0; i--){
342 taps[i].delay = ntaps - i - 1;
343 taps[i].off = o;
344 taps[i].id = dshift(0, -32);
345 switch(taps[i].id){
346 default:
347 sysfatal("unknown tapid %.8ux\n", taps[i].id);
348 case 0x03727093:
349 case 0x0373b093:
350 case 0x23727093:
351 taps[i].len = 6;
352 break;
353 case 0x4ba00477:
354 taps[i].len = 4;
355 break;
356 }
357 o += taps[i].len;
358 }
359
360 dstate(0x1F, 6); /* 011111 -> Reset->Idle */
361
362 if(o != irlen)
363 sysfatal("wrong tapchain irlen %d %d\n", o, irlen);
364
365 ndaps = 0;
366 for(i=0; i<ntaps; i++){
367 fprint(2, "tap%d: id=%.8ux off=%d len=%d delay=%d\n",
368 i, taps[i].id, taps[i].off, taps[i].len, taps[i].delay);
369
370 switch(taps[i].id){
371 case 0x4ba00477:
372 o = 3;
373 daps = realloc(daps, sizeof(daps[0]) * (ndaps+o));
374 for(j=0; j<o; j++){
375 daps[ndaps].tap = taps+i;
376 daps[ndaps].port = j;
377 daps[ndaps].id = apr(daps+ndaps, 0xFC);
378 fprint(2, "\tdap%d: id=%.8ux\n", j, daps[ndaps].id);
379
380 ndaps++;
381 }
382 break;
383 }
384 }
385
386 for(i=0; i<ndaps; i++){
387 switch(daps[i].id){
388 case 0x44770001:
389 ahbap = daps+i;
390 break;
391 case 0x24770002:
392 apbap = daps+i;
393 break;
394 }
395 }
396 }
397
398 enum {
399 DBGDIDR = 0x000,
400 DBGDEVID = 0xFC8,
401 DBGDSCR = 0x088,
402 RXfull = 1<<30,
403 TXfull = 1<<29,
404 RXfull_1 = 1<<27,
405 TXfull_1 = 1<<26,
406 PipeAdv = 1<<25,
407 InstrCompl_1 = 1<<24,
408 ExtDCCmodeShift = 20,
409 ExtDCCmodeMask = 3<<ExtDCCmodeShift,
410 ADAdiscard = 1<<19,
411 NS = 1<<18,
412 SPNIDdis = 1<<17,
413 SPIDdis = 1<<16,
414 MDBGen = 1<<15,
415 HDBGen = 1<<14,
416 ITRen = 1<<13,
417 UDCCdis = 1<<12,
418 INTdis = 1<<11,
419 DBGack = 1<<10,
420 UND_1 = 1<<8,
421 ADABORT_1 = 1<<7,
422 SDABORT_1 = 1<<6,
423 MOEShift = 2,
424 MOEMask = 15<<MOEShift,
425 RESTARTED = 1<<1,
426 HALTED = 1<<0,
427
428 DBGDRCR = 0x90,
429 RestartReq = 1<<1,
430 HaltReq = 1<<0,
431
432 DBGPRCR = 0x310,
433
434 DBGITR = 0x084, /* Instruction Transfer Register */
435 DBGDTRRX = 0x080, /* Host to Target Data Transfer Register */
436 DBGDTRTX = 0x08C, /* Target to Host Data Transfer Register */
437 };
438
439 typedef struct Arm Arm;
440 struct Arm
441 {
442 u32int dbgbase;
443
444 Dap *dbgap;
445 Dap *memap;
446
447 char *id;
448 };
449 Arm arm[2];
450 u32int
dbgr(Arm * arm,u32int reg)451 dbgr(Arm *arm, u32int reg)
452 {
453 return mmr(arm->dbgap, arm->dbgbase+reg);
454 }
455 void
dbgw(Arm * arm,u32int reg,u32int val)456 dbgw(Arm *arm, u32int reg, u32int val)
457 {
458 mmw(arm->dbgap, arm->dbgbase+reg, val);
459 }
460 u32int
dbgrpoll(Arm * arm,u32int reg,u32int mask,u32int val)461 dbgrpoll(Arm *arm, u32int reg, u32int mask, u32int val)
462 {
463 u32int w;
464
465 for(;;){
466 w = dbgr(arm, reg);
467 if((w & mask) == val)
468 break;
469 }
470 return w;
471 }
472
473 void
startstop(Arm * arm,int stop)474 startstop(Arm *arm, int stop)
475 {
476 u32int s;
477
478 s = dbgr(arm, DBGDSCR);
479 if((s & HALTED) != stop){
480 if(!stop){
481 s &= ~ITRen;
482 dbgw(arm, DBGDSCR, s);
483 }
484 dbgw(arm, DBGDRCR, stop ? HaltReq : RestartReq);
485 s = dbgrpoll(arm, DBGDSCR, HALTED, stop);
486 if(stop){
487 s |= ITRen;
488 dbgw(arm, DBGDSCR, s);
489 }
490 fprint(2, "%s: startstop: %.8ux\n", arm->id, s);
491 }
492 }
493
494 void
armxec(Arm * arm,u32int instr)495 armxec(Arm *arm, u32int instr)
496 {
497 dbgw(arm, DBGITR, instr);
498 dbgrpoll(arm, DBGDSCR, InstrCompl_1, InstrCompl_1);
499 }
500
501 #define ARMV4_5_MRC(CP, op1, Rd, CRn, CRm, op2) \
502 (0xee100010 | (CRm) | ((op2) << 5) | ((CP) << 8) \
503 | ((Rd) << 12) | ((CRn) << 16) | ((op1) << 21))
504 #define ARMV4_5_MCR(CP, op1, Rd, CRn, CRm, op2) \
505 (0xee000010 | (CRm) | ((op2) << 5) | ((CP) << 8) \
506 | ((Rd) << 12) | ((CRn) << 16) | ((op1) << 21))
507
508 void
trrxw(Arm * arm,u32int val)509 trrxw(Arm *arm, u32int val)
510 {
511 dbgrpoll(arm, DBGDSCR, RXfull_1, 0);
512 dbgw(arm, DBGDTRRX, val);
513 }
514 u32int
trtxr(Arm * arm)515 trtxr(Arm *arm)
516 {
517 dbgrpoll(arm, DBGDSCR, TXfull_1, TXfull_1);
518 return dbgr(arm, DBGDTRTX);
519 }
520
521 void
522 armrw(Arm *arm, int reg, u32int val);
523
524 u32int
armrr(Arm * arm,int rn)525 armrr(Arm *arm, int rn)
526 {
527 if(rn == 15){
528 u32int r0;
529
530 r0 = armrr(arm, 0);
531 armxec(arm, 0xE1A0000F);
532 armxec(arm, ARMV4_5_MCR(14, 0, 0, 0, 5, 0));
533 armrw(arm, 0, r0);
534 } else {
535 armxec(arm, ARMV4_5_MCR(14, 0, rn, 0, 5, 0));
536 }
537 return trtxr(arm);
538 }
539 void
armrw(Arm * arm,int rn,u32int val)540 armrw(Arm *arm, int rn, u32int val)
541 {
542 if(rn == 15){
543 u32int r0;
544
545 r0 = armrr(arm, 0);
546 armrw(arm, 0, val);
547 armxec(arm, 0xE1A0F000);
548 armrw(arm, 0, r0);
549 } else {
550 trrxw(arm, val);
551 armxec(arm, ARMV4_5_MRC(14, 0, rn, 0, 5, 0));
552 }
553 }
554
555 /*
556 * mww phys 0xf8000008 0xdf0d
557 * mww phys 0xf8000910 0xf
558 * load_image "/sys/src/boot/zynq/fsbl" 0xfffc0000 bin
559 * reg pc 0xfffc0000
560 */
561 void
boot(char * file,u32int entry)562 boot(char *file, u32int entry)
563 {
564 u32int *buf, *src;
565 int fd, size;
566 u32int dst;
567
568 fprint(2, "load %s", file);
569 if((fd = open(file, OREAD)) < 0)
570 sysfatal("open: %r");
571
572 size = seek(fd, 0, 2);
573 fprint(2, " [%ud]", size);
574 seek(fd, 0, 0);
575 buf = malloc((size+3) & ~3);
576 if(readn(fd, buf, size) != size)
577 sysfatal("read: %r");
578 close(fd);
579
580 /* map ocm */
581 mmw(arm->memap, 0xf8000008, 0xdf0d);
582 mmw(arm->memap, 0xf8000910, 0xf);
583
584 src = buf;
585 for(dst = entry; size > 0; dst += 4, size -= 4){
586 if((dst & 0xF) == 0)
587 fprint(2, ".");
588 mmw(arm->memap, dst, *src++);
589 }
590 free(buf);
591 fprint(2, ".\nentry %.8ux\n", entry);
592
593 armrw(arm, 15, entry);
594 }
595
596 void
usage(void)597 usage(void)
598 {
599 fprint(2, "%s [ -j jtagdev ] entry image\n", argv0);
600 exits("usage");
601 }
602
603 void
main(int argc,char * argv[])604 main(int argc, char *argv[])
605 {
606 char *jtag = "/dev/jtagddd94.0";
607 char *image;
608 u32int entry;
609
610 fmtinstall('H', encodefmt);
611
612 ARGBEGIN {
613 case 'j':
614 jtag = EARGF(usage());
615 break;
616 default:
617 usage();
618 } ARGEND;
619
620 if(argc != 2)
621 usage();
622 entry = strtoul(argv[0], nil, 0);
623 image = argv[1];
624
625 ioinit(jtag);
626 tapreset();
627
628 arm[0].dbgbase = 0x80090000;
629 arm[0].dbgap = apbap;
630 arm[0].memap = ahbap;
631 arm[0].id = "arm0";
632
633 arm[1].dbgbase = 0x80092000;
634 arm[1].dbgap = apbap;
635 arm[1].memap = ahbap;
636 arm[1].id = "arm1";
637
638 startstop(arm+0, 1);
639 startstop(arm+1, 1);
640
641 boot(image, entry);
642
643 startstop(arm+0, 0);
644 startstop(arm+1, 0);
645
646 exits(nil);
647 }
648