+#include <stdio.h>
+#include <string.h>
+#include <errno.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+#include "log.h"
+#include "memory.h"
+#include "serial.h"
+#include "devices.h"
+#include "protocol.h"
+
+#define MIN(a,b) ((a)<(b)?(a):(b))
+
+/* return 0 for end of file. -1 for error */
+int parse_ihex16(const char *line, uint8_t *bytes, int *addr, int *code)
+{
+ unsigned int sum, len, cksum;
+ unsigned int laddr, lcode;
+ int i;
+ const char *p;
+
+ if (line[0] != ':') return -1;
+ if (strlen(line) < 11) return -1;
+ p = &line[1];
+ if (!sscanf(p, "%02x", &len)) return -1;
+ p+=2;
+ if (strlen(line) < (11 + (len * 2))) return -1;
+ if (!sscanf(p, "%04x", &laddr)) return -1;
+ p += 4;
+ *addr = laddr; // little endian address record
+ if (!sscanf(p, "%02x", &lcode)) return -1;
+ p+=2;
+ *code = lcode & 0xFF;
+
+ /* end of file record */
+ if (*code == 1) return 0;
+
+ sum = (len & 0xFF) + ((*addr >> 8) & 0xFF) + (*addr & 0xFF) + (*code & 0xFF);
+
+ i = 0;
+ while (i < len) {
+ unsigned int byte;
+ /* files are little-endian */
+ if (!sscanf(p, "%02x", &byte)) return -1;
+ bytes[i+1] = byte & 0xFF;
+ sum += byte & 0xFF;
+ p += 2;
+
+ if (!sscanf(p, "%02x", &byte)) return -1;
+ bytes[i] = byte & 0xFF;
+ sum += byte & 0xFF;
+ i += 2;
+ p += 2;
+ }
+ if (!sscanf(p, "%02x", &cksum)) return -1;
+ if ( ((sum & 0xFF) + (cksum & 0xFF)) & 0xFF ) return -1;
+ return len;
+}
+
+static void mem_add(mem_t **head, uint32_t start, uint8_t * bytes, int len)
+{
+ /* look for an existing block this overlaps/adjoins */
+ mem_t * block = *head;
+ uint32_t end = start + len;
+
+ for(block = *head; block!=NULL; block=block->next) {
+ int bend = block->start + block->len;
+ /* before and not touching */
+ if (start < block->start && end+1 < block->start) continue;
+ /* after and not touching */
+ if (start > bend+1) continue;
+
+ /* therefore it at least touches */
+
+ /* starts before or on old block */
+ if (start <= block->start) {
+ int pre=0, lap=0, post=0;
+ pre = block->start - start; // bytes before the old block
+ lap = len - pre; // bytes overlapping, from start
+ if (lap > block->len) {
+ post = lap - block->len;
+ lap = block->len;
+ }
+ int newsize = block->len + pre + post;
+ uint8_t * newbytes = malloc(newsize);
+ memcpy(&newbytes[pre], block->bytes, block->len);
+ memcpy(newbytes, bytes, len);
+ free(block->bytes);
+ block->bytes = newbytes;
+ block->len = newsize;
+ block->start -= pre;
+ } else {
+ /* starts part way down / at end */
+ int pre=0, /*lap=0, */ post=0;
+ pre = start - block->start; // gap from start
+ //lap = MIN(block->len - pre, len);
+ if (end > bend) post = end - bend;
+ int newsize = block->len + post;
+ uint8_t * newbytes = malloc(newsize);
+ memcpy(newbytes, block->bytes, block->len);
+ memcpy(&newbytes[pre], bytes, len);
+ block->bytes = newbytes;
+ block->len = newsize;
+ }
+ return;
+ }
+
+ block = calloc(1, sizeof(mem_t));
+ block->start = start;
+ block->len = len;
+ block->bytes = malloc(len);
+ memcpy(block->bytes, bytes, len);
+ block->next = *head;
+ *head = block;
+}
+
+mem_t * load_ihex(FILE *in)
+{
+ uint32_t addrh = 0;
+
+ char buff[1024];
+
+ mem_t * ram = NULL;
+
+ while (!feof(in) && fgets(buff, sizeof(buff), in)!=NULL) {
+ if (buff[0] == '#') continue;
+
+ unsigned char bytes[80];
+ int len, addr, code;
+
+ if ((len=parse_ihex16(buff, bytes, &addr, &code)) <= 0) {
+ if (len < 0) loge("LoadIHEX: Bad line: %s\n", buff);
+ continue;
+ }
+
+ if (code == 4) {
+ addrh = ((bytes[0] << 8) | bytes[1]) << 16;
+ logd("LoadIHEX: Setting high addr 0x%02X%02X", bytes[0], bytes[1]);
+ } else
+ if (code == 1) {
+ /* end of file marker */
+ break;
+ } else
+ if (code == 0) {
+ /* normal code block */
+ uint32_t fulladdr = addrh | addr;
+ mem_add(&ram, fulladdr, bytes, len);
+ }
+ }
+ return ram;
+}
+
+/* summary of the memory blocks in this list */
+void list_mem(const mem_t * head)
+{
+ const mem_t *p = head;
+ while (p!=NULL) {
+ logd("%6X : %d bytes", p->start, p->len);
+ p = p->next;
+ }
+}
+
+/* check that the program will fit */
+int validate_mem(mem_t * head, uint16_t maxmem)
+{
+ if (head == NULL) {
+ loge("MemValidate: No program!");
+ return 1;
+ }
+
+ mem_t * p;
+ int used = 0;
+
+ /* test that all the memory blocks will fit */
+ for (p=head; p!=NULL; p=p->next) {
+ /* just ignore config bytes, we cant write them anyway */
+ if (p->start / 2 >= 0x8000) continue;
+
+ used += p->len / 2;
+
+ if (p->start / 2 >= maxmem || (p->start+p->len)/2 >= maxmem) {
+ loge("MemValidate: Program too large, overlaps bootloader.");
+ return 1;
+ }
+ }
+
+ logd("MemValidate: Used %d of %d leaving %d free.", used, maxmem, maxmem-used);
+
+ return 0;
+}
+
+/* reorganise bytes to work with the boot loader */
+int makesafe_mem(mem_t **head, uint16_t maxmem)
+{
+ /* it must fit safely first */
+ if (validate_mem(*head, maxmem)) return 1;
+
+ mem_t * new = malloc(sizeof(mem_t));
+ new->start = maxmem * 2;
+ new->len = 8;
+ new->bytes = malloc(8);
+ memset(new->bytes, 255, 8);
+
+ int found = 0;
+ /* find the code that goes in words 0-3 and move it */
+ mem_t * p = NULL;
+ for (p = *head; p!=NULL; p=p->next) {
+ if (p->start >= 0 && p->start <= 7) {
+ int pre = p->start;
+ int lap = MIN(8-pre, p->len);
+ memcpy(&new->bytes[pre], p->bytes, lap);
+ p->start += lap;
+ p->len -= lap;
+ memmove(p->bytes, &p->bytes[lap], p->len);
+ found++;
+ }
+ }
+
+ if (!found) {
+ loge("MakeSafeMem: Could not find start vector");
+ free(new);
+ return 1;
+ }
+
+ /* check that there is something sane in here
+ * Warning: this is very enhanced-midrange dependant */
+ found = 0;
+ uint8_t * m = new->bytes;
+ uint16_t lath = 0;
+ for (int i=0; i<4; i++) {
+ uint16_t in = (m[0] << 8) | m[1];
+ if ((in & 0x3F80) == 0x3180) { // MOVLP
+ lath = in & 0x007F;
+ logd("MakeSafeMem: 0x%04X MOVLP 0x%02X", i, lath);
+ }else
+ if ((in & 0x3800) == 0x2800) { // GOTO
+ uint16_t addr = in & 0x07FF;
+ addr |= lath << 8;
+ logd("MakeSafeMem: 0x%04X GOTO 0x%02X", i, addr);
+
+ if ( addr < 4) {
+ // an absolute goto within the reset vector
+ // this is silly and wrong, replace with NOP
+ logd("MakeSafeMem: Rewriting bad 'GOTO 0x%02X' to NOP", addr);
+ m[0] = 0;
+ m[1] = 0;
+ } else {
+ // there is a sane GOTO, hurrah
+ found = 1;
+ }
+ }
+ m += 2;
+ }
+
+ if (!found) {
+ loge("Start Vector did not contain a GOTO");
+ free(new);
+ return 1;
+ }
+
+ new->next = *head;
+ *head = new;
+
+ return 0;
+}