GetMsgType : Take from config

GetMsgType response is hardcoded and always return only control
message support. Read supported message types from config file
and reply in control message

Tested by sending GetMsgType and GetVersion from another EP
Default responses for GetMsgType and GetVersion for type 0
5, 0, 1, 0 and 4, 0, 4, 241, 240, 255, 0, 241, 241, 255, 0, 241, 242, 255, 0, 241, 243, 241, 0

After calling
busctl call au.com.codeconstruct.MCTP1 /au/com/codeconstruct/mctp1 au.com.codeconstruct.MCTP1 RegisterResponder yau 5 1 0xF1F2F3F4
Response for GetMsgType and GetVersion for type 5
5, 0, 2, 0, 5 and 4, 0, 1, 244, 243, 242, 241

Signed-off-by: Nidhin MS <nidhin.ms@intel.com>

Author: msnidhin

docs/mctpd.md

@@ -25,8 +25,39 @@ Service au.com.codeconstruct.MCTP1:
 
 ## Top-level object: `/au/com/codeconstruct/mctp1`
 
-This object serves as the global MCTP daemon namespace; it doesn't contain
-much at present, but hosts two trees of MCTP objects:
+This object serves as the global MCTP daemon namespace.
+It hosts `au.com.codeconstruct.MCTP1` dbus interface to modify mctp properties like
+supported message types.
+```
+NAME                                TYPE      SIGNATURE  RESULT/VALUE  FLAGS
+au.com.codeconstruct.MCTP1          interface -          -             -
+.RegisterResponder                  method    yau        -             -
+```
+
+#### `.RegisterResponder`: `yau`
+
+This method is used to add support for mctp message types other than control
+messages. Once called successfully subsequent response for get message type
+control command will include this new message type also. Also the versions
+passed to this method will be used to respond to get version control command.
+
+`RegisterResponder <msg type> <versions>`
+
+If message type is already registered then dbus call will fail
+
+`<msg type>` Message type defined in DSP0239
+
+`<versions>` Versions supported for this message type formatted as uint32 integers as
+specified in DSP0236
+
+Example for PLDM type with two versions:
+
+```shell
+busctl call au.com.codeconstruct.MCTP1 /au/com/codeconstruct/mctp1 \
+  au.com.codeconstruct.MCTP1 RegisterResponder yau 1 2 0xF1F2F3F4 0xF0F0F0F1
+```
+
+Also it hosts two trees of MCTP objects:
 
  * Interfaces: Local hardware transport bindings that connect us to a MCTP bus
  * Endpoints: MCTP endpoints that `mctpd` knows about, both remote and local

src/mctpd.c

@@ -192,6 +192,12 @@ struct peer {
 	} recovery;
 };
 
+struct msg_type_support {
+	uint8_t msg_type;
+	uint32_t *versions;
+	size_t num_versions;
+};
+
 struct ctx {
 	sd_event *event;
 	sd_bus *bus;
@@ -219,6 +225,10 @@ struct ctx {
 
 	uint8_t uuid[16];
 
+	// Supported message types and their versions
+	struct msg_type_support* supported_msg_types;
+	size_t num_supported_msg_types;
+
 	// Verbose logging
 	bool verbose;
 };
@@ -667,42 +677,64 @@ handle_control_get_version_support(struct ctx *ctx, int sd,
 	struct mctp_ctrl_cmd_get_mctp_ver_support *req = NULL;
 	struct mctp_ctrl_resp_get_mctp_ver_support *resp = NULL;
 	uint32_t *versions = NULL;
-	// space for 4 versions
-	uint8_t respbuf[sizeof(*resp) + 4 * sizeof(*versions)];
+	uint8_t *respbuf = NULL;
 	size_t resp_len;
+	ssize_t i, ver_idx = -1, ver_count = 0;
+	int status;
 
 	if (buf_size < sizeof(struct mctp_ctrl_cmd_get_mctp_ver_support)) {
 		warnx("short Get Version Support message");
 		return -ENOMSG;
 	}
 
 	req = (void *)buf;
-	resp = (void *)respbuf;
-	memset(resp, 0x0, sizeof(*resp));
-	versions = (void *)(resp + 1);
-	switch (req->msg_type_number) {
-	case 0xff: // Base Protocol
-	case 0x00: // Control protocol
-		// from DSP0236 1.3.1  section 12.6.2. Big endian.
-		versions[0] = htonl(0xF1F0FF00);
-		versions[1] = htonl(0xF1F1FF00);
-		versions[2] = htonl(0xF1F2FF00);
-		versions[3] = htonl(0xF1F3F100);
-		resp->number_of_entries = 4;
-		resp->completion_code = MCTP_CTRL_CC_SUCCESS;
-		resp_len = sizeof(*resp) + 4 * sizeof(*versions);
-		break;
-	default:
-		// Unsupported message type
+	if (req->msg_type_number == 0xFF) {
+		// use same version for base spec and control protocol
+		req->msg_type_number = 0;
+	}
+	for (i = 0; i < ctx->num_supported_msg_types; i++) {
+		if (ctx->supported_msg_types[i].msg_type ==
+		    req->msg_type_number) {
+			ver_idx = i;
+			break;
+		}
+	}
+
+	if (ver_idx < 0) {
+		respbuf = malloc(sizeof(struct mctp_ctrl_resp));
+		if (!respbuf) {
+			warnx("Failed to allocate response buffer");
+			return -ENOMEM;
+		}
+		resp = (void *)respbuf;
+		// Nobody registered yet as responder for this type
 		resp->completion_code =
 			MCTP_CTRL_CC_GET_MCTP_VER_SUPPORT_UNSUPPORTED_TYPE;
-		resp_len = sizeof(*resp);
+		resp_len = sizeof(struct mctp_ctrl_resp);
+	} else {
+		ver_count = ctx->supported_msg_types[ver_idx].num_versions;
+		respbuf =
+			malloc(sizeof(*resp) + (ver_count * sizeof(uint32_t)));
+		if (!respbuf) {
+			warnx("Failed to allocate response buffer for versions");
+			return -ENOMEM;
+		}
+		resp = (void *)respbuf;
+		resp->number_of_entries = ver_count;
+		versions = (void *)(resp + 1);
+		memcpy(versions, ctx->supported_msg_types[ver_idx].versions,
+		       ver_count * sizeof(uint32_t));
+		resp->completion_code = MCTP_CTRL_CC_SUCCESS;
+		resp_len = sizeof(*resp) + ver_count * sizeof(uint32_t);
 	}
 
 	resp->ctrl_hdr.command_code = req->ctrl_hdr.command_code;
 	resp->ctrl_hdr.rq_dgram_inst =
 		(req->ctrl_hdr.rq_dgram_inst & IID_MASK) | RQDI_RESP;
-	return reply_message(ctx, sd, resp, resp_len, addr);
+
+	status = reply_message(ctx, sd, resp, resp_len, addr);
+	free(respbuf);
+	return status;
 }
 
 static int handle_control_get_endpoint_id(struct ctx *ctx, int sd,
@@ -761,28 +793,42 @@ static int handle_control_get_message_type_support(
 	const uint8_t *buf, const size_t buf_size)
 {
 	struct mctp_ctrl_cmd_get_msg_type_support *req = NULL;
-	;
 	struct mctp_ctrl_resp_get_msg_type_support *resp = NULL;
-	uint8_t resp_buf[sizeof(*resp) + 1];
-	size_t resp_len;
+	uint8_t *resp_buf, *msg_types;
+	size_t resp_len, type_count;
+	size_t i;
 
 	if (buf_size < sizeof(*req)) {
 		warnx("short Get Message Type Support message");
 		return -ENOMSG;
 	}
 
 	req = (void *)buf;
+	type_count = ctx->num_supported_msg_types;
+	// Allocate extra space for the message types
+	resp_len = sizeof(*resp) + type_count;
+	resp_buf = malloc(resp_len);
+	if (!resp_buf) {
+		warnx("Failed to allocate response buffer");
+		return -ENOMEM;
+	}
+
 	resp = (void *)resp_buf;
-	resp->ctrl_hdr.command_code = req->ctrl_hdr.command_code;
 	resp->ctrl_hdr.rq_dgram_inst =
 		(req->ctrl_hdr.rq_dgram_inst & IID_MASK) | RQDI_RESP;
+	resp->ctrl_hdr.command_code = req->ctrl_hdr.command_code;
+	resp->completion_code = MCTP_CTRL_CC_SUCCESS;
 
-	// Only control messages supported
-	resp->msg_type_count = 1;
-	*((uint8_t *)(resp + 1)) = MCTP_CTRL_HDR_MSG_TYPE;
-	resp_len = sizeof(*resp) + resp->msg_type_count;
+	resp->msg_type_count = type_count;
+	// Append message types after msg_type_count
+	msg_types = (uint8_t *)(resp + 1);
+	for (i = 0; i < type_count; i++) {
+		msg_types[i] = ctx->supported_msg_types[i].msg_type;
+	}
 
-	return reply_message(ctx, sd, resp, resp_len, addr);
+	int result = reply_message(ctx, sd, resp, resp_len, addr);
+	free(resp_buf);
+	return result;
 }
 
 static int
@@ -2829,6 +2875,71 @@ static int method_net_learn_endpoint(sd_bus_message *call, void *data,
 	return rc;
 }
 
+static int method_register_responder(sd_bus_message *call, void *data,
+				     sd_bus_error *berr)
+{
+	struct ctx *ctx = data;
+	uint8_t msg_type;
+	const uint32_t *versions = NULL;
+	size_t versions_len;
+	int rc, i;
+
+	rc = sd_bus_message_read(call, "y", &msg_type);
+	if (rc < 0)
+		goto err;
+	rc = sd_bus_message_read_array(call, 'u', (const void **)&versions,
+				       &versions_len);
+	if (rc < 0)
+		goto err;
+
+	if (versions_len == 0) {
+		warnx("No versions provided for message type %d", msg_type);
+		return sd_bus_error_setf(
+			berr, SD_BUS_ERROR_INVALID_ARGS,
+			"No versions provided for message type %d", msg_type);
+	}
+
+	for (i = 0; i < ctx->num_supported_msg_types; i++) {
+		if (ctx->supported_msg_types[i].msg_type == msg_type) {
+			warnx("Message type %d already registered", msg_type);
+			return sd_bus_error_setf(
+				berr, SD_BUS_ERROR_INVALID_ARGS,
+				"Message type %d already registered", msg_type);
+		}
+	}
+
+	struct msg_type_support *msg_types =
+		realloc(ctx->supported_msg_types,
+			(ctx->num_supported_msg_types + 1) *
+				sizeof(struct msg_type_support));
+	if (!msg_types) {
+		goto oom_err;
+	}
+	ctx->supported_msg_types = msg_types;
+	ctx->supported_msg_types[ctx->num_supported_msg_types].msg_type =
+		msg_type;
+	ctx->supported_msg_types[ctx->num_supported_msg_types].num_versions =
+		versions_len / sizeof(uint32_t);
+	ctx->supported_msg_types[ctx->num_supported_msg_types].versions =
+		malloc(versions_len);
+	if (!ctx->supported_msg_types[ctx->num_supported_msg_types].versions) {
+		goto oom_err;
+	}
+	// Assume callers's responsibility to provide version in uint32 format from spec
+	memcpy(ctx->supported_msg_types[ctx->num_supported_msg_types].versions,
+	       versions, versions_len);
+
+	ctx->num_supported_msg_types++;
+
+	return sd_bus_reply_method_return(call, "");
+oom_err:
+	return sd_bus_error_setf(berr, SD_BUS_ERROR_NO_MEMORY,
+				 "Failed to allocate memory");
+err:
+	set_berr(ctx, rc, berr);
+	return rc;
+}
+
 // clang-format off
 static const sd_bus_vtable bus_link_owner_vtable[] = {
 	SD_BUS_VTABLE_START(0),
@@ -3145,6 +3256,17 @@ static const sd_bus_vtable bus_network_vtable[] = {
 			SD_BUS_VTABLE_PROPERTY_CONST),
 	SD_BUS_VTABLE_END
 };
+
+static const sd_bus_vtable mctp_base_vtable[] = {
+	SD_BUS_VTABLE_START(0),
+	SD_BUS_METHOD_WITH_ARGS("RegisterResponder",
+	SD_BUS_ARGS("y", msg_type,
+	            "au", versions),
+	SD_BUS_NO_RESULT,
+	method_register_responder,
+	0),
+	SD_BUS_VTABLE_END,
+};
 // clang-format on
 
 static int emit_endpoint_added(const struct peer *peer)
@@ -3295,6 +3417,14 @@ static int setup_bus(struct ctx *ctx)
 		goto out;
 	}
 
+	rc = sd_bus_add_object_vtable(ctx->bus, NULL, MCTP_DBUS_PATH,
+				      MCTP_DBUS_NAME,
+				      mctp_base_vtable, ctx);
+	if (rc < 0) {
+		warnx("Adding MCTP base vtable failed: %s", strerror(-rc));
+		goto out;
+	}
+
 	rc = 0;
 out:
 	return rc;
@@ -3994,6 +4124,33 @@ static int parse_config(struct ctx *ctx)
 	return rc;
 }
 
+static void setup_ctrl_cmd_defaults(struct ctx *ctx)
+{
+	ctx->supported_msg_types = NULL;
+	ctx->num_supported_msg_types = 0;
+
+	// Default to supporting only control messages
+	ctx->supported_msg_types = malloc(sizeof(struct msg_type_support));
+	if (!ctx->supported_msg_types) {
+		warnx("Out of memory for supported message types");
+		return;
+	}
+	ctx->num_supported_msg_types = 1;
+	ctx->supported_msg_types[0].msg_type = MCTP_CTRL_HDR_MSG_TYPE;
+
+	ctx->supported_msg_types[0].versions = malloc(sizeof(uint32_t) * 4);
+	if (!ctx->supported_msg_types[0].versions) {
+		warnx("Out of memory for versions");
+		free(ctx->supported_msg_types);
+		return;
+	}
+	ctx->supported_msg_types[0].num_versions = 4;
+	ctx->supported_msg_types[0].versions[0] = htonl(0xF1F0FF00);
+	ctx->supported_msg_types[0].versions[1] = htonl(0xF1F1FF00);
+	ctx->supported_msg_types[0].versions[2] = htonl(0xF1F2FF00);
+	ctx->supported_msg_types[0].versions[3] = htonl(0xF1F3F100);
+}
+
 static void setup_config_defaults(struct ctx *ctx)
 {
 	ctx->mctp_timeout = 250000; // 250ms
@@ -4002,7 +4159,13 @@ static void setup_config_defaults(struct ctx *ctx)
 
 static void free_config(struct ctx *ctx)
 {
+	int i;
+
 	free(ctx->config_filename);
+	for (i = 0; i < ctx->num_supported_msg_types; i++) {
+		free(ctx->supported_msg_types[i].versions);
+	}
+	free(ctx->supported_msg_types);
 }
 
 int main(int argc, char **argv)
@@ -4013,6 +4176,8 @@ int main(int argc, char **argv)
 	setlinebuf(stdout);
 
 	setup_config_defaults(ctx);
+	setup_ctrl_cmd_defaults(ctx);
+
 	mctp_ops_init();
 
 	rc = parse_args(ctx, argc, argv);