path: root/epan/dissectors/packet-macsec.c

/* packet-macsec.c
 * Routines for IEEE 802.1AE MACsec dissection
 * Copyright 2013, Allan W. Nielsen <anielsen@vitesse.com>
 *
 * Wireshark - Network traffic analyzer
 * By Gerald Combs <gerald@wireshark.org>
 * Copyright 1998 Gerald Combs
 *
 * SPDX-License-Identifier: GPL-2.0-or-later
 */

#include "config.h"

#include <epan/packet.h>
#include <epan/etypes.h>
#include <wsutil/wsgcrypt.h>

void proto_register_macsec(void);
void proto_reg_handoff_macsec(void);

static dissector_handle_t macsec_handle;
static dissector_handle_t ethertype_handle;

/* TCI/AN field masks */
#define TCI_MASK     0xFC
#define TCI_V_MASK   0x80
#define TCI_ES_MASK  0x40
#define TCI_SC_MASK  0x20
#define TCI_SCB_MASK 0x10
#define TCI_E_MASK   0x08
#define TCI_C_MASK   0x04
#define AN_MASK      0x03

#define AES_KEY_LEN           (16)
#define ICV_LEN               (16)
#define IV_LEN                (12)

#define HWADDR_LEN            (6)
#define ETHERTYPE_LEN         (2)
#define ETHHDR_LEN            ((HWADDR_LEN * 2) + ETHERTYPE_LEN)

#define SECTAG_LEN_WITH_SC    (14)
#define SECTAG_LEN_WITHOUT_SC (6)

#define AAD_ENCRYPTED_LEN     (28)

#define MAX_PAYLOAD_LEN       (1500)


static int proto_macsec;
static int hf_macsec_TCI;
static int hf_macsec_TCI_V;
static int hf_macsec_TCI_ES;
static int hf_macsec_TCI_SC;
static int hf_macsec_TCI_SCB;
static int hf_macsec_TCI_E;
static int hf_macsec_TCI_C;
static int hf_macsec_AN;
static int hf_macsec_SL;
static int hf_macsec_PN;
static int hf_macsec_SCI_system_identifier;
static int hf_macsec_SCI_port_identifier;
static int hf_macsec_etype;
static int hf_macsec_eth_padding;
static int hf_macsec_ICV;
static int hf_macsec_ICV_check_success;
static int hf_macsec_decrypted_data;

/* Initialize the subtree pointers */
static int ett_macsec;
static int ett_macsec_tci;

/* Decrypting payload buffer */
static uint8_t macsec_payload[MAX_PAYLOAD_LEN];

/* AAD buffer */
static uint8_t aad[MAX_PAYLOAD_LEN];

static const char *psk = NULL;
static unsigned char *psk_bin = NULL;

/* convert a 0-terminated preference key_string that contains a hex number
 *  into its binary representation
 * e.g. key_string "abcd" will be converted into two bytes 0xab, 0xcd
 * return the number of binary bytes or -1 for error */
static int
pref_key_string_to_bin(const char *key_string, unsigned char **key_bin)
{
    int key_string_len;
    int i, j;
    char input[3];

    ws_return_val_if(key_bin == NULL, -1);

    if (NULL == key_string) {
        *key_bin = NULL;
        return -1;
    }

    key_string_len = (int)strlen(key_string);
    if (key_string_len != 2 * AES_KEY_LEN) {
        *key_bin = NULL;
        return (key_string_len / 2);
    }

    *key_bin = (unsigned char *)g_malloc(key_string_len / 2);

    input[2] = '\0';
    for (i = 0, j = 0; i < (key_string_len - 1); i += 2, j++) {
        input[0] = key_string[0 + i];
        input[1] = key_string[1 + i];

        /* attention, brackets are required */
        (*key_bin)[j] = (unsigned char)strtoul((const char *)&input, NULL, 16);
    }

    return (key_string_len / 2);
}

/* Code to actually dissect the packets */
static int dissect_macsec(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void *data _U_) {
    unsigned    sectag_length, data_length, short_length, icv_length;
    unsigned    fcs_length = 0;
    unsigned    data_offset, icv_offset;
    uint8_t     tci_an_field;

    int         icv_check_success = PROTO_CHECKSUM_E_BAD;
    bool        key_provided = false;
    bool        encrypted = false;
    unsigned    payload_len;
    unsigned    offset;

    gcry_cipher_hd_t handle = 0;

    proto_item *macsec_item;
    proto_tree *macsec_tree = NULL;

    tvbuff_t *next_tvb;

    /* Construct the 14-byte ethernet header (6-byte dst MAC, 6-byte src MAC, 2-byte ethernet type)(part of aad) */
    uint8_t header[ETHHDR_LEN] = {0};
    if (pinfo->dl_dst.data != NULL)
    {
        memcpy(header, pinfo->dl_dst.data, HWADDR_LEN);
    }
    if (pinfo->dl_src.data != NULL)
    {
        memcpy((header + HWADDR_LEN), pinfo->dl_src.data, HWADDR_LEN);
    }

    uint8_t e_type[ETHERTYPE_LEN] = {(uint8_t)(ETHERTYPE_MACSEC >> 8), (uint8_t)(ETHERTYPE_MACSEC & 0xff)};
    memcpy(header + (ETHHDR_LEN - ETHERTYPE_LEN), &e_type, ETHERTYPE_LEN);

    /* Parse the encryption key, and set the flag to indicate if the key is provided*/
    if (pref_key_string_to_bin(psk, &psk_bin) == AES_KEY_LEN) {
        key_provided = true;
    }

    tci_an_field = tvb_get_uint8(tvb, 0);

    /* if the frame is an encrypted MACsec frame, remember that */
    if (((tci_an_field & TCI_E_MASK) == TCI_E_MASK) || ((tci_an_field & TCI_C_MASK) == TCI_C_MASK)) {
        encrypted = true;
    }

    if ((tci_an_field & TCI_V_MASK) != 0) {  /* version must be zero */
        return 0;
    }

    icv_length = ICV_LEN;  /* Fixed size for version 0 */

    if (tci_an_field & TCI_SC_MASK) {
        sectag_length = SECTAG_LEN_WITH_SC;  /* optional SCI present */
    } else {
        sectag_length = SECTAG_LEN_WITHOUT_SC;
    }

    /* Check for length too short */
    if (tvb_captured_length(tvb) <= (sectag_length + icv_length)) {
        return 0;
    }

    /* short length field: 1..47 bytes, 0 means 48 bytes or more */
    short_length = (uint32_t)tvb_get_uint8(tvb, 1);

    /* Get the payload section */
    if (short_length != 0) {
        data_length = short_length;
        fcs_length = tvb_reported_length(tvb) - sectag_length - icv_length - short_length;

        /*
         * We know the length, so set it here for the previous ethertype
         * dissector. This will allow us to calculate the FCS correctly.
         */
        set_actual_length(tvb, short_length + sectag_length + icv_length);
    } else {
        /*
         * This assumes that no FCS is present after the ICV, which might not be true!
         * Workaround: turn Ethernet "Assume packets have FCS" = Always, when FCS present.
         * If there's another (non FCS) trailer afterwards, set Ethernet
         * "Fixed ethernet trailer length".
         *
         * TODO: Find better heuristic to detect presence of FCS / trailers.
         */
        data_length = tvb_reported_length(tvb) - sectag_length - icv_length;
    }
    data_offset = sectag_length;
    icv_offset  = data_length + data_offset;

    col_set_str(pinfo->cinfo, COL_PROTOCOL, "MACSEC");
    col_set_str(pinfo->cinfo, COL_INFO, "MACsec frame");

    if (tree) {
        if (encrypted) {
            macsec_item = proto_tree_add_item(tree, proto_macsec, tvb, 0, sectag_length, ENC_NA);
        } else {
            /* Add the EtherType too since this is authentication only. */
            macsec_item = proto_tree_add_item(tree, proto_macsec, tvb, 0, sectag_length + ETHERTYPE_LEN, ENC_NA);
        }
        macsec_tree = proto_item_add_subtree(macsec_item, ett_macsec);

        static int * const flags[] = {
            &hf_macsec_TCI_V,
            &hf_macsec_TCI_ES,
            &hf_macsec_TCI_SC,
            &hf_macsec_TCI_SCB,
            &hf_macsec_TCI_E,
            &hf_macsec_TCI_C,
            NULL
        };

        proto_tree_add_bitmask_with_flags(macsec_tree, tvb, 0,
                hf_macsec_TCI, ett_macsec_tci, flags, ENC_NA, BMT_NO_TFS);

        offset = 0;
        proto_tree_add_item(macsec_tree, hf_macsec_AN, tvb, offset, 1, ENC_NA);
        offset += 1;

        proto_tree_add_item(macsec_tree, hf_macsec_SL, tvb, offset, 1, ENC_NA);
        offset += 1;

        proto_tree_add_item(macsec_tree, hf_macsec_PN, tvb, offset, 4, ENC_BIG_ENDIAN);
        offset += 4;

        if (sectag_length == SECTAG_LEN_WITH_SC) {
            proto_tree_add_item(macsec_tree, hf_macsec_SCI_system_identifier, tvb, offset, HWADDR_LEN, ENC_NA);
            offset += HWADDR_LEN;

            proto_tree_add_item(macsec_tree, hf_macsec_SCI_port_identifier, tvb, offset, 2, ENC_BIG_ENDIAN);
        }
    }

    next_tvb = tvb_new_subset_length(tvb, data_offset, data_length);

    /* Try to decrypt/authenticate the data if a key is provided */
    if (key_provided) {
        /* Build the IV */
        uint8_t iv[IV_LEN] = {0};
        tvb_memcpy(tvb, iv,     6,  HWADDR_LEN); // SI System identifier (source MAC)
        tvb_memcpy(tvb, iv + 6, 12, 2);          // PI Port identifier
        tvb_memcpy(tvb, iv + 8, 2,  4);          // PN Packet number

        if (gcry_cipher_open(&handle, GCRY_CIPHER_AES, GCRY_CIPHER_MODE_GCM, 0))
        {
            ws_warning("gcry_cipher_open fail");
            goto out;
        }

        if (gcry_cipher_setkey(handle, psk_bin, AES_KEY_LEN))
        {
            ws_warning("gcry_cipher_setkey fail");
            goto out;
        }

        if (gcry_cipher_setiv(handle, iv, sizeof(iv)))
        {
            ws_warning("gcry_cipher_setiv fail");
            goto out;
        }

        if (encrypted) {
            payload_len = tvb_captured_length(next_tvb);

            /* For authenticated and encrypted data, the AAD is always 28 bytes and consists of the
            header data and security tag. */
            const uint8_t *buf = tvb_get_ptr(tvb, 0, SECTAG_LEN_WITH_SC);

            memcpy(aad, header, ETHHDR_LEN);
            memcpy(aad + ETHHDR_LEN, buf, SECTAG_LEN_WITH_SC);

            /* Authenticate with the AAD. */
            if (gcry_cipher_authenticate(handle, aad, AAD_ENCRYPTED_LEN))
            {
                ws_warning("gcry_cipher_authenticate fail");
                goto out;
            }

            tvb_memcpy(next_tvb, macsec_payload, 0, payload_len);

            /* Attempt to decrypt into the local buffer. */
            if (gcry_cipher_decrypt(handle, macsec_payload, payload_len, NULL, 0))
            {
                ws_warning("gcry_cipher_decrypt fail");
                goto out;
            }

        } else {
            /* the frame length for the AAD is the complete frame including ethernet header but without the ICV */
            unsigned frame_len = (ETHHDR_LEN + tvb_captured_length(tvb)) - ICV_LEN;

            // For authenticated-only data, the aad is the frame minus the ICV
            // We have to build the AAD since the incoming TVB payload does not have the Ethernet header.
            payload_len = frame_len - ETHHDR_LEN;

            // Copy the header we built previously, then the frame data up to the ICV.
            memcpy(aad, header, ETHHDR_LEN);
            memcpy((aad + ETHHDR_LEN), tvb_get_ptr(tvb, 0, payload_len), payload_len);

            /* Authenticate with the AAD. */
            if (gcry_cipher_authenticate(handle, aad, frame_len))
            {
                ws_warning("gcry_cipher_authenticate fail");
                goto out;
            }
        }

        /* Fetch the ICV and use it to verify the decrypted data. */
        uint8_t icv[ICV_LEN] = {0};
        tvb_memcpy(tvb, icv, icv_offset, icv_length);
        if (gcry_cipher_checktag(handle, icv, sizeof(icv)))
        {
            ws_info("gcry_cipher_checktag fail");
            goto out;
        }

        /* Everything checks out! */
        icv_check_success = PROTO_CHECKSUM_E_GOOD;
    }

out:
    if (0 != handle) {
        gcry_cipher_close(handle);
    }
    // Show the original data.
    call_data_dissector(next_tvb, pinfo, tree);

    ethertype_data_t ethertype_data;

    /* default the next tv_buff to remove ICV */
    /* lets hand over a buffer without ICV to limit effect of wrong padding calculation */
    next_tvb = tvb_new_subset_length(tvb, data_offset + 2, data_length - 2);
    ethertype_data.etype = tvb_get_ntohs(tvb, data_offset);

    // If the data are ok, attempt to continue dissection.
    if (PROTO_CHECKSUM_E_GOOD == icv_check_success)
    {
        if (encrypted) {
            tvbuff_t *plain_tvb;

            plain_tvb = tvb_new_child_real_data(next_tvb, (uint8_t *)wmem_memdup(pinfo->pool, macsec_payload, payload_len),
                                                payload_len, payload_len);
            ethertype_data.etype = tvb_get_ntohs(plain_tvb, 0);

            /* lets hand over a buffer without ICV to limit effect of wrong padding calculation */
            next_tvb = tvb_new_subset_length(plain_tvb, 2, payload_len - 2);

            /* show the decrypted data and original ethertype */
            proto_tree_add_item(tree, hf_macsec_decrypted_data, plain_tvb, 0, payload_len, ENC_NA);

            /* add the decrypted data as a data source for the next dissectors */
            add_new_data_source(pinfo, plain_tvb, "Decrypted Data");

            /* The ethertype is the one from the start of the decrypted data. */
            proto_tree_add_item(tree, hf_macsec_etype, plain_tvb, 0, 2, ENC_BIG_ENDIAN);

        } else {
            /* lets hand over a buffer without ICV to limit effect of wrong padding calculation */
            next_tvb = tvb_new_subset_length(tvb, data_offset + 2, data_length - 2);

            /* The ethertype is the original from the unencrypted data. */
            proto_tree_add_item(tree, hf_macsec_etype, tvb, data_offset, 2, ENC_BIG_ENDIAN);
        }
    }

    /* add the ICV to the sectag subtree */
    proto_tree_add_item(macsec_tree, hf_macsec_ICV, tvb, icv_offset, icv_length, ENC_NA);
    proto_tree_set_appendix(macsec_tree, tvb, icv_offset, icv_length);

    /* If the frame decoded, or was not encrypted, continue dissection */
    if ((PROTO_CHECKSUM_E_GOOD == icv_check_success) || (false == encrypted)) {
        /* help eth padding calculation by subtracting length of the sectag, ethertype, icv, and fcs */
        int pkt_len_saved = pinfo->fd->pkt_len;

        pinfo->fd->pkt_len -= (sectag_length + 2 + icv_length + fcs_length);

        /* continue dissection */
        ethertype_data.payload_offset = 0;
        ethertype_data.fh_tree = macsec_tree;
        /* XXX: This could be another trailer, a FCS, or the Ethernet dissector
            * incorrectly detecting padding if we don't have short_length. */
        ethertype_data.trailer_id = hf_macsec_eth_padding;
        ethertype_data.fcs_len = 0;

        call_dissector_with_data(ethertype_handle, next_tvb, pinfo, tree, &ethertype_data);

        /* restore original value */
        pinfo->fd->pkt_len = pkt_len_saved;
    }

    /* Set icv_check_success to the correct status */
    if (!key_provided) {
        icv_check_success = PROTO_CHECKSUM_E_UNVERIFIED;
    }

    /* If the frame was not verified correctly, append this string to the info line
     * after dissection completes.
     */
    if (PROTO_CHECKSUM_E_BAD == icv_check_success) {
        col_append_str(pinfo->cinfo, COL_INFO, " [Authentication fail]");
    }

    /* add a flag indicating the frame is or is not verified. */
    macsec_item = proto_tree_add_uint(macsec_tree, hf_macsec_ICV_check_success, tvb, 0, 0, icv_check_success);
    proto_item_set_generated(macsec_item);

    /* We called set_actual length if fcs_length !=0, so length is adjusted. */
    return tvb_captured_length(tvb);
}

void
proto_register_macsec(void)
{
    module_t *module;
    static hf_register_info hf[] = {
        { &hf_macsec_TCI,
            { "TCI", "macsec.TCI", FT_UINT8, BASE_HEX,
              NULL, TCI_MASK, "TAG Control Information", HFILL }
        },
        { &hf_macsec_TCI_V,
            { "VER", "macsec.TCI.V", FT_UINT8, BASE_HEX,
              NULL, TCI_V_MASK, "Version", HFILL }
        },
        { &hf_macsec_TCI_ES,
            { "ES", "macsec.TCI.ES", FT_BOOLEAN, 8,
              TFS(&tfs_set_notset), TCI_ES_MASK, "End Station", HFILL }
        },
        { &hf_macsec_TCI_SC,
            { "SC", "macsec.TCI.SC", FT_BOOLEAN, 8,
              TFS(&tfs_set_notset), TCI_SC_MASK, "Secure Channel", HFILL }
        },
        { &hf_macsec_TCI_SCB,
            { "SCB", "macsec.TCI.SCB", FT_BOOLEAN, 8,
              TFS(&tfs_set_notset), TCI_SCB_MASK, "Single Copy Broadcast", HFILL }
        },
        { &hf_macsec_TCI_E,
            { "E", "macsec.TCI.E", FT_BOOLEAN, 8,
              TFS(&tfs_set_notset), TCI_E_MASK, "Encryption", HFILL }
        },
        { &hf_macsec_TCI_C,
            { "C", "macsec.TCI.C", FT_BOOLEAN, 8,
              TFS(&tfs_set_notset), TCI_C_MASK, "Changed Text", HFILL }
        },
        { &hf_macsec_AN,
            { "AN", "macsec.AN", FT_UINT8, BASE_HEX,
              NULL, AN_MASK, "Association Number", HFILL }
        },
        { &hf_macsec_SL,
            { "Short length", "macsec.SL", FT_UINT8, BASE_DEC,
              NULL, 0, NULL, HFILL }
        },
        { &hf_macsec_PN,
            { "Packet number", "macsec.PN", FT_UINT32, BASE_DEC,
              NULL, 0, NULL, HFILL }
        },
        { &hf_macsec_SCI_system_identifier,
            { "System Identifier", "macsec.SCI.system_identifier", FT_ETHER, BASE_NONE,
                NULL, 0, NULL, HFILL }
        },
        { &hf_macsec_SCI_port_identifier,
            { "Port Identifier", "macsec.SCI.port_identifier", FT_UINT16, BASE_DEC,
              NULL, 0, NULL, HFILL }
        },
        { &hf_macsec_etype,
            { "Ethertype", "macsec.etype", FT_UINT16, BASE_HEX,
              NULL, 0, NULL, HFILL }
        },
        { &hf_macsec_eth_padding,
            { "Padding", "macsec.eth_padding", FT_BYTES, BASE_NONE,
              NULL, 0, NULL, HFILL }
        },
        { &hf_macsec_ICV,
            { "ICV", "macsec.ICV", FT_BYTES, BASE_NONE,
              NULL, 0, NULL, HFILL }
        },
        { &hf_macsec_ICV_check_success,
            { "Frame authentication status", "macsec.auth_status", FT_UINT8, BASE_DEC,
              NULL, 0, NULL, HFILL }
        },
        { &hf_macsec_decrypted_data,
            { "Decrypted Data", "macsec.decrypted_data", FT_BYTES, BASE_NONE,
            NULL, 0, NULL, HFILL }
        },
    };

    /* Setup protocol subtree array */
    static int *ett[] = {
        &ett_macsec,
        &ett_macsec_tci
    };

    /* Register the protocol name and description */
    proto_macsec = proto_register_protocol("802.1AE Security tag", "MACsec", "macsec");

    /* Required function calls to register the header fields and subtrees used */
    proto_register_field_array(proto_macsec, hf, array_length(hf));
    proto_register_subtree_array(ett, array_length(ett));

    /* Register the dissector */
    macsec_handle = register_dissector("macsec", dissect_macsec, proto_macsec);

    /* Register the text box to enter the pre-shared key */
    module = prefs_register_protocol(proto_macsec, NULL);
    prefs_register_string_preference(module, "psk", "MACsec Pre-Shared Key",
                                     "Pre-Shared AES-GCM-128 Key as a HEX string (16 bytes).",
                                     &psk);
}

void
proto_reg_handoff_macsec(void)
{
    dissector_add_uint("ethertype", ETHERTYPE_MACSEC, macsec_handle);

    ethertype_handle = find_dissector("ethertype");
}

/*
 * Editor modelines  -  https://www.wireshark.org/tools/modelines.html
 *
 * Local variables:
 * c-basic-offset: 4
 * tab-width: 8
 * indent-tabs-mode: nil
 * End:
 *
 * vi: set shiftwidth=4 tabstop=8 expandtab:
 * :indentSize=4:tabSize=8:noTabs=true:
 */