From 2990a055a8d5175345f245ff5d9766f5be55bfe8 Mon Sep 17 00:00:00 2001 From: Peng85282 Date: Wed, 1 Oct 2025 17:40:41 -0700 Subject: [PATCH] enable mctp-req to send any mctp commands Signed-off-by: Peng85282 --- src/mctp-req.c | 132 +++++++++++++++++++++++++++++++++++-------------- 1 file changed, 94 insertions(+), 38 deletions(-) diff --git a/src/mctp-req.c b/src/mctp-req.c index 6904c1b..f123fd6 100644 --- a/src/mctp-req.c +++ b/src/mctp-req.c @@ -14,6 +14,7 @@ #include #include #include +#include #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 ] [net ] [ifindex lladdr ] [len ]\n"); + "mctp-req [eid ] [net ] [ifindex lladdr ]" + " [timeout ] [type ] [len ] [data ]\n"); fprintf(stderr, "default eid %d net %d len %zd\n", DEFAULT_EID, DEFAULT_NET, DEFAULT_LEN); + fprintf(stderr, + "default to send as payload of code construct echo command if" + " type is not specified"); + fprintf(stderr, " 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); }