#include "types.h"
#include "t124.h"

typedef void (*ep1_x_imm_t)(void *buffer, u32 size, u32 *num_xfer);
ep1_x_imm_t usb_read = (ep1_x_imm_t) ( BOOTROM_EP1_OUT_READ_IMM | 1 );
ep1_x_imm_t usb_write = (ep1_x_imm_t) ( BOOTROM_EP1_IN_WRITE_IMM | 1 );
// int (*usb_write)(void*addr, uint32_t size, void *return_addr) = (void*)BOOTROM_EP1_IN_WRITE_IMM;
// int (*usb_read)(int, int, int) = (void*)BOOTROM_EP1_OUT_READ_IMM;

int mystrlen(char *data) {
    int i=0;
    while(1) {
        if(data[i++] == '\0'){
            break;
        }
    }
    return i-1;
}

void usb_log(char * msg, uint32_t * error){
    usb_write(msg, mystrlen(msg), error);
}

// #define usb_log(msg, error) usb_write(msg, mystrlen(msg), error);

__attribute__((section(".init")))
void payload_main() {
    uint32_t error_code;
    uint32_t mem_off;
    uint32_t mem_sz;
    uint32_t blk_sz;
    char data[12];
    u8 *buffer = (u8*)0x40020000;

    usb_log("cmd_handler", &error_code);
    while(1){
        usb_read(&data, 4, &error_code);
        if(data[0] == 'P' && data[1] == 'E' && data[2] == 'E' && data[3] == 'K') {
            usb_write(&data, 4, &error_code);
            usb_read(&data, 8,&error_code); // Receive uint64_t size and and uint32_t offset
            mem_off = *(uint32_t *)data;
            mem_sz = *(uint32_t *)(data+4);
            for(unsigned int i=0;i<mem_sz;i+=0x200) {
                if((mem_sz - i) < 0x200) {
                    blk_sz = mem_sz - i;
                } else {
                    blk_sz = 0x200;
                }
                usb_write((void *)(mem_off+i), blk_sz, &error_code);
                usb_read(&data, 4, &error_code);
                if(!(data[0] == 'A' && data[1] == 'C' && data[2] == 'K')) {
                    break;
                }
            }
        }
        else if(data[0] == 'P' && data[1] == 'O' && data[2] == 'K' && data[3] == 'E') {
            usb_write(&data, 4, &error_code);
            usb_read(&data, 8, &error_code); // Receive uint64_t size and and uint32_t offset
            mem_off = *(uint32_t *)data;
            mem_sz = *(uint32_t *)(data+4);

            for(unsigned int i=0;i<mem_sz;i+=0x200) {
                if((mem_sz - i) < 0x200) {
                    blk_sz = mem_sz - i;
                } else {
                    blk_sz = 0x200;
                }
                usb_read((void *) (mem_off + i), blk_sz, &error_code);
                usb_log("OK", &error_code);
                usb_read(&data, 4, &error_code);

                if(!(data[0] == 'A' && data[1] == 'C' && data[2] == 'K')) {
                    break;
                }
            }
        }
        usb_log("done", &error_code);
    }
}