Add mctp bridge endpoint support

Add support for MCTP bridge endpoints that can allocate pools of EIDs
for downstream endpoints.

We assume each AssignEndpoint d-bus call will be for an MCTP bridge,
with this we allocate/reserve a max_pool_size eid range contiguous to
bridge's own eid. Later this pool size is updated based on
SET_ENDPOINT_ID command response.

- for static eid assignment via AssignEndpointStatic d-bus call,
add check if eid is part of any other bridge's pool range.

[Fixup and requested change from Jeremy Kerr <[email protected]>]

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

Author: faizana-nvidia

src/mctpd.c

@@ -185,6 +185,10 @@ struct peer {
 		uint8_t endpoint_type;
 		uint8_t medium_spec;
 	} recovery;
+
+	// Pool size
+	uint8_t pool_size;
+	uint8_t pool_start;
 };
 
 struct ctx {
@@ -1344,7 +1348,7 @@ static int endpoint_query_phys(struct ctx *ctx, const dest_phys *dest,
 }
 
 /* returns -ECONNREFUSED if the endpoint returns failure. */
-static int endpoint_send_set_endpoint_id(const struct peer *peer,
+static int endpoint_send_set_endpoint_id(struct peer *peer,
 					 mctp_eid_t *new_eidp)
 {
 	struct sockaddr_mctp_ext addr;
@@ -1412,9 +1416,16 @@ static int endpoint_send_set_endpoint_id(const struct peer *peer,
 
 	alloc = resp->status & 0x3;
 	if (alloc != 0) {
-		// TODO for bridges
-		warnx("%s requested allocation pool, unimplemented",
-		      dest_phys_tostr(dest));
+		peer->pool_size = resp->eid_pool_size;
+		if (peer->ctx->verbose) {
+			fprintf(stderr,
+				"%s requested allocation of pool size = %d\n",
+				dest_phys_tostr(dest), peer->pool_size);
+		}
+		if (peer->pool_size > peer->ctx->max_pool_size) {
+			warnx("Truncate: requested pool size > max pool size config");
+			peer->pool_size = peer->ctx->max_pool_size;
+		}
 	}
 
 	rc = 0;
@@ -1642,11 +1653,66 @@ static int peer_set_mtu(struct ctx *ctx, struct peer *peer, uint32_t mtu)
 	return rc;
 }
 
+struct eid_allocation {
+	mctp_eid_t start;
+	unsigned int extent; /* 0 = only the start EID */
+};
+
+/* Allocate an unused dynamic EID for a peer, optionally with an associated
+ * bridge range (of size @bridged_len).
+ *
+ * We try to find the first allocation that contains the base EID plus the
+ * full range. If no space for that exists, we return the largest
+ * possible range. If the requested range is 0, then the first available
+ * (single) EID will suit as a match, the returned alloc->extent will be zero.
+ *
+ * It is up to the caller to check whether this range is suitable, and
+ * actually reserve that EID (& range) if so.
+ *
+ * returns 0 on success (with @alloc populated), non-zero on failure.
+ */
+static int allocate_eid(struct ctx *ctx, struct net *net,
+			unsigned int bridged_len, struct eid_allocation *alloc)
+{
+	struct eid_allocation cur = { 0 }, best = { 0 };
+	mctp_eid_t eid;
+
+	for (eid = ctx->dyn_eid_min; eid <= ctx->dyn_eid_max; eid++) {
+		if (net->peers[eid]) {
+			// reset our current candidate allocation
+			cur.start = 0;
+			eid += net->peers[eid]->pool_size;
+			continue;
+		}
+
+		// start a new candidate allocation
+		if (!cur.start)
+			cur.start = eid;
+		cur.extent = eid - cur.start;
+
+		// if this suits, we're done
+		if (cur.extent == bridged_len) {
+			*alloc = cur;
+			return 0;
+		}
+
+		if (cur.extent > best.extent)
+			best = cur;
+	}
+
+	if (best.start) {
+		*alloc = best;
+		return 0;
+	}
+
+	return -1;
+}
+
 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)
+			       mctp_eid_t static_eid, bool assign_bridge)
 {
-	mctp_eid_t e, new_eid;
+	mctp_eid_t new_eid;
 	struct net *n = NULL;
 	struct peer *peer = NULL;
 	uint32_t net;
@@ -1671,22 +1737,43 @@ static int endpoint_assign_eid(struct ctx *ctx, sd_bus_error *berr,
 
 		new_eid = static_eid;
 	} else {
-		/* Find an unused dynamic EID */
-		for (e = ctx->dyn_eid_min; e <= ctx->dyn_eid_max; e++) {
-			if (n->peers[e])
-				continue;
-			rc = add_peer(ctx, dest, e, net, &peer);
-			if (rc < 0)
-				return rc;
-			break;
-		}
-		if (e > ctx->dyn_eid_max) {
-			warnx("Ran out of EIDs for net %d, allocating %s", net,
-			      dest_phys_tostr(dest));
+		struct eid_allocation alloc;
+		unsigned int alloc_size = 0;
+
+		if (assign_bridge)
+			alloc_size = ctx->max_pool_size;
+
+		rc = allocate_eid(ctx, n, alloc_size, &alloc);
+		if (rc) {
+			warnx("Cannot allocate any EID (+pool %d) on net %d for %s",
+			      alloc_size, net, dest_phys_tostr(dest));
 			sd_bus_error_setf(berr, SD_BUS_ERROR_FAILED,
 					  "Ran out of EIDs");
 			return -EADDRNOTAVAIL;
 		}
+
+		/* Only allow complete pools for now. In future we could reserve
+		 * this range, in the assumption that the subsequent pool
+		 * request (in the Set Endpoint ID response) will fit in this
+		 * reservation.
+		 */
+		if (alloc.extent < alloc_size) {
+			warnx("Cannot allocate sufficient EIDs (+pool %d) on net %d for %s"
+			      " (largest span %d at %d)",
+			      alloc_size, net, dest_phys_tostr(dest),
+			      alloc.extent, alloc.start);
+			alloc.extent = 0;
+		}
+
+		new_eid = alloc.start;
+
+		rc = add_peer(ctx, dest, new_eid, net, &peer);
+		if (rc < 0)
+			return rc;
+
+		peer->pool_size = alloc.extent;
+		if (peer->pool_size)
+			peer->pool_start = new_eid + 1;
 	}
 
 	rc = endpoint_send_set_endpoint_id(peer, &new_eid);
@@ -1700,6 +1787,10 @@ static int endpoint_assign_eid(struct ctx *ctx, sd_bus_error *berr,
 	}
 
 	if (new_eid != peer->eid) {
+		// avoid allocation for any different EID in response
+		warnx("Mismatch of requested from received EID, resetting the pool");
+		peer->pool_size = 0;
+		peer->pool_start = 0;
 		rc = change_peer_eid(peer, new_eid);
 		if (rc == -EEXIST) {
 			sd_bus_error_setf(
@@ -2102,7 +2193,7 @@ static int method_setup_endpoint(sd_bus_message *call, void *data,
 	}
 
 	/* Set Endpoint ID */
-	rc = endpoint_assign_eid(ctx, berr, dest, &peer, 0);
+	rc = endpoint_assign_eid(ctx, berr, dest, &peer, 0, false);
 	if (rc < 0)
 		goto err;
 
@@ -2155,21 +2246,42 @@ static int method_assign_endpoint(sd_bus_message *call, void *data,
 						  peer->net, peer_path, 0);
 	}
 
-	rc = endpoint_assign_eid(ctx, berr, dest, &peer, 0);
+	rc = endpoint_assign_eid(ctx, berr, dest, &peer, 0, true);
 	if (rc < 0)
 		goto err;
 
 	peer_path = path_from_peer(peer);
 	if (!peer_path)
 		goto err;
 
+	if (peer->pool_size > 0) {
+		//TODO: Implement Allocate EndpointID
+	}
+
 	return sd_bus_reply_method_return(call, "yisb", peer->eid, peer->net,
 					  peer_path, 1);
 err:
 	set_berr(ctx, rc, berr);
 	return rc;
 }
 
+// Checks if given EID belongs to any bridge's pool range
+static bool is_eid_in_bridge_pool(struct net *n, struct ctx *ctx,
+				  mctp_eid_t eid)
+{
+	for (int i = ctx->dyn_eid_min; i <= eid; i++) {
+		struct peer *peer = n->peers[i];
+		if (peer && peer->pool_size > 0) {
+			if (eid >= peer->pool_start &&
+			    eid < peer->pool_start + peer->pool_size) {
+				return true;
+			}
+			i += peer->pool_size;
+		}
+	}
+	return false;
+}
+
 static int method_assign_endpoint_static(sd_bus_message *call, void *data,
 					 sd_bus_error *berr)
 {
@@ -2224,10 +2336,22 @@ static int method_assign_endpoint_static(sd_bus_message *call, void *data,
 			return sd_bus_error_setf(berr,
 						 SD_BUS_ERROR_INVALID_ARGS,
 						 "Address in use");
+		} else {
+			// is requested EID part of any bridge pool range
+			struct net *n = lookup_net(ctx, netid);
+			if (!n) {
+				bug_warn("%s: Bad old net %d", __func__, netid);
+				return -EPROTO;
+			}
+			if (is_eid_in_bridge_pool(n, ctx, eid)) {
+				return sd_bus_error_setf(
+					berr, SD_BUS_ERROR_INVALID_ARGS,
+					"EID belongs to another MCTP bridge pool");
+			}
 		}
 	}
 
-	rc = endpoint_assign_eid(ctx, berr, dest, &peer, eid);
+	rc = endpoint_assign_eid(ctx, berr, dest, &peer, eid, false);
 	if (rc < 0) {
 		goto err;
 	}
@@ -2637,7 +2761,8 @@ static int peer_endpoint_recover(sd_event_source *s, uint64_t usec,
 			 * after which we immediately return as there's no old peer state left to
 			 * maintain.
 			 */
-			return endpoint_assign_eid(ctx, NULL, &phys, &peer, 0);
+			return endpoint_assign_eid(ctx, NULL, &phys, &peer, 0,
+						   false);
 		}
 
 		/* Confirmation of the same device, apply its already allocated EID */