diff --git a/thunks/Makefile b/thunks/Makefile index 1e1f612..84a098b 100644 --- a/thunks/Makefile +++ b/thunks/Makefile @@ -3,7 +3,10 @@ # SPL_THUNK := fel-to-spl-thunk.h -THUNKS := sid_read_root.h +THUNKS := memcpy.h +THUNKS += readl_writel.h +THUNKS += rmr-thunk.h +THUNKS += sid_read_root.h all: $(SPL_THUNK) $(THUNKS) # clean up object files afterwards diff --git a/thunks/memcpy.S b/thunks/memcpy.S new file mode 100644 index 0000000..e280f41 --- /dev/null +++ b/thunks/memcpy.S @@ -0,0 +1,70 @@ +/* + * copy "upwards", increasing destination and source addresses + */ +fel_memcpy_up: + ldr r0, 1f /* dst_addr */ + ldr r1, 2f /* src_addr */ + ldr r2, 3f /* bytes */ + sub r3, r1, r0 + tst r3, #3 /* test LSB for word alignment */ + bne copyup_tail /* unaligned access, copy byte-wise */ +copyup_head: + tst r1, #3 /* word boundary? */ + beq copyup_loop + ldrb r3, [r1], #1 /* load and post-inc */ + strb r3, [r0], #1 /* store and post-inc */ + subs r2, #1 /* r2 -= 1 */ + bpl copyup_head + bx lr /* early return on small byte count (r2 < 0) */ +copyup_loop: + subs r2, #4 /* r2 -= 4 */ + ldrpl r3, [r1], #4 /* load and post-inc */ + strpl r3, [r0], #4 /* store and post-inc */ + bpl copyup_loop /* while (r2 >= 0) */ + add r2, #4 /* r2 = remaining byte count */ +copyup_tail: + subs r2, #1 /* r2 -= 1 */ + bxmi lr /* return on (r2 < 0) */ + ldrb r3, [r1], #1 /* load and post-inc */ + strb r3, [r0], #1 /* store and post-inc */ + b copyup_tail + +1: .word 0 /* dst_addr */ +2: .word 0 /* src_addr */ +3: .word 0 /* bytes */ + +/* + * copy "downwards", using base-relative indexing + */ +fel_memcpy_down: + ldr r0, 1f /* dst_addr */ + ldr r1, 2f /* src_addr */ + ldr r2, 3f /* bytes */ + sub r3, r0, r1 + tst r3, #3 /* test LSB for word alignment */ + bne copydn_tail /* unaligned access, copy byte-wise */ +copydn_head: + add r3, r1, r2 /* r3 = r1 + r2, for alignment check */ + tst r3, #3 /* word boundary? */ + beq copydn_loop + subs r2, #1 /* r2 -= 1 */ + bxmi lr /* early return on small byte count (r2 < 0) */ + ldrb r3, [r1, r2] /* load byte */ + strb r3, [r0, r2] /* store byte */ + b copydn_head +copydn_loop: + subs r2, #4 /* r2 -= 4 */ + ldrpl r3, [r1, r2] /* load word */ + strpl r3, [r0, r2] /* store word */ + bpl copydn_loop /* while (r2 >= 0) */ + add r2, #4 /* r2 = remaining byte count */ +copydn_tail: + subs r2, #1 /* r2 -= 1 */ + bxmi lr /* return on (r2 < 0) */ + ldrb r3, [r1, r2] /* load byte */ + strb r3, [r0, r2] /* store byte */ + b copydn_tail + +1: .word 0 /* dst_addr */ +2: .word 0 /* src_addr */ +3: .word 0 /* bytes */ diff --git a/thunks/memcpy.h b/thunks/memcpy.h new file mode 100644 index 0000000..ddfeaaf --- /dev/null +++ b/thunks/memcpy.h @@ -0,0 +1,55 @@ + /* : */ + htole32(0xe59f0054), /* 0: ldr r0, [pc, #84] */ + htole32(0xe59f1054), /* 4: ldr r1, [pc, #84] */ + htole32(0xe59f2054), /* 8: ldr r2, [pc, #84] */ + htole32(0xe0413000), /* c: sub r3, r1, r0 */ + htole32(0xe3130003), /* 10: tst r3, #3 */ + htole32(0x1a00000b), /* 14: bne 48 */ + /* : */ + htole32(0xe3110003), /* 18: tst r1, #3 */ + htole32(0x0a000004), /* 1c: beq 34 */ + htole32(0xe4d13001), /* 20: ldrb r3, [r1], #1 */ + htole32(0xe4c03001), /* 24: strb r3, [r0], #1 */ + htole32(0xe2522001), /* 28: subs r2, r2, #1 */ + htole32(0x5afffff9), /* 2c: bpl 18 */ + htole32(0xe12fff1e), /* 30: bx lr */ + /* : */ + htole32(0xe2522004), /* 34: subs r2, r2, #4 */ + htole32(0x54913004), /* 38: ldrpl r3, [r1], #4 */ + htole32(0x54803004), /* 3c: strpl r3, [r0], #4 */ + htole32(0x5afffffb), /* 40: bpl 34 */ + htole32(0xe2822004), /* 44: add r2, r2, #4 */ + /* : */ + htole32(0xe2522001), /* 48: subs r2, r2, #1 */ + htole32(0x412fff1e), /* 4c: bxmi lr */ + htole32(0xe4d13001), /* 50: ldrb r3, [r1], #1 */ + htole32(0xe4c03001), /* 54: strb r3, [r0], #1 */ + htole32(0xeafffffa), /* 58: b 48 */ + /* : */ + htole32(0xe59f0058), /* 68: ldr r0, [pc, #88] */ + htole32(0xe59f1058), /* 6c: ldr r1, [pc, #88] */ + htole32(0xe59f2058), /* 70: ldr r2, [pc, #88] */ + htole32(0xe0403001), /* 74: sub r3, r0, r1 */ + htole32(0xe3130003), /* 78: tst r3, #3 */ + htole32(0x1a00000c), /* 7c: bne b4 */ + /* : */ + htole32(0xe0813002), /* 80: add r3, r1, r2 */ + htole32(0xe3130003), /* 84: tst r3, #3 */ + htole32(0x0a000004), /* 88: beq a0 */ + htole32(0xe2522001), /* 8c: subs r2, r2, #1 */ + htole32(0x412fff1e), /* 90: bxmi lr */ + htole32(0xe7d13002), /* 94: ldrb r3, [r1, r2] */ + htole32(0xe7c03002), /* 98: strb r3, [r0, r2] */ + htole32(0xeafffff7), /* 9c: b 80 */ + /* : */ + htole32(0xe2522004), /* a0: subs r2, r2, #4 */ + htole32(0x57913002), /* a4: ldrpl r3, [r1, r2] */ + htole32(0x57803002), /* a8: strpl r3, [r0, r2] */ + htole32(0x5afffffb), /* ac: bpl a0 */ + htole32(0xe2822004), /* b0: add r2, r2, #4 */ + /* : */ + htole32(0xe2522001), /* b4: subs r2, r2, #1 */ + htole32(0x412fff1e), /* b8: bxmi lr */ + htole32(0xe7d13002), /* bc: ldrb r3, [r1, r2] */ + htole32(0xe7c03002), /* c0: strb r3, [r0, r2] */ + htole32(0xeafffffa), /* c4: b b4 */ diff --git a/thunks/readl_writel.S b/thunks/readl_writel.S new file mode 100644 index 0000000..ebe7d0c --- /dev/null +++ b/thunks/readl_writel.S @@ -0,0 +1,41 @@ +/* + * Thunk code for buffered 'long' (i.e. 32-bit) read and write operations + */ + +.equ MAX_WORDS, 0x100 - 12 + +fel_readl_n: + ldr r0, 1f /* read_addr */ + adr r1, 3f /* read_data */ + ldr r2, 2f /* read_count */ + /* limit word count to a maximum value */ + cmp r2, #MAX_WORDS + movgt r2, #MAX_WORDS +read_loop: + subs r2, #1 + bxmi lr + ldr r3, [r0], #4 + str r3, [r1], #4 + b read_loop + +1: .word 0 /* read_addr */ +2: .word 0 /* read_count */ +3: .word 0 /* read_data */ + +fel_writel_n: + ldr r0, 1f /* write_addr */ + adr r1, 3f /* write_data */ + ldr r2, 2f /* write_count */ + /* limit word count to a maximum value */ + cmp r2, #MAX_WORDS + movgt r2, #MAX_WORDS +write_loop: + subs r2, #1 + bxmi lr + ldr r3, [r1], #4 + str r3, [r0], #4 + b write_loop + +1: .word 0 /* write_addr */ +2: .word 0 /* write_count */ +3: .word 0 /* write_data */ diff --git a/thunks/readl_writel.h b/thunks/readl_writel.h new file mode 100644 index 0000000..d50b5ac --- /dev/null +++ b/thunks/readl_writel.h @@ -0,0 +1,24 @@ + /* : */ + htole32(0xe59f0020), /* 0: ldr r0, [pc, #32] */ + htole32(0xe28f1024), /* 4: add r1, pc, #36 */ + htole32(0xe59f201c), /* 8: ldr r2, [pc, #28] */ + htole32(0xe35200f4), /* c: cmp r2, #244 */ + htole32(0xc3a020f4), /* 10: movgt r2, #244 */ + /* : */ + htole32(0xe2522001), /* 14: subs r2, r2, #1 */ + htole32(0x412fff1e), /* 18: bxmi lr */ + htole32(0xe4903004), /* 1c: ldr r3, [r0], #4 */ + htole32(0xe4813004), /* 20: str r3, [r1], #4 */ + htole32(0xeafffffa), /* 24: b 14 */ + /* : */ + htole32(0xe59f0020), /* 34: ldr r0, [pc, #32] */ + htole32(0xe28f1024), /* 38: add r1, pc, #36 */ + htole32(0xe59f201c), /* 3c: ldr r2, [pc, #28] */ + htole32(0xe35200f4), /* 40: cmp r2, #244 */ + htole32(0xc3a020f4), /* 44: movgt r2, #244 */ + /* : */ + htole32(0xe2522001), /* 48: subs r2, r2, #1 */ + htole32(0x412fff1e), /* 4c: bxmi lr */ + htole32(0xe4913004), /* 50: ldr r3, [r1], #4 */ + htole32(0xe4803004), /* 54: str r3, [r0], #4 */ + htole32(0xeafffffa), /* 58: b 48 */ diff --git a/thunks/rmr-thunk.S b/thunks/rmr-thunk.S new file mode 100644 index 0000000..10429b5 --- /dev/null +++ b/thunks/rmr-thunk.S @@ -0,0 +1,24 @@ +/* + * Request AArch32/AArch64 warm reset, using RVBAR and Reset Management Register + */ + +rmr_request: + ldr r0, 1f /* RVBAR register address */ + ldr r1, 2f /* desired entry point (reset vector) */ + str r1, [r0] + dsb + isb /* make sure we write the address */ + + ldr r1, 3f /* RMR mode: bit 1 = RR, bit 0 = AA64 */ + mrc p15, 0, r0, c12, c0, 2 /* read RMR */ + orr r0, r0, r1 /* request warm reset (according to rmr_mode) */ + mcr p15, 0, r0, c12, c0, 2 /* write RMR, trigger reset */ + + isb +0: + wfi + b 0b /* loop */ + +1: .word 0 /* rvbar_reg */ +2: .word 0 /* entry_point */ +3: .word 0 /* rmr_mode (2 = AArch32, 3 = AArch64) */ diff --git a/thunks/rmr-thunk.h b/thunks/rmr-thunk.h new file mode 100644 index 0000000..1e6fcd9 --- /dev/null +++ b/thunks/rmr-thunk.h @@ -0,0 +1,13 @@ + /* : */ + htole32(0xe59f0028), /* 0: ldr r0, [pc, #40] */ + htole32(0xe59f1028), /* 4: ldr r1, [pc, #40] */ + htole32(0xe5801000), /* 8: str r1, [r0] */ + htole32(0xf57ff04f), /* c: dsb sy */ + htole32(0xf57ff06f), /* 10: isb sy */ + htole32(0xe59f101c), /* 14: ldr r1, [pc, #28] */ + htole32(0xee1c0f50), /* 18: mrc 15, 0, r0, cr12, cr0, {2} */ + htole32(0xe1800001), /* 1c: orr r0, r0, r1 */ + htole32(0xee0c0f50), /* 20: mcr 15, 0, r0, cr12, cr0, {2} */ + htole32(0xf57ff06f), /* 24: isb sy */ + htole32(0xe320f003), /* 28: wfi */ + htole32(0xeafffffd), /* 2c: b 28 */