mctpd: add dbus method RegisterVDMTypeSupport

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,21 @@ struct msg_type_support {
 	sd_bus_track *source_peer;
 };
 
+enum vid_format {
+	VID_FORMAT_PCIE = MCTP_GET_VDM_SUPPORT_PCIE_FORMAT_ID,
+	VID_FORMAT_IANA = MCTP_GET_VDM_SUPPORT_IANA_FORMAT_ID,
+};
+
+struct vdm_type_support {
+	enum vid_format 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 +258,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 +1017,75 @@ 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)
+{
+	struct mctp_ctrl_resp_get_vdm_support *resp = NULL;
+	struct mctp_ctrl_cmd_get_vdm_support *req = NULL;
+	size_t resp_len, max_rsp_len, vdm_count;
+	struct vdm_type_support *cur_vdm;
+	uint16_t *cmd_type_ptr;
+	uint8_t *resp_buf;
+	int rc;
+
+	if (buf_size < sizeof(*req)) {
+		warnx("short Get VDM 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_len = max_rsp_len;
+	resp_buf = malloc(max_rsp_len);
+	if (!resp_buf) {
+		warnx("Failed to allocate response buffer");
+		return -ENOMEM;
+	}
+	resp = (void *)resp_buf;
+	cmd_type_ptr = (uint16_t *)(resp + 1);
+	mctp_ctrl_msg_hdr_init_resp(&resp->ctrl_hdr, req->ctrl_hdr);
+
+	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];
+		resp->completion_code = MCTP_CTRL_CC_SUCCESS;
+		resp->vendor_id_set_selector = req->vendor_id_set_selector + 1;
+		if (req->vendor_id_set_selector == (vdm_count - 1)) {
+			resp->vendor_id_set_selector =
+				MCTP_GET_VDM_SUPPORT_NO_MORE_CAP_SET;
+		}
+		resp->vendor_id_format = cur_vdm->format;
+
+		if (cur_vdm->format == VID_FORMAT_PCIE) {
+			// 4 bytes was reserved for VID, but PCIe VID uses only 2 bytes.
+			cmd_type_ptr--;
+			resp_len = max_rsp_len - sizeof(uint16_t);
+			resp->vendor_id_data_pcie =
+				htobe16(cur_vdm->vendor_id.pcie);
+		} else {
+			resp->vendor_id_data_iana =
+				htobe32(cur_vdm->vendor_id.iana);
+		}
+
+		*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 +1289,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 +3511,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 format %d cmd_set 0x%04x",
+			      vdm_type->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 +3624,121 @@ 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;
+	const char *vid_type_str;
+	struct ctx *ctx = data;
+	uint8_t vid_format;
+	uint16_t vid_pcie;
+	uint32_t vid_iana;
+	int rc;
+
+	rc = sd_bus_message_read(call, "y", &vid_format);
+	if (rc < 0)
+		goto err;
+	new_vdm.format = vid_format;
+
+	rc = sd_bus_message_peek_type(call, NULL, &vid_type_str);
+	if (rc < 0) {
+		return sd_bus_error_setf(berr, SD_BUS_ERROR_INVALID_ARGS,
+					 "Failed to read variant type");
+	}
+
+	if (new_vdm.format == VID_FORMAT_PCIE) {
+		if (strcmp(vid_type_str, "q") != 0) {
+			return sd_bus_error_setf(
+				berr, SD_BUS_ERROR_INVALID_ARGS,
+				"Expected format is PCIe but variant contains '%s'",
+				vid_type_str);
+		}
+		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.format == VID_FORMAT_IANA) {
+		if (strcmp(vid_type_str, "u") != 0) {
+			return sd_bus_error_setf(
+				berr, SD_BUS_ERROR_INVALID_ARGS,
+				"Expected format is IANA but variant contains '%s'",
+				vid_type_str);
+		}
+		rc = sd_bus_message_read(call, "v", "u", &vid_iana);
+		if (rc < 0)
+			goto err;
+		new_vdm.vendor_id.iana = vid_iana;
+	} else {
+		return sd_bus_error_setf(berr, SD_BUS_ERROR_INVALID_ARGS,
+					 "Unsupported VID format: %d",
+					 new_vdm.format);
+	}
+
+	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].format != new_vdm.format)
+			continue;
+
+		if (ctx->supported_vdm_types[i].cmd_set != new_vdm.cmd_set)
+			continue;
+
+		bool vid_matches = false;
+		if (new_vdm.format == VID_FORMAT_PCIE) {
+			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 +4101,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 +5063,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 +5111,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(