#define BOOTROM_EP1_IN_WRITE_IMM    0x001065C0
#define BOOTROM_EP1_OUT_READ_IMM    0x00106612

#include "t124.h"

#ifndef _TYPES_H_
#define _TYPES_H_

typedef signed char s8;
typedef signed short s16;
typedef signed int s32;
typedef signed long long s64;
typedef unsigned char u8;
typedef unsigned short u16;
typedef unsigned int u32;
typedef unsigned long long u64;
typedef volatile u8 vu8;
typedef volatile u16 vu16;
typedef volatile u32 vu32;
typedef volatile u64 vu64;
typedef u32 size_t;
typedef u32 uintptr_t;

#endif


typedef void (*ep1_x_imm_t)(void *buffer, uint32_t size, uint32_t *num_xfer);
ep1_x_imm_t usb_recv = (ep1_x_imm_t) ( BOOTROM_EP1_OUT_READ_IMM | 1 );
ep1_x_imm_t usb_send = (ep1_x_imm_t) ( BOOTROM_EP1_IN_WRITE_IMM | 1 );


void send(void *buffer, uint32_t size, uint32_t *num_xfer){
    usb_send(buffer, size, num_xfer);
}

int recv(void *buffer, uint32_t size, uint32_t *num_xfer){
    usb_recv(buffer, size, num_xfer);
    return (int)&num_xfer;
}

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){
    send(msg, mystrlen(msg), error);
}

void recv_data(void *data, uint32_t len) {
    uint32_t rx_err_code;
    uint32_t xfer = 0;
    while(1) {
        recv(data, len, &xfer);
        if(xfer >= len) {
            break;            
        }
    }
}

static inline u32 read32(uintptr_t addr) {
    return *(vu32 *)addr;
}

static inline void write32(uintptr_t addr, u32 val) {
    *(vu32 *)addr = val;
}

static inline void or32(uintptr_t addr, u32 val) {
    write32(addr, read32(addr) | val);
}

void enter_rcm() {
    // Sets RCM strap and reboots into RCM
    or32(PMC_BASE + PMC_SCRATCH0, PMC_SCRATCH0_MODE_RCM);
    or32(PMC_BASE + PMC_CNTRL, PMC_CNTRL_MAIN_RST);
}

void concrete_main(uint32_t debugger){
    enter_rcm();
    // log register r0
    // uint32_t reg_val;
    // uint32_t tx_err_code;
    // uint32_t *tbuf = (uint32_t*)0x40013000;

    // asm volatile("mov %0, r0" : "=r" (reg_val));
    // send(&reg_val, sizeof(reg_val), &tx_err_code);
    // tbuf[0] = reg_val;

    // asm volatile("mov %0, r1" : "=r" (reg_val));
    // send(&reg_val, sizeof(reg_val), &tx_err_code);
    // tbuf[1] = reg_val;

    // asm volatile("mov %0, r2" : "=r" (reg_val));
    // send(&reg_val, sizeof(reg_val), &tx_err_code);
    // tbuf[2] = reg_val;

    // asm volatile("mov %0, r3" : "=r" (reg_val));
    // send(&reg_val, sizeof(reg_val), &tx_err_code);
    // tbuf[3] = reg_val;

    // asm volatile("mov %0, r4" : "=r" (reg_val));
    // send(&reg_val, sizeof(reg_val), &tx_err_code);
    // tbuf[4] = reg_val;

    // asm volatile("mov %0, r5" : "=r" (reg_val));
    // send(&reg_val, sizeof(reg_val), &tx_err_code);
    // tbuf[5] = reg_val;

    // asm volatile("mov %0, r6" : "=r" (reg_val));
    // send(&reg_val, sizeof(reg_val), &tx_err_code);
    // tbuf[6] = reg_val;

    // asm volatile("mov %0, r7" : "=r" (reg_val));
    // send(&reg_val, sizeof(reg_val), &tx_err_code);
    // tbuf[7] = reg_val;
    // usb_log("END", &debugger);

    
}