#include <os_internal.h>
#include "controller.h"
#include "siint.h"
#include <rcp.h>

static void __osPackEepWriteData(u8 address, u8 *buffer);
s32 osEepromWrite(OSMesgQueue *mq, u8 address, u8 *buffer)
{

    s32 ret;
    int i;
    u16 type;
    u8 *ptr;
    __OSContEepromFormat eepromformat;
    OSContStatus sdata;

    ret = 0;
    ptr = (u8 *)&__osEepPifRam.ramarray;
    __osSiGetAccess();
    ret = __osEepStatus(mq, &sdata); //why is this duplicated?
    ret = __osEepStatus(mq, &sdata);

    type = sdata.type & (CONT_EEPROM | CONT_EEP16K);

    if (ret != 0)
    {
        __osSiRelAccess();
        return CONT_NO_RESPONSE_ERROR;
    }
    switch (type)
    {
    case CONT_EEPROM:
        if (address > EEPROM_MAXBLOCKS)
        {
            __osSiRelAccess();
            return -1;
        }

        break;
    case CONT_EEPROM | CONT_EEP16K:
        if (address > EEP16K_MAXBLOCKS) //not technically possible
        {
            __osSiRelAccess();
            return -1;
        }
        break;
    default:
        __osSiRelAccess();
        return CONT_NO_RESPONSE_ERROR;
        break;
    }
    while (sdata.status & CONT_EEPROM_BUSY)
    {
        __osEepStatus(mq, &sdata);
    }
    __osPackEepWriteData(address, buffer);
    ret = __osSiRawStartDma(OS_WRITE, &__osEepPifRam); //send command to pif
    osRecvMesg(mq, NULL, OS_MESG_BLOCK);

    for (i = 0; i < ARRLEN(__osEepPifRam.ramarray) + 1; i++) {
        __osEepPifRam.ramarray[i] = 0xFF;
    }

    __osEepPifRam.pifstatus = 0;

    ret = __osSiRawStartDma(OS_READ, &__osEepPifRam); //recv response
    __osContLastCmd = CONT_CMD_WRITE_EEPROM;
    osRecvMesg(mq, NULL, OS_MESG_BLOCK);
    for (i = 0; i < 4; i++) //skip the first 4 bytes
    {
        ptr++;
    }
    eepromformat = *(__OSContEepromFormat *)ptr;

    //probably indicates an error, from PIF
    ret = CHNL_ERR(eepromformat); //TODO: remove magic constants

    __osSiRelAccess();
    return ret;
}
static void __osPackEepWriteData(u8 address, u8 *buffer)
{
    u8 *ptr;
    __OSContEepromFormat eepromformat;
    int i;
    ptr = (u8 *)&__osEepPifRam.ramarray;
    for (i = 0; i < ARRLEN(__osEepPifRam.ramarray) + 1; i++)
    {
        __osEepPifRam.ramarray[i] = CONT_CMD_NOP;
    }
    __osEepPifRam.pifstatus = CONT_CMD_EXE;

    eepromformat.txsize = CONT_CMD_WRITE_EEPROM_TX;
    eepromformat.rxsize = CONT_CMD_WRITE_EEPROM_RX;
    eepromformat.cmd = CONT_CMD_WRITE_EEPROM;
    eepromformat.address = address;
    for (i = 0; i < ARRLEN(eepromformat.data); i++)
    {
        eepromformat.data[i] = *buffer++;
    }
    for (i = 0; i < 4; i++) //skip the first 4 bytes
    {
        *ptr++ = 0;
    }
    *(__OSContEepromFormat *)(ptr) = eepromformat;
    ptr += sizeof(__OSContEepromFormat);
    ptr[0] = CONT_CMD_END;
}
s32 __osEepStatus(OSMesgQueue *mq, OSContStatus *data)
{
    s32 ret;
    int i;
    u8 *ptr;
    __OSContRequesFormat requestformat;

    //addiu	sp,sp,-48
    //lui	t6,__osEepPifRam
    //addiu	t6,t6,__osEepPifRam
    //sw	ra,20(sp)
    //sw	a0,48(sp)
    //sw	a1,52(sp)
    //sw	zero,44(sp)
    ret = 0;
    //sw	t6,36(sp)
    ptr = (u8 *)__osEepPifRam.ramarray;
    //could be bset or bzero intrinsic
    for (i = 0; i < ARRLEN(__osEepPifRam.ramarray) + 1; i++) //still has the buffer overflow =/, just sets status to 0
    {
        __osEepPifRam.ramarray[i] = 0;
    }
    __osEepPifRam.pifstatus = CONT_CMD_EXE;
    ptr = (u8 *)__osEepPifRam.ramarray;
    for (i = 0; i < 4; i++) //zero 4 bytes?
        *ptr++ = 0;
    requestformat.dummy = CONT_CMD_NOP;
    requestformat.txsize = CONT_CMD_REQUEST_STATUS_TX;
    requestformat.rxsize = CONT_CMD_REQUEST_STATUS_RX;
    requestformat.cmd = CONT_CMD_REQUEST_STATUS;
    requestformat.typeh = CONT_CMD_NOP;
    requestformat.typel = CONT_CMD_NOP;
    requestformat.status = CONT_CMD_NOP;
    requestformat.dummy1 = CONT_CMD_NOP;
    *(__OSContRequesFormat *)ptr = requestformat;
    ptr += sizeof(__OSContRequesFormat);
    *ptr = CONT_CMD_END;

    ret = __osSiRawStartDma(OS_WRITE, &__osEepPifRam);
    osRecvMesg(mq, NULL, OS_MESG_BLOCK);
    __osContLastCmd = CONT_CMD_REQUEST_STATUS;
    ret = __osSiRawStartDma(OS_READ, &__osEepPifRam);
    osRecvMesg(mq, NULL, OS_MESG_BLOCK);

    if (ret != 0)
        return ret;

    ptr = (u8 *)&__osEepPifRam;
    for (i = 0; i < 4; i++)
        *ptr++ = 0;

    requestformat = *(__OSContRequesFormat *)ptr;
    data->errno = CHNL_ERR(requestformat);
    data->type = (requestformat.typel << 8) | requestformat.typeh;
    data->status = requestformat.status;
    if (data->errno != 0)
        return data->errno;
    return 0;
}