Move to the new ARM SCP Messaging Interfaces

The communication protocol used between the AP cores and the SCP
in CSS-based platforms like Juno has undergone a number of changes.
This patch makes the required modifications to the SCP Boot Protocol,
SCPI Protocol and MHU driver code in shared CSS platform code so that
the AP cores are still able to communicate with the SCP.

This patch focuses on the mandatory changes to make it work. The
design of this code needs to be improved but this will come in
a subsequent patch.

The main changes are:

 - MHU communication protocol

   - The command ID and payload size are no longer written into the
     MHU registers directly. Instead, they are stored in the payload
     area. The MHU registers are now used only as a doorbell to kick
     off messages. Same goes for any command result, the AP has to
     pick it up from the payload area.

 - SCP Boot Protocol

   - The BL3-0 image is now expected to embed a checksum. This
     checksum must be passed to the SCP, which uses it to check the
     integrity of the image it received.

   - The BL3-0 image used to be transferred a block (4KB)
     at a time. The SCP now supports receiving up to 128KB at a
     time, which is more than the size of the BL3-0 image.
     Therefore, the image is now sent in one go.

   - The command IDs have changed.

 - SCPI Protocol

   - The size of the SCPI payload has been reduced down from 512
     bytes to 256 bytes. This changes the base address of the
     AP-to-SCP payload area.

   - For commands that have a response, the response is the same SCPI
     header that was sent, except for the size and the status, which
     both must be updated appropriately. Success/Failure of a command
     is determined by looking at the updated status code.

   - Some command IDs have changed.

NOTE: THIS PATCH BREAKS COMPATIBILITY WITH FORMER VERSIONS OF THE SCP
FIRMWARE AND THUS REQUIRES AN UPDATE OF THIS BINARY. THE LATEST SCP
BINARY CAN BE OBTAINED FROM THE ARM CONNECTED COMMUNITY WEBSITE.

Change-Id: Ia5f6b95fe32401ee04a3805035748e8ef6718da7

6 files changed, 270 insertions, 156 deletions

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_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..c6d63f2 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,154 @@
  */
 
 #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"
 
-/* Boot commands sent from AP -> SCP */
-#define BOOT_CMD_START	0x01
-#define BOOT_CMD_DATA	0x02
+/* ID of the MHU slot used for the BOM protocol */
+#define BOM_MHU_SLOT_ID		0
 
-typedef struct {
-	uint32_t image_size;
-} cmd_start_payload;
+/* Boot commands sent from AP -> SCP */
+#define BOOT_CMD_INFO	0x00
+#define BOOT_CMD_DATA	0x01
 
+/* BOM command header */
 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
+	uint32_t id : 8;
+	uint32_t reserved : 24;
+} bom_cmd_t;
 
 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;
 
-	scp_boot_message_send(BOOT_CMD_DATA, sizeof(*cmd_data));
+	assert((uintptr_t) image == BL30_BASE);
 
-	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;
+	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;
+	}
 
-	scp_boot_message_end();
+	/* Extract the checksum from the image */
+	checksum = *(uint32_t *) image;
+	image = (char *) image + sizeof(checksum);
+	image_size -= sizeof(checksum);
 
-	return ok;
-}
+	mhu_secure_init();
 
-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;
+	VERBOSE("Send info about the BL3-0 image to be transferred to SCP\n");
 
-	mhu_secure_init();
+	/*
+	 * Send information about the SCP firmware image about to be transferred
+	 * to SCP
+	 */
+	scp_boot_message_start();
+
+	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));
+	response = scp_boot_message_wait(sizeof(response));
+	scp_boot_message_end();
 
-	/* Initiate communications with SCP */
-	do {
-		cmd_start_payload *cmd_start = scp_boot_message_start();
-		cmd_start->image_size = image_size;
+	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_send(BOOT_CMD_START, sizeof(*cmd_start));
+	/* Transfer BL3-0 image to SCP */
+	scp_boot_message_start();
 
-		response = scp_boot_message_wait(0);
+	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_end();
-	} while (response != BOOT_CMD_ACK);
+	scp_boot_message_send(sizeof(*cmd_data_payload));
+	response = scp_boot_message_wait(sizeof(response));
+	scp_boot_message_end();
 
-	/* 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++;
+	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 {