gdb/proto.c

   1 /* proto.c
   2  * GDB remote serial protocol implementation
   3  * NetWatch system management mode administration console
   4  *
   5  * Copyright 2009, Google Inc.
   6  * All rights reserved.
   7  *
   8  * Redistribution and use in source and binary forms, with or without
   9  * modification, are permitted provided that the following conditions are
  10  * met:
  11  *
  12  *     * Redistributions of source code must retain the above copyright
  13  * notice, this list of conditions and the following disclaimer.
  14  *     * Redistributions in binary form must reproduce the above
  15  * copyright notice, this list of conditions and the following disclaimer
  16  * in the documentation and/or other materials provided with the
  17  * distribution.
  18  *     * Neither the name of Google Inc. nor the names of its
  19  * contributors may be used to endorse or promote products derived from
  20  * this software without specific prior written permission.
  21  *
  22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  25  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  26  * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  27  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  28  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  29  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  30  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  31  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  32  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  33  */
  34
  35 #define GDB_BUF_SIZE            1024
  36
  37 #include <minilib.h>
  38 #include <demap.h>
  39 #include <tables.h>
  40
  41 #include <lwip/tcp.h>
  42
  43 #include "stub.h"
  44
  45 #define GDB_PORT        2159
  46
  47 enum fsm_result { NEEDMORE, OK, FAIL };
  48
  49 static struct tcp_pcb * last_conn = NULL;
  50
  51 void enhexificate (void * ibuf, char * obuf, int len) {
  52         unsigned char * cibuf = ibuf;
  53         while (len > 0) {
  54                 btohex(obuf, *(cibuf++));
  55                 obuf += 2;
  56                 len--;
  57         }
  58
  59         *obuf = '\0';
  60 }
  61
  62 void rle (char * buf) {
  63         char * inptr = buf;
  64         char * outptr = buf;
  65         int lastchar = buf[0];
  66         int repcount = 0;
  67
  68         do {
  69                 if (*inptr == lastchar && repcount < 97) {
  70                         repcount += 1;
  71                 } else {
  72                         *(outptr++) = lastchar;
  73
  74                         if (repcount == 2)
  75                                 *(outptr++) = lastchar;
  76
  77                         while (repcount == 6 || repcount == 7) {
  78                                 *(outptr++) = lastchar;
  79                                 repcount--;
  80                         }
  81
  82                         if (repcount > 2) {
  83                                 *(outptr++) = '*';
  84                                 *(outptr++) = repcount + 29;
  85                         }
  86
  87                         repcount = 1;
  88                         lastchar = *inptr;
  89                 }
  90
  91                 inptr++;
  92         } while (lastchar);
  93
  94         *(outptr) = 0;
  95 }
  96
  97
  98 struct gdb_state {
  99         char data[128];
 100         int writepos;
 101         int readpos;
 102 };
 103
 104
 105 static void close_conn(struct tcp_pcb *pcb, struct gdb_state *state) {
 106         outputf("close_conn: bailing");
 107         set_run_mode(RM_UNENCUMBERED);
 108         last_conn = NULL;
 109         tcp_arg(pcb, NULL);
 110         tcp_sent(pcb, NULL);
 111         tcp_recv(pcb, NULL);
 112         mem_free(state);
 113         tcp_close(pcb);
 114         outputf("close_conn: done");
 115 }
 116
 117 static int dehexbyte (char * p) {
 118         int v0, v1;
 119         v0 = dehexit (p[0]);
 120         v1 = dehexit (p[1]);
 121         if (v0 < 0 || v1 < 0) return -1;
 122         return v0 << 4 | v1;
 123 }
 124
 125 static void finish_and_send(struct tcp_pcb *pcb, char * output_buf, int datalength) {
 126         char * p;
 127         unsigned char checksum = 0;
 128
 129         output_buf[0] = '+';
 130         output_buf[1] = '$';
 131
 132         for (p = output_buf + 2; p < output_buf + datalength + 2; p++)
 133                 checksum += *(unsigned char *)p;
 134
 135         output_buf[datalength + 2] = '#';
 136
 137         btohex(output_buf + datalength + 3, checksum);
 138         tcp_write(pcb, output_buf, datalength + 5, TCP_WRITE_FLAG_COPY);
 139 }
 140
 141 void send_stop_packet() {
 142         if (!last_conn) return;
 143         tcp_write(last_conn, "$T05thread:01;#07", 17, 0);
 144 }
 145
 146 static enum fsm_result recv_fsm(struct tcp_pcb *pcb, struct gdb_state *state) {
 147         char * endp;
 148         char * p;
 149         unsigned char checksum = 0;
 150         int i;
 151         uint64_t addr;
 152         uint64_t length;
 153
 154         char output_buf[256];
 155
 156         /* Make sure we have at least 4 bytes (the size of the smallest legal
 157            packet), and that the packet does in fact start with $. */
 158
 159         if ((state->data[0] == '+') || (state->data[0] == '-')) {
 160                 outputf("GDB: chomp");
 161                 state->readpos++;
 162                 return OK;
 163         }
 164
 165         if (state->writepos < 4) return NEEDMORE;
 166
 167         if (state->data[0] != '$') return FAIL;
 168
 169         /* Find the end; make sure there's room for the checksum after. */
 170
 171         endp = memchr(state->data, '#', state->writepos);
 172
 173         if ((!endp) || (endp - state->data > state->writepos - 3))
 174                 return NEEDMORE;
 175
 176         /* Checksum. */
 177
 178         for (p = state->data + 1; p < endp; p++)
 179                 checksum += *(unsigned char *)p;
 180
 181         if (checksum != dehexbyte(endp + 1))
 182         {
 183                 outputf("GDB: bad checksum: %d vs %d", checksum , dehexbyte(endp + 1));
 184                 return FAIL;
 185         }
 186
 187         /* Null-terminate, for processing convenience */
 188         *endp = '\0';
 189
 190         outputf("GDB: Got \'%s\'", state->data + 1);
 191
 192         /* OK, process the packet */
 193
 194         switch (state->data[1]) {
 195         case '?':
 196                 tcp_write(pcb, "+$T05thread:01;#07", 18, 0);
 197                 break;
 198         case 'g':
 199                 read_registers_32(output_buf + 2);
 200                 finish_and_send(pcb, output_buf, 128);
 201                 break;
 202
 203         case 'm':
 204                 /* Parse the address */
 205                 p = memchr(state->data, ',', endp - state->data);
 206                 if (!p) return FAIL;
 207                 *p = '\0';
 208                 addr = 0;
 209                 length = 0;
 210
 211                 if (!dehexstring(&addr, state->data + 2, -1)) return FAIL;
 212                 if (!dehexstring(&length, p + 1, -1)) return FAIL;
 213
 214                 outputf("GDB: read %d from %d", (uint32_t)length, (uint32_t)addr);
 215
 216                 if (length > 120) length = 120;
 217
 218                 for (i = 0; i < length; i++) {
 219                         p = demap(addr++);
 220                         if (!p) break;
 221                         btohex(output_buf + 2 + (2*i), *(char *)p);
 222                 }
 223
 224                 finish_and_send(pcb, output_buf, 2*i);
 225                 break;
 226
 227         case 's':
 228                 /* Step. */
 229                 set_run_mode(RM_STEPPING);
 230                 tcp_write(pcb, "+", 1, 0);
 231                 break;
 232
 233         default:
 234                 tcp_write(pcb, "+$#00", 5, 0);
 235                 break;
 236         }
 237
 238         state->readpos += (endp - state->data) + 3;
 239
 240         return OK;
 241 }
 242
 243 err_t
 244 gdb_recv(void *arg, struct tcp_pcb *pcb, struct pbuf *p, err_t err) {
 245         struct gdb_state *state = arg;
 246         uint16_t copylen;
 247
 248         if (err != ERR_OK) {
 249                 outputf("GDB: recv err %d", err);
 250                 /* FIXME do something better here? */
 251                 return ERR_OK;
 252         }
 253
 254         if (p == NULL) {
 255                 outputf("GDB: Connection closed");
 256                 close_conn(pcb, state);
 257                 return ERR_OK;
 258         }
 259
 260         if (p->tot_len > (GDB_BUF_SIZE - state->writepos)) {
 261                 /* Overflow! */
 262                 outputf("GDB: Overflow!");
 263                 close_conn(pcb, state);
 264                 return ERR_OK;
 265         }
 266
 267         copylen = pbuf_copy_partial(p, state->data + state->writepos, p->tot_len, 0);
 268
 269         outputf("GDB: Processing %d, wp %d, cp %d", p->tot_len, state->writepos, copylen);
 270
 271         state->writepos += p->tot_len;
 272
 273         tcp_recved(pcb, p->tot_len);
 274         pbuf_free(p);
 275
 276         while (1) {
 277                 switch (recv_fsm(pcb, state)) {
 278                 case NEEDMORE:
 279                         outputf("GDB FSM: blocking");
 280                         goto doneprocessing;
 281
 282                 case OK:
 283                         if (state->readpos == state->writepos) {
 284                                 state->readpos = 0;
 285                                 state->writepos = 0;
 286                                 goto doneprocessing;
 287                         } else {
 288                                 memmove(state->data,
 289                                         state->data + state->readpos,
 290                                         state->writepos - state->readpos);
 291                                 state->writepos -= state->readpos;
 292                                 state->readpos = 0;
 293                         }
 294                         break;
 295                 case FAIL:
 296                         /* Shit */
 297                         outputf("GDB: Protocol error");
 298                         close_conn(pcb, state);
 299                         return ERR_OK;
 300                 }
 301         }
 302
 303 doneprocessing:
 304         return ERR_OK;
 305 }
 306
 307 static err_t gdb_sent(void *arg, struct tcp_pcb *pcb, uint16_t len) {
 308 /*
 309         struct gdb_state *state = arg;
 310         send_fsm(pcb, state);
 311 */
 312         return ERR_OK;
 313 }
 314
 315 static err_t gdb_poll(void *arg, struct tcp_pcb *pcb) {
 316         /* Nothing? */
 317         return ERR_OK;
 318 }
 319
 320
 321 static err_t gdb_accept(void *arg, struct tcp_pcb *pcb, err_t err) {
 322         struct gdb_state *state;
 323
 324         LWIP_UNUSED_ARG(arg);
 325         LWIP_UNUSED_ARG(err);
 326
 327         outputf("GDB: accept");
 328
 329         last_conn = pcb;
 330
 331         set_run_mode(RM_STOPPED);
 332
 333         state = (struct gdb_state *)mem_malloc(sizeof(struct gdb_state));
 334
 335         if (!state)
 336         {
 337                 outputf("gdb_accept: out of memory\n");
 338                 return ERR_MEM;
 339         }
 340
 341         memset(state, 0, sizeof(struct gdb_state));
 342
 343         tcp_arg(pcb, state);
 344         tcp_recv(pcb, gdb_recv);
 345         tcp_sent(pcb, gdb_sent);
 346         tcp_poll(pcb, gdb_poll, 1);
 347 /*
 348         tcp_err(pcb, gdb_err);
 349 */
 350         return ERR_OK;
 351 }
 352
 353
 354 void gdb_init() {
 355         struct tcp_pcb *pcb;
 356         pcb = tcp_new();
 357         tcp_bind(pcb, IP_ADDR_ANY, GDB_PORT);
 358         pcb = tcp_listen(pcb);
 359         tcp_accept(pcb, gdb_accept);
 360 }
 361
 362 PROTOCOL(gdb_init);