enable mctp-req to send any mctp commands

Signed-off-by: Peng85282 <peng.wang@penguinsolutions.com>

Author: Peng85282

src/mctp-req.c

@@ -14,6 +14,7 @@
 #include <string.h>
 #include <err.h>
 #include <sys/socket.h>
+#include <poll.h>
 
 #include "mctp.h"
 #include "mctp-util.h"
@@ -28,10 +29,11 @@ static const uint8_t MCTP_TYPE_VENDOR_PCIE = 0x7e;
 
 /* lladdrlen != -1 to ignore ifindex/lladdr */
 static int mctp_req(unsigned int net, mctp_eid_t eid, unsigned int ifindex,
-		    uint8_t *lladdr, int lladdrlen, uint8_t *data, size_t len)
+		    uint8_t *lladdr, int lladdrlen, uint8_t *data, size_t len,
+		    int8_t type, unsigned int timeout)
 {
 	struct sockaddr_mctp_ext addr;
-	unsigned char *payload, *buf;
+	unsigned char *buf;
 	socklen_t addrlen;
 	int rc, sd, val;
 	size_t i, buf_len;
@@ -45,24 +47,10 @@ static int mctp_req(unsigned int net, mctp_eid_t eid, unsigned int ifindex,
 	addr.smctp_base.smctp_family = AF_MCTP;
 	addr.smctp_base.smctp_network = net;
 	addr.smctp_base.smctp_addr.s_addr = eid;
-	addr.smctp_base.smctp_type = MCTP_TYPE_VENDOR_PCIE;
+	addr.smctp_base.smctp_type = type;
 	addr.smctp_base.smctp_tag = MCTP_TAG_OWNER;
-	printf("req:  sending to (net %d, eid %d), type 0x%x\n", net, eid,
-	       addr.smctp_base.smctp_type);
-
-	buf_len = len + sizeof(VENDOR_TYPE_ECHO);
-	buf = malloc(buf_len);
-	if (!buf)
-		err(EXIT_FAILURE, "malloc");
-	memcpy(buf, VENDOR_TYPE_ECHO, sizeof(VENDOR_TYPE_ECHO));
-	payload = &buf[sizeof(VENDOR_TYPE_ECHO)];
-
-	if (data) {
-		memcpy(payload, data, len);
-	} else {
-		for (i = 0; i < len; i++)
-			payload[i] = i & 0xff;
-	}
+	printf("req:  sending to (net %d, eid %d), type 0x%02x, len %zu\n", net,
+	       eid, type, len);
 
 	/* extended addressing */
 	if (lladdrlen != -1) {
@@ -82,9 +70,23 @@ static int mctp_req(unsigned int net, mctp_eid_t eid, unsigned int ifindex,
 	}
 
 	/* send data */
-	rc = sendto(sd, buf, buf_len, 0, (struct sockaddr *)&addr, addrlen);
-	if (rc != (int)buf_len)
-		err(EXIT_FAILURE, "sendto(%zd)", buf_len);
+	rc = sendto(sd, data, len, 0, (struct sockaddr *)&addr, addrlen);
+	if (rc != (int)len)
+		err(EXIT_FAILURE, "sendto(%zd)", len);
+
+	struct pollfd pfd = { .fd = sd, .events = POLLIN };
+	if (poll(&pfd, 1, timeout) <= 0) {
+		return -1;
+	}
+
+	rc = recvfrom(sd, NULL, 0, MSG_PEEK | MSG_TRUNC, NULL, 0);
+	if (rc < 0)
+		err(EXIT_FAILURE, "recvfrom");
+	buf_len = (size_t)rc;
+
+	buf = malloc(buf_len);
+	if (!buf)
+		err(EXIT_FAILURE, "malloc");
 
 	/* receive response */
 	addrlen = sizeof(addr);
@@ -103,26 +105,46 @@ static int mctp_req(unsigned int net, mctp_eid_t eid, unsigned int ifindex,
 		     sizeof(struct sockaddr_mctp_ext),
 		     sizeof(struct sockaddr_mctp));
 
-	printf("req:  message from (net %d, eid %d) type 0x%x len %zd\n",
+	printf("rsp:  message from (net %d, eid %d) type 0x%02x len %zd\n",
 	       addr.smctp_base.smctp_network, addr.smctp_base.smctp_addr.s_addr,
-	       addr.smctp_base.smctp_type, len);
+	       addr.smctp_base.smctp_type, buf_len);
 	if (addrlen == sizeof(struct sockaddr_mctp_ext)) {
 		printf("      ext ifindex %d ha[0]=0x%02x len %hhu\n",
 		       addr.smctp_ifindex, addr.smctp_haddr[0],
 		       addr.smctp_halen);
 	}
 
-	if (memcmp(buf, VENDOR_TYPE_ECHO, sizeof(VENDOR_TYPE_ECHO)) != 0) {
-		errx(EXIT_FAILURE, "unexpected vendor ID");
+	printf("data:");
+	for (i = 0; i < buf_len; i++) {
+		if (i % 16 == 0)
+			printf("\n%04X\t", i);
+		printf("0x%02x ", buf[i]);
 	}
+	printf("\n");
+
+	if (type == MCTP_TYPE_VENDOR_PCIE &&
+	    !memcmp(data, VENDOR_TYPE_ECHO, sizeof(VENDOR_TYPE_ECHO))) {
+		if (buf_len >= sizeof(VENDOR_TYPE_ECHO) &&
+		    memcmp(buf, VENDOR_TYPE_ECHO, sizeof(VENDOR_TYPE_ECHO))) {
+			errx(EXIT_FAILURE, "unexpected vendor ID");
+		}
 
-	for (i = 0; i < len; i++) {
-		uint8_t exp = data ? data[i] : i & 0xff;
-		if (payload[i] != exp)
+		if (len != buf_len) {
 			errx(EXIT_FAILURE,
-			     "payload mismatch at byte 0x%zx; "
-			     "sent 0x%02x, received 0x%02x",
-			     i, exp, buf[i]);
+			     "unmatched payload length, "
+			     "sent %zd bytes, but received %zd bytes",
+			     len - sizeof(VENDOR_TYPE_ECHO),
+			     buf_len - sizeof(VENDOR_TYPE_ECHO));
+		}
+
+		for (i = sizeof(VENDOR_TYPE_ECHO); i < len; i++) {
+			if (buf[i] != data[i])
+				errx(EXIT_FAILURE,
+				     "payload mismatch at byte 0x%zx; "
+				     "sent 0x%02x, but received 0x%02x",
+				     i - sizeof(VENDOR_TYPE_ECHO), data[i],
+				     buf[i]);
+		}
 	}
 
 	return 0;
@@ -131,21 +153,29 @@ static int mctp_req(unsigned int net, mctp_eid_t eid, unsigned int ifindex,
 static void usage(void)
 {
 	fprintf(stderr,
-		"mctp-req [eid <eid>] [net <net>] [ifindex <ifindex> lladdr <hwaddr>] [len <len>]\n");
+		"mctp-req [eid <eid>] [net <net>] [ifindex <ifindex> lladdr <hwaddr>]"
+		" [timeout <ms>] [type <type>] [len <len>] [data <data>]\n");
 	fprintf(stderr, "default eid %d net %d len %zd\n", DEFAULT_EID,
 		DEFAULT_NET, DEFAULT_LEN);
+	fprintf(stderr,
+		"default to send <data> as payload of code construct echo command if"
+		" type is not specified");
+	fprintf(stderr, "<data> is colon separated hex bytes, e.g. cc:de:f0");
 }
 
 int main(int argc, char **argv)
 {
-	uint8_t *data, lladdr[MAX_ADDR_LEN];
+	uint8_t *data = NULL, *buf, lladdr[MAX_ADDR_LEN];
 	int lladdrlen = -1, datalen = -1;
 	unsigned int net = DEFAULT_NET;
 	mctp_eid_t eid = DEFAULT_EID;
 	size_t len = DEFAULT_LEN, sz;
 	char *endp, *optname, *optval;
 	unsigned int tmp, ifindex;
+	uint8_t type = MCTP_TYPE_VENDOR_PCIE;
+	unsigned int timeout = 1000;
 	bool valid_parse;
+	bool echo_req = true;
 	int i;
 
 	if (!(argc % 2)) {
@@ -174,16 +204,25 @@ int main(int argc, char **argv)
 			net = tmp;
 		} else if (!strcmp(optname, "ifindex")) {
 			ifindex = tmp;
+		} else if (!strcmp(optname, "timeout")) {
+			timeout = tmp;
 		} else if (!strcmp(optname, "len")) {
 			if (tmp > 64 * 1024)
 				errx(EXIT_FAILURE, "Bad len");
 			len = tmp;
+		} else if (!strcmp(optname, "type")) {
+			if (tmp > 0xff)
+				errx(EXIT_FAILURE, "Bad type");
+			type = tmp;
+			echo_req = false;
 		} else if (!strcmp(optname, "data")) {
 			sz = (strlen(optval) + 2) / 3;
-			data = malloc(sz);
+			data = malloc(sz + sizeof(VENDOR_TYPE_ECHO));
 			if (!data)
 				err(EXIT_FAILURE, "malloc");
-			if (parse_hex_addr(optval, data, &sz)) {
+			if (parse_hex_addr(optval,
+					   data + sizeof(VENDOR_TYPE_ECHO),
+					   &sz)) {
 				errx(EXIT_FAILURE, "Bad data");
 			}
 			datalen = sz;
@@ -205,8 +244,25 @@ int main(int argc, char **argv)
 		}
 	}
 
-	if (data)
+	if (data) {
 		len = datalen;
+	} else {
+		data = malloc(len + sizeof(VENDOR_TYPE_ECHO));
+		if (!data)
+			err(EXIT_FAILURE, "malloc");
+		buf = data + sizeof(VENDOR_TYPE_ECHO);
+		for (i = 0; i < len; i++)
+			*buf = i & 0xff;
+	}
+
+	if (echo_req) {
+		memcpy(data, VENDOR_TYPE_ECHO, sizeof(VENDOR_TYPE_ECHO));
+		buf = data;
+		len += sizeof(VENDOR_TYPE_ECHO);
+	} else {
+		buf = data + sizeof(VENDOR_TYPE_ECHO);
+	}
 
-	return mctp_req(net, eid, ifindex, lladdr, lladdrlen, data, len);
+	return mctp_req(net, eid, ifindex, lladdr, lladdrlen, buf, len, type,
+			timeout);
 }