control: Add basic MCTP Control Protocol handler
This will respond to the 4 mandatory MCTP Control Protocol commands.
Applications can register supported types using mctp_control_add_type().
Change-Id: Ia904bcbe118626adf9254ffa71dd8e17fbdfc9b7
Signed-off-by: Matt Johnston <matt@codeconstruct.com.au>
diff --git a/control.c b/control.c
new file mode 100644
index 0000000..fedfe56
--- /dev/null
+++ b/control.c
@@ -0,0 +1,309 @@
+#include <string.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <errno.h>
+
+#include "libmctp-cmds.h"
+#include "libmctp-alloc.h"
+#include "libmctp-log.h"
+#include "core-internal.h"
+
+#include "control.h"
+
+static void fill_resp(const void *req, struct mctp_ctrl_msg_hdr *hdr)
+{
+ const struct mctp_ctrl_msg_hdr *req_hdr = req;
+ hdr->ic_msg_type = MCTP_CTRL_HDR_MSG_TYPE;
+ hdr->rq_dgram_inst = req_hdr->rq_dgram_inst &
+ MCTP_CTRL_HDR_INSTANCE_ID_MASK;
+ hdr->command_code = req_hdr->command_code;
+}
+
+static uint8_t mctp_ctrl_set_endpoint_id(struct mctp_bus *bus, uint8_t src_eid,
+ uint8_t msg_tag, const void *data,
+ size_t len)
+{
+ if (len != sizeof(struct mctp_ctrl_cmd_set_endpoint_id_req)) {
+ return MCTP_CTRL_CC_ERROR_INVALID_LENGTH;
+ }
+ const struct mctp_ctrl_cmd_set_endpoint_id_req *req = data;
+
+ uint8_t op = req->operation & MCTP_CTRL_SET_EID_OP_MASK;
+ if (!(op == MCTP_CTRL_SET_EID_OP_SET ||
+ op == MCTP_CTRL_SET_EID_OP_FORCE)) {
+ return MCTP_CTRL_CC_ERROR_INVALID_DATA;
+ }
+
+ if (mctp_bus_set_eid(bus->binding, req->eid)) {
+ return MCTP_CTRL_CC_ERROR_INVALID_DATA;
+ }
+
+ struct mctp_ctrl_cmd_set_endpoint_id_resp *resp =
+ __mctp_msg_alloc(sizeof(*resp), bus->mctp);
+ if (!resp) {
+ mctp_prdebug("no response buffer");
+ return MCTP_CTRL_CC_ERROR;
+ }
+ memset(resp, 0x00, sizeof(*resp));
+ fill_resp(data, &resp->hdr);
+ resp->completion_code = MCTP_CTRL_CC_SUCCESS;
+ resp->status = MCTP_CTRL_SET_EID_STATUS_ACCEPTED;
+ resp->eid = req->eid;
+ resp->pool_size = 0;
+
+ int rc = mctp_message_tx_alloced(bus->mctp, src_eid, false, msg_tag,
+ resp, sizeof(*resp));
+ if (!rc) {
+ mctp_prdebug("set_endpoint_id response send failed: %d", rc);
+ }
+ return MCTP_CTRL_CC_SUCCESS;
+}
+
+static uint8_t mctp_ctrl_get_endpoint_id(struct mctp_bus *bus, uint8_t src_eid,
+ uint8_t msg_tag, const void *data,
+ size_t len)
+{
+ if (len != sizeof(struct mctp_ctrl_msg_hdr)) {
+ /* Expect empty request */
+ return MCTP_CTRL_CC_ERROR_INVALID_LENGTH;
+ }
+ (void)data;
+
+ struct mctp_ctrl_cmd_get_endpoint_id_resp *resp =
+ __mctp_msg_alloc(sizeof(*resp), bus->mctp);
+ if (!resp) {
+ mctp_prdebug("no response buffer");
+ return MCTP_CTRL_CC_ERROR;
+ }
+ memset(resp, 0x00, sizeof(*resp));
+ fill_resp(data, &resp->hdr);
+ resp->completion_code = MCTP_CTRL_CC_SUCCESS;
+ resp->endpoint_id = bus->eid;
+ resp->endpoint_type = MCTP_CTRL_ENDPOINT_TYPE_SIMPLE |
+ MCTP_CTRL_ENDPOINT_ID_TYPE_STATIC;
+ resp->medium_specific = 0x00;
+
+ int rc = mctp_message_tx_alloced(bus->mctp, src_eid, false, msg_tag,
+ resp, sizeof(*resp));
+ if (!rc) {
+ mctp_prdebug("get_endpoint_id response send failed: %d", rc);
+ }
+ return MCTP_CTRL_CC_SUCCESS;
+}
+
+#define MCTP_PROTOCOL_COUNT 4
+/* Big endian */
+const uint8_t MCTP_PROTOCOL_VERSIONS[MCTP_PROTOCOL_COUNT * 4] = {
+ // 1.0
+ 0xf1,
+ 0xf0,
+ 0xff,
+ 0x00,
+ // 1.1
+ 0xf1,
+ 0xf1,
+ 0xff,
+ 0x00,
+ // 1.2
+ 0xf1,
+ 0xf2,
+ 0xff,
+ 0x00,
+ // 1.3.3
+ 0xf1,
+ 0xf3,
+ 0xf3,
+ 0x00,
+};
+
+static uint8_t mctp_ctrl_get_version(struct mctp_bus *bus, uint8_t src_eid,
+ uint8_t msg_tag, const void *data,
+ size_t len)
+{
+ if (len != sizeof(struct mctp_ctrl_cmd_get_version_req)) {
+ return MCTP_CTRL_CC_ERROR_INVALID_LENGTH;
+ }
+ const struct mctp_ctrl_cmd_get_version_req *req = data;
+
+ switch (req->msg_type) {
+ case 0x00:
+ case 0xff:
+ /* Only have versions for MCTP base or control */
+ break;
+ default:
+ return MCTP_CTRL_VERSIONS_NOT_SUPPORTED;
+ }
+
+ /* Return only the versions for MCTP */
+ size_t total_sz = sizeof(struct mctp_ctrl_cmd_get_version_resp) +
+ sizeof(MCTP_PROTOCOL_VERSIONS);
+
+ struct mctp_ctrl_cmd_get_version_resp *resp =
+ __mctp_msg_alloc(total_sz, bus->mctp);
+ if (!resp) {
+ mctp_prdebug("no response buffer");
+ return MCTP_CTRL_CC_ERROR;
+ }
+ memset(resp, 0x00, total_sz);
+ fill_resp(data, &resp->hdr);
+ resp->completion_code = MCTP_CTRL_CC_SUCCESS;
+ resp->version_count = MCTP_PROTOCOL_COUNT;
+ memcpy(resp->versions, MCTP_PROTOCOL_VERSIONS,
+ sizeof(MCTP_PROTOCOL_VERSIONS));
+
+ int rc = mctp_message_tx_alloced(bus->mctp, src_eid, false, msg_tag,
+ resp, total_sz);
+ if (!rc) {
+ mctp_prdebug("mctp get_version response send failed: %d", rc);
+ }
+ return MCTP_CTRL_CC_SUCCESS;
+}
+
+static uint8_t mctp_ctrl_get_types(struct mctp_bus *bus, uint8_t src_eid,
+ uint8_t msg_tag, const void *data,
+ size_t len)
+{
+ if (len != sizeof(struct mctp_ctrl_msg_hdr)) {
+ return MCTP_CTRL_CC_ERROR_INVALID_LENGTH;
+ }
+ (void)data;
+
+ size_t total_sz = sizeof(struct mctp_ctrl_cmd_get_types_resp) +
+ bus->mctp->control.num_msg_types;
+
+ struct mctp_ctrl_cmd_get_types_resp *resp =
+ __mctp_msg_alloc(total_sz, bus->mctp);
+ if (!resp) {
+ mctp_prdebug("no response buffer");
+ return MCTP_CTRL_CC_ERROR;
+ }
+ memset(resp, 0x00, total_sz);
+ fill_resp(data, &resp->hdr);
+ resp->completion_code = MCTP_CTRL_CC_SUCCESS;
+ resp->type_count = bus->mctp->control.num_msg_types;
+ memcpy(resp->types, bus->mctp->control.msg_types,
+ bus->mctp->control.num_msg_types);
+
+ int rc = mctp_message_tx_alloced(bus->mctp, src_eid, false, msg_tag,
+ resp, total_sz);
+ if (!rc) {
+ mctp_prdebug("mctp get_types response send failed: %d", rc);
+ }
+ return MCTP_CTRL_CC_SUCCESS;
+}
+
+static void reply_error(struct mctp *mctp, uint8_t src_eid, uint8_t msg_tag,
+ const struct mctp_ctrl_msg_hdr *ctrl_hdr, uint8_t ccode)
+{
+ struct mctp_ctrl_cmd_empty_resp *resp =
+ __mctp_msg_alloc(sizeof(*resp), mctp);
+ if (!resp) {
+ mctp_prdebug("no response buffer");
+ return;
+ }
+ memset(resp, 0x00, sizeof(*resp));
+ fill_resp(ctrl_hdr, &resp->hdr);
+ resp->completion_code = ccode;
+
+ int rc = mctp_message_tx_alloced(mctp, src_eid, false, msg_tag, resp,
+ sizeof(*resp));
+ if (!rc) {
+ mctp_prdebug("error response send failed: %d", rc);
+ }
+}
+
+/* Control message request handler. This will respond to the mandatory MCTP control
+ * commands */
+bool mctp_control_handler(struct mctp_bus *bus, mctp_eid_t src_eid,
+ bool tag_owner, uint8_t msg_tag, const void *data,
+ size_t len)
+{
+ if (!tag_owner) {
+ // Not a request
+ return false;
+ }
+
+ if (len < 1) {
+ // No type byte
+ return false;
+ }
+
+ const struct mctp_ctrl_msg_hdr *ctrl_hdr = data;
+ if (ctrl_hdr->ic_msg_type != MCTP_CTRL_HDR_MSG_TYPE) {
+ // Not Control type
+ return false;
+ }
+
+ if (len < sizeof(struct mctp_ctrl_msg_hdr)) {
+ // Drop short messages, but treat as handled
+ return true;
+ }
+
+ if ((ctrl_hdr->rq_dgram_inst &
+ (MCTP_CTRL_HDR_FLAG_REQUEST | MCTP_CTRL_HDR_FLAG_DGRAM)) !=
+ MCTP_CTRL_HDR_FLAG_REQUEST) {
+ // Drop message, isn't a request.
+ // Treat as handled since TO bit was set.
+ return true;
+ }
+
+ // A valid MCTP Control request has been received, process it
+
+ uint8_t cc = MCTP_CTRL_CC_ERROR_UNSUPPORTED_CMD;
+ switch (ctrl_hdr->command_code) {
+ case MCTP_CTRL_CMD_SET_ENDPOINT_ID:
+ cc = mctp_ctrl_set_endpoint_id(bus, src_eid, msg_tag, data,
+ len);
+ break;
+ case MCTP_CTRL_CMD_GET_ENDPOINT_ID:
+ cc = mctp_ctrl_get_endpoint_id(bus, src_eid, msg_tag, data,
+ len);
+ break;
+ case MCTP_CTRL_CMD_GET_VERSION_SUPPORT:
+ cc = mctp_ctrl_get_version(bus, src_eid, msg_tag, data, len);
+ break;
+ case MCTP_CTRL_CMD_GET_MESSAGE_TYPE_SUPPORT:
+ cc = mctp_ctrl_get_types(bus, src_eid, msg_tag, data, len);
+ break;
+ default:
+ cc = MCTP_CTRL_CC_ERROR_UNSUPPORTED_CMD;
+ break;
+ }
+
+ if (cc) {
+ reply_error(bus->mctp, src_eid, msg_tag, ctrl_hdr, cc);
+ }
+
+ // No further handling required.
+ return true;
+}
+
+int mctp_control_add_type(struct mctp *mctp, uint8_t msg_type)
+{
+ /* Check for existing */
+ for (size_t i = 0; i < mctp->control.num_msg_types; i++) {
+ if (mctp->control.msg_types[i] == msg_type) {
+ return 0;
+ }
+ }
+
+ if (mctp->control.num_msg_types == MCTP_CONTROL_MAX_TYPES) {
+ return -ENOSPC;
+ }
+
+ mctp->control.msg_types[mctp->control.num_msg_types] = msg_type;
+ mctp->control.num_msg_types++;
+ return 0;
+}
+
+void mctp_control_remove_type(struct mctp *mctp, uint8_t msg_type)
+{
+ for (size_t i = 0; i < mctp->control.num_msg_types; i++) {
+ if (mctp->control.msg_types[i] == msg_type) {
+ memmove(&mctp->control.msg_types[i],
+ &mctp->control.msg_types[i + 1],
+ mctp->control.num_msg_types - (i + 1));
+ mctp->control.num_msg_types--;
+ }
+ }
+}