pico-mdebug

mdebug firmware for pico / rp2040
git clone http://frotz.net/git/pico-mdebug.git
Log | Files | Refs | README

swd-pio.c (3187B)


      1 // Copyright 2021, Brian Swetland <swetland@frotz.net>
      2 // Licensed under the Apache License, Version 2.0.
      3 
      4 #include <string.h>
      5 #include <stdlib.h>
      6 #include <stdio.h>
      7 
      8 #include "swd.h"
      9 #include "rswdp.h"
     10 
     11 #include "swd-pio.h"
     12 
     13 /* returns 1 if the number of bits set in n is odd */
     14 static unsigned parity(unsigned n) {
     15 	n = (n & 0x55555555) + ((n & 0xaaaaaaaa) >> 1);
     16 	n = (n & 0x33333333) + ((n & 0xcccccccc) >> 2);
     17 	n = (n & 0x0f0f0f0f) + ((n & 0xf0f0f0f0) >> 4);
     18 	n = (n & 0x00ff00ff) + ((n & 0xff00ff00) >> 8);
     19 	n = (n & 0x0000ffff) + ((n & 0xffff0000) >> 16);
     20 	return n & 1;
     21 }
     22 
     23 void swd_init(void) {
     24 	swdio_init();
     25 }
     26 
     27 int swd_write(uint32_t hdr, uint32_t data) {
     28 	unsigned timeout = 64;
     29 	unsigned n;
     30 	unsigned p = parity(data);
     31 	
     32 	if (hdr == WR_BUFFER) {
     33 		// writing to TARGETSEL is done blind
     34 		swdio_write_mode();
     35 		swdio_write_bits(hdr << 8, 16);
     36 
     37 		swdio_read_mode();
     38 		swdio_read_bits(5);
     39 
     40 		swdio_write_mode();
     41 		swdio_write_bits(data, 32);
     42 		swdio_write_bits(p, 1);
     43 		return 0;
     44 	}
     45 
     46 	for (;;) {
     47 		swdio_write_mode();
     48 		swdio_write_bits(hdr << 8, 16);
     49 
     50 		swdio_read_mode();
     51 		n = swdio_read_bits(5);
     52 
     53 		swdio_write_mode();
     54 
     55 		// check the ack bits, ignoring the surrounding turnaround bits
     56 		switch ((n >> 1) & 7) {
     57 		case 1: // OK
     58 			swdio_write_bits(data, 32);
     59 			swdio_write_bits(p, 1);
     60 			return 0;
     61 		case 2: // WAIT
     62 			if (--timeout == 0) {
     63 				return ERR_TIMEOUT;
     64 			}
     65 			continue;
     66 		default: // ERROR
     67 			return ERR_IO;
     68 		}
     69 	}
     70 }
     71 
     72 int swd_read(uint32_t hdr, uint32_t *val) {
     73 	unsigned timeout = 64;
     74 	unsigned n, data, p;
     75 
     76 	for (;;) {
     77 		swdio_write_mode();
     78 		swdio_write_bits(hdr << 8, 16);
     79 
     80 		swdio_read_mode();
     81 		n = swdio_read_bits(4);
     82 
     83 		switch (n >> 1) {
     84 		case 1: // OK
     85 			data = swdio_read_bits(32);
     86 			// read party + turnaround
     87 			p = swdio_read_bits(2) & 1;
     88 			if (p != parity(data)) {
     89 				return ERR_PARITY;
     90 			}
     91 			*val = data;
     92 			return 0;
     93 		case 2: // WAIT
     94 			swdio_read_bits(1);
     95 			if (--timeout == 0) {
     96 				return ERR_TIMEOUT;
     97 			}
     98 			continue;
     99 		default: // ERROR
    100 			swdio_read_bits(1);
    101 			return ERR_IO;
    102 		}
    103 	}
    104 }
    105 
    106 void swd_reset(uint32_t kind) {
    107 	swdio_write_mode();
    108 
    109 	switch (kind) {
    110 	case ATTACH_SWD_RESET:
    111 		// 50+ HI (60 here), 2+ LO (4 here)
    112 		swdio_write_bits(0xffffffff, 32);
    113 		swdio_write_bits(0x0fffffff, 32);
    114 		break;
    115 	case ATTACH_JTAG_TO_SWD:
    116 		swdio_write_bits(0xffffffff, 32);
    117 		swdio_write_bits(0xffffffff, 32);
    118 		swdio_write_bits(0b1110011110011110, 16);
    119 		break;
    120 	case ATTACH_DORMANT_TO_SWD:
    121 		swdio_write_bits(0xffff, 16);
    122 		swdio_write_bits(0x6209F392, 32);
    123 		swdio_write_bits(0x86852D95, 32);
    124 		swdio_write_bits(0xE3DDAFE9, 32);
    125 		swdio_write_bits(0x19BC0EA2, 32);
    126 		swdio_write_bits(0xF1A0, 16);
    127 		break;
    128 	case ATTACH_SWD_TO_DORMANT:
    129 		swdio_write_bits(0xffffffff, 32);
    130 		swdio_write_bits(0xffffffff, 32);
    131 		swdio_write_bits(0xE3BC, 16);
    132 		break;
    133 	default:
    134 	       break;
    135 	}	     
    136 }
    137 
    138 unsigned swd_set_clock(unsigned khz) {
    139 	return swdio_set_freq(khz);
    140 }
    141 
    142 unsigned swo_set_clock(unsigned khz) {
    143 	return khz;
    144 }
    145 
    146 void swd_hw_reset(int assert) {
    147 #if 0
    148     if (assert) {
    149         gpio_set(GPIO_RESET, 0);
    150         gpio_set(GPIO_RESET_TXEN, 1);
    151     } else {
    152         gpio_set(GPIO_RESET, 1);
    153         gpio_set(GPIO_RESET_TXEN, 0);
    154     }
    155 #endif
    156 }