#include <ti/sysbios/BIOS.h>
#include <ti/sysbios/knl/Task.h>
#include <stdio.h>
#include <string.h>
#include <ti/sdo/io/DriverTypes.h>
#include <ti/sdo/edma3/drv/edma3_drv.h>
#include <ti/psp/spi/Spi.h>
#include <ti/sdo/io/Stream.h>
#include <ti/psp/cslr/cslr.h>
#include <ti/psp/cslr/cslr_upp.h>
#include <ti/psp/cslr/cslr_syscfg0_OMAPL138.h>
#include <ti/psp/cslr/soc_OMAPL138.h>
#include <ti/sysbios/family/c64p/EventCombiner.h>
#include <ti/sysbios/family/c64p/Hwi.h>
#include <ti/sysbios/ipc/Semaphore.h>

#include "common.h"
#include "fpga_util.h"
#include "fpga.h"
#include "edma.h"

#define UPP_TELEGRAM_SIZE 16
#define UPP_TELEGRAMS_IN_PACKAGE 128
#define UPP_RX_BUFFER_SIZE ((UPP_TELEGRAM_SIZE / 2) * UPP_TELEGRAMS_IN_PACKAGE)
//#define FPGA_BOOT_VIA_JTAG

static Semaphore_Handle rx_event_upp;
#pragma DATA_ALIGN(upp_rx_buffer, 8);
#pragma DATA_SECTION(upp_rx_buffer, ".upp_rx_image");
static UInt16 upp_rx_buffer[UPP_RX_BUFFER_SIZE];
static CSL_SyscfgRegsOvly syscfg0_regs = (CSL_SyscfgRegsOvly) CSL_SYSCFG_0_REGS;
static CSL_UppRegsOvly upp0_regs = (CSL_UppRegsOvly) CSL_UPP_0_REGS;


static Void UppIsr(UArg arg0);
static Void InitUpp(Void);

static Void UppIsr(UArg arg0) 
{
    Gpio_PinCmdArg pinCmdArg;

    while (upp0_regs->UPIER != 0)
    {
        if (CSL_FEXT(upp0_regs->UPIER, UPP_UPIER_EOWI))
        {
            // Handle EOWI...
            pinCmdArg.pin   = GPIO_PIN_TO_DRIVER_NUMBER(6, 8);
            pinCmdArg.value = 1;
            Gpio_setPinVal(gpio0, &pinCmdArg, NULL);
            upp0_regs->UPIER = CSL_FINS(upp0_regs->UPIER, UPP_UPIER_EOWI, TRUE);
            Semaphore_post(rx_event_upp);
        }
        if (CSL_FEXT(upp0_regs->UPIER, UPP_UPIER_EOLI))
        {
            // Handle EOLI...
            upp0_regs->UPIER = CSL_FINS(upp0_regs->UPIER, UPP_UPIER_EOLI, TRUE);
        }
        if (CSL_FEXT(upp0_regs->UPIER, UPP_UPIER_ERRI))
        {
            // Handle ERRI...
            upp0_regs->UPIER = CSL_FINS(upp0_regs->UPIER, UPP_UPIER_ERRI, TRUE);
        }
        if (CSL_FEXT(upp0_regs->UPIER, UPP_UPIER_UORI))
        {
            // Handle UORI...
            upp0_regs->UPIER = CSL_FINS(upp0_regs->UPIER, UPP_UPIER_UORI, TRUE);
        }
        if (CSL_FEXT(upp0_regs->UPIER, UPP_UPIER_DPEI))
        {
            // Handle DPEI...
            upp0_regs->UPIER = CSL_FINS(upp0_regs->UPIER, UPP_UPIER_DPEI, TRUE);
        }
        if (CSL_FEXT(upp0_regs->UPIER, UPP_UPIER_EOWQ))
        {
            // Handle EOWQ...
            upp0_regs->UPIER = CSL_FINS(upp0_regs->UPIER, UPP_UPIER_EOWQ, TRUE);
        }
        if (CSL_FEXT(upp0_regs->UPIER, UPP_UPIER_EOLQ))
        {
            // Handle EOLQ...
            upp0_regs->UPIER = CSL_FINS(upp0_regs->UPIER, UPP_UPIER_EOLQ, TRUE);
        }
        if (CSL_FEXT(upp0_regs->UPIER, UPP_UPIER_ERRQ))
        {
            // Handle ERRQ...
            upp0_regs->UPIER = CSL_FINS(upp0_regs->UPIER, UPP_UPIER_ERRQ, TRUE);
        }
        if (CSL_FEXT(upp0_regs->UPIER, UPP_UPIER_UORQ))
        {
            // Handle UORQ...
            upp0_regs->UPIER = CSL_FINS(upp0_regs->UPIER, UPP_UPIER_UORQ, TRUE);
        }
        if (CSL_FEXT(upp0_regs->UPIER, UPP_UPIER_DPEQ))
        {
            // Handle DPEQ...
            upp0_regs->UPIER = CSL_FINS(upp0_regs->UPIER, UPP_UPIER_DPEQ, TRUE);
        }
    }

    upp0_regs->UPEOI = CSL_FMKT(UPP_UPEOI_EOI, DONE);
}

static Void InitUpp(Void)
{
    Int i;
    
    EventCombiner_dispatchPlug(94, &UppIsr, 0, 1);
    EventCombiner_enableEvent(94);
    Hwi_enableInterrupt(11);
    
    Psc_ModuleClkCtrl(psc1, 19, Psc_moduleClkCtrl_ENABLE, NULL);
    
    memset(upp_rx_buffer, 0, sizeof(upp_rx_buffer));
    
    syscfg0_regs->PINMUX14 = CSL_FINST(syscfg0_regs->PINMUX14, SYSCFG_PINMUX14_PINMUX14_31_28, UPP_D10);
    syscfg0_regs->PINMUX14 = CSL_FINST(syscfg0_regs->PINMUX14, SYSCFG_PINMUX14_PINMUX14_27_24, UPP_D11);
    syscfg0_regs->PINMUX14 = CSL_FINST(syscfg0_regs->PINMUX14, SYSCFG_PINMUX14_PINMUX14_23_20, UPP_D12);
    syscfg0_regs->PINMUX14 = CSL_FINST(syscfg0_regs->PINMUX14, SYSCFG_PINMUX14_PINMUX14_19_16, UPP_D13);
    syscfg0_regs->PINMUX14 = CSL_FINST(syscfg0_regs->PINMUX14, SYSCFG_PINMUX14_PINMUX14_15_12, UPP_D14);
    syscfg0_regs->PINMUX14 = CSL_FINST(syscfg0_regs->PINMUX14, SYSCFG_PINMUX14_PINMUX14_11_8, UPP_D15);
                             
    syscfg0_regs->PINMUX15 = CSL_FINST(syscfg0_regs->PINMUX15, SYSCFG_PINMUX15_PINMUX15_31_28, UPP_D2);
    syscfg0_regs->PINMUX15 = CSL_FINST(syscfg0_regs->PINMUX15, SYSCFG_PINMUX15_PINMUX15_27_24, UPP_D3);
    syscfg0_regs->PINMUX15 = CSL_FINST(syscfg0_regs->PINMUX15, SYSCFG_PINMUX15_PINMUX15_23_20, UPP_D4);
    syscfg0_regs->PINMUX15 = CSL_FINST(syscfg0_regs->PINMUX15, SYSCFG_PINMUX15_PINMUX15_19_16, UPP_D5);
    syscfg0_regs->PINMUX15 = CSL_FINST(syscfg0_regs->PINMUX15, SYSCFG_PINMUX15_PINMUX15_15_12, UPP_D6);
    syscfg0_regs->PINMUX15 = CSL_FINST(syscfg0_regs->PINMUX15, SYSCFG_PINMUX15_PINMUX15_11_8, UPP_D7);
    syscfg0_regs->PINMUX15 = CSL_FINST(syscfg0_regs->PINMUX15, SYSCFG_PINMUX15_PINMUX15_7_4, UPP_D8);
    syscfg0_regs->PINMUX15 = CSL_FINST(syscfg0_regs->PINMUX15, SYSCFG_PINMUX15_PINMUX15_3_0, UPP_D9);
    
    syscfg0_regs->PINMUX16 = CSL_FINST(syscfg0_regs->PINMUX16, SYSCFG_PINMUX16_PINMUX16_7_4, UPP_D0);
    syscfg0_regs->PINMUX16 = CSL_FINST(syscfg0_regs->PINMUX16, SYSCFG_PINMUX16_PINMUX16_3_0, UPP_D1);
    
    syscfg0_regs->PINMUX13 = CSL_FINST(syscfg0_regs->PINMUX13, SYSCFG_PINMUX13_PINMUX13_31_28, GPIO6_8);
    syscfg0_regs->PINMUX13 = CSL_FINST(syscfg0_regs->PINMUX13, SYSCFG_PINMUX13_PINMUX13_27_24, CH1_ENABLE);
    syscfg0_regs->PINMUX13 = CSL_FINST(syscfg0_regs->PINMUX13, SYSCFG_PINMUX13_PINMUX13_23_20, CH1_START);
    syscfg0_regs->PINMUX13 = CSL_FINST(syscfg0_regs->PINMUX13, SYSCFG_PINMUX13_PINMUX13_19_16, CH1_CLK);
    
    syscfg0_regs->PINMUX3 = CSL_FINST(syscfg0_regs->PINMUX3, SYSCFG_PINMUX3_PINMUX3_15_12, SPI0_SIMO0) |
                            CSL_FINST(syscfg0_regs->PINMUX3, SYSCFG_PINMUX3_PINMUX3_11_8, SPI0_SOMI0) |
                            CSL_FINST(syscfg0_regs->PINMUX3, SYSCFG_PINMUX3_PINMUX3_3_0, SPI0_CLK) |
                            CSL_FINST(syscfg0_regs->PINMUX3, SYSCFG_PINMUX3_PINMUX3_23_20, GPIO8_3);
                             
    upp0_regs->UPPCR = CSL_FINS(upp0_regs->UPPCR, UPP_UPPCR_SWRST, 1);
    
    /* wait some cycles according to the doc */
    for (i = 0; i < 20; i++);
    upp0_regs->UPPCR = CSL_FINS(upp0_regs->UPPCR, UPP_UPPCR_SWRST, 0);
    
    upp0_regs->UPCTL = CSL_FMKT(UPP_UPCTL_DPWA, FULL) |
                       CSL_FMKT(UPP_UPCTL_IWA, 16BIT) |
                       CSL_FMKT(UPP_UPCTL_DRA, SINGLE) |
                       CSL_FMKT(UPP_UPCTL_SDRTXIL, DISABLE) |
                       CSL_FMKT(UPP_UPCTL_CHN, ONE) |
                       CSL_FMKT(UPP_UPCTL_MODE, RECEIVE);                   
    upp0_regs->UPICR = CSL_FMKT(UPP_UPICR_ENAA, ENABLE) |
                       CSL_FMKT(UPP_UPICR_STARTA, ENABLE) |
                       CSL_FMKT(UPP_UPICR_WAITPOLA, NORMAL) |
                       CSL_FMKT(UPP_UPICR_ENAPOLA, NORMAL) |
                       CSL_FMKT(UPP_UPICR_STARTPOLA, NORMAL);
                       
    upp0_regs->UPDLB = CSL_FMKT(UPP_UPDLB_BA, DISABLE) |
                       CSL_FMKT(UPP_UPDLB_AB, DISABLE);
                       
    upp0_regs->UPTCR = CSL_FMKT(UPP_UPTCR_RDSIZEI, 64B);
    
    /* setup UPP and DMA interrupts */
    upp0_regs->UPIES = CSL_FMKT(UPP_UPIES_EOLQ, DISABLED) |
                       CSL_FMKT(UPP_UPIES_EOWQ, DISABLED) |
                       CSL_FMKT(UPP_UPIES_ERRQ, DISABLED) |
                       CSL_FMKT(UPP_UPIES_UORQ, DISABLED) |
                       CSL_FMKT(UPP_UPIES_DPEQ, DISABLED) |
                       CSL_FMKT(UPP_UPIES_EOLI, DISABLED) |
                       CSL_FMKT(UPP_UPIES_EOWI, SET) |
                       CSL_FMKT(UPP_UPIES_ERRI, SET) |
                       CSL_FMKT(UPP_UPIES_UORI, SET) |
                       CSL_FMKT(UPP_UPIES_DPEI, SET);

    upp0_regs->UPIER = CSL_FINS(upp0_regs->UPIER, UPP_UPIER_EOWI, TRUE);
    upp0_regs->UPEOI = CSL_FMKT(UPP_UPEOI_EOI, DONE);
    /* enable UPP peripheral */
    upp0_regs->UPPCR = CSL_FMKT(UPP_UPPCR_EN, ENABLE) |
                       CSL_FMKT(UPP_UPPCR_SOFT, ENABLE) |
                       CSL_FMKT(UPP_UPPCR_FREE, DISABLE) |
                       CSL_FMKT(UPP_UPPCR_RTEMU, ENABLE);
    
    /* setup DMA descriptors */
    upp0_regs->UPID0 = CSL_FMK(UPP_UPID0_ADDRH,
                               (((UInt32) &upp_rx_buffer[0]) >> 3));
    upp0_regs->UPID1 = CSL_FMK(UPP_UPID1_LNCNT, 1) |
                       CSL_FMK(UPP_UPID1_BCNTH, (UPP_RX_BUFFER_SIZE << 1) >> 1);
    upp0_regs->UPID2 = CSL_FMK(UPP_UPID2_LNOFFSETH, 0);
}

Void FPGA_TaskUppDataHandler(UArg a, UArg b)
{
    while (1)
    {
        UInt16 i;
        Gpio_PinCmdArg pinCmdArg;

        Semaphore_pend(rx_event_upp, BIOS_WAIT_FOREVER);

        while (CSL_FEXT(upp0_regs->UPIS2, UPP_UPIS2_PEND) == CSL_UPP_UPIS2_PEND_TRUE)
        {
            asm(" nop");
        }

        i = upp_rx_buffer[0]; /* !!! durch diese Zeile funktioniert der 2. Transfer nicht mehr bzw ich kann keine Aenderungen mit dem Debugger im Speicher mehr sehen */

        //memset(upp_rx_buffer, 0x00, sizeof(upp_rx_buffer));

        /* setup DMA descriptors */
        current_rx_buffer = (current_rx_buffer + 1) % 10;
        upp0_regs->UPID0 = CSL_FMK(UPP_UPID0_ADDRH,
                                   (((UInt32) &upp_rx_buffer[0]) >> 3));
        upp0_regs->UPID1 = CSL_FMK(UPP_UPID1_LNCNT, 1) |
                           CSL_FMK(UPP_UPID1_BCNTH, (UPP_RX_BUFFER_SIZE << 1) >> 1);
        upp0_regs->UPID2 = CSL_FMK(UPP_UPID2_LNOFFSETH, 0);

        pinCmdArg.pin   = GPIO_PIN_TO_DRIVER_NUMBER(6, 8);
        pinCmdArg.value = 0;
        Gpio_setPinVal(gpio0, &pinCmdArg, NULL);
    }
}