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