diff --git a/drivers/st/scmi-msg/clock.c b/drivers/st/scmi-msg/clock.c new file mode 100644 index 000000000..319557cd0 --- /dev/null +++ b/drivers/st/scmi-msg/clock.c @@ -0,0 +1,381 @@ +// SPDX-License-Identifier: BSD-3-Clause +/* + * Copyright (c) 2015-2020, Arm Limited and Contributors. All rights reserved. + * Copyright (c) 2019-2020, Linaro Limited + */ +#include +#include + +#include +#include +#include + +#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) +{ + 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); + 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); + 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 writen 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(size_t 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/st/scmi-msg/clock.h b/drivers/st/scmi-msg/clock.h new file mode 100644 index 000000000..a637934ee --- /dev/null +++ b/drivers/st/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 + +#include + +#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/st/scmi-msg/common.h b/drivers/st/scmi-msg/common.h index 4777dbe6e..63b92aa0b 100644 --- a/drivers/st/scmi-msg/common.h +++ b/drivers/st/scmi-msg/common.h @@ -12,6 +12,7 @@ #include #include "base.h" +#include "clock.h" #define SCMI_VERSION 0x20000U #define SCMI_IMPL_VERSION 0U @@ -94,6 +95,13 @@ typedef void (*scmi_msg_handler_t)(struct scmi_msg *msg); */ 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); + /* * Process Read, process and write response for input SCMI message * diff --git a/drivers/st/scmi-msg/entry.c b/drivers/st/scmi-msg/entry.c index cd62413df..7b06f353e 100644 --- a/drivers/st/scmi-msg/entry.c +++ b/drivers/st/scmi-msg/entry.c @@ -41,6 +41,9 @@ void scmi_process_message(struct scmi_msg *msg) 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; default: break; } diff --git a/include/drivers/st/scmi-msg.h b/include/drivers/st/scmi-msg.h index af683e277..48fac1685 100644 --- a/include/drivers/st/scmi-msg.h +++ b/include/drivers/st/scmi-msg.h @@ -62,4 +62,83 @@ const char *plat_scmi_vendor_name(void); /* Get the name of the SCMI sub-vendor for the platform */ const char *plat_scmi_sub_vendor_name(void); +/* Handlers for SCMI Clock protocol services */ + +/* + * Return number of clock controllers for an agent + * @agent_id: SCMI agent ID + * Return number of clock controllers + */ +size_t plat_scmi_clock_count(unsigned int agent_id); + +/* + * Get clock controller string ID (aka name) + * @agent_id: SCMI agent ID + * @scmi_id: SCMI clock ID + * Return pointer to name or NULL + */ +const char *plat_scmi_clock_get_name(unsigned int agent_id, + unsigned int scmi_id); + +/* + * Get clock possible rate as an array of frequencies in Hertz. + * + * @agent_id: SCMI agent ID + * @scmi_id: SCMI clock ID + * @rates: If NULL, function returns, else output rates array + * @nb_elts: Array size of @rates. + * Return an SCMI compliant error code + */ +int32_t plat_scmi_clock_rates_array(unsigned int agent_id, unsigned int scmi_id, + unsigned long *rates, size_t *nb_elts); + +/* + * Get clock possible rate as range with regular steps in Hertz + * + * @agent_id: SCMI agent ID + * @scmi_id: SCMI clock ID + * @min_max_step: 3 cell array for min, max and step rate data + * Return an SCMI compliant error code + */ +int32_t plat_scmi_clock_rates_by_step(unsigned int agent_id, + unsigned int scmi_id, + unsigned long *min_max_step); + +/* + * Get clock rate in Hertz + * @agent_id: SCMI agent ID + * @scmi_id: SCMI clock ID + * Return clock rate or 0 if not supported + */ +unsigned long plat_scmi_clock_get_rate(unsigned int agent_id, + unsigned int scmi_id); + +/* + * Set clock rate in Hertz + * @agent_id: SCMI agent ID + * @scmi_id: SCMI clock ID + * @rate: Target clock frequency in Hertz + * Return a compliant SCMI error code + */ +int32_t plat_scmi_clock_set_rate(unsigned int agent_id, unsigned int scmi_id, + unsigned long rate); + +/* + * Get clock state (enabled or disabled) + * @agent_id: SCMI agent ID + * @scmi_id: SCMI clock ID + * Return 1 if clock is enabled, 0 if disables, or a negative SCMI error code + */ +int32_t plat_scmi_clock_get_state(unsigned int agent_id, unsigned int scmi_id); + +/* + * Get clock state (enabled or disabled) + * @agent_id: SCMI agent ID + * @scmi_id: SCMI clock ID + * @enable_not_disable: Enable clock if true, disable clock otherwise + * Return a compliant SCMI error code + */ +int32_t plat_scmi_clock_set_state(unsigned int agent_id, unsigned int scmi_id, + bool enable_not_disable); + #endif /* SCMI_MSG_H */