epan/dissectors/packet-lwm.c

   1 /* packet-lwm.c
   2  * Dissector  routines for the ATMEL Lightweight Mesh 1.1.1
   3  * Copyright 2013 Martin Leixner <info@sewio.net>
   4  *
   5  * Wireshark - Network traffic analyzer
   6  * By Gerald Combs <gerald@wireshark.org>
   7  * Copyright 1998 Gerald Combs
   8  *
   9  * SPDX-License-Identifier: GPL-2.0-or-later
  10  *------------------------------------------------------------
  11 */
  12
  13 #include "config.h"
  14
  15 #include <epan/packet.h>
  16 #include <epan/expert.h>
  17
  18 #include <wsutil/filesystem.h>
  19 #include "packet-ieee802154.h"
  20 #include <epan/prefs.h>
  21 #include <epan/strutil.h>
  22 #include <wsutil/wsgcrypt.h>
  23
  24 /*LwMesh lengths*/
  25 #define LWM_HEADER_BASE_LEN            7
  26 #define LWM_MIC_LEN                    4
  27 #define LWM_MULTI_HEADER_LEN           2
  28
  29 /*  Bit-masks for the FCF */
  30 #define LWM_FCF_ACK_REQUEST            0x01
  31 #define LWM_FCF_SEC_EN                 0x02
  32
  33 #define LWM_FCF_LINK_LOCAL             0x04
  34 #define LWM_FCF_MULTICAST              0x08
  35
  36 #define LWM_FCF_RESERVED               0xF0
  37
  38 #define LWM_MULTI_NON_MEM_RAD_MASK          0x000F
  39 #define LWM_MULTI_NON_MEM_RAD_OFFSET        0
  40
  41 #define LWM_MULTI_MAX_NON_MEM_RAD_MASK      0x00F0
  42 #define LWM_MULTI_MAX_NON_MEM_RAD_OFFSET    4
  43
  44 #define LWM_MULTI_MEM_RAD_MASK              0x0F00
  45 #define LWM_MULTI_MEM_RAD_OFFSET            8
  46
  47 #define LWM_MULTI_MAX_MEM_RAD_MASK          0xF000
  48 #define LWM_MULTI_MAX_MEM_RAD_OFFSET        12
  49
  50 /*Endpoints*/
  51 #define LWM_SRC_ENDP_MASK               0xF0
  52 #define LWM_SRC_ENDP_OFFSET             4
  53 #define LWM_DST_ENDP_MASK               0x0F
  54 #define LWM_DST_ENDP_OFFSET             0
  55
  56 /*Defined addresses*/
  57 #define LWM_BCAST_ADDR                    0xFFFF
  58
  59 /*Command IDs*/
  60 #define LWM_CMD_ACK                      0x00
  61 #define LWM_CMD_ROUTE_ERR                0x01
  62 #define LWM_CMD_ROUTE_REQ                0x02
  63 #define LWM_CMD_ROUTE_REPLY              0x03
  64
  65 /*Lengths of command frames*/
  66 #define LWM_CMD_FRAME_ACK_LEN              3
  67 #define LWM_CMD_FRAME_ROUTE_ERR_LEN        6
  68 #define LWM_CMD_FRAME_ROUTE_REQ_LEN        7
  69 #define LWM_CMD_FRAME_ROUTE_REPLY_LEN      8
  70
  71 /*Values for multicast field*/
  72 #define LWM_CMD_MULTI_ADDR_FALSE           0
  73 #define LWM_CMD_MULTI_ADDR_TRUE            1
  74
  75 /*Defined strings*/
  76 #define LWM_CMD_LINKQ_STRING            "(Sent by Originate node)"
  77 #define LWM_CMD_UNKNOWN_VAL_STRING      "Unknown command (0x%02x)"
  78
  79 #define LWM_MULTI_UNICAST_STRING        "(Unicast)"
  80 #define LWM_MULTI_GROUP_STRING          "(Group ID)"
  81
  82 /*  Function declarations */
  83 void proto_register_lwm(void);
  84 void proto_reg_handoff_lwm(void);
  85
  86 /* User string with the decryption key. */
  87 static const char *lwmes_key_str;
  88 static bool         lwmes_key_valid;
  89 static uint8_t      lwmes_key[16];
  90
  91 /* Dissection Routines. */
  92 static int  dissect_lwm                       (tvbuff_t *, packet_info *, proto_tree *, void *data);
  93 static int  dissect_lwm_cmd_frame_ack         (tvbuff_t *, packet_info *, proto_tree *);
  94 static int  dissect_lwm_cmd_frame_route_err   (tvbuff_t *, packet_info *, proto_tree *);
  95 static int  dissect_lwm_cmd_frame_route_req   (tvbuff_t *, packet_info *, proto_tree *);
  96 static int  dissect_lwm_cmd_frame_route_reply (tvbuff_t *, packet_info *, proto_tree *);
  97
  98 /*  Initialize protocol and registered fields. */
  99 static int proto_lwm;
 100
 101 static int hf_lwm_fcf;
 102 static int hf_lwm_fcf_ack_req;
 103 static int hf_lwm_fcf_security;
 104 static int hf_lwm_fcf_linklocal;
 105 static int hf_lwm_fcf_multicast;
 106 static int hf_lwm_fcf_reserved;
 107 static int hf_lwm_seq;
 108 static int hf_lwm_src_addr;
 109 static int hf_lwm_dst_addr;
 110 static int hf_lwm_src_endp;
 111 static int hf_lwm_dst_endp;
 112 static int hf_lwm_multi_nmrad;
 113 static int hf_lwm_multi_mnmrad;
 114 static int hf_lwm_multi_mrad;
 115 static int hf_lwm_multi_mmrad;
 116 static int hf_lwm_mic;
 117 static int hf_lwm_cmd;
 118 static int hf_lwm_cmd_seq;
 119 static int hf_lwm_cmd_cm;
 120 static int hf_lwm_cmd_route_src;
 121 static int hf_lwm_cmd_route_dst;
 122 static int hf_lwm_cmd_route_multi;
 123 static int hf_lwm_cmd_linkquality;
 124 static int hf_lwm_cmd_forwlinkquality;
 125 static int hf_lwm_cmd_revlinkquality;
 126
 127 /* Initialize protocol subtrees. */
 128 static int ett_lwm;
 129 static int ett_lwm_fcf;
 130 static int ett_lwm_cmd_tree;
 131 static int ett_lwm_multi_tree;
 132
 133 static expert_field ei_lwm_mal_error;
 134 static expert_field ei_lwm_n_src_broad;
 135 static expert_field ei_lwm_mismatch_endp;
 136 static expert_field ei_lwm_empty_payload;
 137 static expert_field ei_lwm_no_decryption_key;
 138 static expert_field ei_lwm_decryption_failed;
 139
 140 static dissector_handle_t lwm_handle;
 141
 142 static const value_string lwm_cmd_names[] = {
 143     { LWM_CMD_ACK,          "LwMesh ACK" },
 144     { LWM_CMD_ROUTE_ERR,    "Route Error" },
 145     { LWM_CMD_ROUTE_REQ,    "Route Request" },
 146     { LWM_CMD_ROUTE_REPLY,  "Route Reply" },
 147     { 0, NULL }
 148 };
 149
 150 static const value_string lwm_cmd_multi_names[] = {
 151     { LWM_CMD_MULTI_ADDR_FALSE, "FALSE" },
 152     { LWM_CMD_MULTI_ADDR_TRUE,  "TRUE" },
 153     { 0, NULL }
 154 };
 155
 156 /*FUNCTION:------------------------------------------------------
 157  *  NAME
 158  *      dissect_lwm_heur
 159  *  DESCRIPTION
 160  *      Heuristic interpreter for the Lightweight Mesh.
 161  *  PARAMETERS
 162  *      tvbuff_t *tvb       - pointer to buffer containing raw packet.
 163  *      packet_into *pinfo  - pointer to packet information fields
 164  *      proto_tree *tree    - pointer to data tree Wireshark uses to display packet.
 165  *  RETURNS
 166  *      Boolean value, whether it handles the packet or not.
 167  *---------------------------------------------------------------
 168  */
 169 static bool
 170 dissect_lwm_heur(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void *data)
 171 {
 172     uint8_t endpt, srcep, dstep;
 173
 174     /* 1) first byte must have bits 0000xxxx */
 175     if(tvb_get_uint8(tvb, 0) & LWM_FCF_RESERVED)
 176         return false;
 177
 178     /* The header should be at least long enough for the base header. */
 179     if (tvb_reported_length(tvb) < LWM_HEADER_BASE_LEN)
 180         return false;
 181
 182     /* The endpoints should either both be zero, or both non-zero. */
 183     endpt = tvb_get_uint8(tvb, 6);
 184     srcep = (endpt & LWM_SRC_ENDP_MASK) >> LWM_SRC_ENDP_OFFSET;
 185     dstep = (endpt & LWM_DST_ENDP_MASK) >> LWM_DST_ENDP_OFFSET;
 186     if ((srcep == 0) && (dstep != 0))
 187         return false;
 188     if ((srcep != 0) && (dstep == 0))
 189         return false;
 190
 191     dissect_lwm(tvb, pinfo, tree, data);
 192     return true;
 193 } /* dissect_lwm_heur */
 194
 195 /*FUNCTION:------------------------------------------------------
 196  *  NAME
 197  *      dissect_lwm
 198  *  DESCRIPTION
 199  *      Lightweight Mesh packet dissection routine for Wireshark.
 200  *  PARAMETERS
 201  *      tvbuff_t *tvb       - pointer to buffer containing raw packet.
 202  *      packet_info *pinfo  - pointer to packet information fields
 203  *      proto_tree *tree    - pointer to data tree Wireshark uses to display packet.
 204  *  RETURNS
 205  *      int                 - length of data processed, or 0 if not LWM.
 206  *---------------------------------------------------------------
 207  */
 208 static int dissect_lwm(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void *data _U_)
 209 {
 210     unsigned    lwm_header_len;
 211
 212     uint8_t     lwm_fcf;
 213     bool        lwm_fcf_security;
 214     bool        lwm_fcf_multicast;
 215
 216
 217     uint8_t     lwm_seq;
 218     uint16_t    lwm_src_addr;
 219     uint16_t    lwm_dst_addr;
 220     uint8_t     lwm_endp_field;
 221     uint8_t     lwm_src_endp;
 222     uint8_t     lwm_dst_endp;
 223
 224     proto_tree *lwm_tree        = NULL;
 225     proto_item *ti_proto        = NULL;
 226     proto_item *ti;
 227     tvbuff_t   *new_tvb;
 228
 229     /*---------------------------------------------------------*/
 230
 231     /*Enter name of protocol to info field*/
 232     col_set_str(pinfo->cinfo, COL_PROTOCOL, "LwMesh");
 233     col_clear(pinfo->cinfo, COL_INFO);
 234
 235     /*Set base length of LWM header*/
 236     lwm_header_len = LWM_HEADER_BASE_LEN;
 237
 238     /*--------------------------------------------------*/
 239     /*                                                  */
 240     /*        Create LwMesh dissector tree              */
 241     /*                                                  */
 242     /*--------------------------------------------------*/
 243     if(tree){
 244         /*Create subtree for the LwMesh*/
 245         ti_proto = proto_tree_add_protocol_format(tree, proto_lwm, tvb, 0, -1, "Lightweight Mesh");
 246         lwm_tree = proto_item_add_subtree(ti_proto, ett_lwm);
 247     }
 248
 249     col_set_str(pinfo->cinfo, COL_INFO, "Lightweight Mesh");
 250
 251     /*--------------------------------------------------*/
 252     /*                                                  */
 253     /*        Display LwMesh dissector tree             */
 254     /*                                                  */
 255     /*--------------------------------------------------*/
 256
 257     /*Frame control fields*/
 258     lwm_fcf = tvb_get_uint8(tvb, 0);
 259
 260     lwm_fcf_security  = (lwm_fcf & LWM_FCF_SEC_EN);
 261     lwm_fcf_multicast = (lwm_fcf & LWM_FCF_MULTICAST);
 262
 263     if(tree){
 264         proto_tree *field_tree;
 265         ti = proto_tree_add_uint(lwm_tree, hf_lwm_fcf, tvb, 0, 1, lwm_fcf);
 266
 267         field_tree = proto_item_add_subtree(ti, ett_lwm_fcf);
 268         proto_tree_add_item(field_tree, hf_lwm_fcf_ack_req,   tvb, 0, 1, ENC_NA);
 269
 270         proto_tree_add_item(field_tree, hf_lwm_fcf_security,  tvb, 0, 1, ENC_NA);
 271         proto_tree_add_item(field_tree, hf_lwm_fcf_linklocal, tvb, 0, 1, ENC_NA);
 272         proto_tree_add_item(field_tree, hf_lwm_fcf_multicast, tvb, 0, 1, ENC_NA);
 273         proto_tree_add_item(field_tree, hf_lwm_fcf_reserved,  tvb, 0, 1, ENC_NA);
 274     }
 275
 276     /*Sequence number*/
 277     lwm_seq = tvb_get_uint8(tvb, 1);
 278     proto_item_append_text(ti_proto, ", Sequence Number: %i", lwm_seq);
 279     proto_tree_add_uint(lwm_tree, hf_lwm_seq, tvb, 1, 1, lwm_seq);
 280
 281     /*Network addresses*/
 282
 283     /*Parse Source address*/
 284     lwm_src_addr   = tvb_get_letohs(tvb, 2);
 285
 286     ti = proto_tree_add_uint(lwm_tree, hf_lwm_src_addr, tvb, 2, 2, lwm_src_addr);
 287
 288     if(lwm_src_addr < 0x8000){
 289         proto_item_append_text(ti, " (Routing node)");
 290     }else{
 291         proto_item_append_text(ti, " (Non-routing node)");
 292     }
 293
 294     /*Check value of source address*/
 295     if(lwm_src_addr == LWM_BCAST_ADDR){
 296         expert_add_info(pinfo, lwm_tree, &ei_lwm_n_src_broad);
 297     }
 298
 299     /*Parse Destination address*/
 300     lwm_dst_addr   = tvb_get_letohs(tvb, 4);
 301
 302     if(lwm_dst_addr == LWM_BCAST_ADDR){
 303         proto_tree_add_uint_format_value(lwm_tree, hf_lwm_dst_addr, tvb, 4, 2, lwm_dst_addr,
 304                                          "Broadcast (0x%04x)", lwm_dst_addr);
 305     }else{
 306         ti = proto_tree_add_uint(lwm_tree, hf_lwm_dst_addr, tvb, 4, 2, lwm_dst_addr);
 307
 308         if(lwm_fcf_multicast){
 309             proto_item_append_text(ti, " %s", LWM_MULTI_GROUP_STRING);
 310         }else{
 311             proto_item_append_text(ti, " %s", LWM_MULTI_UNICAST_STRING);
 312
 313             if(lwm_dst_addr < 0x8000){
 314                 proto_item_append_text(ti, " (Routing node)");
 315             }else{
 316                 proto_item_append_text(ti, " (Non-routing node)");
 317             }
 318         }
 319     }
 320
 321     /*Enter description to info field*/
 322     col_append_fstr(pinfo->cinfo, COL_INFO, ", Nwk_Dst: 0x%04x, Nwk_Src: 0x%04x", lwm_dst_addr, lwm_src_addr);
 323
 324     /*Endpoints*/
 325     lwm_endp_field = tvb_get_uint8(tvb, 6);
 326     lwm_src_endp   = (lwm_endp_field & LWM_SRC_ENDP_MASK) >> LWM_SRC_ENDP_OFFSET;
 327     lwm_dst_endp   = (lwm_endp_field & LWM_DST_ENDP_MASK) >> LWM_DST_ENDP_OFFSET;
 328
 329     ti = proto_tree_add_uint(lwm_tree, hf_lwm_src_endp, tvb, 6, 1, lwm_src_endp);
 330     if(lwm_src_endp == 0){
 331         proto_item_append_text(ti, " (Stack command endpoint)");
 332     }
 333
 334     ti = proto_tree_add_uint(lwm_tree, hf_lwm_dst_endp, tvb, 6, 1, lwm_dst_endp);
 335     if(lwm_dst_endp == 0){
 336         proto_item_append_text(ti, " (Stack command endpoint)");
 337     }
 338
 339     if( (lwm_src_endp == 0) && (lwm_dst_endp == 0)){
 340         /*stack command endpoints*/
 341
 342     }
 343     else if( (lwm_src_endp == 0) || (lwm_dst_endp == 0)){
 344         /*If only one endpoint is 0, alert about that*/
 345
 346         col_append_str(pinfo->cinfo, COL_INFO, "[Stack command Endpoints mismatch]");
 347
 348         expert_add_info(pinfo, lwm_tree, &ei_lwm_mismatch_endp);
 349     }
 350
 351     /*Multicast header*/
 352     if( (lwm_fcf_multicast) ){
 353
 354         lwm_header_len  += LWM_MULTI_HEADER_LEN;
 355
 356         if(tree){
 357             proto_tree *multi_tree;
 358             uint16_t    lwm_multi_header;
 359
 360             lwm_multi_header =  tvb_get_letohs(tvb, 7);
 361             multi_tree = proto_tree_add_subtree(lwm_tree, tvb, 7, 2, ett_lwm_multi_tree, NULL, "Multicast Header");
 362
 363             proto_tree_add_uint(multi_tree, hf_lwm_multi_nmrad, tvb, 7, 2,
 364                                 (lwm_multi_header & LWM_MULTI_NON_MEM_RAD_MASK) >> LWM_MULTI_NON_MEM_RAD_OFFSET);
 365             proto_tree_add_uint(multi_tree, hf_lwm_multi_mnmrad, tvb, 7, 2,
 366                                 (lwm_multi_header & LWM_MULTI_MAX_NON_MEM_RAD_MASK) >> LWM_MULTI_MAX_NON_MEM_RAD_OFFSET);
 367             proto_tree_add_uint(multi_tree, hf_lwm_multi_mrad, tvb, 7, 2,
 368                                 (lwm_multi_header & LWM_MULTI_MEM_RAD_MASK) >> LWM_MULTI_MEM_RAD_OFFSET);
 369             proto_tree_add_uint(multi_tree, hf_lwm_multi_mmrad, tvb, 7, 2,
 370                                 (lwm_multi_header & LWM_MULTI_MAX_MEM_RAD_MASK) >> LWM_MULTI_MAX_MEM_RAD_OFFSET);
 371         }
 372     }
 373
 374
 375     /*------------------------------*/
 376     /*                              */
 377     /*       Dissect payload        */
 378     /*                              */
 379     /*------------------------------*/
 380
 381     /*Note: exception will already have occurred if "short header"*/
 382
 383     if (tvb_reported_length(tvb) <= lwm_header_len) {
 384         /*Empty payload*/
 385         expert_add_info(pinfo, lwm_tree, &ei_lwm_empty_payload);
 386         col_append_str(pinfo->cinfo, COL_INFO, "[Empty LwMesh Payload]");
 387
 388         return tvb_captured_length(tvb);
 389     }
 390
 391     new_tvb = tvb_new_subset_remaining(tvb, lwm_header_len);
 392
 393     /*Encrypted data*/
 394     if(lwm_fcf_security){
 395         unsigned rlen;
 396         int   start;
 397         uint32_t lwm_mic;
 398
 399         /*MIC field*/
 400         rlen = tvb_reported_length(new_tvb);
 401         start = (rlen >= LWM_MIC_LEN) ? (rlen-LWM_MIC_LEN) : 0;
 402         /*An exception will occur if there are not enough bytes for the MIC */
 403         proto_tree_add_item_ret_uint(lwm_tree, hf_lwm_mic, new_tvb, start, LWM_MIC_LEN, ENC_LITTLE_ENDIAN, &lwm_mic);
 404
 405         if(lwmes_key_valid)
 406         {
 407             ieee802154_packet *ieee_packet = NULL;
 408             int payload_length = 0;
 409             int length = 0;
 410             int payload_offset = 0;
 411             uint8_t block;
 412             tvbuff_t *decrypted_tvb;
 413             gcry_cipher_hd_t cypher_hd;
 414             uint8_t* vector = NULL;
 415             uint8_t* text =NULL;
 416             uint8_t* text_dec =NULL;
 417             uint8_t i;
 418             uint32_t vmic;
 419             uint32_t nwkSecurityVector[4];
 420             int gcrypt_err;
 421
 422             ieee_packet = (ieee802154_packet *)data;
 423
 424             memset(&nwkSecurityVector, 0, sizeof(nwkSecurityVector));
 425             nwkSecurityVector[0] = lwm_seq;
 426             nwkSecurityVector[1] = ((uint32_t)lwm_dst_addr<< 16) | lwm_dst_endp;
 427             nwkSecurityVector[2]= ((uint32_t) lwm_src_addr<< 16) | lwm_src_endp;
 428             nwkSecurityVector[3] = ((uint32_t)ieee_packet->dst_pan << 16) | (uint8_t)lwm_fcf;
 429
 430             payload_length=tvb_reported_length(new_tvb) - LWM_MIC_LEN;
 431
 432             /* ECB - Nwk security vector*/
 433             text = (uint8_t *)tvb_memdup(pinfo->pool, new_tvb, 0, payload_length);
 434             payload_offset=0;
 435
 436             gcrypt_err = gcry_cipher_open(&cypher_hd, GCRY_CIPHER_AES128, GCRY_CIPHER_MODE_ECB, 0);
 437             /*Decrypt the actual data */
 438             while(payload_length>0)
 439             {
 440                 if(gcrypt_err == 0) {
 441                     gcrypt_err = gcry_cipher_setkey(cypher_hd,(uint8_t *)lwmes_key, 16);
 442                 }
 443                 if(gcrypt_err == 0) {
 444                     gcrypt_err = gcry_cipher_encrypt(cypher_hd,(uint8_t *)nwkSecurityVector,16,(uint8_t *)nwkSecurityVector,16);
 445                 }
 446
 447                 if(gcrypt_err)
 448                 {
 449                     col_add_fstr(pinfo->cinfo, COL_INFO,
 450                          "Encrypted data (%i byte(s)) DECRYPT FAILED",
 451                          tvb_reported_length(new_tvb) - LWM_MIC_LEN);
 452                     expert_add_info(pinfo, lwm_tree, &ei_lwm_decryption_failed);
 453                     tvb_set_reported_length(new_tvb, tvb_reported_length(new_tvb) - LWM_MIC_LEN);
 454                     call_data_dissector(new_tvb, pinfo, lwm_tree);
 455                 }
 456
 457                 text_dec = &text[payload_offset];
 458                 vector = (uint8_t *)nwkSecurityVector;
 459                 block =  (payload_length < 16) ? payload_length : 16;
 460
 461                 for (i = 0; i < block; i++)
 462                 {
 463                     text_dec[i] ^= vector[i];
 464                     /*
 465                      * GCC 12.2.0 gives a false positive Wstringop-overflow warning.
 466                      * https://gitlab.com/wireshark/wireshark/-/issues/18383
 467                      */
 468                     DIAG_OFF_STRINGOP_OVERFLOW()
 469                     vector[i] ^= text_dec[i];
 470                     DIAG_ON_STRINGOP_OVERFLOW()
 471                 }
 472
 473                 payload_offset += block;
 474                 payload_length -= block;
 475                 gcry_cipher_reset(cypher_hd);
 476             }
 477             gcry_cipher_close(cypher_hd);
 478
 479             vmic = nwkSecurityVector[0] ^ nwkSecurityVector[1] ^ nwkSecurityVector[2] ^ nwkSecurityVector[3];
 480             length = tvb_reported_length(new_tvb) - LWM_MIC_LEN;
 481
 482             if(vmic == lwm_mic)
 483             {
 484                 decrypted_tvb = tvb_new_real_data(text,length, length);
 485                 call_data_dissector(decrypted_tvb, pinfo, lwm_tree);
 486                 /* XXX - needed?
 487                    add_new_data_source(pinfo, decrypted_tvb, "Decrypted LWmesh Payload"); */
 488                 col_append_str(pinfo->cinfo, COL_INFO, ",  MIC SUCCESS");
 489
 490             }
 491             else
 492             {
 493                 col_add_fstr(pinfo->cinfo, COL_INFO,
 494                      "Encrypted data (%i byte(s)) MIC FAILURE",
 495                      tvb_reported_length(new_tvb) - LWM_MIC_LEN);
 496                 tvb_set_reported_length(new_tvb, tvb_reported_length(new_tvb) - LWM_MIC_LEN);
 497                 call_data_dissector(new_tvb, pinfo, lwm_tree);
 498             }
 499         }
 500         else
 501         {
 502             col_add_fstr(pinfo->cinfo, COL_INFO,
 503                      "Encrypted data (%i byte(s)) NO DECRYPT KEY",
 504                       tvb_reported_length(new_tvb) - LWM_MIC_LEN);
 505
 506             expert_add_info(pinfo, lwm_tree, &ei_lwm_no_decryption_key);
 507             tvb_set_reported_length(new_tvb, tvb_reported_length(new_tvb) - LWM_MIC_LEN);
 508             call_data_dissector(new_tvb, pinfo, lwm_tree);
 509         }
 510     }
 511     /*stack command endpoint 0 and not secured*/
 512     else if( (lwm_src_endp == 0) && (lwm_dst_endp == 0) ){
 513         proto_tree *lwm_cmd_tree;
 514         uint8_t     lwm_cmd;
 515         unsigned    len;
 516
 517         /*----------------------------------------------------------------------*/
 518         /*                                                                      */
 519         /*  Call command dissector (depends on value of first byte of payload)  */
 520         /*                                                                      */
 521         /*----------------------------------------------------------------------*/
 522         lwm_cmd = tvb_get_uint8(new_tvb, 0);
 523
 524         col_clear(pinfo->cinfo, COL_INFO);  /*XXX: why ?*/
 525         col_add_str(pinfo->cinfo, COL_INFO,
 526             val_to_str(lwm_cmd, lwm_cmd_names, LWM_CMD_UNKNOWN_VAL_STRING));
 527
 528         lwm_cmd_tree = proto_tree_add_subtree(lwm_tree, new_tvb, 0, -1, ett_lwm_cmd_tree, &ti,
 529             val_to_str(lwm_cmd, lwm_cmd_names, LWM_CMD_UNKNOWN_VAL_STRING));
 530
 531         proto_tree_add_uint(lwm_cmd_tree, hf_lwm_cmd, new_tvb, 0, 1, lwm_cmd);
 532
 533         switch (lwm_cmd) {
 534
 535         case LWM_CMD_ACK:
 536             len = dissect_lwm_cmd_frame_ack(new_tvb, pinfo, lwm_cmd_tree);
 537             break;
 538
 539         case LWM_CMD_ROUTE_ERR:
 540             len = dissect_lwm_cmd_frame_route_err(new_tvb, pinfo, lwm_cmd_tree);
 541             break;
 542
 543         case LWM_CMD_ROUTE_REQ:
 544             len = dissect_lwm_cmd_frame_route_req(new_tvb, pinfo, lwm_cmd_tree);
 545             break;
 546
 547         case LWM_CMD_ROUTE_REPLY:
 548             len = dissect_lwm_cmd_frame_route_reply(new_tvb, pinfo, lwm_cmd_tree);
 549             break;
 550
 551         default:
 552             /*Unknown command*/
 553             expert_add_info_format(pinfo, lwm_cmd_tree, &ei_lwm_mal_error, "Unknown command");
 554             call_data_dissector(new_tvb, pinfo, lwm_cmd_tree);
 555             return tvb_captured_length(tvb);
 556         }
 557
 558         proto_item_set_len(ti, len);
 559
 560         /*Here only if additional data after valid 'cmd' data*/
 561         /*Note: exception will have already occurred if tvb was missing required bytes for 'cmd'*/
 562         /*      Report error if additional undissected data*/
 563         if (len < tvb_reported_length(new_tvb)) {
 564             /*unknown additional data*/
 565             expert_add_info_format(pinfo, lwm_cmd_tree, &ei_lwm_mal_error,
 566                 "Size is %i byte(s), instead of %i bytes", tvb_reported_length(new_tvb), len);
 567
 568             new_tvb = tvb_new_subset_remaining(new_tvb, len);
 569             call_data_dissector(new_tvb, pinfo, lwm_tree);
 570         }
 571     }
 572     else{
 573         /*unknown data*/
 574         call_data_dissector(new_tvb, pinfo, lwm_tree);
 575     }
 576     return tvb_captured_length(tvb);
 577 } /* dissect_lwm */
 578
 579 /*FUNCTION:------------------------------------------------------
 580  *  NAME
 581  *      dissect_lwm_cmd_frame_ack
 582  *  DESCRIPTION
 583  *      LwMesh command frame - Ack.
 584  *
 585  *  PARAMETERS
 586  *      tvbuff_t *tvb       - pointer to buffer containing raw packet.
 587  *      packet_info *pinfo  - pointer to packet information fields
 588  *      proto_tree *tree    - pointer to data tree wireshark uses to display packet.
 589  *  RETURNS
 590  *      int length          - amount of data processed
 591  *---------------------------------------------------------------
 592  */
 593 static int dissect_lwm_cmd_frame_ack(tvbuff_t *tvb, packet_info *pinfo, proto_tree *lwm_cmd_tree)
 594 {
 595     uint8_t lwm_seq;
 596
 597     /*Get fields*/
 598     lwm_seq = tvb_get_uint8(tvb, 1);
 599
 600     col_append_fstr(pinfo->cinfo, COL_INFO, ", Sequence number: %d", lwm_seq);
 601
 602     if(lwm_cmd_tree){
 603         proto_item_append_text(proto_tree_get_parent(lwm_cmd_tree), ", Sequence number: %d", lwm_seq);
 604         proto_tree_add_uint(lwm_cmd_tree, hf_lwm_cmd_seq, tvb, 1, 1, lwm_seq);
 605         proto_tree_add_item(lwm_cmd_tree, hf_lwm_cmd_cm,  tvb, 2, 1, ENC_NA);
 606     }
 607
 608     return LWM_CMD_FRAME_ACK_LEN;
 609
 610 } /* dissect_lwm_cmd_frame_ack*/
 611
 612 /*FUNCTION:------------------------------------------------------
 613  *  NAME
 614  *      dissect_lwm_cmd_frame_route_err
 615  *  DESCRIPTION
 616  *      LwMesh command frame - Route error.
 617  *
 618  *  PARAMETERS
 619  *      tvbuff_t *tvb       - pointer to buffer containing raw packet.
 620  *      packet_info *pinfo  - pointer to packet information fields
 621  *      proto_tree *tree    - pointer to data tree wireshark uses to display packet.
 622  *  RETURNS
 623  *      int length          - amount of data processed
 624  *---------------------------------------------------------------
 625  */
 626 static int dissect_lwm_cmd_frame_route_err(tvbuff_t *tvb, packet_info *pinfo _U_, proto_tree *lwm_cmd_tree)
 627 {
 628     if(lwm_cmd_tree){
 629         proto_item *ti;
 630
 631         proto_tree_add_item(lwm_cmd_tree, hf_lwm_cmd_route_src, tvb, 1, 2, ENC_LITTLE_ENDIAN);
 632         ti = proto_tree_add_item(lwm_cmd_tree, hf_lwm_cmd_route_dst, tvb, 3, 2, ENC_LITTLE_ENDIAN);
 633
 634         if(tvb_get_uint8(tvb, 5) == LWM_CMD_MULTI_ADDR_TRUE){
 635             proto_item_append_text(ti, " %s", LWM_MULTI_GROUP_STRING);
 636         }else{
 637             proto_item_append_text(ti, " %s", LWM_MULTI_UNICAST_STRING);
 638         }
 639
 640         proto_tree_add_item(lwm_cmd_tree, hf_lwm_cmd_route_multi, tvb, 5, 1, ENC_NA);
 641     }
 642
 643     return LWM_CMD_FRAME_ROUTE_ERR_LEN;
 644
 645 } /* dissect_lwm_cmd_frame_route_err*/
 646
 647 /*FUNCTION:------------------------------------------------------
 648  *  NAME
 649  *      dissect_lwm_cmd_frame_route_req
 650  *  DESCRIPTION
 651  *      LwMesh command frame - Route Request.
 652  *
 653  *  PARAMETERS
 654  *      tvbuff_t *tvb       - pointer to buffer containing raw packet.
 655  *      packet_info *pinfo  - pointer to packet information fields
 656  *      proto_tree *tree    - pointer to data tree wireshark uses to display packet.
 657  *  RETURNS
 658  *      int length          - amount of data processed
 659  *---------------------------------------------------------------
 660  */
 661 static int dissect_lwm_cmd_frame_route_req(tvbuff_t *tvb, packet_info *pinfo _U_, proto_tree *lwm_cmd_tree)
 662 {
 663     if(lwm_cmd_tree){
 664         proto_item *ti;
 665         uint8_t     lwm_linkqual;
 666
 667         proto_tree_add_item(lwm_cmd_tree, hf_lwm_cmd_route_src, tvb, 1, 2, ENC_LITTLE_ENDIAN);
 668         ti = proto_tree_add_item(lwm_cmd_tree, hf_lwm_cmd_route_dst, tvb, 3, 2, ENC_LITTLE_ENDIAN);
 669
 670         if(tvb_get_uint8(tvb, 5) == LWM_CMD_MULTI_ADDR_TRUE){
 671             proto_item_append_text(ti, " %s", LWM_MULTI_GROUP_STRING);
 672         }else{
 673             proto_item_append_text(ti, " %s", LWM_MULTI_UNICAST_STRING);
 674         }
 675
 676         proto_tree_add_item(lwm_cmd_tree, hf_lwm_cmd_route_multi, tvb, 5, 1, ENC_NA);
 677
 678         lwm_linkqual  = tvb_get_uint8(tvb, 6);
 679         ti = proto_tree_add_uint(lwm_cmd_tree, hf_lwm_cmd_linkquality, tvb, 6, 1, lwm_linkqual);
 680         if(lwm_linkqual == 255){
 681             proto_item_append_text(ti, " %s", LWM_CMD_LINKQ_STRING);
 682         }
 683     }
 684
 685     return LWM_CMD_FRAME_ROUTE_REQ_LEN;
 686
 687 } /* dissect_lwm_cmd_frame_route_req*/
 688
 689 /*FUNCTION:------------------------------------------------------
 690  *  NAME
 691  *      dissect_lwm_cmd_frame_route_reply
 692  *  DESCRIPTION
 693  *      LwMesh command frame - Route Reply.
 694  *
 695  *  PARAMETERS
 696  *      tvbuff_t *tvb       - pointer to buffer containing raw packet.
 697  *      packet_info *pinfo  - pointer to packet information fields
 698  *      proto_tree *tree    - pointer to data tree wireshark uses to display packet.
 699  *  RETURNS
 700  *      int length          - amount of data processed
 701  *---------------------------------------------------------------
 702  */
 703 static int dissect_lwm_cmd_frame_route_reply(tvbuff_t *tvb, packet_info *pinfo _U_, proto_tree *lwm_cmd_tree)
 704 {
 705     if(lwm_cmd_tree){
 706         proto_item *ti;
 707         uint8_t     lwm_revlinkqual;
 708
 709         proto_tree_add_item(lwm_cmd_tree, hf_lwm_cmd_route_src, tvb, 1, 2, ENC_LITTLE_ENDIAN);
 710         ti = proto_tree_add_item(lwm_cmd_tree, hf_lwm_cmd_route_dst, tvb, 3, 2, ENC_LITTLE_ENDIAN);
 711
 712         if(tvb_get_uint8(tvb, 5) == LWM_CMD_MULTI_ADDR_TRUE){
 713             proto_item_append_text(ti, " %s", LWM_MULTI_GROUP_STRING);
 714         }else{
 715             proto_item_append_text(ti, " %s", LWM_MULTI_UNICAST_STRING);
 716         }
 717
 718         proto_tree_add_item(lwm_cmd_tree, hf_lwm_cmd_route_multi, tvb, 5, 1, ENC_NA);
 719         proto_tree_add_item(lwm_cmd_tree, hf_lwm_cmd_forwlinkquality, tvb, 6, 1, ENC_NA);
 720
 721         lwm_revlinkqual = tvb_get_uint8(tvb, 7);
 722         ti = proto_tree_add_uint(lwm_cmd_tree, hf_lwm_cmd_revlinkquality, tvb, 7, 1, lwm_revlinkqual);
 723         if(lwm_revlinkqual == 255){
 724             proto_item_append_text(ti, " %s", LWM_CMD_LINKQ_STRING);
 725         }
 726     }
 727
 728     return LWM_CMD_FRAME_ROUTE_REPLY_LEN;
 729
 730 } /* dissect_lwm_cmd_frame_route_reply*/
 731
 732 /*FUNCTION:------------------------------------------------------
 733  *  NAME
 734  *      proto_register_lwm
 735  *  DESCRIPTION
 736  *      IEEE 802.15.4 protocol registration routine.
 737  *  PARAMETERS
 738  *      none
 739  *  RETURNS
 740  *      void
 741  *---------------------------------------------------------------
 742  */
 743 void proto_register_lwm(void)
 744 {
 745
 746     static hf_register_info hf[] = {
 747
 748         /*Frame control field*/
 749         { &hf_lwm_fcf,
 750         { "Frame control field", "lwm.fcf", FT_UINT8, BASE_HEX, NULL, 0x0,
 751         "Control information for the frame.", HFILL }},
 752
 753         { &hf_lwm_fcf_ack_req,
 754         { "Acknowledgment Request", "lwm.ack_req", FT_BOOLEAN, 8, NULL, LWM_FCF_ACK_REQUEST,
 755         "Specifies whether an acknowledgment is required from the destination node.", HFILL }},
 756
 757         { &hf_lwm_fcf_security,
 758         { "Security Enabled", "lwm.security", FT_BOOLEAN, 8, NULL, LWM_FCF_SEC_EN,
 759         "Specifies whether the frame payload is encrypted.", HFILL }},
 760
 761         { &hf_lwm_fcf_linklocal,
 762         { "Link Local", "lwm.linklocal", FT_BOOLEAN, 8, NULL, LWM_FCF_LINK_LOCAL,
 763         "It may be set to one to prevent neighboring nodes from rebroadcasting a frame.", HFILL }},
 764
 765         { &hf_lwm_fcf_multicast,
 766         { "Multicast", "lwm.multicast", FT_BOOLEAN, 8, NULL, LWM_FCF_MULTICAST,
 767         "If the Multicast subfield is set to one, Multicast Header should be present and the Destination Address is a group address.", HFILL }},
 768
 769         { &hf_lwm_fcf_reserved,
 770         { "Reserved bits", "lwm.fcf.reserved", FT_UINT8, BASE_HEX, NULL, LWM_FCF_RESERVED,
 771         "The 4 bits are reserved.", HFILL }},
 772
 773         /*Other fields*/
 774         { &hf_lwm_seq,
 775         { "Sequence Number", "lwm.seq", FT_UINT8, BASE_DEC, NULL, 0x0,
 776         "Specifies the sequence identifier for the frame.", HFILL }},
 777
 778         { &hf_lwm_src_addr,
 779         { "Network Source Address", "lwm.src_addr", FT_UINT16, BASE_HEX, NULL, 0x0,
 780         "Specifies the network address of the node originating the frame.", HFILL }},
 781
 782         { &hf_lwm_dst_addr,
 783         { "Network Destination Address", "lwm.dst_addr", FT_UINT16, BASE_HEX, NULL, 0x0,
 784         "Specifies the network address of the destination node or group address for multicast messages.", HFILL }},
 785
 786         { &hf_lwm_src_endp,
 787         { "Source Endpoint", "lwm.src_endp", FT_UINT8, BASE_DEC, NULL, 0x0,
 788         "Specifies the source endpoint identifier.", HFILL }},
 789
 790         { &hf_lwm_dst_endp,
 791         { "Destination Endpoint", "lwm.dst_endp", FT_UINT8, BASE_DEC, NULL, 0x0,
 792         "Specifies the destination endpoint identifier.", HFILL }},
 793
 794
 795         /*Multicast header*/
 796         { &hf_lwm_multi_nmrad,
 797         { "Non-member Radius", "lwm.multi_nmrad", FT_UINT16, BASE_DEC, NULL, 0x0,
 798         "Specifies remaining radius (number of hops) for Non-members of multicast group.", HFILL }},
 799
 800         { &hf_lwm_multi_mnmrad,
 801         { "Maximum Non-member Radius", "lwm.multi_mnmrad", FT_UINT16, BASE_DEC, NULL, 0x0,
 802         "Specifies maximum radius (number of hops) for Non-members of multicast group.", HFILL }},
 803
 804         { &hf_lwm_multi_mrad,
 805         { "Member Radius", "lwm.multi_mrad", FT_UINT16, BASE_DEC, NULL, 0x0,
 806         "Specifies remaining radius (number of hops) for Members of multicast group.", HFILL }},
 807
 808         { &hf_lwm_multi_mmrad,
 809         { "Maximum Member Radius", "lwm.multi_mmrad", FT_UINT16, BASE_DEC, NULL, 0x0,
 810         "Specifies maximum radius (number of hops) for Members of multicast group.", HFILL }},
 811
 812
 813         /*MIC, security*/
 814         { &hf_lwm_mic,
 815         { "Message Integrity Code", "lwm.mic", FT_UINT32, BASE_HEX, NULL, 0x0,
 816         "Specifies Message Integrity Code (MIC).", HFILL }},
 817
 818
 819         /*----------------------------------*/
 820         /*                                    */
 821         /*  Command Frames Specific Fields  */
 822         /*                                    */
 823         /*----------------------------------*/
 824
 825         { &hf_lwm_cmd,
 826         { "Command ID", "lwm.cmd", FT_UINT8, BASE_HEX, VALS(lwm_cmd_names), 0x0,
 827         "It contains Command ID value.", HFILL }},
 828
 829         /*  Command Frame - Ack */
 830         { &hf_lwm_cmd_seq,
 831         { "Sequence number", "lwm.cmd.seq", FT_UINT8, BASE_DEC, NULL, 0x0,
 832         "It contains a network sequence number of a frame that is being acknowledged.", HFILL }},
 833
 834         { &hf_lwm_cmd_cm,
 835         { "Control Message", "lwm.cmd.cm", FT_UINT8, BASE_HEX, NULL, 0x0,
 836         "It contains an arbitrary value that can be set on the sending side.", HFILL }},
 837
 838         /* Part of  Command Frames - Route Request, Route Reply*/
 839         { &hf_lwm_cmd_route_src,
 840         { "Source address", "lwm.cmd.route_src", FT_UINT16, BASE_HEX, NULL, 0x0,
 841         "It contains a source network address from the frame that cannot be routed", HFILL }},
 842
 843         { &hf_lwm_cmd_route_dst,
 844         { "Destination Address", "lwm.cmd.route_dst", FT_UINT16, BASE_HEX, NULL, 0x0,
 845         "It contains a destination network address from the frame that cannot be routed", HFILL }},
 846
 847         { &hf_lwm_cmd_route_multi,
 848           { "Multicast", "lwm.cmd.multi", FT_UINT8, BASE_HEX, VALS(lwm_cmd_multi_names), 0x0,
 849         "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 }},
 850
 851         /*  Part of Command Frame - Route Request */
 852         { &hf_lwm_cmd_linkquality,
 853         { "Link Quality", "lwm.cmd.linkq", FT_UINT8, BASE_DEC, NULL, 0x0,
 854         "It contains a link quality value of the potential route accumulated over all hops towards the destination.", HFILL }},
 855
 856         /*  Part of Command Frame - Route Reply */
 857         { &hf_lwm_cmd_forwlinkquality,
 858         { "Forward Link Quality", "lwm.cmd.flinkq", FT_UINT8, BASE_DEC, NULL, 0x0,
 859         "It contains a value of the Link Quality field from the corresponding Route Request Command Frame.", HFILL }},
 860
 861         { &hf_lwm_cmd_revlinkquality,
 862         { "Reverse Link Quality", "lwm.cmd.rlinkq", FT_UINT8, BASE_DEC, NULL, 0x0,
 863         "It contains a link quality value of the discovered route accumulated over all hops towards the originator.", HFILL }},
 864
 865
 866     };
 867
 868     /* Subtrees */
 869     static int *ett[] = {
 870         &ett_lwm,
 871         &ett_lwm_fcf,
 872         &ett_lwm_multi_tree,
 873         &ett_lwm_cmd_tree
 874     };
 875
 876     static ei_register_info ei[] = {
 877         { &ei_lwm_mal_error,     { "lwm.malformed_error",   PI_MALFORMED,      PI_ERROR, "Malformed Packet", EXPFILL }},
 878         { &ei_lwm_n_src_broad,   { "lwm.not_src_broadcast", PI_COMMENTS_GROUP, PI_NOTE,  "Source address can not be broadcast address !", EXPFILL }},
 879         { &ei_lwm_mismatch_endp, { "lwm.mismatch_endp",     PI_COMMENTS_GROUP, PI_WARN,  "Stack command Endpoints mismatch (should be 0, both)!", EXPFILL }},
 880         { &ei_lwm_empty_payload, { "lwm.empty_payload",     PI_COMMENTS_GROUP, PI_WARN,  "Empty LwMesh Payload!", EXPFILL }},
 881         { &ei_lwm_no_decryption_key, { "lwm.no_decryption_key", PI_PROTOCOL,   PI_NOTE,  "No encryption key set - can't decrypt", EXPFILL }},
 882         { &ei_lwm_decryption_failed, { "lwm.decryption_failed", PI_PROTOCOL,   PI_WARN,  "Decryption Failed", EXPFILL }},
 883     };
 884
 885     module_t *lw_module;
 886     expert_module_t* expert_lwm;
 887
 888     /*  Register protocol name and description. */
 889     proto_lwm = proto_register_protocol("Lightweight Mesh (v1.1.1)", "LwMesh", "lwm");
 890
 891     /*  Register header fields and subtrees. */
 892     proto_register_field_array(proto_lwm, hf, array_length(hf));
 893     proto_register_subtree_array(ett, array_length(ett));
 894     expert_lwm = expert_register_protocol(proto_lwm);
 895     expert_register_field_array(expert_lwm, ei, array_length(ei));
 896
 897     lw_module = prefs_register_protocol(proto_lwm,proto_reg_handoff_lwm);
 898
 899     /* Register preferences for a decryption key */
 900     /* TODO: Implement a UAT for multiple keys, and with more advanced key management. */
 901     prefs_register_string_preference(lw_module, "lwmes_key", "Lw Decryption key",
 902             "128-bit decryption key in hexadecimal format", (const char **)&lwmes_key_str);
 903
 904     /*  Register dissector with Wireshark. */
 905     lwm_handle = register_dissector("lwm", dissect_lwm, proto_lwm);
 906
 907 } /* proto_register_lwm */
 908
 909 /*FUNCTION:------------------------------------------------------
 910  *  NAME
 911  *      proto_reg_handoff_lwm
 912  *  DESCRIPTION
 913  *      Registers the lwm dissector with Wireshark.
 914  *      Will be called during Wireshark startup, and whenever
 915  *      preferences are changed.
 916  *  PARAMETERS
 917  *      none
 918  *  RETURNS
 919  *      void
 920  *---------------------------------------------------------------
 921  */
 922 void proto_reg_handoff_lwm(void)
 923 {
 924     static bool initialized = false;
 925     GByteArray      *bytes;
 926     bool             res;
 927
 928     if (!initialized) {
 929         /* Register our dissector with IEEE 802.15.4 */
 930         dissector_add_for_decode_as(IEEE802154_PROTOABBREV_WPAN_PANID, lwm_handle);
 931         heur_dissector_add(IEEE802154_PROTOABBREV_WPAN, dissect_lwm_heur, "Lightweight Mesh over IEEE 802.15.4", "lwm_wlan", proto_lwm, HEURISTIC_ENABLE);
 932
 933         initialized = true;
 934     }
 935     /* Convert key to raw bytes */
 936     bytes = g_byte_array_new();
 937     res = hex_str_to_bytes(lwmes_key_str, bytes, false);
 938     lwmes_key_valid = (res && bytes->len >= IEEE802154_CIPHER_SIZE);
 939     if (lwmes_key_valid) {
 940         memcpy(lwmes_key, bytes->data, IEEE802154_CIPHER_SIZE);
 941     }
 942     g_byte_array_free(bytes, true);
 943
 944 } /* proto_reg_handoff_lwm */
 945
 946 /*
 947  * Editor modelines
 948  *
 949  * Local Variables:
 950  * c-basic-offset: 4
 951  * tab-width: 8
 952  * indent-tabs-mode: nil
 953  * End:
 954  *
 955  * ex: set shiftwidth=4 tabstop=8 expandtab:
 956  * :indentSize=4:tabSize=8:noTabs=true:
 957  */