git.gag.com Git - fw/stlink/blob - gdbserver/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 #ifdef __MINGW32__
  15 #include "mingw.h"
  16 #else
  17 #include <sys/poll.h>
  18 #endif
  19
  20 static const char hex[] = "0123456789abcdef";
  21
  22 int gdb_send_packet(int fd, char* data) {
  23         int length = strlen(data) + 5;
  24         char* packet = malloc(length); /* '$' data (hex) '#' cksum (hex) */
  25
  26         memset(packet, 0, length);
  27
  28         packet[0] = '$';
  29
  30         uint8_t cksum = 0;
  31         for(unsigned int i = 0; i < strlen(data); i++) {
  32                 packet[i + 1] = data[i];
  33                 cksum += data[i];
  34         }
  35
  36         packet[length - 4] = '#';
  37         packet[length - 3] = hex[cksum >> 4];
  38         packet[length - 2] = hex[cksum & 0xf];
  39
  40         while(1) {
  41                 if(write(fd, packet, length) != length) {
  42                         free(packet);
  43                         return -2;
  44                 }
  45
  46                 char ack;
  47                 if(read(fd, &ack, 1) != 1) {
  48                         free(packet);
  49                         return -2;
  50                 }
  51
  52                 if(ack == '+') {
  53                         free(packet);
  54                         return 0;
  55                 }
  56         }
  57 }
  58
  59 #define ALLOC_STEP 1024
  60
  61 int gdb_recv_packet(int fd, char** buffer) {
  62         unsigned packet_size = ALLOC_STEP + 1, packet_idx = 0;
  63         uint8_t cksum = 0;
  64         char recv_cksum[3] = {0};
  65         char* packet_buffer = malloc(packet_size);
  66         unsigned state;
  67
  68 start:
  69         state = 0;
  70         /*
  71          * 0: waiting $
  72          * 1: data, waiting #
  73          * 2: cksum 1
  74          * 3: cksum 2
  75          * 4: fin
  76          */
  77
  78         char c;
  79         while(state != 4) {
  80                 if(read(fd, &c, 1) != 1) {
  81                         return -2;
  82                 }
  83
  84                 switch(state) {
  85                 case 0:
  86                         if(c != '$') {
  87                                 // ignore
  88                         } else {
  89                                 state = 1;
  90                         }
  91                         break;
  92
  93                 case 1:
  94                         if(c == '#') {
  95                                 state = 2;
  96                         } else {
  97                                 packet_buffer[packet_idx++] = c;
  98                                 cksum += c;
  99
 100                                 if(packet_idx == packet_size) {
 101                                         packet_size += ALLOC_STEP;
 102                                         packet_buffer = realloc(packet_buffer, packet_size);
 103                                 }
 104                         }
 105                         break;
 106
 107                 case 2:
 108                         recv_cksum[0] = c;
 109                         state = 3;
 110                         break;
 111
 112                 case 3:
 113                         recv_cksum[1] = c;
 114                         state = 4;
 115                         break;
 116                 }
 117         }
 118
 119         uint8_t recv_cksum_int = strtoul(recv_cksum, NULL, 16);
 120         if(recv_cksum_int != cksum) {
 121                 char nack = '-';
 122                 if(write(fd, &nack, 1) != 1) {
 123                         return -2;
 124                 }
 125
 126                 goto start;
 127         } else {
 128                 char ack = '+';
 129                 if(write(fd, &ack, 1) != 1) {
 130                         return -2;
 131                 }
 132         }
 133
 134         packet_buffer[packet_idx] = 0;
 135         *buffer = packet_buffer;
 136
 137         return packet_idx;
 138 }
 139
 140 // Here we skip any characters which are not \x03, GDB interrupt.
 141 // As we use the mode with ACK, in a (very unlikely) situation of a packet
 142 // lost because of this skipping, it will be resent anyway.
 143 int gdb_check_for_interrupt(int fd) {
 144         struct pollfd pfd;
 145         pfd.fd = fd;
 146         pfd.events = POLLIN;
 147
 148         if(poll(&pfd, 1, 0) != 0) {
 149                 char c;
 150
 151                 if(read(fd, &c, 1) != 1)
 152                         return -2;
 153
 154                 if(c == '\x03') // ^C
 155                         return 1;
 156         }
 157
 158         return 0;
 159 }
 160