diff --git a/include/debug.h b/include/debug.h index 3998e84..c5eabbd 100644 --- a/include/debug.h +++ b/include/debug.h @@ -21,6 +21,7 @@ void set_debug_level(int level); int get_debug_level(); +void hexdump(const void *data, size_t size); void debug(int level, const char *fmt, ...); diff --git a/include/elf.h b/include/elf.h new file mode 100644 index 0000000..591047c --- /dev/null +++ b/include/elf.h @@ -0,0 +1,78 @@ +#ifndef __ELF__H__ +#define __ELF__H__ + +#include + +typedef uint16_t Elf32_Half; +typedef uint32_t Elf32_Word; +typedef uint32_t Elf32_Addr; +typedef uint32_t Elf32_Off; +typedef int32_t Elf32_Sword; + +#define EI_NIDENT 16 +#define EI_MAG0 0 +#define EI_MAG1 1 +#define EI_MAG2 2 +#define EI_MAG3 3 +#define EI_CLASS 4 +#define EI_DATA 5 +#define EI_VERSION 6 +#define EI_PAD 7 + +#define ELFMAG0 0x7F +#define ELFMAG1 'E' +#define ELFMAG2 'L' +#define ELFMAG3 'F' + +#define ELFCLASSNONE 0 +#define ELFCLASS32 1 +#define ELFCLASS64 2 + +#define ELFDATANONE 0 +#define ELFDATA2LSB 1 +#define ELFDATA2MSB 2 + +#define ET_NONE 0 +#define ET_REL 1 +#define ET_EXEC 2 +#define ET_DYN 3 +#define ET_CORE 4 + +#define EM_MIPS 8 + +#define EV_NONE 0 +#define EV_CURRENT 1 + +#define PT_NULL 0 +#define PT_LOAD 1 + +typedef struct { + unsigned char e_ident[EI_NIDENT]; + Elf32_Half e_type; + Elf32_Half e_machine; + Elf32_Word e_version; + Elf32_Addr e_entry; + Elf32_Off e_phoff; + Elf32_Off e_shoff; + Elf32_Word e_flags; + Elf32_Half e_ehsize; + Elf32_Half e_phentsize; + Elf32_Half e_phnum; + Elf32_Half e_shentsize; + Elf32_Half e_shnum; + Elf32_Half e_shstrndx; +} Elf32_Ehdr; + +typedef struct { + Elf32_Word p_type; + Elf32_Off p_offset; + Elf32_Addr p_vaddr; + Elf32_Addr p_paddr; + Elf32_Word p_filesz; + Elf32_Word p_memsz; + Elf32_Word p_flags; + Elf32_Word p_align; +} Elf32_Phdr; + +#endif + diff --git a/include/elfldr.h b/include/elfldr.h new file mode 100644 index 0000000..b4a0b9a --- /dev/null +++ b/include/elfldr.h @@ -0,0 +1,10 @@ +#ifndef __ELFLDR__H__ +#define __ELFLDR__H__ + +int load_elf(void *ingenic, + const char *filename, + const char *args, + const char *initrd); + +#endif + diff --git a/src/Makefile.am b/src/Makefile.am index ac76761..cee8e39 100644 --- a/src/Makefile.am +++ b/src/Makefile.am @@ -2,4 +2,5 @@ AM_CFLAGS = --std=gnu99 -Wall -I../include bin_PROGRAMS = jzboot jzboot_SOURCES = debug.c devmgr.c ingenic.c main.c shell_lex.c \ - usbdev.c shell.c shell_builtins.c config.c spl_cmdset.c usbboot_cmdset.c \ No newline at end of file + usbdev.c shell.c shell_builtins.c config.c spl_cmdset.c \ + usbboot_cmdset.c elfldr.c \ No newline at end of file diff --git a/src/debug.c b/src/debug.c index 8506073..d9fa13b 100644 --- a/src/debug.c +++ b/src/debug.c @@ -45,3 +45,38 @@ void debug(int level, const char *fmt, ...) { va_end(list); } + +void hexdump(const void *data, size_t size) { + const unsigned char *bytes = data; + + for(int i = 0; i < size; i+= 16) { + debug(LEVEL_DEBUG, "%04X ", i); + + int chunk_size = size - i; + if(chunk_size > 16) + chunk_size = 16; + + for(int j = 0; j < chunk_size; j++) { + debug(LEVEL_DEBUG, "%02X ", bytes[i + j]); + + if(j == 7) + debug(LEVEL_DEBUG, " "); + } + + for(int j = 0; j < 16 - chunk_size; j++) { + debug(LEVEL_DEBUG, " "); + + if(j == 8) + debug(LEVEL_DEBUG, " "); + } + + debug(LEVEL_DEBUG, "|"); + + for(int j = 0; j < chunk_size; j++) { + debug(LEVEL_DEBUG, "%c", isprint(bytes[i + j]) ? bytes[i + j] : '.'); + } + + debug(LEVEL_DEBUG, "|\n"); + } +} + diff --git a/src/elfldr.c b/src/elfldr.c new file mode 100644 index 0000000..fc72573 --- /dev/null +++ b/src/elfldr.c @@ -0,0 +1,318 @@ +#include +#include +#include +#include +#include +#include "debug.h" +#include "elfldr.h" +#include "elf.h" +#include "ingenic.h" + +#define max(a, b) ((a) > (b) ? (a) : (b)) +#define ALIGN(a) ((a + 4095) & ~4095) + +#define TRAMP_ARGC 0 // command line argument count +#define TRAMP_ARGV 1 // command line argument array +#define TRAMP_ARG2 2 +#define TRAMP_ARG3 3 +#define TRAMP_ENTRY 4 // Kernel entry point + +static const uint32_t trampoline_template[] = { + 0x3c04ffff, // lui a0, 0xffff + 0x3484ffff, // ori a0,a0,0xffff + + 0x3c05ffff, // lui a1, 0xffff + 0x34a5ffff, // ori a1,a1,0xffff + + 0x3c06ffff, // lui a2, 0xffff + 0x34c6ffff, // ori a2,a2,0xffff + + 0x3c07ffff, // lui a3, 0xffff + 0x34e7ffff, // ori a3,a3,0xffff + + 0x3c08ffff, // lui t0, 0xffff + 0x3508ffff, // ori t0,t0,0xffff + 0x01000008, // jr t0 + 0x00000000, // nop +}; + +static const unsigned char valid_ident[EI_PAD] = { + [EI_MAG0] = ELFMAG0, + [EI_MAG1] = ELFMAG1, + [EI_MAG2] = ELFMAG2, + [EI_MAG3] = ELFMAG3, + [EI_CLASS] = ELFCLASS32, + [EI_DATA] = ELFDATA2LSB, + [EI_VERSION] = EV_CURRENT +}; + +static int load_segment(void *ingenic, void *data, uint32_t base, uint32_t filesz, uint32_t memsz) { + uint32_t end = base + memsz, tail = memsz - filesz; + int ret = 0; + + printf("Loading segment: base 0x%08X, file 0x%08X, mem 0x%08X\n", base, filesz, memsz); + + if(end > ingenic_sdram_size(ingenic) + SDRAM_BASE - STAGE2_CODESIZE) { + fputs(" Segment doesn't fit into SDRAM\n", stderr); + + return -1; + } + + if(filesz && ingenic_load_sdram(ingenic, data, base, filesz) == -1) + return -1; + + if(tail) { + char *dummy_data = malloc(tail); + + if(dummy_data == NULL) + return -1; + + memset(dummy_data, 0, tail); + + ret = ingenic_load_sdram(ingenic, dummy_data, base + filesz, tail); + + free(dummy_data); + } + + return ret; +} + +static int load_elf_image(FILE *elf, void *ingenic, uint32_t *entry, uint32_t *end) { + Elf32_Ehdr ehdr; + Elf32_Phdr phdr; + int i, ret; + char *data; + + if(fread(&ehdr, 1, sizeof(Elf32_Ehdr), elf) != sizeof(Elf32_Ehdr)) { + if(feof(elf)) + errno = EINVAL; + + return -1; + } + + *entry = ehdr.e_entry; + + if(memcmp(ehdr.e_ident, valid_ident, EI_PAD) != 0 || ehdr.e_type != ET_EXEC || ehdr.e_machine != EM_MIPS || ehdr.e_version != EV_CURRENT + || ehdr.e_phoff == 0 || ehdr.e_phentsize != sizeof(Elf32_Phdr)) { + + fputs("Bad ELF identification\n", stderr); + + errno = EINVAL; + + return -1; + } + + fseek(elf, ehdr.e_phoff, SEEK_SET); + + *end = 0; + + for(i = 0; i < ehdr.e_phnum; i++) { + if(fread(&phdr, 1, sizeof(Elf32_Phdr), elf) != sizeof(Elf32_Phdr)) { + if(feof(elf)) + errno = EINVAL; + + return -1; + } + + if(phdr.p_type == PT_LOAD) { + data = malloc(phdr.p_filesz); + + if(data == NULL) + return -1; + + long save = ftell(elf); + fseek(elf, phdr.p_offset, SEEK_SET); + ret = fread(data, 1, phdr.p_filesz, elf); + fseek(elf, save, SEEK_SET); + + if(ret != phdr.p_filesz) { + free(data); + + if(feof(elf)) + errno = EINVAL; + + return -1; + } + + ret = load_segment(ingenic, data, phdr.p_paddr, phdr.p_filesz, phdr.p_memsz); + + free(data); + + if(ret == -1) + return -1; + + *end = max(phdr.p_paddr + phdr.p_memsz, *end); + } + } + + return 0; +} + + +static void trampoline_set(uint32_t *trampoline, int index, + uint32_t value) { + index *= 2; + + trampoline[index] = (trampoline[index] & 0xFFFF0000) | ((value & 0xFFFF0000) >> 16); + trampoline[index + 1] = (trampoline[index + 1] & 0xFFFF0000) | (value & 0x0000FFFF); +} + +static int load_args(void *ingenic, uint32_t base, const char *filename, + const char *const *args, int *pargc, uint32_t *end) { + + size_t total_len = 0; + int argc = 0; + + for(int i = 0; args[i]; i++) { + size_t len = strlen(args[i]); + total_len += len + 1; + + for(int j = 0; j < len; j++) + if(args[i][j] == ' ') + argc++; + + argc++; + } + + *pargc = argc; + *end = base + total_len + sizeof(uint32_t) * argc; + + uint32_t *buf = malloc(sizeof(uint32_t) * argc + total_len); + if(buf == NULL) + return -1; + + char *cmdline = (char *)(buf + argc); + size_t off = 0; + + for(int i = 0; args[i]; i++) { + size_t len = strlen(args[i]); + memcpy(cmdline + off, args[i], len + 1); + + if(args[i + 1]) + cmdline[off + len] = ' '; + + off += len + 1; + } + + printf("Compiled cmdline: '%s'\n", cmdline); + + uint32_t offset = 0; + char *ptr = cmdline; + + for(int i = 0; i < argc; i++) { + buf[i] = base + argc * sizeof(uint32_t) + offset; + + ptr = strchr(ptr, ' '); + + if(ptr == NULL) + break; + + *ptr++ = 0; + offset = ptr - cmdline; + } + + int ret = load_segment(ingenic, + buf, + base, + sizeof(uint32_t) * argc + total_len, + sizeof(uint32_t) * argc + total_len); + free(buf); + + return ret; +} + +int load_elf(void *ingenic, + const char *filename, + const char *args, + const char *initrd) { + + uint32_t entry, end, trampoline_base, args_base; + uint32_t initrd_base, initrd_size; + + int argc; + const char *all_args[4] = { filename, args, NULL, NULL }; + char initrd_args[64]; + + printf( + "Loading kernel %s:\n" + " Command line: '%s'\n", + filename, + args + ); + + FILE *elf = fopen(filename, "rb"); + + if(elf == NULL) + return -1; + + int ret = load_elf_image(elf, ingenic, &entry, &end); + + fclose(elf); + + if(ret == -1) + return -1; + + if(initrd) { + struct stat statbuf; + + initrd_base = ALIGN(end); + + if(stat(initrd, &statbuf) == -1) + return -1; + + initrd_size = statbuf.st_size; + + printf("Loading initrd to 0x%08X, size 0x%08X\n", + initrd_base, initrd_size); + + + end = initrd_base + initrd_size; + + if(end > ingenic_sdram_size(ingenic) + SDRAM_BASE - STAGE2_CODESIZE) { + fputs(" Initrd doesn't fit into SDRAM\n", stderr); + + return -1; + } + + if(ingenic_load_sdram_file(ingenic, initrd_base, initrd) == -1) + return -1; + + snprintf(initrd_args, sizeof(initrd_args), + "rd_start=0x%08X rd_size=0x%08X", + initrd_base, initrd_size); + + all_args[2] = initrd_args; + } + + args_base = ALIGN(end); + + if(load_args(ingenic, args_base, filename, all_args, &argc, &end) == -1) + return -1; + + trampoline_base = ALIGN(end); + end = trampoline_base + sizeof(trampoline_template); + + uint32_t *trampoline = malloc(sizeof(trampoline_template)); + if(trampoline == NULL) + return -1; + + memcpy(trampoline, trampoline_template, sizeof(trampoline_template)); + trampoline_set(trampoline, TRAMP_ARGC, argc); + trampoline_set(trampoline, TRAMP_ARGV, args_base); + trampoline_set(trampoline, TRAMP_ARG2, 0); + trampoline_set(trampoline, TRAMP_ARG3, 0); + trampoline_set(trampoline, TRAMP_ENTRY, entry); + + ret = load_segment(ingenic, trampoline, trampoline_base, sizeof(trampoline_template), sizeof(trampoline_template)); + + free(trampoline); + + if(ret == -1) + return -1; + + printf("Image end: 0x%08X, entry: 0x%08X\n", end, entry); + + //return 0; + return ingenic_go(ingenic, trampoline_base); +} + diff --git a/src/ingenic.c b/src/ingenic.c index 60f8096..2c35d61 100644 --- a/src/ingenic.c +++ b/src/ingenic.c @@ -67,40 +67,6 @@ static const struct { { NULL, 0 } }; -static void hexdump(const void *data, size_t size) { - const unsigned char *bytes = data; - - for(int i = 0; i < size; i+= 16) { - debug(LEVEL_DEBUG, "%04X ", i); - - int chunk_size = size - i; - if(chunk_size > 16) - chunk_size = 16; - - for(int j = 0; j < chunk_size; j++) { - debug(LEVEL_DEBUG, "%02X ", bytes[i + j]); - - if(j == 7) - debug(LEVEL_DEBUG, " "); - } - - for(int j = 0; j < 16 - chunk_size; j++) { - debug(LEVEL_DEBUG, " "); - - if(j == 8) - debug(LEVEL_DEBUG, " "); - } - - debug(LEVEL_DEBUG, "|"); - - for(int j = 0; j < chunk_size; j++) { - debug(LEVEL_DEBUG, "%c", isprint(bytes[i + j]) ? bytes[i + j] : '.'); - } - - debug(LEVEL_DEBUG, "|\n"); - } -} - static uint32_t ingenic_probe(void *usb_hndl) { char magic[9]; diff --git a/src/usbboot_cmdset.c b/src/usbboot_cmdset.c index af6b43a..6d9ec2c 100644 --- a/src/usbboot_cmdset.c +++ b/src/usbboot_cmdset.c @@ -23,10 +23,12 @@ #include "shell.h" #include "app_config.h" #include "ingenic.h" +#include "elfldr.h" static int usbboot_boot(shell_context_t *ctx, int argc, char *argv[]); static int usbboot_load(shell_context_t *ctx, int argc, char *argv[]); static int usbboot_go(shell_context_t *ctx, int argc, char *argv[]); +static int usbboot_load_kernel(shell_context_t *ctx, int argc, char *argv[]); static int usbboot_nquery(shell_context_t *ctx, int argc, char *argv[]); static int usbboot_ndump(shell_context_t *ctx, int argc, char *argv[]); static int usbboot_nerase(shell_context_t *ctx, int argc, char *argv[]); @@ -38,6 +40,7 @@ const shell_command_t usbboot_cmdset[] = { { "boot", "Reconfigure stage2", usbboot_boot, NULL }, { "load", "Load file to SDRAM", usbboot_load, " " }, { "go", "Jump to
", usbboot_go, "
" }, + { "load_kernel", "Load ELF kernel and initrd to memory", usbboot_load_kernel, " [INITRAMFS]" }, { "nquery", "Query NAND information", usbboot_nquery, "" }, { "ndump", "Dump NAND to file", usbboot_ndump, " " }, @@ -151,3 +154,9 @@ static int usbboot_nload(shell_context_t *ctx, int argc, char *argv[]) { return ret; } + +static int usbboot_load_kernel(shell_context_t *ctx, int argc, char *argv[]) { + return load_elf(shell_device(ctx), argv[1], argv[2], + argc == 4 ? argv[3] : NULL); +} +