Merge pull request #296 from danh-arm/sb/scpi-min-changes

Move to the new ARM SCP Messaging Interfaces v2

8 files changed, 305 insertions, 156 deletions

diff --git a/docs/user-guide.md b/docs/user-guide.md
index f10a6f3..00feacc 100644
--- a/docs/user-guide.md
+++ b/docs/user-guide.md
@@ -330,6 +330,15 @@ performed.
 For a better understanding of these options, the ARM development platform memory
 map is explained in the [Firmware Design].
 
+#### ARM CSS platform specific build options
+
+*   `CSS_DETECT_PRE_1_7_0_SCP`: Boolean flag to detect SCP version
+    incompatibility. Version 1.7.0 of the SCP firmware made a non-backwards
+    compatible change to the MTL protocol, used for AP/SCP communication.
+    Trusted Firmware no longer supports earlier SCP versions. If this option is
+    set to 1 then Trusted Firmware will detect if an earlier version is in use.
+    Default is 1.
+
 
 ### Creating a Firmware Image Package
 
diff --git a/plat/arm/css/common/css_bl2_setup.c b/plat/arm/css/common/css_bl2_setup.c
index 2ca7a0b..2e423d9 100644
--- a/plat/arm/css/common/css_bl2_setup.c
+++ b/plat/arm/css/common/css_bl2_setup.c
@@ -43,13 +43,15 @@ int bl2_plat_handle_bl30(image_info_t *bl30_image_info)
 {
 	int ret;
 
+	INFO("BL2: Initiating BL3-0 transfer to SCP\n");
+
 	ret = scp_bootloader_transfer((void *)bl30_image_info->image_base,
 		bl30_image_info->image_size);
 
 	if (ret == 0)
-		INFO("BL2: BL3-0 transferred to SCP\n\r");
+		INFO("BL2: BL3-0 transferred to SCP\n");
 	else
-		ERROR("BL2: BL3-0 transfer failure\n\r");
+		ERROR("BL2: BL3-0 transfer failure\n");
 
 	return ret;
 }
diff --git a/plat/arm/css/common/css_common.mk b/plat/arm/css/common/css_common.mk
index edbfe1e..1b0404b 100644
--- a/plat/arm/css/common/css_common.mk
+++ b/plat/arm/css/common/css_common.mk
@@ -53,3 +53,11 @@ ifneq (${RESET_TO_BL31},0)
 endif
 
 NEED_BL30		:=	yes
+
+# Enable option to detect whether the SCP ROM firmware in use predates version
+# 1.7.0 and therefore, is incompatible.
+CSS_DETECT_PRE_1_7_0_SCP	:=	1
+
+# Process CSS_DETECT_PRE_1_7_0_SCP flag
+$(eval $(call assert_boolean,CSS_DETECT_PRE_1_7_0_SCP))
+$(eval $(call add_define,CSS_DETECT_PRE_1_7_0_SCP))
diff --git a/plat/arm/css/common/css_mhu.c b/plat/arm/css/common/css_mhu.c
index fa4a81d..b1714e2 100644
--- a/plat/arm/css/common/css_mhu.c
+++ b/plat/arm/css/common/css_mhu.c
@@ -29,6 +29,7 @@
  */
 
 #include <arch_helpers.h>
+#include <assert.h>
 #include <bakery_lock.h>
 #include <css_def.h>
 #include <mmio.h>
@@ -51,21 +52,31 @@ ARM_INSTANTIATE_LOCK
 #pragma weak plat_arm_pwrc_setup
 
 
-void mhu_secure_message_start(void)
+/*
+ * Slot 31 is reserved because the MHU hardware uses this register bit to
+ * indicate a non-secure access attempt. The total number of available slots is
+ * therefore 31 [30:0].
+ */
+#define MHU_MAX_SLOT_ID		30
+
+void mhu_secure_message_start(unsigned int slot_id)
 {
+	assert(slot_id <= MHU_MAX_SLOT_ID);
+
 	arm_lock_get();
 
 	/* Make sure any previous command has finished */
-	while (mmio_read_32(MHU_BASE + CPU_INTR_S_STAT) != 0)
+	while (mmio_read_32(MHU_BASE + CPU_INTR_S_STAT) & (1 << slot_id))
 		;
 }
 
-void mhu_secure_message_send(uint32_t command)
+void mhu_secure_message_send(unsigned int slot_id)
 {
-	/* Send command to SCP and wait for it to pick it up */
-	mmio_write_32(MHU_BASE + CPU_INTR_S_SET, command);
-	while (mmio_read_32(MHU_BASE + CPU_INTR_S_STAT) != 0)
-		;
+	assert(slot_id <= MHU_MAX_SLOT_ID);
+	assert(!(mmio_read_32(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);
 }
 
 uint32_t mhu_secure_message_wait(void)
@@ -78,13 +89,15 @@ uint32_t mhu_secure_message_wait(void)
 	return response;
 }
 
-void mhu_secure_message_end(void)
+void mhu_secure_message_end(unsigned int slot_id)
 {
+	assert(slot_id <= MHU_MAX_SLOT_ID);
+
 	/*
-	 * Clear any response we got by writing all ones to the CLEAR
-	 * register
+	 * 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, 0xffffffffu);
+	mmio_write_32(MHU_BASE + SCP_INTR_S_CLEAR, 1 << slot_id);
 
 	arm_lock_release();
 }
@@ -94,10 +107,11 @@ void mhu_secure_init(void)
 	arm_lock_init();
 
 	/*
-	 * Clear the CPU's INTR register to make sure we don't see a stale
-	 * or garbage value and think it's a message we've already sent.
+	 * The STAT register resets to zero. Ensure it is in the expected state,
+	 * as a stale or garbage value would make us think it's a message we've
+	 * already sent.
 	 */
-	mmio_write_32(MHU_BASE + CPU_INTR_S_CLEAR, 0xffffffffu);
+	assert(mmio_read_32(MHU_BASE + CPU_INTR_S_STAT) == 0);
 }
 
 void plat_arm_pwrc_setup(void)
diff --git a/plat/arm/css/common/css_mhu.h b/plat/arm/css/common/css_mhu.h
index c2e5327..2175cdf 100644
--- a/plat/arm/css/common/css_mhu.h
+++ b/plat/arm/css/common/css_mhu.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2015, 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,11 +33,11 @@
 
 #include <stdint.h>
 
-extern void mhu_secure_message_start(void);
-extern void mhu_secure_message_send(uint32_t command);
-extern uint32_t mhu_secure_message_wait(void);
-extern void mhu_secure_message_end(void);
+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);
 
-extern void mhu_secure_init(void);
+void mhu_secure_init(void);
 
 #endif	/* __CSS_MHU_H__ */
diff --git a/plat/arm/css/common/css_scp_bootloader.c b/plat/arm/css/common/css_scp_bootloader.c
index b0bd417..6cf1667 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, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2015, 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,125 +29,172 @@
  */
 
 #include <arch_helpers.h>
+#include <assert.h>
 #include <css_def.h>
+#include <debug.h>
 #include <platform.h>
+#include <stdint.h>
 #include "css_mhu.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
+
 /* Boot commands sent from AP -> SCP */
-#define BOOT_CMD_START	0x01
-#define BOOT_CMD_DATA	0x02
+#define BOOT_CMD_INFO	0x00
+#define BOOT_CMD_DATA	0x01
 
+/* BOM command header */
 typedef struct {
-	uint32_t image_size;
-} cmd_start_payload;
+	uint32_t id : 8;
+	uint32_t reserved : 24;
+} bom_cmd_t;
 
 typedef struct {
-	uint32_t sequence_num;
-	uint32_t offset;
-	uint32_t size;
-} cmd_data_payload;
-
-#define BOOT_DATA_MAX_SIZE  0x1000
-
-/* Boot commands sent from SCP -> AP */
-#define BOOT_CMD_ACK	0x03
-#define BOOT_CMD_NACK	0x04
-
-typedef struct {
-	uint32_t sequence_num;
-} cmd_ack_payload;
+	uint32_t image_size;
+	uint32_t checksum;
+} cmd_info_payload_t;
 
 /*
- * Unlike the runtime protocol, the boot protocol uses the same memory region
+ * 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...
  */
-static void * const cmd_payload = (void *)(MHU_SECURE_BASE + 0x0080);
+#define BOM_SHARED_MEM		(MHU_SECURE_BASE + 0x0080)
+#define BOM_CMD_HEADER		((bom_cmd_t *) BOM_SHARED_MEM)
+#define BOM_CMD_PAYLOAD		((void *) (BOM_SHARED_MEM + sizeof(bom_cmd_t)))
 
-static void *scp_boot_message_start(void)
-{
-	mhu_secure_message_start();
+typedef struct {
+	/* Offset from the base address of the Trusted RAM */
+	uint32_t offset;
+	uint32_t block_size;
+} cmd_data_payload_t;
 
-	return cmd_payload;
+static void scp_boot_message_start(void)
+{
+	mhu_secure_message_start(BOM_MHU_SLOT_ID);
 }
 
-static void scp_boot_message_send(unsigned command, size_t size)
+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((unsigned long)cmd_payload, size);
+		flush_dcache_range(BOM_SHARED_MEM,
+				   sizeof(bom_cmd_t) + payload_size);
 
 	/* Send command to SCP */
-	mhu_secure_message_send(command | (size << 8));
+	mhu_secure_message_send(BOM_MHU_SLOT_ID);
 }
 
 static uint32_t scp_boot_message_wait(size_t size)
 {
-	uint32_t response =  mhu_secure_message_wait();
+	uint32_t mhu_status;
+
+	mhu_status = mhu_secure_message_wait();
+
+	/* Expect an SCP Boot Protocol message, reject any other protocol */
+	if (mhu_status != (1 << BOM_MHU_SLOT_ID)) {
+		ERROR("MHU: Unexpected protocol (MHU status: 0x%x)\n",
+			mhu_status);
+		panic();
+	}
 
 	/* Make sure we see the reply from the SCP and not any stale data */
 	if (MHU_PAYLOAD_CACHED)
-		inv_dcache_range((unsigned long)cmd_payload, size);
+		inv_dcache_range(BOM_SHARED_MEM, size);
 
-	return response & 0xff;
+	return *(uint32_t *) BOM_SHARED_MEM;
 }
 
 static void scp_boot_message_end(void)
 {
-	mhu_secure_message_end();
+	mhu_secure_message_end(BOM_MHU_SLOT_ID);
 }
 
-static int transfer_block(uint32_t sequence_num, uint32_t offset, uint32_t size)
+int scp_bootloader_transfer(void *image, unsigned int image_size)
 {
-	cmd_data_payload *cmd_data = scp_boot_message_start();
-	cmd_data->sequence_num = sequence_num;
-	cmd_data->offset = offset;
-	cmd_data->size = size;
+	uint32_t response;
+	uint32_t checksum;
+	cmd_info_payload_t *cmd_info_payload;
+	cmd_data_payload_t *cmd_data_payload;
+
+	assert((uintptr_t) image == BL30_BASE);
 
-	scp_boot_message_send(BOOT_CMD_DATA, sizeof(*cmd_data));
+	if ((image_size == 0) || (image_size % 4 != 0)) {
+		ERROR("Invalid size for the BL3-0 image. Must be a multiple of "
+			"4 bytes and not zero (current size = 0x%x)\n",
+			image_size);
+		return -1;
+	}
 
-	cmd_ack_payload *cmd_ack = cmd_payload;
-	int ok = scp_boot_message_wait(sizeof(*cmd_ack)) == BOOT_CMD_ACK
-		 && cmd_ack->sequence_num == sequence_num;
+	/* Extract the checksum from the image */
+	checksum = *(uint32_t *) image;
+	image = (char *) image + sizeof(checksum);
+	image_size -= sizeof(checksum);
 
-	scp_boot_message_end();
+	mhu_secure_init();
 
-	return ok;
-}
+	VERBOSE("Send info about the BL3-0 image to be transferred to SCP\n");
 
-int scp_bootloader_transfer(void *image, unsigned int image_size)
-{
-	uintptr_t offset = (uintptr_t)image - MHU_SECURE_BASE;
-	uintptr_t end = offset + image_size;
-	uint32_t response;
+	/*
+	 * Send information about the SCP firmware image about to be transferred
+	 * to SCP
+	 */
+	scp_boot_message_start();
 
-	mhu_secure_init();
+	BOM_CMD_HEADER->id = BOOT_CMD_INFO;
+	cmd_info_payload = BOM_CMD_PAYLOAD;
+	cmd_info_payload->image_size = image_size;
+	cmd_info_payload->checksum = checksum;
+
+	scp_boot_message_send(sizeof(*cmd_info_payload));
+#if CSS_DETECT_PRE_1_7_0_SCP
+	{
+		const uint32_t deprecated_scp_nack_cmd = 0x404;
+		uint32_t mhu_status;
 
-	/* Initiate communications with SCP */
-	do {
-		cmd_start_payload *cmd_start = scp_boot_message_start();
-		cmd_start->image_size = image_size;
+		VERBOSE("Detecting SCP version incompatibility\n");
 
-		scp_boot_message_send(BOOT_CMD_START, sizeof(*cmd_start));
+		mhu_status = mhu_secure_message_wait();
+		if (mhu_status == deprecated_scp_nack_cmd) {
+			ERROR("Detected an incompatible version of the SCP firmware.\n");
+			ERROR("Only versions from v1.7.0 onwards are supported.\n");
+			ERROR("Please update the SCP firmware.\n");
+			return -1;
+		}
 
-		response = scp_boot_message_wait(0);
+		VERBOSE("SCP version looks OK\n");
+	}
+#endif /* CSS_DETECT_PRE_1_7_0_SCP */
+	response = scp_boot_message_wait(sizeof(response));
+	scp_boot_message_end();
+
+	if (response != 0) {
+		ERROR("SCP BOOT_CMD_INFO returned error %u\n", response);
+		return -1;
+	}
+
+	VERBOSE("Transferring BL3-0 image to SCP\n");
 
-		scp_boot_message_end();
-	} while (response != BOOT_CMD_ACK);
+	/* Transfer BL3-0 image to SCP */
+	scp_boot_message_start();
 
-	/* Transfer image to SCP a block at a time */
-	uint32_t sequence_num = 1;
-	size_t size;
-	while ((size = end - offset) != 0) {
-		if (size > BOOT_DATA_MAX_SIZE)
-			size = BOOT_DATA_MAX_SIZE;
-		while (!transfer_block(sequence_num, offset, size))
-			; /* Retry forever */
-		offset += size;
-		sequence_num++;
+	BOM_CMD_HEADER->id = BOOT_CMD_DATA;
+	cmd_data_payload = BOM_CMD_PAYLOAD;
+	cmd_data_payload->offset = (uintptr_t) image - MHU_SECURE_BASE;
+	cmd_data_payload->block_size = image_size;
+
+	scp_boot_message_send(sizeof(*cmd_data_payload));
+	response = scp_boot_message_wait(sizeof(response));
+	scp_boot_message_end();
+
+	if (response != 0) {
+		ERROR("SCP BOOT_CMD_DATA returned error %u\n", response);
+		return -1;
 	}
 
+	VERBOSE("Waiting for SCP to signal it is ready to go on\n");
+
 	/* Wait for SCP to signal it's ready */
 	return scpi_wait_ready();
 }
diff --git a/plat/arm/css/common/css_scpi.c b/plat/arm/css/common/css_scpi.c
index 5bfa2cf..9127259 100644
--- a/plat/arm/css/common/css_scpi.c
+++ b/plat/arm/css/common/css_scpi.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2015, 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,112 +29,160 @@
  */
 
 #include <arch_helpers.h>
+#include <assert.h>
 #include <css_def.h>
+#include <debug.h>
 #include <platform.h>
+#include <string.h>
 #include "css_mhu.h"
 #include "css_scpi.h"
 
-#define MHU_SECURE_SCP_TO_AP_PAYLOAD	(MHU_SECURE_BASE+0x0080)
-#define MHU_SECURE_AP_TO_SCP_PAYLOAD	(MHU_SECURE_BASE+0x0280)
+#define SCPI_SHARED_MEM_SCP_TO_AP	(MHU_SECURE_BASE + 0x0080)
+#define SCPI_SHARED_MEM_AP_TO_SCP	(MHU_SECURE_BASE + 0x0180)
 
-#define SIZE_SHIFT	20	/* Bit position for size value in MHU header */
-#define SIZE_MASK	0x1ff	/* Mask to extract size value in MHU header*/
+#define SCPI_CMD_HEADER_AP_TO_SCP		\
+	((scpi_cmd_t *) SCPI_SHARED_MEM_AP_TO_SCP)
+#define SCPI_CMD_PAYLOAD_AP_TO_SCP		\
+	((void *) (SCPI_SHARED_MEM_AP_TO_SCP + sizeof(scpi_cmd_t)))
 
+/* ID of the MHU slot used for the SCPI protocol */
+#define SCPI_MHU_SLOT_ID		0
 
-void *scpi_secure_message_start(void)
+static void scpi_secure_message_start(void)
 {
-	mhu_secure_message_start();
-
-	/* Return address of payload area. */
-	return (void *)MHU_SECURE_AP_TO_SCP_PAYLOAD;
+	mhu_secure_message_start(SCPI_MHU_SLOT_ID);
 }
 
-void scpi_secure_message_send(unsigned command, size_t size)
+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(MHU_SECURE_AP_TO_SCP_PAYLOAD, size);
+		flush_dcache_range(SCPI_SHARED_MEM_AP_TO_SCP,
+				   sizeof(scpi_cmd_t) + payload_size);
 
-	mhu_secure_message_send(command | (size << SIZE_SHIFT));
+	mhu_secure_message_send(SCPI_MHU_SLOT_ID);
 }
 
-unsigned scpi_secure_message_receive(void **message_out, size_t *size_out)
+static void scpi_secure_message_receive(scpi_cmd_t *cmd)
 {
-	uint32_t response =  mhu_secure_message_wait();
+	uint32_t mhu_status;
+
+	assert(cmd != NULL);
 
-	/* Get size of payload */
-	size_t size = (response >> SIZE_SHIFT) & SIZE_MASK;
+	mhu_status = mhu_secure_message_wait();
 
-	/* Clear size from response */
-	response &= ~(SIZE_MASK << SIZE_SHIFT);
+	/* Expect an SCPI message, reject any other protocol */
+	if (mhu_status != (1 << SCPI_MHU_SLOT_ID)) {
+		ERROR("MHU: Unexpected protocol (MHU status: 0x%x)\n",
+			mhu_status);
+		panic();
+	}
 
 	/* Make sure we don't read stale data */
 	if (MHU_PAYLOAD_CACHED)
-		inv_dcache_range(MHU_SECURE_SCP_TO_AP_PAYLOAD, size);
-
-	if (size_out)
-		*size_out = size;
-
-	if (message_out)
-		*message_out = (void *)MHU_SECURE_SCP_TO_AP_PAYLOAD;
+		inv_dcache_range(SCPI_SHARED_MEM_SCP_TO_AP, sizeof(*cmd));
 
-	return response;
+	memcpy(cmd, (void *) SCPI_SHARED_MEM_SCP_TO_AP, sizeof(*cmd));
 }
 
-void scpi_secure_message_end(void)
+static void scpi_secure_message_end(void)
 {
-	mhu_secure_message_end();
-}
-
-static void scpi_secure_send32(unsigned command, uint32_t message)
-{
-	*(__typeof__(message) *)scpi_secure_message_start() = message;
-	scpi_secure_message_send(command, sizeof(message));
-	scpi_secure_message_end();
+	mhu_secure_message_end(SCPI_MHU_SLOT_ID);
 }
 
 int scpi_wait_ready(void)
 {
+	scpi_cmd_t scpi_cmd;
+
+	VERBOSE("Waiting for SCP_READY command...\n");
+
 	/* Get a message from the SCP */
 	scpi_secure_message_start();
-	size_t size;
-	unsigned command = scpi_secure_message_receive(NULL, &size);
+	scpi_secure_message_receive(&scpi_cmd);
 	scpi_secure_message_end();
 
 	/* We are expecting 'SCP Ready', produce correct error if it's not */
-	scpi_status_t response = SCP_OK;
-	if (command != SCPI_CMD_SCP_READY)
-		response = SCP_E_SUPPORT;
-	else if (size != 0)
-		response = SCP_E_SIZE;
-
-	/* Send our response back to SCP */
-	scpi_secure_send32(command, response);
+	scpi_status_t status = SCP_OK;
+	if (scpi_cmd.id != SCPI_CMD_SCP_READY) {
+		ERROR("Unexpected SCP command: expected command #%u, got command #%u\n",
+		      SCPI_CMD_SCP_READY, scpi_cmd.id);
+		status = SCP_E_SUPPORT;
+	} else if (scpi_cmd.size != 0) {
+		ERROR("SCP_READY command has incorrect size: expected 0, got %u\n",
+		      scpi_cmd.size);
+		status = SCP_E_SIZE;
+	}
+
+	VERBOSE("Sending response for SCP_READY command\n");
+
+	/*
+	 * Send our response back to SCP.
+	 * We are using the same SCPI header, just update the status field.
+	 */
+	scpi_cmd.status = status;
+	scpi_secure_message_start();
+	memcpy((void *) SCPI_SHARED_MEM_AP_TO_SCP, &scpi_cmd, sizeof(scpi_cmd));
+	scpi_secure_message_send(0);
+	scpi_secure_message_end();
 
-	return response == SCP_OK ? 0 : -1;
+	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)
 {
-	uint32_t state = mpidr & 0x0f;	/* CPU ID */
+	scpi_cmd_t *cmd;
+	uint32_t state = 0;
+	uint32_t *payload_addr;
+
+	state |= mpidr & 0x0f;	/* CPU ID */
 	state |= (mpidr & 0xf00) >> 4;	/* Cluster ID */
 	state |= cpu_state << 8;
 	state |= cluster_state << 12;
 	state |= css_state << 16;
-	scpi_secure_send32(SCPI_CMD_SET_CSS_POWER_STATE, state);
+
+	scpi_secure_message_start();
+
+	/* Populate the command header */
+	cmd = SCPI_CMD_HEADER_AP_TO_SCP;
+	cmd->id = SCPI_CMD_SET_CSS_POWER_STATE;
+	cmd->set = SCPI_SET_NORMAL;
+	cmd->sender = 0;
+	cmd->size = sizeof(state);
+	/* Populate the command payload */
+	payload_addr = SCPI_CMD_PAYLOAD_AP_TO_SCP;
+	*payload_addr = state;
+	scpi_secure_message_send(sizeof(state));
+	/*
+	 * SCP does not reply to this command in order to avoid MHU interrupts
+	 * from the sender, which could interfere with its power state request.
+	 */
+
+	scpi_secure_message_end();
 }
 
 uint32_t scpi_sys_power_state(scpi_system_state_t system_state)
 {
-	uint32_t *response;
-	size_t size;
-	uint8_t state = system_state & 0xff;
-
-	/* Send the command */
-	*(__typeof__(state) *)scpi_secure_message_start() = state;
-	scpi_secure_message_send(SCPI_CMD_SYS_POWER_STATE, sizeof(state));
-	scpi_secure_message_receive((void *)&response, &size);
+	scpi_cmd_t *cmd;
+	uint8_t *payload_addr;
+	scpi_cmd_t response;
+
+	scpi_secure_message_start();
+
+	/* Populate the command header */
+	cmd = SCPI_CMD_HEADER_AP_TO_SCP;
+	cmd->id = SCPI_CMD_SYS_POWER_STATE;
+	cmd->set = 0;
+	cmd->sender = 0;
+	cmd->size = sizeof(*payload_addr);
+	/* Populate the command payload */
+	payload_addr = SCPI_CMD_PAYLOAD_AP_TO_SCP;
+	*payload_addr = system_state & 0xff;
+	scpi_secure_message_send(sizeof(*payload_addr));
+
+	scpi_secure_message_receive(&response);
+
 	scpi_secure_message_end();
-	return *response;
+
+	return response.status;
 }
diff --git a/plat/arm/css/common/css_scpi.h b/plat/arm/css/common/css_scpi.h
index 965b36f..379a821 100644
--- a/plat/arm/css/common/css_scpi.h
+++ b/plat/arm/css/common/css_scpi.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-2015, 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,12 +34,31 @@
 #include <stddef.h>
 #include <stdint.h>
 
-extern void *scpi_secure_message_start(void);
-extern void scpi_secure_message_send(unsigned command, size_t size);
-extern unsigned scpi_secure_message_receive(void **message_out,
-						size_t *size_out);
-extern void scpi_secure_message_end(void);
+/*
+ * An SCPI command consists of a header and a payload.
+ * The following structure describes the header. It is 64-bit long.
+ */
+typedef struct {
+	/* Command ID */
+	uint32_t id		: 7;
+	/* Set ID. Identifies whether this is a standard or extended command. */
+	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) */
+	uint32_t size		: 9;
+	uint32_t reserved	: 7;
+	/*
+	 * Status indicating the success of a command.
+	 * See the enum below.
+	 */
+	uint32_t status;
+} scpi_cmd_t;
 
+typedef enum {
+	SCPI_SET_NORMAL = 0,	/* Normal SCPI commands */
+	SCPI_SET_EXTENDED	/* Extended SCPI commands */
+} scpi_set_t;
 
 enum {
 	SCP_OK = 0,	/* Success */
@@ -53,14 +72,16 @@ enum {
 	SCP_E_NOMEM,	/* Invalid memory area or pointer */
 	SCP_E_PWRSTATE,	/* Invalid power state */
 	SCP_E_SUPPORT,	/* Feature not supported or disabled */
+	SCPI_E_DEVICE,	/* Device error */
+	SCPI_E_BUSY,	/* Device is busy */
 };
 
 typedef uint32_t scpi_status_t;
 
 typedef enum {
 	SCPI_CMD_SCP_READY = 0x01,
-	SCPI_CMD_SET_CSS_POWER_STATE = 0x04,
-	SCPI_CMD_SYS_POWER_STATE = 0x08
+	SCPI_CMD_SET_CSS_POWER_STATE = 0x03,
+	SCPI_CMD_SYS_POWER_STATE = 0x05
 } scpi_command_t;
 
 typedef enum {