#
# file: cpusetup.s
#
# PowerPC Startup - initialize BATs, PCI, cache
# for UCOS-II
#
# Author: Ernie Price
# eprice@bitwise.net
#
#########################################################################
        .text
        .align  2

        .global bsp_Start

        .extern BAT_Enable
        .extern InitSRs
        .extern ClearBATs
        .extern CpDstBg, CpDstEn, CpSrcBg       # from the linker command file
        .extern Slim, Sbase                     # from the linker command file
        .extern msrvalue
        .extern main

        .macro  LA reg,symbol
        lis     reg,%hiadj(symbol)
        addi    reg,reg,%lo(symbol)
        .endm
                
        .macro  LWI reg,symbol
        lis     reg,%hiadj(symbol)
        addi    reg,reg,%lo(symbol)
        .endm

TEST_IF_IN_ROM macro
        bl      . + 4
        mflr    r3                      # r3: Current IP
        rlwinm. r3,r3,1,31,31           # Check most-sig byte
        endm

.if 0
delay   macro
        lis     r3, 1
        mtctr   r3
dly\@:
        addi    r3,r3, 1
        bdnz    dly\@
        endm
.endif  

########################################################################
#
#       Enter here from cpuvecs.s
#
bsp_Start:

        TEST_IF_IN_ROM
        bne     doconfig

        xor     r3,r3,r3
        mtmsr   r3                      # turn off high vector bit
        b       skipper

cfgval  macro   ofs, val
        .byte ofs
        .uword val
        endm    

doconfig:

        bl      goload

        .uword  0                       # update instructions increment first
        tblsz   .set .  
        cfgval  0x04,0xffff0006         # PCI_CMD
        onentry .set . - tblsz
#       cfgval  0x0c,0x00004008         # PCI BIST, HDR, LAT, & CLS
        cfgval  0x44,0x80000000         # PCIARB
#       cfgval  0x70,0x00200000         # PWRMNG1
        cfgval  0x78,0xfce00000         # EUMBBAR
        cfgval  0x80,0xffffff00         # SMEMADD1 
        cfgval  0x84,0xffffffff         # SMEMADD2
        cfgval  0x88,0x03030300         # EXTSMEM1
        cfgval  0x8c,0x03030303         # EXTSMEM2 
        cfgval  0x90,0xffffff3f         # EMEMADD1
        cfgval  0x94,0xffffffff         # EMEMADD2
        cfgval  0x98,0x03030300         # EXTEMEM1
        cfgval  0x9c,0x03030303         # EXTEMEM2
        cfgval  0xa0,0x00000001         # MEMPMODE
        cfgval  0xa8,0xff341cc8         # PCIR1
        cfgval  0xac,0x00000000         # PCIR2
        cfgval  0xc0,0x00000001
        cfgval  0xc4,0x00000000
        cfgval  0xc8,0x00000000         # BERRADDR
        cfgval  0xe0,0x00000060         # AMBOR reset DLL
        cfgval  0xe0,0x00000040         # AMBOR release DLL
        cfgval  0xf0,0xfff00000         # MEMCTRL1
        cfgval  0xf4,0x00000312         # MEMCTRL2
#       cfgval  0xf4,0x0000020c         # MEMCTRL2
        cfgval  0xf8,0x03300000         # MEMCTRL3
        cfgval  0xfc,0x37302230         # MEMCTRL4
#       cfgval  0xfc,0x25302220         # MEMCTRL4
        cfgval  0xf0,0xfff80000         # MEMCTRL1 - MEMGO

        tblsz   .set . - tblsz

        .align  2
goload:

        mflr    r6                      # address of above table
        lis     r4, 0xFEC0              # PCI config address
        lis     r5, 0xFEE0              # PCI config data
        li      r3, tblsz / onentry     # # of table entries
        mtctr   r3                      # put in CTR
loadlp:
        lbzu    r3, 4(r6)               # fetch offset in PCI cfg space
        oris    r3, r3, 0x8000          # or in the magic
        stwbrx  r3, 0, r4               # store in PCI address
        sync
        lwzu    r3, 1(r6)               # load the data
        stwbrx  r3, 0, r5               # store in PCI data 
        sync
        bdnz    loadlp                  # while not done

skipper:

#
#       The segment registers and BATs should be initialized even if 
#       data translation is not activated.
#
        bl      InitSRs                 # setup the segment registers
        bl      ClearBATs               # clear the BAT registers

        bl      BAT_Enable              # set one to one BAT mapping

        bl      IcachEnable             # Turn on instruction caching
        bl      DcachEnable             # Turn on data        caching

        TEST_IF_IN_ROM
        beq     ClearBSS                # Skip the copying if not in ROM

# in ROM - clear all of RAM

        lis     r3, 0x200               # clear 32 Mb
        rlwinm  r3,r3,30,2,31           # shift right 2 bits
        mtctr   r3
        lis     r3, 0                   # r3 is zero
        addi    r4,r3,-4                # r4 is zero - 4
clx:
        stwu    r3,4(r4)
        bdnz    clx
#
#       Now move the C initialized data from ROM to RAM
#

        LA      r3, CpSrcBg             # Address of Data section in FLASH
        LA      r4, CpDstBg             # Address of Data Section in RAM
        LA      r5, CpDstEn             # End of Data Section in RAM
        subf    r5,r4,r5                # r5: # bytes to copy
        bl      LclMemcpy               # Copy Data section to RAM

ClearBSS:

#
#       Now clear the C uninitialized data area
#

        LA      r3, Sbase               # r3: address
        LWI     r5, Ssize               # r5: size to be cleared
        li      r4, 0                   # fill value 
        bl      LclMemset               # CLear Bss area

        mfmsr   r3
        ori     r3,r3,0x1000            # enable machine check exceptions
        mtmsr   r3                      # 

#
#       Now setup the stack and the Diab (also EABI) small data areas
#
        lis     r1, 0x10                # initial stack at 1 mB - only temp til OS starts

        addis   r13,r0,_SDA_BASE_@ha    # Initialize r13 to sdata base
        addi    r13,r13,_SDA_BASE_@l    # (provided by linker).
        addis   r2,r0,_SDA2_BASE_@ha    # Initialize r2 to sdata2 base
        addi    r2,r2,_SDA2_BASE_@l     # (provided by linker).

        mfmsr   r3
        stw     r3, msrvalue@sdax(r13)  # partial MSR value for task stack initialization

#
# Insert other initialization code here.
#

        bl      main
        b       .                       # in case main() returns

##############################################################

#       void LclMemcpy(void *src, void *dst, int lth);

LclMemcpy:
        cmpwi   r5, 0                   # if length == 0 : All Done
        beq     LclMemcpyDone

        addi    r3, r3, -1
        addi    r4, r4, -1
        mtctr   r5

memcpyLp:
        lbzu    r6, 1(r3)               # Load  a byte with increment
        stbu    r6, 1(r4)               # Store a byte with increment
        bdnz    memcpyLp                # while len != 0 Loop
LclMemcpyDone:
#       sync
        blr
##############################################################

#       void LclMemset(void *addr, char val, int lth);

LclMemset:                      
        cmpwi   r5, 0                   # if length == 0 : All Done
        beq     LclMemsetDone
        addi    r3,r3, -1
        mtctr   r5
memsetLp:
        stbu    r4, 1(r3)               # Store a byte with increment
        bdnz    memsetLp                # while len != 0 Loop
LclMemsetDone:
#       sync
        blr

        .end