epan/dissectors/packet-rf4ce-secur.c

   1 /* packet-rf4ce-secur.c
   2  * Security related functions and objects for RF4CE dissector
   3  * Copyright (C) Atmosic 2023
   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 #include "packet-rf4ce-secur.h"
  13 #include "packet-zbee-security.h"
  14 #include "packet-ieee802154.h"
  15 #include <wsutil/wsgcrypt.h>
  16 #include <epan/proto_data.h>
  17
  18 #ifdef RF4CE_DEBUG_EN
  19 void rf4ce_print_arr(const char *str, uint8_t *ptr, uint16_t len);
  20 #define RF4CE_PRINT_ARR(s, p, l) rf4ce_print_arr(s, p, l)
  21 #else
  22 #define RF4CE_PRINT_ARR(s, p, l)
  23 #endif /* RF4CE_DEBUG_EN */
  24
  25 static keypair_context_t keypair_context;
  26 static key_exchange_context_t key_exchange_context;
  27 static addr_entry_t addr_table[RF4CE_ADDR_TABLE_SIZE];
  28 static nwk_key_entry_t nwk_key_storage[RF4CE_NWK_KEY_STORAGE_SIZE];
  29 static vendor_secret_entry_t vendor_secret_storage[RF4CE_VENDOR_SECRET_STORAGE_SIZE];
  30
  31 static void keypair_context_calc_key(uint8_t *nwk_key);
  32 static nwk_key_entry_t *nwk_key_storage_get_entry_by_key(uint8_t *nwk_key, bool key_from_gui);
  33
  34 static void reverse(uint8_t *dest, uint8_t *src, uint16_t size);
  35
  36 /* RF4CE GDP 2.0 spec, part 7.4.1 Key Exchange negotiation
  37  * Default secret: This is a 128bit "secret" that is known to all devices that are certified to
  38  * conform to this specification. The value shall be set to the following octet string (lowest order
  39  * octet first)
  40  * Note that this value should be expected to be widely known and the overall link security
  41  * should not depend on this value remaining a secret.
  42  */
  43 uint8_t DEFAULT_SECRET[SEC_STR_LEN] =
  44     {0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
  45      0x88, 0x99, 0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF};
  46
  47 void keypair_context_init(const uint8_t *controller_ieee, const uint8_t *target_ieee, uint8_t expected_transfer_count)
  48 {
  49     if ((controller_ieee == NULL) || (target_ieee == NULL))
  50     {
  51         return;
  52     }
  53     memset(&keypair_context, 0, sizeof(keypair_context_t));
  54
  55     memcpy(keypair_context.controller_addr, controller_ieee, RF4CE_IEEE_ADDR_LEN);
  56     memcpy(keypair_context.target_addr, target_ieee, RF4CE_IEEE_ADDR_LEN);
  57
  58     keypair_context.nwk_key_exchange_transfer_expected = expected_transfer_count;
  59 }
  60
  61 static void keypair_context_calc_key(uint8_t *nwk_key)
  62 {
  63     for (int i = 0; i < keypair_context.nwk_key_exchange_transfer_received; i++)
  64     {
  65         for (int j = 0; j < KEY_LEN; j++)
  66         {
  67             keypair_context.nwk_key_seed[(i + 1) * KEY_LEN + j] ^= keypair_context.nwk_key_seed[i * KEY_LEN + j];
  68         }
  69     }
  70
  71     memcpy(nwk_key, &keypair_context.nwk_key_seed[RF4CE_NWK_KEY_SEED_DATA_LENGTH - KEY_LEN], KEY_LEN);
  72 }
  73
  74 void keypair_context_update_seed(uint8_t *seed, uint8_t seed_seqn)
  75 {
  76     bool is_retransmit = (seed_seqn == keypair_context.nwk_key_exchange_transfer_received - 1);
  77     bool is_latest_seed = (seed_seqn + 1 == keypair_context.nwk_key_exchange_transfer_expected);
  78
  79     /* retransmitt of the latest key seed - we must to re-calculate a NWK key */
  80     if (is_retransmit && is_latest_seed)
  81     {
  82         memcpy(keypair_context.nwk_key_seed, keypair_context.nwk_key_seed_prev, RF4CE_NWK_KEY_SEED_DATA_LENGTH);
  83     }
  84
  85     if (seed_seqn == 0)
  86     {
  87         memcpy(keypair_context.nwk_key_seed_latest, seed, RF4CE_NWK_KEY_SEED_DATA_LENGTH);
  88         keypair_context.nwk_key_exchange_transfer_received = 1;
  89         return;
  90     }
  91
  92     /* Retransmit of the previous key seed. Should take this one */
  93     if (is_retransmit)
  94     {
  95         /* save this one as a candidate */
  96         memcpy(keypair_context.nwk_key_seed_latest, seed, RF4CE_NWK_KEY_SEED_DATA_LENGTH);
  97
  98         /* move on if it's the latest seed to re-calculate a NWK key */
  99         if (!is_latest_seed)
 100         {
 101             return;
 102         }
 103     }
 104
 105     if (seed_seqn == keypair_context.nwk_key_exchange_transfer_received)
 106     {
 107         /* Apply previous key seed, it has been accepted since we received the next one */
 108         for (int i = 0; i < RF4CE_NWK_KEY_SEED_DATA_LENGTH; i++)
 109         {
 110             keypair_context.nwk_key_seed[i] ^= keypair_context.nwk_key_seed_latest[i];
 111         }
 112
 113         /* save this one as a candidate */
 114         memcpy(keypair_context.nwk_key_seed_latest, seed, RF4CE_NWK_KEY_SEED_DATA_LENGTH);
 115         keypair_context.nwk_key_exchange_transfer_received += 1;
 116     }
 117
 118     if (is_latest_seed)
 119     {
 120         uint8_t nwk_key[KEY_LEN] = {0};
 121         addr_entry_t *controller_addr_ent = rf4ce_addr_table_get_addr_entry_by_ieee(keypair_context.controller_addr);
 122         addr_entry_t *target_addr_ent = rf4ce_addr_table_get_addr_entry_by_ieee(keypair_context.target_addr);
 123
 124         /* save the current key seed to avoid retransmitts of the latest one in future */
 125         memcpy(keypair_context.nwk_key_seed_prev, keypair_context.nwk_key_seed, RF4CE_NWK_KEY_SEED_DATA_LENGTH);
 126
 127         for (int i = 0; i < RF4CE_NWK_KEY_SEED_DATA_LENGTH; i++)
 128         {
 129             keypair_context.nwk_key_seed[i] ^= keypair_context.nwk_key_seed_latest[i];
 130         }
 131
 132         keypair_context_calc_key(nwk_key);
 133
 134         nwk_key_storage_add_entry(
 135             nwk_key,
 136             controller_addr_ent,
 137             target_addr_ent,
 138             false, /* key from commissioning session */
 139             true); /* is_pairing_key                 */
 140     }
 141 }
 142
 143 static nwk_key_entry_t *nwk_key_storage_get_entry_by_key(uint8_t *nwk_key, bool key_from_gui)
 144 {
 145     nwk_key_entry_t *entry = NULL;
 146     int idx = 0;
 147
 148     while (idx < RF4CE_NWK_KEY_STORAGE_SIZE)
 149     {
 150         if (nwk_key_storage[idx].is_used && (nwk_key_storage[idx].key_from_gui == key_from_gui) && (memcmp(nwk_key_storage[idx].nwk_key, nwk_key, KEY_LEN) == 0))
 151         {
 152             entry = nwk_key_storage + idx;
 153             break;
 154         }
 155
 156         idx++;
 157     }
 158
 159     return entry;
 160 }
 161
 162 void nwk_key_storage_add_entry(uint8_t *nwk_key, addr_entry_t *controller_addr_ent, addr_entry_t *target_addr_ent, bool key_from_gui, bool is_pairing_key)
 163 {
 164     /* find an existing entry so as not to add duplicates */
 165     nwk_key_entry_t *nwk_key_entry = nwk_key_storage_get_entry_by_key(nwk_key, key_from_gui);
 166
 167     if (nwk_key_entry == NULL)
 168     {
 169         int idx = 0;
 170
 171         while (idx < RF4CE_NWK_KEY_STORAGE_SIZE)
 172         {
 173             if (!nwk_key_storage[idx].is_used)
 174             {
 175                 memcpy(nwk_key_storage[idx].nwk_key, nwk_key, KEY_LEN);
 176                 nwk_key_storage[idx].controller_addr_ent = controller_addr_ent;
 177                 nwk_key_storage[idx].target_addr_ent = target_addr_ent;
 178                 nwk_key_storage[idx].key_from_gui = key_from_gui;
 179                 nwk_key_storage[idx].is_used = true;
 180                 nwk_key_storage[idx].is_pairing_key = is_pairing_key;
 181                 break;
 182             }
 183
 184             idx++;
 185         }
 186     }
 187 }
 188
 189 void nwk_key_storage_release_entry(uint8_t *nwk_key, bool key_from_gui)
 190 {
 191     nwk_key_entry_t *nwk_key_entry = nwk_key_storage_get_entry_by_key(nwk_key, key_from_gui);
 192
 193     if (nwk_key_entry != NULL)
 194     {
 195         nwk_key_entry->is_used = false;
 196     }
 197 }
 198
 199 void rf4ce_addr_table_add_addrs(const void *ieee_addr, uint16_t short_addr)
 200 {
 201     unsigned idx = 0;
 202
 203     if (ieee_addr == NULL)
 204     {
 205         return;
 206     }
 207
 208     /* search for addresses so as not to add duplicates */
 209     while (idx < RF4CE_ADDR_TABLE_SIZE)
 210     {
 211         if (addr_table[idx].is_used && (memcmp(addr_table[idx].ieee_addr, ieee_addr, RF4CE_IEEE_ADDR_LEN) == 0) && addr_table[idx].short_addr == short_addr)
 212         {
 213             return;
 214         }
 215
 216         idx++;
 217     }
 218
 219     /* no duplicates found, search for a free slot */
 220     idx = 0;
 221     while (idx < RF4CE_ADDR_TABLE_SIZE && addr_table[idx].is_used)
 222     {
 223         idx++;
 224     }
 225
 226     if (idx < RF4CE_ADDR_TABLE_SIZE)
 227     {
 228         memcpy(addr_table[idx].ieee_addr, ieee_addr, RF4CE_IEEE_ADDR_LEN);
 229         addr_table[idx].short_addr = short_addr;
 230         addr_table[idx].is_used = true;
 231     }
 232 }
 233
 234 bool rf4ce_addr_table_get_ieee_addr(uint8_t *ieee_addr, packet_info *pinfo, bool is_src)
 235 {
 236     bool addr_found = false;
 237     address_type addr_type;
 238     ieee802154_hints_t *hints;
 239     const void *p_addr = NULL;
 240     uint16_t short_addr = 0xffff;
 241
 242     /* Check inputs */
 243     if ((ieee_addr == NULL) || (pinfo == NULL))
 244     {
 245         return false;
 246     }
 247     if (is_src)
 248     {
 249         addr_type = pinfo->dl_src.type;
 250         p_addr = pinfo->dl_src.data;
 251     }
 252     else
 253     {
 254         addr_type = pinfo->dl_dst.type;
 255         p_addr = pinfo->dl_dst.data;
 256     }
 257     if (addr_type == AT_EUI64)
 258     {
 259         if (p_addr == NULL)
 260         {
 261             return false;
 262         }
 263     }
 264     else
 265     {
 266         /* Get addresses */
 267         hints = (ieee802154_hints_t *)p_get_proto_data(wmem_file_scope(),
 268                                                        pinfo,
 269                                                        proto_get_id_by_filter_name(IEEE802154_PROTOABBREV_WPAN),
 270                                                        0
 271         );
 272         if (hints == NULL)
 273         {
 274             return false;
 275         }
 276         short_addr = (is_src) ? hints->src16 : hints->dst16;
 277     }
 278     /* Search address in address table */
 279     for (unsigned idx = 0; idx < RF4CE_ADDR_TABLE_SIZE; idx++)
 280     {
 281         if (addr_table[idx].is_used)
 282         {
 283             if (addr_type == AT_EUI64)
 284             {
 285                 if (memcmp(addr_table[idx].ieee_addr, p_addr, RF4CE_IEEE_ADDR_LEN) == 0) {
 286                     addr_found = true;
 287                 }
 288             }
 289             else
 290             {
 291                 if (addr_table[idx].short_addr == short_addr) {
 292                     addr_found = true;
 293                 }
 294             }
 295             if (addr_found)
 296             {
 297                 memcpy(ieee_addr, addr_table[idx].ieee_addr, RF4CE_IEEE_ADDR_LEN);
 298                 break;
 299             }
 300         }
 301     }
 302     return addr_found;
 303 }
 304
 305 addr_entry_t *rf4ce_addr_table_get_addr_entry_by_ieee(uint8_t *ieee_addr)
 306 {
 307     addr_entry_t *entry = NULL;
 308     unsigned idx = 0;
 309
 310     while (ieee_addr != NULL && idx < RF4CE_ADDR_TABLE_SIZE)
 311     {
 312         if (addr_table[idx].is_used && memcmp(addr_table[idx].ieee_addr, ieee_addr, RF4CE_IEEE_ADDR_LEN) == 0)
 313         {
 314             entry = addr_table + idx;
 315             break;
 316         }
 317
 318         idx++;
 319     }
 320
 321     return entry;
 322 }
 323
 324 void key_exchange_context_init(void)
 325 {
 326     memset(&key_exchange_context.rand_a, 0, RF4CE_PROFILE_CMD_KEY_EXCHANGE_RAND_A_LENGTH);
 327     memset(&key_exchange_context.rand_b, 0, RF4CE_PROFILE_CMD_KEY_EXCHANGE_RAND_B_LENGTH);
 328     memset(&key_exchange_context.mac_a, 0, RF4CE_IEEE_ADDR_LEN);
 329     memset(&key_exchange_context.mac_b, 0, RF4CE_IEEE_ADDR_LEN);
 330 }
 331
 332 void key_exchange_context_start_procedure(void)
 333 {
 334     if (!key_exchange_context.is_proc_started)
 335     {
 336         key_exchange_context.is_proc_started = true;
 337     }
 338 }
 339
 340 void key_exchange_context_stop_procedure(void)
 341 {
 342     if (key_exchange_context.is_proc_started)
 343     {
 344         key_exchange_context.is_proc_started = false;
 345     }
 346 }
 347
 348 bool key_exchange_context_is_procedure_started(void)
 349 {
 350     return key_exchange_context.is_proc_started;
 351 }
 352
 353 void key_exchange_context_set_rand_a(uint8_t *rand_a)
 354 {
 355     if (rand_a != NULL)
 356     {
 357         memcpy(key_exchange_context.rand_a, rand_a, RF4CE_PROFILE_CMD_KEY_EXCHANGE_RAND_A_LENGTH);
 358     }
 359 }
 360
 361 void key_exchange_context_set_rand_b(uint8_t *rand_b)
 362 {
 363     if (rand_b != NULL)
 364     {
 365         memcpy(key_exchange_context.rand_b, rand_b, RF4CE_PROFILE_CMD_KEY_EXCHANGE_RAND_B_LENGTH);
 366     }
 367 }
 368
 369 void key_exchange_context_set_mac_a(uint8_t *mac_a)
 370 {
 371     if (mac_a != NULL)
 372     {
 373         memcpy(key_exchange_context.mac_a, mac_a, RF4CE_IEEE_ADDR_LEN);
 374     }
 375 }
 376
 377 void key_exchange_context_set_mac_b(uint8_t *mac_b)
 378 {
 379     if (mac_b != NULL)
 380     {
 381         memcpy(key_exchange_context.mac_b, mac_b, RF4CE_IEEE_ADDR_LEN);
 382     }
 383 }
 384
 385 #ifdef RF4CE_DEBUG_EN
 386 void rf4ce_print_arr(const char *str, uint8_t *ptr, uint16_t len)
 387 {
 388   g_print("%s: ", str);
 389   for (uint16_t i = 0; i < len-1; i++)
 390   {
 391     g_print("%02x:", *(ptr+i));
 392   }
 393   g_print("%02x\n", *(ptr+len-1));
 394 }
 395 #endif /* RF4CE_DEBUG_EN */
 396
 397 static bool calc_key_cmac(uint8_t *secret, uint8_t *nwk_key, uint32_t tag_b_pack, uint8_t *key_out)
 398 {
 399     uint8_t mac_a[RF4CE_IEEE_ADDR_LEN];
 400     uint8_t mac_b[RF4CE_IEEE_ADDR_LEN];
 401
 402     uint8_t *rand_a = key_exchange_context.rand_a;
 403     uint8_t *rand_b = key_exchange_context.rand_b;
 404
 405     uint8_t k_dk_data[RF4CE_PROFILE_CMD_KEY_EXCHANGE_RAND_A_LENGTH +
 406                       RF4CE_PROFILE_CMD_KEY_EXCHANGE_RAND_B_LENGTH];
 407     uint8_t k_dk_data_reversed[RF4CE_PROFILE_CMD_KEY_EXCHANGE_RAND_B_LENGTH +
 408                                RF4CE_PROFILE_CMD_KEY_EXCHANGE_RAND_A_LENGTH];
 409
 410     uint8_t context_data[CONTEXT_STR_LEN + RF4CE_IEEE_ADDR_LEN +
 411                          RF4CE_IEEE_ADDR_LEN + KEY_LEN];
 412
 413     uint8_t k_dk_key[KEY_LEN];
 414     uint8_t new_key[KEY_LEN];
 415
 416     uint8_t dummy[KEY_LEN];
 417     uint32_t tag_b_calc;
 418
 419     uint8_t *data_ptr;
 420
 421     reverse(mac_a, key_exchange_context.mac_a, RF4CE_IEEE_ADDR_LEN);
 422     reverse(mac_b, key_exchange_context.mac_b, RF4CE_IEEE_ADDR_LEN);
 423
 424     data_ptr = k_dk_data;
 425     memcpy(data_ptr, rand_a, RF4CE_PROFILE_CMD_KEY_EXCHANGE_RAND_A_LENGTH);
 426     data_ptr += RF4CE_PROFILE_CMD_KEY_EXCHANGE_RAND_A_LENGTH;
 427     memcpy(data_ptr, rand_b, RF4CE_PROFILE_CMD_KEY_EXCHANGE_RAND_B_LENGTH);
 428
 429     data_ptr = k_dk_data_reversed;
 430     memcpy(data_ptr, rand_b, RF4CE_PROFILE_CMD_KEY_EXCHANGE_RAND_B_LENGTH);
 431     data_ptr += RF4CE_PROFILE_CMD_KEY_EXCHANGE_RAND_B_LENGTH;
 432     memcpy(data_ptr, rand_a, RF4CE_PROFILE_CMD_KEY_EXCHANGE_RAND_A_LENGTH);
 433
 434     data_ptr = context_data;
 435     memcpy(data_ptr, CONTEXT_STR, CONTEXT_STR_LEN);
 436     data_ptr += CONTEXT_STR_LEN;
 437     memcpy(data_ptr, mac_a, RF4CE_IEEE_ADDR_LEN);
 438     data_ptr += RF4CE_IEEE_ADDR_LEN;
 439     memcpy(data_ptr, mac_b, RF4CE_IEEE_ADDR_LEN);
 440     data_ptr += RF4CE_IEEE_ADDR_LEN;
 441     memcpy(data_ptr, nwk_key, KEY_LEN);
 442
 443     /* Generic Device Profile Version 2.0
 444      * 7.4.2 Key generation
 445      * Calculate derivation key
 446      * K_dk = AES-128-CMAC (RAND-A || RAND-B, Shared secret)
 447      */
 448     rf4ce_aes_cmac(secret, SEC_STR_LEN, k_dk_data, k_dk_key);
 449
 450     /* Calculate new link key
 451      * Link key = AES-128-CMAC (K_dk, context || label || pairing key)
 452      */
 453     rf4ce_aes_cmac(context_data, sizeof(context_data), k_dk_key, new_key);
 454
 455     /* Calculate TAG-B value
 456      * TAG-B = AES-128-CMAC(link key, RAND-B || RAND-A)
 457      */
 458     rf4ce_aes_cmac(k_dk_data_reversed, sizeof(k_dk_data_reversed), new_key, dummy);
 459     memcpy((uint8_t *)&tag_b_calc, dummy, RF4CE_PROFILE_CMD_KEY_EXCHANGE_TAG_A_LENGTH);
 460
 461     RF4CE_PRINT_ARR("tag_b_calc", (uint8_t *)&tag_b_calc, 4);
 462     RF4CE_PRINT_ARR("   new_key", new_key, 16);
 463
 464     if (tag_b_pack == tag_b_calc)
 465     {
 466         memcpy(key_out, new_key, KEY_LEN);
 467         return true;
 468     }
 469
 470     return false;
 471 }
 472
 473 static bool key_exchange_calc_key_cont(uint8_t *secret, uint32_t tag_b_pack, bool try_pairing_key, uint8_t *new_key_out)
 474 {
 475     bool is_new_key_found = false;
 476
 477     for (unsigned i = 0; i < RF4CE_NWK_KEY_STORAGE_SIZE; i++)
 478     {
 479         if (nwk_key_storage[i].is_used && ((try_pairing_key && nwk_key_storage[i].is_pairing_key) || (!try_pairing_key && nwk_key_storage[i].key_from_gui)))
 480         {
 481             is_new_key_found = calc_key_cmac(secret, nwk_key_storage[i].nwk_key, tag_b_pack, new_key_out);
 482
 483             if (is_new_key_found)
 484             {
 485                 break;
 486             }
 487         }
 488     }
 489
 490     return is_new_key_found;
 491 }
 492
 493 void key_exchange_calc_key(uint32_t tag_b_pack)
 494 {
 495     uint8_t *controller_addr = key_exchange_context.mac_a;
 496     uint8_t *target_addr = key_exchange_context.mac_b;
 497
 498     addr_entry_t *controller_addr_ent = rf4ce_addr_table_get_addr_entry_by_ieee(controller_addr);
 499     addr_entry_t *target_addr_ent = rf4ce_addr_table_get_addr_entry_by_ieee(target_addr);
 500
 501     uint8_t *secret;
 502
 503     uint8_t new_key[KEY_LEN];
 504     bool is_new_key_found = false;
 505
 506     for (unsigned i = 0; i < RF4CE_VENDOR_SECRET_STORAGE_SIZE; i++)
 507     {
 508         if (!vendor_secret_storage[i].is_used)
 509         {
 510             continue;
 511         }
 512
 513         secret = vendor_secret_storage[i].secret;
 514
 515         /* try all the pairing keys first */
 516         is_new_key_found = key_exchange_calc_key_cont(secret, tag_b_pack, true, new_key);
 517
 518         /* try other keys */
 519         if (!is_new_key_found)
 520         {
 521             is_new_key_found = key_exchange_calc_key_cont(secret, tag_b_pack, false, new_key);
 522         }
 523
 524         if (is_new_key_found)
 525         {
 526             nwk_key_storage_add_entry(
 527                 new_key,
 528                 controller_addr_ent,
 529                 target_addr_ent,
 530                 false,  /* key from the Key Exchange procedure */
 531                 false); /* !is_pairing_key */
 532
 533             break;
 534         }
 535     }
 536 }
 537
 538 static vendor_secret_entry_t *vendor_secret_storage_get_entry(uint8_t *secret)
 539 {
 540     vendor_secret_entry_t *entry = NULL;
 541     int idx = 0;
 542
 543     while (idx < RF4CE_VENDOR_SECRET_STORAGE_SIZE)
 544     {
 545         if (vendor_secret_storage[idx].is_used && (memcmp(vendor_secret_storage[idx].secret, secret, SEC_STR_LEN) == 0))
 546         {
 547             entry = vendor_secret_storage + idx;
 548             break;
 549         }
 550
 551         idx++;
 552     }
 553
 554     return entry;
 555 }
 556
 557 void vendor_secret_storage_add_entry(uint8_t *secret)
 558 {
 559     unsigned idx = 0;
 560     vendor_secret_entry_t *entry = vendor_secret_storage_get_entry(secret);
 561
 562     if (entry != NULL)
 563     {
 564         return;
 565     }
 566
 567     while (idx < RF4CE_VENDOR_SECRET_STORAGE_SIZE && vendor_secret_storage[idx].is_used)
 568     {
 569         idx++;
 570     }
 571
 572     if (idx < RF4CE_VENDOR_SECRET_STORAGE_SIZE)
 573     {
 574         memcpy(vendor_secret_storage[idx].secret, secret, SEC_STR_LEN);
 575         vendor_secret_storage[idx].is_used = true;
 576     }
 577 }
 578
 579 void vendor_secret_storage_release_entry(uint8_t *secret)
 580 {
 581     vendor_secret_entry_t *entry = vendor_secret_storage_get_entry(secret);
 582
 583     if (entry != NULL)
 584     {
 585         entry->is_used = false;
 586     }
 587 }
 588
 589 void rf4ce_secur_cleanup(void)
 590 {
 591     int idx = 0;
 592
 593     memset(&keypair_context, 0, sizeof(keypair_context));
 594     memset(addr_table, 0, sizeof(addr_table));
 595
 596     while (idx < RF4CE_NWK_KEY_STORAGE_SIZE)
 597     {
 598         if (nwk_key_storage[idx].is_used && !nwk_key_storage[idx].key_from_gui)
 599         {
 600             nwk_key_storage[idx].is_used = false;
 601         }
 602
 603         idx++;
 604     }
 605 }
 606
 607 static void reverse(uint8_t *dest, uint8_t *src, uint16_t size)
 608 {
 609     for (int i = 0; i < size; i++)
 610     {
 611         dest[size - i - 1] = src[i];
 612     }
 613 }
 614
 615 bool decrypt_data(
 616     const uint8_t *in, uint8_t *out,
 617     uint16_t payload_offset,
 618     uint16_t *len,
 619     uint8_t src_ieee[RF4CE_IEEE_ADDR_LEN], uint8_t dst_ieee[RF4CE_IEEE_ADDR_LEN])
 620 {
 621     bool ret = false;
 622     uint8_t frame_control = *in;
 623     int idx = 0;
 624
 625     if (*len < RF4CE_MIN_NWK_LENGTH || *len > RF4CE_MAX_NWK_LENGTH)
 626     {
 627         return false;
 628     }
 629
 630     while (idx < RF4CE_NWK_KEY_STORAGE_SIZE)
 631     {
 632         if (nwk_key_storage[idx].is_used)
 633         {
 634             uint8_t *data_ptr;
 635
 636             /* Form the nonce (3.5.11.3 Outgoing frame security) */
 637             uint8_t nonce[RF4CE_IEEE_ADDR_LEN + 4 + 1];
 638
 639             data_ptr = nonce;
 640             /* Source IEEE address */
 641             reverse(data_ptr, src_ieee, RF4CE_IEEE_ADDR_LEN);
 642             data_ptr += RF4CE_IEEE_ADDR_LEN;
 643             /* Fetch frame counter from the packet (don't check) */
 644             memcpy(data_ptr, in + 1, 4);
 645             data_ptr += 4;
 646             /* Security level */
 647             *data_ptr = RF4CE_SECUR_CONTROL;
 648
 649             /* Form the auth string (3.5.11.3 Outgoing frame security) */
 650             uint8_t auth[1 + 4 + RF4CE_IEEE_ADDR_LEN];
 651
 652             data_ptr = auth;
 653             /* Frame control field */
 654             *data_ptr = frame_control;
 655             data_ptr += 1;
 656             /* Fetch frame counter from the packet (don't check) */
 657             memcpy(data_ptr, in + 1, 4);
 658             data_ptr += 4;
 659             /* Destination IEEE address */
 660             reverse(data_ptr, dst_ieee, RF4CE_IEEE_ADDR_LEN);
 661
 662             ret = zbee_sec_ccm_decrypt(nwk_key_storage[idx].nwk_key,
 663                                        nonce,
 664                                        auth,
 665                                        in + payload_offset,
 666                                        out,
 667                                        sizeof(auth),
 668                                        *len - payload_offset - RF4CE_CCM_M,
 669                                        RF4CE_CCM_M);
 670
 671             if (ret)
 672             {
 673                 *len = *len - payload_offset - RF4CE_CCM_M;
 674                 break;
 675             }
 676         }
 677
 678         idx++;
 679     }
 680
 681     return ret;
 682 }
 683
 684 // Calculate the CMAC
 685 void rf4ce_aes_cmac(unsigned char *input, unsigned long length, unsigned char *key, unsigned char *mac_value)
 686 {
 687     gcry_mac_hd_t mac_hd;
 688     size_t l = length;
 689
 690     if (gcry_mac_open(&mac_hd, GCRY_MAC_CMAC_AES, 0, NULL))
 691     {
 692         return;
 693     }
 694     if (gcry_mac_setkey(mac_hd, key, KEY_LEN))
 695     {
 696         gcry_mac_close(mac_hd);
 697         return;
 698     }
 699     if (gcry_mac_write(mac_hd, input, length))
 700     {
 701         gcry_mac_close(mac_hd);
 702         return;
 703     }
 704     if (gcry_mac_read(mac_hd, mac_value, &l))
 705     {
 706         gcry_mac_close(mac_hd);
 707         return;
 708     }
 709     gcry_mac_close(mac_hd);
 710 }