stoneship.org.uk Git - bootloader/blob - cli/protocol.c

   1 #include <stdio.h>
   2 #include <unistd.h>
   3 #include <stdlib.h>
   4 #include <string.h>
   5 #include <errno.h>
   6 #include <stdint.h>
   7
   8 #include "log.h"
   9 #include "serial.h"
  10 #include "protocol.h"
  11
  12 static uint8_t checksum(uint8_t * buff, int len)
  13 {
  14         int sum = 0;
  15         int i;
  16
  17         for (i=0;i<len;i++) sum += buff[i];
  18         return sum & 0xFF;
  19 }
  20
  21 void print_memory(uint16_t addr, const unsigned char * buff, int words)
  22 {
  23         int len = (words * 5) + 16;
  24         char * line = malloc(len);
  25         char * out = line;
  26         out += snprintf(out, len-strlen(line), "Mem %04X: ", addr);
  27
  28         const unsigned char *p = &buff[0];
  29         for (int i=0; i<words; i++) {
  30                 out += snprintf(out, len-strlen(line), "%02X%02X ", p[0], p[1] );
  31                 p+=2;
  32         }
  33         logd(line);
  34         free(line);
  35 }
  36
  37 void dumpbuff(const unsigned char * buff, int charlen)
  38 {
  39         int len = (charlen * 3) + 16;
  40         char * line = malloc(len);
  41         char * out = line;
  42         out += snprintf(out, len-strlen(line), "Dump: ");
  43
  44         const unsigned char *p = &buff[0];
  45         for (int i=0; i<charlen; i++) {
  46                 out += snprintf(out, len-strlen(line), "%02X ", p[0] );
  47                 p++;
  48         }
  49         logd(line);
  50         free(line);
  51 }
  52
  53 int loader_readmem(port_t * pt, uint16_t addr, uint8_t * memory, int words)
  54 {
  55         int len = (words * 2) + 4;
  56         unsigned char * buff = malloc(len);
  57
  58         /* request to read memory block */
  59         buff[0] = 'R';
  60         buff[1] = addr >> 8;
  61         buff[2] = addr & 0xFF;
  62         buff[3] = checksum(buff, 3);
  63
  64         int ret = serial_write(pt, buff, 4);
  65         if (ret <= 0) return 1;
  66
  67         bzero(buff, len);
  68
  69         /* read the response */
  70         if (serial_read(pt, buff, 1)<=0) return 1;
  71
  72         if (buff[0] == 'E') {
  73                 loge("ReadMem %04X: Loader gave error", addr);
  74                 return 2;
  75         }
  76
  77         if (buff[0] != 'R') {
  78                 loge("ReadMem %04X: Unknown response to read 0x%02X '%c'", addr, buff[0], buff[0]);
  79                 return 3;
  80         }
  81
  82         /* now read the address, and check it matches */
  83         ret = serial_read(pt, &buff[1], len-1);
  84         if (ret < 1) {
  85                 loge("ReadMem %04X: Error read response", addr);
  86                 return 4;
  87         }
  88         if (ret < len-1) {
  89                 loge("ReadMem %04X: Short read %d of %d", addr, ret, len-1);
  90                 return 5;
  91         }
  92
  93         int sum = checksum(buff, len-1);
  94         if ((sum & 0xFF) != buff[len-1]) {
  95                 loge("ReadMem %04X: Bad checksum. %02X != %02X", addr, sum & 0xFF, buff[len-1]);
  96                 return 6;
  97         }
  98
  99         uint16_t realaddr = (buff[1]<<8) | buff[2];
 100         if (realaddr != addr) {
 101                 loge("ReadMem %04X: Actual Address %04X", addr, realaddr);
 102                 return 7;
 103         }
 104         logd("ReadMem %04X: Read Successful", realaddr);
 105
 106         memcpy(memory, &buff[3], (words * 2));
 107         free(buff);
 108
 109         return 0;
 110 }
 111
 112
 113 int loader_writemem(port_t * pt, uint16_t addr, uint8_t * memory, int words)
 114 {
 115         int len = (words * 2) + 4;
 116         uint8_t * buff = malloc((words * 2) + 4);
 117
 118         /* request to read memory block */
 119         buff[0] = 'W';
 120         buff[1] = addr >> 8;
 121         buff[2] = addr & 0xFF;
 122
 123         unsigned char * p = memory;
 124         unsigned char * q = &buff[3];
 125         for (int i=0; i<words; i++) {
 126                 *q++ = *p++;
 127                 *q++ = *p++;
 128         }
 129         *q = checksum(buff, len-1) & 0xFF;
 130
 131         logd("WriteMem %04X: Sending Write request", addr);
 132         int ret = serial_write(pt, buff, len);
 133         if (ret <= 0) { free(buff); return 1; }
 134
 135         logd("WriteMem %04X: Awaiting Write confirmation", addr);
 136         /* read the response */
 137         if (serial_read(pt, buff, 1)<=0) { free(buff); return 1; }
 138
 139         if (buff[0] == 'E') {
 140                 loge("WriteMem %04X: Bootloader gave an error", addr);
 141                 free(buff);
 142                 return 2;
 143         }
 144
 145         if (buff[0] != 'W') {
 146                 loge("WriteMem %04X: unknown reponse '%c'", addr, buff[0]);
 147                 free(buff);
 148                 return 3;
 149         }
 150
 151         /* now read the address, and check it matches */
 152         ret = serial_read(pt, &buff[1], 3);
 153         if (ret < 3) {
 154                 loge("WriteMem %04X: Error reading rest of response %d of %d", addr, ret, 3);
 155                 free(buff);
 156                 return 4;
 157         }
 158
 159         uint16_t realaddr = (buff[1]<<8) | buff[2];
 160         if (realaddr != addr) {
 161                 loge("WriteMem %04X: Actual location %04X", addr, realaddr);
 162                 free(buff);
 163                 return 5;
 164         }
 165         int sum = checksum(buff, 3);
 166
 167         if ((sum & 0xFF) != buff[3]) {
 168                 loge("WriteMem %04X: Bad checksum on confirmation. %02X != %02X", addr, sum & 0xFF, buff[3]);
 169                 dumpbuff(buff, 4);
 170                 free(buff);
 171                 return 8;
 172         }
 173
 174         logd("WriteMem %04X: Write confirmed.", realaddr);
 175         free(buff);
 176
 177         return 0;
 178 }
 179
 180 /*
 181  * Connect to the bootloader
 182  * read the start of bootloader address
 183  */
 184 int loader_connect(port_t * pt, uint16_t *maxmem, uint16_t *devid)
 185 {
 186         if (pt == NULL) return 1;
 187
 188         logd("Connect: Assert break and wait for acknowledgement");
 189         serial_break(pt, 1);
 190
 191         unsigned char buff[10];
 192         int bootload = 0;
 193         int ret;
 194
 195         logi("Please reset the device to enter bootloader mode");
 196
 197         while ((ret=serial_read(pt, buff, 1))> 0) {
 198                 if (buff[0] == 06) {
 199                         break;
 200                 } else {
 201                         logd("Connect: Want ESC (0x06) got 0x%02X ret=%d", buff[0], ret);
 202                         usleep(100000);
 203                 }
 204         }
 205
 206         if (ret <= 0) {
 207                 if (ret == 0)
 208                         loge("Connect: Serial port closed");
 209                 else
 210                         loge("Connect: read error: %s", strerror(errno));
 211                 return 1;
 212         }
 213
 214         /* Next we should see 'B' 'L' */
 215         while ((ret=serial_read(pt, &buff[1], 2))> 0) {
 216                 if (buff[1] == 'B' && buff[2] == 'L') {
 217                         bootload = 1;
 218                         break;
 219                 } else {
 220                         logd("Connect: Want 'BL' got '%c%c' ret=%d", buff[1], buff[2], ret);
 221                 }
 222                 bzero(buff,3);
 223         }
 224
 225         if (ret <= 0) {
 226                 if (ret == 0)
 227                         loge("Connect: Serial port closed");
 228                 else
 229                         loge("Connect: read error: %s", strerror(errno));
 230                 return 1;
 231         }
 232
 233         /* turn break off now we are in bootloader mode */
 234         serial_break(pt, 0);
 235
 236         if (!bootload) {
 237                 serial_close(pt);
 238                 loge("Connect: Could not find bootloader");
 239                 return 1;
 240         }
 241
 242         logd("Connect: Release break. Read memory size.");
 243
 244         /* now read the bootloaders location, and thus mem size */
 245         ret=serial_read(pt, &buff[3], 5);
 246
 247         if (ret < 3) {
 248                 if (ret == 0)
 249                         loge("Connect: Serial port closed");
 250                 else
 251                         loge("Connect: read error: %s", strerror(errno));
 252                 return 1;
 253         }
 254
 255         if (buff[7] != (checksum(buff,7) & 0xFF)) {
 256                 loge("Connect: Bad checksum");
 257                 return 1;
 258         }
 259
 260         *devid = (buff[3] << 8) | buff[4];
 261         logd("Connect: Device ID: 0x%04X", *devid);
 262
 263         *maxmem = (buff[5] << 8) | buff[6];
 264         logd("Connect: Memory Size: 0x%04X", *maxmem);
 265
 266         return 0;
 267 }