ysyx-workbench/nemu/tools/qemu-diff/src/protocol.c

306 lines
7.3 KiB
C
Raw Normal View History

/* Simple implementation of a GDB remote protocol client.
* Copyright (C) 2015 Red Hat Inc.
*
* This file is part of gdb-toys.
*
* 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 3 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, see <http://www.gnu.org/licenses/>.
*/
#include "common.h"
#include <ctype.h>
#include <err.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <sys/socket.h>
#include <sys/types.h>
struct gdb_conn {
FILE *in;
FILE *out;
bool ack;
};
static uint8_t
hex_nibble(uint8_t hex) {
return isdigit(hex) ? hex - '0' : tolower(hex) - 'a' + 10;
}
uint8_t hex_encode(uint8_t digit) {
return digit > 9 ? 'a' + digit - 10 : '0' + digit;
}
uint16_t gdb_decode_hex(uint8_t msb, uint8_t lsb) {
if (!isxdigit(msb) || !isxdigit(lsb))
return UINT16_MAX;
return 16 * hex_nibble(msb) + hex_nibble(lsb);
}
uint64_t gdb_decode_hex_str(uint8_t *bytes) {
uint64_t value = 0;
uint64_t weight = 1;
while (isxdigit(bytes[0]) && isxdigit(bytes[1])) {
value += weight * gdb_decode_hex(bytes[0], bytes[1]);
bytes += 2;
weight *= 16 * 16;
}
return value;
}
static struct gdb_conn* gdb_begin(int fd) {
struct gdb_conn *conn = calloc(1, sizeof(struct gdb_conn));
if (conn == NULL)
err(1, "calloc");
conn->ack = true;
// duplicate the handle to separate read/write state
int fd2 = dup(fd);
if (fd2 < 0)
err(1, "dup");
// open a FILE* for reading
conn->in = fdopen(fd, "rb");
if (conn->in == NULL)
err(1, "fdopen");
// open a FILE* for writing
conn->out = fdopen(fd2, "wb");
if (conn->out == NULL)
err(1, "fdopen");
// reset line state by acking any earlier input
fputc('+', conn->out);
fflush(conn->out);
return conn;
}
struct gdb_conn* gdb_begin_inet(const char *addr, uint16_t port) {
// fill the socket information
struct sockaddr_in sa = {
.sin_family = AF_INET,
.sin_port = htons(port),
};
if (inet_aton(addr, &sa.sin_addr) == 0)
errx(1, "Invalid address: %s", addr);
// open the socket and start the tcp connection
int fd = socket(AF_INET, SOCK_STREAM, 0);
if (fd < 0)
err(1, "socket");
if (connect(fd, (const struct sockaddr *)&sa, sizeof(sa)) != 0) {
close(fd);
return NULL;
}
socklen_t tmp;
tmp = 1;
int r = setsockopt(fd, SOL_SOCKET, SO_KEEPALIVE, (char *)&tmp, sizeof(tmp));
if (r) {
perror("setsockopt");
assert(0);
}
tmp = 1;
r = setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, (char *)&tmp, sizeof(tmp));
if (r) {
perror("setsockopt");
assert(0);
}
// initialize the rest of gdb on this handle
return gdb_begin(fd);
}
void gdb_end(struct gdb_conn *conn) {
fclose(conn->in);
fclose(conn->out);
free(conn);
}
static void send_packet(FILE *out, const uint8_t *command, size_t size) {
// compute the checksum -- simple mod256 addition
uint8_t sum = 0;
size_t i;
for (i = 0; i < size; ++i)
sum += command[i];
// NB: seems neither escaping nor RLE is generally expected by
// gdbserver. e.g. giving "invalid hex digit" on an RLE'd address.
// So just write raw here, and maybe let higher levels escape/RLE.
fputc('$', out); // packet start
fwrite(command, 1, size, out); // payload
fprintf(out, "#%02X", sum); // packet end, checksum
fflush(out);
if (ferror(out))
err(1, "send");
else if (feof(out))
errx(0, "send: Connection closed");
}
void gdb_send(struct gdb_conn *conn, const uint8_t *command, size_t size) {
bool acked = false;
do {
send_packet(conn->out, command, size);
if (!conn->ack)
break;
// look for '+' ACK or '-' NACK/resend
acked = fgetc(conn->in) == '+';
} while (!acked);
}
static uint8_t* recv_packet(FILE *in, size_t *ret_size, bool* ret_sum_ok) {
size_t i = 0;
size_t size = 4096;
uint8_t *reply = malloc(size);
if (reply == NULL)
err(1, "malloc");
int c;
uint8_t sum = 0;
bool escape = false;
// fast-forward to the first start of packet
while ((c = fgetc(in)) != EOF && c != '$');
while ((c = fgetc(in)) != EOF) {
sum += c;
switch (c) {
case '$': // new packet? start over...
i = 0;
sum = 0;
escape = false;
continue;
case '#': // end of packet
sum -= c; // not part of the checksum
{
uint8_t msb = fgetc(in);
uint8_t lsb = fgetc(in);
*ret_sum_ok = sum == gdb_decode_hex(msb, lsb);
}
*ret_size = i;
// terminate it for good measure
if (i == size) {
reply = realloc(reply, size + 1);
if (reply == NULL)
err(1, "realloc");
}
reply[i] = '\0';
return reply;
case '}': // escape: next char is XOR 0x20
escape = true;
continue;
case '*': // run-length-encoding
// The next character tells how many times to repeat the last
// character we saw. The count is added to 29, so that the
// minimum-beneficial RLE 3 is the first printable character ' '.
// The count character can't be >126 or '$'/'#' packet markers.
if (i > 0) { // need something to repeat!
int c2 = fgetc(in);
if (c2 < 29 || c2 > 126 || c2 == '$' || c2 == '#') {
// invalid count character!
ungetc(c2, in);
} else {
int count = c2 - 29;
// get a bigger buffer if needed
if (i + count > size) {
size *= 2;
reply = realloc(reply, size);
if (reply == NULL)
err(1, "realloc");
}
// fill the repeated character
memset(&reply[i], reply[i - 1], count);
i += count;
sum += c2;
continue;
}
}
}
// XOR an escaped character
if (escape) {
c ^= 0x20;
escape = false;
}
// get a bigger buffer if needed
if (i == size) {
size *= 2;
reply = realloc(reply, size);
if (reply == NULL)
err(1, "realloc");
}
// add one character
reply[i++] = c;
}
if (ferror(in))
err(1, "recv");
else if (feof(in))
errx(0, "recv: Connection closed");
else
errx(1, "recv: Unknown connection error");
}
uint8_t* gdb_recv(struct gdb_conn *conn, size_t *size) {
uint8_t *reply;
bool acked = false;
do {
reply = recv_packet(conn->in, size, &acked);
if (!conn->ack)
break;
// send +/- depending on checksum result, retry if needed
fputc(acked ? '+' : '-', conn->out);
fflush(conn->out);
} while (!acked);
return reply;
}
const char* gdb_start_noack(struct gdb_conn *conn) {
static const char cmd[] = "QStartNoAckMode";
gdb_send(conn, (const uint8_t *)cmd, sizeof(cmd) - 1);
size_t size;
uint8_t *reply = gdb_recv(conn, &size);
bool ok = size == 2 && !strcmp((const char*)reply, "OK");
free(reply);
if (ok)
conn->ack = false;
return ok ? "OK" : "";
}