/* packet-lwm.c

 * Dissector  routines for the ATMEL Lightweight Mesh 1.1.1

 * Copyright 2013 Martin Leixner <info@sewio.net>

 *

 * 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/expert.h>

#include <wsutil/filesystem.h>

#include "packet-ieee802154.h"

#include <epan/prefs.h>

#include <epan/strutil.h>

#include <wsutil/wsgcrypt.h>

/*LwMesh lengths*/

#define LWM_HEADER_BASE_LEN            7

#define LWM_MIC_LEN                    4

#define LWM_MULTI_HEADER_LEN           2

/*  Bit-masks for the FCF */

#define LWM_FCF_ACK_REQUEST            0x01

#define LWM_FCF_SEC_EN                 0x02

#define LWM_FCF_LINK_LOCAL             0x04

#define LWM_FCF_MULTICAST              0x08

#define LWM_FCF_RESERVED               0xF0

#define LWM_MULTI_NON_MEM_RAD_MASK          0x000F

#define LWM_MULTI_NON_MEM_RAD_OFFSET        0

#define LWM_MULTI_MAX_NON_MEM_RAD_MASK      0x00F0

#define LWM_MULTI_MAX_NON_MEM_RAD_OFFSET    4

#define LWM_MULTI_MEM_RAD_MASK              0x0F00

#define LWM_MULTI_MEM_RAD_OFFSET            8

#define LWM_MULTI_MAX_MEM_RAD_MASK          0xF000

#define LWM_MULTI_MAX_MEM_RAD_OFFSET        12

/*Endpoints*/

#define LWM_SRC_ENDP_MASK               0xF0

#define LWM_SRC_ENDP_OFFSET             4

#define LWM_DST_ENDP_MASK               0x0F

#define LWM_DST_ENDP_OFFSET             0

/*Defined addresses*/

#define LWM_BCAST_ADDR                    0xFFFF

/*Command IDs*/

#define LWM_CMD_ACK                      0x00

#define LWM_CMD_ROUTE_ERR                0x01

#define LWM_CMD_ROUTE_REQ                0x02

#define LWM_CMD_ROUTE_REPLY              0x03

/*Lengths of command frames*/

#define LWM_CMD_FRAME_ACK_LEN              3

#define LWM_CMD_FRAME_ROUTE_ERR_LEN        6

#define LWM_CMD_FRAME_ROUTE_REQ_LEN        7

#define LWM_CMD_FRAME_ROUTE_REPLY_LEN      8

/*Values for multicast field*/

#define LWM_CMD_MULTI_ADDR_FALSE           0

#define LWM_CMD_MULTI_ADDR_TRUE            1

/*Defined strings*/

#define LWM_CMD_LINKQ_STRING            "(Sent by Originate node)"

#define LWM_CMD_UNKNOWN_VAL_STRING      "Unknown command (0x%02x)"

#define LWM_MULTI_UNICAST_STRING        "(Unicast)"

#define LWM_MULTI_GROUP_STRING          "(Group ID)"

/*  Function declarations */

void proto_register_lwm(void);

void proto_reg_handoff_lwm(void);

/* User string with the decryption key. */

static const char *lwmes_key_str;

static bool         lwmes_key_valid;

static uint8_t      lwmes_key[16];

/* Dissection Routines. */

static int  dissect_lwm                       (tvbuff_t *, packet_info *, proto_tree *, void *data);

static int  dissect_lwm_cmd_frame_ack         (tvbuff_t *, packet_info *, proto_tree *);

static int  dissect_lwm_cmd_frame_route_err   (tvbuff_t *, packet_info *, proto_tree *);

static int  dissect_lwm_cmd_frame_route_req   (tvbuff_t *, packet_info *, proto_tree *);

static int  dissect_lwm_cmd_frame_route_reply (tvbuff_t *, packet_info *, proto_tree *);

/*  Initialize protocol and registered fields. */

static int proto_lwm;

static int hf_lwm_fcf;

static int hf_lwm_fcf_ack_req;

static int hf_lwm_fcf_security;

static int hf_lwm_fcf_linklocal;

static int hf_lwm_fcf_multicast;

static int hf_lwm_fcf_reserved;

static int hf_lwm_seq;

static int hf_lwm_src_addr;

static int hf_lwm_dst_addr;

static int hf_lwm_src_endp;

static int hf_lwm_dst_endp;

static int hf_lwm_multi_nmrad;

static int hf_lwm_multi_mnmrad;

static int hf_lwm_multi_mrad;

static int hf_lwm_multi_mmrad;

static int hf_lwm_mic;

static int hf_lwm_cmd;

static int hf_lwm_cmd_seq;

static int hf_lwm_cmd_cm;

static int hf_lwm_cmd_route_src;

static int hf_lwm_cmd_route_dst;

static int hf_lwm_cmd_route_multi;

static int hf_lwm_cmd_linkquality;

static int hf_lwm_cmd_forwlinkquality;

static int hf_lwm_cmd_revlinkquality;

/* Initialize protocol subtrees. */

static int ett_lwm;

static int ett_lwm_fcf;

static int ett_lwm_cmd_tree;

static int ett_lwm_multi_tree;

static expert_field ei_lwm_mal_error;

static expert_field ei_lwm_n_src_broad;

static expert_field ei_lwm_mismatch_endp;

static expert_field ei_lwm_empty_payload;

static expert_field ei_lwm_no_decryption_key;

static expert_field ei_lwm_decryption_failed;

static dissector_handle_t lwm_handle;

static const value_string lwm_cmd_names[] = {

    { LWM_CMD_ACK,          "LwMesh ACK" },

    { LWM_CMD_ROUTE_ERR,    "Route Error" },

    { LWM_CMD_ROUTE_REQ,    "Route Request" },

    { LWM_CMD_ROUTE_REPLY,  "Route Reply" },

    { 0, NULL }

};

static const value_string lwm_cmd_multi_names[] = {

    { LWM_CMD_MULTI_ADDR_FALSE, "FALSE" },

    { LWM_CMD_MULTI_ADDR_TRUE,  "TRUE" },

    { 0, NULL }

};

/*FUNCTION:------------------------------------------------------

 *  NAME

 *      dissect_lwm_heur

 *  DESCRIPTION

 *      Heuristic interpreter for the Lightweight Mesh.

 *  PARAMETERS

 *      tvbuff_t *tvb       - pointer to buffer containing raw packet.

 *      packet_into *pinfo  - pointer to packet information fields

 *      proto_tree *tree    - pointer to data tree Wireshark uses to display packet.

 *  RETURNS

 *      Boolean value, whether it handles the packet or not.

 *---------------------------------------------------------------

 */

static bool

dissect_lwm_heur(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void *data)

{

    uint8_t endpt, srcep, dstep;

    /* 1) first byte must have bits 0000xxxx */

    if(tvb_get_uint8(tvb, 0) & LWM_FCF_RESERVED)

        return false;

    /* The header should be at least long enough for the base header. */

    if (tvb_reported_length(tvb) < LWM_HEADER_BASE_LEN)

        return false;

    /* The endpoints should either both be zero, or both non-zero. */

    endpt = tvb_get_uint8(tvb, 6);

    srcep = (endpt & LWM_SRC_ENDP_MASK) >> LWM_SRC_ENDP_OFFSET;

    dstep = (endpt & LWM_DST_ENDP_MASK) >> LWM_DST_ENDP_OFFSET;

    if ((srcep == 0) && (dstep != 0))

        return false;

    if ((srcep != 0) && (dstep == 0))

        return false;

    dissect_lwm(tvb, pinfo, tree, data);

    return true;

} /* dissect_lwm_heur */

/*FUNCTION:------------------------------------------------------

 *  NAME

 *      dissect_lwm

 *  DESCRIPTION

 *      Lightweight Mesh packet dissection routine for Wireshark.

 *  PARAMETERS

 *      tvbuff_t *tvb       - pointer to buffer containing raw packet.

 *      packet_info *pinfo  - pointer to packet information fields

 *      proto_tree *tree    - pointer to data tree Wireshark uses to display packet.

 *  RETURNS

 *      int                 - length of data processed, or 0 if not LWM.

 *---------------------------------------------------------------

 */

static int dissect_lwm(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void *data _U_)

{

    unsigned    lwm_header_len;

    uint8_t     lwm_fcf;

    bool        lwm_fcf_security;

    bool        lwm_fcf_multicast;

    uint8_t     lwm_seq;

    uint16_t    lwm_src_addr;

    uint16_t    lwm_dst_addr;

    uint8_t     lwm_endp_field;

    uint8_t     lwm_src_endp;

    uint8_t     lwm_dst_endp;

    proto_tree *lwm_tree        = NULL;

    proto_item *ti_proto        = NULL;

    proto_item *ti;

    tvbuff_t   *new_tvb;

    /*---------------------------------------------------------*/

    /*Enter name of protocol to info field*/

    col_set_str(pinfo->cinfo, COL_PROTOCOL, "LwMesh");

    col_clear(pinfo->cinfo, COL_INFO);

    /*Set base length of LWM header*/

    lwm_header_len = LWM_HEADER_BASE_LEN;

    /*--------------------------------------------------*/

    /*                                                  */

    /*        Create LwMesh dissector tree              */

    /*                                                  */

    /*--------------------------------------------------*/

    if(tree){

        /*Create subtree for the LwMesh*/

        ti_proto = proto_tree_add_protocol_format(tree, proto_lwm, tvb, 0, -1, "Lightweight Mesh");

        lwm_tree = proto_item_add_subtree(ti_proto, ett_lwm);

    }

    col_set_str(pinfo->cinfo, COL_INFO, "Lightweight Mesh");

    /*--------------------------------------------------*/

    /*                                                  */

    /*        Display LwMesh dissector tree             */

    /*                                                  */

    /*--------------------------------------------------*/

    /*Frame control fields*/

    lwm_fcf = tvb_get_uint8(tvb, 0);

    lwm_fcf_security  = (lwm_fcf & LWM_FCF_SEC_EN);

    lwm_fcf_multicast = (lwm_fcf & LWM_FCF_MULTICAST);

    if(tree){

        proto_tree *field_tree;

        ti = proto_tree_add_uint(lwm_tree, hf_lwm_fcf, tvb, 0, 1, lwm_fcf);

        field_tree = proto_item_add_subtree(ti, ett_lwm_fcf);

        proto_tree_add_item(field_tree, hf_lwm_fcf_ack_req,   tvb, 0, 1, ENC_NA);

        proto_tree_add_item(field_tree, hf_lwm_fcf_security,  tvb, 0, 1, ENC_NA);

        proto_tree_add_item(field_tree, hf_lwm_fcf_linklocal, tvb, 0, 1, ENC_NA);

        proto_tree_add_item(field_tree, hf_lwm_fcf_multicast, tvb, 0, 1, ENC_NA);

        proto_tree_add_item(field_tree, hf_lwm_fcf_reserved,  tvb, 0, 1, ENC_NA);

    }

    /*Sequence number*/

    lwm_seq = tvb_get_uint8(tvb, 1);

    proto_item_append_text(ti_proto, ", Sequence Number: %i", lwm_seq);

    proto_tree_add_uint(lwm_tree, hf_lwm_seq, tvb, 1, 1, lwm_seq);

    /*Network addresses*/

    /*Parse Source address*/

    lwm_src_addr   = tvb_get_letohs(tvb, 2);

    ti = proto_tree_add_uint(lwm_tree, hf_lwm_src_addr, tvb, 2, 2, lwm_src_addr);

    if(lwm_src_addr < 0x8000){

        proto_item_append_text(ti, " (Routing node)");

    }else{

        proto_item_append_text(ti, " (Non-routing node)");

    }

    /*Check value of source address*/

    if(lwm_src_addr == LWM_BCAST_ADDR){

        expert_add_info(pinfo, lwm_tree, &ei_lwm_n_src_broad);

    }

    /*Parse Destination address*/

    lwm_dst_addr   = tvb_get_letohs(tvb, 4);

    if(lwm_dst_addr == LWM_BCAST_ADDR){

        proto_tree_add_uint_format_value(lwm_tree, hf_lwm_dst_addr, tvb, 4, 2, lwm_dst_addr,

                                         "Broadcast (0x%04x)", lwm_dst_addr);

    }else{

        ti = proto_tree_add_uint(lwm_tree, hf_lwm_dst_addr, tvb, 4, 2, lwm_dst_addr);

        if(lwm_fcf_multicast){

            proto_item_append_text(ti, " %s", LWM_MULTI_GROUP_STRING);

        }else{

            proto_item_append_text(ti, " %s", LWM_MULTI_UNICAST_STRING);

            if(lwm_dst_addr < 0x8000){

                proto_item_append_text(ti, " (Routing node)");

            }else{

                proto_item_append_text(ti, " (Non-routing node)");

            }

        }

    }

    /*Enter description to info field*/

    col_append_fstr(pinfo->cinfo, COL_INFO, ", Nwk_Dst: 0x%04x, Nwk_Src: 0x%04x", lwm_dst_addr, lwm_src_addr);

    /*Endpoints*/

    lwm_endp_field = tvb_get_uint8(tvb, 6);

    lwm_src_endp   = (lwm_endp_field & LWM_SRC_ENDP_MASK) >> LWM_SRC_ENDP_OFFSET;

    lwm_dst_endp   = (lwm_endp_field & LWM_DST_ENDP_MASK) >> LWM_DST_ENDP_OFFSET;

    ti = proto_tree_add_uint(lwm_tree, hf_lwm_src_endp, tvb, 6, 1, lwm_src_endp);

    if(lwm_src_endp == 0){

        proto_item_append_text(ti, " (Stack command endpoint)");

    }

    ti = proto_tree_add_uint(lwm_tree, hf_lwm_dst_endp, tvb, 6, 1, lwm_dst_endp);

    if(lwm_dst_endp == 0){

        proto_item_append_text(ti, " (Stack command endpoint)");

    }

    if( (lwm_src_endp == 0) && (lwm_dst_endp == 0)){

        /*stack command endpoints*/

    }

    else if( (lwm_src_endp == 0) || (lwm_dst_endp == 0)){

        /*If only one endpoint is 0, alert about that*/

        col_append_str(pinfo->cinfo, COL_INFO, "[Stack command Endpoints mismatch]");

        expert_add_info(pinfo, lwm_tree, &ei_lwm_mismatch_endp);

    }

    /*Multicast header*/

    if( (lwm_fcf_multicast) ){

        lwm_header_len  += LWM_MULTI_HEADER_LEN;

        if(tree){

            proto_tree *multi_tree;

            uint16_t    lwm_multi_header;

            lwm_multi_header =  tvb_get_letohs(tvb, 7);

            multi_tree = proto_tree_add_subtree(lwm_tree, tvb, 7, 2, ett_lwm_multi_tree, NULL, "Multicast Header");

            proto_tree_add_uint(multi_tree, hf_lwm_multi_nmrad, tvb, 7, 2,

                                (lwm_multi_header & LWM_MULTI_NON_MEM_RAD_MASK) >> LWM_MULTI_NON_MEM_RAD_OFFSET);

            proto_tree_add_uint(multi_tree, hf_lwm_multi_mnmrad, tvb, 7, 2,

                                (lwm_multi_header & LWM_MULTI_MAX_NON_MEM_RAD_MASK) >> LWM_MULTI_MAX_NON_MEM_RAD_OFFSET);

            proto_tree_add_uint(multi_tree, hf_lwm_multi_mrad, tvb, 7, 2,

                                (lwm_multi_header & LWM_MULTI_MEM_RAD_MASK) >> LWM_MULTI_MEM_RAD_OFFSET);

            proto_tree_add_uint(multi_tree, hf_lwm_multi_mmrad, tvb, 7, 2,

                                (lwm_multi_header & LWM_MULTI_MAX_MEM_RAD_MASK) >> LWM_MULTI_MAX_MEM_RAD_OFFSET);

        }

    }

    /*------------------------------*/

    /*                              */

    /*       Dissect payload        */

    /*                              */

    /*------------------------------*/

    /*Note: exception will already have occurred if "short header"*/

    if (tvb_reported_length(tvb) <= lwm_header_len) {

        /*Empty payload*/

        expert_add_info(pinfo, lwm_tree, &ei_lwm_empty_payload);

        col_append_str(pinfo->cinfo, COL_INFO, "[Empty LwMesh Payload]");

        return tvb_captured_length(tvb);

    }

    new_tvb = tvb_new_subset_remaining(tvb, lwm_header_len);

    /*Encrypted data*/

    if(lwm_fcf_security){

        unsigned rlen;

        int   start;

        uint32_t lwm_mic;

        /*MIC field*/

        rlen = tvb_reported_length(new_tvb);

        start = (rlen >= LWM_MIC_LEN) ? (rlen-LWM_MIC_LEN) : 0;

        /*An exception will occur if there are not enough bytes for the MIC */

        proto_tree_add_item_ret_uint(lwm_tree, hf_lwm_mic, new_tvb, start, LWM_MIC_LEN, ENC_LITTLE_ENDIAN, &lwm_mic);

        if(lwmes_key_valid)

        {

            ieee802154_packet *ieee_packet = NULL;

            int payload_length = 0;

            int length = 0;

            int payload_offset = 0;

            uint8_t block;

            tvbuff_t *decrypted_tvb;

            gcry_cipher_hd_t cypher_hd;

            uint8_t* vector = NULL;

            uint8_t* text =NULL;

            uint8_t* text_dec =NULL;

            uint8_t i;

            uint32_t vmic;

            uint32_t nwkSecurityVector[4];

            int gcrypt_err;

            ieee_packet = (ieee802154_packet *)data;

            memset(&nwkSecurityVector, 0, sizeof(nwkSecurityVector));

            nwkSecurityVector[0] = lwm_seq;

            nwkSecurityVector[1] = ((uint32_t)lwm_dst_addr<< 16) | lwm_dst_endp;

            nwkSecurityVector[2]= ((uint32_t) lwm_src_addr<< 16) | lwm_src_endp;

            nwkSecurityVector[3] = ((uint32_t)ieee_packet->dst_pan << 16) | (uint8_t)lwm_fcf;

            payload_length=tvb_reported_length(new_tvb) - LWM_MIC_LEN;

            /* ECB - Nwk security vector*/

            text = (uint8_t *)tvb_memdup(pinfo->pool, new_tvb, 0, payload_length);

            payload_offset=0;

            gcrypt_err = gcry_cipher_open(&cypher_hd, GCRY_CIPHER_AES128, GCRY_CIPHER_MODE_ECB, 0);

            /*Decrypt the actual data */

            while(payload_length>0)

            {

                if(gcrypt_err == 0) {

                    gcrypt_err = gcry_cipher_setkey(cypher_hd,(uint8_t *)lwmes_key, 16);

                }

                if(gcrypt_err == 0) {

                    gcrypt_err = gcry_cipher_encrypt(cypher_hd,(uint8_t *)nwkSecurityVector,16,(uint8_t *)nwkSecurityVector,16);

                }

                if(gcrypt_err)

                {

                    col_add_fstr(pinfo->cinfo, COL_INFO,

                         "Encrypted data (%i byte(s)) DECRYPT FAILED",

                         tvb_reported_length(new_tvb) - LWM_MIC_LEN);

                    expert_add_info(pinfo, lwm_tree, &ei_lwm_decryption_failed);

                    tvb_set_reported_length(new_tvb, tvb_reported_length(new_tvb) - LWM_MIC_LEN);

                    call_data_dissector(new_tvb, pinfo, lwm_tree);

                }

                text_dec = &text[payload_offset];

                vector = (uint8_t *)nwkSecurityVector;

                block =  (payload_length < 16) ? payload_length : 16;

                for (i = 0; i < block; i++)

                {

                    text_dec[i] ^= vector[i];

                    /*

                     * GCC 12.2.0 gives a false positive Wstringop-overflow warning.

                     * https://gitlab.com/wireshark/wireshark/-/issues/18383

                     */

                    DIAG_OFF_STRINGOP_OVERFLOW()

                    vector[i] ^= text_dec[i];

                    DIAG_ON_STRINGOP_OVERFLOW()

                }

                payload_offset += block;

                payload_length -= block;

                gcry_cipher_reset(cypher_hd);

            }

            gcry_cipher_close(cypher_hd);

            vmic = nwkSecurityVector[0] ^ nwkSecurityVector[1] ^ nwkSecurityVector[2] ^ nwkSecurityVector[3];

            length = tvb_reported_length(new_tvb) - LWM_MIC_LEN;

            if(vmic == lwm_mic)

            {

                decrypted_tvb = tvb_new_real_data(text,length, length);

                call_data_dissector(decrypted_tvb, pinfo, lwm_tree);

                /* XXX - needed?

                   add_new_data_source(pinfo, decrypted_tvb, "Decrypted LWmesh Payload"); */

                col_append_str(pinfo->cinfo, COL_INFO, ",  MIC SUCCESS");

            }

            else

            {

                col_add_fstr(pinfo->cinfo, COL_INFO,

                     "Encrypted data (%i byte(s)) MIC FAILURE",

                     tvb_reported_length(new_tvb) - LWM_MIC_LEN);

                tvb_set_reported_length(new_tvb, tvb_reported_length(new_tvb) - LWM_MIC_LEN);

                call_data_dissector(new_tvb, pinfo, lwm_tree);

            }

        }

        else

        {

            col_add_fstr(pinfo->cinfo, COL_INFO,

                     "Encrypted data (%i byte(s)) NO DECRYPT KEY",

                      tvb_reported_length(new_tvb) - LWM_MIC_LEN);

            expert_add_info(pinfo, lwm_tree, &ei_lwm_no_decryption_key);

            tvb_set_reported_length(new_tvb, tvb_reported_length(new_tvb) - LWM_MIC_LEN);

            call_data_dissector(new_tvb, pinfo, lwm_tree);

        }

    }

    /*stack command endpoint 0 and not secured*/

    else if( (lwm_src_endp == 0) && (lwm_dst_endp == 0) ){

        proto_tree *lwm_cmd_tree;

        uint8_t     lwm_cmd;

        unsigned    len;

        char       *str_cmd;

        /*----------------------------------------------------------------------*/

        /*                                                                      */

        /*  Call command dissector (depends on value of first byte of payload)  */

        /*                                                                      */

        /*----------------------------------------------------------------------*/

        lwm_cmd = tvb_get_uint8(new_tvb, 0);

        str_cmd = val_to_str(pinfo->pool, lwm_cmd, lwm_cmd_names, LWM_CMD_UNKNOWN_VAL_STRING);

        col_clear(pinfo->cinfo, COL_INFO);  /*XXX: why ?*/

        col_add_str(pinfo->cinfo, COL_INFO, str_cmd);

        lwm_cmd_tree = proto_tree_add_subtree(lwm_tree, new_tvb, 0, -1, ett_lwm_cmd_tree, &ti, str_cmd);

        proto_tree_add_uint(lwm_cmd_tree, hf_lwm_cmd, new_tvb, 0, 1, lwm_cmd);

        switch (lwm_cmd) {

        case LWM_CMD_ACK:

            len = dissect_lwm_cmd_frame_ack(new_tvb, pinfo, lwm_cmd_tree);

            break;

        case LWM_CMD_ROUTE_ERR:

            len = dissect_lwm_cmd_frame_route_err(new_tvb, pinfo, lwm_cmd_tree);

            break;

        case LWM_CMD_ROUTE_REQ:

            len = dissect_lwm_cmd_frame_route_req(new_tvb, pinfo, lwm_cmd_tree);

            break;

        case LWM_CMD_ROUTE_REPLY:

            len = dissect_lwm_cmd_frame_route_reply(new_tvb, pinfo, lwm_cmd_tree);

            break;

        default:

            /*Unknown command*/

            expert_add_info_format(pinfo, lwm_cmd_tree, &ei_lwm_mal_error, "Unknown command");

            call_data_dissector(new_tvb, pinfo, lwm_cmd_tree);

            return tvb_captured_length(tvb);

        }

        proto_item_set_len(ti, len);

        /*Here only if additional data after valid 'cmd' data*/

        /*Note: exception will have already occurred if tvb was missing required bytes for 'cmd'*/

        /*      Report error if additional undissected data*/

        if (len < tvb_reported_length(new_tvb)) {

            /*unknown additional data*/

            expert_add_info_format(pinfo, lwm_cmd_tree, &ei_lwm_mal_error,

                "Size is %i byte(s), instead of %i bytes", tvb_reported_length(new_tvb), len);

            new_tvb = tvb_new_subset_remaining(new_tvb, len);

            call_data_dissector(new_tvb, pinfo, lwm_tree);

        }

    }

    else{

        /*unknown data*/

        call_data_dissector(new_tvb, pinfo, lwm_tree);

    }

    return tvb_captured_length(tvb);

} /* dissect_lwm */

/*FUNCTION:------------------------------------------------------

 *  NAME

 *      dissect_lwm_cmd_frame_ack

 *  DESCRIPTION

 *      LwMesh command frame - Ack.

 *

 *  PARAMETERS

 *      tvbuff_t *tvb       - pointer to buffer containing raw packet.

 *      packet_info *pinfo  - pointer to packet information fields

 *      proto_tree *tree    - pointer to data tree wireshark uses to display packet.

 *  RETURNS

 *      int length          - amount of data processed

 *---------------------------------------------------------------

 */

static int dissect_lwm_cmd_frame_ack(tvbuff_t *tvb, packet_info *pinfo, proto_tree *lwm_cmd_tree)

{

    uint8_t lwm_seq;

    /*Get fields*/

    lwm_seq = tvb_get_uint8(tvb, 1);

    col_append_fstr(pinfo->cinfo, COL_INFO, ", Sequence number: %d", lwm_seq);

    if(lwm_cmd_tree){

        proto_item_append_text(proto_tree_get_parent(lwm_cmd_tree), ", Sequence number: %d", lwm_seq);

        proto_tree_add_uint(lwm_cmd_tree, hf_lwm_cmd_seq, tvb, 1, 1, lwm_seq);

        proto_tree_add_item(lwm_cmd_tree, hf_lwm_cmd_cm,  tvb, 2, 1, ENC_NA);

    }

    return LWM_CMD_FRAME_ACK_LEN;

} /* dissect_lwm_cmd_frame_ack*/

/*FUNCTION:------------------------------------------------------

 *  NAME

 *      dissect_lwm_cmd_frame_route_err

 *  DESCRIPTION

 *      LwMesh command frame - Route error.

 *

 *  PARAMETERS

 *      tvbuff_t *tvb       - pointer to buffer containing raw packet.

 *      packet_info *pinfo  - pointer to packet information fields

 *      proto_tree *tree    - pointer to data tree wireshark uses to display packet.

 *  RETURNS

 *      int length          - amount of data processed

 *---------------------------------------------------------------

 */

static int dissect_lwm_cmd_frame_route_err(tvbuff_t *tvb, packet_info *pinfo _U_, proto_tree *lwm_cmd_tree)

{

    if(lwm_cmd_tree){

        proto_item *ti;

        proto_tree_add_item(lwm_cmd_tree, hf_lwm_cmd_route_src, tvb, 1, 2, ENC_LITTLE_ENDIAN);

        ti = proto_tree_add_item(lwm_cmd_tree, hf_lwm_cmd_route_dst, tvb, 3, 2, ENC_LITTLE_ENDIAN);

        if(tvb_get_uint8(tvb, 5) == LWM_CMD_MULTI_ADDR_TRUE){

            proto_item_append_text(ti, " %s", LWM_MULTI_GROUP_STRING);

        }else{

            proto_item_append_text(ti, " %s", LWM_MULTI_UNICAST_STRING);

        }

        proto_tree_add_item(lwm_cmd_tree, hf_lwm_cmd_route_multi, tvb, 5, 1, ENC_NA);

    }

    return LWM_CMD_FRAME_ROUTE_ERR_LEN;

} /* dissect_lwm_cmd_frame_route_err*/

/*FUNCTION:------------------------------------------------------

 *  NAME

 *      dissect_lwm_cmd_frame_route_req

 *  DESCRIPTION

 *      LwMesh command frame - Route Request.

 *

 *  PARAMETERS

 *      tvbuff_t *tvb       - pointer to buffer containing raw packet.

 *      packet_info *pinfo  - pointer to packet information fields

 *      proto_tree *tree    - pointer to data tree wireshark uses to display packet.

 *  RETURNS

 *      int length          - amount of data processed

 *---------------------------------------------------------------

 */

static int dissect_lwm_cmd_frame_route_req(tvbuff_t *tvb, packet_info *pinfo _U_, proto_tree *lwm_cmd_tree)

{

    if(lwm_cmd_tree){

        proto_item *ti;

        uint8_t     lwm_linkqual;

        proto_tree_add_item(lwm_cmd_tree, hf_lwm_cmd_route_src, tvb, 1, 2, ENC_LITTLE_ENDIAN);

        ti = proto_tree_add_item(lwm_cmd_tree, hf_lwm_cmd_route_dst, tvb, 3, 2, ENC_LITTLE_ENDIAN);

        if(tvb_get_uint8(tvb, 5) == LWM_CMD_MULTI_ADDR_TRUE){

            proto_item_append_text(ti, " %s", LWM_MULTI_GROUP_STRING);

        }else{

            proto_item_append_text(ti, " %s", LWM_MULTI_UNICAST_STRING);

        }

        proto_tree_add_item(lwm_cmd_tree, hf_lwm_cmd_route_multi, tvb, 5, 1, ENC_NA);

        lwm_linkqual  = tvb_get_uint8(tvb, 6);

        ti = proto_tree_add_uint(lwm_cmd_tree, hf_lwm_cmd_linkquality, tvb, 6, 1, lwm_linkqual);

        if(lwm_linkqual == 255){

            proto_item_append_text(ti, " %s", LWM_CMD_LINKQ_STRING);

        }

    }

    return LWM_CMD_FRAME_ROUTE_REQ_LEN;

} /* dissect_lwm_cmd_frame_route_req*/

/*FUNCTION:------------------------------------------------------

 *  NAME

 *      dissect_lwm_cmd_frame_route_reply

 *  DESCRIPTION

 *      LwMesh command frame - Route Reply.

 *

 *  PARAMETERS

 *      tvbuff_t *tvb       - pointer to buffer containing raw packet.

 *      packet_info *pinfo  - pointer to packet information fields

 *      proto_tree *tree    - pointer to data tree wireshark uses to display packet.

 *  RETURNS

 *      int length          - amount of data processed

 *---------------------------------------------------------------

 */

static int dissect_lwm_cmd_frame_route_reply(tvbuff_t *tvb, packet_info *pinfo _U_, proto_tree *lwm_cmd_tree)

{

    if(lwm_cmd_tree){

        proto_item *ti;

        uint8_t     lwm_revlinkqual;

        proto_tree_add_item(lwm_cmd_tree, hf_lwm_cmd_route_src, tvb, 1, 2, ENC_LITTLE_ENDIAN);

        ti = proto_tree_add_item(lwm_cmd_tree, hf_lwm_cmd_route_dst, tvb, 3, 2, ENC_LITTLE_ENDIAN);

        if(tvb_get_uint8(tvb, 5) == LWM_CMD_MULTI_ADDR_TRUE){

            proto_item_append_text(ti, " %s", LWM_MULTI_GROUP_STRING);

        }else{

            proto_item_append_text(ti, " %s", LWM_MULTI_UNICAST_STRING);

        }

        proto_tree_add_item(lwm_cmd_tree, hf_lwm_cmd_route_multi, tvb, 5, 1, ENC_NA);

        proto_tree_add_item(lwm_cmd_tree, hf_lwm_cmd_forwlinkquality, tvb, 6, 1, ENC_NA);

        lwm_revlinkqual = tvb_get_uint8(tvb, 7);

        ti = proto_tree_add_uint(lwm_cmd_tree, hf_lwm_cmd_revlinkquality, tvb, 7, 1, lwm_revlinkqual);

        if(lwm_revlinkqual == 255){

            proto_item_append_text(ti, " %s", LWM_CMD_LINKQ_STRING);

        }

    }

    return LWM_CMD_FRAME_ROUTE_REPLY_LEN;

} /* dissect_lwm_cmd_frame_route_reply*/

/*FUNCTION:------------------------------------------------------

 *  NAME

 *      proto_register_lwm

 *  DESCRIPTION

 *      IEEE 802.15.4 protocol registration routine.

 *  PARAMETERS

 *      none

 *  RETURNS

 *      void

 *---------------------------------------------------------------

 */

void proto_register_lwm(void)

{

    static hf_register_info hf[] = {

        /*Frame control field*/

        { &hf_lwm_fcf,

        { "Frame control field", "lwm.fcf", FT_UINT8, BASE_HEX, NULL, 0x0,

        "Control information for the frame.", HFILL }},

        { &hf_lwm_fcf_ack_req,

        { "Acknowledgment Request", "lwm.ack_req", FT_BOOLEAN, 8, NULL, LWM_FCF_ACK_REQUEST,

        "Specifies whether an acknowledgment is required from the destination node.", HFILL }},

        { &hf_lwm_fcf_security,

        { "Security Enabled", "lwm.security", FT_BOOLEAN, 8, NULL, LWM_FCF_SEC_EN,

        "Specifies whether the frame payload is encrypted.", HFILL }},

        { &hf_lwm_fcf_linklocal,

        { "Link Local", "lwm.linklocal", FT_BOOLEAN, 8, NULL, LWM_FCF_LINK_LOCAL,

        "It may be set to one to prevent neighboring nodes from rebroadcasting a frame.", HFILL }},

        { &hf_lwm_fcf_multicast,

        { "Multicast", "lwm.multicast", FT_BOOLEAN, 8, NULL, LWM_FCF_MULTICAST,

        "If the Multicast subfield is set to one, Multicast Header should be present and the Destination Address is a group address.", HFILL }},

        { &hf_lwm_fcf_reserved,

        { "Reserved bits", "lwm.fcf.reserved", FT_UINT8, BASE_HEX, NULL, LWM_FCF_RESERVED,

        "The 4 bits are reserved.", HFILL }},

        /*Other fields*/

        { &hf_lwm_seq,

        { "Sequence Number", "lwm.seq", FT_UINT8, BASE_DEC, NULL, 0x0,

        "Specifies the sequence identifier for the frame.", HFILL }},

        { &hf_lwm_src_addr,

        { "Network Source Address", "lwm.src_addr", FT_UINT16, BASE_HEX, NULL, 0x0,

        "Specifies the network address of the node originating the frame.", HFILL }},

        { &hf_lwm_dst_addr,

        { "Network Destination Address", "lwm.dst_addr", FT_UINT16, BASE_HEX, NULL, 0x0,

        "Specifies the network address of the destination node or group address for multicast messages.", HFILL }},

        { &hf_lwm_src_endp,

        { "Source Endpoint", "lwm.src_endp", FT_UINT8, BASE_DEC, NULL, 0x0,

        "Specifies the source endpoint identifier.", HFILL }},

        { &hf_lwm_dst_endp,

        { "Destination Endpoint", "lwm.dst_endp", FT_UINT8, BASE_DEC, NULL, 0x0,

        "Specifies the destination endpoint identifier.", HFILL }},

        /*Multicast header*/

        { &hf_lwm_multi_nmrad,

        { "Non-member Radius", "lwm.multi_nmrad", FT_UINT16, BASE_DEC, NULL, 0x0,

        "Specifies remaining radius (number of hops) for Non-members of multicast group.", HFILL }},

        { &hf_lwm_multi_mnmrad,

        { "Maximum Non-member Radius", "lwm.multi_mnmrad", FT_UINT16, BASE_DEC, NULL, 0x0,

        "Specifies maximum radius (number of hops) for Non-members of multicast group.", HFILL }},

        { &hf_lwm_multi_mrad,

        { "Member Radius", "lwm.multi_mrad", FT_UINT16, BASE_DEC, NULL, 0x0,

        "Specifies remaining radius (number of hops) for Members of multicast group.", HFILL }},

        { &hf_lwm_multi_mmrad,

        { "Maximum Member Radius", "lwm.multi_mmrad", FT_UINT16, BASE_DEC, NULL, 0x0,

        "Specifies maximum radius (number of hops) for Members of multicast group.", HFILL }},

        /*MIC, security*/

        { &hf_lwm_mic,

        { "Message Integrity Code", "lwm.mic", FT_UINT32, BASE_HEX, NULL, 0x0,

        "Specifies Message Integrity Code (MIC).", HFILL }},

        /*----------------------------------*/

        /*                                    */

        /*  Command Frames Specific Fields  */

        /*                                    */

        /*----------------------------------*/

        { &hf_lwm_cmd,

        { "Command ID", "lwm.cmd", FT_UINT8, BASE_HEX, VALS(lwm_cmd_names), 0x0,

        "It contains Command ID value.", HFILL }},

        /*  Command Frame - Ack */

        { &hf_lwm_cmd_seq,

        { "Sequence number", "lwm.cmd.seq", FT_UINT8, BASE_DEC, NULL, 0x0,

        "It contains a network sequence number of a frame that is being acknowledged.", HFILL }},

        { &hf_lwm_cmd_cm,

        { "Control Message", "lwm.cmd.cm", FT_UINT8, BASE_HEX, NULL, 0x0,

        "It contains an arbitrary value that can be set on the sending side.", HFILL }},

        /* Part of  Command Frames - Route Request, Route Reply*/

        { &hf_lwm_cmd_route_src,

        { "Source address", "lwm.cmd.route_src", FT_UINT16, BASE_HEX, NULL, 0x0,

        "It contains a source network address from the frame that cannot be routed", HFILL }},

        { &hf_lwm_cmd_route_dst,

        { "Destination Address", "lwm.cmd.route_dst", FT_UINT16, BASE_HEX, NULL, 0x0,

        "It contains a destination network address from the frame that cannot be routed", HFILL }},

        { &hf_lwm_cmd_route_multi,

          { "Multicast", "lwm.cmd.multi", FT_UINT8, BASE_HEX, VALS(lwm_cmd_multi_names), 0x0,

        "If it set to 0, Destination Address field contains a network address. If it set to 1, Destination Address field contains a group ID.", HFILL }},

        /*  Part of Command Frame - Route Request */

        { &hf_lwm_cmd_linkquality,

        { "Link Quality", "lwm.cmd.linkq", FT_UINT8, BASE_DEC, NULL, 0x0,

        "It contains a link quality value of the potential route accumulated over all hops towards the destination.", HFILL }},

        /*  Part of Command Frame - Route Reply */

        { &hf_lwm_cmd_forwlinkquality,

        { "Forward Link Quality", "lwm.cmd.flinkq", FT_UINT8, BASE_DEC, NULL, 0x0,

        "It contains a value of the Link Quality field from the corresponding Route Request Command Frame.", HFILL }},

        { &hf_lwm_cmd_revlinkquality,

        { "Reverse Link Quality", "lwm.cmd.rlinkq", FT_UINT8, BASE_DEC, NULL, 0x0,

        "It contains a link quality value of the discovered route accumulated over all hops towards the originator.", HFILL }},

    };

    /* Subtrees */

    static int *ett[] = {

        &ett_lwm,

        &ett_lwm_fcf,

        &ett_lwm_multi_tree,

        &ett_lwm_cmd_tree

    };

    static ei_register_info ei[] = {

        { &ei_lwm_mal_error,     { "lwm.malformed_error",   PI_MALFORMED,      PI_ERROR, "Malformed Packet", EXPFILL }},

        { &ei_lwm_n_src_broad,   { "lwm.not_src_broadcast", PI_COMMENTS_GROUP, PI_NOTE,  "Source address can not be broadcast address !", EXPFILL }},

        { &ei_lwm_mismatch_endp, { "lwm.mismatch_endp",     PI_COMMENTS_GROUP, PI_WARN,  "Stack command Endpoints mismatch (should be 0, both)!", EXPFILL }},

        { &ei_lwm_empty_payload, { "lwm.empty_payload",     PI_COMMENTS_GROUP, PI_WARN,  "Empty LwMesh Payload!", EXPFILL }},

        { &ei_lwm_no_decryption_key, { "lwm.no_decryption_key", PI_PROTOCOL,   PI_NOTE,  "No encryption key set - can't decrypt", EXPFILL }},

        { &ei_lwm_decryption_failed, { "lwm.decryption_failed", PI_PROTOCOL,   PI_WARN,  "Decryption Failed", EXPFILL }},

    };

    module_t *lw_module;

    expert_module_t* expert_lwm;

    /*  Register protocol name and description. */

    proto_lwm = proto_register_protocol("Lightweight Mesh (v1.1.1)", "LwMesh", "lwm");

    /*  Register header fields and subtrees. */

    proto_register_field_array(proto_lwm, hf, array_length(hf));

    proto_register_subtree_array(ett, array_length(ett));

    expert_lwm = expert_register_protocol(proto_lwm);

    expert_register_field_array(expert_lwm, ei, array_length(ei));

    lw_module = prefs_register_protocol(proto_lwm,proto_reg_handoff_lwm);

    /* Register preferences for a decryption key */

    /* TODO: Implement a UAT for multiple keys, and with more advanced key management. */

    prefs_register_string_preference(lw_module, "lwmes_key", "Lw Decryption key",

            "128-bit decryption key in hexadecimal format", (const char **)&lwmes_key_str);

    /*  Register dissector with Wireshark. */

    lwm_handle = register_dissector("lwm", dissect_lwm, proto_lwm);

} /* proto_register_lwm */

/*FUNCTION:------------------------------------------------------

 *  NAME

 *      proto_reg_handoff_lwm

 *  DESCRIPTION

 *      Registers the lwm dissector with Wireshark.

 *      Will be called during Wireshark startup, and whenever

 *      preferences are changed.

 *  PARAMETERS

 *      none

 *  RETURNS

 *      void

 *---------------------------------------------------------------

 */

void proto_reg_handoff_lwm(void)

{

    static bool initialized = false;

    GByteArray      *bytes;

    bool             res;

    if (!initialized) {

        /* Register our dissector with IEEE 802.15.4 */

        dissector_add_for_decode_as(IEEE802154_PROTOABBREV_WPAN_PANID, lwm_handle);

        heur_dissector_add(IEEE802154_PROTOABBREV_WPAN, dissect_lwm_heur, "Lightweight Mesh over IEEE 802.15.4", "lwm_wlan", proto_lwm, HEURISTIC_ENABLE);

        initialized = true;

    }

    /* Convert key to raw bytes */

    bytes = g_byte_array_new();

    res = hex_str_to_bytes(lwmes_key_str, bytes, false);

    lwmes_key_valid = (res && bytes->len >= IEEE802154_CIPHER_SIZE);

    if (lwmes_key_valid) {

        memcpy(lwmes_key, bytes->data, IEEE802154_CIPHER_SIZE);

    }

    g_byte_array_free(bytes, true);

} /* proto_reg_handoff_lwm */

/*

 * Editor modelines

 *

 * Local Variables:

 * c-basic-offset: 4

 * tab-width: 8

 * indent-tabs-mode: nil

 * End:

 *

 * ex: set shiftwidth=4 tabstop=8 expandtab:

 * :indentSize=4:tabSize=8:noTabs=true:

 */