diff --git a/common/bl_common.c b/common/bl_common.c
index 193e972..15d5bde 100644
--- a/common/bl_common.c
+++ b/common/bl_common.c
@@ -315,14 +315,9 @@
 	return io_result;
 }
 
-/*******************************************************************************
- * Generic function to load and authenticate an image. The image is actually
- * loaded by calling the 'load_image()' function. Therefore, it returns the
- * same error codes if the loading operation failed, or -EAUTH if the
- * authentication failed. In addition, this function uses recursion to
- * authenticate the parent images up to the root of trust.
- ******************************************************************************/
-int load_auth_image(unsigned int image_id, image_info_t *image_data)
+static int load_auth_image_internal(unsigned int image_id,
+				    image_info_t *image_data,
+				    int is_parent_image)
 {
 	int rc;
 
@@ -332,7 +327,7 @@
 	/* Use recursion to authenticate parent images */
 	rc = auth_mod_get_parent_id(image_id, &parent_id);
 	if (rc == 0) {
-		rc = load_auth_image(parent_id, image_data);
+		rc = load_auth_image_internal(parent_id, image_data, 1);
 		if (rc != 0) {
 			return rc;
 		}
@@ -351,6 +346,7 @@
 				 (void *)image_data->image_base,
 				 image_data->image_size);
 	if (rc != 0) {
+		/* Authentication error, zero memory and flush it right away. */
 		memset((void *)image_data->image_base, 0x00,
 		       image_data->image_size);
 		flush_dcache_range(image_data->image_base,
@@ -362,13 +358,29 @@
 	 * File has been successfully loaded and authenticated.
 	 * Flush the image to main memory so that it can be executed later by
 	 * any CPU, regardless of cache and MMU state.
+	 * Do it only for child images, not for the parents (certificates).
 	 */
-	flush_dcache_range(image_data->image_base, image_data->image_size);
+	if (!is_parent_image) {
+		flush_dcache_range(image_data->image_base,
+				   image_data->image_size);
+	}
 #endif /* TRUSTED_BOARD_BOOT */
 
 	return 0;
 }
 
+/*******************************************************************************
+ * Generic function to load and authenticate an image. The image is actually
+ * loaded by calling the 'load_image()' function. Therefore, it returns the
+ * same error codes if the loading operation failed, or -EAUTH if the
+ * authentication failed. In addition, this function uses recursion to
+ * authenticate the parent images up to the root of trust.
+ ******************************************************************************/
+int load_auth_image(unsigned int image_id, image_info_t *image_data)
+{
+	return load_auth_image_internal(image_id, image_data, 0);
+}
+
 #else /* LOAD_IMAGE_V2 */
 
 /*******************************************************************************
@@ -494,18 +506,12 @@
 	return io_result;
 }
 
-/*******************************************************************************
- * Generic function to load and authenticate an image. The image is actually
- * loaded by calling the 'load_image()' function. Therefore, it returns the
- * same error codes if the loading operation failed, or -EAUTH if the
- * authentication failed. In addition, this function uses recursion to
- * authenticate the parent images up to the root of trust.
- ******************************************************************************/
-int load_auth_image(meminfo_t *mem_layout,
-		    unsigned int image_id,
-		    uintptr_t image_base,
-		    image_info_t *image_data,
-		    entry_point_info_t *entry_point_info)
+static int load_auth_image_internal(meminfo_t *mem_layout,
+				    unsigned int image_id,
+				    uintptr_t image_base,
+				    image_info_t *image_data,
+				    entry_point_info_t *entry_point_info,
+				    int is_parent_image)
 {
 	int rc;
 
@@ -515,8 +521,8 @@
 	/* Use recursion to authenticate parent images */
 	rc = auth_mod_get_parent_id(image_id, &parent_id);
 	if (rc == 0) {
-		rc = load_auth_image(mem_layout, parent_id, image_base,
-				     image_data, NULL);
+		rc = load_auth_image_internal(mem_layout, parent_id, image_base,
+				     image_data, NULL, 1);
 		if (rc != 0) {
 			return rc;
 		}
@@ -536,6 +542,7 @@
 				 (void *)image_data->image_base,
 				 image_data->image_size);
 	if (rc != 0) {
+		/* Authentication error, zero memory and flush it right away. */
 		memset((void *)image_data->image_base, 0x00,
 		       image_data->image_size);
 		flush_dcache_range(image_data->image_base,
@@ -546,13 +553,34 @@
 	 * File has been successfully loaded and authenticated.
 	 * Flush the image to main memory so that it can be executed later by
 	 * any CPU, regardless of cache and MMU state.
+	 * Do it only for child images, not for the parents (certificates).
 	 */
-	flush_dcache_range(image_data->image_base, image_data->image_size);
+	if (!is_parent_image) {
+		flush_dcache_range(image_data->image_base,
+				   image_data->image_size);
+	}
 #endif /* TRUSTED_BOARD_BOOT */
 
 	return 0;
 }
 
+/*******************************************************************************
+ * Generic function to load and authenticate an image. The image is actually
+ * loaded by calling the 'load_image()' function. Therefore, it returns the
+ * same error codes if the loading operation failed, or -EAUTH if the
+ * authentication failed. In addition, this function uses recursion to
+ * authenticate the parent images up to the root of trust.
+ ******************************************************************************/
+int load_auth_image(meminfo_t *mem_layout,
+		    unsigned int image_id,
+		    uintptr_t image_base,
+		    image_info_t *image_data,
+		    entry_point_info_t *entry_point_info)
+{
+	return load_auth_image_internal(mem_layout, image_id, image_base,
+					image_data, entry_point_info, 0);
+}
+
 #endif /* LOAD_IMAGE_V2 */
 
 /*******************************************************************************
diff --git a/drivers/arm/pl011/aarch32/pl011_console.S b/drivers/arm/pl011/aarch32/pl011_console.S
index 21ed7ab..5b73528 100644
--- a/drivers/arm/pl011/aarch32/pl011_console.S
+++ b/drivers/arm/pl011/aarch32/pl011_console.S
@@ -118,15 +118,15 @@
 1:
 	/* Check if the transmit FIFO is full */
 	ldr	r2, [r1, #UARTFR]
-	tst	r2, #PL011_UARTFR_TXFF_BIT
-	beq	1b
+	tst	r2, #PL011_UARTFR_TXFF
+	bne	1b
 	mov	r2, #0xD
 	str	r2, [r1, #UARTDR]
 2:
 	/* Check if the transmit FIFO is full */
 	ldr	r2, [r1, #UARTFR]
-	tst	r2, #PL011_UARTFR_TXFF_BIT
-	beq	2b
+	tst	r2, #PL011_UARTFR_TXFF
+	bne	2b
 	str	r0, [r1, #UARTDR]
 	bx	lr
 putc_error:
@@ -149,8 +149,8 @@
 1:
 	/* Check if the receive FIFO is empty */
 	ldr	r1, [r0, #UARTFR]
-	tst	r1, #PL011_UARTFR_RXFE_BIT
-	beq	1b
+	tst	r1, #PL011_UARTFR_RXFE
+	bne	1b
 	ldr	r1, [r0, #UARTDR]
 	mov	r0, r1
 	bx	lr
diff --git a/include/common/aarch32/el3_common_macros.S b/include/common/aarch32/el3_common_macros.S
index 228d721..dcb4edd 100644
--- a/include/common/aarch32/el3_common_macros.S
+++ b/include/common/aarch32/el3_common_macros.S
@@ -67,14 +67,6 @@
 	orr	r0, r0, #SCR_SIF_BIT
 	stcopr	r0, SCR
 
-	/* -----------------------------------------------------------------
-	 * Reset those registers that may have architecturally unknown reset
-	 * values
-	 * -----------------------------------------------------------------
-	 */
-	mov	r0, #0
-	stcopr	r0, SDCR
-
 	/* -----------------------------------------------------
 	 * Enable the Asynchronous data abort now that the
 	 * exception vectors have been setup.
diff --git a/include/drivers/emmc.h b/include/drivers/emmc.h
index 5f78dce..4bd7328 100644
--- a/include/drivers/emmc.h
+++ b/include/drivers/emmc.h
@@ -131,21 +131,21 @@
 } emmc_ops_t;
 
 typedef struct emmc_csd {
-	unsigned char		not_used:		1;
-	unsigned char		crc:			7;
-	unsigned char		ecc:			2;
-	unsigned char		file_format:		2;
-	unsigned char		tmp_write_protect:	1;
-	unsigned char		perm_write_protect:	1;
-	unsigned char		copy:			1;
-	unsigned char		file_format_grp:	1;
+	unsigned int 	    not_used:		1;
+	unsigned int   	    crc:			7;
+	unsigned int   	    ecc:			2;
+	unsigned int   	    file_format:		2;
+	unsigned int   	    tmp_write_protect:	1;
+	unsigned int   	    perm_write_protect:	1;
+	unsigned int   	    copy:			1;
+	unsigned int   	    file_format_grp:	1;
 
-	unsigned short		reserved_1:		5;
-	unsigned short		write_bl_partial:	1;
-	unsigned short		write_bl_len:		4;
-	unsigned short		r2w_factor:		3;
-	unsigned short		default_ecc:		2;
-	unsigned short		wp_grp_enable:		1;
+	unsigned int 		reserved_1:		5;
+	unsigned int 		write_bl_partial:	1;
+	unsigned int 		write_bl_len:		4;
+	unsigned int 		r2w_factor:		3;
+	unsigned int 		default_ecc:		2;
+	unsigned int 		wp_grp_enable:		1;
 
 	unsigned int		wp_grp_size:		5;
 	unsigned int		erase_grp_mult:		5;
diff --git a/include/lib/aarch32/arch.h b/include/lib/aarch32/arch.h
index 3c5ab26..170fa84 100644
--- a/include/lib/aarch32/arch.h
+++ b/include/lib/aarch32/arch.h
@@ -382,8 +382,8 @@
 
 /* Debug register defines. The format is: coproc, opt1, CRn, CRm, opt2 */
 #define HDCR		p15, 4, c1, c1, 1
-#define SDCR		p15, 0, c1, c3, 1
 #define PMCR		p15, 0, c9, c12, 0
+#define CNTHP_CTL	p15, 4, c14, c2, 1
 
 /* GICv3 CPU Interface system register defines. The format is: coproc, opt1, CRn, CRm, opt2 */
 #define ICC_IAR1	p15, 0, c12, c12, 0
diff --git a/include/lib/aarch32/arch_helpers.h b/include/lib/aarch32/arch_helpers.h
index 0633bca..7955d62 100644
--- a/include/lib/aarch32/arch_helpers.h
+++ b/include/lib/aarch32/arch_helpers.h
@@ -250,6 +250,7 @@
 DEFINE_COPROCR_RW_FUNCS(icc_eoir1_el1, ICC_EOIR1)
 
 DEFINE_COPROCR_RW_FUNCS(hdcr, HDCR)
+DEFINE_COPROCR_RW_FUNCS(cnthp_ctl, CNTHP_CTL)
 DEFINE_COPROCR_READ_FUNC(pmcr, PMCR)
 
 /*
diff --git a/include/lib/aarch32/smcc_macros.S b/include/lib/aarch32/smcc_macros.S
index c80c3e4..4d329f5 100644
--- a/include/lib/aarch32/smcc_macros.S
+++ b/include/lib/aarch32/smcc_macros.S
@@ -109,7 +109,13 @@
 	msr	spsr_und, r9
 	msr	sp_und, r10
 	msr	lr_und, r11
-	msr	spsr, r12
+	/*
+	 * Use the `_fsxc` suffix explicitly to instruct the assembler
+	 * to update all the 32 bits of SPSR. Else, by default, the
+	 * assembler assumes `_fc` suffix which only modifies
+	 * f->[31:24] and c->[7:0] bits of SPSR.
+	 */
+	msr	spsr_fsxc, r12
 
 	/* Restore the rest of the general purpose registers */
 	ldm	r0, {r0-r12}
diff --git a/include/lib/aarch64/arch_helpers.h b/include/lib/aarch64/arch_helpers.h
index 37db031..a013809 100644
--- a/include/lib/aarch64/arch_helpers.h
+++ b/include/lib/aarch64/arch_helpers.h
@@ -280,6 +280,8 @@
 DEFINE_SYSREG_READ_FUNC(ctr_el0)
 
 DEFINE_SYSREG_RW_FUNCS(mdcr_el2)
+DEFINE_SYSREG_RW_FUNCS(hstr_el2)
+DEFINE_SYSREG_RW_FUNCS(cnthp_ctl_el2)
 DEFINE_SYSREG_READ_FUNC(pmcr_el0)
 
 DEFINE_RENAME_SYSREG_RW_FUNCS(icc_sre_el1, ICC_SRE_EL1)
diff --git a/include/plat/arm/common/plat_arm.h b/include/plat/arm/common/plat_arm.h
index 29fcffe..bc32e40 100644
--- a/include/plat/arm/common/plat_arm.h
+++ b/include/plat/arm/common/plat_arm.h
@@ -220,4 +220,7 @@
 unsigned int plat_arm_calc_core_pos(u_register_t mpidr);
 const mmap_region_t *plat_arm_get_mmap(void);
 
+/* Allow platform to override psci_pm_ops during runtime */
+const plat_psci_ops_t *plat_arm_psci_override_pm_ops(plat_psci_ops_t *ops);
+
 #endif /* __PLAT_ARM_H__ */
diff --git a/include/plat/arm/css/common/css_pm.h b/include/plat/arm/css/common/css_pm.h
index 4a6ca81..489275e 100644
--- a/include/plat/arm/css/common/css_pm.h
+++ b/include/plat/arm/css/common/css_pm.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:
@@ -35,6 +35,12 @@
 #include <psci.h>
 #include <types.h>
 
+/* Macros to read the CSS power domain state */
+#define CSS_CORE_PWR_STATE(state)	(state)->pwr_domain_state[ARM_PWR_LVL0]
+#define CSS_CLUSTER_PWR_STATE(state)	(state)->pwr_domain_state[ARM_PWR_LVL1]
+#define CSS_SYSTEM_PWR_STATE(state)	((PLAT_MAX_PWR_LVL > ARM_PWR_LVL1) ?\
+				(state)->pwr_domain_state[ARM_PWR_LVL2] : 0)
+
 int css_pwr_domain_on(u_register_t mpidr);
 void css_pwr_domain_on_finish(const psci_power_state_t *target_state);
 void css_pwr_domain_off(const psci_power_state_t *target_state);
diff --git a/lib/el3_runtime/aarch32/context_mgmt.c b/lib/el3_runtime/aarch32/context_mgmt.c
index 29532e8..51b7759 100644
--- a/lib/el3_runtime/aarch32/context_mgmt.c
+++ b/lib/el3_runtime/aarch32/context_mgmt.c
@@ -243,6 +243,12 @@
 			 * (5 bits) and HPMN is at offset zero within HDCR.
 			 */
 			write_hdcr((read_pmcr() & PMCR_N_BITS) >> PMCR_N_SHIFT);
+
+			/*
+			 * Reset CNTHP_CTL to disable the EL2 physical timer and
+			 * therefore prevent timer interrupts.
+			 */
+			write_cnthp_ctl(0);
 			isb();
 
 			write_scr(read_scr() & ~SCR_NS_BIT);
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index fadc1db..35380f3 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -269,6 +269,18 @@
 			 */
 			write_mdcr_el2((read_pmcr_el0() & PMCR_EL0_N_BITS)
 					>> PMCR_EL0_N_SHIFT);
+			/*
+			 * Avoid unexpected traps of non-secure access to
+			 * certain system registers at EL1 or lower where
+			 * HSTR_EL2 is not completely reset to zero by the
+			 * hardware - zero the entire register.
+			 */
+			write_hstr_el2(0);
+			/*
+			 * Reset CNTHP_CTL_EL2 to disable the EL2 physical timer
+			 * and therefore prevent timer interrupts.
+			 */
+			write_cnthp_ctl_el2(0);
 		}
 	}
 
diff --git a/plat/arm/board/fvp/fvp_pm.c b/plat/arm/board/fvp/fvp_pm.c
index 66c0c3d..139f713 100644
--- a/plat/arm/board/fvp/fvp_pm.c
+++ b/plat/arm/board/fvp/fvp_pm.c
@@ -327,7 +327,7 @@
  * Export the platform handlers via plat_arm_psci_pm_ops. The ARM Standard
  * platform layer will take care of registering the handlers with PSCI.
  ******************************************************************************/
-const plat_psci_ops_t plat_arm_psci_pm_ops = {
+plat_psci_ops_t plat_arm_psci_pm_ops = {
 	.cpu_standby = fvp_cpu_standby,
 	.pwr_domain_on = fvp_pwr_domain_on,
 	.pwr_domain_off = fvp_pwr_domain_off,
diff --git a/plat/arm/board/juno/juno_pm.c b/plat/arm/board/juno/juno_pm.c
index c355d94..c0fa628 100644
--- a/plat/arm/board/juno/juno_pm.c
+++ b/plat/arm/board/juno/juno_pm.c
@@ -76,7 +76,7 @@
  * Export the platform handlers via plat_arm_psci_pm_ops. The ARM Standard
  * platform will take care of registering the handlers with PSCI.
  ******************************************************************************/
-const plat_psci_ops_t plat_arm_psci_pm_ops = {
+plat_psci_ops_t plat_arm_psci_pm_ops = {
 	.pwr_domain_on			= css_pwr_domain_on,
 	.pwr_domain_on_finish		= css_pwr_domain_on_finish,
 	.pwr_domain_off			= css_pwr_domain_off,
diff --git a/plat/arm/common/arm_pm.c b/plat/arm/common/arm_pm.c
index 1e756a9..d2e682d 100644
--- a/plat/arm/common/arm_pm.c
+++ b/plat/arm/common/arm_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:
@@ -38,8 +38,11 @@
 #include <platform_def.h>
 #include <psci.h>
 
+/* Allow ARM Standard platforms to override this function */
+#pragma weak plat_arm_psci_override_pm_ops
+
 /* Standard ARM platforms are expected to export plat_arm_psci_pm_ops */
-extern const plat_psci_ops_t plat_arm_psci_pm_ops;
+extern plat_psci_ops_t plat_arm_psci_pm_ops;
 
 #if ARM_RECOM_STATE_ID_ENC
 extern unsigned int arm_pm_idle_states[];
@@ -151,6 +154,14 @@
 }
 
 /******************************************************************************
+ * Default definition on ARM standard platforms to override the plat_psci_ops.
+ *****************************************************************************/
+const plat_psci_ops_t *plat_arm_psci_override_pm_ops(plat_psci_ops_t *ops)
+{
+	return ops;
+}
+
+/******************************************************************************
  * Helper function to resume the platform from system suspend. Reinitialize
  * the system components which are not in the Always ON power domain.
  * TODO: Unify the platform setup when waking up from cold boot and system
@@ -201,7 +212,7 @@
 int plat_setup_psci_ops(uintptr_t sec_entrypoint,
 				const plat_psci_ops_t **psci_ops)
 {
-	*psci_ops = &plat_arm_psci_pm_ops;
+	*psci_ops = plat_arm_psci_override_pm_ops(&plat_arm_psci_pm_ops);
 
 	/* Setup mailbox with entry point. */
 	arm_program_trusted_mailbox(sec_entrypoint);
diff --git a/plat/arm/css/common/css_common.mk b/plat/arm/css/common/css_common.mk
index 65e125e..86ba6df 100644
--- a/plat/arm/css/common/css_common.mk
+++ b/plat/arm/css/common/css_common.mk
@@ -41,18 +41,18 @@
 BL1_SOURCES		+=	plat/arm/css/common/css_bl1_setup.c
 
 BL2_SOURCES		+=	plat/arm/css/common/css_bl2_setup.c		\
-				plat/arm/css/common/css_mhu.c			\
-				plat/arm/css/common/css_scpi.c
+				plat/arm/css/drivers/scpi/css_mhu.c		\
+				plat/arm/css/drivers/scpi/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_scpi.c
-
-BL31_SOURCES		+=	plat/arm/css/common/css_mhu.c			\
-				plat/arm/css/common/css_pm.c			\
-				plat/arm/css/common/css_scpi.c			\
-				plat/arm/css/common/css_topology.c
+				plat/arm/css/drivers/scpi/css_mhu.c		\
+				plat/arm/css/drivers/scpi/css_scpi.c
 
+BL31_SOURCES		+=	plat/arm/css/common/css_pm.c			\
+				plat/arm/css/common/css_topology.c		\
+				plat/arm/css/drivers/scp/css_pm_scpi.c		\
+				plat/arm/css/drivers/scpi/css_mhu.c		\
+				plat/arm/css/drivers/scpi/css_scpi.c
 
 ifneq (${RESET_TO_BL31},0)
   $(error "Using BL31 as the reset vector is not supported on CSS platforms. \
diff --git a/plat/arm/css/common/css_pm.c b/plat/arm/css/common/css_pm.c
index 7607f61..d4dd0af 100644
--- a/plat/arm/css/common/css_pm.c
+++ b/plat/arm/css/common/css_pm.c
@@ -37,13 +37,7 @@
 #include <plat_arm.h>
 #include <platform.h>
 #include <platform_def.h>
-#include "css_scpi.h"
-
-/* Macros to read the CSS power domain state */
-#define CSS_CORE_PWR_STATE(state)	(state)->pwr_domain_state[ARM_PWR_LVL0]
-#define CSS_CLUSTER_PWR_STATE(state)	(state)->pwr_domain_state[ARM_PWR_LVL1]
-#define CSS_SYSTEM_PWR_STATE(state)	((PLAT_MAX_PWR_LVL > ARM_PWR_LVL1) ?\
-				(state)->pwr_domain_state[ARM_PWR_LVL2] : 0)
+#include "../drivers/scp/css_scp.h"
 
 /* Allow CSS platforms to override `plat_arm_psci_pm_ops` */
 #pragma weak plat_arm_psci_pm_ops
@@ -87,12 +81,7 @@
  ******************************************************************************/
 int css_pwr_domain_on(u_register_t mpidr)
 {
-	/*
-	 * SCP takes care of powering up parent power domains so we
-	 * only need to care about level 0
-	 */
-	scpi_set_css_power_state(mpidr, scpi_power_on, scpi_power_on,
-				 scpi_power_on);
+	css_scp_on(mpidr);
 
 	return PSCI_E_SUCCESS;
 }
@@ -140,30 +129,12 @@
  ******************************************************************************/
 static void css_power_down_common(const psci_power_state_t *target_state)
 {
-	uint32_t cluster_state = scpi_power_on;
-	uint32_t system_state = scpi_power_on;
-
 	/* Prevent interrupts from spuriously waking up this cpu */
 	plat_arm_gic_cpuif_disable();
 
-	/* Check if power down at system power domain level is requested */
-	if (CSS_SYSTEM_PWR_STATE(target_state) == ARM_LOCAL_STATE_OFF)
-			system_state = scpi_power_retention;
-
 	/* Cluster is to be turned off, so disable coherency */
-	if (CSS_CLUSTER_PWR_STATE(target_state) == ARM_LOCAL_STATE_OFF) {
+	if (CSS_CLUSTER_PWR_STATE(target_state) == ARM_LOCAL_STATE_OFF)
 		plat_arm_interconnect_exit_coherency();
-		cluster_state = scpi_power_off;
-	}
-
-	/*
-	 * Ask the SCP to power down the appropriate components depending upon
-	 * their state.
-	 */
-	scpi_set_css_power_state(read_mpidr_el1(),
-				 scpi_power_off,
-				 cluster_state,
-				 system_state);
 }
 
 /*******************************************************************************
@@ -174,6 +145,7 @@
 {
 	assert(CSS_CORE_PWR_STATE(target_state) == ARM_LOCAL_STATE_OFF);
 	css_power_down_common(target_state);
+	css_scp_off(target_state);
 }
 
 /*******************************************************************************
@@ -191,6 +163,7 @@
 
 	assert(CSS_CORE_PWR_STATE(target_state) == ARM_LOCAL_STATE_OFF);
 	css_power_down_common(target_state);
+	css_scp_suspend(target_state);
 }
 
 /*******************************************************************************
@@ -222,34 +195,12 @@
  ******************************************************************************/
 void __dead2 css_system_off(void)
 {
-	uint32_t response;
-
-	/* Send the power down request to the SCP */
-	response = scpi_sys_power_state(scpi_system_shutdown);
-
-	if (response != SCP_OK) {
-		ERROR("CSS System Off: SCP error %u.\n", response);
-		panic();
-	}
-	wfi();
-	ERROR("CSS System Off: operation not handled.\n");
-	panic();
+	css_scp_sys_shutdown();
 }
 
 void __dead2 css_system_reset(void)
 {
-	uint32_t response;
-
-	/* Send the system reset request to the SCP */
-	response = scpi_sys_power_state(scpi_system_reboot);
-
-	if (response != SCP_OK) {
-		ERROR("CSS System Reset: SCP error %u.\n", response);
-		panic();
-	}
-	wfi();
-	ERROR("CSS System Reset: operation not handled.\n");
-	panic();
+	css_scp_sys_reboot();
 }
 
 /*******************************************************************************
@@ -303,43 +254,14 @@
  ******************************************************************************/
 int css_node_hw_state(u_register_t mpidr, unsigned int power_level)
 {
-	int rc, element;
-	unsigned int cpu_state, cluster_state;
-
-	/*
-	 * The format of 'power_level' is implementation-defined, but 0 must
-	 * mean a CPU. We also allow 1 to denote the cluster
-	 */
-	if (power_level != ARM_PWR_LVL0 && power_level != ARM_PWR_LVL1)
-		return PSCI_E_INVALID_PARAMS;
-
-	/* Query SCP */
-	rc = scpi_get_css_power_state(mpidr, &cpu_state, &cluster_state);
-	if (rc != 0)
-		return PSCI_E_INVALID_PARAMS;
-
-	/* Map power states of CPU and cluster to expected PSCI return codes */
-	if (power_level == ARM_PWR_LVL0) {
-		/*
-		 * The CPU state returned by SCP is an 8-bit bit mask
-		 * corresponding to each CPU in the cluster
-		 */
-		element = mpidr & MPIDR_AFFLVL_MASK;
-		return CSS_CPU_PWR_STATE(cpu_state, element) ==
-			CSS_CPU_PWR_STATE_ON ? HW_ON : HW_OFF;
-	} else {
-		assert(cluster_state == CSS_CLUSTER_PWR_STATE_ON ||
-				cluster_state == CSS_CLUSTER_PWR_STATE_OFF);
-		return cluster_state == CSS_CLUSTER_PWR_STATE_ON ? HW_ON :
-			HW_OFF;
-	}
+	return css_scp_get_power_state(mpidr, power_level);
 }
 
 /*******************************************************************************
  * Export the platform handlers via plat_arm_psci_pm_ops. The ARM Standard
  * platform will take care of registering the handlers with PSCI.
  ******************************************************************************/
-const plat_psci_ops_t plat_arm_psci_pm_ops = {
+plat_psci_ops_t plat_arm_psci_pm_ops = {
 	.pwr_domain_on		= css_pwr_domain_on,
 	.pwr_domain_on_finish	= css_pwr_domain_on_finish,
 	.pwr_domain_off		= css_pwr_domain_off,
diff --git a/plat/arm/css/common/css_scp_bootloader.c b/plat/arm/css/common/css_scp_bootloader.c
index d3f671e..049afe4 100644
--- a/plat/arm/css/common/css_scp_bootloader.c
+++ b/plat/arm/css/common/css_scp_bootloader.c
@@ -34,9 +34,9 @@
 #include <debug.h>
 #include <platform.h>
 #include <stdint.h>
-#include "css_mhu.h"
+#include "../drivers/scpi/css_mhu.h"
+#include "../drivers/scpi/css_scpi.h"
 #include "css_scp_bootloader.h"
-#include "css_scpi.h"
 
 /* ID of the MHU slot used for the BOM protocol */
 #define BOM_MHU_SLOT_ID		0
diff --git a/plat/arm/css/drivers/scp/css_pm_scpi.c b/plat/arm/css/drivers/scp/css_pm_scpi.c
new file mode 100644
index 0000000..e22504d
--- /dev/null
+++ b/plat/arm/css/drivers/scp/css_pm_scpi.c
@@ -0,0 +1,166 @@
+/*
+ * 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_helpers.h>
+#include <assert.h>
+#include <css_pm.h>
+#include <debug.h>
+#include "../scpi/css_scpi.h"
+#include "css_scp.h"
+
+/*
+ * This file implements the SCP power management functions using SCPI protocol.
+ */
+
+/*
+ * Helper function to inform power down state to SCP.
+ */
+void css_scp_suspend(const psci_power_state_t *target_state)
+{
+	uint32_t cluster_state = scpi_power_on;
+	uint32_t system_state = scpi_power_on;
+
+	/* Check if power down at system power domain level is requested */
+	if (CSS_SYSTEM_PWR_STATE(target_state) == ARM_LOCAL_STATE_OFF)
+		system_state = scpi_power_retention;
+
+	/* Cluster is to be turned off, so disable coherency */
+	if (CSS_CLUSTER_PWR_STATE(target_state) == ARM_LOCAL_STATE_OFF)
+		cluster_state = scpi_power_off;
+
+	/*
+	 * Ask the SCP to power down the appropriate components depending upon
+	 * their state.
+	 */
+	scpi_set_css_power_state(read_mpidr_el1(),
+				 scpi_power_off,
+				 cluster_state,
+				 system_state);
+}
+
+/*
+ * Helper function to turn off a CPU power domain and its parent power domains
+ * if applicable. Since SCPI doesn't differentiate between OFF and suspend, we
+ * call the suspend helper here.
+ */
+void css_scp_off(const psci_power_state_t *target_state)
+{
+	css_scp_suspend(target_state);
+}
+
+/*
+ * Helper function to turn ON a CPU power domain and its parent power domains
+ * if applicable.
+ */
+void css_scp_on(u_register_t mpidr)
+{
+	/*
+	 * SCP takes care of powering up parent power domains so we
+	 * only need to care about level 0
+	 */
+	scpi_set_css_power_state(mpidr, scpi_power_on, scpi_power_on,
+				 scpi_power_on);
+}
+
+/*
+ * Helper function to get the power state of a power domain node as reported
+ * by the SCP.
+ */
+int css_scp_get_power_state(u_register_t mpidr, unsigned int power_level)
+{
+	int rc, element;
+	unsigned int cpu_state, cluster_state;
+
+	/*
+	 * The format of 'power_level' is implementation-defined, but 0 must
+	 * mean a CPU. We also allow 1 to denote the cluster
+	 */
+	if (power_level != ARM_PWR_LVL0 && power_level != ARM_PWR_LVL1)
+		return PSCI_E_INVALID_PARAMS;
+
+	/* Query SCP */
+	rc = scpi_get_css_power_state(mpidr, &cpu_state, &cluster_state);
+	if (rc != 0)
+		return PSCI_E_INVALID_PARAMS;
+
+	/* Map power states of CPU and cluster to expected PSCI return codes */
+	if (power_level == ARM_PWR_LVL0) {
+		/*
+		 * The CPU state returned by SCP is an 8-bit bit mask
+		 * corresponding to each CPU in the cluster
+		 */
+		element = mpidr & MPIDR_AFFLVL_MASK;
+		return CSS_CPU_PWR_STATE(cpu_state, element) ==
+			CSS_CPU_PWR_STATE_ON ? HW_ON : HW_OFF;
+	} else {
+		assert(cluster_state == CSS_CLUSTER_PWR_STATE_ON ||
+				cluster_state == CSS_CLUSTER_PWR_STATE_OFF);
+		return cluster_state == CSS_CLUSTER_PWR_STATE_ON ? HW_ON :
+			HW_OFF;
+	}
+}
+
+/*
+ * Helper function to shutdown the system via SCPI.
+ */
+void __dead2 css_scp_sys_shutdown(void)
+{
+	uint32_t response;
+
+	/* Send the power down request to the SCP */
+	response = scpi_sys_power_state(scpi_system_shutdown);
+
+	if (response != SCP_OK) {
+		ERROR("CSS System Off: SCP error %u.\n", response);
+		panic();
+	}
+	wfi();
+	ERROR("CSS System Off: operation not handled.\n");
+	panic();
+}
+
+/*
+ * Helper function to reset the system via SCPI.
+ */
+void __dead2 css_scp_sys_reboot(void)
+{
+	uint32_t response;
+
+	/* Send the system reset request to the SCP */
+	response = scpi_sys_power_state(scpi_system_reboot);
+
+	if (response != SCP_OK) {
+		ERROR("CSS System Reset: SCP error %u.\n", response);
+		panic();
+	}
+	wfi();
+	ERROR("CSS System Reset: operation not handled.\n");
+	panic();
+}
diff --git a/plat/arm/css/common/css_mhu.h b/plat/arm/css/drivers/scp/css_scp.h
similarity index 75%
copy from plat/arm/css/common/css_mhu.h
copy to plat/arm/css/drivers/scp/css_scp.h
index 2175cdf..a12798b 100644
--- a/plat/arm/css/common/css_mhu.h
+++ b/plat/arm/css/drivers/scp/css_scp.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, ARM Limited and Contributors. All rights reserved.
+ * 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:
@@ -28,16 +28,16 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#ifndef __CSS_MHU_H__
-#define __CSS_MHU_H__
+#ifndef __CSS_SCP_H__
+#define __CSS_SCP_H__
 
-#include <stdint.h>
+#include <psci.h>
 
-void mhu_secure_message_start(unsigned int slot_id);
-void mhu_secure_message_send(unsigned int slot_id);
-uint32_t mhu_secure_message_wait(void);
-void mhu_secure_message_end(unsigned int slot_id);
-
-void mhu_secure_init(void);
+void css_scp_suspend(const psci_power_state_t *target_state);
+void css_scp_off(const psci_power_state_t *target_state);
+void css_scp_on(u_register_t mpidr);
+int css_scp_get_power_state(u_register_t mpidr, unsigned int power_level);
+void __dead2 css_scp_sys_shutdown(void);
+void __dead2 css_scp_sys_reboot(void);
 
-#endif	/* __CSS_MHU_H__ */
+#endif	/* __CSS_SCP_H__ */
diff --git a/plat/arm/css/common/css_mhu.c b/plat/arm/css/drivers/scpi/css_mhu.c
similarity index 99%
rename from plat/arm/css/common/css_mhu.c
rename to plat/arm/css/drivers/scpi/css_mhu.c
index 265d6c2..302286f 100644
--- a/plat/arm/css/common/css_mhu.c
+++ b/plat/arm/css/drivers/scpi/css_mhu.c
@@ -33,8 +33,8 @@
 #include <bakery_lock.h>
 #include <css_def.h>
 #include <mmio.h>
-#include <platform_def.h>
 #include <plat_arm.h>
+#include <platform_def.h>
 #include "css_mhu.h"
 
 /* SCP MHU secure channel registers */
diff --git a/plat/arm/css/common/css_mhu.h b/plat/arm/css/drivers/scpi/css_mhu.h
similarity index 96%
rename from plat/arm/css/common/css_mhu.h
rename to plat/arm/css/drivers/scpi/css_mhu.h
index 2175cdf..6973a14 100644
--- a/plat/arm/css/common/css_mhu.h
+++ b/plat/arm/css/drivers/scpi/css_mhu.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:
diff --git a/plat/arm/css/common/css_scpi.c b/plat/arm/css/drivers/scpi/css_scpi.c
similarity index 94%
rename from plat/arm/css/common/css_scpi.c
rename to plat/arm/css/drivers/scpi/css_scpi.c
index 90a8939..f419abd 100644
--- a/plat/arm/css/common/css_scpi.c
+++ b/plat/arm/css/drivers/scpi/css_scpi.c
@@ -63,9 +63,11 @@
 
 static void scpi_secure_message_send(size_t payload_size)
 {
-	/* Ensure that any write to the SCPI payload area is seen by SCP before
+	/*
+	 * 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 */
+	 * the CPU then SCP would read stale payload data
+	 */
 	dmbst();
 
 	mhu_secure_message_send(SCPI_MHU_SLOT_ID);
@@ -86,9 +88,11 @@
 		panic();
 	}
 
-	/* Ensure that any read to the SCPI payload area is done after reading
+	/*
+	 * 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 */
+	 * read invalid payload data
+	 */
 	dmbld();
 
 	memcpy(cmd, (void *) SCPI_SHARED_MEM_SCP_TO_AP, sizeof(*cmd));
@@ -137,8 +141,9 @@
 	return status == SCP_OK ? 0 : -1;
 }
 
-void scpi_set_css_power_state(unsigned mpidr, scpi_power_state_t cpu_state,
-		scpi_power_state_t cluster_state, scpi_power_state_t css_state)
+void scpi_set_css_power_state(unsigned int mpidr,
+		scpi_power_state_t cpu_state, scpi_power_state_t cluster_state,
+		scpi_power_state_t css_state)
 {
 	scpi_cmd_t *cmd;
 	uint32_t state = 0;
diff --git a/plat/arm/css/common/css_scpi.h b/plat/arm/css/drivers/scpi/css_scpi.h
similarity index 98%
rename from plat/arm/css/common/css_scpi.h
rename to plat/arm/css/drivers/scpi/css_scpi.h
index 1fb55e4..d1e7cba 100644
--- a/plat/arm/css/common/css_scpi.h
+++ b/plat/arm/css/drivers/scpi/css_scpi.h
@@ -122,7 +122,7 @@
 } scpi_system_state_t;
 
 extern int scpi_wait_ready(void);
-extern void scpi_set_css_power_state(unsigned mpidr,
+extern void scpi_set_css_power_state(unsigned int mpidr,
 					scpi_power_state_t cpu_state,
 					scpi_power_state_t cluster_state,
 					scpi_power_state_t css_state);
diff --git a/plat/common/aarch32/platform_helpers.S b/plat/common/aarch32/platform_helpers.S
index 069d96d..802e1fe 100644
--- a/plat/common/aarch32/platform_helpers.S
+++ b/plat/common/aarch32/platform_helpers.S
@@ -31,27 +31,12 @@
 #include <arch.h>
 #include <asm_macros.S>
 
-	.weak	plat_my_core_pos
 	.weak	plat_reset_handler
 	.weak	plat_disable_acp
 	.weak	platform_mem_init
 	.weak	plat_panic_handler
 
 	/* -----------------------------------------------------
-	 *  int plat_my_core_pos(void);
-	 *  With this function: CorePos	= (ClusterId * 4) +
-	 *				  CoreId
-	 * -----------------------------------------------------
-	 */
-func plat_my_core_pos
-	ldcopr	r0, MPIDR
-	and	r1, r0, #MPIDR_CPU_MASK
-	and	r0, r0, #MPIDR_CLUSTER_MASK
-	add	r0, r1, r0, LSR #6
-	bx	lr
-endfunc	plat_my_core_pos
-
-	/* -----------------------------------------------------
 	 * Placeholder function which should be redefined by
 	 * each platform.
 	 * -----------------------------------------------------
diff --git a/services/spd/trusty/trusty.c b/services/spd/trusty/trusty.c
index 4c688ca..78a68ba 100644
--- a/services/spd/trusty/trusty.c
+++ b/services/spd/trusty/trusty.c
@@ -236,7 +236,12 @@
 	int reg_width = GET_RW(read_ctx_reg(get_el3state_ctx(&ctx->cpu_ctx),
 			       CTX_SPSR_EL3));
 
+	/*
+	 * Get information about the Trusty image. Its absence is a critical
+	 * failure.
+	 */
 	ep_info = bl31_plat_get_next_image_ep_info(SECURE);
+	assert(ep_info);
 
 	cm_el1_sysregs_context_save(NON_SECURE);
 
diff --git a/tools/fiptool/fiptool.c b/tools/fiptool/fiptool.c
index 0084edf..7145815 100644
--- a/tools/fiptool/fiptool.c
+++ b/tools/fiptool/fiptool.c
@@ -82,7 +82,7 @@
 static uuid_t uuid_null = { 0 };
 static int verbose;
 
-static void vlog(int prio, char *msg, va_list ap)
+static void vlog(int prio, const char *msg, va_list ap)
 {
 	char *prefix[] = { "DEBUG", "WARN", "ERROR" };
 
@@ -91,7 +91,7 @@
 	fputc('\n', stderr);
 }
 
-static void log_dbgx(char *msg, ...)
+static void log_dbgx(const char *msg, ...)
 {
 	va_list ap;
 
@@ -100,7 +100,7 @@
 	va_end(ap);
 }
 
-static void log_warnx(char *msg, ...)
+static void log_warnx(const char *msg, ...)
 {
 	va_list ap;
 
@@ -109,7 +109,7 @@
 	va_end(ap);
 }
 
-static void log_err(char *msg, ...)
+static void log_err(const char *msg, ...)
 {
 	char buf[512];
 	va_list ap;
@@ -121,7 +121,7 @@
 	exit(1);
 }
 
-static void log_errx(char *msg, ...)
+static void log_errx(const char *msg, ...)
 {
 	va_list ap;
 
@@ -131,6 +131,26 @@
 	exit(1);
 }
 
+static char *xstrdup(const char *s, const char *msg)
+{
+	char *d;
+
+	d = strdup(s);
+	if (d == NULL)
+		log_errx("strdup: ", msg);
+	return d;
+}
+
+static void *xmalloc(size_t size, const char *msg)
+{
+	void *d;
+
+	d = malloc(size);
+	if (d == NULL)
+		log_errx("malloc: ", msg);
+	return d;
+}
+
 static void add_image(image_t *image)
 {
 	if (nr_images + 1 > MAX_IMAGES)
@@ -210,7 +230,7 @@
 	return NULL;
 }
 
-static int parse_fip(char *filename, fip_toc_header_t *toc_header_out)
+static int parse_fip(const char *filename, fip_toc_header_t *toc_header_out)
 {
 	struct stat st;
 	FILE *fp;
@@ -227,10 +247,7 @@
 	if (fstat(fileno(fp), &st) == -1)
 		log_err("fstat %s", filename);
 
-	buf = malloc(st.st_size);
-	if (buf == NULL)
-		log_err("malloc");
-
+	buf = xmalloc(st.st_size, "failed to load file into memory");
 	if (fread(buf, 1, st.st_size, fp) != st.st_size)
 		log_errx("Failed to read %s", filename);
 	bufend = buf + st.st_size;
@@ -261,16 +278,11 @@
 		 * Build a new image out of the ToC entry and add it to the
 		 * table of images.
 		 */
-		image = malloc(sizeof(*image));
-		if (image == NULL)
-			log_err("malloc");
-
+		image = xmalloc(sizeof(*image),
+		    "failed to allocate memory for image");
 		memcpy(&image->uuid, &toc_entry->uuid, sizeof(uuid_t));
-
-		image->buffer = malloc(toc_entry->size);
-		if (image->buffer == NULL)
-			log_err("malloc");
-
+		image->buffer = xmalloc(toc_entry->size,
+		    "failed to allocate image buffer, is FIP file corrupted?");
 		/* Overflow checks before memory copy. */
 		if (toc_entry->size > (uint64_t)-1 - toc_entry->offset_address)
 			log_errx("FIP %s is corrupted", filename);
@@ -293,7 +305,7 @@
 	return 0;
 }
 
-static image_t *read_image_from_file(uuid_t *uuid, char *filename)
+static image_t *read_image_from_file(const uuid_t *uuid, const char *filename)
 {
 	struct stat st;
 	image_t *image;
@@ -308,15 +320,9 @@
 	if (fstat(fileno(fp), &st) == -1)
 		log_errx("fstat %s", filename);
 
-	image = malloc(sizeof(*image));
-	if (image == NULL)
-		log_err("malloc");
-
+	image = xmalloc(sizeof(*image), "failed to allocate memory for image");
 	memcpy(&image->uuid, uuid, sizeof(uuid_t));
-
-	image->buffer = malloc(st.st_size);
-	if (image->buffer == NULL)
-		log_err("malloc");
+	image->buffer = xmalloc(st.st_size, "failed to allocate image buffer");
 	if (fread(image->buffer, 1, st.st_size, fp) != st.st_size)
 		log_errx("Failed to read %s", filename);
 	image->size = st.st_size;
@@ -325,7 +331,7 @@
 	return image;
 }
 
-static int write_image_to_file(image_t *image, char *filename)
+static int write_image_to_file(const image_t *image, const char *filename)
 {
 	FILE *fp;
 
@@ -570,9 +576,9 @@
 	add_opt(opts, ++i, NULL, 0, 0);
 
 	while (1) {
-		int c, opt_index;
+		int c, opt_index = 0;
 
-		c = getopt_long(argc, argv, "o:", opts, &opt_index);
+		c = getopt_long(argc, argv, "", opts, &opt_index);
 		if (c == -1)
 			break;
 
@@ -582,9 +588,8 @@
 
 			toc_entry = &toc_entries[opt_index];
 			toc_entry->action = DO_PACK;
-			toc_entry->action_arg = strdup(optarg);
-			if (toc_entry->action_arg == NULL)
-				log_err("strdup");
+			toc_entry->action_arg = xstrdup(optarg,
+			    "failed to allocate memory for argument");
 			break;
 		}
 		case OPT_PLAT_TOC_FLAGS:
@@ -641,7 +646,7 @@
 	add_opt(opts, ++i, NULL, 0, 0);
 
 	while (1) {
-		int c, opt_index;
+		int c, opt_index = 0;
 
 		c = getopt_long(argc, argv, "o:", opts, &opt_index);
 		if (c == -1)
@@ -653,9 +658,8 @@
 
 			toc_entry = &toc_entries[opt_index];
 			toc_entry->action = DO_PACK;
-			toc_entry->action_arg = strdup(optarg);
-			if (toc_entry->action_arg == NULL)
-				log_err("strdup");
+			toc_entry->action_arg = xstrdup(optarg,
+			    "failed to allocate memory for argument");
 			break;
 		}
 		case OPT_PLAT_TOC_FLAGS: {
@@ -728,7 +732,7 @@
 	add_opt(opts, ++i, NULL, 0, 0);
 
 	while (1) {
-		int c, opt_index;
+		int c, opt_index = 0;
 
 		c = getopt_long(argc, argv, "fo:", opts, &opt_index);
 		if (c == -1)
@@ -739,9 +743,8 @@
 			unpack_all = 0;
 			toc_entry = &toc_entries[opt_index];
 			toc_entry->action = DO_UNPACK;
-			toc_entry->action_arg = strdup(optarg);
-			if (toc_entry->action_arg == NULL)
-				log_err("strdup");
+			toc_entry->action_arg = xstrdup(optarg,
+			    "failed to allocate memory for argument");
 			break;
 		case 'f':
 			fflag = 1;
@@ -845,7 +848,7 @@
 	add_opt(opts, ++i, NULL, 0, 0);
 
 	while (1) {
-		int c, opt_index;
+		int c, opt_index = 0;
 
 		c = getopt_long(argc, argv, "fo:", opts, &opt_index);
 		if (c == -1)
@@ -977,17 +980,36 @@
 {
 	int i, ret = 0;
 
-	if (argc < 2)
-		usage();
-	argc--, argv++;
+	while (1) {
+		int c, opt_index = 0;
+		static struct option opts[] = {
+			{ "verbose", no_argument, NULL, 'v' },
+			{ NULL, no_argument, NULL, 0 }
+		};
 
-	if (strcmp(argv[0], "-v") == 0 ||
-	    strcmp(argv[0], "--verbose") == 0) {
-		verbose = 1;
-		argc--, argv++;
-		if (argc < 1)
+		/*
+		 * Set POSIX mode so getopt stops at the first non-option
+		 * which is the subcommand.
+		 */
+		c = getopt_long(argc, argv, "+v", opts, &opt_index);
+		if (c == -1)
+			break;
+
+		switch (c) {
+		case 'v':
+			verbose = 1;
+			break;
+		default:
 			usage();
+		}
 	}
+	argc -= optind;
+	argv += optind;
+	/* Reset optind for subsequent getopt processing. */
+	optind = 0;
+
+	if (argc == 0)
+		usage();
 
 	for (i = 0; i < NELEM(cmds); i++) {
 		if (strcmp(cmds[i].name, argv[0]) == 0) {