#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <unistd.h>
#include <stdlib.h>
#include <inttypes.h>
#include <string.h>
#include <ctype.h>
#include <time.h>

/* taken from original code by Rafaël Carré ? */
#if 1 /* ANSI colors */

#   define color(a) printf("%s",a)
char OFF[]      = { 0x1b, 0x5b, 0x31, 0x3b, '0', '0', 0x6d, '\0' };

char GREY[]     = { 0x1b, 0x5b, 0x31, 0x3b, '3', '0', 0x6d, '\0' };
char RED[]      = { 0x1b, 0x5b, 0x31, 0x3b, '3', '1', 0x6d, '\0' };
char GREEN[]    = { 0x1b, 0x5b, 0x31, 0x3b, '3', '2', 0x6d, '\0' };
char YELLOW[]   = { 0x1b, 0x5b, 0x31, 0x3b, '3', '3', 0x6d, '\0' };
char BLUE[]     = { 0x1b, 0x5b, 0x31, 0x3b, '3', '4', 0x6d, '\0' };

#else
    /* disable colors */
#   define color(a)
#endif

#define bug(...) do { fprintf(stderr,"ERROR: "__VA_ARGS__); exit(1); } while(0)
#define bugp(a) do { perror("ERROR: "a); exit(1); } while(0)

#define get32le(a) ((uint32_t) \
    ( buf[a+3] << 24 | buf[a+2] << 16 | buf[a+1] << 8 | buf[a] ))
#define get16le(a) ((uint16_t)( buf[a+1] << 8 | buf[a] ))

#define assert(a) do { if(!(a)) { fprintf(stderr,"Assertion \"%s\" failed in %s() line %d!\n\nPlease send us your firmware!\n",#a,__func__,__LINE__); exit(1); } } while(0)

#define ROUND_UP(val, round) ((((val) + (round) - 1) / (round)) * (round))

/* globals */

size_t sz;  /* file size */
uint8_t *buf; /* file content */
#define PREFIX_SIZE     128
char out_prefix[PREFIX_SIZE]; /* output prefix */

/* definitions */
struct sb_instruction_header_t
{
    uint32_t inst;
} __attribute__((packed));

#define SB_INST_OP(inst)    (((inst) >> 8) & 0xff)
#define SB_INST_UNK(inst)   ((inst) & 0xff)

#define SB_INST_FINISH  0x0
#define SB_INST_LOAD    0x2
#define SB_INST_FILL    0x3
#define SB_INST_CALL    0x5

struct sb_instruction_load_t
{
    struct sb_instruction_header_t hdr;
    uint32_t addr;
    uint32_t len;
    uint32_t crc;
} __attribute__((packed));

struct sb_instruction_fill_t
{
    struct sb_instruction_header_t hdr;
    uint32_t addr;
    uint32_t len;
    uint32_t pattern;
} __attribute__((packed));

struct sb_instruction_call_t
{
    struct sb_instruction_header_t hdr;
    uint32_t addr;
    uint32_t arg;
} __attribute__((packed));

static void *xmalloc(size_t s) /* malloc helper */
{
    void * r = malloc(s);
    if(!r) bugp("malloc");
    return r;
}

static void check(unsigned long filesize)
{
}

static void extract(unsigned long filesize)
{
    size_t pos = 0;
    while(pos < filesize)
    {
        struct sb_instruction_header_t *hdr = (struct sb_instruction_header_t *)&buf[pos];
        if(SB_INST_OP(hdr->inst) == SB_INST_LOAD)
        {
            struct sb_instruction_load_t *load = (struct sb_instruction_load_t *)&buf[pos];
            color(RED);
            printf("LOAD");
            color(OFF);printf(" | ");
            color(BLUE);
            printf("addr=%#08x", load->addr);
            color(OFF);printf(" | ");
            color(GREEN);
            printf("len=%#08x", load->len);
            color(OFF);printf(" | ");
            color(YELLOW);
            printf("crc=%#08x\n", load->crc);
            color(OFF);

            pos += load->len + sizeof(struct sb_instruction_load_t);
            // unsure about rounding
            pos = ROUND_UP(pos, 16);
        }
        else if(SB_INST_OP(hdr->inst) == SB_INST_FILL)
        {
            struct sb_instruction_fill_t *fill = (struct sb_instruction_fill_t *)&buf[pos];
            color(RED);
            printf("FILL");
            color(OFF);printf(" | ");
            color(BLUE);
            printf("addr=%#08x", fill->addr);
            color(OFF);printf(" | ");
            color(GREEN);
            printf("len=%#08x", fill->len);
            color(OFF);printf(" | ");
            color(YELLOW);
            printf("pattern=%#08x\n", fill->pattern);
            color(OFF);

            pos += sizeof(struct sb_instruction_fill_t);
            // fixme: useless as pos is a multiple of 16 and fill struct is 4-bytes wide ?
            pos = ROUND_UP(pos, 16);
        }
        else if(SB_INST_OP(hdr->inst) == SB_INST_CALL)
        {
            struct sb_instruction_call_t *call = (struct sb_instruction_call_t *)&buf[pos];
            color(RED);
            printf("CALL");
            color(OFF);printf(" | ");
            color(BLUE);
            printf("addr=%#08x", call->addr);
            color(OFF);printf(" | ");
            color(GREEN);
            printf("arg=%#08x\n", call->arg);
            color(OFF);

            pos += sizeof(struct sb_instruction_call_t);
            // fixme: useless as pos is a multiple of 16 and call struct is 4-bytes wide ?
            pos = ROUND_UP(pos, 16);
        }
        else
            bug("Unknown instruction %#08x at address %#08lx\n", hdr->inst, (unsigned long)pos);
    }
}

int main(int argc, char **argv)
{
    int fd;
    struct stat st;
    if(argc != 2 && argc != 3)
        bug("Usage: %s <firmware> [<out prefix>]\n",*argv);
    if(argc == 3)
        snprintf(out_prefix, PREFIX_SIZE, "%s", argv[2]);
    else
        strcpy(out_prefix, "");

    if( (fd = open(argv[1],O_RDONLY)) == -1 )
        bugp("opening firmware failed");

    if(fstat(fd,&st) == -1)
        bugp("firmware stat() failed");
    sz = st.st_size;

    buf=xmalloc(sz);
    if(read(fd,buf,sz)!=(ssize_t)sz) /* load the whole file into memory */
        bugp("reading firmware");

    close(fd);

    check(st.st_size);  /* verify header and checksums */
    extract(st.st_size);    /* split in blocks */

    color(OFF);

    free(buf);
    return 0;
}