controller/src/ble_ll_resolv.c

/*
 * 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_hw.h"
#include "ble_ll_conn_priv.h"

#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
struct ble_ll_resolv_data
{
    uint8_t addr_res_enabled;
    uint8_t rl_size;
    uint8_t rl_cnt;
    uint32_t rpa_tmo;
    struct ble_npl_callout rpa_timer;
};
struct ble_ll_resolv_data g_ble_ll_resolv_data;

struct ble_ll_resolv_entry g_ble_ll_resolv_list[MYNEWT_VAL(BLE_LL_RESOLV_LIST_SIZE)];

static int
ble_ll_is_controller_busy(void)
{
    return ble_ll_adv_enabled() || ble_ll_scan_enabled() ||
           g_ble_ll_conn_create_sm;
}
/**
 * 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_controller_busy()) {
        rc = 0;
    } else {
        rc = 1;
    }
    return rc;
}


/**
 * 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 *prand;
    struct ble_encryption_block ecb;
    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;
    }

    /* Get prand */
    prand = addr + 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);

    addr[0] = ecb.cipher_text[15];
    addr[1] = ecb.cipher_text[14];
    addr[2] = ecb.cipher_text[13];
}

/**
 * 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;

    rl = &g_ble_ll_resolv_list[0];
    for (i = 0; i < g_ble_ll_resolv_data.rl_cnt; ++i) {
        OS_ENTER_CRITICAL(sr);
        ble_ll_resolv_gen_priv_addr(rl, 1);
        OS_EXIT_CRITICAL(sr);

        OS_ENTER_CRITICAL(sr);
        ble_ll_resolv_gen_priv_addr(rl, 0);
        OS_EXIT_CRITICAL(sr);
        ++rl;
    }
    ble_npl_callout_reset(&g_ble_ll_resolv_data.rpa_timer,
                     (int32_t)g_ble_ll_resolv_data.rpa_tmo);

    ble_ll_adv_rpa_timeout();
}

/**
 * Called to determine if the IRK is all zero.
 *
 * @param irk
 *
 * @return int 0: IRK is zero . 1: IRK has non-zero value.
 */
int
ble_ll_resolv_irk_nonzero(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 = 0;
    ble_hw_resolv_list_clear();

    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)
{
    rspbuf[0] = g_ble_ll_resolv_data.rl_size;
    *rsplen = 1;
    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(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(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(uint8_t *cmdbuf)
{
    int rc;
    uint8_t addr_type;
    uint8_t *ident_addr;
    struct ble_ll_resolv_entry *rl;

    /* 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;
    }

    addr_type = cmdbuf[0];
    ident_addr = cmdbuf + 1;

    /* 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(ident_addr, addr_type)) {
        return BLE_ERR_INV_HCI_CMD_PARMS;
    }

    rl = &g_ble_ll_resolv_list[g_ble_ll_resolv_data.rl_cnt];
    memset (rl, 0, sizeof(*rl));

    rl->rl_addr_type = addr_type;
    memcpy(&rl->rl_identity_addr[0], ident_addr, BLE_DEV_ADDR_LEN);
    swap_buf(rl->rl_peer_irk, cmdbuf + 7, 16);
    swap_buf(rl->rl_local_irk, cmdbuf + 23, 16);

    /* By default use privacy network mode */
    rl->rl_priv_mode = BLE_HCI_PRIVACY_NETWORK;

    /* Add peer IRK to HW resolving list. Should always succeed since we
     * already checked if there is room for it.
     */
    rc = ble_hw_resolv_list_add(rl->rl_peer_irk);
    BLE_LL_ASSERT (rc == BLE_ERR_SUCCESS);

    /* generate a local and peer RPAs now, those will be updated by timer
     * when resolution is enabled
     */
    ble_ll_resolv_gen_priv_addr(rl, 1);
    ble_ll_resolv_gen_priv_addr(rl, 0);
    ++g_ble_ll_resolv_data.rl_cnt;

    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(uint8_t *cmdbuf)
{
    int position;
    uint8_t addr_type;
    uint8_t *ident_addr;

    /* Must be in proper state */
    if (!ble_ll_resolv_list_chg_allowed()) {
        return BLE_ERR_CMD_DISALLOWED;
    }

    addr_type = cmdbuf[0];
    ident_addr = cmdbuf + 1;

    /* Remove from IRK records */
    position = ble_ll_is_on_resolv_list(ident_addr, 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);
        --g_ble_ll_resolv_data.rl_cnt;

        /* Remove from HW list */
        ble_hw_resolv_list_rmv(position - 1);
        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(uint8_t *cmdbuf)
{
    int rc;
    int32_t tmo;
    uint8_t enabled;

    if (ble_ll_is_controller_busy()) {
        rc = BLE_ERR_CMD_DISALLOWED;
    } else {
        enabled = cmdbuf[0];
        if (enabled <= 1) {
            /* If we change state, we need to disable/enable the RPA timer */
            if ((enabled ^ g_ble_ll_resolv_data.addr_res_enabled) != 0) {
                if (enabled) {
                    tmo = (int32_t)g_ble_ll_resolv_data.rpa_tmo;
                    ble_npl_callout_reset(&g_ble_ll_resolv_data.rpa_timer, tmo);
                } else {
                    ble_npl_callout_stop(&g_ble_ll_resolv_data.rpa_timer);
                }
                g_ble_ll_resolv_data.addr_res_enabled = enabled;
            }
            rc = BLE_ERR_SUCCESS;
        } else {
            rc = BLE_ERR_INV_HCI_CMD_PARMS;
        }
    }

    return rc;
}

int
ble_ll_resolv_peer_addr_rd(uint8_t *cmdbuf, uint8_t *rspbuf, uint8_t *rsplen)
{
    struct ble_ll_resolv_entry *rl;
    uint8_t addr_type;
    uint8_t *ident_addr;
    int rc;

    addr_type = cmdbuf[0];
    ident_addr = cmdbuf + 1;

    rl = ble_ll_resolv_list_find(ident_addr, addr_type);
    if (rl) {
        memcpy(rspbuf, rl->rl_peer_rpa, BLE_DEV_ADDR_LEN);
        rc = BLE_ERR_SUCCESS;
    } else {
        memset(rspbuf, 0, BLE_DEV_ADDR_LEN);
        rc = BLE_ERR_UNK_CONN_ID;
    }

    *rsplen = BLE_DEV_ADDR_LEN;
    return rc;
}

int
ble_ll_resolv_local_addr_rd(uint8_t *cmdbuf, uint8_t *rspbuf, uint8_t *rsplen)
{
    struct ble_ll_resolv_entry *rl;
    uint8_t addr_type;
    uint8_t *ident_addr;
    int rc;

    addr_type = cmdbuf[0];
    ident_addr = cmdbuf + 1;

    rl = ble_ll_resolv_list_find(ident_addr, addr_type);
    if (rl) {
        memcpy(rspbuf, rl->rl_local_rpa, BLE_DEV_ADDR_LEN);
        rc = BLE_ERR_SUCCESS;
    } else {
        memset(rspbuf, 0, BLE_DEV_ADDR_LEN);
        rc = BLE_ERR_UNK_CONN_ID;
    }

    *rsplen = BLE_DEV_ADDR_LEN;
    return rc;
}

/**
 * Set the resolvable private address timeout.
 *
 * @param cmdbuf
 *
 * @return int
 */
int
ble_ll_resolv_set_rpa_tmo(uint8_t *cmdbuf)
{
    uint16_t tmo_secs;

    tmo_secs = get_le16(cmdbuf);
    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);

    /* If resolving is not enabled, we are done here. */
    if (!ble_ll_resolv_enabled()) {
        return BLE_ERR_SUCCESS;
    }

    /* Reset timeout if resolving is enabled */
    ble_npl_callout_reset(&g_ble_ll_resolv_data.rpa_timer,
                     (int32_t)g_ble_ll_resolv_data.rpa_tmo);

    return BLE_ERR_SUCCESS;
}

int
ble_ll_resolve_set_priv_mode(uint8_t *cmdbuf)
{
    struct ble_ll_resolv_entry *rl;

    if (ble_ll_is_controller_busy()) {
        return BLE_ERR_CMD_DISALLOWED;
    }

    /* cmdbuf = addr_type(0) | addr(6) | priv_mode(1) */
    rl = ble_ll_resolv_list_find(&cmdbuf[1], cmdbuf[0]);
    if (!rl) {
        return BLE_ERR_UNK_CONN_ID;
    }

    if (cmdbuf[7] > BLE_HCI_PRIVACY_DEVICE) {
        return BLE_ERR_INV_HCI_CMD_PARMS;
    }

    rl->rl_priv_mode = cmdbuf[7];
    return 0;
}

/**
 * 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) {
        memcpy(addr, rl->rl_local_rpa, BLE_DEV_ADDR_LEN);
    } else {
        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);
}

/**
 * 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)
{
    int rc;
    uint8_t *irk;
    struct ble_ll_resolv_entry *rl;

    rc = 0;
    rl = ble_ll_resolv_list_find(addr, addr_type);
    if (rl) {
        if (local) {
            irk = rl->rl_local_irk;
        } else {
            irk = rl->rl_peer_irk;
        }
        if (ble_ll_resolv_irk_nonzero(irk)) {
            ble_ll_resolv_get_priv_addr(rl, local, rpa);
            rc = 1;
        }
    }

    return rc;
}

/**
 * Resolve a Resolvable Private Address
 *
 * @param rpa
 * @param index
 *
 * @return int
 */
int
ble_ll_resolv_rpa(uint8_t *rpa, uint8_t *irk)
{
    int rc;
    uint32_t *irk32;
    uint32_t *key32;
    uint32_t *pt32;
    struct ble_encryption_block ecb;

    irk32 = (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;
}

/**
 * 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);
}

#endif  /* if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1 */