>From bdbb2bcb70226ca5ad0873b3235f668d5d06a1df Mon Sep 17 00:00:00 2001
From: Tim Sander <tim@krieglstein.org>
Date: Fri, 28 Mar 2014 16:11:22 +0100
Subject: [PATCH] experimental TrustZone for Zynq (defunct)

---
 arch/arm/cpu/armv7/nonsec_virt.S |   18 +-
 arch/arm/cpu/armv7/virt-v7.c     |   20 ++-
 arch/arm/cpu/u-boot.lds          |    1 +
 arch/arm/dts/zynq-zed.dts        |  337 +++++++++++++++++++++++++++++++++++++-
 arch/arm/include/asm/armv7.h     |    8 +-
 board/xilinx/zynq/board.c        |   17 ++
 include/configs/zynq-common.h    |    4 +-
 include/configs/zynq_zed.h       |    7 +
 8 files changed, 395 insertions(+), 17 deletions(-)

diff --git a/arch/arm/cpu/armv7/nonsec_virt.S b/arch/arm/cpu/armv7/nonsec_virt.S
index 6367e09..c0d406c 100644
--- a/arch/arm/cpu/armv7/nonsec_virt.S
+++ b/arch/arm/cpu/armv7/nonsec_virt.S
@@ -52,7 +52,15 @@ _secure_monitor:
 	mcreq	p15, 4, r0, c12, c0, 0		@ write HVBAR
 #endif
 
-	movs	pc, lr				@ return to non-secure SVC
+	@ Reset CNTVOFF to 0 before leaving monitor mode
+	mrc	p15, 0, r0, c0, c1, 1		@ read ID_PFR1
+	ands	r0, r0, #CPUID_ARM_GENTIMER_MASK	@ test arch timer bits
+	movne	r0, #0
+	mcrrne	p15, 4, r0, r0, c14		@ Reset CNTVOFF to zero
+
+	mov     sp,r2					@ restore stack pointer from caller (eq to secure mode stack)
+
+	movs	pc, lr					@ return to non-secure SVC
 
 _hyp_trap:
 	mrs	lr, elr_hyp	@ for older asm: .byte 0x00, 0xe3, 0x0e, 0xe1
@@ -152,11 +160,19 @@ ENTRY(_nonsec_init)
 
 	mrc	p15, 0, ip, c12, c0, 0		@ save secure copy of VBAR
 
+	stmfd sp!,{r2}					@ save r2 as it is clobbered in monitor mode
+	mov r1,sp
 	isb
 	smc	#0				@ call into MONITOR mode
 
 	mcr	p15, 0, ip, c12, c0, 0		@ write non-secure copy of VBAR
 
+	ldr r1,=0xC5087A				@ this is the value the register has in secure mode (ALIGNMENT!)
+	mcr p15,0,r1,c1,c0,0			@ write control register 
+
+	ldr r1,=0xffffffff
+	mcr p15,0,r1,c3,c0,0			@ write Domain Access Control Register
+
 	mov	r1, #1
 	str	r1, [r3, #GICC_CTLR]		@ enable non-secure CPU i/f
 	add	r2, r2, #GIC_DIST_OFFSET
diff --git a/arch/arm/cpu/armv7/virt-v7.c b/arch/arm/cpu/armv7/virt-v7.c
index 2cd604f..b540dbc 100644
--- a/arch/arm/cpu/armv7/virt-v7.c
+++ b/arch/arm/cpu/armv7/virt-v7.c
@@ -46,7 +46,9 @@ static unsigned long get_gicd_base_address(void)
 	 * which we know only for sure for those two CPUs.
 	 */
 	asm("mrc p15, 0, %0, c0, c0, 0\n" : "=r"(midr));
+	debug("midr: %x masked:%x\n",midr,midr & MIDR_PRIMARY_PART_MASK);
 	switch (midr & MIDR_PRIMARY_PART_MASK) {
+	case MIDR_CORTEX_A9_R0P0:
 	case MIDR_CORTEX_A9_R0P1:
 	case MIDR_CORTEX_A15_R0P0:
 	case MIDR_CORTEX_A7_R0P0:
@@ -59,6 +61,7 @@ static unsigned long get_gicd_base_address(void)
 	/* get the GIC base address from the CBAR register */
 	asm("mrc p15, 4, %0, c15, c0, 0\n" : "=r" (periphbase));
 
+	debug("periphbase: %x working_address:%x\n",periphbase, (periphbase & CBAR_MASK)+GIC_DIST_OFFSET);
 	/* the PERIPHBASE can be mapped above 4 GB (lower 8 bits used to
 	 * encode this). Bail out here since we cannot access this without
 	 * enabling paging.
@@ -132,7 +135,8 @@ int armv7_switch_nonsec(void)
 	gic_dist_addr = get_gicd_base_address();
 	if (gic_dist_addr == -1)
 		return -1;
-
+	
+	debug("enable GIC\n");
 	/* enable the GIC distributor */
 	writel(readl(gic_dist_addr + GICD_CTLR) | 0x03,
 	       gic_dist_addr + GICD_CTLR);
@@ -144,14 +148,28 @@ int armv7_switch_nonsec(void)
 	 * from non-secure state. The first 32 interrupts are private per
 	 * CPU and will be set later when enabling the GIC for each core
 	 */
+	debug("allow non secure access\n");
 	for (i = 1; i <= itlinesnr; i++)
 		writel((unsigned)-1, gic_dist_addr + GICD_IGROUPRn + 4 * i);
 
+	debug("set core boot addr\n");
 	smp_set_core_boot_addr((unsigned long)_smp_pen, -1);
+	debug("kick cpus\n");
 	smp_kick_all_cpus();
 
+	/* read Monitor Vector Base Address Register */
+	asm("mrc p15, 0, %0, c12, c0, 1" : "=r"(reg) : : "cc");
+    printf("MVBAR (p15,0,c12,c0,1):%x\n",reg);
+
+	writel(0xffffffff,0xf800430); // unknown trustzone register, enable non-secure mode for ddr?
+	writel(0x00000001,0xf800440); // trustzone: DMA Non-Secure
+	writel(0x0000ffff,0xf800444); // trustzone: DMA IRQ Non-Secure 
+	writel(0x0000000f,0xf800448); // trustzone: DMA DMA Periph Non-Secure
+
 	/* call the non-sec switching code on this CPU also */
+	debug("nonsec init");
 	_nonsec_init();
+	debug("nonsec init finished");
 
 	return 0;
 }
diff --git a/arch/arm/cpu/u-boot.lds b/arch/arm/cpu/u-boot.lds
index 87c2de2..bead35f 100644
--- a/arch/arm/cpu/u-boot.lds
+++ b/arch/arm/cpu/u-boot.lds
@@ -101,6 +101,7 @@ SECTIONS
 	.dynstr : { *(.dynstr*) }
 	.dynamic : { *(.dynamic*) }
 	.plt : { *(.plt*) }
+	.rel.plt  : { *(.rel.plt) }
 	.interp : { *(.interp*) }
 	.gnu : { *(.gnu*) }
 	.ARM.exidx : { *(.ARM.exidx*) }
diff --git a/arch/arm/dts/zynq-zed.dts b/arch/arm/dts/zynq-zed.dts
index 91a5deb..0828b46 100644
--- a/arch/arm/dts/zynq-zed.dts
+++ b/arch/arm/dts/zynq-zed.dts
@@ -1,14 +1,335 @@
 /*
- * Xilinx ZED board DTS
+ * Device Tree Generator version: 1.1
  *
- * Copyright (C) 2013 Xilinx, Inc.
+ * (C) Copyright 2007-2013 Xilinx, Inc.
+ * (C) Copyright 2007-2013 Michal Simek
+ * (C) Copyright 2007-2012 PetaLogix Qld Pty Ltd
  *
- * SPDX-License-Identifier:	GPL-2.0+
  */
-/dts-v1/;
-#include "zynq-7000.dtsi"
 
+/dts-v1/;
 / {
-	model = "Zynq ZED Board";
-	compatible = "xlnx,zynq-zed", "xlnx,zynq-7000";
-};
+	#address-cells = <1>;
+	#size-cells = <1>;
+	compatible = "xlnx,zynq-7000";
+	model = "Xilinx Zynq";
+	aliases {
+		ethernet0 = &ps7_ethernet_0;
+		serial0 = &ps7_uart_1;
+		spi0 = &ps7_qspi_0;
+	} ;
+	chosen {
+		/* bootargs = "init=/bin/sh console=ttyPS0,115200 root=/dev/mmcblk0p2 rw earlyprintk"; */
+		bootargs = "console=ttyPS0,115200 root=/dev/nfs nfsroot=172.19.1.44:/home/sander/xilinx/zynq/ptx/platform-zynq/root,rsize=4096,wsize=8192,tcp ip=dhcp rw earlyprintk isolcpus=1";
+		linux,stdout-path = "/amba@0/serial@e0001000";
+	} ;
+	cpus {
+		#address-cells = <1>;
+		#size-cells = <0>;
+		ps7_cortexa9_0: cpu@0 {
+			bus-handle = <&ps7_axi_interconnect_0>;
+			compatible = "arm,cortex-a9";
+			d-cache-line-size = <0x20>;
+			d-cache-size = <0x8000>;
+			device_type = "cpu";
+			i-cache-line-size = <0x20>;
+			i-cache-size = <0x8000>;
+			interrupt-handle = <&ps7_scugic_0>;
+			reg = <0x0>;
+		} ;
+		ps7_cortexa9_1: cpu@1 {
+			bus-handle = <&ps7_axi_interconnect_0>;
+			compatible = "arm,cortex-a9";
+			d-cache-line-size = <0x20>;
+			d-cache-size = <0x8000>;
+			device_type = "cpu";
+			i-cache-line-size = <0x20>;
+			i-cache-size = <0x8000>;
+			interrupt-handle = <&ps7_scugic_0>;
+			reg = <0x1>;
+		} ;
+	} ;
+	pmu {
+		compatible = "arm,cortex-a9-pmu";
+		interrupt-parent = <&ps7_scugic_0>;
+		interrupts = <0 5 4>, <0 6 4>;
+		reg = <0xf8891000 0x1000>, <0xf8893000 0x1000>;
+		reg-names = "cpu0", "cpu1";
+	} ;
+	ps7_ddr_0: memory@0 {
+		device_type = "memory";
+		reg = <0x0 0x20000000>;
+	} ;
+	ps7_axi_interconnect_0: amba@0 {
+		#address-cells = <1>;
+		#size-cells = <1>;
+		compatible = "xlnx,ps7-axi-interconnect-1.00.a", "simple-bus";
+		ranges ;
+		axi_timer_0: timer@42800000 {
+			clock-frequency = <100000000>;
+			compatible = "xlnx,axi-timer-2.0", "xlnx,xps-timer-1.00.a";
+			interrupt-parent = <&ps7_scugic_0>;
+			interrupts = <0 59 4>;
+			reg = <0x42800000 0x10000>;
+			xlnx,count-width = <0x20>;
+			xlnx,gen0-assert = <0x1>;
+			xlnx,gen1-assert = <0x1>;
+			xlnx,one-timer-only = <0x1>;
+			xlnx,trig0-assert = <0x1>;
+			xlnx,trig1-assert = <0x1>;
+		} ;
+		ps7_afi_0: ps7-afi@f8008000 {
+			compatible = "xlnx,ps7-afi-1.00.a";
+			reg = <0xf8008000 0x1000>;
+		} ;
+		ps7_afi_1: ps7-afi@f8009000 {
+			compatible = "xlnx,ps7-afi-1.00.a";
+			reg = <0xf8009000 0x1000>;
+		} ;
+		ps7_afi_2: ps7-afi@f800a000 {
+			compatible = "xlnx,ps7-afi-1.00.a";
+			reg = <0xf800a000 0x1000>;
+		} ;
+		ps7_afi_3: ps7-afi@f800b000 {
+			compatible = "xlnx,ps7-afi-1.00.a";
+			reg = <0xf800b000 0x1000>;
+		} ;
+		ps7_ddrc_0: ps7-ddrc@f8006000 {
+			compatible = "xlnx,ps7-ddrc-1.00.a", "xlnx,ps7-ddrc";
+			reg = <0xf8006000 0x1000>;
+			xlnx,has-ecc = <0x0>;
+		} ;
+		ps7_dev_cfg_0: ps7-dev-cfg@f8007000 {
+			clock-names = "ref_clk", "fclk0", "fclk1", "fclk2", "fclk3";
+			clocks = <&clkc 12>, <&clkc 15>, <&clkc 16>, <&clkc 17>, <&clkc 18>;
+			compatible = "xlnx,ps7-dev-cfg-1.00.a";
+			interrupt-parent = <&ps7_scugic_0>;
+			interrupts = <0 8 4>;
+			reg = <0xf8007000 0x100>;
+		} ;
+		ps7_dma_s: ps7-dma@f8003000 {
+			#dma-cells = <1>;
+			#dma-channels = <8>;
+			#dma-requests = <4>;
+			clock-names = "apb_pclk";
+			clocks = <&clkc 27>;
+			compatible = "xlnx,ps7-dma-1.00.a", "arm,primecell", "arm,pl330";
+			interrupt-names = "abort", "dma0", "dma1", "dma2", "dma3",
+				"dma4", "dma5", "dma6", "dma7";
+			interrupt-parent = <&ps7_scugic_0>;
+			interrupts = <0 13 4>, <0 14 4>, <0 15 4>, <0 16 4>, <0 17 4>, <0 40 4>, <0 41 4>, <0 42 4>, <0 43 4>;
+			reg = <0xf8003000 0x1000>;
+		} ;
+		ps7_ethernet_0: ps7-ethernet@e000b000 {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			clock-names = "ref_clk", "aper_clk";
+			clocks = <&clkc 13>, <&clkc 30>;
+			compatible = "xlnx,ps7-ethernet-1.00.a";
+			interrupt-parent = <&ps7_scugic_0>;
+			interrupts = <0 22 4>;
+			local-mac-address = [00 0a 35 00 00 00];
+			phy-handle = <&phy0>;
+			phy-mode = "rgmii-id";
+			reg = <0xe000b000 0x1000>;
+			xlnx,eth-mode = <0x1>;
+			xlnx,has-mdio = <0x1>;
+			xlnx,ptp-enet-clock = <111111115>;
+			mdio {
+				#address-cells = <1>;
+				#size-cells = <0>;
+				phy0: phy@0 {
+					compatible = "marvell,88e1510";
+					device_type = "ethernet-phy";
+					reg = <0>;
+				} ;
+			} ;
+		} ;
+		ps7_gpio_0: ps7-gpio@e000a000 {
+			#gpio-cells = <2>;
+			clocks = <&clkc 42>;
+			compatible = "xlnx,ps7-gpio-1.00.a";
+			emio-gpio-width = <64>;
+			gpio-controller ;
+			gpio-mask-high = <0xc0000>;
+			gpio-mask-low = <0xfe81>;
+			interrupt-parent = <&ps7_scugic_0>;
+			interrupts = <0 20 4>;
+			reg = <0xe000a000 0x1000>;
+		} ;
+		ps7_iop_bus_config_0: ps7-iop-bus-config@e0200000 {
+			compatible = "xlnx,ps7-iop-bus-config-1.00.a";
+			reg = <0xe0200000 0x1000>;
+		} ;
+		ps7_ocmc_0: ps7-ocmc@f800c000 {
+			compatible = "xlnx,ps7-ocmc-1.00.a", "xlnx,zynq-ocm-1.0", "mmio-sram";
+			interrupt-parent = <&ps7_scugic_0>;
+			interrupts = <0 3 4>;
+			reg = <0xf800c000 0x1000>;
+		} ;
+		ps7_pl310_0: ps7-pl310@f8f02000 {
+			arm,data-latency = <3 2 2>;
+			arm,tag-latency = <2 2 2>;
+			cache-level = <2>;
+			cache-unified ;
+			compatible = "xlnx,ps7-pl310-1.00.a", "arm,pl310-cache";
+			interrupt-parent = <&ps7_scugic_0>;
+			interrupts = <0 2 4>;
+			reg = <0xf8f02000 0x1000>;
+		} ;
+		ps7_qspi_0: ps7-qspi@e000d000 {
+			clock-names = "ref_clk", "aper_clk";
+			clocks = <&clkc 10>, <&clkc 43>;
+			compatible = "xlnx,ps7-qspi-1.00.a";
+			interrupt-parent = <&ps7_scugic_0>;
+			interrupts = <0 19 4>;
+			is-dual = <0>;
+			num-chip-select = <1>;
+			reg = <0xe000d000 0x1000>;
+			xlnx,fb-clk = <0x1>;
+			xlnx,qspi-mode = <0x0>;
+			#address-cells = <1>;
+			#size-cells = <0>;
+			flash@0 {
+				compatible = "n25q128";
+				reg = <0x0>;
+				spi-max-frequency = <50000000>;
+				#address-cells = <1>;
+				#size-cells = <1>;
+				partition@qspi-fsbl-uboot {
+					label = "qspi-fsbl-uboot";
+					reg = <0x0 0x100000>;
+				};
+				partition@qspi-linux {
+					label = "qspi-linux";
+					reg = <0x100000 0x500000>;
+				};
+				partition@qspi-device-tree {
+					label = "qspi-device-tree";
+					reg = <0x600000 0x20000>;
+				};
+				partition@qspi-bitstream {
+					label = "qspi-bitstream";
+					reg = <0x20000 0x400000>;
+				};
+				partition@qspi-rootfs {
+					label = "qspi-rootfs";
+					reg = <0x420000 0x3000000>;
+				};
+			};
+
+		} ;
+		ps7_qspi_linear_0: ps7-qspi-linear@fc000000 {
+			clock-names = "ref_clk", "aper_clk";
+			clocks = <&clkc 10>, <&clkc 43>;
+			compatible = "xlnx,ps7-qspi-linear-1.00.a";
+			reg = <0xfc000000 0x1000000>;
+		} ;
+		ps7_scugic_0: ps7-scugic@f8f01000 {
+			#address-cells = <2>;
+			#interrupt-cells = <3>;
+			#size-cells = <1>;
+			compatible = "xlnx,ps7-scugic-1.00.a", "arm,cortex-a9-gic", "arm,gic";
+			interrupt-controller ;
+			num_cpus = <2>;
+			num_interrupts = <96>;
+			reg = <0xf8f01000 0x1000>, <0xf8f00100 0x100>;
+		} ;
+		ps7_scutimer_0: ps7-scutimer@f8f00600 {
+			clocks = <&clkc 4>;
+			compatible = "xlnx,ps7-scutimer-1.00.a", "arm,cortex-a9-twd-timer";
+			interrupt-parent = <&ps7_scugic_0>;
+			interrupts = <1 13 0x301>;
+			reg = <0xf8f00600 0x20>;
+		} ;
+		ps7_scuwdt_0: ps7-scuwdt@f8f00620 {
+			clocks = <&clkc 4>;
+			compatible = "xlnx,ps7-scuwdt-1.00.a";
+			device_type = "watchdog";
+			interrupt-parent = <&ps7_scugic_0>;
+			interrupts = <1 14 0x301>;
+			reg = <0xf8f00620 0xe0>;
+		} ;
+		ps7_sd_0: ps7-sdio@e0100000 {
+			clock-frequency = <50000000>;
+			clock-names = "clk_xin", "clk_ahb";
+			clocks = <&clkc 21>, <&clkc 32>;
+			compatible = "xlnx,ps7-sdio-1.00.a", "generic-sdhci", "arasan,sdhci-8.9a";
+			interrupt-parent = <&ps7_scugic_0>;
+			interrupts = <0 24 4>;
+			reg = <0xe0100000 0x1000>;
+			xlnx,has-cd = <0x1>;
+			xlnx,has-power = <0x0>;
+			xlnx,has-wp = <0x1>;
+		} ;
+		ps7_slcr_0: ps7-slcr@f8000000 {
+			compatible = "xlnx,ps7-slcr-1.00.a", "xlnx,zynq-slcr";
+			reg = <0xf8000000 0x1000>;
+			clocks {
+				#address-cells = <1>;
+				#size-cells = <0>;
+				clkc: clkc {
+					#clock-cells = <1>;
+					clock-output-names = "armpll", "ddrpll", "iopll", "cpu_6or4x", "cpu_3or2x",
+						"cpu_2x", "cpu_1x", "ddr2x", "ddr3x", "dci",
+						"lqspi", "smc", "pcap", "gem0", "gem1",
+						"fclk0", "fclk1", "fclk2", "fclk3", "can0",
+						"can1", "sdio0", "sdio1", "uart0", "uart1",
+						"spi0", "spi1", "dma", "usb0_aper", "usb1_aper",
+						"gem0_aper", "gem1_aper", "sdio0_aper", "sdio1_aper", "spi0_aper",
+						"spi1_aper", "can0_aper", "can1_aper", "i2c0_aper", "i2c1_aper",
+						"uart0_aper", "uart1_aper", "gpio_aper", "lqspi_aper", "smc_aper",
+						"swdt", "dbg_trc", "dbg_apb";
+					compatible = "xlnx,ps7-clkc";
+					fclk-enable = <0xf>;
+					ps-clk-frequency = <33333333>;
+				} ;
+			} ;
+		} ;
+		ps7_ttc_0: ps7-ttc@f8001000 {
+			clocks = <&clkc 6>;
+			compatible = "xlnx,ps7-ttc-1.00.a", "cdns,ttc";
+			interrupt-names = "ttc0", "ttc1", "ttc2";
+			interrupt-parent = <&ps7_scugic_0>;
+			interrupts = <0 10 4>, <0 11 4>, <0 12 4>;
+			reg = <0xf8001000 0x1000>;
+		} ;
+		ps7_uart_1: serial@e0001000 {
+			clock-names = "ref_clk", "aper_clk";
+			clocks = <&clkc 24>, <&clkc 41>;
+			compatible = "xlnx,ps7-uart-1.00.a", "xlnx,xuartps";
+			current-speed = <115200>;
+			device_type = "serial";
+			interrupt-parent = <&ps7_scugic_0>;
+			interrupts = <0 50 4>;
+			port-number = <0>;
+			reg = <0xe0001000 0x1000>;
+			xlnx,has-modem = <0x0>;
+		} ;
+		ps7_usb_0: ps7-usb@e0002000 {
+			clocks = <&clkc 28>;
+			compatible = "xlnx,ps7-usb-1.00.a";
+			dr_mode = "host";
+			interrupt-parent = <&ps7_scugic_0>;
+			interrupts = <0 21 4>;
+			phy_type = "ulpi";
+			reg = <0xe0002000 0x1000>;
+		} ;
+		ps7_xadc: ps7-xadc@f8007100 {
+			clocks = <&clkc 12>;
+			compatible = "xlnx,ps7-xadc-1.00.a";
+			interrupt-parent = <&ps7_scugic_0>;
+			interrupts = <0 7 4>;
+			reg = <0xf8007100 0x20>;
+		} ;
+		registers_0: registers@43c00000 {
+			compatible = "xlnx,registers-1.0";
+			reg = < 0x43c00000 0x1000 >;
+			xlnx,s00-axi-addr-width = <0x6>;
+			xlnx,s00-axi-data-width = <0x20>;
+			interrupt-parent = <&ps7_scugic_0>;
+			interrupts = <0 59 1>;
+			iram = <&ps7_ocmc_0>;
+		} ;
+	} ;
+} ;
diff --git a/arch/arm/include/asm/armv7.h b/arch/arm/include/asm/armv7.h
index 395444e..847a504 100644
--- a/arch/arm/include/asm/armv7.h
+++ b/arch/arm/include/asm/armv7.h
@@ -9,9 +9,10 @@
 #define ARMV7_H
 
 /* Cortex-A9 revisions */
-#define MIDR_CORTEX_A9_R0P1	0x410FC091
-#define MIDR_CORTEX_A9_R1P2	0x411FC092
-#define MIDR_CORTEX_A9_R1P3	0x411FC093
+#define MIDR_CORTEX_A9_R0P0		0x410FC090
+#define MIDR_CORTEX_A9_R0P1		0x410FC091
+#define MIDR_CORTEX_A9_R1P2		0x411FC092
+#define MIDR_CORTEX_A9_R1P3		0x411FC093
 #define MIDR_CORTEX_A9_R2P10	0x412FC09A
 
 /* Cortex-A15 revisions */
@@ -77,7 +78,6 @@ void v7_outer_cache_flush_range(u32 start, u32 end);
 void v7_outer_cache_inval_range(u32 start, u32 end);
 
 #if defined(CONFIG_ARMV7_NONSEC) || defined(CONFIG_ARMV7_VIRT)
-
 int armv7_switch_nonsec(void);
 int armv7_switch_hyp(void);
 
diff --git a/board/xilinx/zynq/board.c b/board/xilinx/zynq/board.c
index 485a5e4..bdc7976 100644
--- a/board/xilinx/zynq/board.c
+++ b/board/xilinx/zynq/board.c
@@ -160,3 +160,20 @@ int dram_init(void)
 
 	return 0;
 }
+
+#if defined(CONFIG_ARMV7_NONSEC) || defined(CONFIG_ARMV7_VIRT)
+/* Setting the address at which secondary cores start from.
+ * Versatile Express uses one address for all cores, so ignore corenr
+ */
+void smp_set_core_boot_addr(unsigned long addr, int corenr)
+{
+	/* The SYSFLAGS register on VExpress needs to be cleared first
+	 * by writing to the next address, since any writes to the address
+	 * at offset 0 will only be ORed in
+	 */
+	 /*
+	writel(~0, CONFIG_SYSFLAGS_ADDR + 4);
+	writel(addr, CONFIG_SYSFLAGS_ADDR);
+	*/
+}
+#endif
diff --git a/include/configs/zynq-common.h b/include/configs/zynq-common.h
index 731e69b..02208a8 100644
--- a/include/configs/zynq-common.h
+++ b/include/configs/zynq-common.h
@@ -145,9 +145,7 @@
 	"norboot=echo Copying FIT from NOR flash to RAM... && " \
 		"cp.b ${nor_flash_off} ${load_addr} ${fit_size} && " \
 		"bootm ${load_addr}\0" \
-	"sdboot=echo Copying FIT from SD to RAM... && " \
-		"fatload mmc 0 ${load_addr} ${fit_image} && " \
-		"bootm ${load_addr}\0" \
+	"sdboot=mmcinfo;fatload mmc 0 0x79c0 linuximage;fatload mmc 0 0 zynq-zed.dtb; bootm 0x79c0 - 0\0" \
 	"jtagboot=echo TFTPing FIT to RAM... && " \
 		"tftpboot ${load_addr} ${fit_image} && " \
 		"bootm ${load_addr}\0"
diff --git a/include/configs/zynq_zed.h b/include/configs/zynq_zed.h
index 274140c..ca7959c 100644
--- a/include/configs/zynq_zed.h
+++ b/include/configs/zynq_zed.h
@@ -24,4 +24,11 @@
 
 #include <configs/zynq-common.h>
 
+#define CONFIG_SYSFLAGS_ADDR        0x1c010030
+#define CONFIG_SMP_PEN_ADDR CONFIG_SYSFLAGS_ADDR
+#define CONFIG_SYS_CLK_FREQ  33333333
+
+#define CONFIG_ARMV7_NONSEC
+#define CONFIG_SYSFLAGS_ADDR        0x1c010030
+
 #endif /* __CONFIG_ZYNQ_ZED_H */
-- 
1.7.9.5

