git.gag.com Git - fw/stlink/blob - src/gdb-remote.c

   1 /* -*- tab-width:8 -*- */
   2
   3 /*
   4  Copyright (C)  2011 Peter Zotov <whitequark@whitequark.org>
   5  Use of this source code is governed by a BSD-style
   6  license that can be found in the LICENSE file.
   7 */
   8
   9 #include <stdio.h>
  10 #include <string.h>
  11 #include <stdlib.h>
  12 #include <unistd.h>
  13 #include <stdint.h>
  14
  15 static const char hex[] = "0123456789abcdef";
  16
  17 int gdb_send_packet(int fd, char* data) {
  18         unsigned length = strlen(data) + 5;
  19         char* packet = malloc(length); /* '$' data (hex) '#' cksum (hex) */
  20
  21         memset(packet, 0, length);
  22
  23         packet[0] = '$';
  24
  25         uint8_t cksum = 0;
  26         for(int i = 0; i < strlen(data); i++) {
  27                 packet[i + 1] = data[i];
  28                 cksum += data[i];
  29         }
  30
  31         packet[length - 4] = '#';
  32         packet[length - 3] = hex[cksum >> 4];
  33         packet[length - 2] = hex[cksum & 0xf];
  34
  35         while(1) {
  36                 if(write(fd, packet, length) != length) {
  37                         free(packet);
  38                         return -2;
  39                 }
  40
  41                 char ack;
  42                 if(read(fd, &ack, 1) != 1) {
  43                         free(packet);
  44                         return -2;
  45                 }
  46
  47                 if(ack == '+') {
  48                         free(packet);
  49                         return 0;
  50                 }
  51         }
  52 }
  53
  54 #define ALLOC_STEP 1024
  55
  56 int gdb_recv_packet(int fd, char** buffer) {
  57         unsigned packet_size = ALLOC_STEP + 1, packet_idx = 0;
  58         uint8_t cksum = 0;
  59         char recv_cksum[3] = {0};
  60         char* packet_buffer = malloc(packet_size);
  61         unsigned state;
  62
  63 start:
  64         state = 0;
  65         /*
  66          * 0: waiting $
  67          * 1: data, waiting #
  68          * 2: cksum 1
  69          * 3: cksum 2
  70          * 4: fin
  71          */
  72
  73         char c;
  74         while(state != 4) {
  75                 if(read(fd, &c, 1) != 1) {
  76                         return -2;
  77                 }
  78
  79                 switch(state) {
  80                 case 0:
  81                         if(c != '$') {
  82                                 // ignore
  83                         } else {
  84                                 state = 1;
  85                         }
  86                         break;
  87
  88                 case 1:
  89                         if(c == '#') {
  90                                 state = 2;
  91                         } else {
  92                                 packet_buffer[packet_idx++] = c;
  93                                 cksum += c;
  94
  95                                 if(packet_idx == packet_size) {
  96                                         packet_size += ALLOC_STEP;
  97                                         packet_buffer = realloc(packet_buffer, packet_size);
  98                                 }
  99                         }
 100                         break;
 101
 102                 case 2:
 103                         recv_cksum[0] = c;
 104                         state = 3;
 105                         break;
 106
 107                 case 3:
 108                         recv_cksum[1] = c;
 109                         state = 4;
 110                         break;
 111                 }
 112         }
 113
 114         uint8_t recv_cksum_int = strtoul(recv_cksum, NULL, 16);
 115         if(recv_cksum_int != cksum) {
 116                 char nack = '-';
 117                 if(write(fd, &nack, 1) != 1) {
 118                         return -2;
 119                 }
 120
 121                 goto start;
 122         } else {
 123                 char ack = '+';
 124                 if(write(fd, &ack, 1) != 1) {
 125                         return -2;
 126                 }
 127         }
 128
 129         packet_buffer[packet_idx] = 0;
 130         *buffer = packet_buffer;
 131
 132         return packet_size;
 133 }
 134
 135 int gdb_wait_for_interrupt(int fd) {
 136         char c;
 137         while(1) {
 138                 if(read(fd, &c, 1) != 1)
 139                         return -2;
 140
 141                 if(c == '\x03') // ^C
 142                         return 0;
 143         }
 144 }
 145