Adding upstream version 2.10.0+dfsg.upstream/2.10.0+dfsgupstream

Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>

11 files changed, 1873 insertions, 0 deletions

diff --git a/drivers/scmi-msg/base.c b/drivers/scmi-msg/base.c
new file mode 100644
index 0000000..52502a5
--- /dev/null
+++ b/drivers/scmi-msg/base.c
@@ -0,0 +1,194 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2022, Linaro Limited
+ */
+#include <assert.h>
+#include <string.h>
+
+#include <drivers/scmi-msg.h>
+#include <drivers/scmi.h>
+#include <lib/utils.h>
+#include <lib/utils_def.h>
+
+#include "common.h"
+
+static bool message_id_is_supported(unsigned int message_id);
+
+static void report_version(struct scmi_msg *msg)
+{
+	struct scmi_protocol_version_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		.version = SCMI_PROTOCOL_VERSION_BASE,
+	};
+
+	if (msg->in_size != 0U) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void report_attributes(struct scmi_msg *msg)
+{
+	size_t protocol_count = plat_scmi_protocol_count();
+	struct scmi_protocol_attributes_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		/* Null agent count since agent discovery is not supported */
+		.attributes = SCMI_BASE_PROTOCOL_ATTRIBUTES(protocol_count, 0U),
+	};
+
+	if (msg->in_size != 0U) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void report_message_attributes(struct scmi_msg *msg)
+{
+	struct scmi_protocol_message_attributes_a2p *in_args = (void *)msg->in;
+	struct scmi_protocol_message_attributes_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		/* For this protocol, attributes shall be zero */
+		.attributes = 0U,
+	};
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	if (!message_id_is_supported(in_args->message_id)) {
+		scmi_status_response(msg, SCMI_NOT_FOUND);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void discover_vendor(struct scmi_msg *msg)
+{
+	const char *name = plat_scmi_vendor_name();
+	struct scmi_base_discover_vendor_p2a return_values = {
+		.status = SCMI_SUCCESS,
+	};
+
+	if (msg->in_size != 0U) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	COPY_NAME_IDENTIFIER(return_values.vendor_identifier, name);
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void discover_sub_vendor(struct scmi_msg *msg)
+{
+	const char *name = plat_scmi_sub_vendor_name();
+	struct scmi_base_discover_sub_vendor_p2a return_values = {
+		.status = SCMI_SUCCESS,
+	};
+
+	if (msg->in_size != 0U) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	COPY_NAME_IDENTIFIER(return_values.sub_vendor_identifier, name);
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void discover_implementation_version(struct scmi_msg *msg)
+{
+	struct scmi_protocol_version_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		.version = SCMI_IMPL_VERSION,
+	};
+
+	if (msg->in_size != 0U) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static unsigned int count_protocols_in_list(const uint8_t *protocol_list)
+{
+	unsigned int count = 0U;
+
+	if (protocol_list != NULL) {
+		while (protocol_list[count] != 0U) {
+			count++;
+		}
+	}
+
+	return count;
+}
+
+static void discover_list_protocols(struct scmi_msg *msg)
+{
+	const struct scmi_base_discover_list_protocols_a2p *a2p = NULL;
+	struct scmi_base_discover_list_protocols_p2a p2a = {
+		.status = SCMI_SUCCESS,
+	};
+	const uint8_t *list = NULL;
+	unsigned int count = 0U;
+
+	if (msg->in_size != sizeof(*a2p)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	a2p = (void *)msg->in;
+
+	list = plat_scmi_protocol_list(msg->agent_id);
+	count = count_protocols_in_list(list);
+
+	if (count > a2p->skip) {
+		count = MIN((uint32_t)(count - a2p->skip),
+			    (uint32_t)(msg->out_size - sizeof(p2a)));
+	} else {
+		count = 0U;
+	}
+
+	p2a.num_protocols = count;
+
+	memcpy(msg->out, &p2a, sizeof(p2a));
+	memcpy(msg->out + sizeof(p2a), list + a2p->skip, count);
+	msg->out_size_out = sizeof(p2a) + round_up(count, sizeof(uint32_t));
+}
+
+static const scmi_msg_handler_t scmi_base_handler_table[] = {
+	[SCMI_PROTOCOL_VERSION] = report_version,
+	[SCMI_PROTOCOL_ATTRIBUTES] = report_attributes,
+	[SCMI_PROTOCOL_MESSAGE_ATTRIBUTES] = report_message_attributes,
+	[SCMI_BASE_DISCOVER_VENDOR] = discover_vendor,
+	[SCMI_BASE_DISCOVER_SUB_VENDOR] = discover_sub_vendor,
+	[SCMI_BASE_DISCOVER_IMPLEMENTATION_VERSION] =
+					discover_implementation_version,
+	[SCMI_BASE_DISCOVER_LIST_PROTOCOLS] = discover_list_protocols,
+};
+
+static bool message_id_is_supported(unsigned int message_id)
+{
+	return (message_id < ARRAY_SIZE(scmi_base_handler_table)) &&
+	       (scmi_base_handler_table[message_id] != NULL);
+}
+
+scmi_msg_handler_t scmi_msg_get_base_handler(struct scmi_msg *msg)
+{
+	unsigned int message_id = SPECULATION_SAFE_VALUE(msg->message_id);
+
+	if (message_id >= ARRAY_SIZE(scmi_base_handler_table)) {
+		VERBOSE("Base handle not found %u\n", msg->message_id);
+		return NULL;
+	}
+
+	return scmi_base_handler_table[message_id];
+}
diff --git a/drivers/scmi-msg/base.h b/drivers/scmi-msg/base.h
new file mode 100644
index 0000000..c4a9c64
--- /dev/null
+++ b/drivers/scmi-msg/base.h
@@ -0,0 +1,75 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2020, Linaro Limited
+ */
+
+#ifndef SCMI_MSG_BASE_H
+#define SCMI_MSG_BASE_H
+
+#include <stdint.h>
+
+#define SCMI_PROTOCOL_VERSION_BASE	0x20000U
+
+#define SCMI_DEFAULT_STRING_LENGTH	16U
+
+enum scmi_base_message_id {
+	SCMI_BASE_DISCOVER_VENDOR			= 0x003,
+	SCMI_BASE_DISCOVER_SUB_VENDOR			= 0x004,
+	SCMI_BASE_DISCOVER_IMPLEMENTATION_VERSION	= 0x005,
+	SCMI_BASE_DISCOVER_LIST_PROTOCOLS		= 0x006,
+	SCMI_BASE_DISCOVER_AGENT			= 0x007,
+	SCMI_BASE_NOTIFY_ERRORS				= 0x008,
+};
+
+/*
+ * PROTOCOL_ATTRIBUTES
+ */
+
+#define SCMI_BASE_PROTOCOL_ATTRS_NUM_PROTOCOLS_POS	0
+#define SCMI_BASE_PROTOCOL_ATTRS_NUM_AGENTS_POS		8
+
+#define SCMI_BASE_PROTOCOL_ATTRS_NUM_PROTOCOLS_MASK	0xFFU
+#define SCMI_BASE_PROTOCOL_ATTRS_NUM_AGENTS_MASK	0xFF00U
+
+#define SCMI_BASE_PROTOCOL_ATTRIBUTES(NUM_PROTOCOLS, NUM_AGENTS) \
+	((((NUM_PROTOCOLS) << SCMI_BASE_PROTOCOL_ATTRS_NUM_PROTOCOLS_POS) & \
+	  SCMI_BASE_PROTOCOL_ATTRS_NUM_PROTOCOLS_MASK) | \
+	(((NUM_AGENTS) << SCMI_BASE_PROTOCOL_ATTRS_NUM_AGENTS_POS) & \
+	 SCMI_BASE_PROTOCOL_ATTRS_NUM_AGENTS_MASK))
+
+/*
+ * BASE_DISCOVER_VENDOR
+ */
+struct scmi_base_discover_vendor_p2a {
+	int32_t status;
+	char vendor_identifier[SCMI_DEFAULT_STRING_LENGTH];
+};
+
+/*
+ * BASE_DISCOVER_SUB_VENDOR
+ */
+struct scmi_base_discover_sub_vendor_p2a {
+	int32_t status;
+	char sub_vendor_identifier[SCMI_DEFAULT_STRING_LENGTH];
+};
+
+/*
+ * BASE_DISCOVER_IMPLEMENTATION_VERSION
+ * No special structure right now, see protocol_version.
+ */
+
+/*
+ * BASE_DISCOVER_LIST_PROTOCOLS
+ */
+struct scmi_base_discover_list_protocols_a2p {
+	uint32_t skip;
+};
+
+struct scmi_base_discover_list_protocols_p2a {
+	int32_t status;
+	uint32_t num_protocols;
+	uint32_t protocols[];
+};
+
+#endif /* SCMI_MSG_BASE_H */
diff --git a/drivers/scmi-msg/clock.c b/drivers/scmi-msg/clock.c
new file mode 100644
index 0000000..5aaf68c
--- /dev/null
+++ b/drivers/scmi-msg/clock.c
@@ -0,0 +1,383 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2015-2020, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2020, Linaro Limited
+ */
+#include <cdefs.h>
+#include <string.h>
+
+#include <drivers/scmi-msg.h>
+#include <drivers/scmi.h>
+#include <lib/utils_def.h>
+
+#include "common.h"
+
+#pragma weak plat_scmi_clock_count
+#pragma weak plat_scmi_clock_get_name
+#pragma weak plat_scmi_clock_rates_array
+#pragma weak plat_scmi_clock_rates_by_step
+#pragma weak plat_scmi_clock_get_rate
+#pragma weak plat_scmi_clock_set_rate
+#pragma weak plat_scmi_clock_get_state
+#pragma weak plat_scmi_clock_set_state
+
+static bool message_id_is_supported(unsigned int message_id);
+
+size_t plat_scmi_clock_count(unsigned int agent_id __unused)
+{
+	return 0U;
+}
+
+const char *plat_scmi_clock_get_name(unsigned int agent_id __unused,
+				     unsigned int scmi_id __unused)
+{
+	return NULL;
+}
+
+int32_t plat_scmi_clock_rates_array(unsigned int agent_id __unused,
+				    unsigned int scmi_id __unused,
+				    unsigned long *rates __unused,
+				    size_t *nb_elts __unused,
+				    uint32_t start_idx __unused)
+{
+	return SCMI_NOT_SUPPORTED;
+}
+
+int32_t plat_scmi_clock_rates_by_step(unsigned int agent_id __unused,
+				      unsigned int scmi_id __unused,
+				      unsigned long *steps __unused)
+{
+	return SCMI_NOT_SUPPORTED;
+}
+
+unsigned long plat_scmi_clock_get_rate(unsigned int agent_id __unused,
+				       unsigned int scmi_id __unused)
+{
+	return 0U;
+}
+
+int32_t plat_scmi_clock_set_rate(unsigned int agent_id __unused,
+				 unsigned int scmi_id __unused,
+				 unsigned long rate __unused)
+{
+	return SCMI_NOT_SUPPORTED;
+}
+
+int32_t plat_scmi_clock_get_state(unsigned int agent_id __unused,
+				  unsigned int scmi_id __unused)
+{
+	return SCMI_NOT_SUPPORTED;
+}
+
+int32_t plat_scmi_clock_set_state(unsigned int agent_id __unused,
+				  unsigned int scmi_id __unused,
+				  bool enable_not_disable __unused)
+{
+	return SCMI_NOT_SUPPORTED;
+}
+
+static void report_version(struct scmi_msg *msg)
+{
+	struct scmi_protocol_version_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		.version = SCMI_PROTOCOL_VERSION_CLOCK,
+	};
+
+	if (msg->in_size != 0) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void report_attributes(struct scmi_msg *msg)
+{
+	size_t agent_count = plat_scmi_clock_count(msg->agent_id);
+	struct scmi_protocol_attributes_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		.attributes = SCMI_CLOCK_PROTOCOL_ATTRIBUTES(1U, agent_count),
+	};
+
+	if (msg->in_size != 0) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void report_message_attributes(struct scmi_msg *msg)
+{
+	struct scmi_protocol_message_attributes_a2p *in_args = (void *)msg->in;
+	struct scmi_protocol_message_attributes_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		/* For this protocol, attributes shall be zero */
+		.attributes = 0U,
+	};
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	if (!message_id_is_supported(in_args->message_id)) {
+		scmi_status_response(msg, SCMI_NOT_FOUND);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void scmi_clock_attributes(struct scmi_msg *msg)
+{
+	const struct scmi_clock_attributes_a2p *in_args = (void *)msg->in;
+	struct scmi_clock_attributes_p2a return_values = {
+		.status = SCMI_SUCCESS,
+	};
+	const char *name = NULL;
+	unsigned int clock_id = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
+
+	if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+
+	name = plat_scmi_clock_get_name(msg->agent_id, clock_id);
+	if (name == NULL) {
+		scmi_status_response(msg, SCMI_NOT_FOUND);
+		return;
+	}
+
+	COPY_NAME_IDENTIFIER(return_values.clock_name, name);
+
+	return_values.attributes = plat_scmi_clock_get_state(msg->agent_id,
+							     clock_id);
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void scmi_clock_rate_get(struct scmi_msg *msg)
+{
+	const struct scmi_clock_rate_get_a2p *in_args = (void *)msg->in;
+	unsigned long rate = 0U;
+	struct scmi_clock_rate_get_p2a return_values = {
+		.status = SCMI_SUCCESS,
+	};
+	unsigned int clock_id = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
+
+	if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+	rate = plat_scmi_clock_get_rate(msg->agent_id, clock_id);
+
+	return_values.rate[0] = (uint32_t)rate;
+	return_values.rate[1] = (uint32_t)((uint64_t)rate >> 32);
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void scmi_clock_rate_set(struct scmi_msg *msg)
+{
+	const struct scmi_clock_rate_set_a2p *in_args = (void *)msg->in;
+	unsigned long rate = 0U;
+	int32_t status = 0;
+	unsigned int clock_id = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
+
+	if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+	rate = (unsigned long)(((uint64_t)in_args->rate[1] << 32) |
+			       in_args->rate[0]);
+
+	status = plat_scmi_clock_set_rate(msg->agent_id, clock_id, rate);
+
+	scmi_status_response(msg, status);
+}
+
+static void scmi_clock_config_set(struct scmi_msg *msg)
+{
+	const struct scmi_clock_config_set_a2p *in_args = (void *)msg->in;
+	int32_t status = SCMI_GENERIC_ERROR;
+	bool enable = false;
+	unsigned int clock_id = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
+
+	if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+	enable = in_args->attributes & SCMI_CLOCK_CONFIG_SET_ENABLE_MASK;
+
+	status = plat_scmi_clock_set_state(msg->agent_id, clock_id, enable);
+
+	scmi_status_response(msg, status);
+}
+
+#define RATES_ARRAY_SIZE_MAX	(SCMI_PLAYLOAD_MAX - \
+				 sizeof(struct scmi_clock_describe_rates_p2a))
+
+#define SCMI_RATES_BY_ARRAY(_nb_rates, _rem_rates) \
+	SCMI_CLOCK_DESCRIBE_RATES_NUM_RATES_FLAGS((_nb_rates), \
+						SCMI_CLOCK_RATE_FORMAT_LIST, \
+						(_rem_rates))
+#define SCMI_RATES_BY_STEP \
+	SCMI_CLOCK_DESCRIBE_RATES_NUM_RATES_FLAGS(3U, \
+						SCMI_CLOCK_RATE_FORMAT_RANGE, \
+						0U)
+
+#define RATE_DESC_SIZE		sizeof(struct scmi_clock_rate)
+
+static void write_rate_desc_array_in_buffer(char *dest, unsigned long *rates,
+					    size_t nb_elt)
+{
+	uint32_t *out = (uint32_t *)(uintptr_t)dest;
+	size_t n;
+
+	ASSERT_SYM_PTR_ALIGN(out);
+
+	for (n = 0U; n < nb_elt; n++) {
+		out[2 * n] = (uint32_t)rates[n];
+		out[2 * n + 1] = (uint32_t)((uint64_t)rates[n] >> 32);
+	}
+}
+
+static void scmi_clock_describe_rates(struct scmi_msg *msg)
+{
+	const struct scmi_clock_describe_rates_a2p *in_args = (void *)msg->in;
+	struct scmi_clock_describe_rates_p2a p2a = {
+		.status = SCMI_SUCCESS,
+	};
+	size_t nb_rates;
+	int32_t status;
+	unsigned int clock_id;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	clock_id = SPECULATION_SAFE_VALUE(in_args->clock_id);
+
+	if (clock_id >= plat_scmi_clock_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+	/* Platform may support array rate description */
+	status = plat_scmi_clock_rates_array(msg->agent_id, clock_id, NULL,
+					     &nb_rates, 0);
+	if (status == SCMI_SUCCESS) {
+		/* Currently 12 cells mex, so it's affordable for the stack */
+		unsigned long plat_rates[RATES_ARRAY_SIZE_MAX / RATE_DESC_SIZE];
+		size_t max_nb = RATES_ARRAY_SIZE_MAX / RATE_DESC_SIZE;
+		size_t ret_nb = MIN(nb_rates - in_args->rate_index, max_nb);
+		size_t rem_nb = nb_rates - in_args->rate_index - ret_nb;
+
+		status =  plat_scmi_clock_rates_array(msg->agent_id, clock_id,
+						      plat_rates, &ret_nb,
+						      in_args->rate_index);
+		if (status == SCMI_SUCCESS) {
+			write_rate_desc_array_in_buffer(msg->out + sizeof(p2a),
+							plat_rates, ret_nb);
+
+			p2a.num_rates_flags = SCMI_RATES_BY_ARRAY(ret_nb,
+								  rem_nb);
+			p2a.status = SCMI_SUCCESS;
+
+			memcpy(msg->out, &p2a, sizeof(p2a));
+			msg->out_size_out = sizeof(p2a) +
+					    ret_nb * RATE_DESC_SIZE;
+		}
+	} else if (status == SCMI_NOT_SUPPORTED) {
+		unsigned long triplet[3] = { 0U, 0U, 0U };
+
+		/* Platform may support min§max/step triplet description */
+		status =  plat_scmi_clock_rates_by_step(msg->agent_id, clock_id,
+							triplet);
+		if (status == SCMI_SUCCESS) {
+			write_rate_desc_array_in_buffer(msg->out + sizeof(p2a),
+							triplet, 3U);
+
+			p2a.num_rates_flags = SCMI_RATES_BY_STEP;
+			p2a.status = SCMI_SUCCESS;
+
+			memcpy(msg->out, &p2a, sizeof(p2a));
+			msg->out_size_out = sizeof(p2a) + (3U * RATE_DESC_SIZE);
+		}
+	} else {
+		/* Fallthrough generic exit sequence below with error status */
+	}
+
+	if (status != SCMI_SUCCESS) {
+		scmi_status_response(msg, status);
+	} else {
+		/*
+		 * Message payload is already written to msg->out, and
+		 * msg->out_size_out updated.
+		 */
+	}
+}
+
+static const scmi_msg_handler_t scmi_clock_handler_table[] = {
+	[SCMI_PROTOCOL_VERSION] = report_version,
+	[SCMI_PROTOCOL_ATTRIBUTES] = report_attributes,
+	[SCMI_PROTOCOL_MESSAGE_ATTRIBUTES] = report_message_attributes,
+	[SCMI_CLOCK_ATTRIBUTES] = scmi_clock_attributes,
+	[SCMI_CLOCK_DESCRIBE_RATES] = scmi_clock_describe_rates,
+	[SCMI_CLOCK_RATE_SET] = scmi_clock_rate_set,
+	[SCMI_CLOCK_RATE_GET] = scmi_clock_rate_get,
+	[SCMI_CLOCK_CONFIG_SET] = scmi_clock_config_set,
+};
+
+static bool message_id_is_supported(unsigned int message_id)
+{
+	return (message_id < ARRAY_SIZE(scmi_clock_handler_table)) &&
+	       (scmi_clock_handler_table[message_id] != NULL);
+}
+
+scmi_msg_handler_t scmi_msg_get_clock_handler(struct scmi_msg *msg)
+{
+	const size_t array_size = ARRAY_SIZE(scmi_clock_handler_table);
+	unsigned int message_id = SPECULATION_SAFE_VALUE(msg->message_id);
+
+	if (message_id >= array_size) {
+		VERBOSE("Clock handle not found %u", msg->message_id);
+		return NULL;
+	}
+
+	return scmi_clock_handler_table[message_id];
+}
diff --git a/drivers/scmi-msg/clock.h b/drivers/scmi-msg/clock.h
new file mode 100644
index 0000000..a637934
--- /dev/null
+++ b/drivers/scmi-msg/clock.h
@@ -0,0 +1,150 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019, Linaro Limited
+ */
+
+#ifndef SCMI_MSG_CLOCK_H
+#define SCMI_MSG_CLOCK_H
+
+#include <stdint.h>
+
+#include <lib/utils_def.h>
+
+#define SCMI_PROTOCOL_VERSION_CLOCK	0x20000U
+
+/*
+ * Identifiers of the SCMI Clock Management Protocol commands
+ */
+enum scmi_clock_command_id {
+	SCMI_CLOCK_ATTRIBUTES = 0x003,
+	SCMI_CLOCK_DESCRIBE_RATES = 0x004,
+	SCMI_CLOCK_RATE_SET = 0x005,
+	SCMI_CLOCK_RATE_GET = 0x006,
+	SCMI_CLOCK_CONFIG_SET = 0x007,
+};
+
+/* Protocol attributes */
+#define SCMI_CLOCK_CLOCK_COUNT_MASK			GENMASK(15, 0)
+#define SCMI_CLOCK_MAX_PENDING_TRANSITIONS_MASK		GENMASK(23, 16)
+
+#define SCMI_CLOCK_PROTOCOL_ATTRIBUTES(_max_pending, _clk_count) \
+	((((_max_pending) << 16) & SCMI_CLOCK_MAX_PENDING_TRANSITIONS_MASK) | \
+	 (((_clk_count) & SCMI_CLOCK_CLOCK_COUNT_MASK)))
+
+struct scmi_clock_attributes_a2p {
+	uint32_t clock_id;
+};
+
+#define SCMI_CLOCK_NAME_LENGTH_MAX	16U
+
+struct scmi_clock_attributes_p2a {
+	int32_t status;
+	uint32_t attributes;
+	char clock_name[SCMI_CLOCK_NAME_LENGTH_MAX];
+};
+
+/*
+ * Clock Rate Get
+ */
+
+struct scmi_clock_rate_get_a2p {
+	uint32_t clock_id;
+};
+
+struct scmi_clock_rate_get_p2a {
+	int32_t status;
+	uint32_t rate[2];
+};
+
+/*
+ * Clock Rate Set
+ */
+
+/* If set, set the new clock rate asynchronously */
+#define SCMI_CLOCK_RATE_SET_ASYNC_POS			0
+/* If set, do not send a delayed asynchronous response */
+#define SCMI_CLOCK_RATE_SET_NO_DELAYED_RESPONSE_POS	1
+/* Round up, if set, otherwise round down */
+#define SCMI_CLOCK_RATE_SET_ROUND_UP_POS		2
+/* If set, the platform chooses the appropriate rounding mode */
+#define SCMI_CLOCK_RATE_SET_ROUND_AUTO_POS		3
+
+#define SCMI_CLOCK_RATE_SET_ASYNC_MASK \
+		BIT(SCMI_CLOCK_RATE_SET_ASYNC_POS)
+#define SCMI_CLOCK_RATE_SET_NO_DELAYED_RESPONSE_MASK \
+		BIT(SCMI_CLOCK_RATE_SET_NO_DELAYED_RESPONSE_POS)
+#define SCMI_CLOCK_RATE_SET_ROUND_UP_MASK \
+		BIT(SCMI_CLOCK_RATE_SET_ROUND_UP_POS)
+#define SCMI_CLOCK_RATE_SET_ROUND_AUTO_MASK \
+		BIT(SCMI_CLOCK_RATE_SET_ROUND_AUTO_POS)
+
+struct scmi_clock_rate_set_a2p {
+	uint32_t flags;
+	uint32_t clock_id;
+	uint32_t rate[2];
+};
+
+struct scmi_clock_rate_set_p2a {
+	int32_t status;
+};
+
+/*
+ * Clock Config Set
+ */
+
+#define SCMI_CLOCK_CONFIG_SET_ENABLE_POS	0
+
+#define SCMI_CLOCK_CONFIG_SET_ENABLE_MASK \
+	BIT(SCMI_CLOCK_CONFIG_SET_ENABLE_POS)
+
+struct scmi_clock_config_set_a2p {
+	uint32_t clock_id;
+	uint32_t attributes;
+};
+
+struct scmi_clock_config_set_p2a {
+	int32_t status;
+};
+
+/*
+ * Clock Describe Rates
+ */
+
+#define SCMI_CLOCK_RATE_FORMAT_RANGE			1U
+#define SCMI_CLOCK_RATE_FORMAT_LIST			0U
+
+#define SCMI_CLOCK_DESCRIBE_RATES_REMAINING_MASK	GENMASK_32(31, 16)
+#define SCMI_CLOCK_DESCRIBE_RATES_REMAINING_POS		16
+
+#define SCMI_CLOCK_DESCRIBE_RATES_FORMAT_MASK		BIT(12)
+#define SCMI_CLOCK_DESCRIBE_RATES_FORMAT_POS		12
+
+#define SCMI_CLOCK_DESCRIBE_RATES_COUNT_MASK		GENMASK_32(11, 0)
+
+#define SCMI_CLOCK_DESCRIBE_RATES_NUM_RATES_FLAGS(_count, _fmt, _rem_rates) \
+	( \
+		((_count) & SCMI_CLOCK_DESCRIBE_RATES_COUNT_MASK) | \
+		(((_rem_rates) << SCMI_CLOCK_DESCRIBE_RATES_REMAINING_POS) & \
+		 SCMI_CLOCK_DESCRIBE_RATES_REMAINING_MASK) | \
+		(((_fmt) << SCMI_CLOCK_DESCRIBE_RATES_FORMAT_POS) & \
+		 SCMI_CLOCK_DESCRIBE_RATES_FORMAT_MASK) \
+	)
+
+struct scmi_clock_rate {
+	uint32_t low;
+	uint32_t high;
+};
+
+struct scmi_clock_describe_rates_a2p {
+	uint32_t clock_id;
+	uint32_t rate_index;
+};
+
+struct scmi_clock_describe_rates_p2a {
+	int32_t status;
+	uint32_t num_rates_flags;
+	struct scmi_clock_rate rates[];
+};
+
+#endif /* SCMI_MSG_CLOCK_H */
diff --git a/drivers/scmi-msg/common.h b/drivers/scmi-msg/common.h
new file mode 100644
index 0000000..62f3087
--- /dev/null
+++ b/drivers/scmi-msg/common.h
@@ -0,0 +1,144 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2020, Linaro Limited
+ */
+#ifndef SCMI_MSG_COMMON_H
+#define SCMI_MSG_COMMON_H
+
+#include <assert.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "base.h"
+#include "clock.h"
+#include "power_domain.h"
+#include "reset_domain.h"
+
+#define SCMI_VERSION			0x20000U
+#define SCMI_IMPL_VERSION		0U
+
+#define SCMI_PLAYLOAD_MAX		92U
+
+/*
+ * Copy name identifier in target buffer following the SCMI specification
+ * that state name identifier shall be a null terminated string.
+ */
+#define COPY_NAME_IDENTIFIER(_dst_array, _name)				\
+	do {								\
+		assert(strlen(_name) < sizeof(_dst_array));		\
+		strlcpy((_dst_array), (_name), sizeof(_dst_array));	\
+	} while (0)
+
+/* Common command identifiers shared by all procotols */
+enum scmi_common_message_id {
+	SCMI_PROTOCOL_VERSION = 0x000,
+	SCMI_PROTOCOL_ATTRIBUTES = 0x001,
+	SCMI_PROTOCOL_MESSAGE_ATTRIBUTES = 0x002
+};
+
+/* Common platform-to-agent (p2a) PROTOCOL_VERSION structure */
+struct scmi_protocol_version_p2a {
+	int32_t status;
+	uint32_t version;
+};
+
+/* Generic platform-to-agent (p2a) PROTOCOL_ATTRIBUTES structure */
+struct scmi_protocol_attributes_p2a {
+	int32_t status;
+	uint32_t attributes;
+};
+
+/* Generic agent-to-platform (a2p) PROTOCOL_MESSAGE_ATTRIBUTES structure */
+struct scmi_protocol_message_attributes_a2p {
+	uint32_t message_id;
+};
+
+/* Generic platform-to-agent (p2a) PROTOCOL_MESSAGE_ATTRIBUTES structure */
+struct scmi_protocol_message_attributes_p2a {
+	int32_t status;
+	uint32_t attributes;
+};
+
+/*
+ * struct scmi_msg - SCMI message context
+ *
+ * @agent_id: SCMI agent ID, safely set from secure world
+ * @protocol_id: SCMI protocol ID for the related message, set by caller agent
+ * @message_id: SCMI message ID for the related message, set by caller agent
+ * @in: Address of the incoming message payload copied in secure memory
+ * @in_size: Byte length of the incoming message payload, set by caller agent
+ * @out: Address of of the output message payload message in non-secure memory
+ * @out_size: Byte length of the provisionned output buffer
+ * @out_size_out: Byte length of the output message payload
+ */
+struct scmi_msg {
+	unsigned int agent_id;
+	unsigned int protocol_id;
+	unsigned int message_id;
+	char *in;
+	size_t in_size;
+	char *out;
+	size_t out_size;
+	size_t out_size_out;
+};
+
+/*
+ * Type scmi_msg_handler_t is used by procotol drivers to safely find
+ * the handler function for the incoming message ID.
+ */
+typedef void (*scmi_msg_handler_t)(struct scmi_msg *msg);
+
+/*
+ * scmi_msg_get_base_handler - Return a handler for a base message
+ * @msg - message to process
+ * Return a function handler for the message or NULL
+ */
+scmi_msg_handler_t scmi_msg_get_base_handler(struct scmi_msg *msg);
+
+/*
+ * scmi_msg_get_clock_handler - Return a handler for a clock message
+ * @msg - message to process
+ * Return a function handler for the message or NULL
+ */
+scmi_msg_handler_t scmi_msg_get_clock_handler(struct scmi_msg *msg);
+
+/*
+ * scmi_msg_get_rstd_handler - Return a handler for a reset domain message
+ * @msg - message to process
+ * Return a function handler for the message or NULL
+ */
+scmi_msg_handler_t scmi_msg_get_rstd_handler(struct scmi_msg *msg);
+
+/*
+ * scmi_msg_get_pd_handler - Return a handler for a power domain message
+ * @msg - message to process
+ * Return a function handler for the message or NULL
+ */
+scmi_msg_handler_t scmi_msg_get_pd_handler(struct scmi_msg *msg);
+
+/*
+ * Process Read, process and write response for input SCMI message
+ *
+ * @msg: SCMI message context
+ */
+void scmi_process_message(struct scmi_msg *msg);
+
+/*
+ * Write SCMI response payload to output message shared memory
+ *
+ * @msg: SCMI message context
+ * @payload: Output message payload
+ * @size: Byte size of output message payload
+ */
+void scmi_write_response(struct scmi_msg *msg, void *payload, size_t size);
+
+/*
+ * Write status only SCMI response payload to output message shared memory
+ *
+ * @msg: SCMI message context
+ * @status: SCMI status value returned to caller
+ */
+void scmi_status_response(struct scmi_msg *msg, int32_t status);
+#endif /* SCMI_MSG_COMMON_H */
diff --git a/drivers/scmi-msg/entry.c b/drivers/scmi-msg/entry.c
new file mode 100644
index 0000000..399115c
--- /dev/null
+++ b/drivers/scmi-msg/entry.c
@@ -0,0 +1,91 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2020, Linaro Limited
+ */
+
+#include <assert.h>
+
+#include <drivers/scmi-msg.h>
+#include <drivers/scmi.h>
+
+#include "common.h"
+
+#pragma weak scmi_msg_get_clock_handler
+#pragma weak scmi_msg_get_rstd_handler
+#pragma weak scmi_msg_get_pd_handler
+#pragma weak scmi_msg_get_voltage_handler
+
+scmi_msg_handler_t scmi_msg_get_clock_handler(struct scmi_msg *msg __unused)
+{
+	return NULL;
+}
+
+scmi_msg_handler_t scmi_msg_get_rstd_handler(struct scmi_msg *msg __unused)
+{
+	return NULL;
+}
+
+scmi_msg_handler_t scmi_msg_get_pd_handler(struct scmi_msg *msg __unused)
+{
+	return NULL;
+}
+
+scmi_msg_handler_t scmi_msg_get_voltage_handler(struct scmi_msg *msg __unused)
+{
+	return NULL;
+}
+
+void scmi_status_response(struct scmi_msg *msg, int32_t status)
+{
+	assert(msg->out && msg->out_size >= sizeof(int32_t));
+
+	memcpy(msg->out, &status, sizeof(int32_t));
+	msg->out_size_out = sizeof(int32_t);
+}
+
+void scmi_write_response(struct scmi_msg *msg, void *payload, size_t size)
+{
+	/*
+	 * Output payload shall be at least the size of the status
+	 * Output buffer shall be at least be the size of the status
+	 * Output paylaod shall fit in output buffer
+	 */
+	assert(payload && size >= sizeof(int32_t) && size <= msg->out_size &&
+	       msg->out && msg->out_size >= sizeof(int32_t));
+
+	memcpy(msg->out, payload, size);
+	msg->out_size_out = size;
+}
+
+void scmi_process_message(struct scmi_msg *msg)
+{
+	scmi_msg_handler_t handler = NULL;
+
+	switch (msg->protocol_id) {
+	case SCMI_PROTOCOL_ID_BASE:
+		handler = scmi_msg_get_base_handler(msg);
+		break;
+	case SCMI_PROTOCOL_ID_CLOCK:
+		handler = scmi_msg_get_clock_handler(msg);
+		break;
+	case SCMI_PROTOCOL_ID_RESET_DOMAIN:
+		handler = scmi_msg_get_rstd_handler(msg);
+		break;
+	case SCMI_PROTOCOL_ID_POWER_DOMAIN:
+		handler = scmi_msg_get_pd_handler(msg);
+		break;
+	default:
+		break;
+	}
+
+	if (handler) {
+		handler(msg);
+		return;
+	}
+
+	ERROR("Agent %u Protocol 0x%x Message 0x%x: not supported\n",
+	      msg->agent_id, msg->protocol_id, msg->message_id);
+
+	scmi_status_response(msg, SCMI_NOT_SUPPORTED);
+}
diff --git a/drivers/scmi-msg/power_domain.c b/drivers/scmi-msg/power_domain.c
new file mode 100644
index 0000000..87c41dd
--- /dev/null
+++ b/drivers/scmi-msg/power_domain.c
@@ -0,0 +1,239 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2015-2020, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2020, Linaro Limited
+ */
+#include <cdefs.h>
+#include <string.h>
+
+#include <drivers/scmi-msg.h>
+#include <drivers/scmi.h>
+#include <lib/utils_def.h>
+
+#include "common.h"
+
+#pragma weak plat_scmi_pd_count
+#pragma weak plat_scmi_pd_get_name
+#pragma weak plat_scmi_pd_get_state
+#pragma weak plat_scmi_pd_set_state
+#pragma weak plat_scmi_pd_statistics
+#pragma weak plat_scmi_pd_get_attributes
+
+static bool message_id_is_supported(unsigned int message_id);
+
+size_t plat_scmi_pd_count(unsigned int agent_id __unused)
+{
+	return 0U;
+}
+
+const char *plat_scmi_pd_get_name(unsigned int agent_id __unused,
+				  unsigned int pd_id __unused)
+{
+	return NULL;
+}
+
+unsigned int plat_scmi_pd_statistics(unsigned int agent_id __unused,
+				     unsigned long *pd_id __unused)
+{
+	return 0U;
+}
+
+unsigned int plat_scmi_pd_get_attributes(unsigned int agent_id __unused,
+					 unsigned int pd_id __unused)
+{
+	return 0U;
+}
+
+unsigned int plat_scmi_pd_get_state(unsigned int agent_id __unused,
+				    unsigned int pd_id __unused)
+{
+	return 0U;
+}
+
+int32_t plat_scmi_pd_set_state(unsigned int agent_id __unused,
+			       unsigned int flags __unused,
+			       unsigned int pd_id __unused,
+			       unsigned int state __unused)
+{
+	return SCMI_NOT_SUPPORTED;
+}
+
+static void report_version(struct scmi_msg *msg)
+{
+	struct scmi_protocol_version_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		.version = SCMI_PROTOCOL_VERSION_PD,
+	};
+
+	if (msg->in_size != 0) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void report_attributes(struct scmi_msg *msg)
+{
+	unsigned long addr = 0UL;
+	unsigned int len;
+
+	struct scmi_protocol_attributes_p2a_pd return_values = {
+		.status = SCMI_SUCCESS,
+	};
+
+	if (msg->in_size != 0) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	return_values.attributes = plat_scmi_pd_count(msg->agent_id);
+	len = plat_scmi_pd_statistics(msg->agent_id, &addr);
+	if (len != 0U) {
+		return_values.statistics_addr_low = (unsigned int)addr;
+		return_values.statistics_addr_high = (uint32_t)(addr >> 32);
+		return_values.statistics_len = len;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void report_message_attributes(struct scmi_msg *msg)
+{
+	struct scmi_protocol_message_attributes_a2p *in_args = (void *)msg->in;
+	struct scmi_protocol_message_attributes_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		/* For this protocol, attributes shall be zero */
+		.attributes = 0U,
+	};
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	if (!message_id_is_supported(in_args->message_id)) {
+		scmi_status_response(msg, SCMI_NOT_FOUND);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void scmi_pd_attributes(struct scmi_msg *msg)
+{
+	const struct scmi_pd_attributes_a2p *in_args = (void *)msg->in;
+	struct scmi_pd_attributes_p2a return_values = {
+		.status = SCMI_SUCCESS,
+	};
+	const char *name = NULL;
+	unsigned int pd_id = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	pd_id = SPECULATION_SAFE_VALUE(in_args->pd_id);
+
+	if (pd_id >= plat_scmi_pd_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+	name = plat_scmi_pd_get_name(msg->agent_id, pd_id);
+	if (name == NULL) {
+		scmi_status_response(msg, SCMI_NOT_FOUND);
+		return;
+	}
+
+	COPY_NAME_IDENTIFIER(return_values.pd_name, name);
+
+	return_values.attributes = plat_scmi_pd_get_attributes(msg->agent_id, pd_id);
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void scmi_pd_state_get(struct scmi_msg *msg)
+{
+	const struct scmi_pd_state_get_a2p *in_args = (void *)msg->in;
+	unsigned int state = 0U;
+	struct scmi_pd_state_get_p2a return_values = {
+		.status = SCMI_SUCCESS,
+	};
+	unsigned int pd_id = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	pd_id = SPECULATION_SAFE_VALUE(in_args->pd_id);
+
+	if (pd_id >= plat_scmi_pd_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+	state = plat_scmi_pd_get_state(msg->agent_id, pd_id);
+
+	return_values.power_state = state;
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void scmi_pd_state_set(struct scmi_msg *msg)
+{
+	const struct scmi_pd_state_set_a2p *in_args = (void *)msg->in;
+	unsigned int flags = 0U;
+	int32_t status = 0;
+	unsigned int pd_id = 0U;
+	unsigned int state = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	pd_id = SPECULATION_SAFE_VALUE(in_args->pd_id);
+
+	if (pd_id >= plat_scmi_pd_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+	flags = SPECULATION_SAFE_VALUE(in_args->flags);
+	state = SPECULATION_SAFE_VALUE(in_args->power_state);
+
+	status = plat_scmi_pd_set_state(msg->agent_id, flags, pd_id, state);
+
+	scmi_status_response(msg, status);
+}
+
+static const scmi_msg_handler_t scmi_pd_handler_table[] = {
+	[SCMI_PROTOCOL_VERSION] = report_version,
+	[SCMI_PROTOCOL_ATTRIBUTES] = report_attributes,
+	[SCMI_PROTOCOL_MESSAGE_ATTRIBUTES] = report_message_attributes,
+	[SCMI_PD_ATTRIBUTES] = scmi_pd_attributes,
+	[SCMI_PD_STATE_SET] = scmi_pd_state_set,
+	[SCMI_PD_STATE_GET] = scmi_pd_state_get,
+};
+
+static bool message_id_is_supported(unsigned int message_id)
+{
+	return (message_id < ARRAY_SIZE(scmi_pd_handler_table)) &&
+	       (scmi_pd_handler_table[message_id] != NULL);
+}
+
+scmi_msg_handler_t scmi_msg_get_pd_handler(struct scmi_msg *msg)
+{
+	const size_t array_size = ARRAY_SIZE(scmi_pd_handler_table);
+	unsigned int message_id = SPECULATION_SAFE_VALUE(msg->message_id);
+
+	if (message_id >= array_size) {
+		VERBOSE("pd handle not found %u", msg->message_id);
+		return NULL;
+	}
+
+	return scmi_pd_handler_table[message_id];
+}
diff --git a/drivers/scmi-msg/power_domain.h b/drivers/scmi-msg/power_domain.h
new file mode 100644
index 0000000..48551fd
--- /dev/null
+++ b/drivers/scmi-msg/power_domain.h
@@ -0,0 +1,72 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright 2021 NXP
+ */
+
+#ifndef SCMI_MSG_PD_H
+#define SCMI_MSG_PD_H
+
+#include <stdint.h>
+
+#include <lib/utils_def.h>
+
+#define SCMI_PROTOCOL_VERSION_PD	0x21000U
+
+/*
+ * Identifiers of the SCMI POWER DOMAIN Protocol commands
+ */
+enum scmi_pd_command_id {
+	SCMI_PD_ATTRIBUTES = 0x003,
+	SCMI_PD_STATE_SET = 0x004,
+	SCMI_PD_STATE_GET = 0x005,
+};
+
+/* Protocol attributes */
+struct scmi_pd_attributes_a2p {
+	uint32_t pd_id;
+};
+
+struct scmi_protocol_attributes_p2a_pd {
+	int32_t status;
+	uint32_t attributes;
+	uint32_t statistics_addr_low;
+	uint32_t statistics_addr_high;
+	uint32_t statistics_len;
+};
+
+#define SCMI_PD_NAME_LENGTH_MAX	16U
+
+struct scmi_pd_attributes_p2a {
+	int32_t status;
+	uint32_t attributes;
+	char pd_name[SCMI_PD_NAME_LENGTH_MAX];
+};
+
+/*
+ * Power Domain State Get
+ */
+
+struct scmi_pd_state_get_a2p {
+	uint32_t pd_id;
+};
+
+struct scmi_pd_state_get_p2a {
+	int32_t status;
+	uint32_t power_state;
+};
+
+/*
+ * Power domain State Set
+ */
+
+struct scmi_pd_state_set_a2p {
+	uint32_t flags;
+	uint32_t pd_id;
+	uint32_t power_state;
+};
+
+struct scmi_pd_state_set_p2a {
+	int32_t status;
+};
+
+#endif /* SCMI_MSG_PD_H */
diff --git a/drivers/scmi-msg/reset_domain.c b/drivers/scmi-msg/reset_domain.c
new file mode 100644
index 0000000..76ac47e
--- /dev/null
+++ b/drivers/scmi-msg/reset_domain.c
@@ -0,0 +1,197 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2015-2020, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2020, Linaro Limited
+ */
+#include <cdefs.h>
+#include <string.h>
+
+#include <drivers/scmi-msg.h>
+#include <drivers/scmi.h>
+#include <lib/utils.h>
+#include <lib/utils_def.h>
+
+#include "common.h"
+
+static bool message_id_is_supported(unsigned int message_id);
+
+#pragma weak plat_scmi_rstd_count
+#pragma weak plat_scmi_rstd_get_name
+#pragma weak plat_scmi_rstd_autonomous
+#pragma weak plat_scmi_rstd_set_state
+
+size_t plat_scmi_rstd_count(unsigned int agent_id __unused)
+{
+	return 0U;
+}
+
+const char *plat_scmi_rstd_get_name(unsigned int agent_id __unused,
+				  unsigned int scmi_id __unused)
+{
+	return NULL;
+}
+
+int32_t plat_scmi_rstd_autonomous(unsigned int agent_id __unused,
+				unsigned int scmi_id __unused,
+				unsigned int state __unused)
+{
+	return SCMI_NOT_SUPPORTED;
+}
+
+int32_t plat_scmi_rstd_set_state(unsigned int agent_id __unused,
+			       unsigned int scmi_id __unused,
+			       bool assert_not_deassert __unused)
+{
+	return SCMI_NOT_SUPPORTED;
+}
+
+static void report_version(struct scmi_msg *msg)
+{
+	struct scmi_protocol_version_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		.version = SCMI_PROTOCOL_VERSION_RESET_DOMAIN,
+	};
+
+	if (msg->in_size != 0U) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void report_attributes(struct scmi_msg *msg)
+{
+	struct scmi_protocol_attributes_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		.attributes = plat_scmi_rstd_count(msg->agent_id),
+	};
+
+	if (msg->in_size != 0U) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void report_message_attributes(struct scmi_msg *msg)
+{
+	struct scmi_protocol_message_attributes_a2p *in_args = (void *)msg->in;
+	struct scmi_protocol_message_attributes_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		/* For this protocol, attributes shall be zero */
+		.attributes = 0U,
+	};
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	if (!message_id_is_supported(in_args->message_id)) {
+		scmi_status_response(msg, SCMI_NOT_FOUND);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void reset_domain_attributes(struct scmi_msg *msg)
+{
+	struct scmi_reset_domain_attributes_a2p *in_args = (void *)msg->in;
+	struct scmi_reset_domain_attributes_p2a return_values;
+	const char *name = NULL;
+	unsigned int domain_id = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	domain_id = SPECULATION_SAFE_VALUE(in_args->domain_id);
+
+	if (domain_id >= plat_scmi_rstd_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+	name = plat_scmi_rstd_get_name(msg->agent_id, domain_id);
+	if (name == NULL) {
+		scmi_status_response(msg, SCMI_NOT_FOUND);
+		return;
+	}
+
+	zeromem(&return_values, sizeof(return_values));
+	COPY_NAME_IDENTIFIER(return_values.name, name);
+	return_values.status = SCMI_SUCCESS;
+	return_values.flags = 0U; /* Async and Notif are not supported */
+	return_values.latency = SCMI_RESET_DOMAIN_ATTR_UNK_LAT;
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void reset_request(struct scmi_msg *msg)
+{
+	struct scmi_reset_domain_request_a2p *in_args = (void *)msg->in;
+	struct scmi_reset_domain_request_p2a out_args = {
+		.status = SCMI_SUCCESS,
+	};
+	unsigned int domain_id = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	domain_id = SPECULATION_SAFE_VALUE(in_args->domain_id);
+
+	if (domain_id >= plat_scmi_rstd_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_NOT_FOUND);
+		return;
+	}
+
+	if ((in_args->flags & SCMI_RESET_DOMAIN_AUTO) != 0U) {
+		out_args.status = plat_scmi_rstd_autonomous(msg->agent_id,
+							  domain_id,
+							  in_args->reset_state);
+	} else if ((in_args->flags & SCMI_RESET_DOMAIN_EXPLICIT) != 0U) {
+		out_args.status = plat_scmi_rstd_set_state(msg->agent_id,
+							 domain_id, true);
+	} else {
+		out_args.status = plat_scmi_rstd_set_state(msg->agent_id,
+							 domain_id, false);
+	}
+
+	if (out_args.status != SCMI_SUCCESS) {
+		scmi_status_response(msg, out_args.status);
+	} else {
+		scmi_write_response(msg, &out_args, sizeof(out_args));
+	}
+}
+
+static const scmi_msg_handler_t scmi_rstd_handler_table[] = {
+	[SCMI_PROTOCOL_VERSION] = report_version,
+	[SCMI_PROTOCOL_ATTRIBUTES] = report_attributes,
+	[SCMI_PROTOCOL_MESSAGE_ATTRIBUTES] = report_message_attributes,
+	[SCMI_RESET_DOMAIN_ATTRIBUTES] = reset_domain_attributes,
+	[SCMI_RESET_DOMAIN_REQUEST] = reset_request,
+};
+
+static bool message_id_is_supported(unsigned int message_id)
+{
+	return (message_id < ARRAY_SIZE(scmi_rstd_handler_table)) &&
+	       (scmi_rstd_handler_table[message_id] != NULL);
+}
+
+scmi_msg_handler_t scmi_msg_get_rstd_handler(struct scmi_msg *msg)
+{
+	unsigned int message_id = SPECULATION_SAFE_VALUE(msg->message_id);
+
+	if (message_id >= ARRAY_SIZE(scmi_rstd_handler_table)) {
+		VERBOSE("Reset domain handle not found %u\n", msg->message_id);
+		return NULL;
+	}
+
+	return scmi_rstd_handler_table[message_id];
+}
diff --git a/drivers/scmi-msg/reset_domain.h b/drivers/scmi-msg/reset_domain.h
new file mode 100644
index 0000000..47bee5e
--- /dev/null
+++ b/drivers/scmi-msg/reset_domain.h
@@ -0,0 +1,122 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019, Linaro Limited
+ */
+#ifndef SCMI_MSG_RESET_DOMAIN_H
+#define SCMI_MSG_RESET_DOMAIN_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <lib/utils_def.h>
+
+#define SCMI_PROTOCOL_VERSION_RESET_DOMAIN	0x10000U
+
+#define SCMI_RESET_STATE_ARCH			BIT(31)
+#define SCMI_RESET_STATE_IMPL			0U
+
+/*
+ * Identifiers of the SCMI Reset Domain Management Protocol commands
+ */
+enum scmi_reset_domain_command_id {
+	SCMI_RESET_DOMAIN_ATTRIBUTES = 0x03,
+	SCMI_RESET_DOMAIN_REQUEST = 0x04,
+	SCMI_RESET_DOMAIN_NOTIFY = 0x05,
+};
+
+/*
+ * Identifiers of the SCMI Reset Domain Management Protocol responses
+ */
+enum scmi_reset_domain_response_id {
+	SCMI_RESET_ISSUED = 0x00,
+	SCMI_RESET_COMPLETE = 0x04,
+};
+
+/*
+ * PROTOCOL_ATTRIBUTES
+ */
+
+#define SCMI_RESET_DOMAIN_COUNT_MASK		GENMASK_32(15, 0)
+
+struct scmi_reset_domain_protocol_attributes_p2a {
+	int32_t status;
+	uint32_t attributes;
+};
+
+/* Value for scmi_reset_domain_attributes_p2a:flags */
+#define SCMI_RESET_DOMAIN_ATTR_ASYNC		BIT(31)
+#define SCMI_RESET_DOMAIN_ATTR_NOTIF		BIT(30)
+
+/* Value for scmi_reset_domain_attributes_p2a:latency */
+#define SCMI_RESET_DOMAIN_ATTR_UNK_LAT		0x7fffffffU
+#define SCMI_RESET_DOMAIN_ATTR_MAX_LAT		0x7ffffffeU
+
+/* Macro for scmi_reset_domain_attributes_p2a:name */
+#define SCMI_RESET_DOMAIN_ATTR_NAME_SZ		16U
+
+struct scmi_reset_domain_attributes_a2p {
+	uint32_t domain_id;
+};
+
+struct scmi_reset_domain_attributes_p2a {
+	int32_t status;
+	uint32_t flags;
+	uint32_t latency;
+	char name[SCMI_RESET_DOMAIN_ATTR_NAME_SZ];
+};
+
+/*
+ * RESET
+ */
+
+/* Values for scmi_reset_domain_request_a2p:flags */
+#define SCMI_RESET_DOMAIN_ASYNC			BIT(2)
+#define SCMI_RESET_DOMAIN_EXPLICIT		BIT(1)
+#define SCMI_RESET_DOMAIN_AUTO			BIT(0)
+
+struct scmi_reset_domain_request_a2p {
+	uint32_t domain_id;
+	uint32_t flags;
+	uint32_t reset_state;
+};
+
+struct scmi_reset_domain_request_p2a {
+	int32_t status;
+};
+
+/*
+ * RESET_NOTIFY
+ */
+
+/* Values for scmi_reset_notify_p2a:flags */
+#define SCMI_RESET_DOMAIN_DO_NOTIFY		BIT(0)
+
+struct scmi_reset_domain_notify_a2p {
+	uint32_t domain_id;
+	uint32_t notify_enable;
+};
+
+struct scmi_reset_domain_notify_p2a {
+	int32_t status;
+};
+
+/*
+ * RESET_COMPLETE
+ */
+
+struct scmi_reset_domain_complete_p2a {
+	int32_t status;
+	uint32_t domain_id;
+};
+
+/*
+ * RESET_ISSUED
+ */
+
+struct scmi_reset_domain_issued_p2a {
+	uint32_t domain_id;
+	uint32_t reset_state;
+};
+
+#endif /* SCMI_MSG_RESET_DOMAIN_H */
diff --git a/drivers/scmi-msg/smt.c b/drivers/scmi-msg/smt.c
new file mode 100644
index 0000000..9b079c7
--- /dev/null
+++ b/drivers/scmi-msg/smt.c
@@ -0,0 +1,206 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2015-2019, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2020, Linaro Limited
+ */
+#include <assert.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <drivers/scmi-msg.h>
+#include <drivers/scmi.h>
+#include <lib/cassert.h>
+#include <lib/mmio.h>
+#include <lib/spinlock.h>
+#include <lib/utils.h>
+#include <plat/common/platform.h>
+
+#include "common.h"
+
+/* Legacy SMT/SCMI messages are 128 bytes at most including SMT header */
+#define SCMI_PLAYLOAD_MAX		92U
+#define SCMI_PLAYLOAD_U32_MAX		(SCMI_PLAYLOAD_MAX / sizeof(uint32_t))
+
+/**
+ * struct smt_header - SMT formatted header for SMT base shared memory transfer
+ *
+ * @status: Bit flags, see SMT_STATUS_*
+ * @flags: Bit flags, see SMT_FLAG_*
+ * @length: Byte size of message payload (variable) + ::message_header (32bit)
+ * payload: SCMI message payload data
+ */
+struct smt_header {
+	uint32_t reserved0;
+	uint32_t status;
+	uint64_t reserved1;
+	uint32_t flags;
+	uint32_t length; /* message_header + payload */
+	uint32_t message_header;
+	uint32_t payload[];
+};
+
+CASSERT(SCMI_PLAYLOAD_MAX + sizeof(struct smt_header) <= SMT_BUF_SLOT_SIZE,
+	assert_scmi_message_max_length_fits_in_smt_buffer_slot);
+
+/* Flag set in smt_header::status when SMT does not contain pending message */
+#define SMT_STATUS_FREE			BIT_32(0)
+/* Flag set in smt_header::status when SMT reports an error */
+#define SMT_STATUS_ERROR		BIT_32(1)
+
+/* Flag set in smt_header::flags when SMT uses interrupts */
+#define SMT_FLAG_INTR_ENABLED		BIT_32(1)
+
+/* Bit fields packed in smt_header::message_header */
+#define SMT_MSG_ID_MASK			GENMASK_32(7, 0)
+#define SMT_HDR_MSG_ID(_hdr)		((_hdr) & SMT_MSG_ID_MASK)
+
+#define SMT_MSG_TYPE_MASK		GENMASK_32(9, 8)
+#define SMT_HDR_TYPE_ID(_hdr)		(((_hdr) & SMT_MSG_TYPE_MASK) >> 8)
+
+#define SMT_MSG_PROT_ID_MASK		GENMASK_32(17, 10)
+#define SMT_HDR_PROT_ID(_hdr)		(((_hdr) & SMT_MSG_PROT_ID_MASK) >> 10)
+
+/*
+ * Provision input message payload buffers for fastcall SMC context entries
+ * and for interrupt context execution entries.
+ */
+static uint32_t fast_smc_payload[PLATFORM_CORE_COUNT][SCMI_PLAYLOAD_U32_MAX];
+static uint32_t interrupt_payload[PLATFORM_CORE_COUNT][SCMI_PLAYLOAD_U32_MAX];
+
+/* SMP protection on channel access */
+static struct spinlock smt_channels_lock;
+
+/* If channel is not busy, set busy and return true, otherwise return false */
+static bool channel_set_busy(struct scmi_msg_channel *chan)
+{
+	bool channel_is_busy;
+
+	spin_lock(&smt_channels_lock);
+
+	channel_is_busy = chan->busy;
+
+	if (!channel_is_busy) {
+		chan->busy = true;
+	}
+
+	spin_unlock(&smt_channels_lock);
+
+	return !channel_is_busy;
+}
+
+static void channel_release_busy(struct scmi_msg_channel *chan)
+{
+	chan->busy = false;
+}
+
+static struct smt_header *channel_to_smt_hdr(struct scmi_msg_channel *chan)
+{
+	return (struct smt_header *)chan->shm_addr;
+}
+
+/*
+ * Creates a SCMI message instance in secure memory and pushes it in the SCMI
+ * message drivers. Message structure contains SCMI protocol meta-data and
+ * references to input payload in secure memory and output message buffer
+ * in shared memory.
+ */
+static void scmi_proccess_smt(unsigned int agent_id, uint32_t *payload_buf)
+{
+	struct scmi_msg_channel *chan;
+	struct smt_header *smt_hdr;
+	size_t in_payload_size;
+	uint32_t smt_status;
+	struct scmi_msg msg;
+	bool error = true;
+
+	chan = plat_scmi_get_channel(agent_id);
+	if (chan == NULL) {
+		return;
+	}
+
+	smt_hdr = channel_to_smt_hdr(chan);
+	assert(smt_hdr);
+
+	smt_status = __atomic_load_n(&smt_hdr->status, __ATOMIC_RELAXED);
+
+	if (!channel_set_busy(chan)) {
+		VERBOSE("SCMI channel %u busy", agent_id);
+		goto out;
+	}
+
+	in_payload_size = __atomic_load_n(&smt_hdr->length, __ATOMIC_RELAXED) -
+			  sizeof(smt_hdr->message_header);
+
+	if (in_payload_size > SCMI_PLAYLOAD_MAX) {
+		VERBOSE("SCMI payload too big %zu", in_payload_size);
+		goto out;
+	}
+
+	if ((smt_status & (SMT_STATUS_ERROR | SMT_STATUS_FREE)) != 0U) {
+		VERBOSE("SCMI channel bad status 0x%x",
+			smt_hdr->status & (SMT_STATUS_ERROR | SMT_STATUS_FREE));
+		goto out;
+	}
+
+	/* Fill message */
+	zeromem(&msg, sizeof(msg));
+	msg.in = (char *)payload_buf;
+	msg.in_size = in_payload_size;
+	msg.out = (char *)smt_hdr->payload;
+	msg.out_size = chan->shm_size - sizeof(*smt_hdr);
+
+	assert((msg.out != NULL) && (msg.out_size >= sizeof(int32_t)));
+
+	/* Here the payload is copied in secure memory */
+	memcpy(msg.in, smt_hdr->payload, in_payload_size);
+
+	msg.protocol_id = SMT_HDR_PROT_ID(smt_hdr->message_header);
+	msg.message_id = SMT_HDR_MSG_ID(smt_hdr->message_header);
+	msg.agent_id = agent_id;
+
+	scmi_process_message(&msg);
+
+	/* Update message length with the length of the response message */
+	smt_hdr->length = msg.out_size_out + sizeof(smt_hdr->message_header);
+
+	channel_release_busy(chan);
+	error = false;
+
+out:
+	if (error) {
+		VERBOSE("SCMI error");
+		smt_hdr->status |= SMT_STATUS_ERROR | SMT_STATUS_FREE;
+	} else {
+		smt_hdr->status |= SMT_STATUS_FREE;
+	}
+}
+
+void scmi_smt_fastcall_smc_entry(unsigned int agent_id)
+{
+	scmi_proccess_smt(agent_id,
+			  fast_smc_payload[plat_my_core_pos()]);
+}
+
+void scmi_smt_interrupt_entry(unsigned int agent_id)
+{
+	scmi_proccess_smt(agent_id,
+			  interrupt_payload[plat_my_core_pos()]);
+}
+
+/* Init a SMT header for a shared memory buffer: state it a free/no-error */
+void scmi_smt_init_agent_channel(struct scmi_msg_channel *chan)
+{
+	if (chan != NULL) {
+		struct smt_header *smt_header = channel_to_smt_hdr(chan);
+
+		if (smt_header != NULL) {
+			memset(smt_header, 0, sizeof(*smt_header));
+			smt_header->status = SMT_STATUS_FREE;
+
+			return;
+		}
+	}
+
+	panic();
+}