From f6fbc3b1743848c199c0ad138f3364177652f91b Mon Sep 17 00:00:00 2001 From: Jasmine Cha Date: Thu, 30 Jul 2026 06:07:00 +0000 Subject: [PATCH] mctpd: Support MCTP Discovery Notify command In MCTP networks, endpoints broadcast or send a Discovery Notify control request (0x0D) to inform the Bus Owner when they boot up, reset, or are hot-plugged. Currently, mctpd relies on active bus scanning or static configurations, leaving newly online endpoints undiscovered until the next poll cycle. This change adds support for handling Discovery Notify requests when mctpd is in the Bus Owner role: - Immediately acknowledges Discovery Notify requests over the physical socket with MCTP_CTRL_CC_SUCCESS inside handle_control_discovery_notify() to prevent hardware endpoints (e.g. PCIe VDMs / SMBus sequencers) from dropping subsequent Set Endpoint ID requests while waiting for an ACK. - Asynchronously defers EID assignment and route programming (RTM_NEWROUTE) to the systemd event loop (sd_event_add_defer) to prevent thread blocking. - Safely handles event deferral failures by dropping the request and returning -ENOMEM rather than executing synchronously in receive context, eliminating re-entrance and event loop corruption risks. - Scopes request deduplication to (ifindex, dest_phys) composite keys to prevent cross-bus address collisions on multi-bus platforms. - Enforces per-interface rate-limiting (maximum 5 discovery requests per second) to protect against discovery request floods. - Correctly supports Bridge Endpoints by reserving EID pools and invoking endpoint_allocate_eids() for downstream pool allocation and gateway routing. - Includes comprehensive unit test coverage in test_mctpd.py and test_mctpd_endpoint.py. Assisted-by: Antigravity:Gemini-Next Signed-off-by: Jasmine Cha --- src/mctp-netlink.c | 1 + src/mctpd.c | 268 ++++++++++++++++++++++++++++++++++- tests/test_mctpd.py | 126 ++++++++++++++++ tests/test_mctpd_endpoint.py | 15 +- 4 files changed, 405 insertions(+), 5 deletions(-) diff --git a/src/mctp-netlink.c b/src/mctp-netlink.c index 9cd1571d..4381a523 100644 --- a/src/mctp-netlink.c +++ b/src/mctp-netlink.c @@ -1348,6 +1348,7 @@ int mctp_nl_route_add(struct mctp_nl *nl, uint8_t eid, unsigned int extent, return -1; } msg.nh.nlmsg_type = RTM_NEWROUTE; + msg.nh.nlmsg_flags |= NLM_F_CREATE | NLM_F_REPLACE; if (mtu != 0) { /* Nested diff --git a/src/mctpd.c b/src/mctpd.c index a1d27282..1d30e291 100644 --- a/src/mctpd.c +++ b/src/mctpd.c @@ -140,6 +140,15 @@ enum discovery_state { DISCOVERY_UNDISCOVERED, }; +#define PEER_FLAG_ASSIGN_PENDING (1 << 0) + +struct discovery_notify_ctx { + struct ctx *ctx; + dest_phys phys; + mctp_eid_t existing_eid; + struct discovery_notify_ctx *next; +}; + struct link { enum discovery_state discovered; bool published; @@ -153,6 +162,9 @@ struct link { sd_bus_slot *slot_busowner; sd_event_source *role_defer; + uint64_t last_discovery_time_us; + uint32_t discovery_count; + struct ctx *ctx; }; @@ -226,6 +238,7 @@ struct peer { struct { sd_event_source **sources; } bridge_ep_poll; + uint32_t flags; }; struct msg_type_support { @@ -314,6 +327,7 @@ struct ctx { // bus owner/bridge polling interval in usecs for // checking endpoint's accessibility. uint64_t endpoint_poll; + struct discovery_notify_ctx *pending_discoveries; // interface configuration (from config file), to be matched and // applied on new interface events @@ -364,6 +378,10 @@ static int add_peer(struct ctx *ctx, const dest_phys *dest, mctp_eid_t eid, static int add_peer_from_addr(struct ctx *ctx, const struct sockaddr_mctp_ext *addr, struct peer **ret_peer); +static int change_peer_eid(struct peer *peer, mctp_eid_t new_eid); +static int endpoint_assign_eid(struct ctx *ctx, sd_bus_error *berr, + const dest_phys *dest, struct peer **ret_peer, + mctp_eid_t static_eid, bool assign_bridge); static int remove_peer(struct peer *peer); static int remove_bridged_peers(struct peer *bridge); static int query_peer_properties(struct peer *peer); @@ -750,9 +768,15 @@ static int reply_message(struct ctx *ctx, int sd, const void *resp, if (reply_addr.smctp_addr.s_addr == 0 || reply_addr.smctp_addr.s_addr == 0xff) { - bug_warn("reply_message can't take EID %d", - reply_addr.smctp_addr.s_addr); - return -EPROTO; + /* Validate physical address metadata before falling back to physical send */ + if (addr && addr->smctp_ifindex > 0 && + addr->smctp_halen <= sizeof(addr->smctp_haddr)) { + return reply_message_phys(ctx, sd, resp, resp_len, + addr); + } + warnx("reply_message: EID %d specified without valid physical address info", + reply_addr.smctp_addr.s_addr); + return -EINVAL; } len = mctp_ops.mctp.sendto(sd, resp, resp_len, 0, @@ -1329,6 +1353,209 @@ handle_control_endpoint_discovery(struct ctx *ctx, int sd, return reply_message_phys(ctx, sd, resp, sizeof(*resp), addr); } +/* + * Rate limiting is evaluated per-link (interface) rather than per-physical-MAC address + * for performance and simplicity. On shared multi-drop buses (e.g. I2C/SMBus), a noisy + * endpoint could temporarily consume the 5 req/sec quota; healthy adjacent endpoints will + * naturally retransmit Discovery Notify upon timeout. + */ +static bool link_check_rate_limit(struct link *link_data) +{ + struct timespec ts; + uint64_t now_us; + + if (clock_gettime(CLOCK_MONOTONIC, &ts) < 0) + return true; + + now_us = (uint64_t)ts.tv_sec * 1000000ULL + ts.tv_nsec / 1000ULL; + if (now_us - link_data->last_discovery_time_us > 1000000ULL) { + link_data->last_discovery_time_us = now_us; + link_data->discovery_count = 0; + } + + if (link_data->discovery_count >= 5) { + return false; + } + + link_data->discovery_count++; + return true; +} + +static bool is_discovery_pending(struct ctx *ctx, int ifindex, + const dest_phys *phys) +{ + struct discovery_notify_ctx *d; + struct peer *peer; + + peer = find_peer_by_phys(ctx, phys); + if (peer && (peer->flags & PEER_FLAG_ASSIGN_PENDING)) + return true; + + for (d = ctx->pending_discoveries; d; d = d->next) { + if (d->phys.ifindex == ifindex && match_phys(&d->phys, phys)) + return true; + } + return false; +} + +static void remove_pending_discovery(struct ctx *ctx, + struct discovery_notify_ctx *dctx) +{ + struct discovery_notify_ctx **pp = &ctx->pending_discoveries; + while (*pp) { + if (*pp == dctx) { + *pp = dctx->next; + break; + } + pp = &(*pp)->next; + } +} + +static int deferred_assign_eid_cb(sd_event_source *s, void *userdata) +{ + struct discovery_notify_ctx *dctx = userdata; + struct peer *peer = NULL; + mctp_eid_t target_eid; + bool assign_bridge; + int rc; + + (void)s; /* Floating event source unref is managed automatically by sd_event */ + + peer = find_peer_by_phys(dctx->ctx, &dctx->phys); + target_eid = peer ? peer->eid : dctx->existing_eid; + assign_bridge = + peer ? (peer->pool_size > 0 || peer->slot_bridge != NULL) : + false; + + rc = endpoint_assign_eid(dctx->ctx, NULL, &dctx->phys, &peer, + target_eid, assign_bridge); + + /* Fallback to dynamic allocation if target EID collides (-EEXIST) */ + if (rc == -EEXIST && target_eid != 0) { + if (dctx->ctx->verbose) { + warnx("EID %d occupied for %s, retrying dynamic EID allocation", + target_eid, dest_phys_tostr(&dctx->phys)); + } + rc = endpoint_assign_eid(dctx->ctx, NULL, &dctx->phys, &peer, 0, + assign_bridge); + } + + peer = find_peer_by_phys(dctx->ctx, &dctx->phys); + if (peer) { + peer->flags &= ~PEER_FLAG_ASSIGN_PENDING; + } + + if (rc == 0) { + if (peer && peer->pool_size > 0) { + endpoint_allocate_eids(peer); + } + } else if (dctx->ctx->verbose) { + warnx("Deferred EID assignment failed for %s: %s", + dest_phys_tostr(&dctx->phys), strerror(-rc)); + } + + /* Keep pending context linked until execution completes to prevent deduplication races */ + remove_pending_discovery(dctx->ctx, dctx); + free(dctx); + return 0; +} + +static int handle_control_discovery_notify(struct ctx *ctx, int sd, + const struct sockaddr_mctp_ext *addr, + const uint8_t *buf, + const size_t buf_size) +{ + struct mctp_ctrl_resp_discovery_notify respi = { 0 }, *resp = &respi; + struct discovery_notify_ctx *dctx = NULL; + struct mctp_ctrl_msg_hdr *req = NULL; + struct link *link_data; + struct peer *peer = NULL; + dest_phys phys = { 0 }; + int rc; + + if (buf_size < sizeof(*req)) { + warnx("short Discovery Notify message"); + return -ENOMSG; + } + req = (void *)buf; + + link_data = mctp_nl_get_link_userdata(ctx->nl, addr->smctp_ifindex); + if (!link_data) { + bug_warn("unconfigured interface %d", addr->smctp_ifindex); + return -ENOENT; + } + + if (link_data->role != ENDPOINT_ROLE_BUS_OWNER) { + if (ctx->verbose) { + warnx("Ignoring Discovery Notify on interface %d: not a bus owner", + addr->smctp_ifindex); + } + return 0; + } + + if (!link_check_rate_limit(link_data)) { + if (ctx->verbose) { + warnx("Discovery Notify rate limit exceeded on interface %d", + addr->smctp_ifindex); + } + return 0; + } + + /* Respond immediately over physical socket to acknowledge Discovery Notify */ + mctp_ctrl_msg_hdr_init_resp(&respi.ctrl_hdr, *req); + resp->completion_code = MCTP_CTRL_CC_SUCCESS; + + rc = reply_message_phys(ctx, sd, resp, sizeof(*resp), addr); + if (rc < 0) { + warnx("Failed to send Discovery Notify response on ifindex %d: %s", + addr->smctp_ifindex, strerror(-rc)); + return rc; + } + + phys.ifindex = addr->smctp_ifindex; + phys.hwaddr_len = addr->smctp_halen; + memcpy(phys.hwaddr, addr->smctp_haddr, addr->smctp_halen); + + /* Deduplicate incoming Discovery Notify for both existing and new physical endpoints */ + if (is_discovery_pending(ctx, addr->smctp_ifindex, &phys)) { + return 0; + } + + peer = find_peer_by_phys(ctx, &phys); + if (peer) { + peer->flags |= PEER_FLAG_ASSIGN_PENDING; + } + + dctx = calloc(1, sizeof(*dctx)); + if (!dctx) { + if (peer) + peer->flags &= ~PEER_FLAG_ASSIGN_PENDING; + return -ENOMEM; + } + + dctx->ctx = ctx; + dctx->phys = phys; + dctx->existing_eid = peer ? peer->eid : 0; + + dctx->next = ctx->pending_discoveries; + ctx->pending_discoveries = dctx; + + if (sd_event_add_defer(ctx->event, NULL, deferred_assign_eid_cb, dctx) < + 0) { + if (ctx->verbose) { + warnx("Failed to defer EID assignment event for %s", + dest_phys_tostr(&phys)); + } + remove_pending_discovery(ctx, dctx); + if (peer) + peer->flags &= ~PEER_FLAG_ASSIGN_PENDING; + free(dctx); + return -ENOMEM; + } + + return 0; +} + static int handle_control_unsupported(struct ctx *ctx, int sd, const struct sockaddr_mctp_ext *addr, const uint8_t *buf, const size_t buf_size) @@ -1423,6 +1650,10 @@ static int cb_listen_control_msg(sd_event_source *s, int sd, uint32_t revents, rc = handle_control_endpoint_discovery(ctx, sd, &addr, buf, buf_size); break; + case MCTP_CTRL_CMD_DISCOVERY_NOTIFY: + rc = handle_control_discovery_notify(ctx, sd, &addr, buf, + buf_size); + break; default: if (ctx->verbose) { warnx("Ignoring unsupported command code 0x%02x", @@ -2004,6 +2235,7 @@ static int add_peer(struct ctx *ctx, const dest_phys *dest, mctp_eid_t eid, { struct peer *peer, **tmp; struct net *n; + int rc; n = lookup_net(ctx, net); if (!n) { @@ -2025,6 +2257,26 @@ static int add_peer(struct ctx *ctx, const dest_phys *dest, mctp_eid_t eid, if (!allow_bridged && is_eid_in_bridge_pool(n, ctx, eid)) return -EEXIST; + /* Re-use existing physical peer mapping safely via change_peer_eid */ + if (!allow_bridged) { + peer = find_peer_by_phys(ctx, dest); + if (peer) { + if (peer->eid == eid) { + *ret_peer = peer; + return 0; + } + rc = change_peer_eid(peer, eid); + if (rc < 0) { + if (rc == -EEXIST) + warnx("add_peer: target EID %d already occupied by different peer", + eid); + return rc; + } + *ret_peer = peer; + return 0; + } + } + if (ctx->num_peers == MAX_PEER_SIZE) return -ENOMEM; @@ -2216,6 +2468,8 @@ static int remove_peer(struct peer *peer) static void free_peers(struct ctx *ctx) { + struct discovery_notify_ctx *dctx, *tmp_dctx; + for (size_t i = 0; i < ctx->num_peers; i++) { struct peer *peer = ctx->peers[i]; free(peer->message_types); @@ -2231,6 +2485,14 @@ static void free_peers(struct ctx *ctx) } free(ctx->peers); + + dctx = ctx->pending_discoveries; + while (dctx) { + tmp_dctx = dctx->next; + free(dctx); + dctx = tmp_dctx; + } + ctx->pending_discoveries = NULL; } /* Returns -EEXIST if the new_eid is already used */ diff --git a/tests/test_mctpd.py b/tests/test_mctpd.py index 78537947..c1ff9d5f 100644 --- a/tests/test_mctpd.py +++ b/tests/test_mctpd.py @@ -2339,3 +2339,129 @@ async def test_iface_config_match_path_none(dbus, sysnet, nursery): res = await mctpd.stop_mctpd() assert res == 0 + + +async def test_discovery_notify_bus_owner_success(dbus, mctpd): + """Test Discovery Notify processing when mctpd is in Bus Owner role. + + When an endpoint issues Discovery Notify (0x0D), mctpd immediately + acknowledges over physical socket and defers EID assignment. + """ + ep = mctpd.network.endpoints[0] + + # Endpoint has no EID yet + assert ep.eid is None or ep.eid == 0 + + # Send Discovery Notify (Command Code 0x0D, Request bit set, IID 1) + cmd = MCTPControlCommand(True, 1, 0x0D) + rsp = await ep.send_control(mctpd.network.mctp_socket, cmd) + + # Expect immediate ACK with MCTP_CTRL_CC_SUCCESS (0x00) and IID 1 (0x01 0x0d 0x00) + assert rsp.hex(' ') == '01 0d 00' + + # Allow deferred EID assignment event to execute + await trio.sleep(0.1) + + # Verify endpoint object created on D-Bus and EID assigned + assert ep.eid is not None and ep.eid != 0 + assert await mctpd_mctp_endpoint_control_obj( + dbus, f"/au/com/codeconstruct/mctp1/networks/1/endpoints/{ep.eid}" + ) + + # Verify neighbour and route entries created in kernel + assert len(mctpd.system.neighbours) == 1 + assert mctpd.system.neighbours[0].lladdr == ep.lladdr + assert mctpd.system.neighbours[0].eid == ep.eid + assert len(mctpd.system.routes) == 1 + + +async def test_discovery_notify_deduplication(dbus, mctpd): + """Verify deduplication when multiple Discovery Notify requests arrive in rapid succession.""" + ep = mctpd.network.endpoints[0] + + # Send two Discovery Notify requests back-to-back before yielding to event loop + cmd1 = MCTPControlCommand(True, 1, 0x0D) + cmd2 = MCTPControlCommand(True, 2, 0x0D) + + rsp1 = await ep.send_control(mctpd.network.mctp_socket, cmd1) + rsp2 = await ep.send_control(mctpd.network.mctp_socket, cmd2) + + assert rsp1.hex(' ') == '01 0d 00' + assert rsp2.hex(' ') == '02 0d 00' + + await trio.sleep(0.1) + + # Ensure single EID assignment and valid D-Bus object path + assert ep.eid is not None and ep.eid != 0 + assert await mctpd_mctp_endpoint_control_obj( + dbus, f"/au/com/codeconstruct/mctp1/networks/1/endpoints/{ep.eid}" + ) + + +async def test_discovery_notify_existing_endpoint(dbus, mctpd, routed_ep): + """Verify Discovery Notify on an already assigned endpoint preserves EID and D-Bus object.""" + ep = routed_ep + original_eid = ep.eid + + cmd = MCTPControlCommand(True, 3, 0x0D) + rsp = await ep.send_control(mctpd.network.mctp_socket, cmd) + assert rsp.hex(' ') == '03 0d 00' + + await trio.sleep(0.1) + + # Endpoint EID should remain unchanged + assert ep.eid == original_eid + assert await mctpd_mctp_endpoint_control_obj( + dbus, f"/au/com/codeconstruct/mctp1/networks/1/endpoints/{original_eid}" + ) + + +async def test_discovery_notify_short_message(mctpd): + """Verify truncated Discovery Notify message is rejected without crash.""" + ep = mctpd.network.endpoints[0] + + # Send 1-byte raw message (< sizeof(struct mctp_ctrl_msg_hdr)) + # mctpd should drop without reply (timing out on response wait) + addr = MCTPSockAddr(ep.iface.net, ep.eid or 0, 0, 0x80) + if mctpd.network.mctp_socket.addr_ext: + addr.set_ext(ep.iface.ifindex, ep.lladdr) + + await mctpd.network.mctp_socket.send(addr, bytes([0x80])) + await trio.sleep(0.1) + + # Verify no endpoint was assigned or created from truncated message + assert ep.eid is None or ep.eid == 0 + + +async def test_discovery_notify_cleanup_on_shutdown(dbus, sysnet, nursery): + """Verify free_peers cleans up pending discoveries safely on shutdown.""" + mctpd = MctpdWrapper(dbus, sysnet) + await mctpd.start_mctpd(nursery) + + ep = mctpd.network.endpoints[0] + + # Send Discovery Notify + cmd = MCTPControlCommand(True, 1, 0x0D) + rsp = await ep.send_control(mctpd.network.mctp_socket, cmd) + assert rsp.hex(' ') == '01 0d 00' + + # Immediately stop mctpd before deferred callback completes + res = await mctpd.stop_mctpd() + assert res == 0 + + +async def test_discovery_notify_rate_limit(dbus, mctpd): + """Verify rate-limiting drops excessive Discovery Notify requests exceeding 5 per second.""" + ep = mctpd.network.endpoints[0] + + # Send 5 valid Discovery Notify requests + for i in range(1, 6): + cmd = MCTPControlCommand(True, i, 0x0D) + rsp = await ep.send_control(mctpd.network.mctp_socket, cmd) + assert rsp.hex(' ') == f'{i:02x} 0d 00' + + # 6th request within same second should be rate-limited and dropped + cmd6 = MCTPControlCommand(True, 6, 0x0D) + with trio.move_on_after(0.2) as scope: + await ep.send_control(mctpd.network.mctp_socket, cmd6) + assert scope.cancelled_caught diff --git a/tests/test_mctpd_endpoint.py b/tests/test_mctpd_endpoint.py index f29a967a..89730f46 100644 --- a/tests/test_mctpd_endpoint.py +++ b/tests/test_mctpd_endpoint.py @@ -1,8 +1,9 @@ -import pytest import asyncdbus +import pytest +import trio from mctp_test_utils import ( - mctpd_mctp_iface_control_obj, mctpd_mctp_endpoint_control_obj, + mctpd_mctp_iface_control_obj, ) from mctpenv import ( Endpoint, @@ -218,3 +219,13 @@ async def test_simple(self, dbus, mctpd): mctpd.network.mctp_socket, MCTPControlCommand(True, 0, 0x0B) ) assert rsp.hex(' ') == '00 0b 05' + + async def test_discovery_notify_ignored(self, dbus, mctpd): + """Discovery Notify command on endpoint interface (not bus owner) is ignored.""" + bo = mctpd.network.endpoints[0] + + # Send Discovery Notify (0x0D) to mctpd running in endpoint role + cmd = MCTPControlCommand(True, 1, 0x0D) + with trio.move_on_after(0.5) as scope: + await bo.send_control(mctpd.network.mctp_socket, cmd) + assert scope.cancelled_caught