ncsi: Add Mellanox Get Mac Address handler

Attempted to mirror the upstream Linux driver[1] as closely as reasonably
possible.

[1] https://github.com/torvalds/linux/blob/42226c989789d8da4af1de0c31070c96726d990c/net/ncsi/ncsi-rsp.c#L614-L638

Signed-off-by: Peter Delevoryas <pdel@fb.com>

3 files changed, 121 insertions, 1 deletions

diff --git a/src/ncsi-pkt.h b/src/ncsi-pkt.h
index 606684f..9dd167c 100644
--- a/src/ncsi-pkt.h
+++ b/src/ncsi-pkt.h
@@ -487,4 +487,9 @@ struct ncsi_aen_hncdsc_pkt {
 #define MLX_MAC_ADDR_OFFSET 8
 #define INTEL_MAC_ADDR_OFFSET 1
 
+/* Status offset in OEM response */
+#define MLX_GMA_STATUS_OFFSET 0
+/* OEM Response payload length */
+#define MLX_GMA_PAYLOAD_LEN 24
+
 #endif /* NCSI_PKT_H */
diff --git a/src/ncsi.c b/src/ncsi.c
index 0d4c491..adb3398 100644
--- a/src/ncsi.c
+++ b/src/ncsi.c
@@ -55,12 +55,73 @@ static uint32_t ncsi_calculate_checksum(uint8_t *data, int len)
     return checksum;
 }
 
+/* Response handler for Mellanox command Get Mac Address */
+static int ncsi_rsp_handler_oem_mlx_gma(Slirp *slirp,
+                                        const struct ncsi_pkt_hdr *nh,
+                                        struct ncsi_rsp_pkt_hdr *rnh)
+{
+    uint8_t oob_eth_addr_allocated = 0;
+    struct ncsi_rsp_oem_pkt *rsp;
+    int i;
+
+    rsp = (struct ncsi_rsp_oem_pkt *)rnh;
+
+    /* Set the payload length */
+    rsp->rsp.common.length = htons(MLX_GMA_PAYLOAD_LEN);
+
+    for (i = 0; i < ETH_ALEN; i++) {
+        if (slirp->oob_eth_addr[i] != 0x00) {
+            oob_eth_addr_allocated = 1;
+            break;
+        }
+    }
+    rsp->data[MLX_GMA_STATUS_OFFSET] = oob_eth_addr_allocated;
+
+    /* Set the allocated management address */
+    memcpy(&rsp->data[MLX_MAC_ADDR_OFFSET], slirp->oob_eth_addr, ETH_ALEN);
+
+    return 0;
+}
+
+/* Response handler for Mellanox card */
+static int ncsi_rsp_handler_oem_mlx(Slirp *slirp, const struct ncsi_pkt_hdr *nh,
+                                    struct ncsi_rsp_pkt_hdr *rnh)
+{
+    const struct ncsi_cmd_oem_pkt *cmd;
+    const struct ncsi_rsp_oem_mlx_pkt *cmd_mlx;
+    struct ncsi_rsp_oem_pkt *rsp;
+    struct ncsi_rsp_oem_mlx_pkt *rsp_mlx;
+
+    /* Get the command header */
+    cmd = (const struct ncsi_cmd_oem_pkt *)nh;
+    cmd_mlx = (const struct ncsi_rsp_oem_mlx_pkt *)cmd->data;
+
+    /* Get the response header */
+    rsp = (struct ncsi_rsp_oem_pkt *)rnh;
+    rsp_mlx = (struct ncsi_rsp_oem_mlx_pkt *)rsp->data;
+
+    /* Ensure the OEM response header matches the command's */
+    rsp_mlx->cmd_rev = cmd_mlx->cmd_rev;
+    rsp_mlx->cmd = cmd_mlx->cmd;
+    rsp_mlx->param = cmd_mlx->param;
+    rsp_mlx->optional = cmd_mlx->optional;
+
+    if (cmd_mlx->cmd == NCSI_OEM_MLX_CMD_GMA &&
+        cmd_mlx->param == NCSI_OEM_MLX_CMD_GMA_PARAM)
+        return ncsi_rsp_handler_oem_mlx_gma(slirp, nh, rnh);
+
+    rsp->rsp.common.length = htons(8);
+    rsp->rsp.code = htons(NCSI_PKT_RSP_C_UNSUPPORTED);
+    rsp->rsp.reason = htons(NCSI_PKT_RSP_R_UNKNOWN);
+    return -ENOENT;
+}
+
 static const struct ncsi_rsp_oem_handler {
     unsigned int mfr_id;
     int (*handler)(Slirp *slirp, const struct ncsi_pkt_hdr *nh,
                    struct ncsi_rsp_pkt_hdr *rnh);
 } ncsi_rsp_oem_handlers[] = {
-    { NCSI_OEM_MFR_MLX_ID, NULL },
+    { NCSI_OEM_MFR_MLX_ID, ncsi_rsp_handler_oem_mlx },
     { NCSI_OEM_MFR_BCM_ID, NULL },
     { NCSI_OEM_MFR_INTEL_ID, NULL },
 };
diff --git a/test/ncsitest.c b/test/ncsitest.c
index 4a6cada..c763af2 100644
--- a/test/ncsitest.c
+++ b/test/ncsitest.c
@@ -95,6 +95,59 @@ static void test_ncsi_oem_mlx_unsupported_command(Slirp *slirp)
     assert(ntohl(oem->mfr_id) == 0x8119);
 }
 
+static void test_ncsi_oem_mlx_gma(Slirp *slirp)
+{
+    uint8_t oob_eth_addr[ETH_ALEN] = {0xde, 0xad, 0xbe, 0xef, 0xca, 0xfe};
+    uint8_t command[] = {
+        /* Destination MAC */
+        0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+        /* Source MAC */
+        0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+        /* Ethertype */
+        0x88, 0xf8,
+        /* NC-SI Control packet header */
+        0x00, /* MC ID */
+        0x01, /* Header revision */
+        0x00, /* Reserved */
+        0x01, /* Instance ID */
+        0x50, /* Control Packet Type */
+        0x00, /* Channel ID */
+        0x00, /* Reserved */
+        0x08, /* Payload length */
+        0x00, 0x00, 0x00, 0x00, /* Reserved */
+        0x00, 0x00, 0x00, 0x00, /* Reserved */
+        /* NC-SI OEM packet header */
+        0x00, 0x00, 0x81, 0x19, /* Manufacturer ID: Mellanox */
+        /* Vendor Data */
+        0x00, /* Command Revision */
+        0x00, /* Command ID */
+        0x1b, /* Parameter */
+        0x00, /* Optional data */
+    };
+    const struct ncsi_rsp_oem_pkt *oem = slirp->opaque + ETH_HLEN;
+
+    memset(slirp->oob_eth_addr, 0, ETH_ALEN);
+    slirp->mfr_id = 0x8119;
+    slirp_input(slirp, command, sizeof(command));
+
+    assert(ntohs(oem->rsp.code) == NCSI_PKT_RSP_C_COMPLETED);
+    assert(ntohs(oem->rsp.reason) == NCSI_PKT_RSP_R_NO_ERROR);
+    assert(ntohl(oem->mfr_id) == slirp->mfr_id);
+    assert(ntohs(oem->rsp.common.length) == MLX_GMA_PAYLOAD_LEN);
+    assert(memcmp(slirp->oob_eth_addr, &oem->data[MLX_MAC_ADDR_OFFSET], ETH_ALEN) == 0);
+    assert(oem->data[MLX_GMA_STATUS_OFFSET] == 0);
+
+    memcpy(slirp->oob_eth_addr, oob_eth_addr, ETH_ALEN);
+    slirp_input(slirp, command, sizeof(command));
+
+    assert(ntohs(oem->rsp.code) == NCSI_PKT_RSP_C_COMPLETED);
+    assert(ntohs(oem->rsp.reason) == NCSI_PKT_RSP_R_NO_ERROR);
+    assert(ntohl(oem->mfr_id) == slirp->mfr_id);
+    assert(ntohs(oem->rsp.common.length) == MLX_GMA_PAYLOAD_LEN);
+    assert(memcmp(oob_eth_addr, &oem->data[MLX_MAC_ADDR_OFFSET], ETH_ALEN) == 0);
+    assert(oem->data[MLX_GMA_STATUS_OFFSET] == 1);
+}
+
 static ssize_t send_packet(const void *buf, size_t len, void *opaque)
 {
     assert(len <= NCSI_RESPONSE_CAPACITY);
@@ -115,6 +168,7 @@ int main(int argc, char *argv[])
 
     test_ncsi_get_version_id(slirp);
     test_ncsi_oem_mlx_unsupported_command(slirp);
+    test_ncsi_oem_mlx_gma(slirp);
 
     slirp_cleanup(slirp);
 }