@ This program expects new data to be programmed to the Flash memory
@ It expects 4 bytes containing the length of the data to be uploaded
@ (low byte, then second byte, then third byte, then highest byte)
@ It will then erase any blocks it needs to, and then overwrite the flash
@ with the uploaded data
	
.arm
.text
.org 0

	.set HwUartControl,		0x000004C0
	.set HwUartData,		0x00000480
	.set HwPDDR,			0x00000003
	.set HwControl,			0x00000100
	.set HwControlUartEnable,	0x00000100
	.set HwControlExCkEn,		0x00040000
	.set HwDRFPR,			0x0200
	.set HwSysFlg1,			0x00000140
	.set HwSysFlg1UBusy1,		1<<11
	.set HwSysFlg1UTxFF1,		1<<23
	.set HwSysFlg1URxFE1,		1<<22
	.set _start,			0
	.set FlashMem1Start,		0x70000000
	.set FlashMem1End,		0x70100000
	.set FlashMem2Start,		0x60000000
	.set FlashMem2End,		0x60100000
	.set HwMemCfg1,			0x0180
	.set HwMemCfg2,			0x01C0


@ Serial transmit macro	
	.macro serialTx reg
	LDR r9,[r12,#HwSysFlg1]
	TST r9,#HwSysFlg1UTxFF1
	BNE .-8

	STRB \reg,[r12,#HwUartData]
	.endm

@ Serial receive macro
	.macro serialRx reg
	LDR r9,[r12,#HwSysFlg1]
	TST r9,#HwSysFlg1URxFE1
	BNE .-8

	LDRB \reg,[r12,#HwUartData]
	.endm


@ Code begins
	
@ First set up a few registers
	LDR r0,=0xBDB82C2C
	STR r0,[r12,#HwMemCfg1]

	LDR r0,=0xFFFD3D3D
	STR r0,[r12,#HwMemCfg2]

	LDR r0,=0x81
	STR r0,[r12,#HwDRFPR]

@ Send ready message "..."
	LDR r0,='.'
	serialTx r0
	serialTx r0
	serialTx r0

@Now wait for 1 word for data length into r8
	LDR r8, =0
	serialRx r0
	ORR r8,r8,r0
	serialRx r0
	ORR r8,r8,r0,LSL #8
	serialRx r0
	ORR r8,r8,r0,LSL #16
	serialRx r0
	ORR r8,r8,r0,LSL #24

	MOV r6,r8
	LDR r7,=0xC0000000
	
Loop1:	SUBS r6,r6,#4
	BEQ Loop1Done

	bl ReadWord

	STR r1,[r7]
	ADD r7,r7,#4
	B Loop1

Loop1Done:
	LDR r0,='O'
	serialTx r0
	LDR r0,='K'
	serialTx r0

	LDR r10, =FlashMem1Start

	LDR r0,=0
	ADD r0,r0,r10
	BL FlashErase
	
	LDR r0,=0x00001000
	CMP r8,r0
	ADD r0,r0,r10
	BL FlashErase
	
	LDR r0,=0x00002000
	CMP r8,r0
	ADD r0,r0,r10
	BL FlashErase
	
	LDR r0,=0x00003000
	CMP r8,r0
	ADD r0,r0,r10
	BL FlashErase
	
	LDR r0,=0x00004000
	CMP r8,r0
	ADD r0,r0,r10
	BL FlashErase
	
	LDR r0,=0x00005000
	CMP r8,r0
	ADD r0,r0,r10
	BL FlashErase
	
	LDR r0,=0x00006000
	CMP r8,r0
	ADD r0,r0,r10
	BL FlashErase
	
	LDR r0,=0x00007000
	CMP r8,r0
	ADD r0,r0,r10
	BL FlashErase
	
	LDR r0,=0x00008000
	CMP r8,r0
	ADD r0,r0,r10
	BL FlashErase
	
	LDR r0,=0x00010000
	CMP r8,r0
	ADD r0,r0,r10
	BL FlashErase
	
	LDR r0,=0x00018000
	CMP r8,r0
	ADD r0,r0,r10
	BL FlashErase
	
	LDR r0,=0x00020000
	CMP r8,r0
	ADD r0,r0,r10
	BL FlashErase
	
	LDR r0,=0x00028000
	CMP r8,r0
	ADD r0,r0,r10
	BL FlashErase
		
	LDR r0,=0x00030000
	CMP r8,r0
	ADD r0,r0,r10
	BL FlashErase
	
	LDR r0,=0x00038000
	CMP r8,r0
	ADD r0,r0,r10
	BL FlashErase
		
	LDR r0,=0x00040000
	CMP r8,r0
	ADD r0,r0,r10
	BL FlashErase
	
	LDR r0,=0x00048000
	CMP r8,r0
	ADD r0,r0,r10
	BL FlashErase

	LDR r0,=0x00050000
	CMP r8,r0
	ADD r0,r0,r10
	BL FlashErase
	
	LDR r0,=0x00058000
	CMP r8,r0
	ADD r0,r0,r10
	BL FlashErase
		
	LDR r0,=0x00060000
	CMP r8,r0
	ADD r0,r0,r10
	BL FlashErase
	
	LDR r0,=0x00068000
	CMP r8,r0
	ADD r0,r0,r10
	BL FlashErase
		
	LDR r0,=0x00070000
	CMP r8,r0
	ADD r0,r0,r10
	BL FlashErase
	
	LDR r0,=0x00078000
	CMP r8,r0
	ADD r0,r0,r10
	BL FlashErase
			
	LDR r7,=0xC0000000
	MOV r6,r8
	
Loop2:	SUBS r6,r6,#4
	BEQ Done

	MOV r0,r10
	LDR r1,[r7]
		
	bl FlashWrite

	MOV r1,r1,LSR #16
	ADD r0,r0,#2
	bl FlashWrite
	
	ADD r7,r7,#4
	ADD r10,r10,#4
	
	LDR r0,=FlashMem1End
	TEQ r10,r0
	BEQ SetMem2
	LDR r0,=FlashMem2End
	TEQ r10,r0
	BEQ Done
	B Loop2
	
SetMem2:
	LDR r10,=FlashMem2Start
	B Loop2

Done:	LDR r0,='D'
	serialTx r0
	LDR r0,='o'
	serialTx r0
	LDR r0,='n'
	serialTx r0
	LDR r0,='e'
	serialTx r0
LDone:	B LDone

@ Writes half-word in r1 to address in r0
@ overwrites r2, r3
FlashWrite:
	LDR r2, =0x50
	LDR r3, =0x71000000
	STRH r2,[r3]
	
	LDR r2, =0x40
	STRH r2,[r3] @ Send flash program command
	STRH r1,[r0] @ Send flash data
	
FLoop:	LDRH r2,[r3] @ Read status byte
	TST r2, #0x80
	BEQ FLoop

	LDR r2,=0xFF
	STRH r2,[r3] @ Send flash read array command
	MOV pc,lr

@ Erases block with address in r0
@ overwrites r2, r3
FlashErase:
	LDR r2,='E'
	serialTx r2
	
	LDR r2, =0x20
	STRH r2,[r0]
	
	LDR r2, =0xD0
	STRH r2,[r0]
	
FELoop:	LDRH r2,[r0] @ Read status byte
	TST r2, #0x80
	BEQ FELoop

	LDR r2,=0xFF
	STRH r2,[r0] @ Send flash read array command
	MOV pc,lr

@ overwrites r0, returns read word in r1
ReadWord:
	LDR r1, =0
	serialRx r0
	ORR r1,r1,r0
	serialRx r0
	ORR r1,r1,r0,LSL #8
	serialRx r0
	ORR r1,r1,r0,LSL #16
	serialRx r0
	ORR r1,r1,r0,LSL #24
	MOV pc,lr
	
	.ltorg
	
.org 0x800, 0