preserve atags for stage2

1 files changed, 103 insertions, 2 deletions

diff --git a/kernel_stage1.S b/kernel_stage1.S
index 1e0f614..dd3e6fd 100644
--- a/kernel_stage1.S
+++ b/kernel_stage1.S
@@ -20,8 +20,105 @@ _boot:
 	// occur when the main core overwrites this part of memory
 	wfe
 
+	// we'll use the size of stage1 to determine where we have free
+	// space after it. We'll then copy our atags/fdt there, so
+	// it doesn't get overwritten by stage2 we deploy at 0x0
+atags_magic:
+	.word 0x54410001
+	
 proceed:
-	// copy stage2 of the kernel to address 0x0
+	// load the second word of structure passed to us through r2;
+	// if it's atags, it's second word should be the magic number
+	// Btw, location of ATAGS is always 0x100.
+	ldr r3, [r2, #4]
+	adr r4, atags_magic
+	ldr r4, [r4]
+
+	// compare second word of assumed atags with magic number
+	// to see, if it's really atags and not sth else (i.e. fdt)
+	cmp r3, r4
+
+	// normally at start r0 contains value 0;
+	// value 3 in r0 would tell stage2 code, we found no atags :(
+	movne r0, #3
+	bne stage2_blob_copying
+
+	// if atags was found, copying of it takes place here
+	
+	// the following loop finds, where atags ends
+	// r3 shall point to currently looked-at tag
+	mov r3, r2
+
+find_end_of_atags_loop:
+	// load first word of tag header to r4 (it contains tag size)
+	ldr r4, [r3]
+	// make r3 point at the next tag (by adding 4*tag_size to it)
+	add r3, r4, lsl #2
+
+	// load second word of tag header to r5 (it contains tag type)
+	ldr r5, [r3, #4]
+
+	// if tag value is 0, it is the last tag
+	cmp r5, #0
+	bne find_end_of_atags_loop
+
+	add r3, #8 // make r3 point at the end of last tag
+	sub r3, r2 // get atags size in r3
+	
+	// at this pont r2 and r3 point at start and size of atags,
+	// respectively; now we'll compute, where we're going to have
+	// free space to put atags in; we want to put atags either
+	// right after our blob or, if if it doesn't fit between
+	// blob end and the address stage1 is loaded at, after stage1
+
+	// get blob size to r5
+	adr r5, blob_size
+	ldr r5, [r5]
+
+	// we could only copy atags to a 4-aligned address
+	mov r6, #4
+	bl aling_r5_to_r6
+
+	// compute where atags copied right after blob would end
+	add r6, r5, r3
+	// we can only overwrite stuff before the copying loop
+	adr r7, copy_atags_loop
+	cmp r6, r7
+	ble copy_atags
+
+	// atags wouldn't fit - use memory after stage1 as destination
+	adr r5, _boot
+	adr r6, stage1_size
+	ldr r6, [r6]
+	add r5, r6
+	mov r6, #4
+	bl aling_r5_to_r6
+	
+copy_atags:
+	// now copy atags (r2 - atags start; r3 - atags size;
+	// r5 - destination; r4 - iterator; r6 - buffor)
+	mov r4, #0
+
+copy_atags_loop:
+	ldr r6, [r2, r4]
+	str r6, [r5, r4]
+	add r4, #4
+	cmp r4, r3
+	blo copy_atags_loop
+
+	mov r2, r5 // place the new atags address in r2
+	b stage2_blob_copying // atags stuff done; proceed
+
+// mini-function, that does what the label says; clobbers r7
+aling_r5_to_r6:
+	sub r5, #1
+	sub r7, r6, #1
+	bic r5, r7
+	add r5, r6
+	mov pc, lr
+
+	
+stage2_blob_copying: // copy stage2 of the kernel to address 0x0
 
 	// first, load address of stage2_start to r3 (a PIC way)
 	adr r3, stage2_start
@@ -47,6 +144,8 @@ proceed:
 	
 blob_size:
 	.word stage2_end - stage2_start
+stage1_size:
+	.word stage1_end - _boot
 
 .align 4
 stage2_start:
@@ -61,7 +160,9 @@ loop:
 	add r6, r6, #4
 	cmp r6, r5
 	blo loop
-	
+
         // Call stage2 of the kernel (branch to 0x0,
 	// which is the reset handler).
         bx r4
+	
+stage1_end: