hikey: Add wrapper script to build uefi

Booloader sources are located under device/linaro/bootloader

Change-Id: I8b35f9a292f5037eac2e0a281f5345921a313b93
Signed-off-by: Vishal Bhoj <vishal.bhoj@linaro.org>

8 files changed, 548 insertions, 0 deletions

diff --git a/l-loader/.gitignore b/l-loader/.gitignore
new file mode 100644
index 00000000..b282a0e1
--- /dev/null
+++ b/l-loader/.gitignore
@@ -0,0 +1,3 @@
+/*.o
+/*.img
+/*.bin
diff --git a/l-loader/COPYING b/l-loader/COPYING
new file mode 100644
index 00000000..4ad3f200
--- /dev/null
+++ b/l-loader/COPYING
@@ -0,0 +1,25 @@
+Copyright (c) 2014-2016, Linaro Ltd. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+* Redistributions of source code must retain the above copyright
+  notice, this list of conditions and the following disclaimer.
+* Redistributions in binary form must reproduce the above copyright
+  notice, this list of conditions and the following disclaimer in
+  the documentation and/or other materials provided with the
+  distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
diff --git a/l-loader/Makefile b/l-loader/Makefile
new file mode 100644
index 00000000..93afcd1a
--- /dev/null
+++ b/l-loader/Makefile
@@ -0,0 +1,27 @@
+CROSS_COMPILE?=arm-linux-gnueabihf-
+CC=$(CROSS_COMPILE)gcc
+CFLAGS=-march=armv7-a
+LD=$(CROSS_COMPILE)ld
+OBJCOPY=$(CROSS_COMPILE)objcopy
+BL1=bl1.bin
+PTABLE_LST:=aosp-4g aosp-8g linux-4g linux-8g
+
+all: l-loader.bin ptable.img
+
+%.o: %.S $(DEPS)
+	$(CC) $(CFLAGS) -c -o $@ $<
+
+l-loader.bin: start.o $(BL1)
+	$(LD) -Bstatic -Tl-loader.lds -Ttext 0xf9800800 start.o -o loader
+	$(OBJCOPY) -O binary loader temp
+	python gen_loader.py -o $@ --img_loader=temp --img_bl1=$(BL1)
+	rm -f loader temp
+
+ptable.img:
+	for ptable in $(PTABLE_LST); do \
+		sudo PTABLE=$${ptable} bash -x generate_ptable.sh;\
+		python gen_loader.py -o ptable-$${ptable}.img --img_prm_ptable=prm_ptable.img;\
+	done
+
+clean:
+	rm -f *.o *.img l-loader.bin
diff --git a/l-loader/README.md b/l-loader/README.md
new file mode 100644
index 00000000..e5dcd990
--- /dev/null
+++ b/l-loader/README.md
@@ -0,0 +1,6 @@
+# l-loader
+
+Used to switch from aarch32 to aarch64 and boot. First image to be flashed
+when in recovery mode.
+
+HiKey specific.
diff --git a/l-loader/gen_loader.py b/l-loader/gen_loader.py
new file mode 100755
index 00000000..2d763ca9
--- /dev/null
+++ b/l-loader/gen_loader.py
@@ -0,0 +1,268 @@
+#!/usr/bin/env python
+
+import os
+import os.path
+import sys, getopt
+import binascii
+import struct
+import string
+
+class generator(object):
+    #
+    # struct l_loader_head {
+    #      unsigned int	first_instr;
+    #      unsigned char	magic[16];	@ BOOTMAGICNUMBER!
+    #      unsigned int	l_loader_start;
+    #      unsigned int	l_loader_end;
+    # };
+    file_header = [0, 0, 0, 0, 0, 0, 0]
+
+    #
+    # struct entry_head {
+    #       unsigned char   magic[8];           @ ENTY
+    #       unsigned char   name[8];            @ loader/bl1
+    #       unsigned int    start_lba;
+    #       unsigned int    count_lba;
+    #       unsigned int    flag;               @ boot partition or not
+    # };
+
+    s1_entry_name = ['loader', 'bl1']
+    s2_entry_name = ['primary', 'second']
+
+    block_size = 512
+
+    stage = 0
+
+    # set in self.add()
+    idx = 0
+
+    # set in self.parse()
+    ptable_lba = 0
+    stable_lba = 0
+
+    # file pointer
+    p_entry = 0
+    p_file = 0
+
+    def __init__(self, out_img):
+        try:
+            self.fp = open(out_img, "wb+")
+        except IOError, e:
+            print "*** file open error:", e
+            sys.exit(3)
+        else:
+            self.entry_hd = [[0 for col in range(7)] for row in range(5)]
+
+    def __del__(self):
+        self.fp.close()
+
+    # parse partition from the primary ptable
+    def parse(self, fname):
+        try:
+            fptable = open(fname, "rb")
+        except IOError, e:
+            print "*** file open error:", e
+            sys.exit(3)
+        else:
+            # skip the first block in primary partition table
+            # that is MBR protection information
+            fptable.read(self.block_size)
+            # check whether it's a primary paritition table
+            data = struct.unpack("8s", fptable.read(8))
+            efi_magic = 'EFI PART'
+            if cmp("EFI PART", data[0]):
+                print "It's not partition table image."
+                fptable.close()
+                sys.exit(4)
+            # skip 16 bytes
+            fptable.read(16)
+            # get lba of both primary partition table and secondary partition table
+            data = struct.unpack("QQQQ", fptable.read(32))
+            self.ptable_lba = data[0] - 1
+            self.stable_lba = data[3] + 1
+            # skip 24 bytes
+            fptable.read(24)
+            data = struct.unpack("i", fptable.read(4))
+            pentries = data[0]
+            # skip the reset in this block
+            fptable.read(self.block_size - 84)
+
+            for i in range(1, pentries):
+                # name is encoded as UTF-16
+                d0,lba,d2,name = struct.unpack("32sQ16s72s", fptable.read(128))
+                plainname = unicode(name, "utf-16")
+                if (not cmp(plainname[0:7], 'l-loader'[0:7])):
+                    print 'bl1_lba: ', lba
+                    self.bl1_lba = lba
+                    sys.exit(1)
+
+            fptable.close()
+
+    def add(self, lba, fname):
+        try:
+            fsize = os.path.getsize(fname)
+        except IOError, e:
+            print "*** file open error:", e
+            sys.exit(4)
+        else:
+            blocks = (fsize + self.block_size - 1) / self.block_size
+            if (self.stage == 1):
+                # Boot Area1 in eMMC
+                bootp = 1
+                if self.idx == 0:
+                    self.p_entry = 28
+            elif (self.stage == 2):
+                # User Data Area in eMMC
+                bootp = 0
+                # create an empty block only for stage2
+                # This empty block is used to store entry head
+                print 'p_file: ', self.p_file, 'p_entry: ', self.p_entry
+                if self.idx == 0:
+                    self.fp.seek(self.p_file)
+                    for i in range (0, self.block_size):
+                        zero = struct.pack('x')
+                        self.fp.write(zero)
+                    self.p_file += self.block_size
+                    self.p_entry = 0
+            else:
+                print "wrong stage ", stage, "is specified"
+                sys.exit(4)
+            # Maybe the file size isn't aligned. So pad it.
+            if (self.idx == 0) and (self.stage == 1):
+                if fsize > 2048:
+                    print 'loader size exceeds 2KB. file size: ', fsize
+                    sys.exit(4)
+                else:
+                    left_bytes = 2048 - fsize
+            else:
+                left_bytes = fsize % self.block_size
+                if left_bytes:
+                    left_bytes = self.block_size - left_bytes
+            print 'lba: ', lba, 'blocks: ', blocks, 'bootp: ', bootp, 'fname: ', fname
+            # write images
+            fimg = open(fname, "rb")
+            for i in range (0, blocks):
+                buf = fimg.read(self.block_size)
+                self.fp.seek(self.p_file)
+                self.fp.write(buf)
+                # p_file is the file pointer of the new binary file
+                # At last, it means the total block size of the new binary file
+                self.p_file += self.block_size
+
+            if (self.idx == 0) and (self.stage == 1):
+                self.p_file = 2048
+            print 'p_file: ', self.p_file, 'last block is ', fsize % self.block_size, 'bytes', '  tell: ', self.fp.tell(), 'left_bytes: ', left_bytes
+            if left_bytes:
+                for i in range (0, left_bytes):
+                    zero = struct.pack('x')
+                    self.fp.write(zero)
+                print 'p_file: ', self.p_file, '  pad to: ', self.fp.tell()
+
+            # write entry information at the header
+            if self.stage == 1:
+                byte = struct.pack('8s8siii', 'ENTRYHDR', self.s1_entry_name[self.idx], lba, blocks, bootp)
+            elif self.stage == 2:
+                byte = struct.pack('8s8siii', 'ENTRYHDR', self.s2_entry_name[self.idx], lba, blocks, bootp)
+            self.fp.seek(self.p_entry)
+            self.fp.write(byte)
+            self.p_entry += 28
+            self.idx += 1
+
+            fimg.close()
+
+    def hex2(self, data):
+        return data > 0 and hex(data) or hex(data & 0xffffffff)
+
+    def end(self):
+        if self.stage == 1:
+            self.fp.seek(20)
+            start,end = struct.unpack("ii", self.fp.read(8))
+            print "start: ", self.hex2(start), 'end: ', self.hex2(end)
+            end = start + self.p_file
+            print "start: ", self.hex2(start), 'end: ', self.hex2(end)
+            self.fp.seek(24)
+            byte = struct.pack('i', end)
+            self.fp.write(byte)
+        self.fp.close()
+
+    def create_stage1(self, img_loader, img_bl1, output_img):
+        print '+-----------------------------------------------------------+'
+        print ' Input Images:'
+        print '     loader:                       ', img_loader
+        print '     bl1:                          ', img_bl1
+        print ' Ouput Image:                      ', output_img
+        print '+-----------------------------------------------------------+\n'
+
+        self.stage = 1
+
+        # The first 2KB is reserved
+        # The next 2KB is for loader image
+        self.add(4, img_loader)    # img_loader doesn't exist in partition table
+        print 'self.idx: ', self.idx
+        # bl1.bin starts from 4KB
+        self.add(8, img_bl1)      # img_bl1 doesn't exist in partition table
+
+    def create_stage2(self, img_prm_ptable, img_sec_ptable, output_img):
+        print '+-----------------------------------------------------------+'
+        print ' Input Images:'
+        print '     primary partition table:      ', img_prm_ptable
+        print '     secondary partition table:    ', img_sec_ptable
+        print ' Ouput Image:                      ', output_img
+        print '+-----------------------------------------------------------+\n'
+
+        self.stage = 2
+        self.parse(img_prm_ptable)
+        self.add(self.ptable_lba, img_prm_ptable)
+        if (cmp(img_sec_ptable, 'secondary partition table')):
+            # Doesn't match. It means that secondary ptable is specified.
+            self.add(self.stable_lba, img_sec_ptable)
+        else:
+            print 'Don\'t need secondary partition table'
+
+def main(argv):
+    stage1 = 0
+    stage2 = 0
+    img_prm_ptable = "primary partition table"
+    img_sec_ptable = "secondary partition table"
+    try:
+        opts, args = getopt.getopt(argv,"ho:",["img_loader=","img_bl1=","img_prm_ptable=","img_sec_ptable="])
+    except getopt.GetoptError:
+        print 'gen_loader.py -o <l-loader.bin> --img_loader <l-loader> --img_bl1 <bl1.bin> --img_prm_ptable <prm_ptable.img> --img_sec_ptable <sec_ptable.img>'
+        sys.exit(2)
+    for opt, arg in opts:
+        if opt == '-h':
+            print 'gen_loader.py -o <l-loader.bin> --img_loader <l-loader> --img_bl1 <bl1.bin> --img_prm_ptable <prm_ptable.img> --img_sec_ptable <sec_ptable.img>'
+            sys.exit(1)
+        elif opt == '-o':
+            output_img = arg
+        elif opt in ("--img_loader"):
+            img_loader = arg
+            stage1 = 1
+        elif opt in ("--img_bl1"):
+            img_bl1 = arg
+            stage1 = 1
+        elif opt in ("--img_prm_ptable"):
+            img_prm_ptable = arg
+            stage2 = 1
+        elif opt in ("--img_sec_ptable"):
+            img_sec_ptable = arg
+
+    loader = generator(output_img)
+    loader.idx = 0
+
+    if (stage1 == 1) and (stage2 == 1):
+        print 'There are only loader & BL1 in stage1.'
+        print 'And there are primary partition table, secondary partition table and FIP in stage2.'
+        sys.exit(1)
+    elif (stage1 == 0) and (stage2 == 0):
+        print 'No input images are specified.'
+        sys.exit(1)
+    elif stage1 == 1:
+        loader.create_stage1(img_loader, img_bl1, output_img)
+    elif stage2 == 1:
+        loader.create_stage2(img_prm_ptable, img_sec_ptable, output_img)
+
+    loader.end()
+
+if __name__ == "__main__":
+    main(sys.argv[1:])
diff --git a/l-loader/generate_ptable.sh b/l-loader/generate_ptable.sh
new file mode 100755
index 00000000..3bc232ee
--- /dev/null
+++ b/l-loader/generate_ptable.sh
@@ -0,0 +1,87 @@
+#!/bin/sh
+# Generate partition table for HiKey eMMC
+#
+# tiny: for testing purpose.
+# aosp: 10 entries (same as linux with userdata).
+# linux: 9 entries (same as aosp without userdata).
+
+PTABLE=${PTABLE:-aosp}
+SECTOR_SIZE=512
+TEMP_FILE=$(mktemp /tmp/${PTABLE}.XXXXXX)
+
+case ${PTABLE} in
+  tiny)
+    SECTOR_NUMBER=81920
+    ;;
+  aosp-4g|linux-4g)
+    SECTOR_NUMBER=7471104
+    ;;
+  aosp-8g|linux-8g)
+    SECTOR_NUMBER=15269888
+    ;;
+esac
+
+BK_PTABLE_LBA=$(expr ${SECTOR_NUMBER} - 33)
+echo ${BK_PTABLE_LBA}
+
+# get the partition table
+case ${PTABLE} in
+  tiny)
+    dd if=/dev/zero of=${TEMP_FILE} bs=${SECTOR_SIZE} count=${SECTOR_NUMBER}
+    sgdisk -U -R -v ${TEMP_FILE}
+    sgdisk -n 1:2048:4095 -t 1:0700 -u 1:F9F21F01-A8D4-5F0E-9746-594869AEC3E4 -c 1:"vrl" -p ${TEMP_FILE}
+    sgdisk -n 2:4096:6143 -t 2:0700 -u 2:F9F21F02-A8D4-5F04-9746-594869AEC3E4 -c 2:"vrl_backup" -p ${TEMP_FILE}
+    ;;
+  aosp*)
+    dd if=/dev/zero of=${TEMP_FILE} bs=${SECTOR_SIZE} count=${SECTOR_NUMBER}
+    sgdisk -U 2CB85345-6A91-4043-8203-723F0D28FBE8 -v ${TEMP_FILE}
+    #[1: vrl: 1M-2M]
+    sgdisk -n 1:0:+1M -t 1:0700 -u 1:496847AB-56A1-4CD5-A1AD-47F4ACF055C9 -c 1:"vrl" ${TEMP_FILE}
+    #[2: vrl_backup: 2M-3M]
+    sgdisk -n 2:0:+1M -t 2:0700 -u 2:61A36FC1-8EFB-4899-84D8-B61642EFA723 -c 2:"vrl_backup" ${TEMP_FILE}
+    #[3: mcuimage: 3M-4M]
+    sgdisk -n 3:0:+1M -t 3:0700 -u 3:65007411-962D-4781-9B2C-51DD7DF22CC3 -c 3:"mcuimage" ${TEMP_FILE}
+    #[4: fastboot: 4M-12M]
+    sgdisk -n 4:0:+8M -t 4:EF02 -u 4:496847AB-56A1-4CD5-A1AD-47F4ACF055C9 -c 4:"fastboot" ${TEMP_FILE}
+    #[5: nvme: 12M-14M]
+    sgdisk -n 5:0:+2M -t 5:0700 -u 5:00354BCD-BBCB-4CB3-B5AE-CDEFCB5DAC43 -c 5:"nvme" ${TEMP_FILE}
+    #[6: boot: 14M-78M]
+    sgdisk -n 6:0:+64M -t 6:EF00 -u 6:5C0F213C-17E1-4149-88C8-8B50FB4EC70E -c 6:"boot" ${TEMP_FILE}
+    #[7: reserved: 78M-334M]
+    sgdisk -n 7:0:+256M -t 7:0700 -u 7:BED8EBDC-298E-4A7A-B1F1-2500D98453B7 -c 7:"reserved" ${TEMP_FILE}
+    #[8: cache: 334M-590M]
+    sgdisk -n 8:0:+256M -t 8:8301 -u 8:A092C620-D178-4CA7-B540-C4E26BD6D2E2 -c 8:"cache" ${TEMP_FILE}
+    #[9: system: 590M-2126M]
+    sgdisk -n 9:0:+1536M -t 9:8300 -u 9:FC56E345-2E8E-49AE-B2F8-5B9D263FE377 -c 9:"system" ${TEMP_FILE}
+    #[10: userdata: 2126M-End]
+    sgdisk -n -E -t 10:8300 -u 10:064111F6-463B-4CE1-876B-13F3684CE164 -c 10:"userdata" -p ${TEMP_FILE}
+    ;;
+  linux*)
+    dd if=/dev/zero of=${TEMP_FILE} bs=${SECTOR_SIZE} count=${SECTOR_NUMBER}
+    sgdisk -U 2CB85345-6A91-4043-8203-723F0D28FBE8 -v ${TEMP_FILE}
+    #[1: vrl: 1M-2M]
+    sgdisk -n 1:0:+1M -t 1:0700 -u 1:496847AB-56A1-4CD5-A1AD-47F4ACF055C9 -c 1:"vrl" ${TEMP_FILE}
+    #[2: vrl_backup: 2M-3M]
+    sgdisk -n 2:0:+1M -t 2:0700 -u 2:61A36FC1-8EFB-4899-84D8-B61642EFA723 -c 2:"vrl_backup" ${TEMP_FILE}
+    #[3: mcuimage: 3M-4M]
+    sgdisk -n 3:0:+1M -t 3:0700 -u 3:65007411-962D-4781-9B2C-51DD7DF22CC3 -c 3:"mcuimage" ${TEMP_FILE}
+    #[4: fastboot: 4M-12M]
+    sgdisk -n 4:0:+8M -t 4:EF02 -u 4:496847AB-56A1-4CD5-A1AD-47F4ACF055C9 -c 4:"fastboot" ${TEMP_FILE}
+    #[5: nvme: 12M-14M]
+    sgdisk -n 5:0:+2M -t 5:0700 -u 5:00354BCD-BBCB-4CB3-B5AE-CDEFCB5DAC43 -c 5:"nvme" ${TEMP_FILE}
+    #[6: boot: 14M-78M]
+    sgdisk -n 6:0:+64M -t 6:EF00 -u 6:5C0F213C-17E1-4149-88C8-8B50FB4EC70E -c 6:"boot" ${TEMP_FILE}
+    #[7: reserved: 78M-334M]
+    sgdisk -n 7:0:+256M -t 7:0700 -u 7:BED8EBDC-298E-4A7A-B1F1-2500D98453B7 -c 7:"reserved" ${TEMP_FILE}
+    #[8: cache: 334M-590M]
+    sgdisk -n 8:0:+256M -t 8:8301 -u 8:A092C620-D178-4CA7-B540-C4E26BD6D2E2 -c 8:"cache" ${TEMP_FILE}
+    #[9: system: 590M-End]
+    sgdisk -n -E -t 9:8300 -u 9:FC56E345-2E8E-49AE-B2F8-5B9D263FE377 -c 9:"system" ${TEMP_FILE}
+    ;;
+esac
+
+# get the main and the backup parts of the partition table
+dd if=${TEMP_FILE} of=prm_ptable.img bs=${SECTOR_SIZE} count=34
+dd if=${TEMP_FILE} of=sec_ptable.img skip=${BK_PTABLE_LBA} bs=${SECTOR_SIZE} count=33
+
+rm -f ${TEMP_FILE}
diff --git a/l-loader/l-loader.lds b/l-loader/l-loader.lds
new file mode 100644
index 00000000..41eb16f5
--- /dev/null
+++ b/l-loader/l-loader.lds
@@ -0,0 +1,32 @@
+/*
+ * Copyright (c) 2014 Linaro Ltd.
+ */
+
+OUTPUT_FORMAT("elf32-littlearm", "elf32-littlearm", "elf32-littlearm")
+OUTPUT_ARCH(arm)
+ENTRY(_start)
+SECTIONS
+{
+	. = 0xf9800800;
+	LLOADER_START = .;
+
+	.text :
+	{
+		*(.text)
+		*(.rodata)
+	}
+
+	.data ALIGN(4):
+	{
+		*(.data)
+	}
+
+	. = ALIGN(4);
+
+	.bss ALIGN(4):
+	{
+		*(.bss)
+	}
+
+	LLOADER_BL1_BIN = 0xf9801000;
+}
diff --git a/l-loader/start.S b/l-loader/start.S
new file mode 100644
index 00000000..a0d838a3
--- /dev/null
+++ b/l-loader/start.S
@@ -0,0 +1,100 @@
+	.text
+
+/*
+ * The head of l-loader is defined in below.
+ * struct l_loader_head {
+ *	unsigned int	first_instr;
+ *	unsigned char	magic[16];	@ BOOTMAGICNUMBER!
+ *	unsigned int	l_loader_start;
+ *	unsigned int	l_loader_end;
+ * };
+ */
+
+#define CPU0_CTRL_OFFSET		0x100
+#define CPU7_CTRL_OFFSET		0x800
+#define CPU0_RVBARADDR_OFFSET		0x158
+#define CPU7_RVBARADDR_OFFSET		0x858
+
+#define CPU_CTRL_AARCH64_MODE		(1 << 7)
+
+#define SC_PERIPH_CLKEN3		0x230
+#define SC_PERIPH_RSTDIS3		0x334
+	.global	_start
+_start:
+	b	reset
+@ Android magic number: "BOOTMAGICNUMBER!"
+android_magic:
+	.word	0x544f4f42
+	.word	0x4947414d
+	.word	0x4d554e43
+	.word	0x21524542
+	.word	LLOADER_START		@ LLOADER_START in RAM
+	.word	0			@ LLOADER_END in RAM
+
+entries:
+	@ 5 entries with 7 words
+	.space	140
+
+	.align	7
+
+reset:
+	ldr	r8, =(0xf9800000 + 0x700)
+	str	r0, [r8]		@ download mode (1:usb,2:uart,0:boot)
+
+	ldr	r4, =0xf6504000		@ ACPU_CTRL register base
+	@ set RVBAR for cpu0
+	ldr	r5, =CPU0_RVBARADDR_OFFSET
+	ldr	r6, =LLOADER_BL1_BIN
+	mov	r6, r6, lsr #2
+	str	r6, [r4, r5]
+1:
+	ldr	r0, [r4, r5]
+	cmp	r0, r6
+	bne	1b
+
+	mov	r5, #CPU0_CTRL_OFFSET
+	mov	r6, #CPU7_CTRL_OFFSET
+2:
+	ldr	r0, [r4, r5]		@ Load ACPU_SC_CPUx_CTRL
+	orr	r0, r0, #CPU_CTRL_AARCH64_MODE
+	str	r0, [r4, r5]		@ Save to ACPU_SC_CPUx_CTRL
+	ldr	r0, [r4, r5]
+
+	add	r5, r5, #0x100		@ Iterate ACPU_SC_CPUx_CTRL
+	cmp	r5, r6
+	ble	2b
+
+	/*
+	 * Prepare UART2 & UART3 without baud rate initialization.
+	 * So always output on UART0 in l-loader.
+	 */
+	ldr	r4, =0xf70100e0		@ UART2_RXD IOMG register
+	mov	r0, #0
+	str	r0, [r4]
+	str	r0, [r4, #4]		@ UART2_TXD IOMG register
+	ldr	r0, [r4]
+
+	ldr	r4, =0xf7010188		@ UART3_RXD IOMG register
+	mov	r0, #1
+	str	r0, [r4]
+	str	r0, [r4, #4]		@ UART3_TXD IOMG register
+	ldr	r1, [r4]
+
+	ldr	r4, =0xf7030000		@ PERI_CTRL register base
+	@ By default, CLK_TXCO is the parent of CLK_UART3 in SC_CLK_SEL0
+
+	ldr	r5, =SC_PERIPH_RSTDIS3	@ unreset
+	ldr	r6, =SC_PERIPH_CLKEN3	@ enable PCLK
+	mov	r0, #(3 << 6)		@ bit'6' & bit'7' (UART2 & UART3)
+	str	r0, [r4, r5]
+	str	r0, [r4, r6]
+
+	@ execute warm reset to switch aarch64
+	mov	r2, #3
+	mcr	p15, 0, r2, c12, c0, 2
+	wfi
+panic:
+	b	panic
+
+str_aarch64:
+	.asciz	"\nSwitch to aarch64 mode. CPU0 executes at 0x"