mctpd: add dbus method RegisterVDMTypeSupport

Add a new DBus method RegisterVDMTypeSupport to support vendor defined
message support as responder
Add a new control command handler for get vdm message

Tested:
Unit tests passed
Tested by sending GetVDM command from another endpoint

Signed-off-by: Nidhin MS <nidhin.ms@intel.com>
Signed-off-by: PavanKumarIntel <pavanx.kumar.martha@intel.com>

Author: PavanMarthaIntel

docs/mctpd.md

@@ -32,6 +32,7 @@ the MCTP stack, such as supported message types.
 NAME                                TYPE      SIGNATURE  RESULT/VALUE  FLAGS
 au.com.codeconstruct.MCTP1          interface -          -             -
 .RegisterTypeSupport                  method    yau        -             -
+.RegisterVDMTypeSupport               method    yvq        -             -
 ```
 
 #### `.RegisterTypeSupport`: `yau`
@@ -53,6 +54,28 @@ De-registration is automatic - the specified types (and versions) are registered
 for as long as the dbus sender remains attached to the message bus, and are
 unregistered on disconnect.
 
+#### `.RegisterVDMTypeSupport`: `yvq`
+
+This method is used to add support for MCTP Vendor Defined Message (VDM) types.
+Once called successfully, subsequent responses for Get Vendor Defined Message
+Support control commands will include this new VDM type.
+
+`RegisterVDMTypeSupport <vid format> <vendor id> <command set type>`
+
+If the VDM type is already registered, then dbus call will fail.
+
+ - `<vid format>` Vendor ID format:
+   - `0x00` - PCI/PCIe Vendor ID (16-bit)
+   - `0x01` - IANA Enterprise Number (32-bit)
+ - `<vendor id>` Vendor identifier as a variant type:
+   - For PCIe format: 16-bit unsigned integer (`q`)
+   - For IANA format: 32-bit unsigned integer (`u`)
+ - `<command set type>` Command set type (16-bit unsigned integer) as defined by the vendor
+
+De-registration is automatic - the specified VDM types are registered for as
+long as the dbus sender remains attached to the message bus, and are
+removed when the sender disconnects.
+
 Also it hosts two trees of MCTP objects:
 
  * Interfaces: Local hardware transport bindings that connect us to a MCTP bus

src/mctpd.c

@@ -208,6 +208,16 @@ struct msg_type_support {
 	sd_bus_track *source_peer;
 };
 
+struct vdm_type_support {
+	uint8_t vid_format;
+	union {
+		uint16_t pcie;
+		uint32_t iana;
+	} vendor_id;
+	uint16_t cmd_set;
+	sd_bus_track *source_peer;
+};
+
 struct ctx {
 	sd_event *event;
 	sd_bus *bus;
@@ -243,6 +253,9 @@ struct ctx {
 	struct msg_type_support *supported_msg_types;
 	size_t num_supported_msg_types;
 
+	struct vdm_type_support *supported_vdm_types;
+	size_t num_supported_vdm_types;
+
 	// Verbose logging
 	bool verbose;
 
@@ -999,6 +1012,83 @@ static int handle_control_get_message_type_support(
 	return rc;
 }
 
+static int
+handle_control_get_vdm_type_support(struct ctx *ctx, int sd,
+				    const struct sockaddr_mctp_ext *addr,
+				    const uint8_t *buf, const size_t buf_size)
+{
+	size_t resp_len, max_rsp_len, vdm_count, vid_size_left;
+	struct mctp_ctrl_resp_get_vdm_support *resp = NULL;
+	struct mctp_ctrl_cmd_get_vdm_support *req = NULL;
+	struct vdm_type_support *cur_vdm;
+	uint8_t *resp_buf;
+	int rc;
+
+	if (buf_size < sizeof(*req)) {
+		warnx("short Get Message Type Support message");
+		return -ENOMSG;
+	}
+
+	req = (void *)buf;
+	vdm_count = ctx->num_supported_vdm_types;
+	// Allocate space for 32 bit VID + 16 bit cmd set
+	max_rsp_len = sizeof(*resp) + sizeof(uint16_t);
+	resp_buf = malloc(max_rsp_len);
+	if (!resp_buf) {
+		warnx("Failed to allocate response buffer");
+		return -ENOMEM;
+	}
+	resp = (void *)resp_buf;
+	mctp_ctrl_msg_hdr_init_resp(&resp->ctrl_hdr, req->ctrl_hdr);
+
+	if (vdm_count == 0) {
+		if (ctx->verbose) {
+			warnx("Get VDM Type Support but no VDMs registered");
+		}
+		resp_len = sizeof(struct mctp_ctrl_resp);
+		resp->completion_code = MCTP_CTRL_CC_ERROR;
+	} else if (req->vendor_id_set_selector >= vdm_count) {
+		if (ctx->verbose) {
+			warnx("Get VDM Type Support selector %u out of range (max %zu)",
+			      req->vendor_id_set_selector, vdm_count);
+		}
+		resp_len = sizeof(struct mctp_ctrl_resp);
+		resp->completion_code = MCTP_CTRL_CC_ERROR_INVALID_DATA;
+	} else {
+		cur_vdm =
+			&ctx->supported_vdm_types[req->vendor_id_set_selector];
+		vid_size_left = (cur_vdm->vid_format ==
+				 MCTP_GET_VDM_SUPPORT_PCIE_FORMAT_ID) ?
+					sizeof(uint16_t) :
+					0;
+		resp_len = max_rsp_len - vid_size_left;
+
+		resp->completion_code = MCTP_CTRL_CC_SUCCESS;
+		resp->vendor_id_set_selector =
+			req->vendor_id_set_selector == (vdm_count - 1) ?
+				MCTP_GET_VDM_SUPPORT_NO_MORE_CAP_SET :
+				req->vendor_id_set_selector + 1;
+		resp->vendor_id_format = cur_vdm->vid_format;
+		uint8_t *out_ptr = (uint8_t *)(resp + 1);
+		out_ptr -= vid_size_left;
+
+		if (cur_vdm->vid_format ==
+		    MCTP_GET_VDM_SUPPORT_PCIE_FORMAT_ID) {
+			resp->vendor_id_data_pcie =
+				htobe16(cur_vdm->vendor_id.pcie);
+		} else {
+			resp->vendor_id_data_iana =
+				htobe32(cur_vdm->vendor_id.iana);
+		}
+		uint16_t *cmd_type_ptr = (uint16_t *)out_ptr;
+		*cmd_type_ptr = htobe16(cur_vdm->cmd_set);
+	}
+
+	rc = reply_message(ctx, sd, resp, resp_len, addr);
+	free(resp_buf);
+	return rc;
+}
+
 static int
 handle_control_resolve_endpoint_id(struct ctx *ctx, int sd,
 				   const struct sockaddr_mctp_ext *addr,
@@ -1202,6 +1292,10 @@ static int cb_listen_control_msg(sd_event_source *s, int sd, uint32_t revents,
 		rc = handle_control_get_message_type_support(ctx, sd, &addr,
 							     buf, buf_size);
 		break;
+	case MCTP_CTRL_CMD_GET_VENDOR_MESSAGE_SUPPORT:
+		rc = handle_control_get_vdm_type_support(ctx, sd, &addr, buf,
+							 buf_size);
+		break;
 	case MCTP_CTRL_CMD_RESOLVE_ENDPOINT_ID:
 		rc = handle_control_resolve_endpoint_id(ctx, sd, &addr, buf,
 							buf_size);
@@ -3420,6 +3514,33 @@ static int on_dbus_peer_removed(sd_bus_track *track, void *userdata)
 	return 0;
 }
 
+static int on_dbus_peer_removed_vdm_type(sd_bus_track *track, void *userdata)
+{
+	struct ctx *ctx = userdata;
+	size_t i;
+
+	for (i = 0; i < ctx->num_supported_vdm_types; i++) {
+		struct vdm_type_support *vdm_type =
+			&ctx->supported_vdm_types[i];
+
+		if (vdm_type->source_peer != track)
+			continue;
+		if (ctx->verbose) {
+			warnx("Removing VDM type support entry vid_format %d cmd_set 0x%04x",
+			      vdm_type->vid_format, vdm_type->cmd_set);
+		}
+		if (i != ctx->num_supported_vdm_types - 1) {
+			*vdm_type = ctx->supported_vdm_types
+					    [ctx->num_supported_vdm_types - 1];
+		}
+		ctx->num_supported_vdm_types--;
+		break;
+	}
+
+	sd_bus_track_unref(track);
+	return 0;
+}
+
 static int method_register_type_support(sd_bus_message *call, void *data,
 					sd_bus_error *berr)
 {
@@ -3506,6 +3627,104 @@ static int method_register_type_support(sd_bus_message *call, void *data,
 	return rc;
 }
 
+static int method_register_vdm_type_support(sd_bus_message *call, void *data,
+					    sd_bus_error *berr)
+{
+	struct vdm_type_support new_vdm, *cur_vdm_type, *new_vdm_types_arr;
+	struct ctx *ctx = data;
+	uint16_t vid_pcie;
+	uint32_t vid_iana;
+	int rc;
+
+	rc = sd_bus_message_read(call, "y", &new_vdm.vid_format);
+	if (rc < 0)
+		goto err;
+
+	if (new_vdm.vid_format != MCTP_GET_VDM_SUPPORT_PCIE_FORMAT_ID &&
+	    new_vdm.vid_format != MCTP_GET_VDM_SUPPORT_IANA_FORMAT_ID) {
+		return sd_bus_error_setf(berr, SD_BUS_ERROR_INVALID_ARGS,
+					 "Unsupported VID format: %d",
+					 new_vdm.vid_format);
+	}
+
+	if (new_vdm.vid_format == MCTP_GET_VDM_SUPPORT_PCIE_FORMAT_ID) {
+		rc = sd_bus_message_read(call, "v", "q", &vid_pcie);
+		if (rc < 0)
+			goto err;
+		new_vdm.vendor_id.pcie = vid_pcie;
+	} else if (new_vdm.vid_format == MCTP_GET_VDM_SUPPORT_IANA_FORMAT_ID) {
+		rc = sd_bus_message_read(call, "v", "u", &vid_iana);
+		if (rc < 0)
+			goto err;
+		new_vdm.vendor_id.iana = vid_iana;
+	}
+
+	rc = sd_bus_message_read(call, "q", &new_vdm.cmd_set);
+	if (rc < 0)
+		goto err;
+
+	// Check for duplicates
+	for (size_t i = 0; i < ctx->num_supported_vdm_types; i++) {
+		if (ctx->supported_vdm_types[i].vid_format !=
+		    new_vdm.vid_format)
+			continue;
+
+		if (ctx->supported_vdm_types[i].cmd_set != new_vdm.cmd_set)
+			continue;
+
+		bool vid_matches = false;
+		if (new_vdm.vid_format == MCTP_GET_VDM_SUPPORT_PCIE_FORMAT_ID) {
+			vid_matches =
+				(ctx->supported_vdm_types[i].vendor_id.pcie ==
+				 new_vdm.vendor_id.pcie);
+		} else {
+			vid_matches =
+				(ctx->supported_vdm_types[i].vendor_id.iana ==
+				 new_vdm.vendor_id.iana);
+		}
+
+		if (vid_matches) {
+			return sd_bus_error_setf(berr,
+						 SD_BUS_ERROR_INVALID_ARGS,
+						 "VDM type already registered");
+		}
+	}
+
+	new_vdm_types_arr = realloc(ctx->supported_vdm_types,
+				    (ctx->num_supported_vdm_types + 1) *
+					    sizeof(struct vdm_type_support));
+	if (!new_vdm_types_arr)
+		return sd_bus_error_setf(
+			berr, SD_BUS_ERROR_NO_MEMORY,
+			"Failed to allocate memory for VDM types");
+	ctx->supported_vdm_types = new_vdm_types_arr;
+
+	cur_vdm_type = &ctx->supported_vdm_types[ctx->num_supported_vdm_types];
+	memcpy(cur_vdm_type, &new_vdm, sizeof(struct vdm_type_support));
+
+	// Track peer
+	rc = sd_bus_track_new(ctx->bus, &cur_vdm_type->source_peer,
+			      on_dbus_peer_removed_vdm_type, ctx);
+	if (rc < 0)
+		goto track_err;
+
+	rc = sd_bus_track_add_sender(cur_vdm_type->source_peer, call);
+	if (rc < 0)
+		goto track_err;
+
+	ctx->num_supported_vdm_types++;
+	return sd_bus_reply_method_return(call, "");
+
+track_err:
+	sd_bus_track_unref(cur_vdm_type->source_peer);
+	set_berr(ctx, rc, berr);
+	return rc;
+
+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),
@@ -3868,6 +4087,13 @@ static const sd_bus_vtable mctp_base_vtable[] = {
 	SD_BUS_NO_RESULT,
 	method_register_type_support,
 	0),
+        SD_BUS_METHOD_WITH_ARGS("RegisterVDMTypeSupport",
+        SD_BUS_ARGS("y", format,
+                    "v", format_data,
+                    "q", vendor_subtype),
+        SD_BUS_NO_RESULT,
+        method_register_vdm_type_support,
+        0),
 	SD_BUS_VTABLE_END,
 };
 // clang-format on
@@ -4823,6 +5049,9 @@ static void setup_ctrl_cmd_defaults(struct ctx *ctx)
 	ctx->supported_msg_types = NULL;
 	ctx->num_supported_msg_types = 0;
 
+	ctx->supported_vdm_types = NULL;
+	ctx->num_supported_vdm_types = 0;
+
 	// Default to supporting only control messages
 	ctx->supported_msg_types = malloc(sizeof(struct msg_type_support));
 	if (!ctx->supported_msg_types) {
@@ -4868,6 +5097,9 @@ static void free_ctrl_cmd_defaults(struct ctx *ctx)
 		free(ctx->supported_msg_types[i].versions);
 	}
 	free(ctx->supported_msg_types);
+	free(ctx->supported_vdm_types);
+	ctx->supported_vdm_types = NULL;
+	ctx->num_supported_vdm_types = 0;
 }
 
 static int endpoint_send_allocate_endpoint_ids(

tests/test_mctpd.py

@@ -1290,3 +1290,64 @@ async def test_get_message_types(dbus, mctpd):
     cmd = MCTPControlCommand(True, 0, 0x04, bytes([0x05]))
     rsp = await ep.send_control(mctpd.network.mctp_socket, cmd)
     assert rsp.hex(' ') == '00 04 00 01 f4 f3 f2 f1'
+
+""" Test RegisterVDMTypeSupport with PCIe and IANA VDMs """
+async def test_register_vdm_type_support(mctpd):
+    ep = mctpd.network.endpoints[0]
+    ep.eid = 12
+    iface = mctpd.system.interfaces[0]
+    await mctpd.system.add_route(mctpd.system.Route(ep.eid, 1, iface = iface))
+    await mctpd.system.add_neighbour(
+        mctpd.system.Neighbour(iface, ep.lladdr, ep.eid)
+    )
+
+    # Verify error response when no VDM is registered
+    cmd = MCTPControlCommand(True, 0, 0x06, bytes([0x00]))
+    rsp = await ep.send_control(mctpd.network.mctp_socket, cmd)
+    assert rsp.hex(' ') == '00 06 01'
+
+    async with asyncdbus.MessageBus().connect() as temp_bus:
+        mctp = await mctpd_mctp_base_iface_obj(temp_bus)
+
+        # Register PCIe VDM: format=0x00, VID=0xABCD, command_set=0x0001
+        await mctp.call_register_vdm_type_support(0x00,
+                            asyncdbus.Variant('q', 0xABCD), 0x0001)
+
+        # Register IANA VDM: format=0x01, VID=0x1234ABCD, command_set=0x5678
+        await mctp.call_register_vdm_type_support(0x01,
+                            asyncdbus.Variant('u', 0x1234ABCD), 0x5678)
+
+        # Verify DBus call fails with invalid format 0x02
+        with pytest.raises(asyncdbus.errors.DBusError) as ex:
+            await mctp.call_register_vdm_type_support(0x02,
+                                asyncdbus.Variant('q', 0xABCD), 0x0001)
+        assert "Unsupported VID format" in str(ex.value)
+
+        # Verify DBus call fails with duplicate PCIe VDM
+        with pytest.raises(asyncdbus.errors.DBusError) as ex:
+            await mctp.call_register_vdm_type_support(0x00,
+                            asyncdbus.Variant('q', 0xABCD), 0x0001)
+        assert str(ex.value) == "VDM type already registered"
+
+        # Verify PCIe VDM (selector 0)
+        cmd = MCTPControlCommand(True, 0, 0x06, bytes([0x00]))
+        rsp = await ep.send_control(mctpd.network.mctp_socket, cmd)
+        assert rsp.hex(' ') == '00 06 00 01 00 ab cd 00 01'
+
+        # Verify IANA VDM (selector 1)
+        cmd = MCTPControlCommand(True, 0, 0x06, bytes([0x01]))
+        rsp = await ep.send_control(mctpd.network.mctp_socket, cmd)
+        assert rsp.hex(' ') == '00 06 00 ff 01 12 34 ab cd 56 78'
+
+        # Verify error with incorrect selector
+        cmd = MCTPControlCommand(True, 0, 0x06, bytes([0x05]))
+        rsp = await ep.send_control(mctpd.network.mctp_socket, cmd)
+        assert rsp.hex(' ') == '00 06 02'
+
+    # Give mctpd a moment to process the disconnection
+    await trio.sleep(0.1)
+
+    # Verify VDM types are removed after disconnect
+    cmd = MCTPControlCommand(True, 0, 0x06, bytes([0x00]))
+    rsp = await ep.send_control(mctpd.network.mctp_socket, cmd)
+    assert rsp.hex(' ') == '00 06 01'  # Should be error again