diff --git a/src/include/rom/read_bytes.h b/src/include/rom/read_bytes.h index e786a31e65..3c50d25496 100644 --- a/src/include/rom/read_bytes.h +++ b/src/include/rom/read_bytes.h @@ -5,8 +5,18 @@ typedef long byte_offset_t; +typedef struct tag_t { + char signature[5]; + unsigned char block_count; + unsigned long length; + char data[6]; +} __attribute__ ((packed)) tag_head; + struct stream { int (*init)(void); + int (*init_tags)(void); + void (*get_tags)(void *buf); + void (*load_tag)(int tag); byte_offset_t (*read)(void *vdest, byte_offset_t count); byte_offset_t (*skip)(byte_offset_t count); void (*fini)(void); diff --git a/src/rom/rom_fill_inbuf.c b/src/rom/rom_fill_inbuf.c index 8398a306d0..164a73dd2a 100644 --- a/src/rom/rom_fill_inbuf.c +++ b/src/rom/rom_fill_inbuf.c @@ -7,15 +7,21 @@ #include #ifndef ZKERNEL_START -#define ZKERNEL_START 0xfff80000 +#define ZKERNEL_START 0xfff00000 #endif #ifndef ZKERNEL_MASK #define ZKERNEL_MASK 0x0000ffff #endif +#ifndef _ROMTOP +#define _ROMTOP 0xfffffff0 +#endif + /* The inbuf copy option has been killed... */ +#define K64 (64 * 1024) + static unsigned char *zkernel_start = (unsigned char *)ZKERNEL_START; static unsigned long zkernel_mask = ZKERNEL_MASK; @@ -23,23 +29,20 @@ static unsigned char *nvram; static int block_count; static int block_offset; -#define K64 (64 * 1024) +static unsigned int max_block = (_ROMTOP - ZKERNEL_START)/K64; -static int init_bytes(void) + +static int init_bytes(void); + +int rom_seek(unsigned long int where) { - block_count = 0; - block_offset = 0; - nvram = zkernel_start; + block_count = where / K64; + block_offset = where - (block_count * K64); + nvram = zkernel_start + (K64 * block_count); - printk_debug("%6d:%s() - zkernel_start:0x%08x " - "zkernel_mask:0x%08x\n", - __LINE__, __FUNCTION__, - zkernel_start, zkernel_mask); - - return 0; + return(where); } - static void fini_bytes(void) { return; @@ -56,14 +59,9 @@ static byte_offset_t rom_read_bytes(int cp, void *vdest, byte_offset_t count) block_count++; nvram+= K64; } - if (!(zkernel_mask & (1 << block_count))) { - printk_debug("%6d:%s() - skipping block %d\n", - __LINE__, __FUNCTION__, block_count); - continue; - } - if (block_count > 31) { - printk_emerg( "%6d:%s() - overflowed source buffer\n", - __LINE__, __FUNCTION__); + if (block_count > max_block) { + printk_emerg( "%6d:%s() - overflowed source buffer. max_block = %u\n", + __LINE__, __FUNCTION__, max_block); return bytes; } length = K64 - block_offset; @@ -90,8 +88,132 @@ static byte_offset_t read_bytes(void *vdest, byte_offset_t count) return rom_read_bytes(1, vdest, count); } +static int tag_count=0; +static int tag_blocks=0; +unsigned long tags[32]; + +void rom_load_tag(int tag) +{ + tag_head current; + int i; + + printk_info("Loading tag %u @ %08x\n", tag, tags[tag]); + + if(tag < tag_count) { + rom_seek( tags[tag] ); + read_bytes(¤t, sizeof(tag_head) ); + + printk_info("Tag contents: "); + for(i=0; i<16; i++) + printk_info("%02x ", ( (char *) ¤t)[i] ); + + printk_info("\n"); + + printk_info("TAG block count = %u\n", current.block_count); + + skip_bytes( current.block_count * sizeof(tag_head) ); + } + +} + +int strncmp(char *subject, char *target, int count) +{ + int i; + + for(i=0; iblock_count, (int) ptr); + count = ptr->block_count; + ptr++; + if(count) { + read_bytes(ptr, sizeof(tag_head) * count); + ptr += count; + } + } +} + + +int search_tags(void) +{ + tag_head current; + + + rom_seek(0); + tag_count=0; + tag_blocks=0; + + printk_info("Searching for %u byte tags\n", sizeof(tag_head) ); + + if(!tag_count) { + while( read_bytes(¤t, sizeof(tag_head)) == sizeof(tag_head) ) { + if(strncmp(current.signature, "$TAG$", 5)) + continue; + + // we found a tag, now look at it + tag_blocks++; + tags[tag_count++] = K64 * block_count + block_offset - sizeof(tag_head); + + printk_info("Found tag at %08x block_count = %u\n", tags[tag_count-1], current.block_count); + + if(current.block_count >0) { + skip_bytes( current.block_count * sizeof(tag_head) ); + tag_blocks += current.block_count; + } + + } + } + + if(tag_count) { + rom_load_tag(0); + } else + rom_seek(0); + + return(tag_blocks); +} + +static int init_bytes(void) +{ + int i; + + block_count = 0; + block_offset = 0; + nvram = zkernel_start; + + printk_debug("%6d:%s() - zkernel_start:0x%08x " + "zkernel_mask:0x%08x\n", + __LINE__, __FUNCTION__, + zkernel_start, zkernel_mask); + + i = search_tags(); + + printk_info("init_bytes found %u tags\n", i); + + return 0; +} + static struct stream rom_stream __stream = { .init = init_bytes, + .init_tags = search_tags, + .get_tags = rom_get_tags, + .load_tag = rom_load_tag, .read = read_bytes, .skip = skip_bytes, .fini = fini_bytes,