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 d0cb399..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
 
@@ -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 89d8251..004f70d 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
@@ -1872,7 +1882,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
@@ -1881,7 +1891,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.
 
diff --git a/docs/user-guide.md b/docs/user-guide.md
index 894d69b..70e1abc 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:
@@ -471,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:
@@ -482,6 +505,11 @@
     Trusted Firmware must be compiled with GICv2 only driver using
     `FVP_USE_GIC_DRIVER=FVP_GICV2` build option.
 
+*   `FVP_CLUSTER_COUNT`    : Configures the cluster count to be used to
+     build the topology tree within Trusted Firmware. By default the
+     Trusted Firmware is configured for dual cluster topology and this option
+     can be used to override the default value.
+
 ### Creating a Firmware Image Package
 
 FIPs are automatically created as part of the build instructions described in
@@ -859,6 +887,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
 --------------------------------------
@@ -919,14 +1025,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.
@@ -1211,38 +1318,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
 ---------------------------------
@@ -1344,7 +1419,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/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/lib/aarch64/arch_helpers.h b/include/lib/aarch64/arch_helpers.h
index 067b830..d1ad31d 100644
--- a/include/lib/aarch64/arch_helpers.h
+++ b/include/lib/aarch64/arch_helpers.h
@@ -194,6 +194,8 @@
 DEFINE_SYSOP_FUNC(sev)
 DEFINE_SYSOP_TYPE_FUNC(dsb, sy)
 DEFINE_SYSOP_TYPE_FUNC(dmb, sy)
+DEFINE_SYSOP_TYPE_FUNC(dmb, st)
+DEFINE_SYSOP_TYPE_FUNC(dmb, ld)
 DEFINE_SYSOP_TYPE_FUNC(dsb, ish)
 DEFINE_SYSOP_TYPE_FUNC(dmb, ish)
 DEFINE_SYSOP_FUNC(isb)
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 b2db616..dab7545 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:
@@ -44,7 +44,6 @@
 /* Special value used to verify platform parameters from BL2 to BL31 */
 #define ARM_BL31_PLAT_PARAM_VAL		0x0f1e2d3c4b5a6978ULL
 
-#define ARM_CLUSTER_COUNT		2
 #define ARM_SYSTEM_COUNT		1
 
 #define ARM_CACHE_WRITEBACK_SHIFT	6
@@ -151,14 +150,10 @@
 #define ARM_G0_IRQS			ARM_IRQ_SEC_SGI_0,		\
 					ARM_IRQ_SEC_SGI_6
 
-#define ARM_SHARED_RAM_ATTR		((PLAT_ARM_SHARED_RAM_CACHED ?	\
-						MT_MEMORY : MT_DEVICE)	\
-						| MT_RW | MT_SECURE)
-
 #define ARM_MAP_SHARED_RAM		MAP_REGION_FLAT(		\
 						ARM_SHARED_RAM_BASE,	\
 						ARM_SHARED_RAM_SIZE,	\
-						ARM_SHARED_RAM_ATTR)
+						MT_DEVICE | MT_RW | MT_SECURE)
 
 #define ARM_MAP_NS_DRAM1		MAP_REGION_FLAT(		\
 						ARM_NS_DRAM1_BASE,	\
@@ -218,10 +213,6 @@
  */
 #define PLAT_MAX_OFF_STATE		ARM_LOCAL_STATE_OFF
 
-
-#define PLATFORM_CORE_COUNT		(PLAT_ARM_CLUSTER0_CORE_COUNT + \
-					 PLAT_ARM_CLUSTER1_CORE_COUNT)
-
 /*
  * Some data must be aligned on the biggest cache line size in the platform.
  * This is known only to the platform as it might have a combination of
@@ -239,44 +230,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..e9eebaa 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);
 
@@ -187,6 +179,7 @@
 /*
  * Mandatory functions required in ARM standard platforms
  */
+unsigned int plat_arm_get_cluster_core_count(u_register_t mpidr);
 void plat_arm_gic_driver_init(void);
 void plat_arm_gic_init(void);
 void plat_arm_gic_cpuif_enable(void);
@@ -194,6 +187,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 +200,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 c900278..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:
@@ -37,15 +37,12 @@
 /*************************************************************************
  * Definitions common to all ARM Compute SubSystems (CSS)
  *************************************************************************/
-#define MHU_PAYLOAD_CACHED		0
-
 #define NSROM_BASE			0x1f000000
 #define NSROM_SIZE			0x00001000
 
 /* 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
@@ -78,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
@@ -110,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.
@@ -117,18 +137,13 @@
 #define SCP_BL2_BASE			BL31_BASE
 
 #define SCP_BL2U_BASE			BL31_BASE
-
-#define PLAT_ARM_SHARED_RAM_CACHED	MHU_PAYLOAD_CACHED
+#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/lib/aarch64/xlat_tables.c b/lib/aarch64/xlat_tables.c
index b00dc0b..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
@@ -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_def.h b/plat/arm/board/fvp/fvp_def.h
index 41b872a..dbca280 100644
--- a/plat/arm/board/fvp/fvp_def.h
+++ b/plat/arm/board/fvp/fvp_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:
@@ -33,7 +33,9 @@
 
 #include <arm_def.h>
 
-
+#ifndef FVP_CLUSTER_COUNT
+#define FVP_CLUSTER_COUNT		2
+#endif
 #define FVP_MAX_CPUS_PER_CLUSTER	4
 
 #define FVP_PRIMARY_CPU			0x0
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/fvp_topology.c b/plat/arm/board/fvp/fvp_topology.c
index a212eda..741aad6 100644
--- a/plat/arm/board/fvp/fvp_topology.c
+++ b/plat/arm/board/fvp/fvp_topology.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:
@@ -29,27 +29,47 @@
  */
 
 #include <arch.h>
+#include <cassert.h>
 #include <plat_arm.h>
 #include <platform_def.h>
 #include "drivers/pwrc/fvp_pwrc.h"
 
-/*
- * The FVP power domain tree does not have a single system level power domain
- * i.e. a single root node. The first entry in the power domain descriptor
- * specifies the number of power domains at the highest power level. For the FVP
- * this is 2 i.e. the number of cluster power domains.
- */
-#define FVP_PWR_DOMAINS_AT_MAX_PWR_LVL	ARM_CLUSTER_COUNT
-
 /* The FVP power domain tree descriptor */
-const unsigned char arm_power_domain_tree_desc[] = {
-	/* No of root nodes */
-	FVP_PWR_DOMAINS_AT_MAX_PWR_LVL,
-	/* No of children for the first node */
-	PLAT_ARM_CLUSTER0_CORE_COUNT,
-	/* No of children for the second node */
-	PLAT_ARM_CLUSTER1_CORE_COUNT
-};
+unsigned char fvp_power_domain_tree_desc[FVP_CLUSTER_COUNT + 1];
+
+
+CASSERT(FVP_CLUSTER_COUNT && FVP_CLUSTER_COUNT <= 256, assert_invalid_fvp_cluster_count);
+
+/*******************************************************************************
+ * This function dynamically constructs the topology according to
+ * FVP_CLUSTER_COUNT and returns it.
+ ******************************************************************************/
+const unsigned char *plat_get_power_domain_tree_desc(void)
+{
+	int i;
+
+	/*
+	 * The FVP power domain tree does not have a single system level power domain
+	 * i.e. a single root node. The first entry in the power domain descriptor
+	 * specifies the number of power domains at the highest power level. For the FVP
+	 * this is the number of cluster power domains.
+	 */
+	fvp_power_domain_tree_desc[0] = FVP_CLUSTER_COUNT;
+
+	for (i = 0; i < FVP_CLUSTER_COUNT; i++)
+		fvp_power_domain_tree_desc[i + 1] = FVP_MAX_CPUS_PER_CLUSTER;
+
+	return fvp_power_domain_tree_desc;
+}
+
+/*******************************************************************************
+ * This function returns the core count within the cluster corresponding to
+ * `mpidr`.
+ ******************************************************************************/
+unsigned int plat_arm_get_cluster_core_count(u_register_t mpidr)
+{
+	return FVP_MAX_CPUS_PER_CLUSTER;
+}
 
 /*******************************************************************************
  * This function implements a part of the critical interface between the psci
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 0d671dc..a8267de 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:
@@ -39,9 +39,10 @@
 #include "../fvp_def.h"
 
 /* Required platform porting definitions */
-#define PLAT_NUM_PWR_DOMAINS		(ARM_CLUSTER_COUNT + \
+#define PLAT_NUM_PWR_DOMAINS		(FVP_CLUSTER_COUNT + \
 					PLATFORM_CORE_COUNT)
 #define PLAT_MAX_PWR_LVL		ARM_PWR_LVL1
+#define PLATFORM_CORE_COUNT		(FVP_CLUSTER_COUNT * FVP_MAX_CPUS_PER_CLUSTER)
 
 /*
  * Other platform porting definitions are provided by included headers
@@ -50,8 +51,7 @@
 /*
  * Required ARM standard platform porting definitions
  */
-#define PLAT_ARM_CLUSTER0_CORE_COUNT	4
-#define PLAT_ARM_CLUSTER1_CORE_COUNT	4
+#define PLAT_ARM_CLUSTER_COUNT		FVP_CLUSTER_COUNT
 
 #define PLAT_ARM_TRUSTED_ROM_BASE	0x00000000
 #define PLAT_ARM_TRUSTED_ROM_SIZE	0x04000000	/* 64 MB */
@@ -64,8 +64,6 @@
 
 #define PLAT_ARM_DRAM2_SIZE		MAKE_ULL(0x780000000)
 
-#define PLAT_ARM_SHARED_RAM_CACHED	1
-
 /*
  * Load address of BL33 for this platform port
  */
@@ -144,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 ed4c898..aad2e2e 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -34,6 +34,11 @@
 # The FVP platform depends on this macro to build with correct GIC driver.
 $(eval $(call add_define,FVP_USE_GIC_DRIVER))
 
+# If FVP_CLUSTER_COUNT has been defined, pass it into the build system.
+ifdef FVP_CLUSTER_COUNT
+$(eval $(call add_define,FVP_CLUSTER_COUNT))
+endif
+
 # Choose the GIC sources depending upon the how the FVP will be invoked
 ifeq (${FVP_USE_GIC_DRIVER}, FVP_GICV3)
 FVP_GIC_SOURCES		:=	drivers/arm/gic/common/gic_common.c	\
@@ -57,6 +62,14 @@
 $(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
 
 
@@ -69,13 +82,15 @@
 				lib/cpus/aarch64/cortex_a72.S
 
 BL1_SOURCES		+=	drivers/io/io_semihosting.c			\
-				${FVP_CPU_LIBS}					\
 				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			\
@@ -85,19 +100,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		+=	${FVP_CPU_LIBS}					\
-				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..e1e0af6 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:
@@ -41,11 +41,15 @@
 #include <v2m_def.h>
 #include "../juno_def.h"
 
+/* Required platform porting definitions */
 /* Juno supports system power domain */
 #define PLAT_MAX_PWR_LVL		ARM_PWR_LVL2
 #define PLAT_NUM_PWR_DOMAINS		(ARM_SYSTEM_COUNT + \
-					ARM_CLUSTER_COUNT + \
+					JUNO_CLUSTER_COUNT + \
 					PLATFORM_CORE_COUNT)
+#define PLATFORM_CORE_COUNT		(JUNO_CLUSTER0_CORE_COUNT + \
+					JUNO_CLUSTER1_CORE_COUNT)
+
 /*
  * Other platform porting definitions are provided by included headers
  */
@@ -53,8 +57,7 @@
 /*
  * Required ARM standard platform porting definitions
  */
-#define PLAT_ARM_CLUSTER0_CORE_COUNT	2
-#define PLAT_ARM_CLUSTER1_CORE_COUNT	4
+#define PLAT_ARM_CLUSTER_COUNT		JUNO_CLUSTER_COUNT
 
 /* Use the bypass address */
 #define PLAT_ARM_TRUSTED_ROM_BASE	V2M_FLASH0_BASE + BL1_ROM_BYPASS_OFFSET
@@ -76,7 +79,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 +106,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 +148,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..f27bbb2 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,14 @@
 #define PSRAM_BASE			0x14000000
 #define PSRAM_SIZE			0x02000000
 
+#define JUNO_SSC_VER_PART_NUM		0x030
+
+/*******************************************************************************
+ * Juno topology related constants
+ ******************************************************************************/
+#define JUNO_CLUSTER_COUNT		2
+#define JUNO_CLUSTER0_CORE_COUNT	2
+#define JUNO_CLUSTER1_CORE_COUNT	4
 
 /*******************************************************************************
  * TZC-400 related constants
diff --git a/plat/arm/board/juno/juno_topology.c b/plat/arm/board/juno/juno_topology.c
new file mode 100644
index 0000000..ee4ec44
--- /dev/null
+++ b/plat/arm/board/juno/juno_topology.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 <arm_def.h>
+#include <plat_arm.h>
+#include "juno_def.h"
+
+/*
+ * On Juno, the system power level is the highest power level.
+ * The first entry in the power domain descriptor specifies the
+ * number of system power domains i.e. 1.
+ */
+#define JUNO_PWR_DOMAINS_AT_MAX_PWR_LVL	 ARM_SYSTEM_COUNT
+
+/*
+ * The Juno power domain tree descriptor. The cluster power domains
+ * are arranged so that when the PSCI generic code creates the power
+ * domain tree, the indices of the CPU power domain nodes it allocates
+ * match the linear indices returned by plat_core_pos_by_mpidr()
+ * i.e. CLUSTER1 CPUs are allocated indices from 0 to 3 and the higher
+ * indices for CLUSTER0 CPUs.
+ */
+const unsigned char juno_power_domain_tree_desc[] = {
+	/* No of root nodes */
+	JUNO_PWR_DOMAINS_AT_MAX_PWR_LVL,
+	/* No of children for the root node */
+	JUNO_CLUSTER_COUNT,
+	/* No of children for the first cluster node */
+	JUNO_CLUSTER1_CORE_COUNT,
+	/* No of children for the second cluster node */
+	JUNO_CLUSTER0_CORE_COUNT
+};
+
+/*******************************************************************************
+ * This function returns the Juno topology tree information.
+ ******************************************************************************/
+const unsigned char *plat_get_power_domain_tree_desc(void)
+{
+	return juno_power_domain_tree_desc;
+}
+
+/*******************************************************************************
+ * This function returns the core count within the cluster corresponding to
+ * `mpidr`.
+ ******************************************************************************/
+unsigned int plat_arm_get_cluster_core_count(u_register_t mpidr)
+{
+	return (((mpidr) & 0x100) ? JUNO_CLUSTER1_CORE_COUNT :\
+				JUNO_CLUSTER0_CORE_COUNT);
+}
diff --git a/plat/arm/board/juno/platform.mk b/plat/arm/board/juno/platform.mk
index fae30e7..1876821 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,22 @@
 				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}
+				plat/arm/board/juno/juno_topology.c	\
+				${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/board/juno/tsp/tsp-juno.mk b/plat/arm/board/juno/tsp/tsp-juno.mk
index 2ef964e..4e80607 100644
--- a/plat/arm/board/juno/tsp/tsp-juno.mk
+++ b/plat/arm/board/juno/tsp/tsp-juno.mk
@@ -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:
@@ -28,7 +28,8 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 
-BL32_SOURCES		+=	plat/arm/css/common/css_topology.c	\
+BL32_SOURCES		+=	plat/arm/board/juno/juno_topology.c	\
+				plat/arm/css/common/css_topology.c	\
 				${JUNO_GIC_SOURCES}
 
 include plat/arm/common/tsp/arm_tsp.mk
diff --git a/plat/arm/common/aarch64/arm_common.c b/plat/arm/common/aarch64/arm_common.c
index a211f16..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.
  ******************************************************************************/
@@ -161,3 +151,11 @@
 	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 3202735..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:
@@ -87,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				\
@@ -102,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_pm.c b/plat/arm/common/arm_pm.c
index 2ddc583..1e756a9 100644
--- a/plat/arm/common/arm_pm.c
+++ b/plat/arm/common/arm_pm.c
@@ -192,11 +192,6 @@
 	assert((PLAT_ARM_TRUSTED_MAILBOX_BASE >= ARM_SHARED_RAM_BASE) &&
 		((PLAT_ARM_TRUSTED_MAILBOX_BASE + sizeof(*mailbox)) <= \
 				(ARM_SHARED_RAM_BASE + ARM_SHARED_RAM_SIZE)));
-
-	/* Flush data cache if the mail box shared RAM is cached */
-#if PLAT_ARM_SHARED_RAM_CACHED
-	flush_dcache_range((uintptr_t) mailbox, sizeof(*mailbox));
-#endif
 }
 
 /*******************************************************************************
diff --git a/plat/arm/common/arm_topology.c b/plat/arm/common/arm_topology.c
index cb0bb9c..4430b13 100644
--- a/plat/arm/common/arm_topology.c
+++ b/plat/arm/common/arm_topology.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,26 +29,9 @@
  */
 
 #include <arch.h>
-#include <psci.h>
 #include <plat_arm.h>
 #include <platform_def.h>
 
-#define get_arm_cluster_core_count(mpidr)\
-		(((mpidr) & 0x100) ? PLAT_ARM_CLUSTER1_CORE_COUNT :\
-				PLAT_ARM_CLUSTER0_CORE_COUNT)
-
-/* The power domain tree descriptor which need to be exported by ARM platforms */
-extern const unsigned char arm_power_domain_tree_desc[];
-
-
-/*******************************************************************************
- * This function returns the ARM default topology tree information.
- ******************************************************************************/
-const unsigned char *plat_get_power_domain_tree_desc(void)
-{
-	return arm_power_domain_tree_desc;
-}
-
 /*******************************************************************************
  * This function validates an MPIDR by checking whether it falls within the
  * acceptable bounds. An error code (-1) is returned if an incorrect mpidr
@@ -66,12 +49,12 @@
 	cluster_id = (mpidr >> MPIDR_AFF1_SHIFT) & MPIDR_AFFLVL_MASK;
 	cpu_id = (mpidr >> MPIDR_AFF0_SHIFT) & MPIDR_AFFLVL_MASK;
 
-	if (cluster_id >= ARM_CLUSTER_COUNT)
+	if (cluster_id >= PLAT_ARM_CLUSTER_COUNT)
 		return -1;
 
 	/* Validate cpu_id by checking whether it represents a CPU in
 	   one of the two clusters present on the platform. */
-	if (cpu_id >= get_arm_cluster_core_count(mpidr))
+	if (cpu_id >= plat_arm_get_cluster_core_count(mpidr))
 		return -1;
 
 	return 0;
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 c01f42f..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)))
 
@@ -77,10 +77,10 @@
 
 static void scp_boot_message_send(size_t payload_size)
 {
-	/* Make sure payload can be seen by SCP */
-	if (MHU_PAYLOAD_CACHED)
-		flush_dcache_range(BOM_SHARED_MEM,
-				   sizeof(bom_cmd_t) + payload_size);
+	/* Ensure that any write to the BOM payload area is seen by SCP before
+	 * we write to the MHU register. If these 2 writes were reordered by
+	 * the CPU then SCP would read stale payload data */
+	dmbst();
 
 	/* Send command to SCP */
 	mhu_secure_message_send(BOM_MHU_SLOT_ID);
@@ -99,9 +99,10 @@
 		panic();
 	}
 
-	/* Make sure we see the reply from the SCP and not any stale data */
-	if (MHU_PAYLOAD_CACHED)
-		inv_dcache_range(BOM_SHARED_MEM, size);
+	/* Ensure that any read to the BOM payload area is done after reading
+	 * the MHU register. If these 2 reads were reordered then the CPU would
+	 * read invalid payload data */
+	dmbld();
 
 	return *(uint32_t *) BOM_SHARED_MEM;
 }
diff --git a/plat/arm/css/common/css_scpi.c b/plat/arm/css/common/css_scpi.c
index 0a4eafe..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)
@@ -55,10 +56,10 @@
 
 static void scpi_secure_message_send(size_t payload_size)
 {
-	/* Make sure payload can be seen by SCP */
-	if (MHU_PAYLOAD_CACHED)
-		flush_dcache_range(SCPI_SHARED_MEM_AP_TO_SCP,
-				   sizeof(scpi_cmd_t) + payload_size);
+	/* Ensure that any write to the SCPI payload area is seen by SCP before
+	 * we write to the MHU register. If these 2 writes were reordered by
+	 * the CPU then SCP would read stale payload data */
+	dmbst();
 
 	mhu_secure_message_send(SCPI_MHU_SLOT_ID);
 }
@@ -78,9 +79,10 @@
 		panic();
 	}
 
-	/* Make sure we don't read stale data */
-	if (MHU_PAYLOAD_CACHED)
-		inv_dcache_range(SCPI_SHARED_MEM_SCP_TO_AP, sizeof(*cmd));
+	/* Ensure that any read to the SCPI payload area is done after reading
+	 * the MHU register. If these 2 reads were reordered then the CPU would
+	 * read invalid payload data */
+	dmbld();
 
 	memcpy(cmd, (void *) SCPI_SHARED_MEM_SCP_TO_AP, sizeof(*cmd));
 }
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/arm/css/common/css_topology.c b/plat/arm/css/common/css_topology.c
index 03f81e6..d5f0275 100644
--- a/plat/arm/css/common/css_topology.c
+++ b/plat/arm/css/common/css_topology.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:
@@ -30,33 +30,6 @@
 
 #include <plat_arm.h>
 
-/*
- * On ARM CSS platforms, by default, the system power level is treated as the
- * highest. The first entry in the power domain descriptor specifies the
- * number of system power domains i.e. 1.
- */
-#define CSS_PWR_DOMAINS_AT_MAX_PWR_LVL	ARM_SYSTEM_COUNT
-
-/*
- * The CSS power domain tree descriptor for dual cluster CSS platforms.
- * The cluster power domains are arranged so that when the PSCI generic
- * code creates the power domain tree, the indices of the CPU power
- * domain nodes it allocates match the linear indices returned by
- * plat_core_pos_by_mpidr() i.e. CLUSTER1 CPUs are allocated indices
- * from 0 to 3 and the higher indices for CLUSTER0 CPUs.
- */
-const unsigned char arm_power_domain_tree_desc[] = {
-	/* No of root nodes */
-	CSS_PWR_DOMAINS_AT_MAX_PWR_LVL,
-	/* No of children for the root node */
-	ARM_CLUSTER_COUNT,
-	/* No of children for the first cluster node */
-	PLAT_ARM_CLUSTER1_CORE_COUNT,
-	/* No of children for the second cluster node */
-	PLAT_ARM_CLUSTER0_CORE_COUNT
-};
-
-
 /******************************************************************************
  * This function implements a part of the critical interface between the psci
  * generic layer and the platform that allows the former to query the platform
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/gpio/gpio.c b/plat/mediatek/mt8173/drivers/gpio/gpio.c
index 20473b9..f19c931 100644
--- a/plat/mediatek/mt8173/drivers/gpio/gpio.c
+++ b/plat/mediatek/mt8173/drivers/gpio/gpio.c
@@ -28,10 +28,10 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 #include <debug.h>
-#include <gpio.h>
 #include <mmio.h>
 #include <mt8173_def.h>
 #include <pmic_wrap_init.h>
+#include "gpio.h"
 
 enum {
 	MAX_GPIO_REG_BITS = 16,
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 2801925..45defd8 100644
--- a/plat/mediatek/mt8173/drivers/spm/spm.c
+++ b/plat/mediatek/mt8173/drivers/spm/spm.c
@@ -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..be33c91 100644
--- a/plat/mediatek/mt8173/plat_pm.c
+++ b/plat/mediatek/mt8173/plat_pm.c
@@ -36,7 +36,6 @@
 #include <console.h>
 #include <debug.h>
 #include <errno.h>
-#include <gpio.h>
 #include <mcucfg.h>
 #include <mmio.h>
 #include <mt8173_def.h>
@@ -49,6 +48,7 @@
 #include <spm_hotplug.h>
 #include <spm_mcdi.h>
 #include <spm_suspend.h>
+#include "drivers/gpio/gpio.h"
 
 struct core_context {
 	unsigned long timer_data[8];
@@ -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 465c5fd..8a2b81c 100644
--- a/services/std_svc/psci/psci_common.c
+++ b/services/std_svc/psci/psci_common.c
@@ -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) \