247 lines
6.3 KiB
C
247 lines
6.3 KiB
C
/*
|
|
* Copyright 2014 Google Inc.
|
|
*
|
|
* 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.
|
|
*/
|
|
|
|
#include <endian.h>
|
|
#include <gdb.h>
|
|
#include <libpayload.h>
|
|
|
|
/* MMIO word size is not standardized, but *usually* 32 (even on ARM64) */
|
|
typedef u32 mmio_word_t;
|
|
|
|
static const int timeout_us = 100 * 1000;
|
|
static const char output_overrun[] = "GDB output buffer overrun (try "
|
|
"increasing reply.size)!\n";
|
|
|
|
/* Serial-specific glue code... add more transport layers here when desired. */
|
|
|
|
static void gdb_raw_putchar(u8 c)
|
|
{
|
|
serial_putchar(c);
|
|
}
|
|
|
|
static int gdb_raw_getchar(void)
|
|
{
|
|
u64 start = timer_us(0);
|
|
|
|
while (!serial_havechar())
|
|
if (timer_us(start) > timeout_us)
|
|
return -1;
|
|
|
|
return serial_getchar();
|
|
}
|
|
|
|
void gdb_transport_init(void)
|
|
{
|
|
console_remove_output_driver(serial_putchar);
|
|
}
|
|
|
|
void gdb_transport_teardown(void)
|
|
{
|
|
serial_console_init();
|
|
}
|
|
|
|
/* Hex digit character <-> number conversion (illegal chars undefined!). */
|
|
|
|
static u8 from_hex(unsigned char c)
|
|
{
|
|
static const s8 values[] = {
|
|
-1, 10, 11, 12, 13, 14, 15, -1,
|
|
-1, -1, -1, -1, -1, -1, -1, -1,
|
|
0, 1, 2, 3, 4, 5, 6, 7,
|
|
8, 9, -1, -1, -1, -1, -1, -1,
|
|
};
|
|
|
|
return values[c & 0x1f];
|
|
}
|
|
|
|
static char to_hex(u8 v)
|
|
{
|
|
static const char digits[] = "0123456789abcdef";
|
|
|
|
return digits[v & 0xf];
|
|
}
|
|
|
|
/* Message encode/decode functions (must access whole aligned words for MMIO) */
|
|
|
|
void gdb_message_encode_bytes(struct gdb_message *message, const void *data,
|
|
int length)
|
|
{
|
|
die_if(message->used + length * 2 > message->size, output_overrun);
|
|
const mmio_word_t *aligned =
|
|
(mmio_word_t *)ALIGN_DOWN((uintptr_t)data, sizeof(*aligned));
|
|
mmio_word_t word = be32toh(readl(aligned++));
|
|
while (length--) {
|
|
u8 byte = (word >> ((((void *)aligned - data) - 1) * 8));
|
|
message->buf[message->used++] = to_hex(byte >> 4);
|
|
message->buf[message->used++] = to_hex(byte & 0xf);
|
|
if (length && ++data == (void *)aligned)
|
|
word = be32toh(readl(aligned++));
|
|
}
|
|
}
|
|
|
|
void gdb_message_decode_bytes(const struct gdb_message *message, int offset,
|
|
void *data, int length)
|
|
{
|
|
die_if(offset + 2 * length > message->used, "Decode overrun in GDB "
|
|
"message: %.*s", message->used, message->buf);
|
|
mmio_word_t *aligned =
|
|
(mmio_word_t *)ALIGN_DOWN((uintptr_t)data, sizeof(*aligned));
|
|
int shift = ((void *)(aligned + 1) - data) * 8;
|
|
mmio_word_t word = be32toh(readl(aligned)) >> shift;
|
|
while (length--) {
|
|
word <<= 8;
|
|
word |= from_hex(message->buf[offset++]) << 4;
|
|
word |= from_hex(message->buf[offset++]);
|
|
if (++data - (void *)aligned == sizeof(*aligned))
|
|
writel(htobe32(word), aligned++);
|
|
}
|
|
if (data != (void *)aligned) {
|
|
shift = ((void *)(aligned + 1) - data) * 8;
|
|
clrsetbits_be32(aligned, ~((1 << shift) - 1), word << shift);
|
|
}
|
|
}
|
|
|
|
void gdb_message_encode_zero_bytes(struct gdb_message *message, int length)
|
|
{
|
|
die_if(message->used + length * 2 > message->size, output_overrun);
|
|
memset(message->buf + message->used, '0', length * 2);
|
|
message->used += length * 2;
|
|
}
|
|
|
|
void gdb_message_add_string(struct gdb_message *message, const char *string)
|
|
{
|
|
message->used += strlcpy((char *)message->buf + message->used,
|
|
string, message->size - message->used);
|
|
|
|
/* Check >= instead of > to account for strlcpy's trailing '\0'. */
|
|
die_if(message->used >= message->size, output_overrun);
|
|
}
|
|
|
|
void gdb_message_encode_int(struct gdb_message *message, uintptr_t val)
|
|
{
|
|
int length = sizeof(uintptr_t) * 2 - __builtin_clz(val) / 4;
|
|
die_if(message->used + length > message->size, output_overrun);
|
|
while (length--)
|
|
message->buf[message->used++] =
|
|
to_hex((val >> length * 4) & 0xf);
|
|
}
|
|
|
|
uintptr_t gdb_message_decode_int(const struct gdb_message *message, int offset,
|
|
int length)
|
|
{
|
|
uintptr_t val = 0;
|
|
|
|
die_if(length > sizeof(uintptr_t) * 2, "GDB decoding invalid number: "
|
|
"%.*s", message->used, message->buf);
|
|
|
|
while (length--) {
|
|
val <<= 4;
|
|
val |= from_hex(message->buf[offset++]);
|
|
}
|
|
|
|
return val;
|
|
}
|
|
|
|
/* Like strtok/strsep: writes back offset argument, returns original offset. */
|
|
int gdb_message_tokenize(const struct gdb_message *message, int *offset)
|
|
{
|
|
int token = *offset;
|
|
while (!strchr(",;:", message->buf[(*offset)++]))
|
|
die_if(*offset >= message->used, "Undelimited token in GDB "
|
|
"message at offset %d: %.*s",
|
|
token, message->used, message->buf);
|
|
return token;
|
|
}
|
|
|
|
/* High-level send/receive functions. */
|
|
|
|
void gdb_get_command(struct gdb_message *command)
|
|
{
|
|
enum command_state {
|
|
STATE_WAITING,
|
|
STATE_COMMAND,
|
|
STATE_CHECKSUM0,
|
|
STATE_CHECKSUM1,
|
|
};
|
|
|
|
u8 checksum = 0;
|
|
u8 running_checksum = 0;
|
|
enum command_state state = STATE_WAITING;
|
|
|
|
while (1) {
|
|
int c = gdb_raw_getchar();
|
|
if (c < 0) {
|
|
/*
|
|
* Timeout waiting for a byte. Reset the
|
|
* state machine.
|
|
*/
|
|
state = STATE_WAITING;
|
|
continue;
|
|
}
|
|
|
|
switch (state) {
|
|
case STATE_WAITING:
|
|
if (c == '$') {
|
|
running_checksum = 0;
|
|
command->used = 0;
|
|
state = STATE_COMMAND;
|
|
}
|
|
break;
|
|
case STATE_COMMAND:
|
|
if (c == '#') {
|
|
state = STATE_CHECKSUM0;
|
|
break;
|
|
}
|
|
die_if(command->used >= command->size, "GDB input buf"
|
|
"fer overrun (try increasing command.size)!\n");
|
|
command->buf[command->used++] = c;
|
|
running_checksum += c;
|
|
break;
|
|
case STATE_CHECKSUM0:
|
|
checksum = from_hex(c) << 4;
|
|
state = STATE_CHECKSUM1;
|
|
break;
|
|
case STATE_CHECKSUM1:
|
|
checksum += from_hex(c);
|
|
if (running_checksum == checksum) {
|
|
gdb_raw_putchar('+');
|
|
return;
|
|
} else {
|
|
state = STATE_WAITING;
|
|
gdb_raw_putchar('-');
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
void gdb_send_reply(const struct gdb_message *reply)
|
|
{
|
|
int i;
|
|
int retries = 1 * 1000 * 1000 / timeout_us;
|
|
u8 checksum = 0;
|
|
|
|
for (i = 0; i < reply->used; i++)
|
|
checksum += reply->buf[i];
|
|
|
|
do {
|
|
gdb_raw_putchar('$');
|
|
for (i = 0; i < reply->used; i++)
|
|
gdb_raw_putchar(reply->buf[i]);
|
|
gdb_raw_putchar('#');
|
|
gdb_raw_putchar(to_hex(checksum >> 4));
|
|
gdb_raw_putchar(to_hex(checksum & 0xf));
|
|
} while (gdb_raw_getchar() != '+' && retries--);
|
|
}
|