#include "risc_dsp_caller.h"
#include "erlcommon.h"
#include "libdsp.h"
#include "overlay.h"

#include <stdlib.h>
#include <string.h>

static enum DSP_TASK_MODE dsp_cluster_mode = DSP_TASK_SINGLE;
//
// determines whether task was ever started
//
static int isTaskRunning(struct dsp_task_info * pTask)
{
    return (pTask->status==DSP_TASK_RUNNING
            ||
            pTask->status==DSP_TASK_BLOCKED);
}

static int isTaskTerminated(struct dsp_task_info * pTask)
{
    return (pTask->status==DSP_TASK_TERMINATED);
}

//
//
//
unsigned int remap_addr_risc2dsp_data(struct dsp_task_info task, void *addr){
    int ncore = task.dsp_num;
    unsigned int u_addr = (unsigned int)(addr);
    unsigned int u_base_src = (unsigned int)task.ctx->__data_src;

    unsigned int u_base_dst =  (((unsigned int)task.ctx->__data_dst)&0x1ffff);

    unsigned offset = (u_addr-u_base_src+u_base_dst)>>2;

    switch(ncore)
    {
    case 0:
        // return offset;
        break;
    case 1:
        // return offset+0x8000;
        offset = offset + 0x8000;
        break;
    }
    return offset;
}

unsigned int remap_addr_risc2dsp_text(struct dsp_task_info task, void *addr){
    int ncore = task.dsp_num;
    unsigned int u_addr = (unsigned int)(addr);
    unsigned int u_base_src = (unsigned int)task.ctx->__text_src;

    unsigned int u_base_dst =  (((unsigned int)task.ctx->__text_dst)&0x1ffff);

    unsigned offset = (u_addr-u_base_src+u_base_dst)>>2;

    switch(ncore)
    {
    case 0:
        // return offset;
        break;
    case 1:
        // return offset+0x8000;
        offset = offset + 0x8000;
    }
    return offset;
}

//
// Setup dsp cores running mode (single, paralle or syncronized)
//
void risc_dsp_set_mode(enum DSP_TASK_MODE mode)
{
    dsp_cluster_mode = mode;
}

//
// prepare values for
//
enum ERL_ERROR risc_dsp_init_task(struct dsp_task_info* pTask, unsigned int fnAddress)
{
    if (!pTask)
    {
        return ERL_MEMORY_ALLOC;
    }
    enum ERL_ERROR err_code =   ERL_NO_ERROR;
    // initialize context with empty values
    int i = 0;
    for (i=0; i<DSP_TASK_MAX_PARAMETERS; i+=1)
    {
        pTask->register_ctx[i].reg_offset = OFF_NONE;
        pTask->register_ctx[i].reg_value  = 0;
    }

    pTask->dsp_pc    = fnAddress;
    struct ovl_ctx*  ovl_descr = find_overlay_ctx(pTask->dsp_pc);
    if(ovl_descr)
    {
        pTask->ctx  = (struct ovl_ctx*)malloc(sizeof(struct ovl_ctx));
        memcpy(pTask->ctx, ovl_descr, sizeof(struct ovl_ctx));
    } else
    {
        pTask->ctx = NULL;
        err_code = ERL_OVERLAY_ERROR;
        return err_code;
    }

    pTask->dsp_num   = pTask->ctx->dsp_info->num;
    if(pTask->dsp_num == 0)         pTask->dsp_regs = (struct _dsp_regs*)_REGS0;
    else if(pTask->dsp_num==1)      pTask->dsp_regs = (struct _dsp_regs*)_REGS1;
    else                            { /*TODO: ret error */  }
    pTask->dsp_comm_regs = (common_regs*)_REGSCM;
    pTask->qstr_stop = pTask->ctx->dsp_info->qstr_stop;
    pTask->dsp_sp_value = 0x7ffc;
    pTask->dsp_fp_value = 0x7ffc;
    //
    // split memory for programs if two cores are running
    if((dsp_cluster_mode&DSP_TASK_SYNC)||
       (dsp_cluster_mode&DSP_TASK_PARALLEL))
    {
        if(pTask->dsp_num == 0)
        {
            pTask->dsp_sp_value = 0x3ffc;
            pTask->dsp_fp_value = 0x3ffc;
        }

        // take the gap between xyram0 and xyram1 into account
        unsigned xyram1_size = pTask->ctx->dsp_info->xyram_size;
        unsigned xyram1_end = pTask->ctx->dsp_info->xyram_base + xyram1_size;
                              ;
        if(pTask->dsp_num==1 && (unsigned)pTask->ctx->__data_dst >= xyram1_end)
        {
            pTask->ctx->__data_dst = (unsigned*)((unsigned)pTask->ctx->__data_dst
                                      - xyram1_size);
        }
        if(pTask->dsp_num==1 && (unsigned)pTask->ctx->__bss_dst >= xyram1_end)
        {
            pTask->ctx->__bss_dst -= xyram1_size;
        }
    }
    pTask->dsp_pc       = (pTask->dsp_pc -
                (unsigned int)((*(pTask->ctx)).__text_src) +
                              ((*(pTask->ctx)).__text_offset))>>2;

    pTask->status = DSP_TASK_READY;

    return err_code;
}

enum ERL_ERROR risc_dsp_init_asm_task(struct dsp_task_info* pTask, unsigned int fnAddress,
        reg_pair _register_ctx[], int n_register_ctx)
{
    if (!pTask)
    {
        return ERL_MEMORY_ALLOC;
    }
    enum ERL_ERROR err_code =   ERL_NO_ERROR;
    // initialize context with empty values
    int i = 0;
    for (i=0; i<DSP_TASK_MAX_PARAMETERS; i+=1)
    {
        if(i<n_register_ctx)
        {
            pTask->register_ctx[i].reg_offset = _register_ctx[i].reg_offset;
            pTask->register_ctx[i].reg_value  = _register_ctx[i].reg_value;
        } else
        {
            pTask->register_ctx[i].reg_offset = OFF_NONE;
            pTask->register_ctx[i].reg_value  = 0;
        }
    }

    pTask->dsp_pc    = fnAddress;
    struct ovl_ctx*  ovl_descr = find_overlay_ctx(pTask->dsp_pc);
    if(ovl_descr)
    {
        pTask->ctx  = (struct ovl_ctx*)malloc(sizeof(struct ovl_ctx));
        memcpy(pTask->ctx, ovl_descr, sizeof(struct ovl_ctx));
    } else
    {
        pTask->ctx = NULL;
        err_code = ERL_OVERLAY_ERROR;
        return err_code;
    }

    pTask->dsp_num   = pTask->ctx->dsp_info->num;
    if(pTask->dsp_num == 0)         pTask->dsp_regs = (struct _dsp_regs*)_REGS0;
    else if(pTask->dsp_num==1)      pTask->dsp_regs = (struct _dsp_regs*)_REGS1;
    else                            { /*TODO: ret error */  }
    pTask->dsp_comm_regs = (common_regs*)_REGSCM;
    pTask->qstr_stop = pTask->ctx->dsp_info->qstr_stop;
    pTask->dsp_sp_value = 0x0;
    pTask->dsp_fp_value = 0x0;
    //
    // split memory for programs if two cores are running
    if((dsp_cluster_mode&DSP_TASK_SYNC)||
       (dsp_cluster_mode&DSP_TASK_PARALLEL))
    {
        // take the gap between xyram0 and xyram1 into account
        unsigned xyram1_size = pTask->ctx->dsp_info->xyram_size;
        unsigned xyram1_end = pTask->ctx->dsp_info->xyram_base + xyram1_size;
                              ;
        if(pTask->dsp_num==1 && (unsigned)pTask->ctx->__data_dst >= xyram1_end)
        {
            pTask->ctx->__data_dst = (unsigned*)((unsigned)pTask->ctx->__data_dst
                                      - xyram1_size);
        }
        if(pTask->dsp_num==1 && (unsigned)pTask->ctx->__bss_dst >= xyram1_end)
        {
            pTask->ctx->__bss_dst -= xyram1_size;
        }
    }
    pTask->dsp_pc       = (pTask->dsp_pc -
                (unsigned int)((*(pTask->ctx)).__text_src) +
                              ((*(pTask->ctx)).__text_offset))>>2;

    pTask->status = DSP_TASK_READY;

    return err_code;
}

//
// loads task context into dsp memory, setups dsp specific register to start
// dsp application execution
//
enum ERL_ERROR risc_dsp_load_task(struct dsp_task_info* pTask)
{
    if(!pTask || pTask->status!=DSP_TASK_READY)
        return ERL_UNITIALIZED_ARG;

    // load program into dsp memory
    if(!load_overlay(pTask->ctx))
        return ERL_SYSTEM_ERROR;

    // setup dsp registers
    pTask->dsp_regs->DCSR  = 0;
    pTask->dsp_regs->SR    = 0;
    pTask->dsp_regs->SS    = (*(pTask->ctx)).__exit_dsp;
    pTask->dsp_regs->SP    = 1;
    pTask->dsp_regs->PC    = pTask->dsp_pc;
    pTask->dsp_regs->A7    = pTask->dsp_sp_value;
    pTask->dsp_regs->A6    = pTask->dsp_fp_value;

    int i = 0;
    for(i=0; i<DSP_TASK_MAX_PARAMETERS; i+=1)
    {
        if(pTask->register_ctx[i].reg_offset!=OFF_NONE)
        {
            *((volatile int*)((int)pTask->dsp_regs+pTask->register_ctx[i].reg_offset))
                = pTask->register_ctx[i].reg_value;
        }
    }
    return ERL_NO_ERROR;
}

//
// loads task context into dsp memory, setups dsp specific register to start
// dsp application execution
//
enum ERL_ERROR risc_dsp_prepair_task(struct dsp_task_info* pTask)
{
    if(!pTask || pTask->status!=DSP_TASK_READY)
        return ERL_UNITIALIZED_ARG;

    // setup dsp registers
    pTask->dsp_regs->DCSR   = 0;
    pTask->dsp_regs->SR     = 0;
    pTask->dsp_regs->SS     = (*(pTask->ctx)).__exit_dsp;
    pTask->dsp_regs->SP     = 1;
    pTask->dsp_regs->PC     = pTask->dsp_pc;
    pTask->dsp_regs->A7     = pTask->dsp_sp_value;
    pTask->dsp_regs->A6     = pTask->dsp_fp_value;

    int i = 0;
    for(i=0; i<DSP_TASK_MAX_PARAMETERS; i+=1)
    {
        if(pTask->register_ctx[i].reg_offset!=OFF_NONE)
        {
            *((volatile int*)((int)pTask->dsp_regs+pTask->register_ctx[i].reg_offset))
                = pTask->register_ctx[i].reg_value;
        }
    }
    return ERL_NO_ERROR;
}

//
//
//
enum ERL_ERROR risc_dsp_run_task(struct dsp_task_info* pTask)
{
    pTask->dsp_regs->DCSR   = 0x4000;

    pTask->status = DSP_TASK_RUNNING;

    return ERL_NO_ERROR;
}

//
// starts execution of tasks by writing start bit to dcsr register if the
// parallel mode is set or sync start of the dsp cluster if sync mode is set
//
enum ERL_ERROR risc_dsp_run_tasks(struct dsp_task_info* pTask0,
                                  struct dsp_task_info* pTask1)
{
    enum ERL_ERROR err = ERL_NO_ERROR;
    if(dsp_cluster_mode&DSP_TASK_SYNC)
    {
        // synchronous start in synch mode
        pTask0->dsp_comm_regs->CSR |= 0x1;
        pTask0->status = DSP_TASK_RUNNING;
        pTask1->status = DSP_TASK_RUNNING;
    } else if(dsp_cluster_mode&DSP_TASK_PARALLEL)
    {
        // sequential start
        risc_dsp_run_task(pTask0);
        risc_dsp_run_task(pTask1);
    } else
    {
        err = ERL_PROGRAM_ERROR;
    }
    return err;
}
//
//
//
enum ERL_ERROR risc_dsp_wait_task(struct dsp_task_info* pTask)
{
    if(!pTask)
        return ERL_UNITIALIZED_ARG;

    // terminated already
    if(isTaskTerminated(pTask))
        return ERL_NO_ERROR;

    // run dsp
    const unsigned qstr_mask = (0xf << (pTask->dsp_num * 8));
    const unsigned stop_mask = (1<<pTask->qstr_stop);
    while(1)
    {
        unsigned qstr =  pTask->dsp_comm_regs->QSTR;
        if((qstr & qstr_mask) == stop_mask)
        {
            pTask->status = DSP_TASK_TERMINATED;
            break;
        }
    }

    return ERL_NO_ERROR;
}

enum ERL_ERROR risc_dsp_wait_all_tasks(struct dsp_task_info* pTask0,
                                       struct dsp_task_info* pTask1)
{
    if(!pTask0 ||!pTask1)
        return ERL_UNITIALIZED_ARG;

    if(!isTaskRunning(pTask0)||!isTaskRunning(pTask1))
        return ERL_UNITIALIZED_ARG;

    // run dsp
    const unsigned qstr_mask = ( (0xf<<(pTask0->dsp_num * 8)) |
                                 (0xf<<(pTask1->dsp_num * 8)) );

    const unsigned stop_mask = ( (1<<pTask0->qstr_stop) |
                                 (1<<pTask1->qstr_stop) );
    while(1)
    {
        unsigned qstr =  pTask0->dsp_comm_regs->QSTR;
        if((qstr & qstr_mask) == stop_mask)
        {
            pTask0->status = DSP_TASK_TERMINATED;
            pTask1->status = DSP_TASK_TERMINATED;
            break;
        }
    }
    return ERL_NO_ERROR;
}

enum ERL_ERROR risc_dsp_wait_any_task(struct dsp_task_info* pTask0,
                                      struct dsp_task_info* pTask1,
                                      struct dsp_task_info* pTaskStopped)
{
    enum ERL_ERROR wait_state = ERL_NO_ERROR;

    if(!pTask0 ||!pTask1)
    {
        return ERL_UNITIALIZED_ARG;
    }

    // both tasks are terminated, return pointer to the first task
    if(isTaskTerminated(pTask0)&&isTaskTerminated(pTask1))
    {
        pTaskStopped = pTask0;
        wait_state = ERL_NO_ERROR;
    }

    // first task is terminated already, wait the second task is stopped
    else if(isTaskTerminated(pTask0))
    {
        wait_state = risc_dsp_wait_task(pTask1);
        if(wait_state == ERL_NO_ERROR)
        {
            pTaskStopped = pTask0;
        }
    }
    // second task is terminated already, wait the first task is stopped
    else if(isTaskTerminated(pTask1))
    {
        wait_state = risc_dsp_wait_task(pTask0);
        if(wait_state == ERL_NO_ERROR)
        {
            pTaskStopped = pTask0;
        }
    }
    else
    {
        // both tasks are running
        const unsigned qstr_mask = ( (0xf<<(pTask0->dsp_num * 8)) |
                                     (0xf<<(pTask1->dsp_num * 8)) );

        const unsigned stop0_mask = (1<<pTask0->qstr_stop);
        const unsigned stop1_mask = (1<<pTask1->qstr_stop);
        while(1)
        {
            unsigned qstr =  pTask0->dsp_comm_regs->QSTR;
            if( ((qstr & qstr_mask) & stop0_mask) != 0 )
            {
                pTask0->status = DSP_TASK_TERMINATED;
                pTaskStopped = pTask0;
                wait_state == ERL_NO_ERROR;
                break;
            }
            if( ((qstr & qstr_mask) & stop1_mask) != 0 )
            {
                pTask1->status = DSP_TASK_TERMINATED;
                pTaskStopped = pTask1;
                wait_state == ERL_NO_ERROR;
                break;
            }
        }
    }
    return wait_state;
}

//
//
//
void  risc_dsp_free_task(struct dsp_task_info* pTask)
{
    if(pTask)
        free(pTask);

    pTask = NULL;
}