[bootloader] / cli / memory.c

#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;
}
Commit	Line	Data
9c66c9ff JM	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	}