mspdebug/util/gdb_proto.c

223 lines
4.3 KiB
C

/* MSPDebug - debugging tool for MSP430 MCUs
* Copyright (C) 2009-2011 Daniel Beer
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
#include <stdarg.h>
#include <stdio.h>
#include <sys/time.h>
#include <unistd.h>
#include <string.h>
#include "sockets.h"
#include "gdb_proto.h"
#include "output.h"
#include "util.h"
void gdb_init(struct gdb_data *data, int sock)
{
data->sock = sock;
data->error = 0;
data->head = 0;
data->tail = 0;
data->outlen = 0;
}
void gdb_printf(struct gdb_data *data, const char *fmt, ...)
{
va_list ap;
int len;
va_start(ap, fmt);
len = vsnprintf(data->outbuf + data->outlen,
sizeof(data->outbuf) - data->outlen,
fmt, ap);
va_end(ap);
data->outlen += len;
}
/* Returns -1 for error, 0 for timeout, >0 if data received. */
static int gdb_read(struct gdb_data *data, int timeout_ms)
{
int was_timeout;
int len = sockets_recv(data->sock, data->xbuf, sizeof(data->xbuf), 0,
timeout_ms, &was_timeout);
if (was_timeout)
return 0;
if (len < 0) {
data->error = 1;
pr_error("gdb: recv");
return -1;
}
if (!len) {
data->error = 1;
printc("Connection closed\n");
return -1;
}
data->head = 0;
data->tail = len;
return len;
}
int gdb_peek(struct gdb_data *data, int timeout_ms)
{
if (data->head == data->tail)
return gdb_read(data, timeout_ms);
return data->head != data->tail;
}
int gdb_getc(struct gdb_data *data)
{
int c;
/* If the buffer is empty, receive some more data */
if (data->head == data->tail && gdb_read(data, -1) <= 0)
return -1;
c = data->xbuf[data->head];
data->head++;
return c;
}
static int gdb_flush(struct gdb_data *data)
{
if (sockets_send(data->sock, data->outbuf, data->outlen, 0) < 0) {
data->error = 1;
pr_error("gdb: flush");
return -1;
}
data->outlen = 0;
return 0;
}
int gdb_flush_ack(struct gdb_data *data)
{
int c;
#ifdef DEBUG_GDB
printc("-> %s\n", data->outbuf);
#endif
data->outbuf[data->outlen] = 0;
do {
if (sockets_send(data->sock, data->outbuf,
data->outlen, 0) < 0) {
data->error = 1;
pr_error("gdb: flush_ack");
return -1;
}
do {
c = gdb_getc(data);
if (c < 0)
return -1;
} while (c != '+' && c != '-');
} while (c != '+');
data->outlen = 0;
return 0;
}
void gdb_packet_start(struct gdb_data *data)
{
gdb_printf(data, "$");
}
void gdb_packet_end(struct gdb_data *data)
{
int i;
int c = 0;
for (i = 1; i < data->outlen; i++)
c = (c + data->outbuf[i]) & 0xff;
gdb_printf(data, "#%02x", c);
}
int gdb_send(struct gdb_data *data, const char *msg)
{
gdb_packet_start(data);
gdb_printf(data, "%s", msg);
gdb_packet_end(data);
return gdb_flush_ack(data);
}
int gdb_read_packet(struct gdb_data *data, char *buf)
{
int c;
int len = 0;
int cksum_calc = 0;
int cksum_recv = 0;
/* Wait for packet start */
do {
c = gdb_getc(data);
if (c < 0)
return -1;
} while (c != '$');
/* Read packet payload */
while (len + 1 < GDB_BUF_SIZE) {
c = gdb_getc(data);
if (c < 0)
return -1;
if (c == '#')
break;
buf[len++] = c;
cksum_calc = (cksum_calc + c) & 0xff;
}
buf[len] = 0;
/* Read packet checksum */
c = gdb_getc(data);
if (c < 0)
return -1;
cksum_recv = hexval(c);
c = gdb_getc(data);
if (c < 0)
return -1;
cksum_recv = (cksum_recv << 4) | hexval(c);
#ifdef DEBUG_GDB
printc("<- $%s#%02x\n", buf, cksum_recv);
#endif
if (cksum_recv != cksum_calc) {
printc_err("gdb: bad checksum (calc = 0x%02x, "
"recv = 0x%02x)\n", cksum_calc, cksum_recv);
printc_err("gdb: packet data was: %s\n", buf);
gdb_printf(data, "-");
if (gdb_flush(data) < 0)
return -1;
return 0;
}
/* Send acknowledgement */
gdb_printf(data, "+");
if (gdb_flush(data) < 0)
return -1;
return len;
}