payloads/libpayload/gdb/transport.c

   1 /*
   2  * Copyright 2014 Google Inc.
   3  *
   4  * This program is free software; you can redistribute it and/or
   5  * modify it under the terms of the GNU General Public License as
   6  * published by the Free Software Foundation; either version 2 of
   7  * the License, or (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but without any warranty; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  */
  14
  15 #include <endian.h>
  16 #include <gdb.h>
  17 #include <libpayload.h>
  18
  19 #define OUTPUT_OVERRUN_MSG "GDB output buffer overrun (try increasing reply.size)!\n"
  20
  21 /* MMIO word size is not standardized, but *usually* 32 (even on ARM64) */
  22 typedef u32 mmio_word_t;
  23
  24 static const int timeout_us = 100 * 1000;
  25
  26 /* Serial-specific glue code... add more transport layers here when desired. */
  27
  28 static void gdb_raw_putchar(u8 c)
  29 {
  30         serial_putchar(c);
  31 }
  32
  33 static int gdb_raw_getchar(void)
  34 {
  35         u64 start = timer_us(0);
  36
  37         while (!serial_havechar())
  38                 if (timer_us(start) > timeout_us)
  39                         return -1;
  40
  41         return serial_getchar();
  42 }
  43
  44 void gdb_transport_init(void)
  45 {
  46         console_remove_output_driver(serial_putchar);
  47 }
  48
  49 void gdb_transport_teardown(void)
  50 {
  51         serial_console_init();
  52 }
  53
  54 /* Hex digit character <-> number conversion (illegal chars undefined!). */
  55
  56 static u8 from_hex(unsigned char c)
  57 {
  58         static const s8 values[] = {
  59                 -1, 10, 11, 12, 13, 14, 15, -1,
  60                 -1, -1, -1, -1, -1, -1, -1, -1,
  61                  0,  1,  2,  3,  4,  5,  6,  7,
  62                  8,  9, -1, -1, -1, -1, -1, -1,
  63         };
  64
  65         return values[c & 0x1f];
  66 }
  67
  68 static char to_hex(u8 v)
  69 {
  70         static const char digits[] = "0123456789abcdef";
  71
  72         return digits[v & 0xf];
  73 }
  74
  75 /* Message encode/decode functions (must access whole aligned words for MMIO) */
  76
  77 void gdb_message_encode_bytes(struct gdb_message *message, const void *data,
  78                               int length)
  79 {
  80         die_if(message->used + length * 2 > message->size, OUTPUT_OVERRUN_MSG);
  81         const mmio_word_t *aligned =
  82                 (mmio_word_t *)ALIGN_DOWN((uintptr_t)data, sizeof(*aligned));
  83         mmio_word_t word = be32toh(readl(aligned++));
  84         while (length--) {
  85                 u8 byte = (word >> ((((void *)aligned - data) - 1) * 8));
  86                 message->buf[message->used++] = to_hex(byte >> 4);
  87                 message->buf[message->used++] = to_hex(byte & 0xf);
  88                 if (length && ++data == (void *)aligned)
  89                         word = be32toh(readl(aligned++));
  90         }
  91 }
  92
  93 void gdb_message_decode_bytes(const struct gdb_message *message, int offset,
  94                               void *data, int length)
  95 {
  96         die_if(offset + 2 * length > message->used, "Decode overrun in GDB "
  97                 "message: %.*s", message->used, message->buf);
  98         mmio_word_t *aligned =
  99                 (mmio_word_t *)ALIGN_DOWN((uintptr_t)data, sizeof(*aligned));
 100         int shift = ((void *)(aligned + 1) - data) * 8;
 101         mmio_word_t word = be32toh(readl(aligned)) >> shift;
 102         while (length--) {
 103                 word <<= 8;
 104                 word |= from_hex(message->buf[offset++]) << 4;
 105                 word |= from_hex(message->buf[offset++]);
 106                 if (++data - (void *)aligned == sizeof(*aligned))
 107                         writel(htobe32(word), aligned++);
 108         }
 109         if (data != (void *)aligned) {
 110                 shift = ((void *)(aligned + 1) - data) * 8;
 111                 clrsetbits_be32(aligned, ~((1 << shift) - 1), word << shift);
 112         }
 113 }
 114
 115 void gdb_message_encode_zero_bytes(struct gdb_message *message, int length)
 116 {
 117         die_if(message->used + length * 2 > message->size, OUTPUT_OVERRUN_MSG);
 118         memset(message->buf + message->used, '0', length * 2);
 119         message->used += length * 2;
 120 }
 121
 122 void gdb_message_add_string(struct gdb_message *message, const char *string)
 123 {
 124         message->used += strlcpy((char *)message->buf + message->used,
 125                              string, message->size - message->used);
 126
 127         /* Check >= instead of > to account for strlcpy's trailing '\0'. */
 128         die_if(message->used >= message->size, OUTPUT_OVERRUN_MSG);
 129 }
 130
 131 void gdb_message_encode_int(struct gdb_message *message, uintptr_t val)
 132 {
 133         int length = sizeof(uintptr_t) * 2 - __builtin_clz(val) / 4;
 134         die_if(message->used + length > message->size, OUTPUT_OVERRUN_MSG);
 135         while (length--)
 136                 message->buf[message->used++] =
 137                         to_hex((val >> length * 4) & 0xf);
 138 }
 139
 140 uintptr_t gdb_message_decode_int(const struct gdb_message *message, int offset,
 141                                  int length)
 142 {
 143         uintptr_t val = 0;
 144
 145         die_if(length > sizeof(uintptr_t) * 2, "GDB decoding invalid number: "
 146                "%.*s", message->used, message->buf);
 147
 148         while (length--) {
 149                 val <<= 4;
 150                 val |= from_hex(message->buf[offset++]);
 151         }
 152
 153         return val;
 154 }
 155
 156 /* Like strtok/strsep: writes back offset argument, returns original offset. */
 157 int gdb_message_tokenize(const struct gdb_message *message, int *offset)
 158 {
 159         int token = *offset;
 160         while (!strchr(",;:", message->buf[(*offset)++]))
 161                 die_if(*offset >= message->used, "Undelimited token in GDB "
 162                                 "message at offset %d: %.*s",
 163                                 token, message->used, message->buf);
 164         return token;
 165 }
 166
 167 /* High-level send/receive functions. */
 168
 169 void gdb_get_command(struct gdb_message *command)
 170 {
 171         enum command_state {
 172                 STATE_WAITING,
 173                 STATE_COMMAND,
 174                 STATE_CHECKSUM0,
 175                 STATE_CHECKSUM1,
 176         };
 177
 178         u8 checksum = 0;
 179         u8 running_checksum = 0;
 180         enum command_state state = STATE_WAITING;
 181
 182         while (1) {
 183                 int c = gdb_raw_getchar();
 184                 if (c < 0) {
 185                         /*
 186                          * Timeout waiting for a byte. Reset the
 187                          * state machine.
 188                          */
 189                         state = STATE_WAITING;
 190                         continue;
 191                 }
 192
 193                 switch (state) {
 194                 case STATE_WAITING:
 195                         if (c == '$') {
 196                                 running_checksum = 0;
 197                                 command->used = 0;
 198                                 state = STATE_COMMAND;
 199                         }
 200                         break;
 201                 case STATE_COMMAND:
 202                         if (c == '#') {
 203                                 state = STATE_CHECKSUM0;
 204                                 break;
 205                         }
 206                         die_if(command->used >= command->size, "GDB input buf"
 207                                "fer overrun (try increasing command.size)!\n");
 208                         command->buf[command->used++] = c;
 209                         running_checksum += c;
 210                         break;
 211                 case STATE_CHECKSUM0:
 212                         checksum = from_hex(c) << 4;
 213                         state = STATE_CHECKSUM1;
 214                         break;
 215                 case STATE_CHECKSUM1:
 216                         checksum += from_hex(c);
 217                         if (running_checksum == checksum) {
 218                                 gdb_raw_putchar('+');
 219                                 return;
 220                         } else {
 221                                 state = STATE_WAITING;
 222                                 gdb_raw_putchar('-');
 223                         }
 224                         break;
 225                 }
 226         }
 227 }
 228
 229 void gdb_send_reply(const struct gdb_message *reply)
 230 {
 231         int i;
 232         int retries = 1 * 1000 * 1000 / timeout_us;
 233         u8 checksum = 0;
 234
 235         for (i = 0; i < reply->used; i++)
 236                 checksum += reply->buf[i];
 237
 238         do {
 239                 gdb_raw_putchar('$');
 240                 for (i = 0; i < reply->used; i++)
 241                         gdb_raw_putchar(reply->buf[i]);
 242                 gdb_raw_putchar('#');
 243                 gdb_raw_putchar(to_hex(checksum >> 4));
 244                 gdb_raw_putchar(to_hex(checksum & 0xf));
 245         } while (gdb_raw_getchar() != '+' && retries--);
 246 }