mctpd: Send Discovery Notify on Endpoint role set

meson.build

@@ -60,11 +60,13 @@ toml_dep = declare_dependency(
 executable('mctp',
     sources: ['src/mctp.c'] + netlink_sources + util_sources + ops_sources,
     install: true,
+    c_args: ['-DHAVE_LIBSYSTEMD=0'],
 )
 
 mctp_test = executable('test-mctp',
     sources: ['src/mctp.c'] + netlink_sources + util_sources + test_ops_sources,
     include_directories:  include_directories('src'),
+    c_args: ['-DHAVE_LIBSYSTEMD=0'],
 )
 
 executable('mctp-req',
@@ -92,6 +94,7 @@ if libsystemd.found()
         dependencies: [libsystemd, toml_dep],
         install: true,
         install_dir: get_option('sbindir'),
+        c_args: ['-DHAVE_LIBSYSTEMD=1'],
     )
 
     mctpd_test = executable('test-mctpd',
@@ -100,6 +103,7 @@ if libsystemd.found()
         ] + test_ops_sources + netlink_sources + util_sources,
         include_directories:  include_directories('src'),
         dependencies: [libsystemd, toml_dep],
+        c_args: ['-DHAVE_LIBSYSTEMD=1'],
     )
 endif
 

src/mctp-ops.c

@@ -9,6 +9,9 @@
 
 #include <unistd.h>
 #include <linux/netlink.h>
+#if HAVE_LIBSYSTEMD
+#include <systemd/sd-event.h>
+#endif
 #include <err.h>
 
 #include "mctp.h"
@@ -74,6 +77,12 @@ const struct mctp_ops mctp_ops = {
 		.recvfrom = mctp_op_recvfrom,
 		.close = mctp_op_close,
 	},
+#if HAVE_LIBSYSTEMD
+	.sd_event = {
+		.add_time_relative = sd_event_add_time_relative,
+		.source_set_time_relative = sd_event_source_set_time_relative,
+	},
+#endif
 	.bug_warn = mctp_bug_warn,
 };
 

src/mctp-ops.h

@@ -7,6 +7,7 @@
  */
 #pragma once
 
+#include <stdint.h>
 #include <sys/socket.h>
 #include <stdarg.h>
 
@@ -24,9 +25,23 @@ struct socket_ops {
 	int (*close)(int sd);
 };
 
+struct sd_event;
+struct sd_event_source;
+struct sd_event_ops {
+	int (*add_time_relative)(struct sd_event *e,
+				 struct sd_event_source **ret, clockid_t clock,
+				 uint64_t usec, uint64_t accuracy,
+				 int (*callback)(struct sd_event_source *s,
+						 uint64_t usec, void *userdata),
+				 void *userdata);
+	int (*source_set_time_relative)(struct sd_event_source *s,
+					uint64_t usec);
+};
+
 struct mctp_ops {
 	struct socket_ops mctp;
 	struct socket_ops nl;
+	struct sd_event_ops sd_event;
 	void (*bug_warn)(const char *fmt, va_list args);
 };
 

src/mctpd.c

@@ -126,7 +126,6 @@ enum discovery_state {
 };
 
 struct link {
-	enum discovery_state discovered;
 	bool published;
 	int ifindex;
 	enum endpoint_role role;
@@ -135,6 +134,14 @@ struct link {
 	sd_bus_slot *slot_iface;
 	sd_bus_slot *slot_busowner;
 
+	struct {
+		enum discovery_state flag;
+		sd_event_source *notify_source;
+		dest_phys notify_dest;
+		uint64_t notify_retry_delay;
+		uint8_t notify_tries_left;
+	} discovery;
+
 	struct ctx *ctx;
 };
 
@@ -497,8 +504,9 @@ static int wait_fd_timeout(int fd, short events, uint64_t timeout_usec)
 	if (rc < 0)
 		goto out;
 
-	rc = sd_event_add_time_relative(ev, NULL, CLOCK_MONOTONIC, timeout_usec,
-					0, cb_exit_loop_timeout, NULL);
+	rc = mctp_ops.sd_event.add_time_relative(ev, NULL, CLOCK_MONOTONIC,
+						 timeout_usec, 0,
+						 cb_exit_loop_timeout, NULL);
 	if (rc < 0)
 		goto out;
 
@@ -804,8 +812,8 @@ static int handle_control_set_endpoint_id(struct ctx *ctx, int sd,
 			warnx("ERR: cannot add bus owner to object lists");
 		}
 
-		if (link_data->discovered != DISCOVERY_UNSUPPORTED) {
-			link_data->discovered = DISCOVERY_DISCOVERED;
+		if (link_data->discovery.flag != DISCOVERY_UNSUPPORTED) {
+			link_data->discovery.flag = DISCOVERY_DISCOVERED;
 		}
 		resp->status =
 			SET_MCTP_EID_ASSIGNMENT_STATUS(MCTP_SET_EID_ACCEPTED) |
@@ -816,13 +824,13 @@ static int handle_control_set_endpoint_id(struct ctx *ctx, int sd,
 		return reply_message(ctx, sd, resp, resp_len, addr);
 
 	case MCTP_SET_EID_DISCOVERED:
-		if (link_data->discovered == DISCOVERY_UNSUPPORTED) {
+		if (link_data->discovery.flag == DISCOVERY_UNSUPPORTED) {
 			resp->completion_code = MCTP_CTRL_CC_ERROR_INVALID_DATA;
 			resp_len = sizeof(struct mctp_ctrl_resp);
 			return reply_message(ctx, sd, resp, resp_len, addr);
 		}
 
-		link_data->discovered = DISCOVERY_DISCOVERED;
+		link_data->discovery.flag = DISCOVERY_DISCOVERED;
 		resp->status =
 			SET_MCTP_EID_ASSIGNMENT_STATUS(MCTP_SET_EID_REJECTED) |
 			SET_MCTP_EID_ALLOCATION_STATUS(MCTP_SET_EID_POOL_NONE);
@@ -1060,16 +1068,16 @@ static int handle_control_prepare_endpoint_discovery(
 	resp = (void *)resp;
 	mctp_ctrl_msg_hdr_init_resp(&resp->ctrl_hdr, *req);
 
-	if (link_data->discovered == DISCOVERY_UNSUPPORTED) {
+	if (link_data->discovery.flag == DISCOVERY_UNSUPPORTED) {
 		warnx("received prepare for discovery request to unsupported interface %d",
 		      addr->smctp_ifindex);
 		resp->completion_code = MCTP_CTRL_CC_ERROR_UNSUPPORTED_CMD;
 		return reply_message_phys(ctx, sd, resp,
 					  sizeof(struct mctp_ctrl_resp), addr);
 	}
 
-	if (link_data->discovered == DISCOVERY_DISCOVERED) {
-		link_data->discovered = DISCOVERY_UNDISCOVERED;
+	if (link_data->discovery.flag == DISCOVERY_DISCOVERED) {
+		link_data->discovery.flag = DISCOVERY_UNDISCOVERED;
 		warnx("clear discovered flag of interface %d",
 		      addr->smctp_ifindex);
 	}
@@ -1104,13 +1112,13 @@ handle_control_endpoint_discovery(struct ctx *ctx, int sd,
 		return 0;
 	}
 
-	if (link_data->discovered == DISCOVERY_UNSUPPORTED) {
+	if (link_data->discovery.flag == DISCOVERY_UNSUPPORTED) {
 		resp->completion_code = MCTP_CTRL_CC_ERROR_INVALID_DATA;
 		return reply_message(ctx, sd, resp,
 				     sizeof(struct mctp_ctrl_resp), addr);
 	}
 
-	if (link_data->discovered == DISCOVERY_DISCOVERED) {
+	if (link_data->discovery.flag == DISCOVERY_DISCOVERED) {
 		// if we are already discovered (i.e, assigned an EID), then no reply
 		return 0;
 	}
@@ -3239,8 +3247,8 @@ static int peer_endpoint_recover(sd_event_source *s, uint64_t usec,
 
 reschedule:
 	if (peer->recovery.npolls > 0) {
-		rc = sd_event_source_set_time_relative(peer->recovery.source,
-						       peer->recovery.delay);
+		rc = mctp_ops.sd_event.source_set_time_relative(
+			peer->recovery.source, peer->recovery.delay);
 		if (rc >= 0) {
 			rc = sd_event_source_set_enabled(peer->recovery.source,
 							 SD_EVENT_ONESHOT);
@@ -3275,7 +3283,7 @@ static int method_endpoint_recover(sd_bus_message *call, void *data,
 		peer->recovery.npolls = MCTP_I2C_TSYM_MN1_MIN + 1;
 		peer->recovery.delay =
 			(MCTP_I2C_TSYM_TRECLAIM_MIN_US / 2) - ctx->mctp_timeout;
-		rc = sd_event_add_time_relative(
+		rc = mctp_ops.sd_event.add_time_relative(
 			ctx->event, &peer->recovery.source, CLOCK_MONOTONIC, 0,
 			ctx->mctp_timeout, peer_endpoint_recover, peer);
 		if (rc < 0) {
@@ -3658,6 +3666,88 @@ static int bus_link_get_prop(sd_bus *bus, const char *path,
 	return rc;
 }
 
+static int query_discovery_notify(struct link *link)
+{
+	struct mctp_ctrl_cmd_discovery_notify req = { 0 };
+	struct mctp_ctrl_resp_discovery_notify *resp;
+	struct sockaddr_mctp_ext resp_addr;
+	size_t buf_size;
+	uint8_t *buf;
+	int rc;
+
+	mctp_ctrl_msg_hdr_init_req(&req.ctrl_hdr, mctp_next_iid(link->ctx),
+				   MCTP_CTRL_CMD_DISCOVERY_NOTIFY);
+
+	rc = endpoint_query_phys(link->ctx, &link->discovery.notify_dest,
+				 MCTP_CTRL_HDR_MSG_TYPE, &req, sizeof(req),
+				 &buf, &buf_size, &resp_addr);
+	if (rc < 0)
+		goto free_buf;
+
+	if (buf_size != sizeof(*resp)) {
+		warnx("%s: wrong reply length %zu bytes. dest %s", __func__,
+		      buf_size, dest_phys_tostr(&link->discovery.notify_dest));
+		rc = -ENOMSG;
+		goto free_buf;
+	}
+
+	resp = (void *)buf;
+	if (resp->completion_code != 0) {
+		warnx("Failure completion code 0x%02x from %s",
+		      resp->completion_code,
+		      dest_phys_tostr(&link->discovery.notify_dest));
+		rc = -ECONNREFUSED;
+		goto free_buf;
+	}
+
+free_buf:
+	free(buf);
+	return rc;
+}
+
+static int link_discovery_notify_callback(sd_event_source *source,
+					  uint64_t time, void *userdata)
+{
+	struct link *link = userdata;
+	struct ctx *ctx = link->ctx;
+	int rc;
+
+	// sanity check
+	assert(link->discovery.notify_source == source);
+
+	// Discovery notify succeeded
+	if (link->discovery.flag == DISCOVERY_DISCOVERED)
+		goto disarm;
+
+	rc = query_discovery_notify(link);
+	if (rc < 0) {
+		if (ctx->verbose) {
+			warnx("failed to send discovery notify at retry %d: %s",
+			      link->discovery.notify_tries_left, strerror(-rc));
+		}
+	}
+
+	link->discovery.notify_tries_left -= 1;
+	if (link->discovery.notify_tries_left == 0) {
+		warnx("failed to send discovery notify after all retries");
+		goto disarm;
+	}
+
+	rc = mctp_ops.sd_event.source_set_time_relative(
+		source, link->discovery.notify_retry_delay);
+	if (rc < 0) {
+		warnx("failed to rearm discovery notify timer");
+		goto disarm;
+	}
+
+	return 0;
+
+disarm:
+	sd_event_source_disable_unref(source);
+	link->discovery.notify_source = NULL;
+	return 0;
+}
+
 static int bus_link_set_prop(sd_bus *bus, const char *path,
 			     const char *interface, const char *property,
 			     sd_bus_message *value, void *userdata,
@@ -4495,7 +4585,7 @@ static int add_interface(struct ctx *ctx, int ifindex)
 	if (!link)
 		return -ENOMEM;
 
-	link->discovered = DISCOVERY_UNSUPPORTED;
+	link->discovery.flag = DISCOVERY_UNSUPPORTED;
 	link->published = false;
 	link->ifindex = ifindex;
 	link->ctx = ctx;
@@ -4525,7 +4615,42 @@ static int add_interface(struct ctx *ctx, int ifindex)
 	}
 
 	if (phys_binding == MCTP_PHYS_BINDING_PCIE_VDM) {
-		link->discovered = DISCOVERY_UNDISCOVERED;
+		link->discovery.flag = DISCOVERY_UNDISCOVERED;
+		// TODO: These numbers are respectively MN1 and MT4, specified in DSP0239
+		// control message timing.
+		//
+		// Might need to extract these to macros like MCTP_I2C_TSYM_* in this file,
+		// or a commit to actually centralize those timing at one place, now that
+		// we have support for detecting link binding type.
+		link->discovery.notify_tries_left = 3;
+		link->discovery.notify_retry_delay = 5000000;
+
+		// For PCIe-VDM, we want an all zeroes address for Route-to-Root-Complex.
+		rc = mctp_nl_hwaddr_len_byindex(
+			ctx->nl, ifindex,
+			&link->discovery.notify_dest.hwaddr_len);
+		if (rc < 0) {
+			warnx("Can't find hwaddr_len by index %d", ifindex);
+			return -ENOENT;
+		}
+
+		memset(link->discovery.notify_dest.hwaddr, 0,
+		       link->discovery.notify_dest.hwaddr_len);
+		link->discovery.notify_dest.ifindex = ifindex;
+
+		rc = mctp_ops.sd_event.add_time_relative(
+			ctx->event, &link->discovery.notify_source,
+			CLOCK_MONOTONIC, 0, 0, link_discovery_notify_callback,
+			link);
+		if (rc >= 0) {
+			rc = sd_event_source_set_enabled(
+				link->discovery.notify_source, SD_EVENT_ON);
+		}
+		if (rc < 0) {
+			warnx("Failed to arm discovery notify timer");
+			sd_event_source_disable_unref(
+				link->discovery.notify_source);
+		}
 	}
 
 	link->published = true;

tests/conftest.py

@@ -3,6 +3,7 @@
 
 import pytest
 import asyncdbus
+import trio.testing
 
 import mctpenv
 
@@ -35,3 +36,10 @@ async def mctpd(nursery, dbus, sysnet, config):
 @pytest.fixture
 async def mctp(nursery, sysnet):
     return mctpenv.MctpWrapper(nursery, sysnet)
+
+@pytest.fixture
+def autojump_clock():
+    """
+    Custom autojump clock with a reasonable threshold for non-time I/O waits
+    """
+    return trio.testing.MockClock(autojump_threshold=0.01)