sys64738
/
mspdebug
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Implemented proper reply parsing.

This commit is contained in:
Daniel Beer 2010-01-04 13:03:36 +13:00
parent ec5c61e9c9
commit eb444b9957
1 changed files with 150 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

223
fet.c
View File

@ -162,10 +162,112 @@ static int send_rf2500_data(const char *data, int len)
static char fet_buf[65538]; static char fet_buf[65538];
static int fet_len; static int fet_len;
#define MAX_REPLY_PARAMS 16
/* Recieved packet is parsed into this struct */
static struct {
int command_code;
int state;
int argc;
u_int32_t argv[MAX_REPLY_PARAMS];
char *data;
int datalen;
} fet_reply;
#define BUFFER_BYTE(b, x) ((int)((u_int8_t *)(b))[x]) #define BUFFER_BYTE(b, x) ((int)((u_int8_t *)(b))[x])
#define BUFFER_WORD(b, x) ((BUFFER_BYTE(b, x + 1) << 8) | BUFFER_BYTE(b, x)) #define BUFFER_WORD(b, x) ((BUFFER_BYTE(b, x + 1) << 8) | BUFFER_BYTE(b, x))
#define BUFFER_LONG(b, x) ((BUFFER_WORD(b, x + 2) << 16) | BUFFER_WORD(b, x))
static const char *recv_packet(int *pktlen) #define PTYPE_ACK 0
#define PTYPE_CMD 1
#define PTYPE_PARAM 2
#define PTYPE_DATA 3
#define PTYPE_MIXED 4
#define PTYPE_NAK 5
#define PTYPE_FLASH_ACK 6
static int parse_packet(int plen)
{
u_int16_t c = calc_checksum(fet_buf + 2, plen - 2);
u_int16_t r = BUFFER_WORD(fet_buf, plen);
int i = 2;
int type;
int error;
if (c != r) {
fprintf(stderr, "parse_packet: checksum error (calc %04x,"
" recv %04x)\n", c, r);
return -1;
}
if (plen < 6)
goto too_short;
fet_reply.command_code = fet_buf[i++];
type = fet_buf[i++];
fet_reply.state = fet_buf[i++];
error = fet_buf[i++];
if (error) {
fprintf(stderr, "parse_packet: FET returned error code %d\n",
error);
return -1;
}
/* Parse packet parameters */
if (type == PTYPE_PARAM || type == PTYPE_MIXED) {
int j;
if (i + 2 > plen)
goto too_short;
fet_reply.argc = BUFFER_WORD(fet_buf, i);
i += 2;
if (fet_reply.argc >= MAX_REPLY_PARAMS) {
fprintf(stderr, "parse_packet: too many params: %d\n",
fet_reply.argc);
return -1;
}
for (j = 0; j < fet_reply.argc; j++) {
if (i + 4 > plen)
goto too_short;
fet_reply.argv[j] = BUFFER_LONG(fet_buf, i);
i += 4;
}
} else {
fet_reply.argc = 0;
}
/* Extract a pointer to the data */
if (type == PTYPE_DATA || type == PTYPE_MIXED) {
if (i + 4 > plen)
goto too_short;
fet_reply.datalen = BUFFER_LONG(fet_buf, i);
i += 4;
if (i + fet_reply.datalen > plen)
goto too_short;
fet_reply.data = fet_buf + i;
} else {
fet_reply.data = NULL;
fet_reply.datalen = 0;
}
return 0;
too_short:
fprintf(stderr, "parse_packet: too short (%d bytes)\n",
plen);
return -1;
}
static int recv_packet(void)
{ {
int plen = BUFFER_WORD(fet_buf, 0); int plen = BUFFER_WORD(fet_buf, 0);
@ -182,31 +284,17 @@ static const char *recv_packet(int *pktlen)
int len; int len;
plen = BUFFER_WORD(fet_buf, 0); plen = BUFFER_WORD(fet_buf, 0);
if (fet_len >= plen + 2) { if (fet_len >= plen + 2)
u_int16_t c = calc_checksum(fet_buf + 2, plen - 2); return parse_packet(plen);
u_int16_t r = BUFFER_WORD(fet_buf, plen);
if (pktlen)
*pktlen = plen - 2;
if (c != r) {
fprintf(stderr, "recv_packet: checksum "
"error (calc %04x, recv %04x)\n",
c, r);
return NULL;
}
return fet_buf + 2;
}
len = fet_transport->recv(fet_buf + fet_len, len = fet_transport->recv(fet_buf + fet_len,
sizeof(fet_buf) - fet_len); sizeof(fet_buf) - fet_len);
if (len < 0) if (len < 0)
return NULL; return -1;
fet_len += len; fet_len += len;
} }
return NULL; return -1;
} }
static int send_command(int command_code, static int send_command(int command_code,
@ -278,32 +366,27 @@ static int send_command(int command_code,
return fet_transport->send(buf, i); return fet_transport->send(buf, i);
} }
static const char *xfer(int command_code, static int xfer(int command_code, const u_int32_t *params, int nparams,
const u_int32_t *params, int nparams, const char *data, int datalen)
const char *data, int datalen,
int *recvlen)
{ {
const char *buf;
if (data && fet_is_rf2500) { if (data && fet_is_rf2500) {
if (send_rf2500_data(data, datalen) < 0) if (send_rf2500_data(data, datalen) < 0)
return NULL; return -1;
if (send_command(command_code, params, nparams, NULL, 0) < 0) if (send_command(command_code, params, nparams, NULL, 0) < 0)
return NULL; return -1;
} else if (send_command(command_code, params, nparams, } else if (send_command(command_code, params, nparams,
data, datalen) < 0) data, datalen) < 0)
return NULL; return -1;
buf = recv_packet(recvlen); if (recv_packet() < 0)
if (!buf) return -1;
return NULL;
if (command_code != buf[0]) { if (fet_reply.command_code != command_code) {
fprintf(stderr, "xfer: reply type mismatch\n"); fprintf(stderr, "xfer: reply type mismatch\n");
return NULL; return -1;
} }
return buf; return 0;
} }
/********************************************************************** /**********************************************************************
@ -318,13 +401,13 @@ int fet_open(const struct fet_transport *tr, int proto_flags, int vcc_mv)
fet_is_rf2500 = proto_flags & FET_PROTO_RF2500; fet_is_rf2500 = proto_flags & FET_PROTO_RF2500;
init_codes(); init_codes();
if (!xfer(C_INITIALIZE, NULL, 0, NULL, 0, NULL)) { if (xfer(C_INITIALIZE, NULL, 0, NULL, 0) < 0) {
fprintf(stderr, "fet_open: open failed\n"); fprintf(stderr, "fet_open: open failed\n");
return -1; return -1;
} }
parm[0] = 4; parm[0] = 4;
if (!xfer(39, parm, 1, NULL, 0, NULL)) { if (xfer(39, parm, 1, NULL, 0) < 0) {
fprintf(stderr, "fet_open: init failed\n"); fprintf(stderr, "fet_open: init failed\n");
return -1; return -1;
} }
@ -332,21 +415,21 @@ int fet_open(const struct fet_transport *tr, int proto_flags, int vcc_mv)
/* configure: Spy-Bi-Wire or JTAG */ /* configure: Spy-Bi-Wire or JTAG */
parm[0] = 8; parm[0] = 8;
parm[1] = (proto_flags & FET_PROTO_SPYBIWIRE) ? 1 : 0; parm[1] = (proto_flags & FET_PROTO_SPYBIWIRE) ? 1 : 0;
if (!xfer(C_CONFIGURE, parm, 2, NULL, 0, NULL)) { if (xfer(C_CONFIGURE, parm, 2, NULL, 0) < 0) {
fprintf(stderr, "fet_open: configure failed\n"); fprintf(stderr, "fet_open: configure failed\n");
return -1; return -1;
} }
parm[0] = 0x50; parm[0] = 0x50;
parm[1] = 0; parm[1] = 0;
if (!fet_is_rf2500 && !xfer(C_IDENTIFY, parm, 2, NULL, 0, NULL)) { if (!fet_is_rf2500 && xfer(C_IDENTIFY, parm, 2, NULL, 0) < 0) {
fprintf(stderr, "fet_open: identify failed\n"); fprintf(stderr, "fet_open: identify failed\n");
return -1; return -1;
} }
/* set VCC */ /* set VCC */
parm[0] = vcc_mv; parm[0] = vcc_mv;
if (!xfer(C_VCC, parm, 2, NULL, 0, NULL)) { if (xfer(C_VCC, parm, 2, NULL, 0) < 0) {
fprintf(stderr, "fet_open: set VCC failed\n"); fprintf(stderr, "fet_open: set VCC failed\n");
return -1; return -1;
} }
@ -356,7 +439,7 @@ int fet_open(const struct fet_transport *tr, int proto_flags, int vcc_mv)
*/ */
parm[0] = 0; parm[0] = 0;
parm[1] = 0; parm[1] = 0;
if (fet_is_rf2500 && !xfer(0x28, parm, 2, NULL, 0, NULL)) { if (fet_is_rf2500 && xfer(0x28, parm, 2, NULL, 0) < 0) {
fprintf(stderr, "fet_open: command 0x28 failed\n"); fprintf(stderr, "fet_open: command 0x28 failed\n");
return -1; return -1;
} }
@ -383,7 +466,7 @@ int fet_open(const struct fet_transport *tr, int proto_flags, int vcc_mv)
parm[2] = 0x31; parm[2] = 0x31;
parm[3] = sizeof(data); parm[3] = sizeof(data);
if (!xfer(0x29, parm, 4, data, sizeof(data), NULL)) { if (xfer(0x29, parm, 4, data, sizeof(data)) < 0) {
fprintf(stderr, "fet_open: command 0x29 failed\n"); fprintf(stderr, "fet_open: command 0x29 failed\n");
return -1; return -1;
} }
@ -403,7 +486,7 @@ int fet_reset(int flags)
flags & FET_RESET_HALT ? 1 : 0 flags & FET_RESET_HALT ? 1 : 0
}; };
if (!xfer(C_RESET, parm, 3, NULL, 0, NULL)) { if (xfer(C_RESET, parm, 3, NULL, 0) < 0) {
fprintf(stderr, "fet_reset: reset failed\n"); fprintf(stderr, "fet_reset: reset failed\n");
return -1; return -1;
} }
@ -415,7 +498,7 @@ int fet_close(void)
{ {
u_int32_t parm = 0; u_int32_t parm = 0;
if (!xfer(C_CLOSE, &parm, 1, NULL, 0, NULL)) { if (xfer(C_CLOSE, &parm, 1, NULL, 0) < 0) {
fprintf(stderr, "fet_shutdown: close command failed\n"); fprintf(stderr, "fet_shutdown: close command failed\n");
return -1; return -1;
} }
@ -428,19 +511,19 @@ int fet_close(void)
int fet_get_context(u_int16_t *regs) int fet_get_context(u_int16_t *regs)
{ {
int len;
int i; int i;
const char *buf;
buf = xfer(C_READREGISTERS, NULL, 0, NULL, 0, &len); if (xfer(C_READREGISTERS, NULL, 0, NULL, 0) < 0)
if (len < 72) { return -1;
if (fet_reply.datalen < FET_NUM_REGS * 4) {
fprintf(stderr, "fet_get_context: short reply (%d bytes)\n", fprintf(stderr, "fet_get_context: short reply (%d bytes)\n",
len); fet_reply.datalen);
return -1; return -1;
} }
for (i = 0; i < FET_NUM_REGS; i++) for (i = 0; i < FET_NUM_REGS; i++)
regs[i] = BUFFER_WORD(buf, i * 4 + 8); regs[i] = BUFFER_WORD(fet_reply.data, i * 4);
return 0; return 0;
} }
@ -461,8 +544,8 @@ int fet_set_context(u_int16_t *regs)
buf[i * 4 + 1] = regs[i] >> 8; buf[i * 4 + 1] = regs[i] >> 8;
} }
if (!xfer(C_WRITEREGISTERS, parm, fet_is_rf2500 ? 2 : 1, if (xfer(C_WRITEREGISTERS, parm, fet_is_rf2500 ? 2 : 1,
buf, sizeof(buf), NULL)) { buf, sizeof(buf)) < 0) {
fprintf(stderr, "fet_set_context: context set failed\n"); fprintf(stderr, "fet_set_context: context set failed\n");
return -1; return -1;
} }
@ -476,25 +559,22 @@ int fet_read_mem(u_int16_t addr, char *buffer, int count)
while (count) { while (count) {
int plen = count > 128 ? 128 : count; int plen = count > 128 ? 128 : count;
const char *buf;
int len;
parm[0] = addr; parm[0] = addr;
parm[1] = plen; parm[1] = plen;
buf = xfer(C_READMEMORY, parm, 2, NULL, 0, &len); if (xfer(C_READMEMORY, parm, 2, NULL, 0) < 0) {
if (!buf) {
fprintf(stderr, "fet_read_mem: failed to read " fprintf(stderr, "fet_read_mem: failed to read "
"from 0x%04x\n", addr); "from 0x%04x\n", addr);
return -1; return -1;
} }
if (len < plen + 8) { if (fet_reply.datalen < plen) {
fprintf(stderr, "fet_read_mem: short read " fprintf(stderr, "fet_read_mem: short data: "
"(%d bytes)\n", len); "%d bytes\n", fet_reply.datalen);
return -1; return -1;
} }
memcpy(buffer, buf + 8, plen); memcpy(buffer, fet_reply.data, plen);
buffer += plen; buffer += plen;
count -= plen; count -= plen;
addr += plen; addr += plen;
@ -512,8 +592,8 @@ int fet_write_mem(u_int16_t addr, char *buffer, int count)
parm[0] = addr; parm[0] = addr;
parm[1] = plen; parm[1] = plen;
if (!xfer(C_WRITEMEMORY, parm, fet_is_rf2500 ? 2 : 1, if (xfer(C_WRITEMEMORY, parm, fet_is_rf2500 ? 2 : 1,
buffer, plen, NULL)) { buffer, plen) < 0) {
fprintf(stderr, "fet_write_mem: failed to write " fprintf(stderr, "fet_write_mem: failed to write "
"to 0x%04x\n", addr); "to 0x%04x\n", addr);
return -1; return -1;
@ -533,14 +613,14 @@ int fet_erase(int type, u_int16_t addr)
parm[0] = 2; parm[0] = 2;
parm[1] = 0x26; parm[1] = 0x26;
if (!xfer(C_CONFIGURE, parm, 2, NULL, 0, NULL)) { if (xfer(C_CONFIGURE, parm, 2, NULL, 0) < 0) {
fprintf(stderr, "fet_erase: config (1) failed\n"); fprintf(stderr, "fet_erase: config (1) failed\n");
return -1; return -1;
} }
parm[0] = 5; parm[0] = 5;
parm[1] = 0; parm[1] = 0;
if (!xfer(C_CONFIGURE, parm, 2, NULL, 0, NULL)) { if (xfer(C_CONFIGURE, parm, 2, NULL, 0) < 0) {
fprintf(stderr, "fet_erase: config (2) failed\n"); fprintf(stderr, "fet_erase: config (2) failed\n");
return -1; return -1;
} }
@ -569,7 +649,7 @@ int fet_erase(int type, u_int16_t addr)
} }
parm[1] = addr; parm[1] = addr;
if (!xfer(C_ERASE, parm, 2, NULL, 0, NULL)) { if (xfer(C_ERASE, parm, 2, NULL, 0) < 0) {
fprintf(stderr, "fet_erase: erase command failed\n"); fprintf(stderr, "fet_erase: erase command failed\n");
return -1; return -1;
} }
@ -579,21 +659,18 @@ int fet_erase(int type, u_int16_t addr)
int fet_poll(void) int fet_poll(void)
{ {
const char *reply;
int len;
u_int32_t parm = 0; u_int32_t parm = 0;
/* Without this delay, breakpoints can get lost. */ /* Without this delay, breakpoints can get lost. */
if (usleep(500000) < 0) if (usleep(500000) < 0)
return -1; return -1;
reply = xfer(C_STATE, &parm, 1, NULL, 0, &len); if (xfer(C_STATE, &parm, 1, NULL, 0) < 0) {
if (!reply) {
fprintf(stderr, "fet_poll: polling failed\n"); fprintf(stderr, "fet_poll: polling failed\n");
return -1; return -1;
} }
return reply[6]; return fet_reply.argv[0];
} }
int fet_step(void) int fet_step(void)
@ -602,7 +679,7 @@ int fet_step(void)
2, 0 2, 0
}; };
if (!xfer(C_RUN, parm, 2, NULL, 0, NULL)) { if (xfer(C_RUN, parm, 2, NULL, 0) < 0) {
fprintf(stderr, "fet_step: failed to single-step\n"); fprintf(stderr, "fet_step: failed to single-step\n");
return -1; return -1;
} }
@ -616,7 +693,7 @@ int fet_run(void)
fet_breakpoint_enable ? 3 : 1, 0 fet_breakpoint_enable ? 3 : 1, 0
}; };
if (!xfer(C_RUN, parm, 2, NULL, 0, NULL)) { if (xfer(C_RUN, parm, 2, NULL, 0) < 0) {
fprintf(stderr, "fet_run: run failed\n"); fprintf(stderr, "fet_run: run failed\n");
return -1; return -1;
} }
@ -628,7 +705,7 @@ int fet_stop(void)
{ {
u_int32_t parm = 1; u_int32_t parm = 1;
if (!xfer(C_STATE, &parm, 1, NULL, 0, NULL)) { if (xfer(C_STATE, &parm, 1, NULL, 0) < 0) {
fprintf(stderr, "fet_stop: stop failed\n"); fprintf(stderr, "fet_stop: stop failed\n");
return -1; return -1;
} }
@ -641,7 +718,7 @@ int fet_break(int enable, u_int16_t addr)
u_int32_t parm[2] = { 0, addr }; u_int32_t parm[2] = { 0, addr };
fet_breakpoint_enable = enable; fet_breakpoint_enable = enable;
if (!xfer(C_BREAKPOINT, parm, 2, NULL, 0, NULL)) { if (xfer(C_BREAKPOINT, parm, 2, NULL, 0) < 0) {
fprintf(stderr, "fet_break: set breakpoint failed\n"); fprintf(stderr, "fet_break: set breakpoint failed\n");
return -1; return -1;
} }