/**
* @file main.c
*
* @brief Basic example for an HSR PRP device running on AM57xx devices.
* Requires a full ICSS system (both PRUs) and two external phys connected to
* ICSS MII/MDIO interfaces
*
* \par
* Copyright (C) 2015 Texas Instruments Incorporated - http://www.ti.com/
* \par
*
*/

#include <stdio.h>
#include <stdbool.h>
#include <xdc/std.h>
#include <xdc/cfg/global.h>
#include <xdc/runtime/Error.h>
#include <xdc/runtime/System.h>
#include <ti/sysbios/BIOS.h>
#include <ti/ndk/inc/stkmain.h>

#include <examples/osal/TaskP.h>

/* TI-RTOS Header files */
#include <ti/osal/osal.h>
#include <ti/drv/uart/UART_stdio.h>
#include <ti/board/board.h>
#include <ti/drv/gpio/GPIO.h>
#include <ti/drv/i2c/I2C.h>
#include <ti/drv/spi/SPI.h>

#include <stdint.h>
#include <protocols/hsr_prp/version.h>

#include <ti/drv/icss_emac/icss_emacFwInit.h>
#include <ti/drv/icss_emac/icss_emacLearning.h>
#include <ti/drv/icss_emac/icss_emacStatistics.h>
#include <ti/drv/icss_emac/icss_emacDrv_Def.h>
#include <ti/drv/icss_emac/icss_emacStormControl.h>
#include <ti/transport/ndk/nimu_icss/example/src/osdrv_ndkdeviceconfig.h>
#include <ti/drv/icss_emac/test/src/fw_mem_section.h>
#include <ti/drv/icss_emac/icss_emacDrv.h>
#include <ti/drv/icss_emac/soc/icss_emacSoc.h>
#include <ti/drv/pruss/soc/pruicss_v1.h>
#include <tiswitch_pruss_intc_mapping.h>
#include <ti/csl/src/ip/icss/V1/cslr_icss_intc.h>
#include <ti/csl/src/ip/icss/V1/cslr_icss_cfg.h>
#if defined(SOC_K2G)
#include <ti/csl/src/ip/bootcfg/V3/csl_bootcfgAux.h>
#include <ti/csl/src/ip/bootcfg/V3/csl_bootcfg.h>
#include <ti/board/src/iceK2G/include/iceK2G_ethernet_config.h>
#include <ti/csl/src/ip/mdio/V1/cslr_mdio.h>
#include <ti/csl/src/ip/mdio/V1/csl_mdio.h>
#include <ti/csl/src/ip/mdio/V1/csl_mdioAux.h>
#include <ti/csl/src/ip/icss/V1/cslr_icss_pru_ctrl.h>
#elif defined(SOC_AM572x) || defined(SOC_AM571x) || defined(SOC_AM335x) || defined(SOC_AM437x)
#include <ti/csl/src/ip/mdio/V2/cslr_mdio.h>
#include <ti/csl/src/ip/mdio/V2/csl_mdio.h>
#include <ti/csl/src/ip/mdio/V2/csl_mdioAux.h>
#endif
#include <ti/transport/ndk/nimu_icss/nimu_icssEth.h>
#ifdef SOC_AM335x
#include <ti/csl/src/ip/gpio/V1/gpio_v2.h>
#include <ti/starterware/include/hw/soc_am335x.h>
#endif
#include <ti/drv/spi/SPI.h>
#include <ti/drv/spi/soc/QSPI_v1.h>
/* Flash header file */
#include <ti/drv/spi/SPIver.h>

#include <ti/drv/spi/test/qspi_flash/src/Flash_S25FL/S25FL.h>

#include <examples/board/include/board_i2cLed.h>
#include <examples/board/include/board_gpioLed.h>
#include <examples/board/include/board_misc.h>
#include <examples/board/include/board_phy.h>
#include <examples/board/include/board_spi.h>
#include <examples/board/include/board_eeprom.h>
#include <examples/board/include/delay_us.h>
#include "hsrPrp_firmware.h"
#if defined(SOC_AM335x) || defined(SOC_AM437x)
#include "ti/starterware/include/chipdb.h"
#include "ti/starterware/include/soc_control.h"
#endif

#include "hsr_prp_menu.h"

#ifdef ICSS_PROTOCOL_RED
#include "hsrPrp_red_osBios.h"
#include "hsrPrp_red_nodeTable.h"
#include "hsrPrp_red_snmp.h"
#include "hsrPrp_red_statistics.h"
#include "hsrPrp_red_config.h"
#include "hsrPrp_red.h"
#include "hsrPrp_handle.h"
#include "hsrPrp_red_multicastTable.h"
#include "hsrPrp_red_vlanTable.h"
#endif /* ICSS_PROTOCOL_RED */

#include "hsr_prp_fw_offsets.h"

#ifdef BUILD_HSR_H
#if defined(SOC_AM335x) || defined(SOC_AM437x)
#include "protocols/hsr_prp/firmware/hsr/v1.0/icss_hsr_pru0_bin.h"
#include "protocols/hsr_prp/firmware/hsr/v1.0/icss_hsr_pru1_bin.h"
#else
#include "protocols/hsr_prp/firmware/hsr/v2.1/icss_hsr_pru0_bin.h"
#include "protocols/hsr_prp/firmware/hsr/v2.1/icss_hsr_pru1_bin.h"
#endif
#endif

#ifdef BUILD_PRP
#if defined(SOC_AM335x) || defined(SOC_AM437x)
#include "protocols/hsr_prp/firmware/prp/v1.0/icss_prp_pru0_bin.h"
#include "protocols/hsr_prp/firmware/prp/v1.0/icss_prp_pru1_bin.h"
#else
#include "protocols/hsr_prp/firmware/prp/v2.1/icss_prp_pru0_bin.h"
#include "protocols/hsr_prp/firmware/prp/v2.1/icss_prp_pru1_bin.h"
#endif
#endif


#ifdef SOC_K2G
#define CSL_ICSSCFG_GPCFG0_PRU0_GP_MUX_SEL_MASK                 (0x3C000000U)
#define CSL_ICSSCFG_GPCFG0_PRU0_GP_MUX_SEL_SHIFT                (26U)
#define CSL_ICSSCFG_GPCFG0_PRU0_GP_MUX_SEL_RESETVAL             (0x00000000U)
#define CSL_ICSSCFG_GPCFG0_PRU0_GP_MUX_SEL_MAX                  (0x00000004U)
#define CSL_ICSSCFG_GPCFG0_PRU0_GP_MUX_SEL_MII_MODE_VAL         (2u)
#endif

#define RED_PRINTF_UART

#define RED_REM_NODE_TYPE_STR_DANP     "DANP   "
#define RED_REM_NODE_TYPE_STR_VDANP    "VDANP  "
#define RED_REM_NODE_TYPE_STR_REDBOXP  "REDBOXP"
#define RED_REM_NODE_TYPE_STR_DANH     "DANH   "
#define RED_REM_NODE_TYPE_STR_VDANH    "VDANH  "
#define RED_REM_NODE_TYPE_STR_REDBOXH  "REDBOXH"
#define RED_REM_NODE_TYPE_STR_SANA     "SAN A  "
#define RED_REM_NODE_TYPE_STR_SANB     "SAN B  "
#define RED_REM_NODE_TYPE_STR_SANAB    "SAN AB "
#define RED_REM_NODE_TYPE_STR_UNKNOWN  "UNKNOWN"

#define RED_HSR_LRE_MODE_H             "MODE H"
#define RED_HSR_LRE_MODE_N             "MODE N"
#define RED_HSR_LRE_MODE_T             "MODE T"
#define RED_HSR_LRE_MODE_U             "MODE U"
#define RED_HSR_LRE_MODE_M             "MODE M"
#define RED_HSR_LRE_MODE_UNKNOWN       "UNKNOWN"

#include <examples/hsr_prp/hsr_prp_soc.h>

extern uint8_t check_ip;
extern char IPString[20];

/**
 * Default PCP to Q mapping. See description of function initializePCPtoQueueMap below
 */
uint8_t default_pcp_2_queue_map[8] = {ICSS_EMAC_QUEUE4, ICSS_EMAC_QUEUE4, ICSS_EMAC_QUEUE3, ICSS_EMAC_QUEUE3, \
                                      ICSS_EMAC_QUEUE2, ICSS_EMAC_QUEUE2, ICSS_EMAC_QUEUE1, ICSS_EMAC_QUEUE1
                                     };


#define SRC_ADDR    0x81000000
#define DST_ADDR    0x82000000
#define DUMMY_PARAM     4
#define TEST_PARAM      2

#define BOARD_IDKAM571x     1
#define BOARD_IDKAM572x     2

/*
 *  global data declaration
 */
/** application version */
#define APP_VERSION "2.0.0.0"

/* Board specific definitions */
#ifdef SOC_AM437x
#define MCSPI_INSTANCE         (2U)
#else
#define MCSPI_INSTANCE         (3U)
#endif

#define QSPI_PER_CNT            (1U)
#define QSPI_INSTANCE           (1U)
#define QSPI_OFFSET             (4U)

/*name of the C struct in PRU header file*/
#define PRU0_FIRMWARE_NAME      PRU0_FIRMWARE
#define PRU1_FIRMWARE_NAME      PRU1_FIRMWARE


/** @brief rotary switch value on ICEv2 */
Uint8 nodeAddr;                             /**< rotary switch value on ICEv2 */

/**
 * @brief   Initialize the PCP to Queue Map to default value
 *          i.e. Queue 4 or lowest priority queue
 *
 * @param   emachandle Pointer to EMAC handle. Used to fetch shared RAM base address
 *
 * @return  none
 */
void initializePCPtoQueueMap(ICSS_EmacHandle emachandle);

/**
 * @brief   Load the PCP to Queue Map to shared RAM.
 *          The firmware uses this to
 *
 * @param   emachandle Pointer to EMAC handle. Used to fetch shared RAM base address
 * @param   pcp_2_queue_map pointer to PCP to Q mapping.
 *
 * The mapping is very simple. The memory is an 8 byte char array
 * with each byte containing the queue. The index of the byte corresponds to the
 * PCP value. Here lower value is higher priority queue, see default_pcp_2_queue_map for example
 * So we have as an example (see default_pcp_2_queue_map)
 * PCP         Queue       Mem Offset (QUEUE_2_PCP_MAP_OFFSET is base)
 * 0           Q4          0
 * 1           Q4          1
 * 2           Q3          2
 * ...
 * ...
 * 7           Q1          7
 *
 * @return  none
 */
void loadPCPtoQueueMap(ICSS_EmacHandle emachandle, uint8_t *pcp_2_queue_map);

void getMacid(char *buf_str);

/** \brief PRU-ICSS LLD handle */
PRUICSS_Handle prusshandle;
/** \brief ICSSEmac LLD handle */
ICSS_EmacHandle emachandle;


#if defined(SOC_AM335x)
Board_flashHandle flashHandle;
#elif defined(SOC_K2G)
Board_flashHandle flashHandle;
extern Board_flashHandle boardFlashHandle;
#else
extern S25FL_Handle flashHandle;
#endif

SPI_Handle handle;

uint8_t lclMac[6];
Board_IDInfo boardInfo;
uint32_t ipAddress;

hsrPrpHandle *hsrPrphandle;
extern int collision_pkt_dropped;
extern int num_of_collision_occured;

extern void Board_initSPI(void);
extern void gpioLedPinmuxConfig();

extern PRUICSS_Config pruss_config[2 + 1];

extern ICSS_EmacBaseAddrCfgParams icss_EmacBaseAddrCfgParams[3];
int rxcalBk = 0;

/**Temporary placeholder to copy packets*/
uint8_t  tempFrame[ICSS_EMAC_MAXMTU];

/***********************************************************************/
/* local functions declaration                                         */
/***********************************************************************/

/**
 * @brief   ICSS EMAC LLD handle init
 *
 * @param   PRU-ICSS LLD configuration
 *
 * @return  E_FAIL on failure, S_PASS on success
 */
uint8_t ICSSEmacDRVInit(ICSS_EmacHandle handle, uint8_t instance);

#if defined(SOC_AM572x)
void SOCCtrlGetPortMacAddr(uint32_t portNum, uint8_t *pMacAddr);
#endif
/**
 *  @brief  Function to initialize the HSR/PRP driver handle
 *
 *          Implemented to facilitate multi-instance capability.
 *          Moving all global variables across the driver files to this handle in hsrPrp_handle.h file
 *
 *  @param   hsrPrpHandle   [in]  HSR driver handle
 *
 *  @retval  none
 *
 */
void hsrPrpHandle_init(hsrPrpHandle *hsrPrphandle);

/**
 *  @brief  Function to register the Port 1 LinkReset callback
 *
 *  @param   linkStatus   [in]  link status
 *  @param   arg2         [in]  pointer to a handle
 *
 *  @retval  none
 *
 */
void hsrPrp_Port1linkResetCallBack(uint8_t linkStatus, void *arg2);

/**
 *  @brief  Function to register the Port 2 LinkReset callback
 *
 *  @param   linkStatus   [in]  link status
 *  @param   arg2         [in]  pointer to a handle
 *
 *  @retval  none
 *
 */
void hsrPrp_Port2linkResetCallBack(uint8_t linkStatus, void *arg2);

/***********************************************************************/
/* function definitions                                                */
/***********************************************************************/

/**
 * \brief main task to initialize all software components and run the stack
 *
 * \param a0 not used
 * \param a1 not used
 * \callgraph
 */
Void taskPruss(UArg a0, UArg a1)
{
    uint8_t firmwareLoad_done = 0;
    uint32_t prpMode = (uint32_t)a0;

    HSR_PRP_socInit();

    UART_printf("\nVersion - ");
    UART_printf(IND_PKG_VERSION);

    UART_printf("\nBoard name \t: ");
    UART_printf(boardInfo.boardName);

    UART_printf("\n\rChip Revision \t: ");
    UART_printf(boardInfo.version);
    UART_printf("\r\nBuild Timestamp      : %s %s", __DATE__, __TIME__);

    /*Initialize the PCP 2 Q map*/
    initializePCPtoQueueMap(emachandle);

    PRUICSS_pruDisable(prusshandle, ICSS_EMAC_PORT_1 - 1);
    PRUICSS_pruDisable(prusshandle, ICSS_EMAC_PORT_2 - 1);

    if (prpMode)
    {
        if(PRUICSS_pruWriteMemory(prusshandle, PRU_ICSS_IRAM(0) , 0,
                                  (uint32_t *) PRU0_FIRMWARE_NAME,
                                  sizeof(PRU0_FIRMWARE_NAME)))
        {
            if(PRUICSS_pruWriteMemory(prusshandle, PRU_ICSS_IRAM(1) , 0,
                                      (uint32_t *) PRU1_FIRMWARE_NAME,
                                      sizeof(PRU1_FIRMWARE_NAME)))
            {
                firmwareLoad_done = TRUE;
            }
        }
    }
    else
    {
        if(PRUICSS_pruWriteMemory(prusshandle, PRU_ICSS_IRAM(0) , 0,
                                (uint32_t *) &pru_imem0_rev1_start,
                                &pru_imem0_rev1_end - &pru_imem0_rev1_start))
        {
            if(PRUICSS_pruWriteMemory(prusshandle, PRU_ICSS_IRAM(1) , 0,
                                (uint32_t *) &pru_imem1_rev1_start,
                                &pru_imem1_rev1_end - &pru_imem1_rev1_start))
            {
                firmwareLoad_done = TRUE;
            }
        }
    }

    if(firmwareLoad_done)
    {
        PRUICSS_pruEnable(prusshandle, ICSS_EMAC_PORT_1 - 1);
        PRUICSS_pruEnable(prusshandle, ICSS_EMAC_PORT_2 - 1);

        if (prpMode)
        {
            RedProtocolStart(hsrPrphandle, emachandle);

            /*Load the default PCP to Queue mapping*/
            loadPCPtoQueueMap(emachandle, default_pcp_2_queue_map);
        }
    }
}

#ifdef ICSS_PROTOCOL_RED

/*
 *  ---UART Menu task---
 */
void taskUartMenu(UArg arg0, UArg arg1)
{
    ICSSEMAC_Handle emachandle;
    char rxByte;

    /* Wait for PRU load complete */
    TaskP_sleep(3000);
    emachandle = (ICSSEMAC_Handle)arg0;

    while(1)
    {
        UART_printf("\n\r\n\r");
        printConfig(emachandle);

        rxByte = (char) UART_getc();

        if((rxByte == 'S') || (rxByte == 's'))
        {
            printStatistics(emachandle);

        }

        else if((rxByte == 'L') || (rxByte == 'l'))
        {
            printLreStatistics(emachandle);

        }

        else if((rxByte == 'N') || (rxByte == 'n'))
        {
            if (hsrPrphandle != NULL)
            {
                printNodeTable(hsrPrphandle);
            }
        }

        else if((rxByte == 'M') || (rxByte == 'm'))
        {
            printMulticastFilterSubMenu();
            rxByte = (char) UART_getc();

            if((rxByte == 'D') || (rxByte == 'd'))
            {
                hsrPrp_multicast_filter_config(emachandle, MULTICAST_FILTER_DISABLED);
            }

            else if((rxByte == 'E') || (rxByte == 'e'))
            {
                hsrPrp_multicast_filter_config(emachandle, MULTICAST_FILTER_ENABLED);
            }

            else
            {
                uint8_t bkupMacidString[30];

                UART_gets((char *)bkupMacidString, -2);
#if DEBUG_MC_FLT
                UART_puts((char *)bkupMacidString, -2);
#endif

                if(isValidMacid((char *)bkupMacidString))
                {
                    if((rxByte == 'M') || (rxByte == 'm'))
                    {
                        hsrPrp_multicast_filter_override_hashmask(emachandle, bkupMacidString);
                        UART_printf("\n\rSuccessfully configured new mask");
                    }

                    else if((rxByte == 'A') || (rxByte == 'a'))
                    {
                        hsrPrp_multicast_filter_update_macid(emachandle, bkupMacidString,
                                                             ADD_MULTICAST_MAC_ID);
                        UART_printf("\n\rSuccessfully added");
                    }

                    else if((rxByte == 'R') || (rxByte == 'r'))
                    {
                        hsrPrp_multicast_filter_update_macid(emachandle, bkupMacidString,
                                                             REMOVE_MULTICAST_MAC_ID);
                        UART_printf("\n\rSuccessfully removed");
                    }
                }
            }
        }

        else if((rxByte == 'V') || (rxByte == 'v'))
        {
            printVlanFilterSubMenu();
            rxByte = (char) UART_getc();

            if((rxByte == 'D') || (rxByte == 'd'))
            {
                vlan_filter_config(emachandle, VLAN_FLTR_DIS);
            }

            else if((rxByte == 'E') || (rxByte == 'e'))
            {
                vlan_filter_config(emachandle, VLAN_FLTR_ENA);
            }

            else if((rxByte == 'A') || (rxByte == 'a'))
            {
                char bkupMacidString[30];
                UART_gets(bkupMacidString, -2);

                unsigned int vid;
                sscanf(bkupMacidString, "%u", &vid);

                if(vid <= VLAN_VID_MAX)  /*No need to check for vid >= VLAN_VID_MIN as CWARN.NOEFFECT.UCMP.GE: Comparison of unsigned value against 0 is always true*/
                {
                    vlan_filter_update_vid(emachandle, (uint16_t)vid, ADD_VLAN_VID);
                }
            }

            else if((rxByte == 'R') || (rxByte == 'r'))
            {
                char bkupMacidString[30];
                UART_gets(bkupMacidString, -2);

                unsigned int vid;
                sscanf(bkupMacidString, "%u", &vid);

                if(vid <= VLAN_VID_MAX)  /*No need to check for vid >= VLAN_VID_MIN as CWARN.NOEFFECT.UCMP.GE: Comparison of unsigned value against 0 is always true*/
                {
                    vlan_filter_update_vid(emachandle, (uint16_t)vid, REM_VLAN_VID);
                }
            }

            else if((rxByte == 'H') || (rxByte == 'h'))
            {
                vlan_untagged_frames_config(emachandle,
                                            VLAN_FLTR_UNTAG_HOST_RCV_ALL);
            }

            else if((rxByte == 'N') || (rxByte == 'n'))
            {
                vlan_untagged_frames_config(emachandle,
                                            VLAN_FLTR_UNTAG_HOST_RCV_NAL);
            }

            else if((rxByte == 'P') || (rxByte == 'p'))
            {
                vlan_priotag_frames_config(emachandle,
                                           VLAN_FLTR_PRIOTAG_HOST_RCV_ALL);
            }

            else if((rxByte == 'Q') || (rxByte == 'q'))
            {
                vlan_priotag_frames_config(emachandle,
                                           VLAN_FLTR_PRIOTAG_HOST_RCV_NAL);
            }

            else if((rxByte == 'S') || (rxByte == 's'))
            {
                vlan_sv_frames_config(emachandle,
                                      VLAN_FLTR_SV_VLAN_FLOW_SKIP);
            }

            else if((rxByte == 'F') || (rxByte == 'f'))
            {
                vlan_sv_frames_config(emachandle,
                                      VLAN_FLTR_SV_VLAN_FLOW_TAKE);
            }
        }

        else if((rxByte == 'F') || (rxByte == 'f'))
        {
            char bkupMacidString[30];
            UART_gets(bkupMacidString, -2);

            unsigned int intrPacStatus;
            sscanf(bkupMacidString, "%u", &intrPacStatus);
            intr_pac_config(emachandle, intrPacStatus);
        }

        else if((rxByte == 'I') || (rxByte == 'i'))
        {
            UART_printf("\n\rType IP Address and hit Enter:\t");

            if(readAndAssignIPAddress())
            {
                UART_printf("\n\rSuccessfully assigned new IP address");
            }

        }

        else if((rxByte == 'R') || (rxByte == 'r'))
        {
            runRxTxTest(emachandle);

        }

        else if((rxByte == 'C') || (rxByte == 'c'))
        {
            printConfiguration(emachandle);

        }

        else if((rxByte == 'H') || (rxByte == 'h'))
        {
            printHelpMenu();

        }

        else
        {
            UART_printf("\n\r\n\rSorry didnt catch that. Unknown input\n\r");
            UART_printf("\n\r");
        }

    }

}

#endif /* ICSS_PROTOCOL_RED */

uint32_t asn1ParseLen(const uint8_t **data)
{
    const uint8_t *ptr = *data;
    uint32_t len = *ptr++;
    if (len != 0)
    {
        if (len & 0x80)
        {
            uint8_t lenLen = len & 0x7f;
            len = 0;
            while (lenLen--)
            {
                len = (len << 8) | *ptr++;
            }
        }
    }
    *data = ptr;
    return len;
}

/**
 * @brief LED task for second LED
 *
 * @param arg0 not used
 * @param arg1 not used
 */
void taskLedBlink2(UArg arg0, UArg arg1)
{
    SOCKET s;

    int32_t pktlen;
    void *pRecvBuf;
    HANDLE hRecvBuf;

#if !defined(SOC_K2G)
    gpioLedPinmuxConfig();
#endif
    GPIO_init();

    /*Give an initial delay to make sure LED's are initialized*/
    TaskP_sleep(1000);
    /**
     * Sequence for LED 1 (Device Status LED)
     * Blink second LED Green color on and off
     */
    Board_setTriColorLED(BOARD_TRICOLOR1_YELLOW, 0);
    Board_setTriColorLED(BOARD_TRICOLOR1_GREEN, 0);
    Board_setTriColorLED(BOARD_TRICOLOR1_RED, 0);

    while(GetNdkStatus(0)== NIMU_STAT_PROGRESS || GetNdkStatus(0)==NIMU_STAT_UNKNOWN)
    {
        Task_sleep(100);
    }

    fdOpenSession( TaskSelf() );
    int devIndex = 1;
    static uint8_t prio_mapping[MAX_PRIO_VAL];
    int index;
    for (index = 0; index < MAX_PRIO_VAL; index++)
    {
        prio_mapping[index] = index;
    }

    NETIF_DEVICE *nd = NIMUFindByName("eth0");
    if (nd == NULL)
    {
        UART_printf("\nCannot find eth0\n");
    }
    else
    {
        devIndex = nd->index;
#if 1 // VLAN
        /* Use the VLAN API to create a new VLAN device. */
        devIndex = VLANAddDevice (devIndex, 0, 4, prio_mapping);
        if (devIndex < 0)
        {
            UART_printf("\nError VLAN Failed errcode=%d\n", index);
            devIndex = nd->index;
        }
#endif
    }
    s = socket(AF_RAWETH, SOCK_RAWETH, 0x88ba);
    if (s == INVALID_SOCKET)
    {
        UART_printf("\nFailed to create socket: 0x%x\n", fdError());
    }
    if (setsockopt(s, SOL_SOCKET, SO_IFDEVICE, &devIndex, sizeof(devIndex)) != 0)
    {
        UART_printf("\nFailed to assign SO_IFDEVICE on socket: 0x%x\n", fdError());
    }
    int val;
    int sz = sizeof(val);
    getsockopt(s, SOL_SOCKET, SO_RCVBUF, &val, &sz);
    val = 32768;
    if (setsockopt(s, SOL_SOCKET, SO_RCVBUF, &val, sizeof(val)) != 0)
    {
        UART_printf("\nFailed to assign SO_RCVBUF on socket: 0x%x\n", fdError());
    }
#if 0
    uint16_t priority = 4;
    if (NDK_setsockopt(s, SOL_SOCKET, SO_PRIORITY, &priority, sizeof(priority)) < 0)
    {
        UART_printf("\nFailed to assign SO_IFDEVICE on socket: 0x%x\n", fdError());
    }
#endif
    UART_printf("\nSV receiving\n");

    uint16_t prevSvId = 0xffff;
    uint32_t cnt = 0;

    while(1)
    {
        pktlen = recvnc(s, &pRecvBuf, 0, &hRecvBuf);
        if(pktlen > 0)
        {
            uint16_t svId = 0xffff;
            const uint8_t *data;
            ETHHDR *pkt = (ETHHDR *)pRecvBuf;
            uint16_t Type = NDK_ntohs(pkt->Type);
            data = (Type == ETHERTYPE_VLAN) ? ((uint8_t *)pkt + ETHHDR_SIZE + VLANHDR_SIZE) : ((uint8_t *)pkt + ETHHDR_SIZE);
            data += 2 /* appid */ + 2 /* length */ + 4 /* reserved */;
            if (*data++ == 0x60) /* savpdu */
            {
                uint32_t len = asn1ParseLen(&data);
                if (*data++ == 0x80) /* noasdu */
                {
                    len = asn1ParseLen(&data);
                    data += len;
                    if (*data++ == 0xa2)
                    {
                        len = asn1ParseLen(&data);
                        if (*data++ == 0x30)
                        {
                            len = asn1ParseLen(&data);
                            if (*data == 0x80)
                            {
                                ++data;
                                len = asn1ParseLen(&data);
                                data += len;
                            }
                            if (*data == 0x81)
                            {
                                ++data;
                                len = asn1ParseLen(&data);
                                data += len;
                            }
                            if (*data == 0x82)
                            {
                                ++data;
                                len = asn1ParseLen(&data);
                                svId = ((uint16_t)data[0] << 8) | data[1];
                            }
                        }
                    }
                }
            }
            if (svId != 0xffff)
            {
                if ((svId != prevSvId) && (svId != ((prevSvId + 1) % 4000)))
                {
                    UART_printf("*");
                    if (prevSvId != 0xffff)
                    {
                        UART_printf("\nCurr=%d,Exp=%d\n", svId, prevSvId);
                    }
                }
                prevSvId = svId;
            }
            if ((cnt % 8000) >= 4000)
            {
                Board_setTriColorLED(BOARD_TRICOLOR1_GREEN,  0);
            }
            else
            {
                Board_setTriColorLED(BOARD_TRICOLOR1_GREEN,  1);
            }
            ++cnt;
            /*int i, j;
            UART_printf("\n");
            for (i = 0; i < 123;)
            {
                int sz = ((123 - i) >= 16) ? 16 : (123 - i);
                UART_printf("%08x:", i);
                for (j = 0; j < sz; j++)
                {
                    UART_printf(" %02x", ((uint8_t *)pkt)[i++]);
                }
                UART_printf("\n");
            }*/
            recvncfree(hRecvBuf);
            //break;
        }
        if(pktlen < 0) UART_printf("recvSV fdError %d\n", fdError());
    }
    shutdown(s, SHUT_RDWR);
    fdCloseSession( TaskSelf() );
}


#define PRCM_MODULE_IDLEST_FUNC          (0U)
#define PRCM_MODULE_IDLEST_TRANS         (1U)
#define PRCM_MODULE_IDLEST_IDLE          (2U)
#define PRCM_MODULE_IDLEST_DISABLE       (3U)


/**
 * \brief the application entry point - initializes low level hardware, creates tasks and starts SYSBIOS
 *
 */

//extern ICSS_EmacBaseAddrCfgParams icss_EmacBaseAddrCfgParams[2];

void ICSS_EMAC_testDelay(uint32_t delay)
{
    volatile uint32_t delay1 = delay * 2;

    while(delay1--) ;
}

int main()
{
    TaskP_Params taskParams;
    uint32_t operateMode = 0; //  0 - EMAC, 1 - PRP

    PRUICSS_IntcInitData pruss_intc_initdata = PRUSS_INTC_INITDATA;

    HSR_PRP_socHwinit(PRUICSS_INSTANCE);

    memset(&boardInfo, 0, sizeof(Board_IDInfo));
    Board_getIDInfo(&boardInfo);
    UART_printf("boardName: %s\n", boardInfo.boardName);

#if !defined(SOC_K2G) && !defined(iceAMIC11x)
    Board_phyReset(2);
#endif /*SOC_K2G & iceAMIC11x*/

    prusshandle = PRUICSS_create((PRUICSS_Config *)pruss_config, PRUICSS_INSTANCE);
    emachandle = (ICSS_EmacHandle)malloc(sizeof(ICSS_EmacConfig));

    ICSSEMAC_InitConfig *switchEmacCfg = (ICSSEMAC_InitConfig *)calloc(1, sizeof(ICSSEMAC_InitConfig));
    switchEmacCfg->phyAddr[0] = Board_getPhyAddress(PRUICSS_INSTANCE, 1);

    if (operateMode)
    {
        switchEmacCfg->phyAddr[1] = Board_getPhyAddress(PRUICSS_INSTANCE, 2);
        switchEmacCfg->portMask = ICSS_EMAC_MODE_SWITCH;
        switchEmacCfg->halfDuplexEnable = 0;
        switchEmacCfg->learningEn = 1;
    }
    else
    {
        switchEmacCfg->portMask = ICSS_EMAC_MODE_MAC1;
        switchEmacCfg->halfDuplexEnable = 1;
        switchEmacCfg->learningEn = 0;
    }
    switchEmacCfg->ethPrioQueue[ICSS_EMAC_QUEUE1] = 0;
    switchEmacCfg->ethPrioQueue[ICSS_EMAC_QUEUE2] = 0;
    switchEmacCfg->ethPrioQueue[ICSS_EMAC_QUEUE3] = 0;
    switchEmacCfg->ethPrioQueue[ICSS_EMAC_QUEUE4] = 0;

    switchEmacCfg->enableIntrPacing = ICSS_EMAC_ENABLE_PACING;
    switchEmacCfg->ICSS_EmacIntrPacingMode = ICSS_EMAC_INTR_PACING_MODE1;
    switchEmacCfg->pacingThreshold = 100;

    switchEmacCfg->ICSS_EmacTTSEnableCycPktInterrupt = 0;

    HSR_PRP_socgetMACAddress(lclMac);

    switchEmacCfg->macId = lclMac;

#ifdef SOC_AM437x
    ICSSEmacDRVInit(emachandle, 1);
#else
    ICSSEmacDRVInit(emachandle, PRUICSS_INSTANCE);
#endif

    /*Registering Port 1 Link break callback */
    ICSS_EmacRegisterPort0ISRCallback(emachandle,
                                      (ICSS_EmacCallBack)hsrPrp_Port1linkResetCallBack,
                                      0); // callback arg
    /*Registering Port 2 Link break callback */
    ICSS_EmacRegisterPort1ISRCallback(emachandle,
                                      (ICSS_EmacCallBack)hsrPrp_Port2linkResetCallBack,
                                      0);  // callback arg

    /*Configure interrupts*/
    HSR_PRP_configureInterrupts(switchEmacCfg);

    ((ICSS_EmacObject *)emachandle->object)->pruIcssHandle = prusshandle;
    ((ICSS_EmacObject *)emachandle->object)->emacInitcfg = switchEmacCfg;

    ICSS_EmacInit(emachandle, &pruss_intc_initdata, operateMode ? ICSS_EMAC_MODE_SWITCH : (ICSS_EMAC_MODE_MAC1|ICSS_EMAC_MODE_DUALMAC));

    if (operateMode)
    {
        hsrPrphandle = (hsrPrpHandle *)malloc(sizeof(hsrPrpHandle));
        hsrPrpHandle_init(hsrPrphandle);  /*  Initializing HSR handle */
    }
    else
    {
        hsrPrphandle = NULL;
    }

    TaskP_Params_init(&taskParams);
    //taskParams.stacksize = 8192;
    taskParams.priority = 5;
    taskParams.name = (uint8_t *)"LEDTask";
    TaskP_create(taskLedBlink2, &taskParams);

    TaskP_Params_init(&taskParams);
    taskParams.priority = 15;       /* we need stack prio higher than LED I/O*/
    //taskParams.stacksize = 8192;
    taskParams.name = (uint8_t *)"SwitchTask";
    taskParams.arg0 = (void *)operateMode;
    TaskP_create(taskPruss, &taskParams);

#ifdef ICSS_PROTOCOL_RED

    TaskP_Params_init(&taskParams);
    taskParams.priority = 1;
    taskParams.name = (uint8_t *)"UARTMenuTask";
    taskParams.arg0 = (void *)emachandle;
    TaskP_create(taskUartMenu, &taskParams);

#endif /* ICSS_PROTOCOL_RED */

    if (operateMode)
    {
#ifdef ICSS_PROTOCOL_RED
        RedProtocolInit();
        RedProtocolConfigure(emachandle);

        ICSS_EmacCallBackObject *callBackHandle = ((ICSS_EmacObject *)emachandle->object)->callBackHandle;

        callBackHandle->txCallBack->callBack = (ICSS_EmacCallBack)RedTxPacket;
        callBackHandle->txCallBack->userArg = hsrPrphandle;

        callBackHandle->rxCallBack->callBack = (ICSS_EmacCallBack)RedRxPktGet;
        callBackHandle->rxCallBack->userArg = hsrPrphandle;
#endif /* ICSS_PROTOCOL_RED */

#ifdef ICSS_PROTOCOL_RED
        RedInit(emachandle);
#endif /* ICSS_PROTOCOL_RED */
    }

    TaskP_Params_init(&taskParams);
    taskParams.priority = 10;
    taskParams.name = (uint8_t *)"RxTask";
    taskParams.arg0 = (void *)emachandle;

    ((ICSS_EmacObject *)emachandle->object)->rxTaskHandle = TaskP_create(
                (Task_FuncPtr)ICSS_EMacOsRxTaskFnc, &taskParams);

    TaskP_Params_init(&taskParams);
    taskParams.priority = 7;
    taskParams.name = (uint8_t *)"LinkTask";
    taskParams.arg0 = (void *)emachandle;

    ((ICSS_EmacObject *)emachandle->object)->linkTaskHandle = TaskP_create(
                (Task_FuncPtr)ICSS_EMacOsLinkTaskFnc, &taskParams);

    PRUICSS_pinMuxConfig(prusshandle, 0x0); // PRUSS pinmuxing

    if(OSDRV_addNetifEntry((NIMUInitFn)&EmacInit, emachandle) == 0)
    {
        BIOS_exit(0);
    }

    BIOS_start();
    return 0;
}

/**
 * \brief returns interface MAC as a string
 *
 * \retval buf_str return buffer for MAC string (min length x bytes!)
 */
void getMacid(char *buf_str)
{
    int i;
    Uint8 bMacAddr[6];
    char *buf_ptr;
    NIMU_IF_REQ if_req;
    if_req.name[0] = 0;
    if_req.index   = 1;

    NIMUIoctl(NIMU_GET_DEVICE_MAC, &if_req, &bMacAddr, sizeof(bMacAddr));

    buf_ptr = buf_str;

    for(i = 0; i < 5; i++)
    {
        if(i != 5)
        {
            buf_ptr += sprintf(buf_ptr, "%02x:", (char)bMacAddr[i]);
        }
    }

    buf_ptr += sprintf(buf_ptr, "%02x", (char)bMacAddr[5]);
}

uint8_t ICSSEmacDRVInit(ICSS_EmacHandle handle, uint8_t instance)
{
    /* LLD attributes mallocs */
    ICSS_EmacObject *object = (ICSS_EmacObject *)calloc(1, sizeof(ICSS_EmacObject));

    /* Callback mallocs */
    ICSS_EmacCallBackObject* callBackObj = (ICSS_EmacCallBackObject*)calloc(1, sizeof(ICSS_EmacCallBackObject));
    callBackObj->learningExCallBack=(ICSS_EmacCallBackConfig*)calloc(1, sizeof(ICSS_EmacCallBackConfig));
    callBackObj->rxRTCallBack=(ICSS_EmacCallBackConfig*)calloc(1, sizeof(ICSS_EmacCallBackConfig));
    callBackObj->rxCallBack=(ICSS_EmacCallBackConfig*)calloc(1, sizeof(ICSS_EmacCallBackConfig));
    callBackObj->txCallBack=(ICSS_EmacCallBackConfig*)calloc(1, sizeof(ICSS_EmacCallBackConfig));
    object->callBackHandle = callBackObj;

    /*Allocate memory for learning*/
    object->macTablePtr = (HashTable_t*)calloc(1, NUM_PORTS * sizeof(HashTable_t));

    /*Allocate memory for PRU Statistics*/
    object->pruStat = (ICSS_EmacPruStatistics_t*)calloc(1, NUM_PORTS * sizeof(ICSS_EmacPruStatistics_t));

    /*Allocate memory for Host Statistics*/
    object->hostStat = (ICSS_EmacHostStatistics_t*)calloc(1, NUM_PORTS * sizeof(ICSS_EmacHostStatistics_t));

    /*Allocate memory for Storm Prevention*/
    object->stormPrevPtr = (stormPrevention_t*)calloc(1, NUM_PORTS * sizeof(stormPrevention_t));

    /* Base address initialization */
    ICSS_EmacHwAttrs *hwAttrs = (ICSS_EmacHwAttrs *)calloc(1, sizeof(ICSS_EmacHwAttrs));
    hwAttrs->emacBaseAddrCfg = (ICSS_EmacBaseAddressHandle_T)calloc(1, sizeof(ICSS_EmacBaseAddrCfgParams));

    /* Initialize hwAttrs->emacBaseAddrCfg */
    ICSS_EmacSocGetInitCfg((instance - 1), hwAttrs->emacBaseAddrCfg);

    handle->object = object;
    handle->hwAttrs = hwAttrs;

    return 0;
}

/*Initialize the PCP 2 Q mapping to 0 before starting firmware*/
void initializePCPtoQueueMap(ICSS_EmacHandle emachandle)
{
    uint8_t *bytePtr = (uint8_t *)((((ICSS_EmacHwAttrs *)
                                     (emachandle)->hwAttrs)->emacBaseAddrCfg)->sharedDataRamBaseAddr
                                   + QUEUE_2_PCP_MAP_OFFSET);
    memset(bytePtr, ICSS_EMAC_QUEUE4, 8);
}

/*Initialize the PCP 2 Q mapping to the required values*/
void loadPCPtoQueueMap(ICSS_EmacHandle emachandle, uint8_t *pcp_2_queue_map)
{
    uint8_t *bytePtr = (uint8_t *)((((ICSS_EmacHwAttrs *)
                                     (emachandle)->hwAttrs)->emacBaseAddrCfg)->sharedDataRamBaseAddr
                                   + QUEUE_2_PCP_MAP_OFFSET);
    memcpy(bytePtr, pcp_2_queue_map, 8);
}

void hsrPrpHandle_init(hsrPrpHandle *hsrPrphandle)
{
    hsrPrphandle->redSeqNr = 0;
    hsrPrphandle->supSeqNr = 0;
    hsrPrphandle->collision_pkt_dropped = &collision_pkt_dropped;
    hsrPrphandle->num_of_collision_occured = &num_of_collision_occured;
    hsrPrphandle->redPruCheckTimerHostTableFlag = 0;
    hsrPrphandle->redPruCheckTimerNodeTableClear = 0;
    hsrPrphandle->redPruCheckTimer = NULL;
    hsrPrphandle->redLifeCheckTimer = NULL;
    hsrPrphandle->redSupFrame = NULL;
    hsrPrphandle->redSupTask = NULL;
    hsrPrphandle->hsrPrpNodetableRefreshTask = NULL;
    hsrPrphandle->redLifeCheckSemaphore = NULL;
    hsrPrphandle->nodesTableSemaphore = NULL;
    hsrPrphandle->icssEmacHandle = emachandle;

#if defined(SOC_AM571x) || defined(SOC_AM572x) || defined(SOC_K2G)
    hsrPrphandle->indexArrayBase = (RED_INDEX_ARRAY_ENTRY *)((((
                                       ICSS_EmacHwAttrs *)emachandle->hwAttrs)->emacBaseAddrCfg)->sharedDataRamBaseAddr
                                   + INDEX_ARRAY_NT);
#else
    hsrPrphandle->indexArrayBase = (RED_INDEX_ARRAY_ENTRY *)((((
                                       ICSS_EmacHwAttrs *)emachandle->hwAttrs)->emacBaseAddrCfg)->dataRam0BaseAddr +
                                   INDEX_ARRAY_NT);
#endif

#if defined(SOC_AM571x) || defined(SOC_AM572x) || defined(SOC_K2G)
    hsrPrphandle->binArrayBase = (RED_BIN_ARRAY_ENTRY *)((((
                                     ICSS_EmacHwAttrs *)emachandle->hwAttrs)->emacBaseAddrCfg)->sharedDataRamBaseAddr
                                 + BIN_ARRAY);
#else
    hsrPrphandle->binArrayBase = (RED_BIN_ARRAY_ENTRY *)((((
                                     ICSS_EmacHwAttrs *)emachandle->hwAttrs)->emacBaseAddrCfg)->dataRam0BaseAddr +
                                 BIN_ARRAY);
#endif

    hsrPrphandle->nodeTableBase = (RED_NODE_TABLE_ENTRY *)((((
                                      ICSS_EmacHwAttrs *)emachandle->hwAttrs)->emacBaseAddrCfg)->sharedDataRamBaseAddr
                                  + NODE_TABLE_NT);
}

void hsrPrp_Port1linkResetCallBack(uint8_t linkStatus, void *arg2)
{
    /*
    Here arg2 already points to timeSyncHandle because of the registered callBack
    Yet, explicit usage of timeSyncHandle keeping futuristic approach wherein we might want to add other callbacks and access to handles other than timeSyncHandle
    */
}

void hsrPrp_Port2linkResetCallBack(uint8_t linkStatus, void *arg2)
{
    /*
    Here arg2 already points to timeSyncHandle because of the registered callBack
    Yet, explicit usage of timeSyncHandle keeping futuristic approach wherein we might want to add other callbacks and access to handles other than timeSyncHandle
    */
}
