diff --git a/nimble/host/include/host/ble_gap.h b/nimble/host/include/host/ble_gap.h index e4186b274..dc96eb75b 100644 --- a/nimble/host/include/host/ble_gap.h +++ b/nimble/host/include/host/ble_gap.h @@ -23,6 +23,8 @@ #include #include "host/ble_hs.h" #include "host/ble_hs_adv.h" +#include "syscfg/syscfg.h" + #ifdef __cplusplus extern "C" { #endif @@ -698,6 +700,11 @@ int ble_gap_set_prefered_default_le_phy(uint8_t tx_phys_mask, #define BLE_GAP_LE_PHY_CODED_S8 2 int ble_gap_set_prefered_le_phy(uint16_t conn_handle, uint8_t tx_phys_mask, uint8_t rx_phys_mask, uint16_t phy_opts); + +#if MYNEWT_VAL(BLE_MESH) +int ble_gap_mesh_cb_register(ble_gap_event_fn *cb, void *cb_arg); +#endif + #ifdef __cplusplus } #endif diff --git a/nimble/host/include/host/ble_hs_adv.h b/nimble/host/include/host/ble_hs_adv.h index 23c21559b..c0b46d27e 100644 --- a/nimble/host/include/host/ble_hs_adv.h +++ b/nimble/host/include/host/ble_hs_adv.h @@ -126,6 +126,9 @@ struct ble_hs_adv_fields { #define BLE_HS_ADV_TYPE_SVC_DATA_UUID32 0x20 #define BLE_HS_ADV_TYPE_SVC_DATA_UUID128 0x21 #define BLE_HS_ADV_TYPE_URI 0x24 +#define BLE_HS_ADV_TYPE_MESH_PROV 0x29 +#define BLE_HS_ADV_TYPE_MESH_MESSAGE 0x2a +#define BLE_HS_ADV_TYPE_MESH_BEACON 0x2b #define BLE_HS_ADV_TYPE_MFG_DATA 0xff #define BLE_HS_ADV_FLAGS_LEN 1 diff --git a/nimble/host/mesh/include/mesh/atomic.h b/nimble/host/mesh/include/mesh/atomic.h new file mode 100644 index 000000000..9af3344f8 --- /dev/null +++ b/nimble/host/mesh/include/mesh/atomic.h @@ -0,0 +1,386 @@ +/* atomic operations */ + +/* + * Copyright (c) 1997-2015, Wind River Systems, Inc. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef __ATOMIC_H__ +#define __ATOMIC_H__ + +#ifdef __cplusplus +extern "C" +{ +#endif + +typedef int atomic_t; +typedef atomic_t atomic_val_t; + +/** + * @defgroup atomic_apis Atomic Services APIs + * @ingroup kernel_apis + * @{ + */ + +/** + * @brief Atomic compare-and-set. + * + * This routine performs an atomic compare-and-set on @a target. If the current + * value of @a target equals @a old_value, @a target is set to @a new_value. + * If the current value of @a target does not equal @a old_value, @a target + * is left unchanged. + * + * @param target Address of atomic variable. + * @param old_value Original value to compare against. + * @param new_value New value to store. + * @return 1 if @a new_value is written, 0 otherwise. + */ +static inline int atomic_cas(atomic_t *target, atomic_val_t old_value, + atomic_val_t new_value) +{ + return __atomic_compare_exchange_n(target, &old_value, new_value, + 0, __ATOMIC_SEQ_CST, + __ATOMIC_SEQ_CST); +} + +/** + * + * @brief Atomic addition. + * + * This routine performs an atomic addition on @a target. + * + * @param target Address of atomic variable. + * @param value Value to add. + * + * @return Previous value of @a target. + */ +static inline atomic_val_t atomic_add(atomic_t *target, atomic_val_t value) +{ + return __atomic_fetch_add(target, value, __ATOMIC_SEQ_CST); +} + +/** + * + * @brief Atomic subtraction. + * + * This routine performs an atomic subtraction on @a target. + * + * @param target Address of atomic variable. + * @param value Value to subtract. + * + * @return Previous value of @a target. + */ + +static inline atomic_val_t atomic_sub(atomic_t *target, atomic_val_t value) +{ + return __atomic_fetch_sub(target, value, __ATOMIC_SEQ_CST); +} + +/** + * + * @brief Atomic increment. + * + * This routine performs an atomic increment by 1 on @a target. + * + * @param target Address of atomic variable. + * + * @return Previous value of @a target. + */ + +static inline atomic_val_t atomic_inc(atomic_t *target) +{ + return atomic_add(target, 1); +} + +/** + * + * @brief Atomic decrement. + * + * This routine performs an atomic decrement by 1 on @a target. + * + * @param target Address of atomic variable. + * + * @return Previous value of @a target. + */ + +static inline atomic_val_t atomic_dec(atomic_t *target) +{ + return atomic_sub(target, 1); +} + +/** + * + * @brief Atomic get. + * + * This routine performs an atomic read on @a target. + * + * @param target Address of atomic variable. + * + * @return Value of @a target. + */ + +static inline atomic_val_t atomic_get(const atomic_t *target) +{ + return __atomic_load_n(target, __ATOMIC_SEQ_CST); +} + +/** + * + * @brief Atomic get-and-set. + * + * This routine atomically sets @a target to @a value and returns + * the previous value of @a target. + * + * @param target Address of atomic variable. + * @param value Value to write to @a target. + * + * @return Previous value of @a target. + */ + +static inline atomic_val_t atomic_set(atomic_t *target, atomic_val_t value) +{ + /* This builtin, as described by Intel, is not a traditional + * test-and-set operation, but rather an atomic exchange operation. It + * writes value into *ptr, and returns the previous contents of *ptr. + */ + return __atomic_exchange_n(target, value, __ATOMIC_SEQ_CST); +} + +/** + * + * @brief Atomic clear. + * + * This routine atomically sets @a target to zero and returns its previous + * value. (Hence, it is equivalent to atomic_set(target, 0).) + * + * @param target Address of atomic variable. + * + * @return Previous value of @a target. + */ + +static inline atomic_val_t atomic_clear(atomic_t *target) +{ + return atomic_set(target, 0); +} + +/** + * + * @brief Atomic bitwise inclusive OR. + * + * This routine atomically sets @a target to the bitwise inclusive OR of + * @a target and @a value. + * + * @param target Address of atomic variable. + * @param value Value to OR. + * + * @return Previous value of @a target. + */ + +static inline atomic_val_t atomic_or(atomic_t *target, atomic_val_t value) +{ + return __atomic_fetch_or(target, value, __ATOMIC_SEQ_CST); +} + +/** + * + * @brief Atomic bitwise exclusive OR (XOR). + * + * This routine atomically sets @a target to the bitwise exclusive OR (XOR) of + * @a target and @a value. + * + * @param target Address of atomic variable. + * @param value Value to XOR + * + * @return Previous value of @a target. + */ + +static inline atomic_val_t atomic_xor(atomic_t *target, atomic_val_t value) +{ + return __atomic_fetch_xor(target, value, __ATOMIC_SEQ_CST); +} + +/** + * + * @brief Atomic bitwise AND. + * + * This routine atomically sets @a target to the bitwise AND of @a target + * and @a value. + * + * @param target Address of atomic variable. + * @param value Value to AND. + * + * @return Previous value of @a target. + */ + +static inline atomic_val_t atomic_and(atomic_t *target, atomic_val_t value) +{ + return __atomic_fetch_and(target, value, __ATOMIC_SEQ_CST); +} + +/** + * + * @brief Atomic bitwise NAND. + * + * This routine atomically sets @a target to the bitwise NAND of @a target + * and @a value. (This operation is equivalent to target = ~(target & value).) + * + * @param target Address of atomic variable. + * @param value Value to NAND. + * + * @return Previous value of @a target. + */ + +static inline atomic_val_t atomic_nand(atomic_t *target, atomic_val_t value) +{ + return __atomic_fetch_nand(target, value, __ATOMIC_SEQ_CST); +} + + /** + * @brief Initialize an atomic variable. + * + * This macro can be used to initialize an atomic variable. For example, + * @code atomic_t my_var = ATOMIC_INIT(75); @endcode + * + * @param i Value to assign to atomic variable. + */ +#define ATOMIC_INIT(i) (i) + + /** + * @cond INTERNAL_HIDDEN + */ + +#define ATOMIC_BITS (sizeof(atomic_val_t) * 8) +#define ATOMIC_MASK(bit) (1 << ((bit) & (ATOMIC_BITS - 1))) +#define ATOMIC_ELEM(addr, bit) ((addr) + ((bit) / ATOMIC_BITS)) + + /** + * INTERNAL_HIDDEN @endcond + */ + + /** + * @brief Define an array of atomic variables. + * + * This macro defines an array of atomic variables containing at least + * @a num_bits bits. + * + * @note + * If used from file scope, the bits of the array are initialized to zero; + * if used from within a function, the bits are left uninitialized. + * + * @param name Name of array of atomic variables. + * @param num_bits Number of bits needed. + */ +#define ATOMIC_DEFINE(name, num_bits) \ + atomic_t name[1 + ((num_bits) - 1) / ATOMIC_BITS] + + /** + * @brief Atomically test a bit. + * + * This routine tests whether bit number @a bit of @a target is set or not. + * The target may be a single atomic variable or an array of them. + * + * @param target Address of atomic variable or array. + * @param bit Bit number (starting from 0). + * + * @return 1 if the bit was set, 0 if it wasn't. + */ + static inline int + atomic_test_bit(const atomic_t *target, int bit) + { + atomic_val_t val = atomic_get(ATOMIC_ELEM(target, bit)); + + return (1 & (val >> (bit & (ATOMIC_BITS - 1)))); + } + + /** + * @brief Atomically test and clear a bit. + * + * Atomically clear bit number @a bit of @a target and return its old value. + * The target may be a single atomic variable or an array of them. + * + * @param target Address of atomic variable or array. + * @param bit Bit number (starting from 0). + * + * @return 1 if the bit was set, 0 if it wasn't. + */ + static inline int + atomic_test_and_clear_bit(atomic_t *target, int bit) + { + atomic_val_t mask = ATOMIC_MASK(bit); + atomic_val_t old; + + old = atomic_and(ATOMIC_ELEM(target, bit), ~mask); + + return (old & mask) != 0; + } + + /** + * @brief Atomically set a bit. + * + * Atomically set bit number @a bit of @a target and return its old value. + * The target may be a single atomic variable or an array of them. + * + * @param target Address of atomic variable or array. + * @param bit Bit number (starting from 0). + * + * @return 1 if the bit was set, 0 if it wasn't. + */ + static inline int + atomic_test_and_set_bit(atomic_t *target, int bit) + { + atomic_val_t mask = ATOMIC_MASK(bit); + atomic_val_t old; + + old = atomic_or(ATOMIC_ELEM(target, bit), mask); + + return (old & mask) != 0; + } + + /** + * @brief Atomically clear a bit. + * + * Atomically clear bit number @a bit of @a target. + * The target may be a single atomic variable or an array of them. + * + * @param target Address of atomic variable or array. + * @param bit Bit number (starting from 0). + * + * @return N/A + */ + static inline void + atomic_clear_bit(atomic_t *target, int bit) + { + atomic_val_t mask = ATOMIC_MASK(bit); + + atomic_and(ATOMIC_ELEM(target, bit), ~mask); + } + + /** + * @brief Atomically set a bit. + * + * Atomically set bit number @a bit of @a target. + * The target may be a single atomic variable or an array of them. + * + * @param target Address of atomic variable or array. + * @param bit Bit number (starting from 0). + * + * @return N/A + */ + static inline void + atomic_set_bit(atomic_t *target, int bit) + { + atomic_val_t mask = ATOMIC_MASK(bit); + + atomic_or(ATOMIC_ELEM(target, bit), mask); + } + +/** + * @} + */ + +#ifdef __cplusplus +} +#endif + +#endif /* __ATOMIC_H__ */ diff --git a/nimble/host/mesh/include/mesh/glue.h b/nimble/host/mesh/include/mesh/glue.h new file mode 100644 index 000000000..ad271161a --- /dev/null +++ b/nimble/host/mesh/include/mesh/glue.h @@ -0,0 +1,318 @@ +/* + * 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. + */ + +#ifndef _MESH_GLUE_ +#define _MESH_GLUE_ + +#include + +#include "syscfg/syscfg.h" + +#include "os/os_mbuf.h" +#include "os/os_mbuf.h" +#include "os/os_callout.h" +#include "os/os_eventq.h" + +#include "atomic.h" +#include "nimble/ble.h" +#include "host/ble_hs.h" +#include "host/ble_uuid.h" +#include "../src/ble_sm_priv.h" +#include "../src/ble_hs_hci_priv.h" + +#include "tinycrypt/aes.h" +#include "tinycrypt/constants.h" +#include "tinycrypt/utils.h" +#include "tinycrypt/cmac_mode.h" +#include "tinycrypt/ecc_dh.h" + +#define u8_t uint8_t +#define s8_t int8_t +#define u16_t uint16_t +#define u32_t uint32_t +#define u64_t uint64_t +#define s64_t int64_t +#define s32_t int32_t +typedef size_t ssize_t; + +/** @brief Helper to declare elements of bt_data arrays + * + * This macro is mainly for creating an array of struct bt_data + * elements which is then passed to bt_le_adv_start(). + * + * @param _type Type of advertising data field + * @param _data Pointer to the data field payload + * @param _data_len Number of bytes behind the _data pointer + */ +#define BT_DATA(_type, _data, _data_len) \ + { \ + .type = (_type), \ + .data_len = (_data_len), \ + .data = (const u8_t *)(_data), \ + } + +/** @brief Helper to declare elements of bt_data arrays + * + * This macro is mainly for creating an array of struct bt_data + * elements which is then passed to bt_le_adv_start(). + * + * @param _type Type of advertising data field + * @param _bytes Variable number of single-byte parameters + */ +#define BT_DATA_BYTES(_type, _bytes...) \ + BT_DATA(_type, ((u8_t []) { _bytes }), \ + sizeof((u8_t []) { _bytes })) + +/* EIR/AD data type definitions */ +#define BT_DATA_FLAGS 0x01 /* AD flags */ +#define BT_DATA_UUID16_SOME 0x02 /* 16-bit UUID, more available */ +#define BT_DATA_UUID16_ALL 0x03 /* 16-bit UUID, all listed */ +#define BT_DATA_UUID32_SOME 0x04 /* 32-bit UUID, more available */ +#define BT_DATA_UUID32_ALL 0x05 /* 32-bit UUID, all listed */ +#define BT_DATA_UUID128_SOME 0x06 /* 128-bit UUID, more available */ +#define BT_DATA_UUID128_ALL 0x07 /* 128-bit UUID, all listed */ +#define BT_DATA_NAME_SHORTENED 0x08 /* Shortened name */ +#define BT_DATA_NAME_COMPLETE 0x09 /* Complete name */ +#define BT_DATA_TX_POWER 0x0a /* Tx Power */ +#define BT_DATA_SOLICIT16 0x14 /* Solicit UUIDs, 16-bit */ +#define BT_DATA_SOLICIT128 0x15 /* Solicit UUIDs, 128-bit */ +#define BT_DATA_SVC_DATA16 0x16 /* Service data, 16-bit UUID */ +#define BT_DATA_GAP_APPEARANCE 0x19 /* GAP appearance */ +#define BT_DATA_SOLICIT32 0x1f /* Solicit UUIDs, 32-bit */ +#define BT_DATA_SVC_DATA32 0x20 /* Service data, 32-bit UUID */ +#define BT_DATA_SVC_DATA128 0x21 /* Service data, 128-bit UUID */ +#define BT_DATA_MESH_PROV 0x29 /* Mesh Provisioning PDU */ +#define BT_DATA_MESH_MESSAGE 0x2a /* Mesh Networking PDU */ +#define BT_DATA_MESH_BEACON 0x2b /* Mesh Beacon */ + +#define BT_DATA_MANUFACTURER_DATA 0xff /* Manufacturer Specific Data */ + +#define BT_LE_AD_LIMITED 0x01 /* Limited Discoverable */ +#define BT_LE_AD_GENERAL 0x02 /* General Discoverable */ +#define BT_LE_AD_NO_BREDR 0x04 /* BR/EDR not supported */ + +#define sys_put_be16(a,b) put_be16(b, a) +#define sys_put_le16(a,b) put_le16(b, a) +#define sys_put_be32(a,b) put_be32(b, a) +#define sys_get_be16(a) get_be16(a) +#define sys_get_le16(a) get_le16(a) +#define sys_get_be32(a) get_be32(a) +#define sys_cpu_to_be16(a) htobe16(a) +#define sys_cpu_to_be32(a) htobe32(a) +#define sys_be32_to_cpu(a) be32toh(a) +#define sys_be16_to_cpu(a) be16toh(a) +#define sys_le16_to_cpu(a) le16toh(a) + +#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0])) +#define CODE_UNREACHABLE __builtin_unreachable() +#define __ASSERT(code, str) \ + do { \ + if (!(code)) BT_ERR(str); \ + assert(code); \ + } while (0); + +#define __packed __attribute__((__packed__)) + +#define MSEC_PER_SEC (1000) +#define K_MSEC(ms) (ms) +#define K_SECONDS(s) K_MSEC((s) * MSEC_PER_SEC) +#define K_MINUTES(m) K_SECONDS((m) * 60) +#define K_HOURS(h) K_MINUTES((h) * 60) + +#define BT_GAP_ADV_FAST_INT_MIN_1 0x0030 /* 30 ms */ +#define BT_GAP_ADV_FAST_INT_MAX_1 0x0060 /* 60 ms */ +#define BT_GAP_ADV_FAST_INT_MIN_2 0x00a0 /* 100 ms */ +#define BT_GAP_ADV_FAST_INT_MAX_2 0x00f0 /* 150 ms */ +#define BT_GAP_ADV_SLOW_INT_MIN 0x0640 /* 1 s */ +#define BT_GAP_ADV_SLOW_INT_MAX 0x0780 /* 1.2 s */ + +#define BT_WARN(fmt, ...) BLE_HS_LOG(WARN, "%s: " fmt "\n", __func__, ## __VA_ARGS__); +#define BT_DBG(fmt, ...) BLE_HS_LOG(DEBUG, "%s: " fmt "\n", __func__, ## __VA_ARGS__); +#define BT_INFO(fmt, ...) BLE_HS_LOG(INFO, "%s: " fmt "\n", __func__, ## __VA_ARGS__); +#define BT_ERR(fmt, ...) BLE_HS_LOG(ERROR, "%s: " fmt "\n", __func__, ## __VA_ARGS__); +#define BT_GATT_ERR(_att_err) (-(_att_err)) + +typedef ble_addr_t bt_addr_le_t; + +#define k_fifo_init(queue) os_eventq_init(queue) +#define net_buf_simple_tailroom(buf) OS_MBUF_TRAILINGSPACE(buf) +#define net_buf_tailroom(buf) net_buf_simple_tailroom(buf) +#define net_buf_headroom(buf) ((buf)->om_data - &(buf)->om_databuf[buf->om_pkthdr_len]) +#define net_buf_simple_headroom(buf) net_buf_headroom(buf) +#define net_buf_simple_tail(buf) ((buf)->om_data + (buf)->om_len) + +struct net_buf_simple_state { + /** Offset of the data pointer from the beginning of the storage */ + u16_t offset; + /** Length of data */ + u16_t len; +}; + +static inline struct os_mbuf * NET_BUF_SIMPLE(uint16_t size) +{ + struct os_mbuf *buf; + + buf = os_msys_get(size, 0); + assert(buf); + + return buf; +} + +#define K_NO_WAIT (0) +#define K_FOREVER (-1) + +/* This is by purpose */ +static inline void net_buf_simple_init(struct os_mbuf *buf, + size_t reserve_head) +{ + /* This is called in Zephyr after init. + * Note in Mynewt case we don't care abour reserved head*/ + buf->om_data = &buf->om_databuf[buf->om_pkthdr_len] + reserve_head; + buf->om_len = 0; +} + +void net_buf_put(struct os_eventq *fifo, struct os_mbuf *buf); +void * net_buf_ref(struct os_mbuf *om); +void net_buf_unref(struct os_mbuf *om); +uint16_t net_buf_simple_pull_le16(struct os_mbuf *om); +uint16_t net_buf_simple_pull_be16(struct os_mbuf *om); +uint32_t net_buf_simple_pull_be32(struct os_mbuf *om); +uint8_t net_buf_simple_pull_u8(struct os_mbuf *om); +void net_buf_simple_add_le16(struct os_mbuf *om, uint16_t val); +void net_buf_simple_add_be16(struct os_mbuf *om, uint16_t val); +void net_buf_simple_add_u8(struct os_mbuf *om, uint8_t val); +void net_buf_simple_add_be32(struct os_mbuf *om, uint32_t val); +void net_buf_add_zeros(struct os_mbuf *om, uint8_t len); +void net_buf_simple_push_le16(struct os_mbuf *om, uint16_t val); +void net_buf_simple_push_be16(struct os_mbuf *om, uint16_t val); +void net_buf_simple_push_u8(struct os_mbuf *om, uint8_t val); +void *net_buf_simple_pull(struct os_mbuf *om, uint8_t len); +void *net_buf_simple_add(struct os_mbuf *om, uint8_t len); +bool k_fifo_is_empty(struct os_eventq *q); +void * net_buf_get(struct os_eventq *fifo,s32_t t); +uint8_t *net_buf_simple_push(struct os_mbuf *om, uint8_t len); +void net_buf_reserve(struct os_mbuf *om, size_t reserve); + +#define net_buf_add_mem(a,b,c) os_mbuf_append(a,b,c) +#define net_buf_simple_add_mem(a,b,c) os_mbuf_append(a,b,c) +#define net_buf_add_u8(a,b) net_buf_simple_add_u8(a,b) +#define net_buf_add(a,b) net_buf_simple_add(a,b) + +#define net_buf_clone(a, b) os_mbuf_dup(a) +#define net_buf_add_be32(a, b) net_buf_simple_add_be32(a, b) +#define net_buf_add_be16(a, b) net_buf_simple_add_be16(a, b) + +#define bt_le_adv_stop() ble_gap_adv_stop() +#define BT_GATT_CCC_NOTIFY BLE_GATT_CHR_PROP_NOTIFY +#define bt_gatt_attr ble_gatt_attr + +/** Description of different data types that can be encoded into + * advertising data. Used to form arrays that are passed to the + * bt_le_adv_start() function. + */ +struct bt_data { + u8_t type; + u8_t data_len; + const u8_t *data; +}; + +struct bt_pub_key_cb { + /** @brief Callback type for Public Key generation. + * + * Used to notify of the local public key or that the local key is not + * available (either because of a failure to read it or because it is + * being regenerated). + * + * @param key The local public key, or NULL in case of no key. + */ + void (*func)(const u8_t key[64]); + + struct bt_pub_key_cb *_next; +}; + +typedef void (*bt_dh_key_cb_t)(const u8_t key[32]); +int bt_dh_key_gen(const u8_t remote_pk[64], bt_dh_key_cb_t cb); +int bt_pub_key_gen(struct bt_pub_key_cb *new_cb); +uint8_t *bt_pub_key_get(void); +int bt_rand(void *buf, size_t len); +const char * bt_hex(const void *buf, size_t len); +int bt_encrypt_be(const uint8_t *key, const uint8_t *plaintext, uint8_t *enc_data); +void bt_mesh_register_gatt(void); +int bt_le_adv_start(const struct ble_gap_adv_params *param, + const struct bt_data *ad, size_t ad_len, + const struct bt_data *sd, size_t sd_len); + +struct k_delayed_work { + struct os_callout work; +}; + +void k_work_init(struct os_callout *work, os_event_fn handler); +void k_delayed_work_init(struct k_delayed_work *w, os_event_fn *f); +void k_delayed_work_cancel(struct k_delayed_work *w); +void k_delayed_work_submit(struct k_delayed_work *w, uint32_t ms); +int64_t k_uptime_get(void); +u32_t k_uptime_get_32(void); +void k_work_submit(struct os_callout *w); +void k_work_add_arg(struct os_callout *w, void *arg); +void k_delayed_work_add_arg(struct k_delayed_work *w, void *arg); +uint32_t k_delayed_work_remaining_get(struct k_delayed_work *w); + +static inline void net_buf_simple_save(struct os_mbuf *buf, + struct net_buf_simple_state *state) +{ + state->offset = net_buf_simple_headroom(buf); + state->len = buf->om_len; +} + +static inline void net_buf_simple_restore(struct os_mbuf *buf, + struct net_buf_simple_state *state) +{ + buf->om_data = &buf->om_databuf[buf->om_pkthdr_len] + state->offset; + buf->om_len = state->len; +} + +static inline void sys_memcpy_swap(void *dst, const void *src, size_t length) +{ + __ASSERT(((src < dst && (src + length) <= dst) || + (src > dst && (dst + length) <= src)), + "Source and destination buffers must not overlap"); + + src += length - 1; + + for (; length > 0; length--) { + *((u8_t *)dst++) = *((u8_t *)src--); + } +} + +#define popcount(x) __builtin_popcount(x) + +static inline unsigned int find_lsb_set(u32_t op) +{ + return __builtin_ffs(op); +} + +static inline unsigned int find_msb_set(u32_t op) +{ + if (!op) + return 0; + + return 32 - __builtin_clz(op); +} + +#endif diff --git a/nimble/host/mesh/include/mesh/mesh.h b/nimble/host/mesh/include/mesh/mesh.h new file mode 100644 index 000000000..27bc32b96 --- /dev/null +++ b/nimble/host/mesh/include/mesh/mesh.h @@ -0,0 +1,617 @@ +/** @file + * @brief Bluetooth Mesh Profile APIs. + */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ +#ifndef __BT_MESH_H +#define __BT_MESH_H + +#include +#include +#include +#include + +#include "mesh/glue.h" + +/** + * @brief Parsing state of a buffer. + * + * This is used for temporarily storing the parsing state of a buffer + * while giving control of the parsing to a routine which we don't + * control. + */ + +#define BIT(x) (1U << (x)) +#define BIT_MASK(n) (BIT(n) - 1) + +/** + * @brief Bluetooth Mesh Proxy + * @defgroup bt_mesh_proxy Bluetooth Mesh Proxy + * @ingroup bt_mesh + * @{ + */ + +/** + * @brief Enable advertising with Node Identity. + * + * This API requires that GATT Proxy support has been enabled. Once called + * each subnet will start advertising using Node Identity for the next + * 60 seconds. + * + * @return 0 on success, or (negative) error code on failure. + */ +int +bt_mesh_proxy_identity_enable(void); + +/** + * @} + */ + +/** + * @brief Bluetooth Mesh Composition + * @defgroup bt_comp Bluetooth Mesh Composition + * @ingroup bt_mesh + * @{ + */ + +#define BT_MESH_ADDR_UNASSIGNED 0x0000 +#define BT_MESH_ADDR_ALL_NODES 0xffff +#define BT_MESH_ADDR_PROXIES 0xfffc +#define BT_MESH_ADDR_FRIENDS 0xfffd +#define BT_MESH_ADDR_RELAYS 0xfffe + +#define BT_MESH_KEY_UNUSED 0xffff +#define BT_MESH_KEY_DEV 0xfffe + +/** + * @brief Bluetooth Mesh Element + * @defgroup bt_mesh_elem Bluetooth Mesh Element + * @ingroup bt_comp + * @{ + */ + +/** Helper to define a mesh element within an array. + * + * @param _loc Location Descriptor. + * @param _mods Array of models + * @param _vnd_mods Array of vendor models. + */ +#define BT_MESH_ELEM(_loc, _mods, _vnd_mods) \ +{ \ + .loc = (_loc), \ + .model_count = ARRAY_SIZE(_mods), \ + .models = (_mods), \ + .vnd_model_count = ARRAY_SIZE(_vnd_mods), \ + .vnd_models = (_vnd_mods), \ +} + +/** Abstraction that describes a Mesh Element */ +struct bt_mesh_elem +{ + /* Unicast Address. Set at runtime during provisioning. */ + u16_t addr; + + /* Location Descriptor (GATT Bluetooth Namespace Descriptors) */ + const u16_t loc; + + const u8_t model_count; + const u8_t vnd_model_count; + + struct bt_mesh_model * const models; + struct bt_mesh_model * const vnd_models; +}; + +/** + * @} + */ + +/** + * @brief Bluetooth Mesh Model + * @defgroup bt_mesh_model Bluetooth Mesh Model + * @ingroup bt_comp + * @{ + */ + +/* Foundation Models */ +#define BT_MESH_MODEL_ID_CFG_SRV 0x0000 +#define BT_MESH_MODEL_ID_CFG_CLI 0x0001 +#define BT_MESH_MODEL_ID_HEALTH_SRV 0x0002 +#define BT_MESH_MODEL_ID_HEALTH_CLI 0x0003 + +/* Models from the Mesh Model Specification */ +#define BT_MESH_MODEL_ID_GEN_ONOFF_SRV 0x1000 +#define BT_MESH_MODEL_ID_GEN_ONOFF_CLI 0x1001 +#define BT_MESH_MODEL_ID_GEN_LEVEL_SRV 0x1002 +#define BT_MESH_MODEL_ID_GEN_LEVEL_CLI 0x1003 +#define BT_MESH_MODEL_ID_GEN_DEF_TRANS_TIME_SRV 0x1004 +#define BT_MESH_MODEL_ID_GEN_DEF_TRANS_TIME_CLI 0x1005 +#define BT_MESH_MODEL_ID_GEN_POWER_ONOFF_SRV 0x1006 +#define BT_MESH_MODEL_ID_GEN_POWER_ONOFF_SETUP_SRV 0x1007 +#define BT_MESH_MODEL_ID_GEN_POWER_ONOFF_CLI 0x1008 +#define BT_MESH_MODEL_ID_GEN_POWER_LEVEL_SRV 0x1009 +#define BT_MESH_MODEL_ID_GEN_POWER_LEVEL_SETUP_SRV 0x100a +#define BT_MESH_MODEL_ID_GEN_POWER_LEVEL_CLI 0x100b +#define BT_MESH_MODEL_ID_GEN_BATTERY_SRV 0x100c +#define BT_MESH_MODEL_ID_GEN_BATTERY_CLI 0x100d +#define BT_MESH_MODEL_ID_GEN_LOCATION_SRV 0x100e +#define BT_MESH_MODEL_ID_GEN_LOCATION_SETUPSRV 0x100f +#define BT_MESH_MODEL_ID_GEN_LOCATION_CLI 0x1010 +#define BT_MESH_MODEL_ID_GEN_ADMIN_PROP_SRV 0x1011 +#define BT_MESH_MODEL_ID_GEN_MANUFACTURER_PROP_SRV 0x1012 +#define BT_MESH_MODEL_ID_GEN_USER_PROP_SRV 0x1013 +#define BT_MESH_MODEL_ID_GEN_CLIENT_PROP_SRV 0x1014 +#define BT_MESH_MODEL_ID_GEN_PROP_CLI 0x1015 +#define BT_MESH_MODEL_ID_SENSOR_SRV 0x1100 +#define BT_MESH_MODEL_ID_SENSOR_SETUP_SRV 0x1101 +#define BT_MESH_MODEL_ID_SENSOR_CLI 0x1102 +#define BT_MESH_MODEL_ID_TIME_SRV 0x1200 +#define BT_MESH_MODEL_ID_TIME_SETUP_SRV 0x1201 +#define BT_MESH_MODEL_ID_TIME_CLI 0x1202 +#define BT_MESH_MODEL_ID_SCENE_SRV 0x1203 +#define BT_MESH_MODEL_ID_SCENE_SETUP_SRV 0x1204 +#define BT_MESH_MODEL_ID_SCENE_CLI 0x1205 +#define BT_MESH_MODEL_ID_SCHEDULER_SRV 0x1206 +#define BT_MESH_MODEL_ID_SCHEDULER_SETUP_SRV 0x1207 +#define BT_MESH_MODEL_ID_SCHEDULER_CLI 0x1208 +#define BT_MESH_MODEL_ID_LIGHT_LIGHTNESS_SRV 0x1300 +#define BT_MESH_MODEL_ID_LIGHT_LIGHTNESS_SETUP_SRV 0x1301 +#define BT_MESH_MODEL_ID_LIGHT_LIGHTNESS_CLI 0x1302 +#define BT_MESH_MODEL_ID_LIGHT_CTL_SRV 0x1303 +#define BT_MESH_MODEL_ID_LIGHT_CTL_SETUP_SRV 0x1304 +#define BT_MESH_MODEL_ID_LIGHT_CTL_CLI 0x1305 +#define BT_MESH_MODEL_ID_LIGHT_CTL_TEMP_SRV 0x1306 +#define BT_MESH_MODEL_ID_LIGHT_HSL_SRV 0x1307 +#define BT_MESH_MODEL_ID_LIGHT_HSL_SETUP_SRV 0x1308 +#define BT_MESH_MODEL_ID_LIGHT_HSL_CLI 0x1309 +#define BT_MESH_MODEL_ID_LIGHT_HSL_HUE_SRV 0x130a +#define BT_MESH_MODEL_ID_LIGHT_HSL_SAT_SRV 0x130b +#define BT_MESH_MODEL_ID_LIGHT_XYL_SRV 0x130c +#define BT_MESH_MODEL_ID_LIGHT_XYL_SETUP_SRV 0x130d +#define BT_MESH_MODEL_ID_LIGHT_XYL_CLI 0x130e +#define BT_MESH_MODEL_ID_LIGHT_LC_SRV 0x130f +#define BT_MESH_MODEL_ID_LIGHT_LC_SETUPSRV 0x1310 +#define BT_MESH_MODEL_ID_LIGHT_LC_CLI 0x1311 + +/** Message sending context. */ +struct bt_mesh_msg_ctx +{ + /** NetKey Index of the subnet to send the message on. */ + u16_t net_idx; + + /** AppKey Index to encrypt the message with. */ + u16_t app_idx; + + /** Remote address. */ + u16_t addr; + + /** Received TTL value. Not used for sending. */ + u8_t recv_ttl :7; + + /** Whether friend credentials are used. */ + u8_t friend_cred :1; + + /** TTL, or BT_MESH_TTL_DEFAULT for default TTL. */ + u8_t send_ttl; +}; + +struct bt_mesh_model_op +{ + /* OpCode encoded using the BT_MESH_MODEL_OP_* macros */ + const u32_t opcode; + + /* Minimum required message length */ + const size_t min_len; + + /* Message handler for the opcode */ + void + (* const func)(struct bt_mesh_model *model, struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf); +}; + +#define BT_MESH_MODEL_OP_1(b0) (b0) +#define BT_MESH_MODEL_OP_2(b0, b1) (((b0) << 8) | (b1)) +#define BT_MESH_MODEL_OP_3(b0, cid) ((((b0) << 16) | 0xc00000) | (cid)) + +#define BT_MESH_MODEL_OP_END { 0, 0, NULL } +#define BT_MESH_MODEL_NO_OPS ((struct bt_mesh_model_op []) \ + { BT_MESH_MODEL_OP_END }) + +/** Helper to define an empty model array */ +#define BT_MESH_MODEL_EMPTY ((struct bt_mesh_model []){}) + +#define BT_MESH_MODEL(_id, _op, _pub, _user_data) \ +{ \ + .id = (_id), \ + .op = _op, \ + .keys = { [0 ... (MYNEWT_VAL(BLE_MESH_MODEL_KEY_COUNT) - 1)] = \ + BT_MESH_KEY_UNUSED }, \ + .pub = _pub, \ + .groups = { [0 ... (MYNEWT_VAL(BLE_MESH_MODEL_GROUP_COUNT) - 1)] = \ + BT_MESH_ADDR_UNASSIGNED }, \ + .user_data = _user_data, \ +} + +#define BT_MESH_MODEL_VND(_company, _id, _op, _pub, _user_data) \ +{ \ + .vnd.company = (_company), \ + .vnd.id = (_id), \ + .op = _op, \ + .pub = _pub, \ + .keys = { [0 ... (MYNEWT_VAL(BLE_MESH_MODEL_KEY_COUNT) - 1)] = \ + BT_MESH_KEY_UNUSED }, \ + .groups = { [0 ... (MYNEWT_VAL(BLE_MESH_MODEL_GROUP_COUNT) - 1)] = \ + BT_MESH_ADDR_UNASSIGNED }, \ + .user_data = _user_data, \ +} + +/** Mesh Configuration Server Model Context */ +struct bt_mesh_cfg +{ + struct bt_mesh_model *model; + + u8_t net_transmit; /* Network Transmit state */ + u8_t relay; /* Relay Mode state */ + u8_t relay_retransmit; /* Relay Retransmit state */ + u8_t beacon; /* Secure Network Beacon state */ + u8_t gatt_proxy; /* GATT Proxy state */ + u8_t frnd; /* Friend state */ + u8_t default_ttl; /* Default TTL */ + + /* Heartbeat Publication */ + struct + { + struct k_delayed_work timer; + + u16_t dst;u16_t count;u8_t period;u8_t ttl;u16_t feat;u16_t net_idx; + } hb_pub; + + /* Heartbeat Subscription */ + struct + { + s64_t expiry; + + u16_t src;u16_t dst;u16_t count;u8_t min_hops;u8_t max_hops; + + /* Optional subscription tracking function */ + void + (*func)(u8_t hops, u16_t feat); + } hb_sub; +}; + +extern const struct bt_mesh_model_op bt_mesh_cfg_op[]; + +#define BT_MESH_MODEL_CFG_SRV(srv_data) \ + BT_MESH_MODEL(BT_MESH_MODEL_ID_CFG_SRV, \ + bt_mesh_cfg_op, NULL, srv_data) + +/** @def BT_MESH_TRANSMIT + * + * @brief Encode transmission count & interval steps. + * + * @param count Number of retransmissions (first transmission is excluded). + * @param int_ms Interval steps in milliseconds. Must be greater than 0 + * and a multiple of 10. + * + * @return Mesh transmit value that can be used e.g. for the default + * values of the configuration model data. + */ +#define BT_MESH_TRANSMIT(count, int_ms) ((count) | (((int_ms / 10) - 1) << 3)) + +/** Mesh Health Server Model Context */ +struct bt_mesh_health +{ + struct bt_mesh_model *model; + + /* Health Period (divider) */ + u8_t period; + + /* Fetch current faults */ + int + (*fault_get_cur)(struct bt_mesh_model *model, u8_t *test_id, + u16_t *company_id, + u8_t *faults, + u8_t *fault_count); + + /* Fetch registered faults */ + int + (*fault_get_reg)(struct bt_mesh_model *model, u16_t company_id, + u8_t *test_id, + u8_t *faults, + u8_t *fault_count); + + /* Clear registered faults */ + int + (*fault_clear)(struct bt_mesh_model *model, u16_t company_id); + + /* Run a specific test */ + int + (*fault_test)(struct bt_mesh_model *model, u8_t test_id, + u16_t company_id); + + /* Attention Timer state */ + struct + { + struct k_delayed_work timer; + void + (*on)(struct bt_mesh_model *model); + void + (*off)(struct bt_mesh_model *model); + } attention; +}; + +int +bt_mesh_fault_update(struct bt_mesh_elem *elem); + +extern const struct bt_mesh_model_op bt_mesh_health_op[]; +extern struct bt_mesh_model_pub bt_mesh_health_pub; + +#define BT_MESH_MODEL_HEALTH_SRV(srv_data) \ + BT_MESH_MODEL(BT_MESH_MODEL_ID_HEALTH_SRV, \ + bt_mesh_health_op, &bt_mesh_health_pub, \ + srv_data) + +struct bt_mesh_model_pub +{ + /* Self-reference for easy lookup */ + struct bt_mesh_model *mod; + + u16_t addr; /* Publish Address */ + u16_t key; /* Publish AppKey Index */ + + u32_t ttl :8, /* Publish Time to Live */ + retransmit :8, /* Retransmit Count & Interval Steps */ + period :8, /* Publish Period */ + period_div :4, /* Divisor for the Period */ + cred :1; /* Friendship Credentials Flag */ + + /* Publish callback */ + void + (*func)(struct bt_mesh_model *mod); + + /* Publish Period Timer */ + struct k_delayed_work timer; +}; + +/** Abstraction that describes a Mesh Model instance */ +struct bt_mesh_model +{ + union + { + const u16_t id; + struct + { + u16_t company;u16_t id; + } vnd; + }; + + /* The Element this Model belongs to */ + struct bt_mesh_elem *elem; + + /* Model Publication */ + struct bt_mesh_model_pub * const pub; + + /* AppKey List */ + u16_t keys[MYNEWT_VAL(BLE_MESH_MODEL_KEY_COUNT)]; + + /* Subscription List (group or virtual addresses) */ + u16_t groups[MYNEWT_VAL(BLE_MESH_MODEL_GROUP_COUNT)]; + + const struct bt_mesh_model_op * const op; + + /* Model-specific user data */ + void *user_data; +}; + +typedef void +(*bt_mesh_cb_t)(int err, void *cb_data); + +void +bt_mesh_model_msg_init(struct os_mbuf *msg, u32_t opcode); + +/** Special TTL value to request using configured default TTL */ +#define BT_MESH_TTL_DEFAULT 0xff + +/** Maximum allowed TTL value */ +#define BT_MESH_TTL_MAX 0x7f + +/** + * @brief Send an Access Layer message. + * + * @param model Mesh (client) Model that the message belongs to. + * @param ctx Message context, includes keys, TTL, etc. + * @param msg Access Layer payload (the actual message to be sent). + * @param cb Optional "message sent" callback. + * @param cb_data User data to be passed to the callback. + * + * @return 0 on success, or (negative) error code on failure. + */ +int +bt_mesh_model_send(struct bt_mesh_model *model, struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *msg, bt_mesh_cb_t cb, void *cb_data); + +/** + * @brief Send a model publication message. + * + * @param model Mesh (client) Model that's publishing the message. + * @param msg Access Layer message to publish. + * + * @return 0 on success, or (negative) error code on failure. + */ +int +bt_mesh_model_publish(struct bt_mesh_model *model, struct os_mbuf *msg); + +/** + * @} + */ + +/** Node Composition */ +struct bt_mesh_comp +{ + u16_t cid;u16_t pid;u16_t vid; + + size_t elem_count; + struct bt_mesh_elem *elem; +}; + +/** + * @} + */ + +/** + * @brief Bluetooth Mesh Provisioning + * @defgroup bt_mesh_prov Bluetooth Mesh Provisioning + * @ingroup bt_mesh + * @{ + */ + +typedef enum +{ + BT_MESH_NO_OUTPUT = 0, + BT_MESH_BLINK = BIT(0), + BT_MESH_BEEP = BIT(1), + BT_MESH_VIBRATE = BIT(2), + BT_MESH_DISPLAY_NUMBER = BIT(3), + BT_MESH_DISPLAY_STRING = BIT(4), +} bt_mesh_output_action; + +typedef enum +{ + BT_MESH_NO_INPUT = 0, + BT_MESH_PUSH = BIT(0), + BT_MESH_TWIST = BIT(1), + BT_MESH_ENTER_NUMBER = BIT(2), + BT_MESH_ENTER_STRING = BIT(3), +} bt_mesh_input_action; + +struct bt_mesh_prov +{ + const u8_t *uuid; + + u8_t *static_val;u8_t static_val_len; + + u8_t output_size;u16_t output_actions; + + u8_t input_size;u16_t input_actions; + + int + (*output_number)(bt_mesh_output_action act, u32_t num); + int + (*output_string)(const char *str); + + int + (*input)(bt_mesh_input_action act, u8_t size); + + void + (*complete)(void); +}; + +int +bt_mesh_input_string(const char *str); +int +bt_mesh_input_number(u32_t num); + +/** + * @} + */ + +/** + * @brief Bluetooth Mesh + * @defgroup bt_mesh Bluetooth Mesh + * @ingroup bluetooth + * @{ + */ + +/* Primary Network Key index */ +#define BT_MESH_NET_PRIMARY 0x000 + +#define BT_MESH_RELAY_DISABLED 0x00 +#define BT_MESH_RELAY_ENABLED 0x01 +#define BT_MESH_RELAY_NOT_SUPPORTED 0x02 + +#define BT_MESH_BEACON_DISABLED 0x00 +#define BT_MESH_BEACON_ENABLED 0x01 + +#define BT_MESH_GATT_PROXY_DISABLED 0x00 +#define BT_MESH_GATT_PROXY_ENABLED 0x01 +#define BT_MESH_GATT_PROXY_NOT_SUPPORTED 0x02 + +#define BT_MESH_FRIEND_DISABLED 0x00 +#define BT_MESH_FRIEND_ENABLED 0x01 +#define BT_MESH_FRIEND_NOT_SUPPORTED 0x02 + +#define BT_MESH_NODE_IDENTITY_STOPPED 0x00 +#define BT_MESH_NODE_IDENTITY_RUNNING 0x01 +#define BT_MESH_NODE_IDENTITY_NOT_SUPPORTED 0x02 + +/* Features */ +#define BT_MESH_FEAT_RELAY BIT(0) +#define BT_MESH_FEAT_PROXY BIT(1) +#define BT_MESH_FEAT_FRIEND BIT(2) +#define BT_MESH_FEAT_LOW_POWER BIT(3) + +/** @brief Initialize Mesh support + * + * @param own_addr_type Node address type + * @param prov Node provisioning information. + * @param comp Node Composition. + * + * @return Zero on success or (negative) error code otherwise. + */ +int +bt_mesh_init(uint8_t own_addr_type, const struct bt_mesh_prov *prov, + const struct bt_mesh_comp *comp); + +/** @brief Reset the state of the local Mesh node. + * + * Resets the state of the node, which in turn causes it to start sending + * out unprovisioned beacons. + */ +void +bt_mesh_reset(void); + +/** @brief Provision the local Mesh Node. + * + * This API should normally not be used directly by the application. The + * only exception is for testing purposes where manual provisioning is + * desired without an actual external provisioner. + * + * @param net_key Network Key + * @param net_idx Network Key Index + * @param flags Provisioning Flags + * @param iv_index IV Index + * @param seq Sequence Number (0 if newly provisioned). + * @param addr Primary element address + * @param dev_key Device Key + * + * @return Zero on success or (negative) error code otherwise. + */ +int +bt_mesh_provision(const u8_t net_key[16], u16_t net_idx, +u8_t flags, + u32_t iv_index, u32_t seq, + u16_t addr, + const u8_t dev_key[16]); + +/** @brief Toggle the Low Power feature of the local device + * + * Enables or disables the Low Power feature of the local device. This is + * exposed as a run-time feature, since the device might want to change + * this e.g. based on being plugged into a stable power source or running + * from a battery power source. + * + * @param enable true to enable LPN functionality, false to disable it. + * + * @return Zero on success or (negative) error code otherwise. + */ +int +bt_mesh_lpn_set(bool enable); + +/** + * @} + */ + +#endif /* __BT_MESH_H */ diff --git a/nimble/host/mesh/pkg.yml b/nimble/host/mesh/pkg.yml new file mode 100644 index 000000000..57993e948 --- /dev/null +++ b/nimble/host/mesh/pkg.yml @@ -0,0 +1,38 @@ +# +# 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. +# + +pkg.name: net/nimble/host/mesh +pkg.description: Bluetooth Mesh +pkg.author: "Apache Mynewt " +pkg.homepage: "http://mynewt.apache.org/" +pkg.keywords: + - ble + - bluetooth + - mesh + +pkg.deps: + - kernel/os + - net/nimble + - util/mem + - net/nimble/host + - crypto/tinycrypt + +pkg.req_apis: + - log + - stats diff --git a/nimble/host/mesh/src/access.c b/nimble/host/mesh/src/access.c new file mode 100644 index 000000000..42fe03766 --- /dev/null +++ b/nimble/host/mesh/src/access.c @@ -0,0 +1,533 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include + +#include +#include "mesh/mesh.h" + +#define BT_DBG_ENABLED (MYNEWT_VAL(BLE_MESH_DEBUG_ACCESS)) +#include "host/ble_hs_log.h" + +#include "mesh_priv.h" +#include "adv.h" +#include "net.h" +#include "lpn.h" +#include "transport.h" +#include "access.h" +#include "foundation.h" + +static const struct bt_mesh_comp *dev_comp; +static u16_t dev_primary_addr; + +static const struct { + const u16_t id; + int (*const init)(struct bt_mesh_model *model, bool primary); +} const model_init[] = { + { BT_MESH_MODEL_ID_CFG_SRV, bt_mesh_conf_init }, + { BT_MESH_MODEL_ID_HEALTH_SRV, bt_mesh_health_init }, +}; + +void bt_mesh_model_foreach(void (*func)(struct bt_mesh_model *mod, + struct bt_mesh_elem *elem, + bool vnd, bool primary, + void *user_data), + void *user_data) +{ + int i, j; + + for (i = 0; i < dev_comp->elem_count; i++) { + struct bt_mesh_elem *elem = &dev_comp->elem[i]; + + for (j = 0; j < elem->model_count; j++) { + struct bt_mesh_model *model = &elem->models[j]; + + func(model, elem, false, i == 0, user_data); + } + + for (j = 0; j < elem->vnd_model_count; j++) { + struct bt_mesh_model *model = &elem->vnd_models[j]; + + func(model, elem, true, i == 0, user_data); + } + } +} + +s32_t bt_mesh_model_pub_period_get(struct bt_mesh_model *mod) +{ + int period; + + if (!mod->pub) { + return 0; + } + + switch (mod->pub->period >> 6) { + case 0x00: + /* 1 step is 100 ms */ + period = K_MSEC((mod->pub->period & BIT_MASK(6)) * 100); + break; + case 0x01: + /* 1 step is 1 second */ + period = K_SECONDS(mod->pub->period & BIT_MASK(6)); + break; + case 0x02: + /* 1 step is 10 seconds */ + period = K_SECONDS((mod->pub->period & BIT_MASK(6)) * 10); + break; + case 0x03: + /* 1 step is 10 minutes */ + period = K_MINUTES((mod->pub->period & BIT_MASK(6)) * 10); + break; + default: + CODE_UNREACHABLE; + } + + return period >> mod->pub->period_div; +} + +static void mod_publish(struct os_event *work) +{ + struct bt_mesh_model_pub *pub = work->ev_arg; + s32_t period_ms; + + BT_DBG(""); + + if (pub->func) { + pub->func(pub->mod); + } + + period_ms = bt_mesh_model_pub_period_get(pub->mod); + BT_DBG("period %u ms", period_ms); + if (period_ms) { + k_delayed_work_submit(&pub->timer, period_ms); + } +} + +static void mod_init(struct bt_mesh_model *mod, struct bt_mesh_elem *elem, + bool vnd, bool primary, void *user_data) +{ + int i; + + mod->elem = elem; + + if (mod->pub) { + mod->pub->mod = mod; + k_delayed_work_init(&mod->pub->timer, mod_publish); + } + + for (i = 0; i < ARRAY_SIZE(mod->keys); i++) { + mod->keys[i] = BT_MESH_KEY_UNUSED; + } + + if (vnd) { + return; + } + + for (i = 0; i < ARRAY_SIZE(model_init); i++) { + if (model_init[i].id == mod->id) { + model_init[i].init(mod, primary); + } + } +} + +int bt_mesh_comp_register(const struct bt_mesh_comp *comp) +{ + /* There must be at least one element */ + if (!comp->elem_count) { + return -EINVAL; + } + + dev_comp = comp; + + bt_mesh_model_foreach(mod_init, NULL); + + return 0; +} + +void bt_mesh_comp_provision(u16_t addr) +{ + int i; + + dev_primary_addr = addr; + + BT_DBG("addr 0x%04x elem_count %zu", addr, dev_comp->elem_count); + + for (i = 0; i < dev_comp->elem_count; i++) { + struct bt_mesh_elem *elem = &dev_comp->elem[i]; + + elem->addr = addr++; + + BT_DBG("addr 0x%04x mod_count %u vnd_mod_count %u", + elem->addr, elem->model_count, elem->vnd_model_count); + } +} + +void bt_mesh_comp_unprovision(void) +{ + BT_DBG(""); + + dev_primary_addr = BT_MESH_ADDR_UNASSIGNED; + + bt_mesh_model_foreach(mod_init, NULL); +} + +u16_t bt_mesh_primary_addr(void) +{ + return dev_primary_addr; +} + +u16_t *bt_mesh_model_find_group(struct bt_mesh_model *mod, u16_t addr) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(mod->groups); i++) { + if (mod->groups[i] == addr) { + return &mod->groups[i]; + } + } + + return NULL; +} + +static struct bt_mesh_model *bt_mesh_elem_find_group(struct bt_mesh_elem *elem, + u16_t group_addr) +{ + struct bt_mesh_model *model; + u16_t *match; + int i; + + for (i = 0; i < elem->model_count; i++) { + model = &elem->models[i]; + + match = bt_mesh_model_find_group(model, group_addr); + if (match) { + return model; + } + } + + for (i = 0; i < elem->vnd_model_count; i++) { + model = &elem->vnd_models[i]; + + match = bt_mesh_model_find_group(model, group_addr); + if (match) { + return model; + } + } + + return NULL; +} + +struct bt_mesh_elem *bt_mesh_elem_find(u16_t addr) +{ + int i; + + for (i = 0; i < dev_comp->elem_count; i++) { + struct bt_mesh_elem *elem = &dev_comp->elem[i]; + + if (BT_MESH_ADDR_IS_GROUP(addr) || + BT_MESH_ADDR_IS_VIRTUAL(addr)) { + if (bt_mesh_elem_find_group(elem, addr)) { + return elem; + } + } else if (elem->addr == addr) { + return elem; + } + } + + return NULL; +} + +u8_t bt_mesh_elem_count(void) +{ + return dev_comp->elem_count; +} + +static bool model_has_key(struct bt_mesh_model *mod, u16_t key) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(mod->keys); i++) { + if (mod->keys[i] == key) { + return true; + } + } + + return false; +} + +static const struct bt_mesh_model_op *find_op(struct bt_mesh_model *models, + u8_t model_count, + u16_t app_idx, u32_t opcode, + struct bt_mesh_model **model) +{ + u8_t i; + + for (i = 0; i < model_count; i++) { + const struct bt_mesh_model_op *op; + + *model = &models[i]; + + if (!model_has_key(*model, app_idx)) { + continue; + } + + for (op = (*model)->op; op->func; op++) { + if (op->opcode == opcode) { + return op; + } + } + } + + *model = NULL; + return NULL; +} + +static int get_opcode(struct os_mbuf *buf, u32_t *opcode) +{ + switch (buf->om_data[0] >> 6) { + case 0x00: + case 0x01: + if (buf->om_data[0] == 0x7f) { + BT_ERR("Ignoring RFU OpCode"); + return -EINVAL; + } + + *opcode = net_buf_simple_pull_u8(buf); + return 0; + case 0x02: + if (buf->om_len < 2) { + BT_ERR("Too short payload for 2-octet OpCode"); + return -EINVAL; + } + + *opcode = net_buf_simple_pull_be16(buf); + return 0; + case 0x03: + if (buf->om_len < 3) { + BT_ERR("Too short payload for 3-octet OpCode"); + return -EINVAL; + } + + *opcode = net_buf_simple_pull_u8(buf) << 16; + *opcode |= net_buf_simple_pull_le16(buf); + return 0; + } + + CODE_UNREACHABLE; +} + +bool bt_mesh_fixed_group_match(u16_t addr) +{ + /* Check for fixed group addresses */ + switch (addr) { + case BT_MESH_ADDR_ALL_NODES: + return true; + case BT_MESH_ADDR_PROXIES: + /* TODO: Proxy not yet supported */ + return false; + case BT_MESH_ADDR_FRIENDS: + return (bt_mesh_friend_get() == BT_MESH_FRIEND_ENABLED); + case BT_MESH_ADDR_RELAYS: + return (bt_mesh_relay_get() == BT_MESH_RELAY_ENABLED); + default: + return false; + } +} + +void bt_mesh_model_recv(struct bt_mesh_net_rx *rx, struct os_mbuf *buf) +{ + struct bt_mesh_model *models, *model; + const struct bt_mesh_model_op *op; + u32_t opcode; + u8_t count; + int i; + + BT_DBG("app_idx 0x%04x src 0x%04x dst 0x%04x", rx->ctx.app_idx, + rx->ctx.addr, rx->dst); + BT_DBG("len %u: %s", buf->om_len, bt_hex(buf->om_data, buf->om_len)); + + if (get_opcode(buf, &opcode) < 0) { + BT_WARN("Unable to decode OpCode"); + return; + } + + BT_DBG("OpCode 0x%08x", opcode); + + for (i = 0; i < dev_comp->elem_count; i++) { + struct bt_mesh_elem *elem = &dev_comp->elem[i]; + + if (BT_MESH_ADDR_IS_UNICAST(rx->dst)) { + if (elem->addr != rx->dst) { + continue; + } + } else if (BT_MESH_ADDR_IS_GROUP(rx->dst) || + BT_MESH_ADDR_IS_VIRTUAL(rx->dst)) { + if (!bt_mesh_elem_find_group(elem, rx->dst)) { + continue; + } + } else if (i != 0 || !bt_mesh_fixed_group_match(rx->dst)) { + continue; + } + + /* SIG models cannot contain 3-byte (vendor) OpCodes, and + * vendor models cannot contain SIG (1- or 2-byte) OpCodes, so + * we only need to do the lookup in one of the model lists. + */ + if (opcode < 0x10000) { + models = elem->models; + count = elem->model_count; + } else { + models = elem->vnd_models; + count = elem->vnd_model_count; + } + + op = find_op(models, count, rx->ctx.app_idx, opcode, &model); + if (op) { + struct net_buf_simple_state state; + + if (buf->om_len < op->min_len) { + BT_ERR("Too short message for OpCode 0x%08x", + opcode); + continue; + } + + /* The callback will likely parse the buffer, so + * store the parsing state in case multiple models + * receive the message. + */ + net_buf_simple_save(buf, &state); + op->func(model, &rx->ctx, buf); + net_buf_simple_restore(buf, &state); + + } else { + BT_DBG("No OpCode 0x%08x for elem %d", opcode, i); + } + } +} + +void bt_mesh_model_msg_init(struct os_mbuf *msg, u32_t opcode) +{ + net_buf_simple_init(msg, 0); + + if (opcode < 0x100) { + /* 1-byte OpCode */ + net_buf_simple_add_u8(msg, opcode); + return; + } + + if (opcode < 0x10000) { + /* 2-byte OpCode */ + net_buf_simple_add_be16(msg, opcode); + return; + } + + /* 3-byte OpCode */ + net_buf_simple_add_u8(msg, ((opcode >> 16) & 0xff)); + net_buf_simple_add_le16(msg, opcode & 0xffff); +} + +int bt_mesh_model_send(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *msg, bt_mesh_cb_t cb, + void *cb_data) +{ + struct bt_mesh_net_tx tx = { + .sub = bt_mesh_subnet_get(ctx->net_idx), + .ctx = ctx, + .src = model->elem->addr, + }; + + if (ctx->friend_cred && !bt_mesh_lpn_established()) { + BT_ERR("Friendship Credentials requested without a Friend"); + return -EINVAL; + } + + BT_DBG("net_idx 0x%04x app_idx 0x%04x dst 0x%04x", ctx->net_idx, + ctx->app_idx, ctx->addr); + BT_DBG("len %u: %s", msg->om_len, bt_hex(msg->om_data, msg->om_len)); + + if (net_buf_simple_tailroom(msg) < 4) { + BT_ERR("Not enough tailroom for TransMIC"); + return -EINVAL; + } + + if (msg->om_len > BT_MESH_TX_SDU_MAX - 4) { + BT_ERR("Too big message"); + return -EMSGSIZE; + } + + if (!model_has_key(model, ctx->app_idx)) { + BT_ERR("Model not bound to AppKey 0x%04x", ctx->app_idx); + return -EINVAL; + } + + return bt_mesh_trans_send(&tx, msg, cb, cb_data); +} + +int bt_mesh_model_publish(struct bt_mesh_model *model, + struct os_mbuf *msg) +{ + struct bt_mesh_app_key *key; + struct bt_mesh_msg_ctx ctx; + + if (!model->pub) { + return -ENOTSUP; + } + + if (model->pub->key == BT_MESH_KEY_UNUSED || + model->pub->addr == BT_MESH_ADDR_UNASSIGNED) { + return -EADDRNOTAVAIL; + } + + key = bt_mesh_app_key_find(model->pub->key); + if (!key) { + return -EADDRNOTAVAIL; + } + + ctx.net_idx = key->net_idx; + ctx.app_idx = key->app_idx; + ctx.addr = model->pub->addr; + ctx.friend_cred = model->pub->cred; + ctx.send_ttl = model->pub->ttl; + + return bt_mesh_model_send(model, &ctx, msg, NULL, NULL); +} + +struct bt_mesh_model *bt_mesh_model_find_vnd(struct bt_mesh_elem *elem, + u16_t company, u16_t id) +{ + u8_t i; + + for (i = 0; i < elem->vnd_model_count; i++) { + if (elem->vnd_models[i].vnd.company == company && + elem->vnd_models[i].vnd.id == id) { + return &elem->vnd_models[i]; + } + } + + return NULL; +} + +struct bt_mesh_model *bt_mesh_model_find(struct bt_mesh_elem *elem, + u16_t id) +{ + u8_t i; + + for (i = 0; i < elem->model_count; i++) { + if (elem->models[i].id == id) { + return &elem->models[i]; + } + } + + return NULL; +} + +const struct bt_mesh_comp *bt_mesh_comp_get(void) +{ + return dev_comp; +} diff --git a/nimble/host/mesh/src/access.h b/nimble/host/mesh/src/access.h new file mode 100644 index 000000000..bf07ad6ea --- /dev/null +++ b/nimble/host/mesh/src/access.h @@ -0,0 +1,61 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef __ACCESS_H__ +#define __ACCESS_H__ + +#include "mesh/mesh.h" + +void +bt_mesh_elem_register(struct bt_mesh_elem *elem, u8_t count); + +u8_t +bt_mesh_elem_count(void); + +/* Find local element based on unicast or group address */ +struct bt_mesh_elem * +bt_mesh_elem_find(u16_t addr); + +struct bt_mesh_model * +bt_mesh_model_find_vnd(struct bt_mesh_elem *elem, u16_t company, u16_t id); +struct bt_mesh_model * +bt_mesh_model_find(struct bt_mesh_elem *elem, u16_t id); + +u16_t * +bt_mesh_model_find_group(struct bt_mesh_model *mod, u16_t addr); + +bool +bt_mesh_fixed_group_match(u16_t addr); + +void +bt_mesh_model_foreach( + void + (*func)(struct bt_mesh_model *mod, struct bt_mesh_elem *elem, bool vnd, + bool primary, void *user_data), + void *user_data); + +s32_t +bt_mesh_model_pub_period_get(struct bt_mesh_model *mod); + +void +bt_mesh_comp_provision(u16_t addr); +void +bt_mesh_comp_unprovision(void); + +u16_t +bt_mesh_primary_addr(void); + +const struct bt_mesh_comp * +bt_mesh_comp_get(void); + +void +bt_mesh_model_recv(struct bt_mesh_net_rx *rx, struct os_mbuf *buf); + +int +bt_mesh_comp_register(const struct bt_mesh_comp *comp); +#endif diff --git a/nimble/host/mesh/src/adv.c b/nimble/host/mesh/src/adv.c new file mode 100644 index 000000000..28d019d82 --- /dev/null +++ b/nimble/host/mesh/src/adv.c @@ -0,0 +1,348 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "mesh/mesh.h" + +#define BT_DBG_ENABLED (MYNEWT_VAL(BLE_MESH_DEBUG_ADV)) +#include "host/ble_hs_log.h" +#include "host/ble_hs_adv.h" +#include "host/ble_gap.h" +#include "nimble/hci_common.h" + +#include "adv.h" +#include "foundation.h" +#include "net.h" +#include "beacon.h" +#include "prov.h" +#include "proxy.h" + +/* Window and Interval are equal for continuous scanning */ +#define MESH_SCAN_INTERVAL 0x10 +#define MESH_SCAN_WINDOW 0x10 + +/* Convert from ms to 0.625ms units */ +#define ADV_INT(_ms) ((_ms) * 8 / 5) + +/* Pre-5.0 controllers enforce a minimum interval of 100ms + * whereas 5.0+ controllers can go down to 20ms. + */ +#define ADV_INT_DEFAULT K_MSEC(100) +#define ADV_INT_FAST K_MSEC(20) + +/* TinyCrypt PRNG consumes a lot of stack space, so we need to have + * an increased call stack whenever it's used. + */ +#define ADV_STACK_SIZE 768 + +struct os_task adv_task; +static struct os_eventq adv_queue; +static uint8_t g_own_addr_type; + +static os_membuf_t adv_buf_mem[OS_MEMPOOL_SIZE( + MYNEWT_VAL(BLE_MESH_ADV_BUF_COUNT), + BT_MESH_ADV_DATA_SIZE + BT_MESH_ADV_USER_DATA_SIZE)]; + +struct os_mbuf_pool adv_os_mbuf_pool; +static struct os_mempool adv_buf_mempool; + +static const u8_t adv_type[] = { + [BT_MESH_ADV_PROV] = BLE_HS_ADV_TYPE_MESH_PROV, + [BT_MESH_ADV_DATA] = BLE_HS_ADV_TYPE_MESH_MESSAGE, + [BT_MESH_ADV_BEACON] = BLE_HS_ADV_TYPE_MESH_BEACON, +}; + + +static inline void adv_sent(struct os_mbuf *buf, int err) +{ + if (BT_MESH_ADV(buf)->busy) { + BT_MESH_ADV(buf)->busy = 0; + + if (BT_MESH_ADV(buf)->sent) { + BT_MESH_ADV(buf)->sent(buf, err); + } + } + + net_buf_unref(buf); +} + +static inline void adv_send(struct os_mbuf *buf) +{ + /* XXX: For BT5 we could have better adv interval */ + const s32_t adv_int_min = ADV_INT_DEFAULT; + struct ble_gap_adv_params param = { 0 }; + u16_t duration, adv_int; + struct bt_mesh_adv *adv = BT_MESH_ADV(buf); + struct bt_data ad; + int err; + + adv_int = max(adv_int_min, adv->adv_int); + duration = (adv->count + 1) * (adv_int + 10); + + BT_DBG("buf %p, type %u len %u:", buf, adv->type, + buf->om_len); + BT_DBG("count %u interval %ums duration %ums", + adv->count + 1, adv_int, duration); + + ad.type = adv_type[BT_MESH_ADV(buf)->type]; + ad.data_len = buf->om_len; + ad.data = buf->om_data; + + param.itvl_min = ADV_INT(adv_int); + param.itvl_max = param.itvl_min; + param.conn_mode = BLE_GAP_CONN_MODE_NON; + + err = bt_le_adv_start(¶m, &ad, 1, NULL, 0); + adv_sent(buf, err); + if (err) { + BT_ERR("Advertising failed: err %d", err); + return; + } + + BT_DBG("Advertising started. Sleeping %u ms", duration); + + os_time_delay(OS_TICKS_PER_SEC * duration / 1000); + + err = bt_le_adv_stop(); + if (err) { + BT_ERR("Stopping advertising failed: err %d", err); + return; + } + + BT_DBG("Advertising stopped"); +} + +static void +adv_thread(void *args) +{ + static struct os_event *ev; + struct os_mbuf *adv_data; + struct bt_mesh_adv *adv; +#if (MYNEWT_VAL(BLE_MESH_PROXY)) + s32_t timeout; + struct os_eventq *eventq_pool = &adv_queue; +#endif + + BT_DBG("started"); + + while (1) { +#if (MYNEWT_VAL(BLE_MESH_PROXY)) + ev = os_eventq_get_no_wait(&adv_queue); + while (!ev) { + timeout = bt_mesh_proxy_adv_start(); + BT_DBG("Proxy Advertising up to %d ms", timeout); + + // FIXME: should we redefine K_SECONDS macro instead in glue? + if (timeout != K_FOREVER) { + timeout = OS_TICKS_PER_SEC * timeout / 1000; + } + + ev = os_eventq_poll(&eventq_pool, 1, timeout); + bt_mesh_proxy_adv_stop(); + } +#else + ev = os_eventq_get(&adv_queue); +#endif + + if (!ev || !ev->ev_arg) { + continue; + } + + adv_data = ev->ev_arg; + adv = BT_MESH_ADV(adv_data); + + /* busy == 0 means this was canceled */ + if (adv->busy) { + adv_send(adv_data); + } + + os_sched(NULL); + } +} + +void bt_mesh_adv_update(void) +{ + static struct os_event ev = { }; + + BT_DBG(""); + + os_eventq_put(&adv_queue, &ev); +} + +struct os_mbuf *bt_mesh_adv_create(enum bt_mesh_adv_type type, u8_t xmit_count, + u8_t xmit_int, s32_t timeout) +{ + struct os_mbuf *adv_data; + struct bt_mesh_adv *adv; + + adv_data = os_mbuf_get_pkthdr(&adv_os_mbuf_pool, sizeof(struct bt_mesh_adv)); + if (!adv_data) { + return NULL; + } + + adv = BT_MESH_ADV(adv_data); + memset(adv, 0, sizeof(*adv)); + + adv->type = type; + adv->count = xmit_count; + adv->adv_int = xmit_int; + adv->ref_cnt = 1; + adv->ev.ev_arg = adv_data; + return adv_data; +} + +void bt_mesh_adv_send(struct os_mbuf *buf, bt_mesh_adv_func_t sent) +{ + BT_DBG("buf %p, type 0x%02x len %u: %s", buf, BT_MESH_ADV(buf)->type, buf->om_len, + bt_hex(buf->om_data, buf->om_len)); + + BT_MESH_ADV(buf)->sent = sent; + BT_MESH_ADV(buf)->busy = 1; + BT_MESH_ADV(buf)->ev.ev_cb = NULL; /* does not matter */ + + net_buf_put(&adv_queue, net_buf_ref(buf)); +} + +static void bt_mesh_scan_cb(const bt_addr_le_t *addr, s8_t rssi, + u8_t adv_type, struct os_mbuf *buf) +{ + if (adv_type != BLE_HCI_ADV_TYPE_ADV_NONCONN_IND) { + return; + } + + BT_DBG("len %u: %s", buf->om_len, bt_hex(buf->om_data, buf->om_len)); + + while (buf->om_len > 1) { + struct net_buf_simple_state state; + u8_t len, type; + + len = net_buf_simple_pull_u8(buf); + /* Check for early termination */ + if (len == 0) { + return; + } + + if (len > buf->om_len || buf->om_len < 1) { + BT_WARN("AD malformed"); + return; + } + + net_buf_simple_save(buf, &state); + + type = net_buf_simple_pull_u8(buf); + + switch (type) { + case BLE_HS_ADV_TYPE_MESH_MESSAGE: + bt_mesh_net_recv(buf, rssi, BT_MESH_NET_IF_ADV); + break; +#if MYNEWT_VAL(BLE_MESH_PB_ADV) + case BLE_HS_ADV_TYPE_MESH_PROV: + bt_mesh_pb_adv_recv(buf); + break; +#endif + case BLE_HS_ADV_TYPE_MESH_BEACON: + bt_mesh_beacon_recv(buf); + break; + default: + break; + } + + net_buf_simple_restore(buf, &state); + net_buf_simple_pull(buf, len); + } +} + +void bt_mesh_adv_init(uint8_t own_addr_type) +{ + os_stack_t *pstack; + int rc; + + pstack = malloc(sizeof(os_stack_t) * ADV_STACK_SIZE); + assert(pstack); + + g_own_addr_type = own_addr_type; + + rc = os_mempool_init(&adv_buf_mempool, MYNEWT_VAL(BLE_MESH_ADV_BUF_COUNT), + BT_MESH_ADV_DATA_SIZE + BT_MESH_ADV_USER_DATA_SIZE, + adv_buf_mem, "adv_buf_pool"); + assert(rc == 0); + + rc = os_mbuf_pool_init(&adv_os_mbuf_pool, &adv_buf_mempool, + BT_MESH_ADV_DATA_SIZE + BT_MESH_ADV_USER_DATA_SIZE, + MYNEWT_VAL(BLE_MESH_ADV_BUF_COUNT)); + assert(rc == 0); + + os_eventq_init(&adv_queue); + + os_task_init(&adv_task, "mesh_adv", adv_thread, NULL, + MYNEWT_VAL(BLE_MESH_ADV_TASK_PRIO), OS_WAIT_FOREVER, pstack, + ADV_STACK_SIZE); +} + +int +ble_adv_gap_mesh_cb(struct ble_gap_event *event, void *arg) +{ +#if MYNEWT_VAL(BLE_EXT_ADV) + struct ble_gap_ext_disc_desc *ext_desc; +#endif + struct ble_gap_disc_desc *desc; + struct os_mbuf *buf = NULL; + + BT_DBG("event->type %d", event->type); + + switch (event->type) { +#if MYNEWT_VAL(BLE_EXT_ADV) + case BLE_GAP_EVENT_EXT_DISC: + ext_desc = &event->ext_disc; + buf = os_mbuf_get_pkthdr(&adv_os_mbuf_pool, 0); + if (!buf || os_mbuf_append(buf, ext_desc->data, ext_desc->length_data)) { + BT_ERR("Could not append data"); + goto done; + } + bt_mesh_scan_cb(&ext_desc->addr, ext_desc->rssi, + ext_desc->legacy_event_type, buf); + break; +#endif + case BLE_GAP_EVENT_DISC: + desc = &event->disc; + buf = os_mbuf_get_pkthdr(&adv_os_mbuf_pool, 0); + if (!buf || os_mbuf_append(buf, desc->data, desc->length_data)) { + BT_ERR("Could not append data"); + goto done; + } + + bt_mesh_scan_cb(&desc->addr, desc->rssi, desc->event_type, buf); + break; + default: + break; + } + +done: + if (buf) { + os_mbuf_free_chain(buf); + } + + return 0; +} + +int bt_mesh_scan_enable(void) +{ + struct ble_gap_disc_params scan_param = + { .passive = 1, .filter_duplicates = 0, .itvl = + MESH_SCAN_INTERVAL, .window = MESH_SCAN_WINDOW }; + + BT_DBG(""); + + return ble_gap_disc(g_own_addr_type, BLE_HS_FOREVER, &scan_param, NULL, NULL); +} + +int bt_mesh_scan_disable(void) +{ + BT_DBG(""); + + return ble_gap_disc_cancel(); +} diff --git a/nimble/host/mesh/src/adv.h b/nimble/host/mesh/src/adv.h new file mode 100644 index 000000000..eb44d6a1b --- /dev/null +++ b/nimble/host/mesh/src/adv.h @@ -0,0 +1,67 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef __ADV_H__ +#define __ADV_H__ + +/* Maximum advertising data payload for a single data type */ +#include "mesh/mesh.h" + +#define BT_MESH_ADV_DATA_SIZE 31 +#define BT_MESH_ADV_USER_DATA_SIZE (sizeof(struct os_mbuf_pkthdr) + \ + sizeof(struct bt_mesh_adv)) + \ + sizeof(struct os_mbuf) + +#define BT_MESH_ADV(om) ((struct bt_mesh_adv *) OS_MBUF_USRHDR(om)) + +enum bt_mesh_adv_type +{ + BT_MESH_ADV_PROV, BT_MESH_ADV_DATA, BT_MESH_ADV_BEACON, +}; + +typedef void +(*bt_mesh_adv_func_t)(struct os_mbuf *adv_data, int err); + +struct bt_mesh_adv +{ + bt_mesh_adv_func_t sent; + u8_t type :2, busy :1; + u8_t count :3, adv_int :5; + union + { + /* Generic User Data */ + u8_t user_data[2]; + + /* For transport layer segment sending */ + struct + { + u8_t tx_id; + u8_t attempts :6, new_key :1, friend_cred :1; + } seg; + }; + + int ref_cnt; + struct os_event ev; +}; + +/* xmit_count: Number of retransmissions, i.e. 0 == 1 transmission */ +struct os_mbuf * bt_mesh_adv_create(enum bt_mesh_adv_type type, u8_t xmit_count, + u8_t xmit_int, s32_t timeout); + +void bt_mesh_adv_send(struct os_mbuf *buf, bt_mesh_adv_func_t sent); + +void bt_mesh_adv_update(void); + +void bt_mesh_adv_init(uint8_t own_addr_type); + +int bt_mesh_scan_enable(void); + +int bt_mesh_scan_disable(void); + +int ble_adv_gap_mesh_cb(struct ble_gap_event *event, void *arg); +#endif diff --git a/nimble/host/mesh/src/beacon.c b/nimble/host/mesh/src/beacon.c new file mode 100644 index 000000000..e665306bf --- /dev/null +++ b/nimble/host/mesh/src/beacon.c @@ -0,0 +1,387 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include "os/os_mbuf.h" +#include "mesh/mesh.h" + +#define BT_DBG_ENABLED (MYNEWT_VAL(BLE_MESH_DEBUG_BEACON)) +#include "host/ble_hs_log.h" + +#include "adv.h" +#include +#include "net.h" +#include "prov.h" +#include "crypto.h" +#include "beacon.h" +#include "foundation.h" + +#define UNPROVISIONED_INTERVAL (K_SECONDS(5)) +#define PROVISIONED_INTERVAL (K_SECONDS(10)) + +#define BEACON_TYPE_UNPROVISIONED 0x00 +#define BEACON_TYPE_SECURE 0x01 + +/* 3 transmissions, 20ms interval */ +#define UNPROV_XMIT_COUNT 2 +#define UNPROV_XMIT_INT 20 + +/* 1 transmission, 20ms interval */ +#define PROV_XMIT_COUNT 0 +#define PROV_XMIT_INT 20 + +static struct k_delayed_work beacon_timer; + +static struct { + u16_t net_idx; + u8_t data[21]; +} beacon_cache[MYNEWT_VAL(BLE_MESH_SUBNET_COUNT)]; + +static struct bt_mesh_subnet *cache_check(u8_t data[21]) +{ + struct bt_mesh_subnet *sub; + int i; + + for (i = 0; i < ARRAY_SIZE(beacon_cache); i++) { + if (memcmp(beacon_cache[i].data, data, 21)) { + continue; + } + + sub = bt_mesh_subnet_get(beacon_cache[i].net_idx); + if (sub) { + BT_DBG("Match found in cache"); + return sub; + } + } + + return NULL; +} + +static void cache_add(u8_t data[21], u16_t net_idx) +{ + memcpy(beacon_cache[net_idx].data, data, 21); +} + +static void beacon_complete(struct os_mbuf *buf, int err) +{ + struct bt_mesh_subnet *sub; + + BT_DBG("err %d", err); + + sub = &bt_mesh.sub[BT_MESH_ADV(buf)->user_data[0]]; + sub->beacon_sent = k_uptime_get(); +} + +#define BEACON_INTERVAL(sub) K_SECONDS(10 * ((sub)->beacons_last + 1)) + +void bt_mesh_beacon_create(struct bt_mesh_subnet *sub, + struct os_mbuf *buf) +{ + struct bt_mesh_subnet_keys *keys; + u8_t flags; + + net_buf_simple_add_u8(buf, BEACON_TYPE_SECURE); + + if (sub->kr_flag) { + flags = BT_MESH_NET_FLAG_KR; + keys = &sub->keys[1]; + } else { + flags = 0x00; + keys = &sub->keys[0]; + } + + if (bt_mesh.iv_update) { + flags |= BT_MESH_NET_FLAG_IVU; + } + + net_buf_simple_add_u8(buf, flags); + + /* Network ID */ + net_buf_simple_add_mem(buf, keys->net_id, 8); + + /* IV Index */ + net_buf_simple_add_be32(buf, bt_mesh.iv_index); + + net_buf_simple_add_mem(buf, sub->auth, 8); + + BT_DBG("net_idx 0x%04x flags 0x%02x NetID %s", sub->net_idx, + flags, bt_hex(keys->net_id, 8)); + BT_DBG("IV Index 0x%08x Auth %s", bt_mesh.iv_index, + bt_hex(sub->auth, 8)); +} + +static int secure_beacon_send(void) +{ + s64_t threshold; + int i; + + BT_DBG(""); + + /* If the interval has passed or is within 5 seconds from now + * send a beacon. + */ + threshold = k_uptime_get() + K_SECONDS(5); + + for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) { + struct bt_mesh_subnet *sub = &bt_mesh.sub[i]; + struct os_mbuf *buf; + + if (sub->net_idx == BT_MESH_KEY_UNUSED) { + continue; + } + + if (sub->beacon_sent + BEACON_INTERVAL(sub) > threshold) { + continue; + } + + buf = bt_mesh_adv_create(BT_MESH_ADV_BEACON, PROV_XMIT_COUNT, + PROV_XMIT_INT, K_NO_WAIT); + if (!buf) { + BT_ERR("Unable to allocate beacon buffer"); + return -ENOBUFS; + } + + BT_MESH_ADV(buf)->user_data[0] = i; + + bt_mesh_beacon_create(sub, buf); + + bt_mesh_adv_send(buf, beacon_complete); + net_buf_unref(buf); + } + + return 0; +} + +static int unprovisioned_beacon_send(void) +{ +#if (MYNEWT_VAL(BLE_MESH_PB_ADV)) + struct os_mbuf *buf; + + BT_DBG("unprovisioned_beacon_send"); + + buf = bt_mesh_adv_create(BT_MESH_ADV_BEACON, UNPROV_XMIT_COUNT, + UNPROV_XMIT_INT, K_NO_WAIT); + if (!buf) { + BT_ERR("Unable to allocate beacon buffer"); + return -ENOBUFS; + } + + net_buf_add_u8(buf, BEACON_TYPE_UNPROVISIONED); + net_buf_add_mem(buf, bt_mesh_prov_get_uuid(), 16); + + /* OOB Info (2 bytes) + URI Hash (4 bytes) */ + net_buf_add_zeros(buf, 2 + 4); + + bt_mesh_adv_send(buf, NULL); + net_buf_unref(buf); + +#endif /* MYNEWT_VAL(BLE_MESH_PB_ADV) */ + return 0; +} + +static void update_beacon_observation(void) +{ + static bool first_half; + int i; + + /* Observation period is 20 seconds, whereas the beacon timer + * runs every 10 seconds. We process what's happened during the + * window only after the seconnd half. + */ + first_half = !first_half; + if (first_half) { + return; + } + + for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) { + struct bt_mesh_subnet *sub = &bt_mesh.sub[i]; + + if (sub->net_idx == BT_MESH_KEY_UNUSED) { + continue; + } + + sub->beacons_last = sub->beacons_cur; + sub->beacons_cur = 0; + } +} + +static void beacon_send(struct os_event *work) +{ + /* Don't send anything if we have an active provisioning link */ + if ((MYNEWT_VAL(BLE_MESH_PROV)) && bt_prov_active()) { + k_delayed_work_submit(&beacon_timer, UNPROVISIONED_INTERVAL); + return; + } + + BT_DBG(""); + + if (bt_mesh_is_provisioned()) { + update_beacon_observation(); + secure_beacon_send(); + + /* Only resubmit if beaconing is still enabled */ + if (bt_mesh_beacon_get() == BT_MESH_BEACON_ENABLED || + bt_mesh.ivu_initiator) { + k_delayed_work_submit(&beacon_timer, + PROVISIONED_INTERVAL); + } + } else { + unprovisioned_beacon_send(); + k_delayed_work_submit(&beacon_timer, UNPROVISIONED_INTERVAL); + } + +} + +static void secure_beacon_recv(struct os_mbuf *buf) +{ + u8_t *data, *net_id, *auth; + struct bt_mesh_subnet *sub; + u32_t iv_index; + u8_t flags; + bool new_key; + + if (buf->om_len < 21) { + BT_ERR("Too short secure beacon (len %u)", buf->om_len); + return; + } + + sub = cache_check(buf->om_data); + if (sub) { + /* We've seen this beacon before - just update the stats */ + goto update_stats; + } + + /* So we can add to the cache if auth matches */ + data = buf->om_data; + + flags = net_buf_simple_pull_u8(buf); + net_id = buf->om_data; + net_buf_simple_pull(buf, 8); + iv_index = net_buf_simple_pull_be32(buf); + auth = buf->om_data; + + BT_DBG("flags 0x%02x id %s iv_index 0x%08x", + flags, bt_hex(net_id, 8), iv_index); + + sub = bt_mesh_subnet_find(net_id, flags, iv_index, auth, &new_key); + if (!sub) { + BT_DBG("No subnet that matched beacon"); + return; + } + + if (sub->kr_phase == BT_MESH_KR_PHASE_2 && !new_key) { + BT_WARN("Ignoring Phase 2 KR Update secured using old key"); + return; + } + + cache_add(data, sub->net_idx); + + /* If we have NetKey0 accept initiation only from it */ + if (bt_mesh_subnet_get(BT_MESH_KEY_PRIMARY) && + sub->net_idx != BT_MESH_KEY_PRIMARY) { + BT_WARN("Ignoring secure beacon on non-primary subnet"); + goto update_stats; + } + + BT_DBG("net_idx 0x%04x iv_index 0x%08x, current iv_index 0x%08x", + sub->net_idx, iv_index, bt_mesh.iv_index); + + if (bt_mesh.ivu_initiator && + bt_mesh.iv_update == BT_MESH_IV_UPDATE(flags)) { + bt_mesh_beacon_ivu_initiator(false); + } + + bt_mesh_iv_update(iv_index, BT_MESH_IV_UPDATE(flags)); + + if (bt_mesh_kr_update(sub, BT_MESH_KEY_REFRESH(flags), new_key)) { + bt_mesh_net_beacon_update(sub); + } + +update_stats: + if (bt_mesh_beacon_get() == BT_MESH_BEACON_ENABLED && + sub->beacons_cur < 0xff) { + sub->beacons_cur++; + } +} + +void bt_mesh_beacon_recv(struct os_mbuf *buf) +{ + u8_t type; + + BT_DBG("%u bytes: %s", buf->om_len, bt_hex(buf->om_data, buf->om_len)); + + if (buf->om_len < 1) { + BT_ERR("Too short beacon"); + return; + } + + type = net_buf_simple_pull_u8(buf); + switch (type) { + case BEACON_TYPE_UNPROVISIONED: + BT_DBG("Ignoring unprovisioned device beacon"); + break; + case BEACON_TYPE_SECURE: + secure_beacon_recv(buf); + break; + default: + BT_WARN("Unknown beacon type 0x%02x", type); + break; + } +} + +void bt_mesh_beacon_init(void) +{ + k_delayed_work_init(&beacon_timer, beacon_send); + + /* Start beaconing since we're unprovisioned */ + k_work_submit(&beacon_timer.work); +} + +void bt_mesh_beacon_ivu_initiator(bool enable) +{ + bt_mesh.ivu_initiator = enable; + + if (enable) { + k_work_submit(&beacon_timer.work); + } else if (bt_mesh_beacon_get() == BT_MESH_BEACON_DISABLED) { + k_delayed_work_cancel(&beacon_timer); + } +} + +void bt_mesh_beacon_enable(void) +{ + int i; + + if (!bt_mesh_is_provisioned()) { + k_work_submit(&beacon_timer.work); + return; + } + + for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) { + struct bt_mesh_subnet *sub = &bt_mesh.sub[i]; + + if (sub->net_idx == BT_MESH_KEY_UNUSED) { + continue; + } + + sub->beacons_last = 0; + sub->beacons_cur = 0; + + bt_mesh_net_beacon_update(sub); + } + + k_work_submit(&beacon_timer.work); +} + +void bt_mesh_beacon_disable(void) +{ + if (!bt_mesh.ivu_initiator) { + k_delayed_work_cancel(&beacon_timer); + } +} diff --git a/nimble/host/mesh/src/beacon.h b/nimble/host/mesh/src/beacon.h new file mode 100644 index 000000000..35e47989e --- /dev/null +++ b/nimble/host/mesh/src/beacon.h @@ -0,0 +1,31 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef __BEACON_H__ +#define __BEACON_H__ + +#include "os/os_mbuf.h" + +void +bt_mesh_beacon_enable(void); +void +bt_mesh_beacon_disable(void); + +void +bt_mesh_beacon_ivu_initiator(bool enable); + +void +bt_mesh_beacon_recv(struct os_mbuf *buf); + +void +bt_mesh_beacon_create(struct bt_mesh_subnet *sub, struct os_mbuf *buf); + +void +bt_mesh_beacon_init(void); + +#endif diff --git a/nimble/host/mesh/src/cfg.c b/nimble/host/mesh/src/cfg.c new file mode 100644 index 000000000..da55de975 --- /dev/null +++ b/nimble/host/mesh/src/cfg.c @@ -0,0 +1,3155 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include + +#include "mesh/mesh.h" + +#define BT_DBG_ENABLED MYNEWT_VAL(BLE_MESH_DEBUG_MODEL) +#include "host/ble_hs_log.h" + +#include "mesh_priv.h" +#include "adv.h" +#include "net.h" +#include "lpn.h" +#include "transport.h" +#include "crypto.h" +#include "access.h" +#include "beacon.h" +#include "proxy.h" +#include "foundation.h" + +#define DEFAULT_TTL 7 + +static struct bt_mesh_cfg *conf; + +static struct label { + u16_t addr; + u8_t uuid[16]; +} labels[MYNEWT_VAL(BLE_MESH_LABEL_COUNT)]; + +static inline void key_idx_unpack(struct os_mbuf *buf, + u16_t *idx1, u16_t *idx2) +{ + *idx1 = sys_get_le16(&buf->om_data[0]) & 0xfff; + *idx2 = sys_get_le16(&buf->om_data[1]) >> 4; + net_buf_simple_pull(buf, 3); +} + +static inline void key_idx_pack(struct os_mbuf *buf, + u16_t idx1, u16_t idx2) +{ + net_buf_simple_add_le16(buf, idx1 | ((idx2 & 0x00f) << 12)); + net_buf_simple_add_u8(buf, idx2 >> 4); +} + +static void hb_send(struct bt_mesh_model *model) +{ + + struct bt_mesh_cfg *cfg = model->user_data; + u16_t feat = 0; + struct __packed { + u8_t init_ttl; + u16_t feat; + } hb; + struct bt_mesh_msg_ctx ctx = { + .net_idx = cfg->hb_pub.net_idx, + .app_idx = BT_MESH_KEY_UNUSED, + .addr = cfg->hb_pub.dst, + .send_ttl = cfg->hb_pub.ttl, + }; + struct bt_mesh_net_tx tx = { + .sub = bt_mesh_subnet_get(cfg->hb_pub.net_idx), + .ctx = &ctx, + .src = model->elem->addr, + }; + + hb.init_ttl = cfg->hb_pub.ttl; + + if (bt_mesh_relay_get() == BT_MESH_RELAY_ENABLED) { + feat |= BT_MESH_FEAT_RELAY; + } + + if (bt_mesh_friend_get() == BT_MESH_FRIEND_ENABLED) { + feat |= BT_MESH_FEAT_FRIEND; + } + +#if (MYNEWT_VAL(BLE_MESH_LOW_POWER)) + if (bt_mesh.lpn.state != BT_MESH_LPN_DISABLED) { + feat |= BT_MESH_FEAT_LOW_POWER; + } +#endif + + hb.feat = sys_cpu_to_be16(feat); + + BT_DBG("InitTTL %u feat 0x%02x", cfg->hb_pub.ttl, feat); + + bt_mesh_ctl_send(&tx, TRANS_CTL_OP_HEARTBEAT, &hb, sizeof(hb), NULL); +} + +static int comp_add_elem(struct os_mbuf *buf, struct bt_mesh_elem *elem, + bool primary) +{ + struct bt_mesh_model *mod; + int i; + + if (net_buf_simple_tailroom(buf) < + 4 + (elem->model_count * 2) + (elem->vnd_model_count * 2)) { + BT_ERR("Too large device composition"); + return -E2BIG; + } + + net_buf_simple_add_le16(buf, elem->loc); + + net_buf_simple_add_u8(buf, elem->model_count); + net_buf_simple_add_u8(buf, elem->vnd_model_count); + + for (i = 0; i < elem->model_count; i++) { + mod = &elem->models[i]; + net_buf_simple_add_le16(buf, mod->id); + } + + for (i = 0; i < elem->vnd_model_count; i++) { + mod = &elem->vnd_models[i]; + net_buf_simple_add_le16(buf, mod->vnd.company); + net_buf_simple_add_le16(buf, mod->vnd.id); + } + + return 0; +} + +static int comp_get_page_0(struct os_mbuf *buf) +{ + u16_t feat = 0; + const struct bt_mesh_comp *comp; + int i; + + comp = bt_mesh_comp_get(); + + if ((MYNEWT_VAL(BLE_MESH_RELAY))) { + feat |= BT_MESH_FEAT_RELAY; + } + + if ((MYNEWT_VAL(BLE_MESH_LOW_POWER))) { + feat |= BT_MESH_FEAT_LOW_POWER; + } + + net_buf_simple_add_le16(buf, comp->cid); + net_buf_simple_add_le16(buf, comp->pid); + net_buf_simple_add_le16(buf, comp->vid); + net_buf_simple_add_le16(buf, MYNEWT_VAL(BLE_MESH_CRPL)); + net_buf_simple_add_le16(buf, feat); + + for (i = 0; i < comp->elem_count; i++) { + int err; + + err = comp_add_elem(buf, &comp->elem[i], i == 0); + if (err) { + return err; + } + } + + return 0; +} + +static void dev_comp_data_get(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct os_mbuf *sdu = NET_BUF_SIMPLE(BT_MESH_TX_SDU_MAX); + u8_t page; + + BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s", + ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len, + bt_hex(buf->om_data, buf->om_len)); + + page = net_buf_simple_pull_u8(buf); + if (page != 0) { + BT_WARN("Composition page %u not available", page); + page = 0; + } + + bt_mesh_model_msg_init(sdu, OP_DEV_COMP_DATA_STATUS); + + net_buf_simple_add_u8(sdu, page); + if (comp_get_page_0(sdu) < 0) { + BT_ERR("Unable to get composition page 0"); + goto done; + } + + if (bt_mesh_model_send(model, ctx, sdu, NULL, NULL)) { + BT_ERR("Unable to send Device Composition Status response"); + } + +done: + os_mbuf_free_chain(sdu); +} + +static struct bt_mesh_model *get_model(struct bt_mesh_elem *elem, + struct os_mbuf *buf, bool *vnd) +{ + if (buf->om_len < 4) { + u16_t id; + + id = net_buf_simple_pull_le16(buf); + + BT_DBG("ID 0x%04x addr 0x%04x", id, elem->addr); + + *vnd = false; + + return bt_mesh_model_find(elem, id); + } else { + u16_t company, id; + + company = net_buf_simple_pull_le16(buf); + id = net_buf_simple_pull_le16(buf); + + BT_DBG("Company 0x%04x ID 0x%04x addr 0x%04x", company, id, + elem->addr); + + *vnd = true; + + return bt_mesh_model_find_vnd(elem, company, id); + } +} + +static bool app_key_is_valid(u16_t app_idx) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(bt_mesh.app_keys); i++) { + struct bt_mesh_app_key *key = &bt_mesh.app_keys[i]; + + if (key->net_idx != BT_MESH_KEY_UNUSED && + key->app_idx == app_idx) { + return true; + } + } + + return false; +} + +static u8_t _mod_pub_set(struct bt_mesh_model *model, u16_t pub_addr, + u16_t app_idx, u8_t cred_flag, u8_t ttl, u8_t period, + u8_t retransmit) +{ + if (!model->pub) { + return STATUS_NVAL_PUB_PARAM; + } + + if (!(MYNEWT_VAL(BLE_MESH_LOW_POWER)) && cred_flag) { + return STATUS_FEAT_NOT_SUPP; + } + + if (!model->pub->func && period) { + return STATUS_NVAL_PUB_PARAM; + } + + if (pub_addr == BT_MESH_ADDR_UNASSIGNED) { + model->pub->addr = BT_MESH_ADDR_UNASSIGNED; + model->pub->key = 0; + model->pub->cred = 0; + model->pub->ttl = 0; + model->pub->period = 0; + model->pub->retransmit = 0; + + if (model->pub->func) { + k_delayed_work_cancel(&model->pub->timer); + } + + return STATUS_SUCCESS; + } + + if (!bt_mesh_app_key_find(app_idx)) { + return STATUS_INVALID_APPKEY; + } + + model->pub->addr = pub_addr; + model->pub->key = app_idx; + model->pub->cred = cred_flag; + model->pub->ttl = ttl; + model->pub->period = period; + model->pub->retransmit = retransmit; + + if (model->pub->func) { + s32_t period_ms; + + period_ms = bt_mesh_model_pub_period_get(model); + BT_DBG("period %u ms", period_ms); + + if (period_ms) { + k_delayed_work_submit(&model->pub->timer, period_ms); + } else { + k_delayed_work_cancel(&model->pub->timer); + } + } + + return STATUS_SUCCESS; +} + +static u8_t mod_bind(struct bt_mesh_model *model, u16_t key_idx) +{ + int i; + + BT_DBG("key_idx 0x%04x", key_idx); + + if (!app_key_is_valid(key_idx)) { + return STATUS_INVALID_APPKEY; + } + + for (i = 0; i < ARRAY_SIZE(model->keys); i++) { + /* Treat existing binding as success */ + if (model->keys[i] == key_idx) { + return STATUS_SUCCESS; + } + } + + for (i = 0; i < ARRAY_SIZE(model->keys); i++) { + if (model->keys[i] == BT_MESH_KEY_UNUSED) { + model->keys[i] = key_idx; + return STATUS_SUCCESS; + } + } + + return STATUS_INSUFF_RESOURCES; +} + +static u8_t mod_unbind(struct bt_mesh_model *model, u16_t key_idx) +{ + int i; + + BT_DBG("model %p key_idx 0x%04x", model, key_idx); + + if (!app_key_is_valid(key_idx)) { + return STATUS_INVALID_APPKEY; + } + + for (i = 0; i < ARRAY_SIZE(model->keys); i++) { + if (model->keys[i] != key_idx) { + continue; + } + + model->keys[i] = BT_MESH_KEY_UNUSED; + + if (model->pub && model->pub->key == key_idx) { + _mod_pub_set(model, BT_MESH_ADDR_UNASSIGNED, + BT_MESH_KEY_UNUSED, 0, 0, 0, 0); + } + + return STATUS_SUCCESS; + } + + return STATUS_CANNOT_BIND; +} + +static struct bt_mesh_app_key *app_key_alloc(u16_t app_idx) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(bt_mesh.app_keys); i++) { + struct bt_mesh_app_key *key = &bt_mesh.app_keys[i]; + + if (key->net_idx == BT_MESH_KEY_UNUSED) { + return key; + } + } + + return NULL; +} + +static u8_t app_key_set(u16_t net_idx, u16_t app_idx, const u8_t val[16], + bool update) +{ + struct bt_mesh_app_keys *keys; + struct bt_mesh_app_key *key; + struct bt_mesh_subnet *sub; + + BT_DBG("net_idx 0x%04x app_idx %04x update %u val %s", + net_idx, app_idx, update, bt_hex(val, 16)); + + sub = bt_mesh_subnet_get(net_idx); + if (!sub) { + return STATUS_INVALID_NETKEY; + } + + key = bt_mesh_app_key_find(app_idx); + if (update) { + if (!key) { + return STATUS_INVALID_APPKEY; + } + + if (key->net_idx != net_idx) { + return STATUS_INVALID_BINDING; + } + + keys = &key->keys[1]; + + /* The AppKey Update message shall generate an error when node + * is in normal operation, Phase 2, or Phase 3 or in Phase 1 + * when the AppKey Update message on a valid AppKeyIndex when + * the AppKey value is different. + */ + if (sub->kr_phase != BT_MESH_KR_PHASE_1) { + return STATUS_CANNOT_UPDATE; + } + + if (key->updated) { + if (memcmp(keys->val, val, 16)) { + return STATUS_CANNOT_UPDATE; + } else { + return STATUS_SUCCESS; + } + } + + key->updated = true; + } else { + if (key) { + if (key->net_idx == net_idx && + !memcmp(key->keys[0].val, val, 16)) { + return STATUS_SUCCESS; + } + + if (key->net_idx == net_idx) { + return STATUS_IDX_ALREADY_STORED; + } else { + return STATUS_INVALID_NETKEY; + } + } + + key = app_key_alloc(app_idx); + if (!key) { + return STATUS_INSUFF_RESOURCES; + } + + keys = &key->keys[0]; + } + + if (bt_mesh_app_id(val, &keys->id)) { + if (update) { + key->updated = false; + } + + return STATUS_STORAGE_FAIL; + } + + BT_DBG("app_idx 0x%04x AID 0x%02x", app_idx, keys->id); + + key->net_idx = net_idx; + key->app_idx = app_idx; + memcpy(keys->val, val, 16); + + return STATUS_SUCCESS; +} + +static void app_key_add(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 4 + 4); + u16_t key_net_idx, key_app_idx; + u8_t status; + + key_idx_unpack(buf, &key_net_idx, &key_app_idx); + + BT_DBG("AppIdx 0x%04x NetIdx 0x%04x", key_app_idx, key_net_idx); + + bt_mesh_model_msg_init(msg, OP_APP_KEY_STATUS); + + status = app_key_set(key_net_idx, key_app_idx, buf->om_data, false); + BT_DBG("status 0x%02x", status); + net_buf_simple_add_u8(msg, status); + + key_idx_pack(msg, key_net_idx, key_app_idx); + + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send App Key Status response"); + } + + os_mbuf_free_chain(msg); +} + +static void app_key_update(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 4 + 4); + u16_t key_net_idx, key_app_idx; + u8_t status; + + key_idx_unpack(buf, &key_net_idx, &key_app_idx); + + BT_DBG("AppIdx 0x%04x NetIdx 0x%04x", key_app_idx, key_net_idx); + + bt_mesh_model_msg_init(msg, OP_APP_KEY_STATUS); + + status = app_key_set(key_net_idx, key_app_idx, buf->om_data, true); + BT_DBG("status 0x%02x", status); + net_buf_simple_add_u8(msg, status); + + key_idx_pack(msg, key_net_idx, key_app_idx); + + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send App Key Status response"); + } + + os_mbuf_free_chain(msg); +} + +static void _mod_unbind(struct bt_mesh_model *mod, struct bt_mesh_elem *elem, + bool vnd, bool primary, void *user_data) +{ + u16_t *key_idx = user_data; + + mod_unbind(mod, *key_idx); +} + +static void _app_key_del(struct bt_mesh_app_key *key) +{ + bt_mesh_model_foreach(_mod_unbind, &key->app_idx); + + key->net_idx = BT_MESH_KEY_UNUSED; + memset(key->keys, 0, sizeof(key->keys)); +} + +static void app_key_del(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 4 + 4); + u16_t key_net_idx, key_app_idx; + struct bt_mesh_app_key *key; + u8_t status; + + key_idx_unpack(buf, &key_net_idx, &key_app_idx); + + BT_DBG("AppIdx 0x%04x NetIdx 0x%04x", key_app_idx, key_net_idx); + + if (!bt_mesh_subnet_get(key_net_idx)) { + status = STATUS_INVALID_NETKEY; + goto send_status; + } + + key = bt_mesh_app_key_find(key_app_idx); + if (!key) { + /* Treat as success since the client might have missed a + * previous response and is resending the request. + */ + status = STATUS_SUCCESS; + goto send_status; + } + + if (key->net_idx != key_net_idx) { + status = STATUS_INVALID_BINDING; + goto send_status; + } + + _app_key_del(key); + status = STATUS_SUCCESS; + +send_status: + bt_mesh_model_msg_init(msg, OP_APP_KEY_STATUS); + + net_buf_simple_add_u8(msg, status); + + key_idx_pack(msg, key_net_idx, key_app_idx); + + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send App Key Status response"); + } + + os_mbuf_free_chain(msg); +} + +/* Index list length: 3 bytes for every pair and 2 bytes for an odd idx */ +#define IDX_LEN(num) (((num) / 2) * 3 + ((num) % 2) * 2) + +static void app_key_get(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct os_mbuf *msg = + NET_BUF_SIMPLE(2 + 3 + 4 + + IDX_LEN(MYNEWT_VAL(BLE_MESH_APP_KEY_COUNT))); + u16_t get_idx, i, prev; + u8_t status; + + get_idx = net_buf_simple_pull_le16(buf); + if (get_idx > 0xfff) { + BT_ERR("Invalid NetKeyIndex 0x%04x", get_idx); + goto done; + } + + BT_DBG("idx 0x%04x", get_idx); + + bt_mesh_model_msg_init(msg, OP_APP_KEY_LIST); + + if (!bt_mesh_subnet_get(get_idx)) { + status = STATUS_INVALID_NETKEY; + } else { + status = STATUS_SUCCESS; + } + + net_buf_simple_add_u8(msg, status); + net_buf_simple_add_le16(msg, get_idx); + + if (status != STATUS_SUCCESS) { + goto send_status; + } + + prev = BT_MESH_KEY_UNUSED; + for (i = 0; i < ARRAY_SIZE(bt_mesh.app_keys); i++) { + struct bt_mesh_app_key *key = &bt_mesh.app_keys[i]; + + if (key->net_idx != get_idx) { + continue; + } + + if (prev == BT_MESH_KEY_UNUSED) { + prev = key->app_idx; + continue; + } + + key_idx_pack(msg, prev, key->app_idx); + prev = BT_MESH_KEY_UNUSED; + } + + if (prev != BT_MESH_KEY_UNUSED) { + net_buf_simple_add_le16(msg, prev); + } + +send_status: + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send AppKey List"); + } + +done: + os_mbuf_free_chain(msg); +} + +static void beacon_get(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + /* Needed size: opcode (2 bytes) + msg + MIC */ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 1 + 4); + + BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s", + ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len, + bt_hex(buf->om_data, buf->om_len)); + + bt_mesh_model_msg_init(msg, OP_BEACON_STATUS); + net_buf_simple_add_u8(msg, bt_mesh_beacon_get()); + + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send Config Beacon Status response"); + } + os_mbuf_free_chain(msg); +} + +static void beacon_set(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + /* Needed size: opcode (2 bytes) + msg + MIC */ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 1 + 4); + struct bt_mesh_cfg *cfg = model->user_data; + + BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s", + ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len, + bt_hex(buf->om_data, buf->om_len)); + + if (!cfg) { + BT_WARN("No Configuration Server context available"); + } else if (buf->om_data[0] == 0x00 || buf->om_data[0] == 0x01) { + if (buf->om_data[0] != cfg->beacon) { + cfg->beacon = buf->om_data[0]; + + if (cfg->beacon) { + bt_mesh_beacon_enable(); + } else { + bt_mesh_beacon_disable(); + } + } + } else { + BT_WARN("Invalid Config Beacon value 0x%02x", buf->om_data[0]); + return; + } + + bt_mesh_model_msg_init(msg, OP_BEACON_STATUS); + net_buf_simple_add_u8(msg, bt_mesh_beacon_get()); + + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send Config Beacon Status response"); + } + + os_mbuf_free_chain(msg); + +} + +static void default_ttl_get(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + /* Needed size: opcode (2 bytes) + msg + MIC */ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 1 + 4); + + BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s", + ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len, + bt_hex(buf->om_data, buf->om_len)); + + bt_mesh_model_msg_init(msg, OP_DEFAULT_TTL_STATUS); + net_buf_simple_add_u8(msg, bt_mesh_default_ttl_get()); + + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send Default TTL Status response"); + } + + os_mbuf_free_chain(msg); + +} + +static void default_ttl_set(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + /* Needed size: opcode (2 bytes) + msg + MIC */ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 1 + 4); + struct bt_mesh_cfg *cfg = model->user_data; + + BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s", + ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len, + bt_hex(buf->om_data, buf->om_len)); + + if (!cfg) { + BT_WARN("No Configuration Server context available"); + } else if (buf->om_data[0] <= 0x7f && buf->om_data[0] != 0x01) { + cfg->default_ttl = buf->om_data[0]; + } else { + BT_WARN("Prohibited Default TTL value 0x%02x", buf->om_data[0]); + goto done; + } + + bt_mesh_model_msg_init(msg, OP_DEFAULT_TTL_STATUS); + net_buf_simple_add_u8(msg, bt_mesh_default_ttl_get()); + + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send Default TTL Status response"); + } + +done: + os_mbuf_free_chain(msg); +} + +static void send_gatt_proxy_status(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx) +{ + /* Needed size: opcode (2 bytes) + msg + MIC */ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 1 + 4); + + bt_mesh_model_msg_init(msg, OP_GATT_PROXY_STATUS); + net_buf_simple_add_u8(msg, bt_mesh_gatt_proxy_get()); + + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send GATT Proxy Status"); + } + + os_mbuf_free_chain(msg); + +} + +static void gatt_proxy_get(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s", + ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len, + bt_hex(buf->om_data, buf->om_len)); + + send_gatt_proxy_status(model, ctx); +} + +static void gatt_proxy_set(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct bt_mesh_cfg *cfg = model->user_data; + struct bt_mesh_subnet *sub; + + BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s", + ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len, + bt_hex(buf->om_data, buf->om_len)); + + if (buf->om_data[0] != 0x00 && buf->om_data[0] != 0x01) { + BT_WARN("Invalid GATT Proxy value 0x%02x", buf->om_data[0]); + return; + } + + if (!(MYNEWT_VAL(BLE_MESH_GATT_PROXY)) || + bt_mesh_gatt_proxy_get() == BT_MESH_GATT_PROXY_NOT_SUPPORTED) { + goto send_status; + } + + if (!cfg) { + BT_WARN("No Configuration Server context available"); + goto send_status; + } + + BT_DBG("GATT Proxy 0x%02x -> 0x%02x", cfg->gatt_proxy, buf->om_data[0]); + + if (cfg->gatt_proxy == buf->om_data[0]) { + goto send_status; + } + + cfg->gatt_proxy = buf->om_data[0]; + + sub = bt_mesh_subnet_get(cfg->hb_pub.net_idx); + if ((cfg->hb_pub.feat & BT_MESH_FEAT_PROXY) && sub) { + hb_send(model); + } + +send_status: + send_gatt_proxy_status(model, ctx); +} + +static void net_transmit_get(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + /* Needed size: opcode (2 bytes) + msg + MIC */ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 1 + 4); + + BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s", + ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len, + bt_hex(buf->om_data, buf->om_len)); + + bt_mesh_model_msg_init(msg, OP_NET_TRANSMIT_STATUS); + net_buf_simple_add_u8(msg, bt_mesh_net_transmit_get()); + + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send Config Network Transmit Status"); + } + + os_mbuf_free_chain(msg); + +} + +static void net_transmit_set(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + /* Needed size: opcode (2 bytes) + msg + MIC */ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 1 + 4); + struct bt_mesh_cfg *cfg = model->user_data; + + BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s", + ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len, + bt_hex(buf->om_data, buf->om_len)); + + BT_DBG("Transmit 0x%02x (count %u interval %ums)", buf->om_data[0], + TRANSMIT_COUNT(buf->om_data[0]), TRANSMIT_INT(buf->om_data[0])); + + if (!cfg) { + BT_WARN("No Configuration Server context available"); + } else { + cfg->net_transmit = buf->om_data[0]; + } + + bt_mesh_model_msg_init(msg, OP_NET_TRANSMIT_STATUS); + net_buf_simple_add_u8(msg, bt_mesh_net_transmit_get()); + + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send Network Transmit Status"); + } + + os_mbuf_free_chain(msg); +} + +static void relay_get(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + /* Needed size: opcode (2 bytes) + msg + MIC */ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 2 + 4); + + BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s", + ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len, + bt_hex(buf->om_data, buf->om_len)); + + bt_mesh_model_msg_init(msg, OP_RELAY_STATUS); + net_buf_simple_add_u8(msg, bt_mesh_relay_get()); + net_buf_simple_add_u8(msg, bt_mesh_relay_retransmit_get()); + + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send Config Relay Status response"); + } + + os_mbuf_free_chain(msg); + +} + +static void relay_set(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + /* Needed size: opcode (2 bytes) + msg + MIC */ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 2 + 4); + struct bt_mesh_cfg *cfg = model->user_data; + + BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s", + ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len, + bt_hex(buf->om_data, buf->om_len)); + + if (!cfg) { + BT_WARN("No Configuration Server context available"); + } else if (buf->om_data[0] == 0x00 || buf->om_data[0] == 0x01) { + struct bt_mesh_subnet *sub; + + cfg->relay = buf->om_data[0]; + cfg->relay_retransmit = buf->om_data[1]; + + BT_DBG("Relay 0x%02x Retransmit 0x%02x (count %u interval %u)", + cfg->relay, cfg->relay_retransmit, + TRANSMIT_COUNT(cfg->relay_retransmit), + TRANSMIT_INT(cfg->relay_retransmit)); + + sub = bt_mesh_subnet_get(cfg->hb_pub.net_idx); + if ((cfg->hb_pub.feat & BT_MESH_FEAT_RELAY) && sub) { + hb_send(model); + } + } else { + BT_WARN("Invalid Relay value 0x%02x", buf->om_data[0]); + goto done; + } + + bt_mesh_model_msg_init(msg, OP_RELAY_STATUS); + net_buf_simple_add_u8(msg, bt_mesh_relay_get()); + net_buf_simple_add_u8(msg, bt_mesh_relay_retransmit_get()); + + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send Relay Status response"); + } + +done: + os_mbuf_free_chain(msg); + +} + +static void send_mod_pub_status(struct bt_mesh_model *cfg_mod, + struct bt_mesh_msg_ctx *ctx, + u16_t elem_addr, u16_t pub_addr, + bool vnd, struct bt_mesh_model *mod, + u8_t status, u8_t *mod_id) +{ + /* Needed size: opcode (2 bytes) + msg + MIC */ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 14 + 4); + + bt_mesh_model_msg_init(msg, OP_MOD_PUB_STATUS); + + net_buf_simple_add_u8(msg, status); + net_buf_simple_add_le16(msg, elem_addr); + + if (status != STATUS_SUCCESS) { + memset(net_buf_simple_add(msg, 7), 0, 7); + } else { + u16_t idx_cred; + + net_buf_simple_add_le16(msg, pub_addr); + + idx_cred = mod->pub->key | (u16_t)mod->pub->cred << 12; + net_buf_simple_add_le16(msg, idx_cred); + net_buf_simple_add_u8(msg, mod->pub->ttl); + net_buf_simple_add_u8(msg, mod->pub->period); + net_buf_simple_add_u8(msg, mod->pub->retransmit); + } + + if (vnd) { + memcpy(net_buf_simple_add(msg, 4), mod_id, 4); + } else { + memcpy(net_buf_simple_add(msg, 2), mod_id, 2); + } + + bt_mesh_model_send(cfg_mod, ctx, msg, NULL, NULL); + + os_mbuf_free_chain(msg); +} + +static void mod_pub_get(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + u16_t elem_addr, pub_addr = 0; + struct bt_mesh_model *mod; + struct bt_mesh_elem *elem; + u8_t *mod_id, status; + bool vnd; + + elem_addr = net_buf_simple_pull_le16(buf); + mod_id = buf->om_data; + + BT_DBG("elem_addr 0x%04x", elem_addr); + + elem = bt_mesh_elem_find(elem_addr); + if (!elem) { + mod = NULL; + vnd = (buf->om_len == 4); + status = STATUS_INVALID_ADDRESS; + goto send_status; + } + + mod = get_model(elem, buf, &vnd); + if (!mod) { + status = STATUS_INVALID_MODEL; + goto send_status; + } + + if (!mod->pub) { + status = STATUS_NVAL_PUB_PARAM; + goto send_status; + } + + pub_addr = mod->pub->addr; + status = STATUS_SUCCESS; + +send_status: + send_mod_pub_status(model, ctx, elem_addr, pub_addr, vnd, mod, + status, mod_id); +} + +static void mod_pub_set(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + u8_t retransmit, status, pub_ttl, pub_period, cred_flag; + u16_t elem_addr, pub_addr, pub_app_idx; + struct bt_mesh_model *mod; + struct bt_mesh_elem *elem; + u8_t *mod_id; + bool vnd; + + elem_addr = net_buf_simple_pull_le16(buf); + pub_addr = net_buf_simple_pull_le16(buf); + pub_app_idx = net_buf_simple_pull_le16(buf); + cred_flag = ((pub_app_idx >> 12) & BIT_MASK(1)); + pub_app_idx &= BIT_MASK(12); + + pub_ttl = net_buf_simple_pull_u8(buf); + if (pub_ttl > 0x7f && pub_ttl != BT_MESH_TTL_DEFAULT) { + BT_ERR("Invalid TTL value 0x%02x", pub_ttl); + return; + } + + pub_period = net_buf_simple_pull_u8(buf); + retransmit = net_buf_simple_pull_u8(buf); + mod_id = buf->om_data; + + BT_DBG("elem_addr 0x%04x pub_addr 0x%04x cred_flag %u", + elem_addr, pub_addr, cred_flag); + BT_DBG("pub_app_idx 0x%03x, pub_ttl %u pub_period 0x%02x", + pub_app_idx, pub_ttl, pub_period); + BT_DBG("retransmit 0x%02x (count %u interval %ums)", retransmit, + TRANSMIT_COUNT(retransmit), TRANSMIT_INT(retransmit)); + + elem = bt_mesh_elem_find(elem_addr); + if (!elem) { + mod = NULL; + vnd = (buf->om_len == 4); + status = STATUS_INVALID_ADDRESS; + goto send_status; + } + + mod = get_model(elem, buf, &vnd); + if (!mod) { + status = STATUS_INVALID_MODEL; + goto send_status; + } + + status = _mod_pub_set(mod, pub_addr, pub_app_idx, cred_flag, pub_ttl, + pub_period, retransmit); + +send_status: + send_mod_pub_status(model, ctx, elem_addr, pub_addr, vnd, mod, + status, mod_id); +} + +#if MYNEWT_VAL(BLE_MESH_LABEL_COUNT) > 0 +static u16_t va_find(u8_t *label_uuid, struct label **free_slot) +{ + int i; + + if (free_slot) { + *free_slot = NULL; + } + + for (i = 0; i < ARRAY_SIZE(labels); i++) { + if (!BT_MESH_ADDR_IS_VIRTUAL(labels[i].addr)) { + if (free_slot) { + *free_slot = &labels[i]; + } + continue; + } + + if (!memcmp(labels[i].uuid, label_uuid, 16)) { + return labels[i].addr; + } + } + + return BT_MESH_ADDR_UNASSIGNED; +} + +static u8_t va_add(u8_t *label_uuid, u16_t *addr) +{ + struct label *free_slot; + + *addr = va_find(label_uuid, &free_slot); + if (*addr != BT_MESH_ADDR_UNASSIGNED) { + return STATUS_SUCCESS; + } + + if (!free_slot) { + return STATUS_INSUFF_RESOURCES; + } + + if (bt_mesh_virtual_addr(label_uuid, addr) < 0) { + return STATUS_UNSPECIFIED; + } + + free_slot->addr = *addr; + memcpy(free_slot->uuid, label_uuid, 16); + + return STATUS_SUCCESS; +} + +static void mod_pub_va_set(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + u8_t retransmit, status, pub_ttl, pub_period, cred_flag; + u16_t elem_addr, pub_addr, pub_app_idx; + struct bt_mesh_model *mod; + struct bt_mesh_elem *elem; + u8_t *label_uuid; + u8_t *mod_id; + bool vnd; + + elem_addr = net_buf_simple_pull_le16(buf); + label_uuid = buf->om_data; + net_buf_simple_pull(buf, 16); + + pub_app_idx = net_buf_simple_pull_le16(buf); + cred_flag = ((pub_app_idx >> 12) & BIT_MASK(1)); + pub_app_idx &= BIT_MASK(12); + pub_ttl = net_buf_simple_pull_u8(buf); + if (pub_ttl > 0x7f && pub_ttl != BT_MESH_TTL_DEFAULT) { + BT_ERR("Invalid TTL value 0x%02x", pub_ttl); + return; + } + + pub_period = net_buf_simple_pull_u8(buf); + retransmit = net_buf_simple_pull_u8(buf); + mod_id = buf->om_data; + + BT_DBG("elem_addr 0x%04x pub_addr 0x%04x cred_flag %u", + elem_addr, pub_addr, cred_flag); + BT_DBG("pub_app_idx 0x%03x, pub_ttl %u pub_period 0x%02x", + pub_app_idx, pub_ttl, pub_period); + BT_DBG("retransmit 0x%02x (count %u interval %ums)", retransmit, + TRANSMIT_COUNT(retransmit), TRANSMIT_INT(retransmit)); + + elem = bt_mesh_elem_find(elem_addr); + if (!elem) { + mod = NULL; + vnd = (buf->om_len == 4); + pub_addr = 0; + status = STATUS_INVALID_ADDRESS; + goto send_status; + } + + mod = get_model(elem, buf, &vnd); + if (!mod) { + pub_addr = 0; + status = STATUS_INVALID_MODEL; + goto send_status; + } + + status = va_add(label_uuid, &pub_addr); + if (status == STATUS_SUCCESS) { + status = _mod_pub_set(mod, pub_addr, pub_app_idx, cred_flag, + pub_ttl, pub_period, retransmit); + } + +send_status: + send_mod_pub_status(model, ctx, elem_addr, pub_addr, vnd, mod, + status, mod_id); +} +#else +static void mod_pub_va_set(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + u8_t *mod_id, status; + struct bt_mesh_model *mod; + struct bt_mesh_elem *elem; + u16_t elem_addr, pub_addr = 0; + bool vnd; + + elem_addr = net_buf_simple_pull_le16(buf); + net_buf_simple_pull(buf, 16); + mod_id = net_buf_simple_pull(buf, 4); + + BT_DBG("elem_addr 0x%04x", elem_addr); + + elem = bt_mesh_elem_find(elem_addr); + if (!elem) { + mod = NULL; + vnd = (buf->om_len == 4); + status = STATUS_INVALID_ADDRESS; + goto send_status; + } + + mod = get_model(elem, buf, &vnd); + if (!mod) { + status = STATUS_INVALID_MODEL; + goto send_status; + } + + if (!mod->pub) { + status = STATUS_NVAL_PUB_PARAM; + goto send_status; + } + + pub_addr = mod->pub->addr; + status = STATUS_INSUFF_RESOURCES; + +send_status: + send_mod_pub_status(model, ctx, elem_addr, pub_addr, vnd, mod, + status, mod_id); +} +#endif /* MYNEWT_VAL(BLE_MESH_LABEL_COUNT) > 0 */ + +static void send_mod_sub_status(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, u8_t status, + u16_t elem_addr, u16_t sub_addr, u8_t *mod_id, + bool vnd) +{ + /* Needed size: opcode (2 bytes) + msg + MIC */ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 9 + 4); + + bt_mesh_model_msg_init(msg, OP_MOD_SUB_STATUS); + + net_buf_simple_add_u8(msg, status); + net_buf_simple_add_le16(msg, elem_addr); + if (status) { + net_buf_simple_add_le16(msg, BT_MESH_ADDR_UNASSIGNED); + } else { + net_buf_simple_add_le16(msg, sub_addr); + } + + if (vnd) { + memcpy(net_buf_simple_add(msg, 4), mod_id, 4); + } else { + memcpy(net_buf_simple_add(msg, 2), mod_id, 2); + } + + bt_mesh_model_send(model, ctx, msg, NULL, NULL); + os_mbuf_free_chain(msg); +} + +static void mod_sub_add(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + u16_t elem_addr, sub_addr; + struct bt_mesh_model *mod; + struct bt_mesh_elem *elem; + u8_t *mod_id; + u8_t status; + bool vnd; + int i; + + elem_addr = net_buf_simple_pull_le16(buf); + sub_addr = net_buf_simple_pull_le16(buf); + + BT_DBG("elem_addr 0x%04x, sub_addr 0x%04x", elem_addr, sub_addr); + + mod_id = buf->om_data; + + elem = bt_mesh_elem_find(elem_addr); + if (!elem) { + mod = NULL; + vnd = (buf->om_len == 4); + status = STATUS_INVALID_ADDRESS; + goto send_status; + } + + mod = get_model(elem, buf, &vnd); + if (!mod) { + status = STATUS_INVALID_MODEL; + goto send_status; + } + + if (!BT_MESH_ADDR_IS_GROUP(sub_addr)) { + status = STATUS_INVALID_ADDRESS; + goto send_status; + } + + if (bt_mesh_model_find_group(mod, sub_addr)) { + /* Tried to add existing subscription */ + status = STATUS_SUCCESS; + goto send_status; + } + + for (i = 0; i < ARRAY_SIZE(mod->groups); i++) { + if (mod->groups[i] == BT_MESH_ADDR_UNASSIGNED) { + mod->groups[i] = sub_addr; + break; + } + } + + if (i == ARRAY_SIZE(mod->groups)) { + status = STATUS_INSUFF_RESOURCES; + } else { + status = STATUS_SUCCESS; + + if ((MYNEWT_VAL(BLE_MESH_LOW_POWER))) { + bt_mesh_lpn_group_add(sub_addr); + } + } + +send_status: + send_mod_sub_status(model, ctx, status, elem_addr, sub_addr, + mod_id, vnd); +} + +static void mod_sub_del(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + u16_t elem_addr, sub_addr; + struct bt_mesh_model *mod; + struct bt_mesh_elem *elem; + u8_t *mod_id; + u16_t *match; + u8_t status; + bool vnd; + + elem_addr = net_buf_simple_pull_le16(buf); + sub_addr = net_buf_simple_pull_le16(buf); + + BT_DBG("elem_addr 0x%04x sub_addr 0x%04x", elem_addr, sub_addr); + + mod_id = buf->om_data; + + elem = bt_mesh_elem_find(elem_addr); + if (!elem) { + mod = NULL; + vnd = (buf->om_len == 4); + status = STATUS_INVALID_ADDRESS; + goto send_status; + } + + mod = get_model(elem, buf, &vnd); + if (!mod) { + status = STATUS_INVALID_MODEL; + goto send_status; + } + + if (!BT_MESH_ADDR_IS_GROUP(sub_addr)) { + status = STATUS_INVALID_ADDRESS; + goto send_status; + } + + /* An attempt to remove a non-existing address shall be treated + * as a success. + */ + status = STATUS_SUCCESS; + + if ((MYNEWT_VAL(BLE_MESH_LOW_POWER))) { + bt_mesh_lpn_group_del(&sub_addr, 1); + } + + match = bt_mesh_model_find_group(mod, sub_addr); + if (match) { + *match = BT_MESH_ADDR_UNASSIGNED; + } + +send_status: + send_mod_sub_status(model, ctx, status, elem_addr, sub_addr, + mod_id, vnd); +} + +static void mod_sub_overwrite(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + u16_t elem_addr, sub_addr; + struct bt_mesh_model *mod; + struct bt_mesh_elem *elem; + u8_t *mod_id; + u8_t status; + bool vnd; + + elem_addr = net_buf_simple_pull_le16(buf); + sub_addr = net_buf_simple_pull_le16(buf); + + BT_DBG("elem_addr 0x%04x sub_addr 0x%04x", elem_addr, sub_addr); + + mod_id = buf->om_data; + + elem = bt_mesh_elem_find(elem_addr); + if (!elem) { + mod = NULL; + vnd = (buf->om_len == 4); + status = STATUS_INVALID_ADDRESS; + goto send_status; + } + + mod = get_model(elem, buf, &vnd); + if (!mod) { + status = STATUS_INVALID_MODEL; + goto send_status; + } + + if (!BT_MESH_ADDR_IS_GROUP(sub_addr)) { + status = STATUS_INVALID_ADDRESS; + goto send_status; + } + + if ((MYNEWT_VAL(BLE_MESH_LOW_POWER))) { + bt_mesh_lpn_group_del(mod->groups, ARRAY_SIZE(mod->groups)); + } + + /* Clear all subscriptions (0x0000 is the unassigned address) */ + memset(mod->groups, 0, sizeof(mod->groups)); + + if (ARRAY_SIZE(mod->groups) > 0) { + mod->groups[0] = sub_addr; + status = STATUS_SUCCESS; + + if ((MYNEWT_VAL(BLE_MESH_LOW_POWER))) { + bt_mesh_lpn_group_add(sub_addr); + } + } else { + status = STATUS_INSUFF_RESOURCES; + } + + +send_status: + send_mod_sub_status(model, ctx, status, elem_addr, sub_addr, + mod_id, vnd); +} + +static void mod_sub_del_all(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct bt_mesh_model *mod; + struct bt_mesh_elem *elem; + u16_t elem_addr; + u8_t *mod_id; + u8_t status; + bool vnd; + + elem_addr = net_buf_simple_pull_le16(buf); + + BT_DBG("elem_addr 0x%04x", elem_addr); + + mod_id = buf->om_data; + + elem = bt_mesh_elem_find(elem_addr); + if (!elem) { + mod = NULL; + vnd = (buf->om_len == 4); + status = STATUS_INVALID_ADDRESS; + goto send_status; + } + + mod = get_model(elem, buf, &vnd); + if (!mod) { + status = STATUS_INVALID_MODEL; + goto send_status; + } + + if ((MYNEWT_VAL(BLE_MESH_LOW_POWER))) { + bt_mesh_lpn_group_del(mod->groups, ARRAY_SIZE(mod->groups)); + } + + /* Clear all subscriptions (0x0000 is the unassigned address) */ + memset(mod->groups, 0, sizeof(mod->groups)); + + status = STATUS_SUCCESS; + +send_status: + send_mod_sub_status(model, ctx, status, elem_addr, + BT_MESH_ADDR_UNASSIGNED, mod_id, vnd); +} + +static void mod_sub_get(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct os_mbuf *msg = + NET_BUF_SIMPLE(2 + 5 + 4 + + MYNEWT_VAL(BLE_MESH_MODEL_GROUP_COUNT) * 2); + struct bt_mesh_model *mod; + struct bt_mesh_elem *elem; + u16_t addr, id; + int i; + + addr = net_buf_simple_pull_le16(buf); + id = net_buf_simple_pull_le16(buf); + + BT_DBG("addr 0x%04x id 0x%04x", addr, id); + + bt_mesh_model_msg_init(msg, OP_MOD_SUB_LIST); + + elem = bt_mesh_elem_find(addr); + if (!elem) { + net_buf_simple_add_u8(msg, STATUS_INVALID_ADDRESS); + net_buf_simple_add_le16(msg, addr); + net_buf_simple_add_le16(msg, id); + goto send_list; + } + + mod = bt_mesh_model_find(elem, id); + if (!mod) { + net_buf_simple_add_u8(msg, STATUS_INVALID_MODEL); + net_buf_simple_add_le16(msg, addr); + net_buf_simple_add_le16(msg, id); + goto send_list; + } + + net_buf_simple_add_u8(msg, STATUS_SUCCESS); + + net_buf_simple_add_le16(msg, addr); + net_buf_simple_add_le16(msg, id); + + for (i = 0; i < ARRAY_SIZE(mod->groups); i++) { + if (mod->groups[i] != BT_MESH_ADDR_UNASSIGNED) { + net_buf_simple_add_le16(msg, mod->groups[i]); + } + } + +send_list: + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send Model Subscription List"); + } + + os_mbuf_free_chain(msg); + +} + +static void mod_sub_get_vnd(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct os_mbuf *msg = + NET_BUF_SIMPLE(2 + 7 + 4 + + MYNEWT_VAL(BLE_MESH_MODEL_GROUP_COUNT) * 2); + struct bt_mesh_model *mod; + struct bt_mesh_elem *elem; + u16_t company, addr, id; + int i; + + addr = net_buf_simple_pull_le16(buf); + company = net_buf_simple_pull_le16(buf); + id = net_buf_simple_pull_le16(buf); + + BT_DBG("addr 0x%04x company 0x%04x id 0x%04x", addr, company, id); + + bt_mesh_model_msg_init(msg, OP_MOD_SUB_LIST_VND); + + elem = bt_mesh_elem_find(addr); + if (!elem) { + net_buf_simple_add_u8(msg, STATUS_INVALID_ADDRESS); + net_buf_simple_add_le16(msg, addr); + net_buf_simple_add_le16(msg, company); + net_buf_simple_add_le16(msg, id); + goto send_list; + } + + mod = bt_mesh_model_find_vnd(elem, company, id); + if (!mod) { + net_buf_simple_add_u8(msg, STATUS_INVALID_MODEL); + net_buf_simple_add_le16(msg, addr); + net_buf_simple_add_le16(msg, company); + net_buf_simple_add_le16(msg, id); + goto send_list; + } + + net_buf_simple_add_u8(msg, STATUS_SUCCESS); + + net_buf_simple_add_le16(msg, addr); + net_buf_simple_add_le16(msg, company); + net_buf_simple_add_le16(msg, id); + + for (i = 0; i < ARRAY_SIZE(mod->groups); i++) { + if (mod->groups[i] != BT_MESH_ADDR_UNASSIGNED) { + net_buf_simple_add_le16(msg, mod->groups[i]); + } + } + +send_list: + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send Vendor Model Subscription List"); + } + + os_mbuf_free_chain(msg); + +} + +#if MYNEWT_VAL(BLE_MESH_LABEL_COUNT) > 0 +static void mod_sub_va_add(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + u16_t elem_addr, sub_addr; + struct bt_mesh_model *mod; + struct bt_mesh_elem *elem; + u8_t *label_uuid; + u8_t *mod_id; + u8_t status; + bool vnd; + int i; + + elem_addr = net_buf_simple_pull_le16(buf); + label_uuid = buf->om_data; + net_buf_simple_pull(buf, 16); + + BT_DBG("elem_addr 0x%04x", elem_addr); + + mod_id = buf->om_data; + elem = bt_mesh_elem_find(elem_addr); + if (!elem) { + mod = NULL; + vnd = (buf->om_len == 4); + sub_addr = BT_MESH_ADDR_UNASSIGNED; + status = STATUS_INVALID_ADDRESS; + goto send_status; + } + + mod = get_model(elem, buf, &vnd); + if (!mod) { + sub_addr = BT_MESH_ADDR_UNASSIGNED; + status = STATUS_INVALID_MODEL; + goto send_status; + } + + status = va_add(label_uuid, &sub_addr); + if (status != STATUS_SUCCESS) { + goto send_status; + } + + if (bt_mesh_model_find_group(mod, sub_addr)) { + /* Tried to add existing subscription */ + status = STATUS_SUCCESS; + goto send_status; + } + + for (i = 0; i < ARRAY_SIZE(mod->groups); i++) { + if (mod->groups[i] == BT_MESH_ADDR_UNASSIGNED) { + mod->groups[i] = sub_addr; + break; + } + } + + if (i == ARRAY_SIZE(mod->groups)) { + status = STATUS_INSUFF_RESOURCES; + } else { + if ((MYNEWT_VAL(BLE_MESH_LOW_POWER))) { + bt_mesh_lpn_group_add(sub_addr); + } + + status = STATUS_SUCCESS; + } + +send_status: + send_mod_sub_status(model, ctx, status, elem_addr, sub_addr, + mod_id, vnd); +} + +static void mod_sub_va_del(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + u16_t elem_addr, sub_addr; + struct bt_mesh_model *mod; + struct bt_mesh_elem *elem; + u8_t *label_uuid; + u8_t *mod_id; + u16_t *match; + u8_t status; + bool vnd; + + elem_addr = net_buf_simple_pull_le16(buf); + label_uuid = buf->om_data; + net_buf_simple_pull(buf, 16); + + BT_DBG("elem_addr 0x%04x", elem_addr); + + mod_id = buf->om_data; + + elem = bt_mesh_elem_find(elem_addr); + if (!elem) { + mod = NULL; + vnd = (buf->om_len == 4); + sub_addr = BT_MESH_ADDR_UNASSIGNED; + status = STATUS_INVALID_ADDRESS; + goto send_status; + } + + mod = get_model(elem, buf, &vnd); + if (!mod) { + sub_addr = BT_MESH_ADDR_UNASSIGNED; + status = STATUS_INVALID_MODEL; + goto send_status; + } + + sub_addr = va_find(label_uuid, NULL); + if (sub_addr == BT_MESH_ADDR_UNASSIGNED) { + status = STATUS_CANNOT_REMOVE; + goto send_status; + } + + match = bt_mesh_model_find_group(mod, sub_addr); + if (match) { + *match = BT_MESH_ADDR_UNASSIGNED; + status = STATUS_SUCCESS; + } else { + status = STATUS_CANNOT_REMOVE; + } + +send_status: + send_mod_sub_status(model, ctx, status, elem_addr, sub_addr, + mod_id, vnd); +} + +static void mod_sub_va_overwrite(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + u16_t elem_addr, sub_addr; + struct bt_mesh_model *mod; + struct bt_mesh_elem *elem; + u8_t *label_uuid; + u8_t *mod_id; + u8_t status; + bool vnd; + + elem_addr = net_buf_simple_pull_le16(buf); + label_uuid = buf->om_data; + net_buf_simple_pull(buf, 16); + + BT_DBG("elem_addr 0x%04x", elem_addr); + + mod_id = buf->om_data; + + elem = bt_mesh_elem_find(elem_addr); + if (!elem) { + mod = NULL; + vnd = (buf->om_len == 4); + status = STATUS_INVALID_ADDRESS; + goto send_status; + } + + mod = get_model(elem, buf, &vnd); + if (!mod) { + status = STATUS_INVALID_MODEL; + goto send_status; + } + + if ((MYNEWT_VAL(BLE_MESH_LOW_POWER))) { + bt_mesh_lpn_group_del(mod->groups, ARRAY_SIZE(mod->groups)); + } + + /* Clear all subscriptions (0x0000 is the unassigned address) */ + memset(mod->groups, 0, sizeof(mod->groups)); + + if (ARRAY_SIZE(mod->groups) > 0) { + status = va_add(label_uuid, &sub_addr); + if (status == STATUS_SUCCESS) { + mod->groups[0] = sub_addr; + + if ((MYNEWT_VAL(BLE_MESH_LOW_POWER))) { + bt_mesh_lpn_group_add(sub_addr); + } + } + } else { + status = STATUS_INSUFF_RESOURCES; + } + +send_status: + send_mod_sub_status(model, ctx, status, elem_addr, sub_addr, + mod_id, vnd); +} +#else +static void mod_sub_va_add(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct bt_mesh_model *mod; + struct bt_mesh_elem *elem; + u16_t elem_addr; + u8_t *mod_id; + u8_t status; + bool vnd; + + elem_addr = net_buf_simple_pull_le16(buf); + net_buf_simple_pull(buf, 16); + + mod_id = buf->om_data; + + elem = bt_mesh_elem_find(elem_addr); + if (!elem) { + mod = NULL; + vnd = (buf->om_len == 4); + status = STATUS_INVALID_ADDRESS; + goto send_status; + } + + mod = get_model(elem, buf, &vnd); + if (!mod) { + status = STATUS_INVALID_MODEL; + goto send_status; + } + + status = STATUS_INSUFF_RESOURCES; + +send_status: + send_mod_sub_status(model, ctx, status, elem_addr, + BT_MESH_ADDR_UNASSIGNED, mod_id, vnd); +} + +static void mod_sub_va_del(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct bt_mesh_elem *elem; + u16_t elem_addr; + u8_t *mod_id; + u8_t status; + bool vnd; + + elem_addr = net_buf_simple_pull_le16(buf); + net_buf_simple_pull(buf, 16); + + mod_id = buf->om_data; + + elem = bt_mesh_elem_find(elem_addr); + if (!elem) { + vnd = (buf->om_len == 4); + status = STATUS_INVALID_ADDRESS; + goto send_status; + } + + if (!get_model(elem, buf, &vnd)) { + status = STATUS_INVALID_MODEL; + goto send_status; + } + + status = STATUS_INSUFF_RESOURCES; + +send_status: + send_mod_sub_status(model, ctx, status, elem_addr, + BT_MESH_ADDR_UNASSIGNED, mod_id, vnd); +} + +static void mod_sub_va_overwrite(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct bt_mesh_elem *elem; + u16_t elem_addr; + u8_t *mod_id; + u8_t status; + bool vnd; + + elem_addr = net_buf_simple_pull_le16(buf); + net_buf_simple_pull(buf, 18); + + mod_id = buf->om_data; + + elem = bt_mesh_elem_find(elem_addr); + if (!elem) { + vnd = (buf->om_len == 4); + status = STATUS_INVALID_ADDRESS; + goto send_status; + } + + if (!get_model(elem, buf, &vnd)) { + status = STATUS_INVALID_MODEL; + goto send_status; + } + + status = STATUS_INSUFF_RESOURCES; + +send_status: + send_mod_sub_status(model, ctx, status, elem_addr, + BT_MESH_ADDR_UNASSIGNED, mod_id, vnd); +} +#endif /* MYNEWT_VAL(BLE_MESH_LABEL_COUNT) > 0 */ + +static void send_net_key_status(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + u16_t idx, u8_t status) +{ + /* Needed size: opcode (2 bytes) + msg + MIC */ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 3 + 4); + + bt_mesh_model_msg_init(msg, OP_NET_KEY_STATUS); + + net_buf_simple_add_u8(msg, status); + net_buf_simple_add_le16(msg, idx); + + bt_mesh_model_send(model, ctx, msg, NULL, NULL); + + os_mbuf_free_chain(msg); + +} + +static void net_key_add(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct bt_mesh_subnet *sub; + u16_t idx; + int err; + + idx = net_buf_simple_pull_le16(buf); + if (idx > 0xfff) { + BT_ERR("Invalid NetKeyIndex 0x%04x", idx); + return; + } + + BT_DBG("idx 0x%04x", idx); + + sub = bt_mesh_subnet_get(idx); + if (!sub) { + int i; + + for (sub = NULL, i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) { + if (bt_mesh.sub[i].net_idx == BT_MESH_KEY_UNUSED) { + sub = &bt_mesh.sub[i]; + break; + } + } + + if (!sub) { + send_net_key_status(model, ctx, idx, + STATUS_INSUFF_RESOURCES); + return; + } + } + + /* Check for already existing subnet */ + if (sub->net_idx == idx) { + u8_t status; + + if (memcmp(buf->om_data, sub->keys[0].net, 16)) { + status = STATUS_IDX_ALREADY_STORED; + } else { + status = STATUS_SUCCESS; + } + + send_net_key_status(model, ctx, idx, status); + return; + } + + err = bt_mesh_net_keys_create(&sub->keys[0], buf->om_data); + if (err) { + send_net_key_status(model, ctx, idx, STATUS_UNSPECIFIED); + return; + } + + sub->net_idx = idx; + + if ((MYNEWT_VAL(BLE_MESH_GATT_PROXY))) { + sub->node_id = BT_MESH_NODE_IDENTITY_STOPPED; + bt_mesh_proxy_beacon_send(sub); + bt_mesh_adv_update(); + } else { + sub->node_id = BT_MESH_NODE_IDENTITY_NOT_SUPPORTED; + } + + send_net_key_status(model, ctx, idx, STATUS_SUCCESS); +} + +static void net_key_update(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct bt_mesh_subnet *sub; + u16_t idx; + int err; + + idx = net_buf_simple_pull_le16(buf); + if (idx > 0xfff) { + BT_ERR("Invalid NetKeyIndex 0x%04x", idx); + return; + } + + BT_DBG("idx 0x%04x", idx); + + sub = bt_mesh_subnet_get(idx); + if (!sub) { + send_net_key_status(model, ctx, idx, STATUS_INVALID_NETKEY); + return; + } + + /* The node shall successfully process a NetKey Update message on a + * valid NetKeyIndex when the NetKey value is different and the Key + * Refresh procedure has not been started, or when the NetKey value is + * the same in Phase 1. The NetKey Update message shall generate an + * error when the node is in Phase 2, or Phase 3. + */ + switch (sub->kr_phase) { + case BT_MESH_KR_NORMAL: + if (!memcmp(buf->om_data, sub->keys[0].net, 16)) { + return; + } + break; + case BT_MESH_KR_PHASE_1: + if (!memcmp(buf->om_data, sub->keys[1].net, 16)) { + send_net_key_status(model, ctx, idx, STATUS_SUCCESS); + return; + } + break; + case BT_MESH_KR_PHASE_2: + case BT_MESH_KR_PHASE_3: + send_net_key_status(model, ctx, idx, STATUS_CANNOT_UPDATE); + return; + } + + err = bt_mesh_net_keys_create(&sub->keys[1], buf->om_data); + if (!err && ((MYNEWT_VAL(BLE_MESH_LOW_POWER)) || + (MYNEWT_VAL(BLE_MESH_FRIEND)))) { + err = bt_mesh_friend_cred_update(ctx->net_idx, 1, buf->om_data); + } + + if (err) { + send_net_key_status(model, ctx, idx, STATUS_UNSPECIFIED); + return; + } + + sub->kr_phase = BT_MESH_KR_PHASE_1; + + bt_mesh_net_beacon_update(sub); + + send_net_key_status(model, ctx, idx, STATUS_SUCCESS); +} + +static void hb_pub_disable(struct bt_mesh_cfg *cfg) +{ + BT_DBG(""); + + cfg->hb_pub.dst = BT_MESH_ADDR_UNASSIGNED; + cfg->hb_pub.count = 0; + cfg->hb_pub.ttl = 0; + cfg->hb_pub.period = 0; + + k_delayed_work_cancel(&cfg->hb_pub.timer); +} + +static void net_key_del(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct bt_mesh_cfg *cfg = model->user_data; + struct bt_mesh_subnet *sub; + u16_t del_idx, i; + u8_t status; + + del_idx = net_buf_simple_pull_le16(buf); + if (del_idx > 0xfff) { + BT_ERR("Invalid NetKeyIndex 0x%04x", del_idx); + return; + } + + BT_DBG("idx 0x%04x", del_idx); + + sub = bt_mesh_subnet_get(del_idx); + if (!sub) { + /* This could be a retry of a previous attempt that had its + * response lost, so pretend that it was a success. + */ + status = STATUS_SUCCESS; + goto send_status; + } + + /* The key that the message was encrypted with cannot be removed. + * The NetKey List must contain a minimum of one NetKey. + */ + if (ctx->net_idx == del_idx) { + status = STATUS_CANNOT_REMOVE; + goto send_status; + } + + if (cfg->hb_pub.net_idx == del_idx) { + hb_pub_disable(cfg); + } + + /* Delete any app keys bound to this NetKey index */ + for (i = 0; i < ARRAY_SIZE(bt_mesh.app_keys); i++) { + struct bt_mesh_app_key *key = &bt_mesh.app_keys[i]; + + if (key->net_idx == del_idx) { + _app_key_del(key); + } + } + + memset(sub, 0, sizeof(*sub)); + sub->net_idx = BT_MESH_KEY_UNUSED; + + status = STATUS_SUCCESS; + +send_status: + send_net_key_status(model, ctx, del_idx, status); +} + +static void net_key_get(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct os_mbuf *msg = + NET_BUF_SIMPLE(2 + 4 + + IDX_LEN(MYNEWT_VAL(BLE_MESH_SUBNET_COUNT))); + u16_t prev, i; + + bt_mesh_model_msg_init(msg, OP_NET_KEY_LIST); + + prev = BT_MESH_KEY_UNUSED; + for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) { + struct bt_mesh_subnet *sub = &bt_mesh.sub[i]; + + if (sub->net_idx == BT_MESH_KEY_UNUSED) { + continue; + } + + if (prev == BT_MESH_KEY_UNUSED) { + prev = sub->net_idx; + continue; + } + + key_idx_pack(msg, prev, sub->net_idx); + prev = BT_MESH_KEY_UNUSED; + } + + if (prev != BT_MESH_KEY_UNUSED) { + net_buf_simple_add_le16(msg, prev); + } + + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send NetKey List"); + } + + os_mbuf_free_chain(msg); +} + +static void node_identity_get(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + /* Needed size: opcode (2 bytes) + msg + MIC */ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 4 + 4); + struct bt_mesh_subnet *sub; + u8_t node_id; + u16_t idx; + + BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s", + ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len, + bt_hex(buf->om_data, buf->om_len)); + + idx = net_buf_simple_pull_le16(buf); + if (idx > 0xfff) { + BT_ERR("Invalid NetKeyIndex 0x%04x", idx); + goto done; + } + + bt_mesh_model_msg_init(msg, OP_NODE_IDENTITY_STATUS); + + sub = bt_mesh_subnet_get(idx); + if (!sub) { + net_buf_simple_add_u8(msg, STATUS_INVALID_NETKEY); + node_id = 0x00; + } else { + net_buf_simple_add_u8(msg, STATUS_SUCCESS); + node_id = sub->node_id; + } + + net_buf_simple_add_le16(msg, idx); + net_buf_simple_add_u8(msg, node_id); + + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send Node Identity Status"); + } + +done: + os_mbuf_free_chain(msg); +} + +static void node_identity_set(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + /* Needed size: opcode (2 bytes) + msg + MIC */ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 4 + 4); + struct bt_mesh_subnet *sub; + u8_t node_id; + u16_t idx; + + BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s", + ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len, + bt_hex(buf->om_data, buf->om_len)); + + idx = net_buf_simple_pull_le16(buf); + if (idx > 0xfff) { + BT_WARN("Invalid NetKeyIndex 0x%04x", idx); + goto done; + } + + node_id = net_buf_simple_pull_u8(buf); + if (node_id != 0x00 && node_id != 0x01) { + BT_WARN("Invalid Node ID value 0x%02x", node_id); + goto done; + } + + bt_mesh_model_msg_init(msg, OP_NODE_IDENTITY_STATUS); + + sub = bt_mesh_subnet_get(idx); + if (!sub) { + net_buf_simple_add_u8(msg, STATUS_INVALID_NETKEY); + net_buf_simple_add_le16(msg, idx); + net_buf_simple_add_u8(msg, node_id); + } else { + net_buf_simple_add_u8(msg, STATUS_SUCCESS); + net_buf_simple_add_le16(msg, idx); + + if ((MYNEWT_VAL(BLE_MESH_GATT_PROXY))) { + sub->node_id = node_id; + bt_mesh_adv_update(); + } + + net_buf_simple_add_u8(msg, sub->node_id); + } + + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send Node Identity Status"); + } + +done: + os_mbuf_free_chain(msg); + +} + +static void create_mod_app_status(struct os_mbuf *msg, + struct bt_mesh_model *mod, bool vnd, + u16_t elem_addr, u16_t app_idx, + u8_t status, u8_t *mod_id) +{ + bt_mesh_model_msg_init(msg, OP_MOD_APP_STATUS); + + net_buf_simple_add_u8(msg, status); + net_buf_simple_add_le16(msg, elem_addr); + net_buf_simple_add_le16(msg, app_idx); + + if (vnd) { + memcpy(net_buf_simple_add(msg, 4), mod_id, 4); + } else { + memcpy(net_buf_simple_add(msg, 2), mod_id, 2); + } +} + +static void mod_app_bind(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 9 + 4); + u16_t elem_addr, key_app_idx; + struct bt_mesh_model *mod; + struct bt_mesh_elem *elem; + u8_t *mod_id, status; + bool vnd; + + elem_addr = net_buf_simple_pull_le16(buf); + key_app_idx = net_buf_simple_pull_le16(buf); + mod_id = buf->om_data; + + elem = bt_mesh_elem_find(elem_addr); + if (!elem) { + mod = NULL; + vnd = (buf->om_len == 4); + status = STATUS_INVALID_ADDRESS; + goto send_status; + } + + mod = get_model(elem, buf, &vnd); + if (!mod) { + status = STATUS_INVALID_MODEL; + goto send_status; + } + + /* Configuration Server only allows device key based access */ + if (model == mod) { + BT_ERR("Client tried to bind AppKey to Configuration Model"); + status = STATUS_CANNOT_BIND; + goto send_status; + } + + status = mod_bind(mod, key_app_idx); + +send_status: + BT_DBG("status 0x%02x", status); + create_mod_app_status(msg, mod, vnd, elem_addr, key_app_idx, status, + mod_id); + + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send Model App Bind Status response"); + } + + os_mbuf_free_chain(msg); + +} + +static void mod_app_unbind(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 9 + 4); + u16_t elem_addr, key_app_idx; + struct bt_mesh_model *mod; + struct bt_mesh_elem *elem; + u8_t *mod_id, status; + bool vnd; + + elem_addr = net_buf_simple_pull_le16(buf); + key_app_idx = net_buf_simple_pull_le16(buf); + mod_id = buf->om_data; + + elem = bt_mesh_elem_find(elem_addr); + if (!elem) { + mod = NULL; + vnd = (buf->om_len == 4); + status = STATUS_INVALID_ADDRESS; + goto send_status; + } + + mod = get_model(elem, buf, &vnd); + if (!mod) { + status = STATUS_INVALID_MODEL; + goto send_status; + } + + status = mod_unbind(mod, key_app_idx); + +send_status: + BT_DBG("status 0x%02x", status); + create_mod_app_status(msg, mod, vnd, elem_addr, key_app_idx, status, + mod_id); + + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send Model App Unbind Status response"); + } + + os_mbuf_free_chain(msg); +} + +#define KEY_LIST_LEN (MYNEWT_VAL(BLE_MESH_MODEL_KEY_COUNT) * 2) + +static void mod_app_get(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 9 + KEY_LIST_LEN + 4); + struct bt_mesh_model *mod; + struct bt_mesh_elem *elem; + u8_t *mod_id, status; + u16_t elem_addr; + bool vnd; + + elem_addr = net_buf_simple_pull_le16(buf); + mod_id = buf->om_data; + + BT_DBG("elem_addr 0x%04x", elem_addr); + + elem = bt_mesh_elem_find(elem_addr); + if (!elem) { + mod = NULL; + vnd = (buf->om_len == 4); + status = STATUS_INVALID_ADDRESS; + goto send_list; + } + + mod = get_model(elem, buf, &vnd); + if (!mod) { + status = STATUS_INVALID_MODEL; + goto send_list; + } + + status = STATUS_SUCCESS; + +send_list: + if (vnd) { + bt_mesh_model_msg_init(msg, OP_VND_MOD_APP_LIST); + } else { + bt_mesh_model_msg_init(msg, OP_SIG_MOD_APP_LIST); + } + + net_buf_simple_add_u8(msg, status); + net_buf_simple_add_le16(msg, elem_addr); + + if (vnd) { + net_buf_simple_add_mem(msg, mod_id, 4); + } else { + net_buf_simple_add_mem(msg, mod_id, 2); + } + + if (mod) { + int i; + + for (i = 0; i < ARRAY_SIZE(mod->keys); i++) { + if (mod->keys[i] != BT_MESH_KEY_UNUSED) { + net_buf_simple_add_le16(msg, mod->keys[i]); + } + } + } + + bt_mesh_model_send(model, ctx, msg, NULL, NULL); + os_mbuf_free_chain(msg); +} + +static void node_reset(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + /* Needed size: opcode (2 bytes) + msg + MIC */ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 0 + 4); + struct bt_mesh_cfg *cfg = model->user_data; + int i; + + BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s", + ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len, + bt_hex(buf->om_data, buf->om_len)); + + + bt_mesh_model_msg_init(msg, OP_NODE_RESET_STATUS); + + /* Send the response first since we wont have any keys left to + * send it later. + */ + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send Node Reset Status"); + } + + /* Delete all app keys */ + for (i = 0; i < ARRAY_SIZE(bt_mesh.app_keys); i++) { + struct bt_mesh_app_key *key = &bt_mesh.app_keys[i]; + + if (key->net_idx != BT_MESH_KEY_UNUSED) { + _app_key_del(key); + } + } + + for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) { + struct bt_mesh_subnet *sub = &bt_mesh.sub[i]; + + if (cfg->hb_pub.net_idx == sub->net_idx) { + hb_pub_disable(cfg); + } + + memset(sub, 0, sizeof(*sub)); + sub->net_idx = BT_MESH_KEY_UNUSED; + } + + memset(labels, 0, sizeof(labels)); + + bt_mesh_reset(); + os_mbuf_free_chain(msg); +} + +static void send_friend_status(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx) +{ + /* Needed size: opcode (2 bytes) + msg + MIC */ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 1 + 4); + struct bt_mesh_cfg *cfg = model->user_data; + + bt_mesh_model_msg_init(msg, OP_FRIEND_STATUS); + net_buf_simple_add_u8(msg, cfg->frnd); + + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send Friend Status"); + } + os_mbuf_free_chain(msg); +} + +static void friend_get(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s", + ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len, + bt_hex(buf->om_data, buf->om_len)); + + send_friend_status(model, ctx); +} + +static void friend_set(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct bt_mesh_cfg *cfg = model->user_data; + struct bt_mesh_subnet *sub; + + BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x len %u: %s", + ctx->net_idx, ctx->app_idx, ctx->addr, buf->om_len, + bt_hex(buf->om_data, buf->om_len)); + + if (buf->om_data[0] != 0x00 && buf->om_data[0] != 0x01) { + BT_WARN("Invalid Friend value 0x%02x", buf->om_data[0]); + return; + } + + if (!cfg) { + BT_WARN("No Configuration Server context available"); + goto send_status; + } + + BT_DBG("Friend 0x%02x -> 0x%02x", cfg->frnd, buf->om_data[0]); + + if (cfg->frnd == buf->om_data[0]) { + goto send_status; + } + + cfg->frnd = buf->om_data[0]; + + sub = bt_mesh_subnet_get(cfg->hb_pub.net_idx); + if ((cfg->hb_pub.feat & BT_MESH_FEAT_FRIEND) && sub) { + hb_send(model); + } + +send_status: + send_friend_status(model, ctx); +} + +static void lpn_timeout_get(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + /* Needed size: opcode (2 bytes) + msg + MIC */ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 5 + 4); + u16_t lpn_addr; + + lpn_addr = net_buf_simple_pull_le16(buf); + + BT_DBG("net_idx 0x%04x app_idx 0x%04x src 0x%04x lpn_addr 0x%02x", + ctx->net_idx, ctx->app_idx, ctx->addr, lpn_addr); + + bt_mesh_model_msg_init(msg, OP_LPN_TIMEOUT_STATUS); + net_buf_simple_add_le16(msg, lpn_addr); + memset(net_buf_simple_add(msg, 3), 0, 3); + + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send Friend Status"); + } + os_mbuf_free_chain(msg); +} + +static void send_krp_status(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + u16_t idx, u8_t phase, u8_t status) +{ + /* Needed size: opcode (2 bytes) + msg + MIC */ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 4 + 4); + + bt_mesh_model_msg_init(msg, OP_KRP_STATUS); + + net_buf_simple_add_u8(msg, status); + net_buf_simple_add_le16(msg, idx); + net_buf_simple_add_u8(msg, phase); + + bt_mesh_model_send(model, ctx, msg, NULL, NULL); + os_mbuf_free_chain(msg); +} + +static void krp_get(struct bt_mesh_model *model, struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct bt_mesh_subnet *sub; + u16_t idx; + + idx = net_buf_simple_pull_le16(buf); + if (idx > 0xfff) { + BT_ERR("Invalid NetKeyIndex 0x%04x", idx); + return; + } + + BT_DBG("idx 0x%04x", idx); + + sub = bt_mesh_subnet_get(idx); + if (!sub) { + send_krp_status(model, ctx, idx, 0x00, STATUS_INVALID_NETKEY); + } else { + send_krp_status(model, ctx, idx, sub->kr_phase, + STATUS_SUCCESS); + } +} + +static void krp_set(struct bt_mesh_model *model, struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct bt_mesh_subnet *sub; + u8_t phase; + u16_t idx; + + idx = net_buf_simple_pull_le16(buf); + phase = net_buf_simple_pull_u8(buf); + + if (idx > 0xfff) { + BT_ERR("Invalid NetKeyIndex 0x%04x", idx); + return; + } + + BT_DBG("idx 0x%04x transition 0x%02x", idx, phase); + + sub = bt_mesh_subnet_get(idx); + if (!sub) { + send_krp_status(model, ctx, idx, 0x00, STATUS_INVALID_NETKEY); + return; + } + + BT_DBG("%u -> %u", sub->kr_phase, phase); + + if (phase < BT_MESH_KR_PHASE_2 || phase > BT_MESH_KR_PHASE_3 || + (sub->kr_phase == BT_MESH_KR_NORMAL && + phase == BT_MESH_KR_PHASE_2)) { + BT_WARN("Prohibited transition %u -> %u", sub->kr_phase, phase); + return; + } + + if (sub->kr_phase == BT_MESH_KR_PHASE_1 && + phase == BT_MESH_KR_PHASE_2) { + sub->kr_phase = BT_MESH_KR_PHASE_2; + sub->kr_flag = 1; + bt_mesh_net_beacon_update(sub); + } else if ((sub->kr_phase == BT_MESH_KR_PHASE_1 || + sub->kr_phase == BT_MESH_KR_PHASE_2) && + phase == BT_MESH_KR_PHASE_3) { + memcpy(&sub->keys[0], &sub->keys[1], sizeof(sub->keys[0])); + if ((MYNEWT_VAL(BLE_MESH_LOW_POWER)) || + (MYNEWT_VAL(BLE_MESH_FRIEND))) { + bt_mesh_friend_cred_refresh(ctx->net_idx); + } + sub->kr_phase = BT_MESH_KR_NORMAL; + sub->kr_flag = 0; + bt_mesh_net_beacon_update(sub); + } + + send_krp_status(model, ctx, idx, sub->kr_phase, STATUS_SUCCESS); +} + +static u8_t hb_log(u16_t val) +{ + if (!val) { + return 0x00; + } else if (val == 0xffff) { + return 0xff; + } else { + return 32 - __builtin_clz(val); + } +} + +static u16_t hb_pwr2(u8_t val, u8_t sub) +{ + if (!val) { + return 0x0000; + } else if (val == 0xff || val == 0x11) { + return 0xffff; + } else { + return (1 << (val - sub)); + } +} + +struct hb_pub_param { + u16_t dst; + u8_t count_log; + u8_t period_log; + u8_t ttl; + u16_t feat; + u16_t net_idx; +} __packed; + +static void hb_pub_send_status(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, u8_t status, + struct hb_pub_param *orig_msg) +{ + /* Needed size: opcode (1 byte) + msg + MIC */ + struct os_mbuf *msg = NET_BUF_SIMPLE(1 + 10 + 4); + struct bt_mesh_cfg *cfg = model->user_data; + + BT_DBG("src 0x%04x status 0x%02x", ctx->addr, status); + + bt_mesh_model_msg_init(msg, OP_HEARTBEAT_PUB_STATUS); + + net_buf_simple_add_u8(msg, status); + + if (orig_msg) { + memcpy(net_buf_simple_add(msg, sizeof(*orig_msg)), orig_msg, + sizeof(*orig_msg)); + goto send; + } + + net_buf_simple_add_le16(msg, cfg->hb_pub.dst); + net_buf_simple_add_u8(msg, hb_log(cfg->hb_pub.count)); + net_buf_simple_add_u8(msg, cfg->hb_pub.period); + net_buf_simple_add_u8(msg, cfg->hb_pub.ttl); + net_buf_simple_add_le16(msg, cfg->hb_pub.feat); + net_buf_simple_add_le16(msg, cfg->hb_pub.net_idx); + +send: + bt_mesh_model_send(model, ctx, msg, NULL, NULL); + os_mbuf_free_chain(msg); +} + +static void hearbeat_pub_get(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + BT_DBG("src 0x%04x", ctx->addr); + + hb_pub_send_status(model, ctx, STATUS_SUCCESS, NULL); +} + +static void hearbeat_pub_set(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct hb_pub_param *param = (void *)buf->om_data; + struct bt_mesh_cfg *cfg = model->user_data; + u16_t dst, feat, idx; + u8_t status; + + BT_DBG("src 0x%04x", ctx->addr); + + dst = sys_le16_to_cpu(param->dst); + /* All other address types but virtual are valid */ + if (BT_MESH_ADDR_IS_VIRTUAL(dst)) { + status = STATUS_INVALID_ADDRESS; + goto failed; + } + + if (param->count_log > 0x11 && param->count_log != 0xff) { + status = STATUS_CANNOT_SET; + goto failed; + } + + if (param->period_log > 0x10) { + status = STATUS_CANNOT_SET; + goto failed; + } + + if (param->ttl > 0x7f && param->ttl != BT_MESH_TTL_DEFAULT) { + BT_ERR("Invalid TTL value 0x%02x", param->ttl); + return; + } + + feat = sys_le16_to_cpu(param->feat); + + idx = sys_le16_to_cpu(param->net_idx); + if (idx > 0xfff) { + BT_ERR("Invalid NetKeyIndex 0x%04x", idx); + return; + } + + if (!bt_mesh_subnet_get(idx)) { + status = STATUS_INVALID_NETKEY; + goto failed; + } + + cfg->hb_pub.dst = dst; + cfg->hb_pub.period = param->period_log; + cfg->hb_pub.feat = feat; + cfg->hb_pub.net_idx = idx; + + if (dst == BT_MESH_ADDR_UNASSIGNED) { + hb_pub_disable(cfg); + } else { + /* 2^(n-1) */ + cfg->hb_pub.count = hb_pwr2(param->count_log, 1); + cfg->hb_pub.ttl = param->ttl; + + BT_DBG("period %u ms", hb_pwr2(param->period_log, 1) * 1000); + + /* The first Heartbeat message shall be published as soon + * as possible after the Heartbeat Publication Period state + * has been configured for periodic publishing. + */ + if (param->period_log) { + k_work_submit(&cfg->hb_pub.timer.work); + } else { + k_delayed_work_cancel(&cfg->hb_pub.timer); + } + } + + hb_pub_send_status(model, ctx, STATUS_SUCCESS, NULL); + + return; + +failed: + hb_pub_send_status(model, ctx, status, param); +} + +static void hb_sub_send_status(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, u8_t status) +{ + /* Needed size: opcode (2 bytes) + msg + MIC */ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 9 + 4); + struct bt_mesh_cfg *cfg = model->user_data; + u16_t period; + s64_t uptime; + + BT_DBG("src 0x%04x status 0x%02x", ctx->addr, status); + + uptime = k_uptime_get(); + if (uptime > cfg->hb_sub.expiry) { + period = 0; + } else { + period = (cfg->hb_sub.expiry - uptime) / 1000; + } + + bt_mesh_model_msg_init(msg, OP_HEARTBEAT_SUB_STATUS); + + net_buf_simple_add_u8(msg, status); + + net_buf_simple_add_le16(msg, cfg->hb_sub.src); + net_buf_simple_add_le16(msg, cfg->hb_sub.dst); + net_buf_simple_add_u8(msg, hb_log(period)); + net_buf_simple_add_u8(msg, hb_log(cfg->hb_sub.count)); + net_buf_simple_add_u8(msg, cfg->hb_sub.min_hops); + net_buf_simple_add_u8(msg, cfg->hb_sub.max_hops); + + bt_mesh_model_send(model, ctx, msg, NULL, NULL); + os_mbuf_free_chain(msg); +} + +static void hearbeat_sub_get(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + BT_DBG("src 0x%04x", ctx->addr); + + hb_sub_send_status(model, ctx, STATUS_SUCCESS); +} + +static void hearbeat_sub_set(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct bt_mesh_cfg *cfg = model->user_data; + u16_t sub_src, sub_dst; + u8_t sub_period; + s32_t period_ms; + + BT_DBG("src 0x%04x", ctx->addr); + + sub_src = net_buf_simple_pull_le16(buf); + sub_dst = net_buf_simple_pull_le16(buf); + sub_period = net_buf_simple_pull_u8(buf); + + BT_DBG("sub_src 0x%04x sub_dst 0x%04x period 0x%02x", + sub_src, sub_dst, sub_period); + + if (sub_src != BT_MESH_ADDR_UNASSIGNED && + !BT_MESH_ADDR_IS_UNICAST(sub_src)) { + BT_WARN("Prohibited source address"); + return; + } + + if (BT_MESH_ADDR_IS_VIRTUAL(sub_dst)) { + BT_WARN("Prohibited destination address"); + return; + } + + if (sub_period > 0x11) { + BT_WARN("Prohibited subscription period 0x%02x", sub_period); + return; + } + + if (sub_src == BT_MESH_ADDR_UNASSIGNED || + sub_dst == BT_MESH_ADDR_UNASSIGNED || + sub_period == 0x00) { + cfg->hb_sub.src = BT_MESH_ADDR_UNASSIGNED; + cfg->hb_sub.dst = BT_MESH_ADDR_UNASSIGNED; + period_ms = 0; + } else { + cfg->hb_sub.src = sub_src; + cfg->hb_sub.dst = sub_dst; + period_ms = hb_pwr2(sub_period, 1) * 1000; + } + + BT_DBG("period_ms %u", period_ms); + + cfg->hb_sub.count = 0; + cfg->hb_sub.min_hops = 0x7f; + cfg->hb_sub.max_hops = 0; + + if (period_ms) { + cfg->hb_sub.expiry = k_uptime_get() + period_ms; + } + + hb_sub_send_status(model, ctx, STATUS_SUCCESS); +} + +const struct bt_mesh_model_op bt_mesh_cfg_op[] = { + { OP_DEV_COMP_DATA_GET, 1, dev_comp_data_get }, + { OP_APP_KEY_ADD, 19, app_key_add }, + { OP_APP_KEY_UPDATE, 19, app_key_update }, + { OP_APP_KEY_DEL, 3, app_key_del }, + { OP_APP_KEY_GET, 2, app_key_get }, + { OP_BEACON_GET, 0, beacon_get }, + { OP_BEACON_SET, 1, beacon_set }, + { OP_DEFAULT_TTL_GET, 0, default_ttl_get }, + { OP_DEFAULT_TTL_SET, 1, default_ttl_set }, + { OP_GATT_PROXY_GET, 0, gatt_proxy_get }, + { OP_GATT_PROXY_SET, 1, gatt_proxy_set }, + { OP_NET_TRANSMIT_GET, 0, net_transmit_get }, + { OP_NET_TRANSMIT_SET, 1, net_transmit_set }, + { OP_RELAY_GET, 0, relay_get }, + { OP_RELAY_SET, 2, relay_set }, + { OP_MOD_PUB_GET, 4, mod_pub_get }, + { OP_MOD_PUB_SET, 11, mod_pub_set }, + { OP_MOD_PUB_VA_SET, 24, mod_pub_va_set }, + { OP_MOD_SUB_ADD, 6, mod_sub_add }, + { OP_MOD_SUB_VA_ADD, 20, mod_sub_va_add }, + { OP_MOD_SUB_DEL, 6, mod_sub_del }, + { OP_MOD_SUB_VA_DEL, 20, mod_sub_va_del }, + { OP_MOD_SUB_OVERWRITE, 6, mod_sub_overwrite }, + { OP_MOD_SUB_VA_OVERWRITE, 20, mod_sub_va_overwrite }, + { OP_MOD_SUB_DEL_ALL, 4, mod_sub_del_all }, + { OP_MOD_SUB_GET, 4, mod_sub_get }, + { OP_MOD_SUB_GET_VND, 6, mod_sub_get_vnd }, + { OP_NET_KEY_ADD, 18, net_key_add }, + { OP_NET_KEY_UPDATE, 18, net_key_update }, + { OP_NET_KEY_DEL, 2, net_key_del }, + { OP_NET_KEY_GET, 0, net_key_get }, + { OP_NODE_IDENTITY_GET, 2, node_identity_get }, + { OP_NODE_IDENTITY_SET, 3, node_identity_set }, + { OP_MOD_APP_BIND, 6, mod_app_bind }, + { OP_MOD_APP_UNBIND, 6, mod_app_unbind }, + { OP_SIG_MOD_APP_GET, 4, mod_app_get }, + { OP_VND_MOD_APP_GET, 6, mod_app_get }, + { OP_NODE_RESET, 0, node_reset }, + { OP_FRIEND_GET, 0, friend_get }, + { OP_FRIEND_SET, 1, friend_set }, + { OP_LPN_TIMEOUT_GET, 2, lpn_timeout_get }, + { OP_KRP_GET, 2, krp_get }, + { OP_KRP_SET, 3, krp_set }, + { OP_HEARTBEAT_PUB_GET, 0, hearbeat_pub_get }, + { OP_HEARTBEAT_PUB_SET, 9, hearbeat_pub_set }, + { OP_HEARTBEAT_SUB_GET, 0, hearbeat_sub_get }, + { OP_HEARTBEAT_SUB_SET, 5, hearbeat_sub_set }, + BT_MESH_MODEL_OP_END, +}; + +static void hb_publish(struct os_event *work) +{ + struct bt_mesh_cfg *cfg = work->ev_arg; + struct bt_mesh_model *model = cfg->model; + struct bt_mesh_subnet *sub; + u16_t period_ms; + + BT_DBG(""); + + sub = bt_mesh_subnet_get(cfg->hb_pub.net_idx); + if (!sub) { + BT_ERR("No matching subnet for idx 0x%02x", + cfg->hb_pub.net_idx); + cfg->hb_pub.dst = BT_MESH_ADDR_UNASSIGNED; + return; + } + + hb_send(model); + + if (cfg->hb_pub.count != 0xffff) { + cfg->hb_pub.count--; + } + + period_ms = hb_pwr2(cfg->hb_pub.period, 1) * 1000; + if (period_ms) { + k_delayed_work_submit(&cfg->hb_pub.timer, period_ms); + } +} + +static bool conf_is_valid(struct bt_mesh_cfg *cfg) +{ + if (cfg->relay > 0x02) { + return false; + } + + if (cfg->beacon > 0x01) { + return false; + } + + if (cfg->default_ttl > 0x7f) { + return false; + } + + return true; +} + +int bt_mesh_conf_init(struct bt_mesh_model *model, bool primary) +{ + struct bt_mesh_cfg *cfg = model->user_data; + + if (!cfg) { + BT_ERR("No Configuration Server context provided"); + return -EINVAL; + } + + if (!conf_is_valid(cfg)) { + BT_ERR("Invalid values in configuration"); + return -EINVAL; + } + + /* Configuration Model security is device-key based */ + model->keys[0] = BT_MESH_KEY_DEV; + + if (!(MYNEWT_VAL(BLE_MESH_RELAY))) { + cfg->relay = BT_MESH_RELAY_NOT_SUPPORTED; + } + + if (!(MYNEWT_VAL(BLE_MESH_FRIEND))) { + cfg->frnd = BT_MESH_RELAY_NOT_SUPPORTED; + } + + if (!(MYNEWT_VAL(BLE_MESH_GATT_PROXY))) { + cfg->gatt_proxy = BT_MESH_GATT_PROXY_NOT_SUPPORTED; + } + + k_delayed_work_init(&cfg->hb_pub.timer, hb_publish); + cfg->hb_sub.expiry = 0; + + cfg->model = model; + + conf = cfg; + + return 0; +} + +void bt_mesh_heartbeat(u16_t src, u16_t dst, u8_t hops, u16_t feat) +{ + struct bt_mesh_cfg *cfg = conf; + + if (!cfg) { + BT_WARN("No configuaration server context available"); + return; + } + + if (src != cfg->hb_sub.src || dst != cfg->hb_sub.dst) { + BT_WARN("No subscription for received heartbeat"); + return; + } + + if (k_uptime_get() > cfg->hb_sub.expiry) { + BT_WARN("Heartbeat subscription period expired"); + return; + } + + cfg->hb_sub.min_hops = min(cfg->hb_sub.min_hops, hops); + cfg->hb_sub.max_hops = max(cfg->hb_sub.max_hops, hops); + + if (cfg->hb_sub.count < 0xffff) { + cfg->hb_sub.count++; + } + + BT_DBG("src 0x%04x dst 0x%04x hops %u min %u max %u count %u", src, + dst, hops, cfg->hb_sub.min_hops, cfg->hb_sub.max_hops, + cfg->hb_sub.count); + + if (cfg->hb_sub.func) { + cfg->hb_sub.func(hops, feat); + } +} + +u8_t bt_mesh_net_transmit_get(void) +{ + if (conf) { + return conf->net_transmit; + } + + return 0; +} + +u8_t bt_mesh_relay_get(void) +{ + if (conf) { + return conf->relay; + } + + return BT_MESH_RELAY_NOT_SUPPORTED; +} + +u8_t bt_mesh_friend_get(void) +{ + BT_DBG("conf %p conf->frnd 0x%02x", conf, conf->frnd); + + if (conf) { + return conf->frnd; + } + + return BT_MESH_FRIEND_NOT_SUPPORTED; +} + +u8_t bt_mesh_relay_retransmit_get(void) +{ + if (conf) { + return conf->relay_retransmit; + } + + return 0; +} + +u8_t bt_mesh_beacon_get(void) +{ + if (conf) { + return conf->beacon; + } + + return BT_MESH_BEACON_DISABLED; +} + +u8_t bt_mesh_gatt_proxy_get(void) +{ + if (conf) { + return conf->gatt_proxy; + } + + return BT_MESH_GATT_PROXY_NOT_SUPPORTED; +} + +u8_t bt_mesh_default_ttl_get(void) +{ + if (conf) { + return conf->default_ttl; + } + + return DEFAULT_TTL; +} + +u8_t *bt_mesh_label_uuid_get(u16_t addr) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(labels); i++) { + if (labels[i].addr == addr) { + return labels[i].uuid; + } + } + + return NULL; +} diff --git a/nimble/host/mesh/src/crypto.c b/nimble/host/mesh/src/crypto.c new file mode 100644 index 000000000..150d75553 --- /dev/null +++ b/nimble/host/mesh/src/crypto.c @@ -0,0 +1,869 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include + +#include +#include +#include +#include +#include + +#define BT_DBG_ENABLED (MYNEWT_VAL(BLE_MESH_DEBUG_CRYPTO)) +#include "host/ble_hs_log.h" + +#include "crypto.h" + +#define NET_MIC_LEN(pdu) (((pdu)[1] & 0x80) ? 8 : 4) + +int bt_mesh_aes_cmac(const u8_t key[16], struct bt_mesh_sg *sg, + size_t sg_len, u8_t mac[16]) +{ + struct tc_aes_key_sched_struct sched; + struct tc_cmac_struct state; + + if (tc_cmac_setup(&state, key, &sched) == TC_CRYPTO_FAIL) { + return -EIO; + } + + for (; sg_len; sg_len--, sg++) { + if (tc_cmac_update(&state, sg->data, + sg->len) == TC_CRYPTO_FAIL) { + return -EIO; + } + } + + if (tc_cmac_final(mac, &state) == TC_CRYPTO_FAIL) { + return -EIO; + } + + return 0; +} + +int bt_mesh_k1(const u8_t *ikm, size_t ikm_len, const u8_t salt[16], + const char *info, u8_t okm[16]) +{ + int err; + + err = bt_mesh_aes_cmac_one(salt, ikm, ikm_len, okm); + if (err < 0) { + return err; + } + + return bt_mesh_aes_cmac_one(okm, info, strlen(info), okm); +} + +int bt_mesh_k2(const u8_t n[16], const u8_t *p, size_t p_len, + u8_t net_id[1], u8_t enc_key[16], u8_t priv_key[16]) +{ + struct bt_mesh_sg sg[3]; + u8_t salt[16]; + u8_t out[16]; + u8_t t[16]; + u8_t pad; + int err; + + BT_DBG("n %s", bt_hex(n, 16)); + BT_DBG("p %s", bt_hex(p, p_len)); + + err = bt_mesh_s1("smk2", salt); + if (err) { + return err; + } + + err = bt_mesh_aes_cmac_one(salt, n, 16, t); + if (err) { + return err; + } + + pad = 0x01; + + sg[0].data = NULL; + sg[0].len = 0; + sg[1].data = p; + sg[1].len = p_len; + sg[2].data = &pad; + sg[2].len = sizeof(pad); + + err = bt_mesh_aes_cmac(t, sg, ARRAY_SIZE(sg), out); + if (err) { + return err; + } + + net_id[0] = out[15] & 0x7f; + + sg[0].data = out; + sg[0].len = sizeof(out); + pad = 0x02; + + err = bt_mesh_aes_cmac(t, sg, ARRAY_SIZE(sg), out); + if (err) { + return err; + } + + memcpy(enc_key, out, 16); + + pad = 0x03; + + err = bt_mesh_aes_cmac(t, sg, ARRAY_SIZE(sg), out); + if (err) { + return err; + } + + memcpy(priv_key, out, 16); + + BT_DBG("NID 0x%02x enc_key %s", net_id[0], bt_hex(enc_key, 16)); + BT_DBG("priv_key %s", bt_hex(priv_key, 16)); + + return 0; +} + +int bt_mesh_k3(const u8_t n[16], u8_t out[8]) +{ + u8_t id64[] = { 'i', 'd', '6', '4', 0x01 }; + u8_t tmp[16]; + u8_t t[16]; + int err; + + err = bt_mesh_s1("smk3", tmp); + if (err) { + return err; + } + + err = bt_mesh_aes_cmac_one(tmp, n, 16, t); + if (err) { + return err; + } + + err = bt_mesh_aes_cmac_one(t, id64, sizeof(id64), tmp); + if (err) { + return err; + } + + memcpy(out, tmp + 8, 8); + + return 0; +} + +int bt_mesh_k4(const u8_t n[16], u8_t out[1]) +{ + u8_t id6[] = { 'i', 'd', '6', 0x01 }; + u8_t tmp[16]; + u8_t t[16]; + int err; + + err = bt_mesh_s1("smk4", tmp); + if (err) { + return err; + } + + err = bt_mesh_aes_cmac_one(tmp, n, 16, t); + if (err) { + return err; + } + + err = bt_mesh_aes_cmac_one(t, id6, sizeof(id6), tmp); + if (err) { + return err; + } + + out[0] = tmp[15] & BIT_MASK(6); + + return 0; +} + +int bt_mesh_id128(const u8_t n[16], const char *s, u8_t out[16]) +{ + const char *id128 = "id128\x01"; + u8_t salt[16]; + int err; + + err = bt_mesh_s1(s, salt); + if (err) { + return err; + } + + return bt_mesh_k1(n, 16, salt, id128, out); +} + +static int bt_mesh_ccm_decrypt(const u8_t key[16], u8_t nonce[13], + const u8_t *enc_msg, size_t msg_len, + const u8_t *aad, size_t aad_len, + u8_t *out_msg, size_t mic_size) +{ + u8_t msg[16], pmsg[16], cmic[16], cmsg[16], Xn[16], mic[16]; + u16_t last_blk, blk_cnt; + size_t i, j; + int err; + + if (msg_len < 1 || aad_len >= 0xff00) { + return -EINVAL; + } + + /* C_mic = e(AppKey, 0x01 || nonce || 0x0000) */ + pmsg[0] = 0x01; + memcpy(pmsg + 1, nonce, 13); + sys_put_be16(0x0000, pmsg + 14); + + err = bt_encrypt_be(key, pmsg, cmic); + if (err) { + return err; + } + + /* X_0 = e(AppKey, 0x09 || nonce || length) */ + if (mic_size == sizeof(u64_t)) { + pmsg[0] = 0x19 | (aad_len ? 0x40 : 0x00); + } else { + pmsg[0] = 0x09 | (aad_len ? 0x40 : 0x00); + } + + memcpy(pmsg + 1, nonce, 13); + sys_put_be16(msg_len, pmsg + 14); + + err = bt_encrypt_be(key, pmsg, Xn); + if (err) { + return err; + } + + /* If AAD is being used to authenticate, include it here */ + if (aad_len) { + sys_put_be16(aad_len, pmsg); + + for (i = 0; i < sizeof(u16_t); i++) { + pmsg[i] = Xn[i] ^ pmsg[i]; + } + + j = 0; + aad_len += sizeof(u16_t); + while (aad_len > 16) { + do { + pmsg[i] = Xn[i] ^ aad[j]; + i++, j++; + } while (i < 16); + + aad_len -= 16; + i = 0; + + err = bt_encrypt_be(key, pmsg, Xn); + if (err) { + return err; + } + } + + for (i = 0; i < aad_len; i++, j++) { + pmsg[i] = Xn[i] ^ aad[j]; + } + + for (i = aad_len; i < 16; i++) { + pmsg[i] = Xn[i]; + } + + err = bt_encrypt_be(key, pmsg, Xn); + if (err) { + return err; + } + } + + last_blk = msg_len % 16; + blk_cnt = (msg_len + 15) / 16; + if (!last_blk) { + last_blk = 16; + } + + for (j = 0; j < blk_cnt; j++) { + if (j + 1 == blk_cnt) { + /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */ + pmsg[0] = 0x01; + memcpy(pmsg + 1, nonce, 13); + sys_put_be16(j + 1, pmsg + 14); + + err = bt_encrypt_be(key, pmsg, cmsg); + if (err) { + return err; + } + + /* Encrypted = Payload[0-15] ^ C_1 */ + for (i = 0; i < last_blk; i++) { + msg[i] = enc_msg[(j * 16) + i] ^ cmsg[i]; + } + + memcpy(out_msg + (j * 16), msg, last_blk); + + /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */ + for (i = 0; i < last_blk; i++) { + pmsg[i] = Xn[i] ^ msg[i]; + } + + for (i = last_blk; i < 16; i++) { + pmsg[i] = Xn[i] ^ 0x00; + } + + err = bt_encrypt_be(key, pmsg, Xn); + if (err) { + return err; + } + + /* MIC = C_mic ^ X_1 */ + for (i = 0; i < sizeof(mic); i++) { + mic[i] = cmic[i] ^ Xn[i]; + } + } else { + /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */ + pmsg[0] = 0x01; + memcpy(pmsg + 1, nonce, 13); + sys_put_be16(j + 1, pmsg + 14); + + err = bt_encrypt_be(key, pmsg, cmsg); + if (err) { + return err; + } + + /* Encrypted = Payload[0-15] ^ C_1 */ + for (i = 0; i < 16; i++) { + msg[i] = enc_msg[(j * 16) + i] ^ cmsg[i]; + } + + memcpy(out_msg + (j * 16), msg, 16); + + /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */ + for (i = 0; i < 16; i++) { + pmsg[i] = Xn[i] ^ msg[i]; + } + + err = bt_encrypt_be(key, pmsg, Xn); + if (err) { + return err; + } + } + } + + if (memcmp(mic, enc_msg + msg_len, mic_size)) { + return -EBADMSG; + } + + return 0; +} + +static int bt_mesh_ccm_encrypt(const u8_t key[16], u8_t nonce[13], + const u8_t *msg, size_t msg_len, + const u8_t *aad, size_t aad_len, + u8_t *out_msg, size_t mic_size) +{ + u8_t pmsg[16], cmic[16], cmsg[16], mic[16], Xn[16]; + u16_t blk_cnt, last_blk; + size_t i, j; + int err; + + BT_DBG("key %s", bt_hex(key, 16)); + BT_DBG("nonce %s", bt_hex(nonce, 13)); + BT_DBG("msg (len %zu) %s", msg_len, bt_hex(msg, msg_len)); + BT_DBG("aad_len %zu mic_size %zu", aad_len, mic_size); + + /* Unsupported AAD size */ + if (aad_len >= 0xff00) { + return -EINVAL; + } + + /* C_mic = e(AppKey, 0x01 || nonce || 0x0000) */ + pmsg[0] = 0x01; + memcpy(pmsg + 1, nonce, 13); + sys_put_be16(0x0000, pmsg + 14); + + err = bt_encrypt_be(key, pmsg, cmic); + if (err) { + return err; + } + + /* X_0 = e(AppKey, 0x09 || nonce || length) */ + if (mic_size == sizeof(u64_t)) { + pmsg[0] = 0x19 | (aad_len ? 0x40 : 0x00); + } else { + pmsg[0] = 0x09 | (aad_len ? 0x40 : 0x00); + } + + memcpy(pmsg + 1, nonce, 13); + sys_put_be16(msg_len, pmsg + 14); + + err = bt_encrypt_be(key, pmsg, Xn); + if (err) { + return err; + } + + /* If AAD is being used to authenticate, include it here */ + if (aad_len) { + sys_put_be16(aad_len, pmsg); + + for (i = 0; i < sizeof(u16_t); i++) { + pmsg[i] = Xn[i] ^ pmsg[i]; + } + + j = 0; + aad_len += sizeof(u16_t); + while (aad_len > 16) { + do { + pmsg[i] = Xn[i] ^ aad[j]; + i++, j++; + } while (i < 16); + + aad_len -= 16; + i = 0; + + err = bt_encrypt_be(key, pmsg, Xn); + if (err) { + return err; + } + } + + for (i = 0; i < aad_len; i++, j++) { + pmsg[i] = Xn[i] ^ aad[j]; + } + + for (i = aad_len; i < 16; i++) { + pmsg[i] = Xn[i]; + } + + err = bt_encrypt_be(key, pmsg, Xn); + if (err) { + return err; + } + } + + last_blk = msg_len % 16; + blk_cnt = (msg_len + 15) / 16; + if (!last_blk) { + last_blk = 16; + } + + for (j = 0; j < blk_cnt; j++) { + if (j + 1 == blk_cnt) { + /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */ + for (i = 0; i < last_blk; i++) { + pmsg[i] = Xn[i] ^ msg[(j * 16) + i]; + } + for (i = last_blk; i < 16; i++) { + pmsg[i] = Xn[i] ^ 0x00; + } + + err = bt_encrypt_be(key, pmsg, Xn); + if (err) { + return err; + } + + /* MIC = C_mic ^ X_1 */ + for (i = 0; i < sizeof(mic); i++) { + mic[i] = cmic[i] ^ Xn[i]; + } + + /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */ + pmsg[0] = 0x01; + memcpy(pmsg + 1, nonce, 13); + sys_put_be16(j + 1, pmsg + 14); + + err = bt_encrypt_be(key, pmsg, cmsg); + if (err) { + return err; + } + + /* Encrypted = Payload[0-15] ^ C_1 */ + for (i = 0; i < last_blk; i++) { + out_msg[(j * 16) + i] = + msg[(j * 16) + i] ^ cmsg[i]; + } + } else { + /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */ + for (i = 0; i < 16; i++) { + pmsg[i] = Xn[i] ^ msg[(j * 16) + i]; + } + + err = bt_encrypt_be(key, pmsg, Xn); + if (err) { + return err; + } + + /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */ + pmsg[0] = 0x01; + memcpy(pmsg + 1, nonce, 13); + sys_put_be16(j + 1, pmsg + 14); + + err = bt_encrypt_be(key, pmsg, cmsg); + if (err) { + return err; + } + + /* Encrypted = Payload[0-15] ^ C_N */ + for (i = 0; i < 16; i++) { + out_msg[(j * 16) + i] = + msg[(j * 16) + i] ^ cmsg[i]; + } + + } + } + + memcpy(out_msg + msg_len, mic, mic_size); + + return 0; +} + +#if (MYNEWT_VAL(BLE_MESH_PROXY)) +static void create_proxy_nonce(u8_t nonce[13], const u8_t *pdu, + u32_t iv_index) +{ + /* Nonce Type */ + nonce[0] = 0x03; + + /* Pad */ + nonce[1] = 0x00; + + /* Sequence Number */ + nonce[2] = pdu[2]; + nonce[3] = pdu[3]; + nonce[4] = pdu[4]; + + /* Source Address */ + nonce[5] = pdu[5]; + nonce[6] = pdu[6]; + + /* Pad */ + nonce[7] = 0; + nonce[8] = 0; + + /* IV Index */ + sys_put_be32(iv_index, &nonce[9]); +} +#endif /* PROXY */ + +static void create_net_nonce(u8_t nonce[13], const u8_t *pdu, + u32_t iv_index) +{ + /* Nonce Type */ + nonce[0] = 0x00; + + /* FRND + TTL */ + nonce[1] = pdu[1]; + + /* Sequence Number */ + nonce[2] = pdu[2]; + nonce[3] = pdu[3]; + nonce[4] = pdu[4]; + + /* Source Address */ + nonce[5] = pdu[5]; + nonce[6] = pdu[6]; + + /* Pad */ + nonce[7] = 0; + nonce[8] = 0; + + /* IV Index */ + sys_put_be32(iv_index, &nonce[9]); +} + +int bt_mesh_net_obfuscate(u8_t *pdu, u32_t iv_index, + const u8_t privacy_key[16]) +{ + u8_t priv_rand[16] = { 0x00, 0x00, 0x00, 0x00, 0x00, }; + u8_t tmp[16]; + int err, i; + + BT_DBG("IVIndex %u, PrivacyKey %s", iv_index, bt_hex(privacy_key, 16)); + + sys_put_be32(iv_index, &priv_rand[5]); + memcpy(&priv_rand[9], &pdu[7], 7); + + BT_DBG("PrivacyRandom %s", bt_hex(priv_rand, 16)); + + err = bt_encrypt_be(privacy_key, priv_rand, tmp); + if (err) { + return err; + } + + for (i = 0; i < 6; i++) { + pdu[1 + i] ^= tmp[i]; + } + + return 0; +} + +int bt_mesh_net_encrypt(const u8_t key[16], struct os_mbuf *buf, + u32_t iv_index, bool proxy) +{ + u8_t mic_len = NET_MIC_LEN(buf->om_data); + u8_t nonce[13]; + int err; + + BT_DBG("IVIndex %u EncKey %s mic_len %u", iv_index, bt_hex(key, 16), + mic_len); + BT_DBG("PDU (len %u) %s", buf->om_len, bt_hex(buf->om_data, buf->om_len)); + +#if (MYNEWT_VAL(BLE_MESH_PROXY)) + if (proxy) { + create_proxy_nonce(nonce, buf->om_data, iv_index); + } else { + create_net_nonce(nonce, buf->om_data, iv_index); + } +#else + create_net_nonce(nonce, buf->om_data, iv_index); +#endif + + BT_DBG("Nonce %s", bt_hex(nonce, 13)); + + err = bt_mesh_ccm_encrypt(key, nonce, &buf->om_data[7], buf->om_len - 7, + NULL, 0, &buf->om_data[7], mic_len); + if (!err) { + net_buf_simple_add(buf, mic_len); + } + + return err; +} + +int bt_mesh_net_decrypt(const u8_t key[16], struct os_mbuf *buf, + u32_t iv_index, bool proxy) +{ + u8_t mic_len = NET_MIC_LEN(buf->om_data); + u8_t nonce[13]; + + BT_DBG("PDU (%u bytes) %s", buf->om_len, bt_hex(buf->om_data, buf->om_len)); + BT_DBG("iv_index %u, key %s mic_len %u", iv_index, bt_hex(key, 16), + mic_len); + +#if (MYNEWT_VAL(BLE_MESH_PROXY)) + if (proxy) { + create_proxy_nonce(nonce, buf->om_data, iv_index); + } else { + create_net_nonce(nonce, buf->om_data, iv_index); + } +#else + create_net_nonce(nonce, buf->om_data, iv_index); +#endif + + BT_DBG("Nonce %s", bt_hex(nonce, 13)); + + buf->om_len -= mic_len; + + return bt_mesh_ccm_decrypt(key, nonce, &buf->om_data[7], buf->om_len - 7, + NULL, 0, &buf->om_data[7], mic_len); +} + +static void create_app_nonce(u8_t nonce[13], bool dev_key, u8_t aszmic, + u16_t src, u16_t dst, u32_t seq_num, + u32_t iv_index) +{ + if (dev_key) { + nonce[0] = 0x02; + } else { + nonce[0] = 0x01; + } + + sys_put_be32((seq_num | ((u32_t)aszmic << 31)), &nonce[1]); + + sys_put_be16(src, &nonce[5]); + sys_put_be16(dst, &nonce[7]); + + sys_put_be32(iv_index, &nonce[9]); +} + +int bt_mesh_app_encrypt(const u8_t key[16], bool dev_key, u8_t aszmic, + struct os_mbuf *buf, const u8_t *ad, + u8_t mic_len, u16_t src, u16_t dst, + u32_t seq_num, u32_t iv_index) +{ + u8_t nonce[13]; + int err; + + BT_DBG("AppKey %s", bt_hex(key, 16)); + BT_DBG("dev_key %u mic_len %u src 0x%04x dst 0x%04x", dev_key, + mic_len, src, dst); + BT_DBG("seq_num 0x%08x iv_index 0x%08x", seq_num, iv_index); + BT_DBG("Clear: %s", bt_hex(buf->om_data, buf->om_len)); + + create_app_nonce(nonce, dev_key, aszmic, src, dst, seq_num, iv_index); + + BT_DBG("Nonce %s", bt_hex(nonce, 13)); + + err = bt_mesh_ccm_encrypt(key, nonce, buf->om_data, buf->om_len, ad, + ad ? 16 : 0, buf->om_data, mic_len); + if (!err) { + net_buf_simple_add(buf, mic_len); + BT_DBG("Encr: %s", bt_hex(buf->om_data, buf->om_len)); + } + + return err; +} + +int bt_mesh_app_decrypt(const u8_t key[16], bool dev_key, u8_t aszmic, + struct os_mbuf *buf, u8_t mic_len, + struct os_mbuf *out, const u8_t *ad, + u16_t src, u16_t dst, u32_t seq_num, + u32_t iv_index) +{ + u8_t nonce[13]; + int err; + + BT_DBG("EncData (len %u) %s", buf->om_len, bt_hex(buf->om_data, buf->om_len)); + + create_app_nonce(nonce, dev_key, aszmic, src, dst, seq_num, iv_index); + + BT_DBG("AppKey %s", bt_hex(key, 16)); + BT_DBG("Nonce %s", bt_hex(nonce, 13)); + + err = bt_mesh_ccm_decrypt(key, nonce, buf->om_data, buf->om_len, ad, + ad ? 16 : 0, out->om_data, mic_len); + if (!err) { + net_buf_simple_add(out, buf->om_len); + } + + return err; +} + +/* reversed, 8-bit, poly=0x07 */ +static const u8_t crc_table[256] = { + 0x00, 0x91, 0xe3, 0x72, 0x07, 0x96, 0xe4, 0x75, + 0x0e, 0x9f, 0xed, 0x7c, 0x09, 0x98, 0xea, 0x7b, + 0x1c, 0x8d, 0xff, 0x6e, 0x1b, 0x8a, 0xf8, 0x69, + 0x12, 0x83, 0xf1, 0x60, 0x15, 0x84, 0xf6, 0x67, + + 0x38, 0xa9, 0xdb, 0x4a, 0x3f, 0xae, 0xdc, 0x4d, + 0x36, 0xa7, 0xd5, 0x44, 0x31, 0xa0, 0xd2, 0x43, + 0x24, 0xb5, 0xc7, 0x56, 0x23, 0xb2, 0xc0, 0x51, + 0x2a, 0xbb, 0xc9, 0x58, 0x2d, 0xbc, 0xce, 0x5f, + + 0x70, 0xe1, 0x93, 0x02, 0x77, 0xe6, 0x94, 0x05, + 0x7e, 0xef, 0x9d, 0x0c, 0x79, 0xe8, 0x9a, 0x0b, + 0x6c, 0xfd, 0x8f, 0x1e, 0x6b, 0xfa, 0x88, 0x19, + 0x62, 0xf3, 0x81, 0x10, 0x65, 0xf4, 0x86, 0x17, + + 0x48, 0xd9, 0xab, 0x3a, 0x4f, 0xde, 0xac, 0x3d, + 0x46, 0xd7, 0xa5, 0x34, 0x41, 0xd0, 0xa2, 0x33, + 0x54, 0xc5, 0xb7, 0x26, 0x53, 0xc2, 0xb0, 0x21, + 0x5a, 0xcb, 0xb9, 0x28, 0x5d, 0xcc, 0xbe, 0x2f, + + 0xe0, 0x71, 0x03, 0x92, 0xe7, 0x76, 0x04, 0x95, + 0xee, 0x7f, 0x0d, 0x9c, 0xe9, 0x78, 0x0a, 0x9b, + 0xfc, 0x6d, 0x1f, 0x8e, 0xfb, 0x6a, 0x18, 0x89, + 0xf2, 0x63, 0x11, 0x80, 0xf5, 0x64, 0x16, 0x87, + + 0xd8, 0x49, 0x3b, 0xaa, 0xdf, 0x4e, 0x3c, 0xad, + 0xd6, 0x47, 0x35, 0xa4, 0xd1, 0x40, 0x32, 0xa3, + 0xc4, 0x55, 0x27, 0xb6, 0xc3, 0x52, 0x20, 0xb1, + 0xca, 0x5b, 0x29, 0xb8, 0xcd, 0x5c, 0x2e, 0xbf, + + 0x90, 0x01, 0x73, 0xe2, 0x97, 0x06, 0x74, 0xe5, + 0x9e, 0x0f, 0x7d, 0xec, 0x99, 0x08, 0x7a, 0xeb, + 0x8c, 0x1d, 0x6f, 0xfe, 0x8b, 0x1a, 0x68, 0xf9, + 0x82, 0x13, 0x61, 0xf0, 0x85, 0x14, 0x66, 0xf7, + + 0xa8, 0x39, 0x4b, 0xda, 0xaf, 0x3e, 0x4c, 0xdd, + 0xa6, 0x37, 0x45, 0xd4, 0xa1, 0x30, 0x42, 0xd3, + 0xb4, 0x25, 0x57, 0xc6, 0xb3, 0x22, 0x50, 0xc1, + 0xba, 0x2b, 0x59, 0xc8, 0xbd, 0x2c, 0x5e, 0xcf +}; + +u8_t bt_mesh_fcs_calc(const u8_t *data, u8_t data_len) +{ + u8_t fcs = 0xff; + + while (data_len--) { + fcs = crc_table[fcs ^ *data++]; + } + + BT_DBG("fcs 0x%02x", 0xff - fcs); + + return 0xff - fcs; +} + +bool bt_mesh_fcs_check(struct os_mbuf *buf, u8_t received_fcs) +{ + const u8_t *data = buf->om_data; + u16_t data_len = buf->om_len; + u8_t fcs = 0xff; + + while (data_len--) { + fcs = crc_table[fcs ^ *data++]; + } + + return crc_table[fcs ^ received_fcs] == 0xcf; +} + +int bt_mesh_virtual_addr(const u8_t virtual_label[16], u16_t *addr) +{ + u8_t salt[16]; + u8_t tmp[16]; + int err; + + err = bt_mesh_s1("vtad", salt); + if (err) { + return err; + } + + err = bt_mesh_aes_cmac_one(salt, virtual_label, 16, tmp); + if (err) { + return err; + } + + *addr = (sys_get_be16(&tmp[14]) & 0x3fff) | 0x8000; + + return 0; +} + +int bt_mesh_prov_conf_salt(const u8_t conf_inputs[145], u8_t salt[16]) +{ + const u8_t conf_salt_key[16] = { 0 }; + + return bt_mesh_aes_cmac_one(conf_salt_key, conf_inputs, 145, salt); +} + +int bt_mesh_prov_conf_key(const u8_t dhkey[32], const u8_t conf_salt[16], + u8_t conf_key[16]) +{ + return bt_mesh_k1(dhkey, 32, conf_salt, "prck", conf_key); +} + +int bt_mesh_prov_conf(const u8_t conf_key[16], const u8_t rand[16], + const u8_t auth[16], u8_t conf[16]) +{ + struct bt_mesh_sg sg[] = { { rand, 16 }, { auth, 16 } }; + + BT_DBG("ConfirmationKey %s", bt_hex(conf_key, 16)); + BT_DBG("RandomDevice %s", bt_hex(rand, 16)); + BT_DBG("AuthValue %s", bt_hex(auth, 16)); + + return bt_mesh_aes_cmac(conf_key, sg, ARRAY_SIZE(sg), conf); +} + +int bt_mesh_prov_decrypt(const u8_t key[16], u8_t nonce[13], + const u8_t data[25 + 8], u8_t out[25]) +{ + return bt_mesh_ccm_decrypt(key, nonce, data, 25, NULL, 0, out, 8); +} + +int bt_mesh_beacon_auth(const u8_t beacon_key[16], u8_t flags, + const u8_t net_id[8], u32_t iv_index, + u8_t auth[8]) +{ + u8_t msg[13], tmp[16]; + int err; + + BT_DBG("BeaconKey %s", bt_hex(beacon_key, 16)); + BT_DBG("NetId %s", bt_hex(net_id, 8)); + BT_DBG("IV Index 0x%08x", iv_index); + + msg[0] = flags; + memcpy(&msg[1], net_id, 8); + sys_put_be32(iv_index, &msg[9]); + + BT_DBG("BeaconMsg %s", bt_hex(msg, sizeof(msg))); + + err = bt_mesh_aes_cmac_one(beacon_key, msg, sizeof(msg), tmp); + if (!err) { + memcpy(auth, tmp, 8); + } + + return err; +} diff --git a/nimble/host/mesh/src/crypto.h b/nimble/host/mesh/src/crypto.h new file mode 100644 index 000000000..d37b7edab --- /dev/null +++ b/nimble/host/mesh/src/crypto.h @@ -0,0 +1,162 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ +#ifndef __CRYPTO_H__ +#define __CRYPTO_H__ + +#include "mesh/mesh.h" + +struct bt_mesh_sg { + const void *data; + size_t len; +}; + +int bt_mesh_aes_cmac(const u8_t key[16], struct bt_mesh_sg *sg, + size_t sg_len, u8_t mac[16]); + +static inline int bt_mesh_aes_cmac_one(const u8_t key[16], const void *m, + size_t len, u8_t mac[16]) +{ + struct bt_mesh_sg sg = { m, len }; + + return bt_mesh_aes_cmac(key, &sg, 1, mac); +} + +static inline bool bt_mesh_s1(const char *m, u8_t salt[16]) +{ + const u8_t zero[16] = { 0 }; + + return bt_mesh_aes_cmac_one(zero, m, strlen(m), salt); +} + +int bt_mesh_k1(const u8_t *ikm, size_t ikm_len, const u8_t salt[16], + const char *info, u8_t okm[16]); + +#define bt_mesh_k1_str(ikm, ikm_len, salt_str, info, okm) \ +({ \ + const u8_t salt[16] = salt_str; \ + bt_mesh_k1(ikm, ikm_len, salt, info, okm); \ +}) + +int bt_mesh_k2(const u8_t n[16], const u8_t *p, size_t p_len, + u8_t net_id[1], u8_t enc_key[16], u8_t priv_key[16]); + +int bt_mesh_k3(const u8_t n[16], u8_t out[8]); + +int bt_mesh_k4(const u8_t n[16], u8_t out[1]); + +int bt_mesh_id128(const u8_t n[16], const char *s, u8_t out[16]); + +static inline int bt_mesh_id_resolving_key(const u8_t net_key[16], + u8_t resolving_key[16]) +{ + return bt_mesh_k1_str(net_key, 16, "smbt", "smbi", resolving_key); +} + +static inline int bt_mesh_identity_key(const u8_t net_key[16], + u8_t identity_key[16]) +{ + return bt_mesh_id128(net_key, "nkik", identity_key); +} + +static inline int bt_mesh_beacon_key(const u8_t net_key[16], + u8_t beacon_key[16]) +{ + return bt_mesh_id128(net_key, "nkbk", beacon_key); +} + +int bt_mesh_beacon_auth(const u8_t beacon_key[16], u8_t flags, + const u8_t net_id[16], u32_t iv_index, + u8_t auth[8]); + +static inline int bt_mesh_app_id(const u8_t app_key[16], u8_t app_id[1]) +{ + return bt_mesh_k4(app_key, app_id); +} + +static inline int bt_mesh_session_key(const u8_t dhkey[32], + const u8_t prov_salt[16], + u8_t session_key[16]) +{ + return bt_mesh_k1(dhkey, 32, prov_salt, "prsk", session_key); +} + +static inline int bt_mesh_prov_nonce(const u8_t dhkey[32], + const u8_t prov_salt[16], + u8_t nonce[13]) +{ + u8_t tmp[16]; + int err; + + err = bt_mesh_k1(dhkey, 32, prov_salt, "prsn", tmp); + if (!err) { + memcpy(nonce, tmp + 3, 13); + } + + return err; +} + +static inline int bt_mesh_dev_key(const u8_t dhkey[32], + const u8_t prov_salt[16], + u8_t dev_key[16]) +{ + return bt_mesh_k1(dhkey, 32, prov_salt, "prdk", dev_key); +} + +static inline int bt_mesh_prov_salt(const u8_t conf_salt[16], + const u8_t prov_rand[16], + const u8_t dev_rand[16], + u8_t prov_salt[16]) +{ + const u8_t prov_salt_key[16] = { 0 }; + struct bt_mesh_sg sg[] = { + { conf_salt, 16 }, + { prov_rand, 16 }, + { dev_rand, 16 }, + }; + + return bt_mesh_aes_cmac(prov_salt_key, sg, ARRAY_SIZE(sg), prov_salt); +} + +int bt_mesh_net_obfuscate(u8_t *pdu, u32_t iv_index, + const u8_t privacy_key[16]); + +int bt_mesh_net_encrypt(const u8_t key[16], struct os_mbuf *buf, + u32_t iv_index, bool proxy); + +int bt_mesh_net_decrypt(const u8_t key[16], struct os_mbuf *buf, + u32_t iv_index, bool proxy); + +int bt_mesh_app_encrypt(const u8_t key[16], bool dev_key, u8_t aszmic, + struct os_mbuf *buf, const u8_t *ad, + u8_t mic_len, u16_t src, u16_t dst, + u32_t seq_num, u32_t iv_index); + +int bt_mesh_app_decrypt(const u8_t key[16], bool dev_key, u8_t aszmic, + struct os_mbuf *buf, u8_t mic_len, + struct os_mbuf *out, const u8_t *ad, + u16_t src, u16_t dst, u32_t seq_num, + u32_t iv_index); + +u8_t bt_mesh_fcs_calc(const u8_t *data, u8_t data_len); + +bool bt_mesh_fcs_check(struct os_mbuf *buf, u8_t received_fcs); + +int bt_mesh_virtual_addr(const u8_t virtual_label[16], u16_t *addr); + +int bt_mesh_prov_conf_salt(const u8_t conf_inputs[145], u8_t salt[16]); + +int bt_mesh_prov_conf_key(const u8_t dhkey[32], const u8_t conf_salt[16], + u8_t conf_key[16]); + +int bt_mesh_prov_conf(const u8_t conf_key[16], const u8_t rand[16], + const u8_t auth[16], u8_t conf[16]); + +int bt_mesh_prov_decrypt(const u8_t key[16], u8_t nonce[13], + const u8_t data[25 + 8], u8_t out[25]); + +#endif diff --git a/nimble/host/mesh/src/foundation.h b/nimble/host/mesh/src/foundation.h new file mode 100644 index 000000000..decb2dc6a --- /dev/null +++ b/nimble/host/mesh/src/foundation.h @@ -0,0 +1,152 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef __FUNDATION_H__ +#define __FUNDATION_H__ + +#define OP_APP_KEY_ADD BT_MESH_MODEL_OP_1(0x00) +#define OP_APP_KEY_UPDATE BT_MESH_MODEL_OP_1(0x01) +#define OP_DEV_COMP_DATA_STATUS BT_MESH_MODEL_OP_1(0x02) +#define OP_MOD_PUB_SET BT_MESH_MODEL_OP_1(0x03) +#define OP_HEALTH_CURRENT_STATUS BT_MESH_MODEL_OP_1(0x04) +#define OP_HEALTH_FAULT_STATUS BT_MESH_MODEL_OP_1(0x05) +#define OP_HEARTBEAT_PUB_STATUS BT_MESH_MODEL_OP_1(0x06) +#define OP_APP_KEY_DEL BT_MESH_MODEL_OP_2(0x80, 0x00) +#define OP_APP_KEY_GET BT_MESH_MODEL_OP_2(0x80, 0x01) +#define OP_APP_KEY_LIST BT_MESH_MODEL_OP_2(0x80, 0x02) +#define OP_APP_KEY_STATUS BT_MESH_MODEL_OP_2(0x80, 0x03) +#define OP_ATTENTION_GET BT_MESH_MODEL_OP_2(0x80, 0x04) +#define OP_ATTENTION_SET BT_MESH_MODEL_OP_2(0x80, 0x05) +#define OP_ATTENTION_SET_UNREL BT_MESH_MODEL_OP_2(0x80, 0x06) +#define OP_ATTENTION_STATUS BT_MESH_MODEL_OP_2(0x80, 0x07) +#define OP_DEV_COMP_DATA_GET BT_MESH_MODEL_OP_2(0x80, 0x08) +#define OP_BEACON_GET BT_MESH_MODEL_OP_2(0x80, 0x09) +#define OP_BEACON_SET BT_MESH_MODEL_OP_2(0x80, 0x0a) +#define OP_BEACON_STATUS BT_MESH_MODEL_OP_2(0x80, 0x0b) +#define OP_DEFAULT_TTL_GET BT_MESH_MODEL_OP_2(0x80, 0x0c) +#define OP_DEFAULT_TTL_SET BT_MESH_MODEL_OP_2(0x80, 0x0d) +#define OP_DEFAULT_TTL_STATUS BT_MESH_MODEL_OP_2(0x80, 0x0e) +#define OP_FRIEND_GET BT_MESH_MODEL_OP_2(0x80, 0x0f) +#define OP_FRIEND_SET BT_MESH_MODEL_OP_2(0x80, 0x10) +#define OP_FRIEND_STATUS BT_MESH_MODEL_OP_2(0x80, 0x11) +#define OP_GATT_PROXY_GET BT_MESH_MODEL_OP_2(0x80, 0x12) +#define OP_GATT_PROXY_SET BT_MESH_MODEL_OP_2(0x80, 0x13) +#define OP_GATT_PROXY_STATUS BT_MESH_MODEL_OP_2(0x80, 0x14) +#define OP_KRP_GET BT_MESH_MODEL_OP_2(0x80, 0x15) +#define OP_KRP_SET BT_MESH_MODEL_OP_2(0x80, 0x16) +#define OP_KRP_STATUS BT_MESH_MODEL_OP_2(0x80, 0x17) +#define OP_MOD_PUB_GET BT_MESH_MODEL_OP_2(0x80, 0x18) +#define OP_MOD_PUB_STATUS BT_MESH_MODEL_OP_2(0x80, 0x19) +#define OP_MOD_PUB_VA_SET BT_MESH_MODEL_OP_2(0x80, 0x1a) +#define OP_MOD_SUB_ADD BT_MESH_MODEL_OP_2(0x80, 0x1b) +#define OP_MOD_SUB_DEL BT_MESH_MODEL_OP_2(0x80, 0x1c) +#define OP_MOD_SUB_DEL_ALL BT_MESH_MODEL_OP_2(0x80, 0x1d) +#define OP_MOD_SUB_OVERWRITE BT_MESH_MODEL_OP_2(0x80, 0x1e) +#define OP_MOD_SUB_STATUS BT_MESH_MODEL_OP_2(0x80, 0x1f) +#define OP_MOD_SUB_VA_ADD BT_MESH_MODEL_OP_2(0x80, 0x20) +#define OP_MOD_SUB_VA_DEL BT_MESH_MODEL_OP_2(0x80, 0x21) +#define OP_MOD_SUB_VA_OVERWRITE BT_MESH_MODEL_OP_2(0x80, 0x22) +#define OP_NET_TRANSMIT_GET BT_MESH_MODEL_OP_2(0x80, 0x23) +#define OP_NET_TRANSMIT_SET BT_MESH_MODEL_OP_2(0x80, 0x24) +#define OP_NET_TRANSMIT_STATUS BT_MESH_MODEL_OP_2(0x80, 0x25) +#define OP_RELAY_GET BT_MESH_MODEL_OP_2(0x80, 0x26) +#define OP_RELAY_SET BT_MESH_MODEL_OP_2(0x80, 0x27) +#define OP_RELAY_STATUS BT_MESH_MODEL_OP_2(0x80, 0x28) +#define OP_MOD_SUB_GET BT_MESH_MODEL_OP_2(0x80, 0x29) +#define OP_MOD_SUB_LIST BT_MESH_MODEL_OP_2(0x80, 0x2a) +#define OP_MOD_SUB_GET_VND BT_MESH_MODEL_OP_2(0x80, 0x2b) +#define OP_MOD_SUB_LIST_VND BT_MESH_MODEL_OP_2(0x80, 0x2c) +#define OP_LPN_TIMEOUT_GET BT_MESH_MODEL_OP_2(0x80, 0x2d) +#define OP_LPN_TIMEOUT_STATUS BT_MESH_MODEL_OP_2(0x80, 0x2e) +#define OP_HEALTH_FAULT_CLEAR BT_MESH_MODEL_OP_2(0x80, 0x2f) +#define OP_HEALTH_FAULT_CLEAR_UNREL BT_MESH_MODEL_OP_2(0x80, 0x30) +#define OP_HEALTH_FAULT_GET BT_MESH_MODEL_OP_2(0x80, 0x31) +#define OP_HEALTH_FAULT_TEST BT_MESH_MODEL_OP_2(0x80, 0x32) +#define OP_HEALTH_FAULT_TEST_UNREL BT_MESH_MODEL_OP_2(0x80, 0x33) +#define OP_HEALTH_PERIOD_GET BT_MESH_MODEL_OP_2(0x80, 0x34) +#define OP_HEALTH_PERIOD_SET BT_MESH_MODEL_OP_2(0x80, 0x34) +#define OP_HEALTH_PERIOD_SET_UNREL BT_MESH_MODEL_OP_2(0x80, 0x36) +#define OP_HEALTH_PERIOD_STATUS BT_MESH_MODEL_OP_2(0x80, 0x37) +#define OP_HEARTBEAT_PUB_GET BT_MESH_MODEL_OP_2(0x80, 0x38) +#define OP_HEARTBEAT_PUB_SET BT_MESH_MODEL_OP_2(0x80, 0x39) +#define OP_HEARTBEAT_SUB_GET BT_MESH_MODEL_OP_2(0x80, 0x3a) +#define OP_HEARTBEAT_SUB_SET BT_MESH_MODEL_OP_2(0x80, 0x3b) +#define OP_HEARTBEAT_SUB_STATUS BT_MESH_MODEL_OP_2(0x80, 0x3c) +#define OP_MOD_APP_BIND BT_MESH_MODEL_OP_2(0x80, 0x3d) +#define OP_MOD_APP_STATUS BT_MESH_MODEL_OP_2(0x80, 0x3e) +#define OP_MOD_APP_UNBIND BT_MESH_MODEL_OP_2(0x80, 0x3f) +#define OP_NET_KEY_ADD BT_MESH_MODEL_OP_2(0x80, 0x40) +#define OP_NET_KEY_DEL BT_MESH_MODEL_OP_2(0x80, 0x41) +#define OP_NET_KEY_GET BT_MESH_MODEL_OP_2(0x80, 0x42) +#define OP_NET_KEY_LIST BT_MESH_MODEL_OP_2(0x80, 0x43) +#define OP_NET_KEY_STATUS BT_MESH_MODEL_OP_2(0x80, 0x44) +#define OP_NET_KEY_UPDATE BT_MESH_MODEL_OP_2(0x80, 0x45) +#define OP_NODE_IDENTITY_GET BT_MESH_MODEL_OP_2(0x80, 0x46) +#define OP_NODE_IDENTITY_SET BT_MESH_MODEL_OP_2(0x80, 0x47) +#define OP_NODE_IDENTITY_STATUS BT_MESH_MODEL_OP_2(0x80, 0x48) +#define OP_NODE_RESET BT_MESH_MODEL_OP_2(0x80, 0x49) +#define OP_NODE_RESET_STATUS BT_MESH_MODEL_OP_2(0x80, 0x4a) +#define OP_SIG_MOD_APP_GET BT_MESH_MODEL_OP_2(0x80, 0x4b) +#define OP_SIG_MOD_APP_LIST BT_MESH_MODEL_OP_2(0x80, 0x4c) +#define OP_VND_MOD_APP_GET BT_MESH_MODEL_OP_2(0x80, 0x4d) +#define OP_VND_MOD_APP_LIST BT_MESH_MODEL_OP_2(0x80, 0x4e) + +#define STATUS_SUCCESS 0x00 +#define STATUS_INVALID_ADDRESS 0x01 +#define STATUS_INVALID_MODEL 0x02 +#define STATUS_INVALID_APPKEY 0x03 +#define STATUS_INVALID_NETKEY 0x04 +#define STATUS_INSUFF_RESOURCES 0x05 +#define STATUS_IDX_ALREADY_STORED 0x06 +#define STATUS_NVAL_PUB_PARAM 0x07 +#define STATUS_NOT_SUB_MOD 0x08 +#define STATUS_STORAGE_FAIL 0x09 +#define STATUS_FEAT_NOT_SUPP 0x0a +#define STATUS_CANNOT_UPDATE 0x0b +#define STATUS_CANNOT_REMOVE 0x0c +#define STATUS_CANNOT_BIND 0x0d +#define STATUS_TEMP_STATE_CHG_FAIL 0x0e +#define STATUS_CANNOT_SET 0x0f +#define STATUS_UNSPECIFIED 0x10 +#define STATUS_INVALID_BINDING 0x11 + +int +bt_mesh_conf_init(struct bt_mesh_model *model, bool primary); +int +bt_mesh_health_init(struct bt_mesh_model *model, bool primary); + +void +bt_mesh_heartbeat(u16_t src, u16_t dst, u8_t hops, u16_t feat); + +void +bt_mesh_attention(struct bt_mesh_model *model, u8_t time); + +u8_t * +bt_mesh_label_uuid_get(u16_t addr); + +/* Transmission count (N + 1) */ +#define TRANSMIT_COUNT(transmit) (((transmit) & (u8_t)BIT_MASK(3))) +/* Returns transmission interval in milliseconds */ +#define TRANSMIT_INT(transmit) ((((transmit) >> 3) + 1) * 10) + +u8_t +bt_mesh_net_transmit_get(void); +u8_t +bt_mesh_relay_get(void); +u8_t +bt_mesh_friend_get(void); +u8_t +bt_mesh_relay_retransmit_get(void); +u8_t +bt_mesh_beacon_get(void); +u8_t +bt_mesh_gatt_proxy_get(void); +u8_t +bt_mesh_default_ttl_get(void); + +#endif diff --git a/nimble/host/mesh/src/friend.c b/nimble/host/mesh/src/friend.c new file mode 100644 index 000000000..7baa43594 --- /dev/null +++ b/nimble/host/mesh/src/friend.c @@ -0,0 +1,215 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + + +#include "syscfg/syscfg.h" +#if MYNEWT_VAL(BLE_MESH_FRIEND) == 1 + +#include +#include +#include + +#define BT_DBG_ENABLED (MYNEWT_VAL(BLE_MESH_DEBUG_FRIEND)) +#include "host/ble_hs_log.h" + +#include "mesh/mesh.h" +#include "mesh_priv.h" +#include "crypto.h" +#include "adv.h" +#include "net.h" +#include "transport.h" +#include "access.h" +#include "friend.h" + +static int send_friend_update(void) +{ + struct bt_mesh_msg_ctx ctx = { + .net_idx = bt_mesh.sub[0].net_idx, + .app_idx = BT_MESH_KEY_UNUSED, + .addr = bt_mesh.frnd.lpn, + .send_ttl = 0, + .friend_cred = 1, + }; + struct bt_mesh_net_tx tx = { + .sub = &bt_mesh.sub[0], + .ctx = &ctx, + .src = bt_mesh_primary_addr(), + }; + struct bt_mesh_ctl_friend_update upd = { + .flags = 0, + .iv_index = sys_cpu_to_be32(bt_mesh.iv_index), + .md = !k_fifo_is_empty(&bt_mesh.frnd.queue), + }; + + BT_DBG(""); + + return bt_mesh_ctl_send(&tx, TRANS_CTL_OP_FRIEND_UPDATE, &upd, + sizeof(upd), NULL); +} + +int bt_mesh_friend_poll(struct bt_mesh_net_rx *rx, struct os_mbuf *buf) +{ + struct bt_mesh_ctl_friend_poll *msg = (void *)buf->om_data; + struct bt_mesh_friend *frnd = &bt_mesh.frnd; + + if (buf->om_len < sizeof(*msg)) { + BT_WARN("Too short Friend Update"); + return -EINVAL; + } + + BT_DBG("msg->fsn 0x%02x frnd->fsn 0x%02x", msg->fsn, frnd->fsn); + + if (msg->fsn != frnd->fsn) { + frnd->send_last = 1; + } + + frnd->fsn++; + frnd->send_update = 1; + + k_delayed_work_submit(&frnd->timer, frnd->recv_delay); + + return 0; +} + +static int send_friend_offer(s8_t rssi) +{ + struct bt_mesh_msg_ctx ctx = { + .net_idx = bt_mesh.sub[0].net_idx, + .app_idx = BT_MESH_KEY_UNUSED, + .addr = bt_mesh.frnd.lpn, + .send_ttl = 0, + }; + struct bt_mesh_net_tx tx = { + .sub = &bt_mesh.sub[0], + .ctx = &ctx, + .src = bt_mesh_primary_addr(), + }; + struct bt_mesh_ctl_friend_offer off = { + .recv_win = MYNEWT_VAL(BLE_MESH_FRIEND_RECV_WIN), + .queue_size = MYNEWT_VAL(BLE_MESH_FRIEND_QUEUE_SIZE), + .rssi = rssi, + .frnd_counter = bt_mesh.frnd.counter++, + }; + + BT_DBG(""); + + return bt_mesh_ctl_send(&tx, TRANS_CTL_OP_FRIEND_OFFER, &off, + sizeof(off), NULL); +} + +int bt_mesh_friend_req(struct bt_mesh_net_rx *rx, struct os_mbuf *buf) +{ + struct bt_mesh_ctl_friend_req *msg = (void *)buf->om_data; + struct bt_mesh_friend *frnd = &bt_mesh.frnd; + struct bt_mesh_subnet *sub = rx->sub; + + if (buf->om_len < sizeof(*msg)) { + BT_WARN("Too short Friend Request"); + return -EINVAL; + } + + frnd->lpn = rx->ctx.addr; + frnd->rssi = rx->rssi; + frnd->recv_delay = msg->recv_delay; + frnd->poll_to = (((u32_t)msg->poll_to[0] << 16) | + ((u32_t)msg->poll_to[1] << 8) | + ((u32_t)msg->poll_to[2])); + frnd->poll_to *= 100; + frnd->lpn_counter = sys_be16_to_cpu(msg->lpn_counter); + + BT_DBG("LPN 0x%04x rssi %d recv_delay %u poll_to %ums", + frnd->lpn, frnd->rssi, frnd->recv_delay, frnd->poll_to); + + bt_mesh_friend_cred_add(sub->net_idx, sub->keys[0].net, 0, + frnd->lpn, frnd->lpn_counter, frnd->counter); + + frnd->send_offer = 1; + + k_delayed_work_submit(&frnd->timer, K_MSEC(100)); + + return 0; +} + +static void friend_timeout(struct os_event *work) +{ + struct bt_mesh_friend *frnd = &bt_mesh.frnd; + + BT_DBG("send_offer %u send_update %u", frnd->send_offer, + frnd->send_update); + + if (frnd->send_offer) { + frnd->send_offer = 0; + send_friend_offer(frnd->rssi); + return; + } + + if (!frnd->send_update) { + BT_WARN("Friendship lost"); + bt_mesh_friend_cred_del(bt_mesh.sub[0].net_idx, frnd->lpn); + return; + } + + frnd->send_update = 0; + + if (frnd->send_last && frnd->last) { + frnd->send_last = 0; + bt_mesh_adv_send(frnd->last, NULL); + return; + } + + if (frnd->last) { + net_buf_unref(frnd->last); + } + + frnd->last = net_buf_get(&frnd->queue, K_NO_WAIT); + if (frnd->last) { + bt_mesh_adv_send(frnd->last, NULL); + } else { + send_friend_update(); + } + + k_delayed_work_submit(&frnd->timer, frnd->poll_to); +} + +int bt_mesh_friend_init(void) +{ + struct bt_mesh_friend *frnd = &bt_mesh.frnd; + + k_fifo_init(&frnd->queue); + + k_delayed_work_init(&frnd->timer, friend_timeout); + + return 0; +} + +bool bt_mesh_friend_enqueue(struct os_mbuf *buf, u16_t dst) +{ + /* FIXME: Add support for multiple LPNs and group addresses */ + if (!bt_mesh_friend_dst_is_lpn(dst)) { + return false; + } + + if (BT_MESH_ADDR_IS_UNICAST(dst)) { + net_buf_put(&bt_mesh.frnd.queue, net_buf_ref(buf)); + } else { + struct os_mbuf *clone = net_buf_clone(buf, K_NO_WAIT); + + if (clone) { + net_buf_put(&bt_mesh.frnd.queue, clone); + } else { + BT_WARN("Unable to allocate buffer for friend queue"); + return false; + } + } + + BT_DBG("Queued message for LPN"); + + return true; +} + +#endif //MYNEWT_VAL(BLE_MESH_FRIEND) diff --git a/nimble/host/mesh/src/friend.h b/nimble/host/mesh/src/friend.h new file mode 100644 index 000000000..e3b423866 --- /dev/null +++ b/nimble/host/mesh/src/friend.h @@ -0,0 +1,35 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef __FRIEND_H__ +#define __FRIEND_H__ + +#include "mesh/mesh.h" + +static inline bool +bt_mesh_friend_dst_is_lpn(u16_t dst) +{ +#if (MYNEWT_VAL(BLE_MESH_FRIEND)) + return (dst == bt_mesh.frnd.lpn); +#else + return false; +#endif +} + +bool +bt_mesh_friend_enqueue(struct os_mbuf *buf, u16_t dst); + +int +bt_mesh_friend_poll(struct bt_mesh_net_rx *rx, struct os_mbuf *buf); +int +bt_mesh_friend_req(struct bt_mesh_net_rx *rx, struct os_mbuf *buf); + +int +bt_mesh_friend_init(void); + +#endif diff --git a/nimble/host/mesh/src/glue.c b/nimble/host/mesh/src/glue.c new file mode 100644 index 000000000..05aacccc1 --- /dev/null +++ b/nimble/host/mesh/src/glue.c @@ -0,0 +1,486 @@ +/* + * 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 "mesh/glue.h" +#include "adv.h" + +const char * +bt_hex(const void *buf, size_t len) +{ + static const char hex[] = "0123456789abcdef"; + static char hexbufs[4][137]; + static u8_t curbuf; + const u8_t *b = buf; + char *str; + int i; + + str = hexbufs[curbuf++]; + curbuf %= ARRAY_SIZE(hexbufs); + + len = min(len, (sizeof(hexbufs[0]) - 1) / 2); + + for (i = 0; i < len; i++) { + str[i * 2] = hex[b[i] >> 4]; + str[i * 2 + 1] = hex[b[i] & 0xf]; + } + + str[i * 2] = '\0'; + + return str; +} + +void +net_buf_put(struct os_eventq *fifo, struct os_mbuf *om) +{ + struct os_event *ev; + + assert(OS_MBUF_IS_PKTHDR(om)); + ev = &BT_MESH_ADV(om)->ev; + assert(ev); + assert(ev->ev_arg); + + os_eventq_put(fifo, ev); +} + +void * +net_buf_ref(struct os_mbuf *om) +{ + struct bt_mesh_adv *adv; + + /* For bufs with header we count refs*/ + if (OS_MBUF_USRHDR_LEN(om) == 0) { + return NULL; + } + + adv = BT_MESH_ADV(om); + adv->ref_cnt++; + + return om; +} + +void +net_buf_unref(struct os_mbuf *om) +{ + struct bt_mesh_adv *adv; + + /* For bufs with header we count refs*/ + if (OS_MBUF_USRHDR_LEN(om) == 0) { + goto free; + } + + adv = BT_MESH_ADV(om); + if (--adv->ref_cnt > 0) { + return; + } + +free: + os_mbuf_free_chain(om); +} + +int +bt_encrypt_be(const uint8_t *key, const uint8_t *plaintext, uint8_t *enc_data) +{ + struct tc_aes_key_sched_struct s; + + if (tc_aes128_set_encrypt_key(&s, key) == TC_CRYPTO_FAIL) { + return BLE_HS_EUNKNOWN; + } + + if (tc_aes_encrypt(enc_data, plaintext, &s) == TC_CRYPTO_FAIL) { + return BLE_HS_EUNKNOWN; + } + + return 0; +} + +uint16_t +net_buf_simple_pull_le16(struct os_mbuf *om) +{ + uint16_t val; + struct os_mbuf *old = om; + + om = os_mbuf_pullup(om, sizeof(val)); + assert(om == old); + val = get_le16(om->om_data); + os_mbuf_adj(om, sizeof(val)); + + return val; +} + +uint16_t +net_buf_simple_pull_be16(struct os_mbuf *om) +{ + uint16_t val; + struct os_mbuf *old = om; + + om = os_mbuf_pullup(om, sizeof(val)); + assert(om == old); + val = get_be16(om->om_data); + os_mbuf_adj(om, sizeof(val)); + + return val; +} + +uint32_t +net_buf_simple_pull_be32(struct os_mbuf *om) +{ + uint32_t val; + struct os_mbuf *old = om; + + om = os_mbuf_pullup(om, sizeof(val)); + assert(om == old); + val = get_be32(om->om_data); + os_mbuf_adj(om, sizeof(val)); + + return val; +} + +uint8_t +net_buf_simple_pull_u8(struct os_mbuf *om) +{ + uint8_t val; + struct os_mbuf *old = om; + + om = os_mbuf_pullup(om, sizeof(val)); + assert(om == old); + val = om->om_data[0]; + os_mbuf_adj(om, 1); + + return val; +} + +void +net_buf_simple_add_le16(struct os_mbuf *om, uint16_t val) +{ + val = htole16(val); + os_mbuf_append(om, &val, sizeof(val)); +} + +void +net_buf_simple_add_be16(struct os_mbuf *om, uint16_t val) +{ + val = htobe16(val); + os_mbuf_append(om, &val, sizeof(val)); +} + +void +net_buf_simple_add_be32(struct os_mbuf *om, uint32_t val) +{ + val = htobe32(val); + os_mbuf_append(om, &val, sizeof(val)); +} + +void +net_buf_simple_add_u8(struct os_mbuf *om, uint8_t val) +{ + os_mbuf_append(om, &val, 1); +} + +void +net_buf_simple_push_le16(struct os_mbuf *om, uint16_t val) +{ + uint8_t headroom = om->om_data - &om->om_databuf[om->om_pkthdr_len]; + + assert(headroom >= 2); + om->om_data -= 2; + put_le16(om->om_data, val); + om->om_len += 2; + + if (om->om_pkthdr_len) { + OS_MBUF_PKTHDR(om)->omp_len += 2; + } +} + +void +net_buf_simple_push_be16(struct os_mbuf *om, uint16_t val) +{ + uint8_t headroom = om->om_data - &om->om_databuf[om->om_pkthdr_len]; + + assert(headroom >= 2); + om->om_data -= 2; + put_be16(om->om_data, val); + om->om_len += 2; + + if (om->om_pkthdr_len) { + OS_MBUF_PKTHDR(om)->omp_len += 2; + } +} + +void +net_buf_simple_push_u8(struct os_mbuf *om, uint8_t val) +{ + uint8_t headroom = om->om_data - &om->om_databuf[om->om_pkthdr_len]; + + assert(headroom >= 1); + om->om_data -= 1; + om->om_data[0] = val; + om->om_len += 1; + + if (om->om_pkthdr_len) { + OS_MBUF_PKTHDR(om)->omp_len += 1; + } +} + +void +net_buf_add_zeros(struct os_mbuf *om, uint8_t len) +{ + uint8_t z[len]; + int rc; + + memset(z, 0, len); + + rc = os_mbuf_append(om, z, len); + if(rc) { + assert(0); + } +} + +void * +net_buf_simple_pull(struct os_mbuf *om, uint8_t len) +{ + os_mbuf_adj(om, len); + return om->om_data; +} + +void* +net_buf_simple_add(struct os_mbuf *om, uint8_t len) +{ + return os_mbuf_extend(om, len); +} + +bool +k_fifo_is_empty(struct os_eventq *q) +{ + return STAILQ_EMPTY(&q->evq_list); +} + +void * net_buf_get(struct os_eventq *fifo,s32_t t) +{ + struct os_event *ev = os_eventq_get_no_wait(fifo); + + return ev->ev_arg; +} + +uint8_t * +net_buf_simple_push(struct os_mbuf *om, uint8_t len) +{ + uint8_t headroom = om->om_data - &om->om_databuf[om->om_pkthdr_len]; + + assert(headroom >= len); + om->om_data -= len; + om->om_len += len; + + return om->om_data; +} + +void +net_buf_reserve(struct os_mbuf *om, size_t reserve) +{ + /* We need reserve to be done on fresh buf */ + assert(om->om_len == 0); + om->om_data += reserve; +} + +void +k_work_init(struct os_callout *work, os_event_fn handler) +{ + os_callout_init(work, os_eventq_dflt_get(), handler, NULL); +} + +void +k_delayed_work_init(struct k_delayed_work *w, os_event_fn *f) +{ + os_callout_init(&w->work, os_eventq_dflt_get(), f, NULL); +} + +void +k_delayed_work_cancel(struct k_delayed_work *w) +{ + os_callout_stop(&w->work); +} + +void +k_delayed_work_submit(struct k_delayed_work *w, uint32_t ms) +{ + uint32_t ticks; + + os_time_ms_to_ticks(ms, &ticks); + os_callout_reset(&w->work, ticks); +} + +void +k_work_submit(struct os_callout *w) +{ + os_callout_reset(w, 0); +} + +void +k_work_add_arg(struct os_callout *w, void *arg) +{ + w->c_ev.ev_arg = arg; +} + +void +k_delayed_work_add_arg(struct k_delayed_work *w, void *arg) +{ + w->work.c_ev.ev_arg = arg; +} + +uint32_t +k_delayed_work_remaining_get (struct k_delayed_work *w) +{ + int sr; + os_time_t t; + + OS_ENTER_CRITICAL(sr); + + t = os_callout_remaining_ticks(&w->work, os_cputime_get32()); + + OS_EXIT_CRITICAL(sr); + + /* We should return ms */ + return os_cputime_ticks_to_usecs(t) / 1000; +} + +int64_t k_uptime_get(void) +{ + /* We should return ms */ + return os_get_uptime_usec() / 1000; +} + +u32_t k_uptime_get_32(void) +{ + return k_uptime_get(); +} + +static uint8_t pub[64]; +static uint32_t priv[8]; +static bool has_pub = false; + +int +bt_dh_key_gen(const u8_t remote_pk[64], bt_dh_key_cb_t cb) +{ + uint8_t dh[32]; + + if (ble_sm_alg_gen_dhkey((uint8_t *)&remote_pk[0], (uint8_t *)&remote_pk[32], + priv, dh)) { + return -1; + } + + cb(dh); + return 0; +} + +int +bt_rand(void *buf, size_t len) +{ + int rc; + rc = ble_hs_hci_util_rand(buf, len); + if (rc != 0) { + return -1; + } + + return 0; +} + +int +bt_pub_key_gen(struct bt_pub_key_cb *new_cb) +{ + + if (ble_sm_alg_gen_key_pair(pub, priv)) { + assert(0); + return -1; + } + + new_cb->func(pub); + has_pub = true; + + return 0; +} + +uint8_t * +bt_pub_key_get(void) +{ + if (!has_pub) { + return NULL; + } + + return pub; +} + +static int +set_ad(const struct bt_data *ad, size_t ad_len, u8_t *buf, u8_t *buf_len) +{ + int i; + + for (i = 0; i < ad_len; i++) { + buf[(*buf_len)++] = ad[i].data_len + 1; + buf[(*buf_len)++] = ad[i].type; + + memcpy(&buf[*buf_len], ad[i].data, + ad[i].data_len); + *buf_len += ad[i].data_len; + } + + return 0; +} + +int +bt_le_adv_start(const struct ble_gap_adv_params *param, + const struct bt_data *ad, size_t ad_len, + const struct bt_data *sd, size_t sd_len) +{ +#if MYNEWT_VAL(BLE_EXT_ADV) + uint8_t buf[MYNEWT_VAL(BLE_EXT_ADV_MAX_SIZE)]; +#else + uint8_t buf[BLE_HS_ADV_MAX_SZ]; +#endif + uint8_t buf_len = 0; + int err; + + err = set_ad(ad, ad_len, buf, &buf_len); + if (err) { + return err; + } + + err = ble_gap_adv_set_data(buf, buf_len); + if (err != 0) { + return err; + } + + err = ble_gap_adv_start(0x00, NULL, BLE_HS_FOREVER, param, NULL, NULL); + if (err) { + BT_ERR("Advertising failed: err %d", err); + return err; + } + + return 0; +} + +#if MYNEWT_VAL(BLE_MESH_PROXY) +int bt_mesh_proxy_svcs_register(void); +#endif + +void +bt_mesh_register_gatt(void) +{ +#if MYNEWT_VAL(BLE_MESH_PROXY) + bt_mesh_proxy_svcs_register(); +#endif +} diff --git a/nimble/host/mesh/src/health.c b/nimble/host/mesh/src/health.c new file mode 100644 index 000000000..124a8d177 --- /dev/null +++ b/nimble/host/mesh/src/health.c @@ -0,0 +1,451 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include + + +#define BT_DBG_ENABLED (MYNEWT_VAL(BLE_MESH_DEBUG_MODEL)) +#include "host/ble_hs_log.h" + +#include "mesh/mesh.h" +#include "mesh_priv.h" +#include "adv.h" +#include "net.h" +#include "transport.h" +#include "access.h" +#include "foundation.h" + +#define HEALTH_TEST_STANDARD 0x00 + +/* Increasing this requires also increasing the system workqueue */ +#define MAX_FAULTS 32 +#define HEALTH_STATUS_SIZE_MAX (1 + 3 + MAX_FAULTS + 4) + +#if BT_MESH_TX_SDU_MAX < HEALTH_STATUS_SIZE_MAX +#define HEALTH_STATUS_SIZE BT_MESH_TX_SDU_MAX +#else +#define HEALTH_STATUS_SIZE HEALTH_STATUS_SIZE_MAX +#endif + +/* Health Server context of the primary element */ +struct bt_mesh_health *health_srv; + +static void health_get_registered(struct bt_mesh_model *mod, + u16_t company_id, + struct os_mbuf *msg) +{ + struct bt_mesh_health *srv = mod->user_data; + u8_t fault_count; + u8_t *test_id; + int err; + + BT_DBG("Company ID 0x%04x", company_id); + + bt_mesh_model_msg_init(msg, OP_HEALTH_FAULT_STATUS); + + test_id = net_buf_simple_add(msg, 1); + net_buf_simple_add_le16(msg, company_id); + fault_count = net_buf_simple_tailroom(msg) - 4; + + if (srv->fault_get_reg) { + err = srv->fault_get_reg(mod, company_id, test_id, + net_buf_simple_tail(msg), + &fault_count); + if (err) { + BT_ERR("Failed to get faults (err %d)", err); + *test_id = HEALTH_TEST_STANDARD; + } else { + net_buf_simple_add(msg, fault_count); + } + } else { + BT_WARN("No callback for getting faults"); + *test_id = HEALTH_TEST_STANDARD; + } +} + +static size_t health_get_current(struct bt_mesh_model *mod, + struct os_mbuf *msg) +{ + struct bt_mesh_health *srv = mod->user_data; + u8_t *test_id, *company_ptr; + u16_t company_id; + u8_t fault_count; + int err; + + bt_mesh_model_msg_init(msg, OP_HEALTH_CURRENT_STATUS); + + test_id = net_buf_simple_add(msg, 1); + company_ptr = net_buf_simple_add(msg, sizeof(company_id)); + + fault_count = net_buf_simple_tailroom(msg) - 4; + + if (srv->fault_get_cur) { + err = srv->fault_get_cur(mod, test_id, &company_id, + net_buf_simple_tail(msg), + &fault_count); + if (err) { + BT_ERR("Failed to get faults (err %d)", err); + sys_put_le16(0, company_ptr); + *test_id = HEALTH_TEST_STANDARD; + fault_count = 0; + } else { + sys_put_le16(company_id, company_ptr); + } + } else { + BT_WARN("No callback for getting faults"); + net_buf_simple_push_u8(msg, HEALTH_TEST_STANDARD); + net_buf_simple_push_le16(msg, 0); + fault_count = 0; + } + + return fault_count; +} + +static void health_fault_get(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct os_mbuf *msg = NET_BUF_SIMPLE(HEALTH_STATUS_SIZE); + u16_t company_id; + + company_id = net_buf_simple_pull_le16(buf); + + BT_DBG("company_id 0x%04x", company_id); + + health_get_registered(model, company_id, msg); + + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send Health Current Status response"); + } + + os_mbuf_free_chain(msg); +} + +static void health_fault_clear_unrel(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct bt_mesh_health *srv = model->user_data; + u16_t company_id; + + company_id = net_buf_simple_pull_le16(buf); + + BT_DBG("company_id 0x%04x", company_id); + + if (srv->fault_clear) { + srv->fault_clear(model, company_id); + } +} + +static void health_fault_clear(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct os_mbuf *msg = NET_BUF_SIMPLE(HEALTH_STATUS_SIZE); + struct bt_mesh_health *srv = model->user_data; + u16_t company_id; + + company_id = net_buf_simple_pull_le16(buf); + + BT_DBG("company_id 0x%04x", company_id); + + if (srv->fault_clear) { + srv->fault_clear(model, company_id); + } + + health_get_registered(model, company_id, msg); + + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send Health Current Status response"); + } + + os_mbuf_free_chain(msg); +} + +static void health_fault_test_unrel(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct bt_mesh_health *srv = model->user_data; + const struct bt_mesh_comp *comp; + u16_t company_id; + u8_t test_id; + + test_id = net_buf_simple_pull_u8(buf); + company_id = net_buf_simple_pull_le16(buf); + + BT_DBG("test 0x%02x company 0x%04x", test_id, company_id); + + comp = bt_mesh_comp_get(); + if (comp->cid != company_id) { + BT_WARN("CID 0x%04x doesn't match composition CID 0x%04x", + company_id, comp->cid); + return; + } + + if (srv->fault_test) { + srv->fault_test(model, test_id, company_id); + } +} + +static void health_fault_test(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct os_mbuf *msg = NET_BUF_SIMPLE(HEALTH_STATUS_SIZE); + struct bt_mesh_health *srv = model->user_data; + u16_t company_id; + u8_t test_id; + + BT_DBG(""); + + test_id = net_buf_simple_pull_u8(buf); + company_id = net_buf_simple_pull_le16(buf); + + BT_DBG("test 0x%02x company 0x%04x", test_id, company_id); + + if (srv->fault_test) { + srv->fault_test(model, test_id, company_id); + } + + health_get_registered(model, company_id, msg); + + if (bt_mesh_model_send(model, ctx, msg, NULL, NULL)) { + BT_ERR("Unable to send Health Current Status response"); + } + + os_mbuf_free_chain(msg); +} + +static void send_attention_status(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx) +{ + /* Needed size: opcode (2 bytes) + msg + MIC */ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 1 + 4); + struct bt_mesh_health *srv = model->user_data; + u8_t time; + + time = k_delayed_work_remaining_get(&srv->attention.timer) / 1000; + BT_DBG("%u second%s", time, (time == 1) ? "" : "s"); + + bt_mesh_model_msg_init(msg, OP_ATTENTION_STATUS); + + net_buf_simple_add_u8(msg, time); + + bt_mesh_model_send(model, ctx, msg, NULL, NULL); + os_mbuf_free_chain(msg); +} + +static void attention_get(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + BT_DBG(""); + + send_attention_status(model, ctx); +} + +static void attention_set_unrel(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + u8_t time; + + time = net_buf_simple_pull_u8(buf); + + BT_DBG("%u second%s", time, (time == 1) ? "" : "s"); + + bt_mesh_attention(model, time); +} + +static void attention_set(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + BT_DBG(""); + + attention_set_unrel(model, ctx, buf); + + send_attention_status(model, ctx); +} + +static void send_health_period_status(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx) +{ + /* Needed size: opcode (2 bytes) + msg + MIC */ + struct os_mbuf *msg = NET_BUF_SIMPLE(2 + 1 + 4); + struct bt_mesh_health *srv = model->user_data; + + bt_mesh_model_msg_init(msg, OP_HEALTH_PERIOD_STATUS); + + net_buf_simple_add_u8(msg, srv->period); + + bt_mesh_model_send(model, ctx, msg, NULL, NULL); + os_mbuf_free_chain(msg); +} + +static void health_period_get(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + BT_DBG(""); + + send_health_period_status(model, ctx); +} + +static void health_period_set_unrel(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + struct bt_mesh_health *srv = model->user_data; + u8_t period; + + period = net_buf_simple_pull_u8(buf); + if (period > 15) { + BT_WARN("Prohibited period value %u", period); + return; + } + + BT_DBG("period %u", period); + + srv->period = period; +} + +static void health_period_set(struct bt_mesh_model *model, + struct bt_mesh_msg_ctx *ctx, + struct os_mbuf *buf) +{ + BT_DBG(""); + + health_period_set_unrel(model, ctx, buf); + + send_health_period_status(model, ctx); +} + +const struct bt_mesh_model_op bt_mesh_health_op[] = { + { OP_HEALTH_FAULT_GET, 2, health_fault_get }, + { OP_HEALTH_FAULT_CLEAR, 2, health_fault_clear }, + { OP_HEALTH_FAULT_CLEAR_UNREL, 2, health_fault_clear_unrel }, + { OP_HEALTH_FAULT_TEST, 3, health_fault_test }, + { OP_HEALTH_FAULT_TEST_UNREL, 3, health_fault_test_unrel }, + { OP_HEALTH_PERIOD_GET, 0, health_period_get }, + { OP_HEALTH_PERIOD_SET, 1, health_period_set }, + { OP_HEALTH_PERIOD_SET_UNREL, 1, health_period_set_unrel }, + { OP_ATTENTION_GET, 0, attention_get }, + { OP_ATTENTION_SET, 1, attention_set }, + { OP_ATTENTION_SET_UNREL, 1, attention_set_unrel }, + BT_MESH_MODEL_OP_END, +}; + +static void health_pub(struct bt_mesh_model *mod) +{ + struct os_mbuf *msg = NET_BUF_SIMPLE(HEALTH_STATUS_SIZE); + struct bt_mesh_health *srv = mod->user_data; + size_t count; + int err; + + BT_DBG(""); + + count = health_get_current(mod, msg); + if (count) { + mod->pub->period_div = srv->period; + } else { + mod->pub->period_div = 0; + } + + err = bt_mesh_model_publish(mod, msg); + if (err) { + BT_ERR("Publishing failed (err %d)", err); + } + os_mbuf_free_chain(msg); +} + +struct bt_mesh_model_pub bt_mesh_health_pub = { + .func = health_pub, +}; + +int bt_mesh_fault_update(struct bt_mesh_elem *elem) +{ + struct bt_mesh_model *mod; + + mod = bt_mesh_model_find(elem, BT_MESH_MODEL_ID_HEALTH_SRV); + if (!mod) { + return -EINVAL; + } + + k_delayed_work_submit(&mod->pub->timer, K_NO_WAIT); + return 0; +} + +static void attention_off(struct os_event *work) +{ + struct bt_mesh_health *srv = work->ev_arg; + BT_DBG(""); + + if (srv->attention.off) { + srv->attention.off(srv->model); + } +} + +int bt_mesh_health_init(struct bt_mesh_model *model, bool primary) +{ + struct bt_mesh_health *srv = model->user_data; + + if (!srv) { + if (!primary) { + return 0; + } + + BT_ERR("No Health Server context provided"); + return -EINVAL; + } + + k_delayed_work_init(&srv->attention.timer, attention_off); + + srv->model = model; + + if (primary) { + health_srv = srv; + } + + return 0; +} + +void bt_mesh_attention(struct bt_mesh_model *model, u8_t time) +{ + struct bt_mesh_health *srv; + + BT_DBG("bt_mesh_attention"); + if (!model) { + srv = health_srv; + if (!srv) { + BT_WARN("No Health Server available"); + return; + } + + model = srv->model; + } else { + srv = model->user_data; + } + + if (time) { + if (srv->attention.on) { + srv->attention.on(model); + } + + k_delayed_work_submit(&srv->attention.timer, time * 1000); + } else { + k_delayed_work_cancel(&srv->attention.timer); + + if (srv->attention.off) { + srv->attention.off(model); + } + } +} diff --git a/nimble/host/mesh/src/lpn.c b/nimble/host/mesh/src/lpn.c new file mode 100644 index 000000000..2cfd9eb0a --- /dev/null +++ b/nimble/host/mesh/src/lpn.c @@ -0,0 +1,883 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "syscfg/syscfg.h" + +#if MYNEWT_VAL(BLE_MESH_LOW_POWER) == 1 + +#include + +#define BT_DBG_ENABLED (MYNEWT_VAL(BLE_MESH_DEBUG_LOW_POWER)) +#include "host/ble_hs_log.h" + +#include "mesh/mesh.h" +#include "mesh_priv.h" +#include "crypto.h" +#include "adv.h" +#include "net.h" +#include "transport.h" +#include "access.h" +#include "beacon.h" +#include "lpn.h" + +#define LPN_RECV_DELAY MYNEWT_VAL(BLE_MESH_LPN_RECV_DELAY) +#define SCAN_LATENCY min(MYNEWT_VAL(BLE_MESH_LPN_SCAN_LATENCY), \ + LPN_RECV_DELAY) + +#define FRIEND_REQ_RETRY_TIMEOUT K_SECONDS(5) +#define FRIEND_REQ_TIMEOUT (K_MSEC(100) + K_SECONDS(1)) + +#define POLL_RETRY_TIMEOUT K_MSEC(100) + +#define REQ_ATTEMPTS(lpn) ((lpn)->poll_timeout < K_SECONDS(3) ? 2 : 4) + +#define REQ_RETRY_DURATION(lpn) (REQ_ATTEMPTS(lpn) * \ + (LPN_RECV_DELAY + (lpn)->recv_win + \ + POLL_RETRY_TIMEOUT)) + +#define POLL_TIMEOUT(lpn) ((MYNEWT_VAL(BLE_MESH_LPN_POLL_TIMEOUT) * 100) - \ + REQ_RETRY_DURATION(lpn)) + +#define LPN_CRITERIA ((MYNEWT_VAL(BLE_MESH_LPN_MIN_QUEUE_SIZE)) | \ + (MYNEWT_VAL(BLE_MESH_LPN_RSSI_FACTOR) << 3) | \ + (MYNEWT_VAL(BLE_MESH_LPN_RECV_WIN_FACTOR) << 5)) + +#define POLL_TO(to) { (u8_t)((to) >> 16), (u8_t)((to) >> 8), (u8_t)(to) } +#define LPN_POLL_TO POLL_TO(MYNEWT_VAL(BLE_MESH_LPN_POLL_TIMEOUT)) + +#if (MYNEWT_VAL(BLE_MESH_DEBUG_LOW_POWER)) +static const char *state2str(int state) +{ + switch (state) { + case BT_MESH_LPN_DISABLED: + return "disabled"; + case BT_MESH_LPN_CLEAR: + return "clear"; + case BT_MESH_LPN_ENABLED: + return "enabled"; + case BT_MESH_LPN_WAIT_OFFER: + return "wait offer"; + case BT_MESH_LPN_ESTABLISHING: + return "establishing"; + case BT_MESH_LPN_ESTABLISHED: + return "established"; + case BT_MESH_LPN_RECV_DELAY: + return "recv delay"; + case BT_MESH_LPN_WAIT_UPDATE: + return "wait update"; + default: + return "(unknown)"; + } +} +#endif /* CONFIG_BLUETOOTH_MESH_DEBUG_LPN */ + +static inline void lpn_set_state(int state) +{ +#if (MYNEWT_VAL(BLE_MESH_DEBUG_LOW_POWER)) + BT_DBG("%s -> %s", state2str(bt_mesh.lpn.state), state2str(state)); +#endif + bt_mesh.lpn.state = state; +} + +static void clear_friendship(bool disable); + +static void friend_clear_sent(struct os_mbuf *buf, int err) +{ + /* We're switching away from Low Power behavior, so permanently + * enable scanning. + */ + bt_mesh_scan_enable(); + + if (err) { + BT_ERR("Sending Friend Request failed (err %d)", err); + lpn_set_state(BT_MESH_LPN_ENABLED); + clear_friendship(bt_mesh.lpn.disable); + return; + } + + lpn_set_state(BT_MESH_LPN_CLEAR); + k_delayed_work_submit(&bt_mesh.lpn.timer, FRIEND_REQ_TIMEOUT); +} + +static int send_friend_clear(void) +{ + struct bt_mesh_msg_ctx ctx = { + .net_idx = bt_mesh.sub[0].net_idx, + .app_idx = BT_MESH_KEY_UNUSED, + .addr = bt_mesh.lpn.frnd, + .send_ttl = 0, + }; + struct bt_mesh_net_tx tx = { + .sub = &bt_mesh.sub[0], + .ctx = &ctx, + .src = bt_mesh_primary_addr(), + }; + struct bt_mesh_ctl_friend_clear req = { + .lpn_addr = sys_cpu_to_be16(tx.src), + .lpn_counter = sys_cpu_to_be16(bt_mesh.lpn.counter), + }; + + BT_DBG(""); + + return bt_mesh_ctl_send(&tx, TRANS_CTL_OP_FRIEND_CLEAR, &req, + sizeof(req), friend_clear_sent); +} + +static void clear_friendship(bool disable) +{ + struct bt_mesh_lpn *lpn = &bt_mesh.lpn; + + if (lpn->state >= BT_MESH_LPN_ESTABLISHED) { + send_friend_clear(); + lpn->disable = disable; + return; + } + + bt_mesh_rx_reset(); + + k_delayed_work_cancel(&lpn->timer); + + bt_mesh_friend_cred_del(bt_mesh.sub[0].net_idx, lpn->frnd); + + lpn->frnd = BT_MESH_ADDR_UNASSIGNED; + lpn->fsn = 0; + lpn->req_attempts = 0; + lpn->recv_win = 0; + lpn->queue_size = 0; + lpn->disable = 0; + lpn->sent_req = 0; + + /* Set this to 1 to force group subscription when the next + * Friendship is created, in case lpn->groups doesn't get + * modified meanwhile. + */ + lpn->groups_changed = 1; + + if (disable) { + lpn_set_state(BT_MESH_LPN_DISABLED); + return; + } + + lpn_set_state(BT_MESH_LPN_ENABLED); + k_delayed_work_submit(&lpn->timer, FRIEND_REQ_RETRY_TIMEOUT); +} + +static void friend_req_sent(struct os_mbuf *buf, int err) +{ + if (err) { + BT_ERR("Sending Friend Request failed (err %d)", err); + return; + } + + lpn_set_state(BT_MESH_LPN_WAIT_OFFER); + k_delayed_work_submit(&bt_mesh.lpn.timer, FRIEND_REQ_TIMEOUT); +} + +static int send_friend_req(void) +{ + const struct bt_mesh_comp *comp = bt_mesh_comp_get(); + struct bt_mesh_msg_ctx ctx = { + .net_idx = bt_mesh.sub[0].net_idx, + .app_idx = BT_MESH_KEY_UNUSED, + .addr = BT_MESH_ADDR_FRIENDS, + .send_ttl = 0, + }; + struct bt_mesh_net_tx tx = { + .sub = &bt_mesh.sub[0], + .ctx = &ctx, + .src = bt_mesh_primary_addr(), + }; + struct bt_mesh_ctl_friend_req req = { + .criteria = LPN_CRITERIA, + .recv_delay = LPN_RECV_DELAY, + .poll_to = LPN_POLL_TO, + .prev_addr = BT_MESH_ADDR_UNASSIGNED, + .num_elem = comp->elem_count, + .lpn_counter = sys_cpu_to_be16(bt_mesh.lpn.counter), + }; + + BT_DBG(""); + + return bt_mesh_ctl_send(&tx, TRANS_CTL_OP_FRIEND_REQ, &req, + sizeof(req), friend_req_sent); +} + +static inline void group_zero(atomic_t *target) +{ +#if CONFIG_BLUETOOTH_MESH_LPN_GROUPS > 32 + int i; + + for (i = 0; i < ARRAY_SIZE(bt_mesh.lpn.added); i++) { + atomic_set(&target[i], 0); + } +#else + atomic_set(target, 0); +#endif +} + +static inline void group_set(atomic_t *target, atomic_t *source) +{ +#if CONFIG_BLUETOOTH_MESH_LPN_GROUPS > 32 + int i; + + for (i = 0; i < ARRAY_SIZE(bt_mesh.lpn.added); i++) { + atomic_or(&target[i], atomic_get(&source[i])); + } +#else + atomic_or(target, atomic_get(source)); +#endif +} + +static inline void group_clear(atomic_t *target, atomic_t *source) +{ +#if CONFIG_BLUETOOTH_MESH_LPN_GROUPS > 32 + int i; + + for (i = 0; i < ARRAY_SIZE(bt_mesh.lpn.added); i++) { + atomic_and(&target[i], ~atomic_get(&source[i])); + } +#else + atomic_and(target, ~atomic_get(source)); +#endif +} + +static void req_sent(struct os_mbuf *buf, int err) +{ + struct bt_mesh_lpn *lpn = &bt_mesh.lpn; + + if (err) { + BT_ERR("Sending request failed (err %d)", err); + lpn->sent_req = 0; + group_zero(lpn->pending); + return; + } + + lpn->req_attempts++; + + if (lpn->state == BT_MESH_LPN_ESTABLISHING) { + k_delayed_work_submit(&lpn->timer, + LPN_RECV_DELAY + lpn->recv_win); + } else { /* Established Friendship */ + lpn_set_state(BT_MESH_LPN_RECV_DELAY); + /* We start scanning a bit early to elimitate risk of missing + * response data due to HCI and other latencies. + */ + k_delayed_work_submit(&lpn->timer, + LPN_RECV_DELAY - SCAN_LATENCY); + } +} + +static int send_friend_poll(void) +{ + struct bt_mesh_msg_ctx ctx = { + .net_idx = bt_mesh.sub[0].net_idx, + .app_idx = BT_MESH_KEY_UNUSED, + .addr = bt_mesh.lpn.frnd, + .send_ttl = 0, + .friend_cred = 1, + }; + struct bt_mesh_net_tx tx = { + .sub = &bt_mesh.sub[0], + .ctx = &ctx, + .src = bt_mesh_primary_addr(), + }; + struct bt_mesh_lpn *lpn = &bt_mesh.lpn; + u8_t fsn = lpn->fsn; + int err; + + BT_DBG("lpn->sent_req 0x%02x", lpn->sent_req); + + if (lpn->sent_req) { + if (lpn->sent_req != TRANS_CTL_OP_FRIEND_POLL) { + lpn->pending_poll = 1; + } + + return 0; + } + + err = bt_mesh_ctl_send(&tx, TRANS_CTL_OP_FRIEND_POLL, &fsn, 1, + req_sent); + if (err == 0) { + lpn->pending_poll = 0; + lpn->sent_req = TRANS_CTL_OP_FRIEND_POLL; + } + + return err; +} + +void bt_mesh_lpn_disable(void) +{ + if (bt_mesh.lpn.state == BT_MESH_LPN_DISABLED) { + return; + } + + clear_friendship(true); +} + +int bt_mesh_lpn_set(bool enable) +{ + if (enable) { + if (bt_mesh.lpn.state != BT_MESH_LPN_DISABLED) { + return 0; + } + } else { + if (bt_mesh.lpn.state == BT_MESH_LPN_DISABLED) { + return 0; + } + } + + if (!bt_mesh_is_provisioned()) { + if (enable) { + lpn_set_state(BT_MESH_LPN_ENABLED); + } else { + lpn_set_state(BT_MESH_LPN_DISABLED); + } + + return 0; + } + + if (enable) { + send_friend_req(); + } else { + bt_mesh_lpn_disable(); + } + + return 0; +} + +static void friend_response_received(struct bt_mesh_lpn *lpn) +{ + BT_DBG("lpn->sent_req 0x%02x", lpn->sent_req); + + if (lpn->sent_req == TRANS_CTL_OP_FRIEND_POLL) { + lpn->fsn++; + } + + k_delayed_work_cancel(&lpn->timer); + bt_mesh_scan_disable(); + lpn_set_state(BT_MESH_LPN_ESTABLISHED); + lpn->req_attempts = 0; + lpn->sent_req = 0; +} + +void bt_mesh_lpn_msg_received(struct bt_mesh_net_rx *rx) +{ + struct bt_mesh_lpn *lpn = &bt_mesh.lpn; + + if (lpn->sent_req != TRANS_CTL_OP_FRIEND_POLL) { + BT_WARN("Unexpected message withouth a preceding Poll"); + return; + } + + friend_response_received(lpn); + + BT_DBG("Requesting more messages from Friend"); + + send_friend_poll(); +} + +int bt_mesh_lpn_friend_offer(struct bt_mesh_net_rx *rx, + struct os_mbuf *buf) +{ + struct bt_mesh_ctl_friend_offer *msg = (void *)buf->om_data; + struct bt_mesh_lpn *lpn = &bt_mesh.lpn; + struct bt_mesh_subnet *sub = rx->sub; + struct bt_mesh_friend_cred *cred; + u16_t frnd_counter; + int err; + + if (buf->om_len < sizeof(*msg)) { + BT_WARN("Too short Friend Offer"); + return -EINVAL; + } + + if (lpn->state != BT_MESH_LPN_WAIT_OFFER) { + BT_WARN("Ignoring unexpected Friend Offer"); + return 0; + } + + frnd_counter = sys_be16_to_cpu(msg->frnd_counter); + + BT_DBG("recv_win %u queue_size %u sub_list_size %u rssi %d counter %u", + msg->recv_win, msg->queue_size, msg->sub_list_size, msg->rssi, + frnd_counter); + + lpn->frnd = rx->ctx.addr; + + cred = bt_mesh_friend_cred_add(sub->net_idx, sub->keys[0].net, 0, + lpn->frnd, lpn->counter, frnd_counter); + if (!cred) { + lpn->frnd = BT_MESH_ADDR_UNASSIGNED; + return -ENOMEM; + } + + if (sub->kr_flag) { + err = bt_mesh_friend_cred_set(cred, 1, sub->keys[1].net); + if (err) { + bt_mesh_friend_cred_clear(cred); + lpn->frnd = BT_MESH_ADDR_UNASSIGNED; + return err; + } + } + + /* TODO: Add offer acceptance criteria check */ + + k_delayed_work_cancel(&lpn->timer); + + lpn->recv_win = msg->recv_win; + lpn->queue_size = msg->queue_size; + + err = send_friend_poll(); + if (err) { + bt_mesh_friend_cred_clear(cred); + lpn->frnd = BT_MESH_ADDR_UNASSIGNED; + lpn->recv_win = 0; + lpn->queue_size = 0; + return err; + } + + lpn->counter++; + lpn_set_state(BT_MESH_LPN_ESTABLISHING); + + return 0; +} + +int bt_mesh_lpn_friend_clear_cfm(struct bt_mesh_net_rx *rx, + struct os_mbuf *buf) +{ + struct bt_mesh_ctl_friend_clear_confirm *msg = (void *)buf->om_data; + struct bt_mesh_lpn *lpn = &bt_mesh.lpn; + u16_t addr, counter; + + if (buf->om_len < sizeof(*msg)) { + BT_WARN("Too short Friend Clear Confirm"); + return -EINVAL; + } + + if (lpn->state != BT_MESH_LPN_CLEAR) { + BT_WARN("Ignoring unexpected Friend Clear Confirm"); + return 0; + } + + addr = sys_be16_to_cpu(msg->lpn_addr); + counter = sys_be16_to_cpu(msg->lpn_counter); + + BT_DBG("LPNAddress 0x%04x LPNCounter 0x%04x", addr, counter); + + if (addr != bt_mesh_primary_addr() || counter != lpn->counter) { + BT_WARN("Invalid parameters in Friend Clear Confirm"); + return 0; + } + + clear_friendship(bt_mesh.lpn.disable); + + return 0; +} + +static void lpn_group_add(u16_t group) +{ + struct bt_mesh_lpn *lpn = &bt_mesh.lpn; + u16_t *free_slot = NULL; + int i; + + for (i = 0; i < ARRAY_SIZE(lpn->groups); i++) { + if (lpn->groups[i] == group) { + atomic_clear_bit(lpn->to_remove, i); + return; + } + + if (!free_slot && lpn->groups[i] == BT_MESH_ADDR_UNASSIGNED) { + free_slot = &lpn->groups[i]; + } + } + + if (!free_slot) { + BT_WARN("Friend Subscription List exceeded!"); + return; + } + + *free_slot = group; + lpn->groups_changed = 1; +} + +static void lpn_group_del(u16_t group) +{ + struct bt_mesh_lpn *lpn = &bt_mesh.lpn; + int i; + + for (i = 0; i < ARRAY_SIZE(lpn->groups); i++) { + if (lpn->groups[i] == group) { + if (atomic_test_bit(lpn->added, i) || + atomic_test_bit(lpn->pending, i)) { + atomic_set_bit(lpn->to_remove, i); + lpn->groups_changed = 1; + } else { + lpn->groups[i] = BT_MESH_ADDR_UNASSIGNED; + } + } + } +} + +static inline int group_popcount(atomic_t *target) +{ +#if CONFIG_BLUETOOTH_MESH_LPN_GROUPS > 32 + int i, count = 0; + + for (i = 0; i < ARRAY_SIZE(bt_mesh.lpn.added); i++) { + count += popcount(atomic_get(&target[i])); + } +#else + return popcount(atomic_get(target)); +#endif +} + +static bool sub_update(u8_t op) +{ + struct bt_mesh_lpn *lpn = &bt_mesh.lpn; + int added_count = group_popcount(lpn->added); + struct bt_mesh_msg_ctx ctx = { + .net_idx = bt_mesh.sub[0].net_idx, + .app_idx = BT_MESH_KEY_UNUSED, + .addr = lpn->frnd, + .send_ttl = 0, + .friend_cred = 1, + }; + struct bt_mesh_net_tx tx = { + .sub = &bt_mesh.sub[0], + .ctx = &ctx, + .src = bt_mesh_primary_addr(), + }; + struct bt_mesh_ctl_friend_sub req; + size_t i, g; + + if (lpn->sent_req) { + return false; + } + + for (i = 0, g = 0; i < ARRAY_SIZE(lpn->groups); i++) { + if (lpn->groups[i] == BT_MESH_ADDR_UNASSIGNED) { + continue; + } + + if (op == TRANS_CTL_OP_FRIEND_SUB_ADD) { + if (atomic_test_bit(lpn->added, i)) { + continue; + } + } else { + if (!atomic_test_bit(lpn->to_remove, i)) { + continue; + } + } + + if (added_count + g >= lpn->queue_size) { + BT_WARN("Friend Queue Size exceeded"); + break; + } + + req.addr_list[g++] = sys_cpu_to_be16(lpn->groups[i]); + atomic_set_bit(lpn->pending, i); + + if (g == ARRAY_SIZE(req.addr_list)) { + break; + } + } + + if (g == 0) { + group_zero(lpn->pending); + return false; + } + + req.xact = lpn->xact_next++; + + if (bt_mesh_ctl_send(&tx, op, &req, 1 + g * 2, req_sent) < 0) { + group_zero(lpn->pending); + return false; + } + + lpn->xact_pending = req.xact; + lpn->sent_req = op; + return true; +} + +static void lpn_timeout(struct os_event *work) +{ + struct bt_mesh_lpn *lpn = &bt_mesh.lpn; + +#if (MYNEWT_VAL(BLE_MESH_DEBUG_LOW_POWER)) + BT_DBG("state: %s", state2str(lpn->state)); +#endif + + switch (lpn->state) { + case BT_MESH_LPN_DISABLED: + break; + case BT_MESH_LPN_CLEAR: + clear_friendship(bt_mesh.lpn.disable); + break; + case BT_MESH_LPN_ENABLED: + send_friend_req(); + break; + case BT_MESH_LPN_WAIT_OFFER: + BT_WARN("No acceptable Friend Offers received"); + lpn->counter++; + lpn_set_state(BT_MESH_LPN_ENABLED); + k_delayed_work_submit(&lpn->timer, FRIEND_REQ_RETRY_TIMEOUT); + break; + case BT_MESH_LPN_ESTABLISHING: + BT_ERR("Timed out waiting for first Friend Update"); + clear_friendship(false); + break; + case BT_MESH_LPN_ESTABLISHED: + if (lpn->req_attempts < REQ_ATTEMPTS(lpn)) { + u8_t req = lpn->sent_req; + + lpn->sent_req = 0; + + if (!req || req == TRANS_CTL_OP_FRIEND_POLL) { + send_friend_poll(); + } else { + sub_update(req); + } + + break; + } + + BT_ERR("No response from Friend after %u retries", + lpn->req_attempts); + clear_friendship(false); + break; + case BT_MESH_LPN_RECV_DELAY: + bt_mesh_scan_enable(); + lpn_set_state(BT_MESH_LPN_WAIT_UPDATE); + k_delayed_work_submit(&lpn->timer, + lpn->recv_win + SCAN_LATENCY); + break; + case BT_MESH_LPN_WAIT_UPDATE: + BT_WARN("No response from Friend during ReceiveWindow"); + bt_mesh_scan_disable(); + lpn_set_state(BT_MESH_LPN_ESTABLISHED); + k_delayed_work_submit(&lpn->timer, POLL_RETRY_TIMEOUT); + break; + default: + __ASSERT(0, "Unhandled LPN state"); + break; + } +} + +void bt_mesh_lpn_group_add(u16_t group) +{ + BT_DBG("group 0x%04x", group); + + lpn_group_add(group); + + if (!bt_mesh_lpn_established() || bt_mesh.lpn.sent_req) { + return; + } + + sub_update(TRANS_CTL_OP_FRIEND_SUB_ADD); +} + +void bt_mesh_lpn_group_del(u16_t *groups, size_t group_count) +{ + int i; + + for (i = 0; i < group_count; i++) { + if (groups[i] != BT_MESH_ADDR_UNASSIGNED) { + BT_DBG("group 0x%04x", groups[i]); + lpn_group_del(groups[i]); + } + } + + if (!bt_mesh_lpn_established() || bt_mesh.lpn.sent_req) { + return; + } + + sub_update(TRANS_CTL_OP_FRIEND_SUB_REM); +} + +static s32_t poll_timeout(struct bt_mesh_lpn *lpn) +{ + /* If we're waiting for segment acks keep polling at high freq */ + if (bt_mesh_tx_in_progress()) { + return min(POLL_TIMEOUT(lpn), K_SECONDS(1)); + } + + if (lpn->poll_timeout < POLL_TIMEOUT(lpn)) { + lpn->poll_timeout *= 2; + lpn->poll_timeout = min(lpn->poll_timeout, POLL_TIMEOUT(lpn)); + } + + BT_DBG("Poll Timeout is %ums", lpn->poll_timeout); + + return lpn->poll_timeout; +} + +int bt_mesh_lpn_friend_sub_cfm(struct bt_mesh_net_rx *rx, + struct os_mbuf *buf) +{ + struct bt_mesh_ctl_friend_sub_confirm *msg = (void *)buf->om_data; + struct bt_mesh_lpn *lpn = &bt_mesh.lpn; + + if (buf->om_len < sizeof(*msg)) { + BT_WARN("Too short Friend Subscription Confirm"); + return -EINVAL; + } + + BT_DBG("xact 0x%02x", msg->xact); + + if (!lpn->sent_req) { + BT_WARN("No pending subscription list message"); + return 0; + } + + if (msg->xact != lpn->xact_pending) { + BT_WARN("Transaction mismatch (0x%02x != 0x%02x)", + msg->xact, lpn->xact_pending); + return 0; + } + + if (lpn->sent_req == TRANS_CTL_OP_FRIEND_SUB_ADD) { + group_set(lpn->added, lpn->pending); + group_zero(lpn->pending); + } else if (lpn->sent_req == TRANS_CTL_OP_FRIEND_SUB_REM) { + int i; + + group_clear(lpn->added, lpn->pending); + + for (i = 0; i < ARRAY_SIZE(lpn->groups); i++) { + if (atomic_test_and_clear_bit(lpn->pending, i) && + atomic_test_and_clear_bit(lpn->to_remove, i)) { + lpn->groups[i] = BT_MESH_ADDR_UNASSIGNED; + } + } + } else { + BT_WARN("Unexpected Friend Subscription Confirm"); + return 0; + } + + friend_response_received(lpn); + + if (lpn->groups_changed) { + sub_update(TRANS_CTL_OP_FRIEND_SUB_ADD); + sub_update(TRANS_CTL_OP_FRIEND_SUB_REM); + + if (!lpn->sent_req) { + lpn->groups_changed = 0; + } + } + + if (lpn->pending_poll) { + send_friend_poll(); + } + + if (!lpn->sent_req) { + k_delayed_work_submit(&lpn->timer, poll_timeout(lpn)); + } + + return 0; +} + +int bt_mesh_lpn_friend_update(struct bt_mesh_net_rx *rx, + struct os_mbuf *buf) +{ + struct bt_mesh_ctl_friend_update *msg = (void *)buf->om_data; + struct bt_mesh_lpn *lpn = &bt_mesh.lpn; + struct bt_mesh_subnet *sub = rx->sub; + u32_t iv_index; + + if (buf->om_len < sizeof(*msg)) { + BT_WARN("Too short Friend Update"); + return -EINVAL; + } + + if (lpn->sent_req != TRANS_CTL_OP_FRIEND_POLL) { + BT_WARN("Unexpected friend update"); + return 0; + } + + if (sub->kr_phase == BT_MESH_KR_PHASE_2 && !rx->new_key) { + BT_WARN("Ignoring Phase 2 KR Update secured using old key"); + return 0; + } + + if (bt_mesh.ivu_initiator && + bt_mesh.iv_update == BT_MESH_IV_UPDATE(msg->flags)) { + bt_mesh_beacon_ivu_initiator(false); + } + + if (lpn->state == BT_MESH_LPN_ESTABLISHING) { + /* This is normally checked on the transport layer, however + * in this state we're also still accepting master + * credentials so we need to ensure the right ones (Friend + * Credentials) were used for this message. + */ + if (!rx->ctx.friend_cred) { + BT_WARN("Friend Update with wrong credentials"); + return -EINVAL; + } + + BT_INFO("Friendship established with 0x%04x", lpn->frnd); + + /* Set initial poll timeout */ + lpn->poll_timeout = min(POLL_TIMEOUT(lpn), K_SECONDS(1)); + } + + friend_response_received(lpn); + + iv_index = sys_be32_to_cpu(msg->iv_index); + + BT_DBG("flags 0x%02x iv_index 0x%08x md %u", msg->flags, iv_index, + msg->md); + + if (bt_mesh_kr_update(sub, BT_MESH_KEY_REFRESH(msg->flags), + rx->new_key)) { + bt_mesh_net_beacon_update(sub); + } + + bt_mesh_iv_update(iv_index, BT_MESH_IV_UPDATE(msg->flags)); + + if (lpn->groups_changed) { + sub_update(TRANS_CTL_OP_FRIEND_SUB_ADD); + sub_update(TRANS_CTL_OP_FRIEND_SUB_REM); + + if (!lpn->sent_req) { + lpn->groups_changed = 0; + } + } + + if (msg->md) { + BT_DBG("Requesting for more messages"); + send_friend_poll(); + } + + if (!lpn->sent_req) { + k_delayed_work_submit(&lpn->timer, poll_timeout(lpn)); + } + + return 0; +} + +void bt_mesh_lpn_friend_poll(void) +{ + BT_DBG("Requesting more messages"); + send_friend_poll(); +} + +int bt_mesh_lpn_init(void) +{ + struct bt_mesh_lpn *lpn = &bt_mesh.lpn; + + k_delayed_work_init(&lpn->timer, lpn_timeout); + + if (lpn->state == BT_MESH_LPN_ENABLED) { + send_friend_req(); + } + + return 0; +} + +#endif //MYNEWT_VAL(BLE_MESH_LOW_POWER) == 1 diff --git a/nimble/host/mesh/src/lpn.h b/nimble/host/mesh/src/lpn.h new file mode 100644 index 000000000..b1df8b315 --- /dev/null +++ b/nimble/host/mesh/src/lpn.h @@ -0,0 +1,59 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ +#ifndef __LPN_H__ +#define __LPN_H__ + +#include "mesh/mesh.h" + +int +bt_mesh_lpn_friend_update(struct bt_mesh_net_rx *rx, struct os_mbuf *buf); +int +bt_mesh_lpn_friend_offer(struct bt_mesh_net_rx *rx, struct os_mbuf *buf); +int +bt_mesh_lpn_friend_clear_cfm(struct bt_mesh_net_rx *rx, struct os_mbuf *buf); +int +bt_mesh_lpn_friend_sub_cfm(struct bt_mesh_net_rx *rx, struct os_mbuf *buf); + +static inline bool +bt_mesh_lpn_established(void) +{ +#if (MYNEWT_VAL(BLE_MESH_LOW_POWER)) + return (bt_mesh.lpn.state >= BT_MESH_LPN_ESTABLISHED); +#else + return false; +#endif +} + +static inline bool +bt_mesh_lpn_waiting_update(void) +{ +#if (MYNEWT_VAL(BLE_MESH_LOW_POWER)) + return (bt_mesh.lpn.state == BT_MESH_LPN_WAIT_UPDATE); +#else + return false; +#endif +} + +void +bt_mesh_lpn_friend_poll(void); + +void +bt_mesh_lpn_msg_received(struct bt_mesh_net_rx *rx); + +void +bt_mesh_lpn_group_add(u16_t group); +void +bt_mesh_lpn_group_del(u16_t *groups, size_t group_count); + +void +bt_mesh_lpn_disable(void); + +int +bt_mesh_lpn_init(void); + +#endif diff --git a/nimble/host/mesh/src/mesh.c b/nimble/host/mesh/src/mesh.c new file mode 100644 index 000000000..9ab61321c --- /dev/null +++ b/nimble/host/mesh/src/mesh.c @@ -0,0 +1,202 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include + +#include "syscfg/syscfg.h" +#include "os/os_mbuf.h" +#include "mesh/mesh.h" + +#define BT_DBG_ENABLED (MYNEWT_VAL(BLE_MESH_DEBUG)) +#include "host/ble_hs_log.h" + +#include "adv.h" +#include "prov.h" +#include "net.h" +#include "beacon.h" +#include "lpn.h" +#include "friend.h" +#include "transport.h" +#include "access.h" +#include "foundation.h" +#include "proxy.h" +#include "mesh_priv.h" + +static bool provisioned; + +int +bt_mesh_provision(const u8_t net_key[16], u16_t net_idx, u8_t flags, + u32_t iv_index, u32_t seq, u16_t addr, const u8_t dev_key[16]) +{ + int err; + + BLE_HS_LOG(INFO, "Primary Element: 0x%04x", addr); + + if ((MYNEWT_VAL(BLE_MESH_PB_GATT))) { + bt_mesh_proxy_prov_disable(); + } + + err = bt_mesh_net_create(net_idx, flags, net_key, iv_index); + if (err) { + if ((MYNEWT_VAL(BLE_MESH_PB_GATT))) { + bt_mesh_proxy_prov_enable(); + } + + return err; + } + + bt_mesh.seq = seq; + + bt_mesh_comp_provision(addr); + + memcpy(bt_mesh.dev_key, dev_key, 16); + + provisioned = true; + + if (bt_mesh_beacon_get() == BT_MESH_BEACON_ENABLED) { + bt_mesh_beacon_enable(); + } + else { + bt_mesh_beacon_disable(); + } + + if ((MYNEWT_VAL(BLE_MESH_GATT_PROXY)) + && bt_mesh_gatt_proxy_get() != BT_MESH_GATT_PROXY_NOT_SUPPORTED) { + bt_mesh_proxy_gatt_enable(); + bt_mesh_adv_update(); + } + + /* If PB-ADV is disabled then scanning will have been disabled */ + if (!(MYNEWT_VAL(BLE_MESH_PB_ADV))) { + bt_mesh_scan_enable(); + } + + if ((MYNEWT_VAL(BLE_MESH_LOW_POWER))) { + bt_mesh_lpn_init(); + } + + if ((MYNEWT_VAL(BLE_MESH_FRIEND))) { + bt_mesh_friend_init(); + } + + return 0; +} + +void +bt_mesh_reset(void) +{ + if (!provisioned) { + goto enable_beacon; + } + + bt_mesh_comp_unprovision(); + + bt_mesh.iv_index = 0; + bt_mesh.seq = 0; + bt_mesh.iv_update = 0; + bt_mesh.valid = 0; + bt_mesh.last_update = 0; + bt_mesh.ivu_initiator = 0; + + k_delayed_work_cancel(&bt_mesh.ivu_complete); + + if ((MYNEWT_VAL(BLE_MESH_LOW_POWER))) { + bt_mesh_lpn_disable(); + } + + if ((MYNEWT_VAL(BLE_MESH_GATT_PROXY))) { + bt_mesh_proxy_gatt_disable(); + } + + if ((MYNEWT_VAL(BLE_MESH_PB_GATT))) { + bt_mesh_proxy_prov_enable(); + } + + memset(bt_mesh.dev_key, 0, sizeof(bt_mesh.dev_key)); + + memset(bt_mesh.rpl, 0, sizeof(bt_mesh.rpl)); + + provisioned = false; + +enable_beacon: + if ((MYNEWT_VAL(BLE_MESH_PB_ADV))) { + /* Make sure we're scanning for provisioning inviations */ + bt_mesh_scan_enable(); + /* Enable unprovisioned beacon sending */ + bt_mesh_beacon_enable(); + } + else { + bt_mesh_scan_disable(); + bt_mesh_beacon_disable(); + } +} + +bool +bt_mesh_is_provisioned(void) +{ + return provisioned; +} + +static int +bt_mesh_gap_event(struct ble_gap_event *event, void *arg) +{ + ble_adv_gap_mesh_cb(event, arg); + +#if (MYNEWT_VAL(BLE_MESH_PROXY)) + ble_mesh_proxy_gap_event(event, arg); +#endif + + return 0; +} + +int +bt_mesh_init(uint8_t own_addr_type, const struct bt_mesh_prov *prov, + const struct bt_mesh_comp *comp) +{ + int err; + + err = bt_mesh_comp_register(comp); + if (err) { + return err; + } + + if (MYNEWT_VAL(BLE_MESH_PROV)) { + bt_mesh_prov_init(prov); + } + +#if (MYNEWT_VAL(BLE_MESH_PROXY)) + bt_mesh_proxy_init(); + /* Need this to proper link.rx.buf allocation */ + bt_mesh_prov_reset_link(); +#endif + + bt_mesh_net_init(); + + bt_mesh_trans_init(); + + bt_mesh_beacon_init(); + + bt_mesh_adv_init(own_addr_type); + +#if (MYNEWT_VAL(BLE_MESH_PB_ADV)) + /* Make sure we're scanning for provisioning inviations */ + bt_mesh_scan_enable(); + /* Enable unprovisioned beacon sending */ + + bt_mesh_beacon_enable(); +#endif + +#if (MYNEWT_VAL(BLE_MESH_PB_GATT)) + bt_mesh_proxy_prov_enable(); +#endif + + ble_gap_mesh_cb_register(bt_mesh_gap_event, NULL); + + return 0; +} diff --git a/nimble/host/mesh/src/mesh_priv.h b/nimble/host/mesh/src/mesh_priv.h new file mode 100644 index 000000000..697b406a6 --- /dev/null +++ b/nimble/host/mesh/src/mesh_priv.h @@ -0,0 +1,21 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ +#ifndef __MESH_PRIV_H +#define __MESH_PRIV_H + +#define BT_MESH_KEY_PRIMARY 0x0000 +#define BT_MESH_KEY_ANY 0xffff + +#define BT_MESH_ADDR_IS_UNICAST(addr) ((addr) && (addr) < 0x8000) +#define BT_MESH_ADDR_IS_GROUP(addr) ((addr) >= 0xc000 && (addr) <= 0xff00) +#define BT_MESH_ADDR_IS_VIRTUAL(addr) ((addr) >= 0x8000 && (addr) < 0xc000) + +bool +bt_mesh_is_provisioned(void); + +#endif diff --git a/nimble/host/mesh/src/net.c b/nimble/host/mesh/src/net.c new file mode 100644 index 000000000..e6abc7e26 --- /dev/null +++ b/nimble/host/mesh/src/net.c @@ -0,0 +1,1204 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include + +#include "os/os_mbuf.h" +#include "mesh/mesh.h" + +#define BT_DBG_ENABLED MYNEWT_VAL(BLE_MESH_MSG_DEBUG_NET) +#include "host/ble_hs_log.h" + +#include "crypto.h" +#include "adv.h" +#include "mesh_priv.h" +#include "net.h" +#include "lpn.h" +#include "friend.h" +#include "proxy.h" +#include "transport.h" +#include "access.h" +#include "foundation.h" +#include "beacon.h" + +/* Seq limit after IV Update is triggered */ +#define IV_UPDATE_SEQ_LIMIT 8000000 + +#if MYNEWT_VAL(BLE_MESH_IV_UPDATE_TEST) +/* Small test timeout for IV Update Procedure testing */ +#define IV_UPDATE_TIMEOUT K_SECONDS(120) +#else +/* Maximum time to stay in IV Update mode (96 < time < 144) */ +#define IV_UPDATE_TIMEOUT K_HOURS(120) +#endif /* MYNEWT_VAL(BLE_MESH_IV_UPDATE_TEST) */ + +#define IVI(pdu) ((pdu)[0] >> 7) +#define NID(pdu) ((pdu)[0] & 0x7f) +#define CTL(pdu) ((pdu)[1] >> 7) +#define TTL(pdu) ((pdu)[1] & 0x7f) + +/* Determine how many friendship credentials we need */ +#if (MYNEWT_VAL(BLE_MESH_FRIEND)) +#define FRIEND_CRED_COUNT MYNEWT_VAL(BLE_MESH_FRIEND_LPN_COUNT) +#elif (MYNEWT_VAL(BLE_MESH_LOW_POWER)) +#define FRIEND_CRED_COUNT MYNEWT_VAL(BLE_MESH_SUBNET_COUNT) +#else +#define FRIEND_CRED_COUNT 0 +#endif + +#if FRIEND_CRED_COUNT > 0 +static struct bt_mesh_friend_cred friend_cred[FRIEND_CRED_COUNT]; +#endif + +static u64_t msg_cache[MYNEWT_VAL(BLE_MESH_MSG_CACHE_SIZE)]; +static u16_t msg_cache_next; + +/* Singleton network context (the implementation only supports one) */ +struct bt_mesh_net bt_mesh = { + .sub = { + [0 ... (MYNEWT_VAL(BLE_MESH_SUBNET_COUNT) - 1)] = { + .net_idx = BT_MESH_KEY_UNUSED, + } + }, + .app_keys = { + [0 ... (MYNEWT_VAL(BLE_MESH_APP_KEY_COUNT) - 1)] = { + .net_idx = BT_MESH_KEY_UNUSED, + } + }, +}; + +static u32_t dup_cache[4]; +static int dup_cache_next; + +static bool check_dup(struct os_mbuf *data) +{ + const u8_t *tail = net_buf_simple_tail(data); + u32_t val; + int i; + + val = sys_get_be32(tail - 4) ^ sys_get_be32(tail - 8); + + for (i = 0; i < ARRAY_SIZE(dup_cache); i++) { + if (dup_cache[i] == val) { + return true; + } + } + + dup_cache[dup_cache_next++] = val; + dup_cache_next %= ARRAY_SIZE(dup_cache); + + return false; +} + +static u64_t msg_hash(struct os_mbuf *pdu) +{ + u8_t *tpdu_last; + u64_t hash; + + /* Last byte of TransportPDU */ + tpdu_last = net_buf_simple_tail(pdu) - (CTL(pdu->om_data) ? 8 : 4) - 1; + + ((u8_t *)(&hash))[0] = pdu->om_data[0]; + ((u8_t *)(&hash))[1] = (pdu->om_data[1] & 0xc0); + ((u8_t *)(&hash))[2] = *tpdu_last; + memcpy(&((u8_t *)&hash)[3], &pdu->om_data[2], 5); + + return hash; +} + +static void msg_cache_add(u64_t new_hash) +{ + msg_cache[msg_cache_next++] = new_hash; + msg_cache_next %= ARRAY_SIZE(msg_cache); +} + +static bool msg_is_known(u64_t hash) +{ + u16_t i; + + for (i = 0; i < ARRAY_SIZE(msg_cache); i++) { + if (msg_cache[i] == hash) { + return true; + } + } + + return false; +} + +static inline u32_t net_seq(struct os_mbuf *buf) +{ + return ((net_buf_simple_pull_u8(buf) << 16) & 0xff0000) | + ((net_buf_simple_pull_u8(buf) << 8) & 0xff00) | + net_buf_simple_pull_u8(buf); +} + +struct bt_mesh_subnet *bt_mesh_subnet_get(u16_t net_idx) +{ + int i; + + if (net_idx == BT_MESH_KEY_ANY) { + return &bt_mesh.sub[0]; + } + + for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) { + if (bt_mesh.sub[i].net_idx == net_idx) { + return &bt_mesh.sub[i]; + } + } + + return NULL; +} + +int bt_mesh_net_keys_create(struct bt_mesh_subnet_keys *keys, + const u8_t key[16]) +{ + u8_t p[] = { 0 }; + u8_t nid; + int err; + + err = bt_mesh_k2(key, p, sizeof(p), &nid, keys->enc, keys->privacy); + if (err) { + BT_ERR("Unable to generate NID, EncKey & PrivacyKey"); + return err; + } + + memcpy(keys->net, key, 16); + + keys->nid = nid; + + BT_DBG("NID 0x%02x EncKey %s", keys->nid, bt_hex(keys->enc, 16)); + BT_DBG("PrivacyKey %s", bt_hex(keys->privacy, 16)); + + err = bt_mesh_k3(key, keys->net_id); + if (err) { + BT_ERR("Unable to generate Net ID"); + return err; + } + + BT_DBG("NetID %s", bt_hex(keys->net_id, 8)); + +#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY)) + err = bt_mesh_identity_key(key, keys->identity); + if (err) { + BT_ERR("Unable to generate IdentityKey"); + return err; + } + + BT_DBG("IdentityKey %s", bt_hex(keys->identity, 16)); +#endif /* GATT_PROXY */ + + err = bt_mesh_beacon_key(key, keys->beacon); + if (err) { + BT_ERR("Unable to generate beacon key"); + return err; + } + + BT_DBG("BeaconKey %s", bt_hex(keys->beacon, 16)); + + return 0; +} + +#if ((MYNEWT_VAL(BLE_MESH_LOW_POWER)) || \ + (MYNEWT_VAL(BLE_MESH_FRIEND))) +int bt_mesh_friend_cred_set(struct bt_mesh_friend_cred *cred, u8_t idx, + const u8_t net_key[16]) +{ + u16_t lpn_addr, frnd_addr; + int err; + u8_t p[9]; + +#if (MYNEWT_VAL(BLE_MESH_LOW_POWER)) + if (cred->addr == bt_mesh.lpn.frnd) { + lpn_addr = bt_mesh_primary_addr(); + frnd_addr = cred->addr; + } else { + lpn_addr = cred->addr; + frnd_addr = bt_mesh_primary_addr(); + } +#else + lpn_addr = cred->addr; + frnd_addr = bt_mesh_primary_addr(); +#endif + + BT_DBG("LPNAddress 0x%04x FriendAddress 0x%04x", lpn_addr, frnd_addr); + BT_DBG("LPNCounter 0x%04x FriendCounter 0x%04x", cred->lpn_counter, + cred->frnd_counter); + + p[0] = 0x01; + sys_put_be16(lpn_addr, p + 1); + sys_put_be16(frnd_addr, p + 3); + sys_put_be16(cred->lpn_counter, p + 5); + sys_put_be16(cred->frnd_counter, p + 7); + + err = bt_mesh_k2(net_key, p, sizeof(p), &cred->cred[idx].nid, + cred->cred[idx].enc, cred->cred[idx].privacy); + if (err) { + BT_ERR("Unable to generate NID, EncKey & PrivacyKey"); + return err; + } + + BT_DBG("Friend NID 0x%02x EncKey %s", cred->cred[idx].nid, + bt_hex(cred->cred[idx].enc, 16)); + BT_DBG("Friend PrivacyKey %s", bt_hex(cred->cred[idx].privacy, 16)); + + return 0; +} + +void bt_mesh_friend_cred_refresh(u16_t net_idx) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(friend_cred); i++) { + struct bt_mesh_friend_cred *cred = &friend_cred[i]; + + if (cred->addr != BT_MESH_ADDR_UNASSIGNED && + cred->net_idx == net_idx) { + memcpy(&cred->cred[0], &cred->cred[1], + sizeof(cred->cred[0])); + } + } +} + +int bt_mesh_friend_cred_update(u16_t net_idx, u8_t idx, const u8_t net_key[16]) +{ + int err, i; + + for (i = 0; i < ARRAY_SIZE(friend_cred); i++) { + struct bt_mesh_friend_cred *cred = &friend_cred[i]; + + if (cred->addr == BT_MESH_ADDR_UNASSIGNED || + cred->net_idx != net_idx) { + continue; + } + + err = bt_mesh_friend_cred_set(cred, idx, net_key); + if (err) { + return err; + } + } + + return 0; +} + +struct bt_mesh_friend_cred *bt_mesh_friend_cred_add(u16_t net_idx, + const u8_t net_key[16], + u8_t idx, u16_t addr, + u16_t lpn_counter, + u16_t frnd_counter) +{ + struct bt_mesh_friend_cred *cred; + int i, err; + + BT_DBG("net_idx 0x%04x addr 0x%04x idx %u", net_idx, addr, idx); + + for (cred = NULL, i = 0; i < ARRAY_SIZE(friend_cred); i++) { + if ((friend_cred[i].addr == BT_MESH_ADDR_UNASSIGNED) || + (friend_cred[i].addr == addr && + friend_cred[i].net_idx == net_idx)) { + cred = &friend_cred[i]; + break; + } + } + + if (!cred) { + return NULL; + } + + cred->net_idx = net_idx; + cred->addr = addr; + cred->lpn_counter = lpn_counter; + cred->frnd_counter = frnd_counter; + + err = bt_mesh_friend_cred_set(cred, idx, net_key); + if (err) { + bt_mesh_friend_cred_clear(cred); + return NULL; + } + + return cred; +} + +void bt_mesh_friend_cred_clear(struct bt_mesh_friend_cred *cred) +{ + cred->net_idx = BT_MESH_KEY_UNUSED; + cred->addr = BT_MESH_ADDR_UNASSIGNED; + cred->lpn_counter = 0; + cred->frnd_counter = 0; + memset(cred->cred, 0, sizeof(cred->cred)); +} + +int bt_mesh_friend_cred_del(u16_t net_idx, u16_t addr) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(friend_cred); i++) { + struct bt_mesh_friend_cred *cred = &friend_cred[i]; + + if (cred->addr == addr && cred->net_idx == net_idx) { + bt_mesh_friend_cred_clear(cred); + return 0; + } + } + + return -ENOENT; +} + +static int friend_cred_get(u16_t net_idx, u16_t addr, u8_t idx, + u8_t *nid, const u8_t **enc, const u8_t **priv) +{ + int i; + + BT_DBG("net_idx 0x%04x addr 0x%04x idx %u", net_idx, addr, idx); + + for (i = 0; i < ARRAY_SIZE(friend_cred); i++) { + struct bt_mesh_friend_cred *cred = &friend_cred[i]; + + if (cred->net_idx != net_idx) { + continue; + } + + if (addr != BT_MESH_ADDR_UNASSIGNED && cred->addr != addr) { + continue; + } + + if (nid) { + *nid = cred->cred[idx].nid; + } + + if (enc) { + *enc = cred->cred[idx].enc; + } + + if (priv) { + *priv = cred->cred[idx].privacy; + } + + return 0; + } + + return -ENOENT; +} +#else +static inline int friend_cred_get(u16_t net_idx, u16_t addr, u8_t idx, + u8_t *nid, const u8_t **enc, + const u8_t **priv) +{ + return -ENOENT; +} +#endif /* FRIEND || LOW_POWER */ + +int bt_mesh_net_beacon_update(struct bt_mesh_subnet *sub) +{ + struct bt_mesh_subnet_keys *keys; + u8_t flags; + + if (sub->kr_flag) { + BT_DBG("NetIndex %u Using new key", sub->net_idx); + flags = BT_MESH_NET_FLAG_KR; + keys = &sub->keys[1]; + } else { + BT_DBG("NetIndex %u Using current key", sub->net_idx); + flags = 0x00; + keys = &sub->keys[0]; + } + + if (bt_mesh.iv_update) { + flags |= BT_MESH_NET_FLAG_IVU; + } + + BT_DBG("flags 0x%02x, IVI 0x%08x", flags, bt_mesh.iv_index); + + return bt_mesh_beacon_auth(keys->beacon, flags, keys->net_id, + bt_mesh.iv_index, sub->auth); +} + +int bt_mesh_net_create(u16_t idx, u8_t flags, const u8_t key[16], + u32_t iv_index) +{ + struct bt_mesh_subnet *sub; + int err; + + BT_DBG("idx %u iv_index %u", idx, iv_index); + + BT_DBG("NetKey %s", bt_hex(key, 16)); + + if (bt_mesh.valid) { + return -EALREADY; + } + + sub = &bt_mesh.sub[0]; + + err = bt_mesh_net_keys_create(&sub->keys[0], key); + if (err) { + return -EIO; + } + + bt_mesh.valid = 1; + sub->net_idx = idx; + + if ((MYNEWT_VAL(BLE_MESH_GATT_PROXY))) { + sub->node_id = BT_MESH_NODE_IDENTITY_RUNNING; + } else { + sub->node_id = BT_MESH_NODE_IDENTITY_NOT_SUPPORTED; + } + + sub->kr_flag = BT_MESH_KEY_REFRESH(flags); + if (sub->kr_flag) { + memcpy(&sub->keys[1], &sub->keys[0], sizeof(sub->keys[0])); + sub->kr_phase = BT_MESH_KR_PHASE_2; + } + + bt_mesh.iv_index = iv_index; + bt_mesh.iv_update = BT_MESH_IV_UPDATE(flags); + + /* Set initial IV Update procedure state time-stamp */ + bt_mesh.last_update = k_uptime_get(); + + return 0; +} + +bool bt_mesh_kr_update(struct bt_mesh_subnet *sub, u8_t new_kr, bool new_key) +{ + if (new_kr != sub->kr_flag && sub->kr_phase == BT_MESH_KR_NORMAL) { + BT_WARN("KR change in normal operation. Are we blacklisted?"); + return false; + } + + sub->kr_flag = new_kr; + + if (sub->kr_flag) { + if (sub->kr_phase == BT_MESH_KR_PHASE_1) { + BT_DBG("Phase 1 -> Phase 2"); + sub->kr_phase = BT_MESH_KR_PHASE_2; + return true; + } + } else { + switch (sub->kr_phase) { + case BT_MESH_KR_PHASE_1: + if (!new_key) { + /* Ignore */ + break; + } + /* Upon receiving a Secure Network beacon with the KR flag set + * to 0 using the new NetKey in Phase 1, the node shall + * immediately transition to Phase 3, which effectively skips + * Phase 2. + * + * Intentional fall-through. + */ + case BT_MESH_KR_PHASE_2: + BT_DBG("KR Phase 0x%02x -> Normal", sub->kr_phase); + memcpy(&sub->keys[0], &sub->keys[1], + sizeof(sub->keys[0])); + if ((MYNEWT_VAL(BLE_MESH_LOW_POWER)) || + (MYNEWT_VAL(BLE_MESH_FRIEND))) { + bt_mesh_friend_cred_refresh(sub->net_idx); + } + sub->kr_phase = BT_MESH_KR_NORMAL; + return true; + } + } + + return false; +} + +void bt_mesh_rpl_reset(void) +{ + int i; + + /* Discard "old old" IV Index entries from RPL and flag + * any other ones (which are valid) as old. + */ + for (i = 0; i < ARRAY_SIZE(bt_mesh.rpl); i++) { + struct bt_mesh_rpl *rpl = &bt_mesh.rpl[i]; + + if (rpl->src) { + if (rpl->old_iv) { + memset(rpl, 0, sizeof(*rpl)); + } else { + rpl->old_iv = true; + } + } + } +} + +void bt_mesh_iv_update(u32_t iv_index, bool iv_update) +{ + int i; + + if (iv_index < bt_mesh.iv_index || iv_index > bt_mesh.iv_index + 42) { + BT_ERR("IV Index completely out of sync: 0x%08x != 0x%08x", + iv_index, bt_mesh.iv_index); + return; + } + + if (iv_index > bt_mesh.iv_index + 1) { + BT_WARN("Performing IV Index Recovery"); + memset(bt_mesh.rpl, 0, sizeof(bt_mesh.rpl)); + bt_mesh.iv_index = iv_index; + bt_mesh.seq = 0; + goto do_update; + } + + if (iv_update == bt_mesh.iv_update) { + if (iv_index != bt_mesh.iv_index) { + BT_WARN("No update, but IV Index 0x%08x != 0x%08x", + iv_index, bt_mesh.iv_index); + } + return; + } + + if (bt_mesh.iv_update) { + if (iv_index != bt_mesh.iv_index) { + BT_WARN("IV Index mismatch: 0x%08x != 0x%08x", + iv_index, bt_mesh.iv_index); + return; + } + } else { + if (iv_index != bt_mesh.iv_index + 1) { + BT_WARN("Wrong new IV Index: 0x%08x != 0x%08x + 1", + iv_index, bt_mesh.iv_index); + return; + } + } + + if (!(MYNEWT_VAL(BLE_MESH_IV_UPDATE_TEST))) { + s64_t delta = k_uptime_get() - bt_mesh.last_update; + + if (delta < K_HOURS(96)) { + BT_WARN("IV Update before minimum duration"); + return; + } + } + + /* Defer change to Normal Operation if there are pending acks */ + if (!iv_update && bt_mesh_tx_in_progress()) { + BT_WARN("IV Update deferred because of pending transfer"); + bt_mesh.pending_update = 1; + return; + } + +do_update: + bt_mesh.iv_update = iv_update; + + if (bt_mesh.iv_update) { + bt_mesh.iv_index = iv_index; + BT_DBG("IV Update state entered. New index 0x%08x", + bt_mesh.iv_index); + + bt_mesh_rpl_reset(); + + k_delayed_work_submit(&bt_mesh.ivu_complete, + IV_UPDATE_TIMEOUT); + } else { + BT_DBG("Normal mode entered"); + bt_mesh.seq = 0; + k_delayed_work_cancel(&bt_mesh.ivu_complete); + } + + /* Store time-stamp of the IV procedure state change */ + bt_mesh.last_update = k_uptime_get(); + + for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) { + if (bt_mesh.sub[i].net_idx != BT_MESH_KEY_UNUSED) { + bt_mesh_net_beacon_update(&bt_mesh.sub[i]); + } + } +} + +int bt_mesh_net_resend(struct bt_mesh_subnet *sub, struct os_mbuf *buf, + bool new_key, bool friend_cred, bt_mesh_adv_func_t cb) +{ + const u8_t *enc, *priv; + int err; + + BT_DBG("net_idx 0x%04x, len %u", sub->net_idx, buf->om_len); + + if (friend_cred) { + err = friend_cred_get(sub->net_idx, BT_MESH_ADDR_UNASSIGNED, + new_key, NULL, &enc, &priv); + if (err) { + return err; + } + } else { + enc = sub->keys[new_key].enc; + priv = sub->keys[new_key].privacy; + } + + err = bt_mesh_net_obfuscate(buf->om_data, BT_MESH_NET_IVI_TX, priv); + if (err) { + BT_ERR("deobfuscate failed (err %d)", err); + return err; + } + + err = bt_mesh_net_decrypt(enc, buf, BT_MESH_NET_IVI_TX, false); + if (err) { + BT_ERR("decrypt failed (err %d)", err); + return err; + } + + /* Update with a new sequence number */ + buf->om_data[2] = (bt_mesh.seq >> 16); + buf->om_data[3] = (bt_mesh.seq >> 8); + buf->om_data[4] = bt_mesh.seq++; + + err = bt_mesh_net_encrypt(enc, buf, BT_MESH_NET_IVI_TX, false); + if (err) { + BT_ERR("encrypt failed (err %d)", err); + return err; + } + + err = bt_mesh_net_obfuscate(buf->om_data, BT_MESH_NET_IVI_TX, priv); + if (err) { + BT_ERR("obfuscate failed (err %d)", err); + return err; + } + + bt_mesh_adv_send(buf, cb); + + if (!bt_mesh.iv_update && bt_mesh.seq > IV_UPDATE_SEQ_LIMIT) { + bt_mesh_beacon_ivu_initiator(true); + bt_mesh_iv_update(bt_mesh.iv_index + 1, true); + } + + return 0; +} + +#if (MYNEWT_VAL(BLE_MESH_LOCAL_INTERFACE)) +static void bt_mesh_net_local(struct os_event *work) +{ + struct os_mbuf *buf; + + while ((buf = net_buf_get(&bt_mesh.local_queue, K_NO_WAIT))) { + BT_DBG("len %u: %s", buf->om_len, bt_hex(buf->om_data, buf->om_len)); + bt_mesh_net_recv(buf, 0, BT_MESH_NET_IF_LOCAL); + net_buf_unref(buf); + } +} +#endif + +int bt_mesh_net_encode(struct bt_mesh_net_tx *tx, struct os_mbuf *buf, + bool proxy) +{ + const bool ctl = (tx->ctx->app_idx == BT_MESH_KEY_UNUSED); + u8_t nid; + const u8_t *enc, *priv; + u8_t *seq; + int err; + + if (ctl && net_buf_simple_tailroom(buf) < 8) { + BT_ERR("Insufficient MIC space for CTL PDU"); + return -EINVAL; + } else if (net_buf_simple_tailroom(buf) < 4) { + BT_ERR("Insufficient MIC space for PDU"); + return -EINVAL; + } + + BT_DBG("src 0x%04x dst 0x%04x ctl %u seq 0x%06x", + tx->src, tx->ctx->addr, ctl, bt_mesh.seq); + + net_buf_simple_push_be16(buf, tx->ctx->addr); + net_buf_simple_push_be16(buf, tx->src); + + seq = net_buf_simple_push(buf, 3); + seq[0] = (bt_mesh.seq >> 16); + seq[1] = (bt_mesh.seq >> 8); + seq[2] = bt_mesh.seq++; + + if (ctl) { + net_buf_simple_push_u8(buf, tx->ctx->send_ttl | 0x80); + } else { + net_buf_simple_push_u8(buf, tx->ctx->send_ttl); + } + + if (tx->sub->kr_phase == BT_MESH_KR_PHASE_2) { + if (tx->ctx->friend_cred) { + err = friend_cred_get(tx->sub->net_idx, + BT_MESH_ADDR_UNASSIGNED, + 1, &nid, &enc, &priv); + if (err) { + return err; + } + } else { + nid = tx->sub->keys[1].nid; + enc = tx->sub->keys[1].enc; + priv = tx->sub->keys[1].privacy; + } + } else { + if (tx->ctx->friend_cred) { + err = friend_cred_get(tx->sub->net_idx, + BT_MESH_ADDR_UNASSIGNED, + 0, &nid, &enc, &priv); + if (err) { + return err; + } + } else { + nid = tx->sub->keys[0].nid; + enc = tx->sub->keys[0].enc; + priv = tx->sub->keys[0].privacy; + } + } + + net_buf_simple_push_u8(buf, (nid | (BT_MESH_NET_IVI_TX & 1) << 7)); + + err = bt_mesh_net_encrypt(enc, buf, BT_MESH_NET_IVI_TX, proxy); + if (err) { + return err; + } + + return bt_mesh_net_obfuscate(buf->om_data, BT_MESH_NET_IVI_TX, priv); +} + +int bt_mesh_net_send(struct bt_mesh_net_tx *tx, struct os_mbuf *buf, + bt_mesh_adv_func_t cb) +{ + int err; + + BT_DBG("src 0x%04x dst 0x%04x len %u headroom %zu tailroom %zu", + tx->src, tx->ctx->addr, buf->om_len, net_buf_headroom(buf), + net_buf_tailroom(buf)); + BT_DBG("Payload len %u: %s", buf->om_len, bt_hex(buf->om_data, buf->om_len)); + BT_DBG("Seq 0x%06x", bt_mesh.seq); + +#if (MYNEWT_VAL(BLE_MESH_LOW_POWER)) + /* Communication between LPN & Friend should always be using + * the Friendship Credentials. Any other destination should + * use the Master Credentials. + */ + if (bt_mesh_lpn_established()) { + tx->ctx->friend_cred = (tx->ctx->addr == bt_mesh.lpn.frnd); + } +#endif + + if (tx->ctx->send_ttl == BT_MESH_TTL_DEFAULT) { + tx->ctx->send_ttl = bt_mesh_default_ttl_get(); + } + + err = bt_mesh_net_encode(tx, buf, false); + if (err) { + goto done; + } + + /* Deliver to GATT Proxy Clients if necessary */ + if ((MYNEWT_VAL(BLE_MESH_GATT_PROXY))) { + if (bt_mesh_proxy_relay(buf, tx->ctx->addr) && + BT_MESH_ADDR_IS_UNICAST(tx->ctx->addr)) { + err = 0; + goto done; + } + } + +#if (MYNEWT_VAL(BLE_MESH_LOCAL_INTERFACE)) + /* Deliver to local network interface if necessary */ + if (bt_mesh_fixed_group_match(tx->ctx->addr) || + bt_mesh_elem_find(tx->ctx->addr)) { + net_buf_put(&bt_mesh.local_queue, net_buf_ref(buf)); + if (cb) { + cb(buf, 0); + } + k_work_submit(&bt_mesh.local_work); + } else { + bt_mesh_adv_send(buf, cb); + } +#else + bt_mesh_adv_send(buf, cb); +#endif + +done: + net_buf_unref(buf); + return err; +} + +static bool auth_match(struct bt_mesh_subnet_keys *keys, + const u8_t net_id[8], u8_t flags, + u32_t iv_index, const u8_t auth[8]) +{ + u8_t net_auth[8]; + + if (memcmp(net_id, keys->net_id, 8)) { + return false; + } + + bt_mesh_beacon_auth(keys->beacon, flags, keys->net_id, iv_index, + net_auth); + + if (memcmp(auth, net_auth, 8)) { + BT_WARN("Authentication Value %s != %s", + bt_hex(auth, 8), bt_hex(net_auth, 8)); + return false; + } + + return true; +} + +struct bt_mesh_subnet *bt_mesh_subnet_find(const u8_t net_id[8], u8_t flags, + u32_t iv_index, const u8_t auth[8], + bool *new_key) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) { + struct bt_mesh_subnet *sub = &bt_mesh.sub[i]; + + if (sub->net_idx == BT_MESH_KEY_UNUSED) { + continue; + } + + if (auth_match(&sub->keys[0], net_id, flags, iv_index, auth)) { + *new_key = false; + return sub; + } + + if (sub->kr_phase == BT_MESH_KR_NORMAL) { + continue; + } + + if (auth_match(&sub->keys[1], net_id, flags, iv_index, auth)) { + *new_key = true; + return sub; + } + } + + return NULL; +} + +static int net_decrypt(struct bt_mesh_subnet *sub, u8_t idx, const u8_t *data, + size_t data_len, struct bt_mesh_net_rx *rx, + struct os_mbuf *buf) +{ + const u8_t *enc, *priv; + + BT_DBG("NID 0x%02x, PDU NID 0x%02x net_idx 0x%04x idx %u", + sub->keys[idx].nid, NID(data), sub->net_idx, idx); + + if (NID(data) == sub->keys[idx].nid) { + rx->ctx.friend_cred = false; + enc = sub->keys[idx].enc; + priv = sub->keys[idx].privacy; + rx->ctx.friend_cred = 0; + } else { + u8_t nid; + + if (friend_cred_get(sub->net_idx, BT_MESH_ADDR_UNASSIGNED, idx, + &nid, &enc, &priv)) { + return -ENOENT; + } + + if (nid != NID(data)) { + return -ENOENT; + } + + rx->ctx.friend_cred = 1; + } + + BT_DBG("IVI %u net->iv_index 0x%08x", IVI(data), bt_mesh.iv_index); + + rx->old_iv = (IVI(data) != (bt_mesh.iv_index & 0x01)); + + net_buf_simple_init(buf, 0); + memcpy(net_buf_simple_add(buf, data_len), data, data_len); + + if (bt_mesh_net_obfuscate(buf->om_data, BT_MESH_NET_IVI_RX(rx), priv)) { + return -ENOENT; + } + + if (msg_is_known(rx->hash)) { + return -EALREADY; + } + + rx->ctx.addr = sys_get_be16(&buf->om_data[5]); + if (!BT_MESH_ADDR_IS_UNICAST(rx->ctx.addr)) { + BT_WARN("Ignoring non-unicast src addr 0x%04x", rx->ctx.addr); + return -EINVAL; + } + + BT_DBG("src 0x%04x", rx->ctx.addr); + + if ((MYNEWT_VAL(BLE_MESH_PROXY)) && + rx->net_if == BT_MESH_NET_IF_PROXY_CFG) { + return bt_mesh_net_decrypt(enc, buf, BT_MESH_NET_IVI_RX(rx), + true); + } + + return bt_mesh_net_decrypt(enc, buf, BT_MESH_NET_IVI_RX(rx), false); +} + +static int net_find_and_decrypt(const u8_t *data, size_t data_len, + struct bt_mesh_net_rx *rx, + struct os_mbuf *buf) +{ + int i; + + BT_DBG(""); + + for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) { + struct bt_mesh_subnet *sub = &bt_mesh.sub[i]; + int err; + + if (sub->net_idx == BT_MESH_KEY_UNUSED) { + continue; + } + + err = net_decrypt(sub, 0, data, data_len, rx, buf); + if (!err) { + rx->ctx.net_idx = sub->net_idx; + rx->sub = sub; + return true; + } + + if (sub->kr_phase == BT_MESH_KR_NORMAL) { + continue; + } + + err = net_decrypt(sub, 1, data, data_len, rx, buf); + if (!err) { + rx->ctx.net_idx = sub->net_idx; + rx->sub = sub; + rx->new_key = 1; + return true; + } + } + + return false; +} + +#if ((MYNEWT_VAL(BLE_MESH_RELAY)) || \ + (MYNEWT_VAL(BLE_MESH_FRIEND)) || \ + (MYNEWT_VAL(BLE_MESH_GATT_PROXY))) +static void bt_mesh_net_relay(struct os_mbuf *sbuf, + struct bt_mesh_net_rx *rx) +{ + const u8_t *enc, *priv; + struct os_mbuf *buf; + u8_t nid, transmit; + + BT_DBG("TTL %u CTL %u dst 0x%04x", rx->ctx.recv_ttl, CTL(sbuf->om_data), + rx->dst); + + if (rx->ctx.recv_ttl <= 1) { + return; + } + + transmit = bt_mesh_relay_retransmit_get(); + buf = bt_mesh_adv_create(BT_MESH_ADV_DATA, TRANSMIT_COUNT(transmit), + TRANSMIT_INT(transmit), K_NO_WAIT); + if (!buf) { + BT_ERR("Out of relay buffers"); + return; + } + + net_buf_add_mem(buf, sbuf->om_data, sbuf->om_len); + + /* Only decrement TTL for non-locally originated packets */ + if (rx->net_if != BT_MESH_NET_IF_LOCAL) { + /* Leave CTL bit intact */ + buf->om_data[1] &= 0x80; + buf->om_data[1] |= rx->ctx.recv_ttl - 1; + } + + if (rx->sub->kr_phase == BT_MESH_KR_PHASE_2) { + if (bt_mesh_friend_dst_is_lpn(rx->dst)) { + if (friend_cred_get(rx->sub->net_idx, + BT_MESH_ADDR_UNASSIGNED, + 1, &nid, &enc, &priv)) { + BT_ERR("friend_cred_get failed"); + goto done; + } + } else { + enc = rx->sub->keys[1].enc; + priv = rx->sub->keys[1].privacy; + nid = rx->sub->keys[1].nid; + } + } else { + if (bt_mesh_friend_dst_is_lpn(rx->dst)) { + if (friend_cred_get(rx->sub->net_idx, + BT_MESH_ADDR_UNASSIGNED, + 0, &nid, &enc, &priv)) { + BT_ERR("friend_cred_get failed"); + goto done; + } + } else { + enc = rx->sub->keys[0].enc; + priv = rx->sub->keys[0].privacy; + nid = rx->sub->keys[0].nid; + } + } + + BT_DBG("Relaying packet. TTL is now %u", TTL(buf->om_data)); + + /* Update NID if RX or TX is with friend credentials */ + if (rx->ctx.friend_cred || bt_mesh_friend_dst_is_lpn(rx->dst)) { + buf->om_data[0] &= 0x80; /* Clear everything except IVI */ + buf->om_data[0] |= nid; + } + + /* We re-encrypt and obfuscate using the received IVI rather than + * the normal TX IVI (which may be different) since the transport + * layer nonce includes the IVI. + */ + if (bt_mesh_net_encrypt(enc, buf, BT_MESH_NET_IVI_RX(rx), false)) { + BT_ERR("Re-encrypting failed"); + goto done; + } + + if (bt_mesh_net_obfuscate(buf->om_data, BT_MESH_NET_IVI_RX(rx), priv)) { + BT_ERR("Re-obfuscating failed"); + goto done; + } + + if ((MYNEWT_VAL(BLE_MESH_FRIEND))) { + if (bt_mesh_friend_enqueue(buf, rx->dst) && + BT_MESH_ADDR_IS_UNICAST(rx->dst)) { + goto done; + } + } + + if ((MYNEWT_VAL(BLE_MESH_GATT_PROXY))) { + if (bt_mesh_proxy_relay(buf, rx->dst) && + BT_MESH_ADDR_IS_UNICAST(rx->dst)) { + goto done; + } + } + + if (rx->net_if != BT_MESH_NET_IF_ADV || + bt_mesh_relay_get() == BT_MESH_RELAY_ENABLED) { + bt_mesh_adv_send(buf, NULL); + } + +done: + net_buf_unref(buf); +} +#endif /* RELAY || FRIEND || GATT_PROXY */ + +int bt_mesh_net_decode(struct os_mbuf *data, enum bt_mesh_net_if net_if, + struct bt_mesh_net_rx *rx, struct os_mbuf *buf, + struct net_buf_simple_state *state) +{ + if (data->om_len < 18) { + BT_WARN("Dropping too short mesh packet (len %u)", data->om_len); + BT_WARN("%s", bt_hex(data->om_data, data->om_len)); + return -EINVAL; + } + + if (net_if == BT_MESH_NET_IF_ADV && check_dup(data)) { + return -EINVAL; + } + + BT_DBG("%u bytes: %s", data->om_len, bt_hex(data->om_data, data->om_len)); + + rx->net_if = net_if; + + if (net_if == BT_MESH_NET_IF_ADV) { + rx->hash = msg_hash(data); + } + + if (!net_find_and_decrypt(data->om_data, data->om_len, rx, buf)) { + BT_DBG("Unable to find matching net for packet"); + return -ENOENT; + } + + /* Initialize AppIdx to a sane value */ + rx->ctx.app_idx = BT_MESH_KEY_UNUSED; + + /* Save parsing state so the buffer can later be relayed */ + if (state) { + net_buf_simple_save(buf, state); + } + + rx->ctx.recv_ttl = TTL(buf->om_data); + + /* Default to responding with TTL 0 for non-routed messages */ + if (rx->ctx.recv_ttl == 0) { + rx->ctx.send_ttl = 0; + } else { + rx->ctx.send_ttl = BT_MESH_TTL_DEFAULT; + } + + rx->ctl = CTL(buf->om_data); + net_buf_simple_pull(buf, 2); /* SRC, already parsed by net_decrypt() */ + rx->seq = net_seq(buf); + net_buf_simple_pull(buf, 2); + rx->dst = net_buf_simple_pull_be16(buf); + + BT_DBG("Decryption successful. Payload len %u", buf->om_len); + + if (net_if != BT_MESH_NET_IF_LOCAL && bt_mesh_elem_find(rx->ctx.addr)) { + BT_DBG("Dropping locally originated packet"); + return -EBADMSG; + } + + if (net_if == BT_MESH_NET_IF_ADV) { + msg_cache_add(rx->hash); + } + + BT_DBG("src 0x%04x dst 0x%04x ttl %u", rx->ctx.addr, rx->dst, + rx->ctx.recv_ttl); + BT_DBG("PDU: %s", bt_hex(buf->om_data, buf->om_len)); + + return 0; +} + +void bt_mesh_net_recv(struct os_mbuf *data, s8_t rssi, + enum bt_mesh_net_if net_if) +{ + struct os_mbuf *buf = NET_BUF_SIMPLE(29); + struct net_buf_simple_state state; + struct bt_mesh_net_rx rx; + + BT_DBG("rssi %d net_if %u", rssi, net_if); + + if (!bt_mesh_is_provisioned()) { + goto done; + } + + if (bt_mesh_net_decode(data, net_if, &rx, buf, &state)) { + goto done; + } + + if ((MYNEWT_VAL(BLE_MESH_GATT_PROXY)) && + net_if == BT_MESH_NET_IF_PROXY) { + bt_mesh_proxy_addr_add(data, rx.ctx.addr); + } + + if (bt_mesh_fixed_group_match(rx.dst) || bt_mesh_elem_find(rx.dst)) { + bt_mesh_trans_recv(buf, &rx); + + if (BT_MESH_ADDR_IS_UNICAST(rx.dst)) { + goto done; + } + } + +#if ((MYNEWT_VAL(BLE_MESH_RELAY)) || \ + (MYNEWT_VAL(BLE_MESH_FRIEND)) || \ + (MYNEWT_VAL(BLE_MESH_GATT_PROXY))) + net_buf_simple_restore(buf, &state); + bt_mesh_net_relay(buf, &rx); +#endif /* MYNEWT_VAL(BLE_MESH_RELAY) || FRIEND || GATT_PROXY */ + +done: + os_mbuf_free_chain(buf); +} + +static void ivu_complete(struct os_event *work) +{ + BT_DBG(""); + + bt_mesh_beacon_ivu_initiator(true); + bt_mesh_iv_update(bt_mesh.iv_index, false); +} + +void bt_mesh_net_init(void) +{ + k_delayed_work_init(&bt_mesh.ivu_complete, ivu_complete); + +#if (MYNEWT_VAL(BLE_MESH_LOCAL_INTERFACE)) + k_work_init(&bt_mesh.local_work, bt_mesh_net_local); +#endif +} diff --git a/nimble/host/mesh/src/net.h b/nimble/host/mesh/src/net.h new file mode 100644 index 000000000..b4e254b92 --- /dev/null +++ b/nimble/host/mesh/src/net.h @@ -0,0 +1,308 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef __NET_H__ +#define __NET_H__ + +#define BT_MESH_NET_FLAG_KR BIT(0) +#define BT_MESH_NET_FLAG_IVU BIT(1) + +#define BT_MESH_KR_NORMAL 0x00 +#define BT_MESH_KR_PHASE_1 0x01 +#define BT_MESH_KR_PHASE_2 0x02 +#define BT_MESH_KR_PHASE_3 0x03 + +#define BT_MESH_IV_UPDATE(flags) ((flags >> 1) & 0x01) +#define BT_MESH_KEY_REFRESH(flags) (flags & 0x01) + +#include +#include "mesh/mesh.h" + +struct bt_mesh_app_key +{ + u16_t net_idx; + u16_t app_idx;bool updated; + struct bt_mesh_app_keys + { + u8_t id; + u8_t val[16]; + } keys[2]; +}; + +/* Friendship Credentials */ +struct bt_mesh_friend_cred +{ + u16_t net_idx; + u16_t addr; + + u16_t lpn_counter; + u16_t frnd_counter; + + struct + { + u8_t nid; /* NID */ + u8_t enc[16]; /* EncKey */ + u8_t privacy[16]; /* PrivacyKey */ + } cred[2]; +}; + +struct bt_mesh_subnet +{ + s64_t beacon_sent; /* Time stamp of last sent beacon */ + u8_t beacons_last; /* Number of beacons during last + * observation window + */ + u8_t beacons_cur; /* Number of beaconds observed during + * currently ongoing window. + */ + + u16_t net_idx; /* NetKeyIndex */ + + bool kr_flag; /* Key Refresh Flag */ + u8_t kr_phase; /* Key Refresh Phase */ + + u8_t node_id; /* Node Identity State */ + + u8_t auth[8]; /* Beacon Authentication Value */ + + struct bt_mesh_subnet_keys + { + u8_t net[16]; /* NetKey */ + u8_t nid; /* NID */ + u8_t enc[16]; /* EncKey */ + u8_t net_id[8]; /* Network ID */ +#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY)) + u8_t identity[16]; /* IdentityKey */ +#endif + u8_t privacy[16]; /* PrivacyKey */ + u8_t beacon[16]; /* BeaconKey */ + } keys[2]; +}; + +struct bt_mesh_rpl +{ + u16_t src;bool old_iv; + u32_t seq; +}; + +struct bt_mesh_friend +{ + u16_t lpn; + u8_t recv_delay; + u8_t fsn :1, send_last :1, send_offer :1, send_update :1; + s32_t poll_to; + u16_t lpn_counter; + u16_t counter; + s8_t rssi; + + struct k_delayed_work timer; + + struct os_mbuf *last; + /* TU BYLO KFIFO*/ + struct os_eventq queue; +}; + +#if (MYNEWT_VAL(BLE_MESH_LOW_POWER)) +#define LPN_GROUPS MYNEWT_VAL(BLE_MESH_LOW_POWER) +#else +#define LPN_GROUPS 0 +#endif + +/* Low Power Node state */ +struct bt_mesh_lpn +{ + enum __packed + { + BT_MESH_LPN_DISABLED, /* LPN feature is disabled */ + BT_MESH_LPN_CLEAR, /* Clear in progress */ + BT_MESH_LPN_ENABLED, /* LPN enabled, but no Friend */ + BT_MESH_LPN_WAIT_OFFER, /* Friend Req sent */ + BT_MESH_LPN_ESTABLISHING, /* First Friend Poll sent */ + BT_MESH_LPN_ESTABLISHED, /* Friendship established */ + BT_MESH_LPN_RECV_DELAY, /* Poll sent, waiting ReceiveDelay */ + BT_MESH_LPN_WAIT_UPDATE, /* Waiting for Update or message */ + } state; + + /* Transaction Number (used for subscription list) */ + u8_t xact_next; + u8_t xact_pending; + u8_t sent_req; + + /* Address of our Friend when we're a LPN. Unassigned if we don't + * have a friend yet. + */ + u16_t frnd; + + /* Value from the friend offer */ + u8_t recv_win; + + u8_t req_attempts; /* Number of Request attempts */ + + s32_t poll_timeout; + + u8_t groups_changed :1, /* Friend Subscription List needs updating */ + pending_poll :1, /* Poll to be sent after subscription */ + disable :1, /* Disable LPN after clearing */ + fsn :1; /* Friend Sequence Number */ + + /* Friend Queue Size */ + u8_t queue_size; + + /* LPNCounter */ + u16_t counter; + + /* Next LPN related action timer */ + struct k_delayed_work timer; + + /* Subscribed groups */ + u16_t groups[LPN_GROUPS]; + + /* Bit fields for tracking which groups the Friend knows about */ + ATOMIC_DEFINE(added, LPN_GROUPS);ATOMIC_DEFINE(pending, LPN_GROUPS);ATOMIC_DEFINE(to_remove, LPN_GROUPS); +}; + +struct bt_mesh_net +{ + u32_t iv_index; /* Current IV Index */ + u32_t seq :24, /* Next outgoing sequence number */ + iv_update :1, /* 1 if IV Update in Progress */ + ivu_initiator :1, /* IV Update initiated by us */ + pending_update :1, /* Update blocked by SDU in progress */ + valid :1; /* 0 if unused */ + + s64_t last_update; /* Time since last IV Update change */ + +#if MYNEWT_VAL(BLE_MESH_LOCAL_INTERFACE) + /* Local network interface */ + struct os_callout local_work; + struct os_eventq local_queue; + struct os_event local_ev; +#endif + +#if MYNEWT_VAL(BLE_MESH_FRIEND) + struct bt_mesh_friend frnd; /* Friend state */ +#endif + +#if (MYNEWT_VAL(BLE_MESH_LOW_POWER)) + struct bt_mesh_lpn lpn; /* Low Power Node state */ +#endif + + /* Timer to transition IV Update in Progress state */ + struct k_delayed_work ivu_complete; + + u8_t dev_key[16]; + + struct bt_mesh_app_key app_keys[MYNEWT_VAL(BLE_MESH_APP_KEY_COUNT)]; + + struct bt_mesh_subnet sub[MYNEWT_VAL(BLE_MESH_SUBNET_COUNT)]; + + struct bt_mesh_rpl rpl[MYNEWT_VAL(BLE_MESH_CRPL)]; +}; + +/* Network interface */ +enum bt_mesh_net_if +{ + BT_MESH_NET_IF_ADV, + BT_MESH_NET_IF_LOCAL, + BT_MESH_NET_IF_PROXY, + BT_MESH_NET_IF_PROXY_CFG, +}; + +/* Decoding context for Network/Transport data */ +struct bt_mesh_net_rx +{ + struct bt_mesh_subnet *sub; + struct bt_mesh_msg_ctx ctx; + u64_t hash; /* Hash for the relay cache */ + u32_t seq; /* Sequence Number */ + u16_t dst; /* Destination address */ + u8_t old_iv :1, /* iv_index - 1 was used */ + new_key :1, /* Data was encrypted with updated key */ + ctl :1, /* Network Control */ + net_if :2; /* Network interface */ + s8_t rssi; +}; + +/* Encoding context for Network/Transport data */ +struct bt_mesh_net_tx +{ + struct bt_mesh_subnet *sub; + struct bt_mesh_msg_ctx *ctx; + u16_t src; +}; + +extern struct bt_mesh_net bt_mesh; + +#define BT_MESH_NET_IVI_TX (bt_mesh.iv_index - bt_mesh.iv_update) +#define BT_MESH_NET_IVI_RX(rx) (bt_mesh.iv_index - (rx)->old_iv) + +int +bt_mesh_net_keys_create(struct bt_mesh_subnet_keys *keys, const u8_t key[16]); + +int +bt_mesh_net_create(u16_t idx, u8_t flags, const u8_t key[16], u32_t iv_index); + +int +bt_mesh_friend_cred_set(struct bt_mesh_friend_cred *cred, u8_t idx, + const u8_t net_key[16]); +void +bt_mesh_friend_cred_refresh(u16_t net_idx); +int +bt_mesh_friend_cred_update(u16_t net_idx, u8_t idx, const u8_t net_key[16]); +struct bt_mesh_friend_cred * +bt_mesh_friend_cred_add(u16_t net_idx, const u8_t net_key[16], u8_t idx, + u16_t addr, u16_t lpn_counter, u16_t frnd_counter); +void +bt_mesh_friend_cred_clear(struct bt_mesh_friend_cred *cred); +int +bt_mesh_friend_cred_del(u16_t net_idx, u16_t addr); + +bool +bt_mesh_kr_update(struct bt_mesh_subnet *sub, u8_t new_kr, bool new_key); + +int +bt_mesh_net_beacon_update(struct bt_mesh_subnet *sub); + +void +bt_mesh_rpl_reset(void); + +void +bt_mesh_iv_update(u32_t iv_index, bool iv_update); + +struct bt_mesh_subnet * +bt_mesh_subnet_get(u16_t net_idx); + +struct bt_mesh_subnet * +bt_mesh_subnet_find(const u8_t net_id[8], u8_t flags, u32_t iv_index, + const u8_t auth[8], bool *new_key); + +int +bt_mesh_net_encode(struct bt_mesh_net_tx *tx, struct os_mbuf *buf, +bool proxy); + +int +bt_mesh_net_send(struct bt_mesh_net_tx *tx, struct os_mbuf *buf, + bt_mesh_adv_func_t cb); + +int +bt_mesh_net_resend(struct bt_mesh_subnet *sub, struct os_mbuf *buf, +bool new_key, + bool friend_cred, bt_mesh_adv_func_t cb); + +int +bt_mesh_net_decode(struct os_mbuf *data, enum bt_mesh_net_if net_if, + struct bt_mesh_net_rx *rx, struct os_mbuf *buf, + struct net_buf_simple_state *state); + +void +bt_mesh_net_recv(struct os_mbuf *data, s8_t rssi, enum bt_mesh_net_if net_if); + +void +bt_mesh_net_init(void); + +#endif diff --git a/nimble/host/mesh/src/prov.c b/nimble/host/mesh/src/prov.c new file mode 100644 index 000000000..90193777f --- /dev/null +++ b/nimble/host/mesh/src/prov.c @@ -0,0 +1,1506 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ +#include "syscfg/syscfg.h" +#if MYNEWT_VAL(BLE_MESH_PROV) == 1 + +#include +#include "mesh/atomic.h" + +#define BT_DBG_ENABLED (MYNEWT_VAL(BLE_MESH_DEBUG_PROV)) +#include "host/ble_hs_log.h" + +#include "mesh/mesh.h" +#include "mesh_priv.h" + +#include + +#include "crypto.h" +#include "adv.h" +#include "net.h" +#include "access.h" +#include "foundation.h" +#include "proxy.h" +#include "prov.h" + +/* 3 transmissions, 20ms interval */ +#define PROV_XMIT_COUNT 2 +#define PROV_XMIT_INT 20 + +#define AUTH_METHOD_NO_OOB 0x00 +#define AUTH_METHOD_STATIC 0x01 +#define AUTH_METHOD_OUTPUT 0x02 +#define AUTH_METHOD_INPUT 0x03 + +#define OUTPUT_OOB_BLINK 0x00 +#define OUTPUT_OOB_BEEP 0x01 +#define OUTPUT_OOB_VIBRATE 0x02 +#define OUTPUT_OOB_NUMBER 0x03 +#define OUTPUT_OOB_STRING 0x04 + +#define INPUT_OOB_PUSH 0x00 +#define INPUT_OOB_TWIST 0x01 +#define INPUT_OOB_NUMBER 0x02 +#define INPUT_OOB_STRING 0x03 + +#define PROV_ERR_NONE 0x00 +#define PROV_ERR_NVAL_PDU 0x01 +#define PROV_ERR_NVAL_FMT 0x02 +#define PROV_ERR_UNEXP_PDU 0x03 +#define PROV_ERR_CFM_FAILED 0x04 +#define PROV_ERR_RESOURCES 0x05 +#define PROV_ERR_DECRYPT 0x06 +#define PROV_ERR_UNEXP_ERR 0x07 +#define PROV_ERR_ADDR 0x08 + +#define PROV_INVITE 0x00 +#define PROV_CAPABILITIES 0x01 +#define PROV_START 0x02 +#define PROV_PUB_KEY 0x03 +#define PROV_INPUT_COMPLETE 0x04 +#define PROV_CONFIRM 0x05 +#define PROV_RANDOM 0x06 +#define PROV_DATA 0x07 +#define PROV_COMPLETE 0x08 +#define PROV_FAILED 0x09 + +#define PROV_ALG_P256 0x00 + +#define GPCF(gpc) (gpc & 0x03) +#define GPC_START(last_seg) (((last_seg) << 2) | 0x00) +#define GPC_ACK 0x01 +#define GPC_CONT(seg_id) (((seg_id) << 2) | 0x02) +#define GPC_CTL(op) (((op) << 2) | 0x03) + +#define START_PAYLOAD_MAX 20 +#define CONT_PAYLOAD_MAX 23 + +#define START_LAST_SEG(gpc) (gpc >> 2) +#define CONT_SEG_INDEX(gpc) (gpc >> 2) + +#define BEARER_CTL(gpc) (gpc >> 2) +#define LINK_OPEN 0x00 +#define LINK_ACK 0x01 +#define LINK_CLOSE 0x02 + +#define CLOSE_REASON_SUCCESS 0x00 +#define CLOSE_REASON_TIMEOUT 0x01 +#define CLOSE_REASON_FAILED 0x02 + +#define XACT_SEG_DATA(_seg) (&link.rx.buf->om_data[20 + ((_seg - 1) * 23)]) +#define XACT_SEG_RECV(_seg) (link.rx.seg &= ~(1 << (_seg))) + +#define XACT_NVAL 0xff + +enum { + REMOTE_PUB_KEY, /* Remote key has been received */ + LOCAL_PUB_KEY, /* Local public key is available */ + LINK_ACTIVE, /* Link has been opened */ + HAVE_DHKEY, /* DHKey has been calcualted */ + SEND_CONFIRM, /* Waiting to send Confirm value */ + WAIT_NUMBER, /* Waiting for number input from user */ + WAIT_STRING, /* Waiting for string input from user */ + + NUM_FLAGS, +}; + +struct prov_link { + ATOMIC_DEFINE(flags, NUM_FLAGS); +#if (MYNEWT_VAL(BLE_MESH_PB_GATT)) + uint16_t conn_handle; /* GATT connection */ +#endif + u8_t dhkey[32]; /* Calculated DHKey */ + u8_t expect; /* Next expected PDU */ + + u8_t oob_method; + u8_t oob_action; + u8_t oob_size; + + u8_t conf[16]; /* Remote Confirmation */ + u8_t rand[16]; /* Local Random */ + u8_t auth[16]; /* Authentication Value */ + + u8_t conf_salt[16]; /* ConfirmationSalt */ + u8_t conf_key[16]; /* ConfirmationKey */ + u8_t conf_inputs[145]; /* ConfirmationInputs */ + u8_t prov_salt[16]; /* Provisioning Salt */ + + u32_t id; /* Link ID */ + + struct { + u8_t id; /* Transaction ID */ + u8_t prev_id; /* Previous Transaction ID */ + u8_t seg; /* Bit-field of unreceived segments */ + u8_t last_seg; /* Last segment (to check length) */ + u8_t fcs; /* Expected FCS value */ + struct os_mbuf *buf; + } rx; + +#if (MYNEWT_VAL(BLE_MESH_PB_ADV)) + struct { + /* Start timestamp of the transaction */ + s64_t start; + + /* Pending outgoing buffer(s) */ + struct os_mbuf *buf[3]; + + /* Retransmit timer */ + struct k_delayed_work retransmit; + } tx; +#endif +}; + +struct prov_rx { + u32_t link_id; + u8_t xact_id; + u8_t gpc; +}; + +#define RETRANSMIT_TIMEOUT K_MSEC(500) +#define BUF_TIMEOUT K_MSEC(400) +#define TRANSACTION_TIMEOUT K_SECONDS(30) + +#if (MYNEWT_VAL(BLE_MESH_PB_GATT)) +#define PROV_BUF_HEADROOM 5 +#else +#define PROV_BUF_HEADROOM 0 +static struct os_mbuf *rx_buf; +#endif + +#define PROV_BUF(len) NET_BUF_SIMPLE(PROV_BUF_HEADROOM + len) + +static struct prov_link link; + +static const struct bt_mesh_prov *prov; + +static void close_link(u8_t err, u8_t reason); + +#if (MYNEWT_VAL(BLE_MESH_PB_ADV)) +static void buf_sent(struct os_mbuf *buf, int err) +{ + BT_DBG("buf_sent"); + + if (!link.tx.buf[0]) { + return; + } + + k_delayed_work_submit(&link.tx.retransmit, RETRANSMIT_TIMEOUT); +} + +static void free_segments(void) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(link.tx.buf); i++) { + struct os_mbuf *buf = link.tx.buf[i]; + + if (!buf) { + break; + } + + link.tx.buf[i] = NULL; + /* Mark as canceled */ + BT_MESH_ADV(buf)->busy = 0; + net_buf_unref(buf); + } +} + +static void prov_clear_tx(void) +{ + BT_DBG(""); + + k_delayed_work_cancel(&link.tx.retransmit); + + free_segments(); +} + +static void reset_link(void) +{ + atomic_clear_bit(link.flags, LINK_ACTIVE); + + prov_clear_tx(); + + /* Clear everything except the retransmit delayed work config */ + memset(&link, 0, offsetof(struct prov_link, tx.retransmit)); + + link.rx.prev_id = XACT_NVAL; + + if (bt_pub_key_get()) { + atomic_set_bit(link.flags, LOCAL_PUB_KEY); + } + +#if (MYNEWT_VAL(BLE_MESH_PB_GATT)) + link.rx.buf = bt_mesh_proxy_get_buf(); +#else + net_buf_simple_init(rx_buf, 0); + link.rx.buf = rx_buf; +#endif + + /* Disable Attention Timer if it was set */ + if (link.conf_inputs[0]) { + bt_mesh_attention(NULL, 0); + } +} + +static struct os_mbuf *adv_buf_create(void) +{ + struct os_mbuf *buf; + + buf = bt_mesh_adv_create(BT_MESH_ADV_PROV, PROV_XMIT_COUNT, + PROV_XMIT_INT, BUF_TIMEOUT); + if (!buf) { + BT_ERR("Out of provisioning buffers"); + return NULL; + } + + return buf; +} + +static u8_t pending_ack = XACT_NVAL; + +static void ack_complete(struct os_mbuf *buf, int err) +{ + BT_DBG("xact %u complete", (u8_t)pending_ack); + pending_ack = XACT_NVAL; +} + +static void gen_prov_ack_send(u8_t xact_id) +{ + bt_mesh_adv_func_t complete; + struct os_mbuf *buf; + + BT_DBG("xact_id %u", xact_id); + + if (pending_ack == xact_id) { + BT_DBG("Not sending duplicate ack"); + return; + } + + buf = adv_buf_create(); + if (!buf) { + return; + } + + if (pending_ack == XACT_NVAL) { + pending_ack = xact_id; + complete = ack_complete; + } else { + complete = NULL; + } + + net_buf_add_be32(buf, link.id); + net_buf_add_u8(buf, xact_id); + net_buf_add_u8(buf, GPC_ACK); + + bt_mesh_adv_send(buf, complete); + net_buf_unref(buf); +} + +static void send_reliable(void) +{ + int i; + + link.tx.start = k_uptime_get(); + + for (i = 0; i < ARRAY_SIZE(link.tx.buf); i++) { + struct os_mbuf *buf = link.tx.buf[i]; + + if (!buf) { + break; + } + + if (i + 1 < ARRAY_SIZE(link.tx.buf) && link.tx.buf[i + 1]) { + bt_mesh_adv_send(buf, NULL); + } else { + bt_mesh_adv_send(buf, buf_sent); + } + } +} + +static int bearer_ctl_send(u8_t op, void *data, u8_t data_len) +{ + struct os_mbuf *buf; + + BT_DBG("op 0x%02x data_len %u", op, data_len); + + prov_clear_tx(); + + buf = adv_buf_create(); + if (!buf) { + return -ENOBUFS; + } + + net_buf_add_be32(buf, link.id); + /* Transaction ID, always 0 for Bearer messages */ + net_buf_add_u8(buf, 0x00); + net_buf_add_u8(buf, GPC_CTL(op)); + net_buf_add_mem(buf, data, data_len); + + link.tx.buf[0] = buf; + send_reliable(); + + return 0; +} + +static u8_t last_seg(u8_t len) +{ + if (len <= START_PAYLOAD_MAX) { + return 0; + } + + len -= START_PAYLOAD_MAX; + + return 1 + (len / CONT_PAYLOAD_MAX); +} + +static int prov_send_adv(struct os_mbuf *msg) +{ + static u8_t id = 0x80; + struct os_mbuf *start, *buf; + u8_t seg_len, seg_id; + + BT_DBG("prov_send_adv len %u: %s", msg->om_len, bt_hex(msg->om_data, msg->om_len)); + + prov_clear_tx(); + + start = adv_buf_create(); + if (!start) { + return -ENOBUFS; + } + + net_buf_add_be32(start, link.id); + net_buf_add_u8(start, id); + + net_buf_add_u8(start, GPC_START(last_seg(msg->om_len))); + net_buf_add_be16(start, msg->om_len); + net_buf_add_u8(start, bt_mesh_fcs_calc(msg->om_data, msg->om_len)); + + link.tx.buf[0] = start; + + seg_len = min(msg->om_len, START_PAYLOAD_MAX); + BT_DBG("seg 0 len %u: %s", seg_len, bt_hex(msg->om_data, seg_len)); + net_buf_add_mem(start, msg->om_data, seg_len); + net_buf_simple_pull(msg, seg_len); + + buf = start; + for (seg_id = 1; msg->om_len > 0; seg_id++) { + if (seg_id >= ARRAY_SIZE(link.tx.buf)) { + BT_ERR("Too big message"); + free_segments(); + return -E2BIG; + } + + buf = adv_buf_create(); + if (!buf) { + free_segments(); + return -ENOBUFS; + } + + link.tx.buf[seg_id] = buf; + + seg_len = min(msg->om_len, CONT_PAYLOAD_MAX); + + BT_DBG("seg_id %u len %u: %s", seg_id, seg_len, + bt_hex(msg->om_data, seg_len)); + + net_buf_add_be32(buf, link.id); + net_buf_add_u8(buf, id); + net_buf_add_u8(buf, GPC_CONT(seg_id)); + net_buf_add_mem(buf, msg->om_data, seg_len); + net_buf_simple_pull(msg, seg_len); + } + + id++; + + send_reliable(); + + return 0; +} + +#endif /* MYNEWT_VAL(BLE_MESH_PB_ADV) */ + +#if (MYNEWT_VAL(BLE_MESH_PB_GATT)) +static int prov_send_gatt(struct os_mbuf *msg) +{ + if (!link.conn_handle) { + BT_ERR("No connection handle!?"); + return -ENOTCONN; + } + + return bt_mesh_proxy_send(link.conn_handle, BT_MESH_PROXY_PROV, msg); +} +#endif /* MYNEWT_VAL(BLE_MESH_PB_GATT) */ + +static inline int prov_send(struct os_mbuf *buf) +{ +#if (MYNEWT_VAL(BLE_MESH_PB_GATT)) + if (link.conn_handle) { + return prov_send_gatt(buf); + } +#endif +#if (MYNEWT_VAL(BLE_MESH_PB_ADV)) + return prov_send_adv(buf); +#else + return 0; +#endif +} + +static void prov_buf_init(struct os_mbuf *buf, u8_t type) +{ + net_buf_simple_init(buf, PROV_BUF_HEADROOM); + net_buf_simple_add_u8(buf, type); +} + +static void prov_invite(const u8_t *data) +{ + struct os_mbuf *buf = PROV_BUF(12); + + BT_INFO("Attention Duration: %u seconds", data[0]); + + if (data[0]) { + bt_mesh_attention(NULL, data[0]); + } + + link.conf_inputs[0] = data[0]; + + prov_buf_init(buf, PROV_CAPABILITIES); + + /* Number of Elements supported */ + net_buf_simple_add_u8(buf, bt_mesh_elem_count()); + + /* Supported algorithms - FIPS P-256 Eliptic Curve */ + net_buf_simple_add_be16(buf, BIT(PROV_ALG_P256)); + + /* Public Key Type */ + /*PTS OOB*/ + net_buf_simple_add_u8(buf, 0x00); + + /* Static OOB Type */ + net_buf_simple_add_u8(buf, prov->static_val ? BIT(0) : 0x00); + + /* Output OOB Size */ + net_buf_simple_add_u8(buf, prov->output_size); + + /* Output OOB Action */ + net_buf_simple_add_be16(buf, prov->output_actions); + + /* Input OOB Size */ + net_buf_simple_add_u8(buf, prov->input_size); + + /* Input OOB Action */ + net_buf_simple_add_be16(buf, prov->input_actions); + + memcpy(&link.conf_inputs[1], &buf->om_data[1], 11); + + if (prov_send(buf)) { + BT_ERR("Failed to send capabilities"); + close_link(PROV_ERR_RESOURCES, CLOSE_REASON_FAILED); + goto done; + } + + link.expect = PROV_START; + +done: + os_mbuf_free_chain(buf); +} + +static void prov_capabilities(const u8_t *data) +{ + u16_t algorithms, output_action, input_action; + + BT_INFO("Elements: %u", data[0]); + + algorithms = sys_get_be16(&data[1]); + BT_INFO("Algorithms: %u", algorithms); + + BT_INFO("Public Key Type: 0x%02x", data[3]); + BT_INFO("Static OOB Type: 0x%02x", data[4]); + BT_INFO("Output OOB Size: %u", data[5]); + + output_action = sys_get_be16(&data[6]); + BT_INFO("Output OOB Action: 0x%04x", output_action); + + BT_INFO("Input OOB Size: %u", data[8]); + + input_action = sys_get_be16(&data[9]); + BT_INFO("Input OOB Action: 0x%04x", input_action); +} + +static bt_mesh_output_action output_action(u8_t action) +{ + switch (action) { + case OUTPUT_OOB_BLINK: + return BT_MESH_BLINK; + case OUTPUT_OOB_BEEP: + return BT_MESH_BEEP; + case OUTPUT_OOB_VIBRATE: + return BT_MESH_VIBRATE; + case OUTPUT_OOB_NUMBER: + return BT_MESH_DISPLAY_NUMBER; + case OUTPUT_OOB_STRING: + return BT_MESH_DISPLAY_STRING; + default: + return BT_MESH_NO_OUTPUT; + } +} + +static bt_mesh_input_action input_action(u8_t action) +{ + switch (action) { + case INPUT_OOB_PUSH: + return BT_MESH_PUSH; + case INPUT_OOB_TWIST: + return BT_MESH_TWIST; + case INPUT_OOB_NUMBER: + return BT_MESH_ENTER_NUMBER; + case INPUT_OOB_STRING: + return BT_MESH_ENTER_STRING; + default: + return BT_MESH_NO_INPUT; + } +} + +static int prov_auth(u8_t method, u8_t action, u8_t size) +{ + bt_mesh_output_action output; + bt_mesh_input_action input; + + switch (method) { + case AUTH_METHOD_NO_OOB: + if (action || size) { + return -EINVAL; + } + + memset(link.auth, 0, sizeof(link.auth)); + return 0; + case AUTH_METHOD_STATIC: + if (action || size) { + return -EINVAL; + } + + memcpy(link.auth, prov->static_val, prov->static_val_len); + return 0; + + case AUTH_METHOD_OUTPUT: + output = output_action(action); + if (!output) { + return -EINVAL; + } + + if (!(prov->output_actions & output)) { + return -EINVAL; + } + + if (size > prov->output_size) { + return -EINVAL; + } + + if (output == BT_MESH_DISPLAY_STRING) { + u8_t i, str[9]; + + bt_rand(str, size); + + /* Normalize to '0' .. '9' & 'A' .. 'Z' */ + for (i = 0; i < size; i++) { + str[i] %= 36; + if (str[i] < 10) { + str[i] += '\0'; + } else { + str[i] += 'A' - 10; + } + } + str[size] = '\0'; + + memcpy(link.auth, str, size); + + return prov->output_string((char *)str); + } else { + u32_t div[8] = { 10, 100, 1000, 10000, 100000, + 1000000, 10000000, 100000000 }; + u32_t num; + + bt_rand(&num, sizeof(num)); + num %= div[size - 1]; + + sys_put_be32(num, &link.auth[12]); + + return prov->output_number(output, num); + } + + case AUTH_METHOD_INPUT: + input = input_action(action); + if (!input) { + return -EINVAL; + } + + if (!(prov->input_actions & input)) { + return -EINVAL; + } + + if (size > prov->input_size) { + return -EINVAL; + } + + if (input == BT_MESH_ENTER_STRING) { + atomic_set_bit(link.flags, WAIT_STRING); + } else { + atomic_set_bit(link.flags, WAIT_NUMBER); + } + + return prov->input(input, size); + + default: + return -EINVAL; + } +} + +static void prov_start(const u8_t *data) +{ + BT_INFO("Algorithm: 0x%02x", data[0]); + BT_INFO("Public Key: 0x%02x", data[1]); + BT_INFO("Auth Method: 0x%02x", data[2]); + BT_INFO("Auth Action: 0x%02x", data[3]); + BT_INFO("Auth Size: 0x%02x", data[4]); + + if (data[0] != PROV_ALG_P256) { + BT_ERR("Unknown algorithm 0x%02x", data[0]); + return; + } + + memcpy(&link.conf_inputs[12], data, 5); + + link.expect = PROV_PUB_KEY; + + if (prov_auth(data[2], data[3], data[4]) < 0) { + BT_ERR("Invalid authentication method"); + } +} + +static void send_confirm(void) +{ + struct os_mbuf *cfm = PROV_BUF(17); + + BT_DBG("ConfInputs[0] %s", bt_hex(link.conf_inputs, 64)); + BT_DBG("ConfInputs[64] %s", bt_hex(&link.conf_inputs[64], 64)); + BT_DBG("ConfInputs[128] %s", bt_hex(&link.conf_inputs[128], 17)); + + if (bt_mesh_prov_conf_salt(link.conf_inputs, link.conf_salt)) { + BT_ERR("Unable to generate confirmation salt"); + close_link(PROV_ERR_UNEXP_ERR, CLOSE_REASON_FAILED); + goto done; + } + + BT_DBG("ConfirmationSalt: %s", bt_hex(link.conf_salt, 16)); + + if (bt_mesh_prov_conf_key(link.dhkey, link.conf_salt, link.conf_key)) { + BT_ERR("Unable to generate confirmation key"); + close_link(PROV_ERR_UNEXP_ERR, CLOSE_REASON_FAILED); + goto done; + } + + BT_DBG("ConfirmationKey: %s", bt_hex(link.conf_key, 16)); + + if (bt_rand(link.rand, 16)) { + BT_ERR("Unable to generate random number"); + close_link(PROV_ERR_UNEXP_ERR, CLOSE_REASON_FAILED); + goto done; + } + + BT_DBG("LocalRandom: %s", bt_hex(link.rand, 16)); + + prov_buf_init(cfm, PROV_CONFIRM); + + if (bt_mesh_prov_conf(link.conf_key, link.rand, link.auth, + net_buf_simple_add(cfm, 16))) { + BT_ERR("Unable to generate confirmation value"); + close_link(PROV_ERR_UNEXP_ERR, CLOSE_REASON_FAILED); + goto done; + } + + if (prov_send(cfm)) { + BT_ERR("Failed to send Provisioning Confirm"); + close_link(PROV_ERR_RESOURCES, CLOSE_REASON_FAILED); + goto done; + } + + link.expect = PROV_RANDOM; + +done: + os_mbuf_free_chain(cfm); +} + +int bt_mesh_input_number(u32_t num) +{ + if (!atomic_test_and_clear_bit(link.flags, WAIT_NUMBER)) { + return -EINVAL; + } + + if (!atomic_test_bit(link.flags, HAVE_DHKEY)) { + return 0; + } + + sys_put_be32(num, &link.auth[12]); + + if (atomic_test_and_clear_bit(link.flags, SEND_CONFIRM)) { + send_confirm(); + } + + return 0; +} + +int bt_mesh_input_string(const char *str) +{ + if (!atomic_test_and_clear_bit(link.flags, WAIT_STRING)) { + return -EINVAL; + } + + if (!atomic_test_bit(link.flags, HAVE_DHKEY)) { + return 0; + } + + strncpy((char *)link.auth, str, prov->input_size); + + if (atomic_test_and_clear_bit(link.flags, SEND_CONFIRM)) { + send_confirm(); + } + + return 0; +} + +static void prov_dh_key_cb(const u8_t key[32]) +{ + BT_DBG("%p", key); + + if (!key) { + BT_ERR("DHKey generation failed"); + close_link(PROV_ERR_UNEXP_ERR, CLOSE_REASON_FAILED); + return; + } + + sys_memcpy_swap(link.dhkey, key, 32); + + BT_DBG("DHkey: %s", bt_hex(link.dhkey, 32)); + + atomic_set_bit(link.flags, HAVE_DHKEY); + + if (atomic_test_bit(link.flags, WAIT_NUMBER) || + atomic_test_bit(link.flags, WAIT_STRING)) { + return; + } + + if (atomic_test_and_clear_bit(link.flags, SEND_CONFIRM)) { + send_confirm(); + } +} + +static void send_pub_key(void) +{ + struct os_mbuf *buf = PROV_BUF(65); + const u8_t *key; + + key = bt_pub_key_get(); + if (!key) { + BT_ERR("No public key available"); + close_link(PROV_ERR_RESOURCES, CLOSE_REASON_FAILED); + goto done; + } + + prov_buf_init(buf, PROV_PUB_KEY); + + /* Swap X and Y halves independently to big-endian */ + sys_memcpy_swap(net_buf_simple_add(buf, 32), key, 32); + sys_memcpy_swap(net_buf_simple_add(buf, 32), &key[32], 32); + + memcpy(&link.conf_inputs[81], &buf->om_data[1], 64); + + BT_DBG("Local Public Key: %s", bt_hex(&buf->om_data[1], 64)); + + prov_send(buf); + + /* Copy remote key in little-endian for bt_dh_key_gen(). + * X and Y halves are swapped independently. + */ + net_buf_simple_init(buf, 0); + sys_memcpy_swap(buf->om_data, &link.conf_inputs[17], 32); + sys_memcpy_swap(&buf->om_data[32], &link.conf_inputs[49], 32); + + if (bt_dh_key_gen(buf->om_data, prov_dh_key_cb)) { + BT_ERR("Failed to generate DHKey"); + close_link(PROV_ERR_UNEXP_ERR, CLOSE_REASON_FAILED); + goto done; + } + + link.expect = PROV_CONFIRM; + +done: + os_mbuf_free_chain(buf); +} + +static void prov_pub_key(const u8_t *data) +{ + BT_INFO("Remote Public Key: %s", bt_hex(data, 64)); + + memcpy(&link.conf_inputs[17], data, 64); + + if (!atomic_test_bit(link.flags, LOCAL_PUB_KEY)) { + /* Clear retransmit timer */ +#if (MYNEWT_VAL(BLE_MESH_PB_ADV)) + prov_clear_tx(); +#endif + atomic_set_bit(link.flags, REMOTE_PUB_KEY); + BT_WARN("Waiting for local public key"); + return; + } + + send_pub_key(); +} + +static void pub_key_ready(const u8_t *pkey) +{ + if (!pkey) { + BT_WARN("Public key not available"); + return; + } + + BT_DBG("Local public key ready"); + + atomic_set_bit(link.flags, LOCAL_PUB_KEY); + + if (atomic_test_and_clear_bit(link.flags, REMOTE_PUB_KEY)) { + send_pub_key(); + } +} + +static void prov_input_complete(const u8_t *data) +{ + BT_DBG(""); +} + +static void prov_confirm(const u8_t *data) +{ + BT_INFO("Remote Confirm: %s", bt_hex(data, 16)); + + memcpy(link.conf, data, 16); + + if (!atomic_test_bit(link.flags, HAVE_DHKEY)) { +#if (MYNEWT_VAL(BLE_MESH_PB_ADV)) + prov_clear_tx(); +#endif + atomic_set_bit(link.flags, SEND_CONFIRM); + } else { + send_confirm(); + } +} + +static void prov_random(const u8_t *data) +{ + struct os_mbuf *rnd = PROV_BUF(16); + u8_t conf_verify[16]; + + BT_INFO("Remote Random: %s", bt_hex(data, 16)); + + if (bt_mesh_prov_conf(link.conf_key, data, link.auth, conf_verify)) { + BT_ERR("Unable to calculate confirmation verification"); + close_link(PROV_ERR_UNEXP_ERR, CLOSE_REASON_FAILED); + goto done; + } + + if (memcmp(conf_verify, link.conf, 16)) { + BT_ERR("Invalid confirmation value"); + BT_DBG("Received: %s", bt_hex(link.conf, 16)); + BT_DBG("Calculated: %s", bt_hex(conf_verify, 16)); + close_link(PROV_ERR_CFM_FAILED, CLOSE_REASON_FAILED); + goto done; + } + + prov_buf_init(rnd, PROV_RANDOM); + net_buf_simple_add_mem(rnd, link.rand, 16); + + if (prov_send(rnd)) { + BT_ERR("Failed to send Provisioning Random"); + close_link(PROV_ERR_RESOURCES, CLOSE_REASON_FAILED); + goto done; + } + + if (bt_mesh_prov_salt(link.conf_salt, data, link.rand, + link.prov_salt)) { + BT_ERR("Failed to generate provisioning salt"); + close_link(PROV_ERR_UNEXP_ERR, CLOSE_REASON_FAILED); + goto done; + } + + BT_DBG("ProvisioningSalt: %s", bt_hex(link.prov_salt, 16)); + + link.expect = PROV_DATA; + +done: + os_mbuf_free_chain(rnd); +} + +static void prov_data(const u8_t *data) +{ + struct os_mbuf *msg = PROV_BUF(1); + u8_t session_key[16]; + u8_t nonce[13]; + u8_t dev_key[16]; + u8_t pdu[25]; + u8_t flags; + u32_t iv_index; + u16_t addr; + u16_t net_idx; + int err; + + BT_DBG(""); + + err = bt_mesh_session_key(link.dhkey, link.prov_salt, session_key); + if (err) { + BT_ERR("Unable to generate session key"); + close_link(PROV_ERR_UNEXP_ERR, CLOSE_REASON_FAILED); + goto done; + } + + BT_DBG("SessionKey: %s", bt_hex(session_key, 16)); + + err = bt_mesh_prov_nonce(link.dhkey, link.prov_salt, nonce); + if (err) { + BT_ERR("Unable to generate session nonce"); + close_link(PROV_ERR_UNEXP_ERR, CLOSE_REASON_FAILED); + goto done; + } + + BT_DBG("Nonce: %s", bt_hex(nonce, 13)); + + err = bt_mesh_prov_decrypt(session_key, nonce, data, pdu); + if (err) { + BT_ERR("Unable to decrypt provisioning data"); + close_link(PROV_ERR_DECRYPT, CLOSE_REASON_FAILED); + goto done; + } + + err = bt_mesh_dev_key(link.dhkey, link.prov_salt, dev_key); + if (err) { + BT_ERR("Unable to generate device key"); + close_link(PROV_ERR_UNEXP_ERR, CLOSE_REASON_FAILED); + goto done; + } + + BT_DBG("DevKey: %s", bt_hex(dev_key, 16)); + + net_idx = sys_get_be16(&pdu[16]); + flags = pdu[18]; + iv_index = sys_get_be32(&pdu[19]); + addr = sys_get_be16(&pdu[23]); + + BT_DBG("net_idx %u iv_index 0x%08x, addr 0x%04x", + net_idx, iv_index, addr); + + prov_buf_init(msg, PROV_COMPLETE); + prov_send(msg); + + /* Ignore any further PDUs on this link */ + link.expect = 0; + + bt_mesh_provision(pdu, net_idx, flags, iv_index, 0, addr, dev_key); + + if (prov->complete) { + prov->complete(); + } + +done: + os_mbuf_free_chain(msg); +} + +static void prov_complete(const u8_t *data) +{ + BT_INFO(""); +} + +static void prov_failed(const u8_t *data) +{ + BT_WARN("Error: 0x%02x", data[0]); +} + +static const struct { + void (*func)(const u8_t *data); + u16_t len; +} prov_handlers[] = { + { prov_invite, 1 }, + { prov_capabilities, 11 }, + { prov_start, 5, }, + { prov_pub_key, 64 }, + { prov_input_complete, 0 }, + { prov_confirm, 16 }, + { prov_random, 16 }, + { prov_data, 33 }, + { prov_complete, 0 }, + { prov_failed, 1 }, +}; + +static void close_link(u8_t err, u8_t reason) +{ +#if (MYNEWT_VAL(BLE_MESH_PB_GATT)) + if (link.conn_handle) { + bt_mesh_pb_gatt_close(link.conn_handle); + return; + } +#endif + +#if (MYNEWT_VAL(BLE_MESH_PB_ADV)) + if (err) { + struct os_mbuf *buf = PROV_BUF(2); + + prov_buf_init(buf, PROV_FAILED); + net_buf_simple_add_u8(buf, err); + prov_send(buf); + os_mbuf_free_chain(buf); + } + + link.rx.seg = 0; + bearer_ctl_send(LINK_CLOSE, &reason, sizeof(reason)); +#endif + + atomic_clear_bit(link.flags, LINK_ACTIVE); + + /* Disable Attention Timer if it was set */ + if (link.conf_inputs[0]) { + bt_mesh_attention(NULL, 0); + } +} + +#if (MYNEWT_VAL(BLE_MESH_PB_ADV)) +static void prov_retransmit(struct os_event *work) +{ + int i; + + BT_DBG("prov_retransmit"); + + if (!atomic_test_bit(link.flags, LINK_ACTIVE)) { + BT_WARN("Link not active"); + return; + } + + if (k_uptime_get() - link.tx.start > TRANSACTION_TIMEOUT) { + BT_WARN("Giving up transaction"); + reset_link(); + return; + } + + for (i = 0; i < ARRAY_SIZE(link.tx.buf); i++) { + struct os_mbuf *buf = link.tx.buf[i]; + + if (!buf) { + break; + } + + if (BT_MESH_ADV(buf)->busy) { + continue; + } + + BT_DBG("%u bytes: %s", buf->om_len, bt_hex(buf->om_data, buf->om_len)); + + if (i + 1 < ARRAY_SIZE(link.tx.buf) && link.tx.buf[i + 1]) { + bt_mesh_adv_send(buf, NULL); + } else { + bt_mesh_adv_send(buf, buf_sent); + } + + } +} + +static void link_open(struct prov_rx *rx, struct os_mbuf *buf) +{ + BT_INFO("link open: len %u", buf->om_len); + + if (buf->om_len < 16) { + BT_ERR("Too short bearer open message (len %u)", buf->om_len); + return; + } + + if (atomic_test_bit(link.flags, LINK_ACTIVE)) { + BT_WARN("Ignoring bearer open: link already active"); + return; + } + + if (memcmp(buf->om_data, prov->uuid, 16)) { + BT_DBG("Bearer open message not for us"); + return; + } + + link.id = rx->link_id; + atomic_set_bit(link.flags, LINK_ACTIVE); + net_buf_simple_init(link.rx.buf, 0); + + bearer_ctl_send(LINK_ACK, NULL, 0); + + link.expect = PROV_INVITE; +} + +static void link_ack(struct prov_rx *rx, struct os_mbuf *buf) +{ + BT_INFO("Link ack: len %u", buf->om_len); +} + +static void link_close(struct prov_rx *rx, struct os_mbuf *buf) +{ + BT_INFO("Link close: len %u", buf->om_len); + + reset_link(); +} + +static void gen_prov_ctl(struct prov_rx *rx, struct os_mbuf *buf) +{ + BT_DBG("op 0x%02x len %u", BEARER_CTL(rx->gpc), buf->om_len); + + switch (BEARER_CTL(rx->gpc)) { + case LINK_OPEN: + link_open(rx, buf); + break; + case LINK_ACK: + if (!atomic_test_bit(link.flags, LINK_ACTIVE)) { + return; + } + + link_ack(rx, buf); + break; + case LINK_CLOSE: + if (!atomic_test_bit(link.flags, LINK_ACTIVE)) { + return; + } + + link_close(rx, buf); + break; + default: + BT_ERR("Unknown bearer opcode: 0x%02x", BEARER_CTL(rx->gpc)); + return; + } +} + +static void prov_msg_recv(void) +{ + u8_t type = link.rx.buf->om_data[0]; + + BT_DBG("type 0x%02x len %u", type, link.rx.buf->om_len); + + if (type != PROV_FAILED && type != link.expect) { + BT_WARN("Unexpected msg 0x%02x != 0x%02x", type, link.expect); + close_link(PROV_ERR_UNEXP_PDU, CLOSE_REASON_FAILED); + return; + } + + if (!bt_mesh_fcs_check(link.rx.buf, link.rx.fcs)) { + BT_ERR("Incorrect FCS"); + return; + } + + gen_prov_ack_send(link.rx.id); + link.rx.prev_id = link.rx.id; + link.rx.id = 0; + + if (type >= ARRAY_SIZE(prov_handlers)) { + BT_ERR("Unknown provisioning PDU type 0x%02x", type); + close_link(PROV_ERR_NVAL_PDU, CLOSE_REASON_FAILED); + return; + } + + if (1 + prov_handlers[type].len != link.rx.buf->om_len) { + BT_ERR("Invalid length %u for type 0x%02x", + link.rx.buf->om_len, type); + close_link(PROV_ERR_NVAL_FMT, CLOSE_REASON_FAILED); + return; + } + + prov_handlers[type].func(&link.rx.buf->om_data[1]); +} + +static void gen_prov_cont(struct prov_rx *rx, struct os_mbuf *buf) +{ + u8_t seg = CONT_SEG_INDEX(rx->gpc); + + BT_DBG("len %u, seg_index %u", buf->om_len, seg); + + if (!link.rx.seg && link.rx.prev_id == rx->xact_id) { + BT_WARN("Resending ack"); + gen_prov_ack_send(rx->xact_id); + return; + } + + if (rx->xact_id != link.rx.id) { + BT_WARN("Data for unknown transaction (%u != %u)", + rx->xact_id, link.rx.id); + return; + } + + if (seg > link.rx.last_seg) { + BT_ERR("Invalid segment index %u", seg); + close_link(PROV_ERR_NVAL_FMT, CLOSE_REASON_FAILED); + return; + } else if (seg == link.rx.last_seg) { + u8_t expect_len; + + expect_len = (link.rx.buf->om_len - 20 - + (23 * (link.rx.last_seg - 1))); + if (expect_len != buf->om_len) { + BT_ERR("Incorrect last seg len: %u != %u", + expect_len, buf->om_len); + close_link(PROV_ERR_NVAL_FMT, CLOSE_REASON_FAILED); + return; + } + } + + if (!(link.rx.seg & BIT(seg))) { + BT_WARN("Ignoring already received segment"); + return; + } + + memcpy(XACT_SEG_DATA(seg), buf->om_data, buf->om_len); + XACT_SEG_RECV(seg); + + if (!link.rx.seg) { + prov_msg_recv(); + } +} + +static void gen_prov_ack(struct prov_rx *rx, struct os_mbuf *buf) +{ + BT_DBG("len %u", buf->om_len); +} + +static void gen_prov_start(struct prov_rx *rx, struct os_mbuf *buf) +{ + if (link.rx.seg) { + BT_WARN("Got Start while there are unreceived segments"); + return; + } + + if (link.rx.prev_id == rx->xact_id) { + BT_WARN("Resending ack"); + gen_prov_ack_send(rx->xact_id); + return; + } + + link.rx.buf->om_len = net_buf_simple_pull_be16(buf); + link.rx.id = rx->xact_id; + link.rx.fcs = net_buf_simple_pull_u8(buf); + + BT_DBG("len %u last_seg %u total_len %u fcs 0x%02x", buf->om_len, + START_LAST_SEG(rx->gpc), link.rx.buf->om_len, link.rx.fcs); + + if (link.rx.buf->om_len < 1) { + BT_ERR("Ignoring zero-length provisioning PDU"); + close_link(PROV_ERR_NVAL_FMT, CLOSE_REASON_FAILED); + return; + } + + if (link.rx.buf->om_len > OS_MBUF_TRAILINGSPACE(link.rx.buf)) { + BT_ERR("Too large provisioning PDU (%u bytes)", + link.rx.buf->om_len); + close_link(PROV_ERR_NVAL_FMT, CLOSE_REASON_FAILED); + return; + } + + if (START_LAST_SEG(rx->gpc) > 0 && link.rx.buf->om_len <= 20) { + BT_ERR("Too small total length for multi-segment PDU"); + close_link(PROV_ERR_NVAL_FMT, CLOSE_REASON_FAILED); + return; + } + + link.rx.seg = (1 << (START_LAST_SEG(rx->gpc) + 1)) - 1; + link.rx.last_seg = START_LAST_SEG(rx->gpc); + memcpy(link.rx.buf->om_data, buf->om_data, buf->om_len); + XACT_SEG_RECV(0); + + if (!link.rx.seg) { + prov_msg_recv(); + } +} + +static const struct { + void (*const func)(struct prov_rx *rx, struct os_mbuf *buf); + const u8_t require_link; + const u8_t min_len; +} gen_prov[] = { + { gen_prov_start, true, 3 }, + { gen_prov_ack, true, 0 }, + { gen_prov_cont, true, 0 }, + { gen_prov_ctl, false, 0 }, +}; + +static void gen_prov_recv(struct prov_rx *rx, struct os_mbuf *buf) +{ + if (buf->om_len < gen_prov[GPCF(rx->gpc)].min_len) { + BT_ERR("Too short GPC message type %u", GPCF(rx->gpc)); + return; + } + + if (!atomic_test_bit(link.flags, LINK_ACTIVE) && + gen_prov[GPCF(rx->gpc)].require_link) { + BT_DBG("Ignoring message that requires active link"); + return; + } + + BT_INFO("prov_action: %d", GPCF(rx->gpc)); + gen_prov[GPCF(rx->gpc)].func(rx, buf); +} + +void bt_mesh_pb_adv_recv(struct os_mbuf *buf) +{ + struct prov_rx rx; + + if (!bt_prov_active() && bt_mesh_is_provisioned()) { + BT_DBG("Ignoring provisioning PDU - already provisioned"); + return; + } + + if (buf->om_len < 6) { + BT_WARN("Too short provisioning packet (len %u)", buf->om_len); + return; + } + + rx.link_id = net_buf_simple_pull_be32(buf); + rx.xact_id = net_buf_simple_pull_u8(buf); + rx.gpc = net_buf_simple_pull_u8(buf); + + BT_DBG("link_id 0x%08x xact_id %u", rx.link_id, rx.xact_id); + + if (atomic_test_bit(link.flags, LINK_ACTIVE) && link.id != rx.link_id) { + BT_DBG("Ignoring mesh beacon for unknown link"); + return; + } + + gen_prov_recv(&rx, buf); +} +#endif /* MYNEWT_VAL(BLE_MESH_PB_ADV) */ + +#if (MYNEWT_VAL(BLE_MESH_PB_GATT)) +int bt_mesh_pb_gatt_recv(uint16_t conn_handle, struct os_mbuf *buf) +{ + u8_t type; + + BT_DBG("%u bytes: %s", buf->om_len, bt_hex(buf->om_data, buf->om_len)); + + if (link.conn_handle != conn_handle) { + BT_WARN("Data for unexpected connection"); + return -ENOTCONN; + } + + if (buf->om_len < 1) { + BT_WARN("Too short provisioning packet (len %u)", buf->om_len); + return -EINVAL; + } + + type = net_buf_simple_pull_u8(buf); + if (type != PROV_FAILED && type != link.expect) { + BT_WARN("Unexpected msg 0x%02x != 0x%02x", type, link.expect); + return -EINVAL; + } + + if (type >= ARRAY_SIZE(prov_handlers)) { + BT_ERR("Unknown provisioning PDU type 0x%02x", type); + return -EINVAL; + } + + if (prov_handlers[type].len != buf->om_len) { + BT_ERR("Invalid length %u for type 0x%02x", buf->om_len, type); + return -EINVAL; + } + + prov_handlers[type].func(buf->om_data); + + return 0; +} + +int bt_mesh_pb_gatt_open(uint16_t conn_handle) +{ + BT_DBG("conn_handle %d", conn_handle); + + if (atomic_test_and_set_bit(link.flags, LINK_ACTIVE)) { + BT_ERR("Link already opened?"); + return -EBUSY; + } + + link.conn_handle = conn_handle; + link.expect = PROV_INVITE; + + return 0; +} + +int bt_mesh_pb_gatt_close(uint16_t conn_handle) +{ + bool pub_key; + + BT_DBG("conn_handle %d", conn_handle); + + if (link.conn_handle != conn_handle) { + BT_ERR("Not connected"); + return -ENOTCONN; + } + + /* Disable Attention Timer if it was set */ + if (link.conf_inputs[0]) { + bt_mesh_attention(NULL, 0); + } + + pub_key = atomic_test_bit(link.flags, LOCAL_PUB_KEY); + memset(&link, 0, sizeof(link)); + + if (pub_key) { + atomic_set_bit(link.flags, LOCAL_PUB_KEY); + } + + return 0; +} +#endif /* MYNEWT_VAL(BLE_MESH_PB_GATT) */ + +const u8_t *bt_mesh_prov_get_uuid(void) +{ + return prov->uuid; +} + +bool bt_prov_active(void) +{ + return atomic_test_bit(link.flags, LINK_ACTIVE); +} + +void bt_mesh_prov_init(const struct bt_mesh_prov *prov_info) +{ + static struct bt_pub_key_cb pub_key_cb = { + .func = pub_key_ready, + }; +#if !(MYNEWT_VAL(BLE_MESH_PB_GATT)) + rx_buf = NET_BUF_SIMPLE(65); + +#endif + + if (bt_pub_key_gen(&pub_key_cb)) { + BT_ERR("Failed to generate public key"); + } + + prov = prov_info; + +#if (MYNEWT_VAL(BLE_MESH_PB_ADV)) + k_delayed_work_init(&link.tx.retransmit, prov_retransmit); + link.rx.prev_id = XACT_NVAL; + +#if (MYNEWT_VAL(BLE_MESH_PB_GATT)) + link.rx.buf = bt_mesh_proxy_get_buf(); +#else + net_buf_simple_init(rx_buf, 0); + link.rx.buf = rx_buf; +#endif + +#endif /* MYNEWT_VAL(BLE_MESH_PB_ADV) */ +} + +void bt_mesh_prov_reset_link(void) { +#if (MYNEWT_VAL(BLE_MESH_PB_GATT)) + link.rx.buf = bt_mesh_proxy_get_buf(); +#else + net_buf_simple_init(rx_buf, 0); + link.rx.buf = rx_buf; +#endif +} +#endif //MYNEWT_VAL(BLE_MESH_PROV) == 1 diff --git a/nimble/host/mesh/src/prov.h b/nimble/host/mesh/src/prov.h new file mode 100644 index 000000000..46ceaebd2 --- /dev/null +++ b/nimble/host/mesh/src/prov.h @@ -0,0 +1,38 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef __PROV_H__ +#define __PROV_H__ + +#include "os/os_mbuf.h" +#include "mesh/mesh.h" +#include "../src/ble_hs_conn_priv.h" + +void +bt_mesh_pb_adv_recv(struct os_mbuf *buf); + +bool +bt_prov_active(void); + +int +bt_mesh_pb_gatt_open(uint16_t conn_handle); +int +bt_mesh_pb_gatt_close(uint16_t conn_handle); +int +bt_mesh_pb_gatt_recv(uint16_t conn_handle, struct os_mbuf *buf); + +const u8_t * +bt_mesh_prov_get_uuid(void); + +void +bt_mesh_prov_init(const struct bt_mesh_prov *prov); + +void +bt_mesh_prov_reset_link(void); + +#endif diff --git a/nimble/host/mesh/src/proxy.c b/nimble/host/mesh/src/proxy.c new file mode 100644 index 000000000..c9f0d80f2 --- /dev/null +++ b/nimble/host/mesh/src/proxy.c @@ -0,0 +1,1219 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "syscfg/syscfg.h" + +#if MYNEWT_VAL(BLE_MESH_PROXY) == 1 + +#include "mesh/mesh.h" + +#define BT_DBG_ENABLED (MYNEWT_VAL(BLE_MESH_DEBUG_PROXY)) +#include "host/ble_hs_log.h" +#include "host/ble_att.h" +#include "services/gatt/ble_svc_gatt.h" +#include "../../host/src/ble_hs_priv.h" + +#include "mesh_priv.h" +#include "adv.h" +#include "net.h" +#include "prov.h" +#include "beacon.h" +#include "foundation.h" +#include "access.h" +#include "proxy.h" + +#define PDU_TYPE(data) (data[0] & BIT_MASK(6)) +#define PDU_SAR(data) (data[0] >> 6) + +#define SAR_COMPLETE 0x00 +#define SAR_FIRST 0x01 +#define SAR_CONT 0x02 +#define SAR_LAST 0x03 + +#define CFG_FILTER_SET 0x00 +#define CFG_FILTER_ADD 0x01 +#define CFG_FILTER_REMOVE 0x02 +#define CFG_FILTER_STATUS 0x03 + +/** @def BT_UUID_MESH_PROV + * @brief Mesh Provisioning Service + */ +ble_uuid16_t BT_UUID_MESH_PROV = BLE_UUID16_INIT(0x1827); +#define BT_UUID_MESH_PROV_VAL 0x1827 +/** @def BT_UUID_MESH_PROXY + * @brief Mesh Proxy Service + */ +ble_uuid16_t BT_UUID_MESH_PROXY = BLE_UUID16_INIT(0x1828); +#define BT_UUID_MESH_PROXY_VAL 0x1828 +/** @def BT_UUID_GATT_CCC + * @brief GATT Client Characteristic Configuration + */ +ble_uuid16_t BT_UUID_GATT_CCC = BLE_UUID16_INIT(0x2902); +#define BT_UUID_GATT_CCC_VAL 0x2902 +/** @def BT_UUID_MESH_PROV_DATA_IN + * @brief Mesh Provisioning Data In + */ +ble_uuid16_t BT_UUID_MESH_PROV_DATA_IN = BLE_UUID16_INIT(0x2adb); +#define BT_UUID_MESH_PROV_DATA_IN_VAL 0x2adb +/** @def BT_UUID_MESH_PROV_DATA_OUT + * @brief Mesh Provisioning Data Out + */ +ble_uuid16_t BT_UUID_MESH_PROV_DATA_OUT = BLE_UUID16_INIT(0x2adc); +#define BT_UUID_MESH_PROV_DATA_OUT_VAL 0x2adc +/** @def BT_UUID_MESH_PROXY_DATA_IN + * @brief Mesh Proxy Data In + */ +ble_uuid16_t BT_UUID_MESH_PROXY_DATA_IN = BLE_UUID16_INIT(0x2add); +#define BT_UUID_MESH_PROXY_DATA_IN_VAL 0x2add +/** @def BT_UUID_MESH_PROXY_DATA_OUT + * @brief Mesh Proxy Data Out + */ +ble_uuid16_t BT_UUID_MESH_PROXY_DATA_OUT = BLE_UUID16_INIT(0x2ade); +#define BT_UUID_MESH_PROXY_DATA_OUT_VAL 0x2ade + +#define PDU_HDR(sar, type) (sar << 6 | (type & BIT_MASK(6))) + +#define CLIENT_BUF_SIZE 68 + +static const struct ble_gap_adv_params slow_adv_param = { + .conn_mode = (BLE_GAP_CONN_MODE_UND ), + .itvl_min = BT_GAP_ADV_SLOW_INT_MIN, + .itvl_max = BT_GAP_ADV_SLOW_INT_MAX, +}; + +static const struct ble_gap_adv_params fast_adv_param = { + .conn_mode = (BLE_GAP_CONN_MODE_UND), + .itvl_min = BT_GAP_ADV_FAST_INT_MIN_2, + .itvl_max = BT_GAP_ADV_FAST_INT_MAX_2, +}; + +static const struct ble_gap_adv_params *proxy_adv_param = &fast_adv_param; + +static bool proxy_adv_enabled; + +#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY)) +static void proxy_send_beacons(struct os_event *work); +#endif + +static struct bt_mesh_proxy_client { + uint16_t conn_handle; + u16_t filter[MYNEWT_VAL(BLE_MESH_PROXY_FILTER_SIZE)]; + enum __packed { + NONE, + WHITELIST, + BLACKLIST, + PROV, + } filter_type; + u8_t msg_type; +#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY)) + struct os_callout send_beacons; +#endif + struct os_mbuf *buf; +} clients[MYNEWT_VAL(BLE_MAX_CONNECTIONS)] = { + [0 ... (MYNEWT_VAL(BLE_MAX_CONNECTIONS) - 1)] = { 0 }, +}; + +/* Track which service is enabled */ +static enum { + MESH_GATT_NONE, + MESH_GATT_PROV, + MESH_GATT_PROXY, +} gatt_svc = MESH_GATT_NONE; + +static struct { + uint16_t proxy_h; + uint16_t proxy_data_out_h; + uint16_t prov_h; + uint16_t prov_data_in_h; + uint16_t prov_data_out_h; +} svc_handles; + +static void resolve_svc_handles(void) +{ + int rc; + + /* Either all handles are already resolved, or none of them */ + if (svc_handles.prov_data_out_h) { + return; + } + + /* + * We assert if attribute is not found since at this stage all attributes + * shall be already registered and thus shall be found. + */ + + rc = ble_gatts_find_svc(BLE_UUID16_DECLARE(BT_UUID_MESH_PROXY_VAL), + &svc_handles.proxy_h); + assert(rc == 0); + + rc = ble_gatts_find_chr(BLE_UUID16_DECLARE(BT_UUID_MESH_PROXY_VAL), + BLE_UUID16_DECLARE(BT_UUID_MESH_PROXY_DATA_OUT_VAL), + NULL, &svc_handles.proxy_data_out_h); + assert(rc == 0); + + rc = ble_gatts_find_svc(BLE_UUID16_DECLARE(BT_UUID_MESH_PROV_VAL), + &svc_handles.prov_h); + assert(rc == 0); + + rc = ble_gatts_find_chr(BLE_UUID16_DECLARE(BT_UUID_MESH_PROV_VAL), + BLE_UUID16_DECLARE(BT_UUID_MESH_PROV_DATA_IN_VAL), + NULL, &svc_handles.prov_data_in_h); + assert(rc == 0); + + rc = ble_gatts_find_chr(BLE_UUID16_DECLARE(BT_UUID_MESH_PROV_VAL), + BLE_UUID16_DECLARE(BT_UUID_MESH_PROV_DATA_OUT_VAL), + NULL, &svc_handles.prov_data_out_h); + assert(rc == 0); +} + +static struct bt_mesh_proxy_client *find_client(uint16_t conn_handle) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(clients); i++) { + if (clients[i].conn_handle == conn_handle) { + return &clients[i]; + } + } + + return NULL; +} + +#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY)) +static int proxy_segment_and_send(uint16_t conn_handle, u8_t type, + struct os_mbuf *msg); + +static int filter_set(struct bt_mesh_proxy_client *client, + struct os_mbuf *buf) +{ + u8_t type; + + if (buf->om_len < 1) { + BT_WARN("Too short Filter Set message"); + return -EINVAL; + } + + type = net_buf_simple_pull_u8(buf); + BT_DBG("type 0x%02x", type); + + switch (type) { + case 0x00: + memset(client->filter, 0, sizeof(client->filter)); + client->filter_type = WHITELIST; + break; + case 0x01: + memset(client->filter, 0, sizeof(client->filter)); + client->filter_type = BLACKLIST; + break; + default: + BT_WARN("Prohibited Filter Type 0x%02x", type); + return -EINVAL; + } + + return 0; +} + +static void filter_add(struct bt_mesh_proxy_client *client, u16_t addr) +{ + int i; + + BT_DBG("addr 0x%02x", addr); + + if (addr == BT_MESH_ADDR_UNASSIGNED) { + return; + } + + for (i = 0; i < ARRAY_SIZE(client->filter); i++) { + if (client->filter[i] == addr) { + return; + } + } + + for (i = 0; i < ARRAY_SIZE(client->filter); i++) { + if (client->filter[i] == BT_MESH_ADDR_UNASSIGNED) { + client->filter[i] = addr; + return; + } + } +} + +static void filter_remove(struct bt_mesh_proxy_client *client, u16_t addr) +{ + int i; + + BT_DBG("addr 0x%02x", addr); + + if (addr == BT_MESH_ADDR_UNASSIGNED) { + return; + } + + for (i = 0; i < ARRAY_SIZE(client->filter); i++) { + if (client->filter[i] == addr) { + client->filter[i] = BT_MESH_ADDR_UNASSIGNED; + return; + } + } +} + +static void send_filter_status(struct bt_mesh_proxy_client *client, + struct bt_mesh_net_rx *rx, + struct os_mbuf *buf) +{ + struct bt_mesh_net_tx tx = { + .sub = rx->sub, + .ctx = &rx->ctx, + .src = bt_mesh_primary_addr(), + }; + u16_t filter_size; + int i, err; + + /* Configuration messages always have dst unassigned */ + tx.ctx->addr = BT_MESH_ADDR_UNASSIGNED; + + net_buf_simple_init(buf, 10); + + net_buf_simple_add_u8(buf, CFG_FILTER_STATUS); + + if (client->filter_type == WHITELIST) { + net_buf_simple_add_u8(buf, 0x00); + } else { + net_buf_simple_add_u8(buf, 0x01); + } + + for (filter_size = 0, i = 0; i < ARRAY_SIZE(client->filter); i++) { + if (client->filter[i] != BT_MESH_ADDR_UNASSIGNED) { + filter_size++; + } + } + + net_buf_simple_add_be16(buf, filter_size); + + BT_DBG("%u bytes: %s", buf->om_len, bt_hex(buf->om_data, buf->om_len)); + + err = bt_mesh_net_encode(&tx, buf, true); + if (err) { + BT_ERR("Encoding Proxy cfg message failed (err %d)", err); + return; + } + + err = proxy_segment_and_send(client->conn_handle, BT_MESH_PROXY_CONFIG, buf); + if (err) { + BT_ERR("Failed to send proxy cfg message (err %d)", err); + } +} + +static void proxy_cfg(struct bt_mesh_proxy_client *client) +{ + struct os_mbuf *buf = NET_BUF_SIMPLE(29); + struct bt_mesh_net_rx rx; + u8_t opcode; + int err; + + err = bt_mesh_net_decode(client->buf, BT_MESH_NET_IF_PROXY_CFG, + &rx, buf, NULL); + if (err) { + BT_ERR("Failed to decode Proxy Configuration (err %d)", err); + goto done; + } + + BT_DBG("%u bytes: %s", buf->om_len, bt_hex(buf->om_data, buf->om_len)); + + if (buf->om_len < 1) { + BT_WARN("Too short proxy configuration PDU"); + goto done; + } + + opcode = net_buf_simple_pull_u8(buf); + switch (opcode) { + case CFG_FILTER_SET: + filter_set(client, buf); + send_filter_status(client, &rx, buf); + break; + case CFG_FILTER_ADD: + while (buf->om_len >= 2) { + u16_t addr; + + addr = net_buf_simple_pull_be16(buf); + filter_add(client, addr); + } + send_filter_status(client, &rx, buf); + break; + case CFG_FILTER_REMOVE: + while (buf->om_len >= 2) { + u16_t addr; + + addr = net_buf_simple_pull_be16(buf); + filter_remove(client, addr); + } + send_filter_status(client, &rx, buf); + break; + default: + BT_WARN("Unhandled configuration OpCode 0x%02x", opcode); + break; + } + +done: + os_mbuf_free_chain(buf); +} + +static int beacon_send(uint16_t conn_handle, struct bt_mesh_subnet *sub) +{ + struct os_mbuf *buf = NET_BUF_SIMPLE(23); + int rc; + + net_buf_simple_init(buf, 1); + bt_mesh_beacon_create(sub, buf); + + rc = proxy_segment_and_send(conn_handle, BT_MESH_PROXY_BEACON, buf); + os_mbuf_free_chain(buf); + return rc; +} + +static void proxy_send_beacons(struct os_event *work) +{ + struct bt_mesh_proxy_client *client; + int i; + + client = work->ev_arg; + + for (i = 0; i < ARRAY_SIZE(bt_mesh.sub); i++) { + struct bt_mesh_subnet *sub = &bt_mesh.sub[i]; + + if (sub->net_idx != BT_MESH_KEY_UNUSED) { + beacon_send(client->conn_handle, sub); + } + } +} + +void bt_mesh_proxy_beacon_send(struct bt_mesh_subnet *sub) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(clients); i++) { + if (clients[i].conn_handle) { + beacon_send(clients[i].conn_handle, sub); + } + } +} + +int bt_mesh_proxy_identity_enable(void) +{ + /* FIXME: Add support for multiple subnets */ + struct bt_mesh_subnet *sub = &bt_mesh.sub[0]; + + BT_DBG(""); + + if (!bt_mesh_is_provisioned()) { + return -EAGAIN; + } + + if (sub->net_idx == BT_MESH_KEY_UNUSED) { + return -ENOENT; + } + + if (sub->node_id == BT_MESH_NODE_IDENTITY_NOT_SUPPORTED) { + return -ENOTSUP; + } + + if (sub->node_id == BT_MESH_NODE_IDENTITY_RUNNING) { + return 0; + } + + sub->node_id = BT_MESH_NODE_IDENTITY_RUNNING; + bt_mesh_adv_update(); + + return 0; +} + +#endif /* GATT_PROXY */ + +static void proxy_complete_pdu(struct bt_mesh_proxy_client *client) +{ + switch (client->msg_type) { +#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY)) + case BT_MESH_PROXY_NET_PDU: + BT_INFO("Mesh Network PDU"); + bt_mesh_net_recv(client->buf, 0, BT_MESH_NET_IF_PROXY); + break; + case BT_MESH_PROXY_BEACON: + BT_INFO("Mesh Beacon PDU"); + bt_mesh_beacon_recv(client->buf); + break; + case BT_MESH_PROXY_CONFIG: + BT_INFO("Mesh Configuration PDU"); + proxy_cfg(client); + break; +#endif +#if (MYNEWT_VAL(BLE_MESH_PB_GATT)) + case BT_MESH_PROXY_PROV: + BT_INFO("Mesh Provisioning PDU"); + bt_mesh_pb_gatt_recv(client->conn_handle, client->buf); + break; +#endif + default: + BT_WARN("Unhandled Message Type 0x%02x", client->msg_type); + break; + } + + net_buf_simple_init(client->buf, 0); +} + +static int proxy_recv(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, void *arg) +{ + struct bt_mesh_proxy_client *client; + const u8_t *data = ctxt->om->om_data; + u16_t len = ctxt->om->om_len; + + client = find_client(conn_handle); + + if (!client) { + return -ENOTCONN; + } + + if (len < 1) { + BT_WARN("Too small Proxy PDU"); + return -EINVAL; + } + + if ((attr_handle == svc_handles.prov_data_in_h) != + (PDU_TYPE(data) == BT_MESH_PROXY_PROV)) { + BT_WARN("Proxy PDU type doesn't match GATT service"); + return -EINVAL; + } + + if (len - 1 > net_buf_simple_tailroom(client->buf)) { + BT_WARN("Too big proxy PDU"); + return -EINVAL; + } + + switch (PDU_SAR(data)) { + case SAR_COMPLETE: + if (client->buf->om_len) { + BT_WARN("Complete PDU while a pending incomplete one"); + return -EINVAL; + } + + client->msg_type = PDU_TYPE(data); + net_buf_simple_add_mem(client->buf, data + 1, len - 1); + proxy_complete_pdu(client); + break; + + case SAR_FIRST: + if (client->buf->om_len) { + BT_WARN("First PDU while a pending incomplete one"); + return -EINVAL; + } + + client->msg_type = PDU_TYPE(data); + net_buf_simple_add_mem(client->buf, data + 1, len - 1); + break; + + case SAR_CONT: + if (!client->buf->om_len) { + BT_WARN("Continuation with no prior data"); + return -EINVAL; + } + + if (client->msg_type != PDU_TYPE(data)) { + BT_WARN("Unexpected message type in continuation"); + return -EINVAL; + } + + net_buf_simple_add_mem(client->buf, data + 1, len - 1); + break; + + case SAR_LAST: + if (!client->buf->om_len) { + BT_WARN("Last SAR PDU with no prior data"); + return -EINVAL; + } + + if (client->msg_type != PDU_TYPE(data)) { + BT_WARN("Unexpected message type in last SAR PDU"); + return -EINVAL; + } + + net_buf_simple_add_mem(client->buf, data + 1, len - 1); + proxy_complete_pdu(client); + break; + } + + return len; +} + +static void proxy_connected(uint16_t conn_handle) +{ + struct bt_mesh_proxy_client *client; + int i; + + BT_INFO("conn_handle %d", conn_handle); + + /* Since we use ADV_OPT_ONE_TIME */ + proxy_adv_enabled = false; + +#if MYNEWT_VAL(BLE_MAX_CONNECTIONS) > 1 + /* Try to re-enable advertising in case it's possible */ + bt_mesh_adv_update(); +#endif + + for (client = NULL, i = 0; i < ARRAY_SIZE(clients); i++) { + if (!clients[i].conn_handle) { + client = &clients[i]; + break; + } + } + + if (!client) { + BT_ERR("No free Proxy Client objects"); + return; + } + + client->conn_handle = conn_handle; + client->filter_type = NONE; + memset(client->filter, 0, sizeof(client->filter)); + net_buf_simple_init(client->buf, 0); +} + +static void proxy_disconnected(uint16_t conn_handle, int reason) +{ + int i; + + BT_INFO("conn_handle %d reason %d", conn_handle, reason); + + for (i = 0; i < ARRAY_SIZE(clients); i++) { + struct bt_mesh_proxy_client *client = &clients[i]; + + if (client->conn_handle == conn_handle) { + if ((MYNEWT_VAL(BLE_MESH_PB_GATT)) && + client->filter_type == PROV) { + bt_mesh_pb_gatt_close(conn_handle); + } + + client->conn_handle = 0; + break; + } + } + + bt_mesh_adv_update(); +} + +struct os_mbuf *bt_mesh_proxy_get_buf(void) +{ + struct os_mbuf *buf = clients[0].buf; + + net_buf_simple_init(buf, 0); + + return buf; +} + +#if (MYNEWT_VAL(BLE_MESH_PB_GATT)) +static void prov_ccc_write(uint16_t conn_handle) +{ + struct bt_mesh_proxy_client *client; + + BT_DBG("conn_handle %d", conn_handle); + + /* If a connection exists there must be a client */ + client = find_client(conn_handle); + __ASSERT(client, "No client for connection"); + + if (client->filter_type == NONE) { + client->filter_type = PROV; + bt_mesh_pb_gatt_open(conn_handle); + } +} + +int bt_mesh_proxy_prov_enable(void) +{ + uint16_t handle; + int rc; + int i; + + BT_DBG(""); + + rc = ble_gatts_find_svc(BLE_UUID16_DECLARE(BT_UUID_MESH_PROV_VAL), &handle); + assert(rc == 0); + ble_gatts_svc_set_visibility(handle, 1); + /* FIXME: figure out end handle */ + ble_svc_gatt_changed(svc_handles.prov_h, 0xffff); + + gatt_svc = MESH_GATT_PROV; + proxy_adv_param = &fast_adv_param; + + for (i = 0; i < ARRAY_SIZE(clients); i++) { + if (clients[i].conn_handle) { + clients[i].filter_type = PROV; + } + } + + + return 0; +} + +int bt_mesh_proxy_prov_disable(void) +{ + uint16_t handle; + int rc; + int i; + + BT_DBG(""); + + rc = ble_gatts_find_svc(BLE_UUID16_DECLARE(BT_UUID_MESH_PROV_VAL), &handle); + assert(rc == 0); + ble_gatts_svc_set_visibility(handle, 0); + /* FIXME: figure out end handle */ + ble_svc_gatt_changed(svc_handles.prov_h, 0xffff); + + gatt_svc = MESH_GATT_NONE; + + for (i = 0; i < ARRAY_SIZE(clients); i++) { + struct bt_mesh_proxy_client *client = &clients[i]; + + if (clients->conn_handle && client->filter_type == PROV) { + bt_mesh_pb_gatt_close(client->conn_handle); + client->filter_type = NONE; + } + } + + return 0; +} +#endif /* MYNEWT_VAL(BLE_MESH_PB_GATT) */ + +#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY)) +static void proxy_ccc_write(uint16_t conn_handle) +{ + struct bt_mesh_proxy_client *client; + + BT_DBG("conn_handle %d", conn_handle); + + client = find_client(conn_handle); + __ASSERT(client, "No client for connection"); + + if (client->filter_type == NONE) { + client->filter_type = WHITELIST; + k_work_add_arg(&client->send_beacons, client); + k_work_submit(&client->send_beacons); + } +} + +int bt_mesh_proxy_gatt_enable(void) +{ + uint16_t handle; + int rc; + int i; + + BT_DBG(""); + + rc = ble_gatts_find_svc(BLE_UUID16_DECLARE(BT_UUID_MESH_PROXY_VAL), &handle); + assert(rc == 0); + ble_gatts_svc_set_visibility(handle, 1); + /* FIXME: figure out end handle */ + ble_svc_gatt_changed(svc_handles.proxy_h, 0xffff); + + gatt_svc = MESH_GATT_PROXY; + + for (i = 0; i < ARRAY_SIZE(clients); i++) { + if (clients[i].conn_handle) { + clients[i].filter_type = WHITELIST; + } + } + + return 0; +} + +int bt_mesh_proxy_gatt_disable(void) +{ + uint16_t handle; + int rc; + int i; + + BT_DBG(""); + + rc = ble_gatts_find_svc(BLE_UUID16_DECLARE(BT_UUID_MESH_PROXY_VAL), &handle); + assert(rc == 0); + ble_gatts_svc_set_visibility(handle, 0); + /* FIXME: figure out end handle */ + ble_svc_gatt_changed(svc_handles.proxy_h, 0xffff); + + gatt_svc = MESH_GATT_NONE; + + for (i = 0; i < ARRAY_SIZE(clients); i++) { + struct bt_mesh_proxy_client *client = &clients[i]; + + if (clients->conn_handle && (client->filter_type == WHITELIST || + client->filter_type == BLACKLIST)) { + client->filter_type = NONE; + } + } + + return 0; +} + +void bt_mesh_proxy_addr_add(struct os_mbuf *buf, u16_t addr) +{ + struct bt_mesh_proxy_client *client = NULL; + int i; + + for (i = 0; i < ARRAY_SIZE(clients); i++) { + client = &clients[i]; + if (client->buf == buf) { + break; + } + } + + assert(client); + + BT_DBG("filter_type %u addr 0x%04x", client->filter_type, addr); + + if (client->filter_type == WHITELIST) { + filter_add(client, addr); + } else if (client->filter_type == BLACKLIST) { + filter_remove(client, addr); + } +} + +static bool client_filter_match(struct bt_mesh_proxy_client *client, + u16_t addr) +{ + int i; + + BT_DBG("filter_type %u addr 0x%04x", client->filter_type, addr); + + if (client->filter_type == WHITELIST) { + for (i = 0; i < ARRAY_SIZE(client->filter); i++) { + if (client->filter[i] == addr) { + return true; + } + } + + return false; + } + + if (client->filter_type == BLACKLIST) { + for (i = 0; i < ARRAY_SIZE(client->filter); i++) { + if (client->filter[i] == addr) { + return false; + } + } + + return true; + } + + return false; +} + +bool bt_mesh_proxy_relay(struct os_mbuf *buf, u16_t dst) +{ + bool relayed = false; + int i; + + BT_DBG("%u bytes to dst 0x%04x", buf->om_len, dst); + + for (i = 0; i < ARRAY_SIZE(clients); i++) { + struct bt_mesh_proxy_client *client = &clients[i]; + struct os_mbuf *msg; + + if (!client->conn_handle) { + continue; + } + + if (!client_filter_match(client, dst)) { + continue; + } + + /* Proxy PDU sending modifies the original buffer, + * so we need to make a copy. + */ + msg = NET_BUF_SIMPLE(32); + net_buf_simple_init(msg, 1); + net_buf_simple_add_mem(msg, buf->om_data, buf->om_len); + + bt_mesh_proxy_send(client->conn_handle, BT_MESH_PROXY_NET_PDU, msg); + os_mbuf_free_chain(msg); + relayed = true; + } + + return relayed; +} + +#endif /* MYNEWT_VAL(BLE_MESH_GATT_PROXY) */ + +static int proxy_send(uint16_t conn_handle, const void *data, u16_t len) +{ + struct os_mbuf *om; + + BT_DBG("%u bytes: %s", len, bt_hex(data, len)); + +#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY)) + if (gatt_svc == MESH_GATT_PROXY) { + om = ble_hs_mbuf_from_flat(data, len); + assert(om); + ble_gattc_notify_custom(conn_handle, svc_handles.proxy_data_out_h, om); + } +#endif + +#if (MYNEWT_VAL(BLE_MESH_PB_GATT)) + if (gatt_svc == MESH_GATT_PROV) { + om = ble_hs_mbuf_from_flat(data, len); + assert(om); + ble_gattc_notify_custom(conn_handle, svc_handles.prov_data_out_h, om); + } +#endif + + return 0; +} + +static int proxy_segment_and_send(uint16_t conn_handle, u8_t type, + struct os_mbuf *msg) +{ + u16_t mtu; + + BT_DBG("conn_handle %d type 0x%02x len %u: %s", conn_handle, type, msg->om_len, + bt_hex(msg->om_data, msg->om_len)); + + /* ATT_MTU - OpCode (1 byte) - Handle (2 bytes) */ + mtu = ble_att_mtu(conn_handle) - 3; + if (mtu > msg->om_len) { + net_buf_simple_push_u8(msg, PDU_HDR(SAR_COMPLETE, type)); + return proxy_send(conn_handle, msg->om_data, msg->om_len); + } + + net_buf_simple_push_u8(msg, PDU_HDR(SAR_FIRST, type)); + proxy_send(conn_handle, msg->om_data, mtu); + net_buf_simple_pull(msg, mtu); + + while (msg->om_len) { + if (msg->om_len + 1 < mtu) { + net_buf_simple_push_u8(msg, PDU_HDR(SAR_LAST, type)); + proxy_send(conn_handle, msg->om_data, msg->om_len); + break; + } + + net_buf_simple_push_u8(msg, PDU_HDR(SAR_CONT, type)); + proxy_send(conn_handle, msg->om_data, mtu); + net_buf_simple_pull(msg, mtu); + } + + return 0; +} + +int bt_mesh_proxy_send(uint16_t conn_handle, u8_t type, + struct os_mbuf *msg) +{ + struct bt_mesh_proxy_client *client = find_client(conn_handle); + + if (!client) { + BT_ERR("No Proxy Client found"); + return -ENOTCONN; + } + + if ((client->filter_type == PROV) != (type == BT_MESH_PROXY_PROV)) { + BT_ERR("Invalid PDU type for Proxy Client"); + return -EINVAL; + } + + return proxy_segment_and_send(conn_handle, type, msg); +} + +#if (MYNEWT_VAL(BLE_MESH_PB_GATT)) +static u8_t prov_svc_data[20] = { 0x27, 0x18, }; + +static const struct bt_data prov_ad[] = { + BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)), + BT_DATA_BYTES(BT_DATA_UUID16_ALL, 0x27, 0x18), + BT_DATA(BT_DATA_SVC_DATA16, prov_svc_data, sizeof(prov_svc_data)), +}; + +static const struct bt_data prov_sd[] = { + BT_DATA(BT_DATA_NAME_COMPLETE, "mynewtproxy", + (sizeof("mynewtproxy") - 1)), +}; +#endif /* PB_GATT */ + +#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY)) +static s64_t node_id_start; + +#define ID_TYPE_NET 0x00 +#define ID_TYPE_NODE 0x01 + +#define NODE_ID_LEN 19 +#define NET_ID_LEN 11 + +static u8_t proxy_svc_data[NODE_ID_LEN] = { 0x28, 0x18, }; + +static const struct bt_data node_id_ad[] = { + BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)), + BT_DATA_BYTES(BT_DATA_UUID16_ALL, 0x28, 0x18), + BT_DATA(BT_DATA_SVC_DATA16, proxy_svc_data, NODE_ID_LEN), +}; + +static const struct bt_data net_id_ad[] = { + BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)), + BT_DATA_BYTES(BT_DATA_UUID16_ALL, 0x28, 0x18), + BT_DATA(BT_DATA_SVC_DATA16, proxy_svc_data, NET_ID_LEN), +}; + +static int node_id_adv(struct bt_mesh_subnet *sub) +{ + u8_t tmp[16]; + int err; + + BT_DBG(""); + + proxy_svc_data[2] = ID_TYPE_NODE; + + err = bt_rand(proxy_svc_data + 11, 8); + if (err) { + return err; + } + + memset(tmp, 0, 6); + memcpy(tmp + 6, proxy_svc_data + 11, 8); + sys_put_be16(bt_mesh_primary_addr(), tmp + 14); + + err = bt_encrypt_be(sub->keys[sub->kr_flag].identity, tmp, tmp); + if (err) { + return err; + } + + memcpy(proxy_svc_data + 3, tmp + 8, 8); + + err = bt_le_adv_start(proxy_adv_param, node_id_ad, + ARRAY_SIZE(node_id_ad), NULL, 0); + if (err) { + BT_ERR("Failed to advertise using Node ID (err %d)", err); + return err; + } + + proxy_adv_enabled = true; + + return 0; +} + +static int net_id_adv(struct bt_mesh_subnet *sub) +{ + int err; + + BT_DBG(""); + + proxy_svc_data[2] = ID_TYPE_NET; + + BT_DBG("Advertising with NetId %s", + bt_hex(sub->keys[sub->kr_flag].net_id, 8)); + + memcpy(proxy_svc_data + 3, sub->keys[sub->kr_flag].net_id, 8); + + err = bt_le_adv_start(proxy_adv_param, + net_id_ad, ARRAY_SIZE(net_id_ad), NULL, 0); + if (err) { + BT_ERR("Failed to advertise using Network ID (err %d)", err); + return err; + } + + proxy_adv_enabled = true; + + return 0; +} + +static s32_t gatt_proxy_advertise(void) +{ + /* TODO: Add support for multiple subnets */ + struct bt_mesh_subnet *sub = &bt_mesh.sub[0]; + s32_t remaining = K_FOREVER; + + BT_DBG(""); + + if (sub->net_idx == BT_MESH_KEY_UNUSED) { + BT_WARN("First subnet is not valid"); + return remaining; + } + + if (node_id_start) { + s64_t active = k_uptime_get() - node_id_start; + + BT_DBG("Node Id active for %d ms", active); + + if (active < K_SECONDS(60)) { + remaining = K_SECONDS(60) - active; + } else { + sub->node_id = BT_MESH_NODE_IDENTITY_STOPPED; + node_id_start = 0; + } + } + + if (sub->node_id == BT_MESH_NODE_IDENTITY_RUNNING) { + proxy_adv_param = &fast_adv_param; + if (node_id_adv(sub) == 0 && !node_id_start) { + node_id_start = k_uptime_get(); + remaining = K_SECONDS(60); + } + } else if (bt_mesh_gatt_proxy_get() == BT_MESH_GATT_PROXY_ENABLED) { + proxy_adv_param = &slow_adv_param; + net_id_adv(sub); + } + + return remaining; +} +#endif /* GATT_PROXY */ + +s32_t bt_mesh_proxy_adv_start(void) +{ + BT_DBG(""); + +#if (MYNEWT_VAL(BLE_MESH_PB_GATT)) + if (!bt_mesh_is_provisioned()) { + if (bt_le_adv_start(proxy_adv_param, + prov_ad, ARRAY_SIZE(prov_ad), + prov_sd, ARRAY_SIZE(prov_sd)) == 0) { + proxy_adv_enabled = true; + if (proxy_adv_param == &fast_adv_param) { + proxy_adv_param = &slow_adv_param; + return K_SECONDS(60); + } + } + } +#endif /* PB_GATT */ + +#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY)) + if (bt_mesh_is_provisioned()) { + return gatt_proxy_advertise(); + } +#endif /* GATT_PROXY */ + + return K_FOREVER; +} + +void bt_mesh_proxy_adv_stop(void) +{ + int err; + + BT_DBG("adv_enabled %u", proxy_adv_enabled); + + if (!proxy_adv_enabled) { + return; + } + + err = bt_le_adv_stop(); + if (err) { + BT_ERR("Failed to stop advertising (err %d)", err); + } else { + proxy_adv_enabled = false; + } +} + +int +ble_mesh_proxy_gap_event(struct ble_gap_event *event, void *arg) +{ +// BT_DBG("event %d", event->type); + + if (event->type == BLE_GAP_EVENT_CONNECT) { + proxy_connected(event->connect.conn_handle); + } else if (event->type == BLE_GAP_EVENT_DISCONNECT) { + proxy_disconnected(event->disconnect.conn.conn_handle, + event->disconnect.reason); + } else if (event->type == BLE_GAP_EVENT_SUBSCRIBE) { + if (event->subscribe.attr_handle == svc_handles.proxy_data_out_h) { +#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY)) + proxy_ccc_write(event->subscribe.conn_handle); +#endif + } else if (event->subscribe.attr_handle == + svc_handles.prov_data_out_h) { +#if (MYNEWT_VAL(BLE_MESH_PB_GATT)) + prov_ccc_write(event->subscribe.conn_handle); +#endif + } + } + + return 0; +} + +static int +dummy_access_cb(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, void *arg) +{ + /* + * We should never never enter this callback - it's attached to notify-only + * characteristic which are notified directly from mbuf. And we can't pass + * NULL as access_cb because gatts will assert on init... + */ + BLE_HS_DBG_ASSERT(0); + return 0; +} + +static const struct ble_gatt_svc_def svc_defs [] = { + { + .type = BLE_GATT_SVC_TYPE_PRIMARY, + .uuid = BLE_UUID16_DECLARE(BT_UUID_MESH_PROXY_VAL), + .characteristics = (struct ble_gatt_chr_def[]) { { + .uuid = BLE_UUID16_DECLARE(BT_UUID_MESH_PROXY_DATA_IN_VAL), + .access_cb = proxy_recv, + .flags = BLE_GATT_CHR_F_WRITE_NO_RSP, + }, { + .uuid = BLE_UUID16_DECLARE(BT_UUID_MESH_PROXY_DATA_OUT_VAL), + .access_cb = dummy_access_cb, + .flags = BLE_GATT_CHR_F_NOTIFY, + }, { + 0, /* No more characteristics in this service. */ + } }, + }, { + .type = BLE_GATT_SVC_TYPE_PRIMARY, + .uuid = BLE_UUID16_DECLARE(BT_UUID_MESH_PROV_VAL), + .characteristics = (struct ble_gatt_chr_def[]) { { + .uuid = BLE_UUID16_DECLARE(BT_UUID_MESH_PROV_DATA_IN_VAL), + .access_cb = proxy_recv, + .flags = BLE_GATT_CHR_F_WRITE_NO_RSP, + }, { + .uuid = BLE_UUID16_DECLARE(BT_UUID_MESH_PROV_DATA_OUT_VAL), + .access_cb = dummy_access_cb, + .flags = BLE_GATT_CHR_F_NOTIFY, + }, { + 0, /* No more characteristics in this service. */ + } }, + }, { + 0, /* No more services. */ + }, +}; + +int bt_mesh_proxy_svcs_register(void) +{ + int rc; + + rc = ble_gatts_count_cfg(svc_defs); + assert(rc == 0); + + rc = ble_gatts_add_svcs(svc_defs); + assert(rc == 0); + + return 0; +} + +int bt_mesh_proxy_init(void) +{ + int i; + + for (i = 0; i < MYNEWT_VAL(BLE_MAX_CONNECTIONS); ++i) { +#if (MYNEWT_VAL(BLE_MESH_GATT_PROXY)) + k_work_init(&clients[i].send_beacons, proxy_send_beacons); +#endif + clients[i].buf = NET_BUF_SIMPLE(CLIENT_BUF_SIZE); + } + +#if (MYNEWT_VAL(BLE_MESH_PB_GATT)) + memcpy(prov_svc_data + 2, bt_mesh_prov_get_uuid(), 16); +#endif + + resolve_svc_handles(); + + ble_gatts_svc_set_visibility(svc_handles.proxy_h, 0); + ble_gatts_svc_set_visibility(svc_handles.prov_h, 0); + + return 0; +} + +#endif //MYNEWT_VAL(BLE_MESH_PROXY) diff --git a/nimble/host/mesh/src/proxy.h b/nimble/host/mesh/src/proxy.h new file mode 100644 index 000000000..655139147 --- /dev/null +++ b/nimble/host/mesh/src/proxy.h @@ -0,0 +1,41 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef __PROXY_H__ +#define __PROXY_H__ + +#define BT_MESH_PROXY_NET_PDU 0x00 +#define BT_MESH_PROXY_BEACON 0x01 +#define BT_MESH_PROXY_CONFIG 0x02 +#define BT_MESH_PROXY_PROV 0x03 + +#include "mesh/mesh.h" + +int bt_mesh_proxy_send(uint16_t conn_handle, u8_t type, struct os_mbuf *msg); + +int bt_mesh_proxy_prov_enable(void); +int bt_mesh_proxy_prov_disable(void); + +int bt_mesh_proxy_gatt_enable(void); +int bt_mesh_proxy_gatt_disable(void); + +void bt_mesh_proxy_beacon_send(struct bt_mesh_subnet *sub); + +struct os_mbuf * bt_mesh_proxy_get_buf(void); + +s32_t bt_mesh_proxy_adv_start(void); +void bt_mesh_proxy_adv_stop(void); + +bool bt_mesh_proxy_relay(struct os_mbuf *buf, u16_t dst); +void bt_mesh_proxy_addr_add(struct os_mbuf *buf, u16_t addr); + +int bt_mesh_proxy_init(void); + +int ble_mesh_proxy_gap_event(struct ble_gap_event *event, void *arg); + +#endif diff --git a/nimble/host/mesh/src/transport.c b/nimble/host/mesh/src/transport.c new file mode 100644 index 000000000..98fbecddd --- /dev/null +++ b/nimble/host/mesh/src/transport.c @@ -0,0 +1,1135 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + + +#include +#include + +#include "mesh/mesh.h" +#include "mesh_priv.h" + +#define BT_DBG_ENABLED (MYNEWT_VAL(BLE_MESH_DEBUG_TRANS)) +#include "host/ble_hs_log.h" + +#include "crypto.h" +#include "adv.h" +#include "net.h" +#include "lpn.h" +#include "friend.h" +#include "access.h" +#include "foundation.h" +#include "transport.h" + +#define AID_MASK ((u8_t)(BIT_MASK(6))) +#define OP_MASK ((u8_t)(BIT_MASK(7))) + +#define SEG(data) ((data)[0] >> 7) +#define AKF(data) (((data)[0] >> 6) & 0x01) +#define AID(data) ((data)[0] & AID_MASK) +#define MIC_SIZE(data) (((data)[1] & 0x80) ? 8 : 4) + +#define CTL_OP(data) ((data)[0] & OP_MASK) +#define CTL_HDR(op, seg) ((op & OP_MASK) | (seg << 7)) + +#define SZMIC(mic_len) (mic_len == 8 ? 1 : 0) + +#define UNSEG_HDR(akf, aid) ((akf << 6) | (aid & AID_MASK)) +#define SEG_HDR(akf, aid) (UNSEG_HDR(akf, aid) | 0x80) + +#define BLOCK_COMPLETE(seg_n) (u32_t)(((u64_t)1 << (seg_n + 1)) - 1) + +#define SEQ_AUTH(iv_index, seq) (((u64_t)iv_index) << 24 | (u64_t)seq) + +/* Retransmit timeout after which to retransmit unacked segments */ +#define SEG_RETRANSMIT_TIMEOUT K_MSEC(400) + +/* Number of retransmit attempts (after the initial transmit) per segment */ +#define SEG_RETRANSMIT_ATTEMPTS 4 + +/* How long to wait for available buffers before giving up */ +#define BUF_TIMEOUT K_NO_WAIT + +static struct seg_tx { + struct bt_mesh_subnet *sub; + struct os_mbuf *seg[BT_MESH_TX_SEG_COUNT]; + u64_t seq_auth; + u8_t seg_n; /* Last segment index */ + u8_t nack_count; /* Number of not acked segments */ + bt_mesh_cb_t cb; + void *cb_data; + struct k_delayed_work retransmit; /* Retransmit timer */ +} seg_tx[MYNEWT_VAL(BLE_MESH_TX_SEG_MSG_COUNT)]; + +static struct seg_rx { + struct bt_mesh_subnet *sub; + u64_t seq_auth; + u8_t seg_n:5, + ctl:1, + in_use:1; + u8_t hdr; + u8_t ttl; + u16_t src; + u16_t dst; + u32_t block; + u32_t last; + struct k_delayed_work ack; + struct os_mbuf *buf; +} seg_rx[MYNEWT_VAL(BLE_MESH_RX_SEG_MSG_COUNT)] = { + [0 ... (MYNEWT_VAL(BLE_MESH_RX_SEG_MSG_COUNT) - 1)] = { 0 }, +}; + +static int send_unseg(struct bt_mesh_net_tx *tx, u8_t aid, + struct os_mbuf *sdu) +{ + struct os_mbuf *buf; + u8_t xmit; + + BT_DBG("src 0x%04x dst 0x%04x app_idx 0x%04x sdu_len %u", + tx->src, tx->ctx->addr, tx->ctx->app_idx, sdu->om_len); + + xmit = bt_mesh_net_transmit_get(); + buf = bt_mesh_adv_create(BT_MESH_ADV_DATA, TRANSMIT_COUNT(xmit), + TRANSMIT_INT(xmit), BUF_TIMEOUT); + if (!buf) { + BT_ERR("Out of network buffers"); + return -ENOBUFS; + } + + net_buf_reserve(buf, 9); + + if (tx->ctx->app_idx == BT_MESH_KEY_DEV) { + net_buf_simple_add_u8(buf, UNSEG_HDR(0, 0)); + } else { + net_buf_simple_add_u8(buf, UNSEG_HDR(1, aid)); + } + + net_buf_add_mem(buf, sdu->om_data, sdu->om_len); + + return bt_mesh_net_send(tx, buf, NULL); +} + +bool bt_mesh_tx_in_progress(void) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(seg_tx); i++) { + if (seg_tx[i].nack_count) { + return true; + } + } + + return false; +} + +static void seg_tx_reset(struct seg_tx *tx) +{ + int i; + + k_delayed_work_cancel(&tx->retransmit); + + tx->cb = NULL; + tx->cb_data = NULL; + tx->seq_auth = 0; + tx->sub = NULL; + + if (!tx->nack_count) { + return; + } + + for (i = 0; i <= tx->seg_n; i++) { + if (!tx->seg[i]) { + continue; + } + + net_buf_unref(tx->seg[i]); + tx->seg[i] = NULL; + } + + tx->nack_count = 0; + + if (bt_mesh.pending_update) { + BT_DBG("Proceding with pending IV Update"); + bt_mesh.pending_update = 0; + /* bt_mesh_iv_update() will re-enable the flag if this + * wasn't the only transfer. + */ + bt_mesh_iv_update(bt_mesh.iv_index, false); + } +} + +static inline void seg_tx_complete(struct seg_tx *tx, int err) +{ + if (tx->cb) { + tx->cb(err, tx->cb_data); + } + + seg_tx_reset(tx); +} + +static void seg_sent(struct os_mbuf *buf, int err) +{ + struct seg_tx *tx = &seg_tx[BT_MESH_ADV(buf)->seg.tx_id]; + + k_delayed_work_submit(&tx->retransmit, SEG_RETRANSMIT_TIMEOUT); +} + +static void seg_tx_send_unacked(struct seg_tx *tx) +{ + int i, err; + + for (i = 0; i <= tx->seg_n; i++) { + struct os_mbuf *seg = tx->seg[i]; + + if (!seg) { + continue; + } + + if (BT_MESH_ADV(seg)->busy) { + BT_DBG("Skipping segment that's still advertising"); + continue; + } + + if (!(BT_MESH_ADV(seg)->seg.attempts--)) { + BT_ERR("Ran out of retransmit attempts"); + seg_tx_complete(tx, -ETIMEDOUT); + return; + } + + BT_DBG("resending %u/%u", i, tx->seg_n); + + err = bt_mesh_net_resend(tx->sub, seg, + BT_MESH_ADV(seg)->seg.new_key, + BT_MESH_ADV(seg)->seg.friend_cred, + seg_sent); + if (err) { + BT_ERR("Sending segment failed"); + seg_tx_complete(tx, -EIO); + return; + } + } +} + +static void seg_retransmit(struct os_event *work) +{ + struct seg_tx *tx = work->ev_arg; + seg_tx_send_unacked(tx); +} + +static int send_seg(struct bt_mesh_net_tx *net_tx, u8_t aid, + u8_t mic_len, struct os_mbuf *sdu, + bt_mesh_cb_t cb, void *cb_data) +{ + u8_t seg_hdr, seg_o; + u16_t seq_zero; + struct seg_tx *tx; + int i; + + BT_DBG("src 0x%04x dst 0x%04x app_idx 0x%04x mic_len %u sdu_len %u", + net_tx->src, net_tx->ctx->addr, net_tx->ctx->app_idx, mic_len, + sdu->om_len); + + if (sdu->om_len < 1) { + BT_ERR("Zero-length SDU not allowed"); + return -EINVAL; + } + + if (sdu->om_len > BT_MESH_TX_SDU_MAX) { + BT_ERR("Not enough segment buffers for length %u", sdu->om_len); + return -EMSGSIZE; + } + + for (tx = NULL, i = 0; i < ARRAY_SIZE(seg_tx); i++) { + if (!seg_tx[i].nack_count) { + tx = &seg_tx[i]; + break; + } + } + + if (!tx) { + BT_ERR("No multi-segment message contexts available"); + return -EBUSY; + } + + if (net_tx->ctx->app_idx == BT_MESH_KEY_DEV) { + seg_hdr = SEG_HDR(0, 0); + } else { + seg_hdr = SEG_HDR(1, aid); + } + + seg_o = 0; + tx->seg_n = (sdu->om_len - 1) / 12; + tx->nack_count = tx->seg_n + 1; + tx->seq_auth = SEQ_AUTH(BT_MESH_NET_IVI_TX, bt_mesh.seq); + tx->sub = net_tx->sub; + + seq_zero = tx->seq_auth & 0x1fff; + + BT_DBG("SeqZero 0x%04x", seq_zero); + + for (seg_o = 0; sdu->om_len; seg_o++) { + struct os_mbuf *seg; + u16_t len; + u8_t xmit; + int err; + + xmit = bt_mesh_net_transmit_get(); + seg = bt_mesh_adv_create(BT_MESH_ADV_DATA, TRANSMIT_COUNT(xmit), + TRANSMIT_INT(xmit), BUF_TIMEOUT); + if (!seg) { + BT_ERR("Out of segment buffers"); + seg_tx_reset(tx); + return -ENOBUFS; + } + + BT_MESH_ADV(seg)->seg.tx_id = tx - seg_tx; + BT_MESH_ADV(seg)->seg.attempts = SEG_RETRANSMIT_ATTEMPTS; + BT_MESH_ADV(seg)->seg.new_key = net_tx->sub->kr_flag; + BT_MESH_ADV(seg)->seg.friend_cred = net_tx->ctx->friend_cred; + + net_buf_reserve(seg, 9); + + net_buf_simple_add_u8(seg, seg_hdr); + net_buf_simple_add_u8(seg, (SZMIC(mic_len) << 7) | seq_zero >> 6); + net_buf_simple_add_u8(seg, (((seq_zero & 0x3f) << 2) | + (seg_o >> 3))); + net_buf_simple_add_u8(seg, ((seg_o & 0x07) << 5) | tx->seg_n); + + len = min(sdu->om_len, 12); + net_buf_add_mem(seg, sdu->om_data, len); + net_buf_simple_pull(sdu, len); + + tx->seg[seg_o] = net_buf_ref(seg); + + BT_DBG("Sending %u/%u", seg_o, tx->seg_n); + + err = bt_mesh_net_send(net_tx, seg, seg_sent); + if (err) { + BT_ERR("Sending segment failed"); + seg_tx_reset(tx); + return err; + } + } + + if (bt_mesh_lpn_established()) { + bt_mesh_lpn_friend_poll(); + } + + return 0; +} + +struct bt_mesh_app_key *bt_mesh_app_key_find(u16_t app_idx) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(bt_mesh.app_keys); i++) { + struct bt_mesh_app_key *key = &bt_mesh.app_keys[i]; + + if (key->net_idx != BT_MESH_KEY_UNUSED && + key->app_idx == app_idx) { + return key; + } + } + + return NULL; +} + +int bt_mesh_trans_send(struct bt_mesh_net_tx *tx, struct os_mbuf *msg, + bt_mesh_cb_t cb, void *cb_data) +{ + bool seg = (msg->om_len > 11 || cb); + const u8_t *key; + u8_t mic_len, aid, aszmic; + u8_t *ad; + int err; + + BT_DBG("net_idx 0x%04x app_idx 0x%04x dst 0x%04x", tx->sub->net_idx, + tx->ctx->app_idx, tx->ctx->addr); + BT_DBG("len %u: %s", msg->om_len, bt_hex(msg->om_data, msg->om_len)); + + if (tx->ctx->app_idx == BT_MESH_KEY_DEV) { + key = bt_mesh.dev_key; + aid = 0; + } else { + struct bt_mesh_app_key *app_key; + + app_key = bt_mesh_app_key_find(tx->ctx->app_idx); + if (!app_key) { + return -EINVAL; + } + + if (tx->sub->kr_phase == BT_MESH_KR_PHASE_2 && + app_key->updated) { + key = app_key->keys[1].val; + aid = app_key->keys[1].id; + } else { + key = app_key->keys[0].val; + aid = app_key->keys[0].id; + } + } + + if (!seg || net_buf_simple_tailroom(msg) < 8) { + mic_len = 4; + aszmic = 0; + } else { + mic_len = 8; + aszmic = 1; + } + + if (BT_MESH_ADDR_IS_VIRTUAL(tx->ctx->addr)) { + ad = bt_mesh_label_uuid_get(tx->ctx->addr); + } else { + ad = NULL; + } + + err = bt_mesh_app_encrypt(key, tx->ctx->app_idx == BT_MESH_KEY_DEV, + aszmic, msg, ad, mic_len, tx->src, + tx->ctx->addr, bt_mesh.seq, + BT_MESH_NET_IVI_TX); + if (err) { + return err; + } + + if (seg) { + return send_seg(tx, aid, mic_len, msg, cb, cb_data); + } + + return send_unseg(tx, aid, msg); +} + +static bool is_replay(struct bt_mesh_net_rx *rx) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(bt_mesh.rpl); i++) { + struct bt_mesh_rpl *rpl = &bt_mesh.rpl[i]; + + /* Empty slot */ + if (!rpl->src) { + rpl->src = rx->ctx.addr; + rpl->seq = rx->seq; + rpl->old_iv = rx->old_iv; + return false; + } + + /* Existing slot for given address */ + if (rpl->src == rx->ctx.addr) { + if (rx->old_iv && !rpl->old_iv) { + return true; + } + + if (rpl->seq < rx->seq) { + rpl->seq = rx->seq; + rpl->old_iv = rx->old_iv; + return false; + } else { + return true; + } + } + } + + BT_ERR("RPL is full!"); + return true; +} + +static int sdu_recv(struct bt_mesh_net_rx *rx, u8_t hdr, u8_t mic_size, + u8_t aszmic, struct os_mbuf *buf) +{ + struct os_mbuf *sdu = + NET_BUF_SIMPLE(MYNEWT_VAL(BLE_MESH_RX_SDU_MAX) - 4); + u8_t *ad; + u16_t i; + int err = 0; + + BT_DBG("MIC_SIZE %u ASZMIC %u AKF %u AID 0x%02x", + mic_size, aszmic, AKF(&hdr), AID(&hdr)); + BT_DBG("len %u: %s", buf->om_len, bt_hex(buf->om_data, buf->om_len)); + + if (buf->om_len < 1 + mic_size) { + BT_ERR("Too short SDU + MIC"); + err = -EINVAL; + goto done; + } + + if (BT_MESH_ADDR_IS_VIRTUAL(rx->dst)) { + ad = bt_mesh_label_uuid_get(rx->dst); + } else { + ad = NULL; + } + + /* Adjust the length to not contain the MIC at the end */ + buf->om_len -= mic_size; + + if (!AKF(&hdr)) { + net_buf_simple_init(sdu, 0); + err = bt_mesh_app_decrypt(bt_mesh.dev_key, true, aszmic, buf, + mic_size, sdu, ad, rx->ctx.addr, + rx->dst, rx->seq, + BT_MESH_NET_IVI_RX(rx)); + if (err) { + BT_ERR("Unable to decrypt with DevKey"); + err = -EINVAL; + goto done; + } + + rx->ctx.app_idx = BT_MESH_KEY_DEV; + bt_mesh_model_recv(rx, sdu); + goto done; + } + + for (i = 0; i < ARRAY_SIZE(bt_mesh.app_keys); i++) { + struct bt_mesh_app_key *key = &bt_mesh.app_keys[i]; + struct bt_mesh_app_keys *keys; + + /* Check that this AppKey matches received net_idx */ + if (key->net_idx != rx->sub->net_idx) { + continue; + } + + if (rx->new_key && key->updated) { + keys = &key->keys[1]; + } else { + keys = &key->keys[0]; + } + + /* Check that the AppKey ID matches */ + if (AID(&hdr) != keys->id) { + continue; + } + + net_buf_simple_init(sdu, 0); + err = bt_mesh_app_decrypt(keys->val, false, aszmic, buf, + mic_size, sdu, ad, rx->ctx.addr, + rx->dst, rx->seq, + BT_MESH_NET_IVI_RX(rx)); + if (err) { + BT_WARN("Unable to decrypt with AppKey %u", i); + continue; + + } + + rx->ctx.app_idx = key->app_idx; + + if (is_replay(rx)) { + BT_WARN("Replay: src 0x%04x dst 0x%04x seq 0x%06x", + rx->ctx.addr, rx->dst, rx->seq); + err = -EINVAL; + goto done; + } + + bt_mesh_model_recv(rx, sdu); + } + + BT_WARN("No matching AppKey"); + + err = -EINVAL; +done: + os_mbuf_free_chain(sdu); + return err; +} + +static int trans_ack(struct bt_mesh_net_rx *rx, u8_t hdr, + struct os_mbuf *buf) +{ + struct seg_tx *tx; + unsigned int bit; + u32_t ack; + u16_t seq_zero; + u8_t obo; + int i; + + if (buf->om_len < 6) { + BT_ERR("Too short ack message"); + return -EINVAL; + } + + seq_zero = net_buf_simple_pull_be16(buf); + obo = seq_zero >> 15; + seq_zero = (seq_zero >> 2) & 0x1fff; + + ack = net_buf_simple_pull_be32(buf); + + BT_DBG("OBO %u seq_zero 0x%04x ack 0x%08x", obo, seq_zero, ack); + + for (tx = NULL, i = 0; i < ARRAY_SIZE(seg_tx); i++) { + if ((seg_tx[i].seq_auth & 0x1fff) == seq_zero) { + tx = &seg_tx[i]; + break; + } + } + + if (!tx) { + BT_WARN("Unknown SeqZero for ack"); + return -EINVAL; + } + + if (!ack) { + BT_WARN("SDU canceled"); + seg_tx_complete(tx, -ECANCELED); + return 0; + } + + if (find_msb_set(ack) - 1 > tx->seg_n) { + BT_ERR("Too large segment number in ack"); + return -EINVAL; + } + + k_delayed_work_cancel(&tx->retransmit); + + while ((bit = find_lsb_set(ack))) { + if (tx->seg[bit - 1]) { + BT_DBG("seg %u/%u acked", bit - 1, tx->seg_n); + net_buf_unref(tx->seg[bit - 1]); + tx->seg[bit - 1] = NULL; + tx->nack_count--; + } + + ack &= ~BIT(bit - 1); + } + + if (tx->nack_count) { + seg_tx_send_unacked(tx); + } else { + BT_DBG("SDU TX complete"); + seg_tx_complete(tx, 0); + } + + return 0; +} + +static int trans_heartbeat(struct bt_mesh_net_rx *rx, + struct os_mbuf *buf) +{ + u8_t init_ttl, hops; + u16_t feat; + + if (buf->om_len < 3) { + BT_ERR("Too short heartbeat message"); + return -EINVAL; + } + + init_ttl = (net_buf_simple_pull_u8(buf) & 0x7f); + feat = net_buf_simple_pull_be16(buf); + + hops = (init_ttl - rx->ctx.recv_ttl + 1); + + BT_DBG("src 0x%04x TTL %u InitTTL %u (%u hop%s) feat 0x%04x", + rx->ctx.addr, rx->ctx.recv_ttl, init_ttl, hops, + (hops == 1) ? "" : "s", feat); + + bt_mesh_heartbeat(rx->ctx.addr, rx->dst, hops, feat); + + return 0; +} + +static int ctl_recv(struct bt_mesh_net_rx *rx, u8_t hdr, + struct os_mbuf *buf) +{ + u8_t ctl_op = CTL_OP(&hdr); + + BT_DBG("OpCode 0x%02x len %u", ctl_op, buf->om_len); + + switch (ctl_op) { + case TRANS_CTL_OP_ACK: + return trans_ack(rx, hdr, buf); + case TRANS_CTL_OP_HEARTBEAT: + return trans_heartbeat(rx, buf); + } + +#if (MYNEWT_VAL(BLE_MESH_FRIEND)) + switch (ctl_op) { + case TRANS_CTL_OP_FRIEND_POLL: + return bt_mesh_friend_poll(rx, buf); + case TRANS_CTL_OP_FRIEND_REQ: + return bt_mesh_friend_req(rx, buf); + } +#endif + +#if (MYNEWT_VAL(BLE_MESH_LOW_POWER)) + if (ctl_op == TRANS_CTL_OP_FRIEND_OFFER) { + return bt_mesh_lpn_friend_offer(rx, buf); + } + + if (rx->ctx.addr == bt_mesh.lpn.frnd) { + if (ctl_op == TRANS_CTL_OP_FRIEND_CLEAR_CFM) { + return bt_mesh_lpn_friend_clear_cfm(rx, buf); + } + + if (!rx->ctx.friend_cred) { + BT_WARN("Message from friend with wrong credentials"); + return -EINVAL; + } + + switch (ctl_op) { + case TRANS_CTL_OP_FRIEND_UPDATE: + return bt_mesh_lpn_friend_update(rx, buf); + case TRANS_CTL_OP_FRIEND_SUB_CFM: + return bt_mesh_lpn_friend_sub_cfm(rx, buf); + } + } +#endif /* MYNEWT_VAL(BLE_MESH_LOW_POWER) */ + + BT_WARN("Unhandled TransOpCode 0x%02x", ctl_op); + + return -ENOENT; +} + +static int trans_unseg(struct os_mbuf *buf, struct bt_mesh_net_rx *rx) +{ + u8_t hdr; + + BT_DBG("AFK %u AID 0x%02x", AKF(buf->om_data), AID(buf->om_data)); + + if (buf->om_len < 1) { + BT_ERR("Too small unsegmented PDU"); + return -EINVAL; + } + + hdr = net_buf_simple_pull_u8(buf); + + if (rx->ctl) { + return ctl_recv(rx, hdr, buf); + } else { + return sdu_recv(rx, hdr, 4, 0, buf); + } +} + +static inline s32_t ack_timeout(struct seg_rx *rx) +{ + s32_t to; + + /* 100 ms for every not yet received segment */ + to = K_MSEC(((rx->seg_n + 1) - popcount(rx->block)) * 100); + + /* Make sure we don't send more frequently than the duration for + * each packet (default is 300ms). + */ + return max(to, K_MSEC(400)); +} + +int bt_mesh_ctl_send(struct bt_mesh_net_tx *tx, u8_t ctl_op, void *data, + size_t data_len, bt_mesh_adv_func_t cb) +{ + struct os_mbuf *buf; + u8_t xmit; + + BT_DBG("src 0x%04x dst 0x%04x ttl 0x%02x ctl 0x%02x", tx->src, + tx->ctx->addr, tx->ctx->send_ttl, ctl_op); + BT_DBG("len %zu: %s", data_len, bt_hex(data, data_len)); + + xmit = bt_mesh_net_transmit_get(); + buf = bt_mesh_adv_create(BT_MESH_ADV_DATA, TRANSMIT_COUNT(xmit), + TRANSMIT_INT(xmit), BUF_TIMEOUT); + if (!buf) { + BT_ERR("Out of transport buffers"); + return -ENOBUFS; + } + + /* Reserve space for Network headers */ + net_buf_reserve(buf, 9); + + net_buf_simple_add_u8(buf, CTL_HDR(ctl_op, 0)); + + net_buf_add_mem(buf, data, data_len); + + return bt_mesh_net_send(tx, buf, cb); +} + +static int send_ack(struct bt_mesh_subnet *sub, u16_t src, u16_t dst, + u8_t ttl, u16_t seq_zero, u32_t block) +{ + struct bt_mesh_msg_ctx ctx = { + .net_idx = sub->net_idx, + .app_idx = BT_MESH_KEY_UNUSED, + .addr = dst, + .send_ttl = ttl, + }; + struct bt_mesh_net_tx tx = { + .sub = sub, + .ctx = &ctx, + .src = src, + }; + u8_t buf[6]; + + BT_DBG("SeqZero 0x%04x Block 0x%08x", seq_zero, block); + + if (bt_mesh_lpn_established()) { + BT_WARN("Not sending ack when LPN is enabled"); + return 0; + } + + /* This can happen if the segmented message was destined for a group + * or virtual address. + */ + if (!BT_MESH_ADDR_IS_UNICAST(src)) { + BT_WARN("Not sending ack for non-unicast address"); + return 0; + } + + sys_put_be16(((seq_zero << 2) & 0x7ffc), buf); + sys_put_be32(block, &buf[2]); + + return bt_mesh_ctl_send(&tx, TRANS_CTL_OP_ACK, buf, sizeof(buf), NULL); +} + +static void seg_rx_reset(struct seg_rx *rx) +{ + BT_DBG("rx %p", rx); + + k_delayed_work_cancel(&rx->ack); + + /* We don't reset rx->net and rx->seq_auth here since we need to + * be able to send an ack if we receive a segment after we've + * already received the full SDU. + */ + + rx->in_use = 0; +} + +static void seg_ack(struct os_event *work) +{ + struct seg_rx *rx = work->ev_arg; + + BT_DBG("rx %p", rx); + + if (k_uptime_get_32() - rx->last > (60 * MSEC_PER_SEC)) { + BT_WARN("Incomplete timer expired"); + send_ack(rx->sub, rx->dst, rx->src, rx->ttl, + rx->seq_auth & 0x1fff, 0); + seg_rx_reset(rx); + return; + } + + send_ack(rx->sub, rx->dst, rx->src, rx->ttl, rx->seq_auth & 0x1fff, + rx->block); + + k_delayed_work_submit(&rx->ack, ack_timeout(rx)); +} + +static inline u8_t seg_len(bool ctl) +{ + if (ctl) { + return 8; + } else { + return 12; + } +} + +static inline bool sdu_len_is_ok(bool ctl, u8_t seg_n) +{ + return ((seg_n * seg_len(ctl) + 1) <= MYNEWT_VAL(BLE_MESH_RX_SDU_MAX)); +} + +static struct seg_rx *seg_rx_find(struct bt_mesh_net_rx *net_rx, + const u64_t *seq_auth) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(seg_rx); i++) { + struct seg_rx *rx = &seg_rx[i]; + + if (rx->src != net_rx->ctx.addr || rx->dst != net_rx->dst) { + continue; + } + + if (rx->seq_auth == *seq_auth) { + return rx; + } + + if (rx->in_use) { + BT_WARN("Duplicate SDU from src 0x%04x", + net_rx->ctx.addr); + + /* Clear out the old context since the sender + * has apparently started sending a new SDU. + */ + seg_rx_reset(rx); + return rx; + } + } + + return NULL; +} + +static bool seg_rx_is_valid(struct seg_rx *rx, struct bt_mesh_net_rx *net_rx, + const u8_t *hdr, u8_t seg_n) +{ + if (rx->hdr != *hdr || rx->seg_n != seg_n) { + BT_ERR("Invalid segment for ongoing session"); + return false; + } + + if (rx->src != net_rx->ctx.addr || rx->dst != net_rx->dst) { + BT_ERR("Invalid source or destination for segment"); + return false; + } + + if (rx->ctl != net_rx->ctl) { + BT_ERR("Inconsistent CTL in segment"); + return false; + } + + return true; +} + +static struct seg_rx *seg_rx_alloc(struct bt_mesh_net_rx *net_rx, + const u8_t *hdr, const u64_t *seq_auth, + u8_t seg_n) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(seg_rx); i++) { + struct seg_rx *rx = &seg_rx[i]; + + if (rx->in_use) { + continue; + } + + rx->in_use = 1; + net_buf_simple_init(rx->buf, 0); + rx->sub = net_rx->sub; + rx->ctl = net_rx->ctl; + rx->seq_auth = *seq_auth; + rx->seg_n = seg_n; + rx->hdr = *hdr; + rx->ttl = net_rx->ctx.send_ttl; + rx->src = net_rx->ctx.addr; + rx->dst = net_rx->dst; + rx->block = 0; + + BT_DBG("New RX context. Block Complete 0x%08x", + BLOCK_COMPLETE(seg_n)); + + return rx; + } + + return NULL; +} + +static int trans_seg(struct os_mbuf *buf, struct bt_mesh_net_rx *net_rx) +{ + struct seg_rx *rx; + u64_t seq_auth; + u8_t *hdr = buf->om_data; + u16_t seq_zero; + u8_t seg_n; + u8_t seg_o; + int err; + + if (buf->om_len < 5) { + BT_ERR("Too short segmented message (len %u)", buf->om_len); + return -EINVAL; + } + + BT_DBG("MIC_SIZE %u AKF %u AID 0x%02x", MIC_SIZE(hdr), AKF(hdr), + AID(hdr)); + + net_buf_simple_pull(buf, 1); + + seq_zero = net_buf_simple_pull_be16(buf); + seg_o = (seq_zero & 0x03) << 3; + seq_zero = (seq_zero >> 2) & 0x1fff; + seg_n = net_buf_simple_pull_u8(buf); + seg_o |= seg_n >> 5; + seg_n &= 0x1f; + + BT_DBG("SeqZero 0x%04x SegO %u SegN %u", seq_zero, seg_o, seg_n); + + if (seg_o > seg_n) { + BT_ERR("SegO greater than SegN (%u > %u)", seg_o, seg_n); + return -EINVAL; + } + + seq_auth = SEQ_AUTH(BT_MESH_NET_IVI_RX(net_rx), + (net_rx->seq & 0xffffe000) | seq_zero); + + /* Look for old RX sessions */ + rx = seg_rx_find(net_rx, &seq_auth); + if (rx) { + if (!seg_rx_is_valid(rx, net_rx, hdr, seg_n)) { + return -EINVAL; + } + + if (rx->in_use) { + BT_DBG("Existing RX context. Block 0x%08x", rx->block); + goto found_rx; + } + + if (rx->block == BLOCK_COMPLETE(rx->seg_n)) { + BT_WARN("Got segment for already complete SDU"); + send_ack(net_rx->sub, net_rx->dst, net_rx->ctx.addr, + net_rx->ctx.send_ttl, seq_zero, rx->block); + return 0; + } + + /* We ignore instead of sending block ack 0 since the + * ack timer is always smaller than the incomplete + * timer, i.e. the sender is misbehaving. + */ + BT_WARN("Got segment for canceled SDU"); + return -EINVAL; + } + + /* Bail out early if we're not ready to receive such a large SDU */ + if (!sdu_len_is_ok(&net_rx->ctx, seg_n)) { + BT_ERR("Too big incoming SDU length"); + send_ack(net_rx->sub, net_rx->dst, net_rx->ctx.addr, + net_rx->ctx.send_ttl, seq_zero, 0); + return 0; + } + + /* Look for free slot for a new RX session */ + rx = seg_rx_alloc(net_rx, hdr, &seq_auth, seg_n); + if (!rx) { + /* Warn but don't cancel since the existing slots willl + * eventually be freed up and we'll be able to process + * this one. + */ + BT_WARN("No free slots for new incoming segmented messages"); + return 0; + } + +found_rx: + if (BIT(seg_o) & rx->block) { + BT_WARN("Received already received fragment"); + return 0; + } + + /* All segments, except the last one, must either have 8 bytes of + * payload (for 64bit Net MIC) or 12 bytes of payload (for 32bit + * Net MIC). + */ + if (seg_o == seg_n) { + /* Set the expected final buffer length */ + rx->buf->om_len = seg_n * seg_len(rx->ctl) + buf->om_len; + BT_DBG("Target len %u * %u + %u = %u", seg_n, seg_len(rx->ctl), + buf->om_len, rx->buf->om_len); + + if (rx->buf->om_len > MYNEWT_VAL(BLE_MESH_RX_SDU_MAX)) { + BT_ERR("Too large SDU len"); + send_ack(net_rx->sub, net_rx->dst, net_rx->ctx.addr, + net_rx->ctx.send_ttl, seq_zero, 0); + seg_rx_reset(rx); + return 0; + } + } else { + if (buf->om_len != seg_len(rx->ctl)) { + BT_ERR("Incorrect segment size for message type"); + return -EINVAL; + } + } + + /* Reset the Incomplete Timer */ + rx->last = k_uptime_get_32(); + + if (!k_delayed_work_remaining_get(&rx->ack) && + !bt_mesh_lpn_established()) { + k_delayed_work_submit(&rx->ack, ack_timeout(rx)); + } + + /* Location in buffer can be calculated based on seg_o & rx->ctl */ + memcpy(rx->buf->om_data + (seg_o * seg_len(rx->ctl)), buf->om_data, buf->om_len); + + BT_DBG("Received %u/%u", seg_o, seg_n); + + /* Mark segment as received */ + rx->block |= BIT(seg_o); + + if (rx->block != BLOCK_COMPLETE(seg_n)) { + return 0; + } + + BT_DBG("Complete SDU"); + + /* Set the correct sequence number to be used with the App Nonce */ + net_rx->seq = (rx->seq_auth & 0xffffff); + + k_delayed_work_cancel(&rx->ack); + send_ack(net_rx->sub, net_rx->dst, net_rx->ctx.addr, + net_rx->ctx.send_ttl, seq_zero, rx->block); + + if (net_rx->ctl) { + err = ctl_recv(net_rx, *hdr, rx->buf); + } else { + err = sdu_recv(net_rx, *hdr, MIC_SIZE(hdr), + SZMIC(MIC_SIZE(hdr)), rx->buf); + } + + seg_rx_reset(rx); + + return err; +} + +int bt_mesh_trans_recv(struct os_mbuf *buf, struct bt_mesh_net_rx *rx) +{ + int err; + + BT_DBG("src 0x%04x dst 0x%04x seq 0x%08x", rx->ctx.addr, rx->dst, + rx->seq); + BT_DBG("Payload %s", bt_hex(buf->om_data, buf->om_len)); + + /* If LPN mode is enabled messages are only accepted when we've + * requested the Friend to send them. The messages must also + * be encrypted using the Friend Credentials. + */ + if ((MYNEWT_VAL(BLE_MESH_LOW_POWER)) && + bt_mesh_lpn_established() && + (!bt_mesh_lpn_waiting_update() || !rx->ctx.friend_cred)) { + BT_WARN("Ignoring unexpected message in Low Power mode"); + return -EAGAIN; + } + + if (SEG(buf->om_data)) { + err = trans_seg(buf, rx); + } else { + err = trans_unseg(buf, rx); + } + + /* Notify LPN state machine so a Friend Poll will be sent. If the + * message was a Friend Update it's possible that a Poll was already + * queued for sending, however that's fine since then the + * bt_mesh_lpn_waiting_update() function will return false: + * we still need to go through the actual sending to the bearer and + * wait for ReceiveDelay before transitioning to WAIT_UPDATE state. + */ + if ((MYNEWT_VAL(BLE_MESH_LOW_POWER)) && + bt_mesh_lpn_established() && bt_mesh_lpn_waiting_update()) { + bt_mesh_lpn_msg_received(rx); + } + + return err; +} + +void bt_mesh_rx_reset(void) +{ + int i; + + BT_DBG(""); + + for (i = 0; i < ARRAY_SIZE(seg_rx); i++) { + seg_rx[i].in_use = 0; + } +} + +void bt_mesh_trans_init(void) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(seg_tx); i++) { + k_delayed_work_init(&seg_tx[i].retransmit, seg_retransmit); + k_delayed_work_add_arg(&seg_tx[i].retransmit, &seg_tx[i]); + } + + /* XXX Probably we need mempool for that. + * For now we increase MSYS_1_BLOCK_COUNT + */ + for (i = 0; i < ARRAY_SIZE(seg_rx); i++) { + seg_rx[i].buf = NET_BUF_SIMPLE(MYNEWT_VAL(BLE_MESH_RX_SDU_MAX)); + k_delayed_work_init(&seg_rx[i].ack, seg_ack); + k_delayed_work_add_arg(&seg_rx[i].ack, &seg_rx[i]); + } +} diff --git a/nimble/host/mesh/src/transport.h b/nimble/host/mesh/src/transport.h new file mode 100644 index 000000000..b6cb00115 --- /dev/null +++ b/nimble/host/mesh/src/transport.h @@ -0,0 +1,102 @@ +/* Bluetooth Mesh */ + +/* + * Copyright (c) 2017 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "syscfg/syscfg.h" +#include "mesh/mesh.h" + +#define BT_MESH_TX_SEG_COUNT (MYNEWT_VAL(BLE_MESH_ADV_BUF_COUNT) - 3) +#define BT_MESH_TX_SDU_MAX (BT_MESH_TX_SEG_COUNT * 12) + +#define TRANS_CTL_OP_ACK 0x00 +#define TRANS_CTL_OP_FRIEND_POLL 0x01 +#define TRANS_CTL_OP_FRIEND_UPDATE 0x02 +#define TRANS_CTL_OP_FRIEND_REQ 0x03 +#define TRANS_CTL_OP_FRIEND_OFFER 0x04 +#define TRANS_CTL_OP_FRIEND_CLEAR 0x05 +#define TRANS_CTL_OP_FRIEND_CLEAR_CFM 0x06 +#define TRANS_CTL_OP_FRIEND_SUB_ADD 0x07 +#define TRANS_CTL_OP_FRIEND_SUB_REM 0x08 +#define TRANS_CTL_OP_FRIEND_SUB_CFM 0x09 +#define TRANS_CTL_OP_HEARTBEAT 0x0a + +struct bt_mesh_ctl_friend_poll +{ + u8_t fsn; +}__attribute__((__packed__)); + +struct bt_mesh_ctl_friend_update +{ + u8_t flags; + u32_t iv_index; + u8_t md; +}__attribute__((__packed__)); + +struct bt_mesh_ctl_friend_req +{ + u8_t criteria; + u8_t recv_delay; + u8_t poll_to[3]; + u16_t prev_addr; + u8_t num_elem; + u16_t lpn_counter; +}__attribute__((__packed__)); + +struct bt_mesh_ctl_friend_offer +{ + u8_t recv_win; + u8_t queue_size; + u8_t sub_list_size; + s8_t rssi; + u16_t frnd_counter; +}__attribute__((__packed__)); + +struct bt_mesh_ctl_friend_clear +{ + u16_t lpn_addr; + u16_t lpn_counter; +}__attribute__((__packed__)); + +struct bt_mesh_ctl_friend_clear_confirm +{ + u16_t lpn_addr; + u16_t lpn_counter; +}__attribute__((__packed__)); + +struct bt_mesh_ctl_friend_sub +{ + u8_t xact; + u16_t addr_list[5]; +}__attribute__((__packed__)); + +struct bt_mesh_ctl_friend_sub_confirm +{ + u8_t xact; +}__attribute__((__packed__)); + +struct bt_mesh_app_key * +bt_mesh_app_key_find(u16_t app_idx); + +bool +bt_mesh_tx_in_progress(void); + +void +bt_mesh_rx_reset(void); + +int +bt_mesh_ctl_send(struct bt_mesh_net_tx *tx, u8_t ctl_op, void *data, + size_t data_len, bt_mesh_adv_func_t cb); + +int +bt_mesh_trans_send(struct bt_mesh_net_tx *tx, struct os_mbuf *msg, + bt_mesh_cb_t cb, void *cb_data); + +int +bt_mesh_trans_recv(struct os_mbuf *buf, struct bt_mesh_net_rx *rx); + +void +bt_mesh_trans_init(void); diff --git a/nimble/host/mesh/syscfg.yml b/nimble/host/mesh/syscfg.yml new file mode 100644 index 000000000..95b67f9b0 --- /dev/null +++ b/nimble/host/mesh/syscfg.yml @@ -0,0 +1,322 @@ +# 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. +# + +# Package: net/nimble/host/mesh + +syscfg.defs: + BLE_MESH_PROV: + description: > + Enable provisioning. It is automatically enabled whenever + BLE_MESH_PB_ADV or BLE_MESH_PB_GATT is set. + value: 1 + + BLE_MESH_PB_ADV: + description: > + Enable this option to allow the device to be provisioned over + the advertising bearer. + value: 1 + + BLE_MESH_PROXY: + description: > + Enable proxy. This is automatically set whenever BLE_MESH_PB_GATT or + BLE_MESH_GATT_PROXY is set. + value: 0 + + BLE_MESH_PB_GATT: + description: > + Enable this option to allow the device to be provisioned over + the advertising bearer. + value: 1 + + BLE_MESH_GATT_PROXY: + descruption: > + This option enables support for the Mesh GATT Proxy Service, + i.e. the ability to act as a proxy between a Mesh GATT Client + and a Mesh network. + value: 1 + + BLE_MESH_PROXY_FILTER_SIZE: + descryption: > + This option specifies how many Proxy Filter entries the local + node supports. + value: 1 + + BLE_MESH_LOCAL_INTERFACE: + description: > + This is needed in order to send messages between two local Mesh + elements. Only disable it if you're sure this will not be needed + (which description: >s save a little bit of memory). + value: 1 + + BLE_MESH_SUBNET_COUNT: + description: > + This option specifies how many subnets a Mesh network can + participate in at the same time. + value: 1 + + BLE_MESH_APP_KEY_COUNT: + description: > + This option specifies how many application keys the device can + store per network. + value: 1 + + BLE_MESH_MODEL_KEY_COUNT: + description: > + This option specifies how many application keys each model can + at most be bound to. + value: 1 + + BLE_MESH_MODEL_GROUP_COUNT: + description: > + This option specifies how many group addresses each model can + at most be subscribed to. + value: 1 + + BLE_MESH_LABEL_COUNT: + description: > + This option specifies how many Label UUIDs can be stored. + value: 1 + + BLE_MESH_CRPL: + description: > + This options specifies the maximum capacity of the replay + protection list. This option is similar to the network message + cache size, but has a different purpose. + value: 10 + + BLE_MESH_ADV_TASK_PRIO: + description: > + Advertising task prio (FIXME) + value: 9 + + BLE_MESH_MSG_CACHE_SIZE: + description: > + Number of messages that are cached for the network. This description + prevent unnecessary decryption operations and unnecessary + relays. This option is similar to the replay protection list, + but has a different purpose. + value: 10 + + BLE_MESH_ADV_BUF_COUNT: + description: > + Number of advertising buffers available. The transport layer + reserves ADV_BUF_COUNT - 3 buffers for outgoing segments. The + maximum outgoing SDU size is 12 times this number (out of which + 4 or 8 bytes is used for the Transport Layer MIC). For + example, 5 segments means the maximum SDU size is 60 bytes, + which leaves 56 bytes for application layer data using a + 4-byte MIC and 52 bytes using an 8-byte MIC. + value: 10 + + BLE_MESH_TX_SEG_MSG_COUNT: + description: > + Maximum number of simultaneous outgoing multi-segment and/or + reliable messages. + value: 4 + + BLE_MESH_RX_SEG_MSG_COUNT: + description: > + Maximum number of simultaneous incoming multi-segment and/or + reliable messages. + value: 2 + + BLE_MESH_RX_SDU_MAX: + description: > + Maximum incoming Upper Transport Access PDU length. Leave this + to the value; value, unless you really need to optimize memory + usage. + value: 384 + + BLE_MESH_RELAY: + description: > + Support for acting as a Mesh Relay Node. + value: 0 + + BLE_MESH_LOW_POWER: + description: > + Enable this option to be able to act as a Low Power Node. + value: 1 + + BLE_MESH_LPN_RSSI_FACTOR: + description: > + The contribution of the RSSI measured by the Friend node used + in Friend Offer Delay calculations. 0 = 1, 1 = 1.5, 2 = 2, 3 = 2.5. + value: 0 + + BLE_MESH_LPN_RECV_WIN_FACTOR: + description: > + The contribution of the supported Receive Window used in + Friend Offer Delay calculations. 0 = 1, 1 = 1.5, 2 = 2, 3 = 2.5. + value: 0 + + BLE_MESH_LPN_MIN_QUEUE_SIZE: + description: > + The MinQueueSizeLog field is defined as log_2(N), where N is + the minimum number of maximum size Lower Transport PDUs that + the Friend node can store in its Friend Queue. As an example, + MinQueueSizeLog value 1 gives N = 2, and value 7 gives N = 128. + value: 1 + + BLE_MESH_LPN_RECV_DELAY: + description: > + The ReceiveDelay is the time between the Low Power node + sending a request and listening for a response. This delay + allows the Friend node time to prepare the response. The value + is in units of milliseconds. + value: 20 + + BLE_MESH_LPN_POLL_TIMEOUT: + description: > + PollTimeout timer is used to measure time between two + consecutive requests sent by the Low Power node. If no + requests are received by the Friend node before the + PollTimeout timer expires, then the friendship is considered + terminated. The value is in units of 100 milliseconds. + value: 100 + + BLE_MESH_LPN_SCAN_LATENCY: + description: > + Latency in milliseconds that it takes to enable scanning. This + is in practice how much time in advance before the Receive Window + that scanning is requested to be enabled. + value: 10 + + BLE_MESH_LPN_GROUPS: + description: > + Maximum number of groups that the LPN can subscribe to. + value: 10 + + BLE_MESH_FRIEND: + description: > + Enable this option to be able to act as a Friend Node. + value: 1 + + BLE_MESH_FRIEND_RECV_WIN: + description: > + Receive Window in milliseconds supported by the Friend node. + value: 255 + + BLE_MESH_FRIEND_QUEUE_SIZE: + description: > + Queue Size available on the Friend node. + value: 16 + + BLE_MESH_FRIEND_SUB_LIST_SIZE: + description: > + Size of the Subscription List that can be supported by a + Friend node for a Low Power node. + value: 16 + + BLE_MESH_FRIEND_LPN_COUNT: + description: > + Number of Low Power Nodes the Friend can have a Friendship + with simultaneously. + value: 1 + + BLE_MESH_DEBUG: + description: > + Use this option to enable debug logs for the Bluetooth + Mesh functionality. + value: 0 + + BLE_MESH_DEBUG_NET: + description: > + Use this option to enable Network layer debug logs for the + Bluetooth Mesh functionality. + value: 0 + + BLE_MESH_DEBUG_TRANS: + description: > + Use this option to enable Transport layer debug logs for the + Bluetooth Mesh functionality. + value: 0 + + BLE_MESH_DEBUG_BEACON: + description: > + Use this option to enable Beacon-related debug logs for the + Bluetooth Mesh functionality. + value: 0 + + BLE_MESH_DEBUG_CRYPTO: + description: > + Use this option to enable cryptographic debug logs for the + Bluetooth Mesh functionality. + value: 0 + + BLE_MESH_DEBUG_PROV: + description: > + Use this option to enable Provisioning debug logs for the + Bluetooth Mesh functionality. + value: 0 + + BLE_MESH_DEBUG_ACCESS: + description: > + Use this option to enable Access layer and device composition + related debug logs for Bluetooth Mesh. + value: 0 + + BLE_MESH_DEBUG_MODEL: + description: > + Use this option to enable debug logs for the Foundation + Models. + value: 0 + + BLE_MESH_DEBUG_ADV: + description: > + Use this option to enable advertising debug logs for + the Bluetooth Mesh functionality. + value: 0 + + BLE_MESH_DEBUG_LOW_POWER: + description: > + Use this option to enable Low Power debug logs for the + Bluetooth Mesh functionality. + value: 0 + + BLE_MESH_DEBUG_FRIEND: + description: > + Use this option to enable Friend debug logs for the + Bluetooth Mesh functionality. + value: 0 + + BLE_MESH_DEBUG_PROXY: + description: > + Use this option to enable Proxy protocol debug logs. + value: 0 + + BLE_MESH_IV_UPDATE_TEST: + description: > + This option removes the 96 hour limit of the IV Update + Procedure and lets the state be changed at any time. + value: 0 + + BLE_MESH_DEV_UUID: + description: > + Device UUID + value: ((uint8_t[16]){0x11, 0x22, 0}) + + +syscfg.vals.BLE_MESH_GATT_PROXY: + BLE_MESH_PROXY: 1 + +syscfg.vals.BLE_MESH_PB_GATT: + BLE_MESH_PROXY: 1 + BLE_MESH_PROV: 1 + +syscfg.vals.BLE_MESH_PB_ADV: + BLE_MESH_PROV: 1 diff --git a/nimble/host/pkg.yml b/nimble/host/pkg.yml index bf83156bf..4affd38ce 100644 --- a/nimble/host/pkg.yml +++ b/nimble/host/pkg.yml @@ -39,6 +39,9 @@ pkg.deps.BLE_SM_SC: pkg.deps.BLE_MONITOR_RTT: - hw/drivers/rtt +pkg.deps.BLE_MESH: + - net/nimble/host/mesh + pkg.req_apis: - ble_transport - console diff --git a/nimble/host/src/ble_gap.c b/nimble/host/src/ble_gap.c index e267d18e5..160ab389c 100644 --- a/nimble/host/src/ble_gap.c +++ b/nimble/host/src/ble_gap.c @@ -113,9 +113,19 @@ struct ble_gap_master_state { uint8_t extended:1; } disc; }; + + }; static bssnz_t struct ble_gap_master_state ble_gap_master; + +#if MYNEWT_VAL(BLE_MESH) +struct ble_gap_mesh_state { + ble_gap_event_fn *cb; + void *cb_arg; +} ble_gap_mesh; +#endif + /** * The state of the in-progress slave connection. If no slave connection is * currently in progress, then the op field is set to BLE_GAP_OP_NULL. @@ -521,6 +531,17 @@ ble_gap_set_prefered_le_phy(uint16_t conn_handle, uint8_t tx_phys_mask, buf, sizeof(buf), NULL, 0, NULL); } +#if MYNEWT_VAL(BLE_MESH) +int +ble_gap_mesh_cb_register(ble_gap_event_fn *cb, void *cb_arg) +{ + ble_gap_mesh.cb = cb; + ble_gap_mesh.cb_arg = cb_arg; + + return 0; +} +#endif + /***************************************************************************** * $misc * *****************************************************************************/ @@ -587,6 +608,16 @@ ble_gap_slave_reset_state(void) ble_hs_timer_resched(); } +static bool +ble_gap_has_client(struct ble_gap_master_state *out_state) +{ + if (!out_state) { + return 0; + } + + return out_state->cb; +} + static void ble_gap_master_extract_state(struct ble_gap_master_state *out_state, int reset_state) @@ -638,7 +669,7 @@ ble_gap_master_connect_failure(int status) int rc; ble_gap_master_extract_state(&state, 1); - if (state.cb != NULL) { + if (!ble_gap_has_client(&state)) { memset(&event, 0, sizeof event); event.type = BLE_GAP_EVENT_CONNECT; event.connect.status = status; @@ -686,27 +717,32 @@ ble_gap_is_extended_disc(void) static void ble_gap_disc_report(void *desc) { - struct ble_gap_master_state state; + struct ble_gap_master_state state;; struct ble_gap_event event; - ble_gap_master_extract_state(&state, 0); - - if (state.cb != NULL) { - memset(&event, 0, sizeof event); - if (ble_gap_is_extended_disc()) { + memset(&event, 0, sizeof event); + if (ble_gap_is_extended_disc()) { #if MYNEWT_VAL(BLE_EXT_ADV) - event.type = BLE_GAP_EVENT_EXT_DISC; - event.ext_disc = *((struct ble_gap_ext_disc_desc *)desc); + event.type = BLE_GAP_EVENT_EXT_DISC; + event.ext_disc = *((struct ble_gap_ext_disc_desc *)desc); #else - assert(0); + assert(0); #endif - } else { - event.type = BLE_GAP_EVENT_DISC; - event.disc = *((struct ble_gap_disc_desc *)desc); - } + } else { + event.type = BLE_GAP_EVENT_DISC; + event.disc = *((struct ble_gap_disc_desc *)desc); + } + ble_gap_master_extract_state(&state, 0); + if (ble_gap_has_client(&state)) { state.cb(&event, state.cb_arg); } + +#if MYNEWT_VAL(BLE_MESH) + if (ble_gap_mesh.cb) { + ble_gap_mesh.cb(&event, ble_gap_mesh.cb_arg); + } +#endif } static void @@ -715,14 +751,19 @@ ble_gap_disc_complete(void) struct ble_gap_master_state state; struct ble_gap_event event; + memset(&event, 0, sizeof event); + event.type = BLE_GAP_EVENT_DISC_COMPLETE; + ble_gap_master_extract_state(&state, 1); - - if (state.cb != NULL) { - memset(&event, 0, sizeof event); - event.type = BLE_GAP_EVENT_DISC_COMPLETE; - + if (ble_gap_has_client(&state)) { ble_gap_call_event_cb(&event, state.cb, state.cb_arg); } + +#if MYNEWT_VAL(BLE_MESH) + if (ble_gap_mesh.cb) { + ble_gap_mesh.cb(&event, ble_gap_mesh.cb_arg); + } +#endif } static void @@ -924,6 +965,12 @@ ble_gap_conn_broken(uint16_t conn_handle, int reason) event.disconnect.reason = reason; ble_gap_call_event_cb(&event, snap.cb, snap.cb_arg); +#if MYNEWT_VAL(BLE_MESH) + if (ble_gap_mesh.cb) { + ble_gap_mesh.cb(&event, ble_gap_mesh.cb_arg); + } +#endif + STATS_INC(ble_gap_stats, disconnect); } @@ -1341,6 +1388,12 @@ ble_gap_rx_conn_complete(struct hci_le_conn_complete *evt) event.connect.status = 0; ble_gap_call_conn_event_cb(&event, evt->connection_handle); +#if MYNEWT_VAL(BLE_MESH) + if (ble_gap_mesh.cb) { + ble_gap_mesh.cb(&event, ble_gap_mesh.cb_arg); + } +#endif + ble_gap_rd_rem_sup_feat_tx(evt->connection_handle); return 0; @@ -4138,6 +4191,12 @@ ble_gap_subscribe_event(uint16_t conn_handle, uint16_t attr_handle, event.subscribe.prev_indicate = !!prev_indicate; event.subscribe.cur_indicate = !!cur_indicate; ble_gap_call_conn_event_cb(&event, conn_handle); + +#if MYNEWT_VAL(BLE_MESH) + if (ble_gap_mesh.cb) { + ble_gap_mesh.cb(&event, ble_gap_mesh.cb_arg); + } +#endif } /***************************************************************************** diff --git a/nimble/host/syscfg.yml b/nimble/host/syscfg.yml index 0e054bb60..76bf81591 100644 --- a/nimble/host/syscfg.yml +++ b/nimble/host/syscfg.yml @@ -353,3 +353,11 @@ syscfg.defs: mechanism. value: 8 + + BLE_MESH: + description: > + Enable Bluetooth Mesh + value: 0 + +syscfg.vals.BLE_MESH: + BLE_SM_SC: 1