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

   1 #include <stdio.h>
   2 #include <string.h>
   3 #include <errno.h>
   4 #include <stdint.h>
   5 #include <stdlib.h>
   6
   7 #include "log.h"
   8 #include "memory.h"
   9 #include "serial.h"
  10 #include "devices.h"
  11 #include "protocol.h"
  12
  13 #define MIN(a,b) ((a)<(b)?(a):(b))
  14
  15 /* return 0 for end of file. -1 for error */
  16 int parse_ihex16(const char *line, uint8_t *bytes, int *addr, int *code)
  17 {
  18         unsigned int sum, len, cksum;
  19         unsigned int laddr, lcode;
  20         int i;
  21         const char *p;
  22
  23         if (line[0] != ':') return -1;
  24         if (strlen(line) < 11) return -1;
  25         p = &line[1];
  26         if (!sscanf(p, "%02x", &len)) return -1;
  27         p+=2;
  28         if (strlen(line) < (11 + (len * 2))) return -1;
  29         if (!sscanf(p, "%04x", &laddr)) return -1;
  30         p += 4;
  31         *addr = laddr; // little endian address record
  32         if (!sscanf(p, "%02x", &lcode)) return -1;
  33         p+=2;
  34         *code = lcode & 0xFF;
  35
  36         /* end of file record */
  37         if (*code == 1) return 0;
  38
  39         sum = (len & 0xFF) + ((*addr >> 8) & 0xFF) + (*addr & 0xFF) + (*code & 0xFF);
  40
  41         i = 0;
  42         while (i < len) {
  43                 unsigned int byte;
  44                 /* files are little-endian */
  45                 if (!sscanf(p, "%02x", &byte)) return -1;
  46                 bytes[i+1] = byte & 0xFF;
  47                 sum += byte & 0xFF;
  48                 p += 2;
  49
  50                 if (!sscanf(p, "%02x", &byte)) return -1;
  51                 bytes[i] = byte & 0xFF;
  52                 sum += byte & 0xFF;
  53                 i += 2;
  54                 p += 2;
  55         }
  56         if (!sscanf(p, "%02x", &cksum)) return -1;
  57         if (  ((sum & 0xFF) + (cksum & 0xFF)) & 0xFF ) return -1;
  58         return len;
  59 }
  60
  61 static void mem_add(mem_t **head, uint32_t start, uint8_t * bytes, int len)
  62 {
  63         /* look for an existing block this overlaps/adjoins */
  64         mem_t * block = *head;
  65         uint32_t end = start + len;
  66
  67         for(block = *head; block!=NULL; block=block->next) {
  68                 int bend = block->start + block->len;
  69                 /* before and not touching */
  70                 if (start < block->start && end+1 < block->start) continue;
  71                 /* after and not touching */
  72                 if (start > bend+1) continue;
  73
  74                 /* therefore it at least touches */
  75
  76                 /* starts before or on old block */
  77                 if (start <= block->start) {
  78                         int pre=0, lap=0, post=0;
  79                         pre = block->start - start; // bytes before the old block
  80                         lap = len - pre;        // bytes overlapping, from start
  81                         if (lap > block->len) {
  82                                 post = lap - block->len;
  83                                 lap = block->len;
  84                         }
  85                         int newsize = block->len + pre + post;
  86                         uint8_t * newbytes = malloc(newsize);
  87                         memcpy(&newbytes[pre], block->bytes, block->len);
  88                         memcpy(newbytes, bytes, len);
  89                         free(block->bytes);
  90                         block->bytes = newbytes;
  91                         block->len = newsize;
  92                         block->start -= pre;
  93                 } else {
  94                         /* starts part way down / at end */
  95                         int pre=0, /*lap=0, */ post=0;
  96                         pre = start - block->start; // gap from start
  97                         //lap = MIN(block->len - pre, len);
  98                         if (end > bend) post = end - bend;
  99                         int newsize = block->len + post;
 100                         uint8_t * newbytes = malloc(newsize);
 101                         memcpy(newbytes, block->bytes, block->len);
 102                         memcpy(&newbytes[pre], bytes, len);
 103                         block->bytes = newbytes;
 104                         block->len = newsize;
 105                 }
 106                 return;
 107         }
 108
 109         block = calloc(1, sizeof(mem_t));
 110         block->start = start;
 111         block->len = len;
 112         block->bytes = malloc(len);
 113         memcpy(block->bytes, bytes, len);
 114         block->next = *head;
 115         *head = block;
 116 }
 117
 118 mem_t * load_ihex(FILE *in)
 119 {
 120         uint32_t addrh = 0;
 121
 122         char buff[1024];
 123
 124         mem_t * ram = NULL;
 125
 126         while (!feof(in) && fgets(buff, sizeof(buff), in)!=NULL) {
 127                 if (buff[0] == '#') continue;
 128
 129                 unsigned char bytes[80];
 130                 int len, addr, code;
 131
 132                 if ((len=parse_ihex16(buff, bytes, &addr, &code)) <= 0) {
 133                         if (len < 0) loge("LoadIHEX: Bad line: %s\n", buff);
 134                         continue;
 135                 }
 136
 137                 if (code == 4) {
 138                         addrh = ((bytes[0] << 8) | bytes[1]) << 16;
 139                         logd("LoadIHEX: Setting high addr 0x%02X%02X", bytes[0], bytes[1]);
 140                 } else
 141                 if (code == 1) {
 142                         /* end of file marker */
 143                         break;
 144                 } else
 145                 if (code == 0) {
 146                         /* normal code block */
 147                         uint32_t fulladdr = addrh | addr;
 148                         mem_add(&ram, fulladdr, bytes, len);
 149                 }
 150         }
 151         return ram;
 152 }
 153
 154 /* summary of the memory blocks in this list */
 155 void list_mem(const mem_t * head)
 156 {
 157         const mem_t *p = head;
 158         while (p!=NULL) {
 159                 logd("%6X : %d bytes", p->start, p->len);
 160                 p = p->next;
 161         }
 162 }
 163
 164 /* check that the program will fit */
 165 int validate_mem(mem_t * head, uint16_t maxmem)
 166 {
 167         if (head == NULL) {
 168                 loge("MemValidate: No program!");
 169                 return 1;
 170         }
 171
 172         mem_t * p;
 173         int used = 0;
 174
 175         /* test that all the memory blocks will fit */
 176         for (p=head; p!=NULL; p=p->next) {
 177                 /* just ignore config bytes, we cant write them anyway */
 178                 if (p->start / 2 >= 0x8000) continue;
 179
 180                 used += p->len / 2;
 181
 182                 if (p->start / 2 >= maxmem || (p->start+p->len)/2 >= maxmem) {
 183                         loge("MemValidate: Program too large, overlaps bootloader.");
 184                         return 1;
 185                 }
 186         }
 187
 188         logd("MemValidate: Used %d of %d leaving %d free.", used, maxmem, maxmem-used);
 189
 190         return 0;
 191 }
 192
 193 /* reorganise bytes to work with the boot loader */
 194 int makesafe_mem(mem_t **head, uint16_t maxmem)
 195 {
 196         /* it must fit safely first */
 197         if (validate_mem(*head, maxmem)) return 1;
 198
 199         mem_t * new = malloc(sizeof(mem_t));
 200         new->start = maxmem * 2;
 201         new->len = 8;
 202         new->bytes = malloc(8);
 203         memset(new->bytes, 255, 8);
 204
 205         int found = 0;
 206         /* find the code that goes in words 0-3 and move it */
 207         mem_t * p = NULL;
 208         for (p = *head; p!=NULL; p=p->next) {
 209                 if (p->start >= 0 && p->start <= 7) {
 210                         int pre = p->start;
 211                         int lap = MIN(8-pre, p->len);
 212                         memcpy(&new->bytes[pre], p->bytes, lap);
 213                         p->start += lap;
 214                         p->len -= lap;
 215                         memmove(p->bytes, &p->bytes[lap], p->len);
 216                         found++;
 217                 }
 218         }
 219
 220         if (!found) {
 221                 loge("MakeSafeMem: Could not find start vector");
 222                 free(new);
 223                 return 1;
 224         }
 225
 226         /* check that there is something sane in here
 227          * Warning: this is very enhanced-midrange dependant */
 228         found = 0;
 229         uint8_t * m = new->bytes;
 230         uint16_t lath = 0;
 231         for (int i=0; i<4; i++) {
 232                 uint16_t in = (m[0] << 8) | m[1];
 233                 if ((in & 0x3F80) == 0x3180) { // MOVLP
 234                         lath = in & 0x007F;
 235                         logd("MakeSafeMem: 0x%04X MOVLP 0x%02X", i, lath);
 236                 }else
 237                 if ((in & 0x3800) == 0x2800) { // GOTO
 238                         uint16_t addr = in & 0x07FF;
 239                         addr |= lath << 8;
 240                         logd("MakeSafeMem: 0x%04X GOTO 0x%02X", i, addr);
 241
 242                         if ( addr < 4) {
 243                                 // an absolute goto within the reset vector
 244                                 // this is silly and wrong, replace with NOP
 245                                 logd("MakeSafeMem: Rewriting bad 'GOTO 0x%02X' to NOP", addr);
 246                                 m[0] = 0;
 247                                 m[1] = 0;
 248                         } else {
 249                                 // there is a sane GOTO, hurrah
 250                                 found = 1;
 251                         }
 252                 }
 253                 m += 2;
 254         }
 255
 256         if (!found) {
 257                 loge("Start Vector did not contain a GOTO");
 258                 free(new);
 259                 return 1;
 260         }
 261
 262         new->next = *head;
 263         *head = new;
 264
 265         return 0;
 266 }