mctpd: Handle discovery messages

src/mctp-netlink.c

@@ -1056,6 +1056,76 @@ static int linkmap_add_entry(mctp_nl *nl, struct ifinfomsg *info,
 	return 0;
 }
 
+/* Common parts of RTM_NEWADDR and RTM_DELADDR */
+struct mctp_addralter_msg {
+	struct nlmsghdr nh;
+	struct ifaddrmsg ifmsg;
+	struct rtattr rta;
+	uint8_t data[4];
+};
+static int fill_addralter_args(struct mctp_nl *nl,
+			       struct mctp_addralter_msg *msg,
+			       struct rtattr **prta, size_t *prta_len,
+			       mctp_eid_t eid, const char *linkstr)
+{
+	int ifindex;
+
+	ifindex = mctp_nl_ifindex_byname(nl, linkstr);
+	if (!ifindex) {
+		warnx("invalid device %s", linkstr);
+		return -1;
+	}
+
+	memset(msg, 0x0, sizeof(*msg));
+
+	msg->nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
+
+	msg->ifmsg.ifa_index = ifindex;
+	msg->ifmsg.ifa_family = AF_MCTP;
+
+	msg->rta.rta_type = IFA_LOCAL;
+	msg->rta.rta_len = RTA_LENGTH(sizeof(eid));
+	memcpy(RTA_DATA(&msg->rta), &eid, sizeof(eid));
+
+	msg->nh.nlmsg_len =
+		NLMSG_LENGTH(sizeof(msg->ifmsg)) + RTA_SPACE(sizeof(eid));
+
+	if (prta)
+		*prta = &msg->rta;
+	if (prta_len)
+		*prta_len = msg->rta.rta_len;
+
+	return 0;
+}
+
+int mctp_nl_addr_add(struct mctp_nl *nl, mctp_eid_t eid, const char *linkstr)
+{
+	struct mctp_addralter_msg msg;
+	int rc;
+
+	rc = fill_addralter_args(nl, &msg, NULL, NULL, eid, linkstr);
+	if (rc)
+		return -1;
+
+	msg.nh.nlmsg_type = RTM_NEWADDR;
+
+	return mctp_nl_send(nl, &msg.nh);
+}
+
+int mctp_nl_addr_del(struct mctp_nl *nl, mctp_eid_t eid, const char *linkstr)
+{
+	struct mctp_addralter_msg msg;
+	int rc;
+
+	rc = fill_addralter_args(nl, &msg, NULL, NULL, eid, linkstr);
+	if (rc)
+		return -1;
+
+	msg.nh.nlmsg_type = RTM_DELADDR;
+
+	return mctp_nl_send(nl, &msg.nh);
+}
+
 /* Common parts of RTM_NEWROUTE and RTM_DELROUTE */
 struct mctp_rtalter_msg {
 	struct nlmsghdr		nh;

src/mctp-netlink.h

@@ -69,6 +69,10 @@ int *mctp_nl_net_list(const mctp_nl *nl, size_t *ret_num_nets);
 /* Returns an allocated list of ifindex, caller to free */
 int *mctp_nl_if_list(const mctp_nl *nl, size_t *ret_num_if);
 
+/* MCTP addr helper */
+int mctp_nl_addr_add(struct mctp_nl *nl, uint8_t eid, const char *ifname);
+int mctp_nl_addr_del(struct mctp_nl *nl, uint8_t eid, const char *ifname);
+
 /* MCTP route helper */
 int mctp_nl_route_add(struct mctp_nl *nl, uint8_t eid, const char* ifname,
 		uint32_t mtu);

src/mctpd.c

@@ -126,6 +126,8 @@ struct ctx {
 
 	// Whether we are running as the bus owner
 	bool bus_owner;
+	// Flag for endpoint discovery process;
+	bool discovered;
 
 	// An allocated array of peers, changes address (reallocated) during runtime
 	peer *peers;
@@ -149,6 +151,8 @@ static int emit_endpoint_added(const peer *peer);
 static int emit_endpoint_removed(const peer *peer);
 static int emit_net_added(ctx *ctx, int net);
 static int emit_net_removed(ctx *ctx, int net);
+static int add_peer(ctx *ctx, const dest_phys *dest, mctp_eid_t eid, int net,
+		    peer **ret_peer);
 static int query_peer_properties(peer *peer);
 static int setup_added_peer(peer *peer);
 static void add_peer_route(peer *peer);
@@ -482,6 +486,56 @@ static int reply_message(ctx *ctx, int sd, const void *resp, size_t resp_len,
 	return 0;
 }
 
+/* Replies to a physical address */
+static int reply_message_phys(ctx *ctx, int sd, const void *resp,
+			      size_t resp_len,
+			      const struct sockaddr_mctp_ext *addr)
+{
+	ssize_t len;
+	struct sockaddr_mctp_ext reply_addr = *addr;
+
+	reply_addr.smctp_base.smctp_tag &= ~MCTP_TAG_OWNER;
+
+	len = sendto(sd, resp, resp_len, 0, (struct sockaddr *)&reply_addr,
+		     sizeof(reply_addr));
+
+	if (len < 0) {
+		return -errno;
+	}
+
+	if ((size_t)len != resp_len) {
+		warnx("BUG: short sendto %zd, expected %zu", len, resp_len);
+		return -EPROTO;
+	}
+
+	return 0;
+}
+
+static int discover_peer_from_ext_addr(ctx *ctx, struct sockaddr_mctp_ext *addr)
+{
+	struct peer *peer;
+	struct dest_phys phys;
+	mctp_eid_t eid;
+	int net;
+	int rc;
+
+	phys.ifindex = addr->smctp_ifindex;
+	memcpy(phys.hwaddr, addr->smctp_haddr, addr->smctp_halen);
+	phys.hwaddr_len = addr->smctp_halen;
+	eid = addr->smctp_base.smctp_addr.s_addr;
+	net = addr->smctp_base.smctp_network;
+
+	rc = add_peer(ctx, &phys, eid, net, &peer);
+	if (rc < 0)
+		return rc;
+
+	rc = setup_added_peer(peer);
+	if (rc < 0)
+		return rc;
+
+	return 0;
+}
+
 // Handles new Incoming Set Endpoint ID request
 static int handle_control_set_endpoint_id(ctx *ctx,
 	int sd, struct sockaddr_mctp_ext *addr,
@@ -490,6 +544,8 @@ static int handle_control_set_endpoint_id(ctx *ctx,
 	struct mctp_ctrl_cmd_set_eid *req = NULL;
 	struct mctp_ctrl_resp_set_eid respi = {0}, *resp = &respi;
 	size_t resp_len;
+	mctp_eid_t eid_set;
+	int rc;
 
 	if (buf_size < sizeof(*req)) {
 		warnx("short Set Endpoint ID message");
@@ -500,12 +556,47 @@ static int handle_control_set_endpoint_id(ctx *ctx,
 	resp->ctrl_hdr.command_code = req->ctrl_hdr.command_code;
 	resp->ctrl_hdr.rq_dgram_inst = RQDI_RESP;
 	resp->completion_code = 0;
-	resp->status = 0x01 << 4; // Already assigned, TODO
-	resp->eid_set = local_addr(ctx, addr->smctp_ifindex);
-	resp->eid_pool_size = 0;
 	resp_len = sizeof(struct mctp_ctrl_resp_set_eid);
 
-	// TODO: learn busowner route and neigh
+	eid_set = local_addr(ctx, addr->smctp_ifindex);
+	if (!eid_set) {
+		const char *linkstr =
+			mctp_nl_if_byindex(ctx->nl, addr->smctp_ifindex);
+
+		rc = mctp_nl_addr_add(ctx->nl, req->eid, linkstr);
+		if (rc < 0) {
+			warnx("ERR: cannot add local eid %d to ifindex %d",
+			      req->eid, addr->smctp_ifindex);
+			return rc;
+		}
+
+		rc = discover_peer_from_ext_addr(ctx, addr);
+		if (rc < 0) {
+			warnx("ERR: cannot discover bus owner");
+			mctp_nl_addr_del(ctx->nl, req->eid, linkstr);
+			return rc;
+		}
+
+		resp->status = 0x00; // Assignment accepted
+		resp->eid_set = req->eid;
+		resp->eid_pool_size = 0;
+		if (ctx->verbose)
+			fprintf(stderr, "Accepted set eid %d\n", req->eid);
+
+	} else {
+		resp->status = 0x01 << 4; // Already assigned
+		resp->eid_set = eid_set;
+		resp->eid_pool_size = 0;
+
+		if (ctx->verbose && req->eid != eid_set)
+			fprintf(stderr,
+				"Rejected set eid %d, already assigned with eid %d\n",
+				req->eid, eid_set);
+	}
+
+	if (ctx->verbose && !ctx->discovered)
+		fprintf(stderr, "Setting discovered flag to true\n");
+	ctx->discovered = true;
 
 	return reply_message(ctx, sd, resp, resp_len, addr);
 }
@@ -663,6 +754,75 @@ static int handle_control_resolve_endpoint_id(ctx *ctx,
 	return reply_message(ctx, sd, resp, resp_len, addr);
 }
 
+static int handle_control_prepare_endpoint_discovery(ctx *ctx,
+	int sd, const struct sockaddr_mctp_ext *addr,
+	const uint8_t *buf, const size_t buf_size)
+{
+	struct mctp_ctrl_msg_hdr *req = (void*)buf;
+	struct mctp_ctrl_resp_prepare_discovery respi = {0}, *resp = &respi;
+	int rc;
+	mctp_eid_t *addrs;
+	size_t addrs_num;
+
+	if (buf_size < sizeof(*req)) {
+		warnx("short Prepare for Endpoint Discovery message");
+		return -ENOMSG;
+	}
+
+	resp->ctrl_hdr.command_code = req->command_code;
+	resp->ctrl_hdr.rq_dgram_inst = RQDI_RESP;
+
+	if (ctx->verbose && ctx->discovered)
+		fprintf(stderr, "Clearing discovered flag\n");
+	ctx->discovered = false;
+
+
+	// clear local EIDs
+	addrs = mctp_nl_addrs_byindex(ctx->nl, addr->smctp_ifindex, &addrs_num);
+	if (!addrs) {
+		warnx("BUG: cannot get local EIDs at ifindex %d",
+		      addr->smctp_ifindex);
+		return -ENOENT;
+	}
+	for (size_t i = 0; i < addrs_num; i++) {
+		rc = mctp_nl_addr_del(ctx->nl, addrs[i],
+				      mctp_nl_if_byindex(ctx->nl,
+							 addr->smctp_ifindex));
+		if (rc < 0) {
+			errx(rc, "ERR: cannot remove local eid %d ifindex %d",
+			     addrs[i], addr->smctp_ifindex);
+		}
+	}
+	free(addrs);
+
+	// we need to send to physical location, no entry in routing table yet
+	return reply_message_phys(ctx, sd, resp, sizeof(*resp), addr);
+}
+
+static int
+handle_control_endpoint_discovery(ctx *ctx, int sd,
+				  const struct sockaddr_mctp_ext *addr,
+				  const uint8_t *buf, const size_t buf_size)
+{
+	struct mctp_ctrl_msg_hdr *req = (void *)buf;
+	struct mctp_ctrl_resp_endpoint_discovery respi = { 0 }, *resp = &respi;
+
+	if (buf_size < sizeof(*req)) {
+		warnx("short Endpoint Discovery message");
+		return -ENOMSG;
+	}
+
+	if (ctx->discovered) {
+		return 0;
+	}
+
+	resp->ctrl_hdr.command_code = req->command_code;
+	resp->ctrl_hdr.rq_dgram_inst = RQDI_RESP;
+
+	// we need to send to physical location, no entry in routing table yet
+	return reply_message_phys(ctx, sd, resp, sizeof(*resp), addr);
+}
+
 static int handle_control_unsupported(ctx *ctx,
 	int sd, const struct sockaddr_mctp_ext *addr,
 	const uint8_t *buf, const size_t buf_size)
@@ -743,6 +903,14 @@ static int cb_listen_control_msg(sd_event_source *s, int sd, uint32_t revents,
 			rc = handle_control_resolve_endpoint_id(ctx,
 				sd, &addr, buf, buf_size);
 			break;
+		case MCTP_CTRL_CMD_PREPARE_ENDPOINT_DISCOVERY:
+			rc = handle_control_prepare_endpoint_discovery(ctx,
+				sd, &addr, buf, buf_size);
+			break;
+		case MCTP_CTRL_CMD_ENDPOINT_DISCOVERY:
+			rc = handle_control_endpoint_discovery(ctx, sd, &addr,
+							       buf, buf_size);
+			break;
 		default:
 			if (ctx->verbose) {
 				warnx("Ignoring unsupported command code 0x%02x",