sysautils.s

vxworks 834x的BSP

/* sysAUtils.s - PPC834x system utility assembly routines */

/*
 * Copyright (c) 2005 Wind River Systems, Inc.
 *
 * The right to copy, distribute, modify or otherwise make use
 * of this software may be licensed only pursuant to the terms
 * of an applicable Wind River license agreement.
 */

/*
modification history
--------------------
01a,09aug05,j_b  adapted from ads826x (rev 01b)
*/

/*
DESCRIPTION
This module contains system utility routines written in assembly
language.

NOTE:  These functions are for use before rom images are uncompressed.
       They are not available for general system use.
*/

#define _ASMLANGUAGE
#include <vxWorks.h>
#include <asm.h>
#include <cacheLib.h>
#include "config.h"
#include <regs.h>


    FUNC_EXPORT(sysClearSPRGs)
    FUNC_EXPORT(sysClearMSR)
    FUNC_EXPORT(sysClearSRs)
    FUNC_EXPORT(sysClearIBATs)
    FUNC_EXPORT(sysClearDBATs)
    FUNC_EXPORT(sysClearFPRegs)
    FUNC_EXPORT(sysMulDiv64)
    FUNC_EXPORT(sysAbs)
    FUNC_EXPORT(sysDecGet)
    FUNC_EXPORT(sysDecSet)

/*****************************************************************************
*
* sysClearSPRGs - Clears all 4 SPRGx registers
*
* This function zero's r0 (available working register)
* and clears out all 4 SPRG registers.
*
* RETURNS:
*       NONE

* void  sysClearSPGRs
*       (
*       void
*       )
*/

FUNC_BEGIN(sysClearSPRGs)
    li      r3, 0
    sync                /* not really needed for SPRG, just for consistency */
    mtspr   SPRG0, r3   /* clear SPRG0 */
    mtspr   SPRG1, r3   /* clear SPRG1 */
    mtspr   SPRG2, r3   /* clear SPRG2 */
    mtspr   SPRG3, r3   /* clear SPRG3 */
    isync               /* need context sync instruction after mtspr */
    blr                 /* return to caller */
FUNC_END(sysClearSPRGs)

/*****************************************************************************
*
* sysClearMSR - Clears the MSR register
*
* This function zero's r3 (available working register)
* and writes r3 to the machine state register (MSR).
*
* RETURNS:
*       NONE

* void  sysClearMSR
*       (
*       void
*       )
*/

FUNC_BEGIN(sysClearMSR)
    li      r3, 0   /* clear r3 except for RI */
    sync            /* need synchronizing instruction before mtspr */
    mtmsr   r3      /* msr is now zeroed out, all ints off */
    isync           /* need context synchronizing instruction after mtmsr */
    blr             /* return to caller */
FUNC_END(sysClearMSR)


/*****************************************************************************
*
* sysClearSRs - Clears the 16 segment registers
*
* This function zero's r3 (available working register)
* and writes r3 to the 16 Segment Registers (SR0 - SR15).
*
* RETURNS:
*       NONE

* void  sysClearSRs
*       (
*       void
*       )
*/

FUNC_BEGIN(sysClearSRs)
    li      r3, 0
    isync
    mtsr    0, r3
    isync
    mtsr    1, r3
    isync
    mtsr    2, r3
    isync
    mtsr    3, r3
    isync
    mtsr    4, r3
    isync
    mtsr    5, r3
    isync
    mtsr    6, r3
    isync
    mtsr    7, r3
    isync
    mtsr    8, r3
    isync
    mtsr    9, r3
    isync
    mtsr    10, r3
    isync
    mtsr    11, r3
    isync
    mtsr    12, r3
    isync
    mtsr    13, r3
    isync
    mtsr    14, r3
    isync
    mtsr    15, r3
    isync
    blr         /* return to caller */
FUNC_END(sysClearSRs)


/*****************************************************************************
*
* sysClearIBATs - Clears the Instruction BAT registers
*
* This function zero's r3 (available working register)
* and writes r3 Instruction BAT registers: IBAT0U/L - IBAT3U/L
*
* RETURNS:
*       NONE

* void  sysClearIBATs
*       (
*       void
*       )
*/

FUNC_BEGIN(sysClearIBATs)
    li      r3,0            /* clear r3 */
    isync
    mtspr   IBAT0U,r3       /* SPR 528 (IBAT0U) */
    isync
    mtspr   IBAT0L,r3       /* SPR 529 (IBAT0L) */
    isync
    mtspr   IBAT1U,r3       /* SPR 530 (IBAT1U) */
    isync
    mtspr   IBAT1L,r3       /* SPR 531 (IBAT1L) */
    isync
    mtspr   IBAT2U,r3       /* SPR 532 (IBAT2U) */
    isync
    mtspr   IBAT2L,r3       /* SPR 533 (IBAT2L) */
    isync
    mtspr   IBAT3U,r3       /* SPR 534 (IBAT3U) */
    isync
    mtspr   IBAT3L,r3       /* SPR 535 (IBAT3L) */
    isync
    blr                     /* return to caller */
FUNC_END(sysClearIBATs)

/*****************************************************************************
*
* sysClearDBATs - Clears the Data BAT registers
*
* This function zero's r3 (available working register)
* and writes r3 Data BAT registers: DBAT0U/L - DBAT3U/L
*
* RETURNS:
*       NONE

* void  sysClearDBATs
*       (
*       void
*       )
*/

FUNC_BEGIN(sysClearDBATs)
    li      r3,0            /* clear out r3 */
    mtspr   DBAT0U,r3       /* SPR 536 (DBAT0U) */
    isync
    mtspr   DBAT0L,r3       /* SPR 537 (DBAT0L) */
    isync
    mtspr   DBAT1U,r3       /* SPR 538 (DBAT1U) */
    isync
    mtspr   DBAT1L,r3       /* SPR 539 (DBAT1L) */
    isync
    mtspr   DBAT2U,r3       /* SPR 540 (DBAT2U) */
    isync
    mtspr   DBAT2L,r3       /* SPR 541 (DBAT2L) */
    isync
    mtspr   DBAT3U,r3       /* SPR 542 (DBAT3U) */
    isync
    mtspr   DBAT3L,r3       /* SPR 543 (DBAT3L) */
    isync
    blr                     /* return to caller */
FUNC_END(sysClearDBATs)

/*******************************************************************************
*
* romInvalidateTLBs - invalidate all TLB entries
*
* This routine will invalidate instruction and data TLB entries.
*
* RETURNS: None
*
*/

romInvalidateTLBs:
    isync
    /* invalidate entries within both 64-entry TLBs */
    li      r3,128
    mtctr   r3          /* CTR = 128  */
    li      r3,0        /* r5 = 0    */
    isync               /* context sync req'd before tlbie */
tlbloop:
    tlbie   r3
    sync                 /* sync instr req'd after tlbie      */
    addi    r3,r3,0x1000 /* increment bits 15-19 */
    bdnz    tlbloop      /* decrement CTR, branch if CTR != 0 */
    isync
    blr

/*******************************************************************************
*
* romClearFPRegs
*
* This routine will initialize the FPU's registers.
*
* RETURNS: None
*
* void  sysClearFPRegs
*       (
*       void
*       )
*/

FUNC_BEGIN(sysClearFPRegs)

    /* save current value of MSR */
    mfmsr   r3

    /* Turn on FP */
    li      r4, _PPC_MSR_FP     /* turn on FP so we can init it */
    sync
    mtmsr   r4
    isync

    /* Init the floating point control/status register */
    mtfsfi  7, 0
    mtfsfi  6, 0
    mtfsfi  5, 0
    mtfsfi  4, 0
    mtfsfi  3, 0
    mtfsfi  2, 0
    mtfsfi  1, 0
    mtfsfi  0, 0
    isync

    /*
     * Initialize the floating point data registers to a known state.
     * First save the link register.  By doing a bl to ifpdrValue,
     * the link register has the address of the IEEE value for 1.0
     * The lr can now be moved to a register for use as a pointer for
     * the IEEE value of 1.0
     */

    mflr    r4      /* save link register so we can use it for */
                    /* initializing the fp registers with a good number */

    bl      ifpdrValue  /* link register now has pointer to IEEE value */
    .long   0x3f800000  /* IEEE single-precision value of 1.0 */

ifpdrValue:
    mflr    r5          /* move current lr to r3 for use as pointer */
    mtlr    r4          /* restore lr to previous value */
    lfs     f0, 0(r5)   /* load all 32 FP regs with 1.0 */
    lfs     f1, 0(r5)
    lfs     f2, 0(r5)
    lfs     f3, 0(r5)
    lfs     f4, 0(r5)
    lfs     f5, 0(r5)
    lfs     f6, 0(r5)
    lfs     f7, 0(r5)
    lfs     f8, 0(r5)
    lfs     f9, 0(r5)
    lfs     f10, 0(r5)
    lfs     f11, 0(r5)
    lfs     f12, 0(r5)
    lfs     f13, 0(r5)
    lfs     f14, 0(r5)
    lfs     f15, 0(r5)
    lfs     f16, 0(r5)
    lfs     f17, 0(r5)
    lfs     f18, 0(r5)
    lfs     f19, 0(r5)
    lfs     f20, 0(r5)
    lfs     f21, 0(r5)
    lfs     f22, 0(r5)
    lfs     f23, 0(r5)
    lfs     f24, 0(r5)
    lfs     f25, 0(r5)
    lfs     f26, 0(r5)
    lfs     f27, 0(r5)
    lfs     f28, 0(r5)
    lfs     f29, 0(r5)
    lfs     f30, 0(r5)
    lfs     f31, 0(r5)
    sync

    /*  Restore MPU/MSR to a known state  */

    mtmsr  r3   /* restore saved value */
    isync
    blr         /* Return to caller */
FUNC_END(sysClearFPRegs)


/***************************************************************************
*
* sysMulDiv64 - multiply two 32-bit values to 64-bit,
* then divide by a 32-bit value
*
* Two 32-bit values are multiplied into a 64-bit intermediate value,
* then divided by a third 32-bit value to yield a 32-bit result.
*
* Author: Daniel Goehring
*
* RETURNS: UINT32
*   r3 = (r3 * r4) / r5
*

* UINT32 sysMulDiv64
*   (
*   UINT32  numerator0, /@ numerator 0 @/
*   UINT32  numerator1, /@ numerator 1 @/
*   UINT32  denominator /@ denominator @/
*   );

*/
FUNC_BEGIN(sysMulDiv64)
    mullw   r6, r3, r4      /* R6 = prod[32:63]  low word */
    mulhw   r7, r3, r4      /* R7 = prod[00:31]  high word */

    mfspr   r8, CTR
    addi    r3, r0, 0x20        /* Loop 32 times. */
    mtspr   CTR, r3
    xor     r3, r3, r3          /* R3 = result of the divide */
    xor     r4, r4, r4          /* R4 = 0 */

dLoop:
    rlwinm  r3, r3, 0x1, 0, 30  /* Shift left 1 bit */
    subf    r7, r4, r7
    rlwinm  r7, r7, 0x1, 0, 30  /* Shift left 1 bit */
    rlwinm  r4, r6, 0x1, 31, 31 /* Grab the lower bit  */
    rlwinm  r6, r6, 0x1, 0, 30  /* Shift left 1 bit */
    or      r7, r7, r4      /* Or the two together */
    divw    r4, r7, r5
    add     r3, r3, r4
    mullw   r4, r4, r5
    bdnz    dLoop

    mtspr   CTR, r8
    blr
FUNC_END(sysMulDiv64)

/*******************************************************************************
*
* sysAbs
*
* This routine will get the absolute value of a signed 32 bit number.
*
* RETURNS: | number |
*
* uint32_t  sysAbs
*       (
*       int32_t
*       )
*/

FUNC_BEGIN(sysAbs)
        srawi   r0, r3, 0x1f
        xor     r3, r0, r3
        subf    r3, r0, r3
        blr
FUNC_END(sysAbs)

/*******************************************************************************
*
* sysDecGet
*
* This routine will get the current value of the Decrementer.
*
* RETURNS: None
*
* uint32_t  sysDecGet
*       (
*       void
*       )
*/

FUNC_BEGIN(sysDecGet)
        mfdec   p0
        blr
FUNC_END(sysDecGet)

/*******************************************************************************
*
* sysDecSet
*
* This routine will set the value of the Decrementer to the passed parameter.
*
* RETURNS: None
*
* uint32_t  sysDecGet
*       (
*       uint32_t
*       )
*/

FUNC_BEGIN(sysDecSet)
        mtdec   p0
        blr
FUNC_END(sysDecSet)