Implement ALLOCATE_ENDPOINT_ID support for bridges

Add implementation for the MCTP ALLOCATE_ENDPOINT_ID control command
to enable bridges to allocate EID pools for downstream endpoints.

Update gateway route for downstream EIDs

Signed-off-by: Faizan Ali <[email protected]>

Author: faizana-nvidia

src/mctpd.c

@@ -252,6 +252,7 @@ static int del_local_eid(struct ctx *ctx, uint32_t net, int eid);
 static int add_net(struct ctx *ctx, uint32_t net);
 static void del_net(struct net *net);
 static int add_interface(struct ctx *ctx, int ifindex);
+static int endpoint_allocate_eid(struct peer *peer);
 
 static const sd_bus_vtable bus_endpoint_obmc_vtable[];
 static const sd_bus_vtable bus_endpoint_cc_vtable[];
@@ -2255,7 +2256,18 @@ static int method_assign_endpoint(sd_bus_message *call, void *data,
 		goto err;
 
 	if (peer->pool_size > 0) {
-		//TODO: Implement Allocate EndpointID
+		rc = endpoint_allocate_eid(peer);
+		if (rc < 0) {
+			warnx("Failed to allocate downstream EIDs");
+		} else {
+			if (peer->ctx->verbose) {
+				fprintf(stderr,
+					"Downstream EIDs assigned from %d to %d : pool size %d\n",
+					peer->pool_start,
+					peer->pool_start + peer->pool_size - 1,
+					peer->pool_size);
+			}
+		}
 	}
 
 	return sd_bus_reply_method_return(call, "yisb", peer->eid, peer->net,
@@ -2479,6 +2491,20 @@ static int peer_route_update(struct peer *peer, uint16_t type)
 		return mctp_nl_route_add(peer->ctx->nl, peer->eid, 0,
 					 peer->phys.ifindex, NULL, peer->mtu);
 	} else if (type == RTM_DELROUTE) {
+		if (peer->pool_size > 0) {
+			int rc = 0;
+			struct mctp_fq_addr gw_addr = { 0 };
+			gw_addr.net = peer->net;
+			gw_addr.eid = peer->eid;
+			rc = mctp_nl_route_del(peer->ctx->nl, peer->pool_start,
+					       peer->pool_size - 1,
+					       peer->phys.ifindex, &gw_addr);
+			if (rc < 0)
+				warnx("failed to delete route for peer pool eids %d-%d %s",
+				      peer->pool_start,
+				      peer->pool_start + peer->pool_size - 1,
+				      strerror(-rc));
+		}
 		return mctp_nl_route_del(peer->ctx->nl, peer->eid, 0,
 					 peer->phys.ifindex, NULL);
 	}
@@ -4205,6 +4231,128 @@ static void free_config(struct ctx *ctx)
 	free(ctx->config_filename);
 }
 
+static int endpoint_send_allocate_endpoint_id(struct peer *peer,
+					      mctp_eid_t eid_start,
+					      uint8_t eid_pool_size,
+					      mctp_ctrl_cmd_alloc_eid_op oper,
+					      uint8_t *allocated_pool_size,
+					      mctp_eid_t *allocated_pool_start)
+{
+	struct sockaddr_mctp_ext addr;
+	struct mctp_ctrl_cmd_alloc_eid req = { 0 };
+	struct mctp_ctrl_resp_alloc_eid *resp = NULL;
+	uint8_t *buf = NULL;
+	size_t buf_size;
+	uint8_t iid, stat;
+	int rc;
+
+	iid = mctp_next_iid(peer->ctx);
+	mctp_ctrl_msg_hdr_init_req(&req.ctrl_hdr, iid,
+				   MCTP_CTRL_CMD_ALLOCATE_ENDPOINT_IDS);
+	req.alloc_eid_op = (uint8_t)(oper & 0x03);
+	req.pool_size = eid_pool_size;
+	req.start_eid = eid_start;
+	rc = endpoint_query_peer(peer, MCTP_CTRL_HDR_MSG_TYPE, &req,
+				 sizeof(req), &buf, &buf_size, &addr);
+	if (rc < 0)
+		goto out;
+
+	rc = mctp_ctrl_validate_response(buf, buf_size, sizeof(*resp),
+					 peer_tostr_short(peer), iid,
+					 MCTP_CTRL_CMD_ALLOCATE_ENDPOINT_IDS);
+
+	if (rc)
+		goto out;
+
+	resp = (void *)buf;
+	if (!resp) {
+		warnx("Invalid response buffer");
+		return -ENOMEM;
+	}
+
+	stat = resp->status & 0x03;
+	if (stat == 0x00) {
+		if (peer->ctx->verbose) {
+			fprintf(stderr, "Allocation accepted\n");
+		}
+		if (resp->eid_pool_size != eid_pool_size ||
+		    resp->eid_set != eid_start) {
+			warnx("Unexpected pool start %d pool size %d",
+			      resp->eid_set, resp->eid_pool_size);
+			rc = -1;
+			goto out;
+		}
+		*allocated_pool_size = resp->eid_pool_size;
+		*allocated_pool_start = resp->eid_set;
+	} else {
+		if (stat == 0x1)
+			warnx("Allocation was rejected: already allocated by other bus"
+			      " pool size %d, pool start %d",
+			      resp->eid_pool_size, resp->eid_set);
+		rc = -1;
+		goto out;
+	}
+
+	if (peer->ctx->verbose) {
+		fprintf(stderr, "Allocated size of %d, starting from EID %d\n",
+			resp->eid_pool_size, resp->eid_set);
+	}
+
+out:
+	free(buf);
+	return rc;
+}
+
+static int endpoint_allocate_eid(struct peer *peer)
+{
+	uint8_t allocated_pool_size = 0;
+	mctp_eid_t allocated_pool_start = 0;
+	int rc = 0;
+
+	if (peer->pool_start >= peer->ctx->dyn_eid_max ||
+	    peer->pool_start <= 0) {
+		warnx("Invalid pool start %d", peer->pool_start);
+		return -1;
+	}
+	rc = endpoint_send_allocate_endpoint_id(
+		peer, peer->pool_start, peer->pool_size,
+		mctp_ctrl_cmd_alloc_eid_alloc_eid, &allocated_pool_size,
+		&allocated_pool_start);
+	if (rc) {
+		//reset peer pool
+		peer->pool_size = 0;
+		peer->pool_start = 0;
+		return rc;
+	} else {
+		peer->pool_size = allocated_pool_size;
+		peer->pool_start = allocated_pool_start;
+
+		// add gateway route for all bridge's downstream eids
+		if (peer->pool_size > 0) {
+			struct mctp_fq_addr gw_addr = { 0 };
+			gw_addr.net = peer->net;
+			gw_addr.eid = peer->eid;
+			rc = mctp_nl_route_add(peer->ctx->nl, peer->pool_start,
+					       peer->pool_size - 1,
+					       peer->phys.ifindex, &gw_addr,
+					       peer->mtu);
+			if (rc < 0) {
+				warnx("Failed to add gateway route for EID %d: %s",
+				      gw_addr.eid, strerror(-rc));
+				// If the route already exists, continue polling
+				if (rc == -EEXIST) {
+					rc = 0;
+				} else {
+					return rc;
+				}
+			}
+			// TODO: Polling logic for downstream EID
+		}
+	}
+
+	return rc;
+}
+
 int main(int argc, char **argv)
 {
 	struct ctx ctxi = { 0 }, *ctx = &ctxi;