| /* |
| * Licensed to the Apache Software Foundation (ASF) under one |
| * or more contributor license agreements. See the NOTICE file |
| * distributed with this work for additional information |
| * regarding copyright ownership. The ASF licenses this file |
| * to you under the Apache License, Version 2.0 (the |
| * "License"); you may not use this file except in compliance |
| * with the License. You may obtain a copy of the License at |
| * |
| * http://www.apache.org/licenses/LICENSE-2.0 |
| * |
| * Unless required by applicable law or agreed to in writing, |
| * software distributed under the License is distributed on an |
| * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY |
| * KIND, either express or implied. See the License for the |
| * specific language governing permissions and limitations |
| * under the License. |
| */ |
| #include <stdint.h> |
| #include <assert.h> |
| #include <string.h> |
| #include "syscfg/syscfg.h" |
| #include "os/os.h" |
| #include "nimble/ble.h" |
| #include "nimble/nimble_opt.h" |
| #include "controller/ble_ll.h" |
| #include "controller/ble_ll_resolv.h" |
| #include "controller/ble_ll_hci.h" |
| #include "controller/ble_ll_scan.h" |
| #include "controller/ble_ll_adv.h" |
| #include "controller/ble_ll_sync.h" |
| #include "controller/ble_hw.h" |
| #include "ble_ll_conn_priv.h" |
| #include "ble_ll_priv.h" |
| |
| #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) |
| struct ble_ll_resolv_data |
| { |
| uint8_t addr_res_enabled; |
| uint8_t rl_size; |
| uint8_t rl_cnt_hw; |
| uint8_t rl_cnt; |
| ble_npl_time_t rpa_tmo; |
| struct ble_npl_callout rpa_timer; |
| }; |
| struct ble_ll_resolv_data g_ble_ll_resolv_data; |
| |
| __attribute__((aligned(4))) |
| struct ble_ll_resolv_entry g_ble_ll_resolv_list[MYNEWT_VAL(BLE_LL_RESOLV_LIST_SIZE)]; |
| |
| #if MYNEWT_VAL(BLE_LL_HCI_VS_LOCAL_IRK) |
| struct local_irk_data { |
| uint8_t is_set; |
| uint8_t irk[16]; |
| uint8_t rpa[6]; |
| }; |
| /* 0 is for public, 1 is for static address */ |
| static struct local_irk_data g_local_irk[2]; |
| #endif |
| |
| /** |
| * Called to determine if a change is allowed to the resolving list at this |
| * time. We are not allowed to modify the resolving list if address translation |
| * is enabled and we are either scanning, advertising, or attempting to create |
| * a connection. |
| * |
| * @return int 0: not allowed. 1: allowed. |
| */ |
| static int |
| ble_ll_resolv_list_chg_allowed(void) |
| { |
| int rc; |
| |
| if (g_ble_ll_resolv_data.addr_res_enabled && |
| ble_ll_is_busy(BLE_LL_BUSY_EXCLUDE_CONNECTIONS)) { |
| rc = 0; |
| } else { |
| rc = 1; |
| } |
| return rc; |
| } |
| |
| static void |
| generate_rpa(const uint8_t *irk, uint8_t *rpa) |
| { |
| uint8_t *prand; |
| struct ble_encryption_block ecb; |
| |
| /* Get prand */ |
| prand = rpa + 3; |
| ble_ll_rand_prand_get(prand); |
| |
| /* Calculate hash, hash = ah(local IRK, prand) */ |
| memcpy(ecb.key, irk, 16); |
| memset(ecb.plain_text, 0, 13); |
| ecb.plain_text[13] = prand[2]; |
| ecb.plain_text[14] = prand[1]; |
| ecb.plain_text[15] = prand[0]; |
| |
| /* Calculate hash */ |
| ble_hw_encrypt_block(&ecb); |
| |
| rpa[0] = ecb.cipher_text[15]; |
| rpa[1] = ecb.cipher_text[14]; |
| rpa[2] = ecb.cipher_text[13]; |
| } |
| |
| /** |
| * Called to generate a resolvable private address in rl structure |
| * |
| * @param rl |
| * @param local |
| */ |
| static void |
| ble_ll_resolv_gen_priv_addr(struct ble_ll_resolv_entry *rl, int local) |
| { |
| uint8_t *irk; |
| uint8_t *addr; |
| |
| BLE_LL_ASSERT(rl != NULL); |
| |
| if (local) { |
| addr = rl->rl_local_rpa; |
| irk = rl->rl_local_irk; |
| } else { |
| addr = rl->rl_peer_rpa; |
| irk = rl->rl_peer_irk; |
| } |
| |
| generate_rpa(irk, addr); |
| } |
| |
| /** |
| * Called when the Resolvable private address timer expires. This timer |
| * is used to regenerate local and peers RPA's in the resolving list. |
| */ |
| static void |
| ble_ll_resolv_rpa_timer_cb(struct ble_npl_event *ev) |
| { |
| int i; |
| os_sr_t sr; |
| struct ble_ll_resolv_entry *rl; |
| #if MYNEWT_VAL(BLE_LL_HCI_VS_LOCAL_IRK) |
| struct local_irk_data *irk_data; |
| uint8_t rpa[6]; |
| #endif |
| |
| rl = &g_ble_ll_resolv_list[0]; |
| for (i = 0; i < g_ble_ll_resolv_data.rl_cnt; ++i) { |
| if (rl->rl_has_local) { |
| OS_ENTER_CRITICAL(sr); |
| ble_ll_resolv_gen_priv_addr(rl, 1); |
| OS_EXIT_CRITICAL(sr); |
| } |
| |
| if (rl->rl_has_peer) { |
| OS_ENTER_CRITICAL(sr); |
| ble_ll_resolv_gen_priv_addr(rl, 0); |
| OS_EXIT_CRITICAL(sr); |
| } |
| ++rl; |
| } |
| |
| #if MYNEWT_VAL(BLE_LL_HCI_VS_LOCAL_IRK) |
| for (i = 0; i < ARRAY_SIZE(g_local_irk); i++) { |
| irk_data = &g_local_irk[i]; |
| if (irk_data->is_set) { |
| generate_rpa(irk_data->irk, rpa); |
| OS_ENTER_CRITICAL(sr); |
| memcpy(irk_data->rpa, rpa, 6); |
| OS_EXIT_CRITICAL(sr); |
| } |
| } |
| #endif |
| |
| ble_npl_callout_reset(&g_ble_ll_resolv_data.rpa_timer, |
| g_ble_ll_resolv_data.rpa_tmo); |
| |
| #if MYNEWT_VAL(BLE_LL_ROLE_BROADCASTER) |
| ble_ll_adv_rpa_timeout(); |
| #endif |
| } |
| |
| /** |
| * Called to determine if the IRK is all zero. |
| * |
| * @param irk |
| * |
| * @return int 0: IRK is zero . 1: IRK has non-zero value. |
| */ |
| static int |
| ble_ll_resolv_irk_nonzero(const uint8_t *irk) |
| { |
| int i; |
| int rc; |
| |
| rc = 0; |
| for (i = 0; i < 16; ++i) { |
| if (*irk != 0) { |
| rc = 1; |
| break; |
| } |
| ++irk; |
| } |
| |
| return rc; |
| } |
| |
| /** |
| * Clear the resolving list |
| * |
| * @return int 0: success, BLE error code otherwise |
| */ |
| int |
| ble_ll_resolv_list_clr(void) |
| { |
| /* Check proper state */ |
| if (!ble_ll_resolv_list_chg_allowed()) { |
| return BLE_ERR_CMD_DISALLOWED; |
| } |
| |
| /* Sets total on list to 0. Clears HW resolve list */ |
| g_ble_ll_resolv_data.rl_cnt_hw = 0; |
| g_ble_ll_resolv_data.rl_cnt = 0; |
| ble_hw_resolv_list_clear(); |
| |
| /* stop RPA timer when clearing RL */ |
| ble_npl_callout_stop(&g_ble_ll_resolv_data.rpa_timer); |
| |
| return BLE_ERR_SUCCESS; |
| } |
| |
| /** |
| * Read the size of the resolving list. This is the total number of resolving |
| * list entries allowed by the controller. |
| * |
| * @param rspbuf Pointer to response buffer |
| * |
| * @return int 0: success. |
| */ |
| int |
| ble_ll_resolv_list_read_size(uint8_t *rspbuf, uint8_t *rsplen) |
| { |
| struct ble_hci_le_rd_resolv_list_size_rp *rsp = (void *) rspbuf; |
| |
| rsp->size = g_ble_ll_resolv_data.rl_size; |
| |
| *rsplen = sizeof(*rsp); |
| return BLE_ERR_SUCCESS; |
| } |
| |
| /** |
| * Used to determine if the device is on the resolving list. |
| * |
| * @param addr |
| * @param addr_type Public address (0) or random address (1) |
| * |
| * @return int 0: device is not on resolving list; otherwise the return value |
| * is the 'position' of the device in the resolving list (the index of the |
| * element plus 1). |
| */ |
| static int |
| ble_ll_is_on_resolv_list(const uint8_t *addr, uint8_t addr_type) |
| { |
| int i; |
| struct ble_ll_resolv_entry *rl; |
| |
| rl = &g_ble_ll_resolv_list[0]; |
| for (i = 0; i < g_ble_ll_resolv_data.rl_cnt; ++i) { |
| if ((rl->rl_addr_type == addr_type) && |
| (!memcmp(&rl->rl_identity_addr[0], addr, BLE_DEV_ADDR_LEN))) { |
| return i + 1; |
| } |
| ++rl; |
| } |
| |
| return 0; |
| } |
| |
| /** |
| * Used to determine if the device is on the resolving list. |
| * |
| * @param addr |
| * @param addr_type Public address (0) or random address (1) |
| * |
| * @return Pointer to resolving list entry or NULL if no entry found. |
| */ |
| struct ble_ll_resolv_entry * |
| ble_ll_resolv_list_find(const uint8_t *addr, uint8_t addr_type) |
| { |
| int i; |
| struct ble_ll_resolv_entry *rl; |
| |
| rl = &g_ble_ll_resolv_list[0]; |
| for (i = 0; i < g_ble_ll_resolv_data.rl_cnt; ++i) { |
| if ((rl->rl_addr_type == addr_type) && |
| (!memcmp(&rl->rl_identity_addr[0], addr, BLE_DEV_ADDR_LEN))) { |
| return rl; |
| } |
| ++rl; |
| } |
| |
| return NULL; |
| } |
| |
| /** |
| * Add a device to the resolving list |
| * |
| * @return int |
| */ |
| int |
| ble_ll_resolv_list_add(const uint8_t *cmdbuf, uint8_t len) |
| { |
| const struct ble_hci_le_add_resolv_list_cp *cmd = (const void *) cmdbuf; |
| struct ble_ll_resolv_entry *rl; |
| int rc = BLE_ERR_SUCCESS; |
| |
| if (len != sizeof(*cmd)) { |
| return BLE_ERR_INV_HCI_CMD_PARMS; |
| } |
| |
| /* Must be in proper state */ |
| if (!ble_ll_resolv_list_chg_allowed()) { |
| return BLE_ERR_CMD_DISALLOWED; |
| } |
| |
| /* Check if we have any open entries */ |
| if (g_ble_ll_resolv_data.rl_cnt >= g_ble_ll_resolv_data.rl_size) { |
| return BLE_ERR_MEM_CAPACITY; |
| } |
| |
| /* spec is not clear on how to handle this but make sure host is aware |
| * that new keys are not used in that case |
| */ |
| if (ble_ll_is_on_resolv_list(cmd->peer_id_addr, cmd->peer_addr_type)) { |
| return BLE_ERR_INV_HCI_CMD_PARMS; |
| } |
| |
| /* we keep this sorted in a way that entries with peer_irk are first */ |
| if (ble_ll_resolv_irk_nonzero(cmd->peer_irk)) { |
| memmove(&g_ble_ll_resolv_list[g_ble_ll_resolv_data.rl_cnt_hw + 1], |
| &g_ble_ll_resolv_list[g_ble_ll_resolv_data.rl_cnt_hw], |
| (g_ble_ll_resolv_data.rl_cnt - g_ble_ll_resolv_data.rl_cnt_hw) * |
| sizeof(g_ble_ll_resolv_list[0])); |
| rl = &g_ble_ll_resolv_list[g_ble_ll_resolv_data.rl_cnt_hw]; |
| } else { |
| rl = &g_ble_ll_resolv_list[g_ble_ll_resolv_data.rl_cnt]; |
| } |
| |
| memset (rl, 0, sizeof(*rl)); |
| rl->rl_addr_type = cmd->peer_addr_type; |
| memcpy(rl->rl_identity_addr, cmd->peer_id_addr, BLE_DEV_ADDR_LEN); |
| |
| if (ble_ll_resolv_irk_nonzero(cmd->peer_irk)) { |
| swap_buf(rl->rl_peer_irk, cmd->peer_irk, 16); |
| rl->rl_has_peer = 1; |
| |
| /* generate peer RPA now, those will be updated by timer when |
| * resolution is enabled |
| */ |
| ble_ll_resolv_gen_priv_addr(rl, 0); |
| } |
| |
| if (ble_ll_resolv_irk_nonzero(cmd->local_irk)) { |
| swap_buf(rl->rl_local_irk, cmd->local_irk, 16); |
| rl->rl_has_local = 1; |
| |
| /* generate local RPA now, those will be updated by timer when |
| * resolution is enabled |
| */ |
| ble_ll_resolv_gen_priv_addr(rl, 1); |
| } |
| |
| /* By default use privacy network mode */ |
| rl->rl_priv_mode = BLE_HCI_PRIVACY_NETWORK; |
| |
| /* Add peers IRKs to HW resolving list. Should always succeed since we |
| * already checked if there is room for it. |
| */ |
| if (rl->rl_has_peer) { |
| rc = ble_hw_resolv_list_add(rl->rl_peer_irk); |
| BLE_LL_ASSERT(rc == BLE_ERR_SUCCESS); |
| g_ble_ll_resolv_data.rl_cnt_hw++; |
| } |
| |
| g_ble_ll_resolv_data.rl_cnt++; |
| |
| /* start RPA timer if this was first element added to RL */ |
| if (g_ble_ll_resolv_data.rl_cnt == 1) { |
| ble_npl_callout_reset(&g_ble_ll_resolv_data.rpa_timer, |
| g_ble_ll_resolv_data.rpa_tmo); |
| } |
| |
| return rc; |
| } |
| |
| /** |
| * Remove a device from the resolving list |
| * |
| * @param cmdbuf |
| * |
| * @return int 0: success, BLE error code otherwise |
| */ |
| int |
| ble_ll_resolv_list_rmv(const uint8_t *cmdbuf, uint8_t len) |
| { |
| const struct ble_hci_le_rmv_resolve_list_cp *cmd = (const void *) cmdbuf; |
| int position; |
| |
| if (len != sizeof(*cmd)) { |
| return BLE_ERR_INV_HCI_CMD_PARMS; |
| } |
| |
| /* Must be in proper state */ |
| if (!ble_ll_resolv_list_chg_allowed()) { |
| return BLE_ERR_CMD_DISALLOWED; |
| } |
| |
| /* Remove from IRK records */ |
| position = ble_ll_is_on_resolv_list(cmd->peer_id_addr, cmd->peer_addr_type); |
| if (position) { |
| BLE_LL_ASSERT(position <= g_ble_ll_resolv_data.rl_cnt); |
| |
| memmove(&g_ble_ll_resolv_list[position - 1], |
| &g_ble_ll_resolv_list[position], |
| (g_ble_ll_resolv_data.rl_cnt - position) * |
| sizeof(g_ble_ll_resolv_list[0])); |
| g_ble_ll_resolv_data.rl_cnt--; |
| |
| /* Remove from HW list */ |
| if (position <= g_ble_ll_resolv_data.rl_cnt_hw) { |
| ble_hw_resolv_list_rmv(position - 1); |
| g_ble_ll_resolv_data.rl_cnt_hw--; |
| } |
| |
| /* stop RPA timer if list is empty */ |
| if (g_ble_ll_resolv_data.rl_cnt == 0) { |
| ble_npl_callout_stop(&g_ble_ll_resolv_data.rpa_timer); |
| } |
| |
| return BLE_ERR_SUCCESS; |
| } |
| |
| return BLE_ERR_UNK_CONN_ID; |
| } |
| |
| /** |
| * Called to enable or disable address resolution in the controller |
| * |
| * @param cmdbuf |
| * |
| * @return int |
| */ |
| int |
| ble_ll_resolv_enable_cmd(const uint8_t *cmdbuf, uint8_t len) |
| { |
| const struct ble_hci_le_set_addr_res_en_cp *cmd = (const void *) cmdbuf; |
| |
| if (len != sizeof(*cmd)) { |
| return BLE_ERR_INV_HCI_CMD_PARMS; |
| } |
| |
| if (ble_ll_is_busy(BLE_LL_BUSY_EXCLUDE_CONNECTIONS)) { |
| return BLE_ERR_CMD_DISALLOWED; |
| |
| } |
| |
| if (cmd->enable > 1) { |
| return BLE_ERR_INV_HCI_CMD_PARMS; |
| } |
| |
| g_ble_ll_resolv_data.addr_res_enabled = cmd->enable; |
| |
| return BLE_ERR_SUCCESS; |
| } |
| |
| int |
| ble_ll_resolv_peer_addr_rd(const uint8_t *cmdbuf, uint8_t len, |
| uint8_t *rspbuf, uint8_t *rsplen) |
| { |
| const struct ble_hci_le_rd_peer_resolv_addr_cp *cmd = (const void *) cmdbuf; |
| struct ble_hci_le_rd_peer_resolv_addr_rp *rsp = (void *) rspbuf; |
| struct ble_ll_resolv_entry *rl; |
| int rc; |
| |
| if (len != sizeof(*cmd)) { |
| return BLE_ERR_INV_HCI_CMD_PARMS; |
| } |
| |
| rl = ble_ll_resolv_list_find(cmd->peer_id_addr, cmd->peer_addr_type); |
| if (rl) { |
| memcpy(rsp->rpa, rl->rl_peer_rpa, BLE_DEV_ADDR_LEN); |
| rc = BLE_ERR_SUCCESS; |
| } else { |
| memset(rsp->rpa, 0, BLE_DEV_ADDR_LEN); |
| rc = BLE_ERR_UNK_CONN_ID; |
| } |
| |
| *rsplen = sizeof(*rsp); |
| return rc; |
| } |
| |
| int |
| ble_ll_resolv_local_addr_rd(const uint8_t *cmdbuf, uint8_t len, |
| uint8_t *rspbuf, uint8_t *rsplen) |
| { |
| const struct ble_hci_le_rd_local_resolv_addr_cp *cmd = (const void *) cmdbuf; |
| struct ble_hci_le_rd_local_resolv_addr_rp *rsp = (void *) rspbuf; |
| struct ble_ll_resolv_entry *rl; |
| int rc; |
| |
| if (len != sizeof(*cmd)) { |
| return BLE_ERR_INV_HCI_CMD_PARMS; |
| } |
| |
| rl = ble_ll_resolv_list_find(cmd->peer_id_addr, cmd->peer_addr_type); |
| if (rl) { |
| memcpy(rsp->rpa, rl->rl_local_rpa, BLE_DEV_ADDR_LEN); |
| rc = BLE_ERR_SUCCESS; |
| } else { |
| memset(rsp->rpa, 0, BLE_DEV_ADDR_LEN); |
| rc = BLE_ERR_UNK_CONN_ID; |
| } |
| |
| *rsplen = sizeof(*rsp); |
| return rc; |
| } |
| |
| /** |
| * Set the resolvable private address timeout. |
| * |
| * @param cmdbuf |
| * |
| * @return int |
| */ |
| int |
| ble_ll_resolv_set_rpa_tmo(const uint8_t *cmdbuf, uint8_t len) |
| { |
| const struct ble_hci_le_set_rpa_tmo_cp *cmd = (const void *)cmdbuf; |
| uint16_t tmo_secs; |
| |
| if (len != sizeof(*cmd)) { |
| return BLE_ERR_INV_HCI_CMD_PARMS; |
| } |
| |
| tmo_secs = le16toh(cmd->rpa_timeout); |
| if (!((tmo_secs > 0) && (tmo_secs <= 0xA1B8))) { |
| return BLE_ERR_INV_HCI_CMD_PARMS; |
| } |
| |
| g_ble_ll_resolv_data.rpa_tmo = ble_npl_time_ms_to_ticks32(tmo_secs * 1000); |
| |
| /* restart timer if there is something on RL */ |
| if (g_ble_ll_resolv_data.rl_cnt) { |
| ble_npl_callout_reset(&g_ble_ll_resolv_data.rpa_timer, |
| g_ble_ll_resolv_data.rpa_tmo); |
| } |
| |
| return BLE_ERR_SUCCESS; |
| } |
| |
| int |
| ble_ll_resolve_set_priv_mode(const uint8_t *cmdbuf, uint8_t len) |
| { |
| const struct ble_hci_le_set_privacy_mode_cp *cmd = (const void *) cmdbuf; |
| struct ble_ll_resolv_entry *rl; |
| |
| if (ble_ll_is_busy(BLE_LL_BUSY_EXCLUDE_CONNECTIONS)) { |
| return BLE_ERR_CMD_DISALLOWED; |
| } |
| |
| if (len != sizeof(*cmd)) { |
| return BLE_ERR_INV_HCI_CMD_PARMS; |
| } |
| |
| rl = ble_ll_resolv_list_find(cmd->peer_id_addr, cmd->peer_id_addr_type); |
| if (!rl) { |
| return BLE_ERR_UNK_CONN_ID; |
| } |
| |
| if (cmd->mode > BLE_HCI_PRIVACY_DEVICE) { |
| return BLE_ERR_INV_HCI_CMD_PARMS; |
| } |
| |
| rl->rl_priv_mode = cmd->mode; |
| |
| return BLE_ERR_SUCCESS; |
| } |
| |
| /** |
| * Returns the Resolvable Private address timeout, in os ticks |
| * |
| * |
| * @return uint32_t |
| */ |
| uint32_t |
| ble_ll_resolv_get_rpa_tmo(void) |
| { |
| return g_ble_ll_resolv_data.rpa_tmo; |
| } |
| |
| void |
| ble_ll_resolv_get_priv_addr(struct ble_ll_resolv_entry *rl, int local, |
| uint8_t *addr) |
| { |
| os_sr_t sr; |
| |
| BLE_LL_ASSERT(rl != NULL); |
| BLE_LL_ASSERT(addr != NULL); |
| |
| OS_ENTER_CRITICAL(sr); |
| if (local) { |
| BLE_LL_ASSERT(rl->rl_has_local); |
| memcpy(addr, rl->rl_local_rpa, BLE_DEV_ADDR_LEN); |
| } else { |
| BLE_LL_ASSERT(rl->rl_has_peer); |
| memcpy(addr, rl->rl_peer_rpa, BLE_DEV_ADDR_LEN); |
| } |
| |
| OS_EXIT_CRITICAL(sr); |
| } |
| |
| void |
| ble_ll_resolv_set_peer_rpa(int index, uint8_t *rpa) |
| { |
| os_sr_t sr; |
| struct ble_ll_resolv_entry *rl; |
| |
| OS_ENTER_CRITICAL(sr); |
| rl = &g_ble_ll_resolv_list[index]; |
| memcpy(rl->rl_peer_rpa, rpa, BLE_DEV_ADDR_LEN); |
| OS_EXIT_CRITICAL(sr); |
| } |
| |
| void |
| ble_ll_resolv_set_local_rpa(int index, uint8_t *rpa) |
| { |
| os_sr_t sr; |
| struct ble_ll_resolv_entry *rl; |
| |
| OS_ENTER_CRITICAL(sr); |
| rl = &g_ble_ll_resolv_list[index]; |
| memcpy(rl->rl_local_rpa, rpa, BLE_DEV_ADDR_LEN); |
| OS_EXIT_CRITICAL(sr); |
| } |
| |
| /** |
| * Generate a resolvable private address. |
| * |
| * @param addr |
| * @param addr_type |
| * @param rpa |
| * |
| * @return int |
| */ |
| int |
| ble_ll_resolv_gen_rpa(uint8_t *addr, uint8_t addr_type, uint8_t *rpa, int local) |
| { |
| struct ble_ll_resolv_entry *rl; |
| |
| rl = ble_ll_resolv_list_find(addr, addr_type); |
| if (rl) { |
| if ((local && rl->rl_has_local) || (!local && rl->rl_has_peer)) { |
| ble_ll_resolv_get_priv_addr(rl, local, rpa); |
| return 1; |
| } |
| } |
| |
| return 0; |
| } |
| |
| #if MYNEWT_VAL(BLE_LL_HCI_VS_LOCAL_IRK) |
| int |
| ble_ll_resolv_local_irk_set(uint8_t own_addr_type, const uint8_t *irk) |
| { |
| struct local_irk_data *irk_data; |
| int i; |
| |
| if (own_addr_type >= 2) { |
| return -1; |
| } |
| |
| irk_data = &g_local_irk[own_addr_type]; |
| |
| memcpy(irk_data->irk, irk, 16); |
| |
| irk_data->is_set = 0; |
| |
| for (i = 0; i < 16; i++) { |
| if (irk[i]) { |
| irk_data->is_set = 1; |
| break; |
| } |
| } |
| |
| if (irk_data->is_set) { |
| generate_rpa(irk_data->irk, irk_data->rpa); |
| } |
| |
| return 0; |
| } |
| |
| int |
| ble_ll_resolv_local_rpa_get(uint8_t own_addr_type, uint8_t *rpa) |
| { |
| struct local_irk_data *irk_data; |
| |
| if (own_addr_type >= 2) { |
| return -1; |
| } |
| |
| irk_data = &g_local_irk[own_addr_type]; |
| |
| if (!irk_data->is_set) { |
| return -1; |
| } |
| |
| memcpy(rpa, irk_data->rpa, 6); |
| |
| return 0; |
| } |
| #endif |
| |
| /** |
| * Resolve a Resolvable Private Address |
| * |
| * @param rpa |
| * @param index |
| * |
| * @return int |
| */ |
| int |
| ble_ll_resolv_rpa(const uint8_t *rpa, const uint8_t *irk) |
| { |
| int rc; |
| const uint32_t *irk32; |
| uint32_t *key32; |
| uint32_t *pt32; |
| struct ble_encryption_block ecb; |
| |
| irk32 = (const uint32_t *)irk; |
| key32 = (uint32_t *)&ecb.key[0]; |
| |
| key32[0] = irk32[0]; |
| key32[1] = irk32[1]; |
| key32[2] = irk32[2]; |
| key32[3] = irk32[3]; |
| |
| pt32 = (uint32_t *)&ecb.plain_text[0]; |
| pt32[0] = 0; |
| pt32[1] = 0; |
| pt32[2] = 0; |
| pt32[3] = 0; |
| |
| ecb.plain_text[15] = rpa[3]; |
| ecb.plain_text[14] = rpa[4]; |
| ecb.plain_text[13] = rpa[5]; |
| |
| ble_hw_encrypt_block(&ecb); |
| if ((ecb.cipher_text[15] == rpa[0]) && (ecb.cipher_text[14] == rpa[1]) && |
| (ecb.cipher_text[13] == rpa[2])) { |
| rc = 1; |
| } else { |
| rc = 0; |
| } |
| |
| return rc; |
| } |
| |
| int |
| ble_ll_resolv_peer_rpa_any(const uint8_t *rpa) |
| { |
| int i; |
| |
| for (i = 0; i < g_ble_ll_resolv_data.rl_cnt_hw; i++) { |
| if (ble_ll_resolv_rpa(rpa, g_ble_ll_resolv_list[i].rl_peer_irk)) { |
| return i; |
| } |
| } |
| |
| return -1; |
| } |
| |
| /** |
| * Returns whether or not address resolution is enabled. |
| * |
| * @return uint8_t |
| */ |
| uint8_t |
| ble_ll_resolv_enabled(void) |
| { |
| return g_ble_ll_resolv_data.addr_res_enabled; |
| } |
| |
| /** |
| * Called to reset private address resolution module. |
| */ |
| void |
| ble_ll_resolv_list_reset(void) |
| { |
| g_ble_ll_resolv_data.addr_res_enabled = 0; |
| ble_npl_callout_stop(&g_ble_ll_resolv_data.rpa_timer); |
| ble_ll_resolv_list_clr(); |
| ble_ll_resolv_init(); |
| } |
| |
| void |
| ble_ll_resolv_init(void) |
| { |
| uint8_t hw_size; |
| |
| /* Default is 15 minutes */ |
| g_ble_ll_resolv_data.rpa_tmo = ble_npl_time_ms_to_ticks32(15 * 60 * 1000); |
| |
| hw_size = ble_hw_resolv_list_size(); |
| if (hw_size > MYNEWT_VAL(BLE_LL_RESOLV_LIST_SIZE)) { |
| hw_size = MYNEWT_VAL(BLE_LL_RESOLV_LIST_SIZE); |
| } |
| g_ble_ll_resolv_data.rl_size = hw_size; |
| |
| ble_npl_callout_init(&g_ble_ll_resolv_data.rpa_timer, |
| &g_ble_ll_data.ll_evq, |
| ble_ll_resolv_rpa_timer_cb, |
| NULL); |
| |
| #if MYNEWT_VAL(BLE_LL_HCI_VS_LOCAL_IRK) |
| memset(&g_local_irk, 0, sizeof(g_local_irk)); |
| #endif |
| } |
| |
| #endif /* if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) */ |
| |