ft2004_km02_vxworks/sysLib.c

/* 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"


#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 */);

/* 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 Flash */
    SPI_FLASH_BASE_ADRS,            
    SPI_FLASH_BASE_ADRS,
    SPI_FLASH_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
__asm volatile UINT32 __inline__GetCntFreq (void)
    {
! "r0"
    mrc p15, 0, r0, c14, c0, 0
    }

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)
    {

    /*
    swdo_swj_pad:   can_rxd_1   i2c_2_scl
    tdo_swj_pad:    can_rxd_2   i2c_2_sda

    CAN_1,CAN_2 and I2C_2, only choose ONE.
    */
  writeq(readq(PIN_DEMUX_BASE+REG204)|CAN_TXD_0|CAN_RXD_0|CAN_TXD_1|CAN_RXD_1, 
         PIN_DEMUX_BASE+REG204); /* pad pin DeMux. enable CAN */
 #ifdef DRV_X100DC   
  ftX100DcDevicePciRegister();
 #endif
#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;
  
    }

/*******************************************************************************
*
* 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 */

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;

    while((cha = *pbuf++) != 0)
        charout(cha);
}

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 ;
    }