/**
 * tiescutils.c
 *
*/
/*
 * Copyright (c) 2015, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 **/
#include "ecat_def.h"
#include <xdc/std.h>
#include "error.h"
#include "console_utils.h"
#include "board.h"

#include <ti/sysbios/knl/Task.h>
#include <ti/sysbios/hal/Hwi.h>
#include <xdc/runtime/Error.h>
#include <xdc/runtime/System.h>

// get the firmware loaded as header files!
#include "ecat_frame_handler_bin.h"  // EtherCAT frame handler
#include "ecat_host_interface_bin.h" // EtherCAT host interface

#include "tiescutils.h"
#include "ecat_appl_cnfg.h"
#include "applInterface.h"
#include "ecatslv.h"
#ifdef AM43XX_FAMILY_BUILD
    #include <foc.h>
#endif

//#define PROFILE_ECAT_STACK
#ifdef PROFILE_ECAT_STACK
    #include "tiescbsp.h"
    Uint32 mainloop_delta, mainloop_max, pdi_delta, pdi_max, sync_delta, sync_max;
#endif

#include "soc.h"
#include "board_gpio.h"

#include "osdrv_utils.h"

/* ========================================================================== */
/*                           Macros & Typedefs                                */
/* ========================================================================== */

/** \brief Global Structure pointer holding PRUSS0 memory Map. */
PRUICSS_Handle pruIcss1Handle;
/** \brief Global Structure pointer holding PRUSS1 memory Map. */
PRUICSS_Handle pruIcss0Handle;

/* ========================================================================== */
/*                            Global Variables                                */
/* ========================================================================== */

Uint8 uartInstance = 0;
boardId_t boardType;

extern void PwmISR(UArg a0);

extern void SpiFlashRegisterMSDelayfn(void *);
#if CiA402_DEVICE
    extern UINT16 APPL_GenerateMapping(UINT16 *pInputSize, UINT16 *pOutputSize);
    extern UINT16 CiA402_Init();
#endif

Task_Handle tsk1; // ECAT mainloop
#ifdef ENABLE_PDI_TASK
    Task_Handle pdiTask; // ECAT sw ISR
#else
    #ifdef ENABLE_PDI_SWI
        Void PDIswi(UArg arg1, UArg arg2);
        Swi_Handle pdiSwi; // ECAT sw ISR
    #endif
#endif
Task_Handle uartTask;                   // UART processing
Task_Handle ledTaskHndl;                // LED Control Task

#ifdef ENABLE_SYNC_TASK
    Task_Handle sync0Task; // ECAT SYNC0 ISR
    Void Sync0task(UArg arg0, UArg arg1);
    Task_Handle sync1Task; // ECAT SYNC1 ISR
    Void Sync1task(UArg arg0, UArg arg1);
#endif
Uint32 appState = 0;

#define APPL_BUILD_VER  "3.3.0"
#ifdef AM43XX_FAMILY_BUILD
    #include "foc_pwm.h"
    extern PWMGEN   pwm_drv;
#endif
Void task1(UArg arg0, UArg arg1)
{
    Uint8 u8Err = 0;
    Task_Params taskParams;
    #ifdef ENABLE_PDI_SWI
    Swi_Params swiParams;
    #endif
    CONSOLEUtilsPrintf("TI Industrial SDK Version : ");
    CONSOLEUtilsPrintf(IND_SDK_VERSION);

    CONSOLEUtilsPrintf("\n\rDevice name \t: ");
    CONSOLEUtilsPrintf((const char *)SOCGetSocFamilyName());

    CONSOLEUtilsPrintf("\n\rChip Revision \t: ");
    CONSOLEUtilsPrintf(Board_getChipRevision());

    char t_str[8];
    sprintf(t_str, "%d", Board_getArmClockRate());
    CONSOLEUtilsPrintf("\n\rARM Clock rate \t: ");
    CONSOLEUtilsPrintf(t_str);


    CONSOLEUtilsPrintf("\n\rDevice Type \t: ");

    if(isEtherCATDevice())
    {
        CONSOLEUtilsPrintf("EtherCAT Device\n\r");
    }

    else
    {
        CONSOLEUtilsPrintf("Non-EtherCAT Device\n\r");
    }

    CONSOLEUtilsPrintf("\n\rTI EtherCAT Demo Application Build - ");
    CONSOLEUtilsPrintf(APPL_BUILD_VER);
    CONSOLEUtilsPrintf(" - running on ");
    CONSOLEUtilsPrintf(BOARDGetBoardName());

    /* map the array which contains pru firmware instrcutions.
       If the application is built to execute from internal RAM completely,then
       pru instructions are expcted to be stored in SPI flash and this mapping is
       not required*/

    bsp_set_pru_firmware((Uint32 *)FrameProc, sizeof(FrameProc),
                         (Uint32 *)HostProc, sizeof(HostProc));


    Board_enableGPIOClock(0xFF);

    /* initialize the Hardware and the EtherCAT Slave Controller */
    HW_Init();
    u8Err = MainInit(); // EtherCAT stack init

    #ifdef AM43XX_FAMILY_BUILD

    Hwi_Params hwi_params;
    Error_Block eb;
    Hwi_Params_init(&hwi_params);
    hwi_params.priority = 2;
    hwi_params.enableInt = 1;
    Error_init(&eb);
    Hwi_create(epwm_intr, PwmISR, &hwi_params, &eb);

    // PWM, ADC, EnDAT startup
    icss_adc_endat_app_init(pruIcss0Handle);
    #endif
    #if CiA402_DEVICE
    /*Initialize Axes structures*/
    CiA402_Init();
    /*Create basic mapping*/
    APPL_GenerateMapping(&nPdInputSize, &nPdOutputSize);
    #endif

    /* Create tasks */
    /* Create tree tasks that share a resource*/
    #ifdef ENABLE_PDI_TASK
    Task_Params_init(&taskParams);
    taskParams.priority = 6;
    taskParams.stackSize = 1512;
    taskParams.arg0 = (UArg)pruIcss1Handle;
    pdiTask = Task_create(PDItask, &taskParams, NULL);
    #else
    #ifdef ENABLE_PDI_SWI
    Swi_Params_init(&swiParams);
    swiParams.priority = 6;
    pdiSwi = Swi_create(PDIswi, &swiParams, NULL);
    #endif
    #endif
    #ifdef ENABLE_SYNC_TASK
    Task_Params_init(&taskParams);
    taskParams.priority = 8;
    taskParams.stackSize = 1512;
    taskParams.arg0 = (UArg)pruIcss1Handle;
    sync0Task = Task_create(Sync0task, &taskParams, NULL);

    Task_Params_init(&taskParams);
    taskParams.priority = 8;
    taskParams.stackSize = 1512;
    taskParams.arg0 = (UArg)pruIcss1Handle;
    sync1Task = Task_create(Sync1task, &taskParams, NULL);
    #endif

    Task_Params_init(&taskParams);
    taskParams.priority = 4;
    taskParams.stackSize = 1512;
    ledTaskHndl = Task_create(LEDtask, &taskParams, NULL);

    // PWM, ADC, EnDAT startup

    if(u8Err == 0)
    {
        bRunApplication = TRUE;

        do
        {
            //Task_sleep (1);
            #ifdef PROFILE_ECAT_STACK
            {
                Uint32 mainloop_start, mainloop_stop;
                bsp_get_local_sys_time(&mainloop_start, 0);
            #endif
                MainLoop();
                #ifdef PROFILE_ECAT_STACK
                bsp_get_local_sys_time(&mainloop_stop,  0);

                if(mainloop_stop >= mainloop_start)
                {
                    mainloop_delta = mainloop_stop - mainloop_start;
                }

                else
                {
                    mainloop_delta = 0xFFFFFFFFF - mainloop_start + mainloop_stop;
                }

                if(mainloop_delta > mainloop_max)
                {
                    mainloop_max = mainloop_delta;
                }
            }
                #endif
            Task_yield();
        }
        while(bRunApplication == TRUE);
    }

    HW_Release();

    BIOS_exit(0);
}



#ifdef ENABLE_PDI_TASK
/////////////////////////////////////////////////////////////////////////////////////////
/**
 \brief Interrupt service routine for the interrupts from the EtherCAT Slave Controller
*////////////////////////////////////////////////////////////////////////////////////////
#if AL_EVENT_ENABLED
void HW_EcatIsr(void)
{
    #ifdef PROFILE_ECAT_STACK
    Uint32 pdi_start, pdi_stop;
    /* get the AL event register */
    bsp_get_local_sys_time(&pdi_start,  0);
    #endif
    PDI_Isr();
    #ifdef PROFILE_ECAT_STACK
    bsp_get_local_sys_time(&pdi_stop,   0);
    pdi_delta = pdi_stop - pdi_start;

    if(pdi_delta > pdi_max)
    {
        pdi_max = pdi_delta;
    }

    #endif
}
#endif

Void PDItask(UArg arg1, UArg arg2)
{
    Task_sleep(10 * OS_TICKS_IN_MILLI_SEC);
    #if AL_EVENT_ENABLED
    uint32_t evtOutNum = HOST_AL_EVENT - 20;

    while(1)
    {
        PRUICSS_pruWaitEvent((PRUICSS_Handle)arg1, evtOutNum);
        /* ISR processing */
        HW_EcatIsr();
    }

    #endif
}
#else
#ifdef ENABLE_PDI_SWI
Void PDIswi(UArg arg1, UArg arg2)
{
    #ifdef PROFILE_ECAT_STACK
    Uint32 pdi_start, pdi_stop;
    /* get the AL event register */
    bsp_get_local_sys_time(&pdi_start,  0);
    #endif
    PDI_Isr();
    #ifdef PROFILE_ECAT_STACK
    bsp_get_local_sys_time(&pdi_stop,   0);
    pdi_delta = pdi_stop - pdi_start;

    if(pdi_delta > pdi_max)
    {
        pdi_max = pdi_delta;
    }

    #endif
}
void PDI_Swi()
{
    Swi_post(pdiSwi);
}
#endif
#endif

Void LEDtask(UArg arg0, UArg arg1)
{
    VARVOLATILE Uint8 state = STATE_INIT;

    #ifdef ENABLE_STARTUP_DIGOUT_ANIMATION
    VARVOLATILE Uint8 state = STATE_INIT;
    static Uint16 digout_led_mask = 1;

    while(STATE_INIT == state)
    {
        I2CSetLed(&gI2CObj, digout_led_mask);
        digout_led_mask <<= 1;

        if(digout_led_mask >= 256)
        {
            digout_led_mask = 1;
        }

        HW_EscReadByte(state, ESC_ADDR_ALSTATUS);
        Task_sleep(200 * OS_TICKS_IN_MILLI_SEC);
    }

    #else
    Task_sleep(200 * OS_TICKS_IN_MILLI_SEC);
    Board_setDigOutput(0x6a);
    #endif
    #ifdef AM335X_FAMILY_BUILD

    if(BOARD_ICEV2 == boardType)
    {
        char *lcd_str1 = IND_SDK_VERSION;
        char lcd_str2[] = {"EtherCAT App"};

        Board_setLCDString((uint8_t *)lcd_str1, 0);
        Board_setLCDString((uint8_t *)lcd_str2, 1);
        Board_setLCDScroll(0);
        Board_setLCDScroll(1);
    }

    #endif

    while(1)
    {
        Task_sleep(50 * OS_TICKS_IN_MILLI_SEC);
        Uint32 reset_reg_val;
        //ReadHVS(&gMcSPIObj);// trigger a read of hardware inputs

        #if ESC_EEPROM_EMULATION
        HW_EscReadByte(state, ESC_ADDR_ALSTATUS);
        state &= 0xF;
        /*
         * Since both SPI Flash and HVS (input) are on the same SPI instance(on ICEv2 and IDK),
         * application wil have to make sure that access to both does not happen concurrently.
         * At this context(INIT AND PREOP), it is ensured that input(HVS) read wont happen.
         */
        #ifdef AM335X_FAMILY_BUILD

        if(state <= STATE_PREOP)
        #endif
        {
            if(bsp_get_eeprom_update_status())
            {
                Uint32 t_cur_time;
                #ifdef USE_ECAT_TIMER
                bsp_get_local_sys_time(&t_cur_time, NULL);
                #else
                t_cur_time = Timestamp_get32();
                #endif
                Uint32 t_last_time = bsp_get_eeprom_updated_time();
                t_cur_time = ((t_cur_time >= t_last_time) ? (t_cur_time - t_last_time) :
                              t_cur_time + (0xFFFFFFFF - t_last_time));

                if(t_cur_time >= (ECAT_TIMER_INC_P_MS * 100))
                {
                    bsp_set_eeprom_update_status(0);
                    bsp_eeprom_emulation_flush();
                }
            }
        }

        #endif
        reset_reg_val = bsp_read_dword(pruIcss1Handle, ESC_ADDR_TI_ESC_RESET);

        if((reset_reg_val == TI_ESC_RST_CMD_U) ||
                (reset_reg_val == TI_ESC_RST_CMD_L))
        {
            //EtherCAT master has requested S/W RESET
            HW_RestartTarget();
        }
    }
}



#ifdef ENABLE_SYNC_TASK
Void Sync0task(UArg arg1, UArg arg2)
{
    #ifdef PROFILE_ECAT_STACK
    Uint32 sync_start, sync_stop;
    #endif
    #ifndef EXEC_FROM_IRAM
    #ifndef NO_UART_MSG_APP
    CONSOLEUtilsPrintf("SYNC0 task started\n\r");
    #endif
    #endif
    uint32_t evtOutNum = HOST_SYNC0_EVENT - 20;

    while(1)
    {
        PRUICSS_pruWaitEvent((PRUICSS_Handle)arg1, evtOutNum);
        //Do sync0 event handling
        DISABLE_ESC_INT();
        #ifdef PROFILE_ECAT_STACK
        bsp_get_local_sys_time(&sync_start, 0);
        #endif
        Sync0_Isr();
        #ifdef PROFILE_ECAT_STACK
        bsp_get_local_sys_time(&sync_stop,  0);
        sync_delta = sync_stop - sync_start;

        if(sync_delta > sync_max)
        {
            sync_max = sync_delta;
        }

        #endif
        ENABLE_ESC_INT();
    }
}

Void Sync1task(UArg arg1, UArg arg2)
{
    #ifdef PROFILE_ECAT_STACK
    Uint32 sync_start, sync_stop;
    #endif
    #ifndef EXEC_FROM_IRAM
    #ifndef NO_UART_MSG_APP
    CONSOLEUtilsPrintf("SYNC1 task started\n\r");
    #endif
    #endif
    uint32_t evtOutNum = HOST_SYNC1_EVENT - 20;

    while(1)
    {
        PRUICSS_pruWaitEvent((PRUICSS_Handle)arg1, evtOutNum);
        //Do sync1 event handling
        DISABLE_ESC_INT();
        Sync1_Isr();
        ENABLE_ESC_INT();
    }
}
#endif




Void common_main()
{
    Task_Params taskParams;
    BOARDInit(NULL);
    SDKMMUInit(applMmuEntries);   // needed first
    boardType = BOARDGetId();

    Cache_lock((Ptr)0x80000000, 0x8000);
    Cache_wait();
    Cache_lock((Ptr)0x80008000, 0x8000);
    Cache_wait();
    Cache_lock((Ptr)0x80010000, 0x8000);
    Cache_wait();
    Cache_lock((Ptr)0x80018000, 0x8000);
    Cache_wait();
    Cache_lock((Ptr)0x80020000, 0x8000);
    Cache_wait();
    Cache_lock((Ptr)0x80028000, 0x8000);
    Cache_wait();
    Cache_lock((Ptr)0x80030000, 0x8000);
    Cache_wait();


    pruIcss1Handle = (PRUICSS_Handle)malloc(sizeof(PRUICSS_Config));
    pruIcss1Handle->object = (PRUICSS_V1_Object *)malloc(sizeof(
                                 PRUICSS_V1_Object));
    pruIcss1Handle->hwAttrs = (PRUICSS_HwAttrs *)malloc(sizeof(
                                  PRUICSS_HwAttrs));
    #ifdef AM43XX_FAMILY_BUILD

    ((PRUICSS_V1_Object *)pruIcss1Handle->object)->instance = 1;
    PRUSSDRVInit(pruIcss1Handle);   /* ICSS_M instance 1 */

    pruIcss0Handle = (PRUICSS_Handle)malloc(sizeof(PRUICSS_Config));
    pruIcss0Handle->object = (PRUICSS_V1_Object *)malloc(sizeof(
                                 PRUICSS_V1_Object));
    pruIcss0Handle->hwAttrs = (PRUICSS_HwAttrs *)malloc(sizeof(
                                  PRUICSS_HwAttrs));
    ((PRUICSS_V1_Object *)pruIcss0Handle->object)->instance = 0;
    PRUSSDRVInit(pruIcss0Handle);   /* ICSS_M instance 1 */

    Board_pinMuxConfig(am437xIdkMux);

    #elif defined(AM335X_FAMILY_BUILD)
    ((PRUICSS_V1_Object *)pruIcss1Handle->object)->instance = 0;
    PRUSSDRVInit(pruIcss1Handle);   /* ICSS_M instance 0 */

    Board_pinMuxConfig(iceV2Mux);

    board_init(BOARD_LCD_DISPLAY);
    #endif

    board_init(BOARD_LED_DIGOUT | BOARD_TRICOLOR0_GREEN | BOARD_TRICOLOR1_RED |
               BOARD_HVS_DIGIN | BOARD_FLASH_MEMORY);

    Task_Params_init(&taskParams);
    taskParams.priority = 4;
    tsk1 = Task_create(task1, &taskParams, NULL);

    #ifdef AM335X_FAMILY_BUILD

    if(BOARD_ICEV2 == boardType)
    {
        UTILsInitCpswIcssPorts();
    }

    #endif
    PRUICSS_pinMuxConfig(pruIcss1Handle, 0x0);   // PRUSS pinmuxing
    //Disable PRUs - This is to ensure PRUs are not running when application is not initialized
    PRUICSS_pruDisable(pruIcss1Handle, 0);
    PRUICSS_pruDisable(pruIcss1Handle, 1);
    CONSOLEUtilsInit();
    CONSOLEUtilsSetType(CONSOLE_UTILS_TYPE_UART);

    BIOS_start();
}

Uint8 isEtherCATDevice(void)
{
    Uint32 temp;

    temp = *((Uint32 *) 0x44E10604);

    if((temp & 0x01) && (temp & (1 << 16)) && (temp & (1 << 17)))
    {
        return 1;
    }

    return 0;

}
