442 lines
14 KiB
C
442 lines
14 KiB
C
// vim: set et:
|
|
|
|
#include <string.h>
|
|
|
|
#include <tusb.h>
|
|
|
|
#include "info.h"
|
|
#include "thread.h"
|
|
|
|
#include "m_isp/bsp-feature.h"
|
|
#include "m_isp/mehfet.h"
|
|
|
|
struct cmdlen {
|
|
uint32_t len;
|
|
uint8_t cmd;
|
|
};
|
|
|
|
#define BUFSIZE 256
|
|
|
|
static uint8_t rx_buf[BUFSIZE];
|
|
static uint8_t tx_buf[BUFSIZE];
|
|
|
|
static uint32_t rxavail, rxpos, txpos;
|
|
|
|
// TODO: this is duplicated several times over the codebase, maybe reduce this
|
|
static uint8_t read_byte(void) {
|
|
while (rxavail <= 0) {
|
|
if (!tud_vendor_n_mounted(VND_N_MEHFET) || !tud_vendor_n_available(VND_N_MEHFET)) {
|
|
thread_yield();
|
|
continue;
|
|
}
|
|
|
|
rxpos = 0;
|
|
rxavail = tud_vendor_n_read(VND_N_MEHFET, rx_buf, sizeof rx_buf);
|
|
|
|
if (rxavail == 0) thread_yield();
|
|
}
|
|
|
|
uint8_t rv = rx_buf[rxpos];
|
|
++rxpos;
|
|
--rxavail;
|
|
|
|
return rv;
|
|
}
|
|
/*static void drop_incoming(void) {
|
|
rxavail = 0;
|
|
rxpos = 0;
|
|
|
|
// empty tinyusb internal buffer
|
|
if (tud_vendor_n_mounted(VND_N_MEHFET)) {
|
|
while (tud_vendor_n_available(VND_N_MEHFET)) {
|
|
tud_vendor_n_read(VND_N_MEHFET, rx_buf, sizeof rx_buf);
|
|
}
|
|
}
|
|
}*/
|
|
static void write_flush(void) {
|
|
// TODO: is this needed?
|
|
while (tud_vendor_n_write_available(VND_N_MEHFET) < txpos) {
|
|
thread_yield();
|
|
}
|
|
|
|
tud_vendor_n_write(VND_N_MEHFET, tx_buf, txpos);
|
|
txpos = 0;
|
|
}
|
|
static void write_byte(uint8_t v) {
|
|
if (txpos == sizeof tx_buf) {
|
|
write_flush();
|
|
}
|
|
|
|
tx_buf[txpos] = v;
|
|
++txpos;
|
|
}
|
|
|
|
////////////////
|
|
|
|
static uint32_t plpos;
|
|
static struct cmdlen read_cmd_len(void) {
|
|
uint8_t cmd = read_byte(),
|
|
lastbyte = cmd;
|
|
uint32_t l = 0;
|
|
|
|
//printf("cmd=%02x\n", cmd);
|
|
|
|
for (size_t i = 0; (i < 4) && (lastbyte & 0x80); ++i) {
|
|
lastbyte = read_byte();
|
|
//printf("lenbyte=%02x\n");
|
|
|
|
uint8_t mask = (i == 3) ? 0xff : 0x7f;
|
|
l |= (lastbyte & mask) << (i * 7);
|
|
}
|
|
|
|
//printf("len=0x%x\n");
|
|
plpos = 0;
|
|
return (struct cmdlen){ .len = l, .cmd = cmd & 0x7f };
|
|
}
|
|
|
|
static inline uint8_t read_pl(void) {
|
|
++plpos;
|
|
return read_byte();
|
|
}
|
|
static void flush_pl(uint32_t len) {
|
|
while (plpos < len) read_byte();
|
|
}
|
|
|
|
static void write_resp(enum mehfet_status stat, size_t resplen, const uint8_t* resp) {
|
|
//if (stat != mehfet_ok) drop_incoming();
|
|
|
|
write_byte((stat & 0x7f) | (resplen ? 0x80 : 0));
|
|
|
|
for (size_t i = 0, len2 = resplen; (i < 4) && len2; ++i) {
|
|
uint8_t nextv;
|
|
if (i == 3) {
|
|
nextv = (uint8_t)len2;
|
|
} else {
|
|
nextv = len2 & 0x7f;
|
|
if ((len2 >> 7) != 0) nextv |= 0x80;
|
|
}
|
|
len2 >>= 7;
|
|
|
|
write_byte(nextv);
|
|
}
|
|
|
|
for (size_t i = 0; i < resplen; ++i)
|
|
write_byte(resp[i]);
|
|
|
|
write_flush();
|
|
}
|
|
static void write_resp_str(enum mehfet_status stat, const char* str) {
|
|
write_resp(stat, strlen(str)+1 /* include null terminator */, (const uint8_t*)str);
|
|
}
|
|
|
|
///////////////
|
|
|
|
static uint8_t connstat;
|
|
|
|
void mehfet_init(void) {
|
|
rxavail = 0;
|
|
rxpos = 0;
|
|
txpos = 0;
|
|
plpos = 0;
|
|
|
|
connstat = mehfet_conn_none;
|
|
|
|
mehfet_hw_init();
|
|
}
|
|
void mehfet_deinit(void) {
|
|
if (connstat != mehfet_conn_none) {
|
|
mehfet_hw_disconnect();
|
|
connstat = mehfet_conn_none;
|
|
}
|
|
|
|
mehfet_hw_deinit();
|
|
}
|
|
|
|
void mehfet_task(void) {
|
|
struct cmdlen cmdhdr = read_cmd_len();
|
|
|
|
switch (cmdhdr.cmd) {
|
|
case mehfet_info:
|
|
//printf("in info cmd\n");
|
|
if (cmdhdr.len != 0) write_resp_str(mehfet_badargs, "Info takes no parameters");
|
|
else {
|
|
// TODO: add flag once Loop has been implemented
|
|
uint32_t caps = mehfet_hw_get_caps() /*| mehfet_cap_has_loop*/;
|
|
uint16_t ver = MEHFET_PROTO_VER;
|
|
uint8_t pktbuf_l2 = __builtin_ctz(sizeof tx_buf);
|
|
const char* name = INFO_PRODUCT(INFO_BOARDNAME);
|
|
|
|
size_t bufsize = strlen(name) + 1 + 8;
|
|
if (bufsize > 256) // uuuuuh, (stack size is 512b)
|
|
write_resp_str(mehfet_error, "not enough space for info buffer");
|
|
|
|
uint8_t buf[bufsize];
|
|
buf[0] = (caps >> 0) & 0xff;
|
|
buf[1] = (caps >> 8) & 0xff;
|
|
buf[2] = (caps >> 16) & 0xff;
|
|
buf[3] = (caps >> 24) & 0xff;
|
|
|
|
buf[4] = (ver >> 0) & 0xff;
|
|
buf[5] = (ver >> 8) & 0xff;
|
|
|
|
buf[6] = pktbuf_l2;
|
|
buf[7] = 0; // reserved
|
|
|
|
memcpy(&buf[8], name, bufsize - 8);
|
|
|
|
//printf("infocmd done\n");
|
|
write_resp(mehfet_ok, bufsize, buf);
|
|
}
|
|
break;
|
|
case mehfet_status:
|
|
if (cmdhdr.len != 0) write_resp_str(mehfet_badargs, "Status takes no parameters");
|
|
else write_resp(mehfet_ok, 1, &connstat);
|
|
break;
|
|
case mehfet_connect:
|
|
if (cmdhdr.len != 1) write_resp_str(mehfet_badargs, "Connect takes one parameter byte");
|
|
else if (connstat != mehfet_conn_none) {
|
|
write_resp_str(mehfet_badstate, "Already connected");
|
|
} else {
|
|
enum mehfet_conn conn = read_pl();
|
|
|
|
// check if we can actually do this
|
|
enum mehfet_conn conntyp = conn & mehfet_conn_typemask;
|
|
switch (conntyp) {
|
|
case mehfet_conn_jtag_noentry:
|
|
if (!(mehfet_hw_get_caps() & mehfet_cap_jtag_noentry)) {
|
|
write_resp(mehfet_nocaps, 0, NULL);
|
|
goto EXIT;
|
|
}
|
|
break;
|
|
case mehfet_conn_jtag_entryseq:
|
|
if (!(mehfet_hw_get_caps() & mehfet_cap_jtag_entryseq)) {
|
|
write_resp(mehfet_nocaps, 0, NULL);
|
|
goto EXIT;
|
|
}
|
|
break;
|
|
case mehfet_conn_sbw_entryseq:
|
|
if (!(mehfet_hw_get_caps() & mehfet_cap_sbw_entryseq)) {
|
|
write_resp(mehfet_nocaps, 0, NULL);
|
|
goto EXIT;
|
|
}
|
|
break;
|
|
case mehfet_conn_auto:
|
|
if (mehfet_hw_get_caps() & mehfet_cap_sbw_entryseq)
|
|
conntyp = mehfet_conn_sbw_entryseq;
|
|
else if (mehfet_hw_get_caps() & mehfet_cap_jtag_entryseq)
|
|
conntyp = mehfet_conn_jtag_entryseq;
|
|
else if (mehfet_hw_get_caps() & mehfet_cap_jtag_noentry)
|
|
conntyp = mehfet_conn_jtag_noentry;
|
|
else {
|
|
write_resp_str(mehfet_nocaps, "Connect: no mode implemented in hardware...");
|
|
goto EXIT;
|
|
}
|
|
break;
|
|
default:
|
|
write_resp_str(mehfet_badargs, "Connect: invalid connection mode");
|
|
goto EXIT;
|
|
}
|
|
|
|
const char* resp = mehfet_hw_connect(conntyp | (conn & mehfet_conn_nrstmask));
|
|
if (!resp) {
|
|
connstat = conntyp;
|
|
write_resp(mehfet_ok, 0, NULL);
|
|
} else
|
|
write_resp_str(mehfet_error, resp);
|
|
}
|
|
break;
|
|
case mehfet_disconnect:
|
|
if (cmdhdr.len != 0) write_resp_str(mehfet_badargs, "Disconnect takes no parameters");
|
|
else {
|
|
if (connstat != mehfet_conn_none) mehfet_hw_disconnect();
|
|
connstat = mehfet_conn_none;
|
|
|
|
write_resp(mehfet_ok, 0, NULL);
|
|
}
|
|
break;
|
|
case mehfet_delay:
|
|
if (cmdhdr.len != 4) write_resp_str(mehfet_badargs, "Delay takes 4 parameter bytes");
|
|
else {
|
|
uint32_t v = 0;
|
|
v |= (uint32_t)read_pl() << 0 ;
|
|
v |= (uint32_t)read_pl() << 8 ;
|
|
v |= (uint32_t)read_pl() << 16;
|
|
v |= (uint32_t)read_pl() << 24;
|
|
|
|
bool exact = v & (1u<<31), us = v & (1u<<30);
|
|
v &= (1u << 30) - 1;
|
|
|
|
if (exact) {
|
|
if (us) mehfet_hw_delay_us(v);
|
|
else mehfet_hw_delay_ms(v);
|
|
} else {
|
|
mehfet_hw_timer_start(us, v);
|
|
while (!mehfet_hw_timer_reached()) thread_yield();
|
|
}
|
|
|
|
write_resp(mehfet_ok, 0, NULL);
|
|
}
|
|
break;
|
|
case mehfet_set_clkspeed:
|
|
if (cmdhdr.len != 1) write_resp_str(mehfet_badargs, "SetClkSpeed takes one parameter byte");
|
|
else if (connstat == mehfet_conn_none) write_resp(mehfet_badstate, 0, NULL);
|
|
else {
|
|
mehfet_hw_set_clkspeed(read_pl() != 0);
|
|
write_resp(mehfet_ok, 0, NULL);
|
|
}
|
|
break;
|
|
case mehfet_get_old_lines:
|
|
if (cmdhdr.len != 0) write_resp_str(mehfet_badargs, "GetOldLines takes no parameters");
|
|
else if (connstat == mehfet_conn_none) write_resp(mehfet_badstate, 0, NULL);
|
|
else {
|
|
uint8_t v = mehfet_hw_get_old_lines();
|
|
write_resp(mehfet_ok, 1, &v);
|
|
}
|
|
break;
|
|
case mehfet_tdio_seq:
|
|
if (cmdhdr.len < 6) write_resp_str(mehfet_badargs,
|
|
"TdioSequence: need at least a TMS level, number of cycles and some TDI data (at least 6 bytes)");
|
|
// stack size is 512b
|
|
else if (cmdhdr.len > 128 + 5) write_resp_str(mehfet_badargs,
|
|
"TdioSequence: too much data to process, can do max. 1024 bits (128B) at once");
|
|
else if (connstat == mehfet_conn_none) write_resp(mehfet_badstate, 0, NULL);
|
|
else {
|
|
uint32_t ncyc = 0;
|
|
ncyc |= (uint32_t)read_pl() << 0 ;
|
|
ncyc |= (uint32_t)read_pl() << 8 ;
|
|
ncyc |= (uint32_t)read_pl() << 16;
|
|
ncyc |= (uint32_t)read_pl() << 24;
|
|
|
|
bool tmslvl = read_pl() != 0;
|
|
|
|
size_t nbytes = (ncyc + 7) >> 3;
|
|
if (nbytes != cmdhdr.len - 5) {
|
|
write_resp_str(mehfet_badargs, "TdioSequence: bad ncyc<->payload length");
|
|
} else {
|
|
uint8_t tdi_stuff[nbytes], tdo_stuff[nbytes];
|
|
|
|
for (size_t i = 0; i < nbytes; ++i) tdi_stuff[i] = read_pl();
|
|
|
|
mehfet_hw_tdio_seq(ncyc, tmslvl, tdi_stuff, tdo_stuff);
|
|
|
|
write_resp(mehfet_ok, nbytes, tdo_stuff);
|
|
}
|
|
}
|
|
break;
|
|
case mehfet_tms_seq:
|
|
if (cmdhdr.len < 6) write_resp_str(mehfet_badargs,
|
|
"TmsSequence: need a TDI level, the number of cycles and some TMS data (at least 6 bytes)");
|
|
// stack size is 512b
|
|
else if (cmdhdr.len > 256 + 5) write_resp_str(mehfet_badargs,
|
|
"TmsSequence: too much data to process, can do max. 2048 bits (256B) at once");
|
|
else if (connstat == mehfet_conn_none) write_resp(mehfet_badstate, 0, NULL);
|
|
else {
|
|
uint32_t ncyc = 0;
|
|
ncyc |= (uint32_t)read_pl() << 0 ;
|
|
ncyc |= (uint32_t)read_pl() << 8 ;
|
|
ncyc |= (uint32_t)read_pl() << 16;
|
|
ncyc |= (uint32_t)read_pl() << 24;
|
|
|
|
bool tdilvl = read_pl() != 0;
|
|
|
|
size_t nbytes = (ncyc + 7) >> 3;
|
|
if (nbytes != cmdhdr.len - 5) {
|
|
write_resp_str(mehfet_badargs, "TmsSequence: bad ncyc<->payload length");
|
|
} else {
|
|
uint8_t tms_stuff[nbytes];
|
|
|
|
for (size_t i = 0; i < nbytes; ++i) tms_stuff[i] = read_pl();
|
|
|
|
mehfet_hw_tms_seq(ncyc, tdilvl, tms_stuff);
|
|
|
|
write_resp(mehfet_ok, 0, NULL);
|
|
}
|
|
}
|
|
break;
|
|
case mehfet_tclk_edge:
|
|
if (cmdhdr.len != 1) write_resp_str(mehfet_badargs, "TclkEdge takes one parameter byte");
|
|
else if (connstat == mehfet_conn_none) write_resp(mehfet_badstate, 0, NULL);
|
|
else {
|
|
uint8_t newtclk = read_pl();
|
|
mehfet_hw_tclk_edge(newtclk);
|
|
write_resp(mehfet_ok, 0, NULL);
|
|
}
|
|
break;
|
|
case mehfet_tclk_burst:
|
|
if (cmdhdr.len != 4) write_resp_str(mehfet_badargs, "TclkBurst takes 4 parameter bytes");
|
|
else if (connstat == mehfet_conn_none) write_resp(mehfet_badstate, 0, NULL);
|
|
else {
|
|
uint32_t ncyc = 0;
|
|
ncyc |= (uint32_t)read_pl() << 0 ;
|
|
ncyc |= (uint32_t)read_pl() << 8 ;
|
|
ncyc |= (uint32_t)read_pl() << 16;
|
|
ncyc |= (uint32_t)read_pl() << 24;
|
|
|
|
mehfet_hw_tclk_burst(ncyc);
|
|
write_resp(mehfet_ok, 0, NULL);
|
|
}
|
|
break;
|
|
|
|
case mehfet_reset_tap:
|
|
if (!(mehfet_hw_get_caps() & mehfet_cap_has_reset_tap)) write_resp(mehfet_nocaps, 0, NULL);
|
|
else if (cmdhdr.len != 1) write_resp_str(mehfet_badargs, "ResetTAP takes one parameter byte");
|
|
else if (connstat == mehfet_conn_none) write_resp(mehfet_badstate, 0, NULL);
|
|
else {
|
|
uint8_t v = mehfet_hw_reset_tap(read_pl());
|
|
write_resp(mehfet_ok, 1, &v);
|
|
}
|
|
break;
|
|
case mehfet_irshift:
|
|
if (!(mehfet_hw_get_caps() & mehfet_cap_has_irshift)) write_resp(mehfet_nocaps, 0, NULL);
|
|
else if (cmdhdr.len != 1) write_resp_str(mehfet_badargs, "IRshift takes one parameter byte");
|
|
else if (connstat == mehfet_conn_none) write_resp(mehfet_badstate, 0, NULL);
|
|
else {
|
|
uint8_t newir = read_pl();
|
|
uint8_t oldir = mehfet_hw_shift_ir(newir);
|
|
write_resp(mehfet_ok, 1, &oldir);
|
|
}
|
|
break;
|
|
case mehfet_drshift:
|
|
if (!(mehfet_hw_get_caps() & mehfet_cap_has_drshift)) write_resp(mehfet_nocaps, 0, NULL);
|
|
else if (cmdhdr.len < 5) write_resp_str(mehfet_badargs,
|
|
"DRshift takes at least a bit count and some data (at least 5 bytes)");
|
|
else if (cmdhdr.len > 128 + 4) write_resp_str(mehfet_badargs,
|
|
"DRshift: too much data to process, can do max. 1024 bits (128B)");
|
|
else if (connstat == mehfet_conn_none) write_resp(mehfet_badstate, 0, NULL);
|
|
else {
|
|
uint32_t nbits = 0;
|
|
nbits |= (uint32_t)read_pl() << 0 ;
|
|
nbits |= (uint32_t)read_pl() << 8 ;
|
|
nbits |= (uint32_t)read_pl() << 16;
|
|
nbits |= (uint32_t)read_pl() << 24;
|
|
|
|
size_t nbytes = (nbits + 7) >> 3;
|
|
if (nbytes != cmdhdr.len - 4) {
|
|
write_resp_str(mehfet_badargs, "DRshift: bad nbits<->payload length");
|
|
} else {
|
|
uint8_t newdr[nbytes], olddr[nbytes];
|
|
|
|
for (size_t i = 0; i < nbytes; ++i) newdr[i] = read_pl();
|
|
|
|
mehfet_hw_shift_dr(nbits, newdr, olddr);
|
|
|
|
write_resp(mehfet_ok, nbytes, olddr);
|
|
}
|
|
}
|
|
break;
|
|
|
|
case mehfet_loop:
|
|
// TODO
|
|
write_resp_str(mehfet_nocaps, "not implemented yet, sorry");
|
|
//else if (connstat == mehfet_conn_none) write_resp(mehfet_badstate, 0, NULL);
|
|
break;
|
|
|
|
default:
|
|
write_resp(mehfet_invalidcmd, 0, NULL);
|
|
}
|
|
|
|
EXIT:
|
|
flush_pl(cmdhdr.len);
|
|
}
|
|
|