ncsi: Add OEM command handler

This commit just sets up the OEM command handler to respond with
"unsupported" for now, as verified in the test.

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

3 files changed, 147 insertions, 3 deletions

diff --git a/src/ncsi-pkt.h b/src/ncsi-pkt.h
index 39cf844..606684f 100644
--- a/src/ncsi-pkt.h
+++ b/src/ncsi-pkt.h
@@ -181,6 +181,29 @@ struct ncsi_cmd_snfc_pkt {
     unsigned char pad[22];
 } SLIRP_PACKED;
 
+/* OEM Request Command as per NCSI Specification */
+struct ncsi_cmd_oem_pkt {
+    struct ncsi_cmd_pkt_hdr cmd; /* Command header    */
+    __be32 mfr_id; /* Manufacture ID    */
+    unsigned char data[]; /* OEM Payload Data  */
+} SLIRP_PACKED;
+
+/* OEM Response Packet as per NCSI Specification */
+struct ncsi_rsp_oem_pkt {
+    struct ncsi_rsp_pkt_hdr rsp; /* Command header    */
+    __be32 mfr_id; /* Manufacture ID    */
+    unsigned char data[]; /* Payload data      */
+} SLIRP_PACKED;
+
+/* Mellanox Response Data */
+struct ncsi_rsp_oem_mlx_pkt {
+    unsigned char cmd_rev; /* Command Revision  */
+    unsigned char cmd; /* Command ID        */
+    unsigned char param; /* Parameter         */
+    unsigned char optional; /* Optional data     */
+    unsigned char data[]; /* Data              */
+} SLIRP_PACKED;
+
 /* Get Link Status */
 struct ncsi_rsp_gls_pkt {
     struct ncsi_rsp_pkt_hdr rsp; /* Response header   */
@@ -442,4 +465,26 @@ struct ncsi_aen_hncdsc_pkt {
 #define NCSI_PKT_AEN_CR 0x01 /* Configuration required   */
 #define NCSI_PKT_AEN_HNCDSC 0x02 /* HNC driver status change */
 
+/* OEM Vendor Manufacture ID */
+#define NCSI_OEM_MFR_MLX_ID 0x8119
+#define NCSI_OEM_MFR_BCM_ID 0x113d
+#define NCSI_OEM_MFR_INTEL_ID 0x157
+/* Intel specific OEM command */
+#define NCSI_OEM_INTEL_CMD_GMA 0x06 /* CMD ID for Get MAC */
+#define NCSI_OEM_INTEL_CMD_KEEP_PHY 0x20 /* CMD ID for Keep PHY up */
+/* Broadcom specific OEM Command */
+#define NCSI_OEM_BCM_CMD_GMA 0x01 /* CMD ID for Get MAC */
+/* Mellanox specific OEM Command */
+#define NCSI_OEM_MLX_CMD_GMA 0x00 /* CMD ID for Get MAC */
+#define NCSI_OEM_MLX_CMD_GMA_PARAM 0x1b /* Parameter for GMA */
+#define NCSI_OEM_MLX_CMD_SMAF 0x01 /* CMD ID for Set MC Affinity */
+#define NCSI_OEM_MLX_CMD_SMAF_PARAM 0x07 /* Parameter for SMAF */
+/* Offset in OEM request */
+#define MLX_SMAF_MAC_ADDR_OFFSET 8 /* Offset for MAC in SMAF */
+#define MLX_SMAF_MED_SUPPORT_OFFSET 14 /* Offset for medium in SMAF */
+/* Mac address offset in OEM response */
+#define BCM_MAC_ADDR_OFFSET 28
+#define MLX_MAC_ADDR_OFFSET 8
+#define INTEL_MAC_ADDR_OFFSET 1
+
 #endif /* NCSI_PKT_H */
diff --git a/src/ncsi.c b/src/ncsi.c
index d0632bb..0d4c491 100644
--- a/src/ncsi.c
+++ b/src/ncsi.c
@@ -55,6 +55,59 @@ static uint32_t ncsi_calculate_checksum(uint8_t *data, int len)
     return checksum;
 }
 
+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_BCM_ID, NULL },
+    { NCSI_OEM_MFR_INTEL_ID, NULL },
+};
+
+/* Response handler for OEM command */
+static int ncsi_rsp_handler_oem(Slirp *slirp, const struct ncsi_pkt_hdr *nh,
+                                struct ncsi_rsp_pkt_hdr *rnh)
+{
+    const struct ncsi_rsp_oem_handler *nrh = NULL;
+    const struct ncsi_cmd_oem_pkt *cmd = (const struct ncsi_cmd_oem_pkt *)nh;
+    struct ncsi_rsp_oem_pkt *rsp = (struct ncsi_rsp_oem_pkt *)rnh;
+    uint32_t mfr_id = ntohl(cmd->mfr_id);
+    int i;
+
+    rsp->mfr_id = cmd->mfr_id;
+
+    if (mfr_id != slirp->mfr_id) {
+        goto error;
+    }
+
+    /* Check for manufacturer id and Find the handler */
+    for (i = 0; i < G_N_ELEMENTS(ncsi_rsp_oem_handlers); i++) {
+        if (ncsi_rsp_oem_handlers[i].mfr_id == mfr_id) {
+            if (ncsi_rsp_oem_handlers[i].handler)
+                nrh = &ncsi_rsp_oem_handlers[i];
+            else
+                nrh = NULL;
+
+            break;
+        }
+    }
+
+    if (!nrh) {
+        goto error;
+    }
+
+    /* Process the packet */
+    return nrh->handler(slirp, nh, rnh);
+
+error:
+    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;
+}
+
+
 /* Get Version ID */
 static int ncsi_rsp_handler_gvi(Slirp *slirp, const struct ncsi_pkt_hdr *nh,
                                 struct ncsi_rsp_pkt_hdr *rnh)
@@ -140,7 +193,7 @@ static const struct ncsi_rsp_handler {
                           { NCSI_PKT_RSP_GNS, 172, NULL },
                           { NCSI_PKT_RSP_GNPTS, 172, NULL },
                           { NCSI_PKT_RSP_GPS, 8, NULL },
-                          { NCSI_PKT_RSP_OEM, 0, NULL },
+                          { NCSI_PKT_RSP_OEM, 0, ncsi_rsp_handler_oem },
                           { NCSI_PKT_RSP_PLDM, 0, NULL },
                           { NCSI_PKT_RSP_GPUUID, 20, NULL } };
 
diff --git a/test/ncsitest.c b/test/ncsitest.c
index 12adfa3..4a6cada 100644
--- a/test/ncsitest.c
+++ b/test/ncsitest.c
@@ -43,13 +43,58 @@ static void test_ncsi_get_version_id(Slirp *slirp)
 
     const struct ncsi_rsp_gvi_pkt *gvi = slirp->opaque + ETH_HLEN;
 
-    assert(gvi->rsp.code == NCSI_PKT_RSP_C_COMPLETED);
-    assert(gvi->rsp.code == NCSI_PKT_RSP_R_NO_ERROR);
+    assert(ntohs(gvi->rsp.code) == NCSI_PKT_RSP_C_COMPLETED);
+    assert(ntohs(gvi->rsp.code) == NCSI_PKT_RSP_R_NO_ERROR);
     assert(ntohl(gvi->mf_id) == slirp->mfr_id);
 
     slirp->mfr_id = 0;
 }
 
+static void test_ncsi_oem_mlx_unsupported_command(Slirp *slirp)
+{
+    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 */
+        0xff, /* Command Revision */
+        0xff, /* Command ID */
+        0x00, /* Parameter */
+        0x00, /* Optional data */
+    };
+    const struct ncsi_rsp_oem_pkt *oem = slirp->opaque + ETH_HLEN;
+
+    slirp->mfr_id = 0x00000000;
+    slirp_input(slirp, command, sizeof(command));
+
+    assert(ntohs(oem->rsp.code) == NCSI_PKT_RSP_C_UNSUPPORTED);
+    assert(ntohs(oem->rsp.reason) == NCSI_PKT_RSP_R_UNKNOWN);
+    assert(ntohl(oem->mfr_id) == 0x8119);
+
+    slirp->mfr_id = 0x8119;
+    slirp_input(slirp, command, sizeof(command));
+
+    assert(ntohs(oem->rsp.code) == NCSI_PKT_RSP_C_UNSUPPORTED);
+    assert(ntohs(oem->rsp.reason) == NCSI_PKT_RSP_R_UNKNOWN);
+    assert(ntohl(oem->mfr_id) == 0x8119);
+}
+
 static ssize_t send_packet(const void *buf, size_t len, void *opaque)
 {
     assert(len <= NCSI_RESPONSE_CAPACITY);
@@ -69,6 +114,7 @@ int main(int argc, char *argv[])
     slirp = slirp_new(&config, &callbacks, ncsi_response);
 
     test_ncsi_get_version_id(slirp);
+    test_ncsi_oem_mlx_unsupported_command(slirp);
 
     slirp_cleanup(slirp);
 }