Tegra: relocate code to BL31_BASE during cold boot

This patch adds support to relocate BL3-1 code to BL31_BASE in case
we cold boot to a different address. This is particularly useful to
maintain compatibility with legacy BL2 code.

This patch also checks to see if the image base address matches either
the TZDRAM or TZSRAM base.

Change-Id: I72c96d7f89076701a6ac2537d4c06565c54dab9c
Signed-off-by: Varun Wadekar <vwadekar@nvidia.com>

plat/nvidia/tegra/common/aarch64/tegra_helpers.S

@@ -254,6 +254,47 @@ endfunc plat_crash_console_putc
 	 */
 func plat_reset_handler
 
+	/* ----------------------------------------------------
+	 * Verify if we are running from BL31_BASE address
+	 * ----------------------------------------------------
+	 */
+	adr	x18, bl31_entrypoint
+	mov	x17, #BL31_BASE
+	cmp	x18, x17
+	b.eq	1f
+
+	/* ----------------------------------------------------
+	 * Copy the entire BL31 code to BL31_BASE if we are not
+	 * running from it already
+	 * ----------------------------------------------------
+	 */
+	mov	x0, x17
+	mov	x1, x18
+	mov	x2, #BL31_SIZE
+_loop16:
+	cmp	x2, #16
+	b.lt	_loop1
+	ldp	x3, x4, [x1], #16
+	stp	x3, x4, [x0], #16
+	sub	x2, x2, #16
+	b	_loop16
+	/* copy byte per byte */
+_loop1:
+	cbz	x2, _end
+	ldrb	w3, [x1], #1
+	strb	w3, [x0], #1
+	subs	x2, x2, #1
+	b.ne	_loop1
+
+	/* ----------------------------------------------------
+	 * Jump to BL31_BASE and start execution again
+	 * ----------------------------------------------------
+	 */
+_end:	mov	x0, x20
+	mov	x1, x21
+	br	x17
+1:
+
 	/* -----------------------------------
 	 * derive and save the phys_base addr
 	 * -----------------------------------

plat/nvidia/tegra/common/tegra_bl31_setup.c

@@ -135,6 +135,14 @@ void bl31_early_platform_setup(bl31_params_t *from_bl2,
 	plat_bl31_params_from_bl2.tzdram_size = plat_params->tzdram_size;
 	plat_bl31_params_from_bl2.uart_id = plat_params->uart_id;
 
+	/*
+	 * It is very important that we run either from TZDRAM or TZSRAM base.
+	 * Add an explicit check here.
+	 */
+	if ((plat_bl31_params_from_bl2.tzdram_base != BL31_BASE) &&
+	    (TEGRA_TZRAM_BASE != BL31_BASE))
+		panic();
+
 	/*
 	 * Get the base address of the UART controller to be used for the
 	 * console