sysalib.s

Freescale MPC85xx BSP (8555/8541)。绝对可用的。

/* Copyright 1984-2004 Wind River Systems, Inc. */
#include "copyright_wrs.h"

/*
modification history
--------------------
01m,30jul04,dtr  adding func sysTimeBaseLGet. 
	         Modify instruction parity handler to reboot if errata#30 hit.
01l,22jul04,dtr  Add sysL1CsrSet routine.
01k,05jun04,dtr  Enabling branch prediction and L1 parity error handler.
01j,20nov03,dtr  Work on issue.
01i,22oct03,dtr  Removing compiler warning.
01h,02oct03,dtr  Ensure contiguous cache flush.
01g,17sep03,dtr  Remove LCRR update and removing PCI CAM entry.Set LAWBAR4 
	         to cover max flash of 16MB.
01f,04sep03,dtr  Adding in stack frame pointer init.
01e,02sep03,dtr  Adding sysPCGet function to query if rom resident image.
01d,04aug03,dtr  Fix vision vxworks image.
01c,29jul03,dtr  Removing magic numbers.
01b,07jul03,mil  Added VISION_VXWORKS for no bootrom and local bus access
                 window with bootrom.
01a,03oct02,dtr  Created.
*/

#define _ASMLANGUAGE
#include "vxWorks.h"
#include "asm.h"
#include "config.h"
#include "sysLib.h"
#include "sysL2Cache.h"
#include "arch/ppc/mmuE500Lib.h"
	
	FUNC_EXPORT(sysInit)
	FUNC_EXPORT(_sysInit)
	FUNC_EXPORT(vxL2CTLSet)
	FUNC_EXPORT(vxL2CTLGet)
	FUNC_EXPORT(sysCacheFlush)
	FUNC_IMPORT(usrInit)
	FUNC_EXPORT(sysInWord)
	FUNC_EXPORT(sysOutWord)
	FUNC_EXPORT(sysInLong)
	FUNC_EXPORT(sysOutLong)
        FUNC_EXPORT(sysInByte)
	FUNC_EXPORT(sysOutByte)
	FUNC_EXPORT(sysPciRead32)
	FUNC_EXPORT(sysPciWrite32)
	FUNC_EXPORT(sysPciInByte)
	FUNC_EXPORT(sysPciOutByte)
	FUNC_EXPORT(sysPciInWord)
	FUNC_EXPORT(sysPciOutWord)
	FUNC_EXPORT(sysPciInLong)
	FUNC_EXPORT(sysPciOutLong)
        FUNC_EXPORT(sysPCGet)   /* get the value of the PC register */
        FUNC_EXPORT(sysL1Csr1Set)
        FUNC_EXPORT(sysTimeBaseLGet)
#ifdef INCLUDE_L1_IPARITY_HDLR_INBSP
        FUNC_EXPORT(sysIParityHandler)
        FUNC_EXPORT(sysIvor1Set)
        FUNC_EXPORT(jumpIParity)
#endif  /* INCLUDE_L1_IPARITY_HDLR_INBSP */
	FUNC_EXPORT(disableBranchPrediction)
	_WRS_TEXT_SEG_START
        DATA_IMPORT(inFullVxWorksImage)
	
#define	CACHE_ALIGN_SHIFT	5	/* Cache line size == 2**5 */
	
FUNC_LABEL(_sysInit)		
FUNC_BEGIN(sysInit)
#ifdef VISION_VXWORKS
        /* Set up vector offsets */
	xor   r0, r0, r0
        addi  r3, r0, 0x100
        mtspr IVOR0, r3
        addi  r3, r0, 0x200
        mtspr IVOR1, r3
        addi  r3, r0, 0x300
        mtspr IVOR2, r3
        addi  r3, r0, 0x400
        mtspr IVOR3, r3
        addi  r3, r0, 0x500
        mtspr IVOR4, r3
        addi  r3, r0, 0x600
        mtspr IVOR5, r3
        addi  r3, r0, 0x700
        mtspr IVOR6, r3
        /* skipping IVOR7 0x800: no FPU on e500 */
        addi  r3, r0, 0x900
        mtspr IVOR8, r3
        /* skipping IVOR9 0xa00: no aux processor on e500 */
        addi  r3, r0, 0xb00
        mtspr IVOR10, r3
        addi  r3, r0, 0xc00
        mtspr IVOR11, r3
        addi  r3, r0, 0xd00
        mtspr IVOR12, r3
        addi  r3, r0, 0xe00
        mtspr IVOR13, r3
        addi  r3, r0, 0xf00
        mtspr IVOR14, r3
        addi  r3, r0, 0x1000
        mtspr IVOR15, r3
        addi  r3, r0, 0x1100       /* assigning 0x1100 for SPU */
        mtspr IVOR32, r3
        addi  r3, r0, 0x1200       /* assigning 0x1200 for FP data */
        mtspr IVOR33, r3
        addi  r3, r0, 0x1300       /* assigning 0x1300 for FP round */
        mtspr IVOR34, r3
        addi  r3, r0, 0x1400       /* assigning 0x1400 for perf mon */
        mtspr IVOR35, r3


        /* Initialize PID reg to match TID field of TLB entry (unless using TID=0) */

	xor   r0,r0,r0
	mtspr PID0 , r0
	mtspr PID1 , r0
	mtspr PID2 , r0

        /*
         * Write TLB entry for initial program memory page
         *
         * - Specify EPN, RPN, and TSIZE as appropriate for system
         * - Set valid bit
         * - Specify TID=0
         * - Specify TS=0 or else MSR[IS,DS] must be set to correspond to TS=1
         * - Specify storage attributes (W, I, M, G, E, U0 - U3) as appropriate
         * - Enable supervisor mode fetch, read, and write access (SX, SR, SW)
         */

        /*
         * TLB1 #0.  RAM - Cached 0x00000000 -> 0x000000000.  256MB
         * Attributes: SX/SW/SR
         */


        addis  r4,0,0x1000           /* TLBSEL = TLB1(CAM) , ESEL = 0 */ 
        ori    r4,r4,0x0000
        mtspr  MAS0, r4
        addis  r5,0,0xC000           /* V = 1, IPROT = 1, TID = 0*/
        ori    r5,r5,0x0900          /* TS = 0, TSIZE = 256 MByte page size*/
        mtspr  MAS1, r5
        addis  r6,0,0xf000           /* EPN = 0xfff00000*/
        ori    r6,r6,0x000a          /* WIMGE = 01000*/
        mtspr  MAS2, r6
        addis  r7,0,0xf000           /* RPN = 0xfff00000*/
        ori    r7,r7,0x0015          /* Supervisor XWR*/
        mtspr  MAS3, r7
        tlbwe         
        tlbsync                     

	
        /* Setup the memory mapped register address */
        lis     r6, HIADJ(CCSBAR >> 12)
        addi    r6, r6, LO(CCSBAR >> 12)
        lis     r7, HIADJ(0xFF700000)
        addi    r7, r7,LO(0xFF700000)
	stw     r6, 0(r7)
	sync
	isync
#else  /* VISION_VXWORKS */
        xor p0,p0,p0
	mtspr TCR,p0
	xor   r6, r6, r6
	msync
	isync
	mtspr L1CSR0, r6		/* Disable the Data cache */
        li   r6, 0x0002              
	msync
	isync
	mtspr L1CSR0, r6		/* Invalidate the Data cache */
        li    r6, 0x0000              
        msync
	isync
	mtspr L1CSR1, r6	 /* Disable the Instrunction cache */
        li   r6, 0x0002              
        msync
	isync
	mtspr L1CSR1, r6	/* Invalidate the Instruction cache */
        isync
        li   r6, 0x0000              
	msync
	isync
	mtspr L1CSR1, r6        /* Enable the Instruction cache*/
	msync
	isync
#if 0
	/* Memory mapped region base address */
	
        lis     r6, HIADJ(M85XX_LAWBAR3(CCSBAR))
        addi    r6, r6, LO(M85XX_LAWBAR3(CCSBAR))
	
	/* Initialise the Local Address Windows */
        lis     r7, HIADJ(FLASH1_BASE_ADRS >> LAWBAR_ADRS_SHIFT)
        addi    r7, r7, LO(FLASH1_BASE_ADRS >> LAWBAR_ADRS_SHIFT)
	stw     r7, 0x0(r6)

        lis     r6, HIADJ(M85XX_LAWAR3(CCSBAR))
        addi    r6, r6, LO(M85XX_LAWAR3(CCSBAR))

        lis     r7, HIADJ (LAWAR_ENABLE | \
		           LAWAR_TGTIF_LBC | \
	                   LAWAR_SIZE_16MB )
        addi    r7, r7, LO (LAWAR_ENABLE | \
	                    LAWAR_TGTIF_LBC | \
	                    LAWAR_SIZE_16MB)
	stw     r7, 0x0(r6)
#endif
        mbar 0

#endif  /* VISION_VXWORKS */
#ifdef INCLUDE_BRANCH_PREDICTION
        li   r6, _PPC_BUCSR_FI
        mtspr 1013,r6
        li   r6, _PPC_BUCSR_E
        mtspr 1013,r6
#endif

	lis  r6, HIADJ(inFullVxWorksImage)
	addi r6, r6,LO(inFullVxWorksImage)
	li   r7, TRUE
	stw  r7, 0(r6)
	/* initialize the stack pointer */
	
	lis     sp, HIADJ(RAM_LOW_ADRS)
	addi    sp, sp, LO(RAM_LOW_ADRS)
        addi    sp, sp, -FRAMEBASESZ    /* get frame stack */
        li      r3, BOOT_WARM_AUTOBOOT /* set the default boot code */
	
	b usrInit
FUNC_END(sysInit)


	.balign 32
/******************************************************************************
*
* vxL2CTLSet - Set the value of the L@ cache control register
*
* This routine returns the value written.
*
* SYNOPSIS
* \ss
* UINT32 sysL2CTLSet
*     (
*     UINT32 value,
*     UINT32 *addr
*     )
* \se
*
* RETURNS: This routine returns the value in the L2CTL reg.
*/
	
FUNC_BEGIN(vxL2CTLSet)
	mbar	0 
	isync
	stw	p0,0(p1)
	lwz	p0,0(p1)
	mbar	0
	isync
	blr
FUNC_END(vxL2CTLSet)

	.balign 32
/******************************************************************************
*
* vxL2CTLGet - Get the value of the L2 cache control register
*
* SYNOPSIS
* \ss
* UINT32 sysL2CTLGet
*     (
*     UINT32 *addr
*     )
* \se
*
* RETURNS: This routine returns the value in the L2CTL reg.
*/
FUNC_BEGIN(vxL2CTLGet)
	mbar    0
	isync
	lwz	p1,0x0(p0)
	addi    p0,p1,0x0
	mbar    0
	isync
	blr
FUNC_END(vxL2CTLGet)

/******************************************************************************
* disableBranchPrediction - disables branch prediction 
*
* SYNOPSIS
* \ss
* void disableBranchPrediction
*     (
*     void
*     )
* \se
*
* RETURNS: NONE
*/
FUNC_BEGIN(disableBranchPrediction)
        mfspr p0, 1013
        andi. p1, p0,LO(~_PPC_BUCSR_E)
        isync
        mtspr 1013,p1
        isync
        li    p0, _PPC_BUCSR_FI
        mtspr 1013,p0
        isync
        blr
FUNC_END(disableBranchPrediction)


/****************************************************
 * sysCacheFlush just flushes cache - assume int lock
 * p0 - cache line num
 * p1 - buffer origin
 * p2 - cache align size
 */
	
FUNC_BEGIN(sysCacheFlush)
	/*
	 * p3 contains the count of cache lines to be fetched & flushed.
	 * Convert to a count of pages covered, and fetch a word from
	 * each page to ensure that all addresses involved are in
	 * the TLB so that reloads do not disrupt the flush loop.
	 * A simple shift without round-up is sufficient because
	 * the p3 value is always a multiple of the shift count.
	 */
	srwi	p3, p0, MMU_RPN_SHIFT - CACHE_ALIGN_SHIFT
	mtspr	CTR, p3
        addi    p6,p1,0	
        li      p5,MMU_PAGE_SIZE
	subf    p3,p5,p1
	
	/*
	 * There might be a page boundary between here and the end of
	 * the function, so make sure both pages are in the I-TLB.
	 */
	b	cacheL2DisableLoadItlb
cacheL2DisableLoadDtlb:
	add     p3,p3,p5
	lbzu	p4,0(p3)
	bdnz	cacheL2DisableLoadDtlb
	mtctr   p0         /* Load counter with number of cache lines */
	subf	p1, p2, p1 /* buffer points to text  - cache line size */
l2DisableFlush:
        add	p1, p2, p1		  /* +  cache line size */
	lbzu	p3, 0x0(p1)	       	  /* flush the data cache block */
        bdnz    l2DisableFlush     /* loop till cache ctr is zero */
	sync
	isync
	mtctr   p0         /* Load counter with number of cache lines */
        addi    p1, p6, 0
	subf	p1, p2, p1 /* buffer points to text  - cache line size */
l2DisableClear:
	add	p1, p2, p1  /* point to next cache line */
	dcbf	0,p1			    /* flush newly-loaded line */
	bdnz	l2DisableClear	    /* repeat for all sets and ways */
	sync
	isync	
	blr
cacheL2DisableLoadItlb:
	b	cacheL2DisableLoadDtlb
FUNC_END(sysCacheFlush)

/*****************************************************************************
*
* sysInByte - reads a byte from an io address.
*
* This function reads a byte from a specified io address.
*
* RETURNS: byte from address.

* UCHAR sysInByte
*     (
*     UCHAR *  pAddr		/@ Virtual I/O addr to read from @/
*     )

*/

FUNC_BEGIN(sysInByte)
	eieio			/* Sync I/O operation */
	sync
	lbzx	p0,r0,p0	/* Read byte from I/O space */
	bclr	20,0		/* Return to caller */
FUNC_END(sysInByte)
	
/******************************************************************************
*
* sysOutByte - writes a byte to an io address.
*
* This function writes a byte to a specified io address.
*
* RETURNS: N/A

* VOID sysOutByte
*     (
*     UCHAR *  pAddr,		/@ Virtual I/O addr to write to @/
*     UCHAR    data		/@ data to be written @/
*     )

*/

FUNC_BEGIN(sysOutByte)
	stbx	p1,r0,p0	/* Write a byte to PCI space */
	eieio			/* Sync I/O operation */
	sync
	bclr	20,0		/* Return to caller */
FUNC_END(sysOutByte)
	
/*****************************************************************************
*
* sysInWord - reads a word from an address, swapping the bytes.
*
* This function reads a swapped word from a specified 
* address.
*
* RETURNS:
* Returns swapped 16 bit data from the specified address.

* USHORT sysInWord
*     (
*     ULONG  address,		/@ addr to read from @/
*     )

*/

FUNC_BEGIN(sysInWord)
	eieio			/* Sync I/O operation */
	sync
        lhbrx   p0,r0,p0	/* Read and swap */
        bclr    20,0		/* Return to caller */
FUNC_END(sysInWord)