;++
; Copyright (c) David Vescovi.  All rights reserved.
; Part of Project DrumStix
; Windows Embedded Developers Interest Group (WE-DIG) community project.
; http://www.we-dig.org
; Copyright (c) Microsoft Corporation.  All rights reserved.
;
;
; Abstract:
;
;	Board-level initialization for OEM-specific code.
;	Bootloader flavor.
;
;--

	OPT   2					; disable listing
	INCLUDE kxarm.h
	INCLUDE pxa255.inc
	INCLUDE gumstix.inc
	INCLUDE image_cfg.inc
	OPT   1					; reenable listing

	IMPORT	main
	IMPORT	OALVAtoPA
	IMPORT	OALPAtoVA

	TEXTAREA

;------------------------------------------------------------------------
	; Included within the text section in order that a relative offset can be
	; computed in the code below.
	;
	OPT   2					; disable listing
	INCLUDE	oemaddrtab_cfg.inc
	OPT   1					; reenable listing

;-------------------------------------------------------------------------------
;
; OALStartUp: OEM bootloader startup code.  This routine will:
;
; * Copy the image to RAM if it's not already running there.
;
; * Set up the MMU and Dcache for the bootloader.
;
; * Initialize the first-level page table based up the contents
;   of the MemoryMap array and enable the MMU and caches.
;
; Inputs: None.
; 
; On return: N/A.
;
; Register used:
;
;-------------------------------------------------------------------------------
;
	ALIGN

	LEAF_ENTRY OALStartUp

	; Copy the bootloader image from flash to RAM.  The image is configured
	; to run in RAM, but is stored in flash.  Absolute address references
	; should be avoided until the image has been relocated and the MMU enabled.
	;
	; NOTE: The destination (RAM) address must match the address in the
	; bootloader's .bib file.  The latter dictates the code fix-up addresses.
	;
	ldr		r0, =IMAGE_BOOT_BLDRIMAGE_FLASH_PA_END
	cmp		pc, r0				; if we are already in RAM ...
	bhi		CODEINRAM

	ldr		r8, =IMAGE_BOOT_BLDRIMAGE_FLASH_PA_START; Bootloader is stored at the base of the boot flash.
													; Bootloader is fixed up to run in SDRAM (this value
													; must match the NK start address in the .bib file).
	ldr		r1, =IMAGE_BOOT_BLDRIMAGE_RAM_PA_START
	ldr		r2, =(IMAGE_BOOT_BLDRIMAGE_RAM_SIZE / 16); Bootloader image length (this must be <= the NK
													; length in the .bib file).  We are block-copying 
													; 16-bytes per iteration.
	                                                
	; Do 4x32-bit block copies from flash->RAM (corrupts r4-r7).
	;
10	ldmia	r8!, {r4-r7}		; Loads from flash (post increment).
	stmia	r1!, {r4-r7}		; Stores to RAM (post increment).
	subs	r2, r2, #1
	bne		%B10

	; Verify that the copy succeeded by comparing the flash and RAM contents.
	;
	ldr		r0, =IMAGE_BOOT_BLDRIMAGE_RAM_SIZE
	ldr		r1, =IMAGE_BOOT_BLDRIMAGE_RAM_PA_START
	ldr		r2, =IMAGE_BOOT_BLDRIMAGE_FLASH_PA_START

VERIFY_LOOP
	ldr		r3, [r1], #4		; Read longword from DRAM.
	ldr		r4, [r2], #4		; Read longword from flash.
	cmp		r3, r4				; Compare.
	bne		VERIFY_FAILURE		; Not the same? Fail.
	subs	r0, r0, #4			;
	bne		VERIFY_LOOP			; Continue?
	b		VERIFY_DONE			; Done (success).

VERIFY_FAILURE
    b		VERIFY_FAILURE		; Spin forever.

VERIFY_DONE
	; Now that we've copied ourselves to RAM, jump to the RAM image.  Use the "CodeInRAM" label
	; to determine the RAM-based code address to which we should jump.
	;
	add		r2, pc, #CODEINRAM-(.+8)				; Calculate the relative offset to the 'CodeInRAM' label.
	ldr		r1, =IMAGE_BOOT_BLDRIMAGE_RAM_PA_START	; Get the RAM address to which we copied ourself.
	add		r1, r1, r2								; Calculate the RAM address of the 'CodeInRAM' label.
	mov		pc, r1
	nop
	nop
	nop

CODEINRAM

	; We're now running out of RAM.
	; 
	ldr		r1, =PXA255_BASE_REG_PA_MEMC			; now its safe to re-write FLASH timing
	ldr		r2, =MSC0_INRAM_value					; fast FLASH access while in RAM
	str		r2, [r1, #MSC0_OFFSET]
	ldr		r2, [r1, #MSC0_OFFSET]					; read it back as required

	; Now that we're running out of RAM, construct the first-level Section descriptors
	; to create 1MB mapped regions from the addresses defined in the OEMAddressTable.
	; This will allow us to enable the MMU and use a virtual address space that matches
	; the mapping used by the OS image.
	;
	; We'll create two different mappings from the addresses specified:
	;     [8000 0000 --> 9FFF FFFF] = Cacheable, Bufferable
	;     [A000 0000 --> BFFF FFFF] = NonCacheable, nonBufferable
	;
BUILDTTB

	add		r11, pc, #g_oalAddressTable - (. + 8)	; Pointer to OEMAddressTable.

	; Set the TTB.
	;
	ldr		r9, =BASE_PA_SDRAM				; Physical address of the first-level table (base of SDRAM).
	ldr		r0, =0xFFFFC000
	and		r9, r9, r0						; Mask off TTB[31:0] (must be 0's).
	mcr		p15, 0, r9, c2, c0, 0			; Set the TTB.

	; ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
	; ~~~~~~~~~~ MAP CACHED and BUFFERED SECTION DESCRIPTORS ~~~~~~~~~~~~~~~~~~
	; ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
	mov		r0, #0x0E						; Section (1MB) descriptor; (C=B=1: write-back, read-allocate).
	orr		r0, r0, #0x400					; Set AP.
20  mov		r1, r11							; Pointer to OEMAddressTable.

	; Start Crunching through the OEMAddressTable[]:
	;
	; r2 temporarily holds OEMAddressTable[VA]
	; r3 temporarily holds OEMAddressTable[PHY]
	; r4 temporarily holds OEMAddressTable[#MB]
	;
25	ldr		r2, [r1], #4					; Virtual (cached) address to map physical address to.
	ldr		r3, [r1], #4					; Physical address to map from.
	ldr		r4, [r1], #4					; Number of MB to map.

	cmp		r4, #0							; End of table?
	beq		%F29

	; r2 holds the descriptor address (virtual address)
	; r0 holds the actual section descriptor
	;

	; Create descriptor address.
	;
	ldr     r6, =0xFFF00000
	and     r2, r2, r6				; Only VA[31:20] are valid.
	orr     r2, r9, r2, LSR #18		; Build the descriptor address:  r2 = (TTB[31:14} | VA[31:20] >> 18)

	; Create the descriptor.
	;
	ldr     r6, =0xFFF00000
	and     r3, r3, r6				; Only PA[31:20] are valid for the descriptor and the rest will be static.
	orr     r0, r3, r0				; Build the descriptor: r0 = (PA[31:20] | the rest of the descriptor)

	; Store the descriptor at the proper (physical) address
	;
28	str		r0, [r2], #4
	add		r0, r0, #0x00100000		; Section descriptor for the next 1MB mapping (just add 1MB).
	sub		r4, r4, #1				; Decrement number of MB left.
	cmp		r4, #0					; Done?
	bne		%B28					; No - map next MB.

	bic		r0, r0, #0xF0000000		; Clear section base address field.
	bic		r0, r0, #0x0FF00000		; Clear section base address field.
	b		%B25					; Get and process the next OEMAddressTable element.

	; ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
	; ~~~~~~~~~~ MAP UNCACHED and UNBUFFERED SECTION DESCRIPTORS ~~~~~~~~~~~~~~
	; ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

29	tst		r0, #8					; Test for 'C' bit set (means we just used 
									; above loop structure to map cached and buffered space).
	bic		r0, r0, #0x0C			; Clear cached and buffered bits in the descriptor (clear C&B bits).
	add		r9, r9, #0x0800			; Pointer to the first PTE for "unmapped uncached space" (0x2000 0000 + V_U_Adx).
	bne		%B20					; Repeat the descriptor setup for uncached space (map C=B=0 space).

ACTIVATEMMU
	; The 1st Level Section Descriptors are setup. Initialize the MMU and turn it on.
	;
	nop
	mov		r1, #1
	mcr		p15, 0, r1, c3, c0, 0	; Set up access to domain 0.
	mcr		p15, 0, r0, c8, c7, 0	; Flush the instruction and data TLBs.
	mcr		p15, 0, r1, c7, c10, 4	; Drain the write and fill buffers.

	mov		r1, #0x78				; Bits [6:3] must be written as 1's.
	orr		r1, r1, #0x1			; Enable MMU.
	orr		r1, r1, #0x1000			; Enable IC.
	orr		r1, r1, #0x0800			; Enable BTB.
	orr		r1, r1, #0x4			; Enable DC.
	ldr		r2, =VirtualStart		; Get virtual address of 'VirtualStart' label.
	cmp		r2, #0					; Make sure no stall on "mov pc,r2" below.

	; Enable the MMU.
	;
	mcr     p15, 0, r1, c1, c0, 0	; MMU ON:  All memory accesses are now virtual.

	; Jump to the virtual address of the 'VirtualStart' label.
	;
	mov     pc, r2
	nop
	nop
	nop

	; *************************************************************************
	; *************************************************************************
	; The MMU and caches are now enabled and we're running in a virtual
	; address space.
	;

	ALIGN
    
VirtualStart

	;  Set up a supervisor mode stack.
	;
	; NOTE: These values must match the OEMAddressTable and .bib file entries for
	; the bootloader.
	;
	ldr		sp, =IMAGE_BOOT_STACK_RAM_UA_START

	; Jump to the C entrypoint.
	;
;test
;	b	test
	nop
	nop
;
;	
	nop
	nop
	bl		main				; Jump to main.c::main(), never to return...
	nop
	nop
	nop
    
STALL2
	b		STALL2 


;-------------------------------------------------------------------------------

	LTORG

;-------------------------------------------------------------------------------
;-------------------------------------------------------------------------------
;
; void Launch(UINT32 pFunc): This function launches the program at pFunc (pFunc
;                            is a physical address).  The MMU is disabled just
;                            before jumping to specified address.
;
; Inputs: pFunc (r0) - Physical address of program to Launch.
; 
; On return: None - the launched program never returns.
;
; Register used:
;
;-------------------------------------------------------------------------------
;
	ALIGN
	LEAF_ENTRY Launch

	; r3 now contains the physical launch address.
	;
	mov		r3, r0

	; Compute the physical address of the PhysicalStart tag.  We'll jump to this
	; address once we've turned the MMU and caches off.
	;
	stmdb	sp!, {r3}
	ldr		r0, =PhysicalStart
	bl		OALVAtoPA
	nop
	ldmia	sp!, {r3}

	; r0 now contains the physical address of 'PhysicalStart'.
	; r3 now contains the physical launch address.

	; Next, we disable the MMU, and I&D caches.
	;
	mov		r1, #0x0078
	mcr		p15, 0, r1, c1, c0, 0


	; Jump to 'PhysicalStart'.
	;
	mov		pc, r0
	nop
	nop
	nop
	nop

PhysicalStart

	; Flush the I&D TLBs.
	;
	mcr		p15, 0, r2, c8, c7, 0		; Flush the I&D TLBs

	; Jump to the physical launch address.  This should never return...
	;    
	mov		pc, r3
	nop
	nop
	nop
	nop
	nop
	nop
       
	END