Initial Commit - Copy from Altus Metrum AltOS
This commit is contained in:
651
ao-tools/lib/ao-hex.c
Normal file
651
ao-tools/lib/ao-hex.c
Normal file
@@ -0,0 +1,651 @@
|
||||
/*
|
||||
* Copyright © 2008 Keith Packard <keithp@keithp.com>
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
|
||||
*/
|
||||
|
||||
#include <stdarg.h>
|
||||
#include <ctype.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <string.h>
|
||||
#include "ao-hex.h"
|
||||
#include "ao-verbose.h"
|
||||
|
||||
struct ao_hex_input {
|
||||
FILE *file;
|
||||
int line;
|
||||
char *name;
|
||||
};
|
||||
|
||||
enum ao_hex_read_state {
|
||||
read_marker,
|
||||
read_length,
|
||||
read_address,
|
||||
read_type,
|
||||
read_data,
|
||||
read_checksum,
|
||||
read_newline,
|
||||
read_white,
|
||||
read_done,
|
||||
};
|
||||
|
||||
|
||||
static void
|
||||
ao_hex_error(struct ao_hex_input *input, char *format, ...)
|
||||
{
|
||||
va_list ap;
|
||||
|
||||
va_start(ap, format);
|
||||
fprintf(stderr, "Hex error %s:%d: ", input->name, input->line);
|
||||
vfprintf(stderr, format, ap);
|
||||
fprintf(stderr, "\n");
|
||||
va_end(ap);
|
||||
}
|
||||
|
||||
static void
|
||||
ao_hex_free(struct ao_hex_record *record)
|
||||
{
|
||||
if (!record) return;
|
||||
free(record);
|
||||
}
|
||||
|
||||
static struct ao_hex_record *
|
||||
ao_hex_alloc(uint8_t length)
|
||||
{
|
||||
struct ao_hex_record *record;
|
||||
|
||||
record = calloc(1, sizeof(struct ao_hex_record) + length);
|
||||
record->length = length;
|
||||
return record;
|
||||
}
|
||||
|
||||
static int
|
||||
ishex(char c)
|
||||
{
|
||||
return isdigit(c) || ('a' <= c && c <= 'f') || ('A' <= c && c <= 'F');
|
||||
}
|
||||
|
||||
static int
|
||||
fromhex(char c)
|
||||
{
|
||||
if (isdigit(c))
|
||||
return c - '0';
|
||||
if ('a' <= c && c <= 'f')
|
||||
return c - 'a' + 10;
|
||||
if ('A' <= c && c <= 'F')
|
||||
return c - 'A' + 10;
|
||||
abort();
|
||||
return 0;
|
||||
}
|
||||
|
||||
static uint8_t
|
||||
ao_hex_checksum(struct ao_hex_record *record)
|
||||
{
|
||||
uint8_t checksum = 0;
|
||||
int i;
|
||||
|
||||
checksum += record->length;
|
||||
checksum += record->address >> 8;
|
||||
checksum += record->address & 0xff;
|
||||
checksum += record->type;
|
||||
for (i = 0; i < record->length; i++)
|
||||
checksum += record->data[i];
|
||||
return -checksum;
|
||||
}
|
||||
|
||||
static struct ao_hex_record *
|
||||
ao_hex_read_record(struct ao_hex_input *input)
|
||||
{
|
||||
struct ao_hex_record *record = NULL;
|
||||
enum ao_hex_read_state state = read_marker;
|
||||
char c;
|
||||
int nhexbytes = 0;
|
||||
uint32_t hex = 0;
|
||||
uint32_t ndata = 0;
|
||||
uint8_t checksum;
|
||||
|
||||
while (state != read_done) {
|
||||
c = getc(input->file);
|
||||
if (c == EOF && state != read_white && state != read_marker) {
|
||||
ao_hex_error(input, "Unexpected EOF");
|
||||
goto bail;
|
||||
}
|
||||
if (c == ' ')
|
||||
continue;
|
||||
if (c == '\n')
|
||||
input->line++;
|
||||
switch (state) {
|
||||
case read_marker:
|
||||
if (c == EOF)
|
||||
return NULL;
|
||||
if (c != ':') {
|
||||
ao_hex_error(input, "Missing ':'");
|
||||
goto bail;
|
||||
}
|
||||
state = read_length;
|
||||
nhexbytes = 2;
|
||||
hex = 0;
|
||||
break;
|
||||
case read_length:
|
||||
case read_address:
|
||||
case read_type:
|
||||
case read_data:
|
||||
case read_checksum:
|
||||
if (!ishex(c)) {
|
||||
ao_hex_error(input, "Non-hex char '%c'",
|
||||
c);
|
||||
goto bail;
|
||||
}
|
||||
hex = hex << 4 | fromhex(c);
|
||||
--nhexbytes;
|
||||
if (nhexbytes != 0)
|
||||
break;
|
||||
|
||||
switch (state) {
|
||||
case read_length:
|
||||
record = ao_hex_alloc(hex);
|
||||
if (!record) {
|
||||
ao_hex_error(input, "Out of memory");
|
||||
goto bail;
|
||||
}
|
||||
state = read_address;
|
||||
nhexbytes = 4;
|
||||
break;
|
||||
case read_address:
|
||||
record->address = hex;
|
||||
state = read_type;
|
||||
nhexbytes = 2;
|
||||
break;
|
||||
case read_type:
|
||||
record->type = hex;
|
||||
state = read_data;
|
||||
nhexbytes = 2;
|
||||
ndata = 0;
|
||||
break;
|
||||
case read_data:
|
||||
record->data[ndata] = hex;
|
||||
ndata++;
|
||||
nhexbytes = 2;
|
||||
break;
|
||||
case read_checksum:
|
||||
record->checksum = hex;
|
||||
state = read_newline;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
if (state == read_data)
|
||||
if (ndata == record->length) {
|
||||
nhexbytes = 2;
|
||||
state = read_checksum;
|
||||
}
|
||||
hex = 0;
|
||||
break;
|
||||
case read_newline:
|
||||
if (c != '\n' && c != '\r') {
|
||||
ao_hex_error(input, "Missing newline");
|
||||
goto bail;
|
||||
}
|
||||
state = read_white;
|
||||
break;
|
||||
case read_white:
|
||||
if (!isspace(c)) {
|
||||
if (c == '\n')
|
||||
input->line--;
|
||||
if (c != EOF)
|
||||
ungetc(c, input->file);
|
||||
state = read_done;
|
||||
}
|
||||
break;
|
||||
case read_done:
|
||||
break;
|
||||
}
|
||||
}
|
||||
checksum = ao_hex_checksum(record);
|
||||
if (checksum != record->checksum) {
|
||||
ao_hex_error(input, "Invalid checksum (read 0x%02x computed 0x%02x)\n",
|
||||
record->checksum, checksum);
|
||||
goto bail;
|
||||
}
|
||||
return record;
|
||||
|
||||
bail:
|
||||
ao_hex_free(record);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
void
|
||||
ao_hex_file_free(struct ao_hex_file *hex)
|
||||
{
|
||||
int i;
|
||||
|
||||
if (!hex)
|
||||
return;
|
||||
for (i = 0; i < hex->nrecord; i++)
|
||||
ao_hex_free(hex->records[i]);
|
||||
free(hex);
|
||||
}
|
||||
|
||||
struct ao_hex_file *
|
||||
ao_hex_file_read(FILE *file, char *name)
|
||||
{
|
||||
struct ao_hex_input input;
|
||||
struct ao_hex_file *hex = NULL, *newhex;
|
||||
struct ao_hex_record *record;
|
||||
int srecord = 1;
|
||||
int done = 0;
|
||||
|
||||
hex = calloc(sizeof (struct ao_hex_file) + sizeof (struct ao_hex_record *), 1);
|
||||
if (!hex)
|
||||
return NULL;
|
||||
input.name = name;
|
||||
input.line = 1;
|
||||
input.file = file;
|
||||
while (!done) {
|
||||
record = ao_hex_read_record(&input);
|
||||
if (!record) {
|
||||
if (feof(input.file)) {
|
||||
done = 1;
|
||||
break;
|
||||
} else
|
||||
goto bail;
|
||||
}
|
||||
if (hex->nrecord == srecord) {
|
||||
srecord *= 2;
|
||||
newhex = realloc(hex,
|
||||
sizeof (struct ao_hex_file) +
|
||||
srecord * sizeof (struct ao_hex_record *));
|
||||
if (!newhex)
|
||||
goto bail;
|
||||
hex = newhex;
|
||||
}
|
||||
hex->records[hex->nrecord++] = record;
|
||||
}
|
||||
return hex;
|
||||
|
||||
bail:
|
||||
ao_hex_file_free(hex);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static struct ao_sym *
|
||||
load_symbols(struct ao_hex_file *hex,
|
||||
int *num_symbolsp)
|
||||
{
|
||||
uint32_t extended_addr;
|
||||
uint32_t addr;
|
||||
int i;
|
||||
struct ao_hex_record *record;
|
||||
struct ao_sym *symbols = NULL;
|
||||
struct ao_sym *symbol;
|
||||
int num_symbols = 0;
|
||||
int size_symbols = 0;
|
||||
|
||||
extended_addr = 0;
|
||||
for (i = 0; i < hex->nrecord; i++) {
|
||||
record = hex->records[i];
|
||||
switch (record->type) {
|
||||
case AO_HEX_RECORD_NORMAL:
|
||||
addr = extended_addr + record->address;
|
||||
break;
|
||||
case AO_HEX_RECORD_EOF:
|
||||
break;
|
||||
case AO_HEX_RECORD_EXTENDED_ADDRESS_4:
|
||||
if (record->length != 2)
|
||||
goto bail;
|
||||
extended_addr = ((record->data[0] << 8) | record->data[1]) << 4;
|
||||
break;
|
||||
case AO_HEX_RECORD_EXTENDED_ADDRESS_8:
|
||||
if (record->length != 2)
|
||||
goto bail;
|
||||
extended_addr = (record->data[0] << 24) | (record->data[1] << 16);
|
||||
break;
|
||||
case AO_HEX_RECORD_SYMBOL:
|
||||
addr = extended_addr + record->address;
|
||||
if (num_symbols == size_symbols) {
|
||||
struct ao_sym *new_symbols;
|
||||
int new_size;
|
||||
|
||||
if (!size_symbols)
|
||||
new_size = 16;
|
||||
else
|
||||
new_size = size_symbols * 2;
|
||||
new_symbols = realloc(symbols, new_size * sizeof (struct ao_sym));
|
||||
if (!new_symbols)
|
||||
goto bail;
|
||||
|
||||
symbols = new_symbols;
|
||||
size_symbols = new_size;
|
||||
}
|
||||
symbol = &symbols[num_symbols];
|
||||
memset(symbol, 0, sizeof (struct ao_sym));
|
||||
symbol->name = calloc(record->length + 1, 1);
|
||||
if (!symbol->name)
|
||||
goto bail;
|
||||
memcpy(symbol->name, record->data, record->length);
|
||||
symbol->addr = addr;
|
||||
ao_printf(AO_VERBOSE_EXE, "Add symbol %s: %08x\n", symbol->name, symbol->addr);
|
||||
num_symbols++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
*num_symbolsp = num_symbols;
|
||||
return symbols;
|
||||
bail:
|
||||
for (i = 0; i < num_symbols; i++)
|
||||
free(symbols[i].name);
|
||||
free(symbols);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static void
|
||||
ao_hex_record_set_checksum(struct ao_hex_record *record)
|
||||
{
|
||||
uint8_t cksum = 0;
|
||||
int i;
|
||||
|
||||
cksum += record->length;
|
||||
cksum += record->address >> 8;
|
||||
cksum += record->address;
|
||||
cksum += record->type;
|
||||
for (i = 0; i < record->length; i++)
|
||||
cksum += record->data[i];
|
||||
|
||||
record->checksum = -cksum;
|
||||
}
|
||||
|
||||
struct ao_hex_image *
|
||||
ao_hex_image_create(struct ao_hex_file *hex)
|
||||
{
|
||||
struct ao_hex_image *image;
|
||||
struct ao_hex_record *record;
|
||||
int i;
|
||||
uint32_t addr;
|
||||
uint32_t base, bound;
|
||||
uint32_t offset;
|
||||
uint32_t extended_addr;
|
||||
|
||||
int length;
|
||||
|
||||
/* Find the address bounds of the file
|
||||
*/
|
||||
base = 0xffffffff;
|
||||
bound = 0x0;
|
||||
extended_addr = 0;
|
||||
for (i = 0; i < hex->nrecord; i++) {
|
||||
uint32_t r_bound;
|
||||
record = hex->records[i];
|
||||
switch (record->type) {
|
||||
case AO_HEX_RECORD_NORMAL:
|
||||
addr = extended_addr + record->address;
|
||||
r_bound = addr + record->length;
|
||||
if (addr < base)
|
||||
base = addr;
|
||||
if (r_bound > bound)
|
||||
bound = r_bound;
|
||||
break;
|
||||
case AO_HEX_RECORD_EOF:
|
||||
break;
|
||||
case AO_HEX_RECORD_EXTENDED_ADDRESS_4:
|
||||
if (record->length != 2)
|
||||
return NULL;
|
||||
extended_addr = ((record->data[0] << 8) | record->data[1]) << 4;
|
||||
break;
|
||||
case AO_HEX_RECORD_EXTENDED_ADDRESS_8:
|
||||
if (record->length != 2)
|
||||
return NULL;
|
||||
extended_addr = (record->data[0] << 24) | (record->data[1] << 16);
|
||||
break;
|
||||
case AO_HEX_RECORD_SYMBOL:
|
||||
break;
|
||||
}
|
||||
}
|
||||
length = bound - base;
|
||||
image = calloc(sizeof(struct ao_hex_image) + length, 1);
|
||||
if (!image)
|
||||
return NULL;
|
||||
image->address = base;
|
||||
image->length = length;
|
||||
memset(image->data, 0xff, length);
|
||||
extended_addr = 0;
|
||||
for (i = 0; i < hex->nrecord; i++) {
|
||||
record = hex->records[i];
|
||||
switch (record->type) {
|
||||
case AO_HEX_RECORD_NORMAL:
|
||||
addr = extended_addr + record->address;
|
||||
offset = addr - base;
|
||||
memcpy(image->data + offset, record->data, record->length);
|
||||
break;
|
||||
case AO_HEX_RECORD_EOF:
|
||||
break;
|
||||
case AO_HEX_RECORD_EXTENDED_ADDRESS_4:
|
||||
extended_addr = ((record->data[0] << 8) | record->data[1]) << 4;
|
||||
break;
|
||||
case AO_HEX_RECORD_EXTENDED_ADDRESS_8:
|
||||
extended_addr = (record->data[0] << 24) | (record->data[1] << 16);
|
||||
break;
|
||||
case AO_HEX_RECORD_SYMBOL:
|
||||
break;
|
||||
}
|
||||
}
|
||||
return image;
|
||||
}
|
||||
|
||||
void
|
||||
ao_hex_image_free(struct ao_hex_image *image)
|
||||
{
|
||||
free(image);
|
||||
}
|
||||
|
||||
static uint32_t min(uint32_t a, uint32_t b) { return a < b ? a : b; }
|
||||
static uint32_t max(uint32_t a, uint32_t b) { return a > b ? a : b; }
|
||||
|
||||
struct ao_hex_image *
|
||||
ao_hex_image_cat(struct ao_hex_image *a, struct ao_hex_image *b)
|
||||
{
|
||||
struct ao_hex_image *n;
|
||||
uint32_t base, bound;
|
||||
uint32_t length;
|
||||
|
||||
base = min(a->address, b->address);
|
||||
bound = max(a->address + a->length, b->address + b->length);
|
||||
length = bound - base;
|
||||
|
||||
n = calloc (sizeof (struct ao_hex_image) + length, 1);
|
||||
if (!n)
|
||||
return NULL;
|
||||
n->address = base;
|
||||
n->length = length;
|
||||
memset(n->data, 0xff, length);
|
||||
memcpy(n->data + a->address - n->address, a->data, a->length);
|
||||
memcpy(n->data + b->address - n->address, b->data, b->length);
|
||||
return n;
|
||||
}
|
||||
|
||||
int
|
||||
ao_hex_image_equal(struct ao_hex_image *a, struct ao_hex_image *b)
|
||||
{
|
||||
if (a->length != b->length)
|
||||
return 0;
|
||||
if (memcmp(a->data, b->data, a->length) != 0)
|
||||
return 0;
|
||||
return 1;
|
||||
}
|
||||
|
||||
struct ao_hex_image *
|
||||
ao_hex_load(char *filename, struct ao_sym **symbols, int *num_symbolsp)
|
||||
{
|
||||
FILE *file;
|
||||
struct ao_hex_file *hex_file;
|
||||
struct ao_hex_image *hex_image;
|
||||
|
||||
file = fopen (filename, "r");
|
||||
if (!file)
|
||||
return NULL;
|
||||
|
||||
hex_file = ao_hex_file_read(file, filename);
|
||||
fclose(file);
|
||||
if (!hex_file)
|
||||
return NULL;
|
||||
hex_image = ao_hex_image_create(hex_file);
|
||||
if (!hex_image)
|
||||
return NULL;
|
||||
|
||||
if (symbols)
|
||||
*symbols = load_symbols(hex_file, num_symbolsp);
|
||||
|
||||
ao_hex_file_free(hex_file);
|
||||
return hex_image;
|
||||
}
|
||||
|
||||
#define BYTES_PER_RECORD 32
|
||||
|
||||
static struct ao_hex_file *
|
||||
ao_hex_file_create(struct ao_hex_image *image, struct ao_sym *symbols, int num_symbols)
|
||||
{
|
||||
/* split data into n-byte-sized chunks */
|
||||
uint32_t data_records = (image->length + BYTES_PER_RECORD-1) / BYTES_PER_RECORD;
|
||||
/* extended address and data for each block, EOF, address and data for each symbol */
|
||||
uint32_t total_records = data_records * 2 + 1 + num_symbols * 2;
|
||||
uint32_t offset;
|
||||
uint32_t address;
|
||||
uint32_t length;
|
||||
char *name;
|
||||
struct ao_hex_file *hex_file;
|
||||
int nrecord = 0;
|
||||
int s;
|
||||
struct ao_hex_record *record;
|
||||
|
||||
hex_file = calloc(sizeof (struct ao_hex_file) + sizeof (struct ao_hex_record *) * total_records, 1);
|
||||
if (!hex_file)
|
||||
return NULL;
|
||||
|
||||
/* Add the data
|
||||
*/
|
||||
for (offset = 0; offset < image->length; offset += BYTES_PER_RECORD) {
|
||||
uint32_t address = image->address + offset;
|
||||
uint32_t length = image->length - offset;
|
||||
|
||||
if (length > BYTES_PER_RECORD)
|
||||
length = BYTES_PER_RECORD;
|
||||
|
||||
record = calloc(sizeof (struct ao_hex_record) + 2, 1);
|
||||
record->type = AO_HEX_RECORD_EXTENDED_ADDRESS_8;
|
||||
record->address = 0;
|
||||
record->length = 2;
|
||||
record->data[0] = address >> 24;
|
||||
record->data[1] = address >> 16;
|
||||
ao_hex_record_set_checksum(record);
|
||||
|
||||
hex_file->records[nrecord++] = record;
|
||||
|
||||
record = calloc(sizeof (struct ao_hex_record) + length, 1);
|
||||
record->type = AO_HEX_RECORD_NORMAL;
|
||||
record->address = address;
|
||||
record->length = length;
|
||||
memcpy(record->data, image->data + offset, length);
|
||||
ao_hex_record_set_checksum(record);
|
||||
|
||||
hex_file->records[nrecord++] = record;
|
||||
}
|
||||
|
||||
/* Stick an EOF after the data
|
||||
*/
|
||||
record = calloc(1,sizeof (struct ao_hex_record) + 2);
|
||||
record->type = AO_HEX_RECORD_EOF;
|
||||
record->address = 0;
|
||||
record->length = 0;
|
||||
record->data[0] = 0;
|
||||
record->data[1] = 0;
|
||||
ao_hex_record_set_checksum(record);
|
||||
|
||||
hex_file->records[nrecord++] = record;
|
||||
|
||||
/* Add the symbols
|
||||
*/
|
||||
|
||||
for (s = 0; s < num_symbols; s++) {
|
||||
|
||||
name = symbols[s].name;
|
||||
address = symbols[s].addr;
|
||||
length = strlen (name);
|
||||
|
||||
record = calloc(sizeof (struct ao_hex_record) + 2, 1);
|
||||
record->type = AO_HEX_RECORD_EXTENDED_ADDRESS_8;
|
||||
record->address = 0;
|
||||
record->length = 2;
|
||||
record->data[0] = address >> 24;
|
||||
record->data[1] = address >> 16;
|
||||
ao_hex_record_set_checksum(record);
|
||||
|
||||
hex_file->records[nrecord++] = record;
|
||||
|
||||
record = calloc(sizeof (struct ao_hex_record) + length, 1);
|
||||
record->type = AO_HEX_RECORD_SYMBOL;
|
||||
record->address = address;
|
||||
record->length = length;
|
||||
memcpy(record->data, name, length);
|
||||
ao_hex_record_set_checksum(record);
|
||||
|
||||
hex_file->records[nrecord++] = record;
|
||||
}
|
||||
|
||||
hex_file->nrecord = nrecord;
|
||||
return hex_file;
|
||||
}
|
||||
|
||||
static bool
|
||||
ao_hex_write_record(FILE *file, struct ao_hex_record *record)
|
||||
{
|
||||
int i;
|
||||
|
||||
fputc(':', file);
|
||||
fprintf(file, "%02x", record->length);
|
||||
fprintf(file, "%04x", record->address);
|
||||
fprintf(file, "%02x", record->type);
|
||||
for (i = 0; i < record->length; i++)
|
||||
fprintf(file, "%02x", record->data[i]);
|
||||
fprintf(file, "%02x", record->checksum);
|
||||
fputc('\n', file);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool
|
||||
ao_hex_save(FILE *file, struct ao_hex_image *image,
|
||||
struct ao_sym *symbols, int num_symbols)
|
||||
{
|
||||
struct ao_hex_file *hex_file;
|
||||
int i;
|
||||
bool ret = false;
|
||||
|
||||
hex_file = ao_hex_file_create(image, symbols, num_symbols);
|
||||
if (!hex_file)
|
||||
goto create_failed;
|
||||
|
||||
for (i = 0; i < hex_file->nrecord; i++) {
|
||||
if (!ao_hex_write_record(file, hex_file->records[i]))
|
||||
goto write_failed;
|
||||
}
|
||||
ret = true;
|
||||
|
||||
if (fflush(file) != 0)
|
||||
ret = false;
|
||||
write_failed:
|
||||
ao_hex_file_free(hex_file);
|
||||
create_failed:
|
||||
return ret;
|
||||
}
|
||||
Reference in New Issue
Block a user