/* vim: set ts=4 sw=4 et fdm=marker: */
/* sysLib.c - system-dependent routines */

/*
 *
 * This program is OPEN SOURCE software: you can redistribute it and/or modify it;
 * This program is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY;
 * without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 *
 */
/* includes */

#include <vxWorks.h>
#include <vsbConfig.h>
#include "config.h"
#if defined(INCLUDE_IPFTPS)
#include <ipcom_sysvar.h>
#endif

#define INCLUDE_MMU

#include <sysLib.h>
#include <string.h>
#include <intLib.h>
#include <taskLib.h>
#include <vxLib.h>
#include <muxLib.h>
#include <cacheLib.h>

#ifdef INCLUDE_MMU
#include <arch/arm/mmuArmLib.h>
#include <private/vmLibP.h>
#endif /* INCLUDE_MMU */
#include <dllLib.h>

#ifdef INCLUDE_VXIPI
#include <vxIpiLib.h>
#endif /* INCLUDE_VXIPI */

#ifdef _WRS_CONFIG_SMP
#include <arch/arm/vxAtomicArchLib.h>
#include <vxbTimerLib.h>
#endif /* _WRS_CONFIG_SMP */

#include <ioLib.h>
#include <stdio.h>
#include <logLib.h>
#include <ffsLib.h>
LOCAL UINT32 sysSdhcClkFreqGet(void);
LOCAL void sysGmacAddrSet(UINT8 *);
#ifdef INCLUDE_VXBUS

#include "vxbArmGenIntCtlrV3.h"
#include <hwif/vxbus/vxBus.h>
#include "hwconf.c"
#include "ft2000-4.h"

IMPORT void hardWareInterFaceInit(void);
#ifdef INCLUDE_SIO_UTILS
IMPORT void sysSerialConnectAll(void);
#endif /* INCLUDE_SIO_UTILS */
#endif /* INCLUDE_VXBUS */
#ifdef INCLUDE_PCI_BUS /* BSP PCI bus & config support */
#include <drv/pci/pciConfigLib.h>
#include <drv/pci/pciIntLib.h>
#include <drv/pci/pciAutoConfigLib.h>

/* 24-bit PCI network class ethernet subclass and prog. I/F code */

#include "pciCfgIntStub.c"
#endif /* INCLUDE_PCI_BUS */

#if defined(INCLUDE_PC_CONSOLE) || defined(INCLUDE_WINDML)
#include "vxbM6845Vga.c" /* X100 VGA */
#endif

typedef struct arm_smc_regs {
    int a0;
    int a1;
    int a2;
    int a3;
    int a4;
    int a5;
    int a6;
    int a7;
} ARM_SMC_REGS;
#ifdef _WRS_CONFIG_SMP

/* constant used with IPI to request reboot of application processors */

#define APPCORE_REBOOT 0xFEFEFEF0

LOCAL UINT32 rebootVar = 0; /* it passes the CPU index */

/* it protect from multiple reboots */

LOCAL SPIN_LOCK_ISR_DECL(rebootLock, 0);

/* Non-Boot CPU Start info. Managed by sysCpuEnable */

typedef struct sysMPCoreStartup_s {
    UINT32 newPC; /* Address of 'C' based startup code */
    UINT32 newSP; /* Stack pointer for startup */
    UINT32 newArg; /* vxWorks kernel entry point */
#ifndef _WRS_CONFIG_ARM_LPAE
    UINT32 newSync; /* Translation Table Base and sync */
#else /* _WRS_CONFIG_ARM_LPAE */
    UINT32 ttbcr; /* Translation Table Base Control Register */
    UINT64 newSync; /* 64 bits Translation Table Base and sync */
#endif /* !_WRS_CONFIG_ARM_LPAE */
} sysMPCoreStartup_t;

extern sysMPCoreStartup_t sysMPCoreStartup[VX_SMP_NUM_CPUS];
extern unsigned int arm_mmu_ttbr;

IMPORT void sysInit(void);
IMPORT void sysInit32(void);
IMPORT void mmuCortexA8DacrSet(UINT32 dacrVal);
IMPORT STATUS sysArmGicDevInit(void);
IMPORT void mmuCortexA8AEnable(UINT32 cacheState);
IMPORT void mmuCortexA8ADisable(void);
IMPORT void armInitExceptionModes(void);
IMPORT void sysCpuInit(void);
IMPORT void cacheCortexA9MPCoreSMPInit(void);
IMPORT void mmuCortexA8TLBIDFlushAll(void);
IMPORT MMU_LEVEL_1_DESC *mmuCortexA8TtbrGet(void);

UINT32 sysCpuAvailableGet(void);
STATUS sysCpuEnable(unsigned int, void (*startFunc)(void), char *);
void sysUsDelay(int us);
#endif /* _WRS_CONFIG_SMP */
int uartf(const char *fmt, /* format string */... /* optional arguments to format */);

/* {{{ XXX: added */
IMPORT void bspPinMuxInitialize(void);
IMPORT void bspDriverRegiser(void);
/* }}} */

/* imports */

IMPORT char end[]; /* end of system, created by ld */
#ifndef _ARCH_SUPPORTS_PROTECT_INTERRUPT_STACK
IMPORT VOIDFUNCPTR _func_armIntStackSplit; /* ptr to fn to split stack */
#endif /* !_ARCH_SUPPORTS_PROTECT_INTERRUPT_STACK */

/* globals */

#if defined(INCLUDE_MMU)

/* general parameters*/
#define MMU_PAGE_SIZE 4096
#define MMU_PAGE_SIZE_SHIFT 12
#define PAGE_SIZE MMU_PAGE_SIZE

/*
 * The following structure describes the various different parts of the
 * memory map to be used only during initialization by
 * vm(Base)GlobalMapInit() when INCLUDE_MMU_BASIC/FULL/GLOBAL_MAP are
 * defined.
 *
 * Clearly, this structure is only needed if the CPU has an MMU!
 *
 * The following are not the smallest areas that could be allocated for a
 * working system. If the amount of memory used by the page tables is
 * critical, they could be reduced.
 */

PHYS_MEM_DESC sysPhysMemDesc[] = {

    { LOCAL_MEM_LOCAL_ADRS, /* virtual address */
      LOCAL_MEM_LOCAL_ADRS, /* physical address */
      LOCAL_MEM_SIZE, MMU_ATTR_VALID_MSK | MMU_ATTR_PROT_MSK | MMU_ATTR_WRITEALLOCATE_MSK,
#if defined(_WRS_CONFIG_SMP) /* needs to be shared */
      MMU_ATTR_VALID | MMU_ATTR_SUP_RWX | MMU_ATTR_WRITEALLOCATE_SHARED
#else
      MMU_ATTR_VALID | MMU_ATTR_SUP_RWX | MMU_ATTR_WRITEALLOCATE
#endif /* _WRS_CONFIG_SMP */
    },

    { PIN_DEMUX_BASE, /*  pin MUX/DEMUX */
      PIN_DEMUX_BASE, 0x10000, MMU_ATTR_VALID_MSK | MMU_ATTR_PROT_MSK | MMU_ATTR_DEVICE_SHARED_MSK,
      MMU_ATTR_VALID | MMU_ATTR_SUP_RW | MMU_ATTR_DEVICE_SHARED },

    { UART_0_BASE_ADR, /*  UART */
      UART_0_BASE_ADR, 0x18000, MMU_ATTR_VALID_MSK | MMU_ATTR_PROT_MSK | MMU_ATTR_DEVICE_SHARED_MSK,
      MMU_ATTR_VALID | MMU_ATTR_SUP_RW | MMU_ATTR_DEVICE_SHARED },
    { 0x28207000, /*  CAN*/
      0x28207000, 0x1000, MMU_ATTR_VALID_MSK | MMU_ATTR_PROT_MSK | MMU_ATTR_DEVICE_SHARED_MSK,
      MMU_ATTR_VALID | MMU_ATTR_SUP_RW | MMU_ATTR_DEVICE_SHARED },
    { 0x2820b000, /*  GMAC*/
      0x2820b000, 0x00009000, MMU_ATTR_VALID_MSK | MMU_ATTR_PROT_MSK | MMU_ATTR_DEVICE_SHARED_MSK,
      MMU_ATTR_VALID | MMU_ATTR_SUP_RW | MMU_ATTR_DEVICE_SHARED },
    { 0x29900000, /*  GIC */
      0x29900000, 0x100000, MMU_ATTR_VALID_MSK | MMU_ATTR_PROT_MSK | MMU_ATTR_DEVICE_SHARED_MSK,
      MMU_ATTR_VALID | MMU_ATTR_SUP_RW | MMU_ATTR_DEVICE_SHARED },

    { FT_PCI_CONFIG_ADDR, /* PCI Memory address spcae */
      FT_PCI_CONFIG_ADDR, 0x10000000, MMU_ATTR_VALID_MSK | MMU_ATTR_PROT_MSK | MMU_ATTR_DEVICE_SHARED_MSK,
      MMU_ATTR_VALID | MMU_ATTR_SUP_RW | MMU_ATTR_DEVICE_SHARED },

    { 0x50000000, /* PCI io32Addr */
      0x50000000, 0x08000000, MMU_ATTR_VALID_MSK | MMU_ATTR_PROT_MSK | MMU_ATTR_DEVICE_SHARED_MSK,
      MMU_ATTR_VALID | MMU_ATTR_SUP_RW | MMU_ATTR_DEVICE_SHARED },

    { 0x58000000, /* PCI mem32Addr */
      0x58000000, 0x27000000, MMU_ATTR_VALID_MSK | MMU_ATTR_PROT_MSK | MMU_ATTR_DEVICE_SHARED_MSK,
      MMU_ATTR_VALID | MMU_ATTR_SUP_RW | MMU_ATTR_DEVICE_SHARED },

#ifdef DRV_FTQSPI
    { /* Qspi BootRom */
      SPI_FLASH_BASE_ADRS, SPI_FLASH_BASE_ADRS, QSPI_PROTECT_SIZE,
      MMU_ATTR_VALID_MSK | MMU_ATTR_PROT_MSK | MMU_ATTR_DEVICE_SHARED_MSK,
      MMU_ATTR_VALID | MMU_ATTR_SUP_RO | MMU_ATTR_DEVICE_SHARED },

    { /* Qspi Flash */
      SPI_FLASH_BASE_ADRS + QSPI_PROTECT_SIZE, SPI_FLASH_BASE_ADRS + QSPI_PROTECT_SIZE, SPI_FLASH_SIZE - QSPI_PROTECT_SIZE,
      MMU_ATTR_VALID_MSK | MMU_ATTR_PROT_MSK | MMU_ATTR_DEVICE_SHARED_MSK,
      MMU_ATTR_VALID | MMU_ATTR_SUP_RW | MMU_ATTR_DEVICE_SHARED },
#endif

};

int sysPhysMemDescNumEnt = NELEMENTS(sysPhysMemDesc);

#endif /* defined(INCLUDE_MMU) */

int sysBus = BUS_TYPE_PCI; /* system bus type (VME_BUS, etc) */
int sysCpu = CPU; /* system CPU type (e.g. ARMARCH6) */
char *sysBootLine = BOOT_LINE_ADRS; /* address of boot line */
char *sysExcMsg = EXC_MSG_ADRS; /* catastrophic message area */
int sysProcNum; /* processor number of this CPU */

/* locals */

/* defines */

/* externals */

#ifndef _ARCH_SUPPORTS_PROTECT_INTERRUPT_STACK
IMPORT void sysIntStackSplit(char *, long);
#endif /* !_ARCH_SUPPORTS_PROTECT_INTERRUPT_STACK */

/* globals */

char *sysPhysMemTop(void);

/* included source files */

#include <mem/nullNvRam.c>

/*******************************************************************************
*
* sysModel - return the model name of the CPU board
*
* This routine returns the model name of the CPU board.
*
* RETURNS: A pointer to a string identifying the board and CPU.
*
* ERRNO
*/

char *sysModel(void)
{
    return SYS_MODEL;
}

/*******************************************************************************
*
* sysBspRev - return the BSP version with the revision eg 1.2/<x>
*
* This function returns a pointer to a BSP version with the revision.
* e.g. 1.2/<x>. BSP_REV is concatenated to BSP_VERSION to form the
* BSP identification string.
*
* RETURNS: A pointer to the BSP version/revision string.
*
* ERRNO
*/

char *sysBspRev(void)
{
    return (BSP_VERSION BSP_REV);
}
extern VIRT_ADDR mmuPhysToVirt(PHYS_ADDR physAddr /* physical address to be translated */
);

extern PHYS_ADDR mmuVirtToPhys(VIRT_ADDR virtAddr /* virtual address to be translated */
);
#ifndef _WRS_CONFIG_ARM_LPAE
extern void cacheCortexA9LibInstall(VIRT_ADDR(physToVirt)(PHYS_ADDR), PHYS_ADDR(virtToPhys)(VIRT_ADDR));
#else
extern void cacheCortexA15LibInstall(VIRT_ADDR(physToVirt)(PHYS_ADDR), PHYS_ADDR(virtToPhys)(VIRT_ADDR));
#endif
void writeq(int data, int addr)
{
    *((volatile UINT32 *)addr) = (UINT32)data;
}
int readq(int addr)
{
    return *((volatile UINT32 *)addr);
}
void v8_outer_disable_l3cache(void)
{
    int i, pstate;

    for (i = 0; i < 8; i++)
        writeq(0x1, 0x3A200010 + i * 0x10000);

    for (i = 0; i < 8; i++) {
        do {
            pstate = readq(0x3A200018 + i * 0x10000);
        } while ((pstate & 0xf) != (0x1 << 2));
    }
}

void v8_outer_cache_flush_all(void)
{
    int i, pstate;
    for (i = 0; i < 8; i++)
        writeq(0x1, 0x3A200010 + i * 0x10000);
    for (i = 0; i < 8; i++) {
        do {
            pstate = readq(0x3A200018 + i * 0x10000);
        } while ((pstate & 0xf) != (0x1 << 2));
    }
    for (i = 0; i < 8; i++)
        writeq(0x3, 0x3A200010 + i * 0x10000);
}
void sysHwInit0(void)
{
    /*v8_outer_cache_flush_all();*/
    excVecBaseSet(LOCAL_MEM_LOCAL_ADRS);

#ifdef INCLUDE_CACHE_SUPPORT
#ifndef _WRS_CONFIG_ARM_LPAE
    cacheCortexA9LibInstall(mmuPhysToVirt, mmuVirtToPhys);
#else
    cacheCortexA15LibInstall(mmuPhysToVirt, mmuVirtToPhys);
#endif
#endif
    mmuCortexA8LibInstall(mmuPhysToVirt, mmuVirtToPhys);
}
#ifdef _WRS_CONFIG_SMP
/*******************************************************************************
*
* sysMsDelay - delay the specified amount of time (in milliseconds)
*
* This routine delays for approximately one millisecond. It just calls
* sysUsDelay to delay.
*
* RETURNS: N/A
*/

void sysMsDelay(UINT delay /* length of time in ms to delay */
)
{
    sysUsDelay(1000 * delay);
}

/*******************************************************************************
*
* sysDelay - fixed 1ms delay.
*
* This routine consumes approximately 1ms of time. It just calls sysMsDelay.
*
* RETURNS: N/A
*/

void sysDelay(void)
{
    sysMsDelay(1);
}

LOCAL UINT32 genTimerFreq = 0;

/*******************************************************************************
*
* armGenGetSysCount - get system counter count
*
* This routine gets the system counter count value.
*
* RETURNS: The count value.
*
* ERRNO: N/A
*/

LOCAL UINT64 armGenGetSysCount(void)
{
    UINT64 count1;

    _WRS_ASM("ISB");

    count1 = __inline__GetPhyTimerCnt();

    return count1;
}

/*******************************************************************************
*
* armGenGetFreq - get generic timer frequency
*
* This routine gets the generic timer frequency.
*
* RETURNS: The timer frequency.
*
* ERRNO: N/A
*/
#ifndef __DCC__
LOCAL void armGenGetFreq(void)
{
    _WRS_ASM("MRC p15, 0, %0, c14, c0, 0" : "=r"(genTimerFreq));
}
#else
/* clang-format off */
__asm volatile UINT32 __macro__GetCntFreq (void)
{
! "r0"
   mrc p15, 0, r0, c14, c0, 0
}

LOCAL UINT32 __inline__GetCntFreq (void)
{
    return __macro__GetCntFreq();
}
/* clang-format on */

LOCAL UINT32 armGenGetFreq(void)
{
    static UINT32 freq = 0;

    freq = __inline__GetCntFreq();
    genTimerFreq = freq; /* update global value. */

    return freq;
}

#endif
/*******************************************************************************
*
* sysUsDelay - delay the specified amount of time (in microseconds)
*
* This routine delays for approximately the requested microseconds. The accuracy
* of the delay increases as the requested delay increases due to a certain
* amount of overhead.
*
* NOTE:  This routine will not relinquish the CPU, as it is meant to perform a
* busy loop delay.
*
* RETURNS: N/A
*
* ERRNO: N/A
*
*\NOMANUAL
*/

void sysUsDelay(int us)
{
    volatile UINT64 oldVal;
    volatile UINT64 newVal;
    volatile UINT64 incElapsed = 0;
    volatile UINT64 totalDelta;
    volatile UINT64 maxCount;

    if (us <= 0) {
        printf("usDelay, wrong parameter: %d us\n", us);
        return;
    }

    totalDelta = (genTimerFreq * (UINT64)us + 1000000 - 1) / 1000000;
    maxCount = 0xffffffffffffffffull;

    oldVal = armGenGetSysCount();
    while (incElapsed < totalDelta) {
        newVal = armGenGetSysCount();

        if (newVal == oldVal)
            continue;

        if (newVal > oldVal) {
            incElapsed += (newVal - oldVal);
        } else {
            incElapsed += ((maxCount - oldVal) + newVal);
        }

        oldVal = newVal;
    }
}

#endif /*_WRS_CONFIG_SMP*/

/*******************************************************************************
*
* sysHwInit - initialize the CPU board hardware
*
* This routine initializes various features of the hardware.
* Normally, it is called from usrInit() in usrConfig.c.
*
* NOTE: This routine should not be called directly by the user.
*
* RETURNS: N/A
*
* ERRNO
*/
extern void vxbftCanRegister(void);
#ifdef INCLUDE_DRV_STORAGE_NVME
extern void vxbNvmeStorageRegister(void);
#endif
#ifdef DRV_X100DC
extern void ftX100DcDevicePciRegister(void);
#endif

void sysHwInit(void)
{
    uartf("...............................\r\n");
    
    bspPinMuxInitialize();
    
#ifdef DRV_X100DC
    ftX100DcDevicePciRegister();
#endif

    bspDriverRegiser();
    
#ifdef INCLUDE_VXBUS
    hardWareInterFaceInit();
#endif /* INCLUDE_VXBUS */
#ifdef DRV_PCIBUS_FT
    ftPciRegister();
#endif
#ifdef INCLUDE_DRV_STORAGE_NVME
    vxbNvmeStorageRegister();
#endif

#ifdef FORCE_DEFAULT_BOOT_LINE
    strncpy(sysBootLine, DEFAULT_BOOT_LINE, strlen(DEFAULT_BOOT_LINE) + 1);
#endif /* FORCE_DEFAULT_BOOT_LINE */

#ifdef _WRS_CONFIG_SMP
    _vxb_delayRtn = (void (*)())sysDelay;
    _vxb_msDelayRtn = (void (*)(int))sysMsDelay;
    _vxb_usDelayRtn = (void (*)(int))sysUsDelay;
    armGenGetFreq();
#endif /*_WRS_CONFIG_SMP*/
}

/*******************************************************************************
*
* sysHwInit2 - additional system configuration and initialization
*
* This routine connects system interrupts and does any additional
* configuration necessary.  Note that this is called from
* sysClkConnect() in the timer driver.
*
* RETURNS: N/A
*
* ERRNO
*/
void sysHwInit2(void)
{
    static BOOL initialised = FALSE;

    if (initialised) {
        return;
    }

#ifdef INCLUDE_VXBUS

    vxbDevInit();

#ifdef INCLUDE_SIO_UTILS
    sysSerialConnectAll();
#endif /* INCLUDE_SIO_UTILS */

    taskSpawn("tDevConn", 11, 0, 10000, vxbDevConnect, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9);
#endif /* INCLUDE_VXBUS */

    initialised = TRUE;

#if defined(INCLUDE_IPFTPS)
    ipcom_sysvar_set("ipftps.root", TFFS_FLASH_MOUNT_POINTOT, IPCOM_SYSVAR_FLAG_OVERWRITE);
    ipcom_sysvar_set("ipftps.dir", TFFS_FLASH_MOUNT_POINTOT, IPCOM_SYSVAR_FLAG_OVERWRITE);
#endif
}

/*******************************************************************************
*
* sysPhysMemTop - get the address of the top of physical memory
*
* This routine returns the address of the first missing byte of memory,
* which indicates the top of memory.
*
* Normally, the user specifies the amount of physical memory with the
* macro LOCAL_MEM_SIZE in config.h.  BSPs that support run-time
* memory sizing do so only if the macro LOCAL_MEM_AUTOSIZE is defined.
* If not defined, then LOCAL_MEM_SIZE is assumed to be, and must be, the
* true size of physical memory.
*
* NOTE: Do no adjust LOCAL_MEM_SIZE to reserve memory for application
* use.  See sysMemTop() for more information on reserving memory.
*
* RETURNS: The address of the top of physical memory.
*
* ERRNO
*
* SEE ALSO: sysMemTop()
*/

char *sysPhysMemTop(void)
{
    static char *physTop = NULL;

    if (physTop == NULL) {
        physTop = (char *)(LOCAL_MEM_LOCAL_ADRS + LOCAL_MEM_SIZE);
    }

    return physTop;
}

/*******************************************************************************
*
* sysMemTop - get the address of the top of VxWorks memory
*
* This routine returns a pointer to the first byte of memory not
* controlled or used by VxWorks.
*
* The user can reserve memory space by defining the macro USER_RESERVED_MEM
* in config.h.  This routine returns the address of the reserved memory
* area.  The value of USER_RESERVED_MEM is in bytes.
*
* RETURNS: The address of the top of VxWorks memory.
*
* ERRNO
*/

char *sysMemTop(void)
{
    static char *memTop = NULL;
#ifdef INCLUDE_EDR_PM
    memTop = (char *)sysPhysMemTop() - USER_RESERVED_MEM - PM_RESERVED_MEM;
#else
    memTop = (char *)sysPhysMemTop() - USER_RESERVED_MEM;
#endif /* INCLUDE_EDR_PM */
    return (memTop);
}

/*******************************************************************************
*
* sysToMonitor - transfer control to the ROM monitor
*
* This routine transfers control to the ROM monitor.  It is usually called
* only by reboot() -- which services ^X -- and bus errors at interrupt
* level.  However, in some circumstances, the user may wish to introduce a
* new <startType> to enable special boot ROM facilities.
*
* RETURNS: Does not return.
*
* ERRNO
*/
/*Please refer to FT-1500A-4V1.5.pdf for GPIO general description.*/

STATUS sysToMonitor(int startType /* passed to ROM to tell it how to boot */
)
{
    ARM_SMC_REGS input = { 0 };
    ARM_SMC_REGS output = { 0 };

#ifdef _WRS_CONFIG_ARM_LPAE

    /* reset TTBCR */

    mmuCortexA8TtbcrSet(0);
#endif /* _WRS_CONFIG_ARM_LPAE */
    input.a0 = 0x84000009;
    armSmcCall(&input, &output);
    while (1) {
    }

    return OK; /* in case we ever continue from ROM monitor */
}

#if defined(_WRS_CONFIG_SMP)

extern void armv7a_secondary_wake();
extern void arm_int_enable();

/*******************************************************************************
*
* sysCpuStart - vxWorks startup
*
* This routine establishes a CPUs vxWorks envirnonment.  Note that the MMU
* and caches have been enabled before this routine is called.
*
* This is NOT callable from C.
*
* RETURNS: N/A
*
* ERRNO: N/A
*
* \NOMANUAL
*/

IMPORT void excVBARSet(UINT32 adr);

LOCAL void sysCpuStart(void (*startFunc)(void), UINT32 cpuNum)
{
    /* sysMsDelay(cpuNum*100);*/
#ifdef INCLUDE_VFP
    vfpEnable();
#endif

    excVBARSet(LOCAL_MEM_LOCAL_ADRS);

    /* Initialise ARM exception mode registers */

    armInitExceptionModes();

    /* Enable Local S/W interrupts */

    sysArmGicDevInit();
    sysMPCoreStartup[cpuNum].newSync = 0;

    /* Enter the Kernel */
    if (cpuNum == VX_SMP_NUM_CPUS - 1)
        uartf("sysCpuStart(). cpu to start : %d  done! \r\n", cpuNum);

    (*startFunc)();
}

/*******************************************************************************
*
* sysCpuEnable - enable a multi core CPU
*
* This routine brings a CPU out of reset
*
* RETURNS: OK or ERROR
*
* ERRNO: N/A
*
* \NOMANUAL
*/

STATUS sysCpuEnable(unsigned int cpuNum, void (*startFunc)(void), char *stackPtr)
{
    static INT32 sysCpuEnableFirst = 1;

    /* Validate cpuNum */

    if (cpuNum < 1 || cpuNum >= VX_SMP_NUM_CPUS)
        return (ERROR);

    if (sysCpuEnableFirst == 1) {
        /*
         * IPIs cannot be connected in sysToMonitor, because interrupt handlers
         * cannot be installed in interrupt context.
         * sysToMonitor can be called from interrupt context as well
         * (WIND00321261).
         */

        /*
         * Connect and enable IPI for Application Processor cores.
         * The INT_LVL_MPCORE_IPI08 is sent by core 0 to all the AP cores.
         */

        vxIpiConnect(INT_LVL_MPCORE_IPI08, (IPI_HANDLER_FUNC)(sysToMonitor), (void *)(APPCORE_REBOOT));

        vxIpiEnable(INT_LVL_MPCORE_IPI08);

        /*
         * Connect and enable IPI for core0.
         * The INT_LVL_MPCORE_RESET is sent by an Application processor core
         * to core0.
         */

        vxIpiConnect(INT_LVL_MPCORE_RESET, (IPI_HANDLER_FUNC)(sysToMonitor), (void *)&rebootVar);

        vxIpiEnable(INT_LVL_MPCORE_RESET);

        sysCpuEnableFirst = 0;
    }

    /* Setup init values */

    sysMPCoreStartup[cpuNum].newPC = (UINT32)sysCpuStart;
    sysMPCoreStartup[cpuNum].newSP = (UINT32)stackPtr;
    sysMPCoreStartup[cpuNum].newArg = (UINT32)startFunc;
#ifndef _WRS_CONFIG_ARM_LPAE
    sysMPCoreStartup[cpuNum].newSync = mmuCortexA8TtbrGetAll();
#else /* _WRS_CONFIG_ARM_LPAE */
    sysMPCoreStartup[cpuNum].ttbcr = mmuCortexA8TtbcrGet();
    sysMPCoreStartup[cpuNum].newSync = mmuCortexA8TtbrGet64();
#endif /* !_WRS_CONFIG_ARM_LPAE */

    arm_mmu_ttbr = sysMPCoreStartup[cpuNum].newSync;

    /* Make sure data hits memory */

    cacheFlush((CACHE_TYPE)DATA_CACHE, (void *)sysMPCoreStartup, (size_t)(sizeof(sysMPCoreStartup)));

    cachePipeFlush();
#if 0
    if (cpuNum == (VX_SMP_NUM_CPUS-1))
    {
    *(volatile unsigned int *)(FT_SECONDARY_BOOT_ADDR) = (UINT32)sysCpuInit;

     cacheFlush((CACHE_TYPE)DATA_CACHE, FT_SECONDARY_BOOT_ADDR &(~0x3f), 64);


    /* Setup CPU for init */

    VX_SYNC_BARRIER ();

    /* wake up core1 from wfe using sev. */

    /*WRS_ASM ("sev");*/

    armv7a_secondary_wake();
   /* uartf("sysCpuEnable(). cpu to waken : %d  done! \r\n", cpuNum);*/
    }
#else
    {
        ARM_SMC_REGS input = { 0 };
        ARM_SMC_REGS output = { 0 };
        PHYS_ADDR physAddr;

        if (vmTranslate(NULL, (VIRT_ADDR)sysCpuInit, &physAddr) == ERROR)
            return ERROR;

        input.a0 = 0x84000003;
        input.a1 = cpuNum;
        if (2 == input.a1) {
            input.a1 = 0x100; /* Core2's number is 0x100 */
        }
        if (3 == input.a1) {
            input.a1 = 0x101; /* Core3's number is 0x101 */
        }
        /*input.a2 = (UINT32)(physAddr >> 32);*/
        input.a2 = (UINT32)(physAddr & 0xFFFFFFFF);
        armSmcCall(&input, &output);

        VX_SYNC_BARRIER();
#ifdef _WRS_CONFIG_ARM_LPAE
        sysMsDelay(cpuNum * 100);
#endif

        /* wake up core from wfe using sev. */
    }
#endif

    return OK;
}

/*******************************************************************************
*
* sysCpuAvailableGet - return the number of CPUs available
*
* This routine gets the number of CPUs available.
*
* RETURNS:  number of CPU cores available
*
* ERRNO: N/A
*/

UINT32 sysCpuAvailableGet(void)
{
    return VX_SMP_NUM_CPUS;
}

#endif /* if defined (_WRS_CONFIG_SMP) */
/****************************************************************************
*
* sysProcNumGet - get the processor number
*
* This routine returns the processor number for the CPU board, which is
* set with sysProcNumSet().
*
* RETURNS: The processor number for the CPU board.
*
* ERRNO
*
* SEE ALSO: sysProcNumSet()
*/

int sysProcNumGet(void)
{
    return 0;
}

/****************************************************************************
*
* sysProcNumSet - set the processor number
*
* Set the processor number for the CPU board.  Processor numbers should be
* unique on a single backplane.
*
* NOTE
* By convention, only processor 0 should dual-port its memory.
*
* RETURNS: N/A
*
* ERRNO
*
* SEE ALSO: sysProcNumGet()
*/

void sysProcNumSet(int procNum /* processor number */
)
{
    sysProcNum = procNum;
}

#ifdef INCLUDE_SIO_UTILS
/******************************************************************************
*
* bspSerialChanGet - get the SIO_CHAN device associated with a serial channel
*
* The sysSerialChanGet() routine returns a pointer to the SIO_CHAN
* device associated with a specified serial channel. It is called
* by usrRoot() to obtain pointers when creating the system serial
* devices, `/tyCo/x'. It is also used by the WDB agent to locate its
* serial channel.  The VxBus function requires that the BSP provide a
* function named bspSerialChanGet() to provide the information about
* any non-VxBus serial channels, provided by the BSP.  As this BSP
* does not support non-VxBus serial channels, this routine always
* returns ERROR.
*
* RETURNS: ERROR, always
*
* ERRNO
*
* \NOMANUAL
*/

SIO_CHAN *bspSerialChanGet(int channel /* serial channel */
)
{
    return ((SIO_CHAN *)ERROR);
}
#endif /* INCLUDE_SIO_UTILS */

#ifdef DRV_PCIBUS_FT

LOCAL UCHAR sysPciExIntRoute[4] = { 60, 61, 62, 63 };

/*******************************************************************************
*
* sysPciExAutoconfigInclude - PCI Express autoconfig support routine
*
* This routine performs the PCI Express auto configuration support function.
*
* RETURNS: OK or ERROR
*/

STATUS sysPciExAutoconfigInclude(PCI_SYSTEM *pSys, /* PCI_SYSTEM structure pointer */
                                 PCI_LOC *pLoc, /* pointer to function in question */
                                 UINT devVend /* deviceID/vendorID of device */
)
{
    if (pLoc->bus == 0 && pLoc->device == 0 && pLoc->function == 0)
        return ERROR;

    return OK;
}

/*******************************************************************************
*
* sysPciExAutoconfigIntrAssign - PCI Express autoconfig support routine
*
* This routine peforms the PCI Express auto configuration interrupt assignment
* support function.
*
* RETURNS: PCI interrupt line number given pin mask
*/
UCHAR pci_int_log[32] = { 0 };
int pci_int_idx = 0;
UCHAR sysPciExAutoconfigIntrAssign(PCI_SYSTEM *pSys, /* PCI_SYSTEM structure pointer */
                                   PCI_LOC *pLoc, /* pointer to function in question */
                                   UCHAR pin /* contents of PCI int pin register */
)
{
    pci_int_log[pci_int_idx++] = pin;
    if (pci_int_idx == 32) {
        pci_int_idx = 0;
    }

    return sysPciExIntRoute[pin - 1];
}

#endif /* DRV_PCIBUS_M85XX */

/*   this is for multi cluster*/
#if defined(_WRS_CONFIG_SMP)
UINT32 cpuIndexMap[4] = { 0, 1, 0x100, 0x101 };

UINT32 vxCpuIdGetByIndex(UINT32 idx)
{
    if (idx >= VX_MAX_SMP_CPUS)
        return (UINT32)-1;

    return cpuIndexMap[idx];
}
#endif /* _WRS_CONFIG_SMP  */
UCHAR sysPciInByte(ULONG address)
{
    return *(volatile UCHAR *)address;
}
void sysPciOutByte(ULONG address, UCHAR data)
{
    *(volatile UCHAR *)address = data;
}

UINT16 sysPciInWord(ULONG address)
{
    return *(volatile UINT16 *)address;
}
void sysPciOutWord(ULONG address, UINT16 data)
{
    *(volatile UINT16 *)address = data;
}

UINT sysPciInLong(ULONG address)
{
    return *(volatile UINT *)address;
}
void sysPciOutLong(ULONG address, UINT data)
{
    *(volatile UINT *)address = data;
}

extern int fioFormatV(FAST const char *fmt, /* format string */
                      va_list vaList, /* pointer to varargs list */
                      FUNCPTR outRoutine, /* handler for args as they're formatted */
                      int outarg /* argument to routine */
);

UINT32 uart_tx_fifo_addr = UART_1_BASE_ADR; /* FT UART BASE ADDR */

/*  prime cell sio constants */
#define UARTFR 0x18 /* Flag register */
#define FLG_UTXFF (1 << 5) /* UART Tx FIFO Full */
#define FLG_UTXFE (1 << 7) /* UART Tx FIFO Empty */
static void charout(char cha)
{
    volatile UINT32 reg_val32;

    /* for PrimeCell sio */
    reg_val32 = *(volatile UINT32 *)(uart_tx_fifo_addr + UARTFR);

    /* is the transmitter ready to accept a character? */

    while ((reg_val32 & FLG_UTXFF) != 0x00) {
        reg_val32 = *(volatile UINT32 *)(uart_tx_fifo_addr + UARTFR);
    }

    *(volatile UINT32 *)uart_tx_fifo_addr = cha;
}

void stringout(char *str)
{
    char *pbuf = str;
    char cha;
    volatile UINT32 reg_val32;

    while ((cha = *pbuf++) != 0)
        charout(cha);
    do {
        reg_val32 = *(volatile UINT32 *)(uart_tx_fifo_addr + UARTFR);
    } while ((reg_val32 & FLG_UTXFE) == 0);
}

static int bufferPut(char *inbuf, /* pointer to source buffer */
                     int length, /* number of bytes to copy */
                     char **outptr /* pointer to destination buffer */
)
{
    bcopy(inbuf, *outptr, length);
    *outptr += length;

    return (OK);
}

int uartf(const char *fmt, /* format string */
          ... /* optional arguments to format */
)
{
    char charArray[128] = {
        0,
    };
    char *buffer = charArray;
    va_list vaList; /* traverses argument list */
    int nChars;

    va_start(vaList, fmt);
    nChars = fioFormatV(fmt, vaList, bufferPut, (int)&buffer);
    va_end(vaList);

    *buffer = EOS;

    stringout(charArray);
    return (nChars);
}

/*******************************************************************************
*
* sysSdhcClkFreqGet - get sdhc freq
*
* This routine get sdhc freq.
*
* RETURNS: freq
*/

LOCAL UINT32 sysSdhcClkFreqGet(void)
{
    return 600000000;
}

/*******************************************************************************
*
* sysGmacAddrSet - set gmac mac address
*
* This routine set gmac mac address.
*
* RETURNS: void
*/

LOCAL void sysGmacAddrSet(UINT8 *macAddr)
{
    static int addr = 0x38;

    macAddr[0] = 0;
    macAddr[1] = 0;
    macAddr[2] = 0x3e;
    macAddr[3] = 0x2;
    macAddr[4] = 0x2;
    macAddr[5] = addr;

    addr += 2;
    return;
}