diff --git a/apps/cs_initiator/pkg.yml b/apps/cs_initiator/pkg.yml new file mode 100644 index 0000000000..785274ce30 --- /dev/null +++ b/apps/cs_initiator/pkg.yml @@ -0,0 +1,37 @@ +# 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: apps/cs_initiator +pkg.type: app +pkg.description: Simple BLE central application. +pkg.author: "Apache Mynewt " +pkg.homepage: "http://mynewt.apache.org/" +pkg.keywords: + +pkg.deps: + - "@apache-mynewt-core/kernel/os" + - "@apache-mynewt-core/sys/console" + - "@apache-mynewt-core/sys/log" + - "@apache-mynewt-core/sys/log/modlog" + - "@apache-mynewt-core/sys/stats" + - "@apache-mynewt-core/crypto/mbedtls" + - "@apache-mynewt-core/test/testutil" + - nimble/host + - nimble/host/util + - nimble/host/services/gap + - nimble/host/services/gatt + - nimble/host/store/config diff --git a/apps/cs_initiator/src/cs_initiator.h b/apps/cs_initiator/src/cs_initiator.h new file mode 100644 index 0000000000..4fa3a221e7 --- /dev/null +++ b/apps/cs_initiator/src/cs_initiator.h @@ -0,0 +1,106 @@ +/* + * 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 H_CS_INITIATOR_ +#define H_CS_INITIATOR_ + +#include "os/mynewt.h" +#include "modlog/modlog.h" +#ifdef __cplusplus +extern "C" { +#endif + +struct ble_hs_adv_fields; +struct ble_gap_conn_desc; +struct ble_hs_cfgble_hs_cfg; + +#define GATT_SVR_SVC_CHANNEL_SOUNDING_UUID 0xffff +#define GATT_SVR_CHR_TOD_TOA_UUID 0xfffe +#define GATT_SVR_CHR_TOD_TOA_UUID 0xfffe + +/** Misc. */ +void print_bytes(const uint8_t *bytes, int len); +void print_mbuf(const struct os_mbuf *om); +char *addr_str(const void *addr); +void print_uuid(const ble_uuid_t *uuid); +void print_conn_desc(const struct ble_gap_conn_desc *desc); +void print_adv_fields(const struct ble_hs_adv_fields *fields); + +/** Peer. */ +struct peer_dsc { + SLIST_ENTRY(peer_dsc) next; + struct ble_gatt_dsc dsc; +}; +SLIST_HEAD(peer_dsc_list, peer_dsc); + +struct peer_chr { + SLIST_ENTRY(peer_chr) next; + struct ble_gatt_chr chr; + + struct peer_dsc_list dscs; +}; +SLIST_HEAD(peer_chr_list, peer_chr); + +struct peer_svc { + SLIST_ENTRY(peer_svc) next; + struct ble_gatt_svc svc; + + struct peer_chr_list chrs; +}; +SLIST_HEAD(peer_svc_list, peer_svc); + +struct peer; +typedef void peer_disc_fn(const struct peer *peer, int status, void *arg); + +struct peer { + SLIST_ENTRY(peer) next; + + uint16_t conn_handle; + + /** List of discovered GATT services. */ + struct peer_svc_list svcs; + + /** Keeps track of where we are in the service discovery process. */ + uint16_t disc_prev_chr_val; + struct peer_svc *cur_svc; + + /** Callback that gets executed when service discovery completes. */ + peer_disc_fn *disc_cb; + void *disc_cb_arg; +}; + +int peer_disc_all(uint16_t conn_handle, peer_disc_fn *disc_cb, + void *disc_cb_arg); +const struct peer_dsc * +peer_dsc_find_uuid(const struct peer *peer, const ble_uuid_t *svc_uuid, + const ble_uuid_t *chr_uuid, const ble_uuid_t *dsc_uuid); +const struct peer_chr * +peer_chr_find_uuid(const struct peer *peer, const ble_uuid_t *svc_uuid, + const ble_uuid_t *chr_uuid); +const struct peer_svc * +peer_svc_find_uuid(const struct peer *peer, const ble_uuid_t *uuid); +int peer_delete(uint16_t conn_handle); +int peer_add(uint16_t conn_handle); +int peer_init(int max_peers, int max_svcs, int max_chrs, int max_dscs); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/apps/cs_initiator/src/main.c b/apps/cs_initiator/src/main.c new file mode 100644 index 0000000000..1ab6b68c8c --- /dev/null +++ b/apps/cs_initiator/src/main.c @@ -0,0 +1,505 @@ +/* + * 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 +#include +#include "os/mynewt.h" +#include "bsp/bsp.h" + +/* BLE */ +#include "nimble/ble.h" +#include "host/ble_hs.h" +#include "host/ble_cs.h" +#include "host/util/util.h" + +/* Mandatory services. */ +#include "services/gap/ble_svc_gap.h" +#include "services/gatt/ble_svc_gatt.h" + +/* Application-specified header. */ +#include "cs_initiator.h" + +static int cs_initiator_gap_event(struct ble_gap_event *event, void *arg); +static bool cs_started = false; +static uint32_t tod_toa_i; +static uint32_t toa_tod_r; + +static int +cs_initiator_cs_event(struct ble_cs_event *event, void *arg) +{ + int rc; + + switch (event->type) { + case BLE_CS_EVENT_CS_PROCEDURE_COMPLETE: + tod_toa_i = event->procedure_complete.time_diff_ns; + MODLOG_DFLT(INFO, "Received ToD_ToA = %d\n", tod_toa_i); + + if (toa_tod_r) { + MODLOG_DFLT(INFO, "ToF = %d\n", (tod_toa_i - toa_tod_r) / 2); + tod_toa_i = 0; + toa_tod_r = 0; + } + return 0; + default: + return 0; + } +} + +static void +cs_initiator_start_cs(uint16_t conn_handle) +{ + struct ble_cs_initiator_procedure_start_params cmd; + int rc; + + cmd.conn_handle = conn_handle; + cmd.cb = cs_initiator_cs_event; + cmd.cb_arg = NULL; + rc = ble_cs_initiator_procedure_start(&cmd); + if (rc) { + MODLOG_DFLT(INFO, "Failed to read local supported CS capabilities," + "err %dt", rc); + } +} + +static int +cs_initiator_on_subscribe(uint16_t conn_handle, + const struct ble_gatt_error *error, + struct ble_gatt_attr *attr, + void *arg) +{ + MODLOG_DFLT(INFO, "Subscribe complete; status=%d conn_handle=%d " + "attr_handle=%d\n", + error->status, conn_handle, attr->handle); + + return 0; +} + +static void +cs_initiator_subscribe_tod_toa(const struct peer *peer) +{ + const struct peer_dsc *dsc; + uint8_t value[2]; + int rc; + + dsc = peer_dsc_find_uuid(peer, + BLE_UUID16_DECLARE(GATT_SVR_SVC_CHANNEL_SOUNDING_UUID), + BLE_UUID16_DECLARE(GATT_SVR_CHR_TOD_TOA_UUID), + BLE_UUID16_DECLARE(BLE_GATT_DSC_CLT_CFG_UUID16)); + if (dsc == NULL) { + MODLOG_DFLT(ERROR, "Error: Peer lacks a CCCD for ToD_ToA characteristic\n"); + goto err; + } + + value[0] = 1; + value[1] = 0; + rc = ble_gattc_write_flat(peer->conn_handle, dsc->dsc.handle, + value, sizeof value, cs_initiator_on_subscribe, NULL); + if (rc != 0) { + MODLOG_DFLT(ERROR, "Error: Failed to subscribe to characteristic; " + "rc=%d\n", rc); + goto err; + } + + return; + +err: + /* Terminate the connection. */ + ble_gap_terminate(peer->conn_handle, BLE_ERR_REM_USER_CONN_TERM); +} + +/** + * Called when service discovery of the specified peer has completed. + */ +static void +cs_initiator_on_disc_complete(const struct peer *peer, int status, void *arg) +{ + + if (status != 0) { + /* Service discovery failed. Terminate the connection. */ + MODLOG_DFLT(ERROR, "Error: Service discovery failed; status=%d " + "conn_handle=%d\n", status, peer->conn_handle); + ble_gap_terminate(peer->conn_handle, BLE_ERR_REM_USER_CONN_TERM); + return; + } + + /* Service discovery has completed successfully. Now we have a complete + * list of services, characteristics, and descriptors that the peer + * supports. + */ + MODLOG_DFLT(ERROR, "Service discovery complete; status=%d " + "conn_handle=%d\n", status, peer->conn_handle); + + /* Now perform three concurrent GATT procedures against the peer: read, + * write, and subscribe to notifications. + */ + cs_initiator_subscribe_tod_toa(peer); +} + +/** + * Initiates the GAP general discovery procedure. + */ +static void +cs_initiator_scan(void) +{ + uint8_t own_addr_type; + struct ble_gap_disc_params disc_params; + int rc; + + /* Figure out address to use while advertising (no privacy for now) */ + rc = ble_hs_id_infer_auto(0, &own_addr_type); + if (rc != 0) { + MODLOG_DFLT(ERROR, "error determining address type; rc=%d\n", rc); + return; + } + + /* Tell the controller to filter duplicates; we don't want to process + * repeated advertisements from the same device. + */ + disc_params.filter_duplicates = 1; + + /** + * Perform a passive scan. I.e., don't send follow-up scan requests to + * each advertiser. + */ + disc_params.passive = 1; + + /* Use defaults for the rest of the parameters. */ + disc_params.itvl = 0; + disc_params.window = 0; + disc_params.filter_policy = 0; + disc_params.limited = 0; + + rc = ble_gap_disc(own_addr_type, BLE_HS_FOREVER, &disc_params, + cs_initiator_gap_event, NULL); + if (rc != 0) { + MODLOG_DFLT(ERROR, "Error initiating GAP discovery procedure; rc=%d\n", + rc); + } +} + +/** + * Indicates whether we should tre to connect to the sender of the specified + * advertisement. The function returns a positive result if the device + * advertises connectability and support for the Alert Notification service. + */ +static int +cs_initiator_should_connect(const struct ble_gap_disc_desc *disc) +{ + struct ble_hs_adv_fields fields; + int rc; + int i; + + /* The device has to be advertising connectability. */ + if (disc->event_type != BLE_HCI_ADV_RPT_EVTYPE_ADV_IND && + disc->event_type != BLE_HCI_ADV_RPT_EVTYPE_DIR_IND) { + + return 0; + } + + rc = ble_hs_adv_parse_fields(&fields, disc->data, disc->length_data); + if (rc != 0) { + return 0; + } + + /* The device has to advertise support for the Alert Notification + * service (0x1811). + */ + for (i = 0; i < fields.num_uuids16; i++) { + if (ble_uuid_u16(&fields.uuids16[i].u) == GATT_SVR_SVC_CHANNEL_SOUNDING_UUID) { + return 1; + } + } + + return 0; +} + +/** + * Connects to the sender of the specified advertisement of it looks + * interesting. A device is "interesting" if it advertises connectability and + * support for the Alert Notification service. + */ +static void +cs_initiator_connect_if_interesting(const struct ble_gap_disc_desc *disc) +{ + uint8_t own_addr_type; + int rc; + struct ble_gap_conn_params conn_params = { + .scan_itvl = 0x0010, + .scan_window = 0x0010, + .itvl_min = BLE_GAP_INITIAL_CONN_ITVL_MIN, + .itvl_max = BLE_GAP_INITIAL_CONN_ITVL_MAX, + .latency = BLE_GAP_INITIAL_CONN_LATENCY, + .supervision_timeout = BLE_GAP_INITIAL_SUPERVISION_TIMEOUT, + .min_ce_len = BLE_GAP_INITIAL_CONN_MIN_CE_LEN, + .max_ce_len = BLE_GAP_INITIAL_CONN_MAX_CE_LEN, + }; + + /* Don't do anything if we don't care about this advertiser. */ + if (!cs_initiator_should_connect(disc)) { + return; + } + + /* Scanning must be stopped before a connection can be initiated. */ + rc = ble_gap_disc_cancel(); + if (rc != 0) { + MODLOG_DFLT(DEBUG, "Failed to cancel scan; rc=%d\n", rc); + return; + } + + /* Figure out address to use for connect (no privacy for now) */ + rc = ble_hs_id_infer_auto(0, &own_addr_type); + if (rc != 0) { + MODLOG_DFLT(ERROR, "error determining address type; rc=%d\n", rc); + return; + } + + /* Try to connect the the advertiser. Allow 30 seconds (30000 ms) for + * timeout. + */ + rc = ble_gap_connect(own_addr_type, &disc->addr, 30000, &conn_params, + cs_initiator_gap_event, NULL); + if (rc != 0) { + MODLOG_DFLT(ERROR, "Error: Failed to connect to device; addr_type=%d " + "addr=%s\n; rc=%d", + disc->addr.type, addr_str(disc->addr.val), rc); + return; + } +} + +/** + * The nimble host executes this callback when a GAP event occurs. The + * application associates a GAP event callback with each connection that is + * established. cs_initiator uses the same callback for all connections. + * + * @param event The event being signalled. + * @param arg Application-specified argument; unused by + * cs_initiator. + * + * @return 0 if the application successfully handled the + * event; nonzero on failure. The semantics + * of the return code is specific to the + * particular GAP event being signalled. + */ +static int +cs_initiator_gap_event(struct ble_gap_event *event, void *arg) +{ + struct ble_gap_conn_desc desc; + struct ble_hs_adv_fields fields; + int rc; + + switch (event->type) { + case BLE_GAP_EVENT_DISC: + rc = ble_hs_adv_parse_fields(&fields, event->disc.data, + event->disc.length_data); + if (rc != 0) { + return 0; + } + + /* An advertisment report was received during GAP discovery. */ + print_adv_fields(&fields); + + /* Try to connect to the advertiser if it looks interesting. */ + cs_initiator_connect_if_interesting(&event->disc); + return 0; + + case BLE_GAP_EVENT_CONNECT: + /* A new connection was established or a connection attempt failed. */ + if (event->connect.status == 0) { + /* Connection successfully established. */ + MODLOG_DFLT(INFO, "Connection established "); + + rc = ble_gap_conn_find(event->connect.conn_handle, &desc); + assert(rc == 0); + print_conn_desc(&desc); + MODLOG_DFLT(INFO, "\n"); + + /* Remember peer. */ + rc = peer_add(event->connect.conn_handle); + if (rc != 0) { + MODLOG_DFLT(ERROR, "Failed to add peer; rc=%d\n", rc); + return 0; + } + + rc = ble_gap_security_initiate(event->connect.conn_handle); + if (rc) { + MODLOG_DFLT(INFO, "Failed to pair"); + } + + /* Perform service discovery. */ + rc = peer_disc_all(event->connect.conn_handle, + cs_initiator_on_disc_complete, NULL); + if (rc != 0) { + MODLOG_DFLT(ERROR, "Failed to discover services; rc=%d\n", rc); + return 0; + } + } else { + /* Connection attempt failed; resume scanning. */ + MODLOG_DFLT(ERROR, "Error: Connection failed; status=%d\n", + event->connect.status); + cs_initiator_scan(); + } + + return 0; + + case BLE_GAP_EVENT_DISCONNECT: + /* Connection terminated. */ + MODLOG_DFLT(INFO, "disconnect; reason=%d ", event->disconnect.reason); + print_conn_desc(&event->disconnect.conn); + MODLOG_DFLT(INFO, "\n"); + + /* Forget about peer. */ + peer_delete(event->disconnect.conn.conn_handle); + + /* Resume scanning. */ + cs_initiator_scan(); + return 0; + + case BLE_GAP_EVENT_DISC_COMPLETE: + MODLOG_DFLT(INFO, "discovery complete; reason=%d\n", + event->disc_complete.reason); + return 0; + + case BLE_GAP_EVENT_PAIRING_COMPLETE: + MODLOG_DFLT(INFO, "received pairing complete: " + "conn_handle=%d status=%d\n", + event->pairing_complete.conn_handle, + event->pairing_complete.status); + + return 0; + + case BLE_GAP_EVENT_ENC_CHANGE: + /* Encryption has been enabled or disabled for this connection. */ + MODLOG_DFLT(INFO, "encryption change event; status=%d ", + event->enc_change.status); + rc = ble_gap_conn_find(event->enc_change.conn_handle, &desc); + assert(rc == 0); + print_conn_desc(&desc); + + if (!cs_started) { + cs_started = true; + cs_initiator_start_cs(event->connect.conn_handle); + } + + return 0; + + case BLE_GAP_EVENT_NOTIFY_RX: + /* Peer sent us a notification or indication. */ + MODLOG_DFLT(INFO, "received %s; conn_handle=%d attr_handle=%d " + "attr_len=%d\n", + event->notify_rx.indication ? + "indication" : + "notification", + event->notify_rx.conn_handle, + event->notify_rx.attr_handle, + OS_MBUF_PKTLEN(event->notify_rx.om)); + + toa_tod_r = get_le32(event->notify_rx.om->om_data); + MODLOG_DFLT(INFO, "Received ToA_ToD = %d\n", toa_tod_r); + + if (tod_toa_i) { + MODLOG_DFLT(INFO, "ToF = %d\n", (tod_toa_i - toa_tod_r) / 2); + tod_toa_i = 0; + toa_tod_r = 0; + } + + /* Attribute data is contained in event->notify_rx.attr_data. */ + return 0; + + case BLE_GAP_EVENT_MTU: + MODLOG_DFLT(INFO, "mtu update event; conn_handle=%d cid=%d mtu=%d\n", + event->mtu.conn_handle, + event->mtu.channel_id, + event->mtu.value); + return 0; + + case BLE_GAP_EVENT_REPEAT_PAIRING: + /* We already have a bond with the peer, but it is attempting to + * establish a new secure link. This app sacrifices security for + * convenience: just throw away the old bond and accept the new link. + */ + + /* Delete the old bond. */ + rc = ble_gap_conn_find(event->repeat_pairing.conn_handle, &desc); + assert(rc == 0); + ble_store_util_delete_peer(&desc.peer_id_addr); + + /* Return BLE_GAP_REPEAT_PAIRING_RETRY to indicate that the host should + * continue with the pairing operation. + */ + return BLE_GAP_REPEAT_PAIRING_RETRY; + + default: + return 0; + } +} + +static void +cs_initiator_on_reset(int reason) +{ + MODLOG_DFLT(ERROR, "Resetting state; reason=%d\n", reason); +} + +static void +cs_initiator_on_sync(void) +{ + int rc; + + /* Make sure we have proper identity address set (public preferred) */ + rc = ble_hs_util_ensure_addr(0); + assert(rc == 0); + + /* Begin scanning for a peripheral to connect to. */ + cs_initiator_scan(); +} + +/** + * main + * + * All application logic and NimBLE host work is performed in default task. + * + * @return int NOTE: this function should never return! + */ +int +mynewt_main(int argc, char **argv) +{ + int rc; + + /* Initialize OS */ + sysinit(); + + /* Configure the host. */ + ble_hs_cfg.reset_cb = cs_initiator_on_reset; + ble_hs_cfg.sync_cb = cs_initiator_on_sync; + ble_hs_cfg.store_status_cb = ble_store_util_status_rr; + + /* Initialize data structures to track connected peers. */ + rc = peer_init(MYNEWT_VAL(BLE_MAX_CONNECTIONS), 64, 64, 64); + assert(rc == 0); + + /* Set the default device name. */ + rc = ble_svc_gap_device_name_set("nimble-cs_initiator"); + assert(rc == 0); + + /* os start should never return. If it does, this should be an error */ + while (1) { + os_eventq_run(os_eventq_dflt_get()); + } + + return 0; +} diff --git a/apps/cs_initiator/src/misc.c b/apps/cs_initiator/src/misc.c new file mode 100644 index 0000000000..394b7a41f4 --- /dev/null +++ b/apps/cs_initiator/src/misc.c @@ -0,0 +1,209 @@ +/* + * 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 +#include +#include +#include "host/ble_hs.h" +#include "host/ble_uuid.h" +#include "cs_initiator.h" + +/** + * Utility function to log an array of bytes. + */ +void +print_bytes(const uint8_t *bytes, int len) +{ + int i; + + for (i = 0; i < len; i++) { + MODLOG_DFLT(DEBUG, "%s0x%02x", i != 0 ? ":" : "", bytes[i]); + } +} + +void +print_mbuf(const struct os_mbuf *om) +{ + int colon; + + colon = 0; + while (om != NULL) { + if (colon) { + MODLOG_DFLT(DEBUG, ":"); + } else { + colon = 1; + } + print_bytes(om->om_data, om->om_len); + om = SLIST_NEXT(om, om_next); + } +} + +char * +addr_str(const void *addr) +{ + static char buf[6 * 2 + 5 + 1]; + const uint8_t *u8p; + + u8p = addr; + sprintf(buf, "%02x:%02x:%02x:%02x:%02x:%02x", + u8p[5], u8p[4], u8p[3], u8p[2], u8p[1], u8p[0]); + + return buf; +} + +void +print_uuid(const ble_uuid_t *uuid) +{ + char buf[BLE_UUID_STR_LEN]; + + MODLOG_DFLT(DEBUG, "%s", ble_uuid_to_str(uuid, buf)); +} + +/** + * Logs information about a connection to the console. + */ +void +print_conn_desc(const struct ble_gap_conn_desc *desc) +{ + MODLOG_DFLT(DEBUG, "handle=%d our_ota_addr_type=%d our_ota_addr=%s ", + desc->conn_handle, desc->our_ota_addr.type, + addr_str(desc->our_ota_addr.val)); + MODLOG_DFLT(DEBUG, "our_id_addr_type=%d our_id_addr=%s ", + desc->our_id_addr.type, addr_str(desc->our_id_addr.val)); + MODLOG_DFLT(DEBUG, "peer_ota_addr_type=%d peer_ota_addr=%s ", + desc->peer_ota_addr.type, addr_str(desc->peer_ota_addr.val)); + MODLOG_DFLT(DEBUG, "peer_id_addr_type=%d peer_id_addr=%s ", + desc->peer_id_addr.type, addr_str(desc->peer_id_addr.val)); + MODLOG_DFLT(DEBUG, "conn_itvl=%d conn_latency=%d supervision_timeout=%d " + "encrypted=%d authenticated=%d bonded=%d", + desc->conn_itvl, desc->conn_latency, + desc->supervision_timeout, + desc->sec_state.encrypted, + desc->sec_state.authenticated, + desc->sec_state.bonded); +} + + +void +print_adv_fields(const struct ble_hs_adv_fields *fields) +{ + char s[BLE_HS_ADV_MAX_SZ]; + const uint8_t *u8p; + int i; + + if (fields->flags != 0) { + MODLOG_DFLT(DEBUG, " flags=0x%02x\n", fields->flags); + } + + if (fields->uuids16 != NULL) { + MODLOG_DFLT(DEBUG, " uuids16(%scomplete)=", + fields->uuids16_is_complete ? "" : "in"); + for (i = 0; i < fields->num_uuids16; i++) { + print_uuid(&fields->uuids16[i].u); + MODLOG_DFLT(DEBUG, " "); + } + MODLOG_DFLT(DEBUG, "\n"); + } + + if (fields->uuids32 != NULL) { + MODLOG_DFLT(DEBUG, " uuids32(%scomplete)=", + fields->uuids32_is_complete ? "" : "in"); + for (i = 0; i < fields->num_uuids32; i++) { + print_uuid(&fields->uuids32[i].u); + MODLOG_DFLT(DEBUG, " "); + } + MODLOG_DFLT(DEBUG, "\n"); + } + + if (fields->uuids128 != NULL) { + MODLOG_DFLT(DEBUG, " uuids128(%scomplete)=", + fields->uuids128_is_complete ? "" : "in"); + for (i = 0; i < fields->num_uuids128; i++) { + print_uuid(&fields->uuids128[i].u); + MODLOG_DFLT(DEBUG, " "); + } + MODLOG_DFLT(DEBUG, "\n"); + } + + if (fields->name != NULL) { + assert(fields->name_len < sizeof s - 1); + memcpy(s, fields->name, fields->name_len); + s[fields->name_len] = '\0'; + MODLOG_DFLT(DEBUG, " name(%scomplete)=%s\n", + fields->name_is_complete ? "" : "in", s); + } + + if (fields->tx_pwr_lvl_is_present) { + MODLOG_DFLT(DEBUG, " tx_pwr_lvl=%d\n", fields->tx_pwr_lvl); + } + + if (fields->slave_itvl_range != NULL) { + MODLOG_DFLT(DEBUG, " slave_itvl_range="); + print_bytes(fields->slave_itvl_range, BLE_HS_ADV_SLAVE_ITVL_RANGE_LEN); + MODLOG_DFLT(DEBUG, "\n"); + } + + if (fields->svc_data_uuid16 != NULL) { + MODLOG_DFLT(DEBUG, " svc_data_uuid16="); + print_bytes(fields->svc_data_uuid16, fields->svc_data_uuid16_len); + MODLOG_DFLT(DEBUG, "\n"); + } + + if (fields->public_tgt_addr != NULL) { + MODLOG_DFLT(DEBUG, " public_tgt_addr="); + u8p = fields->public_tgt_addr; + for (i = 0; i < fields->num_public_tgt_addrs; i++) { + MODLOG_DFLT(DEBUG, "public_tgt_addr=%s ", addr_str(u8p)); + u8p += BLE_HS_ADV_PUBLIC_TGT_ADDR_ENTRY_LEN; + } + MODLOG_DFLT(DEBUG, "\n"); + } + + if (fields->appearance_is_present) { + MODLOG_DFLT(DEBUG, " appearance=0x%04x\n", fields->appearance); + } + + if (fields->adv_itvl_is_present) { + MODLOG_DFLT(DEBUG, " adv_itvl=0x%04x\n", fields->adv_itvl); + } + + if (fields->svc_data_uuid32 != NULL) { + MODLOG_DFLT(DEBUG, " svc_data_uuid32="); + print_bytes(fields->svc_data_uuid32, fields->svc_data_uuid32_len); + MODLOG_DFLT(DEBUG, "\n"); + } + + if (fields->svc_data_uuid128 != NULL) { + MODLOG_DFLT(DEBUG, " svc_data_uuid128="); + print_bytes(fields->svc_data_uuid128, fields->svc_data_uuid128_len); + MODLOG_DFLT(DEBUG, "\n"); + } + + if (fields->uri != NULL) { + MODLOG_DFLT(DEBUG, " uri="); + print_bytes(fields->uri, fields->uri_len); + MODLOG_DFLT(DEBUG, "\n"); + } + + if (fields->mfg_data != NULL) { + MODLOG_DFLT(DEBUG, " mfg_data="); + print_bytes(fields->mfg_data, fields->mfg_data_len); + MODLOG_DFLT(DEBUG, "\n"); + } +} diff --git a/apps/cs_initiator/src/peer.c b/apps/cs_initiator/src/peer.c new file mode 100644 index 0000000000..db83080799 --- /dev/null +++ b/apps/cs_initiator/src/peer.c @@ -0,0 +1,807 @@ +/* + * 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 +#include +#include "host/ble_hs.h" +#include "cs_initiator.h" + +static void *peer_svc_mem; +static struct os_mempool peer_svc_pool; + +static void *peer_chr_mem; +static struct os_mempool peer_chr_pool; + +static void *peer_dsc_mem; +static struct os_mempool peer_dsc_pool; + +static void *peer_mem; +static struct os_mempool peer_pool; +static SLIST_HEAD(, peer) peers; + +static struct peer_svc * +peer_svc_find_range(struct peer *peer, uint16_t attr_handle); +static struct peer_svc * +peer_svc_find(struct peer *peer, uint16_t svc_start_handle, + struct peer_svc **out_prev); +int +peer_svc_is_empty(const struct peer_svc *svc); + +uint16_t +chr_end_handle(const struct peer_svc *svc, const struct peer_chr *chr); +int +chr_is_empty(const struct peer_svc *svc, const struct peer_chr *chr); +static struct peer_chr * +peer_chr_find(const struct peer_svc *svc, uint16_t chr_def_handle, + struct peer_chr **out_prev); +static void +peer_disc_chrs(struct peer *peer); + +static int +peer_dsc_disced(uint16_t conn_handle, const struct ble_gatt_error *error, + uint16_t chr_val_handle, const struct ble_gatt_dsc *dsc, + void *arg); + +static struct peer * +peer_find(uint16_t conn_handle) +{ + struct peer *peer; + + SLIST_FOREACH(peer, &peers, next) { + if (peer->conn_handle == conn_handle) { + return peer; + } + } + + return NULL; +} + +static void +peer_disc_complete(struct peer *peer, int rc) +{ + peer->disc_prev_chr_val = 0; + + /* Notify caller that discovery has completed. */ + if (peer->disc_cb != NULL) { + peer->disc_cb(peer, rc, peer->disc_cb_arg); + } +} + +static struct peer_dsc * +peer_dsc_find_prev(const struct peer_chr *chr, uint16_t dsc_handle) +{ + struct peer_dsc *prev; + struct peer_dsc *dsc; + + prev = NULL; + SLIST_FOREACH(dsc, &chr->dscs, next) { + if (dsc->dsc.handle >= dsc_handle) { + break; + } + + prev = dsc; + } + + return prev; +} + +static struct peer_dsc * +peer_dsc_find(const struct peer_chr *chr, uint16_t dsc_handle, + struct peer_dsc **out_prev) +{ + struct peer_dsc *prev; + struct peer_dsc *dsc; + + prev = peer_dsc_find_prev(chr, dsc_handle); + if (prev == NULL) { + dsc = SLIST_FIRST(&chr->dscs); + } else { + dsc = SLIST_NEXT(prev, next); + } + + if (dsc != NULL && dsc->dsc.handle != dsc_handle) { + dsc = NULL; + } + + if (out_prev != NULL) { + *out_prev = prev; + } + return dsc; +} + +static int +peer_dsc_add(struct peer *peer, uint16_t chr_val_handle, + const struct ble_gatt_dsc *gatt_dsc) +{ + struct peer_dsc *prev; + struct peer_dsc *dsc; + struct peer_svc *svc; + struct peer_chr *chr; + + svc = peer_svc_find_range(peer, chr_val_handle); + if (svc == NULL) { + /* Can't find service for discovered descriptor; this shouldn't + * happen. + */ + assert(0); + return BLE_HS_EUNKNOWN; + } + + chr = peer_chr_find(svc, chr_val_handle, NULL); + if (chr == NULL) { + /* Can't find characteristic for discovered descriptor; this shouldn't + * happen. + */ + assert(0); + return BLE_HS_EUNKNOWN; + } + + dsc = peer_dsc_find(chr, gatt_dsc->handle, &prev); + if (dsc != NULL) { + /* Descriptor already discovered. */ + return 0; + } + + dsc = os_memblock_get(&peer_dsc_pool); + if (dsc == NULL) { + /* Out of memory. */ + return BLE_HS_ENOMEM; + } + memset(dsc, 0, sizeof *dsc); + + dsc->dsc = *gatt_dsc; + + if (prev == NULL) { + SLIST_INSERT_HEAD(&chr->dscs, dsc, next); + } else { + SLIST_NEXT(prev, next) = dsc; + } + + return 0; +} + +static void +peer_disc_dscs(struct peer *peer) +{ + struct peer_chr *chr; + struct peer_svc *svc; + int rc; + + /* Search through the list of discovered characteristics for the first + * characteristic that contains undiscovered descriptors. Then, discover + * all descriptors belonging to that characteristic. + */ + SLIST_FOREACH(svc, &peer->svcs, next) { + SLIST_FOREACH(chr, &svc->chrs, next) { + if (!chr_is_empty(svc, chr) && + SLIST_EMPTY(&chr->dscs) && + peer->disc_prev_chr_val <= chr->chr.def_handle) { + + rc = ble_gattc_disc_all_dscs(peer->conn_handle, + chr->chr.val_handle, + chr_end_handle(svc, chr), + peer_dsc_disced, peer); + if (rc != 0) { + peer_disc_complete(peer, rc); + } + + peer->disc_prev_chr_val = chr->chr.val_handle; + return; + } + } + } + + /* All descriptors discovered. */ + peer_disc_complete(peer, 0); +} + +static int +peer_dsc_disced(uint16_t conn_handle, const struct ble_gatt_error *error, + uint16_t chr_val_handle, const struct ble_gatt_dsc *dsc, + void *arg) +{ + struct peer *peer; + int rc; + + peer = arg; + assert(peer->conn_handle == conn_handle); + + switch (error->status) { + case 0: + rc = peer_dsc_add(peer, chr_val_handle, dsc); + break; + + case BLE_HS_EDONE: + /* All descriptors in this characteristic discovered; start discovering + * descriptors in the next characteristic. + */ + if (peer->disc_prev_chr_val > 0) { + peer_disc_dscs(peer); + } + rc = 0; + break; + + default: + /* Error; abort discovery. */ + rc = error->status; + break; + } + + if (rc != 0) { + /* Error; abort discovery. */ + peer_disc_complete(peer, rc); + } + + return rc; +} + +uint16_t +chr_end_handle(const struct peer_svc *svc, const struct peer_chr *chr) +{ + const struct peer_chr *next_chr; + + next_chr = SLIST_NEXT(chr, next); + if (next_chr != NULL) { + return next_chr->chr.def_handle - 1; + } else { + return svc->svc.end_handle; + } +} + +int +chr_is_empty(const struct peer_svc *svc, const struct peer_chr *chr) +{ + return chr_end_handle(svc, chr) <= chr->chr.val_handle; +} + +static struct peer_chr * +peer_chr_find_prev(const struct peer_svc *svc, uint16_t chr_val_handle) +{ + struct peer_chr *prev; + struct peer_chr *chr; + + prev = NULL; + SLIST_FOREACH(chr, &svc->chrs, next) { + if (chr->chr.val_handle >= chr_val_handle) { + break; + } + + prev = chr; + } + + return prev; +} + +static struct peer_chr * +peer_chr_find(const struct peer_svc *svc, uint16_t chr_val_handle, + struct peer_chr **out_prev) +{ + struct peer_chr *prev; + struct peer_chr *chr; + + prev = peer_chr_find_prev(svc, chr_val_handle); + if (prev == NULL) { + chr = SLIST_FIRST(&svc->chrs); + } else { + chr = SLIST_NEXT(prev, next); + } + + if (chr != NULL && chr->chr.val_handle != chr_val_handle) { + chr = NULL; + } + + if (out_prev != NULL) { + *out_prev = prev; + } + return chr; +} + +static void +peer_chr_delete(struct peer_chr *chr) +{ + struct peer_dsc *dsc; + + while ((dsc = SLIST_FIRST(&chr->dscs)) != NULL) { + SLIST_REMOVE_HEAD(&chr->dscs, next); + os_memblock_put(&peer_dsc_pool, dsc); + } + + os_memblock_put(&peer_chr_pool, chr); +} + +static int +peer_chr_add(struct peer *peer, uint16_t svc_start_handle, + const struct ble_gatt_chr *gatt_chr) +{ + struct peer_chr *prev; + struct peer_chr *chr; + struct peer_svc *svc; + + svc = peer_svc_find(peer, svc_start_handle, NULL); + if (svc == NULL) { + /* Can't find service for discovered characteristic; this shouldn't + * happen. + */ + assert(0); + return BLE_HS_EUNKNOWN; + } + + chr = peer_chr_find(svc, gatt_chr->def_handle, &prev); + if (chr != NULL) { + /* Characteristic already discovered. */ + return 0; + } + + chr = os_memblock_get(&peer_chr_pool); + if (chr == NULL) { + /* Out of memory. */ + return BLE_HS_ENOMEM; + } + memset(chr, 0, sizeof *chr); + + chr->chr = *gatt_chr; + + if (prev == NULL) { + SLIST_INSERT_HEAD(&svc->chrs, chr, next); + } else { + SLIST_NEXT(prev, next) = chr; + } + + return 0; +} + +static int +peer_chr_disced(uint16_t conn_handle, const struct ble_gatt_error *error, + const struct ble_gatt_chr *chr, void *arg) +{ + struct peer *peer; + int rc; + + peer = arg; + assert(peer->conn_handle == conn_handle); + + switch (error->status) { + case 0: + rc = peer_chr_add(peer, peer->cur_svc->svc.start_handle, chr); + break; + + case BLE_HS_EDONE: + /* All characteristics in this service discovered; start discovering + * characteristics in the next service. + */ + if (peer->disc_prev_chr_val > 0) { + peer_disc_chrs(peer); + } + rc = 0; + break; + + default: + rc = error->status; + break; + } + + if (rc != 0) { + /* Error; abort discovery. */ + peer_disc_complete(peer, rc); + } + + return rc; +} + +static void +peer_disc_chrs(struct peer *peer) +{ + struct peer_svc *svc; + int rc; + + /* Search through the list of discovered service for the first service that + * contains undiscovered characteristics. Then, discover all + * characteristics belonging to that service. + */ + SLIST_FOREACH(svc, &peer->svcs, next) { + if (!peer_svc_is_empty(svc) && SLIST_EMPTY(&svc->chrs)) { + peer->cur_svc = svc; + rc = ble_gattc_disc_all_chrs(peer->conn_handle, + svc->svc.start_handle, + svc->svc.end_handle, + peer_chr_disced, peer); + if (rc != 0) { + peer_disc_complete(peer, rc); + } + return; + } + } + + /* All characteristics discovered. */ + peer_disc_dscs(peer); +} + +int +peer_svc_is_empty(const struct peer_svc *svc) +{ + return svc->svc.end_handle <= svc->svc.start_handle; +} + +static struct peer_svc * +peer_svc_find_prev(struct peer *peer, uint16_t svc_start_handle) +{ + struct peer_svc *prev; + struct peer_svc *svc; + + prev = NULL; + SLIST_FOREACH(svc, &peer->svcs, next) { + if (svc->svc.start_handle >= svc_start_handle) { + break; + } + + prev = svc; + } + + return prev; +} + +static struct peer_svc * +peer_svc_find(struct peer *peer, uint16_t svc_start_handle, + struct peer_svc **out_prev) +{ + struct peer_svc *prev; + struct peer_svc *svc; + + prev = peer_svc_find_prev(peer, svc_start_handle); + if (prev == NULL) { + svc = SLIST_FIRST(&peer->svcs); + } else { + svc = SLIST_NEXT(prev, next); + } + + if (svc != NULL && svc->svc.start_handle != svc_start_handle) { + svc = NULL; + } + + if (out_prev != NULL) { + *out_prev = prev; + } + return svc; +} + +static struct peer_svc * +peer_svc_find_range(struct peer *peer, uint16_t attr_handle) +{ + struct peer_svc *svc; + + SLIST_FOREACH(svc, &peer->svcs, next) { + if (svc->svc.start_handle <= attr_handle && + svc->svc.end_handle >= attr_handle) { + + return svc; + } + } + + return NULL; +} + +const struct peer_svc * +peer_svc_find_uuid(const struct peer *peer, const ble_uuid_t *uuid) +{ + const struct peer_svc *svc; + + SLIST_FOREACH(svc, &peer->svcs, next) { + if (ble_uuid_cmp(&svc->svc.uuid.u, uuid) == 0) { + return svc; + } + } + + return NULL; +} + +const struct peer_chr * +peer_chr_find_uuid(const struct peer *peer, const ble_uuid_t *svc_uuid, + const ble_uuid_t *chr_uuid) +{ + const struct peer_svc *svc; + const struct peer_chr *chr; + + svc = peer_svc_find_uuid(peer, svc_uuid); + if (svc == NULL) { + return NULL; + } + + SLIST_FOREACH(chr, &svc->chrs, next) { + if (ble_uuid_cmp(&chr->chr.uuid.u, chr_uuid) == 0) { + return chr; + } + } + + return NULL; +} + +const struct peer_dsc * +peer_dsc_find_uuid(const struct peer *peer, const ble_uuid_t *svc_uuid, + const ble_uuid_t *chr_uuid, const ble_uuid_t *dsc_uuid) +{ + const struct peer_chr *chr; + const struct peer_dsc *dsc; + + chr = peer_chr_find_uuid(peer, svc_uuid, chr_uuid); + if (chr == NULL) { + return NULL; + } + + SLIST_FOREACH(dsc, &chr->dscs, next) { + if (ble_uuid_cmp(&dsc->dsc.uuid.u, dsc_uuid) == 0) { + return dsc; + } + } + + return NULL; +} + +static int +peer_svc_add(struct peer *peer, const struct ble_gatt_svc *gatt_svc) +{ + struct peer_svc *prev; + struct peer_svc *svc; + + svc = peer_svc_find(peer, gatt_svc->start_handle, &prev); + if (svc != NULL) { + /* Service already discovered. */ + return 0; + } + + svc = os_memblock_get(&peer_svc_pool); + if (svc == NULL) { + /* Out of memory. */ + return BLE_HS_ENOMEM; + } + memset(svc, 0, sizeof *svc); + + svc->svc = *gatt_svc; + SLIST_INIT(&svc->chrs); + + if (prev == NULL) { + SLIST_INSERT_HEAD(&peer->svcs, svc, next); + } else { + SLIST_INSERT_AFTER(prev, svc, next); + } + + return 0; +} + +static void +peer_svc_delete(struct peer_svc *svc) +{ + struct peer_chr *chr; + + while ((chr = SLIST_FIRST(&svc->chrs)) != NULL) { + SLIST_REMOVE_HEAD(&svc->chrs, next); + peer_chr_delete(chr); + } + + os_memblock_put(&peer_svc_pool, svc); +} + +static int +peer_svc_disced(uint16_t conn_handle, const struct ble_gatt_error *error, + const struct ble_gatt_svc *service, void *arg) +{ + struct peer *peer; + int rc; + + peer = arg; + assert(peer->conn_handle == conn_handle); + + switch (error->status) { + case 0: + rc = peer_svc_add(peer, service); + break; + + case BLE_HS_EDONE: + /* All services discovered; start discovering characteristics. */ + if (peer->disc_prev_chr_val > 0) { + peer_disc_chrs(peer); + } + rc = 0; + break; + + default: + rc = error->status; + break; + } + + if (rc != 0) { + /* Error; abort discovery. */ + peer_disc_complete(peer, rc); + } + + return rc; +} + + +int +peer_disc_all(uint16_t conn_handle, peer_disc_fn *disc_cb, void *disc_cb_arg) +{ + struct peer_svc *svc; + struct peer *peer; + int rc; + + peer = peer_find(conn_handle); + if (peer == NULL) { + return BLE_HS_ENOTCONN; + } + + /* Undiscover everything first. */ + while ((svc = SLIST_FIRST(&peer->svcs)) != NULL) { + SLIST_REMOVE_HEAD(&peer->svcs, next); + peer_svc_delete(svc); + } + + peer->disc_prev_chr_val = 1; + peer->disc_cb = disc_cb; + peer->disc_cb_arg = disc_cb_arg; + + rc = ble_gattc_disc_all_svcs(conn_handle, peer_svc_disced, peer); + if (rc != 0) { + return rc; + } + + return 0; +} + +int +peer_delete(uint16_t conn_handle) +{ + struct peer_svc *svc; + struct peer *peer; + int rc; + + peer = peer_find(conn_handle); + if (peer == NULL) { + return BLE_HS_ENOTCONN; + } + + SLIST_REMOVE(&peers, peer, peer, next); + + while ((svc = SLIST_FIRST(&peer->svcs)) != NULL) { + SLIST_REMOVE_HEAD(&peer->svcs, next); + peer_svc_delete(svc); + } + + rc = os_memblock_put(&peer_pool, peer); + if (rc != 0) { + return BLE_HS_EOS; + } + + return 0; +} + +int +peer_add(uint16_t conn_handle) +{ + struct peer *peer; + + /* Make sure the connection handle is unique. */ + peer = peer_find(conn_handle); + if (peer != NULL) { + return BLE_HS_EALREADY; + } + + peer = os_memblock_get(&peer_pool); + if (peer == NULL) { + /* Out of memory. */ + return BLE_HS_ENOMEM; + } + + memset(peer, 0, sizeof *peer); + peer->conn_handle = conn_handle; + + SLIST_INSERT_HEAD(&peers, peer, next); + + return 0; +} + +static void +peer_free_mem(void) +{ + free(peer_mem); + peer_mem = NULL; + + free(peer_svc_mem); + peer_svc_mem = NULL; + + free(peer_chr_mem); + peer_chr_mem = NULL; + + free(peer_dsc_mem); + peer_dsc_mem = NULL; +} + +int +peer_init(int max_peers, int max_svcs, int max_chrs, int max_dscs) +{ + int rc; + + /* Free memory first in case this function gets called more than once. */ + peer_free_mem(); + + peer_mem = malloc( + OS_MEMPOOL_BYTES(max_peers, sizeof (struct peer))); + if (peer_mem == NULL) { + rc = BLE_HS_ENOMEM; + goto err; + } + + rc = os_mempool_init(&peer_pool, max_peers, + sizeof (struct peer), peer_mem, + "peer_pool"); + if (rc != 0) { + rc = BLE_HS_EOS; + goto err; + } + + peer_svc_mem = malloc( + OS_MEMPOOL_BYTES(max_svcs, sizeof (struct peer_svc))); + if (peer_svc_mem == NULL) { + rc = BLE_HS_ENOMEM; + goto err; + } + + rc = os_mempool_init(&peer_svc_pool, max_svcs, + sizeof (struct peer_svc), peer_svc_mem, + "peer_svc_pool"); + if (rc != 0) { + rc = BLE_HS_EOS; + goto err; + } + + peer_chr_mem = malloc( + OS_MEMPOOL_BYTES(max_chrs, sizeof (struct peer_chr))); + if (peer_chr_mem == NULL) { + rc = BLE_HS_ENOMEM; + goto err; + } + + rc = os_mempool_init(&peer_chr_pool, max_chrs, + sizeof (struct peer_chr), peer_chr_mem, + "peer_chr_pool"); + if (rc != 0) { + rc = BLE_HS_EOS; + goto err; + } + + peer_dsc_mem = malloc( + OS_MEMPOOL_BYTES(max_dscs, sizeof (struct peer_dsc))); + if (peer_dsc_mem == NULL) { + rc = BLE_HS_ENOMEM; + goto err; + } + + rc = os_mempool_init(&peer_dsc_pool, max_dscs, + sizeof (struct peer_dsc), peer_dsc_mem, + "peer_dsc_pool"); + if (rc != 0) { + rc = BLE_HS_EOS; + goto err; + } + + return 0; + +err: + peer_free_mem(); + return rc; +} diff --git a/apps/cs_initiator/syscfg.yml b/apps/cs_initiator/syscfg.yml new file mode 100644 index 0000000000..3e1759c906 --- /dev/null +++ b/apps/cs_initiator/syscfg.yml @@ -0,0 +1,45 @@ +# 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. +# + +syscfg.vals: + CONSOLE_IMPLEMENTATION: full + LOG_IMPLEMENTATION: full + STATS_IMPLEMENTATION: full + + # DEBUG logging is a bit noisy; use INFO. + LOG_LEVEL: 1 + + # Default task settings + OS_MAIN_STACK_SIZE: 336 + +# Disable peripheral and broadcaster roles. + BLE_ROLE_BROADCASTER: 0 + BLE_ROLE_CENTRAL: 1 + BLE_ROLE_OBSERVER: 1 + BLE_ROLE_PERIPHERAL: 0 + + # Whether to save data to sys/config, or just keep it in RAM. + BLE_STORE_CONFIG_PERSIST: 0 + + BLE_VERSION: 54 + BLE_CHANNEL_SOUNDING: 1 + # MBEDTLS_CTR_DRBG_C: 1 + QUASI_CHANNEL_SOUNDING: 1 + +syscfg.vals.BLE_CONTROLLER: + TIMER_3: 1 diff --git a/apps/cs_reflector/pkg.yml b/apps/cs_reflector/pkg.yml new file mode 100644 index 0000000000..7b2af375cc --- /dev/null +++ b/apps/cs_reflector/pkg.yml @@ -0,0 +1,45 @@ +# 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: apps/cs_reflector +pkg.type: app +pkg.description: Simple BLE peripheral application. +pkg.author: "Apache Mynewt " +pkg.homepage: "http://mynewt.apache.org/" +pkg.keywords: + +pkg.deps: + - "@apache-mynewt-core/boot/split" + - "@mcuboot/boot/bootutil" + - "@apache-mynewt-core/kernel/os" + - "@apache-mynewt-core/mgmt/imgmgr" + - "@apache-mynewt-core/mgmt/smp" + - "@apache-mynewt-core/mgmt/smp/transport/ble" + - "@apache-mynewt-core/sys/console" + - "@apache-mynewt-core/sys/log" + - "@apache-mynewt-core/sys/log/modlog" + - "@apache-mynewt-core/sys/stats" + - "@apache-mynewt-core/sys/sysinit" + - "@apache-mynewt-core/sys/id" + - "@apache-mynewt-core/test/testutil" + - nimble/host + - nimble/host/services/ans + - nimble/host/services/dis + - nimble/host/services/gap + - nimble/host/services/gatt + - nimble/host/store/config + - nimble/host/util diff --git a/apps/cs_reflector/src/cs_reflector.h b/apps/cs_reflector/src/cs_reflector.h new file mode 100644 index 0000000000..72a1d28489 --- /dev/null +++ b/apps/cs_reflector/src/cs_reflector.h @@ -0,0 +1,58 @@ +/* + * 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 H_CS_REFLECTOR_ +#define H_CS_REFLECTOR_ + +#include +#include "nimble/ble.h" +#include "modlog/modlog.h" +#ifdef __cplusplus +extern "C" { +#endif + +struct ble_hs_cfg; +struct ble_gatt_register_ctxt; + +/** GATT server. */ +#define GATT_SVR_SVC_CHANNEL_SOUNDING_UUID 0xffff +#define GATT_SVR_CHR_TOD_TOA_UUID 0xfffe + +void gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg); +int gatt_svr_init(void); +int gatt_svr_indicate_toa_tod(uint16_t conn_handle, uint32_t toa_tod_val); + +/* PHY support */ +#if MYNEWT_VAL(CS_REFLECTOR_LE_PHY_SUPPORT) +#define CONN_HANDLE_INVALID 0xffff + +void phy_init(void); +void phy_conn_changed(uint16_t handle); +void phy_update(uint8_t phy); +#endif + +/** Misc. */ +void print_bytes(const uint8_t *bytes, int len); +void print_addr(const void *addr); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/apps/cs_reflector/src/gatt_svr.c b/apps/cs_reflector/src/gatt_svr.c new file mode 100644 index 0000000000..a637a95cab --- /dev/null +++ b/apps/cs_reflector/src/gatt_svr.c @@ -0,0 +1,167 @@ +/* + * 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 +#include +#include +#include "bsp/bsp.h" +#include "host/ble_hs.h" +#include "host/ble_uuid.h" +#include "cs_reflector.h" + +/* 50f89303-7cd9-4e86-9b66-2c7441ac3cd9 */ +static const ble_uuid128_t gatt_svc_toa_tod_uuid = + BLE_UUID128_INIT(0xd9, 0x3c, 0xac, 0x41, 0x74, 0x2c, 0x66, 0x9b, + 0x86, 0x4e, 0xd9, 0x7c, 0x03, 0x93, 0xf8, 0x50); + +/* 50f89303-7cd9-4e86-9b66-2c7441ac3cda */ +static const ble_uuid128_t gatt_chr_toa_tod_uuid = + BLE_UUID128_INIT(0xda, 0x3c, 0xac, 0x41, 0x74, 0x2c, 0x66, 0x9b, + 0x86, 0x4e, 0xd9, 0x7c, 0x03, 0x93, 0xf8, 0x50); + +static uint16_t toa_tod_val_handle; + +static int +gatt_access_cb(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, + void *arg); + +static const struct ble_gatt_svc_def gatt_svr_svcs[] = { + { + /*** Service: ToA_ToD samples */ + .type = BLE_GATT_SVC_TYPE_PRIMARY, + .uuid = &gatt_svc_toa_tod_uuid.u, + .characteristics = (struct ble_gatt_chr_def[]) { + { + /* Characteristic: indicate ToA_ToD samples */ + .uuid = &gatt_chr_toa_tod_uuid.u, + .access_cb = gatt_access_cb, + .val_handle = &toa_tod_val_handle, + .flags = BLE_GATT_CHR_F_INDICATE, + }, + { + 0, /* No more characteristics in this service. */ + } + } + }, + + { + 0, /* No more services. */ + }, +}; + + +static int +gatt_access_cb(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt, void *arg) +{ + const ble_uuid_t *uuid; + + uuid = ctxt->chr->uuid; + + /* Determine which characteristic is being accessed by examining its + * 128-bit UUID. + */ + + if (ble_uuid_cmp(uuid, &gatt_svc_toa_tod_uuid.u) == 0) { + if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) { + return BLE_ATT_ERR_READ_NOT_PERMITTED; + } + + if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { + return BLE_ATT_ERR_WRITE_NOT_PERMITTED; + } + } + + return BLE_ATT_ERR_UNLIKELY; +} + +void +gatt_svr_register_cb(struct ble_gatt_register_ctxt *ctxt, void *arg) +{ + char buf[BLE_UUID_STR_LEN]; + + switch (ctxt->op) { + case BLE_GATT_REGISTER_OP_SVC: + MODLOG_DFLT(DEBUG, "registered service %s with handle=%d\n", + ble_uuid_to_str(ctxt->svc.svc_def->uuid, buf), + ctxt->svc.handle); + break; + + case BLE_GATT_REGISTER_OP_CHR: + MODLOG_DFLT(DEBUG, "registering characteristic %s with " + "def_handle=%d val_handle=%d\n", + ble_uuid_to_str(ctxt->chr.chr_def->uuid, buf), + ctxt->chr.def_handle, + ctxt->chr.val_handle); + break; + + case BLE_GATT_REGISTER_OP_DSC: + MODLOG_DFLT(DEBUG, "registering descriptor %s with handle=%d\n", + ble_uuid_to_str(ctxt->dsc.dsc_def->uuid, buf), + ctxt->dsc.handle); + break; + + default: + assert(0); + break; + } +} + +int +gatt_svr_indicate_toa_tod(uint16_t conn_handle, uint32_t toa_tod_val) +{ + int rc; + struct os_mbuf *om; + uint8_t toa_tod_buf[sizeof(toa_tod_val)]; + + om = os_msys_get_pkthdr(sizeof(toa_tod_buf), 0); + if (!om) { + return 1; + } + + put_le32(toa_tod_buf, toa_tod_val); + + rc = os_mbuf_append(om, toa_tod_buf, sizeof(toa_tod_buf)); + if (!om) { + return 1; + } + + rc = ble_gatts_indicate_custom(conn_handle, toa_tod_val_handle, om); + + return rc; +} + +int +gatt_svr_init(void) +{ + int rc; + + rc = ble_gatts_count_cfg(gatt_svr_svcs); + if (rc != 0) { + return rc; + } + + rc = ble_gatts_add_svcs(gatt_svr_svcs); + if (rc != 0) { + return rc; + } + + return 0; +} diff --git a/apps/cs_reflector/src/main.c b/apps/cs_reflector/src/main.c new file mode 100644 index 0000000000..dcbefcd281 --- /dev/null +++ b/apps/cs_reflector/src/main.c @@ -0,0 +1,380 @@ +/* + * 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 +#include +#include +#include +#include "os/mynewt.h" +#include "bsp/bsp.h" +#include "hal/hal_gpio.h" +#include "console/console.h" +#include "hal/hal_system.h" +#include "config/config.h" +#include "split/split.h" +#if MYNEWT_VAL(BLE_SVC_DIS_FIRMWARE_REVISION_READ_PERM) >= 0 +#include "bootutil/image.h" +#include "imgmgr/imgmgr.h" +#include "services/dis/ble_svc_dis.h" +#endif + +/* BLE */ +#include "nimble/ble.h" +#include "host/ble_hs.h" +#include "host/ble_cs.h" +#include "host/util/util.h" +#include "services/gap/ble_svc_gap.h" + +/* Application-specified header. */ +#include "cs_reflector.h" + +static int cs_reflector_gap_event(struct ble_gap_event *event, void *arg); + +static int +cs_reflector_cs_event(struct ble_cs_event *event, void *arg) +{ + int rc; + + switch (event->type) { + case BLE_CS_EVENT_CS_PROCEDURE_COMPLETE: + gatt_svr_indicate_toa_tod(event->procedure_complete.conn_handle, + event->procedure_complete.time_diff_ns); + return 0; + default: + return 0; + } +} + +/** + * Logs information about a connection to the console. + */ +static void +cs_reflector_print_conn_desc(struct ble_gap_conn_desc *desc) +{ + MODLOG_DFLT(INFO, "handle=%d our_ota_addr_type=%d our_ota_addr=", + desc->conn_handle, desc->our_ota_addr.type); + print_addr(desc->our_ota_addr.val); + MODLOG_DFLT(INFO, " our_id_addr_type=%d our_id_addr=", + desc->our_id_addr.type); + print_addr(desc->our_id_addr.val); + MODLOG_DFLT(INFO, " peer_ota_addr_type=%d peer_ota_addr=", + desc->peer_ota_addr.type); + print_addr(desc->peer_ota_addr.val); + MODLOG_DFLT(INFO, " peer_id_addr_type=%d peer_id_addr=", + desc->peer_id_addr.type); + print_addr(desc->peer_id_addr.val); + MODLOG_DFLT(INFO, " conn_itvl=%d conn_latency=%d supervision_timeout=%d " + "encrypted=%d authenticated=%d bonded=%d\n", + desc->conn_itvl, desc->conn_latency, + desc->supervision_timeout, + desc->sec_state.encrypted, + desc->sec_state.authenticated, + desc->sec_state.bonded); +} + +/** + * Enables advertising with the following parameters: + * o General discoverable mode. + * o Undirected connectable mode. + */ +static void +cs_reflector_advertise(void) +{ + uint8_t own_addr_type; + struct ble_gap_adv_params adv_params; + struct ble_hs_adv_fields fields; + const char *name; + int rc; + + /* Figure out address to use while advertising (no privacy for now) */ + rc = ble_hs_id_infer_auto(0, &own_addr_type); + if (rc != 0) { + MODLOG_DFLT(ERROR, "error determining address type; rc=%d\n", rc); + return; + } + + /** + * Set the advertisement data included in our advertisements: + * o Flags (indicates advertisement type and other general info). + * o Advertising tx power. + * o Device name. + * o 16-bit service UUIDs (alert notifications). + */ + + memset(&fields, 0, sizeof fields); + + /* Advertise two flags: + * o Discoverability in forthcoming advertisement (general) + * o BLE-only (BR/EDR unsupported). + */ + fields.flags = BLE_HS_ADV_F_DISC_GEN | + BLE_HS_ADV_F_BREDR_UNSUP; + + /* Indicate that the TX power level field should be included; have the + * stack fill this value automatically. This is done by assiging the + * special value BLE_HS_ADV_TX_PWR_LVL_AUTO. + */ + fields.tx_pwr_lvl_is_present = 1; + fields.tx_pwr_lvl = BLE_HS_ADV_TX_PWR_LVL_AUTO; + + name = ble_svc_gap_device_name(); + fields.name = (uint8_t *)name; + fields.name_len = strlen(name); + fields.name_is_complete = 1; + + fields.uuids16 = (ble_uuid16_t[]) { + BLE_UUID16_INIT(GATT_SVR_SVC_CHANNEL_SOUNDING_UUID) + }; + fields.num_uuids16 = 1; + fields.uuids16_is_complete = 1; + + rc = ble_gap_adv_set_fields(&fields); + if (rc != 0) { + MODLOG_DFLT(ERROR, "error setting advertisement data; rc=%d\n", rc); + return; + } + + /* Begin advertising. */ + memset(&adv_params, 0, sizeof adv_params); + adv_params.conn_mode = BLE_GAP_CONN_MODE_UND; + adv_params.disc_mode = BLE_GAP_DISC_MODE_GEN; + rc = ble_gap_adv_start(own_addr_type, NULL, BLE_HS_FOREVER, + &adv_params, cs_reflector_gap_event, NULL); + if (rc != 0) { + MODLOG_DFLT(ERROR, "error enabling advertisement; rc=%d\n", rc); + return; + } +} + +/** + * The nimble host executes this callback when a GAP event occurs. The + * application associates a GAP event callback with each connection that forms. + * cs_reflector uses the same callback for all connections. + * + * @param event The type of event being signalled. + * @param ctxt Various information pertaining to the event. + * @param arg Application-specified argument; unuesd by + * cs_reflector. + * + * @return 0 if the application successfully handled the + * event; nonzero on failure. The semantics + * of the return code is specific to the + * particular GAP event being signalled. + */ +static int +cs_reflector_gap_event(struct ble_gap_event *event, void *arg) +{ + struct ble_gap_conn_desc desc; + int rc; + + switch (event->type) { + case BLE_GAP_EVENT_CONNECT: + /* A new connection was established or a connection attempt failed. */ + MODLOG_DFLT(INFO, "connection %s; status=%d ", + event->connect.status == 0 ? "established" : "failed", + event->connect.status); + if (event->connect.status == 0) { + rc = ble_gap_conn_find(event->connect.conn_handle, &desc); + assert(rc == 0); + cs_reflector_print_conn_desc(&desc); + +#if MYNEWT_VAL(CS_REFLECTOR_LE_PHY_SUPPORT) + phy_conn_changed(event->connect.conn_handle); +#endif + } + MODLOG_DFLT(INFO, "\n"); + + if (event->connect.status != 0) { + /* Connection failed; resume advertising. */ + cs_reflector_advertise(); + } + return 0; + + case BLE_GAP_EVENT_DISCONNECT: + MODLOG_DFLT(INFO, "disconnect; reason=%d ", event->disconnect.reason); + cs_reflector_print_conn_desc(&event->disconnect.conn); + MODLOG_DFLT(INFO, "\n"); + +#if MYNEWT_VAL(CS_REFLECTOR_LE_PHY_SUPPORT) + phy_conn_changed(CONN_HANDLE_INVALID); +#endif + + /* Connection terminated; resume advertising. */ + cs_reflector_advertise(); + return 0; + + case BLE_GAP_EVENT_CONN_UPDATE: + /* The central has updated the connection parameters. */ + MODLOG_DFLT(INFO, "connection updated; status=%d ", + event->conn_update.status); + rc = ble_gap_conn_find(event->conn_update.conn_handle, &desc); + assert(rc == 0); + cs_reflector_print_conn_desc(&desc); + MODLOG_DFLT(INFO, "\n"); + return 0; + + case BLE_GAP_EVENT_ADV_COMPLETE: + MODLOG_DFLT(INFO, "advertise complete; reason=%d", + event->adv_complete.reason); + cs_reflector_advertise(); + return 0; + + case BLE_GAP_EVENT_ENC_CHANGE: + /* Encryption has been enabled or disabled for this connection. */ + MODLOG_DFLT(INFO, "encryption change event; status=%d ", + event->enc_change.status); + rc = ble_gap_conn_find(event->connect.conn_handle, &desc); + assert(rc == 0); + cs_reflector_print_conn_desc(&desc); + MODLOG_DFLT(INFO, "\n"); + return 0; + + case BLE_GAP_EVENT_SUBSCRIBE: + MODLOG_DFLT(INFO, "subscribe event; conn_handle=%d attr_handle=%d " + "reason=%d prevn=%d curn=%d previ=%d curi=%d\n", + event->subscribe.conn_handle, + event->subscribe.attr_handle, + event->subscribe.reason, + event->subscribe.prev_notify, + event->subscribe.cur_notify, + event->subscribe.prev_indicate, + event->subscribe.cur_indicate); + return 0; + + case BLE_GAP_EVENT_MTU: + MODLOG_DFLT(INFO, "mtu update event; conn_handle=%d cid=%d mtu=%d\n", + event->mtu.conn_handle, + event->mtu.channel_id, + event->mtu.value); + return 0; + + case BLE_GAP_EVENT_REPEAT_PAIRING: + /* We already have a bond with the peer, but it is attempting to + * establish a new secure link. This app sacrifices security for + * convenience: just throw away the old bond and accept the new link. + */ + + /* Delete the old bond. */ + rc = ble_gap_conn_find(event->repeat_pairing.conn_handle, &desc); + assert(rc == 0); + ble_store_util_delete_peer(&desc.peer_id_addr); + + /* Return BLE_GAP_REPEAT_PAIRING_RETRY to indicate that the host should + * continue with the pairing operation. + */ + return BLE_GAP_REPEAT_PAIRING_RETRY; + +#if MYNEWT_VAL(CS_REFLECTOR_LE_PHY_SUPPORT) + case BLE_GAP_EVENT_PHY_UPDATE_COMPLETE: + /* XXX: assume symmetric phy for now */ + phy_update(event->phy_updated.tx_phy); + return 0; +#endif + } + + return 0; +} + +static void +cs_reflector_on_reset(int reason) +{ + MODLOG_DFLT(ERROR, "Resetting state; reason=%d\n", reason); +} + +static void +cs_reflector_on_sync(void) +{ + int rc; + + /* Make sure we have proper identity address set (public preferred) */ + rc = ble_hs_util_ensure_addr(0); + assert(rc == 0); + + /* Begin advertising. */ + cs_reflector_advertise(); +} + +/** + * main + * + * The main task for the project. This function initializes the packages, + * then starts serving events from default event queue. + * + * @return int NOTE: this function should never return! + */ +int +mynewt_main(int argc, char **argv) +{ +#if MYNEWT_VAL(BLE_SVC_DIS_FIRMWARE_REVISION_READ_PERM) >= 0 + struct image_version ver; + static char ver_str[IMGMGR_NMGR_MAX_VER]; +#endif + int rc; + + /* Initialize OS */ + sysinit(); + + /* Initialize the NimBLE host configuration. */ + ble_hs_cfg.reset_cb = cs_reflector_on_reset; + ble_hs_cfg.sync_cb = cs_reflector_on_sync; + ble_hs_cfg.gatts_register_cb = gatt_svr_register_cb; + ble_hs_cfg.store_status_cb = ble_store_util_status_rr; + + rc = gatt_svr_init(); + assert(rc == 0); + +#if MYNEWT_VAL(BLE_SVC_DIS_FIRMWARE_REVISION_READ_PERM) >= 0 + /* Set firmware version in DIS */ + imgr_my_version(&ver); + imgr_ver_str(&ver, ver_str); + ble_svc_dis_firmware_revision_set(ver_str); +#endif + +#if MYNEWT_VAL(CS_REFLECTOR_LE_PHY_SUPPORT) + phy_init(); +#endif + + conf_load(); + + /* If this app is acting as the loader in a split image setup, jump into + * the second stage application instead of starting the OS. + */ +#if MYNEWT_VAL(SPLIT_LOADER) + { + void *entry; + rc = split_app_go(&entry, true); + if (rc == 0) { + hal_system_start(entry); + } + } +#endif + + struct ble_cs_reflector_setup_params params; + params.cb = cs_reflector_cs_event; + params.cb_arg = NULL; + ble_cs_reflector_setup(¶ms); + + /* + * As the last thing, process events from default event queue. + */ + while (1) { + os_eventq_run(os_eventq_dflt_get()); + } + return 0; +} diff --git a/apps/cs_reflector/src/misc.c b/apps/cs_reflector/src/misc.c new file mode 100644 index 0000000000..3f9b605480 --- /dev/null +++ b/apps/cs_reflector/src/misc.c @@ -0,0 +1,43 @@ +/* + * 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 "cs_reflector.h" + +/** + * Utility function to log an array of bytes. + */ +void +print_bytes(const uint8_t *bytes, int len) +{ + int i; + + for (i = 0; i < len; i++) { + MODLOG_DFLT(INFO, "%s0x%02x", i != 0 ? ":" : "", bytes[i]); + } +} + +void +print_addr(const void *addr) +{ + const uint8_t *u8p; + + u8p = addr; + MODLOG_DFLT(INFO, "%02x:%02x:%02x:%02x:%02x:%02x", + u8p[5], u8p[4], u8p[3], u8p[2], u8p[1], u8p[0]); +} diff --git a/apps/cs_reflector/src/phy.c b/apps/cs_reflector/src/phy.c new file mode 100644 index 0000000000..9dde863142 --- /dev/null +++ b/apps/cs_reflector/src/phy.c @@ -0,0 +1,128 @@ +/* + * 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 "os/mynewt.h" +#include "bsp/bsp.h" +#include "hal/hal_gpio.h" +#include "host/ble_gap.h" +#include "cs_reflector.h" + +#if MYNEWT_VAL(CS_REFLECTOR_LE_PHY_SUPPORT) + +static const int button_gpio[4] = MYNEWT_VAL(CS_REFLECTOR_LE_PHY_BUTTON_GPIO); +static const int led_gpio[3] = MYNEWT_VAL(CS_REFLECTOR_LE_PHY_LED_GPIO); + +#define PHY_TO_PTR(_mask, _opts) (void *)(((_opts) << 16) | ((_mask))) +#define PTR_TO_PHY_MASK(_ptr) (uint8_t)(((int)_ptr) & 0x0ff) +#define PTR_TO_PHY_OPTS(_ptr) (uint8_t)(((int)_ptr) >> 16) + +static struct os_event gpio_event; + +static uint16_t conn_handle = CONN_HANDLE_INVALID; + +static void +gpio_irq_handler(void *arg) +{ + gpio_event.ev_arg = arg; + os_eventq_put(os_eventq_dflt_get(), &gpio_event); +} + +static void +gpio_event_handler(struct os_event *ev) +{ + uint8_t phy_mask; + uint8_t phy_opts; + int sr; + + OS_ENTER_CRITICAL(sr); + phy_mask = PTR_TO_PHY_MASK(ev->ev_arg); + phy_opts = PTR_TO_PHY_OPTS(ev->ev_arg); + OS_EXIT_CRITICAL(sr); + + if (conn_handle != CONN_HANDLE_INVALID) { + ble_gap_set_prefered_le_phy(conn_handle, phy_mask, phy_mask, phy_opts); + } +} + +static void +setup_button_gpio(int button, uint8_t phy_mask, uint8_t phy_opts) +{ + if (button < 0) { + return; + } + + hal_gpio_irq_init(button, gpio_irq_handler, PHY_TO_PTR(phy_mask, phy_opts), + HAL_GPIO_TRIG_FALLING, HAL_GPIO_PULL_UP); + hal_gpio_irq_enable(button); +} + +void +phy_init(void) +{ + gpio_event.ev_cb = gpio_event_handler; + + /* + * XXX: we could make this configurable, but for now assume all pins are + * valid, buttons gpio pins are pulled-up and LEDs are active-low - this + * is valid for nRF52840 PDK. + */ + setup_button_gpio(button_gpio[0], BLE_GAP_LE_PHY_1M_MASK, + BLE_GAP_LE_PHY_CODED_ANY); + setup_button_gpio(button_gpio[1], BLE_GAP_LE_PHY_2M_MASK, + BLE_GAP_LE_PHY_CODED_ANY); + setup_button_gpio(button_gpio[2], BLE_GAP_LE_PHY_CODED_MASK, + BLE_GAP_LE_PHY_CODED_S2); + setup_button_gpio(button_gpio[3], BLE_GAP_LE_PHY_CODED_MASK, + BLE_GAP_LE_PHY_CODED_S8); + + hal_gpio_init_out(led_gpio[0], 1); + hal_gpio_init_out(led_gpio[1], 1); + hal_gpio_init_out(led_gpio[2], 1); +} + +void +phy_conn_changed(uint16_t handle) +{ + uint8_t phy = 0; + + conn_handle = handle; + + if (handle != CONN_HANDLE_INVALID) { + /* XXX: assume symmetric phy for now */ + ble_gap_read_le_phy(handle, &phy, &phy); + } + + phy_update(phy); +} + +void +phy_update(uint8_t phy) +{ + if (conn_handle == CONN_HANDLE_INVALID) { + hal_gpio_write(led_gpio[0], 1); + hal_gpio_write(led_gpio[1], 1); + hal_gpio_write(led_gpio[2], 1); + } else { + hal_gpio_write(led_gpio[0], !(phy == BLE_GAP_LE_PHY_1M)); + hal_gpio_write(led_gpio[1], !(phy == BLE_GAP_LE_PHY_2M)); + hal_gpio_write(led_gpio[2], !(phy == BLE_GAP_LE_PHY_CODED)); + } +} + +#endif diff --git a/apps/cs_reflector/syscfg.yml b/apps/cs_reflector/syscfg.yml new file mode 100644 index 0000000000..fc243434cf --- /dev/null +++ b/apps/cs_reflector/syscfg.yml @@ -0,0 +1,83 @@ +# 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. +# + +syscfg.defs: + CS_REFLECTOR_LE_PHY_SUPPORT: + description: > + Enable support for changing PHY preference on active connection. + PHY preference change is triggered by configured GPIO pins. + Current PHY is indicated using LEDs connected to configured + GPIO pins. + value: 0 + CS_REFLECTOR_LE_PHY_BUTTON_GPIO: + description: > + GPIO pins for changing PHY preference on active connection. This + is an array of 4 GPIO pin numbers for 1M, 2M, LE Coded S=2 and + LE Coded S=8 respectively. + value: "(int[]){ BUTTON_1, BUTTON_2, BUTTON_3, BUTTON_4 }" + CS_REFLECTOR_LE_PHY_LED_GPIO: + description: > + GPIO pins for indicating current PHY on active connection. This + is an array of 3 GPIO pin numbers for 1M, 2M and LE Coded + respectively. + value: "(int[]){ LED_1, LED_2, LED_3 }" + +syscfg.vals: + CONSOLE_IMPLEMENTATION: full + LOG_IMPLEMENTATION: full + STATS_IMPLEMENTATION: full + + # Disable central and observer roles. + BLE_ROLE_BROADCASTER: 1 + BLE_ROLE_CENTRAL: 0 + BLE_ROLE_OBSERVER: 0 + BLE_ROLE_PERIPHERAL: 1 + + # Configure DIS + BLE_SVC_DIS_FIRMWARE_REVISION_READ_PERM: 1 + + # Log reboot messages to a flash circular buffer. + REBOOT_LOG_FCB: 1 + LOG_FCB: 1 + CONFIG_FCB: 1 + + # Enable smp commands. + STATS_MGMT: 1 + LOG_MGMT: 1 + CONFIG_MGMT: 1 + + # OS main/default task + OS_MAIN_STACK_SIZE: 512 + + # Lots of smaller mbufs are required for smp using typical BLE ATT MTU + # values. + MSYS_1_BLOCK_COUNT: 22 + MSYS_1_BLOCK_SIZE: 110 + + BLE_SVC_GAP_DEVICE_NAME: '"nimble-cs_reflector"' + + # Whether to save data to sys/config, or just keep it in RAM. + BLE_STORE_CONFIG_PERSIST: 0 + + BLE_VERSION: 54 + BLE_CHANNEL_SOUNDING: 1 + MBEDTLS_CTR_DRBG_C: 1 + QUASI_CHANNEL_SOUNDING: 1 + +syscfg.vals.BLE_CONTROLLER: + TIMER_3: 1 diff --git a/babblesim/hw/bsp/nrf52_bsim/src/hal_bsp.c b/babblesim/hw/bsp/nrf52_bsim/src/hal_bsp.c index 502f9a9277..4fb99c6631 100644 --- a/babblesim/hw/bsp/nrf52_bsim/src/hal_bsp.c +++ b/babblesim/hw/bsp/nrf52_bsim/src/hal_bsp.c @@ -129,6 +129,9 @@ nrf52_periph_create_timers(void) rc = os_cputime_init(MYNEWT_VAL(OS_CPUTIME_FREQ)); assert(rc == 0); #endif + + rc = hal_timer_config(3, 16000000); + assert(rc == 0); } static struct uart_dev os_bsp_uart0; diff --git a/babblesim/targets/cs_initiator/pkg.yml b/babblesim/targets/cs_initiator/pkg.yml new file mode 100644 index 0000000000..8475b8c3f2 --- /dev/null +++ b/babblesim/targets/cs_initiator/pkg.yml @@ -0,0 +1,29 @@ +# 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: babblesim/targets/cs_initiator +pkg.type: target +pkg.descrption: Sample target for running cs_initiator on BabbleSim +pkg.author: "Apache Mynewt " +pkg.homepage: "https://mynewt.apache.org/" + +pkg.cflags: + - -Wimplicit-function-declaration + - -Wno-unused-variable + - -Wno-unused-function + - -m32 diff --git a/babblesim/targets/cs_initiator/syscfg.yml b/babblesim/targets/cs_initiator/syscfg.yml new file mode 100644 index 0000000000..fe7bca8070 --- /dev/null +++ b/babblesim/targets/cs_initiator/syscfg.yml @@ -0,0 +1,22 @@ +# 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. +# + +syscfg.vals: + BLE_LL_PUBLIC_DEV_ADDR: 0xbabb1e000002 + BLE_MONITOR_UART: 1 + BLE_MONITOR_UART_DEV: '"uart1"' diff --git a/babblesim/targets/cs_initiator/target.yml b/babblesim/targets/cs_initiator/target.yml new file mode 100644 index 0000000000..6176c18ed0 --- /dev/null +++ b/babblesim/targets/cs_initiator/target.yml @@ -0,0 +1,21 @@ +# 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. +# + +target.app: "@apache-mynewt-nimble/apps/cs_initiator" +target.bsp: "@apache-mynewt-nimble/babblesim/hw/bsp/nrf52_bsim" +target.build_profile: debug diff --git a/babblesim/targets/cs_reflector/pkg.yml b/babblesim/targets/cs_reflector/pkg.yml new file mode 100644 index 0000000000..fe8c6a10c6 --- /dev/null +++ b/babblesim/targets/cs_reflector/pkg.yml @@ -0,0 +1,29 @@ +# 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: babblesim/targets/cs_reflector +pkg.type: target +pkg.descrption: Sample target for running cs_reflector on BabbleSim +pkg.author: "Apache Mynewt " +pkg.homepage: "https://mynewt.apache.org/" + +pkg.cflags: + - -Wimplicit-function-declaration + - -Wno-unused-variable + - -Wno-unused-function + - -m32 diff --git a/babblesim/targets/cs_reflector/syscfg.yml b/babblesim/targets/cs_reflector/syscfg.yml new file mode 100644 index 0000000000..f05f3bf0d6 --- /dev/null +++ b/babblesim/targets/cs_reflector/syscfg.yml @@ -0,0 +1,22 @@ +# 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. +# + +syscfg.vals: + BLE_LL_PUBLIC_DEV_ADDR: 0xbabb1e000003 + BLE_MONITOR_UART: 1 + BLE_MONITOR_UART_DEV: '"uart1"' diff --git a/babblesim/targets/cs_reflector/target.yml b/babblesim/targets/cs_reflector/target.yml new file mode 100644 index 0000000000..6d955a0784 --- /dev/null +++ b/babblesim/targets/cs_reflector/target.yml @@ -0,0 +1,21 @@ +# 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. +# + +target.app: "@apache-mynewt-nimble/apps/cs_reflector" +target.bsp: "@apache-mynewt-nimble/babblesim/hw/bsp/nrf52_bsim" +target.build_profile: debug diff --git a/nimble/controller/include/controller/ble_ll.h b/nimble/controller/include/controller/ble_ll.h index 752ca43e97..884b2dacd3 100644 --- a/nimble/controller/include/controller/ble_ll.h +++ b/nimble/controller/include/controller/ble_ll.h @@ -33,6 +33,10 @@ #include "hal/hal_timer.h" #endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +#include "controller/ble_ll_cs.h" +#endif + #ifdef __cplusplus extern "C" { #endif @@ -243,6 +247,9 @@ extern STATS_SECT_DECL(ble_ll_stats) ble_ll_stats; #if MYNEWT_VAL(BLE_LL_ISO_BROADCASTER) #define BLE_LL_STATE_BIG (9) #endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +#define BLE_LL_STATE_CS (10) +#endif /* LL Features */ #define BLE_LL_FEAT_LE_ENCRYPTION (0x0000000000001) diff --git a/nimble/controller/include/controller/ble_ll_conn.h b/nimble/controller/include/controller/ble_ll_conn.h index 7d6af874dd..1c78d4449f 100644 --- a/nimble/controller/include/controller/ble_ll_conn.h +++ b/nimble/controller/include/controller/ble_ll_conn.h @@ -201,6 +201,10 @@ struct ble_ll_conn_subrate_req_params { uint16_t supervision_tmo; }; +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +struct ble_ll_cs_sm; +#endif + /* Connection state machine */ struct ble_ll_conn_sm { @@ -273,7 +277,7 @@ struct ble_ll_conn_sm uint8_t vers_nr; uint8_t conn_features; uint8_t remote_features[7]; - uint16_t pending_ctrl_procs; + uint32_t pending_ctrl_procs; uint16_t event_cntr; uint16_t completed_pkts; uint16_t comp_id; @@ -397,6 +401,10 @@ struct ble_ll_conn_sm uint16_t css_slot_idx_pending; uint8_t css_period_idx; #endif + +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + struct ble_ll_cs_sm *cssm; +#endif }; /* Role */ diff --git a/nimble/controller/include/controller/ble_ll_cs.h b/nimble/controller/include/controller/ble_ll_cs.h index fb7cb4a59b..e54c6d4b15 100644 --- a/nimble/controller/include/controller/ble_ll_cs.h +++ b/nimble/controller/include/controller/ble_ll_cs.h @@ -28,6 +28,35 @@ extern "C" { #endif +void ble_ll_cs_init(void); +void ble_ll_cs_reset(void); + +void ble_ll_cs_capabilities_pdu_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr); +void ble_ll_cs_config_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr); +void ble_ll_cs_security_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr); +void ble_ll_cs_start_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr); +void ble_ll_cs_terminate_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr); + +int ble_ll_cs_rx_capabilities_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf); +void ble_ll_cs_rx_capabilities_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr); +void ble_ll_cs_rx_capabilities_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error); +int ble_ll_cs_rx_fae_req(struct ble_ll_conn_sm *connsm, struct os_mbuf *om); +void ble_ll_cs_rx_fae_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr); +void ble_ll_cs_rx_fae_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error); +int ble_ll_cs_rx_config_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf); +void ble_ll_cs_rx_config_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr); +void ble_ll_cs_rx_config_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error); +int ble_ll_cs_rx_security_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf); +void ble_ll_cs_rx_security_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr); +void ble_ll_cs_rx_security_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error); +int ble_ll_cs_rx_cs_start_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf); +int ble_ll_cs_rx_cs_start_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf); +int ble_ll_cs_rx_cs_start_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf); +void ble_ll_cs_rx_cs_start_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error); +int ble_ll_cs_rx_cs_terminate_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf); +void ble_ll_cs_rx_cs_terminate_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr); +void ble_ll_cs_rx_cs_terminate_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error); + /* HCI handlers */ int ble_ll_cs_hci_rd_loc_supp_cap(uint8_t *rspbuf, uint8_t *rsplen); int ble_ll_cs_hci_rd_rem_supp_cap(const uint8_t *cmdbuf, uint8_t cmdlen); @@ -44,6 +73,14 @@ int ble_ll_cs_hci_proc_enable(const uint8_t *cmdbuf, uint8_t cmdlen); int ble_ll_cs_hci_test(const uint8_t *cmdbuf, uint8_t cmdlen, uint8_t *rspbuf, uint8_t *rsplen); int ble_ll_cs_hci_test_end(void); +int ble_ll_cs_sync_rx_isr_start(struct ble_mbuf_hdr *rxhdr, uint32_t aa); +int ble_ll_cs_sync_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr); +void ble_ll_cs_sync_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *rxhdr); +void ble_ll_cs_sync_wfr_timer_exp(void); + +void ble_ll_cs_proc_rm_from_sched(void *cb_arg); +void ble_ll_cs_proc_halt(void); + #ifdef __cplusplus } #endif diff --git a/nimble/controller/include/controller/ble_ll_ctrl.h b/nimble/controller/include/controller/ble_ll_ctrl.h index 89502a45a3..76723ae9ac 100644 --- a/nimble/controller/include/controller/ble_ll_ctrl.h +++ b/nimble/controller/include/controller/ble_ll_ctrl.h @@ -43,7 +43,13 @@ extern "C" { #define BLE_LL_CTRL_PROC_CIS_CREATE (11) #define BLE_LL_CTRL_PROC_SUBRATE_REQ (12) #define BLE_LL_CTRL_PROC_SUBRATE_UPDATE (13) -#define BLE_LL_CTRL_PROC_NUM (14) +#define BLE_LL_CTRL_PROC_CS_CAP_XCHG (14) +#define BLE_LL_CTRL_PROC_CS_FAE_REQ (15) +#define BLE_LL_CTRL_PROC_CS_CONF (16) +#define BLE_LL_CTRL_PROC_CS_SEC_START (17) +#define BLE_LL_CTRL_PROC_CS_START (18) +#define BLE_LL_CTRL_PROC_CS_TERMINATE (19) +#define BLE_LL_CTRL_PROC_NUM (20) #define BLE_LL_CTRL_PROC_IDLE (255) /* Checks if a particular control procedure is running */ @@ -100,14 +106,33 @@ extern "C" { #define BLE_LL_CTRL_SUBRATE_IND (0x27) #define BLE_LL_CTRL_CHAN_REPORTING_IND (0x28) #define BLE_LL_CTRL_CHAN_STATUS_IND (0x29) +#define BLE_LL_CTRL_PERIODIC_SYNC_WR_IND (0x2A) +#define BLE_LL_CTRL_FEATURE_EXT_REQ (0x2B) +#define BLE_LL_CTRL_FEATURE_EXT_RSP (0x2C) +#define BLE_LL_CTRL_CS_SEC_RSP (0x2D) +#define BLE_LL_CTRL_CS_CAPABILITIES_REQ (0x2E) +#define BLE_LL_CTRL_CS_CAPABILITIES_RSP (0x2F) +#define BLE_LL_CTRL_CS_CONFIG_REQ (0x30) +#define BLE_LL_CTRL_CS_CONFIG_RSP (0x31) +#define BLE_LL_CTRL_CS_REQ (0x32) +#define BLE_LL_CTRL_CS_RSP (0x33) +#define BLE_LL_CTRL_CS_IND (0x34) +#define BLE_LL_CTRL_CS_TERMINATE_REQ (0x35) +#define BLE_LL_CTRL_CS_FAE_REQ (0x36) +#define BLE_LL_CTRL_CS_FAE_RSP (0x37) +#define BLE_LL_CTRL_CS_CHANNEL_MAP_IND (0x38) +#define BLE_LL_CTRL_CS_SEC_REQ (0x39) +#define BLE_LL_CTRL_CS_TERMINATE_RSP (0x3A) /* Maximum opcode value */ -#define BLE_LL_CTRL_OPCODES (BLE_LL_CTRL_CHAN_STATUS_IND + 1) +#define BLE_LL_CTRL_OPCODES (BLE_LL_CTRL_CS_TERMINATE_RSP + 1) extern const uint8_t g_ble_ll_ctrl_pkt_lengths[BLE_LL_CTRL_OPCODES]; /* Maximum LL control PDU size */ -#if MYNEWT_VAL(BLE_ISO) +#if MYNEWT_VAL(BLE_CHANNEL_SOUNDING) +#define BLE_LL_CTRL_MAX_PDU_LEN (72) +#elif MYNEWT_VAL(BLE_ISO) #define BLE_LL_CTRL_MAX_PDU_LEN (42) #elif MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) #define BLE_LL_CTRL_MAX_PDU_LEN (35) @@ -292,6 +317,27 @@ struct ble_ll_len_req #define BLE_LL_CTRL_CHAN_REPORTING_IND_LEN (3) #define BLE_LL_CTRL_CHAN_STATUS_IND_LEN (10) +/* Not implemented */ +#define BLE_LL_CTRL_PERIODIC_SYNC_WR_IND_LEN (0) +#define BLE_LL_CTRL_FEATURE_EXT_REQ_LEN (0) +#define BLE_LL_CTRL_FEATURE_EXT_RSP_LEN (0) + +/* Channel Sounding */ +#define BLE_LL_CTRL_CS_SEC_RSP_LEN (20) +#define BLE_LL_CTRL_CS_CAPABILITIES_REQ_LEN (25) +#define BLE_LL_CTRL_CS_CAPABILITIES_RSP_LEN (25) +#define BLE_LL_CTRL_CS_CONFIG_REQ_LEN (27) +#define BLE_LL_CTRL_CS_CONFIG_RSP_LEN (1) +#define BLE_LL_CTRL_CS_REQ_LEN (28) +#define BLE_LL_CTRL_CS_RSP_LEN (21) +#define BLE_LL_CTRL_CS_IND_LEN (18) +#define BLE_LL_CTRL_CS_TERMINATE_REQ_LEN (4) +#define BLE_LL_CTRL_CS_FAE_REQ_LEN (0) +#define BLE_LL_CTRL_CS_FAE_RSP_LEN (72) +#define BLE_LL_CTRL_CS_CHANNEL_MAP_IND_LEN (12) +#define BLE_LL_CTRL_CS_SEC_REQ_LEN (20) +#define BLE_LL_CTRL_CS_TERMINATE_RSP_LEN (4) + /* API */ struct ble_ll_conn_sm; void ble_ll_ctrl_proc_start(struct ble_ll_conn_sm *connsm, int ctrl_proc, diff --git a/nimble/controller/include/controller/ble_ll_sched.h b/nimble/controller/include/controller/ble_ll_sched.h index 5daa647fab..0d3d75ed51 100644 --- a/nimble/controller/include/controller/ble_ll_sched.h +++ b/nimble/controller/include/controller/ble_ll_sched.h @@ -71,6 +71,7 @@ extern uint8_t g_ble_ll_sched_offset_ticks; #define BLE_LL_SCHED_TYPE_SYNC (7) #define BLE_LL_SCHED_TYPE_SCAN_AUX (8) #define BLE_LL_SCHED_TYPE_BIG (9) +#define BLE_LL_SCHED_TYPE_CS (10) #if MYNEWT_VAL(BLE_LL_EXT) #define BLE_LL_SCHED_TYPE_EXTERNAL (255) #endif @@ -222,6 +223,10 @@ uint32_t ble_ll_sched_css_get_conn_interval_us(void); int ble_ll_sched_iso_big(struct ble_ll_sched_item *sch, int first, int fixed); #endif /* BLE_LL_ISO_BROADCASTER */ +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +int ble_ll_sched_cs_proc(struct ble_ll_sched_item *sch); +#endif /* BLE_LL_CHANNEL_SOUNDING */ + #ifdef __cplusplus } #endif diff --git a/nimble/controller/include/controller/ble_phy.h b/nimble/controller/include/controller/ble_phy.h index fe70044234..43cba87894 100644 --- a/nimble/controller/include/controller/ble_phy.h +++ b/nimble/controller/include/controller/ble_phy.h @@ -78,6 +78,10 @@ struct os_mbuf; /* Maximun PDU length. Includes LL header of 2 bytes and 255 bytes payload. */ #define BLE_PHY_MAX_PDU_LEN (257) +#define BLE_PHY_CS_TIMER_NONE (0) +#define BLE_PHY_CS_TIMER_START (1) +#define BLE_PHY_CS_TIMER_CAPTURE (2) + /* Wait for response timer */ typedef void (*ble_phy_tx_end_func)(void *arg); @@ -235,6 +239,14 @@ static inline int ble_ll_phy_to_phy_mode(int phy, int phy_options) return phy_mode; } +void ble_phy_get_txend_time(uint32_t *cputime, uint32_t *rem_us, uint32_t *rem_ns); +void ble_phy_get_rxend_time(uint32_t *cputime, uint32_t *rem_us, uint32_t *rem_ns); +typedef uint8_t (*ble_phy_tx_cs_sync_cb_t)(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte); +uint32_t ble_phy_cs_timer_read(void); +void ble_phy_cs_sync_mode_set(uint8_t mode); +int ble_phy_tx_cs_sync(ble_phy_tx_cs_sync_cb_t pktcb, void *pktcb_arg); +int ble_phy_cs_sync_configure(uint8_t chan, uint32_t access_addr, uint8_t cs_timer); + #if MYNEWT_VAL(BLE_LL_DTM) void ble_phy_enable_dtm(void); void ble_phy_disable_dtm(void); diff --git a/nimble/controller/pkg.yml b/nimble/controller/pkg.yml index 2f22e3ebf5..e5ba7b600c 100644 --- a/nimble/controller/pkg.yml +++ b/nimble/controller/pkg.yml @@ -42,7 +42,14 @@ pkg.deps: - nimble - nimble/transport +pkg.deps.TEST: + - "@apache-mynewt-core/crypto/mbedtls" + pkg.init: ble_ll_init: - $before:ble_transport_hs_init - $before:ble_transport_ll_init + +pkg.cflags.TEST: + - -Irepos/mbedtls/include + - -Irepos/include/mbedtls diff --git a/nimble/controller/src/ble_ll.c b/nimble/controller/src/ble_ll.c index 7e87891ab5..d2599d0e7b 100644 --- a/nimble/controller/src/ble_ll.c +++ b/nimble/controller/src/ble_ll.c @@ -48,6 +48,7 @@ #include "controller/ble_fem.h" #include "controller/ble_ll_isoal.h" #include "controller/ble_ll_iso_big.h" +#include "controller/ble_ll_cs.h" #if MYNEWT_VAL(BLE_LL_EXT) #include "controller/ble_ll_ext.h" #endif @@ -799,6 +800,11 @@ ble_ll_wfr_timer_exp(void *arg) case BLE_LL_STATE_EXTERNAL: ble_ll_ext_wfr_timer_exp(); break; +#endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + case BLE_LL_STATE_CS: + ble_ll_cs_sync_wfr_timer_exp(); + break; #endif default: break; @@ -979,6 +985,11 @@ ble_ll_rx_pkt_in(void) case BLE_LL_STATE_EXTERNAL: ble_ll_ext_rx_pkt_in(m, ble_hdr); break; +#endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + case BLE_LL_STATE_CS: + ble_ll_cs_sync_rx_pkt_in(m, ble_hdr); + break; #endif default: /* Any other state should never occur */ @@ -1130,6 +1141,11 @@ ble_ll_rx_start(uint8_t *rxbuf, uint8_t chan, struct ble_mbuf_hdr *rxhdr) case BLE_LL_STATE_EXTERNAL: rc = ble_ll_ext_rx_isr_start(pdu_type, rxhdr); break; +#endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + case BLE_LL_STATE_CS: + rc = ble_ll_cs_sync_rx_isr_start(rxhdr, ble_phy_access_addr_get()); + break; #endif default: /* Should not be in this state! */ @@ -1202,6 +1218,13 @@ ble_ll_rx_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr) } #endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + if (BLE_MBUF_HDR_RX_STATE(rxhdr) == BLE_LL_STATE_CS) { + rc = ble_ll_cs_sync_rx_isr_end(rxbuf, rxhdr); + return rc; + } +#endif + /* If the CRC checks, make sure lengths check! */ badpkt = 0; if (crcok) { @@ -1685,6 +1708,10 @@ ble_ll_reset(void) ble_ll_iso_big_reset(); #endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + ble_ll_cs_reset(); +#endif + /* Re-initialize the PHY */ rc = ble_phy_init(); @@ -1963,6 +1990,10 @@ ble_ll_init(void) ble_ll_iso_big_init(); #endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + ble_ll_cs_init(); +#endif + #if MYNEWT_VAL(BLE_LL_EXT) ble_ll_ext_init(); #endif diff --git a/nimble/controller/src/ble_ll_conn.c b/nimble/controller/src/ble_ll_conn.c index cee6f976b2..aefc922848 100644 --- a/nimble/controller/src/ble_ll_conn.c +++ b/nimble/controller/src/ble_ll_conn.c @@ -1961,6 +1961,11 @@ ble_ll_conn_set_csa(struct ble_ll_conn_sm *connsm, bool chsel) connsm->data_chan_index = ble_ll_conn_calc_dci(connsm, 1); } +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +void ble_ll_cs_sm_init(struct ble_ll_conn_sm *connsm); +void ble_ll_cs_sm_free(struct ble_ll_conn_sm *connsm); +#endif + /** * Create a new connection state machine. This is done once per * connection when the HCI command "create connection" is issued to the @@ -2081,6 +2086,10 @@ ble_ll_conn_sm_new(struct ble_ll_conn_sm *connsm) ble_ll_conn_css_update_list(connsm); } #endif + +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + ble_ll_cs_sm_init(connsm); +#endif } void @@ -2206,6 +2215,10 @@ ble_ll_conn_end(struct ble_ll_conn_sm *connsm, uint8_t ble_err) } #endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + ble_ll_cs_sm_free(connsm); +#endif + #if MYNEWT_VAL(BLE_LL_CFG_FEAT_CTRL_TO_HOST_FLOW_CONTROL) ble_ll_conn_cth_flow_free_credit(connsm, connsm->cth_flow_pending); #endif @@ -2275,7 +2288,7 @@ ble_ll_conn_end(struct ble_ll_conn_sm *connsm, uint8_t ble_err) connsm->event_cntr, (uint32_t)ble_err); } -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) || MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) void ble_ll_conn_get_anchor(struct ble_ll_conn_sm *connsm, uint16_t conn_event, uint32_t *anchor, uint8_t *anchor_usecs) @@ -3931,9 +3944,13 @@ ble_ll_conn_enqueue_pkt(struct ble_ll_conn_sm *connsm, struct os_mbuf *om, struct ble_mbuf_hdr *ble_hdr; int lifo; - /* Set mbuf length and packet length if a control PDU */ + /* Set overall packet length if a control PDU */ if (hdr_byte == BLE_LL_LLID_CTRL) { - om->om_len = length; + /* Set mbuf length if not chained mbufs */ + if (SLIST_NEXT(om, om_next) == NULL) { + om->om_len = length; + } + OS_MBUF_PKTHDR(om)->omp_len = length; } diff --git a/nimble/controller/src/ble_ll_cs.c b/nimble/controller/src/ble_ll_cs.c index a9a181a849..959c400ec5 100644 --- a/nimble/controller/src/ble_ll_cs.c +++ b/nimble/controller/src/ble_ll_cs.c @@ -26,81 +26,1775 @@ #include "controller/ble_ll_conn.h" #include "controller/ble_ll_hci.h" #include "controller/ble_ll_cs.h" +#include "controller/ble_ll_tmr.h" +#include "ble_ll_conn_priv.h" +#include "ble_ll_cs_priv.h" +#include "os/os_mbuf.h" + +struct ble_ll_cs_supp_cap g_ble_ll_cs_local_cap; +struct ble_ll_cs_sm g_ble_ll_cs_sm[MYNEWT_VAL(BLE_MAX_CONNECTIONS)]; +static const uint8_t t_ip1[] = {10, 20, 30, 40, 50, 60, 80, 145}; +static const uint8_t t_ip2[] = {10, 20, 30, 40, 50, 60, 80, 145}; +static const uint8_t t_fcs[] = {15, 20, 30, 40, 50, 60, 80, 100, 120, 150}; +static const uint8_t t_pm[] = {10, 20, 40}; +static const uint8_t default_channel_classification[10] = { + 0xFC, 0xFF, 0x7F, 0xFC, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x1F +}; +static uint8_t g_ble_ll_cs_chan_class[10]; +uint8_t g_ble_ll_cs_chan_count = 0; +uint8_t g_ble_ll_cs_chan_indices[72]; + +#define MIN_CONN_EVENT_COUNT_BEFORE_START 10 +#define OFFSET_FROM_CONN_EVENT_TICKS 10 +#define OFFSET_MIN_USECS 500 +#define OFFSET_MAX_USECS 4000000 + +extern int8_t g_ble_ll_tx_power; +void ble_ll_ctrl_rej_ext_ind_make(uint8_t rej_opcode, uint8_t err, uint8_t *ctrdata); int ble_ll_cs_hci_rd_loc_supp_cap(uint8_t *rspbuf, uint8_t *rsplen) { - return BLE_ERR_UNSUPPORTED; + const struct ble_ll_cs_supp_cap *cap = &g_ble_ll_cs_local_cap; + struct ble_hci_le_cs_rd_loc_supp_cap_rp *rsp = (void *)rspbuf; + + rsp->num_config_supported = cap->max_number_of_configs; + rsp->max_consecutive_procedures_supported = htole16(cap->max_number_of_procedures); + rsp->num_antennas_supported = cap->number_of_antennas; + rsp->max_antenna_paths_supported = cap->max_number_of_antenna_paths; + rsp->roles_supported = cap->roles_supported; + rsp->optional_modes_supported = cap->mode_types; + rsp->rtt_capability = cap->rtt_capability; + rsp->rtt_aa_only_n = cap->rtt_aa_only_n; + rsp->rtt_sounding_n = cap->rtt_sounding_n; + rsp->rtt_random_payload_n = cap->rtt_random_sequence_n; + rsp->optional_nadm_sounding_capability = htole16(cap->nadm_sounding_capability); + rsp->optional_nadm_random_capability = htole16(cap->nadm_random_sequence_capability); + rsp->optional_cs_sync_phys_supported = cap->cs_sync_phy_capability; + rsp->optional_subfeatures_supported = htole16(0x000f & + (cap->no_fae << 1 | + cap->channel_selection << 2 | + cap->sounding_pct_estimate << 3)); + rsp->optional_t_ip1_times_supported = htole16(cap->t_ip1_capability); + rsp->optional_t_ip2_times_supported = htole16(cap->t_ip2_capability); + rsp->optional_t_fcs_times_supported = htole16(cap->t_fcs_capability); + rsp->optional_t_pm_times_supported = htole16(cap->t_pm_capability); + rsp->t_sw_time_supported = cap->t_sw; + rsp->optional_tx_snr_capability = cap->tx_snr_capablity; + + *rsplen = sizeof(*rsp); + + return BLE_ERR_SUCCESS; +} + +void +ble_ll_cs_capabilities_pdu_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + const struct ble_ll_cs_supp_cap *cap = &g_ble_ll_cs_local_cap; + + *dptr = cap->mode_types; + dptr[1] = cap->rtt_capability; + dptr[2] = cap->rtt_aa_only_n; + dptr[3] = cap->rtt_sounding_n; + dptr[4] = cap->rtt_random_sequence_n; + put_le16(dptr + 5, cap->nadm_sounding_capability); + put_le16(dptr + 7, cap->nadm_random_sequence_capability); + dptr[9] = cap->cs_sync_phy_capability; + dptr[10] = cap->number_of_antennas | cap->max_number_of_antenna_paths << 4; + dptr[11] = cap->roles_supported | + cap->no_fae << 3 | + cap->channel_selection << 4 | + cap->sounding_pct_estimate << 5; + dptr[12] = cap->max_number_of_configs; + put_le16(dptr + 13, cap->max_number_of_procedures); + dptr[15] = cap->t_sw; + put_le16(dptr + 16, cap->t_ip1_capability); + put_le16(dptr + 18, cap->t_ip2_capability); + put_le16(dptr + 20, cap->t_fcs_capability); + put_le16(dptr + 22, cap->t_pm_capability); + dptr[24] = cap->tx_snr_capablity << 1; +} + +static void +ble_ll_cs_update_rem_capabilities(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + struct ble_ll_cs_supp_cap *cap = &connsm->cssm->remote_cap; + + cap->mode_types = *dptr & 0x01; + cap->rtt_capability = dptr[1] & 0x05; + cap->rtt_aa_only_n = dptr[2]; + cap->rtt_sounding_n = dptr[3]; + cap->rtt_random_sequence_n = dptr[4]; + cap->nadm_sounding_capability = get_le16(dptr + 5) & 0x01; + cap->nadm_random_sequence_capability = get_le16(dptr + 7) & 0x01; + cap->cs_sync_phy_capability = dptr[9] & 0x06; + + cap->number_of_antennas = dptr[10] & 0b00001111; + cap->max_number_of_antenna_paths = dptr[10] >> 4; + + cap->roles_supported = dptr[11] & 0b00000011; + cap->no_fae = (dptr[11] & 0b00001000) >> 3; + cap->channel_selection = (dptr[11] & 0b00010000) >> 4; + cap->sounding_pct_estimate = (dptr[11] & 0b00100000) >> 5; + + cap->max_number_of_configs = dptr[12]; + cap->max_number_of_procedures = get_le16(dptr + 13); + cap->t_sw = dptr[15]; + cap->t_ip1_capability = get_le16(dptr + 16) & 0x00FF; + cap->t_ip2_capability = get_le16(dptr + 18) & 0x00FF; + cap->t_fcs_capability = get_le16(dptr + 20) & 0x03FF; + cap->t_pm_capability = get_le16(dptr + 22) & 0x07; + cap->tx_snr_capablity = (dptr[24] >> 1) & 0b01111111; + + /* The capabilites contain info about allowed values for + * CS procedures. Ignore the RFU values here. + * We will be able to reject/renegotiate unsupported values + * if the remote controller will use them in the procedures. + */ + + if (cap->number_of_antennas > 4) { + cap->number_of_antennas = 4; + } + + if (cap->max_number_of_antenna_paths > 4) { + cap->max_number_of_antenna_paths = 4; + } + + if (cap->max_number_of_antenna_paths < cap->number_of_antennas) { + cap->number_of_antennas = cap->max_number_of_antenna_paths; + } + + if (cap->max_number_of_configs > 4) { + cap->max_number_of_configs = 4; + } + + if (!(cap->t_sw == 0x00 || + cap->t_sw == 0x01 || + cap->t_sw == 0x02 || + cap->t_sw == 0x04 || + cap->t_sw == 0x0A)) { + /* If the remote does not support a valid duration of the antenna switch period, + * lets assume it does not support the antenna switching at all. + */ + cap->number_of_antennas = 1; + cap->t_sw = 0; + } +} + +static void +ble_ll_cs_ev_rd_rem_supp_cap(struct ble_ll_conn_sm *connsm, uint8_t status) +{ + const struct ble_ll_cs_supp_cap *cap = &connsm->cssm->remote_cap; + struct ble_hci_ev_le_subev_cs_rd_rem_supp_cap_complete *ev; + struct ble_hci_ev *hci_ev; + + if (ble_ll_hci_is_le_event_enabled( + BLE_HCI_LE_SUBEV_CS_RD_REM_SUPP_CAP_COMPLETE)) { + hci_ev = ble_transport_alloc_evt(0); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + memset(ev, 0, sizeof(*ev)); + ev->subev_code = BLE_HCI_LE_SUBEV_CS_RD_REM_SUPP_CAP_COMPLETE; + ev->status = status; + ev->conn_handle = htole16(connsm->conn_handle); + + if (status == BLE_ERR_SUCCESS) { + ev->num_config_supported = cap->max_number_of_configs; + ev->max_consecutive_procedures_supported = htole16(cap->max_number_of_procedures); + ev->num_antennas_supported = cap->number_of_antennas; + ev->max_antenna_paths_supported = cap->max_number_of_antenna_paths; + ev->roles_supported = cap->roles_supported; + ev->optional_modes_supported = cap->mode_types; + ev->rtt_capability = cap->rtt_capability; + ev->rtt_aa_only_n = cap->rtt_aa_only_n; + ev->rtt_sounding_n = cap->rtt_sounding_n; + ev->rtt_random_payload_n = cap->rtt_random_sequence_n; + ev->optional_nadm_sounding_capability = htole16(cap->nadm_sounding_capability); + ev->optional_nadm_random_capability = htole16(cap->nadm_random_sequence_capability); + ev->optional_cs_sync_phys_supported = cap->cs_sync_phy_capability; + ev->optional_subfeatures_supported = htole16(cap->no_fae << 1 | + cap->channel_selection << 2 | + cap->sounding_pct_estimate << 3); + ev->optional_t_ip1_times_supported = htole16(cap->t_ip1_capability); + ev->optional_t_ip2_times_supported = htole16(cap->t_ip2_capability); + ev->optional_t_fcs_times_supported = htole16(cap->t_fcs_capability); + ev->optional_t_pm_times_supported = htole16(cap->t_pm_capability); + ev->t_sw_time_supported = cap->t_sw; + ev->optional_tx_snr_capability = cap->tx_snr_capablity; + } + + ble_ll_hci_event_send(hci_ev); + } + } +} + +int +ble_ll_cs_rx_capabilities_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, + uint8_t *rspbuf) +{ + ble_ll_cs_update_rem_capabilities(connsm, dptr); + + ble_ll_cs_capabilities_pdu_make(connsm, rspbuf); + + return BLE_LL_CTRL_CS_CAPABILITIES_RSP; +} + +void +ble_ll_cs_rx_capabilities_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error) +{ + /* Stop the control procedure and send an event to the host */ + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_CAP_XCHG); + ble_ll_cs_ev_rd_rem_supp_cap(connsm, ble_error); +} + +void +ble_ll_cs_rx_capabilities_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + if (!IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CAP_XCHG)) { + /* Ignore */ + return; + } + + ble_ll_cs_update_rem_capabilities(connsm, dptr); + + /* Stop the control procedure and send an event to the host */ + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_CAP_XCHG); + ble_ll_cs_ev_rd_rem_supp_cap(connsm, BLE_ERR_SUCCESS); } int ble_ll_cs_hci_rd_rem_supp_cap(const uint8_t *cmdbuf, uint8_t cmdlen) { - return BLE_ERR_UNSUPPORTED; + const struct ble_hci_le_cs_rd_rem_supp_cap_cp *cmd = (const void *)cmdbuf; + struct ble_ll_conn_sm *connsm; + + if (cmdlen != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* If no connection handle exit with error */ + connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle)); + if (!connsm) { + return BLE_ERR_UNK_CONN_ID; + } + + /* If already pending exit with error */ + if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CAP_XCHG)) { + return BLE_ERR_CMD_DISALLOWED; + } + + ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_CAP_XCHG, NULL); + + return BLE_ERR_SUCCESS; } int ble_ll_cs_hci_wr_cached_rem_supp_cap(const uint8_t *cmdbuf, uint8_t cmdlen, uint8_t *rspbuf, uint8_t *rsplen) { - return BLE_ERR_UNSUPPORTED; + const struct ble_hci_le_cs_wr_cached_rem_supp_cap_cp *cmd = (const void *)cmdbuf; + struct ble_hci_le_cs_wr_cached_rem_supp_cap_rp *rsp = (void *)rspbuf; + struct ble_ll_cs_supp_cap *cap; + struct ble_ll_conn_sm *connsm; + uint16_t subfeatures; + + connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle)); + if (!connsm) { + return BLE_ERR_UNK_CONN_ID; + } + + cap = &connsm->cssm->remote_cap; + + cap->max_number_of_configs = cmd->num_config_supported; + cap->max_number_of_procedures = le16toh(cmd->max_consecutive_procedures_supported); + cap->number_of_antennas = cmd->num_antennas_supported; + cap->max_number_of_antenna_paths = cmd->max_antenna_paths_supported; + cap->roles_supported = cmd->roles_supported; + cap->mode_types = cmd->optional_modes_supported; + cap->rtt_capability = cmd->rtt_capability; + cap->rtt_aa_only_n = cmd->rtt_aa_only_n; + cap->rtt_sounding_n = cmd->rtt_sounding_n; + cap->rtt_random_sequence_n = cmd->rtt_random_payload_n; + cap->nadm_sounding_capability = le16toh(cmd->optional_nadm_sounding_capability); + cap->nadm_random_sequence_capability = le16toh(cmd->optional_nadm_random_capability); + cap->cs_sync_phy_capability = cmd->optional_cs_sync_phys_supported; + + subfeatures = le16toh(cmd->optional_subfeatures_supported); + cap->no_fae = (subfeatures >> 1) & 1; + cap->channel_selection = (subfeatures >> 2) & 1; + cap->sounding_pct_estimate = (subfeatures >> 3) & 1; + + cap->t_ip1_capability = le16toh(cmd->optional_t_ip1_times_supported); + cap->t_ip2_capability = le16toh(cmd->optional_t_ip2_times_supported); + cap->t_fcs_capability = le16toh(cmd->optional_t_fcs_times_supported); + cap->t_pm_capability = le16toh(cmd->optional_t_pm_times_supported); + cap->t_sw = cmd->t_sw_time_supported; + cap->tx_snr_capablity = cmd->optional_tx_snr_capability; + + rsp->conn_handle = cmd->conn_handle; + *rsplen = sizeof(*rsp); + + return BLE_ERR_SUCCESS; +} + +int +ble_ll_cs_rx_security_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, + uint8_t *rspbuf) +{ + uint8_t *iv = connsm->cssm->drbg_ctx.iv; + uint8_t *in = connsm->cssm->drbg_ctx.in; + uint8_t *pv = connsm->cssm->drbg_ctx.pv; + + if (!connsm->flags.encrypted) { + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_SEC_REQ, + BLE_ERR_INSUFFICIENT_SEC, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + /* Vectors concatenation is done in the follwing manner: + * CS_IV = CS_IV_P || CS_IV_C + * The CS_IV_C is concatenated with the CS_IV_P. The least significant + * octet of CS_IV_C becomes the least significant octet of CS_IV. The most + * significant octet of CS_IV_P becomes the most significant octet of CS_IV. + */ + + /* Save Central's vector */ + memcpy(iv, dptr, 8); + memcpy(in, dptr + 8, 4); + memcpy(pv, dptr + 12, 8); + + /* Generate Peripheral's vector */ + ble_ll_rand_data_get(iv + 8, 8); + ble_ll_rand_data_get(in + 4, 4); + ble_ll_rand_data_get(pv + 8, 8); + + memcpy(rspbuf, iv + 8, 8); + memcpy(rspbuf + 8, in + 4, 4); + memcpy(rspbuf + 12, pv + 8, 8); + + ble_ll_cs_drbg_init(&connsm->cssm->drbg_ctx); + + connsm->cssm->start_procedure_count = 0; + + return BLE_LL_CTRL_CS_SEC_RSP; +} + +static void +ble_ll_cs_ev_sec_enable_complete(struct ble_ll_conn_sm *connsm, uint8_t status) +{ + struct ble_hci_ev_le_subev_cs_sec_enable_complete *ev; + struct ble_hci_ev *hci_ev; + + if (ble_ll_hci_is_le_event_enabled( + BLE_HCI_LE_SUBEV_CS_SEC_ENABLE_COMPLETE)) { + hci_ev = ble_transport_alloc_evt(0); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + ev->subev_code = BLE_HCI_LE_SUBEV_CS_SEC_ENABLE_COMPLETE; + ev->status = status; + ev->conn_handle = htole16(connsm->conn_handle); + + ble_ll_hci_event_send(hci_ev); + } + } +} + +void +ble_ll_cs_rx_security_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + int rc = 0; + struct ble_ll_cs_drbg_ctx *drbg_ctx = &connsm->cssm->drbg_ctx; + + if (!IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_SEC_START)) { + /* Ignore */ + return; + } + + /* Save Peripheral's vector */ + memcpy(drbg_ctx->iv + 8, dptr, 8); + memcpy(drbg_ctx->in + 4, dptr + 8, 4); + memcpy(drbg_ctx->pv + 8, dptr + 12, 8); + + rc = ble_ll_cs_drbg_init(drbg_ctx); + + connsm->cssm->start_procedure_count = 0; + + /* Stop the control procedure and send an event to the host */ + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_SEC_START); + ble_ll_cs_ev_sec_enable_complete(connsm, rc ? BLE_ERR_INV_LMP_LL_PARM : + BLE_ERR_SUCCESS); +} + +void +ble_ll_cs_rx_security_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error) +{ + /* Stop the control procedure and send an event to the host */ + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_SEC_START); + ble_ll_cs_ev_sec_enable_complete(connsm, ble_error); +} + +void +ble_ll_cs_security_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + uint8_t *iv = connsm->cssm->drbg_ctx.iv; + uint8_t *in = connsm->cssm->drbg_ctx.in; + uint8_t *pv = connsm->cssm->drbg_ctx.pv; + + /* Generate Central's vector */ + ble_ll_rand_data_get(iv, 8); + ble_ll_rand_data_get(in, 4); + ble_ll_rand_data_get(pv, 8); + + memcpy(dptr, iv, 8); + memcpy(dptr + 8, in, 4); + memcpy(dptr + 12, pv, 8); } int ble_ll_cs_hci_sec_enable(const uint8_t *cmdbuf, uint8_t cmdlen) { - return BLE_ERR_UNSUPPORTED; + const struct ble_hci_le_cs_sec_enable_cp *cmd = (const void *)cmdbuf; + struct ble_ll_conn_sm *connsm; + + connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle)); + if (!connsm) { + return BLE_ERR_UNK_CONN_ID; + } + + if (!connsm->flags.encrypted) { + return BLE_ERR_INSUFFICIENT_SEC; + } + + ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_SEC_START, NULL); + + return BLE_ERR_SUCCESS; } int ble_ll_cs_hci_set_def_settings(const uint8_t *cmdbuf, uint8_t cmdlen, uint8_t *rspbuf, uint8_t *rsplen) { - return BLE_ERR_UNSUPPORTED; + const struct ble_hci_le_cs_set_def_settings_cp *cmd = (const void *)cmdbuf; + struct ble_hci_le_cs_set_def_settings_rp *rsp = (void *)rspbuf; + const struct ble_ll_cs_supp_cap *cap = &g_ble_ll_cs_local_cap; + struct ble_ll_conn_sm *connsm; + struct ble_ll_cs_sm *cssm; + uint8_t i; + + connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle)); + if (!connsm) { + return BLE_ERR_UNK_CONN_ID; + } + + cssm = connsm->cssm; + + /* Check if a disabled role is used in CS configs */ + for (i = 0; i < ARRAY_SIZE(cssm->config); i++) { + struct ble_ll_cs_config *conf = &cssm->config[i]; + + if (conf->config_enabled && (1 << conf->role) & ~cmd->role_enable) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + } + + if ((cmd->role_enable & ~cap->roles_supported) != 0 || + (cap->number_of_antennas < cmd->cs_sync_antenna_selection && + cmd->cs_sync_antenna_selection < 0xFE)) { + /* Unsupported role or antenna selection used */ + return BLE_ERR_UNSUPPORTED; + } + + /* Allowed Transmit_Power_Level range: -127 to +20, + * (Complement system + special meaning for 0x7E and 0x7F) + */ + if (!(IN_RANGE(cmd->max_tx_power, 0x00, 0x14) || + IN_RANGE(cmd->max_tx_power, 0x7E, 0xFF))) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (cmd->max_tx_power == 0x7E) { + /* TODO: Set transmitter to minimum transmit power level + * supported by the board. + */ + cssm->max_tx_power = 0x80; + } else if (cmd->max_tx_power == 0x7F) { + /* TODO: Set transmitter to maximum transmit power level + * supported by the board. + */ + cssm->max_tx_power = 0x14; + } else { + /* TODO: Set transmitter to the nearest transmit power level + * supported by the board. + */ + cssm->max_tx_power = cmd->max_tx_power; + } + + cssm->roles_enabled = cmd->role_enable; + cssm->cs_sync_antenna_selection = cmd->cs_sync_antenna_selection; + + rsp->conn_handle = cmd->conn_handle; + *rsplen = sizeof(*rsp); + + return BLE_ERR_SUCCESS; +} + +int +ble_ll_cs_rx_fae_req(struct ble_ll_conn_sm *connsm, struct os_mbuf *om) +{ + /* Space for response code */ + om->om_len = 1; + OS_MBUF_PKTLEN(om) = om->om_len; + os_mbuf_append(om, connsm->cssm->local_fae_table, 72); + + return BLE_LL_CTRL_CS_FAE_RSP; +} + +static void +ble_ll_cs_ev_rd_rem_fae_complete(struct ble_ll_conn_sm *connsm, uint8_t status) +{ + struct ble_hci_ev_le_subev_cs_rd_rem_fae_complete *ev; + struct ble_hci_ev *hci_ev; + + if (ble_ll_hci_is_le_event_enabled( + BLE_HCI_LE_SUBEV_CS_RD_REM_FAE_COMPLETE)) { + hci_ev = ble_transport_alloc_evt(0); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + memset(ev, 0, sizeof(*ev)); + ev->subev_code = BLE_HCI_LE_SUBEV_CS_RD_REM_FAE_COMPLETE; + ev->status = status; + ev->conn_handle = htole16(connsm->conn_handle); + + if (status == BLE_ERR_SUCCESS) { + memcpy(ev->remote_fae_table, connsm->cssm->remote_fae_table, 72); + } + + ble_ll_hci_event_send(hci_ev); + } + } +} + +void +ble_ll_cs_rx_fae_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + if (!IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_FAE_REQ)) { + /* Ignore */ + return; + } + + memcpy(connsm->cssm->remote_fae_table, dptr, 72); + + /* Stop the control procedure and send an event to the host */ + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_FAE_REQ); + ble_ll_cs_ev_rd_rem_fae_complete(connsm, BLE_ERR_SUCCESS); +} + +void +ble_ll_cs_rx_fae_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error) +{ + /* Stop the control procedure and send an event to the host */ + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_FAE_REQ); + ble_ll_cs_ev_rd_rem_fae_complete(connsm, ble_error); } int ble_ll_cs_hci_rd_rem_fae(const uint8_t *cmdbuf, uint8_t cmdlen) { - return BLE_ERR_UNSUPPORTED; + const struct ble_hci_le_cs_rd_rem_fae_cp *cmd = (const void *)cmdbuf; + struct ble_ll_conn_sm *connsm; + + connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle)); + if (!connsm) { + return BLE_ERR_UNK_CONN_ID; + } + + ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_FAE_REQ, NULL); + + return BLE_ERR_SUCCESS; } int ble_ll_cs_hci_wr_cached_rem_fae(const uint8_t *cmdbuf, uint8_t cmdlen, uint8_t *rspbuf, uint8_t *rsplen) { - return BLE_ERR_UNSUPPORTED; + const struct ble_hci_le_cs_wr_cached_rem_fae_cp *cmd = (const void *)cmdbuf; + struct ble_hci_le_cs_wr_cached_rem_fae_rp *rsp = (void *)rspbuf; + struct ble_ll_conn_sm *connsm; + + connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle)); + if (!connsm) { + return BLE_ERR_UNK_CONN_ID; + } + + memcpy(connsm->cssm->remote_fae_table, cmd->remote_fae_table, 72); + + rsp->conn_handle = cmd->conn_handle; + *rsplen = sizeof(*rsp); + + return BLE_ERR_SUCCESS; +} + +void +ble_ll_cs_config_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + uint8_t config_id = connsm->cssm->config_req_id; + uint8_t action = connsm->cssm->config_req_action; + const struct ble_ll_cs_config *conf; + + assert(config_id < ARRAY_SIZE(connsm->cssm->config)); + + *dptr = config_id | action << 6; + + if (action == 0x00) { + /* Removing the config, all remaining fields are RFU. */ + memset(dptr + 1, 0, 26); + + return; + } + + conf = &connsm->cssm->tmp_config; + memcpy(dptr + 1, conf->chan_map, 10); + dptr[11] = conf->chan_map_repetition; + dptr[12] = conf->main_mode; + dptr[13] = conf->sub_mode; + dptr[14] = conf->main_mode_min_steps; + dptr[15] = conf->main_mode_max_steps; + dptr[16] = conf->main_mode_repetition; + dptr[17] = conf->mode_0_steps; + dptr[18] = conf->cs_sync_phy; + dptr[19] = conf->rtt_type | + conf->role << 4; + dptr[20] = conf->chan_sel | + conf->ch3cshape << 4; + dptr[21] = conf->ch3cjump; + dptr[22] = conf->t_ip1_index; + dptr[23] = conf->t_ip2_index; + dptr[24] = conf->t_fcs_index; + dptr[25] = conf->t_pm_index; + /* RFU octet */ + dptr[26] = 0x00; +} + +static int +ble_ll_cs_verify_config(struct ble_ll_cs_config *conf) +{ + if (conf->chan_map[9] & 0x80) { + return 1; + } + + if (conf->chan_map_repetition < 1) { + return 1; + } + + /* Valid combinations of Main_Mode and Sub_Mode selections */ + if (conf->main_mode == 0x01) { + if (conf->sub_mode != 0xFF) { + return 1; + } + } else if (conf->main_mode == 0x02) { + if (conf->sub_mode != 0x01 && + conf->sub_mode != 0x03 && + conf->sub_mode != 0xFF) { + return 1; + } + } else if (conf->main_mode == 0x03) { + if (conf->sub_mode != 0x02 && + conf->sub_mode != 0xFF) { + return 1; + } + } else { + return 1; + } + + if (conf->sub_mode == 0xFF) { + /* RFU if Sub_Mode is None */ + conf->main_mode_min_steps = 0x00; + conf->main_mode_max_steps = 0x00; + } + + if (conf->main_mode_repetition > 0x03) { + return 1; + } + + if (conf->mode_0_steps < 1 || conf->mode_0_steps > 3) { + return 1; + } + + if (conf->cs_sync_phy & 0xF0) { + return 1; + } + + if (conf->rtt_type > 0x06) { + return 1; + } + + if (conf->chan_sel > 0x01) { + return 1; + } + + if (conf->chan_sel == 0x01) { + if (conf->ch3cshape > 0x01) { + return 1; + } + + if (!IN_RANGE(conf->ch3cjump, 2, 8)) { + return 1; + } + } + + if (conf->t_ip1_index > 7) { + return 1; + } + + if (conf->t_ip2_index > 7) { + return 1; + } + + if (conf->t_fcs_index > 9) { + return 1; + } + + if (conf->t_pm_index > 2) { + return 1; + } + + return 0; +} + +static void +ble_ll_cs_ev_config_complete(struct ble_ll_conn_sm *connsm, uint8_t config_id, + uint8_t action, uint8_t status) +{ + struct ble_hci_ev_le_subev_cs_config_complete *ev; + const struct ble_ll_cs_config *conf; + struct ble_hci_ev *hci_ev; + + if (ble_ll_hci_is_le_event_enabled( + BLE_HCI_LE_SUBEV_CS_CONFIG_COMPLETE)) { + hci_ev = ble_transport_alloc_evt(0); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + memset(ev, 0, sizeof(*ev)); + ev->subev_code = BLE_HCI_LE_SUBEV_CS_CONFIG_COMPLETE; + ev->status = status; + ev->conn_handle = htole16(connsm->conn_handle); + ev->config_id = config_id; + ev->action = action; + + if (action != 0x00 && status == BLE_ERR_SUCCESS) { + conf = &connsm->cssm->config[config_id]; + ev->main_mode_type = conf->main_mode; + ev->sub_mode_type = conf->sub_mode; + ev->min_main_mode_steps = conf->main_mode_min_steps; + ev->max_main_mode_steps = conf->main_mode_max_steps; + ev->main_mode_repetition = conf->main_mode_repetition; + ev->mode_0_steps = conf->mode_0_steps; + ev->role = conf->role; + ev->rtt_type = conf->rtt_type; + ev->cs_sync_phy = conf->cs_sync_phy; + memcpy(ev->channel_map, conf->chan_map, 10); + ev->channel_map_repetition = conf->chan_map_repetition; + ev->channel_selection_type = conf->chan_sel; + ev->ch3c_shape = conf->ch3cshape; + ev->ch3c_jump = conf->ch3cjump; + ev->reserved = 0x00; + ev->t_ip1_time = conf->t_ip1; + ev->t_ip2_time = conf->t_ip2; + ev->t_fcs_time = conf->t_fcs; + ev->t_pm_time = conf->t_pm; + } + + ble_ll_hci_event_send(hci_ev); + } + } +} + +int +ble_ll_cs_rx_config_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, + uint8_t *rspbuf) +{ + struct ble_ll_cs_config *conf; + uint8_t config_id = *dptr & 0b00111111; + uint8_t action = (*dptr & 0b11000000) >> 6; + struct ble_ll_cs_sm *cssm = connsm->cssm; + + if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CONF)) { + if (CONN_IS_CENTRAL(connsm)) { + /* Reject CS config initiated by peripheral */ + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_CONFIG_REQ, + BLE_ERR_LMP_COLLISION, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } else { + /* Take no further action in the Peripheral-initiated procedure + * and proceed to handle the Central-initiated procedure. + */ + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_CONF); + } + } + + if (config_id >= ARRAY_SIZE(cssm->config)) { + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_CONFIG_REQ, + BLE_ERR_INV_LMP_LL_PARM, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + conf = &cssm->config[config_id]; + if (conf->config_in_use) { + /* CS procedure in progress exit with error */ + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_CONFIG_REQ, + BLE_ERR_CMD_DISALLOWED, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + /* Respond with LL_CS_CONFIG_RSP PDU */ + *rspbuf = config_id | action << 6; + + if (action == 0x00) { + /* CS configuration removed. */ + memset(conf, 0, sizeof(*conf)); + + return BLE_LL_CTRL_CS_CONFIG_RSP; + } + + conf = &cssm->tmp_config; + memset(conf, 0, sizeof(*conf)); + memcpy(conf->chan_map, dptr + 1, 10); + conf->chan_map_repetition = dptr[11]; + conf->main_mode = dptr[12]; + conf->sub_mode = dptr[13]; + conf->main_mode_min_steps = dptr[14]; + conf->main_mode_max_steps = dptr[15]; + conf->main_mode_repetition = dptr[16]; + conf->mode_0_steps = dptr[17]; + conf->cs_sync_phy = dptr[18]; + conf->rtt_type = dptr[19] & 0b00001111; + conf->role = (~dptr[19] >> 4) & 0b00000001; + conf->chan_sel = (dptr[20] & 0b00001111); + conf->ch3cshape = (dptr[20] & 0b11110000) >> 4; + conf->ch3cjump = dptr[21]; + conf->t_ip1_index = dptr[22]; + conf->t_ip2_index = dptr[23]; + conf->t_fcs_index = dptr[24]; + conf->t_pm_index = dptr[25]; + + if (ble_ll_cs_verify_config(conf)) { + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_CONFIG_REQ, + BLE_ERR_UNSUPP_LMP_LL_PARM, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + conf->t_ip1 = t_ip1[conf->t_ip1_index]; + conf->t_ip2 = t_ip2[conf->t_ip2_index]; + conf->t_fcs = t_fcs[conf->t_fcs_index]; + conf->t_pm = t_pm[conf->t_pm_index]; + conf->config_enabled = 1; + + memcpy(&cssm->config[config_id], conf, sizeof(*conf)); + memset(conf, 0, sizeof(*conf)); + + return BLE_LL_CTRL_CS_CONFIG_RSP; +} + +void +ble_ll_cs_rx_config_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + uint8_t config_id = *dptr & 0b00111111; + uint8_t action = (*dptr & 0b11000000) >> 6; + struct ble_ll_cs_sm *cssm = connsm->cssm; + + if (config_id != cssm->config_req_id || + !IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CONF)) { + return; + } + + /* Configure CS config locally */ + memcpy(&cssm->config[config_id], &cssm->tmp_config, sizeof(cssm->tmp_config)); + memset(&cssm->tmp_config, 0, sizeof(cssm->tmp_config)); + + /* Stop the control procedure and send an event to the host */ + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_CONF); + ble_ll_cs_ev_config_complete(connsm, config_id, action, BLE_ERR_SUCCESS); +} + +void +ble_ll_cs_rx_config_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error) +{ + struct ble_ll_cs_sm *cssm = connsm->cssm; + + memset(&cssm->tmp_config, 0, sizeof(cssm->tmp_config)); + + /* Stop the control procedure and send an event to the host */ + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_CONF); + ble_ll_cs_ev_config_complete(connsm, cssm->config_req_id, + cssm->config_req_action, ble_error); +} + +static int +ble_ll_cs_select_capability(uint8_t capability_values_count, + uint8_t *out_index, uint16_t local_capability, + uint16_t remote_capability) +{ + uint16_t common_capability = local_capability & remote_capability; + uint8_t i; + + for (i = 0; i < capability_values_count; i++) { + if ((common_capability >> i) & 1) { + *out_index = i; + return 0; + } + } + + return 1; } int ble_ll_cs_hci_create_config(const uint8_t *cmdbuf, uint8_t cmdlen) { - return BLE_ERR_UNSUPPORTED; + const struct ble_hci_le_cs_create_config_cp *cmd = (const void *)cmdbuf; + struct ble_ll_conn_sm *connsm; + struct ble_ll_cs_sm *cssm; + struct ble_ll_cs_config *conf; + + if (cmdlen != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* If no connection handle exit with error */ + connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle)); + if (!connsm) { + return BLE_ERR_UNK_CONN_ID; + } + + cssm = connsm->cssm; + if (cmd->config_id >= ARRAY_SIZE(cssm->config)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + conf = &cssm->config[cmd->config_id]; + + /* If already pending or CS procedure in progress exit with error */ + if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CONF) || + conf->config_in_use) { + return BLE_ERR_CMD_DISALLOWED; + } + + /* Save the CS configuration in temporary variable as the config + * might be rejected by the remote. + */ + conf = &cssm->tmp_config; + memset(conf, 0, sizeof(*conf)); + conf->config_enabled = 1; + conf->main_mode = cmd->main_mode_type; + conf->sub_mode = cmd->sub_mode_type; + conf->main_mode_min_steps = cmd->min_main_mode_steps; + conf->main_mode_max_steps = cmd->max_main_mode_steps; + conf->main_mode_repetition = cmd->main_mode_repetition; + conf->mode_0_steps = cmd->mode_0_steps; + conf->role = cmd->role; + conf->rtt_type = cmd->rtt_type; + conf->cs_sync_phy = cmd->cs_sync_phy; + memcpy(conf->chan_map, cmd->channel_map, 10); + conf->chan_map_repetition = cmd->channel_map_repetition; + conf->chan_sel = cmd->channel_selection_type; + conf->ch3cshape = cmd->ch3c_shape; + conf->ch3cjump = cmd->ch3c_jump; + + if (ble_ll_cs_select_capability(ARRAY_SIZE(t_ip1), &conf->t_ip1_index, + cssm->remote_cap.t_ip1_capability, + g_ble_ll_cs_local_cap.t_ip1_capability)) { + memset(conf, 0, sizeof(*conf)); + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (ble_ll_cs_select_capability(ARRAY_SIZE(t_ip2), &conf->t_ip2_index, + cssm->remote_cap.t_ip2_capability, + g_ble_ll_cs_local_cap.t_ip2_capability)) { + memset(conf, 0, sizeof(*conf)); + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (ble_ll_cs_select_capability(ARRAY_SIZE(t_fcs), &conf->t_fcs_index, + cssm->remote_cap.t_fcs_capability, + g_ble_ll_cs_local_cap.t_fcs_capability)) { + memset(conf, 0, sizeof(*conf)); + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (ble_ll_cs_select_capability(ARRAY_SIZE(t_pm), &conf->t_pm_index, + cssm->remote_cap.t_pm_capability, + g_ble_ll_cs_local_cap.t_pm_capability)) { + memset(conf, 0, sizeof(*conf)); + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + conf->t_ip1 = t_ip1[conf->t_ip1_index]; + conf->t_ip2 = t_ip2[conf->t_ip2_index]; + conf->t_fcs = t_fcs[conf->t_fcs_index]; + conf->t_pm = t_pm[conf->t_pm_index]; + + if (cmd->create_context == 0x01) { + /* Configure the CS config in the remote controller */ + cssm->config_req_id = cmd->config_id; + cssm->config_req_action = 0x01; + ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_CONF, NULL); + } else { + ble_ll_cs_ev_config_complete(connsm, cmd->config_id, 0x01, BLE_ERR_SUCCESS); + } + + return BLE_ERR_SUCCESS; } int ble_ll_cs_hci_remove_config(const uint8_t *cmdbuf, uint8_t cmdlen) { - return BLE_ERR_UNSUPPORTED; + const struct ble_hci_le_cs_remove_config_cp *cmd = (const void *)cmdbuf; + struct ble_ll_conn_sm *connsm; + struct ble_ll_cs_sm *cssm; + struct ble_ll_cs_config *conf; + + if (cmdlen != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* If no connection handle exit with error */ + connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle)); + if (!connsm) { + return BLE_ERR_UNK_CONN_ID; + } + + cssm = connsm->cssm; + if (cmd->config_id >= ARRAY_SIZE(cssm->config)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + conf = &cssm->config[cmd->config_id]; + + /* If already pending or CS procedure in progress exit with error */ + if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CONF) || + conf->config_in_use) { + return BLE_ERR_CMD_DISALLOWED; + } + + /* Remove the CS config locally */ + memset(conf, 0, sizeof(*conf)); + + /* Configure the CS config in the remote controller */ + cssm->config_req_id = cmd->config_id; + cssm->config_req_action = 0x00; + ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_CONF, NULL); + + return BLE_ERR_SUCCESS; +} + +static int +ble_ll_cs_proc_set_chan_class(const uint8_t *channel_classification) +{ + uint8_t i, j, next_id, byte; + + /* TODO: + * 1. The interval between two successive commands sent shall be at least 1 second. + * Otherwise, the Controller shall return the error code Command Disallowed (0x0C). + * + * 2. Combine the Host chan_class with local chan_class capabilities? + */ + + if (channel_classification[0] & 0b00000011 || + channel_classification[2] & 0b10000000 || + channel_classification[3] & 0b00000011 || + channel_classification[9] & 0b11100000) { + /* Channels 0, 1, 23, 24, 25, 77, 78, and the bit 79 (non-channel) + * are RFU. At least 15 channels shall be enabled. + */ + return -1; + } + + for (i = 0, j = 0; i < ARRAY_SIZE(g_ble_ll_cs_chan_class); ++i) { + byte = channel_classification[i]; + next_id = i * 8; + + while (byte) { + if (byte & 1) { + g_ble_ll_cs_chan_indices[j++] = next_id; + } + ++next_id; + byte >>= 1; + } + } + + g_ble_ll_cs_chan_count = j; + if (g_ble_ll_cs_chan_count < 15) { + return -1; + } + + memcpy(g_ble_ll_cs_chan_class, channel_classification, + sizeof(g_ble_ll_cs_chan_class)); + + return 0; } int ble_ll_cs_hci_set_chan_class(const uint8_t *cmdbuf, uint8_t cmdlen) { - return BLE_ERR_UNSUPPORTED; + int rc; + const struct ble_hci_le_cs_set_chan_class_cp *cmd = (const void *)cmdbuf; + + rc = ble_ll_cs_proc_set_chan_class(cmd->channel_classification); + if (rc) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + return BLE_ERR_SUCCESS; } int ble_ll_cs_hci_set_proc_params(const uint8_t *cmdbuf, uint8_t cmdlen, uint8_t *rspbuf, uint8_t *rsplen) { - return BLE_ERR_UNSUPPORTED; + const struct ble_hci_le_cs_set_proc_params_cp *cmd = (const void *)cmdbuf; + struct ble_hci_le_cs_set_proc_params_rp *rsp = (void *)rspbuf; + struct ble_ll_conn_sm *connsm; + struct ble_ll_cs_config *conf; + struct ble_ll_cs_pref_proc_params *params; + + if (cmdlen != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* If no connection handle exit with error */ + connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle)); + if (!connsm) { + return BLE_ERR_UNK_CONN_ID; + } + + if (cmd->config_id >= ARRAY_SIZE(connsm->cssm->config)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + conf = &connsm->cssm->config[cmd->config_id]; + + /* If CS configuration with Config_ID does not exists */ + if (!conf->config_enabled) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* If CS measurement is enabled exit with error */ + if (connsm->cssm->measurement_enabled) { + return BLE_ERR_CMD_DISALLOWED; + } + + if (g_ble_ll_cs_chan_count < 15) { + return BLE_ERR_INSUFFICIENT_CHAN; + } + + params = &conf->pref_proc_params; + params->max_procedure_len = htole16(cmd->max_procedure_len); + params->min_procedure_interval = htole16(cmd->min_procedure_interval); + params->max_procedure_interval = htole16(cmd->max_procedure_interval); + params->max_procedure_count = htole16(cmd->max_procedure_count); + params->min_subevent_len = get_le24(cmd->min_subevent_len); + params->max_subevent_len = get_le24(cmd->max_subevent_len); + params->aci = cmd->tone_antenna_config_selection; + params->phy = cmd->phy; + params->tx_power_delta = cmd->tx_power_delta; + params->preferred_peer_antenna = cmd->preferred_peer_antenna; + params->snr_control_initiator = cmd->snr_control_initiator & 0x0F; + params->snr_control_reflector = cmd->snr_control_reflector & 0x0F; + params->params_ready = 1; + + rsp->conn_handle = cmd->conn_handle; + *rsplen = sizeof(*rsp); + + return BLE_ERR_SUCCESS; } int ble_ll_cs_hci_proc_enable(const uint8_t *cmdbuf, uint8_t cmdlen) { - return BLE_ERR_UNSUPPORTED; + const struct ble_hci_le_cs_proc_enable_cp *cmd = (const void *)cmdbuf; + struct ble_ll_conn_sm *connsm; + struct ble_ll_cs_config *conf; + + if (cmdlen != sizeof(*cmd) || cmd->config_id >= ARRAY_SIZE(connsm->cssm->config)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* If no connection handle exit with error */ + connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle)); + if (!connsm) { + return BLE_ERR_UNK_CONN_ID; + } + + conf = &connsm->cssm->config[cmd->config_id]; + if (!conf->config_enabled) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (cmd->enable) { + if (!conf->pref_proc_params.params_ready || + connsm->cssm->measurement_enabled) { + return BLE_ERR_CMD_DISALLOWED; + } + + if (g_ble_ll_cs_chan_count < 15) { + return BLE_ERR_INSUFFICIENT_CHAN; + } + + /* Start scheduling CS procedures */ + ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_START, NULL); + } else { + if (!connsm->cssm->measurement_enabled) { + return BLE_ERR_CMD_DISALLOWED; + } + + connsm->cssm->terminate_measurement = 1; + connsm->cssm->terminate_config_id = cmd->config_id; + connsm->cssm->terminate_error_code = BLE_ERR_SUCCESS; + + /* Terminate the CS measurement early */ + ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_TERMINATE, NULL); + } + + return BLE_ERR_SUCCESS; +} + +static void +ble_ll_cs_ev_cs_proc_enable_complete(struct ble_ll_conn_sm *connsm, + uint8_t config_id, uint8_t status) +{ + struct ble_hci_ev_le_subev_cs_proc_enable_complete *ev; + const struct ble_ll_cs_proc_params *params; + struct ble_hci_ev *hci_ev; + + if (ble_ll_hci_is_le_event_enabled( + BLE_HCI_LE_SUBEV_CS_PROC_ENABLE_COMPLETE)) { + hci_ev = ble_transport_alloc_evt(0); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + memset(ev, 0, sizeof(*ev)); + ev->subev_code = BLE_HCI_LE_SUBEV_CS_PROC_ENABLE_COMPLETE; + ev->status = status; + ev->conn_handle = htole16(connsm->conn_handle); + ev->config_id = config_id; + ev->state = connsm->cssm->measurement_enabled; + + if (status == BLE_ERR_SUCCESS) { + params = &connsm->cssm->config[config_id].proc_params; + ev->tone_antenna_config_selection = params->aci; + ev->selected_tx_power = g_ble_ll_tx_power; + put_le24(ev->subevent_len, params->subevent_len); + ev->subevents_per_event = params->subevents_per_event; + ev->subevent_interval = htole16(params->subevent_interval); + ev->event_interval = htole16(params->event_interval); + ev->procedure_interval = htole16(params->procedure_interval); + ev->procedure_count = htole16(params->max_procedure_count); + } + + ble_ll_hci_event_send(hci_ev); + } + } +} + +void +ble_ll_cs_start_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + uint8_t config_id = connsm->cssm->config_req_id; + uint8_t subevents_per_event; + uint16_t anchor_conn_event = connsm->event_cntr + MIN_CONN_EVENT_COUNT_BEFORE_START; + struct ble_ll_cs_pref_proc_params *params; + struct ble_ll_cs_proc_params *ps; + uint32_t offset_ticks; + uint32_t offset; + uint32_t offset_min = OFFSET_MIN_USECS; + uint32_t offset_max = OFFSET_MAX_USECS; + uint32_t subevent_len; + /* Handle only one CS procedure for now */ + uint16_t procedure_interval = 0; + uint16_t event_interval; + uint16_t subevent_interval; + + assert(config_id < ARRAY_SIZE(connsm->cssm->config)); + params = &connsm->cssm->config[config_id].pref_proc_params; + subevent_len = params->min_subevent_len; + + /* Select the offset so that it will not overlap with a connection event */ + offset_ticks = connsm->conn_sch.end_time - connsm->conn_sch.start_time; + offset = ble_ll_tmr_t2u(offset_ticks + OFFSET_FROM_CONN_EVENT_TICKS); + + if (offset_min < offset) { + offset_min = offset; + } + offset_max = ble_ll_tmr_t2u(connsm->conn_itvl_ticks); + + /* TODO: There can be multiple scheduling approaches. + * For now use a simple one: schedule one CS subevent per CS event, + * the subevent takes whole CS event, the subevent do not exceed + * the underlying LE connection interval. + * The number of subevents translates into the frequency of sending + * subevent result events to the host. Each new CS event/subevent can help + * get the sync back on track in case of de-sync. + */ + event_interval = 1; + subevents_per_event = 1; + subevent_interval = 0; + + /* LL_CS_REQ parameters are just a suggestion and may be resuggested + * by the peer in LL_CS_RSP. + */ + + ps = &connsm->cssm->config[config_id].proc_params; + memset(ps, 0, sizeof(*ps)); + ps->max_procedure_len = params->max_procedure_len; + ps->procedure_interval = procedure_interval; + ps->max_procedure_count = params->max_procedure_count; + ps->preferred_peer_antenna = params->preferred_peer_antenna; + ps->tx_snr_i = params->snr_control_initiator; + ps->tx_snr_r = params->snr_control_reflector; + + *dptr = config_id; + put_le16(dptr + 1, anchor_conn_event); + put_le24(dptr + 3, offset_min); + put_le24(dptr + 6, offset_max); + put_le16(dptr + 9, params->max_procedure_len); + put_le16(dptr + 11, event_interval); + dptr[13] = subevents_per_event; + put_le16(dptr + 14, subevent_interval); + put_le24(dptr + 16, subevent_len); + put_le16(dptr + 19, procedure_interval); + put_le16(dptr + 21, params->max_procedure_count); + dptr[23] = params->aci; + dptr[24] = params->preferred_peer_antenna; + dptr[25] = params->phy; + dptr[26] = params->tx_power_delta; + dptr[27] = params->snr_control_initiator | + params->snr_control_reflector << 4; +} + +static int +ble_ll_cs_start_ind_make(struct ble_ll_conn_sm *connsm, uint8_t *rspbuf) +{ + uint8_t config_id = connsm->cssm->config_req_id; + struct ble_ll_cs_proc_params *ps; + + assert(config_id < ARRAY_SIZE(connsm->cssm->config)); + ps = &connsm->cssm->config[config_id].proc_params; + + *rspbuf = config_id; + put_le16(rspbuf + 1, ps->anchor_conn_event); + put_le24(rspbuf + 3, ps->event_offset); + put_le16(rspbuf + 6, ps->event_interval); + rspbuf[8] = ps->subevents_per_event; + put_le16(rspbuf + 9, ps->subevent_interval); + put_le24(rspbuf + 11, ps->subevent_len); + rspbuf[14] = ps->aci; + rspbuf[15] = ps->phy; + rspbuf[16] = ps->tx_power_delta; + rspbuf[17] = 0x00; + + return BLE_LL_CTRL_CS_IND; +} + +static int +ble_ll_cs_start_rsp_make(struct ble_ll_conn_sm *connsm, uint8_t *rspbuf) +{ + uint8_t config_id = connsm->cssm->config_req_id; + struct ble_ll_cs_proc_params *ps; + + assert(config_id < ARRAY_SIZE(connsm->cssm->config)); + ps = &connsm->cssm->config[config_id].proc_params; + + *rspbuf = config_id; + put_le16(rspbuf + 1, ps->anchor_conn_event); + put_le24(rspbuf + 3, ps->offset_min); + put_le24(rspbuf + 6, ps->offset_max); + put_le16(rspbuf + 9, ps->event_interval); + rspbuf[11] = ps->subevents_per_event; + put_le16(rspbuf + 12, ps->subevent_interval); + put_le24(rspbuf + 14, ps->subevent_len); + rspbuf[17] = ps->aci; + rspbuf[18] = ps->phy; + rspbuf[19] = ps->tx_power_delta; + /* 1 RFU octet */ + rspbuf[20] = 0x00; + + return BLE_LL_CTRL_CS_RSP; +} + +static int +validate_cs_start_parameters(struct ble_ll_conn_sm *connsm, + struct ble_ll_cs_proc_params *ps) +{ + int err = 0; + uint32_t conn_itvl_us = connsm->conn_itvl * BLE_LL_CONN_ITVL_USECS; + + if (conn_itvl_us < ps->offset_max) { + /* TODO: Shall be less than the LE connection interval, but how much? */ + ps->offset_max = conn_itvl_us - 1; + err = 1; + } + + if (ps->offset_min < 500 || ps->offset_max < ps->offset_min) { + ps->offset_min = 500; + err = 1; + } + + if (ps->anchor_conn_event < connsm->event_cntr + 3) { + /* XXX: +3 to ensure that LL_CS_RSP and LL_CS_IND will make it on time + * before the first CS step. But maybe that's a bit of an exaggeration. + */ + ps->anchor_conn_event = connsm->event_cntr + 3; + err = 1; + } + + if (!IN_RANGE(ps->event_interval, BLE_LL_CS_EVENT_INTERVAL_MIN, + BLE_LL_CS_EVENT_INTERVAL_MAX)) { + ps->event_interval = BLE_LL_CS_EVENT_INTERVAL_MIN; + err = 1; + } + + if (!IN_RANGE(ps->subevents_per_event, BLE_LL_CS_SUBEVENTS_PER_PROCEDURE_MIN, + BLE_LL_CS_SUBEVENTS_PER_PROCEDURE_MAX)) { + ps->subevents_per_event = BLE_LL_CS_SUBEVENTS_PER_PROCEDURE_MIN; + err = 1; + } + + if (ps->subevents_per_event == 1) { + ps->subevent_interval = 0; + } else if (!IN_RANGE(ps->subevent_interval, BLE_LL_CS_SUBEVENTS_INTERVAL_MIN, + BLE_LL_CS_SUBEVENTS_INTERVAL_MAX)) { + ps->subevent_interval = BLE_LL_CS_SUBEVENTS_INTERVAL_MIN; + err = 1; + } + + if (!IN_RANGE(ps->subevent_len, BLE_LL_CS_SUBEVENT_LEN_MIN, + BLE_LL_CS_SUBEVENT_LEN_MAX)) { + ps->subevent_len = BLE_LL_CS_SUBEVENT_LEN_MIN; + err = 1; + } + + if (ps->aci > 7) { + /* Resuggest ACI with ID 0 */ + ps->aci = 0; + err = 1; + } + + if (ps->phy == 0) { + /* Resuggest using LE 1M PHY */ + ps->phy = 1; + err = 1; + } + + if (!(IN_RANGE(ps->tx_power_delta, 0x00, 0x14) || + IN_RANGE(ps->tx_power_delta, 0x7E, 0xFF))) { + /* Invalid range. Resuggest to not change the power level */ + ps->tx_power_delta = 0x00; + err = 1; + } + + if ((ps->tx_snr_i > 4 && ps->tx_snr_i != 0xF) || + (ps->tx_snr_r > 4 && ps->tx_snr_r != 0xF) || + ps->preferred_peer_antenna == 0) { + /* We cannot resuggest these, so reject them */ + return -1; + } + + return err; +} + +int +ble_ll_cs_rx_cs_start_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf) +{ + uint8_t config_id = *dptr & 0b00111111; + struct ble_ll_cs_config *conf; + struct ble_ll_cs_proc_params *ps; + int rc; + + /* TODO: take into account the struct ble_ll_cs_pref_proc_params *params */ + + if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_START)) { + if (CONN_IS_CENTRAL(connsm)) { + /* Reject CS config initiated by peripheral */ + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_REQ, + BLE_ERR_LMP_COLLISION, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } else { + /* Take no further action in the Peripheral-initiated procedure + * and proceed to handle the Central-initiated procedure. + */ + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_START); + } + } + + if (config_id >= ARRAY_SIZE(connsm->cssm->config)) { + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_REQ, + BLE_ERR_INV_LMP_LL_PARM, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + conf = &connsm->cssm->config[config_id]; + + ps = &conf->proc_params; + memset(ps, 0, sizeof(*ps)); + + ps->anchor_conn_event = get_le16(dptr + 1); + ps->offset_min = get_le24(dptr + 3); + ps->offset_max = get_le24(dptr + 6); + ps->max_procedure_len = get_le16(dptr + 9); + ps->event_interval = get_le16(dptr + 11); + ps->subevents_per_event = dptr[13]; + ps->subevent_interval = get_le16(dptr + 14); + ps->subevent_len = get_le24(dptr + 16); + ps->procedure_interval = get_le16(dptr + 19); + ps->max_procedure_count = get_le16(dptr + 21); + ps->aci = dptr[23]; + ps->preferred_peer_antenna = dptr[24] & 0b00001111; + ps->phy = dptr[25] & 0b00001111; + ps->tx_power_delta = dptr[26]; + ps->tx_snr_i = dptr[27] & 0b00001111; + ps->tx_snr_r = (dptr[27] >> 4) & 0b00001111; + + /* In Peripheral role some parameters can be resuggested in LL_CS_RSP. */ + rc = validate_cs_start_parameters(connsm, ps); + if (rc < 0 || (rc > 0 && CONN_IS_CENTRAL(connsm))) { + memset(ps, 0, sizeof(*ps)); + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_REQ, + BLE_ERR_INV_LMP_LL_PARM, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + /* In Central role reply with LL_CS_IND instead of LL_CS_RSP */ + if (CONN_IS_CENTRAL(connsm)) { + rc = ble_ll_cs_proc_scheduling_start(connsm, config_id); + if (rc) { + memset(ps, 0, sizeof(*ps)); + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_REQ, rc, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + ble_ll_cs_ev_cs_proc_enable_complete(connsm, config_id, BLE_ERR_SUCCESS); + + return ble_ll_cs_start_ind_make(connsm, rspbuf); + } + + return ble_ll_cs_start_rsp_make(connsm, rspbuf); +} + +int +ble_ll_cs_rx_cs_start_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf) +{ + int rc; + struct ble_ll_cs_config *conf; + struct ble_ll_cs_proc_params *ps; + uint8_t config_id = *dptr & 0b00111111; + + if (!IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_START) || + CONN_IS_PERIPHERAL(connsm)) { + /* Ignore */ + return BLE_ERR_MAX; + } + + if (config_id != connsm->cssm->config_req_id) { + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_RSP, + BLE_ERR_INV_LMP_LL_PARM, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + conf = &connsm->cssm->config[config_id]; + + ps = &conf->proc_params; + ps->anchor_conn_event = get_le16(dptr + 1); + ps->offset_min = get_le24(dptr + 3); + ps->offset_max = get_le24(dptr + 6); + ps->event_interval = get_le16(dptr + 9); + ps->subevents_per_event = dptr[11]; + ps->subevent_interval = get_le16(dptr + 12); + ps->subevent_len = get_le24(dptr + 14); + ps->aci = dptr[17]; + ps->phy = dptr[18] & 0b00001111; + ps->tx_power_delta = dptr[19]; + + rc = validate_cs_start_parameters(connsm, ps); + if (rc) { + memset(ps, 0, sizeof(*ps)); + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_RSP, + BLE_ERR_INV_LMP_LL_PARM, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + ps->event_offset = ps->offset_min; + + /* Stop the control procedure and send an event to the host */ + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_START); + + rc = ble_ll_cs_proc_scheduling_start(connsm, config_id); + if (rc) { + memset(ps, 0, sizeof(*ps)); + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_RSP, rc, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + ble_ll_cs_ev_cs_proc_enable_complete(connsm, config_id, BLE_ERR_SUCCESS); + + return ble_ll_cs_start_ind_make(connsm, rspbuf); +} + +int +ble_ll_cs_rx_cs_start_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr, + uint8_t *rspbuf) +{ + int rc; + uint8_t config_id = *dptr & 0b00111111; + struct ble_ll_cs_proc_params *ps; + struct ble_ll_cs_config *conf; + + if (CONN_IS_CENTRAL(connsm)) { + /* Ignore unexpected response */ + return BLE_ERR_MAX; + } + + if (config_id != connsm->cssm->config_req_id) { + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_IND, + BLE_ERR_INV_LMP_LL_PARM, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + conf = &connsm->cssm->config[config_id]; + ps = &conf->proc_params; + + /* Overwrite ressugested values */ + ps->anchor_conn_event = get_le16(dptr + 1); + ps->event_offset = get_le24(dptr + 3); + ps->event_interval = get_le16(dptr + 6); + ps->subevents_per_event = dptr[8]; + ps->subevent_interval = get_le16(dptr + 9); + ps->subevent_len = get_le24(dptr + 11); + ps->aci = dptr[14]; + ps->phy = dptr[15]; + ps->tx_power_delta = dptr[16]; + + rc = validate_cs_start_parameters(connsm, ps); + if (rc) { + memset(ps, 0, sizeof(*ps)); + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_IND, + BLE_ERR_INV_LMP_LL_PARM, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + rc = ble_ll_cs_proc_scheduling_start(connsm, config_id); + if (rc) { + memset(ps, 0, sizeof(*ps)); + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_IND, rc, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + ble_ll_cs_ev_cs_proc_enable_complete(connsm, config_id, BLE_ERR_SUCCESS); + + return BLE_ERR_MAX; +} + +void +ble_ll_cs_rx_cs_start_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error) +{ + /* Stop the control procedure and send an event to the host */ + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_START); + ble_ll_cs_ev_cs_proc_enable_complete(connsm, connsm->cssm->config_req_id, ble_error); +} + +void +ble_ll_cs_terminate_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + struct ble_ll_cs_sm *cssm = connsm->cssm; + uint8_t config_id = cssm->terminate_config_id; + + assert(config_id < ARRAY_SIZE(cssm->config)); + + *dptr = config_id; + put_le16(dptr + 1, cssm->start_procedure_count + cssm->procedure_count); + dptr[3] = cssm->terminate_error_code; +} + +int +ble_ll_cs_rx_cs_terminate_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, + uint8_t *rspbuf) +{ + struct ble_ll_cs_sm *cssm = connsm->cssm; + uint8_t config_id = *dptr & 0b00111111; + uint16_t procedure_count = get_le16(dptr + 1); + struct ble_ll_cs_proc_params *params; + + if (config_id >= ARRAY_SIZE(cssm->config)) { + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_TERMINATE_REQ, + BLE_ERR_INV_LMP_LL_PARM, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + params = &cssm->config[config_id].proc_params; + + if (procedure_count > cssm->start_procedure_count + params->max_procedure_count || + procedure_count < cssm->start_procedure_count) { + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_TERMINATE_REQ, + BLE_ERR_CMD_DISALLOWED, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + cssm->terminate_measurement = 1; + + *rspbuf = config_id; + put_le16(rspbuf + 1, cssm->start_procedure_count + cssm->procedure_count); + rspbuf[3] = BLE_ERR_SUCCESS; + + return BLE_LL_CTRL_CS_TERMINATE_RSP; +} + +void +ble_ll_cs_rx_cs_terminate_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + struct ble_ll_cs_sm *cssm = connsm->cssm; + uint8_t config_id = *dptr & 0b00111111; + uint8_t error_code = dptr[3]; + uint16_t procedure_count = get_le16(dptr + 1); + uint16_t total_procedure_count; + + if (config_id != cssm->config_req_id || + !IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_TERMINATE)) { + return; + } + + total_procedure_count = cssm->start_procedure_count + cssm->procedure_count; + if (total_procedure_count < procedure_count) { + cssm->start_procedure_count = procedure_count; + } else { + cssm->start_procedure_count = total_procedure_count; + } + + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_TERMINATE); + + /* TODO: In d1.0r06 the LL_CS_TERMINATE_IND was replaced with + * LL_CS_TERMINATE_REQ/RSP, but not all descriptions and diagrams + * have been updated yet. + */ + ble_ll_cs_ev_cs_proc_enable_complete(connsm, config_id, error_code); +} + +void +ble_ll_cs_rx_cs_terminate_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error) +{ + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_TERMINATE); + ble_ll_cs_ev_cs_proc_enable_complete(connsm, connsm->cssm->config_req_id, ble_error); } int @@ -116,4 +1810,66 @@ ble_ll_cs_hci_test_end(void) return BLE_ERR_UNSUPPORTED; } +void +ble_ll_cs_init(void) +{ + struct ble_ll_cs_supp_cap *cap = &g_ble_ll_cs_local_cap; + + /* Set local CS capabilities. Only basic features supported for now. */ + cap->mode_types = 0x00; + cap->rtt_capability = 0x00; + cap->rtt_aa_only_n = 0x00; + cap->rtt_sounding_n = 0x00; + cap->rtt_random_sequence_n = 0x00; + cap->nadm_sounding_capability = 0x0000; + cap->nadm_random_sequence_capability = 0x0000; + cap->cs_sync_phy_capability = 0x00; + cap->number_of_antennas = 0x01; + cap->max_number_of_antenna_paths = 0x01; + cap->roles_supported = 0x03; + cap->no_fae = 0x00; + cap->channel_selection = 0x00; + cap->sounding_pct_estimate = 0x00; + cap->max_number_of_configs = 0x04; + cap->max_number_of_procedures = 0x0001; + cap->t_sw = 0x0A; + cap->t_ip1_capability = 0x0080; + cap->t_ip2_capability = 0x0080; + cap->t_fcs_capability = 0x0200; + cap->t_pm_capability = 0x0004; + cap->tx_snr_capablity = 0x00; + + ble_ll_cs_proc_set_chan_class(default_channel_classification); +} + +void +ble_ll_cs_reset(void) +{ + ble_ll_cs_init(); +} + +void +ble_ll_cs_sm_init(struct ble_ll_conn_sm *connsm) +{ + uint8_t i; + + for (i = 0; i < ARRAY_SIZE(g_ble_ll_cs_sm); i++) { + if (g_ble_ll_cs_sm[i].connsm == NULL) { + connsm->cssm = &g_ble_ll_cs_sm[i]; + memset(connsm->cssm, 0, sizeof(*connsm->cssm)); + connsm->cssm->connsm = connsm; + break; + } + } +} + +void +ble_ll_cs_sm_free(struct ble_ll_conn_sm *connsm) +{ + if (connsm->cssm) { + memset(connsm->cssm, 0, sizeof(*connsm->cssm)); + connsm->cssm = NULL; + } +} + #endif /* BLE_LL_CHANNEL_SOUNDING */ diff --git a/nimble/controller/src/ble_ll_cs_drbg.c b/nimble/controller/src/ble_ll_cs_drbg.c new file mode 100644 index 0000000000..0f40cf9f73 --- /dev/null +++ b/nimble/controller/src/ble_ll_cs_drbg.c @@ -0,0 +1,686 @@ +/* + * 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 +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +#include +#include +#include +#include "nimble/ble.h" +#include "mbedtls/aes.h" +#include "ble_ll_cs_drbg_priv.h" + +static const uint8_t rtt_seq_len[] = {0, 4, 12, 4, 8, 12, 16}; + +/** + * Security function e generates 128-bit encrypted_data from a 128-bit key + * and 128-bit data using the AES-128-bit block cypher. + */ +int +ble_ll_cs_drbg_e(const uint8_t *key, const uint8_t *data, uint8_t *out_data) +{ + struct ble_encryption_block ecb; + int rc = 0; + + /* The cryptographic function uses the leftmost to rightmost + * representation (MSO to LSO). + */ + swap_buf(ecb.key, key, BLE_ENC_BLOCK_SIZE); + swap_buf(ecb.plain_text, data, BLE_ENC_BLOCK_SIZE); + +#if BABBLESIM || MYNEWT_VAL(SELFTEST) + /* Use software to encrypt the data */ + mbedtls_aes_context aes_ctx; + mbedtls_aes_init(&aes_ctx); + mbedtls_aes_setkey_enc(&aes_ctx, ecb.key, 16 * 8); + rc = mbedtls_aes_crypt_ecb(&aes_ctx, MBEDTLS_AES_ENCRYPT, ecb.plain_text, ecb.cipher_text); + mbedtls_aes_free(&aes_ctx); +#else + /* Use hardware to encrypt the data */ + rc = ble_hw_encrypt_block(&ecb); +#endif + + if (!rc) { + swap_buf(out_data, ecb.cipher_text, BLE_ENC_BLOCK_SIZE); + } else { + rc = -1; + } + + return rc; +} + +/** + * DRBG chain function f7. + * - k - 128-bit key + * - in - an input bit string whose length is a multiple of 128 bits and + * generates an output that is 128 bits long using a cipher block + * chaining technique. + * - out - processed bit string + */ +int +ble_ll_cs_drbg_f7(const uint8_t *k, const uint8_t *in, uint8_t len, uint8_t *out) +{ + int rc; + int i; + const uint64_t *block; + uint64_t *hout = (uint64_t *)out; + + memset(hout, 0, 16); + + /* Starting with the leftmost bits (MSO) of input_bit_string, split into + * 128-bit blocks + */ + for (i = len - 16; i >= 0; i -= 16) { + block = (uint64_t *)&in[i]; + /* XOR a 128-bit block in two steps */ + *hout ^= *block; + *(hout + 1) ^= *(block + 1); + + rc = ble_ll_cs_drbg_e(k, out, out); + if (rc) { + break; + } + } + + return rc; +} + +/** + * DRBG derivation function f8. + * - input - 320-bit input bit string + * - sm - output, generated 256-bit seed material (SM) + */ +int +ble_ll_cs_drbg_f8(const uint8_t *input, uint8_t *sm) +{ + int rc; + uint8_t k[16] = {0x0F, 0x0E, 0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x08, + 0x07, 0x06, 0x05, 0x04, 0x03, 0x02, 0x01, 0x00}; + uint8_t k2[16] = {0}; + uint8_t x[16] = {0}; + /* buf contains V || S */ + uint8_t buf[80] = {0}; + uint8_t *s = buf; + uint8_t *v = buf + 64; + + /* S = 0x0000002800000020 || input_bit_string || 0x80 || + * 0x000000000000000000000000000000 + */ + s[15] = 0x80; + memcpy(s + 16, input, 40); + put_le64(s + 56, 0x0000002800000020); + + /* K2 = f7( K, V || S ) */ + rc = ble_ll_cs_drbg_f7(k, buf, 80, k2); + if (rc) { + return rc; + } + + /* V = 0x00000001000000000000000000000000 */ + v[12] = 0x01; + + /* X = f7( K, V || S ) */ + rc = ble_ll_cs_drbg_f7(k, buf, 80, x); + if (rc) { + return rc; + } + + /* Calculate the most significant part of SM: + * SM = e( K2, X ) + */ + rc = ble_ll_cs_drbg_e(k2, x, sm + 16); + if (rc) { + return rc; + } + + /* Calculate the least significant part of SM and concatenate + * both parts: + * SM = SM || e( K2, SM ) + */ + rc = ble_ll_cs_drbg_e(k2, sm + 16, sm); + + return rc; +} + +/** + * DRBG update function f9 is used to update and refresh a DRBG 128-bit + * temporal key K and a 128-bit nonce vector V using a 256-bit seed material (SM) + * that may carry fresh entropy. The SM value may also be 0 if f9 is called for + * backtracking purposes. + * - sm - 256-bit seed material + * - k_in - 128-bit key + * - v_in - 128-bit nonce vector + */ +int +ble_ll_cs_drbg_f9(const uint8_t *sm, uint8_t *k, uint8_t *v) +{ + int rc; + uint8_t x[32] = {0}; + uint64_t *x_p = (uint64_t *)x; + uint64_t *sm_p = (uint64_t *)sm; + + /* V = V[127:8] || (( V[7:0] + 1 ) mod 2^8) */ + v[0]++; + rc = ble_ll_cs_drbg_e(k, v, x + 16); + if (rc) { + return rc; + } + + v[0]++; + /* Again V = V[127:8] || (( V[7:0] + 1 ) mod 2^8) */ + rc = ble_ll_cs_drbg_e(k, v, x); + if (rc) { + return rc; + } + + /* X = X ⊕ SM */ + x_p[0] ^= sm_p[0]; + x_p[1] ^= sm_p[1]; + x_p[2] ^= sm_p[2]; + x_p[3] ^= sm_p[3]; + + memcpy(v, x, 16); + memcpy(k, x + 16, 16); + + return 0; +} + +/** + * DRBG instantiation function h9. + * - iv - 128-bit initialization vector (CS_IV) + * - in - 64-bit instantiation nonce (CS_IN) + * - pv - 128-bit personalization vector (CS_PV) + * - key - output, 128-bit temporal key (K_DRBG) + * - nonce_v - output, 128-bit nonce vector (V_DRBG) + */ +int +ble_ll_cs_drbg_h9(const uint8_t *iv, const uint8_t *in, const uint8_t *pv, + uint8_t *key, uint8_t *nonce_v) +{ + int rc; + uint8_t input_bit_string[40] = {0}; + uint8_t sm[32] = {0}; + + /* 320-bit input bit string created from concatenated vectors + * CS_IV || CS_IN || CS_PV + */ + memcpy(input_bit_string, pv, 16); + memcpy(input_bit_string + 16, in, 8); + memcpy(input_bit_string + 24, iv, 16); + + /* Generate seed material (SM) */ + rc = ble_ll_cs_drbg_f8(input_bit_string, sm); + if (rc) { + return rc; + } + + /* Generate K_DRBG and V_DRBG */ + memset(key, 0, 16); + memset(nonce_v, 0, 16); + rc = ble_ll_cs_drbg_f9(sm, key, nonce_v); + + return rc; +} + +/** + * Random bit generation function CS_DRBG + * - transaction_id - CSTransactionID, + * - drbg_ctx - the drbg context, already inited with keys, + * - output - output buffer, + * - len - number of bytes to be generated. + */ +int +ble_ll_cs_drbg_rand(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint16_t step_count, uint8_t transaction_id, + uint8_t *output, uint8_t len) +{ + int rc; + uint8_t rand_len = 0; + uint8_t nonce[16]; + struct ble_ll_cs_transaction_cache *cache = &drbg_ctx->t_cache[transaction_id]; + + /* Set the fixed values of the DRGB nonce */ + memcpy(nonce, drbg_ctx->nonce_v, sizeof(nonce)); + /* Set the Transaction_Counter */ + if (cache->last_step_count != step_count) { + cache->transaction_counter = 0; + cache->last_step_count = step_count; + } + nonce[0] += cache->transaction_counter; + /* Set the Transaction_Identifier */ + nonce[1] += transaction_id; + /* Set the CS Step_Counter */ + put_le16(&nonce[2], step_count + get_le16(&nonce[2])); + + while (len > 0) { + if (cache->free_bytes > 0) { + /* Use bytes from previous DRBG invocation */ + + if (len < cache->free_bytes) { + rand_len = len; + } else { + rand_len = cache->free_bytes; + } + + cache->free_bytes -= rand_len; + /* [0] is LSO, [15] is MSO. Return cached bytes starting from MSO. */ + memcpy(output, cache->random_bytes + cache->free_bytes, rand_len); + + len -= rand_len; + output += rand_len; + } else { + /* Invoke CS_DRBG to get fresh 128-bit sequence */ + rc = ble_ll_cs_drbg_e(drbg_ctx->key, nonce, cache->random_bytes); + if (rc) { + return rc; + } + + cache->free_bytes = sizeof(cache->random_bytes); + /* Increment the transaction counter */ + ++nonce[0]; + ++cache->transaction_counter; + } + } + + return 0; +} + +/** Channel Sounding random number generation function hr1 */ +int +ble_ll_cs_drbg_rand_hr1(struct ble_ll_cs_drbg_ctx *drbg_ctx, uint16_t step_count, + uint8_t transaction_id, uint8_t r, uint8_t *r_out) +{ + int rc; + uint16_t t_rand; + uint8_t random_bits; + + if (r <= 1) { + *r_out = 0; + + return 0; + } + + rc = ble_ll_cs_drbg_rand(drbg_ctx, step_count, + transaction_id, &random_bits, 1); + if (rc) { + return rc; + } + + t_rand = r * random_bits; + + if ((t_rand & 0xFF) < (256 % r)) { + rc = ble_ll_cs_drbg_rand(drbg_ctx, step_count, + transaction_id, &random_bits, 1); + *r_out = ((256 * random_bits * r) + t_rand) / 65536; + } else { + *r_out = t_rand / 256; + } + + return rc; +} + +int +ble_ll_cs_drbg_shuffle_cr1(struct ble_ll_cs_drbg_ctx *drbg_ctx, uint16_t step_count, + uint8_t transaction_id, uint8_t *channel_array, + uint8_t *shuffled_array, uint8_t len) +{ + int rc; + uint8_t i, j; + + for (i = 0; i < len; ++i) { + rc = ble_ll_cs_drbg_rand_hr1(drbg_ctx, step_count, transaction_id, i + 1, &j); + if (rc) { + return rc; + } + + if (i != j) { + shuffled_array[i] = shuffled_array[j]; + } + + shuffled_array[j] = channel_array[i]; + } + + return 0; +} + +static int +cs_autocorrelation_score(uint32_t sequence) +{ + int i, k, score = 0; + uint32_t c, s; + + for (k = 1; k <= 3; ++k) { + c = 0; + s = sequence; + for (i = 1; i <= 32 - k; ++i) { + c += (s & 1) ^ ((s >> k) & 1); + s >>= 1; + } + score += abs(2 * c - (32 - k)); + } + + return score; +} + +int +ble_ll_cs_drbg_generate_aa(struct ble_ll_cs_drbg_ctx *drbg_ctx, uint16_t step_count, + uint32_t *initiator_aa, uint32_t *reflector_aa) +{ + int rc; + uint8_t buf[16]; + uint32_t s0, s1, s2, s3; + + rc = ble_ll_cs_drbg_rand(drbg_ctx, step_count, + BLE_LL_CS_DRBG_ACCESS_ADDRESS, + buf, sizeof(buf)); + + if (rc) { + return rc; + } + + /* The buf[15] is the first generated octet */ + s0 = get_le32(&buf[12]); + s1 = get_le32(&buf[8]); + s2 = get_le32(&buf[4]); + s3 = get_le32(&buf[0]); + + /* The sequence with the lower autocorrelation score is selected + * as the CS Access Address. See 2.2.1 Channel Sounding Access Address + * selection rules. + */ + if (cs_autocorrelation_score(s0) < cs_autocorrelation_score(s1)) { + *initiator_aa = s0; + } else { + *initiator_aa = s1; + } + + if (cs_autocorrelation_score(s2) < cs_autocorrelation_score(s3)) { + *reflector_aa = s2; + } else { + *reflector_aa = s3; + } + + return 0; +} + +int +ble_ll_cs_drbg_rand_marker_position(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint16_t step_count, uint8_t rtt_type, + uint8_t *position1, uint8_t *position2) +{ + int rc; + uint8_t rand_range; + + if (rtt_type == BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE) { + rand_range = 29; + } else { /* BLE_LL_CS_RTT_96_BIT_SOUNDING_SEQUENCE */ + rand_range = 64; + } + + rc = ble_ll_cs_drbg_rand_hr1(drbg_ctx, step_count, + BLE_LL_CS_DRBG_SEQ_MARKER_POSITION, + rand_range, position1); + if (rc) { + return -1; + } + + if (rtt_type == BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE) { + *position2 = 0xFF; + + return 0; + } + + rc = ble_ll_cs_drbg_rand_hr1(drbg_ctx, step_count, + BLE_LL_CS_DRBG_SEQ_MARKER_POSITION, + 75, position2); + if (rc) { + return -1; + } + + *position2 += 67; + if (*position2 > 92) { + /* Omit the second marker */ + *position2 = 0xFF; + } + + return 0; +} + +int +ble_ll_cs_drbg_rand_marker_selection(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint8_t step_count, uint8_t *marker_selection) +{ + int rc; + + if (drbg_ctx->marker_selection_free_bits == 0) { + rc = ble_ll_cs_drbg_rand(drbg_ctx, step_count, + BLE_LL_CS_DRBG_SEQ_MARKER_SIGNAL, + &drbg_ctx->marker_selection_cache, 1); + if (rc) { + return rc; + } + + drbg_ctx->marker_selection_free_bits = 8; + } + + *marker_selection = drbg_ctx->marker_selection_cache & 0x80; + drbg_ctx->marker_selection_cache <<= 1; + --drbg_ctx->marker_selection_free_bits; + + return 0; +} + +int +ble_ll_cs_drbg_rand_main_mode_steps(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint8_t step_count, + uint8_t main_mode_min_steps, + uint8_t main_mode_max_steps, + uint8_t *main_mode_steps) +{ + int rc; + uint8_t r; + uint8_t r_out; + + r = main_mode_max_steps - main_mode_min_steps + 1; + rc = ble_ll_cs_drbg_rand_hr1(drbg_ctx, step_count, + BLE_LL_CS_DRBG_SUBEVT_SUBMODE, + r, &r_out); + if (rc) { + return rc; + } + + *main_mode_steps = r_out + main_mode_min_steps; + + return 0; +} + +static int +ble_ll_cs_drbg_apply_marker_signal(struct ble_ll_cs_drbg_ctx *drbg_ctx, uint8_t step_count, + uint8_t *buf, uint8_t position) +{ + int rc; + uint16_t *byte_ptr; + uint16_t marker_signal; + uint8_t marker_selection; + uint8_t byte_id = 0; + uint8_t bit_offset = 0; + + rc = ble_ll_cs_drbg_rand_marker_selection(drbg_ctx, step_count, &marker_selection); + if (rc) { + return rc; + } + + if (marker_selection) { + /* '0011' in transmission order */ + marker_signal = 0b1100; + } else { + /* '1100' in transmission order */ + marker_signal = 0b0011; + } + + byte_id = position / 8; + byte_ptr = (uint16_t *) &buf[byte_id]; + bit_offset = position % 8; + *byte_ptr &= ~(0xF << bit_offset); + *byte_ptr |= ~(marker_signal << bit_offset); + + return 0; +} + +static int +ble_ll_cs_generate_sounding_sequence(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint8_t step_count, uint8_t rtt_type, + uint8_t *buf, uint8_t sequence_len) +{ + int rc; + uint8_t i; + uint8_t position1; + uint8_t position2; + + for (i = 0; i < sequence_len; ++i) { + buf[i] = 0b10101010; + } + + rc = ble_ll_cs_drbg_rand_marker_position(drbg_ctx, step_count, rtt_type, + &position1, &position2); + if (rc) { + return rc; + } + + rc = ble_ll_cs_drbg_apply_marker_signal(drbg_ctx, step_count, buf, position1); + if (rc) { + return rc; + } + + if (position2 != 0xFF) { + rc = ble_ll_cs_drbg_apply_marker_signal(drbg_ctx, step_count, buf, position2); + } + + return rc; +} + +int +ble_ll_cs_drbg_generate_sync_sequence(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint8_t step_count, + uint8_t rtt_type, + uint8_t *buf, + uint8_t *sequence_len) +{ + int rc = -1; + + *sequence_len = rtt_seq_len[rtt_type]; + + switch (rtt_type) { + case BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE: + rc = ble_ll_cs_generate_sounding_sequence(drbg_ctx, step_count, rtt_type, + buf, *sequence_len); + break; + case BLE_LL_CS_RTT_96_BIT_SOUNDING_SEQUENCE: + rc = ble_ll_cs_generate_sounding_sequence(drbg_ctx, step_count, rtt_type, + buf, *sequence_len); + break; + case BLE_LL_CS_RTT_32_BIT_RANDOM_SEQUENCE: + case BLE_LL_CS_RTT_64_BIT_RANDOM_SEQUENCE: + case BLE_LL_CS_RTT_96_BIT_RANDOM_SEQUENCE: + case BLE_LL_CS_RTT_128_BIT_RANDOM_SEQUENCE: + rc = ble_ll_cs_drbg_rand(drbg_ctx, step_count, + BLE_LL_CS_DRBG_RAND_SEQ_GENERATION, + buf, *sequence_len); + break; + default: + break; + } + + return rc; +} + +int +ble_ll_cs_drbg_rand_tone_ext_presence(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint8_t step_count, uint8_t *presence) +{ + int rc; + + if (drbg_ctx->tone_ext_presence_free_bits == 0) { + rc = ble_ll_cs_drbg_rand(drbg_ctx, step_count, + BLE_LL_CS_DRBG_T_PM_TONE_SLOT_PRESENCE, + &drbg_ctx->tone_ext_presence_cache, 1); + if (rc) { + return rc; + } + + drbg_ctx->tone_ext_presence_free_bits = 8; + } + + *presence = drbg_ctx->tone_ext_presence_cache & 0x80 ? 1 : 0; + drbg_ctx->tone_ext_presence_cache <<= 1; + --drbg_ctx->tone_ext_presence_free_bits; + + return 0; +} + +int +ble_ll_cs_drbg_rand_antenna_path_perm_id(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint16_t step_count, uint8_t n_ap, + uint8_t *ap_id) +{ + int rc; + uint8_t i; + uint8_t n_ap_f = 0; + + if (n_ap <= 1) { + *ap_id = 0; + + return 0; + } + + assert(n_ap <= 4); + + for (i = 1; i <= n_ap; ++i) { + n_ap_f *= i; + } + + rc = ble_ll_cs_drbg_rand_hr1(drbg_ctx, step_count, + BLE_LL_CS_DRBG_ANTENNA_PATH_PERMUTATION, + n_ap_f, ap_id); + + return rc; +} + +void +ble_ll_cs_drbg_clear_cache(struct ble_ll_cs_drbg_ctx *drbg_ctx) +{ + memset(drbg_ctx->t_cache, 0, sizeof(drbg_ctx->t_cache)); + drbg_ctx->marker_selection_cache = 0; + drbg_ctx->marker_selection_free_bits = 0; +} + +int +ble_ll_cs_drbg_init(struct ble_ll_cs_drbg_ctx *drbg_ctx) +{ + /* Calculate temporal key K and nonce vector V */ + return ble_ll_cs_drbg_h9(drbg_ctx->iv, drbg_ctx->in, drbg_ctx->pv, + drbg_ctx->key, drbg_ctx->nonce_v); +} + +void +ble_ll_cs_drbg_free(struct ble_ll_cs_drbg_ctx *drbg_ctx) +{ + memset(drbg_ctx, 0, sizeof(*drbg_ctx)); +} +#endif /* BLE_LL_CHANNEL_SOUNDING */ diff --git a/nimble/controller/src/ble_ll_cs_drbg_priv.h b/nimble/controller/src/ble_ll_cs_drbg_priv.h new file mode 100644 index 0000000000..6e49e06789 --- /dev/null +++ b/nimble/controller/src/ble_ll_cs_drbg_priv.h @@ -0,0 +1,129 @@ +/* + * 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 H_BLE_LL_CS_DRBG_PRIV +#define H_BLE_LL_CS_DRBG_PRIV + +#ifdef __cplusplus +extern "C" { +#endif + +#define BLE_LL_CS_DRBG_HOP_CHAN_NON_MODE0 (0x00) +#define BLE_LL_CS_DRBG_HOP_CHAN_MODE0 (0x01) +#define BLE_LL_CS_DRBG_SUBEVT_SUBMODE (0x02) +#define BLE_LL_CS_DRBG_T_PM_TONE_SLOT_PRESENCE (0x03) +#define BLE_LL_CS_DRBG_ANTENNA_PATH_PERMUTATION (0x04) +#define BLE_LL_CS_DRBG_ACCESS_ADDRESS (0x05) +#define BLE_LL_CS_DRBG_SEQ_MARKER_POSITION (0x06) +#define BLE_LL_CS_DRBG_SEQ_MARKER_SIGNAL (0x07) +#define BLE_LL_CS_DRBG_RAND_SEQ_GENERATION (0x08) +#define BLE_LL_CS_DRBG_BACKTRACKING_RESISTANCE (0x09) +#define BLE_LL_CS_DRBG_TRANSACTION_IDS_NUMBER (0x0a) + +#define BLE_LL_CS_RTT_AA_ONLY (0x00) +#define BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE (0x01) +#define BLE_LL_CS_RTT_96_BIT_SOUNDING_SEQUENCE (0x02) +#define BLE_LL_CS_RTT_32_BIT_RANDOM_SEQUENCE (0x03) +#define BLE_LL_CS_RTT_64_BIT_RANDOM_SEQUENCE (0x04) +#define BLE_LL_CS_RTT_96_BIT_RANDOM_SEQUENCE (0x05) +#define BLE_LL_CS_RTT_128_BIT_RANDOM_SEQUENCE (0x06) + +struct ble_ll_cs_transaction_cache { + uint16_t last_step_count; + /* 1-octet CSTransactionCounter per each CS Transaction ID. Should + * be reset each time the nonce V CS Step_Counter field is set to + * a new value. + */ + uint8_t transaction_counter; + /* Random bits cache */ + uint8_t random_bytes[16]; + /* The number of cached bytes that have been already used */ + uint8_t free_bytes; +}; + +/* DRBG context */ +struct ble_ll_cs_drbg_ctx { + /* Initialization vector, entropy input */ + uint8_t iv[16]; + /* Instantiation nonce */ + uint8_t in[8]; + /* Personalization vector/string */ + uint8_t pv[16]; + /* Temporal key K */ + uint8_t key[16]; + /* DRBG nonce/counter (V), the starting value from which the DRBG operates. + * Initialized once per LE Connection. + */ + uint8_t nonce_v[16]; + /* Cache bits generated with single DRBG transation */ + struct ble_ll_cs_transaction_cache t_cache[BLE_LL_CS_DRBG_TRANSACTION_IDS_NUMBER]; + + uint8_t marker_selection_free_bits; + uint8_t marker_selection_cache; + + uint8_t tone_ext_presence_free_bits; + uint8_t tone_ext_presence_cache; +}; + +int ble_ll_cs_drbg_e(const uint8_t *key, const uint8_t *data, uint8_t *out_data); +int ble_ll_cs_drbg_f7(const uint8_t *k, const uint8_t *in, uint8_t len, uint8_t *out); +int ble_ll_cs_drbg_f8(const uint8_t *input, uint8_t *sm); +int ble_ll_cs_drbg_f9(const uint8_t *sm, uint8_t *k, uint8_t *v); +int ble_ll_cs_drbg_h9(const uint8_t *iv, const uint8_t *in, const uint8_t *pv, + uint8_t *key, uint8_t *nonce_v); +int ble_ll_cs_drbg_rand(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint16_t step_count, uint8_t transaction_id, + uint8_t *output, uint8_t len); +int ble_ll_cs_drbg_rand_hr1(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint16_t step_count, uint8_t transaction_id, + uint8_t r, uint8_t *r_out); +int ble_ll_cs_drbg_shuffle_cr1(struct ble_ll_cs_drbg_ctx *drbg_ctx, uint16_t step_count, + uint8_t transaction_id, uint8_t *channel_array, + uint8_t *shuffled_array, uint8_t len); +int ble_ll_cs_drbg_generate_aa(struct ble_ll_cs_drbg_ctx *drbg_ctx, uint16_t step_count, + uint32_t *initiator_aa, uint32_t *reflector_aa); +int ble_ll_cs_drbg_rand_marker_position(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint16_t step_count, uint8_t rtt_type, + uint8_t *position1, uint8_t *position2); +int ble_ll_cs_drbg_rand_marker_selection(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint8_t step_count, uint8_t *marker_selection); +int ble_ll_cs_drbg_rand_main_mode_steps(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint8_t step_count, + uint8_t main_mode_min_steps, + uint8_t main_mode_max_steps, + uint8_t *main_mode_steps); +int ble_ll_cs_drbg_generate_sync_sequence(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint8_t step_count, + uint8_t rtt_type, + uint8_t *buf, + uint8_t *sequence_len); +int ble_ll_cs_drbg_rand_antenna_path_perm_id(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint16_t step_count, uint8_t n_ap, + uint8_t *ap_id); +int ble_ll_cs_drbg_rand_tone_ext_presence(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint8_t step_count, uint8_t *presence); +int ble_ll_cs_drbg_init(struct ble_ll_cs_drbg_ctx *drbg_ctx); +void ble_ll_cs_drbg_free(struct ble_ll_cs_drbg_ctx *drbg_ctx); +void ble_ll_cs_drbg_clear_cache(struct ble_ll_cs_drbg_ctx *drbg_ctx); + +#ifdef __cplusplus +} +#endif + +#endif /* H_BLE_LL_CS_DRBG_PRIV */ diff --git a/nimble/controller/src/ble_ll_cs_priv.h b/nimble/controller/src/ble_ll_cs_priv.h new file mode 100644 index 0000000000..a88c5bb89d --- /dev/null +++ b/nimble/controller/src/ble_ll_cs_priv.h @@ -0,0 +1,346 @@ +/* + * 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 H_BLE_LL_CS_PRIV +#define H_BLE_LL_CS_PRIV + +#include +#include "controller/ble_ll_conn.h" +#include "ble_ll_cs_drbg_priv.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define BLE_LL_CS_MODE0 (0) +#define BLE_LL_CS_MODE1 (1) +#define BLE_LL_CS_MODE2 (2) +#define BLE_LL_CS_MODE3 (3) + +#define BLE_LL_CS_ROLE_INITIATOR (0) +#define BLE_LL_CS_ROLE_REFLECTOR (1) + +#define BLE_LL_CS_CONFIG_MAX_NUM 4 +/* CS Event interval in units of the number of connection intervals */ +#define BLE_LL_CS_EVENT_INTERVAL_MIN (0x0001) +#define BLE_LL_CS_EVENT_INTERVAL_MAX (0xFFFF) +#define BLE_LL_CS_SUBEVENTS_PER_PROCEDURE_MIN (1) +#define BLE_LL_CS_SUBEVENTS_PER_PROCEDURE_MAX (32) +/* CS Subevent interval in units of 625usec */ +#define BLE_LL_CS_SUBEVENTS_INTERVAL_MIN (1) +#define BLE_LL_CS_SUBEVENTS_INTERVAL_MAX (32) +/* CS Subevent interval in microseconds */ +#define BLE_LL_CS_SUBEVENT_LEN_MIN (1250) +#define BLE_LL_CS_SUBEVENT_LEN_MAX (4000000) + +/* CS Event interval in units of the number of connection intervals */ +#define BLE_LL_CS_SUBEVENTS_INTERVAL_UNIT_US (625) +#define BLE_LL_CS_PROCEDURE_LEN_UNIT_US (625) + +#define BLE_LL_CS_STEPS_PER_SUBEVENT_MIN (2) +#define BLE_LL_CS_STEPS_PER_SUBEVENT_MAX (160) +#define BLE_LL_CS_STEPS_PER_PROCEDURE_MAX (256) + +#define BLE_LL_CS_SYNC_PHY_1M (0x01) +#define BLE_LL_CS_SYNC_PHY_2M (0x02) +/* The duration of the CS_SYNC (T_SY) without sequence in usec */ +#define BLE_LL_CS_SYNC_TIME_1M (44) +#define BLE_LL_CS_SYNC_TIME_2M (26) + +typedef int (*ble_ll_cs_sched_cb_func)(struct ble_ll_cs_sm *cssm); + +struct ble_ll_cs_aci { + uint8_t n_ap; + uint8_t n_a_antennas; + uint8_t n_b_antennas; +}; + +struct ble_ll_cs_supp_cap { + uint8_t mode_types; + uint8_t roles_supported; + uint8_t rtt_capability; + uint8_t rtt_aa_only_n; + uint8_t rtt_sounding_n; + uint8_t rtt_random_sequence_n; + uint16_t nadm_sounding_capability; + uint16_t nadm_random_sequence_capability; + uint8_t cs_sync_phy_capability; + uint8_t number_of_antennas; + uint8_t max_number_of_antenna_paths; + uint8_t no_fae; + uint8_t channel_selection; + uint8_t sounding_pct_estimate; + uint8_t max_number_of_configs; + uint16_t max_number_of_procedures; + uint16_t t_ip1_capability; + uint16_t t_ip2_capability; + uint16_t t_fcs_capability; + uint16_t t_pm_capability; + uint8_t t_sw; + uint8_t tx_snr_capablity; +}; + +struct ble_ll_cs_pref_proc_params { + uint16_t max_procedure_len; + uint16_t min_procedure_interval; + uint16_t max_procedure_interval; + uint16_t max_procedure_count; + uint32_t min_subevent_len; + uint32_t max_subevent_len; + uint8_t aci; + uint8_t phy; + uint8_t tx_power_delta; + uint8_t preferred_peer_antenna; + uint8_t snr_control_initiator; + uint8_t snr_control_reflector; + uint8_t params_ready; +}; + +/* Negotiated parameters for CS procedures */ +struct ble_ll_cs_proc_params { + /* The number of consecutive CS procedures to invoke */ + uint16_t max_procedure_count; + /* The event counter that will be the anchor point to + * the start of the first CS procedure. + */ + uint16_t anchor_conn_event; + /* The offset (usec) from the event counter to the CS subevent anchor point */ + uint32_t event_offset; + uint32_t offset_min; + uint32_t offset_max; + /* Max CS procedure length in 0.625us units */ + uint16_t max_procedure_len; + /* Max CS subevent length in microseconds */ + uint32_t subevent_len; + uint16_t procedure_interval; + uint16_t event_interval; + uint16_t subevent_interval; + uint8_t subevents_per_event; + /* Selected Anntena Configuration ID */ + uint8_t aci; + /* Bit map of preferred peer-ordered antenna elements */ + uint8_t preferred_peer_antenna; + /* Bit map of the remote device’s Tx PHY */ + uint8_t phy; + /* Transmit power delta, in signed dB, to indicate the recommended difference + * between the remote device’s power level for the CS tones and RTT packets + * and the existing power level for the PHY indicated by the PHY parameter. + */ + uint8_t tx_power_delta; + /* SNR Output Index (SOI) for SNR control adjustment */ + uint8_t tx_snr_i; + uint8_t tx_snr_r; +}; + +struct ble_ll_cs_config { + uint8_t config_in_use; + uint8_t config_enabled; + /* The role to use in CS procedure + * 0x00 = Initiator, + * 0x01 = Reflector + */ + uint8_t role; + /* Map of allowed channels for this CS config */ + uint8_t chan_map[10]; + /* The number of times the map represented by the Channel_Map field is to + * be cycled through for non-mode 0 steps within a CS procedure + */ + uint8_t chan_map_repetition; + uint8_t main_mode; + uint8_t sub_mode; + uint8_t main_mode_min_steps; + uint8_t main_mode_max_steps; + uint8_t main_mode_repetition; + uint8_t mode_0_steps; + /* PHY used for mode 0, 1 and 3 (use LE 1M PHY)*/ + uint8_t cs_sync_phy; + /* Type of RTT (Round-Trip Time) packets */ + uint8_t rtt_type; + /* The Channel Selection Algorithm to use */ + uint8_t chan_sel; + /* Parameters for #3c algorithm */ + uint8_t ch3cshape; + uint8_t ch3cjump; + /* Timings (indexes) selected from capabilities */ + uint8_t t_ip1_index; + uint8_t t_ip2_index; + uint8_t t_fcs_index; + uint8_t t_pm_index; + /* Timings (usec) selected from capabilities */ + uint8_t t_ip1; + uint8_t t_ip2; + uint8_t t_fcs; + uint8_t t_pm; + /* CS procedure parameters preferred by our Host */ + struct ble_ll_cs_pref_proc_params pref_proc_params; + /* Final negationed CS procedure params */ + struct ble_ll_cs_proc_params proc_params; +}; + +struct ble_ll_cs_step_result { + uint8_t sounding_pct_estimate; + uint8_t packet_rssi; + uint8_t packet_quality; + uint8_t packet_nadm; + uint32_t time_of_departure; + uint32_t time_of_arrival; + uint32_t packet_pct1; + uint32_t packet_pct2; + uint16_t measured_freq_offset; + uint32_t tone_pct[5]; + uint8_t tone_quality_ind[5]; +}; + +struct ble_ll_cs_subevent { + struct ble_hci_ev *hci_ev; + unsigned int subev; + uint8_t num_steps_reported; +}; + +struct ble_ll_cs_sm { + struct ble_ll_conn_sm *connsm; + struct ble_ll_cs_supp_cap remote_cap; + struct ble_ll_cs_config config[BLE_LL_CS_CONFIG_MAX_NUM]; + + /* Default Settings */ + uint8_t roles_enabled; + uint8_t cs_sync_antenna_selection; + uint8_t max_tx_power; + + /* Cached FAE tables */ + uint8_t remote_fae_table[72]; + uint8_t local_fae_table[72]; + + struct ble_ll_cs_config *active_config; + uint8_t active_config_id; + + /* Arguments for ble_ll_cs_config_req_make */ + uint8_t config_req_id; + uint8_t config_req_action; + struct ble_ll_cs_config tmp_config; + + /* Arguments for ble_ll_cs_hci_proc_enable */ + uint8_t terminate_config_id; + uint8_t terminate_error_code; + + /* DRBG context, initialized onece per LE Connection */ + struct ble_ll_cs_drbg_ctx drbg_ctx; + + /* Helper flags */ + uint8_t measurement_enabled; + uint8_t terminate_measurement; + + /* Scheduling data for current CS procedure */ + struct ble_ll_sched_item sch; + sched_cb_func cb; + uint32_t duration_usecs; + uint32_t anchor_usecs; + ble_ll_cs_sched_cb_func sched_cb; + uint32_t anchor_cputime; + uint8_t anchor_rem_usecs; + uint8_t rx_window_offset_usecs; + + uint8_t step_mode; + uint8_t step_state; + uint8_t subevent_state; + uint8_t tone_ext_presence_i; + uint8_t tone_ext_presence_r; + uint8_t n_ap; + uint8_t antenna_path_count; + + /* Counters of complete CS procedures/events/subevents/steps */ + uint16_t procedure_count; + uint16_t events_in_procedure_count; + uint16_t subevents_in_procedure_count; + uint16_t subevents_in_event_count; + uint16_t steps_in_procedure_count; + uint8_t steps_in_subevent_count; + + /* Down-counters of remaining steps */ + uint8_t mode0_step_count; + uint8_t repetition_count; + uint8_t main_step_count; + + /* Anchor time of current CS procedure */ + uint32_t procedure_anchor_usecs; + /* Anchor time of current CS event */ + uint32_t event_anchor_usecs; + /* Anchor time of current CS subevent */ + uint32_t subevent_anchor_usecs; + /* Anchor time of next CS step */ + uint32_t step_anchor_usecs; + uint32_t event_interval_usecs; + uint32_t subevent_interval_usecs; + uint32_t procedure_interval_usecs; + /* Estimated time of the step (ToF not included) */ + uint32_t step_duration_usecs; + uint32_t mode0_step_duration_usecs; + uint32_t mode1_step_duration_usecs; + uint32_t mode2_step_duration_usecs; + uint32_t mode3_step_duration_usecs; + /* Time of antenna swith */ + uint8_t t_sw; + /* Time of CS_SYNC packet without sequence */ + uint8_t t_sy; + /* Time of CS_SYNC sequence only */ + uint8_t t_sy_seq; + + /* CS Access Addresses to use in current step */ + uint32_t initiator_aa; + uint32_t reflector_aa; + uint32_t tx_aa; + uint32_t rx_aa; + /* PHY channel */ + uint8_t channel; + /* Cached main mode channels that will be used in repetiton steps */ + uint8_t repetition_channels[3]; + uint8_t cs_sync_antenna; + + /* Cache for HCI Subevent Result event */ + struct ble_ll_cs_subevent buffered_subevent; + struct ble_ll_cs_step_result step_result; + uint8_t cs_schedule_status; + uint8_t proc_abort_reason; + uint8_t subev_abort_reason; + + /* Channel selection stuff */ + uint8_t mode0_channels[72]; + uint8_t non_mode0_channels[72]; + uint8_t mode0_next_chan_id; + uint8_t non_mode0_next_chan_id; + + /* Procedure counter for backtracking resistance */ + uint16_t start_procedure_count; +}; + +int ble_ll_cs_proc_scheduling_start(struct ble_ll_conn_sm *connsm, uint8_t config_id); +int ble_ll_cs_proc_schedule_next_tx_or_rx(struct ble_ll_cs_sm *cssm); +void ble_ll_cs_proc_sync_lost(struct ble_ll_cs_sm *cssm); +void ble_ll_cs_proc_set_now_as_anchor_point(struct ble_ll_cs_sm *cssm); + +int ble_ll_cs_sync_tx_start(struct ble_ll_cs_sm *cssm); +int ble_ll_cs_sync_rx_start(struct ble_ll_cs_sm *cssm); +int ble_ll_cs_tone_tx_start(struct ble_ll_cs_sm *cssm); +int ble_ll_cs_tone_rx_start(struct ble_ll_cs_sm *cssm); + +#ifdef __cplusplus +} +#endif + +#endif /* H_BLE_LL_CS_PRIV */ diff --git a/nimble/controller/src/ble_ll_cs_proc.c b/nimble/controller/src/ble_ll_cs_proc.c new file mode 100644 index 0000000000..f284470d08 --- /dev/null +++ b/nimble/controller/src/ble_ll_cs_proc.c @@ -0,0 +1,1386 @@ +/* + * 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 +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +#include +#include "controller/ble_ll_utils.h" +#include "controller/ble_ll_conn.h" +#include "controller/ble_ll_sched.h" +#include "controller/ble_ll_tmr.h" +#include "controller/ble_ll_hci.h" +#include "ble_ll_cs_priv.h" + +extern struct ble_ll_cs_supp_cap g_ble_ll_cs_local_cap; +extern uint8_t g_ble_ll_cs_chan_count; +extern uint8_t g_ble_ll_cs_chan_indices[72]; +struct ble_ll_cs_sm *g_ble_ll_cs_sm_current; + +/* States within step */ +#define STEP_STATE_INIT (0) +#define STEP_STATE_CS_SYNC_I (1) +#define STEP_STATE_CS_SYNC_R (2) +#define STEP_STATE_CS_TONE_I (3) +#define STEP_STATE_CS_TONE_R (4) +#define STEP_STATE_CS_TONE_EXT_I (5) +#define STEP_STATE_CS_TONE_EXT_R (6) +#define STEP_STATE_COMPLETE (7) + +#define SUBEVENT_STATE_MODE0_STEP (0) +#define SUBEVENT_STATE_REPETITION_STEP (1) +#define SUBEVENT_STATE_SUBMODE_STEP (2) +#define SUBEVENT_STATE_MAINMODE_STEP (3) + +#define CS_SCHEDULE_NEW_STEP (0) +#define CS_SCHEDULE_NEW_SUBEVENT (1) +#define CS_SCHEDULE_NEW_EVENT (2) +#define CS_SCHEDULE_NEW_PROCEDURE (3) +#define CS_SCHEDULE_COMPLETED (4) + +#define BLE_LL_CONN_ITVL_USECS (1250) + +/* The ramp-down window in µs */ +#define T_RD (5) +/* The guard time duration in µs */ +#define T_GD (10) +/* The requency measurement period in µs */ +#define T_FM (80) + +#define TIME_DIFF_NOT_AVAILABLE (0x00008000) +/* TODO: Hardcorded for 16MHz timer, should be configurable. */ +/* units of 0.5 nanoseconds */ +#define GET_TIME_DIFF(_diff_ticks, _center_delta_us) (_diff_ticks * 125 - _center_delta_us * 2000); + +#define SUBEVENT_DONE_STATUS_COMPLETED (0x0) +#define SUBEVENT_DONE_STATUS_PARTIAL (0x1) +#define SUBEVENT_DONE_STATUS_ABORTED (0xF) + +#define PROC_DONE_STATUS_COMPLETED (0x0) +#define PROC_DONE_STATUS_PARTIAL (0x1) +#define PROC_DONE_STATUS_ABORTED (0xF) + +#define PROC_ABORT_SUCCESS (0x00) +#define PROC_ABORT_REQUESTED (0x01) +#define PROC_ABORT_CHANNEL_MAP_CHANNELS (0x02) +#define PROC_ABORT_CHANNEL_MAP_UPDATE (0x03) +#define PROC_ABORT_UNSPECIFIED (0x0F) + +#define SUBEVENT_ABORT_SUCCESS (0x00) +#define SUBEVENT_ABORT_REQUESTED (0x01) +#define SUBEVENT_ABORT_NO_CS_SYNC (0x02) +#define SUBEVENT_ABORT_LIMITED_RESOURCES (0x03) +#define SUBEVENT_ABORT_UNSPECIFIED (0x0F) + +static struct ble_ll_cs_aci aci_table[] = { + {1, 1, 1}, {2, 2, 1}, {3, 3, 1}, {4, 4, 1}, + {2, 1, 2}, {3, 1, 3}, {4, 1, 4}, {4, 2, 2} +}; +static const uint8_t rtt_seq_len[] = {0, 4, 12, 4, 8, 12, 16}; + +/* For queueing the HCI Subevent Result (Continue) events */ +static struct ble_npl_event subevent_pool[MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING_SUBEVENT_EV_MAX_CNT)]; + +static int +ble_ll_cs_generate_channel(struct ble_ll_cs_sm *cssm) +{ + int rc; + uint8_t transaction_id; + uint8_t *channel_array; + uint8_t *next_channel_id; + + if (cssm->step_mode == BLE_LL_CS_MODE0) { + transaction_id = BLE_LL_CS_DRBG_HOP_CHAN_MODE0; + channel_array = cssm->mode0_channels; + next_channel_id = &cssm->mode0_next_chan_id; + } else { + transaction_id = BLE_LL_CS_DRBG_HOP_CHAN_NON_MODE0; + channel_array = cssm->non_mode0_channels; + next_channel_id = &cssm->non_mode0_next_chan_id; + } + + if (g_ble_ll_cs_chan_count <= *next_channel_id) { + rc = ble_ll_cs_drbg_shuffle_cr1(&cssm->drbg_ctx, cssm->steps_in_procedure_count, + transaction_id, g_ble_ll_cs_chan_indices, + channel_array, g_ble_ll_cs_chan_count); + if (rc) { + return rc; + } + + *next_channel_id = 0; + } + + cssm->channel = channel_array[(*next_channel_id)++]; + +#if BABBLESIM + cssm->channel %= 40; +#endif + + return 0; +} + +static int +ble_ll_cs_backtracking_resistance(struct ble_ll_cs_sm *cssm) +{ + cssm->drbg_ctx.nonce_v[1] += BLE_LL_CS_DRBG_BACKTRACKING_RESISTANCE; + + return ble_ll_cs_drbg_f9(0, cssm->drbg_ctx.key, cssm->drbg_ctx.nonce_v); +} + +/** + * Called when scheduled event needs to be halted. This normally should not be called + * and is only called when a scheduled item executes but scanning for sync/chain + * is stil ongoing + * Context: Interrupt + */ +void +ble_ll_cs_proc_halt(void) +{ +} + +/** + * Called when a scheduled event has been removed from the scheduler + * without being run. + */ +void +ble_ll_cs_proc_rm_from_sched(void *cb_args) +{ +} + +static int +ble_ll_cs_init_subevent(struct ble_ll_cs_subevent *subevent, + struct ble_ll_cs_sm *cssm) +{ + struct ble_hci_ev_le_subev_cs_subevent_result *ev; + struct ble_hci_ev *hci_ev; + const struct ble_ll_cs_proc_params *params = &cssm->active_config->proc_params; + + if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_CS_SUBEVENT_RESULT)) { + return 0; + } + + hci_ev = ble_transport_alloc_evt(0); + if (!hci_ev) { + return BLE_ERR_MEM_CAPACITY; + } + + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *)hci_ev->data; + + ev->subev_code = BLE_HCI_LE_SUBEV_CS_SUBEVENT_RESULT; + ev->conn_handle = htole16(cssm->connsm->conn_handle); + ev->config_id = cssm->active_config_id; + ev->start_acl_conn_event_counter = params->anchor_conn_event + + cssm->events_in_procedure_count * + params->event_interval; + ev->procedure_counter = cssm->procedure_count; + ev->frequency_compensation = 0xC000; + ev->reference_power_level = 0x7F; + ev->num_antenna_paths = cssm->n_ap; + ev->num_steps_reported = 0; + + memset(subevent, 0, sizeof(*subevent)); + subevent->hci_ev = hci_ev; + subevent->subev = BLE_HCI_LE_SUBEV_CS_SUBEVENT_RESULT; + + return 0; +} + +static int +ble_ll_cs_init_subevent_continue(struct ble_ll_cs_subevent *subevent, + struct ble_ll_cs_sm *cssm) +{ + struct ble_hci_ev_le_subev_cs_subevent_result_continue *ev; + struct ble_hci_ev *hci_ev; + + if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_CS_SUBEVENT_RESULT_CONTINUE)) { + return 0; + } + + hci_ev = ble_transport_alloc_evt(0); + if (!hci_ev) { + return BLE_ERR_MEM_CAPACITY; + } + + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *)hci_ev->data; + + ev->subev_code = BLE_HCI_LE_SUBEV_CS_SUBEVENT_RESULT_CONTINUE; + ev->conn_handle = htole16(cssm->connsm->conn_handle); + ev->config_id = cssm->active_config_id; + ev->abort_reason = 0x00; + ev->num_antenna_paths = cssm->n_ap; + ev->num_steps_reported = 0; + + memset(subevent, 0, sizeof(*subevent)); + subevent->hci_ev = hci_ev; + subevent->subev = BLE_HCI_LE_SUBEV_CS_SUBEVENT_RESULT_CONTINUE; + + return 0; +} + +/** + * Send HCI_LE_CS_Subevent_Result (or _Continue) + * + * Context: Link Layer task. + * + */ +static void +ble_ll_cs_proc_send_subevent(struct ble_npl_event *ev) +{ + struct ble_ll_cs_subevent *subevent; + struct ble_hci_ev *hci_ev; + + hci_ev = ble_npl_event_get_arg(ev); + BLE_LL_ASSERT(hci_ev); + + ble_ll_hci_event_send(hci_ev); + + ble_npl_event_set_arg(ev, NULL); +} + +static int +ble_ll_cs_proc_queue_subevent(struct ble_ll_cs_subevent *subevent, + uint8_t cs_schedule_status, + uint8_t proc_abort_reason, + uint8_t subev_abort_reason) +{ + int i; + struct ble_hci_ev *hci_ev = subevent->hci_ev; + struct ble_npl_event *ev; + uint8_t abort_reason = 0x00; + uint8_t procedure_done; + uint8_t subevent_done; + + switch (cs_schedule_status) { + case CS_SCHEDULE_NEW_STEP: + procedure_done = PROC_DONE_STATUS_PARTIAL; + subevent_done = SUBEVENT_DONE_STATUS_PARTIAL; + break; + case CS_SCHEDULE_NEW_SUBEVENT: + case CS_SCHEDULE_NEW_EVENT: + procedure_done = PROC_DONE_STATUS_PARTIAL; + subevent_done = SUBEVENT_DONE_STATUS_COMPLETED; + break; + case CS_SCHEDULE_NEW_PROCEDURE: + case CS_SCHEDULE_COMPLETED: + if (proc_abort_reason == PROC_ABORT_SUCCESS) { + procedure_done = PROC_DONE_STATUS_COMPLETED; + } else { + procedure_done = PROC_DONE_STATUS_ABORTED; + } + + if (subev_abort_reason == SUBEVENT_ABORT_SUCCESS) { + subevent_done = SUBEVENT_DONE_STATUS_COMPLETED; + } else { + subevent_done = SUBEVENT_DONE_STATUS_ABORTED; + } + + abort_reason = subev_abort_reason << 4 | proc_abort_reason; + break; + default: + BLE_LL_ASSERT(0); + } + + if (subevent->subev == BLE_HCI_LE_SUBEV_CS_SUBEVENT_RESULT) { + struct ble_hci_ev_le_subev_cs_subevent_result *ev = + (struct ble_hci_ev_le_subev_cs_subevent_result *)hci_ev->data; + + ev->procedure_done_status = procedure_done; + ev->subevent_done_status = subevent_done; + ev->abort_reason = abort_reason; + ev->num_steps_reported = subevent->num_steps_reported; + } else { + struct ble_hci_ev_le_subev_cs_subevent_result_continue *ev = + (struct ble_hci_ev_le_subev_cs_subevent_result_continue *)hci_ev->data; + + ev->procedure_done_status = procedure_done; + ev->subevent_done_status = subevent_done; + ev->abort_reason = abort_reason; + ev->num_steps_reported = subevent->num_steps_reported; + } + + memset(subevent, 0, sizeof(*subevent)); + + for (i = 0; i < ARRAY_SIZE(subevent_pool); ++i) { + ev = &subevent_pool[i]; + + if (ble_npl_event_get_arg(ev) == NULL) { + break; + } + + ev = NULL; + } + + if (!ev) { + ble_transport_free(hci_ev); + return 1; + } + + ble_npl_event_init(ev, ble_ll_cs_proc_send_subevent, hci_ev); + ble_ll_event_add(ev); + + return 0; +} + +static void +ble_ll_cs_proc_add_step_result(struct ble_ll_cs_sm *cssm) +{ + int rc; + struct ble_hci_ev *hci_ev; + struct ble_ll_cs_config *conf = cssm->active_config; + struct ble_ll_cs_step_result *result = &cssm->step_result; + struct cs_steps_data *step_data; + uint8_t *data; + int32_t t_sy_center_delta_us; + int32_t time_diff_ticks; + int32_t time_diff = TIME_DIFF_NOT_AVAILABLE; + uint8_t t_ip1 = conf->t_ip1; + uint8_t t_ip2 = conf->t_ip2; + uint8_t t_sw = cssm->t_sw; + uint8_t t_pm = conf->t_pm; + uint8_t n_ap = cssm->n_ap; + uint8_t t_sy = cssm->t_sy + cssm->t_sy_seq; + uint8_t data_len = 0; + uint8_t role = cssm->active_config->role; + uint8_t i; + + /* Estimate the size of the step results */ + if (cssm->step_mode == BLE_LL_CS_MODE0) { + data_len = 3; + if (role == BLE_LL_CS_ROLE_INITIATOR) { + data_len += 2; + } + } else if (cssm->step_mode == BLE_LL_CS_MODE1) { + data_len = 6; + if (result->sounding_pct_estimate) { + data_len += 8; + } + t_sy_center_delta_us = t_sy + T_RD + t_ip1; + } else if (cssm->step_mode == BLE_LL_CS_MODE2) { + data_len = 1 + (n_ap + 1) * 3 * 2; + } else if (cssm->step_mode == BLE_LL_CS_MODE3) { + data_len = 6 + 1 + (n_ap + 1) * 3 * 2; + if (result->sounding_pct_estimate) { + data_len += 8; + } + t_sy_center_delta_us = t_sy + T_RD + 2 * (t_sw + t_pm) * (n_ap + 1) + t_ip2; + } + + BLE_LL_ASSERT(cssm->buffered_subevent.hci_ev); + hci_ev = cssm->buffered_subevent.hci_ev; + + /* Validate if the step results will fit into the current hci event + * buffer or mark it as pending and create a new buffer. + */ + if (hci_ev->length + data_len <= BLE_HCI_MAX_DATA_LEN) { + ++cssm->buffered_subevent.num_steps_reported; + } else { + BLE_LL_ASSERT(cssm->steps_in_subevent_count != 0); + + rc = ble_ll_cs_proc_queue_subevent(&cssm->buffered_subevent, CS_SCHEDULE_NEW_STEP, + PROC_ABORT_SUCCESS, SUBEVENT_ABORT_SUCCESS); + BLE_LL_ASSERT(rc == 0); + + ble_ll_cs_init_subevent_continue(&cssm->buffered_subevent, cssm); + hci_ev = cssm->buffered_subevent.hci_ev; + + BLE_LL_ASSERT(hci_ev); + BLE_LL_ASSERT(hci_ev->length + data_len <= BLE_HCI_MAX_DATA_LEN); + + cssm->buffered_subevent.hci_ev = hci_ev; + cssm->buffered_subevent.num_steps_reported = 1; + } + + step_data = (struct cs_steps_data *)(hci_ev->data + hci_ev->length); + step_data->mode = cssm->step_mode; + step_data->channel = cssm->channel; + data = step_data->data; + + if (cssm->step_mode == BLE_LL_CS_MODE1 || cssm->step_mode == BLE_LL_CS_MODE3) { + if (role == BLE_LL_CS_ROLE_INITIATOR) { + if (result->time_of_arrival > result->time_of_departure) { + time_diff_ticks = result->time_of_arrival - result->time_of_departure; + time_diff = GET_TIME_DIFF(time_diff_ticks, t_sy_center_delta_us); + } + } else { /* BLE_LL_CS_ROLE_REFLECTOR */ + if (result->time_of_arrival < result->time_of_departure) { + time_diff_ticks = result->time_of_departure - result->time_of_arrival; + time_diff = GET_TIME_DIFF(time_diff_ticks, t_sy_center_delta_us); + } + } + + if (time_diff != TIME_DIFF_NOT_AVAILABLE && (time_diff < -0x7FFF || 0x7FFF < time_diff)) { + time_diff = TIME_DIFF_NOT_AVAILABLE; + } + } + + if (cssm->step_mode == BLE_LL_CS_MODE0) { + data[0] = result->packet_quality; + data[1] = result->packet_rssi; + data[2] = cssm->cs_sync_antenna; + data_len = 3; + + if (role == BLE_LL_CS_ROLE_INITIATOR) { + put_le16(data + 3, result->measured_freq_offset); + data_len += 2; + } + + } else if (cssm->step_mode == BLE_LL_CS_MODE1) { + data[0] = result->packet_quality; + data[1] = result->packet_nadm; + data[2] = result->packet_rssi; + put_le16(data + 3, time_diff); + data[5] = cssm->cs_sync_antenna; + data_len = 6; + + if (result->sounding_pct_estimate) { + put_le32(data + 6, result->packet_pct1); + put_le32(data + 10, result->packet_pct2); + data_len += 8; + } + + } else if (cssm->step_mode == BLE_LL_CS_MODE2) { + data[0] = cssm->active_config->proc_params.aci; + data_len = 1; + + for (i = 0; i < cssm->n_ap + 1; ++i) { + put_le24(data + data_len, result->tone_pct[i]); + data_len += 3; + } + + for (i = 0; i < cssm->n_ap + 1; ++i) { + data[data_len] = result->tone_quality_ind[i]; + ++data_len; + } + + } else if (cssm->step_mode == BLE_LL_CS_MODE3) { + data[0] = result->packet_quality; + data[1] = result->packet_nadm; + data[2] = result->packet_rssi; + put_le16(data + 3, time_diff); + data[5] = cssm->cs_sync_antenna; + data_len = 6; + + if (result->sounding_pct_estimate) { + put_le32(data + 6, result->packet_pct1); + put_le32(data + 10, result->packet_pct2); + data_len += 8; + } + + data[data_len] = cssm->active_config->proc_params.aci; + data_len += 1; + + for (i = 0; i < cssm->n_ap + 1; ++i) { + put_le24(data + data_len, result->tone_pct[i]); + data_len += 3; + } + + for (i = 0; i < cssm->n_ap + 1; ++i) { + data[data_len++] = result->tone_quality_ind[i]; + } + } + + hci_ev->length += data_len; + step_data->data_len = data_len; + memset(result, 0, sizeof(*result)); +} + +static uint8_t +ble_ll_cs_proc_set_t_sw(struct ble_ll_cs_sm *cssm) +{ + uint8_t t_sw; + uint8_t t_sw_i; + uint8_t t_sw_r; + uint8_t aci = cssm->active_config->proc_params.aci; + + if (cssm->active_config->role == BLE_LL_CS_ROLE_INITIATOR) { + t_sw_i = g_ble_ll_cs_local_cap.t_sw; + t_sw_r = cssm->remote_cap.t_sw; + } else { /* BLE_LL_CS_ROLE_REFLECTOR */ + t_sw_i = cssm->remote_cap.t_sw; + t_sw_r = g_ble_ll_cs_local_cap.t_sw; + } + + if (aci == 0) { + t_sw = 0; + } else if (IN_RANGE(aci, 1, 3)) { + t_sw = t_sw_i; + } else if (IN_RANGE(aci, 4, 6)) { + t_sw = t_sw_r; + } else { /* ACI == 7 */ + if (g_ble_ll_cs_local_cap.t_sw > cssm->remote_cap.t_sw) { + t_sw = g_ble_ll_cs_local_cap.t_sw; + } else { + t_sw = cssm->remote_cap.t_sw; + } + } + + cssm->t_sw = t_sw; + + return t_sw; +} + +static int +ble_ll_cs_proc_calculate_timing(struct ble_ll_cs_sm *cssm) +{ + struct ble_ll_cs_config *conf = cssm->active_config; + const struct ble_ll_cs_proc_params *params = &conf->proc_params; + uint8_t t_fcs = conf->t_fcs; + uint8_t t_ip1 = conf->t_ip1; + uint8_t t_ip2 = conf->t_ip2; + uint8_t t_pm = conf->t_pm; + uint8_t t_sw; + uint8_t n_ap = cssm->n_ap; + uint8_t t_sy; + uint8_t t_sy_seq; + uint8_t sequence_len; + + t_sw = ble_ll_cs_proc_set_t_sw(cssm); + + /* CS packets with no Sounding Sequence or Random Sequence fields take 44 µs + * to transmit when sent using the LE 1M PHY and 26 µs to transmit when using + * the LE 2M and the LE 2M 2BT PHYs. CS packets that include a Sounding Sequence + * or Random Sequence field take proportionally longer to transmit based on + * the length of the field and the PHY selection. + */ + + sequence_len = rtt_seq_len[conf->rtt_type]; + + switch (conf->cs_sync_phy) { + case BLE_LL_CS_SYNC_PHY_1M: + t_sy = BLE_LL_CS_SYNC_TIME_1M; + t_sy_seq = sequence_len; + break; + case BLE_LL_CS_SYNC_PHY_2M: + t_sy = BLE_LL_CS_SYNC_TIME_2M; + t_sy_seq = sequence_len / 2; + break; + } + + cssm->mode0_step_duration_usecs = t_ip1 + T_GD + T_FM + 2 * (t_sy + T_RD) + t_fcs; + + cssm->mode1_step_duration_usecs = t_ip1 + 2 * (t_sy + t_sy_seq + T_RD) + t_fcs; + + cssm->mode2_step_duration_usecs = t_ip2 + 2 * ((t_sw + t_pm) * (n_ap + 1) + T_RD) + t_fcs; + + cssm->mode3_step_duration_usecs = t_ip2 + 2 * ((t_sy + t_sy_seq + T_GD + T_RD) + + (t_sw + t_pm) * (n_ap + 1)) + t_fcs; + + cssm->t_sy = t_sy; + cssm->t_sy_seq = t_sy_seq; + + cssm->subevent_interval_usecs = params->subevent_interval * BLE_LL_CS_SUBEVENTS_INTERVAL_UNIT_US; + + cssm->event_interval_usecs = params->event_interval * cssm->connsm->conn_itvl * + BLE_LL_CONN_ITVL_USECS; + + cssm->procedure_interval_usecs = params->procedure_interval * cssm->connsm->conn_itvl * + BLE_LL_CONN_ITVL_USECS; + return 0; +} + +static void +ble_ll_cs_set_step_duration(struct ble_ll_cs_sm *cssm) +{ + switch (cssm->step_mode) { + case BLE_LL_CS_MODE0: + cssm->step_duration_usecs = cssm->mode0_step_duration_usecs; + break; + case BLE_LL_CS_MODE1: + cssm->step_duration_usecs = cssm->mode1_step_duration_usecs; + break; + case BLE_LL_CS_MODE2: + cssm->step_duration_usecs = cssm->mode2_step_duration_usecs; + break; + case BLE_LL_CS_MODE3: + cssm->step_duration_usecs = cssm->mode3_step_duration_usecs; + break; + default: + BLE_LL_ASSERT(0); + } +} + +static int +ble_lL_cs_validate_step_duration(struct ble_ll_cs_sm *cssm) +{ + const struct ble_ll_cs_config *conf = cssm->active_config; + const struct ble_ll_cs_proc_params *params = &conf->proc_params; + uint32_t max_procedure_len = conf->proc_params.max_procedure_len; + uint32_t max_procedure_len_usecs = max_procedure_len * BLE_LL_CS_PROCEDURE_LEN_UNIT_US; + uint32_t max_subevent_len_usecs = conf->proc_params.subevent_len; + uint32_t step_duration_usecs = cssm->step_duration_usecs; + uint32_t total_subevent_usecs = cssm->step_anchor_usecs - cssm->subevent_anchor_usecs; + uint32_t total_procedure_usecs = cssm->step_anchor_usecs - cssm->procedure_anchor_usecs; + + if (total_procedure_usecs + step_duration_usecs > max_procedure_len_usecs || + cssm->steps_in_procedure_count + 1 >= BLE_LL_CS_STEPS_PER_PROCEDURE_MAX) { + /* The CS procedure is complete */ + + /* A CS procedure is considered complete and closed when at least one of the following + * conditions occurs: + * • The execution of the next CS step in its entirety would cause the extent of the CS + * procedure to exceed T_MAX_PROCEDURE_LEN. + * • The combined number of mode-0 steps and non-mode‑0 steps executed is equal to + * N_STEPS_MAX as described in [Vol 6] Part B, Section 4.5.18.1. + * • The number of CS subevents executed is equal to + * N_MAX_SUBEVENTS_PER_PROCEDURE. + * TODO: + * • If Channel Selection Algorithm #3b is used for non-mode-0 steps, and the channel + * map generated from CSFilteredChM has been used for CSNumRepetitions cycles + * for non-mode‑0 steps including the use for both Main_Mode and Sub_Mode steps. + * • If Channel Selection Algorithm #3c is used for non-mode-0 steps, and + * CSNumRepetitions invocations of Channel Selection Algorithm #3c have been + * completed for non-mode‑0 steps including the use for both Main_Mode and + * Sub_Mode steps. + */ + + return CS_SCHEDULE_NEW_PROCEDURE; + } + + if (total_subevent_usecs + step_duration_usecs > max_subevent_len_usecs || + cssm->steps_in_subevent_count + 1 >= BLE_LL_CS_STEPS_PER_SUBEVENT_MAX) { + + if (cssm->subevents_in_procedure_count + 1 >= BLE_LL_CS_SUBEVENTS_PER_PROCEDURE_MAX) { + return CS_SCHEDULE_NEW_PROCEDURE; + } + + if (cssm->subevents_in_event_count + 1 >= params->subevents_per_event) { + total_procedure_usecs = cssm->event_anchor_usecs + cssm->event_interval_usecs + + cssm->mode0_step_duration_usecs - cssm->procedure_anchor_usecs; + + if (total_procedure_usecs > max_procedure_len_usecs) { + return CS_SCHEDULE_NEW_PROCEDURE; + } + + return CS_SCHEDULE_NEW_EVENT; + } + + return CS_SCHEDULE_NEW_SUBEVENT; + } + + return CS_SCHEDULE_NEW_STEP; +} + +static int +ble_ll_cs_setup_next_subevent(struct ble_ll_cs_sm *cssm) +{ + const struct ble_ll_cs_proc_params *params = &cssm->active_config->proc_params; + + ++cssm->subevents_in_procedure_count; + ++cssm->subevents_in_event_count; + cssm->steps_in_subevent_count = 0; + + cssm->subevent_anchor_usecs += cssm->event_interval_usecs; + cssm->step_anchor_usecs = cssm->subevent_anchor_usecs; + + return 0; +} + +static int +ble_ll_cs_setup_next_event(struct ble_ll_cs_sm *cssm) +{ + const struct ble_ll_cs_proc_params *params = &cssm->active_config->proc_params; + + ++cssm->events_in_procedure_count; + ++cssm->subevents_in_procedure_count; + cssm->subevents_in_event_count = 0; + cssm->steps_in_subevent_count = 0; + + cssm->event_anchor_usecs += cssm->event_interval_usecs; + cssm->subevent_anchor_usecs = cssm->event_anchor_usecs; + cssm->step_anchor_usecs = cssm->event_anchor_usecs; + + return 0; +} + +static int +ble_ll_cs_setup_next_procedure(struct ble_ll_cs_sm *cssm) +{ + const struct ble_ll_cs_proc_params *params = &cssm->active_config->proc_params; + + ++cssm->procedure_count; + cssm->events_in_procedure_count = 0; + cssm->subevents_in_procedure_count = 0; + cssm->subevents_in_event_count = 0; + cssm->steps_in_procedure_count = 0; + cssm->steps_in_subevent_count = 0; + + cssm->procedure_anchor_usecs += cssm->procedure_anchor_usecs; + cssm->step_anchor_usecs = cssm->procedure_anchor_usecs; + cssm->subevent_anchor_usecs = cssm->step_anchor_usecs; + + return ble_ll_cs_backtracking_resistance(cssm); +} + +static int +ble_ll_cs_proc_subevent_next_state(struct ble_ll_cs_sm *cssm) +{ + int rc; + const struct ble_ll_cs_config *conf = cssm->active_config; + + if (cssm->steps_in_subevent_count == 0) { + ble_ll_cs_drbg_clear_cache(&cssm->drbg_ctx); + cssm->mode0_step_count = conf->mode_0_steps; + + if (cssm->subevents_in_procedure_count == 0) { + cssm->main_step_count = 0xFF; + } else { + cssm->repetition_count = conf->main_mode_repetition; + } + } + + if (cssm->mode0_step_count > 0) { + cssm->subevent_state = SUBEVENT_STATE_MODE0_STEP; + cssm->step_mode = BLE_LL_CS_MODE0; + --cssm->mode0_step_count; + + } else if (cssm->repetition_count > 0) { + cssm->subevent_state = SUBEVENT_STATE_REPETITION_STEP; + cssm->step_mode = conf->main_mode; + --cssm->repetition_count; + + } else if (cssm->main_step_count == 0) { + cssm->subevent_state = SUBEVENT_STATE_SUBMODE_STEP; + cssm->step_mode = conf->sub_mode; + cssm->main_step_count = 0xFF; + + } else { + cssm->subevent_state = SUBEVENT_STATE_MAINMODE_STEP; + cssm->step_mode = conf->main_mode; + } + + ble_ll_cs_set_step_duration(cssm); + + rc = ble_lL_cs_validate_step_duration(cssm); + if (rc) { + /* Step does not fit in current subevent. */ + switch (rc) { + case CS_SCHEDULE_NEW_SUBEVENT: + ble_ll_cs_setup_next_subevent(cssm); + cssm->cs_schedule_status = CS_SCHEDULE_NEW_SUBEVENT; + break; + case CS_SCHEDULE_NEW_EVENT: + ble_ll_cs_setup_next_event(cssm); + cssm->cs_schedule_status = CS_SCHEDULE_NEW_EVENT; + break; + case CS_SCHEDULE_NEW_PROCEDURE: + if (conf->proc_params.max_procedure_count <= cssm->procedure_count + 1 || + cssm->terminate_measurement) { + /* All CS procedures have been completed or + * the CS procedure repeat series has been terminated. + */ + cssm->cs_schedule_status = CS_SCHEDULE_COMPLETED; + + if (cssm->terminate_measurement) { + cssm->proc_abort_reason = PROC_ABORT_REQUESTED; + } + + return 0; + } + + ble_ll_cs_setup_next_procedure(cssm); + cssm->cs_schedule_status = CS_SCHEDULE_NEW_PROCEDURE; + break; + default: + BLE_LL_ASSERT(0); + } + + cssm->step_mode = BLE_LL_CS_MODE0; + ble_ll_cs_set_step_duration(cssm); + + if (ble_lL_cs_validate_step_duration(cssm) != CS_SCHEDULE_NEW_STEP) { + return 1; + } + + return 0; + } + + cssm->cs_schedule_status = CS_SCHEDULE_NEW_STEP; + + if (cssm->subevent_state == SUBEVENT_STATE_MAINMODE_STEP && + conf->sub_mode != 0xFF) { + if (cssm->main_step_count == 0xFF) { + /* Rand the number of Main_Mode steps to execute + * before a Sub_Mode insertion. + */ + rc = ble_ll_cs_drbg_rand_main_mode_steps( + &cssm->drbg_ctx, cssm->steps_in_procedure_count, + conf->main_mode_min_steps, conf->main_mode_max_steps, + &cssm->main_step_count); + + if (rc) { + return rc; + } + } + + --cssm->main_step_count; + } + + return 0; +} + +static int +ble_ll_cs_setup_next_step(struct ble_ll_cs_sm *cssm) +{ + int rc; + const struct ble_ll_cs_config *conf = cssm->active_config; + + cssm->step_anchor_usecs = cssm->anchor_usecs; + ++cssm->steps_in_procedure_count; + ++cssm->steps_in_subevent_count; + + rc = ble_ll_cs_proc_subevent_next_state(cssm); + if (rc || cssm->cs_schedule_status == CS_SCHEDULE_COMPLETED) { + return rc; + } + + if (cssm->subevent_state != SUBEVENT_STATE_REPETITION_STEP) { + rc = ble_ll_cs_generate_channel(cssm); + if (rc) { + return rc; + } + + /* Update channel cache used in repetition steps */ + if (cssm->subevent_state == SUBEVENT_STATE_MAINMODE_STEP && + conf->main_mode_repetition) { + cssm->repetition_channels[0] = cssm->repetition_channels[1]; + cssm->repetition_channels[1] = cssm->repetition_channels[2]; + cssm->repetition_channels[2] = cssm->channel; + } + } else { + cssm->channel = cssm->repetition_channels[ + conf->main_mode_repetition - cssm->repetition_count]; + } + + /* Generate CS Access Address */ + rc = ble_ll_cs_drbg_generate_aa(&cssm->drbg_ctx, + cssm->steps_in_procedure_count, + &cssm->initiator_aa, &cssm->reflector_aa); + if (rc) { + return rc; + } + + if (conf->role == BLE_LL_CS_ROLE_INITIATOR) { + cssm->tx_aa = cssm->initiator_aa; + cssm->rx_aa = cssm->reflector_aa; + } else { /* BLE_LL_CS_ROLE_REFLECTOR */ + cssm->rx_aa = cssm->initiator_aa; + cssm->tx_aa = cssm->reflector_aa; + } + + cssm->antenna_path_count = cssm->n_ap; + cssm->anchor_usecs = cssm->step_anchor_usecs; + + /* TODO: Generate antenna ID if multiple antennas available */ + cssm->cs_sync_antenna = 0x01; + + rc = ble_ll_cs_drbg_rand_tone_ext_presence( + &cssm->drbg_ctx, cssm->steps_in_procedure_count, &cssm->tone_ext_presence_i); + + if (rc) { + return rc; + } + + rc = ble_ll_cs_drbg_rand_tone_ext_presence( + &cssm->drbg_ctx, cssm->steps_in_procedure_count, &cssm->tone_ext_presence_r); + + return rc; +} + +static void +ble_ll_cs_proc_mode0_next_state(struct ble_ll_cs_sm *cssm) +{ + uint32_t duration = 0; + uint32_t delay = 0; + uint8_t state = cssm->step_state; + uint8_t t_ip1 = cssm->active_config->t_ip1; + uint8_t t_fcs = cssm->active_config->t_fcs; + uint8_t t_sy = cssm->t_sy; + + switch (state) { + case STEP_STATE_INIT: + state = STEP_STATE_CS_SYNC_I; + duration = t_sy; + delay = 0; + break; + case STEP_STATE_CS_SYNC_I: + state = STEP_STATE_CS_SYNC_R; + duration = t_sy; + delay = T_RD + t_ip1; + break; + case STEP_STATE_CS_SYNC_R: + state = STEP_STATE_CS_TONE_R; + duration = T_FM; + delay = T_GD; + break; + case STEP_STATE_CS_TONE_R: + state = STEP_STATE_COMPLETE; + duration = 0; + delay = T_RD + t_fcs; + break; + default: + BLE_LL_ASSERT(0); + } + + cssm->duration_usecs = duration; + cssm->anchor_usecs += delay; + cssm->step_state = state; +} + +static void +ble_ll_cs_proc_mode1_next_state(struct ble_ll_cs_sm *cssm) +{ + uint32_t duration = 0; + uint32_t delay = 0; + uint8_t state = cssm->step_state; + uint8_t t_ip1 = cssm->active_config->t_ip1; + uint8_t t_fcs = cssm->active_config->t_fcs; + uint8_t t_sy = cssm->t_sy + cssm->t_sy_seq; + + switch (state) { + case STEP_STATE_INIT: + state = STEP_STATE_CS_SYNC_I; + duration = t_sy; + delay = 0; + break; + case STEP_STATE_CS_SYNC_I: + state = STEP_STATE_CS_SYNC_R; + duration = t_sy; + delay = T_RD + t_ip1; + break; + case STEP_STATE_CS_SYNC_R: + state = STEP_STATE_COMPLETE; + duration = 0; + delay = T_RD + t_fcs; + break; + default: + BLE_LL_ASSERT(0); + } + + cssm->duration_usecs = duration; + cssm->anchor_usecs += delay; + cssm->step_state = state; +} + +static void +ble_ll_cs_proc_mode2_next_state(struct ble_ll_cs_sm *cssm) +{ + uint32_t duration = 0; + uint32_t delay = 0; + uint8_t state = cssm->step_state; + uint8_t t_ip2 = cssm->active_config->t_ip2; + uint8_t t_fcs = cssm->active_config->t_fcs; + uint8_t t_pm = cssm->active_config->t_pm; + + switch (state) { + case STEP_STATE_INIT: + state = STEP_STATE_CS_TONE_I; + duration = t_pm; + delay = 0; + break; + case STEP_STATE_CS_TONE_I: + duration = t_pm; + delay = cssm->t_sw; + if (cssm->antenna_path_count != 0) { + --cssm->antenna_path_count; + } else { + state = STEP_STATE_CS_TONE_EXT_I; + } + break; + case STEP_STATE_CS_TONE_EXT_I: + state = STEP_STATE_CS_TONE_R; + cssm->antenna_path_count = cssm->n_ap; + delay = T_RD + t_ip2; + break; + case STEP_STATE_CS_TONE_R: + duration = t_pm; + delay = cssm->t_sw; + if (cssm->antenna_path_count != 0) { + --cssm->antenna_path_count; + } else { + state = STEP_STATE_CS_TONE_EXT_R; + } + break; + case STEP_STATE_CS_TONE_EXT_R: + state = STEP_STATE_COMPLETE; + cssm->antenna_path_count = cssm->n_ap; + duration = 0; + delay = T_RD + t_fcs; + break; + default: + BLE_LL_ASSERT(0); + } + + cssm->duration_usecs = duration; + cssm->anchor_usecs += delay; + cssm->step_state = state; +} + +static void +ble_ll_cs_proc_mode3_next_state(struct ble_ll_cs_sm *cssm) +{ + uint32_t duration = 0; + uint32_t delay = 0; + uint8_t state = cssm->step_state; + uint8_t t_ip2 = cssm->active_config->t_ip2; + uint8_t t_fcs = cssm->active_config->t_fcs; + uint8_t t_sy = cssm->t_sy + cssm->t_sy_seq; + uint8_t t_pm = cssm->active_config->t_pm; + + switch (state) { + case STEP_STATE_INIT: + state = STEP_STATE_CS_SYNC_I; + duration = t_sy; + delay = 0; + break; + case STEP_STATE_CS_SYNC_I: + state = STEP_STATE_CS_TONE_I; + duration = t_pm; + delay = T_GD; + break; + case STEP_STATE_CS_TONE_I: + duration = t_pm; + delay = cssm->t_sw; + if (cssm->antenna_path_count != 0) { + --cssm->antenna_path_count; + } else { + state = STEP_STATE_CS_TONE_EXT_I; + } + break; + case STEP_STATE_CS_TONE_EXT_I: + state = STEP_STATE_CS_TONE_R; + cssm->antenna_path_count = cssm->n_ap; + delay = T_RD + t_ip2; + break; + case STEP_STATE_CS_TONE_R: + duration = t_pm; + delay = cssm->t_sw; + if (cssm->antenna_path_count != 0) { + --cssm->antenna_path_count; + } else { + state = STEP_STATE_CS_TONE_EXT_R; + } + break; + case STEP_STATE_CS_TONE_EXT_R: + state = STEP_STATE_CS_SYNC_R; + cssm->antenna_path_count = cssm->n_ap; + duration = t_sy; + delay = T_RD; + break; + case STEP_STATE_CS_SYNC_R: + state = STEP_STATE_COMPLETE; + duration = 0; + delay = T_RD + t_fcs; + break; + default: + BLE_LL_ASSERT(0); + } + + cssm->duration_usecs = duration; + cssm->anchor_usecs += delay; + cssm->step_state = state; +} + +static void +ble_ll_cs_proc_step_next_state(struct ble_ll_cs_sm *cssm) +{ + switch (cssm->step_mode) { + case BLE_LL_CS_MODE0: + ble_ll_cs_proc_mode0_next_state(cssm); + break; + case BLE_LL_CS_MODE1: + ble_ll_cs_proc_mode1_next_state(cssm); + break; + case BLE_LL_CS_MODE2: + ble_ll_cs_proc_mode2_next_state(cssm); + break; + case BLE_LL_CS_MODE3: + ble_ll_cs_proc_mode3_next_state(cssm); + break; + default: + BLE_LL_ASSERT(0); + } +} + +static int +ble_ll_cs_proc_next_state(struct ble_ll_cs_sm *cssm) +{ + int rc; + + if (cssm->step_state != STEP_STATE_INIT) { + ble_ll_cs_proc_step_next_state(cssm); + + if (cssm->step_state != STEP_STATE_COMPLETE) { + /* Continue pending step */ + return 0; + } + + /* Save step results */ + ble_ll_cs_proc_add_step_result(cssm); + } + + /* Setup a new step */ + + cssm->step_state = STEP_STATE_INIT; + + rc = ble_ll_cs_setup_next_step(cssm); + if (rc) { + if (!cssm->proc_abort_reason) { + cssm->proc_abort_reason = PROC_ABORT_UNSPECIFIED; + } + + cssm->cs_schedule_status = CS_SCHEDULE_COMPLETED; + } + + if (cssm->cs_schedule_status == CS_SCHEDULE_COMPLETED) { + rc = ble_ll_cs_proc_queue_subevent(&cssm->buffered_subevent, cssm->cs_schedule_status, + cssm->proc_abort_reason, cssm->subev_abort_reason); + BLE_LL_ASSERT(rc == 0); + + return 1; + } + + ble_ll_cs_proc_step_next_state(cssm); + + if (cssm->cs_schedule_status != CS_SCHEDULE_NEW_STEP) { + rc = ble_ll_cs_proc_queue_subevent(&cssm->buffered_subevent, cssm->cs_schedule_status, + cssm->proc_abort_reason, cssm->subev_abort_reason); + BLE_LL_ASSERT(rc == 0); + + rc = ble_ll_cs_init_subevent(&cssm->buffered_subevent, cssm); + BLE_LL_ASSERT(rc == 0); + } + + return 0; +} + +static int +ble_ll_cs_proc_skip_txrx(struct ble_ll_cs_sm *cssm) +{ + int rc; + + rc = ble_ll_cs_proc_schedule_next_tx_or_rx(cssm); + (void) rc; + + return 0; +} + +static ble_ll_cs_sched_cb_func +ble_ll_cs_proc_sched_cb_get(struct ble_ll_cs_sm *cssm) +{ + ble_ll_cs_sched_cb_func cb; + + cssm->rx_window_offset_usecs = 0; + + if (cssm->active_config->role == BLE_LL_CS_ROLE_INITIATOR) { + switch (cssm->step_state) { + case STEP_STATE_CS_SYNC_I: + cb = ble_ll_cs_sync_tx_start; + break; + case STEP_STATE_CS_SYNC_R: + cb = ble_ll_cs_sync_rx_start; + cssm->rx_window_offset_usecs = 2; + break; + case STEP_STATE_CS_TONE_I: + cb = ble_ll_cs_tone_tx_start; + break; + case STEP_STATE_CS_TONE_R: + cb = ble_ll_cs_tone_rx_start; + break; + case STEP_STATE_CS_TONE_EXT_I: + if (cssm->tone_ext_presence_i) { + cb = ble_ll_cs_tone_tx_start; + } else { + cb = ble_ll_cs_proc_skip_txrx; + } + break; + case STEP_STATE_CS_TONE_EXT_R: + if (cssm->tone_ext_presence_r) { + cb = ble_ll_cs_tone_rx_start; + } else { + cb = ble_ll_cs_proc_skip_txrx; + } + break; + default: + BLE_LL_ASSERT(0); + } + } else { /* BLE_LL_CS_ROLE_REFLECTOR */ + switch (cssm->step_state) { + case STEP_STATE_CS_SYNC_I: + cb = ble_ll_cs_sync_rx_start; + cssm->rx_window_offset_usecs = 2; + break; + case STEP_STATE_CS_SYNC_R: + cb = ble_ll_cs_sync_tx_start; + break; + case STEP_STATE_CS_TONE_I: + cb = ble_ll_cs_tone_rx_start; + break; + case STEP_STATE_CS_TONE_R: + cb = ble_ll_cs_tone_tx_start; + break; + case STEP_STATE_CS_TONE_EXT_I: + if (cssm->tone_ext_presence_i) { + cb = ble_ll_cs_tone_rx_start; + } else { + cb = ble_ll_cs_proc_skip_txrx; + } + break; + case STEP_STATE_CS_TONE_EXT_R: + if (cssm->tone_ext_presence_r) { + cb = ble_ll_cs_tone_tx_start; + } else { + cb = ble_ll_cs_proc_skip_txrx; + } + break; + default: + BLE_LL_ASSERT(0); + } + } + + return cb; +} + +static int +ble_ll_cs_proc_sched_cb(struct ble_ll_sched_item *sch) +{ + int rc; + struct ble_ll_cs_sm *cssm = sch->cb_arg; + + BLE_LL_ASSERT(cssm != NULL); + + rc = cssm->sched_cb(cssm); + if (rc) { + return BLE_LL_SCHED_STATE_DONE; + } + + return BLE_LL_SCHED_STATE_RUNNING; +} + +int +ble_ll_cs_proc_schedule_next_tx_or_rx(struct ble_ll_cs_sm *cssm) +{ + int rc; + ble_ll_cs_sched_cb_func cb; + + rc = ble_ll_cs_proc_next_state(cssm); + if (rc) { + return rc; + } + + cb = ble_ll_cs_proc_sched_cb_get(cssm); + cssm->anchor_usecs -= cssm->rx_window_offset_usecs; + cssm->anchor_cputime = ble_ll_tmr_u2t_r(cssm->anchor_usecs, &cssm->anchor_rem_usecs); + + if (cssm->anchor_cputime - g_ble_ll_sched_offset_ticks > ble_ll_tmr_get()) { + cssm->sch.start_time = cssm->anchor_cputime - g_ble_ll_sched_offset_ticks; + } else { + cssm->sch.start_time = ble_ll_tmr_get(); + } + + cssm->sched_cb = cb; + cssm->sch.end_time = cssm->sch.start_time + ble_ll_tmr_u2t_up(cssm->duration_usecs); + cssm->sch.remainder = 0; + cssm->sch.sched_type = BLE_LL_SCHED_TYPE_CS; + cssm->sch.cb_arg = cssm; + cssm->sch.sched_cb = ble_ll_cs_proc_sched_cb; + + rc = ble_ll_sched_cs_proc(&cssm->sch); + + return rc; +} + +void +ble_ll_cs_proc_set_now_as_anchor_point(struct ble_ll_cs_sm *cssm) +{ + cssm->anchor_usecs = ble_ll_tmr_t2u(ble_ll_tmr_get()); +} + +int +ble_ll_cs_proc_scheduling_start(struct ble_ll_conn_sm *connsm, uint8_t config_id) +{ + int rc; + struct ble_ll_cs_sm *cssm = connsm->cssm; + struct ble_ll_cs_config *conf; + const struct ble_ll_cs_proc_params *params; + uint32_t anchor_ticks; + uint8_t anchor_rem_usecs; + + conf = &cssm->config[config_id]; + cssm->active_config = conf; + cssm->active_config_id = config_id; + params = &conf->proc_params; + + ble_ll_conn_get_anchor(connsm, params->anchor_conn_event, + &anchor_ticks, &anchor_rem_usecs); + + ble_ll_tmr_add(&anchor_ticks, &anchor_rem_usecs, params->event_offset); + + if (anchor_ticks - g_ble_ll_sched_offset_ticks < ble_ll_tmr_get()) { + /* The start happend too late for the negotiated event counter. */ + return BLE_ERR_INV_LMP_LL_PARM; + } + + g_ble_ll_cs_sm_current = cssm; + cssm->anchor_usecs = ble_ll_tmr_t2u(anchor_ticks); + cssm->step_mode = BLE_LL_CS_MODE0; + cssm->step_state = STEP_STATE_INIT; + cssm->n_ap = aci_table[params->aci].n_ap; + cssm->procedure_anchor_usecs = cssm->anchor_usecs; + cssm->event_anchor_usecs = cssm->anchor_usecs; + cssm->subevent_anchor_usecs = cssm->anchor_usecs; + cssm->step_anchor_usecs = cssm->anchor_usecs; + + cssm->steps_in_procedure_count = ~0; + cssm->steps_in_subevent_count = ~0; + cssm->procedure_count = 0; + + cssm->mode0_next_chan_id = 0xFF; + cssm->non_mode0_next_chan_id = 0xFF; + + if (g_ble_ll_cs_local_cap.sounding_pct_estimate && + cssm->active_config->rtt_type != BLE_LL_CS_RTT_AA_ONLY) { + cssm->step_result.sounding_pct_estimate = 1; + } + + ble_ll_cs_proc_calculate_timing(cssm); + + rc = ble_ll_cs_init_subevent(&cssm->buffered_subevent, cssm); + BLE_LL_ASSERT(rc == 0); + + rc = ble_ll_cs_proc_schedule_next_tx_or_rx(cssm); + if (rc) { + return BLE_ERR_UNSPECIFIED; + } + + return BLE_ERR_SUCCESS; +} + +void +ble_ll_cs_proc_sync_lost(struct ble_ll_cs_sm *cssm) +{ + ble_phy_disable(); + ble_phy_cs_sync_mode_set(0); + ble_ll_state_set(BLE_LL_STATE_STANDBY); + + ble_ll_cs_proc_schedule_next_tx_or_rx(cssm); +} +#endif /* BLE_LL_CHANNEL_SOUNDING */ diff --git a/nimble/controller/src/ble_ll_cs_sync.c b/nimble/controller/src/ble_ll_cs_sync.c new file mode 100644 index 0000000000..054b959ba5 --- /dev/null +++ b/nimble/controller/src/ble_ll_cs_sync.c @@ -0,0 +1,384 @@ +/* + * 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 +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +#include +#include "controller/ble_ll.h" +#include "controller/ble_ll_conn.h" +#include "controller/ble_ll_sched.h" +#include "controller/ble_ll_tmr.h" +#include "controller/ble_hw.h" +#include "controller/ble_ll_hci.h" +#include "mbedtls/aes.h" +#include "ble_ll_priv.h" +#include "ble_ll_cs_priv.h" + +extern struct ble_ll_cs_supp_cap g_ble_ll_cs_local_cap; +extern struct ble_ll_cs_sm g_ble_ll_cs_sm[MYNEWT_VAL(BLE_MAX_CONNECTIONS)]; +extern struct ble_ll_cs_sm *g_ble_ll_cs_sm_current; +extern int8_t g_ble_ll_tx_power; + +static uint8_t +ble_ll_cs_sync_calc_seq_quality(struct ble_ll_cs_sm *cssm, uint8_t *rxpdu, + struct ble_mbuf_hdr *hdr) +{ + /* TODO: Check if all bits match the expected sequence. + * For now returns "all bits match". + */ + return 0; +} + +static uint8_t +ble_ll_cs_sync_calc_nadm(struct ble_ll_cs_sm *cssm, uint8_t *rxpdu, + struct ble_mbuf_hdr *hdr) +{ + uint8_t nadm; + + if (cssm->active_config->rtt_type == 0x00) { + /* No sequence, so NADM unknown. */ + nadm = 0xFF; + } else { + /* TODO: Estimate how much a received GFSK modulated packet signal + * differs from the expected packet signal. Use the normalized attack + * detector metric (NADM). For now return "Attack extremely unlikely". + */ + nadm = 0x00; + } + + return nadm; +} + +static uint8_t +ble_ll_cs_sync_make(struct ble_ll_cs_sm *cssm, uint8_t *buf) +{ + int i; + int rc; + uint32_t aa; + struct ble_ll_cs_config *conf = cssm->active_config; + uint8_t sequence_len; + uint8_t bits; + + if (conf->rtt_type != BLE_LL_CS_RTT_AA_ONLY) { + rc = ble_ll_cs_drbg_generate_sync_sequence( + &cssm->drbg_ctx, cssm->steps_in_procedure_count, conf->rtt_type, + buf, &sequence_len); + + if (rc) { + return 0; + } + } + + /* Shift by 4 bits to make space for trailer bits */ + if (sequence_len > 0) { + assert(sequence_len < BLE_PHY_MAX_PDU_LEN); + buf[sequence_len] = 0; + bits = 0; + for (i = sequence_len - 1; i >= 0; --i) { + bits = buf[i] >> 4; + buf[i] = (buf[i] << 4) & 0xF0; + buf[i + 1] = (buf[i + 1] & 0xF0) | bits; + } + } + + if (conf->role == BLE_LL_CS_ROLE_INITIATOR) { + aa = cssm->initiator_aa; + } else { + aa = cssm->reflector_aa; + } + + /* Set the trailer bits */ + if (aa & 0x80000000) { + buf[0] |= 0x0A; + } else { + buf[0] |= 0x05; + } + ++sequence_len; + + return sequence_len; +} + +/** + * Called when a CS pkt has been transmitted. + * Schedule next CS pkt reception. + * + * Context: interrupt + */ +void +ble_ll_cs_sync_tx_end_cb(void *arg) +{ + struct ble_ll_cs_sm *cssm = g_ble_ll_cs_sm_current; + uint32_t cputime; + uint32_t rem_us; + uint32_t rem_ns; + uint32_t anchor_usecs; + + assert(cssm != NULL); + + ble_phy_get_txend_time(&cputime, &rem_us, &rem_ns); + anchor_usecs = ble_ll_tmr_t2u(cputime) + rem_us; + + cssm->anchor_usecs = anchor_usecs; + if (cssm->step_mode != BLE_LL_CS_MODE0 && + cssm->active_config->role == BLE_LL_CS_ROLE_REFLECTOR) { + cssm->step_result.time_of_departure = cssm->anchor_usecs; + } + + ble_phy_disable(); + ble_phy_cs_sync_mode_set(0); + ble_ll_state_set(BLE_LL_STATE_STANDBY); + + ble_ll_cs_proc_schedule_next_tx_or_rx(cssm); +} + +static uint8_t +ble_ll_cs_sync_tx_make(uint8_t *dptr, void *arg, uint8_t *hdr_byte) +{ + uint8_t pdu_len; + struct ble_ll_cs_sm *cssm = arg; + + /* TODO: Unused fields in CS Sync packet */ + pdu_len = ble_ll_cs_sync_make(cssm, dptr); + *hdr_byte = 0; + + return pdu_len; +} + +int +ble_ll_cs_sync_tx_start(struct ble_ll_cs_sm *cssm) +{ + int rc; + uint8_t cs_timer; + + BLE_LL_ASSERT(ble_ll_state_get() == BLE_LL_STATE_STANDBY); + + ble_phy_cs_sync_mode_set(1); + + ble_ll_tx_power_set(g_ble_ll_tx_power); + + if (cssm->step_mode == BLE_LL_CS_MODE0) { + cs_timer = BLE_PHY_CS_TIMER_NONE; + } else if (cssm->active_config->role == BLE_LL_CS_ROLE_INITIATOR) { + cs_timer = BLE_PHY_CS_TIMER_START; + cssm->step_result.time_of_departure = cssm->anchor_usecs; + } else { /* BLE_LL_CS_ROLE_REFLECTOR */ + cs_timer = BLE_PHY_CS_TIMER_CAPTURE; + } + + rc = ble_phy_cs_sync_configure(cssm->channel, cssm->tx_aa, cs_timer); + if (rc) { + ble_ll_cs_proc_sync_lost(cssm); + return 1; + } + + rc = ble_phy_tx_set_start_time(cssm->anchor_cputime, cssm->anchor_rem_usecs); + if (rc) { + ble_ll_cs_proc_sync_lost(cssm); + return 1; + } + + ble_phy_set_txend_cb(ble_ll_cs_sync_tx_end_cb, cssm); + + rc = ble_phy_tx_cs_sync(ble_ll_cs_sync_tx_make, cssm); + if (rc) { + ble_ll_cs_proc_sync_lost(cssm); + return 1; + } + + ble_ll_state_set(BLE_LL_STATE_CS); + + return 0; +} + +int +ble_ll_cs_sync_rx_start(struct ble_ll_cs_sm *cssm) +{ + int rc; + uint32_t wfr_usecs; + uint8_t cs_timer; + + BLE_LL_ASSERT(ble_ll_state_get() == BLE_LL_STATE_STANDBY); + + ble_phy_cs_sync_mode_set(1); + + if (cssm->step_mode == BLE_LL_CS_MODE0) { + cs_timer = BLE_PHY_CS_TIMER_NONE; + } else if (cssm->active_config->role == BLE_LL_CS_ROLE_INITIATOR) { + cs_timer = BLE_PHY_CS_TIMER_CAPTURE; + } else { /* BLE_LL_CS_ROLE_REFLECTOR */ + cs_timer = BLE_PHY_CS_TIMER_START; + cssm->step_result.time_of_arrival = cssm->anchor_usecs; + } + + rc = ble_phy_cs_sync_configure(cssm->channel, cssm->rx_aa, cs_timer); + if (rc) { + ble_ll_cs_proc_sync_lost(cssm); + return 1; + } + + rc = ble_phy_rx_set_start_time(cssm->anchor_cputime, cssm->anchor_rem_usecs); + + /* Accept the risk of being late just for testing + * with slower clocks without increasing the T_RD. + * The nRF52 is too slow for this CS ping pong. + */ + if (rc && rc != BLE_PHY_ERR_RX_LATE) { + ble_ll_cs_proc_sync_lost(cssm); + return 1; + } + + wfr_usecs = cssm->duration_usecs; + ble_phy_wfr_enable(BLE_PHY_WFR_ENABLE_RX, 0, wfr_usecs); + + ble_ll_state_set(BLE_LL_STATE_CS); + + return 0; +} + +static struct ble_ll_cs_sm * +ble_ll_cs_sync_find_sm_match_aa(uint32_t aa) +{ + int i; + struct ble_ll_cs_sm *cssm; + + for (i = 0; i < ARRAY_SIZE(g_ble_ll_cs_sm); ++i) { + cssm = &g_ble_ll_cs_sm[i]; + + if (cssm->rx_aa == aa) { + return cssm; + } + } + + return NULL; +} + +/** + * Called when a receive PDU has started. + * Check if the frame is the next expected CS_SYNC packet. + * + * Context: interrupt + * + * @return int + * < 0: A frame we dont want to receive. + * = 0: Continue to receive frame. Dont go from rx to tx + */ +int +ble_ll_cs_sync_rx_isr_start(struct ble_mbuf_hdr *rxhdr, uint32_t aa) +{ + struct ble_ll_cs_sm *cssm; + + cssm = ble_ll_cs_sync_find_sm_match_aa(aa); + + if (cssm == NULL) { + /* This is not the expected packet. Skip the frame. */ + return -1; + } + + return 0; +} + +/** + * Called when received a complete CS_SYNC packet. + * + * Context: Interrupt + * + * @param rxpdu + * @param rxhdr + * + * @return int + * < 0: Disable the phy after reception. + * == 0: Success. Do not disable the PHY. + * > 0: Do not disable PHY as that has already been done. + */ +int +ble_ll_cs_sync_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr) +{ + struct ble_ll_cs_sm *cssm = g_ble_ll_cs_sm_current; + uint32_t cputime; + uint32_t rem_us; + uint32_t rem_ns; + uint32_t anchor_usecs; + + /* Packet type was verified in isr_start */ + + assert(cssm != NULL); + + ble_phy_get_rxend_time(&cputime, &rem_us, &rem_ns); + anchor_usecs = ble_ll_tmr_t2u(cputime) + rem_us; + + cssm->anchor_usecs = anchor_usecs; + if (cssm->step_mode != BLE_LL_CS_MODE0 && + cssm->active_config->role == BLE_LL_CS_ROLE_INITIATOR) { + cssm->step_result.time_of_arrival = cssm->anchor_usecs; + } + + cssm->step_result.packet_rssi = rxhdr->rxinfo.rssi; + cssm->step_result.packet_quality = + ble_ll_cs_sync_calc_seq_quality(cssm, rxbuf, rxhdr); + cssm->step_result.packet_nadm = + ble_ll_cs_sync_calc_nadm(cssm, rxbuf, rxhdr); + + if (g_ble_ll_cs_local_cap.sounding_pct_estimate && + cssm->active_config->rtt_type != BLE_LL_CS_RTT_AA_ONLY) { + /* TODO: Read PCT estimates from sounding sequence. + * For now set "Phase Correction Term is not available". + */ + cssm->step_result.packet_pct1 = 0xFFFFFFFF; + cssm->step_result.packet_pct2 = 0xFFFFFFFF; + } + + ble_ll_cs_proc_schedule_next_tx_or_rx(cssm); + + ble_phy_disable(); + ble_phy_cs_sync_mode_set(0); + ble_ll_state_set(BLE_LL_STATE_STANDBY); + + return 1; +} + +/** + * Process a received PDU. + * + * Context: Link Layer task. + * + * @param pdu_type + * @param rxbuf + */ +void +ble_ll_cs_sync_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *rxhdr) +{ +} + +/** + * Called when the wait for response timer expires while in the sync state. + * + * Context: Interrupt. + */ +void +ble_ll_cs_sync_wfr_timer_exp(void) +{ + struct ble_ll_cs_sm *cssm = g_ble_ll_cs_sm_current; + + assert(cssm != NULL); + ble_ll_cs_proc_set_now_as_anchor_point(cssm); + + ble_ll_cs_proc_sync_lost(cssm); +} + +#endif /* BLE_LL_CHANNEL_SOUNDING */ diff --git a/nimble/controller/src/ble_ll_cs_tone.c b/nimble/controller/src/ble_ll_cs_tone.c new file mode 100644 index 0000000000..d2dadccbed --- /dev/null +++ b/nimble/controller/src/ble_ll_cs_tone.c @@ -0,0 +1,140 @@ +/* + * 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 +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +#include +#include "controller/ble_ll.h" +#include "controller/ble_ll_sched.h" +#include "controller/ble_ll_tmr.h" +#include "ble_ll_cs_priv.h" + +extern struct ble_ll_cs_sm *g_ble_ll_cs_sm_current; + +/** + * Called when a CS tone has been transmitted. + * + * Context: interrupt + */ +void +ble_ll_cs_tone_tx_end_cb(void *arg) +{ + struct ble_ll_cs_sm *cssm = g_ble_ll_cs_sm_current; + + assert(cssm != NULL); + ble_ll_cs_proc_set_now_as_anchor_point(cssm); + ble_ll_cs_proc_schedule_next_tx_or_rx(cssm); +} + +int +ble_ll_cs_tone_tx_start(struct ble_ll_cs_sm *cssm) +{ + /* TODO: Start TX of CS tones */ + + cssm->anchor_usecs += cssm->duration_usecs; + + return ble_ll_cs_proc_schedule_next_tx_or_rx(cssm); +} + +int +ble_ll_cs_tone_rx_start(struct ble_ll_cs_sm *cssm) +{ + /* TODO: Start RX of CS tones */ + + cssm->anchor_usecs += cssm->duration_usecs; + + return ble_ll_cs_proc_schedule_next_tx_or_rx(cssm); +} + +/** + * Called when a receive PDU has started. + * Check if the frame is the next expected CS_SYNC packet. + * + * Context: interrupt + * + * @return int + * < 0: A frame we dont want to receive. + * = 0: Continue to receive frame. Dont go from rx to tx + */ +int +ble_ll_cs_tone_rx_isr_start(void) +{ + return 0; +} + +/** + * Called when received a complete CS_TONE packet. + * + * Context: Interrupt + * + * @param rxpdu + * @param rxhdr + * + * @return int + * < 0: Disable the phy after reception. + * == 0: Success. Do not disable the PHY. + * > 0: Do not disable PHY as that has already been done. + */ +int +ble_ll_cs_tone_rx_isr_end(void) +{ + uint8_t i; + struct ble_ll_cs_sm *cssm = g_ble_ll_cs_sm_current; + + assert(cssm != NULL); + ble_ll_cs_proc_set_now_as_anchor_point(cssm); + + ble_phy_disable(); + ble_ll_state_set(BLE_LL_STATE_STANDBY); + + if (cssm->step_mode == BLE_LL_CS_MODE0) { + /* TODO: Read measured frequency offset. + * For now set "Frequency offset is not available". + */ + cssm->step_result.measured_freq_offset = 0xC000; + } else if (cssm->step_mode == BLE_LL_CS_MODE2 || + cssm->step_mode == BLE_LL_CS_MODE3) { + for (i = 0; i < cssm->n_ap; ++i) { + cssm->step_result.tone_pct[i] = 0; + cssm->step_result.tone_quality_ind[i] = 0; + } + } + + ble_ll_cs_proc_schedule_next_tx_or_rx(cssm); + + return 1; +} + +/** + * Called when the wait for response timer expires while in the sync state. + * + * Context: Interrupt. + */ +void +ble_ll_cs_tone_wfr_timer_exp(void) +{ + struct ble_ll_cs_sm *cssm = g_ble_ll_cs_sm_current; + + assert(cssm != NULL); + ble_ll_cs_proc_set_now_as_anchor_point(cssm); + + ble_ll_cs_proc_sync_lost(cssm); +} + +#endif /* BLE_LL_CHANNEL_SOUNDING */ diff --git a/nimble/controller/src/ble_ll_ctrl.c b/nimble/controller/src/ble_ll_ctrl.c index da12a5f313..53139d2d2f 100644 --- a/nimble/controller/src/ble_ll_ctrl.c +++ b/nimble/controller/src/ble_ll_ctrl.c @@ -49,6 +49,10 @@ #include "console/console.h" #endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +#include "controller/ble_ll_cs.h" +#endif + /* * XXX: * 1) Do I need to keep track of which procedures have already been done? @@ -128,6 +132,23 @@ const uint8_t g_ble_ll_ctrl_pkt_lengths[BLE_LL_CTRL_OPCODES] = BLE_LL_CTRL_SUBRATE_IND_LEN, BLE_LL_CTRL_CHAN_REPORTING_IND_LEN, BLE_LL_CTRL_CHAN_STATUS_IND_LEN, + BLE_LL_CTRL_PERIODIC_SYNC_WR_IND_LEN, + BLE_LL_CTRL_FEATURE_EXT_REQ_LEN, + BLE_LL_CTRL_FEATURE_EXT_RSP_LEN, + BLE_LL_CTRL_CS_SEC_RSP_LEN, + BLE_LL_CTRL_CS_CAPABILITIES_REQ_LEN, + BLE_LL_CTRL_CS_CAPABILITIES_RSP_LEN, + BLE_LL_CTRL_CS_CONFIG_REQ_LEN, + BLE_LL_CTRL_CS_CONFIG_RSP_LEN, + BLE_LL_CTRL_CS_REQ_LEN, + BLE_LL_CTRL_CS_RSP_LEN, + BLE_LL_CTRL_CS_IND_LEN, + BLE_LL_CTRL_CS_TERMINATE_REQ_LEN, + BLE_LL_CTRL_CS_FAE_REQ_LEN, + BLE_LL_CTRL_CS_FAE_RSP_LEN, + BLE_LL_CTRL_CS_CHANNEL_MAP_IND_LEN, + BLE_LL_CTRL_CS_SEC_REQ_LEN, + BLE_LL_CTRL_CS_TERMINATE_RSP_LEN }; /** @@ -2017,6 +2038,26 @@ ble_ll_ctrl_rx_reject_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr, ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_SUBRATE_UPDATE); break; #endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + case BLE_LL_CTRL_PROC_CS_CAP_XCHG: + ble_ll_cs_rx_capabilities_req_rejected(connsm, ble_error); + break; + case BLE_LL_CTRL_PROC_CS_FAE_REQ: + ble_ll_cs_rx_fae_req_rejected(connsm, ble_error); + break; + case BLE_LL_CTRL_PROC_CS_CONF: + ble_ll_cs_rx_config_req_rejected(connsm, ble_error); + break; + case BLE_LL_CTRL_PROC_CS_SEC_START: + ble_ll_cs_rx_security_req_rejected(connsm, ble_error); + break; + case BLE_LL_CTRL_PROC_CS_START: + ble_ll_cs_rx_cs_start_rejected(connsm, ble_error); + break; + case BLE_LL_CTRL_PROC_CS_TERMINATE: + ble_ll_cs_rx_cs_terminate_req_rejected(connsm, ble_error); + break; +#endif default: break; @@ -2550,6 +2591,34 @@ ble_ll_ctrl_proc_init(struct ble_ll_conn_sm *connsm, int ctrl_proc, void *data) &connsm->subrate_trans); break; #endif +#endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + case BLE_LL_CTRL_PROC_CS_CAP_XCHG: + opcode = BLE_LL_CTRL_CS_CAPABILITIES_REQ; + ble_ll_cs_capabilities_pdu_make(connsm, ctrdata); + break; + case BLE_LL_CTRL_PROC_CS_FAE_REQ: + opcode = BLE_LL_CTRL_CS_FAE_REQ; + /* No command parameters in LL_CS_FAE_REQ PDU */ + break; + case BLE_LL_CTRL_PROC_CS_CONF: + opcode = BLE_LL_CTRL_CS_CONFIG_REQ; + ble_ll_cs_config_req_make(connsm, ctrdata); + break; +#if MYNEWT_VAL(BLE_LL_ROLE_CENTRAL) + case BLE_LL_CTRL_PROC_CS_SEC_START: + opcode = BLE_LL_CTRL_CS_SEC_REQ; + ble_ll_cs_security_req_make(connsm, ctrdata); + break; +#endif + case BLE_LL_CTRL_PROC_CS_START: + opcode = BLE_LL_CTRL_CS_REQ; + ble_ll_cs_start_req_make(connsm, ctrdata); + break; + case BLE_LL_CTRL_PROC_CS_TERMINATE: + opcode = BLE_LL_CTRL_CS_TERMINATE_REQ; + ble_ll_cs_terminate_req_make(connsm, ctrdata); + break; #endif default: BLE_LL_ASSERT(0); @@ -3020,6 +3089,53 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om) case BLE_LL_CTRL_SUBRATE_IND: rsp_opcode = ble_ll_ctrl_rx_subrate_ind(connsm, dptr, rspdata); break; +#endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + case BLE_LL_CTRL_CS_CAPABILITIES_REQ: + rsp_opcode = ble_ll_cs_rx_capabilities_req(connsm, dptr, rspdata); + break; + case BLE_LL_CTRL_CS_CAPABILITIES_RSP: + ble_ll_cs_rx_capabilities_rsp(connsm, dptr); + break; + case BLE_LL_CTRL_CS_FAE_REQ: + rsp_opcode = ble_ll_cs_rx_fae_req(connsm, om); + break; + case BLE_LL_CTRL_CS_FAE_RSP: + ble_ll_cs_rx_fae_rsp(connsm, dptr); + break; + case BLE_LL_CTRL_CS_CONFIG_REQ: + rsp_opcode = ble_ll_cs_rx_config_req(connsm, dptr, rspdata); + break; + case BLE_LL_CTRL_CS_CONFIG_RSP: + ble_ll_cs_rx_config_rsp(connsm, dptr); + break; +#if MYNEWT_VAL(BLE_LL_ROLE_PERIPHERAL) + case BLE_LL_CTRL_CS_SEC_REQ: + rsp_opcode = ble_ll_cs_rx_security_req(connsm, dptr, rspdata); + break; +#endif +#if MYNEWT_VAL(BLE_LL_ROLE_CENTRAL) + case BLE_LL_CTRL_CS_SEC_RSP: + ble_ll_cs_rx_security_rsp(connsm, dptr); + break; +#endif + case BLE_LL_CTRL_CS_REQ: + rsp_opcode = ble_ll_cs_rx_cs_start_req(connsm, dptr, rspdata); + break; + case BLE_LL_CTRL_CS_RSP: + rsp_opcode = ble_ll_cs_rx_cs_start_rsp(connsm, dptr, rspdata); + break; +#if MYNEWT_VAL(BLE_LL_ROLE_PERIPHERAL) + case BLE_LL_CTRL_CS_IND: + rsp_opcode = ble_ll_cs_rx_cs_start_ind(connsm, dptr, rspdata); + break; +#endif + case BLE_LL_CTRL_CS_TERMINATE_REQ: + rsp_opcode = ble_ll_cs_rx_cs_terminate_req(connsm, dptr, rspdata); + break; + case BLE_LL_CTRL_CS_TERMINATE_RSP: + ble_ll_cs_rx_cs_terminate_rsp(connsm, dptr); + break; #endif default: /* Nothing to do here */ diff --git a/nimble/controller/src/ble_ll_sched.c b/nimble/controller/src/ble_ll_sched.c index f2987dfdf0..6824fedb96 100644 --- a/nimble/controller/src/ble_ll_sched.c +++ b/nimble/controller/src/ble_ll_sched.c @@ -38,6 +38,7 @@ #endif #include "ble_ll_priv.h" #include "ble_ll_conn_priv.h" +#include "ble_ll_cs_priv.h" #define BLE_LL_SCHED_MAX_DELAY_ANY (0x7fffffff) @@ -183,9 +184,14 @@ ble_ll_sched_preempt(struct ble_ll_sched_item *sch, ble_ll_ext_sched_removed(entry); break; #endif - default: - BLE_LL_ASSERT(0); - break; +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + case BLE_LL_SCHED_TYPE_CS: + ble_ll_cs_proc_rm_from_sched(entry->cb_arg); + break; +#endif + default: + BLE_LL_ASSERT(0); + break; } entry = next; @@ -866,6 +872,25 @@ ble_ll_sched_iso_big(struct ble_ll_sched_item *sch, int first, int fixed) } #endif /* BLE_LL_ISO_BROADCASTER */ +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +int +ble_ll_sched_cs_proc(struct ble_ll_sched_item *sch) +{ + os_sr_t sr; + int rc; + + OS_ENTER_CRITICAL(sr); + + rc = ble_ll_sched_insert(sch, 0, preempt_none); + + OS_EXIT_CRITICAL(sr); + + ble_ll_sched_restart(); + + return rc; +} +#endif /* BLE_LL_CHANNEL_SOUNDING */ + /** * Remove a schedule element * @@ -1015,6 +1040,11 @@ ble_ll_sched_execute_item(struct ble_ll_sched_item *sch) case BLE_LL_STATE_EXTERNAL: ble_ll_ext_halt(); break; +#endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + case BLE_LL_STATE_CS: + ble_ll_cs_proc_halt(); + break; #endif default: BLE_LL_ASSERT(0); diff --git a/nimble/controller/syscfg.yml b/nimble/controller/syscfg.yml index e77a1de44b..bcbfdb78c6 100644 --- a/nimble/controller/syscfg.yml +++ b/nimble/controller/syscfg.yml @@ -532,6 +532,10 @@ syscfg.defs: - (BLE_VERSION >= 54) if 1 value: MYNEWT_VAL(BLE_CHANNEL_SOUNDING) state: experimental + BLE_LL_CHANNEL_SOUNDING_SUBEVENT_EV_MAX_CNT: + description: > + The maximum number of queued HCI Subevent Results (Continue) events. + value: 5 BLE_LL_SYSINIT_STAGE: description: > diff --git a/nimble/controller/test/src/ble_ll_cs_drbg_test.c b/nimble/controller/test/src/ble_ll_cs_drbg_test.c new file mode 100644 index 0000000000..1edb920713 --- /dev/null +++ b/nimble/controller/test/src/ble_ll_cs_drbg_test.c @@ -0,0 +1,369 @@ +/* + * 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 +#include +#include +#include "ble_ll_cs_drbg_priv.h" + +extern uint8_t g_ble_ll_cs_chan_indices[72]; + +static void +ble_ll_cs_drbg_e_test(void) +{ + uint8_t key[16] = {0}; + uint8_t data[16] = {0}; + uint8_t out[16] = {0}; + uint8_t expected_out[16] = {0}; + + /* Sample data from BT spec Vol 6, Part C, 1.1 Encrypt Command + * Swap because the copy-pasted strings are in leftmost (MSO) to rightmost + * (LSO) orientation. + */ + swap_buf(key, (uint8_t [16]) { + 0x4C, 0x68, 0x38, 0x41, 0x39, 0xF5, 0x74, 0xD8, + 0x36, 0xBC, 0xF3, 0x4E, 0x9D, 0xFB, 0x01, 0xBF + }, 16); + + swap_buf(data, (uint8_t [16]) { + 0x02, 0x13, 0x24, 0x35, 0x46, 0x57, 0x68, 0x79, + 0xac, 0xbd, 0xce, 0xdf, 0xe0, 0xf1, 0x02, 0x13 + }, 16); + + swap_buf(expected_out, (uint8_t [16]) { + 0x99, 0xad, 0x1b, 0x52, 0x26, 0xa3, 0x7e, 0x3e, + 0x05, 0x8e, 0x3b, 0x8e, 0x27, 0xc2, 0xc6, 0x66 + }, 16); + + ble_ll_cs_drbg_e(key, data, out); + TEST_ASSERT(memcmp(out, expected_out, sizeof(out)) == 0); +} + +static void +ble_ll_cs_drbg_f7_test(void) +{ + uint8_t v_s[80]; + uint8_t k[16]; + uint8_t k2[16] = {0}; + uint8_t x[16] = {0}; + uint8_t expected_k2[16] = {0}; + uint8_t expected_x[16] = {0}; + + /* Sample data from BT spec Vol 6, Part C, 7. Deterministic + * random bit generator sample data. + * Swap because the copy-pasted strings are in leftmost (MSO) + * to rightmost (LSO) orientation. + */ + swap_buf(k, (uint8_t [16]) { + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, + 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F + }, 16); + + swap_buf(v_s, (uint8_t [80]) { + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x28, 0x00, 0x00, 0x00, 0x20, + 0xE1, 0x0B, 0xC2, 0x8A, 0x0B, 0xFD, 0xDF, 0xE9, + 0x3E, 0x7F, 0x51, 0x86, 0xE0, 0xCA, 0x0B, 0x3B, + 0x9F, 0xF4, 0x77, 0xC1, 0x86, 0x73, 0x84, 0x0D, + 0xC9, 0x80, 0xDE, 0xDF, 0x98, 0x82, 0xED, 0x44, + 0x64, 0xA6, 0x74, 0x96, 0x78, 0x68, 0xF1, 0x43, + 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 + }, 80); + + swap_buf(expected_k2, (uint8_t [16]) { + 0x8B, 0x2B, 0x06, 0xDC, 0x52, 0x2D, 0x3E, 0x0A, + 0xF0, 0xA5, 0x0C, 0xAF, 0x48, 0x10, 0xE0, 0x35 + }, 16); + + TEST_ASSERT(ble_ll_cs_drbg_f7(k, v_s, sizeof(v_s), k2) == 0); + TEST_ASSERT(memcmp(k2, expected_k2, sizeof(k2)) == 0); + + v_s[76] = 0x01; + swap_buf(expected_x, (uint8_t [16]) { + 0xA3, 0x4F, 0xBE, 0x57, 0xF8, 0xF9, 0x7E, 0x34, + 0x9D, 0x15, 0xA3, 0x76, 0x79, 0x60, 0x74, 0x64 + }, 16); + + TEST_ASSERT(ble_ll_cs_drbg_f7(k, v_s, sizeof(v_s), x) == 0); + TEST_ASSERT(memcmp(x, expected_x, sizeof(x)) == 0); +} + +static void +ble_ll_cs_drbg_f8_test(void) +{ + uint8_t input_bit_string[40] = {0}; + uint8_t expected_sm[32] = {0}; + uint8_t sm[32] = {0}; + + /* Sample data from BT spec Vol 6, Part C, 7. Deterministic + * random bit generator sample data. + * Swap because the copy-pasted strings are in leftmost (MSO) + * to rightmost (LSO) orientation. + */ + + /* 320-bit input bit string created from concatenated vectors + * CS_IV || CS_IN || CS_PV + */ + swap_buf(input_bit_string, (uint8_t [40]) { + 0xE1, 0x0B, 0xC2, 0x8A, 0x0B, 0xFD, 0xDF, 0xE9, + 0x3E, 0x7F, 0x51, 0x86, 0xE0, 0xCA, 0x0B, 0x3B, + 0x9F, 0xF4, 0x77, 0xC1, 0x86, 0x73, 0x84, 0x0D, + 0xC9, 0x80, 0xDE, 0xDF, 0x98, 0x82, 0xED, 0x44, + 0x64, 0xA6, 0x74, 0x96, 0x78, 0x68, 0xF1, 0x43 + }, 40); + + swap_buf(expected_sm, (uint8_t [32]) { + 0xB6, 0x02, 0xB1, 0xB2, 0x8C, 0x6F, 0x0A, 0x3D, + 0xDA, 0xE6, 0x37, 0xB4, 0x84, 0x25, 0x08, 0x7D, + 0xDC, 0x18, 0x8C, 0x89, 0xA1, 0xB0, 0xCD, 0xFD, + 0xA1, 0xE8, 0xFC, 0x66, 0xC9, 0x99, 0x97, 0x50 + }, 32); + + TEST_ASSERT(ble_ll_cs_drbg_f8(input_bit_string, sm) == 0); + TEST_ASSERT(memcmp(sm, expected_sm, sizeof(sm)) == 0); +} + +static void +ble_ll_cs_drbg_f9_test(void) +{ + uint8_t sm[32] = {0}; + uint8_t k[16] = {0}; + uint8_t v[16] = {0}; + uint8_t expected_k[16] = {0}; + uint8_t expected_v[16] = {0}; + + /* First call to f9 from instantiation function h9, + * K and V vectors filled with zeros. + * + * Sample data from BT spec Vol 6, Part C, 7. Deterministic + * random bit generator sample data. + * Swap because the copy-pasted strings are in leftmost (MSO) + * to rightmost (LSO) orientation. + */ + + swap_buf(sm, (uint8_t [32]) { + 0xB6, 0x02, 0xB1, 0xB2, 0x8C, 0x6F, 0x0A, 0x3D, + 0xDA, 0xE6, 0x37, 0xB4, 0x84, 0x25, 0x08, 0x7D, + 0xDC, 0x18, 0x8C, 0x89, 0xA1, 0xB0, 0xCD, 0xFD, + 0xA1, 0xE8, 0xFC, 0x66, 0xC9, 0x99, 0x97, 0x50 + }, 32); + + swap_buf(expected_k, (uint8_t [16]) { + 0xEE, 0xE0, 0x4D, 0x7C, 0x76, 0x11, 0x3A, 0x5C, + 0xEC, 0x99, 0x2A, 0xE3, 0x20, 0xC2, 0x4D, 0x27 + }, 16); + + swap_buf(expected_v, (uint8_t [16]) { + 0xDF, 0x90, 0x56, 0x47, 0xC1, 0x06, 0x6E, 0x6F, + 0x52, 0xC0, 0x3E, 0xDF, 0xB8, 0x2B, 0x69, 0x28 + }, 16); + + TEST_ASSERT(ble_ll_cs_drbg_f9(sm, k, v) == 0); + TEST_ASSERT(memcmp(k, expected_k, sizeof(k)) == 0); + TEST_ASSERT(memcmp(v, expected_v, sizeof(v)) == 0); +} + +static void +cs_drbg_init(struct ble_ll_cs_drbg_ctx *ctx) +{ + memset(ctx, 0, sizeof(*ctx)); + + /* CS_IV = CS_IV_P || CS_IV_C */ + swap_buf(ctx->iv, (uint8_t [16]) { + 0xE1, 0x0B, 0xC2, 0x8A, 0x0B, 0xFD, 0xDF, 0xE9, + 0x3E, 0x7F, 0x51, 0x86, 0xE0, 0xCA, 0x0B, 0x3B + }, 16); + + /* CS_IN = CS_IN_P || CS_IN_C */ + swap_buf(ctx->in, (uint8_t [8]) { + 0x9F, 0xF4, 0x77, 0xC1, 0x86, 0x73, 0x84, 0x0D + }, 8); + + /* CS_PV = CS_PV_P || CS_PV_C */ + swap_buf(ctx->pv, (uint8_t [16]) { + 0xC9, 0x80, 0xDE, 0xDF, 0x98, 0x82, 0xED, 0x44, + 0x64, 0xA6, 0x74, 0x96, 0x78, 0x68, 0xF1, 0x43 + }, 16); + + ble_ll_cs_drbg_init(ctx); +} + +static void +ble_ll_cs_drbg_rand_test(void) +{ + struct ble_ll_cs_drbg_ctx ctx; + uint8_t output[20] = {0}; + uint8_t expected_output[20] = {0}; + + /* Test if subsequent drgb generator calls returns expected bit sequences. */ + + cs_drbg_init(&ctx); + + /* First round - request full 128-bit batch */ + swap_buf(expected_output, (uint8_t [16]) { + 0x79, 0x74, 0x1F, 0xD1, 0x8F, 0x57, 0x7B, 0x45, + 0xD0, 0x9A, 0x66, 0x5A, 0x7F, 0x1F, 0x28, 0x58 + }, 16); + + TEST_ASSERT(ble_ll_cs_drbg_rand( + &ctx, 0x00, BLE_LL_CS_DRBG_HOP_CHAN_NON_MODE0, + output, 16) == 0); + TEST_ASSERT(memcmp(output, expected_output, 16) == 0); +} + +static void +ble_ll_cs_drbg_chan_selection_3b_test(void) +{ + struct ble_ll_cs_drbg_ctx ctx; + uint8_t filtered_channels[72] = {0}; + uint8_t shuffled_channels[72] = {0}; + uint8_t expected_shuffled_channels[19] = {0}; + + cs_drbg_init(&ctx); + + memcpy(filtered_channels, (uint8_t [19]) { + 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, + 13, 14, 15, 16, 17, 18, 19, 20 + }, 19); + + memcpy(expected_shuffled_channels, (uint8_t [19]) { + 11, 7, 14, 18, 9, 19, 10, 8, 5, 2, 4, 15, 16, 13, 12, 6, 17, 20, 3 + }, 19); + assert(ble_ll_cs_drbg_shuffle_cr1(&ctx, 0x00, BLE_LL_CS_DRBG_HOP_CHAN_MODE0, + filtered_channels, shuffled_channels, 19) == 0); + assert(memcmp(shuffled_channels, expected_shuffled_channels, 19) == 0); + + memcpy(expected_shuffled_channels, (uint8_t [19]) { + 6, 12, 5, 10, 3, 2, 18, 17, 16, 8, 11, 7, 19, 4, 13, 20, 9, 15, 14 + }, 19); + assert(ble_ll_cs_drbg_shuffle_cr1(&ctx, 0x03, BLE_LL_CS_DRBG_HOP_CHAN_NON_MODE0, + filtered_channels, shuffled_channels, 19) == 0); + assert(memcmp(shuffled_channels, expected_shuffled_channels, 19) == 0); +} + +static void +ble_ll_cs_drbg_generate_aa_test(void) +{ + struct ble_ll_cs_drbg_ctx ctx; + uint32_t initiator_aa; + uint32_t reflector_aa; + uint32_t expected_initiator_aa; + uint32_t expected_reflector_aa; + + cs_drbg_init(&ctx); + + /* Step 0 */ + assert(ble_ll_cs_drbg_generate_aa(&ctx, 0, &initiator_aa, &reflector_aa) == 0); + expected_initiator_aa = get_be32((uint8_t [4]) {0x6C, 0x37, 0x6A, 0xB8}); + expected_reflector_aa = get_be32((uint8_t [4]) {0xF0, 0x79, 0xBC, 0x3A}); + assert(initiator_aa == expected_initiator_aa); + assert(reflector_aa == expected_reflector_aa); + + /* Step 1 */ + assert(ble_ll_cs_drbg_generate_aa(&ctx, 1, &initiator_aa, &reflector_aa) == 0); + expected_initiator_aa = get_be32((uint8_t [4]) {0x01, 0x1C, 0xAE, 0x4E}); + expected_reflector_aa = get_be32((uint8_t [4]) {0xD0, 0x6A, 0xCD, 0xDA}); + assert(initiator_aa == expected_initiator_aa); + assert(reflector_aa == expected_reflector_aa); + + /* Step 2 */ + assert(ble_ll_cs_drbg_generate_aa(&ctx, 2, &initiator_aa, &reflector_aa) == 0); + expected_initiator_aa = get_be32((uint8_t [4]) {0x64, 0x06, 0x12, 0x14}); + expected_reflector_aa = get_be32((uint8_t [4]) {0x28, 0x94, 0x2F, 0x38}); + assert(initiator_aa == expected_initiator_aa); + assert(reflector_aa == expected_reflector_aa); + + /* Step 14 */ + assert(ble_ll_cs_drbg_generate_aa(&ctx, 14, &initiator_aa, &reflector_aa) == 0); + expected_initiator_aa = get_be32((uint8_t [4]) {0xF7, 0x21, 0x97, 0x86}); + expected_reflector_aa = get_be32((uint8_t [4]) {0x57, 0x17, 0x64, 0x70}); + assert(initiator_aa == expected_initiator_aa); + assert(reflector_aa == expected_reflector_aa); +} + +static void +ble_ll_cs_drbg_rand_marker_position_test(void) +{ + uint8_t position1; + uint8_t position2; + struct ble_ll_cs_drbg_ctx ctx; + + cs_drbg_init(&ctx); + + /* Step 9 */ + assert(ble_ll_cs_drbg_rand_marker_position( + &ctx, 9, BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE, + &position1, &position2) == 0); + assert(position1 == 12 && position2 == 0xFF); + + assert(ble_ll_cs_drbg_rand_marker_position( + &ctx, 9, BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE, + &position1, &position2) == 0); + assert(position1 == 4 && position2 == 0xFF); + + /* Step 14 */ + assert(ble_ll_cs_drbg_rand_marker_position( + &ctx, 14, BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE, + &position1, &position2) == 0); + assert(position1 == 8 && position2 == 0xFF); + + assert(ble_ll_cs_drbg_rand_marker_position( + &ctx, 14, BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE, + &position1, &position2) == 0); + assert(position1 == 11 && position2 == 0xFF); +} + +static void +ble_ll_cs_drbg_rand_marker_selection_test(void) +{ + uint8_t marker_selection; + struct ble_ll_cs_drbg_ctx ctx; + + cs_drbg_init(&ctx); + + /* Step 9 */ + assert(ble_ll_cs_drbg_rand_marker_selection(&ctx, 0x09, &marker_selection) == 0); + assert(marker_selection == 0x00); + + assert(ble_ll_cs_drbg_rand_marker_selection(&ctx, 0x09, &marker_selection) == 0); + assert(marker_selection == 0x80); + + memset(ctx.t_cache, 0, sizeof(ctx.t_cache)); + + /* Step 14 */ + assert(ble_ll_cs_drbg_rand_marker_selection(&ctx, 14, &marker_selection) == 0); + assert(marker_selection == 0x80); + + assert(ble_ll_cs_drbg_rand_marker_selection(&ctx, 14, &marker_selection) == 0); + assert(marker_selection == 0x80); +} + +TEST_SUITE(ble_ll_cs_drbg_test_suite) { + ble_ll_cs_drbg_e_test(); + ble_ll_cs_drbg_f7_test(); + ble_ll_cs_drbg_f8_test(); + ble_ll_cs_drbg_f9_test(); + ble_ll_cs_drbg_rand_test(); + ble_ll_cs_drbg_chan_selection_3b_test(); + ble_ll_cs_drbg_generate_aa_test(); + ble_ll_cs_drbg_rand_marker_position_test(); + ble_ll_cs_drbg_rand_marker_selection_test(); +} diff --git a/nimble/controller/test/src/ble_ll_test.c b/nimble/controller/test/src/ble_ll_test.c index 2b36cb1f3c..c90e9f0d48 100644 --- a/nimble/controller/test/src/ble_ll_test.c +++ b/nimble/controller/test/src/ble_ll_test.c @@ -25,6 +25,7 @@ TEST_SUITE_DECL(ble_ll_aa_test_suite); TEST_SUITE_DECL(ble_ll_crypto_test_suite); TEST_SUITE_DECL(ble_ll_csa2_test_suite); +TEST_SUITE_DECL(ble_ll_cs_drbg_test_suite); int main(int argc, char **argv) @@ -32,6 +33,7 @@ main(int argc, char **argv) ble_ll_aa_test_suite(); ble_ll_crypto_test_suite(); ble_ll_csa2_test_suite(); + ble_ll_cs_drbg_test_suite(); return tu_any_failed; } diff --git a/nimble/controller/test/syscfg.yml b/nimble/controller/test/syscfg.yml index 6edad438bb..30d1a1bef9 100644 --- a/nimble/controller/test/syscfg.yml +++ b/nimble/controller/test/syscfg.yml @@ -23,3 +23,7 @@ syscfg.vals: MCU_TIMER_POLLER_PRIO: 1 MCU_UART_POLLER_PRIO: 2 NATIVE_SOCKETS_PRIO: 3 + MBEDTLS_AES_C: 1 + BLE_VERSION: 54 + BLE_CHANNEL_SOUNDING: 1 + BLE_LL_CHANNEL_SOUNDING: 1 diff --git a/nimble/drivers/nrf5x/src/ble_phy.c b/nimble/drivers/nrf5x/src/ble_phy.c index f556f65024..cfdc0a30c8 100644 --- a/nimble/drivers/nrf5x/src/ble_phy.c +++ b/nimble/drivers/nrf5x/src/ble_phy.c @@ -162,6 +162,11 @@ struct ble_phy_obj uint16_t txtx_time_us; uint8_t txtx_time_anchor; + uint8_t phy_cs_timer; + uint32_t txend_time_us; + uint32_t txend_time_ns; + uint32_t rxend_time_us; + uint32_t rxend_time_ns; }; static struct ble_phy_obj g_ble_phy_data; @@ -204,6 +209,13 @@ static const uint8_t g_ble_phy_t_txdelay[BLE_PHY_NUM_MODE] = { [BLE_PHY_MODE_1M] = 1, [BLE_PHY_MODE_2M] = 1, }; +/* delay between EVENTS_ADDRESS and txd access address */ +static const uint8_t g_ble_phy_t_txaddrdelay[BLE_PHY_NUM_MODE] = { + [BLE_PHY_MODE_1M] = 1, + [BLE_PHY_MODE_2M] = 1, + [BLE_PHY_MODE_CODED_125KBPS] = 1, + [BLE_PHY_MODE_CODED_500KBPS] = 1, +}; /* delay between EVENTS_END and end of txd packet */ static const uint8_t g_ble_phy_t_txenddelay[BLE_PHY_NUM_MODE] = { [BLE_PHY_MODE_1M] = 1, @@ -1069,6 +1081,12 @@ ble_phy_tx_end_isr(void) #endif transition = g_ble_phy_data.phy_transition; + if (g_ble_phy_data.phy_cs_timer == BLE_PHY_CS_TIMER_START) { + phy_ppi_radio_address_to_timer3_start_disable(); + } else if (g_ble_phy_data.phy_cs_timer == BLE_PHY_CS_TIMER_CAPTURE) { + phy_ppi_radio_address_to_timer3_capture0_disable(); + } + if (g_ble_phy_data.txend_cb) { g_ble_phy_data.txend_cb(g_ble_phy_data.txend_arg); } @@ -1254,6 +1272,8 @@ ble_phy_rx_end_isr(void) ble_phy_mode_apply(g_ble_phy_data.phy_tx_phy_mode); #endif + g_ble_phy_data.rxend_time_us = NRF_TIMER0->CC[2] - g_ble_phy_t_rxenddelay[ble_hdr->rxinfo.phy_mode]; + /* * Let's schedule TX now and we will just cancel it after processing RXed * packet if we don't need TX. @@ -1437,6 +1457,12 @@ ble_phy_rx_start_isr(void) } #endif + if (g_ble_phy_data.phy_cs_timer == BLE_PHY_CS_TIMER_START) { + phy_ppi_radio_address_to_timer3_start_disable(); + } else if (g_ble_phy_data.phy_cs_timer == BLE_PHY_CS_TIMER_CAPTURE) { + phy_ppi_radio_address_to_timer3_capture0_disable(); + } + /* Call Link Layer receive start function */ rc = ble_ll_rx_start(dptr + 3, g_ble_phy_data.phy_chan, @@ -2348,3 +2374,77 @@ ble_phy_tifs_txtx_set(uint16_t usecs, uint8_t anchor) g_ble_phy_data.txtx_time_us = usecs; g_ble_phy_data.txtx_time_anchor = anchor; } + +void +ble_phy_get_txend_time(uint32_t *cputime, uint32_t *rem_us, uint32_t *rem_ns) +{ + *cputime = g_ble_phy_data.phy_start_cputime; + *rem_us = g_ble_phy_data.txend_time_us; + *rem_ns = g_ble_phy_data.txend_time_ns; +} + +void +ble_phy_get_rxend_time(uint32_t *cputime, uint32_t *rem_us, uint32_t *rem_ns) +{ + *cputime = g_ble_phy_data.phy_start_cputime; + *rem_us = g_ble_phy_data.rxend_time_us; + *rem_ns = g_ble_phy_data.rxend_time_ns; +} + +uint32_t +ble_phy_cs_timer_read(void) +{ + return NRF_TIMER3->CC[0]; +} + +void +ble_phy_cs_sync_mode_set(uint8_t mode) +{ +#if !BABBLESIM + if (mode == 0) { + /* Configure back the registers */ + NRF_RADIO->CRCCNF = (RADIO_CRCCNF_SKIPADDR_Skip << RADIO_CRCCNF_SKIPADDR_Pos) | RADIO_CRCCNF_LEN_Three; + NRF_RADIO->PCNF0 = NRF_PCNF0; + } else { + /* CS SYNC packet has no PDU or CRC */ + NRF_RADIO->CRCCNF = 0; + NRF_RADIO->PCNF0 = NRF_LFLEN_BITS << RADIO_PCNF0_LFLEN_Pos; + } +#endif +} + +int +ble_phy_tx_cs_sync(ble_phy_tx_cs_sync_cb_t pktcb, void *pktcb_arg) +{ + int rc; + + /* Temporary, for babblesim testing */ + rc = ble_phy_tx(pktcb, pktcb_arg, BLE_PHY_TRANSITION_NONE); + /* TODO: Adjustments for CS SYNC packet: + * - Turn off CRC + * - set 4 bits in S1 + */ + return rc; +} + +int +ble_phy_cs_sync_configure(uint8_t chan, uint32_t access_addr, uint8_t cs_timer) +{ + int rc; + + ble_phy_setchan(chan, access_addr, 0); + + if (cs_timer == BLE_PHY_CS_TIMER_START) { + /* Measure time between ADDRESS events of TX and RX CS_SYNCs */ + nrf_timer_task_trigger(NRF_TIMER3, NRF_TIMER_TASK_CLEAR); + phy_ppi_radio_address_to_timer3_start_enable(); + } else if (cs_timer == BLE_PHY_CS_TIMER_CAPTURE) { + /* Prepare to catch the time of ADDRESS event of TX CS_SYNC. */ + nrf_timer_cc_set(NRF_TIMER3, 0, 8); + phy_ppi_radio_address_to_timer3_capture0_enable(); + } + + g_ble_phy_data.phy_cs_timer = cs_timer; + + return 0; +} diff --git a/nimble/drivers/nrf5x/src/nrf52/phy.c b/nimble/drivers/nrf5x/src/nrf52/phy.c index 30a19966b7..936dcc6bcc 100644 --- a/nimble/drivers/nrf5x/src/nrf52/phy.c +++ b/nimble/drivers/nrf5x/src/nrf52/phy.c @@ -177,6 +177,13 @@ phy_ppi_init(void) nrf_ppi_channel_endpoint_setup(NRF_PPI, NRF_PPI_CHANNEL5, (uint32_t)&(NRF_TIMER0->EVENTS_COMPARE[3]), (uint32_t)&(NRF_RADIO->TASKS_DISABLE)); + + nrf_ppi_channel_endpoint_setup(NRF_PPI, NRF_PPI_CHANNEL8, + (uint32_t)&(NRF_RADIO->EVENTS_ADDRESS), + (uint32_t)&(NRF_TIMER3->TASKS_START)); + nrf_ppi_channel_endpoint_setup(NRF_PPI, NRF_PPI_CHANNEL9, + (uint32_t)&(NRF_RADIO->EVENTS_ADDRESS), + (uint32_t)&(NRF_TIMER3->TASKS_CAPTURE[0])); } void diff --git a/nimble/drivers/nrf5x/src/nrf52/phy_ppi.h b/nimble/drivers/nrf5x/src/nrf52/phy_ppi.h index a77b718339..51b7a6efea 100644 --- a/nimble/drivers/nrf5x/src/nrf52/phy_ppi.h +++ b/nimble/drivers/nrf5x/src/nrf52/phy_ppi.h @@ -82,6 +82,30 @@ phy_ppi_radio_address_to_ccm_crypt_disable(void) nrf_ppi_channels_disable(NRF_PPI, PPI_CHEN_CH25_Msk); } +static inline void +phy_ppi_radio_address_to_timer3_start_enable(void) +{ + nrf_ppi_channels_enable(NRF_PPI, PPI_CHEN_CH8_Msk); +} + +static inline void +phy_ppi_radio_address_to_timer3_start_disable(void) +{ + nrf_ppi_channels_disable(NRF_PPI, PPI_CHEN_CH8_Msk); +} + +static inline void +phy_ppi_radio_address_to_timer3_capture0_enable(void) +{ + nrf_ppi_channels_enable(NRF_PPI, PPI_CHEN_CH9_Msk); +} + +static inline void +phy_ppi_radio_address_to_timer3_capture0_disable(void) +{ + nrf_ppi_channels_disable(NRF_PPI, PPI_CHEN_CH9_Msk); +} + static inline void phy_ppi_wfr_enable(void) { @@ -105,6 +129,7 @@ phy_ppi_disable(void) { nrf_ppi_channels_disable(NRF_PPI, PPI_CHEN_CH4_Msk | PPI_CHEN_CH5_Msk | PPI_CHEN_CH6_Msk | PPI_CHEN_CH7_Msk | + PPI_CHEN_CH8_Msk | PPI_CHEN_CH20_Msk | PPI_CHEN_CH21_Msk | PPI_CHEN_CH23_Msk | PPI_CHEN_CH25_Msk | PPI_CHEN_CH31_Msk); diff --git a/nimble/host/include/host/ble_cs.h b/nimble/host/include/host/ble_cs.h index 3abbd11e65..e4ed025c81 100644 --- a/nimble/host/include/host/ble_cs.h +++ b/nimble/host/include/host/ble_cs.h @@ -31,6 +31,7 @@ struct ble_cs_event { { struct { + uint32_t time_diff_ns; uint16_t conn_handle; uint8_t status; } procedure_complete; diff --git a/nimble/host/src/ble_cs.c b/nimble/host/src/ble_cs.c index cef622206b..b893db7157 100644 --- a/nimble/host/src/ble_cs.c +++ b/nimble/host/src/ble_cs.c @@ -31,6 +31,42 @@ #include "sys/queue.h" #include "ble_hs_hci_priv.h" +#define BLE_HS_CS_MODE0 (0) +#define BLE_HS_CS_MODE1 (1) +#define BLE_HS_CS_MODE2 (2) +#define BLE_HS_CS_MODE3 (3) + +#define BLE_HS_CS_ROLE_INITIATOR (0) +#define BLE_HS_CS_ROLE_REFLECTOR (1) + +#define BLE_HS_CS_RTT_AA_ONLY (0x00) +#define BLE_HS_CS_RTT_32_BIT_SOUNDING_SEQUENCE (0x01) +#define BLE_HS_CS_RTT_96_BIT_SOUNDING_SEQUENCE (0x02) +#define BLE_HS_CS_RTT_32_BIT_RANDOM_SEQUENCE (0x03) +#define BLE_HS_CS_RTT_64_BIT_RANDOM_SEQUENCE (0x04) +#define BLE_HS_CS_RTT_96_BIT_RANDOM_SEQUENCE (0x05) +#define BLE_HS_CS_RTT_128_BIT_RANDOM_SEQUENCE (0x06) + +#define BLE_HS_CS_SUBEVENT_DONE_STATUS_COMPLETED (0x0) +#define BLE_HS_CS_SUBEVENT_DONE_STATUS_PARTIAL (0x1) +#define BLE_HS_CS_SUBEVENT_DONE_STATUS_ABORTED (0xF) + +#define BLE_HS_CS_PROC_DONE_STATUS_COMPLETED (0x0) +#define BLE_HS_CS_PROC_DONE_STATUS_PARTIAL (0x1) +#define BLE_HS_CS_PROC_DONE_STATUS_ABORTED (0xF) + +#define TIME_DIFF_NOT_AVAILABLE (0x00008000) + +#define IN_RANGE(_n, _min, _max) (((_n) >= (_min)) && ((_n) <= (_max))) +#define N_AP_MAX (4) + +static uint8_t aci_table[] = {1, 2, 3, 4, 2, 3, 4, 4}; +static uint8_t local_role; +static uint8_t sounding_pct_estimate; +static uint8_t rtt_type; +static int32_t time_diff_sum; +static uint32_t time_diff_count; + struct ble_cs_rd_rem_supp_cap_cp { uint16_t conn_handle; } __attribute__((packed)); @@ -115,7 +151,6 @@ struct ble_cs_create_config_cp { uint8_t channel_selection_type; uint8_t ch3c_shape; uint8_t ch3c_jump; - uint8_t companion_signal_enable; } __attribute__((packed)); struct ble_cs_remove_config_cp { @@ -189,7 +224,8 @@ ble_cs_call_event_cb(struct ble_cs_event *event) } static void -ble_cs_call_procedure_complete_cb(uint16_t conn_handle, uint8_t status) +ble_cs_call_procedure_complete_cb(uint16_t conn_handle, uint8_t status, + uint32_t time_diff_ns) { struct ble_cs_event event; @@ -197,6 +233,7 @@ ble_cs_call_procedure_complete_cb(uint16_t conn_handle, uint8_t status) event.type = BLE_CS_EVENT_CS_PROCEDURE_COMPLETE; event.procedure_complete.conn_handle = conn_handle; event.procedure_complete.status = status; + event.procedure_complete.time_diff_ns = time_diff_ns; ble_cs_call_event_cb(&event); } @@ -218,7 +255,8 @@ ble_cs_rd_loc_supp_cap(void) rp.optional_t_ip2_times_supported = le16toh(rp.optional_t_ip2_times_supported); rp.optional_t_fcs_times_supported = le16toh(rp.optional_t_fcs_times_supported); rp.optional_t_pm_times_supported = le16toh(rp.optional_t_pm_times_supported); - (void) rp; + + sounding_pct_estimate = (rp.optional_subfeatures_supported >> 3) & 1; return rc; } @@ -361,7 +399,7 @@ ble_cs_create_config(const struct ble_cs_create_config_cp *cmd) cp.channel_selection_type = cmd->channel_selection_type; cp.ch3c_shape = cmd->ch3c_shape; cp.ch3c_jump = cmd->ch3c_jump; - cp.companion_signal_enable = cmd->companion_signal_enable; + cp.reserved = 0x00; return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CS_CREATE_CONFIG), @@ -521,8 +559,12 @@ ble_hs_hci_evt_le_cs_rd_rem_fae_complete(uint8_t subevent, const void *data, */ cmd.mode_0_steps = 0x03; /* Take the Initiator role */ - cmd.role = 0x00; - cmd.rtt_type = 0x01; + local_role = BLE_HS_CS_ROLE_INITIATOR; + cmd.role = local_role; + + rtt_type = BLE_HS_CS_RTT_32_BIT_SOUNDING_SEQUENCE; + cmd.rtt_type = rtt_type; + cmd.cs_sync_phy = 0x01; memcpy(cmd.channel_map, (uint8_t[10]) {0x0a, 0xfa, 0xcf, 0xac, 0xfa, 0xc0}, 10); cmd.channel_map_repetition = 0x01; @@ -531,8 +573,6 @@ ble_hs_hci_evt_le_cs_rd_rem_fae_complete(uint8_t subevent, const void *data, /* Ignore these as used only with #3c algorithm */ cmd.ch3c_shape = 0x00; cmd.ch3c_jump = 0x00; - /* EDLC/ECLD attack protection not supported */ - cmd.companion_signal_enable = 0x00; /* Create CS config */ rc = ble_cs_create_config(&cmd); @@ -564,7 +604,7 @@ ble_hs_hci_evt_le_cs_sec_enable_complete(uint8_t subevent, const void *data, cmd.conn_handle = le16toh(ev->conn_handle); cmd.config_id = 0x00; /* The maximum duration of each CS procedure (time = N × 0.625 ms) */ - cmd.max_procedure_len = 8; + cmd.max_procedure_len = 800; /* The maximum number of consecutive CS procedures to be scheduled * as part of this measurement */ @@ -577,8 +617,8 @@ ble_hs_hci_evt_le_cs_sec_enable_complete(uint8_t subevent, const void *data, /* Minimum/maximum suggested durations for each CS subevent in microseconds. * 1250us and 5000us selected. */ - cmd.min_subevent_len = 1250; - cmd.max_subevent_len = 5000; + cmd.min_subevent_len = 10000; + cmd.max_subevent_len = 10000; /* Use ACI 0 as we have only one antenna on each side */ cmd.tone_antenna_config_selection = 0x00; /* Use LE 1M PHY for CS procedures */ @@ -628,7 +668,7 @@ ble_hs_hci_evt_le_cs_config_complete(uint8_t subevent, const void *data, rc = ble_cs_sec_enable(&cmd); if (rc) { BLE_HS_LOG(DEBUG, "Failed to enable CS security"); - ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), ev->status); + ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), ev->status, 0); } return 0; @@ -647,16 +687,248 @@ ble_hs_hci_evt_le_cs_proc_enable_complete(uint8_t subevent, const void *data, return 0; } +static int +ble_cs_add_mode0_result(const uint8_t *data, uint8_t data_len) +{ + uint16_t measured_freq_offset; + uint8_t packet_quality; + uint8_t packet_rssi; + uint8_t cs_sync_antenna; + + if (!IN_RANGE(data_len, 3, 5)) { + /* Ignore invalid formatted results */ + return 1; + } + + packet_quality = data[0]; + packet_rssi = data[1]; + cs_sync_antenna = data[2]; + + if (local_role == BLE_HS_CS_ROLE_INITIATOR) { + if (data_len < 5) { + /* Ignore invalid formatted results */ + return 1; + } + + measured_freq_offset = get_le16(data + 3); + } + + (void)measured_freq_offset; + (void)packet_quality; + (void)packet_rssi; + (void)cs_sync_antenna; + + return 0; +} + +static int +ble_cs_add_mode1_result(const uint8_t *data, uint8_t data_len) +{ + uint8_t packet_quality; + uint8_t packet_nadm; + uint8_t packet_rssi; + uint8_t cs_sync_antenna; + uint32_t packet_pct1; + uint32_t packet_pct2; + int16_t time_diff; + + if (!IN_RANGE(data_len, 6, 14)) { + /* Ignore invalid formatted results */ + return 1; + } + + packet_quality = data[0]; + packet_nadm = data[1]; + packet_rssi = data[2]; + time_diff = get_le16(data + 3); + cs_sync_antenna = data[5]; + + if (data_len == 14) { + packet_pct1 = get_le32(data + 6); + packet_pct2 = get_le32(data + 10); + } + + (void)packet_quality; + (void)packet_nadm; + (void)packet_rssi; + (void)cs_sync_antenna; + /* TODO: Extract IQ samples */ + (void)packet_pct1; + (void)packet_pct2; + + if (time_diff != TIME_DIFF_NOT_AVAILABLE) { + time_diff_sum += time_diff; + ++time_diff_count; + } + + return 0; +} + +static int +ble_cs_add_mode2_result(const uint8_t *data, uint8_t data_len) +{ + uint32_t tone_pct[N_AP_MAX + 1]; + uint8_t tone_quality_ind[N_AP_MAX + 1]; + uint8_t aci; + uint8_t n_ap; + uint8_t i; + + aci = *(data++); + n_ap = aci_table[aci]; + + if (data_len < 1 + n_ap * 4) { + /* Ignore invalid formatted results */ + return 1; + } + + for (i = 0; i < n_ap + 1; ++i) { + tone_pct[i] = get_le24(data); + data += 3; + } + + for (i = 0; i < n_ap + 1; ++i) { + tone_quality_ind[i] = *(data++); + } + + /* TODO: Extract IQ samples */ + (void)tone_pct; + (void)tone_quality_ind; + + return 0; +} + +static int +ble_cs_add_mode3_result(const uint8_t *data_buf, uint8_t data_len) +{ + const uint8_t *data = data_buf; + uint32_t tone_pct[N_AP_MAX + 1]; + uint8_t tone_quality_ind[N_AP_MAX + 1]; + uint32_t packet_pct1; + uint32_t packet_pct2; + int16_t time_diff; + uint8_t aci; + uint8_t n_ap; + uint8_t packet_quality; + uint8_t packet_nadm; + uint8_t packet_rssi; + uint8_t cs_sync_antenna; + uint8_t i; + + packet_quality = data[0]; + packet_nadm = data[1]; + packet_rssi = data[2]; + time_diff = get_le16(data + 3); + cs_sync_antenna = data[5]; + data += 6; + + if (sounding_pct_estimate && rtt_type != BLE_HS_CS_RTT_AA_ONLY) { + packet_pct1 = get_le32(data); + data += 4; + packet_pct2 = get_le32(data + 10); + data += 4; + } + + aci = *(data++); + n_ap = aci_table[aci]; + + for (i = 0; i < n_ap + 1; ++i) { + tone_pct[i] = get_le24(data); + data += 3; + } + + for (i = 0; i < n_ap + 1; ++i) { + tone_quality_ind[i] = *(data++); + } + + if (data_len < data - data_buf) { + /* Ignore invalid formatted results */ + return 1; + } + + (void)tone_pct; + (void)tone_quality_ind; + (void)packet_pct1; + (void)packet_pct2; + (void)aci; + (void)n_ap; + (void)packet_quality; + (void)packet_nadm; + (void)packet_rssi; + (void)cs_sync_antenna; + + if (time_diff != TIME_DIFF_NOT_AVAILABLE) { + time_diff_sum += time_diff; + ++time_diff_count; + } + + return 0; +} + +static int +ble_cs_add_steps(const struct cs_steps_data *step_data, uint8_t step_count) +{ + int rc; + const void *data; + uint8_t data_len; + uint8_t i; + + for (i = 0; i < step_count; ++i) { + data = step_data->data; + data_len = step_data->data_len; + + if (data_len == 0) { + /* Ignore step with missing results */ + continue; + } + + switch (step_data->mode) { + case BLE_HS_CS_MODE0: + rc = ble_cs_add_mode0_result(data, data_len); + break; + case BLE_HS_CS_MODE1: + rc = ble_cs_add_mode1_result(data, data_len); + break; + case BLE_HS_CS_MODE2: + rc = ble_cs_add_mode2_result(data, data_len); + break; + case BLE_HS_CS_MODE3: + rc = ble_cs_add_mode3_result(data, data_len); + break; + default: + rc = 1; + } + + if (rc) { + /* Ignore invalid formatted results */ + return 0; + } + + step_data += step_data->data_len; + } + return rc; +} + int ble_hs_hci_evt_le_cs_subevent_result(uint8_t subevent, const void *data, unsigned int len) { + int rc; const struct ble_hci_ev_le_subev_cs_subevent_result *ev = data; + uint32_t time_diff_ns; - if (len != sizeof(*ev)) { + if (len < sizeof(*ev)) { return BLE_HS_ECONTROLLER; } + rc = ble_cs_add_steps(ev->steps, ev->num_steps_reported); + + if (!rc && time_diff_count && ev->subevent_done_status != BLE_HS_CS_SUBEVENT_DONE_STATUS_PARTIAL) { + time_diff_ns = time_diff_sum / time_diff_count * 0.5; + time_diff_sum = 0; + time_diff_count = 0; + ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), 0, time_diff_ns); + } + return 0; } @@ -664,12 +936,23 @@ int ble_hs_hci_evt_le_cs_subevent_result_continue(uint8_t subevent, const void *data, unsigned int len) { + int rc; const struct ble_hci_ev_le_subev_cs_subevent_result_continue *ev = data; + uint32_t time_diff_ns; - if (len != sizeof(*ev)) { + if (len < sizeof(*ev)) { return BLE_HS_ECONTROLLER; } + rc = ble_cs_add_steps(ev->steps, ev->num_steps_reported); + + if (!rc && time_diff_count && ev->subevent_done_status != BLE_HS_CS_SUBEVENT_DONE_STATUS_PARTIAL) { + time_diff_ns = time_diff_sum / time_diff_count * 0.5; + time_diff_sum = 0; + time_diff_count = 0; + ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), 0, time_diff_ns); + } + return 0; } @@ -689,7 +972,6 @@ ble_hs_hci_evt_le_cs_test_end_complete(uint8_t subevent, const void *data, int ble_cs_initiator_procedure_start(const struct ble_cs_initiator_procedure_start_params *params) { - struct ble_hci_le_cs_rd_loc_supp_cap_rp rsp; struct ble_cs_rd_rem_supp_cap_cp cmd; int rc; @@ -701,6 +983,12 @@ ble_cs_initiator_procedure_start(const struct ble_cs_initiator_procedure_start_p * 5. Start the CS Security Start procedure */ + (void) ble_cs_set_chan_class; + (void) ble_cs_remove_config; + (void) ble_cs_wr_cached_rem_fae; + (void) ble_cs_wr_cached_rem_supp_cap; + (void) ble_cs_rd_loc_supp_cap; + cs_state.cb = params->cb; cs_state.cb_arg = params->cb_arg; diff --git a/nimble/include/nimble/ble.h b/nimble/include/nimble/ble.h index c42d5c5ba4..abd77b3058 100644 --- a/nimble/include/nimble/ble.h +++ b/nimble/include/nimble/ble.h @@ -115,7 +115,7 @@ struct ble_mbuf_hdr_rxinfo #define BLE_MBUF_HDR_F_MIC_FAILURE (0x0020) #define BLE_MBUF_HDR_F_SCAN_RSP_TXD (0x0010) #define BLE_MBUF_HDR_F_SCAN_RSP_RXD (0x0008) -#define BLE_MBUF_HDR_F_RXSTATE_MASK (0x0007) +#define BLE_MBUF_HDR_F_RXSTATE_MASK (0x000f) /* Transmit info. NOTE: no flags defined */ struct ble_mbuf_hdr_txinfo @@ -298,6 +298,9 @@ enum ble_error_codes BLE_ERR_LIMIT_REACHED = 0x43, BLE_ERR_OPERATION_CANCELLED = 0x44, BLE_ERR_PACKET_TOO_LONG = 0x45, + BLE_ERR_TOO_LATE = 0x46, + BLE_ERR_TOO_EARLY = 0x47, + BLE_ERR_INSUFFICIENT_CHAN = 0x48, BLE_ERR_MAX = 0xff }; diff --git a/nimble/include/nimble/hci_common.h b/nimble/include/nimble/hci_common.h index 252b033973..a965966e5f 100644 --- a/nimble/include/nimble/hci_common.h +++ b/nimble/include/nimble/hci_common.h @@ -1178,6 +1178,7 @@ struct ble_hci_le_cs_rd_loc_supp_cap_rp { uint16_t optional_t_fcs_times_supported; uint16_t optional_t_pm_times_supported; uint8_t t_sw_time_supported; + uint8_t optional_tx_snr_capability; } __attribute__((packed)); #define BLE_HCI_OCF_LE_CS_RD_REM_SUPP_CAP (0x008A) @@ -1207,6 +1208,7 @@ struct ble_hci_le_cs_wr_cached_rem_supp_cap_cp { uint16_t optional_t_fcs_times_supported; uint16_t optional_t_pm_times_supported; uint8_t t_sw_time_supported; + uint8_t optional_tx_snr_capability; } __attribute__((packed)); struct ble_hci_le_cs_wr_cached_rem_supp_cap_rp { uint16_t conn_handle; @@ -1261,7 +1263,7 @@ struct ble_hci_le_cs_create_config_cp { uint8_t channel_selection_type; uint8_t ch3c_shape; uint8_t ch3c_jump; - uint8_t companion_signal_enable; + uint8_t reserved; } __attribute__((packed)); #define BLE_HCI_OCF_LE_CS_REMOVE_CONFIG (0x0091) @@ -1322,7 +1324,7 @@ struct ble_hci_le_cs_test_cp { uint8_t t_pm_time; uint8_t t_sw_time; uint8_t tone_antenna_config_selection; - uint8_t companion_signal_enable; + uint8_t reserved; uint16_t drbg_nonce; uint16_t override_config; uint8_t override_parameters_length; @@ -2215,6 +2217,7 @@ struct ble_hci_ev_le_subev_cs_rd_rem_supp_cap_complete { uint16_t optional_t_fcs_times_supported; uint16_t optional_t_pm_times_supported; uint8_t t_sw_time_supported; + uint8_t optional_tx_snr_capability; } __attribute__((packed)); #define BLE_HCI_LE_SUBEV_CS_RD_REM_FAE_COMPLETE (0x2D) @@ -2253,7 +2256,7 @@ struct ble_hci_ev_le_subev_cs_config_complete { uint8_t channel_selection_type; uint8_t ch3c_shape; uint8_t ch3c_jump; - uint8_t companion_signal_enable; + uint8_t reserved; uint8_t t_ip1_time; uint8_t t_ip2_time; uint8_t t_fcs_time;