diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index 3555b4e..00c893b 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -388,7 +388,11 @@
 	  the data on the host computer connected to the target via
 	  debugging channel (JTAG, SWD). If unsure say N
 
+config ARM_SMCCC
+	bool
+
 config ARM_SECURE_MONITOR
+	select ARM_SMCCC
 	bool
 
 config ARM_PSCI
diff --git a/arch/arm/boards/freescale-mx6sx-sabresdb/board.c b/arch/arm/boards/freescale-mx6sx-sabresdb/board.c
index 028b1dd..0fd9af8 100644
--- a/arch/arm/boards/freescale-mx6sx-sabresdb/board.c
+++ b/arch/arm/boards/freescale-mx6sx-sabresdb/board.c
@@ -161,7 +161,7 @@
 }
 fs_initcall(imx6sx_sdb_setup_pmic_voltages);
 
-int ar8031_phy_fixup(struct phy_device *phydev)
+static int ar8031_phy_fixup(struct phy_device *phydev)
 {
 	/*
 	 * Enable 1.8V(SEL_1P5_1P8_POS_REG) on
diff --git a/arch/arm/boards/nxp-imx8mq-evk/ddr_init.c b/arch/arm/boards/nxp-imx8mq-evk/ddr_init.c
index 81691b2..44103b5 100644
--- a/arch/arm/boards/nxp-imx8mq-evk/ddr_init.c
+++ b/arch/arm/boards/nxp-imx8mq-evk/ddr_init.c
@@ -2,7 +2,8 @@
  * Copyright 2017 NXP
  *
  * SPDX-License-Identifier:	GPL-2.0+
- * Generated code from MX8_DDR_tool
+ *
+ * Generated code from MX8M_DDR_tool
  */
 
 #include "ddr.h"
@@ -21,7 +22,7 @@
 	reg32_write(0x303a00f8,tmp);
 	reg32_write(0x30391000,0x8f000000);
 	reg32_write(0x30391004,0x8f000000);
-	reg32_write(0x30360068,0xbbe580);
+	reg32_write(0x30360068,0xece580);
 	tmp=reg32_read(0x30360060);
 	tmp &= ~0x80;
 	reg32_write(0x30360060,tmp);
@@ -41,18 +42,19 @@
 	reg32_write(0x30391000,0x8f000006);
 	reg32_write(0x3d400304,0x1);
 	reg32_write(0x3d400030,0x1);
-	reg32_write(0x3d400000,0x83080020);
+	reg32_write(0x3d400000,0xa3080020);
+	reg32_write(0x3d400028,0x0);
+	reg32_write(0x3d400020,0x203);
+	reg32_write(0x3d400024,0x186a000);
 	reg32_write(0x3d400064,0x6100e0);
 	reg32_write(0x3d4000d0,0xc003061c);
 	reg32_write(0x3d4000d4,0x9e0000);
 	reg32_write(0x3d4000dc,0xd4002d);
 	reg32_write(0x3d4000e0,0x310008);
-	reg32_write(0x3d4000e8,0x46004d);
-	reg32_write(0x3d4000ec,0x15004d);
-	reg32_write(0x3d4000f4,0x639);
+	reg32_write(0x3d4000e8,0x66004a);
+	reg32_write(0x3d4000ec,0x16004a);
 	reg32_write(0x3d400100,0x1a201b22);
 	reg32_write(0x3d400104,0x60633);
-	reg32_write(0x3d400108,0x70e1214);
 	reg32_write(0x3d40010c,0xc0c000);
 	reg32_write(0x3d400110,0xf04080f);
 	reg32_write(0x3d400114,0x2040c0c);
@@ -64,78 +66,77 @@
 	reg32_write(0x3d400144,0xa00050);
 	reg32_write(0x3d400180,0x3200018);
 	reg32_write(0x3d400184,0x28061a8);
+	reg32_write(0x3d400188,0x0);
 	reg32_write(0x3d400190,0x497820a);
 	reg32_write(0x3d400194,0x80303);
-	reg32_write(0x3d4001b4,0x170a);
-	reg32_write(0x3d4001b0,0x11);
 	reg32_write(0x3d4001a0,0xe0400018);
 	reg32_write(0x3d4001a4,0xdf00e4);
-	reg32_write(0x3d4001a8,0x0);
+	reg32_write(0x3d4001a8,0x80000000);
+	reg32_write(0x3d4001b0,0x11);
+	reg32_write(0x3d4001b4,0x170a);
 	reg32_write(0x3d4001c0,0x1);
 	reg32_write(0x3d4001c4,0x1);
+	reg32_write(0x3d4000f4,0x639);
+	reg32_write(0x3d400108,0x70e1214);
 	reg32_write(0x3d400200,0x15);
 	reg32_write(0x3d40020c,0x0);
 	reg32_write(0x3d400210,0x1f1f);
 	reg32_write(0x3d400204,0x80808);
 	reg32_write(0x3d400214,0x7070707);
 	reg32_write(0x3d400218,0x48080707);
+	reg32_write(0x3d402020,0x1);
+	reg32_write(0x3d402024,0x518b00);
+	reg32_write(0x3d402050,0x20d040);
+	reg32_write(0x3d402064,0x14002f);
+	reg32_write(0x3d4020dc,0x940009);
+	reg32_write(0x3d4020e0,0x310000);
+	reg32_write(0x3d4020e8,0x66004a);
+	reg32_write(0x3d4020ec,0x16004a);
+	reg32_write(0x3d402100,0xb070508);
+	reg32_write(0x3d402104,0x3040b);
+	reg32_write(0x3d402108,0x305090c);
+	reg32_write(0x3d40210c,0x505000);
+	reg32_write(0x3d402110,0x4040204);
+	reg32_write(0x3d402114,0x2030303);
+	reg32_write(0x3d402118,0x1010004);
+	reg32_write(0x3d40211c,0x301);
+	reg32_write(0x3d402130,0x20300);
+	reg32_write(0x3d402134,0xa100002);
+	reg32_write(0x3d402138,0x31);
+	reg32_write(0x3d402144,0x220011);
+	reg32_write(0x3d402180,0xa70006);
+	reg32_write(0x3d402190,0x3858202);
+	reg32_write(0x3d402194,0x80303);
+	reg32_write(0x3d4021b4,0x502);
 	reg32_write(0x3d400244,0x0);
-	reg32_write(0x3d400490,0x1);
-	reg32_write(0x3d400250,0x29001f01);
+	reg32_write(0x3d400250,0x29001505);
 	reg32_write(0x3d400254,0x2c);
-	reg32_write(0x3d400264,0x900093e7);
+	reg32_write(0x3d40025c,0x5900575b);
+	reg32_write(0x3d400264,0x9);
 	reg32_write(0x3d40026c,0x2005574);
-	reg32_write(0x3d400400,0x400);
+	reg32_write(0x3d400300,0x16);
+	reg32_write(0x3d400304,0x0);
+	reg32_write(0x3d40030c,0x0);
+	reg32_write(0x3d400320,0x1);
+	reg32_write(0x3d40036c,0x11);
+	reg32_write(0x3d400400,0x111);
+	reg32_write(0x3d400404,0x10f3);
 	reg32_write(0x3d400408,0x72ff);
-	reg32_write(0x3d400494,0x10e00);
-	reg32_write(0x3d400498,0x620096);
-	reg32_write(0x3d40049c,0x10e00);
-	reg32_write(0x3d4004a0,0x12c);
+	reg32_write(0x3d400490,0x1);
+	reg32_write(0x3d400494,0x1110d00);
+	reg32_write(0x3d400498,0x620790);
+	reg32_write(0x3d40049c,0x100001);
+	reg32_write(0x3d4004a0,0x41f);
 	reg32_write(0x30391000,0x8f000004);
 	reg32_write(0x30391000,0x8f000000);
-	reg32_write(0x3d400304,0x0);
 	reg32_write(0x3d400030,0xa8);
+	do{
+		tmp=reg32_read(0x3d400004);
+		if(tmp&0x223) break;
+	}while(1);
 	reg32_write(0x3d400320,0x0);
 	reg32_write(0x3d000000,0x1);
 	reg32_write(0x3d4001b0,0x10);
-	reg32_write(0x3d402100,0xa040305);
-	reg32_write(0x3d402104,0x30407);
-	reg32_write(0x3d402108,0x203060b);
-	reg32_write(0x3d40210c,0x505000);
-	reg32_write(0x3d402110,0x2040202);
-	reg32_write(0x3d402114,0x2030202);
-	reg32_write(0x3d402118,0x1010004);
-	reg32_write(0x3d40211c,0x301);
-	reg32_write(0x3d402138,0x1d);
-	reg32_write(0x3d402144,0x14000a);
-	reg32_write(0x3d403024,0x30d400);
-	reg32_write(0x3d402050,0x20d040);
-	reg32_write(0x3d402190,0x3818200);
-	reg32_write(0x3d4021b4,0x100);
-	reg32_write(0x3d402064,0xc001c);
-	reg32_write(0x3d4020dc,0x840000);
-	reg32_write(0x3d4020e8,0x46004d);
-	reg32_write(0x3d4020ec,0x15004d);
-	reg32_write(0x3d4020e0,0x310000);
-	reg32_write(0x3d403100,0x6010102);
-	reg32_write(0x3d403104,0x30404);
-	reg32_write(0x3d403108,0x203060b);
-	reg32_write(0x3d40310c,0x505000);
-	reg32_write(0x3d403110,0x2040202);
-	reg32_write(0x3d403114,0x2030202);
-	reg32_write(0x3d403118,0x1010004);
-	reg32_write(0x3d40311c,0x301);
-	reg32_write(0x3d403138,0x8);
-	reg32_write(0x3d403144,0x50003);
-	reg32_write(0x3d403024,0xc3500);
-	reg32_write(0x3d403050,0x20d040);
-	reg32_write(0x3d403190,0x3818200);
-	reg32_write(0x3d4031b4,0x100);
-	reg32_write(0x3d403064,0x30007);
-	reg32_write(0x3d4030dc,0x840000);
-	reg32_write(0x3d4030e8,0x46004d);
-	reg32_write(0x3d4030ec,0x15004d);
-	reg32_write(0x3d4030e0,0x310000);
 	reg32_write(0x3c040280,0x0);
 	reg32_write(0x3c040284,0x1);
 	reg32_write(0x3c040288,0x2);
@@ -218,6 +219,7 @@
 
 	/* enable port 0 */
 	reg32_write(DDRC_PCTRL_0(0), 0x00000001);
-	tmp = reg32_read(DDRC_CRCPARSTAT(0));
-	reg32_write(DDRC_RFSHCTL3(0), 0x00000000);
+	/* enable DDR auto-refresh mode */
+	tmp = reg32_read(DDRC_RFSHCTL3(0)) & ~0x1;
+	reg32_write(DDRC_RFSHCTL3(0), tmp);
 }
\ No newline at end of file
diff --git a/arch/arm/boards/nxp-imx8mq-evk/ddrphy_train.c b/arch/arm/boards/nxp-imx8mq-evk/ddrphy_train.c
index 156d7cf..1b30ff7 100644
--- a/arch/arm/boards/nxp-imx8mq-evk/ddrphy_train.c
+++ b/arch/arm/boards/nxp-imx8mq-evk/ddrphy_train.c
@@ -2,6 +2,8 @@
  * Copyright 2017 NXP
  *
  * SPDX-License-Identifier:	GPL-2.0+
+ *
+ * Generated code from MX8M_DDR_tool
  */
 
 #include "ddr.h"
@@ -32,14 +34,6 @@
 	reg32_write(0x3c44857c,0x1ff);
 	reg32_write(0x3c44c17c,0x1ff);
 	reg32_write(0x3c44c57c,0x1ff);
-	reg32_write(0x3c84017c,0x1ff);
-	reg32_write(0x3c84057c,0x1ff);
-	reg32_write(0x3c84417c,0x1ff);
-	reg32_write(0x3c84457c,0x1ff);
-	reg32_write(0x3c84817c,0x1ff);
-	reg32_write(0x3c84857c,0x1ff);
-	reg32_write(0x3c84c17c,0x1ff);
-	reg32_write(0x3c84c57c,0x1ff);
 	reg32_write(0x3c000154,0x1ff);
 	reg32_write(0x3c004154,0x1ff);
 	reg32_write(0x3c008154,0x1ff);
@@ -52,22 +46,16 @@
 	reg32_write(0x3c024154,0x1ff);
 	reg32_write(0x3c080314,0x19);
 	reg32_write(0x3c480314,0x7);
-	reg32_write(0x3c880314,0x7);
 	reg32_write(0x3c0800b8,0x2);
-	reg32_write(0x3c4800b8,0x2);
-	reg32_write(0x3c8800b8,0x2);
+	reg32_write(0x3c4800b8,0x1);
 	reg32_write(0x3c240810,0x0);
 	reg32_write(0x3c640810,0x0);
-	reg32_write(0x3ca40810,0x0);
 	reg32_write(0x3c080090,0xab);
 	reg32_write(0x3c0800e8,0x0);
 	reg32_write(0x3c480090,0xab);
 	reg32_write(0x3c0800e8,0x0);
-	reg32_write(0x3c880090,0xab);
-	reg32_write(0x3c0800e8,0x0);
-	reg32_write(0x3c080158,0x7);
+	reg32_write(0x3c080158,0x3);
 	reg32_write(0x3c480158,0xa);
-	reg32_write(0x3c880158,0xa);
 	reg32_write(0x3c040134,0xe00);
 	reg32_write(0x3c040534,0xe00);
 	reg32_write(0x3c044134,0xe00);
@@ -84,14 +72,6 @@
 	reg32_write(0x3c448534,0xe00);
 	reg32_write(0x3c44c134,0xe00);
 	reg32_write(0x3c44c534,0xe00);
-	reg32_write(0x3c840134,0xe00);
-	reg32_write(0x3c840534,0xe00);
-	reg32_write(0x3c844134,0xe00);
-	reg32_write(0x3c844534,0xe00);
-	reg32_write(0x3c848134,0xe00);
-	reg32_write(0x3c848534,0xe00);
-	reg32_write(0x3c84c134,0xe00);
-	reg32_write(0x3c84c534,0xe00);
 	reg32_write(0x3c040124,0xfbe);
 	reg32_write(0x3c040524,0xfbe);
 	reg32_write(0x3c044124,0xfbe);
@@ -108,14 +88,6 @@
 	reg32_write(0x3c448524,0xfbe);
 	reg32_write(0x3c44c124,0xfbe);
 	reg32_write(0x3c44c524,0xfbe);
-	reg32_write(0x3c840124,0xfbe);
-	reg32_write(0x3c840524,0xfbe);
-	reg32_write(0x3c844124,0xfbe);
-	reg32_write(0x3c844524,0xfbe);
-	reg32_write(0x3c848124,0xfbe);
-	reg32_write(0x3c848524,0xfbe);
-	reg32_write(0x3c84c124,0xfbe);
-	reg32_write(0x3c84c524,0xfbe);
 	reg32_write(0x3c00010c,0x63);
 	reg32_write(0x3c00410c,0x63);
 	reg32_write(0x3c00810c,0x63);
@@ -130,8 +102,7 @@
 	reg32_write(0x3c0801d4,0x4);
 	reg32_write(0x3c080140,0x0);
 	reg32_write(0x3c080020,0x320);
-	reg32_write(0x3c480020,0x64);
-	reg32_write(0x3c880020,0x19);
+	reg32_write(0x3c480020,0xa7);
 	reg32_write(0x3c080220,0x9);
 	reg32_write(0x3c0802c8,0xdc);
 	reg32_write(0x3c04010c,0x5a1);
@@ -151,33 +122,21 @@
 	reg32_write(0x3c44850c,0x5a1);
 	reg32_write(0x3c44c10c,0x5a1);
 	reg32_write(0x3c44c50c,0x5a1);
-	reg32_write(0x3c8802c8,0xdc);
-	reg32_write(0x3c84010c,0x5a1);
-	reg32_write(0x3c84050c,0x5a1);
-	reg32_write(0x3c84410c,0x5a1);
-	reg32_write(0x3c84450c,0x5a1);
-	reg32_write(0x3c84810c,0x5a1);
-	reg32_write(0x3c84850c,0x5a1);
-	reg32_write(0x3c84c10c,0x5a1);
-	reg32_write(0x3c84c50c,0x5a1);
 	reg32_write(0x3c0803e8,0x1);
 	reg32_write(0x3c4803e8,0x1);
-	reg32_write(0x3c8803e8,0x1);
 	reg32_write(0x3c080064,0x1);
 	reg32_write(0x3c480064,0x1);
-	reg32_write(0x3c880064,0x1);
-	reg32_write(0x3c0803c0,0x660);
+	reg32_write(0x3c0803c0,0x0);
 	reg32_write(0x3c0803c4,0x0);
 	reg32_write(0x3c0803c8,0x4444);
 	reg32_write(0x3c0803cc,0x8888);
-	reg32_write(0x3c0803d0,0x5665);
+	reg32_write(0x3c0803d0,0x5555);
 	reg32_write(0x3c0803d4,0x0);
 	reg32_write(0x3c0803d8,0x0);
 	reg32_write(0x3c0803dc,0xf000);
 	reg32_write(0x3c080094,0x0);
 	reg32_write(0x3c0800b4,0x0);
 	reg32_write(0x3c4800b4,0x0);
-	reg32_write(0x3c8800b4,0x0);
 	reg32_write(0x3c080180,0x2);
 
 	//enable APB bus to access DDRPHY RAM
@@ -195,31 +154,32 @@
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54008,0x131f);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54009,0xc8);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5400b,0x2);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5400d,0x100);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54012,0x310);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54019,0x2dd4);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5401a,0x31);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5401b,0x4d46);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5401c,0x4d08);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5401e,0x15);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5401b,0x4a66);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5401c,0x4a08);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5401e,0x16);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5401f,0x2dd4);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54020,0x31);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54021,0x4d46);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54022,0x4d08);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54024,0x15);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54021,0x4a66);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54022,0x4a08);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54024,0x16);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5402b,0x1000);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5402c,0x3);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54032,0xd400);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54033,0x312d);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54034,0x4600);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54035,0x84d);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54036,0x4d);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54037,0x1500);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54034,0x6600);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54035,0x84a);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54036,0x4a);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54037,0x1600);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54038,0xd400);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54039,0x312d);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5403a,0x4600);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5403b,0x84d);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5403c,0x4d);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5403d,0x1500);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5403a,0x6600);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5403b,0x84a);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5403c,0x4a);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5403d,0x1600);
 
 	//disable APB bus to access DDRPHY RAM
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0xd0000, 0x1);
@@ -233,96 +193,57 @@
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0xd0099, 0x1);
 
 	//set the PHY input clock to the desired frequency for pstate 1
-	reg32_write(0x3038a008,0x7070000);
-	reg32_write(0x3038a004,0x5000000);
 	reg32_write(0x3038a088,0x7070000);
-	reg32_write(0x3038a084,0x2010000);
+	reg32_write(0x3038a084,0x4030000);
 	reg32_write(0x303a00ec,0xffff);
 	tmp=reg32_read(0x303a00f8);
 	tmp |= 0x20;
 	reg32_write(0x303a00f8,tmp);
-	reg32_write(0x30389804,0x1000000);
+	reg32_write(0x30360068,0xf5a406);
+	tmp=reg32_read(0x30360060);
+	tmp &= ~0x80;
+	reg32_write(0x30360060,tmp);
+	tmp=reg32_read(0x30360060);
+	tmp |= 0x200;
+	reg32_write(0x30360060,tmp);
+	tmp=reg32_read(0x30360060);
+	tmp &= ~0x20;
+	reg32_write(0x30360060,tmp);
+	tmp=reg32_read(0x30360060);
+	tmp &= ~0x10;
+	reg32_write(0x30360060,tmp);
+	do{
+		tmp=reg32_read(0x30360060);
+		if(tmp&0x80000000) break;
+	}while(1);
+	reg32_write(0x30389808,0x1000000);
 
 	//enable APB bus to access DDRPHY RAM
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0xd0000, 0x0);
 
-	reg32_write(0x3c150008,0x101);
-	reg32_write(0x3c15000c,0x190);
+	reg32_write(0x3c150008,0x1);
+	reg32_write(0x3c15000c,0x29c);
 	reg32_write(0x3c150020,0x121f);
-	reg32_write(0x3c150064,0x84);
+	reg32_write(0x3c150064,0x994);
 	reg32_write(0x3c150068,0x31);
 	reg32_write(0x3c15006c,0x4d46);
 	reg32_write(0x3c150070,0x4d08);
 	reg32_write(0x3c150074,0x0);
 	reg32_write(0x3c150078,0x15);
-	reg32_write(0x3c15007c,0x84);
+	reg32_write(0x3c15007c,0x994);
 	reg32_write(0x3c150080,0x31);
 	reg32_write(0x3c150084,0x4d46);
 	reg32_write(0x3c150088,0x4d08);
 	reg32_write(0x3c15008c,0x0);
 	reg32_write(0x3c150090,0x15);
-	reg32_write(0x3c1500c8,0x8400);
-	reg32_write(0x3c1500cc,0x3100);
+	reg32_write(0x3c1500c8,0x9400);
+	reg32_write(0x3c1500cc,0x3109);
 	reg32_write(0x3c1500d0,0x4600);
 	reg32_write(0x3c1500d4,0x84d);
 	reg32_write(0x3c1500d8,0x4d);
 	reg32_write(0x3c1500dc,0x1500);
-	reg32_write(0x3c1500e0,0x8400);
-	reg32_write(0x3c1500e4,0x3100);
-	reg32_write(0x3c1500e8,0x4600);
-	reg32_write(0x3c1500ec,0x84d);
-	reg32_write(0x3c1500f0,0x4d);
-	reg32_write(0x3c1500f4,0x1500);
-	reg32_write(0x3c1500f8,0x0);
-
-	//disable APB bus to access DDRPHY RAM
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0xd0000, 0x1);
-	//Reset MPU and run
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0xd0099, 0x9);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0xd0099, 0x1);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0xd0099, 0x0);
-	wait_ddrphy_training_complete();
-
-	//configure DDRPHY-FW DMEM structure @clock2...
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0xd0099, 0x1);
-
-	//set the PHY input clock to the desired frequency for pstate 2
-	reg32_write(0x3038a008,0x7070000);
-	reg32_write(0x3038a004,0x2000000);
-	reg32_write(0x3038a088,0x7070000);
-	reg32_write(0x3038a084,0x2010000);
-	reg32_write(0x303a00ec,0xffff);
-	tmp=reg32_read(0x303a00f8);
-	tmp |= 0x20;
-	reg32_write(0x303a00f8,tmp);
-	reg32_write(0x30389804,0x1000000);
-
-	//enable APB bus to access DDRPHY RAM
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0xd0000, 0x0);
-
-	reg32_write(0x3c150008,0x102);
-	reg32_write(0x3c15000c,0x64);
-	reg32_write(0x3c150020,0x121f);
-	reg32_write(0x3c150064,0x84);
-	reg32_write(0x3c150068,0x31);
-	reg32_write(0x3c15006c,0x4d46);
-	reg32_write(0x3c150070,0x4d08);
-	reg32_write(0x3c150074,0x0);
-	reg32_write(0x3c150078,0x15);
-	reg32_write(0x3c15007c,0x84);
-	reg32_write(0x3c150080,0x31);
-	reg32_write(0x3c150084,0x4d46);
-	reg32_write(0x3c150088,0x4d08);
-	reg32_write(0x3c15008c,0x0);
-	reg32_write(0x3c150090,0x15);
-	reg32_write(0x3c1500c8,0x8400);
-	reg32_write(0x3c1500cc,0x3100);
-	reg32_write(0x3c1500d0,0x4600);
-	reg32_write(0x3c1500d4,0x84d);
-	reg32_write(0x3c1500d8,0x4d);
-	reg32_write(0x3c1500dc,0x1500);
-	reg32_write(0x3c1500e0,0x8400);
-	reg32_write(0x3c1500e4,0x3100);
+	reg32_write(0x3c1500e0,0x9400);
+	reg32_write(0x3c1500e4,0x3109);
 	reg32_write(0x3c1500e8,0x4600);
 	reg32_write(0x3c1500ec,0x84d);
 	reg32_write(0x3c1500f0,0x4d);
@@ -344,7 +265,7 @@
 	tmp=reg32_read(0x303a00f8);
 	tmp |= 0x20;
 	reg32_write(0x303a00f8,tmp);
-	reg32_write(0x30360068,0xbbe580);
+	reg32_write(0x30360068,0xece580);
 	tmp=reg32_read(0x30360060);
 	tmp &= ~0x80;
 	reg32_write(0x30360060,tmp);
@@ -380,28 +301,28 @@
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54012,0x310);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54019,0x2dd4);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5401a,0x31);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5401b,0x4d46);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5401c,0x4d08);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5401e,0x15);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5401b,0x4a66);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5401c,0x4a08);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5401e,0x16);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5401f,0x2dd4);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54020,0x31);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54021,0x4d46);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54022,0x4d08);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54024,0x15);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54021,0x4a66);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54022,0x4a08);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54024,0x16);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5402b,0x1000);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5402c,0x3);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54032,0xd400);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54033,0x312d);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54034,0x4600);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54035,0x84d);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54036,0x4d);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54037,0x1500);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54034,0x6600);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54035,0x84a);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54036,0x4a);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54037,0x1600);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54038,0xd400);
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x54039,0x312d);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5403a,0x4600);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5403b,0x84d);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5403c,0x4d);
-	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5403d,0x1500);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5403a,0x6600);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5403b,0x84a);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5403c,0x4a);
+	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0x5403d,0x1600);
 
 	//disable APB bus to access DDRPHY RAM
 	reg32_write(IP2APB_DDRPHY_IPS_BASE_ADDR(0) + 4 * 0xd0000, 0x1);
@@ -912,10 +833,6 @@
 	reg32_write(0x3c480030,0xc9);
 	reg32_write(0x3c480034,0x7d1);
 	reg32_write(0x3c480038,0x2c);
-	reg32_write(0x3c88002c,0x65);
-	reg32_write(0x3c880030,0xc9);
-	reg32_write(0x3c880034,0x7d1);
-	reg32_write(0x3c880038,0x2c);
 	reg32_write(0x3c240030,0x0);
 	reg32_write(0x3c240034,0x173);
 	reg32_write(0x3c240038,0x60);
@@ -928,8 +845,6 @@
 	reg32_write(0x3c080044,0x3);
 	reg32_write(0x3c480040,0x5a);
 	reg32_write(0x3c480044,0x3);
-	reg32_write(0x3c880040,0x5a);
-	reg32_write(0x3c880044,0x3);
 	reg32_write(0x3c100200,0xe0);
 	reg32_write(0x3c100204,0x12);
 	reg32_write(0x3c100208,0xe0);
@@ -942,12 +857,6 @@
 	reg32_write(0x3c50020c,0x12);
 	reg32_write(0x3c500210,0xe0);
 	reg32_write(0x3c500214,0x12);
-	reg32_write(0x3c900200,0xe0);
-	reg32_write(0x3c900204,0x12);
-	reg32_write(0x3c900208,0xe0);
-	reg32_write(0x3c90020c,0x12);
-	reg32_write(0x3c900210,0xe0);
-	reg32_write(0x3c900214,0x12);
 	reg32_write(0x3c1003f4,0xf);
 	reg32_write(0x3c040044,0x1);
 	reg32_write(0x3c040048,0x1);
diff --git a/arch/arm/boards/nxp-imx8mq-evk/lowlevel.c b/arch/arm/boards/nxp-imx8mq-evk/lowlevel.c
index 1ed918e..438c70c 100644
--- a/arch/arm/boards/nxp-imx8mq-evk/lowlevel.c
+++ b/arch/arm/boards/nxp-imx8mq-evk/lowlevel.c
@@ -23,6 +23,7 @@
 #include <asm/cache.h>
 #include <asm/sections.h>
 #include <asm/mmu.h>
+#include <mach/atf.h>
 
 #include "ddr.h"
 
@@ -65,6 +66,26 @@
 	BUG_ON(ret);
 }
 
+/*
+ * Power-on execution flow of start_nxp_imx8mq_evk() might not be
+ * obvious for a very first read, so here's, hopefully helpful,
+ * summary:
+ *
+ * 1. MaskROM uploads PBL into OCRAM and that's where this function is
+ *    executed for the first time
+ *
+ * 2. DDR is initialized and full i.MX image is loaded to the
+ *    beginning of RAM
+ *
+ * 3. start_nxp_imx8mq_evk, now in RAM, is executed again
+ *
+ * 4. BL31 blob is uploaded to OCRAM and the control is transfer to it
+ *
+ * 5. BL31 exits EL3 into EL2 at address MX8MQ_ATF_BL33_BASE_ADDR,
+ *    executing start_nxp_imx8mq_evk() the third time
+ *
+ * 6. Standard barebox boot flow continues
+ */
 ENTRY_FUNCTION(start_nxp_imx8mq_evk, r0, r1, r2)
 {
 	arm_cpu_lowlevel_init();
@@ -72,9 +93,34 @@
 	if (IS_ENABLED(CONFIG_DEBUG_LL))
 		setup_uart();
 
-	if (get_pc() < MX8MQ_DDR_CSD1_BASE_ADDR)
+	if (get_pc() < MX8MQ_DDR_CSD1_BASE_ADDR) {
+		/*
+		 * We assume that we were just loaded by MaskROM into
+		 * SRAM if we are not running from DDR. We also assume
+		 * that means DDR needs to be initialized for the
+		 * first time.
+		 */
 		nxp_imx8mq_evk_sram_setup();
+	}
+	/*
+	 * Straight from the power-on we are at EL3, so the following
+	 * code _will_ load and jump to ATF.
+	 *
+	 * However when we are re-executed upon exit from ATF's
+	 * initialization routine, it is EL2 which means we'll skip
+	 * loadting ATF blob again
+	 */
+	if (current_el() == 3) {
+		const u8 *bl31;
+		size_t bl31_size;
 
+		get_builtin_firmware(imx_imx8m_bl31_bin, &bl31, &bl31_size);
+		imx8mq_atf_load_bl31(bl31, bl31_size);
+	}
+
+	/*
+	 * Standard entry we hit once we initialized both DDR and ATF
+	 */
 	barebox_arm_entry(MX8MQ_DDR_CSD1_BASE_ADDR,
 			  SZ_2G + SZ_1G, __dtb_imx8mq_evk_start);
 }
diff --git a/arch/arm/boards/zii-vf610-dev/lowlevel.c b/arch/arm/boards/zii-vf610-dev/lowlevel.c
index f3d6750..c771d81 100644
--- a/arch/arm/boards/zii-vf610-dev/lowlevel.c
+++ b/arch/arm/boards/zii-vf610-dev/lowlevel.c
@@ -43,7 +43,7 @@
 	ZII_PLATFORM_VF610_DEV_REV_C	= 0x05,
 };
 
-unsigned int get_system_type(void)
+static unsigned int get_system_type(void)
 {
 #define GPIO_PDIR 0x10
 
@@ -111,7 +111,7 @@
 		if (IS_ENABLED(CONFIG_DEBUG_LL)) {
 			relocate_to_current_adr();
 			setup_c();
-			puts_ll("*********************************\n");
+			puts_ll("\n*********************************\n");
 			puts_ll("* Unknown system type: ");
 			puthex_ll(system_type);
 			puts_ll("\n* Assuming devboard revision B\n");
diff --git a/arch/arm/cpu/Makefile b/arch/arm/cpu/Makefile
index 5b4b832..874d723 100644
--- a/arch/arm/cpu/Makefile
+++ b/arch/arm/cpu/Makefile
@@ -21,8 +21,8 @@
 endif
 
 obj-$(CONFIG_ARM_PSCI) += psci.o
-obj-$(CONFIG_ARM_SECURE_MONITOR) += smccc-call.o
-AFLAGS_smccc-call.o	:=-Wa,-march=armv7-a
+obj-pbl-$(CONFIG_ARM_SMCCC) += smccc-call$(S64).o
+AFLAGS_smccc-call$(S64).o :=-Wa,-march=armv$(if $(S64),8,7)-a
 obj-$(CONFIG_ARM_SECURE_MONITOR) += sm.o sm_as.o
 AFLAGS_sm_as.o		:=-Wa,-march=armv7-a
 
diff --git a/arch/arm/cpu/smccc-call_64.S b/arch/arm/cpu/smccc-call_64.S
new file mode 100644
index 0000000..44888fb
--- /dev/null
+++ b/arch/arm/cpu/smccc-call_64.S
@@ -0,0 +1,52 @@
+/*
+ * Copyright (c) 2015, Linaro Limited
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License Version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/linkage.h>
+#include <linux/arm-smccc.h>
+#include <asm/asm-offsets.h>
+
+	.macro SMCCC instr
+	.cfi_startproc
+	\instr	#0
+	ldr	x4, [sp]
+	stp	x0, x1, [x4, #ARM_SMCCC_RES_X0_OFFS]
+	stp	x2, x3, [x4, #ARM_SMCCC_RES_X2_OFFS]
+	ldr	x4, [sp, #8]
+	cbz	x4, 1f /* no quirk structure */
+	ldr	x9, [x4, #ARM_SMCCC_QUIRK_ID_OFFS]
+	cmp	x9, #ARM_SMCCC_QUIRK_QCOM_A6
+	b.ne	1f
+	str	x6, [x4, ARM_SMCCC_QUIRK_STATE_OFFS]
+1:	ret
+	.cfi_endproc
+	.endm
+
+/*
+ * void arm_smccc_smc(unsigned long a0, unsigned long a1, unsigned long a2,
+ *		  unsigned long a3, unsigned long a4, unsigned long a5,
+ *		  unsigned long a6, unsigned long a7, struct arm_smccc_res *res,
+ *		  struct arm_smccc_quirk *quirk)
+ */
+ENTRY(__arm_smccc_smc)
+	SMCCC	smc
+ENDPROC(__arm_smccc_smc)
+
+/*
+ * void arm_smccc_hvc(unsigned long a0, unsigned long a1, unsigned long a2,
+ *		  unsigned long a3, unsigned long a4, unsigned long a5,
+ *		  unsigned long a6, unsigned long a7, struct arm_smccc_res *res,
+ *		  struct arm_smccc_quirk *quirk)
+ */
+ENTRY(__arm_smccc_hvc)
+	SMCCC	hvc
+ENDPROC(__arm_smccc_hvc)
\ No newline at end of file
diff --git a/arch/arm/include/asm/asm-offsets.h b/arch/arm/include/asm/asm-offsets.h
new file mode 100644
index 0000000..2f84e83
--- /dev/null
+++ b/arch/arm/include/asm/asm-offsets.h
@@ -0,0 +1 @@
+#include <generated/asm-offsets.h>
\ No newline at end of file
diff --git a/arch/arm/include/asm/errata.h b/arch/arm/include/asm/errata.h
index 98137b5..f020369 100644
--- a/arch/arm/include/asm/errata.h
+++ b/arch/arm/include/asm/errata.h
@@ -86,3 +86,12 @@
 		"mcr	p15, 0, r0, c15, c0, 1\n"
 	);
 }
+
+static inline void enable_arm_errata_cortexa8_enable_ibe(void)
+{
+	__asm__ __volatile__ (
+		"mrc	p15, 0, r0, c1, c0, 1\n"
+		"orr	r0, r0, #1 << 6\n"
+		"mcr	p15, 0, r0, c1, c0, 1\n"
+	);
+}
diff --git a/arch/arm/lib/asm-offsets.c b/arch/arm/lib/asm-offsets.c
index 7bf6d12..cdff6f7 100644
--- a/arch/arm/lib/asm-offsets.c
+++ b/arch/arm/lib/asm-offsets.c
@@ -9,8 +9,13 @@
  */
 
 #include <linux/kbuild.h>
+#include <linux/arm-smccc.h>
 
 int main(void)
 {
-	return 0;
+  DEFINE(ARM_SMCCC_RES_X0_OFFS,		offsetof(struct arm_smccc_res, a0));
+  DEFINE(ARM_SMCCC_RES_X2_OFFS,		offsetof(struct arm_smccc_res, a2));
+  DEFINE(ARM_SMCCC_QUIRK_ID_OFFS,	offsetof(struct arm_smccc_quirk, id));
+  DEFINE(ARM_SMCCC_QUIRK_STATE_OFFS,	offsetof(struct arm_smccc_quirk, state));
+  return 0;
 }
diff --git a/arch/arm/mach-imx/Kconfig b/arch/arm/mach-imx/Kconfig
index 73b7ea1..7cb9138 100644
--- a/arch/arm/mach-imx/Kconfig
+++ b/arch/arm/mach-imx/Kconfig
@@ -461,6 +461,8 @@
 	bool "NXP i.MX8MQ EVK Board"
 	select ARCH_IMX8MQ
 	select FIRMWARE_IMX_LPDDR4_PMU_TRAIN
+	select FIRMWARE_IMX8MQ_ATF
+	select ARM_SMCCC
 
 endif
 
diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
index 28fe60d..595a751 100644
--- a/arch/arm/mach-imx/Makefile
+++ b/arch/arm/mach-imx/Makefile
@@ -17,7 +17,7 @@
 obj-$(CONFIG_ARCH_IMX7) += imx7.o
 obj-$(CONFIG_ARCH_VF610) += vf610.o
 obj-$(CONFIG_ARCH_IMX8MQ) += imx8mq.o
-lwl-$(CONFIG_ARCH_IMX8MQ) += imx8-ddrc.o
+lwl-$(CONFIG_ARCH_IMX8MQ) += imx8-ddrc.o atf.o
 obj-$(CONFIG_ARCH_IMX_XLOAD) += xload.o
 obj-$(CONFIG_IMX_IIM)	+= iim.o
 obj-$(CONFIG_NAND_IMX) += nand.o
diff --git a/arch/arm/mach-imx/atf.c b/arch/arm/mach-imx/atf.c
new file mode 100644
index 0000000..c1b358d
--- /dev/null
+++ b/arch/arm/mach-imx/atf.c
@@ -0,0 +1,41 @@
+#include <common.h>
+#include <mach/atf.h>
+
+/**
+ * imx8mq_atf_load_bl31 - Load ATF BL31 blob and transfer control to it
+ *
+ * @fw:		Pointer to the BL31 blob
+ * @fw_size:	Size of the BL31 blob
+ *
+ * This function:
+
+ *     1. Copies built-in BL31 blob to an address i.MX8M's BL31
+ *        expects to be placed
+ *
+ *     2. Sets up temporary stack pointer for EL2, which is execution
+ *        level that BL31 will drop us off at after it completes its
+ *        initialization routine
+ *
+ *     3. Transfers control to BL31
+ *
+ * NOTE: This function expects NXP's implementation of ATF that can be
+ * found at:
+ *     https://source.codeaurora.org/external/imx/imx-atf
+ *
+ * any other implementation may or may not work
+ *
+ */
+void imx8mq_atf_load_bl31(const void *fw, size_t fw_size)
+{
+	void __noreturn (*bl31)(void) = (void *)MX8MQ_ATF_BL31_BASE_ADDR;
+
+	if (WARN_ON(fw_size > MX8MQ_ATF_BL31_SIZE_LIMIT))
+		return;
+
+	memcpy(bl31, fw, fw_size);
+
+	asm volatile("msr sp_el2, %0" : :
+		     "r" (MX8MQ_ATF_BL33_BASE_ADDR - 16) :
+		     "cc");
+	bl31();
+}
\ No newline at end of file
diff --git a/arch/arm/mach-imx/boot.c b/arch/arm/mach-imx/boot.c
index ab25f75..f1fc4047 100644
--- a/arch/arm/mach-imx/boot.c
+++ b/arch/arm/mach-imx/boot.c
@@ -28,6 +28,7 @@
 #include <mach/imx6-regs.h>
 #include <mach/imx7-regs.h>
 #include <mach/vf610-regs.h>
+#include <mach/imx8mq.h>
 
 
 static void
@@ -424,32 +425,12 @@
 	imx_boot_save_loc(imx6_get_boot_source);
 }
 
-#define IMX7_SRC_SBMR1	0x58
-#define IMX7_SRC_SBMR2	0x70
+#define IMX7_BOOT_SW_INFO_POINTER_ADDR		0x000001E8
+#define IMX8M_BOOT_SW_INFO_POINTER_ADDR_A0	0x000009e8
+#define IMX8M_BOOT_SW_INFO_POINTER_ADDR_B0	0x00000968
 
-/*
- * Re-defined to match the naming in reference manual
- */
-#define BOOT_CFG(m, l)	BOOT_CFG1(m, l)
-
-#define IMX_BOOT_SW_INFO_POINTER_ADDR	0x000001E8
 #define IMX_BOOT_SW_INFO_BDT_SD		0x1
 
-static unsigned int imx7_bootsource_internal(uint32_t r)
-{
-	return FIELD_GET(BOOT_CFG(15, 12), r);
-}
-
-static int imx7_boot_instance_spi_nor(uint32_t r)
-{
-	return FIELD_GET(BOOT_CFG(11, 9), r);
-}
-
-static int imx7_boot_instance_mmc(uint32_t r)
-{
-	return FIELD_GET(BOOT_CFG(11, 10), r);
-}
-
 struct imx_boot_sw_info {
 	uint8_t  reserved_1;
 	uint8_t  boot_device_instance;
@@ -460,60 +441,26 @@
 	uint32_t reserved_3[3];
 } __packed;
 
-void imx7_get_boot_source(enum bootsource *src, int *instance)
+static void __imx7_get_boot_source(enum bootsource *src, int *instance,
+				   unsigned long boot_sw_info_pointer_addr)
 {
-	void __iomem *src_base = IOMEM(MX7_SRC_BASE_ADDR);
-	uint32_t sbmr1 = readl(src_base + IMX7_SRC_SBMR1);
-	uint32_t sbmr2 = readl(src_base + IMX7_SRC_SBMR2);
+	const struct imx_boot_sw_info *info;
 
-	if (imx6_bootsource_reserved(sbmr2))
-		return;
+	info = (const void *)(unsigned long)
+		readl(boot_sw_info_pointer_addr);
 
-	if (imx6_bootsource_serial(sbmr2)) {
-		/*
-		 * On i.MX7 ROM code will try to bood from uSDHC1
-		 * before entering serial mode. It doesn't seem to be
-		 * reflected in the contents of SBMR1 in any way when
-		 * that happens, so we check "Boot_SW_Info" structure
-		 * (per 6.6.14 Boot information for software) to see
-		 * if that really happened.
-		 *
-		 * FIXME: This behaviour can be inhibited by
-		 * DISABLE_SDMMC_MFG, but location of that fuse
-		 * doesn't seem to be documented anywhere. Once that
-		 * is known it should be taken into account here.
-		 */
-		const struct imx_boot_sw_info *info;
-
-		info = (const void *)(unsigned long)
-			readl(IMX_BOOT_SW_INFO_POINTER_ADDR);
-
-		if (info->boot_device_type == IMX_BOOT_SW_INFO_BDT_SD) {
-			*src = BOOTSOURCE_MMC;
-			/*
-			 * We are expecting to only ever boot from
-			 * uSDHC1 here
-			 */
-			WARN_ON(*instance = info->boot_device_instance);
-			return;
-		}
-
-		*src = BOOTSOURCE_SERIAL;
-		return;
-	}
-
-	switch (imx7_bootsource_internal(sbmr1)) {
+	switch (info->boot_device_type) {
 	case 1:
 	case 2:
 		*src = BOOTSOURCE_MMC;
-		*instance = imx7_boot_instance_mmc(sbmr1);
+		*instance = info->boot_device_instance;
 		break;
 	case 3:
 		*src = BOOTSOURCE_NAND;
 		break;
 	case 6:
-		*src = BOOTSOURCE_SPI_NOR,
-		*instance = imx7_boot_instance_spi_nor(sbmr1);
+		*src = BOOTSOURCE_SPI_NOR;
+		*instance = info->boot_device_instance;
 		break;
 	case 4:
 		*src = BOOTSOURCE_SPI; /* Really: qspi */
@@ -526,6 +473,11 @@
 	}
 }
 
+void imx7_get_boot_source(enum bootsource *src, int *instance)
+{
+	__imx7_get_boot_source(src, instance, IMX7_BOOT_SW_INFO_POINTER_ADDR);
+}
+
 void imx7_boot_save_loc(void)
 {
 	imx_boot_save_loc(imx7_get_boot_source);
@@ -626,6 +578,17 @@
 }
 
 void imx8_get_boot_source(enum bootsource *src, int *instance)
-	__alias(imx7_get_boot_source);
+{
+	unsigned long addr;
 
-void imx8_boot_save_loc(void) __alias(imx7_boot_save_loc);
+	addr = (imx8mq_cpu_revision() == IMX_CHIP_REV_1_0) ?
+		IMX8M_BOOT_SW_INFO_POINTER_ADDR_A0 :
+		IMX8M_BOOT_SW_INFO_POINTER_ADDR_B0;
+
+	__imx7_get_boot_source(src, instance, addr);
+}
+
+void imx8_boot_save_loc(void)
+{
+	imx_boot_save_loc(imx8_get_boot_source);
+}
diff --git a/arch/arm/mach-imx/cpu_init.c b/arch/arm/mach-imx/cpu_init.c
index 5b93d12..dfbd5ba 100644
--- a/arch/arm/mach-imx/cpu_init.c
+++ b/arch/arm/mach-imx/cpu_init.c
@@ -22,6 +22,7 @@
 	arm_cpu_lowlevel_init();
 
 	enable_arm_errata_709718_war();
+	enable_arm_errata_cortexa8_enable_ibe();
 }
 
 void imx6_cpu_lowlevel_init(void)
@@ -51,4 +52,4 @@
 {
 	arm_cpu_lowlevel_init();
 }
-#endif
\ No newline at end of file
+#endif
diff --git a/arch/arm/mach-imx/imx.c b/arch/arm/mach-imx/imx.c
index 6fe53f3..ad22766 100644
--- a/arch/arm/mach-imx/imx.c
+++ b/arch/arm/mach-imx/imx.c
@@ -157,6 +157,17 @@
 }
 postcore_initcall(imx_init);
 
+const struct imx_reset_reason imx7_reset_reasons[] = {
+	{ IMX_SRC_SRSR_IPP_RESET,       RESET_POR,   0 },
+	{ IMX_SRC_SRSR_WDOG1_RESET,     RESET_WDG,   0 },
+	{ IMX_SRC_SRSR_JTAG_RESET,      RESET_JTAG,  0 },
+	{ IMX_SRC_SRSR_JTAG_SW_RESET,   RESET_JTAG,  0 },
+	{ IMX_SRC_SRSR_WDOG3_RESET,     RESET_WDG,   1 },
+	{ IMX_SRC_SRSR_WDOG4_RESET,     RESET_WDG,   2 },
+	{ IMX_SRC_SRSR_TEMPSENSE_RESET, RESET_THERM, 0 },
+	{ /* sentinel */ }
+};
+
 const struct imx_reset_reason imx_reset_reasons[] = {
 	{ IMX_SRC_SRSR_IPP_RESET,     RESET_POR,  0 },
 	{ IMX_SRC_SRSR_WDOG1_RESET,   RESET_WDG,  0 },
diff --git a/arch/arm/mach-imx/imx7.c b/arch/arm/mach-imx/imx7.c
index e49baf6..ca11e83 100644
--- a/arch/arm/mach-imx/imx7.c
+++ b/arch/arm/mach-imx/imx7.c
@@ -168,17 +168,6 @@
 	.cpu_off = imx7_cpu_off,
 };
 
-static const struct imx_reset_reason imx7_reset_reasons[] = {
-	{ IMX_SRC_SRSR_IPP_RESET,       RESET_POR,   0 },
-	{ IMX_SRC_SRSR_WDOG1_RESET,     RESET_WDG,   0 },
-	{ IMX_SRC_SRSR_JTAG_RESET,      RESET_JTAG,  0 },
-	{ IMX_SRC_SRSR_JTAG_SW_RESET,   RESET_JTAG,  0 },
-	{ IMX_SRC_SRSR_WDOG3_RESET,     RESET_WDG,   1 },
-	{ IMX_SRC_SRSR_WDOG4_RESET,     RESET_WDG,   2 },
-	{ IMX_SRC_SRSR_TEMPSENSE_RESET, RESET_THERM, 0 },
-	{ /* sentinel */ }
-};
-
 int imx7_init(void)
 {
 	const char *cputypestr;
diff --git a/arch/arm/mach-imx/imx8mq.c b/arch/arm/mach-imx/imx8mq.c
index f3246e4..4d00da5 100644
--- a/arch/arm/mach-imx/imx8mq.c
+++ b/arch/arm/mach-imx/imx8mq.c
@@ -18,56 +18,24 @@
 #include <asm/system.h>
 #include <mach/generic.h>
 #include <mach/revision.h>
-#include <mach/imx8mq-regs.h>
+#include <mach/imx8mq.h>
+#include <mach/reset-reason.h>
 
-#define IMX8MQ_ROM_VERSION_A0	0x800
-#define IMX8MQ_ROM_VERSION_B0	0x83C
+#include <linux/arm-smccc.h>
 
-#define MX8MQ_ANATOP_DIGPROG	0x6c
-
-static void imx8mq_silicon_revision(void)
-{
-	void __iomem *anatop = IOMEM(MX8MQ_ANATOP_BASE_ADDR);
-	uint32_t reg = readl(anatop + MX8MQ_ANATOP_DIGPROG);
-	uint32_t type = (reg >> 16) & 0xff;
-	uint32_t rom_version;
-	const char *cputypestr;
-
-	reg &= 0xff;
-
-	if (reg == IMX_CHIP_REV_1_0) {
-		/*
-		 * For B0 chip, the DIGPROG is not updated, still TO1.0.
-		 * we have to check ROM version further
-		 */
-		rom_version = readl(IOMEM(IMX8MQ_ROM_VERSION_A0));
-		if (rom_version != IMX_CHIP_REV_1_0) {
-			rom_version = readl(IOMEM(IMX8MQ_ROM_VERSION_B0));
-			if (rom_version >= IMX_CHIP_REV_2_0)
-				reg = IMX_CHIP_REV_2_0;
-		}
-	}
-
-	switch (type) {
-	case 0x82:
-		cputypestr = "i.MX8MQ";
-		break;
-	default:
-		cputypestr = "unknown i.MX8M";
-		break;
-	};
-
-	imx_set_silicon_revision(cputypestr, reg);
-}
+#define FSL_SIP_BUILDINFO			0xC2000003
+#define FSL_SIP_BUILDINFO_GET_COMMITHASH	0x00
 
 static int imx8mq_init_syscnt_frequency(void)
 {
-	void __iomem *syscnt = IOMEM(MX8MQ_SYSCNT_CTRL_BASE_ADDR);
-	/*
-	 * Update with accurate clock frequency
-	 */
-	set_cntfrq(syscnt_get_cntfrq(syscnt));
-	syscnt_enable(syscnt);
+	if (current_el() == 3) {
+		void __iomem *syscnt = IOMEM(MX8MQ_SYSCNT_CTRL_BASE_ADDR);
+		/*
+		 * Update with accurate clock frequency
+		 */
+		set_cntfrq(syscnt_get_cntfrq(syscnt));
+		syscnt_enable(syscnt);
+	}
 
 	return 0;
 }
@@ -79,7 +47,37 @@
 
 int imx8mq_init(void)
 {
-	imx8mq_silicon_revision();
+	void __iomem *anatop = IOMEM(MX8MQ_ANATOP_BASE_ADDR);
+	void __iomem *src = IOMEM(MX8MQ_SRC_BASE_ADDR);
+	uint32_t type = FIELD_GET(DIGPROG_MAJOR,
+				  readl(anatop + MX8MQ_ANATOP_DIGPROG));
+	struct arm_smccc_res res;
+	const char *cputypestr;
+
+	imx8_boot_save_loc();
+
+	switch (type) {
+	case IMX8M_CPUTYPE_IMX8MQ:
+		cputypestr = "i.MX8MQ";
+		break;
+	default:
+		cputypestr = "unknown i.MX8M";
+		break;
+	};
+
+	imx_set_silicon_revision(cputypestr, imx8mq_cpu_revision());
+	/*
+	 * Reset reasons seem to be identical to that of i.MX7
+	 */
+	imx_set_reset_reason(src + IMX7_SRC_SRSR, imx7_reset_reasons);
+
+	if (IS_ENABLED(CONFIG_ARM_SMCCC) &&
+	    IS_ENABLED(CONFIG_FIRMWARE_IMX8MQ_ATF)) {
+		arm_smccc_smc(FSL_SIP_BUILDINFO,
+			      FSL_SIP_BUILDINFO_GET_COMMITHASH,
+			      0, 0, 0, 0, 0, 0, &res);
+		pr_info("i.MX ARM Trusted Firmware: %s\n", (char *)&res.a0);
+	}
 
 	return 0;
 }
diff --git a/arch/arm/mach-imx/include/mach/atf.h b/arch/arm/mach-imx/include/mach/atf.h
new file mode 100644
index 0000000..aeb24ba
--- /dev/null
+++ b/arch/arm/mach-imx/include/mach/atf.h
@@ -0,0 +1,13 @@
+#ifndef __IMX_ATF_H__
+#define __IMX_ATF_H__
+
+#include <linux/sizes.h>
+#include <asm/system.h>
+
+#define MX8MQ_ATF_BL31_SIZE_LIMIT	SZ_64K
+#define MX8MQ_ATF_BL31_BASE_ADDR	0x00910000
+#define MX8MQ_ATF_BL33_BASE_ADDR	0x40200000
+
+void imx8mq_atf_load_bl31(const void *fw, size_t fw_size);
+
+#endif
\ No newline at end of file
diff --git a/arch/arm/mach-imx/include/mach/imx-header.h b/arch/arm/mach-imx/include/mach/imx-header.h
new file mode 100644
index 0000000..c9b2a58
--- /dev/null
+++ b/arch/arm/mach-imx/include/mach/imx-header.h
@@ -0,0 +1,128 @@
+#ifndef __IMX_HEADER_H__
+#define __IMX_HEADER_H__
+
+#include <linux/types.h>
+
+#define HEADER_LEN 0x1000	/* length of the blank area + IVT + DCD */
+
+/*
+ * ============================================================================
+ * i.MX flash header v1 handling. Found on i.MX35 and i.MX51
+ * ============================================================================
+ */
+#define DCD_BARKER       0xb17219e9
+
+struct imx_flash_header {
+	uint32_t app_code_jump_vector;
+	uint32_t app_code_barker;
+	uint32_t app_code_csf;
+	uint32_t dcd_ptr_ptr;
+	uint32_t super_root_key;
+	uint32_t dcd;
+	uint32_t app_dest;
+	uint32_t dcd_barker;
+	uint32_t dcd_block_len;
+} __attribute__((packed));
+
+struct imx_boot_data {
+	uint32_t start;
+	uint32_t size;
+	uint32_t plugin;
+} __attribute__((packed));
+
+struct imx_dcd_rec_v1 {
+	uint32_t type;
+	uint32_t addr;
+	uint32_t val;
+} __attribute__((packed));
+
+#define TAG_IVT_HEADER	0xd1
+#define IVT_VERSION	0x40
+#define TAG_DCD_HEADER	0xd2
+#define DCD_VERSION	0x40
+#define TAG_UNLOCK	0xb2
+#define TAG_NOP		0xc0
+#define TAG_WRITE	0xcc
+#define TAG_CHECK	0xcf
+#define PARAMETER_FLAG_MASK	(1 << 3)
+#define PARAMETER_FLAG_SET	(1 << 4)
+
+struct imx_ivt_header {
+	uint8_t tag;
+	uint16_t length;
+	uint8_t version;
+} __attribute__((packed));
+
+struct imx_flash_header_v2 {
+	struct imx_ivt_header header;
+
+	uint32_t entry;
+	uint32_t reserved1;
+	uint32_t dcd_ptr;
+	uint32_t boot_data_ptr;
+	uint32_t self;
+	uint32_t csf;
+	uint32_t reserved2;
+
+	struct imx_boot_data boot_data;
+	struct imx_ivt_header dcd_header;
+} __attribute__((packed));
+
+static inline bool is_imx_flash_header_v2(const void *blob)
+{
+	const struct imx_flash_header_v2 *hdr = blob;
+
+	return  hdr->header.tag == TAG_IVT_HEADER &&
+		hdr->header.version >= IVT_VERSION;
+}
+
+struct config_data {
+	uint32_t image_load_addr;
+	uint32_t image_dcd_offset;
+	uint32_t image_size;
+	uint32_t load_size;
+	char *outfile;
+	char *srkfile;
+	int header_version;
+	off_t header_gap;
+	uint32_t first_opcode;
+	int cpu_type;
+	int (*check)(const struct config_data *data, uint32_t cmd,
+		     uint32_t addr, uint32_t mask);
+	int (*write_mem)(const struct config_data *data, uint32_t addr,
+			 uint32_t val, int width, int set_bits, int clear_bits);
+	int (*nop)(const struct config_data *data);
+	int csf_space;
+	char *csf;
+};
+
+#define MAX_RECORDS_DCD_V2 1024
+struct imx_dcd_v2_write_rec {
+	uint32_t addr;
+	uint32_t val;
+} __attribute__((packed));
+
+struct imx_dcd_v2_write {
+	uint8_t tag;
+	uint16_t length;
+	uint8_t param;
+	struct imx_dcd_v2_write_rec data[MAX_RECORDS_DCD_V2];
+} __attribute__((packed));
+
+struct imx_dcd_v2_check {
+	uint8_t tag;
+	uint16_t length;
+	uint8_t param;
+	uint32_t addr;
+	uint32_t mask;
+	uint32_t count;
+} __attribute__((packed));
+
+enum imx_dcd_v2_check_cond {
+	until_all_bits_clear = 0, /* until ((*address & mask) == 0) { ...} */
+	until_any_bit_clear = 1, /* until ((*address & mask) != mask) { ...} */
+	until_all_bits_set = 2, /* until ((*address & mask) == mask) { ...} */
+	until_any_bit_set = 3, /* until ((*address & mask) != 0) { ...} */
+} __attribute__((packed));
+
+#endif
diff --git a/arch/arm/mach-imx/include/mach/imx8mq.h b/arch/arm/mach-imx/include/mach/imx8mq.h
new file mode 100644
index 0000000..f4a537d
--- /dev/null
+++ b/arch/arm/mach-imx/include/mach/imx8mq.h
@@ -0,0 +1,43 @@
+#ifndef __MACH_IMX8MQ_H
+#define __MACH_IMX8MQ_H
+
+#include <io.h>
+#include <mach/generic.h>
+#include <mach/imx8mq-regs.h>
+#include <mach/revision.h>
+#include <linux/bitfield.h>
+
+#define IMX8MQ_ROM_VERSION_A0	0x800
+#define IMX8MQ_ROM_VERSION_B0	0x83C
+
+#define MX8MQ_ANATOP_DIGPROG	0x6c
+
+#define DIGPROG_MAJOR	GENMASK(23, 8)
+#define DIGPROG_MINOR	GENMASK(7, 0)
+
+#define IMX8M_CPUTYPE_IMX8MQ	0x8240
+
+static inline int imx8mq_cpu_revision(void)
+{
+	void __iomem *anatop = IOMEM(MX8MQ_ANATOP_BASE_ADDR);
+	uint32_t revision = FIELD_GET(DIGPROG_MINOR,
+				      readl(anatop + MX8MQ_ANATOP_DIGPROG));
+
+	if (revision == IMX_CHIP_REV_1_0) {
+		uint32_t rom_version;
+		/*
+		 * For B0 chip, the DIGPROG is not updated, still TO1.0.
+		 * we have to check ROM version further
+		 */
+		rom_version = readl(IOMEM(IMX8MQ_ROM_VERSION_A0));
+		if (rom_version != IMX_CHIP_REV_1_0) {
+			rom_version = readl(IOMEM(IMX8MQ_ROM_VERSION_B0));
+			if (rom_version >= IMX_CHIP_REV_2_0)
+				revision = IMX_CHIP_REV_2_0;
+		}
+	}
+
+	return revision;
+}
+
+#endif /* __MACH_IMX8_H */
\ No newline at end of file
diff --git a/arch/arm/mach-imx/include/mach/ocotp.h b/arch/arm/mach-imx/include/mach/ocotp.h
index 76a6da5..e758238 100644
--- a/arch/arm/mach-imx/include/mach/ocotp.h
+++ b/arch/arm/mach-imx/include/mach/ocotp.h
@@ -2,6 +2,7 @@
 #define __MACH_IMX_OCOTP_H
 
 #include <linux/bitfield.h>
+#include <linux/types.h>
 
 #define OCOTP_SHADOW_OFFSET		0x400
 #define OCOTP_SHADOW_SPACING		0x10
diff --git a/arch/arm/mach-imx/include/mach/reset-reason.h b/arch/arm/mach-imx/include/mach/reset-reason.h
index 0f644a8..91a8171 100644
--- a/arch/arm/mach-imx/include/mach/reset-reason.h
+++ b/arch/arm/mach-imx/include/mach/reset-reason.h
@@ -33,5 +33,6 @@
 void imx_set_reset_reason(void __iomem *, const struct imx_reset_reason *);
 
 extern const struct imx_reset_reason imx_reset_reasons[];
+extern const struct imx_reset_reason imx7_reset_reasons[];
 
 #endif /* __MACH_RESET_REASON_H__ */
diff --git a/arch/arm/mach-imx/xload-esdhc.c b/arch/arm/mach-imx/xload-esdhc.c
index 08ba9b0..55d6c69 100644
--- a/arch/arm/mach-imx/xload-esdhc.c
+++ b/arch/arm/mach-imx/xload-esdhc.c
@@ -14,10 +14,12 @@
 #include <common.h>
 #include <io.h>
 #include <mci.h>
+#include <mach/atf.h>
 #include <mach/imx6-regs.h>
 #include <mach/imx8mq-regs.h>
 #include <mach/xload.h>
 #include <linux/sizes.h>
+#include <mach/imx-header.h>
 #include "../../../drivers/mci/sdhci.h"
 #include "../../../drivers/mci/imx-esdhc.h"
 
@@ -218,10 +220,11 @@
 }
 
 static int
-esdhc_start_image(struct esdhc *esdhc, ptrdiff_t address, u32 offset)
+esdhc_start_image(struct esdhc *esdhc, ptrdiff_t address, ptrdiff_t entry, u32 offset)
 {
+
 	void *buf = (void *)address;
-	u32 *ivt = buf + offset + SZ_1K;
+	struct imx_flash_header_v2 *hdr = buf + offset + SZ_1K;
 	int ret, len;
 	void __noreturn (*bb)(void);
 	unsigned int ofs;
@@ -233,14 +236,45 @@
 	if (ret)
 		return ret;
 
-	if (*(u32 *)(ivt) != 0x402000d1) {
-		pr_debug("IVT header not found on SD card. Found 0x%08x instead of 0x402000d1\n",
-				*ivt);
+	if (!is_imx_flash_header_v2(hdr)) {
+		pr_debug("IVT header not found on SD card. "
+			 "Found tag: 0x%02x length: 0x%04x version: %02x\n",
+			 hdr->header.tag, hdr->header.length,
+			 hdr->header.version);
 		return -EINVAL;
 	}
 
 	pr_debug("Check ok, loading image\n");
 
+	ofs = offset + hdr->entry - hdr->boot_data.start;
+
+	if (entry != address) {
+		/*
+		 * Passing entry different from address is interpreted
+		 * as a request to place the image such that its entry
+		 * point would be exactly at 'entry', that is:
+		 *
+		 *     buf + ofs = entry
+		 *
+		 * solving the above for 'buf' gvies us the
+		 * adjustement that needs to be made:
+		 *
+		 *     buf = entry - ofs
+		 *
+		 */
+		if (WARN_ON(entry - ofs < address)) {
+			/*
+			 * We want to make sure we won't try to place
+			 * the start of the image before the beginning
+			 * of the memory buffer we were given in
+			 * address.
+			 */
+			return -EINVAL;
+		}
+
+		buf = (void *)(entry - ofs);
+	}
+
 	ret = esdhc_read_blocks(esdhc, buf, offset + len);
 	if (ret) {
 		pr_err("Loading image failed with %d\n", ret);
@@ -249,8 +283,6 @@
 
 	pr_debug("Image loaded successfully\n");
 
-	ofs = offset + *(ivt + 1) - *(ivt + 8);
-
 	bb = buf + ofs;
 
 	bb();
@@ -291,7 +323,7 @@
 
 	esdhc.is_mx6 = 1;
 
-	return esdhc_start_image(&esdhc, 0x10000000, 0);
+	return esdhc_start_image(&esdhc, 0x10000000, 0x10000000, 0);
 }
 
 /**
@@ -323,5 +355,6 @@
 
 	esdhc.is_mx6 = 1;
 
-	return esdhc_start_image(&esdhc, MX8MQ_DDR_CSD1_BASE_ADDR, SZ_32K);
+	return esdhc_start_image(&esdhc, MX8MQ_DDR_CSD1_BASE_ADDR,
+				 MX8MQ_ATF_BL33_BASE_ADDR, SZ_32K);
 }
\ No newline at end of file
diff --git a/drivers/i2c/busses/i2c-imx.c b/drivers/i2c/busses/i2c-imx.c
index 4ff4f5c..67937da 100644
--- a/drivers/i2c/busses/i2c-imx.c
+++ b/drivers/i2c/busses/i2c-imx.c
@@ -200,13 +200,13 @@
 	reg_cr = fsl_i2c_read_reg(i2c_fsl, FSL_I2C_I2CR);
 	reg_sr = fsl_i2c_read_reg(i2c_fsl, FSL_I2C_I2SR);
 
-	dev_dbg(adapter->dev, "CONTROL:\t"
+	dev_dbg(&adapter->dev, "CONTROL:\t"
 		"IEN =%d, IIEN=%d, MSTA=%d, MTX =%d, TXAK=%d, RSTA=%d\n",
 		(reg_cr & I2CR_IEN  ? 1 : 0), (reg_cr & I2CR_IIEN ? 1 : 0),
 		(reg_cr & I2CR_MSTA ? 1 : 0), (reg_cr & I2CR_MTX  ? 1 : 0),
 		(reg_cr & I2CR_TXAK ? 1 : 0), (reg_cr & I2CR_RSTA ? 1 : 0));
 
-	dev_dbg(adapter->dev, "STATUS:\t"
+	dev_dbg(&adapter->dev, "STATUS:\t"
 		"ICF =%d, IAAS=%d, IB  =%d, IAL =%d, SRW =%d, IIF =%d, RXAK=%d\n",
 		(reg_sr & I2SR_ICF  ? 1 : 0), (reg_sr & I2SR_IAAS ? 1 : 0),
 		(reg_sr & I2SR_IBB  ? 1 : 0), (reg_sr & I2SR_IAL  ? 1 : 0),
diff --git a/drivers/mci/imx-esdhc.c b/drivers/mci/imx-esdhc.c
index c4daa9d..db96a81 100644
--- a/drivers/mci/imx-esdhc.c
+++ b/drivers/mci/imx-esdhc.c
@@ -455,10 +455,13 @@
 	wait_on_timeout(10 * MSECOND,
 			esdhc_read32(regs + SDHCI_PRESENT_STATE) & PRSSTAT_SDSTB);
 
-	clk = SYSCTL_PEREN | SYSCTL_CKEN;
+	clk = SYSCTL_PEREN | SYSCTL_CKEN | SYSCTL_INITA;
 
 	esdhc_setbits32(regs + SDHCI_CLOCK_CONTROL__TIMEOUT_CONTROL__SOFTWARE_RESET,
 			clk);
+
+	wait_on_timeout(1 * MSECOND,
+			!(esdhc_read32(regs + SDHCI_CLOCK_CONTROL) & SYSCTL_INITA));
 }
 
 static void esdhc_set_ios(struct mci_host *mci, struct mci_ios *ios)
diff --git a/drivers/pinctrl/imx-iomux-v3.c b/drivers/pinctrl/imx-iomux-v3.c
index 3845187..d176199 100644
--- a/drivers/pinctrl/imx-iomux-v3.c
+++ b/drivers/pinctrl/imx-iomux-v3.c
@@ -117,6 +117,9 @@
 		if (of_get_property(np, "input-schmitt-enable", NULL))
 			share_conf_val |= SHARE_CONF_PAD_CTL_HYS;
 
+		if (of_get_property(np, "input-enable", NULL))
+			share_conf_val |= IMX_PAD_SION;
+
 		if (of_get_property(np, "bias-pull-up", NULL))
 			share_conf_val |= SHARE_CONF_PAD_CTL_PUE;
 	} else {
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index 8494574..fed628c 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -38,7 +38,7 @@
 
 config DRIVER_SPI_IMX_2_3
 	bool
-	depends on ARCH_IMX50 || ARCH_IMX51 || ARCH_IMX53 || ARCH_IMX6 || ARCH_IMX7
+	depends on ARCH_IMX50 || ARCH_IMX51 || ARCH_IMX53 || ARCH_IMX6 || ARCH_IMX7 || ARCH_IMX8MQ
 	default y
 
 config DRIVER_SPI_MXS
diff --git a/firmware/Kconfig b/firmware/Kconfig
index b644964..a6f79e8 100644
--- a/firmware/Kconfig
+++ b/firmware/Kconfig
@@ -7,4 +7,7 @@
 config FIRMWARE_IMX_LPDDR4_PMU_TRAIN
        	bool
 
+config FIRMWARE_IMX8MQ_ATF
+        bool
+
 endmenu
diff --git a/firmware/Makefile b/firmware/Makefile
index c82e81a..7f4dc49 100644
--- a/firmware/Makefile
+++ b/firmware/Makefile
@@ -9,6 +9,8 @@
 	imx/lpddr4_pmu_train_2d_dmem.bin \
 	imx/lpddr4_pmu_train_2d_imem.bin
 
+firmware-$(CONFIG_FIRMWARE_IMX8MQ_ATF) += imx/imx8m-bl31.bin
+
 # Create $(fwabs) from $(CONFIG_EXTRA_FIRMWARE_DIR) -- if it doesn't have a
 # leading /, it's relative to $(srctree).
 fwdir := $(subst $(quote),,$(CONFIG_EXTRA_FIRMWARE_DIR))
@@ -55,6 +57,6 @@
 targets := $(patsubst $(obj)/%,%, \
                                 $(shell find $(obj) -name \*.gen.S 2>/dev/null))
 
-# just to build a built-in.o. Otherwise compilation fails when no devicetree is
-# created.
+# just to build a built-in.o. Otherwise compilation fails when no
+# firmware is built.
 obj- += dummy.o
diff --git a/firmware/imx/imx8m-bl31.bin b/firmware/imx/imx8m-bl31.bin
new file mode 100755
index 0000000..b2310e4
--- /dev/null
+++ b/firmware/imx/imx8m-bl31.bin
Binary files differ
diff --git a/include/gpio.h b/include/gpio.h
index f5262aa..e42fa23 100644
--- a/include/gpio.h
+++ b/include/gpio.h
@@ -1,6 +1,9 @@
 #ifndef __GPIO_H
 #define __GPIO_H
 
+#include <linux/types.h>
+#include <linux/list.h>
+
 #ifdef CONFIG_GENERIC_GPIO
 void gpio_set_value(unsigned gpio, int value);
 int gpio_get_value(unsigned gpio);
diff --git a/include/hab.h b/include/hab.h
index fb7149e..78c2b86 100644
--- a/include/hab.h
+++ b/include/hab.h
@@ -18,6 +18,8 @@
 #ifndef __HABV4_H
 #define __HABV4_H
 
+#include <errno.h>
+
 #ifdef CONFIG_HABV4
 int imx28_hab_get_status(void);
 int imx6_hab_get_status(void);
diff --git a/include/linux/arm-smccc.h b/include/linux/arm-smccc.h
new file mode 100644
index 0000000..1b38b7b
--- /dev/null
+++ b/include/linux/arm-smccc.h
@@ -0,0 +1,135 @@
+/*
+ * Copyright (c) 2015, Linaro Limited
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#ifndef __LINUX_ARM_SMCCC_H
+#define __LINUX_ARM_SMCCC_H
+
+/*
+ * This file provides common defines for ARM SMC Calling Convention as
+ * specified in
+ * http://infocenter.arm.com/help/topic/com.arm.doc.den0028a/index.html
+ */
+
+/* This constant is shifted by 31, make sure it's of an unsigned type */
+#define ARM_SMCCC_STD_CALL		0UL
+#define ARM_SMCCC_FAST_CALL		1UL
+#define ARM_SMCCC_TYPE_SHIFT		31
+
+#define ARM_SMCCC_SMC_32		0
+#define ARM_SMCCC_SMC_64		1
+#define ARM_SMCCC_CALL_CONV_SHIFT	30
+
+#define ARM_SMCCC_OWNER_MASK		0x3F
+#define ARM_SMCCC_OWNER_SHIFT		24
+
+#define ARM_SMCCC_FUNC_MASK		0xFFFF
+
+#define ARM_SMCCC_IS_FAST_CALL(smc_val)	\
+	((smc_val) & (ARM_SMCCC_FAST_CALL << ARM_SMCCC_TYPE_SHIFT))
+#define ARM_SMCCC_IS_64(smc_val) \
+	((smc_val) & (ARM_SMCCC_SMC_64 << ARM_SMCCC_CALL_CONV_SHIFT))
+#define ARM_SMCCC_FUNC_NUM(smc_val)	((smc_val) & ARM_SMCCC_FUNC_MASK)
+#define ARM_SMCCC_OWNER_NUM(smc_val) \
+	(((smc_val) >> ARM_SMCCC_OWNER_SHIFT) & ARM_SMCCC_OWNER_MASK)
+
+#define ARM_SMCCC_CALL_VAL(type, calling_convention, owner, func_num) \
+	(((type) << ARM_SMCCC_TYPE_SHIFT) | \
+	((calling_convention) << ARM_SMCCC_CALL_CONV_SHIFT) | \
+	(((owner) & ARM_SMCCC_OWNER_MASK) << ARM_SMCCC_OWNER_SHIFT) | \
+	((func_num) & ARM_SMCCC_FUNC_MASK))
+
+#define ARM_SMCCC_OWNER_ARCH		0
+#define ARM_SMCCC_OWNER_CPU		1
+#define ARM_SMCCC_OWNER_SIP		2
+#define ARM_SMCCC_OWNER_OEM		3
+#define ARM_SMCCC_OWNER_STANDARD	4
+#define ARM_SMCCC_OWNER_TRUSTED_APP	48
+#define ARM_SMCCC_OWNER_TRUSTED_APP_END	49
+#define ARM_SMCCC_OWNER_TRUSTED_OS	50
+#define ARM_SMCCC_OWNER_TRUSTED_OS_END	63
+
+#define ARM_SMCCC_QUIRK_NONE		0
+#define ARM_SMCCC_QUIRK_QCOM_A6		1 /* Save/restore register a6 */
+
+#ifndef __ASSEMBLY__
+
+#include <linux/linkage.h>
+#include <linux/types.h>
+/**
+ * struct arm_smccc_res - Result from SMC/HVC call
+ * @a0-a3 result values from registers 0 to 3
+ */
+struct arm_smccc_res {
+	unsigned long a0;
+	unsigned long a1;
+	unsigned long a2;
+	unsigned long a3;
+};
+
+/**
+ * struct arm_smccc_quirk - Contains quirk information
+ * @id: quirk identification
+ * @state: quirk specific information
+ * @a6: Qualcomm quirk entry for returning post-smc call contents of a6
+ */
+struct arm_smccc_quirk {
+	int	id;
+	union {
+		unsigned long a6;
+	} state;
+};
+
+/**
+ * __arm_smccc_smc() - make SMC calls
+ * @a0-a7: arguments passed in registers 0 to 7
+ * @res: result values from registers 0 to 3
+ * @quirk: points to an arm_smccc_quirk, or NULL when no quirks are required.
+ *
+ * This function is used to make SMC calls following SMC Calling Convention.
+ * The content of the supplied param are copied to registers 0 to 7 prior
+ * to the SMC instruction. The return values are updated with the content
+ * from register 0 to 3 on return from the SMC instruction.  An optional
+ * quirk structure provides vendor specific behavior.
+ */
+asmlinkage void __arm_smccc_smc(unsigned long a0, unsigned long a1,
+			unsigned long a2, unsigned long a3, unsigned long a4,
+			unsigned long a5, unsigned long a6, unsigned long a7,
+			struct arm_smccc_res *res, struct arm_smccc_quirk *quirk);
+
+/**
+ * __arm_smccc_hvc() - make HVC calls
+ * @a0-a7: arguments passed in registers 0 to 7
+ * @res: result values from registers 0 to 3
+ * @quirk: points to an arm_smccc_quirk, or NULL when no quirks are required.
+ *
+ * This function is used to make HVC calls following SMC Calling
+ * Convention.  The content of the supplied param are copied to registers 0
+ * to 7 prior to the HVC instruction. The return values are updated with
+ * the content from register 0 to 3 on return from the HVC instruction.  An
+ * optional quirk structure provides vendor specific behavior.
+ */
+asmlinkage void __arm_smccc_hvc(unsigned long a0, unsigned long a1,
+			unsigned long a2, unsigned long a3, unsigned long a4,
+			unsigned long a5, unsigned long a6, unsigned long a7,
+			struct arm_smccc_res *res, struct arm_smccc_quirk *quirk);
+
+#define arm_smccc_smc(...) __arm_smccc_smc(__VA_ARGS__, NULL)
+
+#define arm_smccc_smc_quirk(...) __arm_smccc_smc(__VA_ARGS__)
+
+#define arm_smccc_hvc(...) __arm_smccc_hvc(__VA_ARGS__, NULL)
+
+#define arm_smccc_hvc_quirk(...) __arm_smccc_hvc(__VA_ARGS__)
+
+#endif /*__ASSEMBLY__*/
+#endif /*__LINUX_ARM_SMCCC_H*/
diff --git a/scripts/imx/Makefile b/scripts/imx/Makefile
index 335e3e6..4fb62a6 100644
--- a/scripts/imx/Makefile
+++ b/scripts/imx/Makefile
@@ -8,6 +8,7 @@
 
 HOSTCFLAGS_imx.o = -I$(srctree)/arch/arm/mach-imx/include
 HOSTCFLAGS_imx-image.o = -I$(srctree) -I$(srctree)/arch/arm/mach-imx/include
+HOSTCFLAGS_imx-usb-loader.o += -I$(srctree) -I$(srctree)/arch/arm/mach-imx/include
 ifdef CONFIG_ARCH_IMX_IMXIMAGE_SSL_SUPPORT
 HOSTCFLAGS_imx-image.o += -DIMXIMAGE_SSL_SUPPORT
 HOSTLOADLIBES_imx-image  = `pkg-config --libs openssl`
diff --git a/scripts/imx/imx.h b/scripts/imx/imx.h
index 92a3fd3..20fb1e8 100644
--- a/scripts/imx/imx.h
+++ b/scripts/imx/imx.h
@@ -1,116 +1,4 @@
 
-#define HEADER_LEN 0x1000	/* length of the blank area + IVT + DCD */
-
-/*
- * ============================================================================
- * i.MX flash header v1 handling. Found on i.MX35 and i.MX51
- * ============================================================================
- */
-#define DCD_BARKER       0xb17219e9
-
-struct imx_flash_header {
-	uint32_t app_code_jump_vector;
-	uint32_t app_code_barker;
-	uint32_t app_code_csf;
-	uint32_t dcd_ptr_ptr;
-	uint32_t super_root_key;
-	uint32_t dcd;
-	uint32_t app_dest;
-	uint32_t dcd_barker;
-	uint32_t dcd_block_len;
-} __attribute__((packed));
-
-struct imx_boot_data {
-	uint32_t start;
-	uint32_t size;
-	uint32_t plugin;
-} __attribute__((packed));
-
-struct imx_dcd_rec_v1 {
-	uint32_t type;
-	uint32_t addr;
-	uint32_t val;
-} __attribute__((packed));
-
-#define TAG_IVT_HEADER	0xd1
-#define IVT_VERSION	0x40
-#define TAG_DCD_HEADER	0xd2
-#define DCD_VERSION	0x40
-#define TAG_UNLOCK	0xb2
-#define TAG_NOP		0xc0
-#define TAG_WRITE	0xcc
-#define TAG_CHECK	0xcf
-#define PARAMETER_FLAG_MASK	(1 << 3)
-#define PARAMETER_FLAG_SET	(1 << 4)
-
-struct imx_ivt_header {
-	uint8_t tag;
-	uint16_t length;
-	uint8_t version;
-} __attribute__((packed));
-
-struct imx_flash_header_v2 {
-	struct imx_ivt_header header;
-
-	uint32_t entry;
-	uint32_t reserved1;
-	uint32_t dcd_ptr;
-	uint32_t boot_data_ptr;
-	uint32_t self;
-	uint32_t csf;
-	uint32_t reserved2;
-
-	struct imx_boot_data boot_data;
-	struct imx_ivt_header dcd_header;
-} __attribute__((packed));
-
-struct config_data {
-	uint32_t image_load_addr;
-	uint32_t image_dcd_offset;
-	uint32_t image_size;
-	uint32_t load_size;
-	char *outfile;
-	char *srkfile;
-	int header_version;
-	off_t header_gap;
-	uint32_t first_opcode;
-	int cpu_type;
-	int (*check)(const struct config_data *data, uint32_t cmd,
-		     uint32_t addr, uint32_t mask);
-	int (*write_mem)(const struct config_data *data, uint32_t addr,
-			 uint32_t val, int width, int set_bits, int clear_bits);
-	int (*nop)(const struct config_data *data);
-	int csf_space;
-	char *csf;
-};
-
-#define MAX_RECORDS_DCD_V2 1024
-struct imx_dcd_v2_write_rec {
-	uint32_t addr;
-	uint32_t val;
-} __attribute__((packed));
-
-struct imx_dcd_v2_write {
-	uint8_t tag;
-	uint16_t length;
-	uint8_t param;
-	struct imx_dcd_v2_write_rec data[MAX_RECORDS_DCD_V2];
-} __attribute__((packed));
-
-struct imx_dcd_v2_check {
-	uint8_t tag;
-	uint16_t length;
-	uint8_t param;
-	uint32_t addr;
-	uint32_t mask;
-	uint32_t count;
-} __attribute__((packed));
-
-enum imx_dcd_v2_check_cond {
-	until_all_bits_clear = 0, /* until ((*address & mask) == 0) { ...} */
-	until_any_bit_clear = 1, /* until ((*address & mask) != mask) { ...} */
-	until_all_bits_set = 2, /* until ((*address & mask) == mask) { ...} */
-	until_any_bit_set = 3, /* until ((*address & mask) != 0) { ...} */
-} __attribute__((packed));
+#include <mach/imx-header.h>
 
 int parse_config(struct config_data *data, const char *filename);