aboutsummaryrefslogtreecommitdiffstats
path: root/firmware/zpu/lib/gdbstub2.c
diff options
context:
space:
mode:
Diffstat (limited to 'firmware/zpu/lib/gdbstub2.c')
-rw-r--r--firmware/zpu/lib/gdbstub2.c506
1 files changed, 506 insertions, 0 deletions
diff --git a/firmware/zpu/lib/gdbstub2.c b/firmware/zpu/lib/gdbstub2.c
new file mode 100644
index 000000000..4c63dfce2
--- /dev/null
+++ b/firmware/zpu/lib/gdbstub2.c
@@ -0,0 +1,506 @@
+/* -*- c++ -*- */
+/*
+ * Copyright 2009 Ettus Research LLC
+ *
+ * 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/>.
+ */
+
+/*
+ * Implement a eensy weensy part of the GDB Remote Serial Protocol
+ *
+ * See Appendix D of the GDB manual
+ *
+ * m<addr>,<length> -- read <length> bytes of memory starting at <addr>
+ * Reply:
+ * XX... XX... is memory contents in hex
+ * ENN ENN NN is a hex error number
+ *
+ * M<addr>,<length>:XX... -- write memory, data in hex
+ * Reply:
+ * OK for success
+ * ENN for an error. NN is a hex error number
+ *
+ * X<addr>,<length>:XX... -- write memory, data in binary
+ * Reply:
+ * OK for success
+ * ENN for an error. NN is a hex error number
+ *
+ * c<addr> -- continue. <addr> is the address to resume (goto).
+ * Reply: <none>
+ *
+ * \x80 New Format...
+ */
+
+#include "gdbstub2.h"
+#include "loader_parser.h"
+#include "hal_uart.h"
+#include <stdbool.h>
+#include <stddef.h>
+
+#define MAX_PACKET 1024
+
+/*
+ * Get raw character from serial port, no echo.
+ */
+static inline int
+gdb_getc(void)
+{
+ return hal_uart_getc();
+}
+
+/*
+ * Put character to serial port. Raw output.
+ */
+static inline void
+gdb_putc(int ch)
+{
+ hal_uart_putc(ch);
+}
+
+// ------------------------------------------------------------------------
+
+#define GDB_ESCAPE 0x7d
+
+static unsigned char hex_table[16] = {
+ '0', '1', '2', '3', '4', '5', '6', '7',
+ '8', '9', 'a', 'b', 'c', 'd', 'e', 'f'
+};
+
+static int
+put_hex8_checksum(int ch, int checksum)
+{
+ unsigned char t = hex_table[(ch >> 4) & 0xf];
+ checksum += t;
+ gdb_putc(t);
+
+ t = hex_table[ch & 0xf];
+ checksum += t;
+ gdb_putc(t);
+ return checksum;
+}
+
+static void
+put_hex8(int ch)
+{
+ put_hex8_checksum(ch, 0);
+}
+
+static bool
+hex4_to_bin(int ch, int *value)
+{
+ if ('0' <= ch && ch <= '9'){
+ *value = ch - '0';
+ return true;
+ }
+ if ('a' <= ch && ch <= 'f'){
+ *value = ch - 'a' + 10;
+ return true;
+ }
+ if ('A' <= ch && ch <= 'F'){
+ *value = ch - 'A' + 10;
+ return true;
+ }
+ *value = 0;
+ return false;
+}
+
+static bool
+hex8_to_bin(const unsigned char *s, int *value)
+{
+ int v0, v1;
+ if (hex4_to_bin(s[0], &v0) && hex4_to_bin(s[1], &v1)){
+ *value = (v0 << 4) | v1;
+ return true;
+ }
+ return false;
+}
+
+static bool
+hex_to_bin_array(unsigned char *binary_data, const unsigned char *hex_data, size_t nbytes)
+{
+ for (size_t i = 0; i < nbytes; i++){
+ int t;
+ if (!hex8_to_bin(&hex_data[2*i], &t))
+ return false;
+ binary_data[i] = t;
+ }
+ return true;
+}
+
+static bool
+needs_escaping(int ch)
+{
+ return ch == '$' || ch == '#' || ch == GDB_ESCAPE;
+}
+
+/*
+ * \brief Wait for a packet.
+ * \param[out] pkt_buf gets the received packet payload.
+ * \param[in] max_size is the maximum number of bytes to write into \p pkt_buf.
+ * \param[out] actual_size is the number of bytes written to \p pkt_buf.
+ *
+ * \returns true iff the payload fits and the checksum is OK.
+ *
+ * Packets have this format:
+ *
+ * $<packet-data>#<checksum>
+ *
+ * Where <packet-data> is anything and <checksum> is a two byte hex
+ * checksum. In <packet-data> '$', '#' and 0x7d are escaped with 0x7d.
+ * The checksum is computed as the modulo 256 sum of all characters
+ * btween the leading '$' and the trailing '#' (an 8-bit unsigned
+ * checksum).
+ */
+static bool
+get_packet(unsigned char *pkt_buf, size_t max_size, size_t *actual_size)
+{
+ typedef enum states {
+ LOOKING_FOR_DOLLAR,
+ LOOKING_FOR_HASH,
+ CSUM1,
+ CSUM2,
+ } state_t;
+
+ *actual_size = 0;
+ unsigned char csum[2] = {0, 0};
+ state_t state = LOOKING_FOR_DOLLAR;
+ size_t pi = 0;
+
+ while (1){
+ int ch = gdb_getc();
+
+ switch (state){
+ case LOOKING_FOR_DOLLAR:
+ if (ch == '$'){
+ pi = 0;
+ state = LOOKING_FOR_HASH;
+ }
+ else if (ch == '#'){ // most likely missed the $
+ return false;
+ }
+ break;
+
+ case LOOKING_FOR_HASH:
+ if (ch == '$'){
+ return false;
+ }
+ else if (ch == '#'){
+ state = CSUM1;
+ }
+ else {
+ if (pi >= max_size) // payload too big
+ return false;
+
+ if (ch == GDB_ESCAPE)
+ ch = gdb_getc();
+
+ pkt_buf[pi++] = ch;
+ }
+ break;
+
+ case CSUM1:
+ csum[0] = ch;
+ state = CSUM2;
+ break;
+
+ case CSUM2:
+ csum[1] = ch;
+ *actual_size = pi;
+
+ // accept .. as a correct checksum
+ if (csum[0] == '.' && csum[1] == '.')
+ return true;
+
+ int expected_checksum;
+ if (!hex8_to_bin(csum, &expected_checksum))
+ return false;
+
+ int checksum = 0;
+ for (size_t i = 0; i < pi; i++)
+ checksum += pkt_buf[i];
+
+ checksum &= 0xff;
+ return checksum == expected_checksum;
+ }
+ }
+}
+
+static void
+put_packet_trailer(int checksum)
+{
+ gdb_putc('#');
+ put_hex8(checksum & 0xff);
+ gdb_putc('\r');
+ gdb_putc('\n');
+}
+
+static void
+put_packet(const unsigned char *pkt_buf, size_t size)
+{
+ gdb_putc('$');
+
+ int checksum = 0;
+ for (size_t i = 0; i < size; i++){
+ int ch = pkt_buf[i];
+ if (needs_escaping(ch))
+ gdb_putc(GDB_ESCAPE);
+ gdb_putc(ch);
+ checksum += ch;
+ }
+ put_packet_trailer(checksum);
+}
+
+/*!
+ * Read a hex number
+ *
+ * \param[inout] bufptr - pointer to pointer to buffer (updated on return)
+ * \param[in] end - one past end of valid data in buf
+ * \param[out] value - the parsed value
+ *
+ * \returns true iff a valid hex number was read from bufptr
+ */
+static bool
+parse_number(const unsigned char **bufptr, const unsigned char *end, unsigned int *value)
+{
+ const unsigned char *buf = *bufptr;
+ unsigned int v = 0;
+ bool valid = false;
+ int nibble;
+
+ while (buf < end && hex4_to_bin(*buf, &nibble)){
+ valid = true;
+ v = (v << 4) | nibble;
+ buf++;
+ }
+
+ *value = v;
+ *bufptr = buf;
+ return valid;
+}
+
+static bool
+parse_char(const unsigned char **bufptr, const unsigned char *end, unsigned char *ch)
+{
+ const unsigned char *buf = *bufptr;
+ if (buf < end){
+ *ch = *buf++;
+ *bufptr = buf;
+ return true;
+ }
+ return false;
+}
+
+static bool
+expect_char(const unsigned char **bufptr, const unsigned char *end, unsigned char expected)
+{
+ unsigned char ch;
+ return parse_char(bufptr, end, &ch) && ch == expected;
+}
+
+static bool
+expect_end(const unsigned char **bufptr, const unsigned char *end)
+{
+ return *bufptr == end;
+}
+
+static bool
+parse_addr_length(const unsigned char **bufptr, const unsigned char *end,
+ unsigned int *addr, unsigned int *length)
+{
+ return (parse_number(bufptr, end, addr)
+ && expect_char(bufptr, end, ',')
+ && parse_number(bufptr, end, length));
+}
+
+static void
+put_error(int error)
+{
+ unsigned char buf[3];
+ buf[0] = 'E';
+ buf[1] = hex_table[(error >> 4) & 0xf];
+ buf[2] = hex_table[error & 0xf];
+
+ put_packet(buf, sizeof(buf));
+}
+
+static void
+put_ok(void)
+{
+ const unsigned char buf[2] = "OK";
+ put_packet(buf, sizeof(buf));
+}
+
+/*
+ * Read memory and send the reply.
+ * We do it on the fly so that our packet size is effectively unlimited
+ */
+static void
+read_memory(unsigned int addr, unsigned int nbytes)
+{
+ int checksum = 0;
+ gdb_putc('$');
+
+ if ((addr & 0x3) == 0 && (nbytes & 0x3) == 0){ // word aligned
+ union {
+ unsigned int i;
+ unsigned char c[4];
+ } u;
+
+ unsigned int *p = (unsigned int *) addr;
+ unsigned int length = nbytes / 4;
+
+ for (unsigned int i = 0; i < length; i++){
+ u.i = p[i]; // do a word read
+ checksum = put_hex8_checksum(u.c[0], checksum);
+ checksum = put_hex8_checksum(u.c[1], checksum);
+ checksum = put_hex8_checksum(u.c[2], checksum);
+ checksum = put_hex8_checksum(u.c[3], checksum);
+ }
+ }
+ else { // byte aligned
+ unsigned char *p = (unsigned char *) addr;
+ for (unsigned int i = 0; i < nbytes; i++)
+ checksum = put_hex8_checksum(p[i], checksum);
+ }
+
+ put_packet_trailer(checksum);
+}
+
+static unsigned int
+get_unaligned_int(const unsigned char *p)
+{
+ // we're bigendian
+ return (p[0] << 24) | (p[1] << 16) | (p[2] << 8) | (p[3]);
+}
+
+static bool
+write_memory(unsigned int addr, size_t nbytes,
+ const unsigned char *data)
+{
+ if ((addr & 0x3) == 0 && (nbytes & 0x3) == 0){ // word-aligned dst
+ unsigned int *dst = (unsigned int *) addr;
+ size_t length = nbytes / 4;
+ for (size_t i = 0; i < length; i++){
+ unsigned int t = get_unaligned_int(&data[4*i]);
+ dst[i] = t; // word writes
+ }
+ }
+ else { // non-word-aligned dst
+ unsigned char *dst = (unsigned char *) addr;
+ for (size_t i = 0; i < nbytes; i++){
+ dst[i] = data[i];
+ }
+ }
+ return true;
+}
+
+void
+gdbstub2_main_loop(void)
+{
+ unsigned char inpkt[MAX_PACKET + 24];
+ unsigned char binary_data[MAX_PACKET/2] __attribute__((aligned (4)));
+
+ hal_uart_set_mode(UART_MODE_RAW); //tell UART HAL not to map \n to \r\n
+
+ while (1){
+ size_t inpkt_len;
+ bool ok = get_packet(inpkt, sizeof(inpkt), &inpkt_len);
+ if (!ok){
+ gdb_putc('-');
+ continue;
+ }
+ gdb_putc('+');
+
+ const unsigned char *buf = inpkt;
+ const unsigned char *end = inpkt + inpkt_len;
+ unsigned char ch;
+
+ if (!parse_char(&buf, end, &ch)){ // empty packet
+ put_packet(0, 0);
+ continue;
+ }
+
+ unsigned int addr;
+ unsigned int length;
+
+ switch(ch){
+ case 'm': // m<addr>,<length> -- read <length> bytes starting at <addr>
+ if (!(parse_addr_length(&buf, end, &addr, &length) && expect_end(&buf, end))){
+ put_error(1);
+ }
+ else {
+ read_memory(addr, length);
+ }
+ break;
+
+ case 'M': // M<addr>,<length>:XX... -- write <length> bytes starting at <addr>
+ // XX... is the data in hex
+ if (!(parse_addr_length(&buf, end, &addr, &length)
+ && expect_char(&buf, end, ':')
+ && (end - buf) == 2 * length)){
+ put_error(1);
+ }
+ else {
+ if (!hex_to_bin_array(binary_data, buf, length))
+ put_error(2);
+ else if (!write_memory(addr, length, binary_data))
+ put_error(3);
+ else
+ put_ok();
+ }
+ break;
+
+ case 'X': // X<addr>,<length>:XX... -- write <length> bytes starting at <addr>
+ // XX... is the data in binary
+ if (!(parse_addr_length(&buf, end, &addr, &length)
+ && expect_char(&buf, end, ':')
+ && (end - buf) == length)){
+ put_error(1);
+ }
+ else {
+ if (!write_memory(addr, length, buf))
+ put_error(3);
+ else
+ put_ok();
+ }
+ break;
+
+ case 'c': // c<addr> -- continue. <addr> is the address to resume (goto).
+ if (!(parse_number(&buf, end, &addr)
+ && expect_end(&buf, end))){
+ put_error(1);
+ }
+ else {
+ typedef void (*fptr_t)(void);
+ (*(fptr_t) addr)(); // most likely no return
+ }
+ break;
+/*
+ case 0x80:
+ {
+ unsigned char *output = binary_data; // reuse
+ size_t sizeof_output = sizeof(binary_data);
+ size_t actual_olen;
+ loader_parser(buf, end-buf,
+ output, sizeof_output, &actual_olen);
+ put_packet(output, actual_olen);
+ }
+ break;
+*/
+ default: // unknown packet type
+ put_packet(0, 0);
+ break;
+ }
+ }
+}
generated by cgit v1.2.3 (git 2.39.1) at 2024-12-28 12:51:23 +0000