Add Allocate Endpoint ID support for MCTP Bridge

* New mctpd method to send Allocate Endpoint ID mctp control message
* Check for presence of contiguous available EIDs of pool size.
* Send Allocate Endpoint ID for dynamic pool eid from AssignEndoint and
  AssignEndpointStatic d-bus methods

Signed-off-by: Faizan Ali <faizana@nvidia.com>

Author: faizana-nvidia

src/mctpd.c

@@ -250,6 +250,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[];
@@ -2087,6 +2088,15 @@ static int method_assign_endpoint(sd_bus_message *call, void *data, sd_bus_error
 		// Dynamic pool EID starts after bridge's EID
 		peer->pool_start = peer->eid + 1;
 		// Call for Allocate EndpointID
+		rc = endpoint_allocate_eid(peer);
+		if (rc < 0) {
+			warnx("Failed to allocate downstream EIDs");
+		} else {
+			if (peer->ctx->verbose) {
+				printf("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",
@@ -2164,6 +2174,15 @@ static int method_assign_endpoint_static(sd_bus_message *call, void *data,
 		// Dynamic pool EID starts after bridge's EID
 		peer->pool_start = peer->eid + 1;
 		// Call for Allocate EndpointID
+		rc = endpoint_allocate_eid(peer);
+		if (rc < 0) {
+			warnx("Failed to allocate downstream EIDs");
+		} else {
+			if (peer->ctx->verbose) {
+				printf("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",
@@ -2196,7 +2215,6 @@ static int method_learn_endpoint(sd_bus_message *call, void *data, sd_bus_error
 	if (rc < 0)
 		return sd_bus_error_setf(berr, SD_BUS_ERROR_INVALID_ARGS,
 			"Bad physaddr");
-
 	rc = get_endpoint_peer(ctx, berr, dest, &peer, &eid);
 	if (rc == -EEXIST) {
 		/* We have a conflict with an existing endpoint, so can't
@@ -2308,6 +2326,15 @@ static int method_assign_bridge_static(sd_bus_message *call, void *data,
 		peer->pool_size = min(peer->pool_size, pool_size);
 
 		//call for Allocate EndpointID
+		rc = endpoint_allocate_eid(peer);
+		if (rc < 0) {
+			msg_len += snprintf(msg + msg_len, sizeof(msg) - msg_len,
+				". Failed to allocate downstream EIDs");
+		} else {
+			msg_len += snprintf(msg + msg_len, sizeof(msg) - msg_len,
+				". Downstream EIDs assigned from %d to %d : pool size %d",
+				peer->pool_start, peer->pool_start + peer->pool_size - 1, peer->pool_size);
+		}
 	}
 
 	return sd_bus_reply_method_return(call, "yisbs",
@@ -2876,6 +2903,7 @@ static const sd_bus_vtable bus_link_owner_vtable[] = {
 		SD_BUS_PARAM(found),
 		method_learn_endpoint,
 		0),
+
 	SD_BUS_METHOD_WITH_NAMES("AssignBridgeStatic",
 		"ayyyy",
 		SD_BUS_PARAM(physaddr)
@@ -3964,6 +3992,123 @@ 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);
+	req.ctrl_hdr.rq_dgram_inst = RQDI_REQ | iid;
+	req.ctrl_hdr.command_code = 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("%s Invalid response Buffer\n", __func__);
+		return -ENOMEM;
+	}
+
+	stat = resp->status & 0x03;
+	if (stat == 0x00) {
+		if(peer->ctx->verbose) {
+			fprintf(stderr, "%s Allocation Accepted \n", __func__);
+		}
+	}
+	else if (stat == 0x1) {
+		warnx("%s Allocation was rejected as it was Allocated by other bus \n", __func__);
+	}
+
+	*allocated_pool_size = resp->eid_pool_size;
+	*allocated_pool_start = resp->eid_set;
+	if(peer->ctx->verbose) {
+		fprintf(stderr, "%s Allocated size of %d, starting from EID %d\n", __func__,
+				resp->eid_pool_size, resp->eid_set);
+	}
+
+	return 0;
+out:
+	free(buf);
+	return rc;
+}
+
+static mctp_eid_t get_pool_start (struct peer *peer, mctp_eid_t eid_start, uint8_t pool_size)
+{
+	uint8_t count = 0;
+	mctp_eid_t pool_start = eid_alloc_max;
+	struct net *n = lookup_net(peer->ctx, peer->net);
+
+	if (!n) {
+		warnx("BUG: Unknown net %d : failed to get pool start\n", peer->net);
+		return eid_alloc_max;
+	}
+
+	for (mctp_eid_t e = eid_start; e <= eid_alloc_max; e++) {
+		if (n->peers[e] == NULL) {
+			if(pool_start == eid_alloc_max) {
+				pool_start = e;
+			}
+			count++;
+			if (count == pool_size) return pool_start;
+		} else {
+			pool_start = eid_alloc_max;
+			count = 0;
+		}
+	}
+
+	return eid_alloc_max;
+}
+
+static int endpoint_allocate_eid(struct peer* peer)
+{
+	uint8_t allocated_pool_size = 0;
+	mctp_eid_t allocated_pool_start = 0;
+	int rc = 0;
+
+	/* Find pool sized contiguous unused eids to allocate on the bridge. */
+	peer->pool_start = get_pool_start(peer, peer->pool_start, peer->pool_size);
+	if (peer->pool_start == eid_alloc_max) {
+		warnx("%s failed to find contiguous EIDs of required size", __func__);
+		return -1;
+	} else {
+		if (peer->ctx->verbose)
+			fprintf(stderr, "%s Asking for contiguous EIDs for pool with start eid : %d\n", __func__, peer->pool_start);
+	}
+
+	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) {
+		warnx("%s failed to allocate endpoints, returned %s %d\n",
+				__func__, strerror(-rc), rc);
+	} else {
+		peer->pool_size = allocated_pool_size;
+		peer->pool_start = allocated_pool_start;
+
+		// Polling logic for downstream EID
+	}
+
+	return rc;
+}
+
 int main(int argc, char **argv)
 {
 	struct ctx ctxi = {0}, *ctx = &ctxi;