diff --git a/Makefile b/Makefile
index add2e9f..ac688ba 100644
--- a/Makefile
+++ b/Makefile
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2013-2016, ARM Limited and Contributors. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions are met:
@@ -294,9 +294,9 @@
 include plat/compat/plat_compat.mk
 endif
 
-# Include the CPU specific operations makefile. By default all CPU errata
-# workarounds and CPU specific optimisations are disabled. This can be
-# overridden by the platform.
+# Include the CPU specific operations makefile, which provides default
+# values for all CPU errata workarounds and CPU specific optimisations.
+# This can be overridden by the platform.
 include lib/cpus/cpu-ops.mk
 
 
diff --git a/bl31/runtime_svc.c b/bl31/runtime_svc.c
index 5b7a21c..f011f11 100644
--- a/bl31/runtime_svc.c
+++ b/bl31/runtime_svc.c
@@ -103,8 +103,8 @@
 		 */
 		rc = validate_rt_svc_desc(&rt_svc_descs[index]);
 		if (rc) {
-			ERROR("Invalid runtime service descriptor 0x%lx (%s)\n",
-					(uintptr_t) &rt_svc_descs[index],
+			ERROR("Invalid runtime service descriptor %p (%s)\n",
+					(void *) &rt_svc_descs[index],
 					rt_svc_descs[index].name);
 			goto error;
 		}
diff --git a/common/bl_common.c b/common/bl_common.c
index 0eeef83..d5b095a 100644
--- a/common/bl_common.c
+++ b/common/bl_common.c
@@ -229,7 +229,8 @@
 		return io_result;
 	}
 
-	INFO("Loading image id=%u at address 0x%lx\n", image_id, image_base);
+	INFO("Loading image id=%u at address %p\n", image_id,
+		(void *) image_base);
 
 	/* Find the size of the image */
 	io_result = io_size(image_handle, &image_size);
@@ -242,8 +243,8 @@
 	/* Check that the memory where the image will be loaded is free */
 	if (!is_mem_free(mem_layout->free_base, mem_layout->free_size,
 			 image_base, image_size)) {
-		WARN("Failed to reserve memory: 0x%lx - 0x%lx\n",
-			image_base, image_base + image_size);
+		WARN("Failed to reserve memory: %p - %p\n", (void *) image_base,
+		     (void *) (image_base + image_size));
 		dump_load_info(image_base, image_size, mem_layout);
 		io_result = -ENOMEM;
 		goto exit;
@@ -268,8 +269,8 @@
 		reserve_mem(&mem_layout->free_base, &mem_layout->free_size,
 				image_base, image_size);
 	} else {
-		INFO("Skip reserving memory: 0x%lx - 0x%lx\n",
-				image_base, image_base + image_size);
+		INFO("Skip reserving memory: %p - %p\n", (void *) image_base,
+		     (void *) (image_base + image_size));
 	}
 
 	image_data->image_base = image_base;
@@ -284,8 +285,8 @@
 	 */
 	flush_dcache_range(image_base, image_size);
 
-	INFO("Image id=%u loaded: 0x%lx - 0x%lx\n", image_id, image_base,
-	     image_base + image_size);
+	INFO("Image id=%u loaded: %p - %p\n", image_id, (void *) image_base,
+	     (void *) (image_base + image_size));
 
 exit:
 	io_close(image_handle);
diff --git a/common/tf_printf.c b/common/tf_printf.c
index c68b990..c1d4188 100644
--- a/common/tf_printf.c
+++ b/common/tf_printf.c
@@ -68,6 +68,7 @@
  * %u - unsigned 32 bit decimal format
  * %ld and %lld - signed 64 bit decimal format
  * %lu and %llu - unsigned 64 bit decimal format
+ * %p - pointer format
  * Exits on all other formats.
  *******************************************************************/
 
@@ -107,6 +108,14 @@
 				str = va_arg(args, char *);
 				string_print(str);
 				break;
+			case 'p':
+				unum = (uint64_t)va_arg(args, void *);
+
+				if (unum)
+					string_print("0x");
+
+				unsigned_num_print(unum, 16);
+				break;
 			case 'x':
 				if (bit64)
 					unum = va_arg(args, uint64_t);
diff --git a/docs/cpu-specific-build-macros.md b/docs/cpu-specific-build-macros.md
index d9b7108..c57dc7e 100644
--- a/docs/cpu-specific-build-macros.md
+++ b/docs/cpu-specific-build-macros.md
@@ -26,8 +26,8 @@
 or a set of processor revisions. This is checked by reset handler at runtime.
 Each errata workaround is identified by its `ID` as specified in the processor's
 errata notice document. The format of the define used to enable/disable the
-errata is `ERRATA_<Processor name>_<ID>` where the `Processor name`
-is either `A57` for the `Cortex_A57` CPU or `A53` for `Cortex_A53` CPU.
+errata workaround is `ERRATA_<Processor name>_<ID>`, where the `Processor name`
+is for example `A57` for the `Cortex_A57` CPU.
 
 All workarounds are disabled by default. The platform is reponsible for
 enabling these workarounds according to its requirement by defining the
@@ -74,6 +74,23 @@
      sequence. Each Cortex-A57 based platform must make its own decision on
      whether to use the optimization.
 
+*    `A53_DISABLE_NON_TEMPORAL_HINT`: This flag disables the cache non-temporal
+     hint. The LDNP/STNP instructions as implemented on Cortex-A53 do not behave
+     in a way most programmers expect, and will most probably result in a
+     significant speed degradation to any code that employs them. The ARMv8-A
+     architecture (see ARM DDI 0487A.h, section D3.4.3) allows cores to ignore
+     the non-temporal hint and treat LDNP/STNP as LDP/STP instead. Enabling this
+     flag enforces this behaviour. This needs to be enabled only for revisions
+     <= r0p3 of the CPU and is enabled by default.
+
+*    `A57_DISABLE_NON_TEMPORAL_HINT`: This flag has the same behaviour as
+     `A53_DISABLE_NON_TEMPORAL_HINT` but for Cortex-A57. This needs to be
+     enabled only for revisions <= r1p2 of the CPU and is enabled by default,
+     as recommended in section "4.7 Non-Temporal Loads/Stores" of the
+     [Cortex-A57 Software Optimization Guide][A57 SW Optimization Guide].
+
 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
 
-_Copyright (c) 2014, ARM Limited and Contributors. All rights reserved._
+_Copyright (c) 2014-2016, ARM Limited and Contributors. All rights reserved._
+
+[A57 SW Optimization Guide]: http://infocenter.arm.com/help/topic/com.arm.doc.uan0015b/Cortex_A57_Software_Optimization_Guide_external.pdf
diff --git a/docs/firmware-design.md b/docs/firmware-design.md
index 7ae1de3..cd8c7aa 100644
--- a/docs/firmware-design.md
+++ b/docs/firmware-design.md
@@ -1194,6 +1194,72 @@
 on FVP, BL31 and TSP need to know the limit address that their PROGBITS
 sections must not overstep. The platform code must provide those.
 
+Trusted Firmware provides a mechanism to verify at boot time that the memory
+to load a new image is free to prevent overwriting a previously loaded image.
+For this mechanism to work, the platform must specify the memory available in
+the system as regions, where each region consists of base address, total size
+and the free area within it (as defined in the `meminfo_t` structure). Trusted
+Firmware retrieves these memory regions by calling the corresponding platform
+API:
+
+*   `meminfo_t *bl1_plat_sec_mem_layout(void)`
+*   `meminfo_t *bl2_plat_sec_mem_layout(void)`
+*   `void bl2_plat_get_scp_bl2_meminfo(meminfo_t *scp_bl2_meminfo)`
+*   `void bl2_plat_get_bl32_meminfo(meminfo_t *bl32_meminfo)`
+*   `void bl2_plat_get_bl33_meminfo(meminfo_t *bl33_meminfo)`
+
+For example, in the case of BL1 loading BL2, `bl1_plat_sec_mem_layout()` will
+return the region defined by the platform where BL1 intends to load BL2. The
+`load_image()` function will check that the memory where BL2 will be loaded is
+within the specified region and marked as free.
+
+The actual number of regions and their base addresses and sizes is platform
+specific. The platform may return the same region or define a different one for
+each API. However, the overlap verification mechanism applies only to a single
+region. Hence, it is the platform responsibility to guarantee that different
+regions do not overlap, or that if they do, the overlapping images are not
+accessed at the same time. This could be used, for example, to load temporary
+images (e.g. certificates) or firmware images prior to being transfered to its
+corresponding processor (e.g. the SCP BL2 image).
+
+To reduce fragmentation and simplify the tracking of free memory, all the free
+memory within a region is always located in one single buffer defined by its
+base address and size. Trusted Firmware implements a top/bottom load approach:
+after a new image is loaded, it checks how much memory remains free above and
+below the image. The smallest area is marked as unavailable, while the larger
+area becomes the new free memory buffer. Platforms should take this behaviour
+into account when defining the base address for each of the images. For example,
+if an image is loaded near the middle of the region, small changes in image size
+could cause a flip between a top load and a bottom load, which may result in an
+unexpected memory layout.
+
+The following diagram is an example of an image loaded in the bottom part of
+the memory region. The region is initially free (nothing has been loaded yet):
+
+               Memory region
+               +----------+
+               |          |
+               |          |  <<<<<<<<<<<<<  Free
+               |          |
+               |----------|                 +------------+
+               |  image   |  <<<<<<<<<<<<<  |   image    |
+               |----------|                 +------------+
+               | xxxxxxxx |  <<<<<<<<<<<<<  Marked as unavailable
+               +----------+
+
+And the following diagram is an example of an image loaded in the top part:
+
+               Memory region
+               +----------+
+               | xxxxxxxx |  <<<<<<<<<<<<<  Marked as unavailable
+               |----------|                 +------------+
+               |  image   |  <<<<<<<<<<<<<  |   image    |
+               |----------|                 +------------+
+               |          |
+               |          |  <<<<<<<<<<<<<  Free
+               |          |
+               +----------+
+
 
 ####  Memory layout on ARM development platforms
 
@@ -1204,9 +1270,9 @@
     Trusted SRAM. The amount of Trusted SRAM available to load the bootloader
     images is reduced by the size of the shared memory.
 
-    The shared memory is used to store the entrypoint mailboxes for each CPU.
-    On Juno, this is also used for the MHU payload when passing messages to and
-    from the SCP.
+    The shared memory is used to store the CPUs' entrypoint mailbox. On Juno,
+    this is also used for the MHU payload when passing messages to and from the
+    SCP.
 
 *   On FVP, BL1 is originally sitting in the Trusted ROM at address `0x0`. On
     Juno, BL1 resides in flash memory at address `0x0BEC0000`. BL1 read-write
@@ -1229,9 +1295,47 @@
     *   Secure region of DRAM (top 16MB of DRAM configured by the TrustZone
         controller)
 
+    When BL32 is loaded into Trusted SRAM, its NOBITS sections are allowed to
+    overlay BL2. This memory layout is designed to give the BL32 image as much
+    memory as possible when it is loaded into Trusted SRAM.
+
+The memory regions for the overlap detection mechanism at boot time are
+defined as follows (shown per API):
+
+*   `meminfo_t *bl1_plat_sec_mem_layout(void)`
+
+    This region corresponds to the whole Trusted SRAM except for the shared
+    memory at the base. This region is initially free. At boot time, BL1 will
+    mark the BL1(rw) section within this region as occupied. The BL1(rw) section
+    is placed at the top of Trusted SRAM.
+
+*   `meminfo_t *bl2_plat_sec_mem_layout(void)`
+
+    This region corresponds to the whole Trusted SRAM as defined by
+    `bl1_plat_sec_mem_layout()`, but with the BL1(rw) section marked as
+    occupied. This memory region is used to check that BL2 and BL31 do not
+    overlap with each other. BL2_BASE and BL1_RW_BASE are carefully chosen so
+    that the memory for BL31 is top loaded above BL2.
+
+*   `void bl2_plat_get_scp_bl2_meminfo(meminfo_t *scp_bl2_meminfo)`
+
+    This region is an exact copy of the region defined by
+    `bl2_plat_sec_mem_layout()`. Being a disconnected copy means that all the
+    changes made to this region by the Trusted Firmware will not be propagated.
+    This approach is valid because the SCP BL2 image is loaded temporarily
+    while it is being transferred to the SCP, so this memory is reused
+    afterwards.
+
+*   `void bl2_plat_get_bl32_meminfo(meminfo_t *bl32_meminfo)`
+
+    This region depends on the location of the BL32 image. Currently, ARM
+    platforms support three different locations (detailed below): Trusted SRAM,
+    Trusted DRAM and the TZC-Secured DRAM.
+
+*   `void bl2_plat_get_bl33_meminfo(meminfo_t *bl33_meminfo)`
+
-When BL32 is loaded into Trusted SRAM, its NOBITS sections are allowed to
-overlay BL2. This memory layout is designed to give the BL32 image as much
-memory as possible when it is loaded into Trusted SRAM.
+    This region corresponds to the Non-Secure DDR-DRAM, excluding the
+    TZC-Secured area.
 
 The location of the BL32 image will result in different memory maps. This is
 illustrated for both FVP and Juno in the following diagrams, using the TSP as
diff --git a/docs/porting-guide.md b/docs/porting-guide.md
index cdb722a..56ddbb1 100644
--- a/docs/porting-guide.md
+++ b/docs/porting-guide.md
@@ -72,10 +72,20 @@
 
 2.1 Common mandatory modifications
 ----------------------------------
-A platform port must enable the Memory Management Unit (MMU) with identity
-mapped page tables, and enable both the instruction and data caches for each BL
-stage. In ARM standard platforms, each BL stage configures the MMU in
-the platform-specific architecture setup function, `blX_plat_arch_setup()`.
+
+A platform port must enable the Memory Management Unit (MMU) as well as the
+instruction and data caches for each BL stage. Setting up the translation
+tables is the responsibility of the platform port because memory maps differ
+across platforms. A memory translation library (see `lib/aarch64/xlat_helpers.c`
+and `lib/aarch64/xlat_tables.c`) is provided to help in this setup. Note that
+although this library supports non-identity mappings, this is intended only for
+re-mapping peripheral physical addresses and allows platforms with high I/O
+addresses to reduce their virtual address space. All other addresses
+corresponding to code and data must currently use an identity mapping.
+
+In ARM standard platforms, each BL stage configures the MMU in the
+platform-specific architecture setup function, `blX_plat_arch_setup()`, and uses
+an identity mapping for all addresses.
 
 If the build option `USE_COHERENT_MEM` is enabled, each platform can allocate a
 block of identity mapped secure memory with Device-nGnRE attributes aligned to
@@ -85,7 +95,7 @@
 possible for the firmware to place variables in it using the following C code
 directive:
 
-    __attribute__ ((section("bakery_lock")))
+    __section("bakery_lock")
 
 Or alternatively the following assembler code directive:
 
@@ -1860,7 +1870,7 @@
     Return   : int
 
 This API is used by the crash reporting mechanism to initialize the crash
-console. It should only use the general purpose registers x0 to x2 to do the
+console. It must only use the general purpose registers x0 to x4 to do the
 initialization and returns 1 on success.
 
 ### Function : plat_crash_console_putc
@@ -1869,7 +1879,7 @@
     Return   : int
 
 This API is used by the crash reporting mechanism to print a character on the
-designated crash console. It should only use general purpose registers x1 and
+designated crash console. It must only use general purpose registers x1 and
 x2 to do its work. The parameter and the return value are in general purpose
 register x0.
 
@@ -1986,8 +1996,8 @@
 _Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved._
 
 
-[ARM GIC Architecture Specification 2.0]: http://arminfo.emea.arm.com/help/topic/com.arm.doc.ihi0048b/IHI0048B_gic_architecture_specification.pdf
-[ARM GIC Architecture Specification 3.0]: http://arminfo.emea.arm.com/help/topic/com.arm.doc.ihi0069a/IHI0069A_gic_architecture_specification.pdf
+[ARM GIC Architecture Specification 2.0]: http://infocenter.arm.com/help/topic/com.arm.doc.ihi0048b/index.html
+[ARM GIC Architecture Specification 3.0]: http://infocenter.arm.com/help/topic/com.arm.doc.ihi0069b/index.html
 [IMF Design Guide]:                       interrupt-framework-design.md
 [User Guide]:                             user-guide.md
 [FreeBSD]:                                http://www.freebsd.org
diff --git a/docs/user-guide.md b/docs/user-guide.md
index 3337f88..767fc42 100644
--- a/docs/user-guide.md
+++ b/docs/user-guide.md
@@ -413,6 +413,25 @@
     any register that is not part of the SBSA generic UART specification.
     Default value is 0 (a full PL011 compliant UART is present).
 
+*   `CTX_INCLUDE_FPREGS`: Boolean option that, when set to 1, will cause the FP
+    registers to be included when saving and restoring the CPU context. Default
+    is 0.
+
+*   `DISABLE_PEDANTIC`: When set to 1 it will disable the -pedantic option in
+    the GCC command line. Default is 0.
+
+*   `BUILD_STRING`: Input string for VERSION_STRING, which allows the TF build
+    to be uniquely identified. Defaults to the current git commit id.
+
+*   `VERSION_STRING`: String used in the log output for each TF image. Defaults
+    to a string formed by concatenating the version number, build type and build
+    string.
+
+*   `BUILD_MESSAGE_TIMESTAMP`: String used to identify the time and date of the
+    compilation of each build. It must be set to a C string (including quotes
+    where applicable). Defaults to a string that contains the time and date of
+    the compilation.
+
 #### ARM development platform specific build options
 
 *   `ARM_TSP_RAM_LOCATION`: location of the TSP binary. Options:
@@ -456,6 +475,12 @@
     Trusted Watchdog may be disabled at build time for testing or development
     purposes.
 
+*   `ARM_CONFIG_CNTACR`: boolean option to unlock access to the CNTBase<N>
+    frame registers by setting the CNTCTLBase.CNTACR<N> register bits. The
+    frame number <N> is defined by 'PLAT_ARM_NSTIMER_FRAME_ID', which should
+    match the frame used by the Non-Secure image (normally the Linux kernel).
+    Default is true (access to the frame is allowed).
+
 #### ARM CSS platform specific build options
 
 *   `CSS_DETECT_PRE_1_7_0_SCP`: Boolean flag to detect SCP version
@@ -465,6 +490,10 @@
     set to 1 then Trusted Firmware will detect if an earlier version is in use.
     Default is 1.
 
+*   `CSS_LOAD_SCP_IMAGES`: Boolean flag, which when set, adds SCP_BL2 and
+    SCP_BL2U to the FIP and FWU_FIP respectively, and enables them to be loaded
+    during boot. Default is 1.
+
 #### ARM FVP platform specific build options
 
 *   `FVP_USE_GIC_DRIVER`   : Selects the GIC driver to be built. Options:
@@ -853,6 +882,84 @@
 *   MMU disabled;
 *   Caches disabled.
 
+### Booting an EL3 payload
+
+The EL3 payload image is a standalone image and is not part of the FIP. It is
+not loaded by the Trusted Firmware. Therefore, there are 2 possible scenarios:
+
+*   The EL3 payload may reside in non-volatile memory (NVM) and execute in
+    place. In this case, booting it is just a matter of specifying the right
+    address in NVM through `EL3_PAYLOAD_BASE` when building the TF.
+
+*   The EL3 payload needs to be loaded in volatile memory (e.g. DRAM) at
+    run-time.
+
+To help in the latter scenario, the `SPIN_ON_BL1_EXIT=1` build option can be
+used. The infinite loop that it introduces in BL1 stops execution at the right
+moment for a debugger to take control of the target and load the payload (for
+example, over JTAG).
+
+It is expected that this loading method will work in most cases, as a debugger
+connection is usually available in a pre-production system. The user is free to
+use any other platform-specific mechanism to load the EL3 payload, though.
+
+#### Booting an EL3 payload on FVP
+
+The EL3 payloads boot flow requires the CPU's mailbox to be cleared at reset for
+the secondary CPUs holding pen to work properly. Unfortunately, its reset value
+is undefined on the FVP platform and the FVP platform code doesn't clear it.
+Therefore, one must modify the way the model is normally invoked in order to
+clear the mailbox at start-up.
+
+One way to do that is to create an 8-byte file containing all zero bytes using
+the following command:
+
+    dd if=/dev/zero of=mailbox.dat bs=1 count=8
+
+and pre-load it into the FVP memory at the mailbox address (i.e. `0x04000000`)
+using the following model parameters:
+
+    --data cluster0.cpu0=mailbox.dat@0x04000000   [Base FVPs]
+    --data=mailbox.dat@0x04000000                 [Foundation FVP]
+
+To provide the model with the EL3 payload image, the following methods may be
+used:
+
+1.  If the EL3 payload is able to execute in place, it may be programmed into
+    flash memory. On Base Cortex and AEM FVPs, the following model parameter
+    loads it at the base address of the NOR FLASH1 (the NOR FLASH0 is already
+    used for the FIP):
+
+        -C bp.flashloader1.fname="/path/to/el3-payload"
+
+    On Foundation FVP, there is no flash loader component and the EL3 payload
+    may be programmed anywhere in flash using method 3 below.
+
+2.  When using the `SPIN_ON_BL1_EXIT=1` loading method, the following DS-5
+    command may be used to load the EL3 payload ELF image over JTAG:
+
+        load /path/to/el3-payload.elf
+
+3.  The EL3 payload may be pre-loaded in volatile memory using the following
+    model parameters:
+
+        --data cluster0.cpu0="/path/to/el3-payload"@address  [Base FVPs]
+        --data="/path/to/el3-payload"@address                [Foundation FVP]
+
+    The address provided to the FVP must match the `EL3_PAYLOAD_BASE` address
+    used when building the Trusted Firmware.
+
+#### Booting an EL3 payload on Juno
+
+If the EL3 payload is able to execute in place, it may be programmed in flash
+memory by adding an entry in the `SITE1/HBI0262x/images.txt` configuration file
+on the Juno SD card (where `x` depends on the revision of the Juno board).
+Refer to the [Juno Getting Started Guide], section 2.3 "Flash memory
+programming" for more information.
+
+Alternatively, the same DS-5 command mentioned in the FVP section above can
+be used to load the EL3 payload's ELF file over JTAG on Juno.
+
 
 8.  Preparing the images to run on FVP
 --------------------------------------
@@ -913,14 +1020,15 @@
 This version of the ARM Trusted Firmware has been tested on the following ARM
 FVPs (64-bit versions only).
 
-*   `Foundation_Platform` (Version 9.4, Build 9.4.59)
-*   `FVP_Base_AEMv8A-AEMv8A` (Version 7.0, Build 0.8.7004)
-*   `FVP_Base_Cortex-A57x4-A53x4` (Version 7.0, Build 0.8.7004)
-*   `FVP_Base_Cortex-A57x1-A53x1` (Version 7.0, Build 0.8.7004)
-*   `FVP_Base_Cortex-A57x2-A53x4` (Version 7.0, Build 0.8.7004)
+*   `Foundation_Platform` (Version 9.5, Build 9.5.40)
+*   `FVP_Base_AEMv8A-AEMv8A` (Version 7.2, Build 0.8.7202)
+*   `FVP_Base_Cortex-A57x4-A53x4` (Version 7.2, Build 0.8.7202)
+*   `FVP_Base_Cortex-A57x1-A53x1` (Version 7.2, Build 0.8.7202)
+*   `FVP_Base_Cortex-A57x2-A53x4` (Version 7.2, Build 0.8.7202)
 
 NOTE: The build numbers quoted above are those reported by launching the FVP
-with the `--version` parameter.
+with the `--version` parameter. `Foundation_Platform` tarball for `--version`
+9.5.40 is labeled as version 9.5.41.
 
 NOTE: The software will not work on Version 1.0 of the Foundation FVP.
 The commands below would report an `unhandled argument` error in this case.
@@ -1205,38 +1313,6 @@
 `SYS_ID` register.  Setting this to `0x0` allows the ARM Trusted Firmware to
 detect the legacy VE memory map while configuring the GIC.
 
-### Booting an EL3 payload on FVP
-
-Booting an EL3 payload on FVP requires a couple of changes to the way the
-model is normally invoked.
-
-First of all, the EL3 payload image is not part of the FIP and is not loaded by
-the Trusted Firmware. Therefore, it must be loaded in memory some other way.
-There are 2 ways of doing that:
-
-1.  It can be loaded over JTAG at the appropriate time. The infinite loop
-    introduced in BL1 when compiling the Trusted Firmware with
-    `SPIN_ON_BL1_EXIT=1` stops execution at the right moment for a debugger to
-    take control of the target and load the payload.
-
-2.  It can be pre-loaded in the FVP memory using the following model parameter:
-
-        --data="<path-to-binary>"@<base-address-of-binary>
-
-    The base address provided to the FVP must match the `EL3_PAYLOAD_BASE`
-    address used when building the Trusted Firmware.
-
-Secondly, the EL3 payloads boot flow requires the CPUs mailbox to be cleared
-at reset for the secondary CPUs holding pen to work properly. Unfortunately,
-its reset value is undefined on FVP. One way to clear it is to create an
-8-byte file containing all zero bytes and pre-load it into the FVP memory at the
-mailbox address (i.e. `0x04000000`) using the same `--data` FVP parameter as
-described above.
-
-The following command creates such a file called `mailbox.dat`:
-
-    dd if=/dev/zero of=mailbox.dat bs=1 count=8
-
 
 10.  Running the software on Juno
 ---------------------------------
@@ -1338,7 +1414,7 @@
 
 - - - - - - - - - - - - - - - - - - - - - - - - - -
 
-_Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved._
+_Copyright (c) 2013-2016, ARM Limited and Contributors. All rights reserved._
 
 
 [Firmware Design]:             firmware-design.md
diff --git a/drivers/arm/ccn/ccn.c b/drivers/arm/ccn/ccn.c
index aef891b..28d2709 100644
--- a/drivers/arm/ccn/ccn.c
+++ b/drivers/arm/ccn/ccn.c
@@ -268,7 +268,7 @@
 /*******************************************************************************
  * This function executes the necessary operations to add or remove Request node
  * IDs specified in the 'rn_id_map' bitmap from the snoop/DVM domains specified
- * in the 'hn_id_map'. The 'region_id' specifies the ID of the first HN-F/HN-I
+ * in the 'hn_id_map'. The 'region_id' specifies the ID of the first HN-F/MN
  * on which the operation should be performed. 'op_reg_offset' specifies the
  * type of operation (add/remove). 'stat_reg_offset' specifies the register
  * which should be polled to determine if the operation has completed or not.
@@ -310,35 +310,6 @@
 }
 
 /*******************************************************************************
- * This function reads the bitmap of Home nodes on the basis of the
- * 'mn_hn_id_reg_offset' parameter from the Miscellaneous node's (MN)
- * programmer's view. The MN has a register which carries the bitmap of present
- * Home nodes of each type i.e. HN-Fs, HN-Is & HN-Ds. It calls
- * 'ccn_snoop_dvm_do_op()' with this information to perform the actual
- * operation.
- ******************************************************************************/
-static void ccn_snoop_dvm_domain_common(unsigned long long rn_id_map,
-					unsigned int hn_op_reg_offset,
-					unsigned int hn_stat_reg_offset,
-					unsigned int mn_hn_id_reg_offset,
-					unsigned int hn_region_id)
-{
-	unsigned long long mn_hn_id_map;
-
-	assert(ccn_plat_desc);
-	assert(ccn_plat_desc->periphbase);
-
-	mn_hn_id_map = ccn_reg_read(ccn_plat_desc->periphbase,
-				    MN_REGION_ID,
-				    mn_hn_id_reg_offset);
-	ccn_snoop_dvm_do_op(rn_id_map,
-			    mn_hn_id_map,
-			    hn_region_id,
-			    hn_op_reg_offset,
-			    hn_stat_reg_offset);
-}
-
-/*******************************************************************************
  * The following functions provide the boot and runtime API to the platform for
  * adding and removing master interfaces from the snoop/DVM domains. A bitmap of
  * master interfaces IDs is passed as a parameter. It is converted into a bitmap
@@ -357,17 +328,18 @@
 	unsigned long long rn_id_map;
 
 	rn_id_map = ccn_master_to_rn_id_map(master_iface_map);
-	ccn_snoop_dvm_domain_common(rn_id_map,
-				    HNF_SDC_SET_OFFSET,
-				    HNF_SDC_STAT_OFFSET,
-				    MN_HNF_NODEID_OFFSET,
-				    HNF_REGION_ID_START);
+	ccn_snoop_dvm_do_op(rn_id_map,
+			    CCN_GET_HN_NODEID_MAP(ccn_plat_desc->periphbase,
+						  MN_HNF_NODEID_OFFSET),
+			    HNF_REGION_ID_START,
+			    HNF_SDC_SET_OFFSET,
+			    HNF_SDC_STAT_OFFSET);
 
-	ccn_snoop_dvm_domain_common(rn_id_map,
-				    MN_DDC_SET_OFF,
-				    MN_DDC_STAT_OFFSET,
-				    MN_HNI_NODEID_OFFSET,
-				    MN_REGION_ID);
+	ccn_snoop_dvm_do_op(rn_id_map,
+			    CCN_GET_MN_NODEID_MAP(ccn_plat_desc->periphbase),
+			    MN_REGION_ID,
+			    MN_DDC_SET_OFFSET,
+			    MN_DDC_STAT_OFFSET);
 }
 
 void ccn_exit_snoop_dvm_domain(unsigned long long master_iface_map)
@@ -375,17 +347,18 @@
 	unsigned long long rn_id_map;
 
 	rn_id_map = ccn_master_to_rn_id_map(master_iface_map);
-	ccn_snoop_dvm_domain_common(rn_id_map,
-				    HNF_SDC_CLR_OFFSET,
-				    HNF_SDC_STAT_OFFSET,
-				    MN_HNF_NODEID_OFFSET,
-				    HNF_REGION_ID_START);
+	ccn_snoop_dvm_do_op(rn_id_map,
+			    CCN_GET_HN_NODEID_MAP(ccn_plat_desc->periphbase,
+						  MN_HNF_NODEID_OFFSET),
+			    HNF_REGION_ID_START,
+			    HNF_SDC_CLR_OFFSET,
+			    HNF_SDC_STAT_OFFSET);
 
-	ccn_snoop_dvm_domain_common(rn_id_map,
-				    MN_DDC_CLR_OFFSET,
-				    MN_DDC_STAT_OFFSET,
-				    MN_HNI_NODEID_OFFSET,
-				    MN_REGION_ID);
+	ccn_snoop_dvm_do_op(rn_id_map,
+			    CCN_GET_MN_NODEID_MAP(ccn_plat_desc->periphbase),
+			    MN_REGION_ID,
+			    MN_DDC_CLR_OFFSET,
+			    MN_DDC_STAT_OFFSET);
 }
 
 void ccn_enter_dvm_domain(unsigned long long master_iface_map)
@@ -393,11 +366,11 @@
 	unsigned long long rn_id_map;
 
 	rn_id_map = ccn_master_to_rn_id_map(master_iface_map);
-	ccn_snoop_dvm_domain_common(rn_id_map,
-				    MN_DDC_SET_OFF,
-				    MN_DDC_STAT_OFFSET,
-				    MN_HNI_NODEID_OFFSET,
-				    MN_REGION_ID);
+	ccn_snoop_dvm_do_op(rn_id_map,
+			    CCN_GET_MN_NODEID_MAP(ccn_plat_desc->periphbase),
+			    MN_REGION_ID,
+			    MN_DDC_SET_OFFSET,
+			    MN_DDC_STAT_OFFSET);
 }
 
 void ccn_exit_dvm_domain(unsigned long long master_iface_map)
@@ -405,11 +378,11 @@
 	unsigned long long rn_id_map;
 
 	rn_id_map = ccn_master_to_rn_id_map(master_iface_map);
-	ccn_snoop_dvm_domain_common(rn_id_map,
-				    MN_DDC_CLR_OFFSET,
-				    MN_DDC_STAT_OFFSET,
-				    MN_HNI_NODEID_OFFSET,
-				    MN_REGION_ID);
+	ccn_snoop_dvm_do_op(rn_id_map,
+			    CCN_GET_MN_NODEID_MAP(ccn_plat_desc->periphbase),
+			    MN_REGION_ID,
+			    MN_DDC_CLR_OFFSET,
+			    MN_DDC_STAT_OFFSET);
 }
 
 /*******************************************************************************
diff --git a/drivers/arm/ccn/ccn_private.h b/drivers/arm/ccn/ccn_private.h
index e92e870..8b15472 100644
--- a/drivers/arm/ccn/ccn_private.h
+++ b/drivers/arm/ccn/ccn_private.h
@@ -147,7 +147,7 @@
 #define MN_HNI_NODEID_OFFSET	0x01C0
 #define MN_SN_NODEID_OFFSET	0x01D0
 #define MN_DDC_STAT_OFFSET	DOMAIN_CTRL_STAT_OFFSET
-#define MN_DDC_SET_OFF		DOMAIN_CTRL_SET_OFFSET
+#define MN_DDC_SET_OFFSET	DOMAIN_CTRL_SET_OFFSET
 #define MN_DDC_CLR_OFFSET	DOMAIN_CTRL_CLR_OFFSET
 #define MN_ID_OFFSET		REGION_ID_OFFSET
 
@@ -236,4 +236,21 @@
  */
 #define FOR_EACH_PRESENT_MASTER_INTERFACE(iface_id, bit_map)	\
 			FOR_EACH_BIT(iface_id, bit_map)
+
+/*
+ * Macro that returns the node id bit map for the Miscellaneous Node
+ */
+#define CCN_GET_MN_NODEID_MAP(periphbase)				\
+	(1 << get_node_id(ccn_reg_read(periphbase, MN_REGION_ID,	\
+						REGION_ID_OFFSET)))
+
+/*
+ * This macro returns the bitmap of Home nodes on the basis of the
+ * 'mn_hn_id_reg_offset' parameter from the Miscellaneous node's (MN)
+ * programmer's view. The MN has a register which carries the bitmap of present
+ * Home nodes of each type i.e. HN-Fs, HN-Is & HN-Ds.
+ */
+#define CCN_GET_HN_NODEID_MAP(periphbase, mn_hn_id_reg_offset)		\
+	ccn_reg_read(periphbase, MN_REGION_ID, mn_hn_id_reg_offset)
+
 #endif /* __CCN_PRIVATE_H__ */
diff --git a/drivers/arm/gic/common/gic_common.c b/drivers/arm/gic/common/gic_common.c
index 17be61d..cfa0d23 100644
--- a/drivers/arm/gic/common/gic_common.c
+++ b/drivers/arm/gic/common/gic_common.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -31,6 +31,7 @@
 #include <assert.h>
 #include <gic_common.h>
 #include <mmio.h>
+#include "gic_common_private.h"
 
 /*******************************************************************************
  * GIC Distributor interface accessors for reading entire registers
@@ -239,7 +240,10 @@
 }
 
 /*******************************************************************************
- * GIC Distributor interface accessors for individual interrupt manipulation
+ * GIC Distributor functions for accessing the GIC registers
+ * corresponding to a single interrupt ID. These functions use bitwise
+ * operations or appropriate register accesses to modify or return
+ * the bit-field corresponding the single interrupt ID.
  ******************************************************************************/
 unsigned int gicd_get_igroupr(uintptr_t base, unsigned int id)
 {
@@ -306,3 +310,8 @@
 
 	gicd_write_icactiver(base, id, (1 << bit_num));
 }
+
+void gicd_set_ipriorityr(uintptr_t base, unsigned int id, unsigned int pri)
+{
+	mmio_write_8(base + GICD_IPRIORITYR + id, pri & GIC_PRI_MASK);
+}
diff --git a/drivers/arm/gic/common/gic_common_private.h b/drivers/arm/gic/common/gic_common_private.h
new file mode 100644
index 0000000..2919b7f
--- /dev/null
+++ b/drivers/arm/gic/common/gic_common_private.h
@@ -0,0 +1,104 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * 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.
+ */
+
+#ifndef GIC_COMMON_PRIVATE_H_
+#define GIC_COMMON_PRIVATE_H_
+
+#include <gic_common.h>
+#include <mmio.h>
+#include <stdint.h>
+
+/*******************************************************************************
+ * GIC Distributor interface register accessors that are common to GICv3 & GICv2
+ ******************************************************************************/
+static inline unsigned int gicd_read_ctlr(uintptr_t base)
+{
+	return mmio_read_32(base + GICD_CTLR);
+}
+
+static inline unsigned int gicd_read_typer(uintptr_t base)
+{
+	return mmio_read_32(base + GICD_TYPER);
+}
+
+static inline unsigned int gicd_read_iidr(uintptr_t base)
+{
+	return mmio_read_32(base + GICD_IIDR);
+}
+
+static inline void gicd_write_ctlr(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GICD_CTLR, val);
+}
+
+/*******************************************************************************
+ * GIC Distributor function prototypes for accessing entire registers.
+ * Note: The raw register values correspond to multiple interrupt IDs and
+ * the number of interrupt IDs involved depends on the register accessed.
+ ******************************************************************************/
+unsigned int gicd_read_igroupr(uintptr_t base, unsigned int id);
+unsigned int gicd_read_isenabler(uintptr_t base, unsigned int id);
+unsigned int gicd_read_icenabler(uintptr_t base, unsigned int id);
+unsigned int gicd_read_ispendr(uintptr_t base, unsigned int id);
+unsigned int gicd_read_icpendr(uintptr_t base, unsigned int id);
+unsigned int gicd_read_isactiver(uintptr_t base, unsigned int id);
+unsigned int gicd_read_icactiver(uintptr_t base, unsigned int id);
+unsigned int gicd_read_ipriorityr(uintptr_t base, unsigned int id);
+unsigned int gicd_read_icfgr(uintptr_t base, unsigned int id);
+unsigned int gicd_read_nsacr(uintptr_t base, unsigned int id);
+void gicd_write_igroupr(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_isenabler(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_icenabler(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_ispendr(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_icpendr(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_isactiver(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_icactiver(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_ipriorityr(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_icfgr(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_nsacr(uintptr_t base, unsigned int id, unsigned int val);
+
+/*******************************************************************************
+ * GIC Distributor function prototypes for accessing the GIC registers
+ * corresponding to a single interrupt ID. These functions use bitwise
+ * operations or appropriate register accesses to modify or return
+ * the bit-field corresponding the single interrupt ID.
+ ******************************************************************************/
+unsigned int gicd_get_igroupr(uintptr_t base, unsigned int id);
+void gicd_set_igroupr(uintptr_t base, unsigned int id);
+void gicd_clr_igroupr(uintptr_t base, unsigned int id);
+void gicd_set_isenabler(uintptr_t base, unsigned int id);
+void gicd_set_icenabler(uintptr_t base, unsigned int id);
+void gicd_set_ispendr(uintptr_t base, unsigned int id);
+void gicd_set_icpendr(uintptr_t base, unsigned int id);
+void gicd_set_isactiver(uintptr_t base, unsigned int id);
+void gicd_set_icactiver(uintptr_t base, unsigned int id);
+void gicd_set_ipriorityr(uintptr_t base, unsigned int id, unsigned int pri);
+
+#endif /* GIC_COMMON_PRIVATE_H_ */
diff --git a/drivers/arm/gic/gic_v2.c b/drivers/arm/gic/gic_v2.c
index dc5dc08..05399c3 100644
--- a/drivers/arm/gic/gic_v2.c
+++ b/drivers/arm/gic/gic_v2.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -261,10 +261,6 @@
  */
 void gicd_set_ipriorityr(uintptr_t base, unsigned int id, unsigned int pri)
 {
-	unsigned int reg = base + GICD_IPRIORITYR + (id & ~3);
-	unsigned int shift = (id & 3) << 3;
-	unsigned int reg_val = mmio_read_32(reg);
-
 	/*
 	 * Enforce ARM recommendation to manage priority values such
 	 * that group1 interrupts always have a lower priority than
@@ -278,17 +274,12 @@
 		pri >= GIC_HIGHEST_SEC_PRIORITY &&
 			pri <= GIC_LOWEST_SEC_PRIORITY);
 
-	reg_val &= ~(GIC_PRI_MASK << shift);
-	reg_val |= (pri & GIC_PRI_MASK) << shift;
-	mmio_write_32(reg, reg_val);
+	mmio_write_8(base + GICD_IPRIORITYR + id, pri & GIC_PRI_MASK);
 }
 
 void gicd_set_itargetsr(uintptr_t base, unsigned int id, unsigned int target)
 {
-	unsigned byte_off = id & ((1 << ITARGETSR_SHIFT) - 1);
-	unsigned int reg_val = gicd_read_itargetsr(base, id);
-
-	gicd_write_itargetsr(base, id, reg_val | (target << (byte_off << 3)));
+	mmio_write_8(base + GICD_ITARGETSR + id, target & GIC_TARGET_CPU_MASK);
 }
 
 /*******************************************************************************
diff --git a/drivers/arm/gic/gic_v3.c b/drivers/arm/gic/gic_v3.c
index f429662..11185b2 100644
--- a/drivers/arm/gic/gic_v3.c
+++ b/drivers/arm/gic/gic_v3.c
@@ -60,8 +60,8 @@
 			/* Disable this print for now as it appears every time
 			 * when using PSCI CPU_SUSPEND.
 			 * TODO: Print this only the first time for each CPU.
-			 * INFO("GICv3 - Found RDIST for MPIDR(0x%lx) at 0x%lx\n",
-			 *	mpidr, addr);
+			 * INFO("GICv3 - Found RDIST for MPIDR(0x%lx) at %p\n",
+			 *	mpidr, (void *) addr);
 			 */
 			return addr;
 		}
diff --git a/drivers/arm/gic/v2/gicv2_helpers.c b/drivers/arm/gic/v2/gicv2_helpers.c
index 1f904c5..b60a5cd 100644
--- a/drivers/arm/gic/v2/gicv2_helpers.c
+++ b/drivers/arm/gic/v2/gicv2_helpers.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -33,6 +33,7 @@
 #include <assert.h>
 #include <debug.h>
 #include <gic_common.h>
+#include "../common/gic_common_private.h"
 #include "gicv2_private.h"
 
 /*
@@ -101,10 +102,7 @@
  */
 void gicd_set_itargetsr(uintptr_t base, unsigned int id, unsigned int target)
 {
-	unsigned byte_off = id & ((1 << ITARGETSR_SHIFT) - 1);
-	unsigned int reg_val = gicd_read_itargetsr(base, id);
-
-	gicd_write_itargetsr(base, id, reg_val | (target << (byte_off << 3)));
+	mmio_write_8(base + GICD_ITARGETSR + id, target & GIC_TARGET_CPU_MASK);
 }
 
 /*******************************************************************************
@@ -166,7 +164,7 @@
 			gicd_clr_igroupr(gicd_base, irq_num);
 
 			/* Set the priority of this interrupt */
-			gicd_write_ipriorityr(gicd_base,
+			gicd_set_ipriorityr(gicd_base,
 					      irq_num,
 					      GIC_HIGHEST_SEC_PRIORITY);
 
@@ -213,7 +211,7 @@
 			sec_ppi_sgi_mask |= 1U << irq_num;
 
 			/* Set the priority of this interrupt */
-			gicd_write_ipriorityr(gicd_base,
+			gicd_set_ipriorityr(gicd_base,
 					    irq_num,
 					    GIC_HIGHEST_SEC_PRIORITY);
 		}
diff --git a/drivers/arm/gic/v2/gicv2_main.c b/drivers/arm/gic/v2/gicv2_main.c
index cf93926..305a8b0 100644
--- a/drivers/arm/gic/v2/gicv2_main.c
+++ b/drivers/arm/gic/v2/gicv2_main.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -34,6 +34,7 @@
 #include <debug.h>
 #include <gic_common.h>
 #include <gicv2.h>
+#include "../common/gic_common_private.h"
 #include "gicv2_private.h"
 
 static const gicv2_driver_data_t *driver_data;
diff --git a/drivers/arm/gic/v3/gicv3_helpers.c b/drivers/arm/gic/v3/gicv3_helpers.c
index 2fb98cb..07ae54c 100644
--- a/drivers/arm/gic/v3/gicv3_helpers.c
+++ b/drivers/arm/gic/v3/gicv3_helpers.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -33,6 +33,7 @@
 #include <assert.h>
 #include <debug.h>
 #include <gic_common.h>
+#include "../common/gic_common_private.h"
 #include "gicv3_private.h"
 
 /*
@@ -194,6 +195,15 @@
 	gicr_write_isenabler0(base, (1 << bit_num));
 }
 
+/*
+ * Accessor to set the byte corresponding to interrupt ID
+ * in GIC Re-distributor IPRIORITYR.
+ */
+void gicr_set_ipriorityr(uintptr_t base, unsigned int id, unsigned int pri)
+{
+	mmio_write_8(base + GICR_IPRIORITYR + id, pri & GIC_PRI_MASK);
+}
+
 /******************************************************************************
  * This function marks the core as awake in the re-distributor and
  * ensures that the interface is active.
@@ -330,7 +340,7 @@
 				gicd_clr_igrpmodr(gicd_base, irq_num);
 
 			/* Set the priority of this interrupt */
-			gicd_write_ipriorityr(gicd_base,
+			gicd_set_ipriorityr(gicd_base,
 					      irq_num,
 					      GIC_HIGHEST_SEC_PRIORITY);
 
@@ -404,7 +414,7 @@
 				gicr_clr_igrpmodr0(gicr_base, irq_num);
 
 			/* Set the priority of this interrupt */
-			gicr_write_ipriorityr(gicr_base,
+			gicr_set_ipriorityr(gicr_base,
 					    irq_num,
 					    GIC_HIGHEST_SEC_PRIORITY);
 
diff --git a/drivers/arm/gic/v3/gicv3_main.c b/drivers/arm/gic/v3/gicv3_main.c
index d5cd0ed..6c6c7af 100644
--- a/drivers/arm/gic/v3/gicv3_main.c
+++ b/drivers/arm/gic/v3/gicv3_main.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -34,6 +34,7 @@
 #include <debug.h>
 #include <gic_common.h>
 #include <gicv3.h>
+#include "../common/gic_common_private.h"
 #include "gicv3_private.h"
 
 static const gicv3_driver_data_t *driver_data;
diff --git a/drivers/arm/gic/v3/gicv3_private.h b/drivers/arm/gic/v3/gicv3_private.h
index c8b311a..5e2409f 100644
--- a/drivers/arm/gic/v3/gicv3_private.h
+++ b/drivers/arm/gic/v3/gicv3_private.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -84,16 +84,24 @@
 	 ((typer_val >> 32) & 0xffffff))
 
 /*******************************************************************************
- * Private function prototypes
+ * Private GICv3 function prototypes for accessing entire registers.
+ * Note: The raw register values correspond to multiple interrupt IDs and
+ * the number of interrupt IDs involved depends on the register accessed.
  ******************************************************************************/
 unsigned int gicd_read_igrpmodr(uintptr_t base, unsigned int id);
 unsigned int gicr_read_ipriorityr(uintptr_t base, unsigned int id);
+void gicd_write_igrpmodr(uintptr_t base, unsigned int id, unsigned int val);
+void gicr_write_ipriorityr(uintptr_t base, unsigned int id, unsigned int val);
+
+/*******************************************************************************
+ * Private GICv3 function prototypes for accessing the GIC registers
+ * corresponding to a single interrupt ID. These functions use bitwise
+ * operations or appropriate register accesses to modify or return
+ * the bit-field corresponding the single interrupt ID.
+ ******************************************************************************/
 unsigned int gicd_get_igrpmodr(uintptr_t base, unsigned int id);
 unsigned int gicr_get_igrpmodr0(uintptr_t base, unsigned int id);
 unsigned int gicr_get_igroupr0(uintptr_t base, unsigned int id);
-unsigned int gicv3_get_pending_grp1_interrupt_id(unsigned int pending_grp);
-void gicd_write_igrpmodr(uintptr_t base, unsigned int id, unsigned int val);
-void gicr_write_ipriorityr(uintptr_t base, unsigned int id, unsigned int val);
 void gicd_set_igrpmodr(uintptr_t base, unsigned int id);
 void gicr_set_igrpmodr0(uintptr_t base, unsigned int id);
 void gicr_set_isenabler0(uintptr_t base, unsigned int id);
@@ -101,6 +109,11 @@
 void gicd_clr_igrpmodr(uintptr_t base, unsigned int id);
 void gicr_clr_igrpmodr0(uintptr_t base, unsigned int id);
 void gicr_clr_igroupr0(uintptr_t base, unsigned int id);
+void gicr_set_ipriorityr(uintptr_t base, unsigned int id, unsigned int pri);
+
+/*******************************************************************************
+ * Private GICv3 helper function prototypes
+ ******************************************************************************/
 void gicv3_spis_configure_defaults(uintptr_t gicd_base);
 void gicv3_ppi_sgi_configure_defaults(uintptr_t gicr_base);
 void gicv3_secure_spis_configure(uintptr_t gicd_base,
@@ -179,6 +192,11 @@
 	mmio_write_32(base + GICR_WAKER, val);
 }
 
+/*******************************************************************************
+ * GIC Re-distributor functions for accessing entire registers.
+ * Note: The raw register values correspond to multiple interrupt IDs and
+ * the number of interrupt IDs involved depends on the register accessed.
+ ******************************************************************************/
 static inline unsigned int gicr_read_icenabler0(uintptr_t base)
 {
 	return mmio_read_32(base + GICR_ICENABLER0);
diff --git a/drivers/arm/pl011/pl011_console.S b/drivers/arm/pl011/pl011_console.S
index 47608da..f29f895 100644
--- a/drivers/arm/pl011/pl011_console.S
+++ b/drivers/arm/pl011/pl011_console.S
@@ -54,7 +54,7 @@
 	 *     w1 - Uart clock in Hz
 	 *     w2 - Baud rate
 	 * Out: return 1 on success else 0 on error
-	 * Clobber list : x1, x2
+	 * Clobber list : x1, x2, x3, x4
 	 * -----------------------------------------------
 	 */
 func console_core_init
@@ -64,6 +64,20 @@
 	/* Check baud rate and uart clock for sanity */
 	cbz	w1, core_init_fail
 	cbz	w2, core_init_fail
+	/* Disable uart before programming */
+	ldr	w3, [x0, #UARTCR]
+	mov	w4, #PL011_UARTCR_UARTEN
+	bic	w3, w3, w4
+	str	w3, [x0, #UARTCR]
+	/* Flush the transmit FIFO */
+	ldr	w3, [x0, #UARTLCR_H]
+	mov	w4, #PL011_UARTLCR_H_FEN
+	bic	w3, w3, w4
+	str	w3, [x0, #UARTLCR_H]
+	/* Wait for the end of Tx of current character */
+busy_loop:
+	ldr	w3, [x0, #UARTFR]
+	tbnz	w3, #PL011_UARTFR_BUSY_BIT, busy_loop
 	/* Program the baudrate */
 	/* Divisor =  (Uart clock * 4) / baudrate */
 	lsl	w1, w1, #2
diff --git a/drivers/console/console.S b/drivers/console/console.S
index b772363..797b564 100644
--- a/drivers/console/console.S
+++ b/drivers/console/console.S
@@ -54,7 +54,7 @@
 	 *     w1 - Uart clock in Hz
 	 *     w2 - Baud rate
 	 * out: return 1 on success else 0 on error
-	 * Clobber list : x1 - x3
+	 * Clobber list : x1 - x4
 	 * -----------------------------------------------
 	 */
 func console_init
diff --git a/drivers/io/io_fip.c b/drivers/io/io_fip.c
index d291423..99cf15b 100644
--- a/drivers/io/io_fip.c
+++ b/drivers/io/io_fip.c
@@ -128,7 +128,7 @@
 
 
 /* Open a connection to the FIP device */
-static int fip_dev_open(const uintptr_t dev_spec __attribute__((unused)),
+static int fip_dev_open(const uintptr_t dev_spec __unused,
 			 io_dev_info_t **dev_info)
 {
 	assert(dev_info != NULL);
diff --git a/drivers/io/io_memmap.c b/drivers/io/io_memmap.c
index d45107e..ff4efa8 100644
--- a/drivers/io/io_memmap.c
+++ b/drivers/io/io_memmap.c
@@ -95,7 +95,7 @@
 
 
 /* Open a connection to the memmap device */
-static int memmap_dev_open(const uintptr_t dev_spec __attribute__((unused)),
+static int memmap_dev_open(const uintptr_t dev_spec __unused,
 			   io_dev_info_t **dev_info)
 {
 	assert(dev_info != NULL);
diff --git a/drivers/io/io_semihosting.c b/drivers/io/io_semihosting.c
index 63d0f68..30ca99c 100644
--- a/drivers/io/io_semihosting.c
+++ b/drivers/io/io_semihosting.c
@@ -91,7 +91,7 @@
 
 
 /* Open a file on the semi-hosting device */
-static int sh_file_open(io_dev_info_t *dev_info __attribute__((unused)),
+static int sh_file_open(io_dev_info_t *dev_info __unused,
 		const uintptr_t spec, io_entity_t *entity)
 {
 	int result = -ENOENT;
diff --git a/include/bl31/runtime_svc.h b/include/bl31/runtime_svc.h
index 87f2dd2..03f906e 100644
--- a/include/bl31/runtime_svc.h
+++ b/include/bl31/runtime_svc.h
@@ -88,7 +88,7 @@
  */
 #define DECLARE_RT_SVC(_name, _start, _end, _type, _setup, _smch) \
 	static const rt_svc_desc_t __svc_desc_ ## _name \
-		__attribute__ ((section("rt_svc_descs"), used)) = { \
+		__section("rt_svc_descs") __used = { \
 			.start_oen = _start, \
 			.end_oen = _end, \
 			.call_type = _type, \
diff --git a/include/common/context_mgmt.h b/include/common/context_mgmt.h
index 141b348..11786a1 100644
--- a/include/common/context_mgmt.h
+++ b/include/common/context_mgmt.h
@@ -33,7 +33,6 @@
 
 #include <arch.h>
 #include <bl_common.h>
-#include <common_def.h>
 
 /*******************************************************************************
  * Forward declarations
@@ -45,10 +44,10 @@
  ******************************************************************************/
 void cm_init(void);
 void *cm_get_context_by_mpidr(uint64_t mpidr,
-			      uint32_t security_state) __warn_deprecated;
+			      uint32_t security_state) __deprecated;
 void cm_set_context_by_mpidr(uint64_t mpidr,
 			     void *context,
-			     uint32_t security_state) __warn_deprecated;
+			     uint32_t security_state) __deprecated;
 void *cm_get_context_by_index(unsigned int cpu_idx,
 			      unsigned int security_state);
 void cm_set_context_by_index(unsigned int cpu_idx,
@@ -58,7 +57,7 @@
 void cm_set_context(void *context, uint32_t security_state);
 inline void cm_set_next_context(void *context);
 void cm_init_context(uint64_t mpidr,
-		     const struct entry_point_info *ep) __warn_deprecated;
+		     const struct entry_point_info *ep) __deprecated;
 void cm_init_my_context(const struct entry_point_info *ep);
 void cm_init_context_by_index(unsigned int cpu_idx,
 			      const struct entry_point_info *ep);
diff --git a/include/drivers/arm/arm_gic.h b/include/drivers/arm/arm_gic.h
index 8c1f03f..37ff5c9 100644
--- a/include/drivers/arm/arm_gic.h
+++ b/include/drivers/arm/arm_gic.h
@@ -31,7 +31,6 @@
 #ifndef __ARM_GIC_H__
 #define __ARM_GIC_H__
 
-#include <common_def.h>
 #include <stdint.h>
 
 /*******************************************************************************
@@ -41,18 +40,18 @@
 		  uintptr_t gicd_base,
 		  uintptr_t gicr_base,
 		  const unsigned int *irq_sec_ptr,
-		  unsigned int num_irqs) __warn_deprecated;
-void arm_gic_setup(void) __warn_deprecated;
-void arm_gic_cpuif_deactivate(void) __warn_deprecated;
-void arm_gic_cpuif_setup(void) __warn_deprecated;
-void arm_gic_pcpu_distif_setup(void) __warn_deprecated;
+		  unsigned int num_irqs) __deprecated;
+void arm_gic_setup(void) __deprecated;
+void arm_gic_cpuif_deactivate(void) __deprecated;
+void arm_gic_cpuif_setup(void) __deprecated;
+void arm_gic_pcpu_distif_setup(void) __deprecated;
 
 uint32_t arm_gic_interrupt_type_to_line(uint32_t type,
-				uint32_t security_state) __warn_deprecated;
-uint32_t arm_gic_get_pending_interrupt_type(void) __warn_deprecated;
-uint32_t arm_gic_get_pending_interrupt_id(void) __warn_deprecated;
-uint32_t arm_gic_acknowledge_interrupt(void) __warn_deprecated;
-void arm_gic_end_of_interrupt(uint32_t id) __warn_deprecated;
-uint32_t arm_gic_get_interrupt_type(uint32_t id) __warn_deprecated;
+				uint32_t security_state) __deprecated;
+uint32_t arm_gic_get_pending_interrupt_type(void) __deprecated;
+uint32_t arm_gic_get_pending_interrupt_id(void) __deprecated;
+uint32_t arm_gic_acknowledge_interrupt(void) __deprecated;
+void arm_gic_end_of_interrupt(uint32_t id) __deprecated;
+uint32_t arm_gic_get_interrupt_type(uint32_t id) __deprecated;
 
 #endif /* __GIC_H__ */
diff --git a/include/drivers/arm/cci400.h b/include/drivers/arm/cci400.h
index a5dc9a0..bfadc8f 100644
--- a/include/drivers/arm/cci400.h
+++ b/include/drivers/arm/cci400.h
@@ -76,7 +76,6 @@
 
 #ifndef __ASSEMBLY__
 
-#include <common_def.h>
 #include <stdint.h>
 
 /* Function declarations */
@@ -92,10 +91,10 @@
  */
 void cci_init(uintptr_t cci_base,
 		int slave_iface3_cluster_ix,
-		int slave_iface4_cluster_ix) __warn_deprecated;
+		int slave_iface4_cluster_ix) __deprecated;
 
-void cci_enable_cluster_coherency(unsigned long mpidr) __warn_deprecated;
-void cci_disable_cluster_coherency(unsigned long mpidr) __warn_deprecated;
+void cci_enable_cluster_coherency(unsigned long mpidr) __deprecated;
+void cci_disable_cluster_coherency(unsigned long mpidr) __deprecated;
 
 #endif /* __ASSEMBLY__ */
 #endif /* __CCI_400_H__ */
diff --git a/include/drivers/arm/gic_common.h b/include/drivers/arm/gic_common.h
index 6a322a2..dd8efdc 100644
--- a/include/drivers/arm/gic_common.h
+++ b/include/drivers/arm/gic_common.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -107,67 +107,4 @@
 	(GIC_HIGHEST_NS_PRIORITY << 16)	|	\
 	(GIC_HIGHEST_NS_PRIORITY << 24))
 
-#ifndef __ASSEMBLY__
-
-#include <mmio.h>
-#include <stdint.h>
-
-/*******************************************************************************
- * GIC Distributor interface register accessors that are common to GICv3 & GICv2
- ******************************************************************************/
-static inline unsigned int gicd_read_ctlr(uintptr_t base)
-{
-	return mmio_read_32(base + GICD_CTLR);
-}
-
-static inline unsigned int gicd_read_typer(uintptr_t base)
-{
-	return mmio_read_32(base + GICD_TYPER);
-}
-
-static inline unsigned int gicd_read_iidr(uintptr_t base)
-{
-	return mmio_read_32(base + GICD_IIDR);
-}
-
-static inline void gicd_write_ctlr(uintptr_t base, unsigned int val)
-{
-	mmio_write_32(base + GICD_CTLR, val);
-}
-
-/*******************************************************************************
- * GIC Distributor function prototypes
- ******************************************************************************/
-unsigned int gicd_read_igroupr(uintptr_t base, unsigned int id);
-unsigned int gicd_read_isenabler(uintptr_t base, unsigned int id);
-unsigned int gicd_read_icenabler(uintptr_t base, unsigned int id);
-unsigned int gicd_read_ispendr(uintptr_t base, unsigned int id);
-unsigned int gicd_read_icpendr(uintptr_t base, unsigned int id);
-unsigned int gicd_read_isactiver(uintptr_t base, unsigned int id);
-unsigned int gicd_read_icactiver(uintptr_t base, unsigned int id);
-unsigned int gicd_read_ipriorityr(uintptr_t base, unsigned int id);
-unsigned int gicd_read_icfgr(uintptr_t base, unsigned int id);
-unsigned int gicd_read_nsacr(uintptr_t base, unsigned int id);
-void gicd_write_igroupr(uintptr_t base, unsigned int id, unsigned int val);
-void gicd_write_isenabler(uintptr_t base, unsigned int id, unsigned int val);
-void gicd_write_icenabler(uintptr_t base, unsigned int id, unsigned int val);
-void gicd_write_ispendr(uintptr_t base, unsigned int id, unsigned int val);
-void gicd_write_icpendr(uintptr_t base, unsigned int id, unsigned int val);
-void gicd_write_isactiver(uintptr_t base, unsigned int id, unsigned int val);
-void gicd_write_icactiver(uintptr_t base, unsigned int id, unsigned int val);
-void gicd_write_ipriorityr(uintptr_t base, unsigned int id, unsigned int val);
-void gicd_write_icfgr(uintptr_t base, unsigned int id, unsigned int val);
-void gicd_write_nsacr(uintptr_t base, unsigned int id, unsigned int val);
-unsigned int gicd_get_igroupr(uintptr_t base, unsigned int id);
-void gicd_set_igroupr(uintptr_t base, unsigned int id);
-void gicd_clr_igroupr(uintptr_t base, unsigned int id);
-void gicd_set_isenabler(uintptr_t base, unsigned int id);
-void gicd_set_icenabler(uintptr_t base, unsigned int id);
-void gicd_set_ispendr(uintptr_t base, unsigned int id);
-void gicd_set_icpendr(uintptr_t base, unsigned int id);
-void gicd_set_isactiver(uintptr_t base, unsigned int id);
-void gicd_set_icactiver(uintptr_t base, unsigned int id);
-
-
-#endif /* __ASSEMBLY__ */
 #endif /* __GIC_COMMON_H__ */
diff --git a/include/drivers/arm/pl011.h b/include/drivers/arm/pl011.h
index d5ea890..ce6cdcf 100644
--- a/include/drivers/arm/pl011.h
+++ b/include/drivers/arm/pl011.h
@@ -71,6 +71,7 @@
 
 #define PL011_UARTFR_TXFF_BIT	5	/* Transmit FIFO full bit in UARTFR register */
 #define PL011_UARTFR_RXFE_BIT	4	/* Receive FIFO empty bit in UARTFR register */
+#define PL011_UARTFR_BUSY_BIT	3	/* UART busy bit in UARTFR register */
 
 /* Control reg bits */
 #if !PL011_GENERIC_UART
diff --git a/include/drivers/auth/img_parser_mod.h b/include/drivers/auth/img_parser_mod.h
index d80e0fb..79d7d79 100644
--- a/include/drivers/auth/img_parser_mod.h
+++ b/include/drivers/auth/img_parser_mod.h
@@ -77,7 +77,7 @@
 /* Macro to register an image parser library */
 #define REGISTER_IMG_PARSER_LIB(_type, _name, _init, _check_int, _get_param) \
 	static const img_parser_lib_desc_t __img_parser_lib_desc_##_type \
-	__attribute__ ((section(".img_parser_lib_descs"), used)) = { \
+	__section(".img_parser_lib_descs") __used = { \
 		.img_type = _type, \
 		.name = _name, \
 		.init = _init, \
diff --git a/include/lib/bakery_lock.h b/include/lib/bakery_lock.h
index 86adb9c..8a53891 100644
--- a/include/lib/bakery_lock.h
+++ b/include/lib/bakery_lock.h
@@ -100,8 +100,7 @@
 void bakery_lock_get(bakery_lock_t *bakery);
 void bakery_lock_release(bakery_lock_t *bakery);
 
-#define DEFINE_BAKERY_LOCK(_name) bakery_lock_t _name \
-			__attribute__ ((section("bakery_lock")))
+#define DEFINE_BAKERY_LOCK(_name) bakery_lock_t _name __section("bakery_lock")
 
 #define DECLARE_BAKERY_LOCK(_name) extern bakery_lock_t _name
 
diff --git a/include/lib/cassert.h b/include/lib/cassert.h
index e8089cb..00ee4d5 100644
--- a/include/lib/cassert.h
+++ b/include/lib/cassert.h
@@ -40,6 +40,6 @@
  * compiler warning.
  ******************************************************************************/
 #define CASSERT(cond, msg)	\
-	typedef char msg[(cond) ? 1 : -1] __attribute__((unused))
+	typedef char msg[(cond) ? 1 : -1] __unused
 
 #endif /* __CASSERT_H__ */
diff --git a/include/lib/cpus/aarch64/cortex_a57.h b/include/lib/cpus/aarch64/cortex_a57.h
index c81259c..c512129 100644
--- a/include/lib/cpus/aarch64/cortex_a57.h
+++ b/include/lib/cpus/aarch64/cortex_a57.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -61,8 +61,9 @@
  ******************************************************************************/
 #define CPUACTLR_EL1			S3_1_C15_C2_0	/* Instruction def. */
 
-#define CPUACTLR_NO_ALLOC_WBWA         (1 << 49)
-#define CPUACTLR_DCC_AS_DCCI           (1 << 44)
+#define CPUACTLR_DIS_OVERREAD		(1 << 52)
+#define CPUACTLR_NO_ALLOC_WBWA		(1 << 49)
+#define CPUACTLR_DCC_AS_DCCI		(1 << 44)
 
 /*******************************************************************************
  * L2 Control register specific definitions.
diff --git a/include/plat/arm/board/common/board_arm_def.h b/include/plat/arm/board/common/board_arm_def.h
index b4e4313..db2a8df 100644
--- a/include/plat/arm/board/common/board_arm_def.h
+++ b/include/plat/arm/board/common/board_arm_def.h
@@ -39,9 +39,7 @@
  */
 
 /* Size of cacheable stacks */
-#if DEBUG_XLAT_TABLE
-# define PLATFORM_STACK_SIZE 0x800
-#elif IMAGE_BL1
+#if IMAGE_BL1
 #if TRUSTED_BOARD_BOOT
 # define PLATFORM_STACK_SIZE 0x1000
 #else
diff --git a/include/plat/arm/common/aarch64/arm_macros.S b/include/plat/arm/common/aarch64/arm_macros.S
index 384bb51..3b19a7d 100644
--- a/include/plat/arm/common/aarch64/arm_macros.S
+++ b/include/plat/arm/common/aarch64/arm_macros.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -30,7 +30,6 @@
 #ifndef __ARM_MACROS_S__
 #define __ARM_MACROS_S__
 
-#include <cci.h>
 #include <gic_common.h>
 #include <gicv2.h>
 #include <gicv3.h>
@@ -117,31 +116,4 @@
 exit_print_gic_regs:
 	.endm
 
-
-.section .rodata.cci_reg_name, "aS"
-cci_iface_regs:
-	.asciz "cci_snoop_ctrl_cluster0", "cci_snoop_ctrl_cluster1" , ""
-
-	/* ------------------------------------------------
-	 * The below required platform porting macro prints
-	 * out relevant interconnect registers whenever an
-	 * unhandled exception is taken in BL31.
-	 * Clobbers: x0 - x9, sp
-	 * ------------------------------------------------
-	 */
-	.macro plat_print_interconnect_regs
-	adr	x6, cci_iface_regs
-	/* Store in x7 the base address of the first interface */
-	mov_imm	x7, (PLAT_ARM_CCI_BASE + SLAVE_IFACE_OFFSET(	\
-			PLAT_ARM_CCI_CLUSTER0_SL_IFACE_IX))
-	ldr	w8, [x7, #SNOOP_CTRL_REG]
-	/* Store in x7 the base address of the second interface */
-	mov_imm	x7, (PLAT_ARM_CCI_BASE + SLAVE_IFACE_OFFSET(	\
-			PLAT_ARM_CCI_CLUSTER1_SL_IFACE_IX))
-	ldr	w9, [x7, #SNOOP_CTRL_REG]
-	/* Store to the crash buf and print to console */
-	bl	str_in_crash_buf_print
-	.endm
-
-
 #endif /* __ARM_MACROS_S__ */
diff --git a/include/plat/arm/common/aarch64/cci_macros.S b/include/plat/arm/common/aarch64/cci_macros.S
new file mode 100644
index 0000000..40f9d7e
--- /dev/null
+++ b/include/plat/arm/common/aarch64/cci_macros.S
@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * 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.
+ */
+#ifndef __CCI_MACROS_S__
+#define __CCI_MACROS_S__
+
+#include <cci.h>
+#include <platform_def.h>
+
+.section .rodata.cci_reg_name, "aS"
+cci_iface_regs:
+	.asciz "cci_snoop_ctrl_cluster0", "cci_snoop_ctrl_cluster1" , ""
+
+	/* ------------------------------------------------
+	 * The below required platform porting macro prints
+	 * out relevant interconnect registers whenever an
+	 * unhandled exception is taken in BL31.
+	 * Clobbers: x0 - x9, sp
+	 * ------------------------------------------------
+	 */
+	.macro plat_print_interconnect_regs
+	adr	x6, cci_iface_regs
+	/* Store in x7 the base address of the first interface */
+	mov_imm	x7, (PLAT_ARM_CCI_BASE + SLAVE_IFACE_OFFSET(	\
+			PLAT_ARM_CCI_CLUSTER0_SL_IFACE_IX))
+	ldr	w8, [x7, #SNOOP_CTRL_REG]
+	/* Store in x7 the base address of the second interface */
+	mov_imm	x7, (PLAT_ARM_CCI_BASE + SLAVE_IFACE_OFFSET(	\
+			PLAT_ARM_CCI_CLUSTER1_SL_IFACE_IX))
+	ldr	w9, [x7, #SNOOP_CTRL_REG]
+	/* Store to the crash buf and print to console */
+	bl	str_in_crash_buf_print
+	.endm
+
+#endif /* __CCI_MACROS_S__ */
diff --git a/include/plat/arm/common/arm_config.h b/include/plat/arm/common/arm_config.h
index 24c1f0a..0380849 100644
--- a/include/plat/arm/common/arm_config.h
+++ b/include/plat/arm/common/arm_config.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -35,8 +35,8 @@
 enum arm_config_flags {
 	/* Whether Base memory map is in use */
 	ARM_CONFIG_BASE_MMAP		= 0x1,
-	/* Whether CCI should be enabled */
-	ARM_CONFIG_HAS_CCI		= 0x2,
+	/* Whether interconnect should be enabled */
+	ARM_CONFIG_HAS_INTERCONNECT	= 0x2,
 	/* Whether TZC should be configured */
 	ARM_CONFIG_HAS_TZC		= 0x4
 };
diff --git a/include/plat/arm/common/arm_def.h b/include/plat/arm/common/arm_def.h
index 6049171..8d75363 100644
--- a/include/plat/arm/common/arm_def.h
+++ b/include/plat/arm/common/arm_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -235,44 +235,31 @@
 #define BL1_RO_LIMIT			(PLAT_ARM_TRUSTED_ROM_BASE	\
 					 + PLAT_ARM_TRUSTED_ROM_SIZE)
 /*
- * Put BL1 RW at the top of the Trusted SRAM. BL1_RW_BASE is calculated using
- * the current BL1 RW debug size plus a little space for growth.
+ * Put BL1 RW at the top of the Trusted SRAM.
  */
-#if TRUSTED_BOARD_BOOT
 #define BL1_RW_BASE			(ARM_BL_RAM_BASE +		\
 						ARM_BL_RAM_SIZE -	\
-						0x9000)
-#else
-#define BL1_RW_BASE			(ARM_BL_RAM_BASE +		\
-						ARM_BL_RAM_SIZE -	\
-						0x6000)
-#endif
+						PLAT_ARM_MAX_BL1_RW_SIZE)
 #define BL1_RW_LIMIT			(ARM_BL_RAM_BASE + ARM_BL_RAM_SIZE)
 
 /*******************************************************************************
  * BL2 specific defines.
  ******************************************************************************/
 /*
- * Put BL2 just below BL31. BL2_BASE is calculated using the current BL2 debug
- * size plus a little space for growth.
+ * Put BL2 just below BL31.
  */
-#if TRUSTED_BOARD_BOOT
-#define BL2_BASE			(BL31_BASE - 0x1D000)
-#else
-#define BL2_BASE			(BL31_BASE - 0xC000)
-#endif
+#define BL2_BASE			(BL31_BASE - PLAT_ARM_MAX_BL2_SIZE)
 #define BL2_LIMIT			BL31_BASE
 
 /*******************************************************************************
  * BL31 specific defines.
  ******************************************************************************/
 /*
- * Put BL31 at the top of the Trusted SRAM. BL31_BASE is calculated using the
- * current BL31 debug size plus a little space for growth.
+ * Put BL31 at the top of the Trusted SRAM.
  */
 #define BL31_BASE			(ARM_BL_RAM_BASE +		\
 						ARM_BL_RAM_SIZE -	\
-						0x1D000)
+						PLAT_ARM_MAX_BL31_SIZE)
 #define BL31_PROGBITS_LIMIT		BL1_RW_BASE
 #define BL31_LIMIT			(ARM_BL_RAM_BASE + ARM_BL_RAM_SIZE)
 
diff --git a/include/plat/arm/common/plat_arm.h b/include/plat/arm/common/plat_arm.h
index 3b6a04b..8d7e83b 100644
--- a/include/plat/arm/common/plat_arm.h
+++ b/include/plat/arm/common/plat_arm.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -37,11 +37,6 @@
 #include <stdint.h>
 #include <xlat_tables.h>
 
-/*
- * Extern declarations common to ARM standard platforms
- */
-extern const mmap_region_t plat_arm_mmap[];
-
 #define ARM_CASSERT_MMAP						\
 	CASSERT((ARRAY_SIZE(plat_arm_mmap) + ARM_BL_REGIONS)		\
 		<= MAX_MMAP_REGIONS,					\
@@ -131,9 +126,6 @@
 #endif /* __ARM_RECOM_STATE_ID_ENC__ */
 
 
-/* CCI utility functions */
-void arm_cci_init(void);
-
 /* IO storage utility functions */
 void arm_io_setup(void);
 
@@ -194,6 +186,9 @@
 void plat_arm_gic_pcpu_init(void);
 void plat_arm_security_setup(void);
 void plat_arm_pwrc_setup(void);
+void plat_arm_interconnect_init(void);
+void plat_arm_interconnect_enter_coherency(void);
+void plat_arm_interconnect_exit_coherency(void);
 
 /*
  * Optional functions required in ARM standard platforms
@@ -204,6 +199,6 @@
 	uintptr_t *dev_handle,
 	uintptr_t *image_spec);
 unsigned int plat_arm_calc_core_pos(u_register_t mpidr);
-
+const mmap_region_t *plat_arm_get_mmap(void);
 
 #endif /* __PLAT_ARM_H__ */
diff --git a/include/plat/arm/css/common/css_def.h b/include/plat/arm/css/common/css_def.h
index 7a5d193..f92126b 100644
--- a/include/plat/arm/css/common/css_def.h
+++ b/include/plat/arm/css/common/css_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -43,7 +43,6 @@
 /* Following covers CSS Peripherals excluding NSROM and NSRAM  */
 #define CSS_DEVICE_BASE			0x20000000
 #define CSS_DEVICE_SIZE			0x0e000000
-#define MHU_BASE			0x2b1f0000
 
 #define NSRAM_BASE			0x2e000000
 #define NSRAM_SIZE			0x00008000
@@ -76,31 +75,47 @@
  * SCP <=> AP boot configuration
  *
  * The SCP/AP boot configuration is a 32-bit word located at a known offset from
- * the start of the Trusted SRAM. Part of this configuration is which CPU is the
- * primary, according to the shift and mask definitions below.
+ * the start of the Trusted SRAM.
  *
  * Note that the value stored at this address is only valid at boot time, before
  * the SCP_BL2 image is transferred to SCP.
  */
-#define SCP_BOOT_CFG_ADDR		(ARM_TRUSTED_SRAM_BASE + 0x80)
-#define PRIMARY_CPU_SHIFT		8
-#define PRIMARY_CPU_BIT_WIDTH		4
-
-/*
- * Base address of the first memory region used for communication between AP
- * and SCP. Used by the BOM and SCPI protocols.
- *
- * Note that this is located at the same address as SCP_BOOT_CFG_ADDR, which
- * means the SCP/AP configuration data gets overwritten when the AP initiates
- * communication with the SCP.
- */
-#define SCP_COM_SHARED_MEM_BASE		(ARM_TRUSTED_SRAM_BASE + 0x80)
+#define SCP_BOOT_CFG_ADDR		PLAT_CSS_SCP_COM_SHARED_MEM_BASE
 
 #define CSS_MAP_DEVICE			MAP_REGION_FLAT(		\
 						CSS_DEVICE_BASE,	\
 						CSS_DEVICE_SIZE,	\
 						MT_DEVICE | MT_RW | MT_SECURE)
 
+/* Platform ID address */
+#define SSC_VERSION_OFFSET			0x040
+
+#define SSC_VERSION_CONFIG_SHIFT		28
+#define SSC_VERSION_MAJOR_REV_SHIFT		24
+#define SSC_VERSION_MINOR_REV_SHIFT		20
+#define SSC_VERSION_DESIGNER_ID_SHIFT		12
+#define SSC_VERSION_PART_NUM_SHIFT		0x0
+#define SSC_VERSION_CONFIG_MASK			0xf
+#define SSC_VERSION_MAJOR_REV_MASK		0xf
+#define SSC_VERSION_MINOR_REV_MASK		0xf
+#define SSC_VERSION_DESIGNER_ID_MASK		0xff
+#define SSC_VERSION_PART_NUM_MASK		0xfff
+
+#ifndef __ASSEMBLY__
+
+/* SSC_VERSION related accessors */
+
+/* Returns the part number of the platform */
+#define GET_SSC_VERSION_PART_NUM(val)				\
+		(((val) >> SSC_VERSION_PART_NUM_SHIFT) &	\
+		SSC_VERSION_PART_NUM_MASK)
+
+/* Returns the configuration number of the platform */
+#define GET_SSC_VERSION_CONFIG(val)				\
+		(((val) >> SSC_VERSION_CONFIG_SHIFT) &		\
+		SSC_VERSION_CONFIG_MASK)
+
+#endif /* __ASSEMBLY__ */
 
 /*************************************************************************
  * Required platform porting definitions common to all
@@ -108,6 +123,13 @@
  ************************************************************************/
 
 /*
+ * The loading of SCP images(SCP_BL2 or SCP_BL2U) is done if there
+ * respective base addresses are defined (i.e SCP_BL2_BASE, SCP_BL2U_BASE).
+ * Hence, `CSS_LOAD_SCP_IMAGES` needs to be set to 1 if BL2 needs to load
+ * an SCP_BL2/SCP_BL2U image.
+ */
+#if CSS_LOAD_SCP_IMAGES
+/*
  * Load address of SCP_BL2 in CSS platform ports
  * SCP_BL2 is loaded to the same place as BL31.  Once SCP_BL2 is transferred to the
  * SCP, it is discarded and BL31 is loaded over the top.
@@ -115,16 +137,13 @@
 #define SCP_BL2_BASE			BL31_BASE
 
 #define SCP_BL2U_BASE			BL31_BASE
+#endif /* CSS_LOAD_SCP_IMAGES */
 
 /* Load address of Non-Secure Image for CSS platform ports */
 #define PLAT_ARM_NS_IMAGE_OFFSET	0xE0000000
 
 /* TZC related constants */
 #define PLAT_ARM_TZC_FILTERS		REG_ATTR_FILTER_BIT_ALL
-#define PLAT_ARM_TZC_BASE		0x2a4a0000
-
-/* System timer related constants */
-#define PLAT_ARM_NSTIMER_FRAME_ID	1
 
 /* Trusted mailbox base address common to all CSS */
 #define PLAT_ARM_TRUSTED_MAILBOX_BASE	ARM_TRUSTED_SRAM_BASE
diff --git a/include/plat/common/common_def.h b/include/plat/common/common_def.h
index 744c22e..916720c 100644
--- a/include/plat/common/common_def.h
+++ b/include/plat/common/common_def.h
@@ -70,13 +70,6 @@
   #define MAKE_ULL(x)			x
 #endif
 
-/*
- * Macros to wrap declarations of deprecated APIs within Trusted Firmware.
- * The callers of these APIs will continue to compile with a warning as long
- * as the build flag ERROR_DEPRECATED is zero.
- */
-#define __warn_deprecated	__attribute__ ((deprecated))
-
 #define BL2_IMAGE_DESC {				\
 	.image_id = BL2_IMAGE_ID,			\
 	.image_info.h.version = VERSION_1,		\
diff --git a/include/plat/common/platform.h b/include/plat/common/platform.h
index 687c221..f37a80f 100644
--- a/include/plat/common/platform.h
+++ b/include/plat/common/platform.h
@@ -273,7 +273,7 @@
  * haven't migrated to the new platform API to compile on platforms which
  * have the compatibility layer disabled.
  */
-unsigned int platform_get_core_pos(unsigned long mpidr) __warn_deprecated;
+unsigned int platform_get_core_pos(unsigned long mpidr) __deprecated;
 
 #endif /* __ENABLE_PLAT_COMPAT__ */
 #endif /* __PLATFORM_H__ */
diff --git a/include/stdlib/sys/cdefs.h b/include/stdlib/sys/cdefs.h
index 16fb151..70c09fd 100644
--- a/include/stdlib/sys/cdefs.h
+++ b/include/stdlib/sys/cdefs.h
@@ -283,8 +283,10 @@
 
 #if __GNUC_PREREQ__(3, 1)
 #define	__noinline	__attribute__ ((__noinline__))
+#define	__deprecated	__attribute__ ((__deprecated__))
 #else
 #define	__noinline
+#define	__deprecated
 #endif
 
 #if __GNUC_PREREQ__(3, 3)
diff --git a/lib/aarch64/xlat_tables.c b/lib/aarch64/xlat_tables.c
index fa1a03d..2f2ca81 100644
--- a/lib/aarch64/xlat_tables.c
+++ b/lib/aarch64/xlat_tables.c
@@ -33,17 +33,21 @@
 #include <assert.h>
 #include <bl_common.h>
 #include <cassert.h>
+#include <debug.h>
 #include <platform_def.h>
 #include <string.h>
 #include <xlat_tables.h>
 
-
-#ifndef DEBUG_XLAT_TABLE
-#define DEBUG_XLAT_TABLE 0
-#endif
-
-#if DEBUG_XLAT_TABLE
-#define debug_print(...) printf(__VA_ARGS__)
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
+#define LVL0_SPACER ""
+#define LVL1_SPACER "  "
+#define LVL2_SPACER "    "
+#define LVL3_SPACER "      "
+#define get_level_spacer(level)		\
+			(((level) == 0) ? LVL0_SPACER : \
+			(((level) == 1) ? LVL1_SPACER : \
+			(((level) == 2) ? LVL2_SPACER : LVL3_SPACER)))
+#define debug_print(...) tf_printf(__VA_ARGS__)
 #else
 #define debug_print(...) ((void)0)
 #endif
@@ -58,7 +62,7 @@
 __aligned(NUM_L1_ENTRIES * sizeof(uint64_t));
 
 static uint64_t xlat_tables[MAX_XLAT_TABLES][XLAT_TABLE_ENTRIES]
-__aligned(XLAT_TABLE_SIZE) __attribute__((section("xlat_table")));
+__aligned(XLAT_TABLE_SIZE) __section("xlat_table");
 
 static unsigned next_xlat;
 static unsigned long max_pa;
@@ -74,12 +78,12 @@
 
 static void print_mmap(void)
 {
-#if DEBUG_XLAT_TABLE
+#if LOG_LEVEL >= LOG_LEVEL_VERBOSE
 	debug_print("mmap:\n");
 	mmap_region_t *mm = mmap;
 	while (mm->size) {
-		debug_print(" %010lx %010lx %10lx %x\n", mm->base_va,
-					mm->base_pa, mm->size, mm->attr);
+		debug_print(" VA:0x%lx  PA:0x%lx  size:0x%lx  attr:0x%x\n",
+				mm->base_va, mm->base_pa, mm->size, mm->attr);
 		++mm;
 	};
 	debug_print("\n");
@@ -209,8 +213,8 @@
 			continue;
 		}
 
-		debug_print("      %010lx %8lx " + 6 - 2 * level, base_va,
-				level_size);
+		debug_print("%s VA:0x%lx size:0x%x ", get_level_spacer(level),
+				base_va, level_size);
 
 		if (mm->base_va >= base_va + level_size) {
 			/* Next region is after area so nothing to map yet */
diff --git a/lib/cpus/aarch64/cortex_a35.S b/lib/cpus/aarch64/cortex_a35.S
index 6a447c0..ba29d6d 100644
--- a/lib/cpus/aarch64/cortex_a35.S
+++ b/lib/cpus/aarch64/cortex_a35.S
@@ -67,16 +67,12 @@
 	 */
 func cortex_a35_reset_func
 	/* ---------------------------------------------
-	 * As a bare minimum enable the SMP bit if it is
-	 * not already set.
+	 * Enable the SMP bit.
 	 * ---------------------------------------------
 	 */
 	mrs	x0, CORTEX_A35_CPUECTLR_EL1
-	tst	x0, #CORTEX_A35_CPUECTLR_SMPEN_BIT
-	b.ne	skip_smp_setup
 	orr	x0, x0, #CORTEX_A35_CPUECTLR_SMPEN_BIT
 	msr	CORTEX_A35_CPUECTLR_EL1, x0
-skip_smp_setup:
 	isb
 	ret
 endfunc cortex_a35_reset_func
diff --git a/lib/cpus/aarch64/cortex_a53.S b/lib/cpus/aarch64/cortex_a53.S
index e4b94e8..c33ba57 100644
--- a/lib/cpus/aarch64/cortex_a53.S
+++ b/lib/cpus/aarch64/cortex_a53.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -86,33 +86,40 @@
 	ret
 endfunc errata_a53_826319_wa
 
-	/* --------------------------------------------------
-	 * Errata Workaround for Cortex A53 Errata #836870.
-	 * This applies only to revision <= r0p3 of Cortex A53.
-	 * From r0p4 and onwards, this errata is enabled by
-	 * default.
+	/* ---------------------------------------------------------------------
+	 * Disable the cache non-temporal hint.
+	 *
+	 * This ignores the Transient allocation hint in the MAIR and treats
+	 * allocations the same as non-transient allocation types. As a result,
+	 * the LDNP and STNP instructions in AArch64 behave the same as the
+	 * equivalent LDP and STP instructions.
+	 *
+	 * This is relevant only for revisions <= r0p3 of Cortex-A53.
+	 * From r0p4 and onwards, the bit to disable the hint is enabled by
+	 * default at reset.
+	 *
 	 * Inputs:
 	 * x0: variant[4:7] and revision[0:3] of current cpu.
 	 * Clobbers : x0 - x5
-	 * --------------------------------------------------
+	 * ---------------------------------------------------------------------
 	 */
-func errata_a53_836870_wa
+func a53_disable_non_temporal_hint
 	/*
 	 * Compare x0 against revision r0p3
 	 */
 	cmp	x0, #3
-	b.ls	apply_836870
+	b.ls	disable_hint
 #if DEBUG
 	b	print_revision_warning
 #else
 	ret
 #endif
-apply_836870:
+disable_hint:
 	mrs	x1, CPUACTLR_EL1
 	orr	x1, x1, #CPUACTLR_DTAH
 	msr	CPUACTLR_EL1, x1
 	ret
-endfunc errata_a53_836870_wa
+endfunc a53_disable_non_temporal_hint
 
 	/* -------------------------------------------------
 	 * The CPU Ops reset function for Cortex-A53.
@@ -138,22 +145,18 @@
 	bl	errata_a53_826319_wa
 #endif
 
-#if ERRATA_A53_836870
+#if ERRATA_A53_836870 || A53_DISABLE_NON_TEMPORAL_HINT
 	mov	x0, x15
-	bl	errata_a53_836870_wa
+	bl	a53_disable_non_temporal_hint
 #endif
 
 	/* ---------------------------------------------
-	 * As a bare minimum enable the SMP bit if it is
-	 * not already set.
+	 * Enable the SMP bit.
 	 * ---------------------------------------------
 	 */
 	mrs	x0, CPUECTLR_EL1
-	tst	x0, #CPUECTLR_SMP_BIT
-	b.ne	skip_smp_setup
 	orr	x0, x0, #CPUECTLR_SMP_BIT
 	msr	CPUECTLR_EL1, x0
-skip_smp_setup:
 	isb
 	ret	x19
 endfunc cortex_a53_reset_func
diff --git a/lib/cpus/aarch64/cortex_a57.S b/lib/cpus/aarch64/cortex_a57.S
index 05799d6..99db25b 100644
--- a/lib/cpus/aarch64/cortex_a57.S
+++ b/lib/cpus/aarch64/cortex_a57.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -105,17 +105,9 @@
 	ret
 #endif
 apply_806969:
-	/*
-	 * Test if errata has already been applied in an earlier
-	 * invocation of the reset handler and does not need to
-	 * be applied again.
-	 */
 	mrs	x1, CPUACTLR_EL1
-	tst	x1, #CPUACTLR_NO_ALLOC_WBWA
-	b.ne	skip_806969
 	orr	x1, x1, #CPUACTLR_NO_ALLOC_WBWA
 	msr	CPUACTLR_EL1, x1
-skip_806969:
 	ret
 endfunc errata_a57_806969_wa
 
@@ -139,20 +131,41 @@
 	ret
 #endif
 apply_813420:
-	/*
-	 * Test if errata has already been applied in an earlier
-	 * invocation of the reset handler and does not need to
-	 * be applied again.
-	 */
 	mrs	x1, CPUACTLR_EL1
-	tst	x1, #CPUACTLR_DCC_AS_DCCI
-	b.ne	skip_813420
 	orr	x1, x1, #CPUACTLR_DCC_AS_DCCI
 	msr	CPUACTLR_EL1, x1
-skip_813420:
 	ret
 endfunc errata_a57_813420_wa
 
+	/* --------------------------------------------------------------------
+	 * Disable the over-read from the LDNP instruction.
+	 *
+	 * This applies to all revisions <= r1p2. The performance degradation
+	 * observed with LDNP/STNP has been fixed on r1p3 and onwards.
+	 *
+	 * Inputs:
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Clobbers : x0 - x5, x30
+	 * ---------------------------------------------------------------------
+	 */
+func a57_disable_ldnp_overread
+	/*
+	 * Compare x0 against revision r1p2
+	 */
+	cmp	x0, #0x12
+	b.ls	disable_hint
+#if DEBUG
+	b	print_revision_warning
+#else
+	ret
+#endif
+disable_hint:
+	mrs	x1, CPUACTLR_EL1
+	orr	x1, x1, #CPUACTLR_DIS_OVERREAD
+	msr	CPUACTLR_EL1, x1
+	ret
+endfunc a57_disable_ldnp_overread
+
 	/* -------------------------------------------------
 	 * The CPU Ops reset function for Cortex-A57.
 	 * Clobbers: x0-x5, x15, x19, x30
@@ -181,17 +194,18 @@
 	bl	errata_a57_813420_wa
 #endif
 
+#if A57_DISABLE_NON_TEMPORAL_HINT
+	mov	x0, x15
+	bl	a57_disable_ldnp_overread
+#endif
+
 	/* ---------------------------------------------
-	 * As a bare minimum enable the SMP bit if it is
-	 * not already set.
+	 * Enable the SMP bit.
 	 * ---------------------------------------------
 	 */
 	mrs	x0, CPUECTLR_EL1
-	tst	x0, #CPUECTLR_SMP_BIT
-	b.ne	skip_smp_setup
 	orr	x0, x0, #CPUECTLR_SMP_BIT
 	msr	CPUECTLR_EL1, x0
-skip_smp_setup:
 	isb
 	ret	x19
 endfunc cortex_a57_reset_func
diff --git a/lib/cpus/aarch64/cpu_helpers.S b/lib/cpus/aarch64/cpu_helpers.S
index e8a1392..e41d95b 100644
--- a/lib/cpus/aarch64/cpu_helpers.S
+++ b/lib/cpus/aarch64/cpu_helpers.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -205,16 +205,17 @@
 endfunc get_cpu_ops_ptr
 
 #if DEBUG
-	/*
-	 * This function prints a warning message to the crash console
-	 * if the CPU revision/part number does not match the errata
-	 * workaround enabled in the build.
-	 * Clobber: x30, x0 - x5
-	 */
 .section .rodata.rev_warn_str, "aS"
 rev_warn_str:
-	.asciz "Warning: Skipping Errata workaround for non matching CPU revision number.\n"
+	.asciz "Warning: Skipping CPU specific reset operation for non-matching CPU revision number.\n"
 
+	/*
+	 * This function prints the above warning message to the crash console.
+	 * It should be called when a CPU specific operation is enabled in the
+	 * build but doesn't apply to this CPU revision/part number.
+	 *
+	 * Clobber: x30, x0 - x5
+	 */
 	.globl	print_revision_warning
 func print_revision_warning
 	mov	x5, x30
diff --git a/lib/cpus/cpu-ops.mk b/lib/cpus/cpu-ops.mk
index a872360..a3a08e1 100644
--- a/lib/cpus/cpu-ops.mk
+++ b/lib/cpus/cpu-ops.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2014-2016, ARM Limited and Contributors. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions are met:
@@ -32,10 +32,26 @@
 # cluster is powered down.
 SKIP_A57_L1_FLUSH_PWR_DWN	?=0
 
+# Flag to disable the cache non-temporal hint.
+# It is enabled by default.
+A53_DISABLE_NON_TEMPORAL_HINT	?=1
+
+# Flag to disable the cache non-temporal hint.
+# It is enabled by default.
+A57_DISABLE_NON_TEMPORAL_HINT	?=1
+
 # Process SKIP_A57_L1_FLUSH_PWR_DWN flag
 $(eval $(call assert_boolean,SKIP_A57_L1_FLUSH_PWR_DWN))
 $(eval $(call add_define,SKIP_A57_L1_FLUSH_PWR_DWN))
 
+# Process A53_DISABLE_NON_TEMPORAL_HINT flag
+$(eval $(call assert_boolean,A53_DISABLE_NON_TEMPORAL_HINT))
+$(eval $(call add_define,A53_DISABLE_NON_TEMPORAL_HINT))
+
+# Process A57_DISABLE_NON_TEMPORAL_HINT flag
+$(eval $(call assert_boolean,A57_DISABLE_NON_TEMPORAL_HINT))
+$(eval $(call add_define,A57_DISABLE_NON_TEMPORAL_HINT))
+
 
 # CPU Errata Build flags. These should be enabled by the
 # platform if the errata needs to be applied.
diff --git a/make_helpers/build_macros.mk b/make_helpers/build_macros.mk
index d6a4e3a..b22eaf9 100644
--- a/make_helpers/build_macros.mk
+++ b/make_helpers/build_macros.mk
@@ -290,6 +290,10 @@
         $(if $(3),$(eval $(call FIP_ADD_PAYLOAD,$(2),--$(3),bl$(1))))
 endef
 
+# Allow overriding the timestamp, for example for reproducible builds, or to
+# synchronize timestamps across multiple projects.
+# This must be set to a C string (including quotes where applicable).
+BUILD_MESSAGE_TIMESTAMP ?= __TIME__", "__DATE__
 
 # MAKE_BL macro defines the targets and options to build each BL image.
 # Arguments:
@@ -315,7 +319,7 @@
 
 $(ELF): $(OBJS) $(LINKERFILE)
 	@echo "  LD      $$@"
-	@echo 'const char build_message[] = "Built : "__TIME__", "__DATE__; \
+	@echo 'const char build_message[] = "Built : "$(BUILD_MESSAGE_TIMESTAMP); \
 	       const char version_string[] = "${VERSION_STRING}";' | \
 		$$(CC) $$(CFLAGS) -xc - -o $(BUILD_DIR)/build_message.o
 	$$(Q)$$(LD) -o $$@ $$(LDFLAGS) -Map=$(MAPFILE) --script $(LINKERFILE) \
diff --git a/plat/arm/board/fvp/aarch64/fvp_common.c b/plat/arm/board/fvp/aarch64/fvp_common.c
index 305505d..f684d97 100644
--- a/plat/arm/board/fvp/aarch64/fvp_common.c
+++ b/plat/arm/board/fvp/aarch64/fvp_common.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -30,7 +30,6 @@
 
 #include <arm_config.h>
 #include <arm_def.h>
-#include <cci.h>
 #include <debug.h>
 #include <gicv2.h>
 #include <mmio.h>
@@ -50,9 +49,9 @@
 /*******************************************************************************
  * arm_config holds the characteristics of the differences between the three FVP
  * platforms (Base, A53_A57 & Foundation). It will be populated during cold boot
- * at each boot stage by the primary before enabling the MMU (to allow cci
- * configuration) & used thereafter. Each BL will have its own copy to allow
- * independent operation.
+ * at each boot stage by the primary before enabling the MMU (to allow
+ * interconnect configuration) & used thereafter. Each BL will have its own copy
+ * to allow independent operation.
  ******************************************************************************/
 arm_config_t arm_config;
 
@@ -209,7 +208,7 @@
 		break;
 	case HBI_BASE_FVP:
 		arm_config.flags |= ARM_CONFIG_BASE_MMAP |
-			ARM_CONFIG_HAS_CCI | ARM_CONFIG_HAS_TZC;
+			ARM_CONFIG_HAS_INTERCONNECT | ARM_CONFIG_HAS_TZC;
 
 		/*
 		 * Check for supported revisions
@@ -230,23 +229,20 @@
 }
 
 
-void fvp_cci_init(void)
+void fvp_interconnect_init(void)
 {
-	/*
-	 * Initialize CCI-400 driver
-	 */
-	if (arm_config.flags & ARM_CONFIG_HAS_CCI)
-		arm_cci_init();
+	if (arm_config.flags & ARM_CONFIG_HAS_INTERCONNECT)
+		plat_arm_interconnect_init();
 }
 
-void fvp_cci_enable(void)
+void fvp_interconnect_enable(void)
 {
-	if (arm_config.flags & ARM_CONFIG_HAS_CCI)
-		cci_enable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr()));
+	if (arm_config.flags & ARM_CONFIG_HAS_INTERCONNECT)
+		plat_arm_interconnect_enter_coherency();
 }
 
-void fvp_cci_disable(void)
+void fvp_interconnect_disable(void)
 {
-	if (arm_config.flags & ARM_CONFIG_HAS_CCI)
-		cci_disable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr()));
+	if (arm_config.flags & ARM_CONFIG_HAS_INTERCONNECT)
+		plat_arm_interconnect_exit_coherency();
 }
diff --git a/plat/arm/board/fvp/fvp_bl1_setup.c b/plat/arm/board/fvp/fvp_bl1_setup.c
index 91bc9c4..cc7feae 100644
--- a/plat/arm/board/fvp/fvp_bl1_setup.c
+++ b/plat/arm/board/fvp/fvp_bl1_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -44,14 +44,14 @@
 	fvp_config_setup();
 
 	/*
-	 * Initialize CCI for this cluster during cold boot.
+	 * Initialize Interconnect for this cluster during cold boot.
 	 * No need for locks as no other CPU is active.
 	 */
-	fvp_cci_init();
+	fvp_interconnect_init();
 	/*
-	 * Enable CCI coherency for the primary CPU's cluster.
+	 * Enable coherency in Interconnect for the primary CPU's cluster.
 	 */
-	fvp_cci_enable();
+	fvp_interconnect_enable();
 }
 
 /*******************************************************************************
diff --git a/plat/arm/board/fvp/fvp_bl31_setup.c b/plat/arm/board/fvp/fvp_bl31_setup.c
index f29af64..2ee3ba5 100644
--- a/plat/arm/board/fvp/fvp_bl31_setup.c
+++ b/plat/arm/board/fvp/fvp_bl31_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -41,17 +41,17 @@
 	fvp_config_setup();
 
 	/*
-	 * Initialize CCI for this cluster during cold boot.
-	 * No need for locks as no other CPU is active.
+	 * Initialize the correct interconnect for this cluster during cold
+	 * boot. No need for locks as no other CPU is active.
 	 */
-	fvp_cci_init();
+	fvp_interconnect_init();
 
 	/*
-	 * Enable CCI coherency for the primary CPU's cluster.
+	 * Enable coherency in interconnect for the primary CPU's cluster.
 	 * Earlier bootloader stages might already do this (e.g. Trusted
 	 * Firmware's BL1 does it) but we can't assume so. There is no harm in
 	 * executing this code twice anyway.
 	 * FVP PSCI code will enable coherency for other clusters.
 	 */
-	fvp_cci_enable();
+	fvp_interconnect_enable();
 }
diff --git a/plat/arm/board/fvp/fvp_pm.c b/plat/arm/board/fvp/fvp_pm.c
index f959fab..3976ef2 100644
--- a/plat/arm/board/fvp/fvp_pm.c
+++ b/plat/arm/board/fvp/fvp_pm.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -86,7 +86,7 @@
 	uint64_t mpidr = read_mpidr_el1();
 
 	/* Disable coherency if this cluster is to be turned off */
-	fvp_cci_disable();
+	fvp_interconnect_disable();
 
 	/* Program the power controller to turn the cluster off */
 	fvp_pwrc_write_pcoffr(mpidr);
@@ -117,7 +117,7 @@
 		fvp_pwrc_write_pponr(mpidr);
 
 		/* Enable coherency if this cluster was off */
-		fvp_cci_enable();
+		fvp_interconnect_enable();
 	}
 
 	/*
diff --git a/plat/arm/board/fvp/fvp_private.h b/plat/arm/board/fvp/fvp_private.h
index e88a45e..bb115e1 100644
--- a/plat/arm/board/fvp/fvp_private.h
+++ b/plat/arm/board/fvp/fvp_private.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -39,9 +39,9 @@
 
 void fvp_config_setup(void);
 
-void fvp_cci_init(void);
-void fvp_cci_enable(void);
-void fvp_cci_disable(void);
+void fvp_interconnect_init(void);
+void fvp_interconnect_enable(void);
+void fvp_interconnect_disable(void);
 
 
 #endif /* __FVP_PRIVATE_H__ */
diff --git a/plat/arm/board/fvp/include/plat_macros.S b/plat/arm/board/fvp/include/plat_macros.S
index 2117843..df66a52 100644
--- a/plat/arm/board/fvp/include/plat_macros.S
+++ b/plat/arm/board/fvp/include/plat_macros.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -31,6 +31,7 @@
 #define __PLAT_MACROS_S__
 
 #include <arm_macros.S>
+#include <cci_macros.S>
 #include <v2m_def.h>
 #include "../fvp_def.h"
 
diff --git a/plat/arm/board/fvp/include/platform_def.h b/plat/arm/board/fvp/include/platform_def.h
index 9b85342..c5e3095 100644
--- a/plat/arm/board/fvp/include/platform_def.h
+++ b/plat/arm/board/fvp/include/platform_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -142,4 +142,30 @@
 
 #define PLAT_ARM_G0_IRQS		ARM_G0_IRQS
 
+/*
+ * PLAT_ARM_MAX_BL1_RW_SIZE is calculated using the current BL1 RW debug size
+ * plus a little space for growth.
+ */
+#if TRUSTED_BOARD_BOOT
+# define PLAT_ARM_MAX_BL1_RW_SIZE	0x9000
+#else
+# define PLAT_ARM_MAX_BL1_RW_SIZE	0x6000
+#endif
+
+/*
+ * PLAT_ARM_MAX_BL2_SIZE is calculated using the current BL2 debug size plus a
+ * little space for growth.
+ */
+#if TRUSTED_BOARD_BOOT
+# define PLAT_ARM_MAX_BL2_SIZE		0x1D000
+#else
+# define PLAT_ARM_MAX_BL2_SIZE		0xC000
+#endif
+
+/*
+ * PLAT_ARM_MAX_BL31_SIZE is calculated using the current BL31 debug size plus a
+ * little space for growth.
+ */
+#define PLAT_ARM_MAX_BL31_SIZE		0x1D000
+
 #endif /* __PLATFORM_DEF_H__ */
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index 22df6d7..c82c21a 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -57,22 +57,35 @@
 $(error "Incorrect GIC driver chosen on FVP port")
 endif
 
+FVP_INTERCONNECT_SOURCES	:= 	drivers/arm/cci/cci.c		\
+					plat/arm/common/arm_cci.c
+
+FVP_SECURITY_SOURCES	:=	drivers/arm/tzc400/tzc400.c		\
+				plat/arm/board/fvp/fvp_security.c	\
+				plat/arm/common/arm_tzc400.c
+
+
 PLAT_INCLUDES		:=	-Iplat/arm/board/fvp/include
 
 
 PLAT_BL_COMMON_SOURCES	:=	plat/arm/board/fvp/aarch64/fvp_common.c
 
-BL1_SOURCES		+=	drivers/io/io_semihosting.c			\
-				lib/cpus/aarch64/aem_generic.S			\
+FVP_CPU_LIBS		:=	lib/cpus/aarch64/aem_generic.S			\
 				lib/cpus/aarch64/cortex_a35.S			\
 				lib/cpus/aarch64/cortex_a53.S			\
 				lib/cpus/aarch64/cortex_a57.S			\
+				lib/cpus/aarch64/cortex_a72.S
+
+BL1_SOURCES		+=	drivers/io/io_semihosting.c			\
 				lib/semihosting/semihosting.c			\
 				lib/semihosting/aarch64/semihosting_call.S	\
 				plat/arm/board/fvp/aarch64/fvp_helpers.S	\
 				plat/arm/board/fvp/fvp_bl1_setup.c		\
 				plat/arm/board/fvp/fvp_err.c			\
-				plat/arm/board/fvp/fvp_io_storage.c
+				plat/arm/board/fvp/fvp_io_storage.c		\
+				${FVP_CPU_LIBS}					\
+				${FVP_INTERCONNECT_SOURCES}
+
 
 BL2_SOURCES		+=	drivers/arm/sp804/sp804_delay_timer.c		\
 				drivers/io/io_semihosting.c			\
@@ -82,22 +95,20 @@
 				plat/arm/board/fvp/fvp_bl2_setup.c		\
 				plat/arm/board/fvp/fvp_err.c			\
 				plat/arm/board/fvp/fvp_io_storage.c		\
-				plat/arm/board/fvp/fvp_security.c
+				${FVP_SECURITY_SOURCES}
 
 BL2U_SOURCES		+=	plat/arm/board/fvp/fvp_bl2u_setup.c		\
-				plat/arm/board/fvp/fvp_security.c
+				${FVP_SECURITY_SOURCES}
 
-BL31_SOURCES		+=	lib/cpus/aarch64/aem_generic.S			\
-				lib/cpus/aarch64/cortex_a35.S			\
-				lib/cpus/aarch64/cortex_a53.S			\
-				lib/cpus/aarch64/cortex_a57.S			\
-				plat/arm/board/fvp/fvp_bl31_setup.c		\
+BL31_SOURCES		+=	plat/arm/board/fvp/fvp_bl31_setup.c		\
 				plat/arm/board/fvp/fvp_pm.c			\
-				plat/arm/board/fvp/fvp_security.c		\
 				plat/arm/board/fvp/fvp_topology.c		\
 				plat/arm/board/fvp/aarch64/fvp_helpers.S	\
 				plat/arm/board/fvp/drivers/pwrc/fvp_pwrc.c	\
-				${FVP_GIC_SOURCES}
+				${FVP_CPU_LIBS}					\
+				${FVP_GIC_SOURCES}				\
+				${FVP_INTERCONNECT_SOURCES}			\
+				${FVP_SECURITY_SOURCES}
 
 # Disable the PSCI platform compatibility layer
 ENABLE_PLAT_COMPAT	:= 	0
diff --git a/plat/arm/board/juno/include/plat_macros.S b/plat/arm/board/juno/include/plat_macros.S
index db0c1d2..d2a88ed 100644
--- a/plat/arm/board/juno/include/plat_macros.S
+++ b/plat/arm/board/juno/include/plat_macros.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -30,6 +30,7 @@
 #ifndef __PLAT_MACROS_S__
 #define __PLAT_MACROS_S__
 
+#include <cci_macros.S>
 #include <css_macros.S>
 
 /*
diff --git a/plat/arm/board/juno/include/platform_def.h b/plat/arm/board/juno/include/platform_def.h
index 924eb0a..deac0ff 100644
--- a/plat/arm/board/juno/include/platform_def.h
+++ b/plat/arm/board/juno/include/platform_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -76,7 +76,11 @@
 #define PLAT_ARM_CCI_CLUSTER0_SL_IFACE_IX	4
 #define PLAT_ARM_CCI_CLUSTER1_SL_IFACE_IX	3
 
+/* System timer related constants */
+#define PLAT_ARM_NSTIMER_FRAME_ID		1
+
 /* TZC related constants */
+#define PLAT_ARM_TZC_BASE		0x2a4a0000
 #define PLAT_ARM_TZC_NS_DEV_ACCESS	(				\
 		TZC_REGION_ACCESS_RDWR(TZC400_NSAID_CCI400)	|	\
 		TZC_REGION_ACCESS_RDWR(TZC400_NSAID_PCIE)	|	\
@@ -99,6 +103,23 @@
 #define PLAT_ARM_GICH_BASE		0x2c04f000
 #define PLAT_ARM_GICV_BASE		0x2c06f000
 
+/* MHU related constants */
+#define PLAT_CSS_MHU_BASE		0x2b1f0000
+
+/*
+ * Base address of the first memory region used for communication between AP
+ * and SCP. Used by the BOM and SCPI protocols.
+ *
+ * Note that this is located at the same address as SCP_BOOT_CFG_ADDR, which
+ * means the SCP/AP configuration data gets overwritten when the AP initiates
+ * communication with the SCP. The configuration data is expected to be a
+ * 32-bit word on all CSS platforms. On Juno, part of this configuration is
+ * which CPU is the primary, according to the shift and mask definitions below.
+ */
+#define PLAT_CSS_SCP_COM_SHARED_MEM_BASE	(ARM_TRUSTED_SRAM_BASE + 0x80)
+#define PLAT_CSS_PRIMARY_CPU_SHIFT		8
+#define PLAT_CSS_PRIMARY_CPU_BIT_WIDTH		4
+
 /*
  * Define a list of Group 1 Secure and Group 0 interrupts as per GICv3
  * terminology. On a GICv2 system or mode, the lists will be merged and treated
@@ -124,5 +145,30 @@
 /* CSS SoC NIC-400 Global Programmers View (GPV) */
 #define PLAT_SOC_CSS_NIC400_BASE	0x2a000000
 
+/*
+ * PLAT_ARM_MAX_BL1_RW_SIZE is calculated using the current BL1 RW debug size
+ * plus a little space for growth.
+ */
+#if TRUSTED_BOARD_BOOT
+# define PLAT_ARM_MAX_BL1_RW_SIZE	0x9000
+#else
+# define PLAT_ARM_MAX_BL1_RW_SIZE	0x6000
+#endif
+
+/*
+ * PLAT_ARM_MAX_BL2_SIZE is calculated using the current BL2 debug size plus a
+ * little space for growth.
+ */
+#if TRUSTED_BOARD_BOOT
+# define PLAT_ARM_MAX_BL2_SIZE		0x1D000
+#else
+# define PLAT_ARM_MAX_BL2_SIZE		0xC000
+#endif
+
+/*
+ * PLAT_ARM_MAX_BL31_SIZE is calculated using the current BL31 debug size plus a
+ * little space for growth.
+ */
+#define PLAT_ARM_MAX_BL31_SIZE		0x1D000
 
 #endif /* __PLATFORM_DEF_H__ */
diff --git a/plat/arm/board/juno/juno_def.h b/plat/arm/board/juno/juno_def.h
index 1f367f2..f4e2259 100644
--- a/plat/arm/board/juno/juno_def.h
+++ b/plat/arm/board/juno/juno_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -50,6 +50,7 @@
 #define PSRAM_BASE			0x14000000
 #define PSRAM_SIZE			0x02000000
 
+#define JUNO_SSC_VER_PART_NUM		0x030
 
 /*******************************************************************************
  * TZC-400 related constants
diff --git a/plat/arm/board/juno/platform.mk b/plat/arm/board/juno/platform.mk
index fae30e7..3ffc7e7 100644
--- a/plat/arm/board/juno/platform.mk
+++ b/plat/arm/board/juno/platform.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2013-2016, ARM Limited and Contributors. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions are met:
@@ -34,6 +34,14 @@
 				plat/common/plat_gicv2.c		\
 				plat/arm/common/arm_gicv2.c
 
+JUNO_INTERCONNECT_SOURCES	:=	drivers/arm/cci/cci.c		\
+					plat/arm/common/arm_cci.c
+
+JUNO_SECURITY_SOURCES	:=	drivers/arm/tzc400/tzc400.c		\
+				plat/arm/board/juno/juno_security.c	\
+				plat/arm/common/arm_tzc400.c
+
+
 PLAT_INCLUDES		:=	-Iplat/arm/board/juno/include
 
 PLAT_BL_COMMON_SOURCES	:=	plat/arm/board/juno/aarch64/juno_helpers.S
@@ -42,19 +50,21 @@
 				lib/cpus/aarch64/cortex_a57.S		\
 				lib/cpus/aarch64/cortex_a72.S		\
 				plat/arm/board/juno/juno_bl1_setup.c	\
-				plat/arm/board/juno/juno_err.c
+				plat/arm/board/juno/juno_err.c		\
+				${JUNO_INTERCONNECT_SOURCES}
 
-BL2_SOURCES		+=	plat/arm/board/juno/juno_security.c	\
-				plat/arm/board/juno/juno_err.c
+BL2_SOURCES		+=	plat/arm/board/juno/juno_err.c		\
+				${JUNO_SECURITY_SOURCES}
 
-BL2U_SOURCES		+=	plat/arm/board/juno/juno_security.c
+BL2U_SOURCES		+=	${JUNO_SECURITY_SOURCES}
 
 BL31_SOURCES		+=	lib/cpus/aarch64/cortex_a53.S		\
 				lib/cpus/aarch64/cortex_a57.S		\
 				lib/cpus/aarch64/cortex_a72.S		\
 				plat/arm/board/juno/juno_pm.c		\
-				plat/arm/board/juno/juno_security.c	\
-				${JUNO_GIC_SOURCES}
+				${JUNO_GIC_SOURCES}			\
+				${JUNO_INTERCONNECT_SOURCES}		\
+				${JUNO_SECURITY_SOURCES}
 
 # Enable workarounds for selected Cortex-A57 erratas.
 ERRATA_A57_806969	:=	0
diff --git a/plat/arm/common/aarch64/arm_common.c b/plat/arm/common/aarch64/arm_common.c
index d42009d..c84a65b 100644
--- a/plat/arm/common/aarch64/arm_common.c
+++ b/plat/arm/common/aarch64/arm_common.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -29,20 +29,16 @@
  */
 #include <arch.h>
 #include <arch_helpers.h>
-#include <cci.h>
 #include <mmio.h>
 #include <plat_arm.h>
 #include <platform_def.h>
 #include <xlat_tables.h>
 
-
-static const int cci_map[] = {
-	PLAT_ARM_CCI_CLUSTER0_SL_IFACE_IX,
-	PLAT_ARM_CCI_CLUSTER1_SL_IFACE_IX
-};
+extern const mmap_region_t plat_arm_mmap[];
 
 /* Weak definitions may be overridden in specific ARM standard platform */
 #pragma weak plat_get_ns_image_entrypoint
+#pragma weak plat_arm_get_mmap
 
 
 /*******************************************************************************
@@ -67,7 +63,7 @@
 		mmap_add_region(coh_start, coh_start,			\
 				coh_limit - coh_start,			\
 				MT_DEVICE | MT_RW | MT_SECURE);		\
-		mmap_add(plat_arm_mmap);				\
+		mmap_add(plat_arm_get_mmap());				\
 		init_xlat_tables();					\
 									\
 		enable_mmu_el##_el(0);					\
@@ -85,7 +81,7 @@
 		mmap_add_region(ro_start, ro_start,			\
 				ro_limit - ro_start,			\
 				MT_MEMORY | MT_RO | MT_SECURE);		\
-		mmap_add(plat_arm_mmap);				\
+		mmap_add(plat_arm_get_mmap());				\
 		init_xlat_tables();					\
 									\
 		enable_mmu_el##_el(0);					\
@@ -138,12 +134,6 @@
 	return spsr;
 }
 
-
-void arm_cci_init(void)
-{
-	cci_init(PLAT_ARM_CCI_BASE, cci_map, ARRAY_SIZE(cci_map));
-}
-
 /*******************************************************************************
  * Configures access to the system counter timer module.
  ******************************************************************************/
@@ -151,11 +141,21 @@
 {
 	unsigned int reg_val;
 
+#if ARM_CONFIG_CNTACR
 	reg_val = (1 << CNTACR_RPCT_SHIFT) | (1 << CNTACR_RVCT_SHIFT);
 	reg_val |= (1 << CNTACR_RFRQ_SHIFT) | (1 << CNTACR_RVOFF_SHIFT);
 	reg_val |= (1 << CNTACR_RWVT_SHIFT) | (1 << CNTACR_RWPT_SHIFT);
 	mmio_write_32(ARM_SYS_TIMCTL_BASE + CNTACR_BASE(PLAT_ARM_NSTIMER_FRAME_ID), reg_val);
+#endif /* ARM_CONFIG_CNTACR */
 
 	reg_val = (1 << CNTNSAR_NS_SHIFT(PLAT_ARM_NSTIMER_FRAME_ID));
 	mmio_write_32(ARM_SYS_TIMCTL_BASE + CNTNSAR, reg_val);
 }
+
+/*******************************************************************************
+ * Returns ARM platform specific memory map regions.
+ ******************************************************************************/
+const mmap_region_t *plat_arm_get_mmap(void)
+{
+	return plat_arm_mmap;
+}
diff --git a/plat/arm/common/aarch64/arm_helpers.S b/plat/arm/common/aarch64/arm_helpers.S
index 87179da..a0338f1 100644
--- a/plat/arm/common/aarch64/arm_helpers.S
+++ b/plat/arm/common/aarch64/arm_helpers.S
@@ -66,7 +66,7 @@
 	 * int plat_crash_console_init(void)
 	 * Function to initialize the crash console
 	 * without a C Runtime to print crash report.
-	 * Clobber list : x0, x1, x2
+	 * Clobber list : x0 - x4
 	 * ---------------------------------------------
 	 */
 func plat_crash_console_init
diff --git a/plat/arm/common/arm_bl1_setup.c b/plat/arm/common/arm_bl1_setup.c
index d0a4c0b..951f48a 100644
--- a/plat/arm/common/arm_bl1_setup.c
+++ b/plat/arm/common/arm_bl1_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -31,7 +31,6 @@
 #include <arch.h>
 #include <arm_def.h>
 #include <bl_common.h>
-#include <cci.h>
 #include <console.h>
 #include <platform_def.h>
 #include <plat_arm.h>
@@ -101,14 +100,14 @@
 	arm_bl1_early_platform_setup();
 
 	/*
-	 * Initialize CCI for this cluster during cold boot.
+	 * Initialize Interconnect for this cluster during cold boot.
 	 * No need for locks as no other CPU is active.
 	 */
-	arm_cci_init();
+	plat_arm_interconnect_init();
 	/*
-	 * Enable CCI coherency for the primary CPU's cluster.
+	 * Enable Interconnect coherency for the primary CPU's cluster.
 	 */
-	cci_enable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr()));
+	plat_arm_interconnect_enter_coherency();
 }
 
 /******************************************************************************
diff --git a/plat/arm/common/arm_bl31_setup.c b/plat/arm/common/arm_bl31_setup.c
index 6c58ff1..5cc8bfb 100644
--- a/plat/arm/common/arm_bl31_setup.c
+++ b/plat/arm/common/arm_bl31_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -33,7 +33,6 @@
 #include <arm_def.h>
 #include <assert.h>
 #include <bl_common.h>
-#include <cci.h>
 #include <console.h>
 #include <debug.h>
 #include <mmio.h>
@@ -178,20 +177,20 @@
 	arm_bl31_early_platform_setup(from_bl2, plat_params_from_bl2);
 
 	/*
-	 * Initialize CCI for this cluster during cold boot.
+	 * Initialize Interconnect for this cluster during cold boot.
 	 * No need for locks as no other CPU is active.
 	 */
-	arm_cci_init();
+	plat_arm_interconnect_init();
 
 	/*
-	 * Enable CCI coherency for the primary CPU's cluster.
+	 * Enable Interconnect coherency for the primary CPU's cluster.
 	 * Earlier bootloader stages might already do this (e.g. Trusted
 	 * Firmware's BL1 does it) but we can't assume so. There is no harm in
 	 * executing this code twice anyway.
 	 * Platform specific PSCI code will enable coherency for other
 	 * clusters.
 	 */
-	cci_enable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr()));
+	plat_arm_interconnect_enter_coherency();
 }
 
 /*******************************************************************************
diff --git a/plat/arm/common/arm_cci.c b/plat/arm/common/arm_cci.c
new file mode 100644
index 0000000..41054c2
--- /dev/null
+++ b/plat/arm/common/arm_cci.c
@@ -0,0 +1,72 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * 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.
+ */
+
+#include <arch.h>
+#include <cci.h>
+#include <plat_arm.h>
+#include <platform_def.h>
+
+static const int cci_map[] = {
+	PLAT_ARM_CCI_CLUSTER0_SL_IFACE_IX,
+	PLAT_ARM_CCI_CLUSTER1_SL_IFACE_IX
+};
+
+/******************************************************************************
+ * The following functions are defined as weak to allow a platform to override
+ * the way ARM CCI driver is initialised and used.
+ *****************************************************************************/
+#pragma weak plat_arm_interconnect_init
+#pragma weak plat_arm_interconnect_enter_coherency
+#pragma weak plat_arm_interconnect_exit_coherency
+
+
+/******************************************************************************
+ * Helper function to initialize ARM CCI driver.
+ *****************************************************************************/
+void plat_arm_interconnect_init(void)
+{
+	cci_init(PLAT_ARM_CCI_BASE, cci_map, ARRAY_SIZE(cci_map));
+}
+
+/******************************************************************************
+ * Helper function to place current master into coherency
+ *****************************************************************************/
+void plat_arm_interconnect_enter_coherency(void)
+{
+	cci_enable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr_el1()));
+}
+
+/******************************************************************************
+ * Helper function to remove current master from coherency
+ *****************************************************************************/
+void plat_arm_interconnect_exit_coherency(void)
+{
+	cci_disable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr_el1()));
+}
diff --git a/plat/arm/common/arm_ccn.c b/plat/arm/common/arm_ccn.c
new file mode 100644
index 0000000..5cb443a
--- /dev/null
+++ b/plat/arm/common/arm_ccn.c
@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * 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.
+ */
+
+#include <arch.h>
+#include <ccn.h>
+#include <plat_arm.h>
+#include <platform_def.h>
+
+static const unsigned char master_to_rn_id_map[] = {
+	PLAT_ARM_CLUSTER_TO_CCN_ID_MAP
+};
+
+static const ccn_desc_t arm_ccn_desc = {
+	.periphbase = PLAT_ARM_CCN_BASE,
+	.num_masters = ARRAY_SIZE(master_to_rn_id_map),
+	.master_to_rn_id_map = master_to_rn_id_map
+};
+
+/******************************************************************************
+ * The following functions are defined as weak to allow a platform to override
+ * the way ARM CCN driver is initialised and used.
+ *****************************************************************************/
+#pragma weak plat_arm_interconnect_init
+#pragma weak plat_arm_interconnect_enter_coherency
+#pragma weak plat_arm_interconnect_exit_coherency
+
+
+/******************************************************************************
+ * Helper function to initialize ARM CCN driver.
+ *****************************************************************************/
+void plat_arm_interconnect_init(void)
+{
+	ccn_init(&arm_ccn_desc);
+}
+
+/******************************************************************************
+ * Helper function to place current master into coherency
+ *****************************************************************************/
+void plat_arm_interconnect_enter_coherency(void)
+{
+	ccn_enter_snoop_dvm_domain(1 << MPIDR_AFFLVL1_VAL(read_mpidr_el1()));
+}
+
+/******************************************************************************
+ * Helper function to remove current master from coherency
+ *****************************************************************************/
+void plat_arm_interconnect_exit_coherency(void)
+{
+	ccn_exit_snoop_dvm_domain(1 << MPIDR_AFFLVL1_VAL(read_mpidr_el1()));
+}
diff --git a/plat/arm/common/arm_common.mk b/plat/arm/common/arm_common.mk
index 0748f92..2647f04 100644
--- a/plat/arm/common/arm_common.mk
+++ b/plat/arm/common/arm_common.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions are met:
@@ -72,6 +72,11 @@
 $(eval $(call assert_boolean,ARM_DISABLE_TRUSTED_WDOG))
 $(eval $(call add_define,ARM_DISABLE_TRUSTED_WDOG))
 
+# Process ARM_CONFIG_CNTACR
+ARM_CONFIG_CNTACR		:=	1
+$(eval $(call assert_boolean,ARM_CONFIG_CNTACR))
+$(eval $(call add_define,ARM_CONFIG_CNTACR))
+
 PLAT_INCLUDES		+=	-Iinclude/common/tbbr				\
 				-Iinclude/plat/arm/common			\
 				-Iinclude/plat/arm/common/aarch64
@@ -82,9 +87,7 @@
 				plat/arm/common/aarch64/arm_helpers.S		\
 				plat/common/aarch64/plat_common.c
 
-BL1_SOURCES		+=	drivers/arm/cci/cci.c				\
-				drivers/arm/ccn/ccn.c				\
-				drivers/arm/sp805/sp805.c			\
+BL1_SOURCES		+=	drivers/arm/sp805/sp805.c			\
 				drivers/io/io_fip.c				\
 				drivers/io/io_memmap.c				\
 				drivers/io/io_storage.c				\
@@ -97,26 +100,18 @@
 BL1_SOURCES		+=	plat/arm/common/arm_pm.c
 endif
 
-BL2_SOURCES		+=	drivers/arm/tzc400/tzc400.c			\
-				drivers/io/io_fip.c				\
+BL2_SOURCES		+=	drivers/io/io_fip.c				\
 				drivers/io/io_memmap.c				\
 				drivers/io/io_storage.c				\
 				plat/arm/common/arm_bl2_setup.c			\
 				plat/arm/common/arm_io_storage.c		\
-				plat/arm/common/arm_security.c			\
 				plat/common/aarch64/platform_up_stack.S
 
-BL2U_SOURCES		+=	drivers/arm/tzc400/tzc400.c			\
-				plat/arm/common/arm_bl2u_setup.c		\
-				plat/arm/common/arm_security.c			\
+BL2U_SOURCES		+=	plat/arm/common/arm_bl2u_setup.c		\
 				plat/common/aarch64/platform_up_stack.S
 
-BL31_SOURCES		+=	drivers/arm/cci/cci.c				\
-				drivers/arm/ccn/ccn.c				\
-				drivers/arm/tzc400/tzc400.c			\
-				plat/arm/common/arm_bl31_setup.c		\
+BL31_SOURCES		+=	plat/arm/common/arm_bl31_setup.c		\
 				plat/arm/common/arm_pm.c			\
-				plat/arm/common/arm_security.c			\
 				plat/arm/common/arm_topology.c			\
 				plat/common/aarch64/platform_mp_stack.S		\
 				plat/common/aarch64/plat_psci_common.c
diff --git a/plat/arm/common/arm_io_storage.c b/plat/arm/common/arm_io_storage.c
index 8c5845c..153fdfe 100644
--- a/plat/arm/common/arm_io_storage.c
+++ b/plat/arm/common/arm_io_storage.c
@@ -277,9 +277,9 @@
 }
 
 int plat_arm_get_alt_image_source(
-	unsigned int image_id __attribute__((unused)),
-	uintptr_t *dev_handle __attribute__((unused)),
-	uintptr_t *image_spec __attribute__((unused)))
+	unsigned int image_id __unused,
+	uintptr_t *dev_handle __unused,
+	uintptr_t *image_spec __unused)
 {
 	/* By default do not try an alternative */
 	return -ENOENT;
diff --git a/plat/arm/common/arm_security.c b/plat/arm/common/arm_tzc400.c
similarity index 100%
rename from plat/arm/common/arm_security.c
rename to plat/arm/common/arm_tzc400.c
diff --git a/plat/arm/css/common/aarch64/css_helpers.S b/plat/arm/css/common/aarch64/css_helpers.S
index 2747618..0763a3e 100644
--- a/plat/arm/css/common/aarch64/css_helpers.S
+++ b/plat/arm/css/common/aarch64/css_helpers.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -117,7 +117,8 @@
 	bl	plat_my_core_pos
 	ldr	x1, =SCP_BOOT_CFG_ADDR
 	ldr	x1, [x1]
-	ubfx	x1, x1, #PRIMARY_CPU_SHIFT, #PRIMARY_CPU_BIT_WIDTH
+	ubfx	x1, x1, #PLAT_CSS_PRIMARY_CPU_SHIFT, \
+			#PLAT_CSS_PRIMARY_CPU_BIT_WIDTH
 	cmp	x0, x1
 	cset	w0, eq
 	ret	x9
diff --git a/plat/arm/css/common/css_common.mk b/plat/arm/css/common/css_common.mk
index 6a8773d..65e125e 100644
--- a/plat/arm/css/common/css_common.mk
+++ b/plat/arm/css/common/css_common.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions are met:
@@ -28,6 +28,10 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 
+
+# By default, SCP images are needed by CSS platforms.
+CSS_LOAD_SCP_IMAGES	?=	1
+
 PLAT_INCLUDES		+=	-Iinclude/plat/arm/css/common			\
 				-Iinclude/plat/arm/css/common/aarch64
 
@@ -38,12 +42,10 @@
 
 BL2_SOURCES		+=	plat/arm/css/common/css_bl2_setup.c		\
 				plat/arm/css/common/css_mhu.c			\
-				plat/arm/css/common/css_scp_bootloader.c	\
 				plat/arm/css/common/css_scpi.c
 
 BL2U_SOURCES		+=	plat/arm/css/common/css_bl2u_setup.c		\
 				plat/arm/css/common/css_mhu.c			\
-				plat/arm/css/common/css_scp_bootloader.c	\
 				plat/arm/css/common/css_scpi.c
 
 BL31_SOURCES		+=	plat/arm/css/common/css_mhu.c			\
@@ -51,17 +53,25 @@
 				plat/arm/css/common/css_scpi.c			\
 				plat/arm/css/common/css_topology.c
 
-ifneq (${TRUSTED_BOARD_BOOT},0)
-$(eval $(call FWU_FIP_ADD_IMG,SCP_BL2U,--scp-fwu-cfg))
-endif
 
 ifneq (${RESET_TO_BL31},0)
   $(error "Using BL31 as the reset vector is not supported on CSS platforms. \
   Please set RESET_TO_BL31 to 0.")
 endif
 
-# Subsystems require a SCP_BL2 image
-$(eval $(call FIP_ADD_IMG,SCP_BL2,--scp-fw))
+# Process CSS_LOAD_SCP_IMAGES flag
+$(eval $(call assert_boolean,CSS_LOAD_SCP_IMAGES))
+$(eval $(call add_define,CSS_LOAD_SCP_IMAGES))
+
+ifeq (${CSS_LOAD_SCP_IMAGES},1)
+  $(eval $(call FIP_ADD_IMG,SCP_BL2,--scp-fw))
+  ifneq (${TRUSTED_BOARD_BOOT},0)
+    $(eval $(call FWU_FIP_ADD_IMG,SCP_BL2U,--scp-fwu-cfg))
+  endif
+
+  BL2U_SOURCES		+=	plat/arm/css/common/css_scp_bootloader.c
+  BL2_SOURCES		+=	plat/arm/css/common/css_scp_bootloader.c
+endif
 
 # Enable option to detect whether the SCP ROM firmware in use predates version
 # 1.7.0 and therefore, is incompatible.
diff --git a/plat/arm/css/common/css_mhu.c b/plat/arm/css/common/css_mhu.c
index b1714e2..265d6c2 100644
--- a/plat/arm/css/common/css_mhu.c
+++ b/plat/arm/css/common/css_mhu.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -33,6 +33,7 @@
 #include <bakery_lock.h>
 #include <css_def.h>
 #include <mmio.h>
+#include <platform_def.h>
 #include <plat_arm.h>
 #include "css_mhu.h"
 
@@ -66,24 +67,26 @@
 	arm_lock_get();
 
 	/* Make sure any previous command has finished */
-	while (mmio_read_32(MHU_BASE + CPU_INTR_S_STAT) & (1 << slot_id))
+	while (mmio_read_32(PLAT_CSS_MHU_BASE + CPU_INTR_S_STAT) &
+							(1 << slot_id))
 		;
 }
 
 void mhu_secure_message_send(unsigned int slot_id)
 {
 	assert(slot_id <= MHU_MAX_SLOT_ID);
-	assert(!(mmio_read_32(MHU_BASE + CPU_INTR_S_STAT) & (1 << slot_id)));
+	assert(!(mmio_read_32(PLAT_CSS_MHU_BASE + CPU_INTR_S_STAT) &
+							(1 << slot_id)));
 
 	/* Send command to SCP */
-	mmio_write_32(MHU_BASE + CPU_INTR_S_SET, 1 << slot_id);
+	mmio_write_32(PLAT_CSS_MHU_BASE + CPU_INTR_S_SET, 1 << slot_id);
 }
 
 uint32_t mhu_secure_message_wait(void)
 {
 	/* Wait for response from SCP */
 	uint32_t response;
-	while (!(response = mmio_read_32(MHU_BASE + SCP_INTR_S_STAT)))
+	while (!(response = mmio_read_32(PLAT_CSS_MHU_BASE + SCP_INTR_S_STAT)))
 		;
 
 	return response;
@@ -97,7 +100,7 @@
 	 * Clear any response we got by writing one in the relevant slot bit to
 	 * the CLEAR register
 	 */
-	mmio_write_32(MHU_BASE + SCP_INTR_S_CLEAR, 1 << slot_id);
+	mmio_write_32(PLAT_CSS_MHU_BASE + SCP_INTR_S_CLEAR, 1 << slot_id);
 
 	arm_lock_release();
 }
@@ -111,7 +114,7 @@
 	 * as a stale or garbage value would make us think it's a message we've
 	 * already sent.
 	 */
-	assert(mmio_read_32(MHU_BASE + CPU_INTR_S_STAT) == 0);
+	assert(mmio_read_32(PLAT_CSS_MHU_BASE + CPU_INTR_S_STAT) == 0);
 }
 
 void plat_arm_pwrc_setup(void)
diff --git a/plat/arm/css/common/css_pm.c b/plat/arm/css/common/css_pm.c
index 6d6646e..b6f94ac 100644
--- a/plat/arm/css/common/css_pm.c
+++ b/plat/arm/css/common/css_pm.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -31,7 +31,6 @@
 #include <arch_helpers.h>
 #include <assert.h>
 #include <cassert.h>
-#include <cci.h>
 #include <css_pm.h>
 #include <debug.h>
 #include <errno.h>
@@ -108,7 +107,7 @@
 	 * if this cluster was off.
 	 */
 	if (CSS_CLUSTER_PWR_STATE(target_state) == ARM_LOCAL_STATE_OFF)
-		cci_enable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr_el1()));
+		plat_arm_interconnect_enter_coherency();
 }
 
 /*******************************************************************************
@@ -153,7 +152,7 @@
 
 	/* Cluster is to be turned off, so disable coherency */
 	if (CSS_CLUSTER_PWR_STATE(target_state) == ARM_LOCAL_STATE_OFF) {
-		cci_disable_snoop_dvm_reqs(MPIDR_AFFLVL1_VAL(read_mpidr()));
+		plat_arm_interconnect_exit_coherency();
 		cluster_state = scpi_power_off;
 	}
 
diff --git a/plat/arm/css/common/css_scp_bootloader.c b/plat/arm/css/common/css_scp_bootloader.c
index 4367459..d3f671e 100644
--- a/plat/arm/css/common/css_scp_bootloader.c
+++ b/plat/arm/css/common/css_scp_bootloader.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -60,7 +60,7 @@
  * Unlike the SCPI protocol, the boot protocol uses the same memory region
  * for both AP -> SCP and SCP -> AP transfers; define the address of this...
  */
-#define BOM_SHARED_MEM		SCP_COM_SHARED_MEM_BASE
+#define BOM_SHARED_MEM		PLAT_CSS_SCP_COM_SHARED_MEM_BASE
 #define BOM_CMD_HEADER		((bom_cmd_t *) BOM_SHARED_MEM)
 #define BOM_CMD_PAYLOAD		((void *) (BOM_SHARED_MEM + sizeof(bom_cmd_t)))
 
diff --git a/plat/arm/css/common/css_scpi.c b/plat/arm/css/common/css_scpi.c
index 829a174..02d573c 100644
--- a/plat/arm/css/common/css_scpi.c
+++ b/plat/arm/css/common/css_scpi.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -37,8 +37,9 @@
 #include "css_mhu.h"
 #include "css_scpi.h"
 
-#define SCPI_SHARED_MEM_SCP_TO_AP	SCP_COM_SHARED_MEM_BASE
-#define SCPI_SHARED_MEM_AP_TO_SCP	(SCP_COM_SHARED_MEM_BASE + 0x100)
+#define SCPI_SHARED_MEM_SCP_TO_AP	PLAT_CSS_SCP_COM_SHARED_MEM_BASE
+#define SCPI_SHARED_MEM_AP_TO_SCP	(PLAT_CSS_SCP_COM_SHARED_MEM_BASE \
+								 + 0x100)
 
 #define SCPI_CMD_HEADER_AP_TO_SCP		\
 	((scpi_cmd_t *) SCPI_SHARED_MEM_AP_TO_SCP)
diff --git a/plat/arm/css/common/css_scpi.h b/plat/arm/css/common/css_scpi.h
index 379a821..4a601f3 100644
--- a/plat/arm/css/common/css_scpi.h
+++ b/plat/arm/css/common/css_scpi.h
@@ -45,7 +45,7 @@
 	uint32_t set		: 1;
 	/* Sender ID to match a reply. The value is sender specific. */
 	uint32_t sender		: 8;
-	/* Size of the payload in bytes (0 – 511) */
+	/* Size of the payload in bytes (0 - 511) */
 	uint32_t size		: 9;
 	uint32_t reserved	: 7;
 	/*
diff --git a/plat/mediatek/common/mtk_sip_svc.c b/plat/mediatek/common/mtk_sip_svc.c
index af28080..cb10af5 100644
--- a/plat/mediatek/common/mtk_sip_svc.c
+++ b/plat/mediatek/common/mtk_sip_svc.c
@@ -55,6 +55,18 @@
 		ret = mt_sip_set_authorized_sreg((uint32_t)x1, (uint32_t)x2);
 		SMC_RET1(handle, ret);
 
+	case MTK_SIP_PWR_ON_MTCMOS:
+		ret = mt_sip_pwr_on_mtcmos((uint32_t)x1);
+		SMC_RET1(handle, ret);
+
+	case MTK_SIP_PWR_OFF_MTCMOS:
+		ret = mt_sip_pwr_off_mtcmos((uint32_t)x1);
+		SMC_RET1(handle, ret);
+
+	case MTK_SIP_PWR_MTCMOS_SUPPORT:
+		ret = mt_sip_pwr_mtcmos_support();
+		SMC_RET1(handle, ret);
+
 	default:
 		ERROR("%s: unhandled SMC (0x%x)\n", __func__, smc_fid);
 		break;
diff --git a/plat/mediatek/common/mtk_sip_svc.h b/plat/mediatek/common/mtk_sip_svc.h
index eb1c2e6..94a6036 100644
--- a/plat/mediatek/common/mtk_sip_svc.h
+++ b/plat/mediatek/common/mtk_sip_svc.h
@@ -43,10 +43,13 @@
 #define MTK_SIP_SVC_VERSION_MINOR	0x1
 
 /* Number of Mediatek SiP Calls implemented */
-#define MTK_SIP_NUM_CALLS		1
+#define MTK_SIP_NUM_CALLS		4
 
 /* Mediatek SiP Service Calls function IDs */
 #define MTK_SIP_SET_AUTHORIZED_SECURE_REG	0x82000001
+#define MTK_SIP_PWR_ON_MTCMOS			0x82000402
+#define MTK_SIP_PWR_OFF_MTCMOS			0x82000403
+#define MTK_SIP_PWR_MTCMOS_SUPPORT		0x82000404
 
 /* Mediatek SiP Calls error code */
 enum {
@@ -62,5 +65,7 @@
  * Return MTK_SIP_E_SUCCESS on success, and MTK_SIP_E_INVALID_PARAM on failure.
  */
 uint64_t mt_sip_set_authorized_sreg(uint32_t sreg, uint32_t val);
-
+uint64_t mt_sip_pwr_on_mtcmos(uint32_t val);
+uint64_t mt_sip_pwr_off_mtcmos(uint32_t val);
+uint64_t mt_sip_pwr_mtcmos_support(void);
 #endif /* __PLAT_SIP_SVC_H__ */
diff --git a/plat/mediatek/mt8173/aarch64/plat_helpers.S b/plat/mediatek/mt8173/aarch64/plat_helpers.S
index 99a054c..af3a407 100644
--- a/plat/mediatek/mt8173/aarch64/plat_helpers.S
+++ b/plat/mediatek/mt8173/aarch64/plat_helpers.S
@@ -63,7 +63,7 @@
 	 * int plat_crash_console_init(void)
 	 * Function to initialize the crash console
 	 * without a C Runtime to print crash report.
-	 * Clobber list : x0, x1, x2
+	 * Clobber list : x0 - x4
 	 * ---------------------------------------------
 	 */
 func plat_crash_console_init
diff --git a/plat/mediatek/mt8173/bl31_plat_setup.c b/plat/mediatek/mt8173/bl31_plat_setup.c
index 03ec690..ec95143 100644
--- a/plat/mediatek/mt8173/bl31_plat_setup.c
+++ b/plat/mediatek/mt8173/bl31_plat_setup.c
@@ -96,6 +96,16 @@
 	/* set LITTLE cores arm64 boot mode */
 	mmio_setbits_32((uintptr_t)&mt8173_mcucfg->mp0_rv_addr[0].rv_addr_hw,
 		MP0_CPUCFG_64BIT);
+
+	/* enable dcm control */
+	mmio_setbits_32((uintptr_t)&mt8173_mcucfg->bus_fabric_dcm_ctrl,
+		ADB400_GRP_DCM_EN | CCI400_GRP_DCM_EN | ADBCLK_GRP_DCM_EN |
+		EMICLK_GRP_DCM_EN | ACLK_GRP_DCM_EN | L2C_IDLE_DCM_EN |
+		INFRACLK_PSYS_DYNAMIC_CG_EN);
+	mmio_setbits_32((uintptr_t)&mt8173_mcucfg->l2c_sram_ctrl,
+		L2C_SRAM_DCM_EN);
+	mmio_setbits_32((uintptr_t)&mt8173_mcucfg->cci_clk_ctrl,
+		MCU_BUS_DCM_EN);
 }
 
 /*******************************************************************************
diff --git a/plat/mediatek/mt8173/drivers/mtcmos/mtcmos.c b/plat/mediatek/mt8173/drivers/mtcmos/mtcmos.c
index f7a1b07..265685f 100644
--- a/plat/mediatek/mt8173/drivers/mtcmos/mtcmos.c
+++ b/plat/mediatek/mt8173/drivers/mtcmos/mtcmos.c
@@ -29,8 +29,11 @@
  */
 #include <mmio.h>
 #include <mt8173_def.h>
+#include <debug.h>
 #include <mtcmos.h>
 #include <spm.h>
+#include <spm_mcdi.h>
+#include <delay_timer.h>
 
 enum {
 	SRAM_ISOINT_B	= 1U << 6,
@@ -64,6 +67,63 @@
 	AUD_PWR_STA_MASK   = 0x1 << 24,
 };
 
+#define SPM_VDE_PWR_CON				0x0210
+#define SPM_MFG_PWR_CON				0x0214
+#define SPM_VEN_PWR_CON				0x0230
+#define SPM_ISP_PWR_CON				0x0238
+#define SPM_DIS_PWR_CON				0x023c
+#define SPM_VEN2_PWR_CON			0x0298
+#define SPM_AUDIO_PWR_CON			0x029c
+#define SPM_MFG_2D_PWR_CON			0x02c0
+#define SPM_MFG_ASYNC_PWR_CON			0x02c4
+#define SPM_USB_PWR_CON				0x02cc
+
+#define MTCMOS_CTRL_SUCCESS			0
+#define MTCMOS_CTRL_ERROR			-1
+
+#define MTCMOS_CTRL_EN				(0x1 << 18)
+
+#define VDE_PWR_ON				0
+#define VEN_PWR_ON				1
+#define ISP_PWR_ON				2
+#define DIS_PWR_ON				3
+#define VEN2_PWR_ON				4
+#define AUDIO_PWR_ON				5
+#define MFG_ASYNC_PWR_ON			6
+#define MFG_2D_PWR_ON				7
+#define MFG_PWR_ON				8
+#define USB_PWR_ON				9
+
+#define VDE_PWR_OFF				10
+#define VEN_PWR_OFF				11
+#define ISP_PWR_OFF				12
+#define DIS_PWR_OFF				13
+#define VEN2_PWR_OFF				14
+#define AUDIO_PWR_OFF				15
+#define MFG_ASYNC_PWR_OFF			16
+#define MFG_2D_PWR_OFF				17
+#define MFG_PWR_OFF				18
+#define USB_PWR_OFF				19
+
+#define VDE_PWR_CON_PWR_STA			7
+#define VEN_PWR_CON_PWR_STA			21
+#define ISP_PWR_CON_PWR_STA			5
+#define DIS_PWR_CON_PWR_STA			3
+#define VEN2_PWR_CON_PWR_STA			20
+#define AUDIO_PWR_CON_PWR_STA			24
+#define MFG_ASYNC_PWR_CON_PWR_STA		23
+#define MFG_2D_PWR_CON_PWR_STA			22
+#define MFG_PWR_CON_PWR_STA			4
+#define USB_PWR_CON_PWR_STA			25
+
+/*
+ * Timeout if the ack is not signled after 1 second.
+ * According to designer, one mtcmos operation should be done
+ * around 10us.
+ */
+#define MTCMOS_ACK_POLLING_MAX_COUNT			10000
+#define MTCMOS_ACK_POLLING_INTERVAL			10
+
 static void mtcmos_ctrl_little_off(unsigned int linear_id)
 {
 	uint32_t reg_pwr_con;
@@ -120,3 +180,117 @@
 	mtcmos_ctrl_little_off(2);
 	mtcmos_ctrl_little_off(3);
 }
+
+uint32_t wait_mtcmos_ack(uint32_t on, uint32_t mtcmos_sta, uint32_t spm_pwr_sta)
+{
+	int i = 0;
+	uint32_t cmp, pwr_sta, pwr_sta_2nd;
+
+	while (1) {
+		cmp = (mmio_read_32(SPM_PCM_PASR_DPD_3) >> mtcmos_sta) & 1;
+		pwr_sta = (mmio_read_32(SPM_PWR_STATUS) >> spm_pwr_sta) & 1;
+		pwr_sta_2nd =
+			(mmio_read_32(SPM_PWR_STATUS_2ND) >> spm_pwr_sta) & 1;
+		if ((cmp == on) && (pwr_sta == on) && (pwr_sta_2nd == on)) {
+			mmio_write_32(SPM_PCM_RESERVE2, 0);
+			return MTCMOS_CTRL_SUCCESS;
+		}
+		udelay(MTCMOS_ACK_POLLING_INTERVAL);
+		i++;
+		if (i > MTCMOS_ACK_POLLING_MAX_COUNT) {
+			INFO("MTCMOS control failed(%d), SPM_PWR_STA(%d),\n"
+				"SPM_PCM_RESERVE=0x%x,SPM_PCM_RESERVE2=0x%x,\n"
+				"SPM_PWR_STATUS=0x%x,SPM_PWR_STATUS_2ND=0x%x\n"
+				"SPM_PCM_PASR_DPD_3 = 0x%x\n",
+				on, spm_pwr_sta, mmio_read_32(SPM_PCM_RESERVE),
+				mmio_read_32(SPM_PCM_RESERVE2),
+				mmio_read_32(SPM_PWR_STATUS),
+				mmio_read_32(SPM_PWR_STATUS_2ND),
+				mmio_read_32(SPM_PCM_PASR_DPD_3));
+			mmio_write_32(SPM_PCM_RESERVE2, 0);
+			return MTCMOS_CTRL_ERROR;
+		}
+	}
+}
+
+uint32_t mtcmos_non_cpu_ctrl(uint32_t on, uint32_t mtcmos_num)
+{
+	uint32_t ret = MTCMOS_CTRL_SUCCESS;
+	uint32_t power_on;
+	uint32_t power_off;
+	uint32_t power_status;
+
+	spm_lock_get();
+	spm_mcdi_prepare_for_mtcmos();
+	mmio_setbits_32(SPM_PCM_RESERVE, MTCMOS_CTRL_EN);
+
+	switch (mtcmos_num) {
+	case SPM_VDE_PWR_CON:
+		power_on = VDE_PWR_ON;
+		power_off = VDE_PWR_OFF;
+		power_status = VDE_PWR_CON_PWR_STA;
+		break;
+	case SPM_MFG_PWR_CON:
+		power_on = MFG_PWR_ON;
+		power_off = MFG_PWR_OFF;
+		power_status = MFG_PWR_CON_PWR_STA;
+		break;
+	case SPM_VEN_PWR_CON:
+		power_on = VEN_PWR_ON;
+		power_off = VEN_PWR_OFF;
+		power_status = VEN_PWR_CON_PWR_STA;
+		break;
+	case SPM_ISP_PWR_CON:
+		power_on = ISP_PWR_ON;
+		power_off = ISP_PWR_OFF;
+		power_status = ISP_PWR_CON_PWR_STA;
+		break;
+	case SPM_DIS_PWR_CON:
+		power_on = DIS_PWR_ON;
+		power_off = DIS_PWR_OFF;
+		power_status = DIS_PWR_CON_PWR_STA;
+		break;
+	case SPM_VEN2_PWR_CON:
+		power_on = VEN2_PWR_ON;
+		power_off = VEN2_PWR_OFF;
+		power_status = VEN2_PWR_CON_PWR_STA;
+		break;
+	case SPM_AUDIO_PWR_CON:
+		power_on = AUDIO_PWR_ON;
+		power_off = AUDIO_PWR_OFF;
+		power_status = AUDIO_PWR_CON_PWR_STA;
+		break;
+	case SPM_MFG_2D_PWR_CON:
+		power_on = MFG_2D_PWR_ON;
+		power_off = MFG_2D_PWR_OFF;
+		power_status = MFG_2D_PWR_CON_PWR_STA;
+		break;
+	case SPM_MFG_ASYNC_PWR_CON:
+		power_on = MFG_ASYNC_PWR_ON;
+		power_off = MFG_ASYNC_PWR_OFF;
+		power_status = MFG_ASYNC_PWR_CON_PWR_STA;
+		break;
+	case SPM_USB_PWR_CON:
+		power_on = USB_PWR_ON;
+		power_off = USB_PWR_OFF;
+		power_status = USB_PWR_CON_PWR_STA;
+		break;
+	default:
+		ret = MTCMOS_CTRL_ERROR;
+		INFO("No mapping MTCMOS(%d), ret = %d\n", mtcmos_num, ret);
+		break;
+	}
+
+	if (ret == MTCMOS_CTRL_SUCCESS) {
+		mmio_setbits_32(SPM_PCM_RESERVE2, on ?
+			(1 << power_on) : (1 << power_off));
+		ret = wait_mtcmos_ack(on, power_on, power_status);
+		VERBOSE("0x%x(%d), PWR_STATUS(0x%x), ret(%d)\n",
+			power_on, on, mmio_read_32(SPM_PWR_STATUS), ret);
+	}
+
+	mmio_clrbits_32(SPM_PCM_RESERVE, MTCMOS_CTRL_EN);
+	spm_lock_release();
+
+	return ret;
+}
diff --git a/plat/mediatek/mt8173/drivers/mtcmos/mtcmos.h b/plat/mediatek/mt8173/drivers/mtcmos/mtcmos.h
index ddcda78..4641f46 100644
--- a/plat/mediatek/mt8173/drivers/mtcmos/mtcmos.h
+++ b/plat/mediatek/mt8173/drivers/mtcmos/mtcmos.h
@@ -37,5 +37,6 @@
  * during CPU_ON psci call.
  */
 void mtcmos_little_cpu_off(void);
+uint32_t mtcmos_non_cpu_ctrl(uint32_t on, uint32_t mtcmos_num);
 
 #endif /* __MTCMOS_H__ */
diff --git a/plat/mediatek/mt8173/drivers/spm/spm.c b/plat/mediatek/mt8173/drivers/spm/spm.c
index 7c6d72b..45defd8 100644
--- a/plat/mediatek/mt8173/drivers/spm/spm.c
+++ b/plat/mediatek/mt8173/drivers/spm/spm.c
@@ -55,9 +55,9 @@
 
 DEFINE_BAKERY_LOCK(spm_lock);
 
-static int spm_hotplug_ready __attribute__ ((section("tzfw_coherent_mem")));
-static int spm_mcdi_ready __attribute__ ((section("tzfw_coherent_mem")));
-static int spm_suspend_ready __attribute__ ((section("tzfw_coherent_mem")));
+static int spm_hotplug_ready __section("tzfw_coherent_mem");
+static int spm_mcdi_ready __section("tzfw_coherent_mem");
+static int spm_suspend_ready __section("tzfw_coherent_mem");
 
 void spm_lock_init(void)
 {
@@ -386,6 +386,8 @@
 {
 	/* Only CPU0 is online during boot, initialize cpu online reserve bit */
 	mmio_write_32(SPM_PCM_RESERVE, 0xFE);
+	mmio_clrbits_32(AP_PLL_CON3, 0xFFFFF);
+	mmio_clrbits_32(AP_PLL_CON4, 0xF);
 	spm_lock_init();
 	spm_register_init();
 }
diff --git a/plat/mediatek/mt8173/drivers/spm/spm.h b/plat/mediatek/mt8173/drivers/spm/spm.h
index f1e7674..f371fd8 100644
--- a/plat/mediatek/mt8173/drivers/spm/spm.h
+++ b/plat/mediatek/mt8173/drivers/spm/spm.h
@@ -129,6 +129,9 @@
 #define SPM_SLEEP_CA15_WFI2_EN			(SPM_BASE + 0xf18)
 #define SPM_SLEEP_CA15_WFI3_EN			(SPM_BASE + 0xf1c)
 
+#define AP_PLL_CON3		0x1020900c
+#define AP_PLL_CON4		0x10209010
+
 #define SPM_PROJECT_CODE	0xb16
 
 #define SPM_REGWR_EN		(1U << 0)
@@ -213,6 +216,7 @@
 #define WAKE_SRC_USB_PDN	(1 << 15)
 #define WAKE_SRC_AFE		(1 << 20)
 #define WAKE_SRC_THERM		(1 << 21)
+#define WAKE_SRC_CIRQ		(1 << 22)
 #define WAKE_SRC_SYSPWREQ	(1 << 24)
 #define WAKE_SRC_SEJ		(1 << 27)
 #define WAKE_SRC_ALL_MD32	(1 << 28)
diff --git a/plat/mediatek/mt8173/drivers/spm/spm_hotplug.c b/plat/mediatek/mt8173/drivers/spm/spm_hotplug.c
index fccd5a2..b89cd97 100644
--- a/plat/mediatek/mt8173/drivers/spm/spm_hotplug.c
+++ b/plat/mediatek/mt8173/drivers/spm/spm_hotplug.c
@@ -40,7 +40,7 @@
  * This driver controls the cpu power in cpu hotplug flow.
  */
 
-#define PCM_HOTPLUG_VALID_MASK	0x00ff0000
+#define PCM_HOTPLUG_VALID_MASK	0x0000ff00
 #define PCM_HOTPLUG_VALID_SHIFT	0x8
 
 /**********************************************************
diff --git a/plat/mediatek/mt8173/drivers/spm/spm_mcdi.c b/plat/mediatek/mt8173/drivers/spm/spm_mcdi.c
index 11f4a4a..3043327 100644
--- a/plat/mediatek/mt8173/drivers/spm/spm_mcdi.c
+++ b/plat/mediatek/mt8173/drivers/spm/spm_mcdi.c
@@ -43,175 +43,199 @@
  * This driver controls the cpu power in cpu idle power saving state.
  */
 
-#define WAKE_SRC_FOR_MCDI	(WAKE_SRC_SYSPWREQ | WAKE_SRC_CPU_IRQ)
+#define WAKE_SRC_FOR_MCDI \
+	(WAKE_SRC_KP | WAKE_SRC_GPT | WAKE_SRC_EINT |		\
+	 WAKE_SRC_MD32 | WAKE_SRC_USB_CD | WAKE_SRC_USB_PDN |	\
+	 WAKE_SRC_AFE | WAKE_SRC_THERM | WAKE_SRC_CIRQ |	\
+	 WAKE_SRC_SYSPWREQ | WAKE_SRC_CPU_IRQ)
 #define PCM_MCDI_HANDSHAKE_SYNC	0xbeefbeef
 #define PCM_MCDI_HANDSHAKE_ACK	0xdeaddead
 #define PCM_MCDI_UPDATE_INFORM	0xabcdabcd
 #define PCM_MCDI_CKECK_DONE	0x12345678
 #define PCM_MCDI_ALL_CORE_AWAKE	0x0
 #define PCM_MCDI_OFFLOADED	0xaa55aa55
+#define PCM_MCDI_CA72_CPUTOP_PWRCTL	(0x1 << 16)
+#define PCM_MCDI_CA53_CPUTOP_PWRCTL	(0x1 << 17)
+#define PCM_MCDI_CA72_PWRSTA_SHIFT	16
+#define PCM_MCDI_CA53_PWRSTA_SHIFT	9
 
 static const unsigned int mcdi_binary[] = {
-	0x1212841f, 0xe2e00036, 0xe2e0003e, 0x1380201f, 0xe2e0003c, 0xe2a00000,
-	0x1b80001f, 0x20000080, 0xe2e0007c, 0x1b80001f, 0x20000003, 0xe2e0005c,
-	0xe2e0004c, 0xe2e0004d, 0xf0000000, 0x17c07c1f, 0xe2e0004f, 0xe2e0006f,
-	0xe2e0002f, 0xe2a00001, 0x1b80001f, 0x20000080, 0xe2e0002e, 0xe2e0003e,
-	0xe2e00032, 0xf0000000, 0x17c07c1f, 0x1212841f, 0xe2e00026, 0xe2e0002e,
-	0x1380201f, 0x1a00001f, 0x100062b4, 0x1910001f, 0x100062b4, 0x81322804,
-	0xe2000004, 0x81202804, 0xe2000004, 0x1b80001f, 0x20000034, 0x1910001f,
-	0x100062b4, 0x81142804, 0xd8000524, 0x17c07c1f, 0xe2e0000e, 0xe2e0000c,
-	0xe2e0000d, 0xf0000000, 0x17c07c1f, 0xe2e0002d, 0x1a00001f, 0x100062b4,
-	0x1910001f, 0x100062b4, 0xa1002804, 0xe2000004, 0xa1122804, 0xe2000004,
-	0x1b80001f, 0x20000080, 0x1910001f, 0x100062b4, 0x81142804, 0xd82007c4,
-	0x17c07c1f, 0xe2e0002f, 0xe2e0002b, 0xe2e00023, 0x1380201f, 0xe2e00022,
-	0xf0000000, 0x17c07c1f, 0x18c0001f, 0x10006b6c, 0x1910001f, 0x10006b6c,
-	0xa1002804, 0xe0c00004, 0xf0000000, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
+	0x1a10001f, 0x10006b04, 0x1890001f, 0x10006b6c, 0x1a40001f, 0x10006210,
+	0x18d0001f, 0x10006210, 0x81002001, 0xd82001c4, 0x17c07c1f, 0xa0900402,
+	0xc2401520, 0x17c07c1f, 0x81052001, 0xd8200284, 0x17c07c1f, 0x80b00402,
+	0xc2401b60, 0x17c07c1f, 0x1a40001f, 0x10006230, 0x18d0001f, 0x10006230,
+	0x8100a001, 0xd82003c4, 0x17c07c1f, 0xa0908402, 0xc2401520, 0x17c07c1f,
+	0x8105a001, 0xd8200484, 0x17c07c1f, 0x80b08402, 0xc2401b60, 0x17c07c1f,
+	0x1a40001f, 0x10006238, 0x18d0001f, 0x10006238, 0x81012001, 0xd82005c4,
+	0x17c07c1f, 0xa0910402, 0xc2401520, 0x17c07c1f, 0x81062001, 0xd8200684,
+	0x17c07c1f, 0x80b10402, 0xc2401b60, 0x17c07c1f, 0x1a40001f, 0x1000623c,
+	0x18d0001f, 0x1000623c, 0x8101a001, 0xd82007c4, 0x17c07c1f, 0xa0918402,
+	0xc2401520, 0x17c07c1f, 0x8106a001, 0xd8200884, 0x17c07c1f, 0x80b18402,
+	0xc2401b60, 0x17c07c1f, 0x1a40001f, 0x10006298, 0x18d0001f, 0x10006298,
+	0x81022001, 0xd82009c4, 0x17c07c1f, 0xa0920402, 0xc2401520, 0x17c07c1f,
+	0x81072001, 0xd8200a84, 0x17c07c1f, 0x80b20402, 0xc2401b60, 0x17c07c1f,
+	0x1a40001f, 0x1000629c, 0x18d0001f, 0x1000629c, 0x8102a001, 0xd8200bc4,
+	0x17c07c1f, 0xa0928402, 0xc2401520, 0x17c07c1f, 0x8107a001, 0xd8200c84,
+	0x17c07c1f, 0x80b28402, 0xc2401b60, 0x17c07c1f, 0x1a40001f, 0x100062c4,
+	0x18d0001f, 0x100062c4, 0x81032001, 0xd8200dc4, 0x17c07c1f, 0xa0930402,
+	0xc2401520, 0x17c07c1f, 0x81082001, 0xd8200e84, 0x17c07c1f, 0x80b30402,
+	0xc2401b60, 0x17c07c1f, 0x1a40001f, 0x100062c0, 0x18d0001f, 0x100062c0,
+	0x8103a001, 0xd8200fc4, 0x17c07c1f, 0xa0938402, 0xc2401520, 0x17c07c1f,
+	0x8108a001, 0xd8201084, 0x17c07c1f, 0x80b38402, 0xc2401b60, 0x17c07c1f,
+	0x1a40001f, 0x10006214, 0x18d0001f, 0x10006214, 0x81042001, 0xd82011c4,
+	0x17c07c1f, 0xa0940402, 0xc2401520, 0x17c07c1f, 0x81092001, 0xd8201284,
+	0x17c07c1f, 0x80b40402, 0xc2401b60, 0x17c07c1f, 0x1a40001f, 0x100062cc,
+	0x18d0001f, 0x100062cc, 0x8104a001, 0xd82013c4, 0x17c07c1f, 0xa0948402,
+	0xc2401520, 0x17c07c1f, 0x8109a001, 0xd8201484, 0x17c07c1f, 0x80b48402,
+	0xc2401b60, 0x17c07c1f, 0x1900001f, 0x10006b6c, 0xe1000002, 0xf0000000,
+	0x17c07c1f, 0xa8c00003, 0x00000004, 0xe2400003, 0xa8c00003, 0x00000008,
+	0xe2400003, 0x1b80001f, 0x00000020, 0x88c00003, 0xffffffef, 0xe2400003,
+	0x88c00003, 0xfffffffd, 0xe2400003, 0xa8c00003, 0x00000001, 0xe2400003,
+	0x88c00003, 0xfffff0ff, 0xe2400003, 0x1b80001f, 0x20000080, 0x1a90001f,
+	0x10001220, 0x69200009, 0x1000623c, 0xd8001964, 0x17c07c1f, 0x69200009,
+	0x10006214, 0xd8001a44, 0x17c07c1f, 0xd0001ae0, 0x17c07c1f, 0x1900001f,
+	0x10001220, 0x8a80000a, 0xfffffff9, 0xe100000a, 0xd0001ae0, 0x17c07c1f,
+	0x1900001f, 0x10001220, 0x8a80000a, 0xff1fbfff, 0xe100000a, 0x1b80001f,
+	0x20000080, 0xf0000000, 0x17c07c1f, 0x1a90001f, 0x10001220, 0x69200009,
+	0x1000623c, 0xd8001ce4, 0x17c07c1f, 0x69200009, 0x10006214, 0xd8001dc4,
+	0x17c07c1f, 0xd0001e60, 0x17c07c1f, 0x1900001f, 0x10001220, 0xaa80000a,
+	0x00000006, 0xe100000a, 0xd0001e60, 0x17c07c1f, 0x1900001f, 0x10001220,
+	0xaa80000a, 0x00e04000, 0xe100000a, 0x1b80001f, 0x20000080, 0x69200009,
+	0x10006214, 0xd8001fc4, 0x17c07c1f, 0xa8c00003, 0x00000f00, 0xe2400003,
+	0xd0002020, 0x17c07c1f, 0xa8c00003, 0x00003f00, 0xe2400003, 0x1b80001f,
+	0x20000080, 0xa8c00003, 0x00000002, 0xe2400003, 0x88c00003, 0xfffffffe,
+	0xe2400003, 0xa8c00003, 0x00000010, 0xe2400003, 0x88c00003, 0xfffffffb,
+	0xe2400003, 0x88c00003, 0xfffffff7, 0xe2400003, 0xf0000000, 0x17c07c1f,
+	0xe2e00036, 0xe2e0003e, 0x1b80001f, 0x00000020, 0xe2e0003c, 0xe8208000,
+	0x10006244, 0x00000000, 0x1b80001f, 0x20000080, 0xe2e0007c, 0x1b80001f,
+	0x20000003, 0xe2e0005c, 0xe2e0004c, 0xe2e0004d, 0xf0000000, 0x17c07c1f,
+	0xe2e0004f, 0xe2e0006f, 0xe2e0002f, 0xe8208000, 0x10006244, 0x00000001,
+	0x1b80001f, 0x20000080, 0xe2e0002e, 0xe2e0003e, 0xe2e0003a, 0xe2e00032,
+	0x1b80001f, 0x00000020, 0x1910001f, 0x10006b6c, 0x09000004, 0x00100000,
+	0x1a10001f, 0x10006b6c, 0xe2000004, 0xf0000000, 0x17c07c1f, 0xe2e00036,
+	0xe2e0003e, 0x1b80001f, 0x00000020, 0xe2e0003c, 0xe2a00000, 0x1b80001f,
+	0x20000080, 0xe2e0007c, 0x1b80001f, 0x20000003, 0xe2e0005c, 0xe2e0004c,
+	0xe2e0004d, 0xf0000000, 0x17c07c1f, 0xe2e0004f, 0xe2e0006f, 0xe2e0002f,
+	0xe2a00001, 0x1b80001f, 0x20000080, 0xe2e0002e, 0xe2e0003e, 0xe2e0003a,
+	0xe2e00032, 0xf0000000, 0x17c07c1f, 0xe2e00026, 0xe2e0002e, 0x1b80001f,
+	0x00000020, 0x1a00001f, 0x100062b4, 0x1910001f, 0x100062b4, 0x81322804,
+	0xe2000004, 0x81202804, 0xe2000004, 0x1b80001f, 0x20000080, 0xe2e0000e,
+	0xe2e0000c, 0xe2e0000d, 0xf0000000, 0x17c07c1f, 0xe2e0002d, 0x1a00001f,
+	0x100062b4, 0x1910001f, 0x100062b4, 0xa1002804, 0xe2000004, 0xa1122804,
+	0xe2000004, 0x1b80001f, 0x20000080, 0xe2e0002f, 0xe2e0002b, 0xe2e00023,
+	0x1b80001f, 0x00000020, 0xe2e00022, 0xf0000000, 0x17c07c1f, 0x1900001f,
+	0x1020020c, 0x1a10001f, 0x1020020c, 0xaa000008, 0x00000001, 0xe1000008,
+	0x1910001f, 0x10006720, 0x820c9001, 0xd82030c8, 0x17c07c1f, 0x1900001f,
+	0x10001220, 0x1a10001f, 0x10001220, 0xa21f0408, 0xe1000008, 0x1b80001f,
+	0x20000080, 0xe2e0006d, 0xe2e0002d, 0x1a00001f, 0x100062b8, 0x1910001f,
+	0x100062b8, 0xa9000004, 0x00000001, 0xe2000004, 0x1b80001f, 0x20000080,
+	0xe2e0002c, 0xe2e0003c, 0xe2e0003e, 0xe2e0003a, 0xe2e00032, 0x1b80001f,
+	0x00000020, 0x1900001f, 0x10006404, 0x1a10001f, 0x10006404, 0xa2168408,
+	0xe1000008, 0xf0000000, 0x17c07c1f, 0x1a10001f, 0x10006918, 0x8a000008,
+	0x00003030, 0xb900010c, 0x01000001, 0xd8203bc4, 0x17c07c1f, 0x1900001f,
+	0x10006404, 0x1a10001f, 0x10006404, 0x8a000008, 0x0000dfff, 0xe1000008,
+	0xe2e00036, 0xe2e0003e, 0x1b80001f, 0x00000020, 0xe2e0002e, 0x1a00001f,
+	0x100062b8, 0x1910001f, 0x100062b8, 0x89000004, 0x0000fffe, 0xe2000004,
+	0x1b80001f, 0x20000080, 0xe2e0006e, 0xe2e0004e, 0xe2e0004c, 0xe2e0004d,
+	0x1900001f, 0x10001220, 0x1a10001f, 0x10001220, 0x8a000008, 0xbfffffff,
+	0xe1000008, 0x1b80001f, 0x20000080, 0x1900001f, 0x1020020c, 0x1a10001f,
+	0x1020020c, 0x8a000008, 0xfffffffe, 0xe1000008, 0xf0000000, 0x17c07c1f,
 	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
 	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
 	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
 	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
 	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
-	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
 	0x17c07c1f, 0x17c07c1f, 0x1840001f, 0x00000001, 0x11407c1f, 0xe8208000,
 	0x10006b6c, 0xa0000000, 0xe8208000, 0x10006310, 0x0b160008, 0x1900001f,
 	0x000f7bde, 0x1a00001f, 0x10200268, 0xe2000004, 0xe8208000, 0x10006600,
-	0x00000000, 0xc2800940, 0x1280041f, 0x1b00001f, 0x21000001, 0x1b80001f,
-	0xd0010000, 0xc2800940, 0x1290841f, 0x69200006, 0xbeefbeef, 0xd8204764,
-	0x17c07c1f, 0xc2800940, 0x1291041f, 0x1910001f, 0x10006358, 0x810b1001,
-	0xd80043e4, 0x17c07c1f, 0x1980001f, 0xdeaddead, 0x69200006, 0xabcdabcd,
-	0xd82044c4, 0x17c07c1f, 0xc2800940, 0x1291841f, 0x88900001, 0x10006814,
+	0x00000000, 0x69200006, 0xbeefbeef, 0xd82045e4, 0x17c07c1f, 0x1910001f,
+	0x10006358, 0x810b1001, 0xd80042a4, 0x17c07c1f, 0x1980001f, 0xdeaddead,
+	0x69200006, 0xabcdabcd, 0xd8204384, 0x17c07c1f, 0x88900001, 0x10006814,
 	0x1910001f, 0x10006400, 0x81271002, 0x1880001f, 0x10006600, 0xe0800004,
-	0x1910001f, 0x10006358, 0x810b1001, 0xd8004684, 0x17c07c1f, 0x1980001f,
-	0x12345678, 0x60a07c05, 0x89100002, 0x10006600, 0x80801001, 0xd8007142,
-	0x17c07c1f, 0xc2800940, 0x1292041f, 0x1a10001f, 0x10006720, 0x82002001,
-	0x82201408, 0xd8204a08, 0x17c07c1f, 0x1a40001f, 0x10006200, 0x1a80001f,
-	0x1000625c, 0xc2400200, 0x17c07c1f, 0xa1400405, 0xc2800940, 0x1292841f,
-	0x1a10001f, 0x10006720, 0x8200a001, 0x82209408, 0xd8204be8, 0x17c07c1f,
-	0x1a40001f, 0x10006218, 0x1a80001f, 0x10006264, 0xc2400200, 0x17c07c1f,
-	0xa1508405, 0xc2800940, 0x1293041f, 0x1a10001f, 0x10006720, 0x82012001,
-	0x82211408, 0xd8204dc8, 0x17c07c1f, 0x1a40001f, 0x1000621c, 0x1a80001f,
-	0x1000626c, 0xc2400200, 0x17c07c1f, 0xa1510405, 0x1a10001f, 0x10006720,
-	0x8201a001, 0x82219408, 0xd8204f68, 0x17c07c1f, 0x1a40001f, 0x10006220,
-	0x1a80001f, 0x10006274, 0xc2400200, 0x17c07c1f, 0xa1518405, 0x1a10001f,
-	0x10006720, 0x82022001, 0x82221408, 0xd82050e8, 0x17c07c1f, 0x1a40001f,
-	0x100062a0, 0x1280041f, 0xc2400660, 0x17c07c1f, 0xa1520405, 0x1a10001f,
-	0x10006720, 0x8202a001, 0x82229408, 0xd8205268, 0x17c07c1f, 0x1a40001f,
-	0x100062a4, 0x1290841f, 0xc2400660, 0x17c07c1f, 0xa1528405, 0x1a10001f,
-	0x10006720, 0x82032001, 0x82231408, 0xd82053e8, 0x17c07c1f, 0x1a40001f,
-	0x100062a8, 0x1291041f, 0xc2400660, 0x17c07c1f, 0xa1530405, 0x1a10001f,
-	0x10006720, 0x8203a001, 0x82239408, 0xd8205568, 0x17c07c1f, 0x1a40001f,
-	0x100062ac, 0x1291841f, 0xc2400660, 0x17c07c1f, 0xa1538405, 0x1b80001f,
-	0x20000208, 0xd82070cc, 0x17c07c1f, 0x81001401, 0xd8205964, 0x17c07c1f,
-	0x1a10001f, 0x10006918, 0x81002001, 0xb1042081, 0xb1003081, 0xb10c3081,
-	0xd8205964, 0x17c07c1f, 0x1a40001f, 0x10006200, 0x1a80001f, 0x1000625c,
-	0xc2400000, 0x17c07c1f, 0x89400005, 0xfffffffe, 0xe8208000, 0x10006f00,
-	0x00000000, 0xe8208000, 0x10006b30, 0x00000000, 0xe8208000, 0x100063e0,
-	0x00000001, 0x81009401, 0xd8205cc4, 0x17c07c1f, 0x1a10001f, 0x10006918,
-	0x8100a001, 0xb104a081, 0xb1003081, 0xd8205cc4, 0x17c07c1f, 0x1a40001f,
-	0x10006218, 0x1a80001f, 0x10006264, 0xc2400000, 0x17c07c1f, 0x89400005,
-	0xfffffffd, 0xe8208000, 0x10006f04, 0x00000000, 0xe8208000, 0x10006b34,
-	0x00000000, 0xe8208000, 0x100063e0, 0x00000002, 0x81011401, 0xd8206024,
-	0x17c07c1f, 0x1a10001f, 0x10006918, 0x81012001, 0xb1052081, 0xb1003081,
-	0xd8206024, 0x17c07c1f, 0x1a40001f, 0x1000621c, 0x1a80001f, 0x1000626c,
-	0xc2400000, 0x17c07c1f, 0x89400005, 0xfffffffb, 0xe8208000, 0x10006f08,
-	0x00000000, 0xe8208000, 0x10006b38, 0x00000000, 0xe8208000, 0x100063e0,
-	0x00000004, 0x81019401, 0xd8206384, 0x17c07c1f, 0x1a10001f, 0x10006918,
-	0x8101a001, 0xb105a081, 0xb1003081, 0xd8206384, 0x17c07c1f, 0x1a40001f,
-	0x10006220, 0x1a80001f, 0x10006274, 0xc2400000, 0x17c07c1f, 0x89400005,
-	0xfffffff7, 0xe8208000, 0x10006f0c, 0x00000000, 0xe8208000, 0x10006b3c,
-	0x00000000, 0xe8208000, 0x100063e0, 0x00000008, 0x81021401, 0xd82066c4,
-	0x17c07c1f, 0x1a10001f, 0x10006918, 0x81022001, 0xb1062081, 0xb1003081,
-	0xd82066c4, 0x17c07c1f, 0x1a40001f, 0x100062a0, 0x1280041f, 0xc2400360,
-	0x17c07c1f, 0x89400005, 0xffffffef, 0xe8208000, 0x10006f10, 0x00000000,
-	0xe8208000, 0x10006b40, 0x00000000, 0xe8208000, 0x100063e0, 0x00000010,
-	0x81029401, 0xd8206a04, 0x17c07c1f, 0x1a10001f, 0x10006918, 0x8102a001,
-	0xb106a081, 0xb1003081, 0xd8206a04, 0x17c07c1f, 0x1a40001f, 0x100062a4,
-	0x1290841f, 0xc2400360, 0x17c07c1f, 0x89400005, 0xffffffdf, 0xe8208000,
+	0x1910001f, 0x10006358, 0x810b1001, 0xd8004504, 0x17c07c1f, 0x1980001f,
+	0x12345678, 0x60a07c05, 0x89100002, 0x10006600, 0x80801001, 0xd8007e02,
+	0x17c07c1f, 0x1890001f, 0x10006b00, 0x82090801, 0xc8800008, 0x17c07c1f,
+	0x1b00001f, 0x3fffe7ff, 0x8a00000c, 0x3fffe7ff, 0xd8204228, 0x17c07c1f,
+	0x1b80001f, 0xd0010000, 0x1a10001f, 0x10006720, 0x82002001, 0x82201408,
+	0xd82049e8, 0x17c07c1f, 0x1a40001f, 0x10006200, 0x1a80001f, 0x1000625c,
+	0xc24029a0, 0x17c07c1f, 0xa1400405, 0x1a10001f, 0x10006720, 0x8200a001,
+	0x82209408, 0xd8204b88, 0x17c07c1f, 0x1a40001f, 0x10006218, 0x1a80001f,
+	0x10006264, 0xc24029a0, 0x17c07c1f, 0xa1508405, 0x1a10001f, 0x10006720,
+	0x82012001, 0x82211408, 0xd8204d28, 0x17c07c1f, 0x1a40001f, 0x1000621c,
+	0x1a80001f, 0x1000626c, 0xc24029a0, 0x17c07c1f, 0xa1510405, 0x1a10001f,
+	0x10006720, 0x8201a001, 0x82219408, 0xd8204ec8, 0x17c07c1f, 0x1a40001f,
+	0x10006220, 0x1a80001f, 0x10006274, 0xc24029a0, 0x17c07c1f, 0xa1518405,
+	0x1a10001f, 0x10006720, 0x82022001, 0x82221408, 0xd8205048, 0x17c07c1f,
+	0x1a40001f, 0x100062a0, 0x1280041f, 0xc2402d80, 0x17c07c1f, 0xa1520405,
+	0x1a10001f, 0x10006720, 0x8202a001, 0x82229408, 0xd82051c8, 0x17c07c1f,
+	0x1a40001f, 0x100062a4, 0x1290841f, 0xc2402d80, 0x17c07c1f, 0xa1528405,
+	0x1a10001f, 0x10006720, 0x82032001, 0x82231408, 0xd82052a8, 0x17c07c1f,
+	0xa1530405, 0x1a10001f, 0x10006720, 0x8203a001, 0x82239408, 0xd8205388,
+	0x17c07c1f, 0xa1538405, 0x1a10001f, 0x10006b00, 0x8108a001, 0xd8205ec4,
+	0x17c07c1f, 0x1a10001f, 0x10006610, 0x8104a001, 0xb1052081, 0xb105a081,
+	0xb1062081, 0xd8005984, 0x17c07c1f, 0x81042001, 0xd8205984, 0x17c07c1f,
+	0x1900001f, 0x1020002c, 0x1a10001f, 0x1020002c, 0xaa000008, 0x00000010,
+	0xe1000008, 0x1910001f, 0x10006720, 0x820c1001, 0xd8205668, 0x17c07c1f,
+	0x1900001f, 0x10001250, 0x1a10001f, 0x10001250, 0xa2110408, 0xe1000008,
+	0x1b80001f, 0x20000080, 0x1900001f, 0x10001220, 0x1a10001f, 0x10001220,
+	0xa21e8408, 0xe1000008, 0x1b80001f, 0x20000080, 0x1a40001f, 0x10006208,
+	0xc24024c0, 0x17c07c1f, 0x1910001f, 0x10006610, 0x81041001, 0xd8005ec4,
+	0x17c07c1f, 0x1a10001f, 0x10006918, 0x8a000008, 0x00000f0f, 0xb900010c,
+	0x1fffe7ff, 0xd8205ec4, 0x17c07c1f, 0x1a40001f, 0x10006208, 0xc2402280,
+	0x17c07c1f, 0x1900001f, 0x10001250, 0x1a10001f, 0x10001250, 0x8a000008,
+	0xfffffffb, 0xe1000008, 0x1b80001f, 0x20000080, 0x1900001f, 0x10001220,
+	0x1a10001f, 0x10001220, 0x8a000008, 0xdfffffff, 0xe1000008, 0x1b80001f,
+	0x20000080, 0x1900001f, 0x1020002c, 0x1a10001f, 0x1020002c, 0x8a000008,
+	0xffffffef, 0xe1000008, 0x1a10001f, 0x10006b00, 0x81082001, 0xd8206144,
+	0x17c07c1f, 0x1a10001f, 0x10006610, 0x81082001, 0xb108a081, 0xd8006144,
+	0x17c07c1f, 0x1a10001f, 0x10006610, 0x8107a001, 0xd8206144, 0x17c07c1f,
+	0x1a40001f, 0x100062b0, 0xc2402fe0, 0x17c07c1f, 0x1b80001f, 0x20000208,
+	0xd8207dcc, 0x17c07c1f, 0x1910001f, 0x10006610, 0x81079001, 0xd80062e4,
+	0x17c07c1f, 0x1a40001f, 0x100062b0, 0xc24035a0, 0x17c07c1f, 0x81001401,
+	0xd8206664, 0x17c07c1f, 0x1a10001f, 0x10006918, 0x81002001, 0xb1042081,
+	0xb900008c, 0x1fffe7ff, 0xd8206664, 0x17c07c1f, 0x1a40001f, 0x10006200,
+	0x1a80001f, 0x1000625c, 0xc24027a0, 0x17c07c1f, 0x89400005, 0xfffffffe,
+	0xe8208000, 0x10006f00, 0x00000000, 0xe8208000, 0x10006b30, 0x00000000,
+	0xe8208000, 0x100063e0, 0x00000001, 0x81009401, 0xd82069e4, 0x17c07c1f,
+	0x1a10001f, 0x10006918, 0x8100a001, 0xb104a081, 0xb900008c, 0x01000001,
+	0xd82069e4, 0x17c07c1f, 0x1a40001f, 0x10006218, 0x1a80001f, 0x10006264,
+	0xc24027a0, 0x17c07c1f, 0x89400005, 0xfffffffd, 0xe8208000, 0x10006f04,
+	0x00000000, 0xe8208000, 0x10006b34, 0x00000000, 0xe8208000, 0x100063e0,
+	0x00000002, 0x81011401, 0xd8206d64, 0x17c07c1f, 0x1a10001f, 0x10006918,
+	0x81012001, 0xb1052081, 0xb900008c, 0x01000001, 0xd8206d64, 0x17c07c1f,
+	0x1a40001f, 0x1000621c, 0x1a80001f, 0x1000626c, 0xc24027a0, 0x17c07c1f,
+	0x89400005, 0xfffffffb, 0xe8208000, 0x10006f08, 0x00000000, 0xe8208000,
+	0x10006b38, 0x00000000, 0xe8208000, 0x100063e0, 0x00000004, 0x81019401,
+	0xd82070e4, 0x17c07c1f, 0x1a10001f, 0x10006918, 0x8101a001, 0xb105a081,
+	0xb900008c, 0x01000001, 0xd82070e4, 0x17c07c1f, 0x1a40001f, 0x10006220,
+	0x1a80001f, 0x10006274, 0xc24027a0, 0x17c07c1f, 0x89400005, 0xfffffff7,
+	0xe8208000, 0x10006f0c, 0x00000000, 0xe8208000, 0x10006b3c, 0x00000000,
+	0xe8208000, 0x100063e0, 0x00000008, 0x1910001f, 0x10006610, 0x81079001,
+	0xd8207844, 0x17c07c1f, 0x81021401, 0xd82074e4, 0x17c07c1f, 0x1a10001f,
+	0x10006918, 0x81022001, 0xb1062081, 0xb900008c, 0x01000001, 0xd82074e4,
+	0x17c07c1f, 0x1a40001f, 0x100062a0, 0x1280041f, 0xc2402b20, 0x17c07c1f,
+	0x89400005, 0xffffffef, 0xe8208000, 0x10006f10, 0x00000000, 0xe8208000,
+	0x10006b40, 0x00000000, 0xe8208000, 0x100063e0, 0x00000010, 0x81029401,
+	0xd8207844, 0x17c07c1f, 0x1a10001f, 0x10006918, 0x8102a001, 0xb106a081,
+	0xb900008c, 0x01000001, 0xd8207844, 0x17c07c1f, 0x1a40001f, 0x100062a4,
+	0x1290841f, 0xc2402b20, 0x17c07c1f, 0x89400005, 0xffffffdf, 0xe8208000,
 	0x10006f14, 0x00000000, 0xe8208000, 0x10006b44, 0x00000000, 0xe8208000,
-	0x100063e0, 0x00000020, 0x81031401, 0xd8206d44, 0x17c07c1f, 0x1a10001f,
-	0x10006918, 0x81032001, 0xb1072081, 0xb1003081, 0xd8206d44, 0x17c07c1f,
-	0x1a40001f, 0x100062a8, 0x1291041f, 0xc2400360, 0x17c07c1f, 0x89400005,
-	0xffffffbf, 0xe8208000, 0x10006f18, 0x00000000, 0xe8208000, 0x10006b48,
-	0x00000000, 0xe8208000, 0x100063e0, 0x00000040, 0x81039401, 0xd8207084,
-	0x17c07c1f, 0x1a10001f, 0x10006918, 0x8103a001, 0xb107a081, 0xb1003081,
-	0xd8207084, 0x17c07c1f, 0x1a40001f, 0x100062ac, 0x1291841f, 0xc2400360,
-	0x17c07c1f, 0x89400005, 0xffffff7f, 0xe8208000, 0x10006f1c, 0x00000000,
-	0xe8208000, 0x10006b4c, 0x00000000, 0xe8208000, 0x100063e0, 0x00000080,
-	0xc2800940, 0x1293841f, 0xd0004260, 0x17c07c1f, 0xc2800940, 0x1294041f,
+	0x100063e0, 0x00000020, 0x81031401, 0xd8207b04, 0x17c07c1f, 0x1a10001f,
+	0x10006918, 0x81032001, 0xb1072081, 0xb900008c, 0x01000001, 0xd8207b04,
+	0x17c07c1f, 0x89400005, 0xffffffbf, 0xe8208000, 0x10006f18, 0x00000000,
+	0xe8208000, 0x10006b48, 0x00000000, 0xe8208000, 0x100063e0, 0x00000040,
+	0x81039401, 0xd8207dc4, 0x17c07c1f, 0x1a10001f, 0x10006918, 0x8103a001,
+	0xb107a081, 0xb900008c, 0x01000001, 0xd8207dc4, 0x17c07c1f, 0x89400005,
+	0xffffff7f, 0xe8208000, 0x10006f1c, 0x00000000, 0xe8208000, 0x10006b4c,
+	0x00000000, 0xe8208000, 0x100063e0, 0x00000080, 0xd0004220, 0x17c07c1f,
 	0xe8208000, 0x10006600, 0x00000000, 0x1ac0001f, 0x55aa55aa, 0x1940001f,
-	0xaa55aa55, 0xc2800940, 0x1294841f, 0x1b80001f, 0x00001000, 0xf0000000,
-	0x17c07c1f
+	0xaa55aa55, 0x1b80001f, 0x00001000, 0xf0000000, 0x17c07c1f
 };
 
 static const struct pcm_desc mcdi_pcm = {
-	.version = "pcm_mcdi_v0.5_20140721_mt8173_v03.04_20150507",
+	.version = "pcm_mcdi_mt8173_20151203_v6",
 	.base = mcdi_binary,
-	.size = 919,
+	.size = 1019,
 	.sess = 2,
 	.replace = 0,
 };
@@ -239,8 +263,8 @@
 	    && ((mmio_read_32(SPM_CLK_CON) & CC_DISABLE_DORM_PWR) == 0)) {
 		/* MCDI is offload? */
 		INFO("%s: SPM_SLEEP_CPU_WAKEUP_EVENT:%x, SPM_CLK_CON %x",
-		     __func__, mmio_read_32(SPM_SLEEP_CPU_WAKEUP_EVENT),
-		     mmio_read_32(SPM_CLK_CON));
+			__func__, mmio_read_32(SPM_SLEEP_CPU_WAKEUP_EVENT),
+			mmio_read_32(SPM_CLK_CON));
 		return;
 	}
 	/* Inform SPM that CPU wants to program CPU_WAKEUP_EVENT and
@@ -301,7 +325,7 @@
 	clear_all_ready();
 }
 
-void spm_mcdi_wfi_sel_enter(unsigned long mpidr)
+static void spm_mcdi_wfi_sel_enter(unsigned long mpidr)
 {
 	int core_id_val = mpidr & MPIDR_CPU_MASK;
 	int cluster_id = (mpidr & MPIDR_CLUSTER_MASK) >> MPIDR_AFFINITY_BITS;
@@ -352,7 +376,7 @@
 	}
 }
 
-void spm_mcdi_wfi_sel_leave(unsigned long mpidr)
+static void spm_mcdi_wfi_sel_leave(unsigned long mpidr)
 {
 	int core_id_val = mpidr & MPIDR_CPU_MASK;
 	int cluster_id = (mpidr & MPIDR_CLUSTER_MASK) >> MPIDR_AFFINITY_BITS;
@@ -400,10 +424,72 @@
 		default:
 			break;
 		}
+	}
+}
+
+static void spm_mcdi_set_cputop_pwrctrl_for_cluster_off(unsigned long mpidr)
+{
+	unsigned long cluster_id = mpidr & MPIDR_CLUSTER_MASK;
+	unsigned long cpu_id = mpidr & MPIDR_CPU_MASK;
+	unsigned int pwr_status, shift, i, flag = 0;
+
+	pwr_status = mmio_read_32(SPM_PWR_STATUS) |
+				 mmio_read_32(SPM_PWR_STATUS_2ND);
+
+	if (cluster_id) {
+		for (i = 0; i < PLATFORM_CLUSTER1_CORE_COUNT; i++) {
+			if (i == cpu_id)
+				continue;
+			shift = i + PCM_MCDI_CA72_PWRSTA_SHIFT;
+			flag |= (pwr_status & (1 << shift)) >> shift;
+		}
+		if (!flag)
+			mmio_setbits_32(SPM_PCM_RESERVE,
+					PCM_MCDI_CA72_CPUTOP_PWRCTL);
+	} else {
+		for (i = 0; i < PLATFORM_CLUSTER0_CORE_COUNT; i++) {
+			if (i == cpu_id)
+				continue;
+			shift = i + PCM_MCDI_CA53_PWRSTA_SHIFT;
+			flag |= (pwr_status & (1 << shift)) >> shift;
+		}
+		if (!flag)
+			mmio_setbits_32(SPM_PCM_RESERVE,
+					PCM_MCDI_CA53_CPUTOP_PWRCTL);
+	}
+}
+
+static void spm_mcdi_clear_cputop_pwrctrl_for_cluster_on(unsigned long mpidr)
+{
+	unsigned long cluster_id = mpidr & MPIDR_CLUSTER_MASK;
+
+	if (cluster_id)
+		mmio_clrbits_32(SPM_PCM_RESERVE,
+				PCM_MCDI_CA72_CPUTOP_PWRCTL);
+	else
+		mmio_clrbits_32(SPM_PCM_RESERVE,
+				PCM_MCDI_CA53_CPUTOP_PWRCTL);
+}
+
+void spm_mcdi_prepare_for_mtcmos(void)
+{
+	const struct pcm_desc *pcmdesc = spm_mcdi.pcmdesc;
+	struct pwr_ctrl *pwrctrl = spm_mcdi.pwrctrl;
+
+	if (is_mcdi_ready() == 0) {
+		if (is_hotplug_ready() == 1)
+			spm_clear_hotplug();
+		set_pwrctrl_pcm_flags(pwrctrl, 0);
+		spm_reset_and_init_pcm();
+		spm_kick_im_to_fetch(pcmdesc);
+		spm_set_power_control(pwrctrl);
+		spm_set_wakeup_event(pwrctrl);
+		spm_kick_pcm_to_run(pwrctrl);
+		set_mcdi_ready();
 	}
 }
 
-void spm_mcdi_prepare(unsigned long mpidr)
+void spm_mcdi_prepare_for_off_state(unsigned long mpidr, unsigned int afflvl)
 {
 	const struct pcm_desc *pcmdesc = spm_mcdi.pcmdesc;
 	struct pwr_ctrl *pwrctrl = spm_mcdi.pwrctrl;
@@ -421,14 +507,17 @@
 		set_mcdi_ready();
 	}
 	spm_mcdi_wfi_sel_enter(mpidr);
+	if (afflvl == MPIDR_AFFLVL1)
+		spm_mcdi_set_cputop_pwrctrl_for_cluster_off(mpidr);
 	spm_lock_release();
 }
 
-void spm_mcdi_finish(unsigned long mpidr)
+void spm_mcdi_finish_for_on_state(unsigned long mpidr, unsigned int afflvl)
 {
 	unsigned long linear_id = platform_get_core_pos(mpidr);
 
 	spm_lock_get();
+	spm_mcdi_clear_cputop_pwrctrl_for_cluster_on(mpidr);
 	spm_mcdi_wfi_sel_leave(mpidr);
 	mmio_write_32(SPM_PCM_SW_INT_CLEAR, (0x1 << linear_id));
 	spm_lock_release();
diff --git a/plat/mediatek/mt8173/drivers/spm/spm_mcdi.h b/plat/mediatek/mt8173/drivers/spm/spm_mcdi.h
index 4df71d6..4d1f990 100644
--- a/plat/mediatek/mt8173/drivers/spm/spm_mcdi.h
+++ b/plat/mediatek/mt8173/drivers/spm/spm_mcdi.h
@@ -31,9 +31,8 @@
 #define __SPM_MCDI_H__
 
 void spm_mcdi_wakeup_all_cores(void);
-void spm_mcdi_wfi_sel_enter(unsigned long mpidr);
-void spm_mcdi_wfi_sel_leave(unsigned long mpidr);
-void spm_mcdi_prepare(unsigned long mpidr);
-void spm_mcdi_finish(unsigned long mpidr);
+void spm_mcdi_prepare_for_mtcmos(void);
+void spm_mcdi_prepare_for_off_state(unsigned long mpidr, unsigned int afflvl);
+void spm_mcdi_finish_for_on_state(unsigned long mpidr, unsigned int afflvl);
 
 #endif /* __SPM_MCDI_H__ */
diff --git a/plat/mediatek/mt8173/drivers/spm/spm_suspend.c b/plat/mediatek/mt8173/drivers/spm/spm_suspend.c
index b4d0a60..6bf37f3 100644
--- a/plat/mediatek/mt8173/drivers/spm/spm_suspend.c
+++ b/plat/mediatek/mt8173/drivers/spm/spm_suspend.c
@@ -61,81 +61,81 @@
  **********************************************************/
 static const unsigned int suspend_binary_ca7[] = {
 	0x81f58407, 0x81f68407, 0x803a0400, 0x803a8400, 0x1b80001f, 0x20000000,
-	0x80300400, 0x80318400, 0x80328400, 0xa1d28407, 0x81f20407, 0x81409801,
-	0xd8000245, 0x17c07c1f, 0x18c0001f, 0x10006234, 0xc0c031a0, 0x1200041f,
+	0x80300400, 0x80318400, 0x80328400, 0xa1d28407, 0x81f20407, 0x81009801,
+	0xd8000244, 0x17c07c1f, 0x18c0001f, 0x10006234, 0xc0c032e0, 0x1200041f,
 	0x80310400, 0x1b80001f, 0x2000000a, 0xa0110400, 0x18c0001f, 0x100062c8,
 	0xe0e00010, 0xe0e00030, 0xe0e00070, 0xe0e000f0, 0x1b80001f, 0x2000001a,
 	0xe0e00ff0, 0xe8208000, 0x10006354, 0xfffe7fff, 0xe8208000, 0x10006834,
-	0x00000010, 0x81f00407, 0xa1dd0407, 0x81fd0407, 0xc2803780, 0x1290041f,
-	0x8880000c, 0x2f7be75f, 0xd8200642, 0x17c07c1f, 0x1b00001f, 0x7fffe7ff,
-	0xd0000680, 0x17c07c1f, 0x1b00001f, 0x7ffff7ff, 0xf0000000, 0x17c07c1f,
-	0x80880001, 0xd8000762, 0x17c07c1f, 0xd00027a0, 0x1200041f, 0xe8208000,
-	0x10006834, 0x00000000, 0x1b00001f, 0x3fffe7ff, 0x1b80001f, 0x20000004,
-	0xd820092c, 0x17c07c1f, 0xe8208000, 0x10006834, 0x00000010, 0xd00011a0,
-	0x17c07c1f, 0x18c0001f, 0x10006608, 0x1910001f, 0x10006608, 0x813b0404,
-	0xe0c00004, 0x1880001f, 0x10006320, 0xc0c03680, 0xe080000f, 0xd8200b23,
-	0x17c07c1f, 0x1b00001f, 0x7ffff7ff, 0xd00011a0, 0x17c07c1f, 0xe080001f,
-	0xe8208000, 0x10006354, 0xffffffff, 0x18c0001f, 0x100062c8, 0xe0e000f0,
-	0xe0e00030, 0xe0e00000, 0x81409801, 0xd8000fe5, 0x17c07c1f, 0x18c0001f,
-	0x10004094, 0x1910001f, 0x1020e374, 0xe0c00004, 0x18c0001f, 0x10004098,
-	0x1910001f, 0x1020e378, 0xe0c00004, 0x18c0001f, 0x10011094, 0x1910001f,
-	0x10213374, 0xe0c00004, 0x18c0001f, 0x10011098, 0x1910001f, 0x10213378,
-	0xe0c00004, 0x1910001f, 0x10213378, 0x18c0001f, 0x10006234, 0xc0c03360,
-	0x17c07c1f, 0xc2803780, 0x1290841f, 0xa1d20407, 0x81f28407, 0xa1d68407,
-	0xa0128400, 0xa0118400, 0xa0100400, 0xa01a8400, 0xa01a0400, 0x19c0001f,
-	0x001c239f, 0x1b00001f, 0x3fffefff, 0xf0000000, 0x17c07c1f, 0x808d8001,
-	0xd8201422, 0x17c07c1f, 0x803d8400, 0x1b80001f, 0x2000001a, 0x80340400,
-	0x17c07c1f, 0x17c07c1f, 0x80310400, 0x81fa0407, 0x81f18407, 0x81f08407,
-	0xa1dc0407, 0x1b80001f, 0x200000b6, 0xd00020e0, 0x17c07c1f, 0x1880001f,
-	0x20000208, 0x81411801, 0xd8001605, 0x17c07c1f, 0xe8208000, 0x1000f600,
-	0xd2000000, 0x1380081f, 0x18c0001f, 0x10006240, 0xe0e00016, 0xe0e0001e,
-	0xe0e0000e, 0xe0e0000f, 0x80368400, 0x1380081f, 0x80370400, 0x1380081f,
-	0x80360400, 0x803e0400, 0x1380081f, 0x80380400, 0x803b0400, 0xa01d8400,
-	0x1b80001f, 0x20000034, 0x803d8400, 0x1b80001f, 0x20000152, 0x803d0400,
-	0x1380081f, 0x18c0001f, 0x1000f5c8, 0x1910001f, 0x1000f5c8, 0xa1000404,
-	0xe0c00004, 0x18c0001f, 0x100125c8, 0x1910001f, 0x100125c8, 0xa1000404,
-	0xe0c00004, 0x1910001f, 0x100125c8, 0x80340400, 0x17c07c1f, 0x17c07c1f,
-	0x80310400, 0xe8208000, 0x10000044, 0x00000100, 0x1b80001f, 0x20000068,
-	0x1b80001f, 0x2000000a, 0x18c0001f, 0x10006240, 0xe0e0000d, 0xd8001e65,
-	0x17c07c1f, 0x18c0001f, 0x100040f4, 0x1910001f, 0x100040f4, 0xa11c8404,
-	0xe0c00004, 0x1b80001f, 0x2000000a, 0x813c8404, 0xe0c00004, 0x18c0001f,
-	0x100110f4, 0x1910001f, 0x100110f4, 0xa11c8404, 0xe0c00004, 0x1b80001f,
-	0x2000000a, 0x813c8404, 0xe0c00004, 0x1b80001f, 0x20000100, 0x81fa0407,
-	0x81f18407, 0x81f08407, 0xe8208000, 0x10006354, 0xfffe7b47, 0x18c0001f,
-	0x65930003, 0xc0c03080, 0x17c07c1f, 0xa1d80407, 0xa1dc0407, 0x18c0001f,
-	0x10006608, 0x1910001f, 0x10006608, 0xa11b0404, 0xe0c00004, 0xc2803780,
-	0x1291041f, 0x8880000c, 0x2f7be75f, 0xd8202222, 0x17c07c1f, 0x1b00001f,
-	0x3fffe7ff, 0xd0002260, 0x17c07c1f, 0x1b00001f, 0xbfffe7ff, 0xf0000000,
-	0x17c07c1f, 0x1890001f, 0x10006608, 0x808b0801, 0xd8202502, 0x17c07c1f,
-	0x1880001f, 0x10006320, 0xc0c03400, 0xe080000f, 0xd8002663, 0x17c07c1f,
-	0xe080001f, 0xa1da0407, 0x81fc0407, 0xa0110400, 0xa0140400, 0xa01d8400,
-	0xd0002fc0, 0x17c07c1f, 0x1b80001f, 0x20000fdf, 0x1890001f, 0x10006608,
-	0x80c98801, 0x810a8801, 0x10918c1f, 0xa0939002, 0x8080080d, 0xd82027a2,
-	0x12007c1f, 0x1b00001f, 0x3fffe7ff, 0x1b80001f, 0x20000004, 0xd800304c,
-	0x17c07c1f, 0x1b00001f, 0xbfffe7ff, 0xd0003040, 0x17c07c1f, 0x81f80407,
-	0x81fc0407, 0x18c0001f, 0x65930006, 0xc0c03080, 0x17c07c1f, 0x18c0001f,
-	0x65930007, 0xc0c03080, 0x17c07c1f, 0x1880001f, 0x10006320, 0xc0c03400,
-	0xe080000f, 0xd8002663, 0x17c07c1f, 0xe080001f, 0x18c0001f, 0x65930005,
-	0xc0c03080, 0x17c07c1f, 0xa1da0407, 0xe8208000, 0x10000048, 0x00000100,
-	0x1b80001f, 0x20000068, 0xa0110400, 0xa0140400, 0x18c0001f, 0x1000f5c8,
-	0x1910001f, 0x1000f5c8, 0x81200404, 0xe0c00004, 0x18c0001f, 0x100125c8,
-	0x1910001f, 0x100125c8, 0x81200404, 0xe0c00004, 0x1910001f, 0x100125c8,
-	0xa01d0400, 0xa01b0400, 0xa0180400, 0x803d8400, 0xa01e0400, 0xa0160400,
-	0xa0170400, 0xa0168400, 0x1b80001f, 0x20000104, 0x81411801, 0xd8002f85,
-	0x17c07c1f, 0x18c0001f, 0x10006240, 0xc0c03360, 0x17c07c1f, 0xe8208000,
-	0x1000f600, 0xd2000001, 0xd8000768, 0x17c07c1f, 0xc2803780, 0x1291841f,
-	0x1b00001f, 0x7ffff7ff, 0xf0000000, 0x17c07c1f, 0x1900001f, 0x10006830,
-	0xe1000003, 0x18c0001f, 0x10006834, 0xe0e00000, 0xe0e00001, 0xf0000000,
-	0x17c07c1f, 0xe0f07f16, 0x1380201f, 0xe0f07f1e, 0x1380201f, 0xe0f07f0e,
-	0x1b80001f, 0x20000104, 0xe0f07f0c, 0xe0f07f0d, 0xe0f07e0d, 0xe0f07c0d,
-	0xe0f0780d, 0xf0000000, 0xe0f0700d, 0xe0f07f0d, 0xe0f07f0f, 0xe0f07f1e,
-	0xf0000000, 0xe0f07f12, 0x11407c1f, 0x81f08407, 0x81f18407, 0x1b80001f,
-	0x20000001, 0xa1d08407, 0xa1d18407, 0x1392841f, 0x812ab401, 0x80ebb401,
-	0xa0c00c04, 0xd8203603, 0x17c07c1f, 0x80c01403, 0xd8203423, 0x01400405,
-	0x1900001f, 0x10006814, 0xf0000000, 0xe1000003, 0xa1d00407, 0x1b80001f,
-	0x20000208, 0x80ea3401, 0x1a00001f, 0x10006814, 0xf0000000, 0xe2000003,
-	0x18c0001f, 0x10006b6c, 0x1910001f, 0x10006b6c, 0xa1002804, 0xf0000000,
-	0xe0c00004, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
+	0x00000010, 0x81f00407, 0xa1dd0407, 0x81fd0407, 0xc2803800, 0x1290041f,
+	0x8880000c, 0x2f7be75f, 0xd8200722, 0x17c07c1f, 0xd82006a9, 0x17c07c1f,
+	0xe8208000, 0x10006814, 0x00000001, 0xc2803800, 0x1293841f, 0x1b00001f,
+	0x7fffe7ff, 0xd0000760, 0x17c07c1f, 0x1b00001f, 0x7ffff7ff, 0xf0000000,
+	0x17c07c1f, 0x80880001, 0xd8000842, 0x17c07c1f, 0xd00028e0, 0x1200041f,
+	0xe8208000, 0x10006834, 0x00000000, 0x1b00001f, 0x3fffe7ff, 0x1b80001f,
+	0x20000004, 0xd8200a0c, 0x17c07c1f, 0xe8208000, 0x10006834, 0x00000010,
+	0xd0001280, 0x17c07c1f, 0x18c0001f, 0x10006608, 0x1910001f, 0x10006608,
+	0x813b0404, 0xe0c00004, 0x1880001f, 0x10006320, 0xc0c03760, 0xe080000f,
+	0xd8200c03, 0x17c07c1f, 0x1b00001f, 0x7ffff7ff, 0xd0001280, 0x17c07c1f,
+	0xe080001f, 0xe8208000, 0x10006354, 0xffffffff, 0x18c0001f, 0x100062c8,
+	0xe0e000f0, 0xe0e00030, 0xe0e00000, 0x81009801, 0xd80010c4, 0x17c07c1f,
+	0x18c0001f, 0x10004094, 0x1910001f, 0x1020e374, 0xe0c00004, 0x18c0001f,
+	0x10004098, 0x1910001f, 0x1020e378, 0xe0c00004, 0x18c0001f, 0x10011094,
+	0x1910001f, 0x10213374, 0xe0c00004, 0x18c0001f, 0x10011098, 0x1910001f,
+	0x10213378, 0xe0c00004, 0x1910001f, 0x10213378, 0x18c0001f, 0x10006234,
+	0xc0c034a0, 0x17c07c1f, 0xc2803800, 0x1290841f, 0xa1d20407, 0x81f28407,
+	0xa1d68407, 0xa0128400, 0xa0118400, 0xa0100400, 0xa01a8400, 0xa01a0400,
+	0x19c0001f, 0x001c239f, 0x1b00001f, 0x3fffefff, 0xf0000000, 0x17c07c1f,
+	0x808d8001, 0xd8201502, 0x17c07c1f, 0x803d8400, 0x1b80001f, 0x2000001a,
+	0x80340400, 0x17c07c1f, 0x17c07c1f, 0x80310400, 0x81fa0407, 0x81f18407,
+	0x81f08407, 0xa1dc0407, 0x1b80001f, 0x200000b6, 0xd0002220, 0x17c07c1f,
+	0x1880001f, 0x20000208, 0x81011801, 0xd80016e4, 0x17c07c1f, 0xe8208000,
+	0x1000f600, 0xd2000000, 0x1380081f, 0x18c0001f, 0x10006240, 0xe0e00016,
+	0xe0e0001e, 0xe0e0000e, 0xe0e0000f, 0x80368400, 0x1380081f, 0x80370400,
+	0x1380081f, 0x80360400, 0x803e0400, 0x1380081f, 0x80380400, 0x803b0400,
+	0xa01d8400, 0x1b80001f, 0x20000034, 0x803d8400, 0x1b80001f, 0x20000152,
+	0x803d0400, 0x1380081f, 0x18c0001f, 0x1000f5c8, 0x1910001f, 0x1000f5c8,
+	0xa1000404, 0xe0c00004, 0x18c0001f, 0x100125c8, 0x1910001f, 0x100125c8,
+	0xa1000404, 0xe0c00004, 0x1910001f, 0x100125c8, 0x80340400, 0x17c07c1f,
+	0x17c07c1f, 0x80310400, 0xe8208000, 0x10000044, 0x00000100, 0x1b80001f,
+	0x20000068, 0x1b80001f, 0x2000000a, 0x18c0001f, 0x10006240, 0xe0e0000d,
+	0x81011801, 0xd8001f64, 0x17c07c1f, 0x18c0001f, 0x100040f4, 0x1910001f,
+	0x100040f4, 0xa11c8404, 0xe0c00004, 0x1b80001f, 0x2000000a, 0x813c8404,
+	0xe0c00004, 0x18c0001f, 0x100110f4, 0x1910001f, 0x100110f4, 0xa11c8404,
+	0xe0c00004, 0x1b80001f, 0x2000000a, 0x813c8404, 0xe0c00004, 0x1b80001f,
+	0x20000100, 0x81fa0407, 0x81f18407, 0x81f08407, 0xe8208000, 0x10006354,
+	0xfffe7b47, 0x18c0001f, 0x65930003, 0xc0c031c0, 0x17c07c1f, 0xc2803800,
+	0x1293041f, 0xa1d80407, 0xa1dc0407, 0x18c0001f, 0x10006608, 0x1910001f,
+	0x10006608, 0xa11b0404, 0xe0c00004, 0xc2803800, 0x1291041f, 0x8880000c,
+	0x2f7be75f, 0xd8202362, 0x17c07c1f, 0x1b00001f, 0x3fffe7ff, 0xd00023a0,
+	0x17c07c1f, 0x1b00001f, 0xbfffe7ff, 0xf0000000, 0x17c07c1f, 0x1890001f,
+	0x10006608, 0x808b0801, 0xd8202642, 0x17c07c1f, 0x1880001f, 0x10006320,
+	0xc0c03540, 0xe080000f, 0xd80027a3, 0x17c07c1f, 0xe080001f, 0xa1da0407,
+	0x81fc0407, 0xa0110400, 0xa0140400, 0xa01d8400, 0xd0003100, 0x17c07c1f,
+	0x1b80001f, 0x20000fdf, 0x1890001f, 0x10006608, 0x80c98801, 0x810a8801,
+	0x10918c1f, 0xa0939002, 0x8080080d, 0xd82028e2, 0x12007c1f, 0x1b00001f,
+	0x3fffe7ff, 0x1b80001f, 0x20000004, 0xd800318c, 0x17c07c1f, 0x1b00001f,
+	0xbfffe7ff, 0xd0003180, 0x17c07c1f, 0x81f80407, 0x81fc0407, 0x18c0001f,
+	0x65930006, 0xc0c031c0, 0x17c07c1f, 0x18c0001f, 0x65930007, 0xc0c031c0,
+	0x17c07c1f, 0x1880001f, 0x10006320, 0xc0c03540, 0xe080000f, 0xd80027a3,
+	0x17c07c1f, 0xe080001f, 0x18c0001f, 0x65930005, 0xc0c031c0, 0x17c07c1f,
+	0xa1da0407, 0xe8208000, 0x10000048, 0x00000100, 0x1b80001f, 0x20000068,
+	0xa0110400, 0xa0140400, 0x18c0001f, 0x1000f5c8, 0x1910001f, 0x1000f5c8,
+	0x81200404, 0xe0c00004, 0x18c0001f, 0x100125c8, 0x1910001f, 0x100125c8,
+	0x81200404, 0xe0c00004, 0x1910001f, 0x100125c8, 0xa01d0400, 0xa01b0400,
+	0xa0180400, 0x803d8400, 0xa01e0400, 0xa0160400, 0xa0170400, 0xa0168400,
+	0x1b80001f, 0x20000104, 0x81011801, 0xd80030c4, 0x17c07c1f, 0x18c0001f,
+	0x10006240, 0xc0c034a0, 0x17c07c1f, 0xe8208000, 0x1000f600, 0xd2000001,
+	0xd8000848, 0x17c07c1f, 0xc2803800, 0x1291841f, 0x1b00001f, 0x7ffff7ff,
+	0xf0000000, 0x17c07c1f, 0x1900001f, 0x10006830, 0xe1000003, 0x18c0001f,
+	0x10006834, 0xe0e00000, 0xe0e00001, 0xf0000000, 0x17c07c1f, 0xe0f07f16,
+	0x1380201f, 0xe0f07f1e, 0x1380201f, 0xe0f07f0e, 0x1b80001f, 0x20000104,
+	0xe0f07f0c, 0xe0f07f0d, 0xe0f07e0d, 0xe0f07c0d, 0xe0f0780d, 0xf0000000,
+	0xe0f0700d, 0xe0f07f0d, 0xe0f07f0f, 0xe0f07f1e, 0xf0000000, 0xe0f07f12,
+	0x11407c1f, 0x81f08407, 0x81f18407, 0x1b80001f, 0x20000001, 0xa1d08407,
+	0xa1d18407, 0x1392841f, 0x812ab401, 0x80ebb401, 0xa0c00c04, 0xd8203743,
+	0x17c07c1f, 0x80c01403, 0xd8203563, 0x01400405, 0xf0000000, 0xa1d00407,
+	0x1b80001f, 0x20000208, 0x80ea3401, 0xf0000000, 0x18c0001f, 0x10006b6c,
+	0x1910001f, 0x10006b6c, 0xa1002804, 0xf0000000, 0xe0c00004, 0x17c07c1f,
 	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
 	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
 	0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f, 0x17c07c1f,
@@ -148,75 +148,78 @@
 	0x17c07c1f, 0x17c07c1f, 0x1840001f, 0x00000001, 0xa1d48407, 0x1990001f,
 	0x10006b08, 0x1a50001f, 0x10006610, 0x8246a401, 0xe8208000, 0x10006b6c,
 	0x00000000, 0x1b00001f, 0x2f7be75f, 0x81469801, 0xd8004305, 0x17c07c1f,
-	0x1b80001f, 0xd00f0000, 0x8880000c, 0x2f7be75f, 0xd8005da2, 0x17c07c1f,
+	0x1b80001f, 0xd00f0000, 0x8880000c, 0x2f7be75f, 0xd8005fa2, 0x17c07c1f,
 	0xd0004340, 0x17c07c1f, 0x1b80001f, 0x500f0000, 0xe8208000, 0x10006354,
-	0xfffe7b47, 0xc0c06940, 0x81401801, 0xd80048e5, 0x17c07c1f, 0x81f60407,
-	0x18c0001f, 0x10006200, 0xc0c05e60, 0x12807c1f, 0xe8208000, 0x1000625c,
-	0x00000001, 0x1b80001f, 0x20000080, 0xc0c05e60, 0x1280041f, 0x18c0001f,
-	0x10006204, 0xc0c06200, 0x1280041f, 0x18c0001f, 0x10006208, 0xc0c05e60,
+	0xfffe7b47, 0xc0c06c00, 0x81401801, 0xd80048e5, 0x17c07c1f, 0x81f60407,
+	0x18c0001f, 0x10006200, 0xc0c06060, 0x12807c1f, 0xe8208000, 0x1000625c,
+	0x00000001, 0x1b80001f, 0x20000080, 0xc0c06060, 0x1280041f, 0x18c0001f,
+	0x10006204, 0xc0c06400, 0x1280041f, 0x18c0001f, 0x10006208, 0xc0c06060,
 	0x12807c1f, 0xe8208000, 0x10006244, 0x00000001, 0x1b80001f, 0x20000080,
-	0xc0c05e60, 0x1280041f, 0x18d0001f, 0x10200200, 0x18c0001f, 0x10006290,
-	0xc0c05e60, 0x1280041f, 0xe8208000, 0x10006404, 0x00003101, 0xc2803780,
+	0xc0c06060, 0x1280041f, 0x18d0001f, 0x10200200, 0x18c0001f, 0x10006290,
+	0xc0c06060, 0x1280041f, 0xe8208000, 0x10006404, 0x00003101, 0xc2803800,
 	0x1292041f, 0x81469801, 0xd8204a45, 0x17c07c1f, 0x1b00001f, 0x2f7be75f,
-	0x1b80001f, 0x30000004, 0x8880000c, 0x2f7be75f, 0xd8005802, 0x17c07c1f,
-	0xc0c064c0, 0x17c07c1f, 0x18c0001f, 0x10006294, 0xe0f07fff, 0xe0e00fff,
+	0x1b80001f, 0x30000004, 0x8880000c, 0x2f7be75f, 0xd8005a02, 0x17c07c1f,
+	0xc0c06780, 0x17c07c1f, 0x18c0001f, 0x10006294, 0xe0f07fff, 0xe0e00fff,
 	0xe0e000ff, 0x81449801, 0xd8004c85, 0x17c07c1f, 0x1a00001f, 0x10006604,
-	0xe2200003, 0xc0c06580, 0x17c07c1f, 0xe2200005, 0xc0c06580, 0x17c07c1f,
+	0xe2200003, 0xc0c06840, 0x17c07c1f, 0xe2200005, 0xc0c06840, 0x17c07c1f,
 	0xa1d38407, 0xa1d98407, 0x1800001f, 0x00000012, 0x1800001f, 0x00000e12,
 	0x1800001f, 0x03800e12, 0x1800001f, 0x038e0e12, 0xe8208000, 0x10006310,
-	0x0b1600f8, 0x1b00001f, 0xbfffe7ff, 0x1b80001f, 0x90100000, 0x80c00400,
-	0xd8204fa3, 0xa1d58407, 0xa1dd8407, 0x1b00001f, 0x3fffefff, 0xd0004e60,
-	0x17c07c1f, 0x1890001f, 0x100063e8, 0x88c0000c, 0x2f7be75f, 0xd80051c3,
-	0x17c07c1f, 0x80c40001, 0xd8005143, 0x17c07c1f, 0x1b00001f, 0xbfffe7ff,
-	0xd0005180, 0x17c07c1f, 0x1b00001f, 0x7ffff7ff, 0xd0004e60, 0x17c07c1f,
-	0x80c40001, 0xd82052c3, 0x17c07c1f, 0xa1de0407, 0x1b00001f, 0x7fffe7ff,
-	0xd0004e60, 0x17c07c1f, 0x18c0001f, 0x10006294, 0xe0e001fe, 0xe0e003fc,
-	0xe0e007f8, 0xe0e00ff0, 0x1b80001f, 0x20000020, 0xe0f07ff0, 0xe0f07f00,
-	0x81449801, 0xd80055a5, 0x17c07c1f, 0x1a00001f, 0x10006604, 0xe2200002,
-	0xc0c06580, 0x17c07c1f, 0xe2200004, 0xc0c06580, 0x17c07c1f, 0x1b80001f,
-	0x200016a8, 0x1800001f, 0x03800e12, 0x1b80001f, 0x20000300, 0x1800001f,
-	0x00000e12, 0x1b80001f, 0x20000300, 0x1800001f, 0x00000012, 0x1b80001f,
-	0x20000104, 0x10007c1f, 0x81f38407, 0x81f98407, 0x81f90407, 0x81f40407,
-	0x1b80001f, 0x200016a8, 0x81401801, 0xd8005da5, 0x17c07c1f, 0xe8208000,
-	0x10006404, 0x00002101, 0x18c0001f, 0x10006290, 0x1212841f, 0xc0c05fe0,
-	0x12807c1f, 0xc0c05fe0, 0x1280041f, 0x18c0001f, 0x10006208, 0x1212841f,
-	0xc0c05fe0, 0x12807c1f, 0xe8208000, 0x10006244, 0x00000000, 0x1b80001f,
-	0x20000080, 0xc0c05fe0, 0x1280041f, 0xe8208000, 0x10200268, 0x000ffffe,
-	0x18c0001f, 0x10006204, 0x1212841f, 0xc0c06340, 0x1280041f, 0x18c0001f,
-	0x10006200, 0x1212841f, 0xc0c05fe0, 0x12807c1f, 0xe8208000, 0x1000625c,
-	0x00000000, 0x1b80001f, 0x20000080, 0xc0c05fe0, 0x1280041f, 0x19c0001f,
-	0x01411820, 0x1ac0001f, 0x55aa55aa, 0x10007c1f, 0xf0000000, 0xd8005f0a,
-	0x17c07c1f, 0xe2e0004f, 0xe2e0006f, 0xe2e0002f, 0xd8205faa, 0x17c07c1f,
-	0xe2e0002e, 0xe2e0003e, 0xe2e00032, 0xf0000000, 0x17c07c1f, 0xd80060aa,
-	0x17c07c1f, 0xe2e00036, 0xe2e0003e, 0x1380201f, 0xe2e0003c, 0xd82061ca,
-	0x17c07c1f, 0x1380201f, 0xe2e0007c, 0x1b80001f, 0x20000003, 0xe2e0005c,
-	0xe2e0004c, 0xe2e0004d, 0xf0000000, 0x17c07c1f, 0xd8206309, 0x17c07c1f,
-	0xe2e0000d, 0xe2e0000c, 0xe2e0001c, 0xe2e0001e, 0xe2e00016, 0xe2e00012,
-	0xf0000000, 0x17c07c1f, 0xd8206489, 0x17c07c1f, 0xe2e00016, 0x1380201f,
-	0xe2e0001e, 0x1380201f, 0xe2e0001c, 0x1380201f, 0xe2e0000c, 0xe2e0000d,
-	0xf0000000, 0x17c07c1f, 0xa1d40407, 0x1391841f, 0xa1d90407, 0x1393041f,
-	0xf0000000, 0x17c07c1f, 0x18d0001f, 0x10006604, 0x10cf8c1f, 0xd8206583,
-	0x17c07c1f, 0xf0000000, 0x17c07c1f, 0xe8208000, 0x11008014, 0x00000002,
-	0xe8208000, 0x11008020, 0x00000101, 0xe8208000, 0x11008004, 0x000000d0,
-	0x1a00001f, 0x11008000, 0xd800684a, 0xe220005d, 0xd820686a, 0xe2200000,
-	0xe2200001, 0xe8208000, 0x11008024, 0x00000001, 0x1b80001f, 0x20000424,
-	0xf0000000, 0x17c07c1f, 0xa1d10407, 0x1b80001f, 0x20000020, 0xf0000000,
-	0x17c07c1f
+	0x0b1600f8, 0x1940001f, 0x00000000, 0x12407c1f, 0x1b00001f, 0xbfffe7ff,
+	0x1b80001f, 0x90100000, 0x17c07c1f, 0xd8004fc5, 0x17c07c1f, 0x8247b001,
+	0x1940001f, 0xffffffff, 0x80c00400, 0xd82050c3, 0xa1d58407, 0xa1dd8407,
+	0x1b00001f, 0x3fffefff, 0xd0004ec0, 0x17c07c1f, 0x1890001f, 0x100063e8,
+	0x88c0000c, 0x2f7be75f, 0xd80052e3, 0x17c07c1f, 0x80c40001, 0xd8005263,
+	0x17c07c1f, 0x1b00001f, 0xbfffe7ff, 0xd00052a0, 0x17c07c1f, 0x1b00001f,
+	0x7ffff7ff, 0xd0004ec0, 0x17c07c1f, 0x80c40001, 0xd82053e3, 0x17c07c1f,
+	0xa1de0407, 0x1b00001f, 0x7fffe7ff, 0xd0004ec0, 0x17c07c1f, 0xe8208000,
+	0x10006814, 0x00000000, 0x18c0001f, 0x10006b00, 0xe0e00000, 0xe0c00009,
+	0x18c0001f, 0x10006294, 0xe0e001fe, 0xe0e003fc, 0xe0e007f8, 0xe0e00ff0,
+	0x1b80001f, 0x20000020, 0xe0f07ff0, 0xe0f07f00, 0x81449801, 0xd80057a5,
+	0x17c07c1f, 0x1a00001f, 0x10006604, 0xe2200002, 0xc0c06840, 0x17c07c1f,
+	0xe2200004, 0xc0c06840, 0x17c07c1f, 0x1b80001f, 0x200016a8, 0x1800001f,
+	0x03800e12, 0x1b80001f, 0x20000300, 0x1800001f, 0x00000e12, 0x1b80001f,
+	0x20000300, 0x1800001f, 0x00000012, 0x1b80001f, 0x20000104, 0x10007c1f,
+	0x81f38407, 0x81f98407, 0x81f90407, 0x81f40407, 0x1b80001f, 0x200016a8,
+	0x81401801, 0xd8005fa5, 0x17c07c1f, 0xe8208000, 0x10006404, 0x00002101,
+	0x18c0001f, 0x10006290, 0x1212841f, 0xc0c061e0, 0x12807c1f, 0xc0c061e0,
+	0x1280041f, 0x18c0001f, 0x10006208, 0x1212841f, 0xc0c061e0, 0x12807c1f,
+	0xe8208000, 0x10006244, 0x00000000, 0x1b80001f, 0x20000080, 0xc0c061e0,
+	0x1280041f, 0xe8208000, 0x10200268, 0x000ffffe, 0x18c0001f, 0x10006204,
+	0x1212841f, 0xc0c065a0, 0x1280041f, 0x18c0001f, 0x10006200, 0x1212841f,
+	0xc0c061e0, 0x12807c1f, 0xe8208000, 0x1000625c, 0x00000000, 0x1b80001f,
+	0x20000080, 0xc0c061e0, 0x1280041f, 0x19c0001f, 0x01411820, 0x1ac0001f,
+	0x55aa55aa, 0x10007c1f, 0xf0000000, 0xd800610a, 0x17c07c1f, 0xe2e0004f,
+	0xe2e0006f, 0xe2e0002f, 0xd82061aa, 0x17c07c1f, 0xe2e0002e, 0xe2e0003e,
+	0xe2e00032, 0xf0000000, 0x17c07c1f, 0xd80062aa, 0x17c07c1f, 0xe2e00036,
+	0xe2e0003e, 0x1380201f, 0xe2e0003c, 0xd82063ca, 0x17c07c1f, 0x1380201f,
+	0xe2e0007c, 0x1b80001f, 0x20000003, 0xe2e0005c, 0xe2e0004c, 0xe2e0004d,
+	0xf0000000, 0x17c07c1f, 0x1a50001f, 0x10006610, 0x8246a401, 0xd8206569,
+	0x17c07c1f, 0xe2e0000d, 0xe2e0000c, 0xe2e0001c, 0xe2e0001e, 0xe2e00016,
+	0xe2e00012, 0xf0000000, 0x17c07c1f, 0x1a50001f, 0x10006610, 0x8246a401,
+	0xd8206749, 0x17c07c1f, 0xe2e00016, 0x1380201f, 0xe2e0001e, 0x1380201f,
+	0xe2e0001c, 0x1380201f, 0xe2e0000c, 0xe2e0000d, 0xf0000000, 0x17c07c1f,
+	0xa1d40407, 0x1391841f, 0xa1d90407, 0x1393041f, 0xf0000000, 0x17c07c1f,
+	0x18d0001f, 0x10006604, 0x10cf8c1f, 0xd8206843, 0x17c07c1f, 0xf0000000,
+	0x17c07c1f, 0xe8208000, 0x11008014, 0x00000002, 0xe8208000, 0x11008020,
+	0x00000101, 0xe8208000, 0x11008004, 0x000000d0, 0x1a00001f, 0x11008000,
+	0xd8006b0a, 0xe220005d, 0xd8206b2a, 0xe2200000, 0xe2200001, 0xe8208000,
+	0x11008024, 0x00000001, 0x1b80001f, 0x20000424, 0xf0000000, 0x17c07c1f,
+	0xa1d10407, 0x1b80001f, 0x20000020, 0xf0000000, 0x17c07c1f
 };
 
 /*
  * PCM binary for suspend scenario
  */
 static const struct pcm_desc suspend_pcm_ca7 = {
-	.version = "pcm_suspend_20150805_V1",
+	.version = "pcm_suspend_20150917_V4",
 	.base = suspend_binary_ca7,
-	.size = 847,
+	.size = 869,
 	.sess = 2,
 	.replace = 0,
 	.vec0 = EVENT_VEC(11, 1, 0, 0),
-	.vec1 = EVENT_VEC(12, 1, 0, 54),
-	.vec2 = EVENT_VEC(30, 1, 0, 143),
-	.vec3 = EVENT_VEC(31, 1, 0, 277),
+	.vec1 = EVENT_VEC(12, 1, 0, 61),
+	.vec2 = EVENT_VEC(30, 1, 0, 150),
+	.vec3 = EVENT_VEC(31, 1, 0, 287),
 };
 
 /*
diff --git a/plat/mediatek/mt8173/include/mcucfg.h b/plat/mediatek/mt8173/include/mcucfg.h
index d2474ac..7c837ba 100644
--- a/plat/mediatek/mt8173/include/mcucfg.h
+++ b/plat/mediatek/mt8173/include/mcucfg.h
@@ -176,4 +176,70 @@
 	MP1_L2RSTDISABLE = 1 << MP1_L2RSTDISABLE_SHIFT
 };
 
+/* cci clock control related */
+enum {
+	MCU_BUS_DCM_EN	= 1 << 8
+};
+
+/* l2c sram control related */
+enum {
+	L2C_SRAM_DCM_EN = 1 << 0
+};
+
+/* bus fabric dcm control related */
+enum {
+	PSYS_ADB400_DCM_EN		= 1 << 29,
+	GPU_ADB400_DCM_EN		= 1 << 28,
+
+	EMI1_ADB400_DCM_EN		= 1 << 27,
+	EMI_ADB400_DCM_EN		= 1 << 26,
+	INFRA_ADB400_DCM_EN		= 1 << 25,
+	L2C_ADB400_DCM_EN		= 1 << 24,
+
+	MP0_ADB400_DCM_EN		= 1 << 23,
+	CCI400_CK_ONLY_DCM_EN		= 1 << 22,
+	L2C_IDLE_DCM_EN			= 1 << 21,
+
+	CA15U_ADB_DYNAMIC_CG_EN		= 1 << 19,
+	CA7L_ADB_DYNAMIC_CG_EN		= 1 << 18,
+	L2C_ADB_DYNAMIC_CG_EN		= 1 << 17,
+
+	EMICLK_EMI1_DYNAMIC_CG_EN	= 1 << 12,
+
+	INFRACLK_PSYS_DYNAMIC_CG_EN	= 1 << 11,
+	EMICLK_GPU_DYNAMIC_CG_EN	= 1 << 10,
+	EMICLK_EMI_DYNAMIC_CG_EN	= 1 << 8,
+
+	CCI400_SLV_RW_DCM_EN		= 1 << 7,
+	CCI400_SLV_DCM_EN		= 1 << 5,
+
+	ACLK_PSYS_DYNAMIC_CG_EN		= 1 << 3,
+	ACLK_GPU_DYNAMIC_CG_EN		= 1 << 2,
+	ACLK_EMI_DYNAMIC_CG_EN		= 1 << 1,
+	ACLK_INFRA_DYNAMIC_CG_EN	= 1 << 0,
+
+	/* adb400 related */
+	ADB400_GRP_DCM_EN = PSYS_ADB400_DCM_EN | GPU_ADB400_DCM_EN |
+			    EMI1_ADB400_DCM_EN | EMI_ADB400_DCM_EN |
+			    INFRA_ADB400_DCM_EN | L2C_ADB400_DCM_EN |
+			    MP0_ADB400_DCM_EN,
+
+	/* cci400 related */
+	CCI400_GRP_DCM_EN = CCI400_CK_ONLY_DCM_EN | CCI400_SLV_RW_DCM_EN |
+			    CCI400_SLV_DCM_EN,
+
+	/* adb clock related */
+	ADBCLK_GRP_DCM_EN = CA15U_ADB_DYNAMIC_CG_EN | CA7L_ADB_DYNAMIC_CG_EN |
+			    L2C_ADB_DYNAMIC_CG_EN,
+
+	/* emi clock related */
+	EMICLK_GRP_DCM_EN = EMICLK_EMI1_DYNAMIC_CG_EN |
+			    EMICLK_GPU_DYNAMIC_CG_EN |
+			    EMICLK_EMI_DYNAMIC_CG_EN,
+
+	/* bus clock related */
+	ACLK_GRP_DCM_EN = ACLK_PSYS_DYNAMIC_CG_EN | ACLK_GPU_DYNAMIC_CG_EN |
+			  ACLK_EMI_DYNAMIC_CG_EN | ACLK_INFRA_DYNAMIC_CG_EN,
+};
+
 #endif  /* __MCUCFG_H__ */
diff --git a/plat/mediatek/mt8173/include/platform_def.h b/plat/mediatek/mt8173/include/platform_def.h
index 7759b3e..0573bc5 100644
--- a/plat/mediatek/mt8173/include/platform_def.h
+++ b/plat/mediatek/mt8173/include/platform_def.h
@@ -31,7 +31,6 @@
 #ifndef __PLATFORM_DEF_H__
 #define __PLATFORM_DEF_H__
 
-#define DEBUG_XLAT_TABLE 0
 
 /*******************************************************************************
  * Platform binary types for linking
@@ -44,9 +43,7 @@
  ******************************************************************************/
 
 /* Size of cacheable stacks */
-#if DEBUG_XLAT_TABLE
-#define PLATFORM_STACK_SIZE 0x800
-#elif IMAGE_BL1
+#if IMAGE_BL1
 #define PLATFORM_STACK_SIZE 0x440
 #elif IMAGE_BL2
 #define PLATFORM_STACK_SIZE 0x400
diff --git a/plat/mediatek/mt8173/plat_pm.c b/plat/mediatek/mt8173/plat_pm.c
index a84d031..f28f8ed 100644
--- a/plat/mediatek/mt8173/plat_pm.c
+++ b/plat/mediatek/mt8173/plat_pm.c
@@ -380,8 +380,8 @@
 
 	mmio_write_32(rv, sec_entrypoint);
 
-	if (afflvl == MPIDR_AFFLVL0)
-		spm_mcdi_prepare(mpidr);
+	if (afflvl < MPIDR_AFFLVL2)
+		spm_mcdi_prepare_for_off_state(mpidr, afflvl);
 
 	if (afflvl >= MPIDR_AFFLVL0)
 		mt_platform_save_context(mpidr);
@@ -390,12 +390,10 @@
 	if (afflvl >= MPIDR_AFFLVL1) {
 		/* Disable coherency if this cluster is to be turned off */
 		plat_cci_disable();
-		disable_scu(mpidr);
-
-		trace_power_flow(mpidr, CLUSTER_SUSPEND);
 	}
 
 	if (afflvl >= MPIDR_AFFLVL2) {
+		disable_scu(mpidr);
 		generic_timer_backup();
 		spm_system_suspend();
 		/* Prevent interrupts from spuriously waking up this cpu */
@@ -420,8 +418,6 @@
 
 	/* Perform the common cluster specific operations */
 	if (afflvl >= MPIDR_AFFLVL1) {
-		enable_scu(mpidr);
-
 		/* Enable coherency if this cluster was off */
 		plat_cci_enable();
 		trace_power_flow(mpidr, CLUSTER_UP);
@@ -451,22 +447,20 @@
 		arm_gic_setup();
 		arm_gic_cpuif_setup();
 		spm_system_suspend_finish();
+		enable_scu(mpidr);
 	}
 
 	/* Perform the common cluster specific operations */
 	if (afflvl >= MPIDR_AFFLVL1) {
-		enable_scu(mpidr);
-
 		/* Enable coherency if this cluster was off */
 		plat_cci_enable();
-		trace_power_flow(mpidr, CLUSTER_UP);
 	}
 
 	if (afflvl >= MPIDR_AFFLVL0)
 		mt_platform_restore_context(mpidr);
 
-	if (afflvl == MPIDR_AFFLVL0)
-		spm_mcdi_finish(mpidr);
+	if (afflvl < MPIDR_AFFLVL2)
+		spm_mcdi_finish_for_on_state(mpidr, afflvl);
 
 	arm_gic_pcpu_distif_setup();
 }
diff --git a/plat/mediatek/mt8173/plat_sip_calls.c b/plat/mediatek/mt8173/plat_sip_calls.c
index 92deb60..e12892f 100644
--- a/plat/mediatek/mt8173/plat_sip_calls.c
+++ b/plat/mediatek/mt8173/plat_sip_calls.c
@@ -29,6 +29,7 @@
  */
 #include <mmio.h>
 #include <mtk_sip_svc.h>
+#include <mtcmos.h>
 
 /* Authorized secure register list */
 enum {
@@ -55,3 +56,30 @@
 
 	return MTK_SIP_E_INVALID_PARAM;
 }
+
+uint64_t mt_sip_pwr_on_mtcmos(uint32_t val)
+{
+	uint32_t ret;
+
+	ret = mtcmos_non_cpu_ctrl(1, val);
+	if (ret)
+		return MTK_SIP_E_INVALID_PARAM;
+	else
+		return MTK_SIP_E_SUCCESS;
+}
+
+uint64_t mt_sip_pwr_off_mtcmos(uint32_t val)
+{
+	uint32_t ret;
+
+	ret = mtcmos_non_cpu_ctrl(0, val);
+	if (ret)
+		return MTK_SIP_E_INVALID_PARAM;
+	else
+		return MTK_SIP_E_SUCCESS;
+}
+
+uint64_t mt_sip_pwr_mtcmos_support(void)
+{
+	return MTK_SIP_E_SUCCESS;
+}
diff --git a/plat/nvidia/tegra/common/aarch64/tegra_helpers.S b/plat/nvidia/tegra/common/aarch64/tegra_helpers.S
index a4caf5e..905c4c5 100644
--- a/plat/nvidia/tegra/common/aarch64/tegra_helpers.S
+++ b/plat/nvidia/tegra/common/aarch64/tegra_helpers.S
@@ -186,7 +186,7 @@
 	 * int plat_crash_console_init(void)
 	 * Function to initialize the crash console
 	 * without a C Runtime to print crash report.
-	 * Clobber list : x0, x1, x2
+	 * Clobber list : x0 - x4
 	 * ---------------------------------------------
 	 */
 func plat_crash_console_init
diff --git a/plat/nvidia/tegra/include/platform_def.h b/plat/nvidia/tegra/include/platform_def.h
index 2a7935f..70ddaa9 100644
--- a/plat/nvidia/tegra/include/platform_def.h
+++ b/plat/nvidia/tegra/include/platform_def.h
@@ -40,9 +40,7 @@
  ******************************************************************************/
 
 /* Size of cacheable stacks */
-#if DEBUG_XLAT_TABLE
-#define PLATFORM_STACK_SIZE 0x800
-#elif IMAGE_BL31
+#if IMAGE_BL31
 #define PLATFORM_STACK_SIZE 0x400
 #endif
 
diff --git a/services/std_svc/psci/psci_common.c b/services/std_svc/psci/psci_common.c
index 7332695..8a2b81c 100644
--- a/services/std_svc/psci/psci_common.c
+++ b/services/std_svc/psci/psci_common.c
@@ -74,7 +74,7 @@
  ******************************************************************************/
 non_cpu_pd_node_t psci_non_cpu_pd_nodes[PSCI_NUM_NON_CPU_PWR_DOMAINS]
 #if USE_COHERENT_MEM
-__attribute__ ((section("tzfw_coherent_mem")))
+__section("tzfw_coherent_mem")
 #endif
 ;
 
@@ -393,6 +393,7 @@
 	unsigned int start_idx, ncpus;
 	plat_local_state_t target_state, *req_states;
 
+	assert(end_pwrlvl <= PLAT_MAX_PWR_LVL);
 	parent_idx = psci_cpu_pd_nodes[cpu_idx].parent_node;
 
 	/* For level 0, the requested state will be equivalent
diff --git a/services/std_svc/psci/psci_off.c b/services/std_svc/psci/psci_off.c
index 9ed6f0c..cef6668 100644
--- a/services/std_svc/psci/psci_off.c
+++ b/services/std_svc/psci/psci_off.c
@@ -129,10 +129,13 @@
 		 * Set the affinity info state to OFF. This writes directly to
 		 * main memory as caches are disabled, so cache maintenance is
 		 * required to ensure that later cached reads of aff_info_state
-		 * return AFF_STATE_OFF.
+		 * return AFF_STATE_OFF.  A dsbish() ensures ordering of the
+		 * update to the affinity info state prior to cache line
+		 * invalidation.
 		 */
 		flush_cpu_data(psci_svc_cpu_data.aff_info_state);
 		psci_set_aff_info_state(AFF_STATE_OFF);
+		dsbish();
 		inv_cpu_data(psci_svc_cpu_data.aff_info_state);
 
 		/*
diff --git a/services/std_svc/psci/psci_on.c b/services/std_svc/psci/psci_on.c
index c37adc2..200e622 100644
--- a/services/std_svc/psci/psci_on.c
+++ b/services/std_svc/psci/psci_on.c
@@ -57,24 +57,6 @@
 }
 
 /*******************************************************************************
- * This function sets the aff_info_state in the per-cpu data of the CPU
- * specified by cpu_idx
- ******************************************************************************/
-static void psci_set_aff_info_state_by_idx(unsigned int cpu_idx,
-					   aff_info_state_t aff_state)
-{
-
-	set_cpu_data_by_index(cpu_idx,
-			      psci_svc_cpu_data.aff_info_state,
-			      aff_state);
-
-	/*
-	 * Flush aff_info_state as it will be accessed with caches turned OFF.
-	 */
-	flush_cpu_data_by_index(cpu_idx, psci_svc_cpu_data.aff_info_state);
-}
-
-/*******************************************************************************
  * Generic handler which is called to physically power on a cpu identified by
  * its mpidr. It performs the generic, architectural, platform setup and state
  * management to power on the target cpu e.g. it will ensure that
@@ -90,6 +72,7 @@
 {
 	int rc;
 	unsigned int target_idx = plat_core_pos_by_mpidr(target_cpu);
+	aff_info_state_t target_aff_state;
 
 	/*
 	 * This function must only be called on platforms where the
@@ -119,8 +102,26 @@
 
 	/*
 	 * Set the Affinity info state of the target cpu to ON_PENDING.
+	 * Flush aff_info_state as it will be accessed with caches
+	 * turned OFF.
 	 */
 	psci_set_aff_info_state_by_idx(target_idx, AFF_STATE_ON_PENDING);
+	flush_cpu_data_by_index(target_idx, psci_svc_cpu_data.aff_info_state);
+
+	/*
+	 * The cache line invalidation by the target CPU after setting the
+	 * state to OFF (see psci_do_cpu_off()), could cause the update to
+	 * aff_info_state to be invalidated. Retry the update if the target
+	 * CPU aff_info_state is not ON_PENDING.
+	 */
+	target_aff_state = psci_get_aff_info_state_by_idx(target_idx);
+	if (target_aff_state != AFF_STATE_ON_PENDING) {
+		assert(target_aff_state == AFF_STATE_OFF);
+		psci_set_aff_info_state_by_idx(target_idx, AFF_STATE_ON_PENDING);
+		flush_cpu_data_by_index(target_idx, psci_svc_cpu_data.aff_info_state);
+
+		assert(psci_get_aff_info_state_by_idx(target_idx) == AFF_STATE_ON_PENDING);
+	}
 
 	/*
 	 * Perform generic, architecture and platform specific handling.
@@ -136,9 +137,11 @@
 	if (rc == PSCI_E_SUCCESS)
 		/* Store the re-entry information for the non-secure world. */
 		cm_init_context_by_index(target_idx, ep);
-	else
+	else {
 		/* Restore the state on error. */
 		psci_set_aff_info_state_by_idx(target_idx, AFF_STATE_OFF);
+		flush_cpu_data_by_index(target_idx, psci_svc_cpu_data.aff_info_state);
+	}
 
 exit:
 	psci_spin_unlock_cpu(target_idx);
diff --git a/services/std_svc/psci/psci_private.h b/services/std_svc/psci/psci_private.h
index 3286cf6..4b91ad5 100644
--- a/services/std_svc/psci/psci_private.h
+++ b/services/std_svc/psci/psci_private.h
@@ -78,6 +78,9 @@
 		get_cpu_data(psci_svc_cpu_data.aff_info_state)
 #define psci_get_aff_info_state_by_idx(idx) \
 		get_cpu_data_by_index(idx, psci_svc_cpu_data.aff_info_state)
+#define psci_set_aff_info_state_by_idx(idx, aff_state) \
+		set_cpu_data_by_index(idx, psci_svc_cpu_data.aff_info_state,\
+					aff_state)
 #define psci_get_suspend_pwrlvl() \
 		get_cpu_data(psci_svc_cpu_data.target_pwrlvl)
 #define psci_set_suspend_pwrlvl(target_lvl) \