From 32f507ce5f66dd9c89a45854688f46bde33c5e3d Mon Sep 17 00:00:00 2001
From: Tobias Klauser <tklauser@distanz.ch>
Date: Wed, 10 Nov 2010 09:20:50 +0100
Subject: Initial import of nios2sim (http://nios2sim.googlecode.com/svn/trunk/
 r16)

---
 load_image.c | 620 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 620 insertions(+)
 create mode 100644 load_image.c

(limited to 'load_image.c')

diff --git a/load_image.c b/load_image.c
new file mode 100644
index 0000000..6a10e47
--- /dev/null
+++ b/load_image.c
@@ -0,0 +1,620 @@
+/*
+    Nios-sim - one simple NIOSII simulator only for personal interest and fun.
+    Copyright (C) 2010  chysun2000@gmail.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.,
+    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+*/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "public.h"
+#include "nor_flash.h"
+
+typedef enum {
+	S_INVALID = 0x00,
+	S0 = 0x01,
+	S1 = 0x02,
+	S2 = 0x03,
+	S3 = 0x04,
+	S4 = 0x05,
+	S5 = 0x06,
+	S6 = 0x07,
+	S7 = 0x08,
+	S8 = 0x09,
+	S9 = 0x0A
+} SREC_TYPE;
+
+static struct image_info image_info;
+
+static int32_t is_line_end(char data)
+{
+	if (data == '\n'){
+		return 0;
+	}
+	else{
+		return -1;
+	}
+}
+
+static void get_line(FILE * fp, char * buf, int32_t buf_len, int32_t * line_len)
+{
+	char data = '\0';
+	int32_t index = 0;
+	
+	memset(buf, 0x00, buf_len);
+	while(1){
+		if(feof(fp) != 0 || ferror(fp) != 0){
+			return ;
+		}
+		
+		data = '\0';
+		if (fread(&data, 1, 1, fp) == 1){
+			if (is_line_end(data) != 0){
+				buf[index++] = data;
+				*line_len = index;
+			}
+			else {
+				return ;
+			}
+		}
+		else {
+			return ;
+		}
+	}
+}
+
+static SREC_TYPE get_srec_type(char * buf)
+{
+	if (buf[1] >= 0x30 && buf[1] <= 0x39){
+		return S0 + buf[1] - 0x30;
+	}
+	else{
+		return S_INVALID;
+	}
+}
+
+static uint32_t __factors [] = {
+	1, 
+	16, 
+	256,
+	16 * 16 * 16, 
+	16 * 16 * 16 * 16,
+	16 * 16 * 16 * 16 * 16,
+	16 * 16 * 16 * 16 * 16 * 16,
+	16 * 16 * 16 * 16 * 16 * 16 * 16
+};
+
+static uint32_t single_ascii_to_hex(char data)
+{
+	if (data >= 0x30 && data <= 0x39)
+		return data - 0x30;
+	if (data >= 0x41 && data <= 0x46)
+		return data - 0x41 + 10;
+	if (data >= 0x61 && data <= 0x66)
+		return data - 0x61 + 10;
+	return 0;
+}
+
+uint32_t ascii_to_hex(char * buf, uint32_t buf_len)
+{
+	uint32_t ret = 0x0000;
+	int32_t i = 0;
+	for (i=0;i<buf_len;i++){
+		ret = ret + (single_ascii_to_hex(buf[i]) * __factors[buf_len - i - 1]);
+	}
+	return ret;
+}
+
+static uint32_t get_srec_data_count(char * buf)
+{
+	uint32_t count = 0;
+	count = ascii_to_hex(buf + 2, 2);
+	return count;
+}
+
+static uint32_t to_le(uint32_t data)
+{
+	uint32_t ret = 0;
+	ret = ret | ((data & 0xFF000000) >> 24);
+	ret = ret | ((data & 0x00FF0000) >> 8);
+	ret = ret | ((data & 0x0000FF00) << 8);
+	ret = ret | ((data & 0x000000FF) << 24);
+	return ret;
+}
+
+#define S0_DATA_OFFSET (8)
+#define S0_ADDR_CRC_CNT (3)
+static uint32_t handle_S0(char * buf, uint32_t data_count, uint32_t * mem_base)
+{
+	int32_t i = 0;
+	char * data_buf = buf;
+	
+	data_buf = data_buf + S0_DATA_OFFSET;
+	for (i=0;i<data_count - S0_ADDR_CRC_CNT; i++){
+		data_buf += 2;
+	}
+	return 0;
+}
+
+#define S3_ADDR_OFFSET (4)
+#define S3_DATA_OFFSET (S3_ADDR_OFFSET + 8)
+#define S3_ADDR_CRC_CNT (5)
+
+static uint32_t get_S3_address(char * buf)
+{
+	uint32_t ret = 0;
+	buf = buf + S3_ADDR_OFFSET;
+	ret = ascii_to_hex(buf, 8);
+	return ret;
+}
+
+static uint32_t handle_S3(char * buf, uint32_t data_count, uint32_t * mem_base, 
+							  uint32_t base_addr)
+{
+	uint32_t start_addr = 0;
+	uint32_t offset = 0;
+	uint32_t data = 0;
+	char * data_buf = buf;
+	int32_t i;
+	
+	start_addr = get_S3_address(buf);
+
+	if (base_addr == 0){
+		offset = 0;
+	}
+	else{
+		offset = (start_addr - base_addr)/4;
+	}
+	data_buf = data_buf + S3_DATA_OFFSET;
+	for (i=0;i<(data_count-S3_ADDR_CRC_CNT)/4;i++){
+		data = to_le(ascii_to_hex(data_buf, 8));
+		data_buf += 8;
+		mem_base[offset + i] = data;
+	}
+
+	return start_addr;
+}
+
+#define S2_ADDR_OFFSET (4)
+#define S2_DATA_OFFSET (S2_ADDR_OFFSET + 6)
+#define S2_ADDR_CRC_CNT (4)
+
+static uint32_t get_S2_address(char * buf)
+{
+	uint32_t ret = 0;
+	buf = buf + S2_ADDR_OFFSET;
+	ret = ascii_to_hex(buf, 6);
+	return ret;
+}
+
+static uint32_t handle_S2(char * buf, uint32_t data_count, uint32_t * mem_base, 
+							  uint32_t base_addr)
+{
+	uint32_t start_addr = 0;
+	uint32_t offset = 0;
+	uint32_t data = 0;
+	char * data_buf = buf;
+	int32_t i;
+	
+	start_addr = get_S2_address(buf);
+
+	if (base_addr == 0){
+		offset = 0;
+	}
+	else{
+		offset = (start_addr - base_addr)/4;
+	}
+	
+	data_buf = data_buf + S2_DATA_OFFSET;
+	for (i=0;i<(data_count-S2_ADDR_CRC_CNT)/4;i++){
+		data = to_le(ascii_to_hex(data_buf, 8));
+		data_buf += 8;
+		mem_base[offset + i] = data;
+	}
+
+	return start_addr;
+}
+
+
+#define S7_DATA_OFFSET (4)
+#define S7_CRC_CNT (2)
+static uint32_t handle_S7(char * buf, uint32_t data_count, uint32_t * mem_base)
+{
+	char * data_buf = buf;
+
+	data_buf = data_buf + S7_DATA_OFFSET;
+	return ascii_to_hex(data_buf, 8);
+}
+
+#define S8_DATA_OFFSET (4)
+#define S8_CRC_CNT (2)
+static uint32_t handle_S8(char * buf, uint32_t data_count, uint32_t * mem_base)
+{
+	char * data_buf = buf;
+
+	data_buf = data_buf + S8_DATA_OFFSET;
+	return ascii_to_hex(data_buf, 6);
+}
+
+static void dummy_srec_handler(char * name)
+{
+	printf("%s: for %s\n",__func__, name);
+}
+
+static void handle_srec_line(char * buf, int line_len, uint32_t * mem_base)
+{
+	SREC_TYPE srec_type = S_INVALID;
+	uint32_t data_count = 0;
+	uint32_t start_addr = 0;
+
+	srec_type = get_srec_type(buf);
+	data_count =  get_srec_data_count(buf);
+
+	switch(srec_type){
+	case S0:
+		handle_S0(buf, data_count, mem_base);
+		break;
+	case S1:
+		dummy_srec_handler("S1");
+		break;
+	case S2:
+		if (image_info.base_addr == 0){
+			image_info.base_addr = handle_S2(buf, data_count, mem_base, image_info.base_addr);
+		}
+		else {
+			handle_S2(buf, data_count, mem_base, image_info.base_addr);
+		}
+		break;
+	case S3:
+		if (image_info.base_addr == 0){
+			image_info.base_addr = handle_S3(buf, data_count, mem_base, image_info.base_addr);
+		}
+		else {
+			handle_S3(buf, data_count, mem_base, image_info.base_addr);
+		}
+		break;
+	case S4:
+		dummy_srec_handler("S4");
+		break;
+	case S5:
+		dummy_srec_handler("S5");
+		break;
+	case S6:
+		dummy_srec_handler("S6");
+		break;
+	case S7:
+		start_addr = handle_S7(buf, data_count, mem_base);
+		set_image_entry_addr(start_addr);
+		break;
+	case S8:
+		start_addr = handle_S8(buf, data_count, mem_base);
+		set_image_entry_addr(start_addr);
+		break;
+	case S9:
+		dummy_srec_handler("S9");
+		break;
+	default:
+		return;
+	}
+	return;
+}
+
+#define SREC_LINE_LENGTH (515)
+static char data_buf[SREC_LINE_LENGTH] = {0};
+
+static void load_srec(const char * image_path_name, uint32_t * image_addr)
+{
+	FILE * fp = NULL;
+	int32_t line_len = 0;
+
+	fp = fopen(image_path_name, "r");
+	if (fp != NULL){
+		while(1){
+			memset(data_buf, 0x00, SREC_LINE_LENGTH);
+			line_len = 0;
+			get_line(fp, data_buf, SREC_LINE_LENGTH, &line_len);
+			if (line_len == 0){
+				break;
+			}
+			else
+			{
+				handle_srec_line(data_buf, line_len, image_addr);
+			}
+		}
+		fclose(fp);
+	}
+	else{
+		printf("ERROR--> can not open image at %s\n", image_path_name);
+	}
+}
+
+void load_image(void)
+{
+	switch(image_info.image_format){
+	case SREC_FMT:
+		load_srec(image_info.path_name, image_info.mem_base);
+		break;
+	default:
+		break;
+	}
+}
+
+void alloc_image_mem(void)
+{
+	char * address = NULL;
+	struct image_info * info = &image_info;
+	uint32_t temp = 0;
+	
+	address = (char *)malloc(info->mem_size);
+	temp =  info->mem_size % 4;
+	if (temp != 0){
+		address = address + temp;
+	}
+	info->mem_base = (uint32_t *)address;
+}
+
+void set_image_pathname(char * pathname)
+{
+	image_info.path_name = pathname;
+}
+
+void set_image_format(IMG_FORMAT format)
+{
+	image_info.image_format = format;
+}
+
+void set_image_memsize(char * size)
+{
+	int32_t str_len = strlen(size);
+	char * temp = NULL;
+
+	if (size[str_len - 1] == 'M' || size[str_len - 1] == 'm'){
+		temp = malloc(str_len);
+		memset(temp, 0x00, str_len);
+		memcpy(temp, size, str_len);
+		temp[str_len - 1] = '\0';
+		image_info.mem_size = atoi(temp) * 1024 * 1024;
+		free(temp);
+	}
+}
+
+void set_image_entry_addr(uint32_t addr)
+{
+	image_info.entry_addr = addr;
+}
+
+void set_image_base_addr(char * addr)
+{
+	sscanf(addr, "0x%X\n", &image_info.base_addr);
+}
+static char cmd_line[256] = {0};
+static uint32_t initrd_start = 0;
+static uint32_t initrd_size = 0;
+static char fs_image[256] = {0};
+
+void print_image_info(void)
+{
+	printf("--------------------------------------------------\n");
+	printf(" NIOS Simulator (Built at %s-%s)\n",__DATE__,__TIME__);
+	printf("--------------------------------------------------\n");
+	printf("Image File:%s\n",image_info.path_name);
+	
+	if (image_info.image_format == SREC_FMT){
+		printf("Format:SREC\n");
+	}
+
+	printf("Mem size:0x%08X, loading at %p\n", image_info.mem_size, 
+		   image_info.mem_base);
+	printf("Base address:0x%08X, Entry address:0x%08X\n",image_info.base_addr,
+		   image_info.entry_addr);
+	printf("Set command line:%s\n", cmd_line);
+	printf("Initrd: 0x%08x size:0x%08x\n",initrd_start, initrd_size);
+	printf("rootfs: %s \n",fs_image);
+	printf("--------------------------------------------------\n");
+}
+
+struct image_info * get_image_info(void)
+{
+	return &image_info;
+}
+
+static struct symbol_obj * sym_hash[256] = {NULL};
+
+static struct symbol_obj * get_head_list(uint32_t addr)
+{
+	return sym_hash[addr % 256];
+}
+
+static void insert_symbol_obj(struct symbol_obj * list, struct symbol_obj * obj)
+{
+	if (list == NULL){
+		sym_hash[obj->addr % 256] = obj;
+	}
+	else{
+		while(list != NULL){
+			if (list->next != NULL){
+				list = list->next;
+			}
+			else{
+				break;
+			}
+			
+		}
+		list->next = obj;
+	}
+}
+
+static struct symbol_obj * alloc_symbol_obj(char * addr, char * name)
+{
+	struct symbol_obj * obj = NULL;
+	uint32_t size = 0;
+	
+	obj = (struct symbol_obj *)malloc(sizeof(struct symbol_obj));
+	memset(obj, 0x00, sizeof(struct symbol_obj));
+
+	if (obj != NULL){
+		obj->addr = ascii_to_hex(addr, 8);
+		obj->next = NULL;
+		size = strlen(name) + 1;
+		obj->sym_name = (char *)malloc(size);
+		if (obj->sym_name != NULL){
+			memset(obj->sym_name, 0x00, size);
+			strncpy(obj->sym_name, name, size - 1);
+		}
+		else{
+			printf("[ERROR] %s --> can not allocate symname %s \n",__func__, name);
+		}
+	}
+
+	return obj;
+}
+
+static void handle_symbol_line(char * line)
+{
+	char addr[10] = {0};
+	char sym_name[64] = {0};
+	char type[2] = {0};
+	struct symbol_obj * obj = NULL;
+	struct symbol_obj * list = NULL;
+
+	memset(sym_name, 0x00, 64);
+	sscanf(line, "%s %s %s\n", addr, type, sym_name);
+	obj = alloc_symbol_obj(addr, sym_name);
+
+	if (obj != NULL){
+		list = get_head_list(obj->addr);
+		insert_symbol_obj(list, obj);
+	}
+}
+
+void load_symbol_file(char * symbol_file)
+{
+	char line[256] = {0};
+	int32_t line_len = 0;
+	FILE * fp = NULL;
+	
+	memset(sym_hash, 0x00, sizeof(sym_hash));
+	fp = fopen(symbol_file, "r");
+	if (fp != NULL){
+		while(1){
+			memset(line, 0x00, 256);
+			line_len = 0;
+			get_line(fp, line, 256, &line_len);
+			if (line_len == 0){
+				break;
+			}
+			else{
+				line[line_len] = '\0';
+				handle_symbol_line(line);
+			}
+		}
+		fclose(fp);
+	}
+}
+
+static struct symbol_obj * find_symbol_obj(struct symbol_obj * list, uint32_t addr)
+{
+	struct symbol_obj * ret = NULL;
+	
+	ret = list;
+	while(ret != NULL){
+		if (ret->addr == addr){
+			break;
+		}
+		else{
+			ret = ret->next;
+		}
+	}
+	return ret;
+}
+
+struct symbol_obj * get_symbol(uint32_t addr)
+{
+	struct symbol_obj * ret = NULL;
+	struct symbol_obj * lst = NULL;
+	
+	lst = get_head_list(addr);
+	if (lst != NULL){
+		ret = find_symbol_obj(lst, addr);
+	}
+
+	return ret;
+}
+
+void set_cmdline(char * optarg)
+{
+	strcpy(cmd_line,optarg);
+}
+
+void init_cmdline(struct NIOS_CPU * cpu)
+{
+
+	char * addr = (char *)image_info.mem_base;
+	
+	/* R7 is stored the start address of command line */
+	memcpy((char *)(addr + 0x700000), cmd_line, 255);
+	/* Set magic number.
+	 * this is copied from arch/nios2/kernel/setup.c <setup_arch>
+	 */
+	if (cmd_line[0] != '\0'){
+		cpu->gp_regs[4] = 0x534f494e;
+		cpu->gp_regs[7] = 0xF00000;
+	}
+	
+}
+
+void set_initrd(char * optarg)
+{
+	sscanf(optarg,"0x%08x,0x%08x",&initrd_start,&initrd_size);
+}
+
+void init_initrd(struct NIOS_CPU * cpu)
+{
+	cpu->gp_regs[5] = initrd_start & 0xFFFFFFFC;
+	cpu->gp_regs[6] = (initrd_start + initrd_size) & 0xFFFFFFFC;
+}
+
+void set_fs_image(char * optarg)
+{
+	sscanf(optarg,"%s",fs_image);
+}
+
+void init_fs_image(struct NIOS_CPU * cpu)
+{
+	int32_t fd = open(fs_image,O_RDWR);
+	int32_t size = 0;
+	uint8_t buf [256] = {0};
+	uint8_t * mem_load_addr = NULL;
+
+	/* set the load memory address */
+	mem_load_addr = nor_flash_mem_addr();
+
+	if (fd){
+		printf("loading rootfs at %p\n", mem_load_addr);
+		while((size = read(fd,buf, 256)) > 0){
+			memcpy(mem_load_addr, buf, size);
+			mem_load_addr += size;
+		}
+
+		close(fd);
+	}
+	else {
+		printf("Can not load rootfs image at %s\n",fs_image);
+	}
+}
+
+/*----------------------------------------------------------------------------*/
+
-- 
cgit v1.2.3-54-g00ecf