同步上游到 5271e1f

.gitignore

@@ -1,3 +1,5 @@
 # Dummy NPL build
 *.o
 /porting/examples/dummy/dummy
+/porting/examples/linux/nimble-linux
+/porting/examples/linux_blemesh/nimble-linux-blemesh

.rat-excludes

@@ -14,6 +14,8 @@ pts-l2cap.txt
 pts-sm.txt
 94654-20170317-085122560.tpg
 94654-20170317-085441153.pts
+uncrustify.cfg
+.style_ignored_dirs
 
 # tinycrypt - BSD License.
 tinycrypt
@@ -26,3 +28,6 @@ queue.h
 
 # mbuf implementation - BSD License
 os_mbuf.c
+
+# Bluetooth Mesh badge sample - Apache 2.0 License
+mesh_badge

.style_ignored_dirs

@@ -0,0 +1,4 @@
+# Skip those directories while doing style checks in the CI. Do not add '/' at
+# the beginning!
+
+ext/tinycrypt

.travis.yml

@@ -17,6 +17,8 @@
 
 language: go
 
+dist: bionic
+
 _addons: &addon_conf
   apt:
     sources:
@@ -26,13 +28,26 @@ _addons: &addon_conf
       - gcc-7-multilib
 
 go:
-  - "1.10"
+  - "1.16"
 
 git:
   depth: false
 
 matrix:
   include:
+    # Style checking
+    - os: linux
+      language: python
+      python:
+        - "3.5"
+      addons:
+        apt:
+          packages:
+            - "python3-pip"
+      env:
+        - TEST=STYLE
+        - DEBUG=1
+
     # newt build <targets>
     - os: linux
       addons: *addon_conf
@@ -100,6 +115,11 @@ matrix:
         - TEST=TEST_ALL
         - VM_AMOUNT=3
         - TARGET_SET=3
+    - os: windows
+      env:
+        - TEST=BUILD_TARGETS_WINDOWS
+        - VM_AMOUNT=1
+        - TARGET_SET=1
 
 before_install:
   - printenv
@@ -107,23 +127,41 @@ before_install:
   - go version
 
 install:
-  - git clone https://github.com/runtimeco/mynewt-travis-ci $HOME/ci
+  - git clone https://github.com/JuulLabs-OSS/mynewt-travis-ci $HOME/ci
   - chmod +x $HOME/ci/*.sh
-  - $HOME/ci/${TRAVIS_OS_NAME}_travis_install.sh
+  - |
+    if [ "${TEST}" == "STYLE" ]; then
+      pip3 install requests
+    else
+      $HOME/ci/${TRAVIS_OS_NAME}_travis_install.sh
+    fi
 
 before_script:
-  - newt version
-  - gcc --version
-  - if [ "${TEST}" != "TEST_ALL" ]; then arm-none-eabi-gcc --version; fi
-  - cp -R $HOME/ci/mynewt-nimble-project.yml project.yml
-  - cp -R $HOME/ci/mynewt-nimble-targets targets
-  - $HOME/ci/prepare_test.sh $VM_AMOUNT
-  - mkdir -p repos && pushd repos/
-  - git clone https://github.com/apache/mynewt-core apache-mynewt-core
-  - popd
+  - |
+    if [ "${TEST}" == "STYLE" ]; then
+      $HOME/ci/install_uncrustify.sh
+    else
+      newt version
+      gcc --version
+      if [ "${TEST}" != "TEST_ALL" ]; then arm-none-eabi-gcc --version; fi
+      cp -R $HOME/ci/mynewt-nimble-project.yml project.yml
+      mkdir -p targets
+      cp -R $HOME/ci/mynewt-nimble-targets targets
+      $HOME/ci/prepare_test.sh $VM_AMOUNT
+      mkdir -p repos && pushd repos/
+      git clone --depth=1 https://github.com/apache/mynewt-core apache-mynewt-core
+      git clone --depth=1 https://github.com/JuulLabs-OSS/mcuboot mcuboot
+      git clone --depth=1 https://github.com/apache/mynewt-mcumgr apache-mynewt-mcumgr
+      popd
+    fi
 
 script:
-  - $HOME/ci/run_test.sh
+  - |
+    if [ "${TEST}" == "STYLE" ]; then
+      python3 $HOME/ci/check_style.py
+    else
+      $HOME/ci/run_test.sh
+    fi
 
 cache:
   directories:

NOTICE

@@ -1,5 +1,5 @@
 Apache Mynewt NimBLE
-Copyright 2015-2018 The Apache Software Foundation
+Copyright 2015-2021 The Apache Software Foundation
 
 This product includes software developed at
 The Apache Software Foundation (http://www.apache.org/).

README.md

@@ -1,164 +1,170 @@
-# 1 介绍
-
-NimBLE 软件包是 RT-Thread 基于 [Apache NimBLE](https://github.com/apache/mynewt-nimble) 开源蓝牙 5.0 协议栈的移植实现，该协议栈提供完整的 Host 层和 Controller 层支持，目前支持 Nordic nRF51 和 nRF52 系列芯片。
-
-## 1.1 主要特性
-
-- 扩展广播(LE Advertising Extensions)
-- 2Mbit/s比特率的物理层
-- 长距离编码(Coded PHY for LE Long Range)
-- 高速不可连接广播(High Duty Cycle Non-Connectable Advertising)
-- 新的跳频算法(Channel Selection Algorithm #2)
-- 隐私1.2(LE Privacy 1.2)
-- 安全管理(SM),支持传统配对(LE Legacy Pairing),安全连接(LE Secure Connections),特定秘钥分发(Transport Specific Key Distribution)
-- 链路层PDU数据长度扩展(LE Data Length Extension)
-- 多角色并发(主机(central)/从机(peripheral), server/client)
-- 同时广播和扫描
-- 低速定向广播(Low Duty Cycle Directed Advertising)
-- 连接参数请求(Connection parameters request procedure)
-- LE Ping
-- 完整的GATT客户端，服务端，以及子功能
-- 抽象HCI接口层
-
-## 1.2 Profile和Service支持
-
-- 警报通知服务(ANS)
-- 即时报警服务(IAS)
-- 链路丢失服务(LLS)
-- 电池服务(BAS)
-- 设备信息服务(DIS)
-- 心率服务(HRS)
-- 自行车速度及步调(CSC)
-- 射频功率(TPS)
-
-## 1.3 Mesh 特性
-
-- 广播和GATT承载(Advertising and GATT bearers)
-- PB-GATT 和 PB-ADV provisioning
-- 模型层(Foundation Models (server role))
-- 支持中继(Relay support)
-- 支持GATT代理(GATT Proxy)
-
-更多关于 NimBLE Stack 的介绍请参考 ``http://mynewt.apache.org/latest/network/docs/index.html``。
-
-## 1.4  目录结构
-
-```
-NimBLE
-   ├───apps                   /* Bluetooth 示例应用程序 */
-   │   ├───blecent
-   │   ├───blecsc
-   │   ├───blehci
-   │   ├───blehr
-   │   ├───blemesh
-   │   ├───blemesh_light
-   │   ├───blemesh_shell
-   │   ├───bleprph
-   │   ├───bleuart
-   │   ├───btshell
-   │   ├───ext_advertiser
-   │   └───ibeacon
-   ├───docs                   /* 官方文档及 API 说明 */
-   ├───ext
-   │   └───tinycrypt          /* Tinycrypt 加密库 */
-   ├───nimble
-   │   ├───controller         /* Controller 实现 */
-   │   │   ├───include
-   │   │   └───src
-   │   ├───drivers            /* Nordic 系列 Phy 驱动 */
-   │   │   ├───nrf51
-   │   │   └───nrf52
-   │   ├───host               /* Host Stack(主机控制器)实现 */
-   │   │   ├───include
-   │   │   ├───mesh           /* Mesh 组网功能 */
-   │   │   ├───pts            /* PTS 测试相关 */
-   │   │   ├───services       /* 通用的 Profile */
-   │   │   │   ├───ans
-   │   │   │   ├───bas
-   │   │   │   ├───bleuart
-   │   │   │   ├───dis
-   │   │   │   ├───gap
-   │   │   │   ├───gatt
-   │   │   │   ├───ias
-   │   │   │   ├───lls
-   │   │   │   └───tps
-   │   │   ├───src
-   │   │   ├───store
-   │   │   ├───tools
-   │   │   └───util
-   │   ├───include
-   │   │   └───nimble
-   │   ├───src
-   │   └───transport          /* HCI 传输抽象层 */
-   │       ├───emspi
-   │       ├───ram
-   │       ├───socket
-   │       └───uart
-   └───porting                /* OS 抽象层及系统配置 */
-       ├───nimble
-       │   ├───include
-       │   └───src
-       └───npl
-           └───rtthread       /* RT-Thread OS 接口实现 */
-               ├───include
-               │   ├───config /* NimBLE 协议栈配置选项 */
-               │   ├───console
-               │   └───nimble
-               └───src
-```
-
-## 1.5 许可证
-
-NimBLE 软件包遵循 Apache-2.0 许可，详见 LICENSE 文件。
-
-## 1.6 依赖
-
-- RT_Thread 3.0+
-
-## 2 获取软件包
-
-使用 NimBLE 软件包需要在 RT-Thread 的包管理中选中它，具体路径如下：
-
-```
-RT-Thread online packages
-    IoT - internet of things  --->
---- NimBLE:An open-source Bluetooth 5.0 stack porting on RT-Thread
-      Bluetooth Role support  --->      
-      Host Stack Configuration  --->
-      Controller Configuration  --->
-      Bluetooth Mesh support  --->
-      HCI Transport support  ----
-      Device Driver support  ----
-      Log level (INFO)  --->
-      Bluetooth Samples (Not enable sample)  --->
-(1)   Maximum number of concurrent connections
-[*]   Device Whitelist Support
-(0)   The number of multi-advertising instances
-[ ]   Extended Advertising Feature Support
-      Version (latest)  --->
-
-```
-
-**Bluetooth Role support**  ：  配置 BLE角色支持(Central/Peripheral/Broadcaster/Observer) ；   
-**Host Stack Configuration**  ：  配置 Host 相关功能；   
-**Controller Configuration**  ：  配置 Controller 相关功能；   
-**Bluetooth Mesh support**  ：  Mesh 特性支持及配置；   
-**HCI Transport support** ： 配置HCI层传输方式   
-**Device Driver support ** ： 底层 SOC Phy 支持   
-**Log level (INFO)**  ：  配置协议栈日志等级；   
-**Bluetooth Samples**  ：  配置示例应用；   
-**Version**  ：  软件包版本选择；   
-
-配置完成后让 RT-Thread 的包管理器自动更新，或者使用 pkgs --update 命令更新包到 BSP 中。
-
-## 3 使用 NimBLE 软件包
-
-配合独立的 nrf52832-nimble bsp 使用，参考 https://github.com/EvalZero/nrf52832-nimble 。
-
-## 4 注意事项
-- NimBLE 当前处于开发阶段，暂时只支持 Nodic nRF52832 MCU，参见 [nrf52832-bsp](https://github.com/EvalZero/nrf52832-nimble)
-
-## 5、联系方式 & 感谢
-
-- 维护：RT-Thread 开发团队
-- 主页：https://github.com/RT-Thread-packages/nimble
+<!--
+#
+# 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.
+#
+-->
+
+<img src="http://mynewt.apache.org/img/logo.svg" width="250" alt="Apache Mynewt">
+
+## Overview
+
+Apache NimBLE is an open-source Bluetooth 5.1 stack (both Host & Controller)
+that completely replaces the proprietary SoftDevice on Nordic chipsets. It is
+part of [Apache Mynewt project](https://github.com/apache/mynewt-core).
+
+Features highlight:
+  - Support for 251 byte packet size
+  - Support for all 4 roles concurrently - Broadcaster, Observer, Peripheral and Central
+  - Support for up to 32 simultaneous connections.
+  - Legacy and SC (secure connections) SMP support (pairing and bonding).
+  - Advertising Extensions.
+  - Periodic Advertising.
+  - Coded (aka Long Range) and 2M PHYs.
+  - Bluetooth Mesh.
+
+## Supported hardware
+
+Controller supports Nordic nRF51 and nRF52 chipsets. Host runs on any board
+and architecture [supported](https://github.com/apache/mynewt-core#overview)
+by Apache Mynewt OS.
+
+
+## Browsing
+
+If you are browsing around the source tree, and want to see some of the
+major functional chunks, here are a few pointers:
+
+- nimble/controller: Contains code for controller including Link Layer and HCI implementation
+([controller](https://github.com/apache/mynewt-nimble/tree/master/nimble/controller))
+
+- nimble/drivers: Contains drivers for supported radio transceivers (Nordic nRF51 and nRF52)
+([drivers](https://github.com/apache/mynewt-nimble/tree/master/nimble/drivers))
+
+- nimble/host: Contains code for host subsystem. This includes protocols like
+L2CAP and ATT, support for HCI commands and events, Generic Access Profile (GAP),
+Generic Attribute Profile (GATT) and Security Manager (SM).
+([host](https://github.com/apache/mynewt-nimble/tree/master/nimble/host))
+
+- nimble/host/mesh: Contains code for Bluetooth Mesh subsystem.
+([mesh](https://github.com/apache/mynewt-nimble/tree/master/nimble/host/mesh))
+
+- nimble/transport: Contains code for supported transport protocols between host
+and controller. This includes UART, emSPI and RAM (used in combined build when
+host and controller run on same CPU)
+([transport](https://github.com/apache/mynewt-nimble/tree/master/nimble/transport))
+
+- porting: Contains implementation of NimBLE Porting Layer (NPL) for supported
+operating systems
+([porting](https://github.com/apache/mynewt-nimble/tree/master/porting))
+
+- ext: Contains external libraries used by NimBLE. Those are used if not
+provided by OS
+([ext](https://github.com/apache/mynewt-nimble/tree/master/ext))
+
+- kernel: Contains the core of the RTOS ([kernel/os](https://github.com/apache/mynewt-core/tree/master/kernel/os))
+
+## Sample Applications
+
+There are also some sample applications that show how to Apache Mynewt NimBLE
+stack. These sample applications are located in the `apps/` directory of
+Apache Mynewt [repo](https://github.com/apache/mynewt-core). Some examples:
+
+* [blecent](https://github.com/apache/mynewt-nimble/tree/master/apps/blecent):
+A basic central device with no user interface.  This application scans for
+a peripheral that supports the alert notification service (ANS). Upon
+discovering such a peripheral, blecent connects and performs a characteristic
+read, characteristic write, and notification subscription.
+* [blehci](https://github.com/apache/mynewt-nimble/tree/master/apps/blehci):
+Implements a BLE controller-only application.  A separate host-only
+implementation, such as Linux's BlueZ, can interface with this application via
+HCI over UART.
+* [bleprph](https://github.com/apache/mynewt-nimble/tree/master/apps/bleprph): An
+  implementation of a minimal BLE peripheral.
+* [btshell](https://github.com/apache/mynewt-nimble/tree/master/apps/btshell): A
+  shell-like application allowing to configure and use most of NimBLE
+  functionality from command line.
+* [bleuart](https://github.com/apache/mynewt-core/tree/master/apps/bleuart):
+Implements a simple BLE peripheral that supports the Nordic
+UART / Serial Port Emulation service
+(https://developer.nordicsemi.com/nRF5_SDK/nRF51_SDK_v8.x.x/doc/8.0.0/s110/html/a00072.html).
+
+# Getting Help
+
+If you are having trouble using or contributing to Apache Mynewt NimBLE, or just
+want to talk to a human about what you're working on, you can contact us via the
+[developers mailing list](mailto:dev@mynewt.apache.org).
+
+Although not a formal channel, you can also find a number of core developers
+on the #mynewt channel on Freenode IRC or #general channel on [Mynewt Slack](https://mynewt.slack.com/join/shared_invite/enQtNjA1MTg0NzgyNzg3LTcyMmZiOGQzOGMxM2U4ODFmMTIwNjNmYTE5Y2UwYjQwZWIxNTE0MTUzY2JmMTEzOWFjYWZkNGM0YmM4MzAxNWQ)
+
+Also, be sure to checkout the [Frequently Asked Questions](https://mynewt.apache.org/faq/answers)
+for some help troubleshooting first.
+
+# Contributing
+
+Anybody who works with Apache Mynewt can be a contributing member of the
+community that develops and deploys it.  The process of releasing an operating
+system for microcontrollers is never done: and we welcome your contributions
+to that effort.
+
+More information can be found at the Community section of the Apache Mynewt
+website, located [here](https://mynewt.apache.org/community).
+
+## Pull Requests
+
+Apache Mynewt welcomes pull request via Github.  Discussions are done on Github,
+but depending on the topic, can also be relayed to the official Apache Mynewt
+developer mailing list dev@mynewt.apache.org.
+
+If you are suggesting a new feature, please email the developer list directly,
+with a description of the feature you are planning to work on.
+
+## Filing Bugs
+
+Bugs can be filed on the
+[Apache Mynewt NimBLE Issues](https://github.com/apache/mynewt-nimble/issues).
+Please label the issue as a "Bug".
+
+Where possible, please include a self-contained reproduction case!
+
+## Feature Requests
+
+Feature requests should also be filed on the
+[Apache Mynewt NimBLE Bug Tracker](https://github.com/apache/mynewt-nimble/issues).
+Please label the issue as a "Feature" or "Enhancement" depending on the scope.
+
+## Writing Tests
+
+We love getting newt tests!  Apache Mynewt is a huge undertaking, and improving
+code coverage is a win for every Apache Mynewt user.
+
+<!--
+TODO
+## Writing Documentation
+
+Contributing to documentation (in addition to writing tests), is a great way
+to get involved with the Apache Mynewt project.
+
+ The Mynewt NimBLE documentation is found in [/docs](/docs).
+-->
+
+# License
+
+The code in this repository is all under either the Apache 2 license, or a
+license compatible with the Apache 2 license.  See the LICENSE file for more
+information.

RELEASE_NOTES.md

@@ -0,0 +1,22 @@
+# RELEASE NOTES
+
+24 March 2021 - Apache NimBLE v1.4.0
+
+For full release notes, please visit the
+[Apache Mynewt Wiki](https://cwiki.apache.org/confluence/display/MYNEWT/Release+Notes).
+
+Apache NimBLE is an open-source Bluetooth 5.1 stack (both Host & Controller) that completely
+replaces the proprietary SoftDevice on Nordic chipsets.
+
+New features in this version of NimBLE include:
+
+* Support for PHY on Dialog Configurable MAC (CMAC)
+* Support for PHY on Nordic nRF5340
+* Support for Apache NuttX port of NimBLE
+* Controller-to-host flow control support
+* Support for USB transport
+* Various bugfixes
+
+If working on next-generation RTOS and Bluetooth protocol stack
+sounds exciting to you, get in touch, by sending a mail to the Apache Mynewt
+Developer's list, dev@mynewt.apache.org.

docs/btshell/btshell_GAP.rst

@@ -411,7 +411,7 @@ Advertising with Extended Advertising enabled
 +------------------------------+--------------------------+----------------------------+-------------------------------------------------------------------------------------+
 |                              | scan\_req\_notif         | [``0``-1]                  | Enable SCAN\_REQ notifications                                                      |
 +------------------------------+--------------------------+----------------------------+-------------------------------------------------------------------------------------+
-| **advertise-set-addr**       |                          |                            | Configure *random* adress for instance                                              |
+| **advertise-set-addr**       |                          |                            | Configure *random* address for instance                                              |
 +------------------------------+--------------------------+----------------------------+-------------------------------------------------------------------------------------+
 |                              | instance                 | [``0``-UINT8\_MAX]         | Advertising instance                                                                |
 +------------------------------+--------------------------+----------------------------+-------------------------------------------------------------------------------------+

docs/index.rst

@@ -46,7 +46,7 @@ ideal wireless technology for the Internet of Things (IoT).
 -  LE Secure Connections featuring FIPS-compliant algorithms.
 -  LE Data Length Extension for higher throughput
 -  **Coming Soon**: Assigning an Internet Protocol (IP) address
-   (complaint with the IPv6 or 6LoWPAN standard) to a Bluetooth device
+   (compliant with the IPv6 or 6LoWPAN standard) to a Bluetooth device
    through Internet Protocol Support Profile (IPSP)
 
 The Bluetooth 5 is backward compatible with previous Bluetooth version

docs/mesh/sample.rst

@@ -9,8 +9,8 @@ no need for configuring public address.
 ::
 
     newt target create blemesh
-    newt target set blemesh app=@apache-mynewt-core/apps/blemesh
-    newt target set blemesh bsp=@apache-mynewt-core/hw/bsp/nrf52840pdk
+    newt target set blemesh app=@apache-mynewt-nimble/apps/blemesh
+    newt target set blemesh bsp=@apache-mynewt-core/hw/bsp/nordic_pca10056
     newt target set blemesh build_profile=optimized
     newt target set blemesh syscfg=BLE_MESH_PB_GATT=1:BLE_MESH_DEV_UUID='(uint8_t[16]){0x22, 0x20, 0}'
 

ext/tinycrypt/src/ecc_dh.c

@@ -188,14 +188,13 @@ int uECC_shared_secret(const uint8_t *public_key, const uint8_t *private_key,
 	r = !EccPoint_isZero(_public, curve);
 
 clear_and_out:
-    // TODO:
 	/* erasing temporary buffer used to store secret: */
 	memset(p2, 0, sizeof(p2));
-	// __asm__ __volatile__("" :: "g"(p2) : "memory");
+	__asm__ __volatile__("" :: "g"(p2) : "memory");
 	memset(tmp, 0, sizeof(tmp));
-	// __asm__ __volatile__("" :: "g"(tmp) : "memory");
+	__asm__ __volatile__("" :: "g"(tmp) : "memory");
 	memset(_private, 0, sizeof(_private));
-	// __asm__ __volatile__("" :: "g"(_private) : "memory");
+	__asm__ __volatile__("" :: "g"(_private) : "memory");
 
 	return r;
 }

nimble/controller/include/controller/ble_hw.h

@@ -39,10 +39,10 @@ uint8_t ble_hw_whitelist_size(void);
 void ble_hw_whitelist_clear(void);
 
 /* Remove a device from the hw whitelist */
-void ble_hw_whitelist_rmv(uint8_t *addr, uint8_t addr_type);
+void ble_hw_whitelist_rmv(const uint8_t *addr, uint8_t addr_type);
 
 /* Add a device to the hw whitelist */
-int ble_hw_whitelist_add(uint8_t *addr, uint8_t addr_type);
+int ble_hw_whitelist_add(const uint8_t *addr, uint8_t addr_type);
 
 /* Enable hw whitelisting */
 void ble_hw_whitelist_enable(void);

nimble/controller/include/controller/ble_ll.h

@@ -55,27 +55,6 @@ extern "C" {
 #endif
 #else
 #define BLE_LL_ASSERT(cond) assert(cond)
-#endif
-/*
- * XXX:
- * I guess this should not depend on the 32768 crystal to be honest. This
- * should be done for TIMER0 as well since the rf clock chews up more current.
- * Deal with this later.
- *
- * Another note: BLE_XTAL_SETTLE_TIME should be bsp related (I guess). There
- * should be a note in there that the converted usecs to ticks value of this
- * should not be 0. Thus: if you are using a 32.768 os cputime freq, the min
- * value of settle time should be 31 usecs. I would suspect all settling times
- * would exceed 31 usecs.
- */
-
-/* Determines if we need to turn on/off rf clock */
-#undef BLE_XCVR_RFCLK
-
-/* We will turn on/off rf clock */
-#if MYNEWT_VAL(BLE_XTAL_SETTLE_TIME) != 0
-#define BLE_XCVR_RFCLK
-
 #endif
 
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) || MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
@@ -90,6 +69,29 @@ extern "C" {
 /* Timing jitter as per spec is +/16 usecs */
 #define BLE_LL_JITTER_USECS         (16)
 
+
+#if MYNEWT_VAL(BLE_LL_SCA) < 0
+#error Invalid SCA value
+#elif MYNEWT_VAL(BLE_LL_SCA) <= 20
+#define BLE_LL_SCA_ENUM     7
+#elif MYNEWT_VAL(BLE_LL_SCA) <= 30
+#define BLE_LL_SCA_ENUM     6
+#elif MYNEWT_VAL(BLE_LL_SCA) <= 50
+#define BLE_LL_SCA_ENUM     5
+#elif MYNEWT_VAL(BLE_LL_SCA) <= 75
+#define BLE_LL_SCA_ENUM     4
+#elif MYNEWT_VAL(BLE_LL_SCA) <= 100
+#define BLE_LL_SCA_ENUM     3
+#elif MYNEWT_VAL(BLE_LL_SCA) <= 150
+#define BLE_LL_SCA_ENUM     2
+#elif MYNEWT_VAL(BLE_LL_SCA) <= 250
+#define BLE_LL_SCA_ENUM     1
+#elif MYNEWT_VAL(BLE_LL_SCA) <= 500
+#define BLE_LL_SCA_ENUM     0
+#else
+#error Invalid SCA value
+#endif
+
 /* Packet queue header definition */
 STAILQ_HEAD(ble_ll_pkt_q, os_mbuf_pkthdr);
 
@@ -101,7 +103,7 @@ STAILQ_HEAD(ble_ll_pkt_q, os_mbuf_pkthdr);
 struct ble_ll_obj
 {
     /* Supported features */
-    uint32_t ll_supp_features;
+    uint64_t ll_supp_features;
 
     /* Current Link Layer state */
     uint8_t ll_state;
@@ -116,13 +118,6 @@ struct ble_ll_obj
     uint8_t ll_pref_tx_phys;
     uint8_t ll_pref_rx_phys;
 
-#ifdef BLE_XCVR_RFCLK
-    uint8_t ll_rfclk_state;
-    uint16_t ll_xtal_ticks;
-    uint32_t ll_rfclk_start_time;
-    struct hal_timer ll_rfclk_timer;
-#endif
-
     /* Task event queue */
     struct ble_npl_eventq ll_evq;
 
@@ -199,9 +194,21 @@ STATS_SECT_START(ble_ll_stats)
     STATS_SECT_ENTRY(aux_scan_rsp_err)
     STATS_SECT_ENTRY(aux_chain_cnt)
     STATS_SECT_ENTRY(aux_chain_err)
+    STATS_SECT_ENTRY(aux_scan_drop)
     STATS_SECT_ENTRY(adv_evt_dropped)
     STATS_SECT_ENTRY(scan_timer_stopped)
     STATS_SECT_ENTRY(scan_timer_restarted)
+    STATS_SECT_ENTRY(periodic_adv_drop_event)
+    STATS_SECT_ENTRY(periodic_chain_drop_event)
+    STATS_SECT_ENTRY(sync_event_failed)
+    STATS_SECT_ENTRY(sync_received)
+    STATS_SECT_ENTRY(sync_chain_failed)
+    STATS_SECT_ENTRY(sync_missed_err)
+    STATS_SECT_ENTRY(sync_crc_err)
+    STATS_SECT_ENTRY(sync_rx_buf_err)
+    STATS_SECT_ENTRY(sync_scheduled)
+    STATS_SECT_ENTRY(sched_state_sync_errs)
+    STATS_SECT_ENTRY(sched_invalid_pdu)
 STATS_SECT_END
 extern STATS_SECT_DECL(ble_ll_stats) ble_ll_stats;
 
@@ -209,38 +216,60 @@ extern STATS_SECT_DECL(ble_ll_stats) ble_ll_stats;
 #define BLE_LL_STATE_STANDBY        (0)
 #define BLE_LL_STATE_ADV            (1)
 #define BLE_LL_STATE_SCANNING       (2)
-#define BLE_LL_STATE_INITIATING     (3)
 #define BLE_LL_STATE_CONNECTION     (4)
 #define BLE_LL_STATE_DTM            (5)
+#define BLE_LL_STATE_SYNC           (6)
+#define BLE_LL_STATE_SCAN_AUX       (7)
 
 /* LL Features */
-#define BLE_LL_FEAT_LE_ENCRYPTION    (0x00000001)
-#define BLE_LL_FEAT_CONN_PARM_REQ    (0x00000002)
-#define BLE_LL_FEAT_EXTENDED_REJ     (0x00000004)
-#define BLE_LL_FEAT_SLAVE_INIT       (0x00000008)
-#define BLE_LL_FEAT_LE_PING          (0x00000010)
-#define BLE_LL_FEAT_DATA_LEN_EXT     (0x00000020)
-#define BLE_LL_FEAT_LL_PRIVACY       (0x00000040)
-#define BLE_LL_FEAT_EXT_SCAN_FILT    (0x00000080)
-#define BLE_LL_FEAT_LE_2M_PHY        (0x00000100)
-#define BLE_LL_FEAT_STABLE_MOD_ID_TX (0x00000200)
-#define BLE_LL_FEAT_STABLE_MOD_ID_RX (0x00000400)
-#define BLE_LL_FEAT_LE_CODED_PHY     (0x00000800)
-#define BLE_LL_FEAT_EXT_ADV          (0x00001000)
-#define BLE_LL_FEAT_PERIODIC_ADV     (0x00002000)
-#define BLE_LL_FEAT_CSA2             (0x00004000)
-#define BLE_LL_FEAT_LE_POWER_CLASS_1 (0x00008000)
-#define BLE_LL_FEAT_MIN_USED_CHAN    (0x00010000)
+#define BLE_LL_FEAT_LE_ENCRYPTION    (0x0000000001)
+#define BLE_LL_FEAT_CONN_PARM_REQ    (0x0000000002)
+#define BLE_LL_FEAT_EXTENDED_REJ     (0x0000000004)
+#define BLE_LL_FEAT_SLAVE_INIT       (0x0000000008)
+#define BLE_LL_FEAT_LE_PING          (0x0000000010)
+#define BLE_LL_FEAT_DATA_LEN_EXT     (0x0000000020)
+#define BLE_LL_FEAT_LL_PRIVACY       (0x0000000040)
+#define BLE_LL_FEAT_EXT_SCAN_FILT    (0x0000000080)
+#define BLE_LL_FEAT_LE_2M_PHY        (0x0000000100)
+#define BLE_LL_FEAT_STABLE_MOD_ID_TX (0x0000000200)
+#define BLE_LL_FEAT_STABLE_MOD_ID_RX (0x0000000400)
+#define BLE_LL_FEAT_LE_CODED_PHY     (0x0000000800)
+#define BLE_LL_FEAT_EXT_ADV          (0x0000001000)
+#define BLE_LL_FEAT_PERIODIC_ADV     (0x0000002000)
+#define BLE_LL_FEAT_CSA2             (0x0000004000)
+#define BLE_LL_FEAT_LE_POWER_CLASS_1 (0x0000008000)
+#define BLE_LL_FEAT_MIN_USED_CHAN    (0x0000010000)
+#define BLE_LL_FEAT_CTE_REQ          (0x0000020000)
+#define BLE_LL_FEAT_CTE_RSP          (0x0000040000)
+#define BLE_LL_FEAT_CTE_TX           (0x0000080000)
+#define BLE_LL_FEAT_CTE_RX           (0x0000100000)
+#define BLE_LL_FEAT_CTE_AOD          (0x0000200000)
+#define BLE_LL_FEAT_CTE_AOA          (0x0000400000)
+#define BLE_LL_FEAT_CTE_RECV         (0x0000800000)
+#define BLE_LL_FEAT_SYNC_TRANS_SEND  (0x0001000000)
+#define BLE_LL_FEAT_SYNC_TRANS_RECV  (0x0002000000)
+#define BLE_LL_FEAT_SCA_UPDATE       (0x0004000000)
+#define BLE_LL_FEAT_REM_PKEY         (0x0008000000)
+#define BLE_LL_FEAT_CIS_MASTER       (0x0010000000)
+#define BLE_LL_FEAT_CIS_SLAVE        (0x0020000000)
+#define BLE_LL_FEAT_ISO_BROADCASTER  (0x0040000000)
+#define BLE_LL_FEAT_SYNC_RECV        (0x0080000000)
+#define BLE_LL_FEAT_ISO_HOST_SUPPORT (0x0100000000)
+#define BLE_LL_FEAT_POWER_CTRL_REQ   (0x0200000000)
+#define BLE_LL_FEAT_POWER_CHANGE_IND (0x0400000000)
+#define BLE_LL_FEAT_PATH_LOSS_MON    (0x0800000000)
 
 /* This is initial mask, so if feature exchange will not happen,
  * but host will want to use this procedure, we will try. If not
  * succeed, feature bit will be cleared.
  * Look at LL Features above to find out what is allowed
  */
-#define BLE_LL_CONN_INITIAL_FEATURES    (0x00000002)
-
+#define BLE_LL_CONN_INITIAL_FEATURES    (0x00000022)
 #define BLE_LL_CONN_CLEAR_FEATURE(connsm, feature)   (connsm->conn_features &= ~(feature))
 
+/* All the features which can be controlled by the Host */
+#define BLE_LL_HOST_CONTROLLED_FEATURES (BLE_LL_FEAT_ISO_HOST_SUPPORT)
+
 /* LL timing */
 #define BLE_LL_IFS                  (150)       /* usecs */
 #define BLE_LL_MAFS                 (300)       /* usecs */
@@ -300,14 +329,14 @@ struct ble_dev_addr
 #define BLE_ADV_PDU_TYPE_ADV_NONCONN_IND    (2)
 #define BLE_ADV_PDU_TYPE_SCAN_REQ           (3)
 #define BLE_ADV_PDU_TYPE_SCAN_RSP           (4)
-#define BLE_ADV_PDU_TYPE_CONNECT_REQ        (5)
+#define BLE_ADV_PDU_TYPE_CONNECT_IND        (5)
 #define BLE_ADV_PDU_TYPE_ADV_SCAN_IND       (6)
 #define BLE_ADV_PDU_TYPE_ADV_EXT_IND        (7)
 #define BLE_ADV_PDU_TYPE_AUX_ADV_IND        BLE_ADV_PDU_TYPE_ADV_EXT_IND
 #define BLE_ADV_PDU_TYPE_AUX_SCAN_RSP       BLE_ADV_PDU_TYPE_ADV_EXT_IND
 #define BLE_ADV_PDU_TYPE_AUX_SYNC_IND       BLE_ADV_PDU_TYPE_ADV_EXT_IND
 #define BLE_ADV_PDU_TYPE_AUX_CHAIN_IND      BLE_ADV_PDU_TYPE_ADV_EXT_IND
-#define BLE_ADV_PDU_TYPE_AUX_CONNECT_REQ    BLE_ADV_PDU_TYPE_CONNECT_REQ
+#define BLE_ADV_PDU_TYPE_AUX_CONNECT_REQ    BLE_ADV_PDU_TYPE_CONNECT_IND
 #define BLE_ADV_PDU_TYPE_AUX_SCAN_REQ       BLE_ADV_PDU_TYPE_SCAN_REQ
 #define BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP    (8)
 
@@ -316,7 +345,7 @@ struct ble_dev_addr
 
 #define BLE_LL_EXT_ADV_ADVA_BIT         (0)
 #define BLE_LL_EXT_ADV_TARGETA_BIT      (1)
-#define BLE_LL_EXT_ADV_RFU_BIT          (2)
+#define BLE_LL_EXT_ADV_CTE_INFO_BIT     (2)
 #define BLE_LL_EXT_ADV_DATA_INFO_BIT    (3)
 #define BLE_LL_EXT_ADV_AUX_PTR_BIT      (4)
 #define BLE_LL_EXT_ADV_SYNC_INFO_BIT    (5)
@@ -325,6 +354,7 @@ struct ble_dev_addr
 #define BLE_LL_EXT_ADV_FLAGS_SIZE       (1)
 #define BLE_LL_EXT_ADV_ADVA_SIZE        (6)
 #define BLE_LL_EXT_ADV_TARGETA_SIZE     (6)
+#define BLE_LL_EXT_ADV_CTE_INFO_SIZE    (1)
 #define BLE_LL_EXT_ADV_DATA_INFO_SIZE   (2)
 #define BLE_LL_EXT_ADV_AUX_PTR_SIZE     (3)
 #define BLE_LL_EXT_ADV_SYNC_INFO_SIZE   (18)
@@ -367,6 +397,12 @@ struct ble_dev_addr
 #define BLE_LL_LLID_DATA_START          (2)
 #define BLE_LL_LLID_CTRL                (3)
 
+#define BLE_LL_LLID_IS_CTRL(hdr) \
+    (((hdr) & BLE_LL_DATA_HDR_LLID_MASK) == BLE_LL_LLID_CTRL)
+#define BLE_LL_LLID_IS_DATA(hdr) \
+    ((((hdr) & BLE_LL_DATA_HDR_LLID_MASK) == BLE_LL_LLID_DATA_START) || \
+     (((hdr) & BLE_LL_DATA_HDR_LLID_MASK) == BLE_LL_LLID_DATA_FRAG))
+
 /*
  * CONNECT_REQ
  *      -> InitA        (6 bytes)
@@ -405,6 +441,18 @@ struct ble_dev_addr
 #define BLE_SCAN_RSP_MAX_LEN        (37)
 #define BLE_SCAN_RSP_MAX_EXT_LEN    (251)
 
+#define BLE_LL_ADDR_SUBTYPE_IDENTITY    (0)
+#define BLE_LL_ADDR_SUBTYPE_RPA         (1)
+#define BLE_LL_ADDR_SUBTYPE_NRPA        (2)
+
+/* ACAD data types */
+#define BLE_LL_ACAD_CHANNEL_MAP_UPDATE_IND 0x28
+
+struct ble_ll_acad_channel_map_update_ind {
+    uint8_t map[5];
+    uint16_t instant;
+} __attribute__((packed));
+
 /*--- External API ---*/
 /* Initialize the Link Layer */
 void ble_ll_init(void);
@@ -412,8 +460,17 @@ void ble_ll_init(void);
 /* Reset the Link Layer */
 int ble_ll_reset(void);
 
+int ble_ll_is_valid_public_addr(const uint8_t *addr);
+
 /* 'Boolean' function returning true if address is a valid random address */
-int ble_ll_is_valid_random_addr(uint8_t *addr);
+int ble_ll_is_valid_random_addr(const uint8_t *addr);
+
+/*
+ * Check if given own_addr_type is valid for current controller configuration
+ * given the random address provided (when applicable)
+ */
+int ble_ll_is_valid_own_addr_type(uint8_t own_addr_type,
+                                  const uint8_t *random_addr);
 
 /* Calculate the amount of time in microseconds a PDU with payload length of
  * 'payload_len' will take to transmit on a PHY 'phy_mode'. */
@@ -424,7 +481,12 @@ uint32_t ble_ll_pdu_tx_time_get(uint16_t payload_len, int phy_mode);
 uint16_t ble_ll_pdu_max_tx_octets_get(uint32_t usecs, int phy_mode);
 
 /* Is this address a resolvable private address? */
-int ble_ll_is_rpa(uint8_t *addr, uint8_t addr_type);
+int ble_ll_is_rpa(const uint8_t *addr, uint8_t addr_type);
+
+int ble_ll_addr_subtype(const uint8_t *addr, uint8_t addr_type);
+
+/* Is this address an identity address? */
+int ble_ll_addr_is_id(uint8_t *addr, uint8_t addr_type);
 
 /* Is 'addr' our device address? 'addr_type' is public (0) or random (!=0) */
 int ble_ll_is_our_devaddr(uint8_t *addr, int addr_type);
@@ -440,12 +502,16 @@ uint8_t *ble_ll_get_our_devaddr(uint8_t addr_type);
 void ble_ll_acl_data_in(struct os_mbuf *txpkt);
 
 /**
- * Allocate a pdu (chain) for reception.
+ * Allocates mbuf for received PDU
  *
- * @param len Length of PDU. This includes the PDU header as well as payload.
- * Does not include MIC if encrypted.
+ * This allocated mbuf (may be chained if necessary) that has capacity large
+ * enough to store received PDU of given length. It does not set mbufs length
+ * as this has to be done by PHY when copying data.
  *
- * @return struct os_mbuf* Pointer to mbuf chain to hold received packet
+ * @param len  Length of PDU, including PDU header and excluding MIC (if encrypted)
+ *
+ * @return mbuf large enough to store received PDU on success
+ *         NULL on failure (oom)
  */
 struct os_mbuf *ble_ll_rxpdu_alloc(uint16_t len);
 
@@ -483,20 +549,26 @@ void ble_ll_event_send(struct ble_npl_event *ev);
 /* Hand received pdu's to LL task  */
 void ble_ll_rx_pdu_in(struct os_mbuf *rxpdu);
 
-/* Set random address */
-int ble_ll_set_random_addr(uint8_t *addr, bool hci_adv_ext);
-
-/* Enable wait for response timer */
-void ble_ll_wfr_enable(uint32_t cputime);
+/*
+ * Set public address
+ *
+ * This can be used to set controller public address from vendor specific storage,
+ * usually should be done in hal_bsp_init().
+ * Shall be *only* called before LL is initialized, i.e. before sysinit stage.
+ */
+int ble_ll_set_public_addr(const uint8_t *addr);
 
-/* Disable wait for response timer */
-void ble_ll_wfr_disable(void);
+/* Set random address */
+int ble_ll_set_random_addr(const uint8_t *cmdbuf, uint8_t len, bool hci_adv_ext);
 
 /* Wait for response timer expiration callback */
 void ble_ll_wfr_timer_exp(void *arg);
 
 /* Read set of features supported by the Link Layer */
-uint32_t ble_ll_read_supp_features(void);
+uint64_t ble_ll_read_supp_features(void);
+
+/* Set host supported features */
+int ble_ll_set_host_feat(const uint8_t *cmdbuf, uint8_t len);
 
 /* Read set of states supported by the Link Layer */
 uint64_t ble_ll_read_supp_states(void);
@@ -511,6 +583,7 @@ void ble_ll_rand_sample(uint8_t rnum);
 int ble_ll_rand_data_get(uint8_t *buf, uint8_t len);
 void ble_ll_rand_prand_get(uint8_t *prand);
 int ble_ll_rand_start(void);
+uint32_t ble_ll_rand(void);
 
 static inline int
 ble_ll_get_addr_type(uint8_t txrxflag)
@@ -528,7 +601,7 @@ ble_ll_usecs_to_ticks_round_up(uint32_t usecs)
     return os_cputime_usecs_to_ticks(usecs + 30);
 }
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
 /* LTK 0x4C68384139F574D836BCF34E9DFB01BF */
 extern const uint8_t g_bletest_LTK[];
 extern uint16_t g_bletest_EDIV;
@@ -539,8 +612,8 @@ extern uint32_t g_bletest_IVm;
 extern uint32_t g_bletest_IVs;
 #endif
 
-#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE)
-void ble_ll_dtm_init();
+#if MYNEWT_VAL(BLE_LL_DTM)
+void ble_ll_dtm_init(void);
 #endif
 
 #ifdef __cplusplus

nimble/controller/include/controller/ble_ll_adv.h

@@ -47,6 +47,7 @@ extern "C" {
 #define BLE_LL_ADV_PDU_ITVL_LD_MS_MAX   (10)            /* msecs */
 #define BLE_LL_ADV_PDU_ITVL_HD_MS_MAX   (3750)          /* usecs */
 #define BLE_LL_ADV_STATE_HD_MAX         (1280)          /* msecs */
+#define BLE_LL_ADV_PERIODIC_ITVL        (1250)          /* usecs */
 
 /* Maximum advertisement data length */
 #define BLE_ADV_LEGACY_DATA_MAX_LEN     (31)
@@ -116,19 +117,16 @@ int ble_ll_adv_start_req(uint8_t adv_chanmask, uint8_t adv_type,
                          uint8_t *init_addr, uint16_t adv_itvl, void *handle);
 
 /* Start or stop advertising */
-int ble_ll_adv_set_enable(uint8_t instance, uint8_t enable, int duration,
-                          uint8_t event);
+int ble_ll_hci_adv_set_enable(const uint8_t *cmdbuf, uint8_t len);
 
-/* Set advertising data */
-int ble_ll_adv_set_adv_data(uint8_t *cmd, uint8_t cmd_len, uint8_t instance,
-                            uint8_t operation);
+/* Set legacy advertising data */
+int ble_ll_hci_set_adv_data(const uint8_t *cmdbuf, uint8_t len);
 
 /* Set scan response data */
-int ble_ll_adv_set_scan_rsp_data(uint8_t *cmd, uint8_t cmd_len,
-                                 uint8_t instance, uint8_t operation);
+int ble_ll_hci_set_scan_rsp_data(const uint8_t *cmd, uint8_t cmd_len);
 
 /* Set advertising parameters */
-int ble_ll_adv_set_adv_params(uint8_t *cmd);
+int ble_ll_adv_set_adv_params(const uint8_t *cmdbuf, uint8_t len);
 
 /* Read advertising channel power */
 int ble_ll_adv_read_txpwr(uint8_t *rspbuf, uint8_t *rsplen);
@@ -172,19 +170,34 @@ int ble_ll_adv_can_chg_whitelist(void);
  */
 void ble_ll_adv_event_rmvd_from_sched(struct ble_ll_adv_sm *advsm);
 
+/*
+ * Called when a periodic event has been removed from the scheduler
+ * without being run.
+ */
+void ble_ll_adv_periodic_rmvd_from_sched(struct ble_ll_adv_sm *advsm);
+
 /* Called to halt currently running advertising event */
 void ble_ll_adv_halt(void);
 
 /* Called to determine if advertising is enabled */
 uint8_t ble_ll_adv_enabled(void);
 
-int ble_ll_adv_set_random_addr(uint8_t *addr, uint8_t instance);
-int ble_ll_adv_remove(uint8_t instance);
+int ble_ll_adv_hci_set_random_addr(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_adv_set_random_addr(const uint8_t *addr, uint8_t instance);
+int ble_ll_adv_remove(const uint8_t *addr, uint8_t len);
 int ble_ll_adv_clear_all(void);
-int ble_ll_adv_ext_set_param(uint8_t *cmdbuf, uint8_t *rspbuf, uint8_t *rsplen);
-int ble_ll_adv_ext_set_adv_data(uint8_t *cmdbuf, uint8_t cmdlen);
-int ble_ll_adv_ext_set_scan_rsp(uint8_t *cmdbuf, uint8_t cmdlen);
-int ble_ll_adv_ext_set_enable(uint8_t *cmdbuf, uint8_t len);
+int ble_ll_adv_ext_set_param(const uint8_t *cmdbuf, uint8_t len,
+                             uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_adv_ext_set_adv_data(const uint8_t *cmdbuf, uint8_t cmdlen);
+int ble_ll_adv_ext_set_scan_rsp(const uint8_t *cmdbuf, uint8_t cmdlen);
+int ble_ll_adv_ext_set_enable(const uint8_t *cmdbuf, uint8_t len);
+
+int ble_ll_adv_periodic_set_param(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_adv_periodic_set_data(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_adv_periodic_enable(const uint8_t *cmdbuf, uint8_t len);
+
+int ble_ll_adv_periodic_set_info_transfer(const uint8_t *cmdbuf, uint8_t len,
+                                          uint8_t *rspbuf, uint8_t *rsplen);
 
 /* Called to notify adv code about RPA rotation */
 void ble_ll_adv_rpa_timeout(void);

nimble/controller/include/controller/ble_ll_conn.h

@@ -45,20 +45,12 @@ extern "C" {
 /* Channel map size */
 #define BLE_LL_CONN_CHMAP_LEN           (5)
 
-/* Definitions for source clock accuracy */
-#define BLE_MASTER_SCA_251_500_PPM      (0)
-#define BLE_MASTER_SCA_151_250_PPM      (1)
-#define BLE_MASTER_SCA_101_150_PPM      (2)
-#define BLE_MASTER_SCA_76_100_PPM       (3)
-#define BLE_MASTER_SCA_51_75_PPM        (4)
-#define BLE_MASTER_SCA_31_50_PPM        (5)
-#define BLE_MASTER_SCA_21_30_PPM        (6)
-#define BLE_MASTER_SCA_0_20_PPM         (7)
-
 /* Definition for RSSI when the RSSI is unknown */
 #define BLE_LL_CONN_UNKNOWN_RSSI        (127)
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#define BLE_LL_CONN_HANDLE_ISO_OFFSET   (0x0100)
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
 /*
  * Encryption states for a connection
  *
@@ -69,6 +61,7 @@ extern "C" {
 enum conn_enc_state {
     CONN_ENC_S_UNENCRYPTED = 1,
     CONN_ENC_S_ENCRYPTED,
+    CONN_ENC_S_ENC_RSP_TO_BE_SENT,
     CONN_ENC_S_ENC_RSP_WAIT,
     CONN_ENC_S_PAUSE_ENC_RSP_WAIT,
     CONN_ENC_S_PAUSED,
@@ -81,14 +74,20 @@ enum conn_enc_state {
 /*
  * Note that the LTK is the key, the SDK is the plain text, and the
  * session key is the cipher text portion of the encryption block.
+ *
+ * NOTE: we have intentionally violated the specification by making the
+ * transmit and receive packet counters 32-bits as opposed to 39 (as per the
+ * specification). We do this to save code space, ram and calculation time. The
+ * only drawback is that any encrypted connection that sends more than 2^32
+ * packets will suffer a MIC failure and thus be disconnected.
  */
 struct ble_ll_conn_enc_data
 {
     uint8_t enc_state;
     uint8_t tx_encrypted;
     uint16_t enc_div;
-    uint16_t tx_pkt_cntr;
-    uint16_t rx_pkt_cntr;
+    uint32_t tx_pkt_cntr;
+    uint32_t rx_pkt_cntr;
     uint64_t host_rand_num;
     uint8_t iv[8];
     struct ble_encryption_block enc_block;
@@ -101,6 +100,7 @@ union ble_ll_conn_sm_flags {
         uint32_t pkt_rxd:1;
         uint32_t terminate_ind_txd:1;
         uint32_t terminate_ind_rxd:1;
+        uint32_t terminate_ind_rxd_acked:1;
         uint32_t allow_slave_latency:1;
         uint32_t slave_set_last_anchor:1;
         uint32_t awaiting_host_reply:1;
@@ -126,6 +126,7 @@ union ble_ll_conn_sm_flags {
         uint32_t aux_conn_req: 1;
         uint32_t rxd_features:1;
         uint32_t pending_hci_rd_features:1;
+        uint32_t pending_initiate_dle:1;
     } cfbit;
     uint32_t conn_flags;
 } __attribute__((packed));
@@ -166,7 +167,16 @@ struct ble_ll_conn_phy_data
 #define CONN_CUR_TX_PHY_MASK(csm)   (1 << ((csm)->phy_data.cur_tx_phy - 1))
 #define CONN_CUR_RX_PHY_MASK(csm)   (1 << ((csm)->phy_data.cur_rx_phy - 1))
 
-#define BLE_PHY_TRANSITION_INVALID    (0xFF)
+struct hci_conn_update
+{
+    uint16_t handle;
+    uint16_t conn_itvl_min;
+    uint16_t conn_itvl_max;
+    uint16_t conn_latency;
+    uint16_t supervision_timeout;
+    uint16_t min_ce_len;
+    uint16_t max_ce_len;
+};
 
 /* Connection state machine */
 struct ble_ll_conn_sm
@@ -182,9 +192,6 @@ struct ble_ll_conn_sm
     /* RSSI */
     int8_t conn_rssi;
 
-    /* For privacy */
-    int8_t rpa_index;
-
     /* Connection data length management */
     uint8_t max_tx_octets;
     uint8_t max_rx_octets;
@@ -198,7 +205,9 @@ struct ble_ll_conn_sm
     uint16_t rem_max_rx_time;
     uint16_t eff_max_tx_time;
     uint16_t eff_max_rx_time;
-    uint8_t max_tx_octets_phy_mode[BLE_PHY_NUM_MODE];
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    uint16_t host_req_max_tx_time;
+#endif
 
 #if (BLE_LL_BT5_PHY_SUPPORTED == 1)
     struct ble_ll_conn_phy_data phy_data;
@@ -228,6 +237,10 @@ struct ble_ll_conn_sm
     uint8_t last_rxd_hdr_byte;  /* note: possibly can make 1 bit since we
                                    only use the MD bit now */
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_CTRL_TO_HOST_FLOW_CONTROL)
+    uint16_t cth_flow_pending;
+#endif
+
     /* connection event mgmt */
     uint8_t reject_reason;
     uint8_t host_reply_opcode;
@@ -270,8 +283,7 @@ struct ble_ll_conn_sm
     uint32_t last_rxd_pdu_cputime;  /* Used exclusively for supervision timer */
 
     /*
-     * Used to mark that direct advertising from the peer was using
-     * identity address as InitA
+     * Used to mark that identity address was used as InitA
      */
     uint8_t inita_identity_used;
 
@@ -279,6 +291,9 @@ struct ble_ll_conn_sm
     uint8_t own_addr_type;
     uint8_t peer_addr_type;
     uint8_t peer_addr[BLE_DEV_ADDR_LEN];
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    uint8_t peer_addr_resolved;
+#endif
 
     /*
      * XXX: TODO. Could save memory. Have single event at LL and put these
@@ -303,7 +318,7 @@ struct ble_ll_conn_sm
     /* For scheduling connections */
     struct ble_ll_sched_item conn_sch;
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING)
     struct ble_npl_callout auth_pyld_timer;
 #endif
 
@@ -315,7 +330,7 @@ struct ble_ll_conn_sm
      * allocate these from a pool? Not sure what to do. For now, I just use
      * a large chunk of memory per connection.
      */
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
     struct ble_ll_conn_enc_data enc_data;
 #endif
     /*
@@ -330,11 +345,11 @@ struct ble_ll_conn_sm
     /* XXX: for now, just store them all */
     struct ble_ll_conn_params conn_cp;
 
-    struct ble_ll_scan_sm *scansm;
-#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
-    struct hci_ext_create_conn initial_params;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+    uint8_t  sync_transfer_mode;
+    uint16_t sync_transfer_skip;
+    uint32_t sync_transfer_sync_timeout;
 #endif
-
 };
 
 /* Flags */
@@ -368,6 +383,38 @@ struct ble_ll_conn_sm *ble_ll_conn_find_active_conn(uint16_t handle);
 /* required for unit testing */
 uint8_t ble_ll_conn_calc_dci(struct ble_ll_conn_sm *conn, uint16_t latency);
 
+/* used to get anchor point for connection event specified */
+void ble_ll_conn_get_anchor(struct ble_ll_conn_sm *connsm, uint16_t conn_event,
+                            uint32_t *anchor, uint8_t *anchor_usecs);
+
+struct ble_ll_scan_addr_data;
+struct ble_ll_scan_pdu_data;
+
+uint8_t ble_ll_conn_tx_connect_ind_pducb(uint8_t *dptr, void *pducb_arg,
+                                         uint8_t *hdr_byte);
+void ble_ll_conn_prepare_connect_ind(struct ble_ll_conn_sm *connsm,
+                                    struct ble_ll_scan_pdu_data *pdu_data,
+                                    uint8_t adva_type, uint8_t *adva,
+                                    uint8_t inita_type, uint8_t *inita,
+                                    int rpa_index, uint8_t channel);
+
+/* Send CONNECT_IND/AUX_CONNECT_REQ */
+int ble_ll_conn_send_connect_req(struct os_mbuf *rxpdu,
+                                 struct ble_ll_scan_addr_data *addrd,
+                                 uint8_t ext);
+/* Cancel connection after AUX_CONNECT_REQ was sent */
+void ble_ll_conn_send_connect_req_cancel(void);
+/* Signal connection created via CONNECT_IND */
+void ble_ll_conn_created_on_legacy(struct os_mbuf *rxpdu,
+                                   struct ble_ll_scan_addr_data *addrd,
+                                   uint8_t *targeta);
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+/* Signal connection created via AUX_CONNECT_REQ */
+void ble_ll_conn_created_on_aux(struct os_mbuf *rxpdu,
+                                struct ble_ll_scan_addr_data *addrd,
+                                uint8_t *targeta);
+#endif
+
 #ifdef __cplusplus
 }
 #endif

nimble/controller/include/controller/ble_ll_ctrl.h

@@ -39,7 +39,9 @@ extern "C" {
 #define BLE_LL_CTRL_PROC_LE_PING        (7)
 #define BLE_LL_CTRL_PROC_DATA_LEN_UPD   (8)
 #define BLE_LL_CTRL_PROC_PHY_UPDATE     (9)
-#define BLE_LL_CTRL_PROC_NUM            (10)
+#define BLE_LL_CTRL_PROC_SCA_UPDATE     (10)
+#define BLE_LL_CTRL_PROC_CIS_CREATE     (11)
+#define BLE_LL_CTRL_PROC_NUM            (12)
 #define BLE_LL_CTRL_PROC_IDLE           (255)
 
 /* Checks if a particular control procedure is running */
@@ -54,41 +56,55 @@ extern "C" {
  *  -> Opcode   (1 byte)
  *  -> Data     (0 - 26 bytes)
  */
-#define BLE_LL_CTRL_CONN_UPDATE_IND     (0)
-#define BLE_LL_CTRL_CHANNEL_MAP_REQ     (1)
-#define BLE_LL_CTRL_TERMINATE_IND       (2)
-#define BLE_LL_CTRL_ENC_REQ             (3)
-#define BLE_LL_CTRL_ENC_RSP             (4)
-#define BLE_LL_CTRL_START_ENC_REQ       (5)
-#define BLE_LL_CTRL_START_ENC_RSP       (6)
-#define BLE_LL_CTRL_UNKNOWN_RSP         (7)
-#define BLE_LL_CTRL_FEATURE_REQ         (8)
-#define BLE_LL_CTRL_FEATURE_RSP         (9)
-#define BLE_LL_CTRL_PAUSE_ENC_REQ       (10)
-#define BLE_LL_CTRL_PAUSE_ENC_RSP       (11)
-#define BLE_LL_CTRL_VERSION_IND         (12)
-#define BLE_LL_CTRL_REJECT_IND          (13)
-#define BLE_LL_CTRL_SLAVE_FEATURE_REQ   (14)
-#define BLE_LL_CTRL_CONN_PARM_REQ       (15)
-#define BLE_LL_CTRL_CONN_PARM_RSP       (16)
-#define BLE_LL_CTRL_REJECT_IND_EXT      (17)
-#define BLE_LL_CTRL_PING_REQ            (18)
-#define BLE_LL_CTRL_PING_RSP            (19)
-#define BLE_LL_CTRL_LENGTH_REQ          (20)
-#define BLE_LL_CTRL_LENGTH_RSP          (21)
-#define BLE_LL_CTRL_PHY_REQ             (22)
-#define BLE_LL_CTRL_PHY_RSP             (23)
-#define BLE_LL_CTRL_PHY_UPDATE_IND      (24)
-#define BLE_LL_CTRL_MIN_USED_CHAN_IND   (25)
+#define BLE_LL_CTRL_CONN_UPDATE_IND     (0x00)
+#define BLE_LL_CTRL_CHANNEL_MAP_REQ     (0x01)
+#define BLE_LL_CTRL_TERMINATE_IND       (0x02)
+#define BLE_LL_CTRL_ENC_REQ             (0x03)
+#define BLE_LL_CTRL_ENC_RSP             (0x04)
+#define BLE_LL_CTRL_START_ENC_REQ       (0x05)
+#define BLE_LL_CTRL_START_ENC_RSP       (0x06)
+#define BLE_LL_CTRL_UNKNOWN_RSP         (0x07)
+#define BLE_LL_CTRL_FEATURE_REQ         (0x08)
+#define BLE_LL_CTRL_FEATURE_RSP         (0x09)
+#define BLE_LL_CTRL_PAUSE_ENC_REQ       (0x0A)
+#define BLE_LL_CTRL_PAUSE_ENC_RSP       (0x0B)
+#define BLE_LL_CTRL_VERSION_IND         (0x0C)
+#define BLE_LL_CTRL_REJECT_IND          (0x0D)
+#define BLE_LL_CTRL_SLAVE_FEATURE_REQ   (0x0E)
+#define BLE_LL_CTRL_CONN_PARM_REQ       (0x0F)
+#define BLE_LL_CTRL_CONN_PARM_RSP       (0x10)
+#define BLE_LL_CTRL_REJECT_IND_EXT      (0x11)
+#define BLE_LL_CTRL_PING_REQ            (0x12)
+#define BLE_LL_CTRL_PING_RSP            (0x13)
+#define BLE_LL_CTRL_LENGTH_REQ          (0x14)
+#define BLE_LL_CTRL_LENGTH_RSP          (0x15)
+#define BLE_LL_CTRL_PHY_REQ             (0x16)
+#define BLE_LL_CTRL_PHY_RSP             (0x17)
+#define BLE_LL_CTRL_PHY_UPDATE_IND      (0x18)
+#define BLE_LL_CTRL_MIN_USED_CHAN_IND   (0x19)
+#define BLE_LL_CTRL_CTE_REQ             (0x1A)
+#define BLE_LL_CTRL_CTE_RSP             (0x1B)
+#define BLE_LL_CTRL_PERIODIC_SYNC_IND   (0x1C)
+#define BLE_LL_CTRL_CLOCK_ACCURACY_REQ  (0x1D)
+#define BLE_LL_CTRL_CLOCK_ACCURACY_RSP  (0x1E)
+#define BLE_LL_CTRL_CIS_REQ             (0x1F)
+#define BLE_LL_CTRL_CIS_RSP             (0x20)
+#define BLE_LL_CTRL_CIS_IND             (0x21)
+#define BLE_LL_CTRL_CIS_TERMINATE_IND   (0x22)
 
 /* Maximum opcode value */
-#define BLE_LL_CTRL_OPCODES             (BLE_LL_CTRL_MIN_USED_CHAN_IND + 1)
+#define BLE_LL_CTRL_OPCODES             (BLE_LL_CTRL_CIS_TERMINATE_IND + 1)
 
 extern const uint8_t g_ble_ll_ctrl_pkt_lengths[BLE_LL_CTRL_OPCODES];
 
-/* Maximum # of payload bytes in a LL control PDU */
-#define BLE_LL_CTRL_MAX_PAYLOAD         (26)
+/* Maximum LL control PDU size */
+#if 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)
+#else
 #define BLE_LL_CTRL_MAX_PDU_LEN         (27)
+#endif
 
 /* LL control connection update request */
 struct ble_ll_conn_upd_req
@@ -240,6 +256,25 @@ struct ble_ll_len_req
 /* Min used channels */
 #define BLE_LL_CTRL_MIN_USED_CHAN_LEN   (2)
 
+/* CTE REQ */
+#define BLE_LL_CTRL_CTE_REQ_LEN         (1)
+
+/* CTE RSP (contains no data) */
+#define BLE_LL_CTRL_CTE_RSP_LEN     (0)
+
+/* Periodic Sync Transfer IND */
+#define BLE_LL_CTRL_PERIODIC_SYNC_IND_LEN   (34)
+
+/* Clock accuracy request/response */
+#define BLE_LL_CTRL_CLOCK_ACCURACY_REQ_LEN  (1)
+#define BLE_LL_CTRL_CLOCK_ACCURACY_RSP_LEN  (1)
+
+/* BLE ISO */
+#define BLE_LL_CTRL_CIS_REQ_LEN         (42)
+#define BLE_LL_CTRL_CIS_RSP_LEN         (8)
+#define BLE_LL_CTRL_CIS_IND_LEN         (15)
+#define BLE_LL_CTRL_CIS_TERMINATE_LEN   (3)
+
 /* API */
 struct ble_ll_conn_sm;
 void ble_ll_ctrl_proc_start(struct ble_ll_conn_sm *connsm, int ctrl_proc);
@@ -280,7 +315,17 @@ int ble_ll_hci_ev_phy_update(struct ble_ll_conn_sm *connsm, uint8_t status);
 void ble_ll_calc_session_key(struct ble_ll_conn_sm *connsm);
 void ble_ll_ctrl_phy_update_proc_complete(struct ble_ll_conn_sm *connsm);
 void ble_ll_ctrl_initiate_dle(struct ble_ll_conn_sm *connsm);
-void ble_ll_hci_ev_send_vendor_err(char *file, uint32_t line);
+void ble_ll_hci_ev_send_vendor_err(const char *file, uint32_t line);
+void ble_ll_hci_ev_send_llcp_trace(uint8_t type, uint16_t handle, uint16_t count,
+                                   void *pdu, size_t length);
+
+uint8_t ble_ll_ctrl_phy_tx_transition_get(uint8_t phy_mask);
+uint8_t ble_ll_ctrl_phy_from_phy_mask(uint8_t phy_mask);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_SCA_UPDATE)
+void ble_ll_hci_ev_sca_update(struct ble_ll_conn_sm *connsm,
+                              uint8_t status, uint8_t peer_sca);
+#endif
 
 #ifdef __cplusplus
 }

nimble/controller/include/controller/ble_ll_hci.h

@@ -24,8 +24,10 @@
 extern "C" {
 #endif
 
+#include "nimble/hci_common.h"
+
 /* For supported commands */
-#define BLE_LL_SUPP_CMD_LEN (40)
+#define BLE_LL_SUPP_CMD_LEN (45)
 extern const uint8_t g_ble_ll_supp_cmds[BLE_LL_SUPP_CMD_LEN];
 
 /* The largest event the controller will send. */
@@ -39,27 +41,33 @@ extern const uint8_t g_ble_ll_supp_cmds[BLE_LL_SUPP_CMD_LEN];
  */
 #define BLE_LL_CFG_NUM_HCI_CMD_PKTS     (1)
 
+typedef void (*ble_ll_hci_post_cmd_complete_cb)(void);
+
 /* Initialize LL HCI */
 void ble_ll_hci_init(void);
 
 /* Used to determine if the LE event is enabled/disabled */
-uint8_t ble_ll_hci_is_le_event_enabled(int subev);
+bool ble_ll_hci_is_le_event_enabled(unsigned int subev);
 
 /* Used to determine if event is enabled/disabled */
-uint8_t ble_ll_hci_is_event_enabled(int evcode);
+bool ble_ll_hci_is_event_enabled(unsigned int evcode);
 
 /* Send event from controller to host */
-int ble_ll_hci_event_send(uint8_t *evbuf);
+int ble_ll_hci_event_send(struct ble_hci_ev *hci_ev);
 
 /* Sends a command complete with a no-op opcode to host */
-int ble_ll_hci_send_noop(void);
+void ble_ll_hci_send_noop(void);
 
 /* Checks the preferref phy masks from set default phy and set phy commands */
-int ble_ll_hci_chk_phy_masks(uint8_t *cmdbuf, uint8_t *txphy, uint8_t *rxphy);
+int ble_ll_hci_chk_phy_masks(uint8_t all_phys, uint8_t tx_phys, uint8_t rx_phys,
+                             uint8_t *txphy, uint8_t *rxphy);
 
 /* Returns true if Extended Advertising HCI commands are in use */
 bool ble_ll_hci_adv_mode_ext(void);
 
+/* Get TX power compensation rounded to integer dB */
+int8_t ble_ll_get_tx_pwr_compensation(void);
+
 #ifdef __cplusplus
 }
 #endif

nimble/controller/include/controller/ble_ll_iso.h

@@ -0,0 +1,53 @@
+/*
+ * 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_ISO
+#define H_BLE_LL_ISO
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int ble_ll_iso_read_tx_sync(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_iso_set_cig_param(const uint8_t *cmdbuf, uint8_t len, uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_iso_set_cig_param_test(const uint8_t *cmdbuf, uint8_t len, uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_iso_create_cis(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_iso_disconnect_cmd(const struct ble_hci_lc_disconnect_cp *cmd);
+int ble_ll_iso_remove_cig(const uint8_t *cmdbuf, uint8_t len, uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_iso_accept_cis_req(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_iso_reject_cis_req(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_iso_create_big(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_iso_create_big_test(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_iso_terminate_big(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_iso_big_create_sync(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_iso_big_terminate_sync(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_iso_setup_iso_data_path(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_iso_remove_iso_data_path(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_iso_transmit_test(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_iso_receive_test(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_iso_read_counters_test(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_iso_end_test(const uint8_t *cmdbuf, uint8_t len);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

nimble/controller/include/controller/ble_ll_resolv.h

@@ -29,12 +29,16 @@ extern "C" {
  *      The identity address is stored in little endian format.
  *      The local rpa is stored in little endian format.
  *      The IRKs are stored in big endian format.
+ *
+ *  Note:
+ *  rl_local_irk and rl_peer_irk need to be word aligned
  */
 struct ble_ll_resolv_entry
 {
     uint8_t rl_addr_type;
-    uint8_t rl_reserved;
     uint8_t rl_priv_mode;
+    uint8_t rl_has_local;
+    uint8_t rl_has_peer;
     uint8_t rl_local_irk[16];
     uint8_t rl_peer_irk[16];
     uint8_t rl_identity_addr[BLE_DEV_ADDR_LEN];
@@ -51,25 +55,28 @@ int ble_ll_resolv_list_clr(void);
 int ble_ll_resolv_list_read_size(uint8_t *rspbuf, uint8_t *rsplen);
 
 /* Add a device to the resolving list */
-int ble_ll_resolv_list_add(uint8_t *cmdbuf);
+int ble_ll_resolv_list_add(const uint8_t *cmdbuf, uint8_t len);
 
 /* Remove a device from the resolving list */
-int ble_ll_resolv_list_rmv(uint8_t *cmdbuf);
+int ble_ll_resolv_list_rmv(const uint8_t *cmdbuf, uint8_t len);
 
 /* Address resolution enable command */
-int ble_ll_resolv_enable_cmd(uint8_t *cmdbuf);
+int ble_ll_resolv_enable_cmd(const uint8_t *cmdbuf, uint8_t len);
 
-int ble_ll_resolv_peer_addr_rd(uint8_t *cmdbuf, uint8_t *rspbuf,
-                               uint8_t *rsplen);
-int ble_ll_resolv_local_addr_rd(uint8_t *cmdbuf, uint8_t *rspbuf,
-                                uint8_t *rsplen);
+int ble_ll_resolv_peer_addr_rd(const uint8_t *cmdbuf, uint8_t len,
+                               uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_resolv_local_addr_rd(const uint8_t *cmdbuf, uint8_t len,
+                                uint8_t *rspbuf, uint8_t *rsplen);
 
 /* Finds 'addr' in resolving list. Doesnt check if address resolution enabled */
 struct ble_ll_resolv_entry *
-ble_ll_resolv_list_find(uint8_t *addr, uint8_t addr_type);
+ble_ll_resolv_list_find(const uint8_t *addr, uint8_t addr_type);
 
-/* Called to determine if the IRK is all zero. */
-int ble_ll_resolv_irk_nonzero(uint8_t *irk);
+static inline int8_t
+ble_ll_resolv_get_idx(struct ble_ll_resolv_entry *rl)
+{
+    return rl - g_ble_ll_resolv_list;
+}
 
 /* Returns true if address resolution is enabled */
 uint8_t ble_ll_resolv_enabled(void);
@@ -77,26 +84,33 @@ uint8_t ble_ll_resolv_enabled(void);
 /* Reset private address resolution */
 void ble_ll_resolv_list_reset(void);
 
+/* Generate local or peer RPA. It is up to caller to make sure required IRK
+ * is present on RL
+ */
 void ble_ll_resolv_get_priv_addr(struct ble_ll_resolv_entry *rl, int local,
                                  uint8_t *addr);
 
 void ble_ll_resolv_set_peer_rpa(int index, uint8_t *rpa);
+void ble_ll_resolv_set_local_rpa(int index, uint8_t *rpa);
 
 /* Generate a resolvable private address. */
 int ble_ll_resolv_gen_rpa(uint8_t *addr, uint8_t addr_type, uint8_t *rpa,
                           int local);
 
 /* Set the resolvable private address timeout */
-int ble_ll_resolv_set_rpa_tmo(uint8_t *cmdbuf);
+int ble_ll_resolv_set_rpa_tmo(const uint8_t *cmdbuf, uint8_t len);
 
 /* Set the privacy mode */
-int ble_ll_resolve_set_priv_mode(uint8_t *cmdbuf);
+int ble_ll_resolve_set_priv_mode(const uint8_t *cmdbuf, uint8_t len);
 
 /* Get the RPA timeout, in seconds */
 uint32_t ble_ll_resolv_get_rpa_tmo(void);
 
 /* Resolve a resolvable private address */
-int ble_ll_resolv_rpa(uint8_t *rpa, uint8_t *irk);
+int ble_ll_resolv_rpa(const uint8_t *rpa, const uint8_t *irk);
+
+/* Try to resolve peer RPA and return index on RL if matched */
+int ble_ll_resolv_peer_rpa_any(const uint8_t *rpa);
 
 /* Initialize resolv*/
 void ble_ll_resolv_init(void);

nimble/controller/include/controller/ble_ll_rfmgmt.h

@@ -0,0 +1,63 @@
+/*
+ * 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_RFMGMT_
+#define H_BLE_LL_RFMGMT_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void ble_ll_rfmgmt_init(void);
+
+#if MYNEWT_VAL(BLE_LL_RFMGMT_ENABLE_TIME) > 0
+
+void ble_ll_rfmgmt_reset(void);
+
+/* Notify rfmgmt that scan window has changed (only called from ble_ll_scan) */
+void ble_ll_rfmgmt_scan_changed(bool enabled, uint32_t next_window);
+
+/* Notify rfmgmt that 1st scheduled item has changed (only called from ble_ll_sched) */
+void ble_ll_rfmgmt_sched_changed(struct ble_ll_sched_item *first);
+
+/* Notify rfmgmt that RF is no longer needed by current event */
+void ble_ll_rfmgmt_release(void);
+
+/* Enables RF immediately and returns tick at which RF will be fully enabled */
+uint32_t ble_ll_rfmgmt_enable_now(void);
+
+/* Returns true only if RF is currently fully enabled (i.e. not off or enabling) */
+bool ble_ll_rfmgmt_is_enabled(void);
+
+#else
+
+static inline void ble_ll_rfmgmt_reset(void) { }
+static inline void ble_ll_rfmgmt_scan_changed(bool e, uint32_t n) { }
+static inline void ble_ll_rfmgmt_sched_changed(struct ble_ll_sched_item *f) { }
+static inline void ble_ll_rfmgmt_release(void) { }
+static inline uint32_t ble_ll_rfmgmt_enable_now(void) { return os_cputime_get32(); }
+static inline bool ble_ll_rfmgmt_is_enabled(void) { return true; }
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_LL_RFMGMT_ */

nimble/controller/include/controller/ble_ll_scan.h

@@ -68,94 +68,92 @@ extern "C" {
 
 #define PHY_UNCODED                    (0)
 #define PHY_CODED                      (1)
-#define PHY_NOT_CONFIGURED             (0xFF)
 
 #define BLE_LL_EXT_ADV_MODE_NON_CONN    (0x00)
 #define BLE_LL_EXT_ADV_MODE_CONN        (0x01)
 #define BLE_LL_EXT_ADV_MODE_SCAN        (0x02)
 
-struct ble_ll_scan_params
+/* All values are stored as ticks */
+struct ble_ll_scan_timing {
+    uint32_t interval;
+    uint32_t window;
+    uint32_t start_time;
+};
+
+struct ble_ll_scan_phy
 {
     uint8_t phy;
-    uint8_t own_addr_type;
-    uint8_t scan_filt_policy;
     uint8_t configured;
     uint8_t scan_type;
     uint8_t scan_chan;
-    uint16_t scan_itvl;
-    uint16_t scan_window;
-    uint32_t scan_win_start_time;
-    uint32_t next_event_start;
+    struct ble_ll_scan_timing timing;
 };
 
-#define BLE_LL_AUX_CHAIN_BIT            0x01
-#define BLE_LL_AUX_INCOMPLETE_BIT       0x02
-#define BLE_LL_AUX_INCOMPLETE_ERR_BIT   0x04
-#define BLE_LL_AUX_HAS_ADDRA            0x08
-#define BLE_LL_AUX_IGNORE_BIT           0x10
-#define BLE_LL_AUX_HAS_DIR_ADDRA        0x20
-#define BLE_LL_AUX_TRUNCATED_SENT       0x40
-#define BLE_LL_AUX_HAS_ADI              0x80
-
-#define BLE_LL_SET_AUX_FLAG(aux_data, flag) ((aux_data)->flags |= flag)
-#define BLE_LL_CHECK_AUX_FLAG(aux_data, flag) (!!((aux_data)->flags & flag))
-
-struct ble_ll_aux_data {
-    uint8_t ref_cnt;
-    uint8_t chan;
-    uint8_t aux_phy;
-    uint8_t aux_primary_phy;
-    uint8_t mode;
-    uint8_t scanning;
-    uint8_t flags;
-    uint16_t adi;
-    uint32_t offset;
-    uint8_t offset_units;
-    uint8_t addr[6];
-    uint8_t addr_type;
-    uint8_t dir_addr[6];
-    uint8_t dir_addr_type;
-    uint8_t evt_type;
-    struct ble_ll_sched_item sch;
-    struct ble_ll_ext_adv_report *evt;
+struct ble_ll_scan_pdu_data {
+    uint8_t hdr_byte;
+    /* ScanA for SCAN_REQ and InitA for CONNECT_IND */
+    union {
+        uint8_t scana[BLE_DEV_ADDR_LEN];
+        uint8_t inita[BLE_DEV_ADDR_LEN];
+    };
+    uint8_t adva[BLE_DEV_ADDR_LEN];
+};
+
+struct ble_ll_scan_addr_data {
+    uint8_t *adva;
+    uint8_t *targeta;
+    uint8_t *adv_addr;
+    uint8_t adva_type : 1;
+    uint8_t targeta_type : 1;
+    uint8_t adv_addr_type : 1;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    uint8_t adva_resolved : 1;
+    uint8_t targeta_resolved : 1;
+    int8_t rpa_index;
+#endif
 };
 
 struct ble_ll_scan_sm
 {
     uint8_t scan_enabled;
+
     uint8_t own_addr_type;
+    uint8_t scan_filt_policy;
     uint8_t scan_filt_dups;
     uint8_t scan_rsp_pending;
     uint8_t scan_rsp_cons_fails;
     uint8_t scan_rsp_cons_ok;
-    int8_t scan_rpa_index;
     uint8_t scan_peer_rpa[BLE_DEV_ADDR_LEN];
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
     ble_npl_time_t scan_nrpa_timer;
     uint8_t scan_nrpa[BLE_DEV_ADDR_LEN];
 #endif
+    struct ble_ll_scan_pdu_data pdu_data;
 
     /* XXX: Shall we count backoff per phy? */
     uint16_t upper_limit;
     uint16_t backoff_count;
     uint32_t scan_win_start_time;
-    struct os_mbuf *scan_req_pdu;
     struct ble_npl_event scan_sched_ev;
     struct hal_timer scan_timer;
+    struct ble_npl_event scan_interrupted_ev;
 
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
-    struct hal_timer duration_timer;
-    struct hal_timer period_timer;
-    uint32_t duration_ticks;
-    uint32_t period_ticks;
+    struct ble_npl_callout duration_timer;
+    struct ble_npl_callout period_timer;
+    ble_npl_time_t duration_ticks;
+    ble_npl_time_t period_ticks;
     uint8_t ext_scanning;
 #endif
 
-    uint8_t cur_phy;
-    uint8_t next_phy;
     uint8_t restart_timer_needed;
-    struct ble_ll_aux_data *cur_aux_data;
-    struct ble_ll_scan_params phy_data[BLE_LL_SCAN_PHY_NUMBER];
+
+    struct ble_ll_scan_phy *scanp;
+    struct ble_ll_scan_phy *scanp_next;
+    struct ble_ll_scan_phy scan_phys[BLE_LL_SCAN_PHY_NUMBER];
+
+    /* Connection sm for initiator scan */
+    struct ble_ll_conn_sm *connsm;
 };
 
 /* Scan types */
@@ -165,13 +163,14 @@ struct ble_ll_scan_sm
 
 /*---- HCI ----*/
 /* Set scanning parameters */
-int ble_ll_scan_set_scan_params(uint8_t *cmd);
+int ble_ll_scan_hci_set_params(const uint8_t *cmdbuf, uint8_t len);
 
 /* Turn scanning on/off */
-int ble_ll_scan_set_enable(uint8_t *cmd, uint8_t ext);
+int ble_ll_scan_hci_set_enable(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_scan_hci_set_ext_enable(const uint8_t *cmdbuf, uint8_t len);
 
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
-int ble_ll_set_ext_scan_params(uint8_t *cmd, uint8_t cmdlen);
+int ble_ll_scan_hci_set_ext_params(const uint8_t *cmdbuf, uint8_t len);
 #endif
 
 /*--- Controller Internal API ---*/
@@ -197,16 +196,15 @@ int ble_ll_scan_can_chg_whitelist(void);
 /* Boolean function returning true if scanning enabled */
 int ble_ll_scan_enabled(void);
 
-/* Boolean function returns true if whitelist is enabled for scanning */
-int ble_ll_scan_whitelist_enabled(void);
-
 /* Initialize the scanner when we start initiating */
-struct hci_create_conn;
-int ble_ll_scan_initiator_start(struct hci_create_conn *hcc,
-                                struct ble_ll_scan_sm **sm);
+struct ble_ll_conn_create_scan;
+struct ble_ll_conn_create_params;
+int
+ble_ll_scan_initiator_start(struct ble_ll_conn_sm *connsm, uint8_t ext,
+                            struct ble_ll_conn_create_scan *cc_scan);
 
-/* Returns the PDU allocated by the scanner */
-struct os_mbuf *ble_ll_scan_get_pdu(void);
+/* Returns storage for PDU data (for SCAN_REQ or CONNECT_IND) */
+struct ble_ll_scan_pdu_data *ble_ll_scan_get_pdu_data(void);
 
 /* Called to set the resolvable private address of the last connected peer */
 void ble_ll_scan_set_peer_rpa(uint8_t *rpa);
@@ -226,36 +224,37 @@ void ble_ll_scan_chk_resume(void);
 /* Called when wait for response timer expires in scanning mode */
 void ble_ll_scan_wfr_timer_exp(void);
 
-int ble_ll_scan_adv_decode_addr(uint8_t pdu_type, uint8_t *rxbuf,
-                                struct ble_mbuf_hdr *ble_hdr,
-                                uint8_t **addr, uint8_t *addr_type,
-                                uint8_t **inita, uint8_t *init_addr_type,
-                                int *ext_mode);
+/* Called when scan could be interrupted  */
+void ble_ll_scan_interrupted(struct ble_ll_scan_sm *scansm);
 
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
-/* Get aux ptr from ext advertising */
-int ble_ll_scan_get_aux_data(struct ble_mbuf_hdr *ble_hdr, uint8_t *rxbuf);
-
-/* Initialize the extended scanner when we start initiating */
-struct hci_ext_create_conn;
-int ble_ll_scan_ext_initiator_start(struct hci_ext_create_conn *hcc,
-                                    struct ble_ll_scan_sm **sm);
-
 /* Called to parse extended advertising*/
-struct ble_ll_ext_adv_report;
-int ble_ll_scan_parse_ext_hdr(struct os_mbuf *om,
-                              uint8_t *adva, uint8_t adva_type,
-                              uint8_t *inita, uint8_t inita_type,
-                              struct ble_mbuf_hdr *ble_hdr,
-                              struct ble_ll_ext_adv_report *parsed_evt);
-
-void ble_ll_scan_aux_data_ref(struct ble_ll_aux_data *aux_scan);
-int ble_ll_scan_aux_data_unref(struct ble_ll_aux_data *aux_scan);
+void ble_ll_scan_end_adv_evt(struct ble_ll_aux_data *aux_data);
 #endif
 
-/* Called to clean up current aux data */
-void ble_ll_scan_clean_cur_aux_data(void);
-void ble_ll_scan_end_adv_evt(struct ble_ll_aux_data *aux_data);
+/* Called to halt currently running scan */
+void ble_ll_scan_halt(void);
+
+uint8_t *ble_ll_get_scan_nrpa(void);
+uint8_t ble_ll_scan_get_own_addr_type(void);
+uint8_t ble_ll_scan_get_filt_policy(void);
+uint8_t ble_ll_scan_get_filt_dups(void);
+uint8_t ble_ll_scan_backoff_kick(void);
+void ble_ll_scan_backoff_update(int success);
+
+int ble_ll_scan_dup_check_ext(uint8_t addr_type, uint8_t *addr, bool has_aux,
+                              uint16_t adi);
+int ble_ll_scan_dup_update_ext(uint8_t addr_type, uint8_t *addr, bool has_aux,
+                               uint16_t adi);
+int ble_ll_scan_have_rxd_scan_rsp(uint8_t *addr, uint8_t txadd, uint8_t ext_adv,
+                                  uint16_t adi);
+void ble_ll_scan_add_scan_rsp_adv(uint8_t *addr, uint8_t txadd, uint8_t ext_adv,
+                                  uint16_t adi);
+
+int
+ble_ll_scan_rx_filter(uint8_t own_addr_type, uint8_t scan_filt_policy,
+                      struct ble_ll_scan_addr_data *addrd, uint8_t *scan_ok);
+int ble_ll_scan_rx_check_init(struct ble_ll_scan_addr_data *addrd);
 
 #ifdef __cplusplus
 }

nimble/controller/include/controller/ble_ll_scan_aux.h

@@ -0,0 +1,54 @@
+/*
+ * 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_SCAN_AUX_
+#define H_BLE_LL_SCAN_AUX_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+
+struct ble_ll_scan_aux_data;
+
+void ble_ll_scan_aux_init(void);
+int ble_ll_scan_aux_sched(struct ble_ll_scan_aux_data *aux, uint32_t pdu_time,
+                          uint8_t pdu_time_rem, uint32_t aux_ptr);
+int ble_ll_scan_aux_rx_isr_start(uint8_t pdu_type, struct ble_mbuf_hdr *rxhdr);
+int ble_ll_scan_aux_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok);
+void ble_ll_scan_aux_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *rxhdr);
+
+void ble_ll_scan_aux_break(struct ble_ll_scan_aux_data *aux);
+void ble_ll_scan_aux_wfr_timer_exp(void);
+void ble_ll_scan_aux_halt(void);
+void ble_ll_scan_aux_sched_remove(struct ble_ll_sched_item *sch);
+
+int ble_ll_scan_aux_rx_isr_end_on_ext(struct ble_ll_scan_sm *scansm,
+                                      struct os_mbuf *rxpdu);
+void ble_ll_scan_aux_pkt_in_on_ext(struct os_mbuf *rxpdu,
+                                   struct ble_mbuf_hdr *rxhdr);
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_LL_SCAN_AUX_ */

nimble/controller/include/controller/ble_ll_sched.h

@@ -25,8 +25,8 @@ extern "C" {
 #endif
 
 /* Time per BLE scheduler slot */
-#define BLE_LL_SCHED_USECS_PER_SLOT         (1250)
-#define BLE_LL_SCHED_32KHZ_TICKS_PER_SLOT   (41)    /* 1 tick = 30.517 usecs */
+#define BLE_LL_SCHED_USECS_PER_SLOT   (1250)
+#define BLE_LL_SCHED_TICKS_PER_SLOT   (41)    /* 1 tick = 30.517 usecs */
 
 /*
  * Worst case time needed for scheduled advertising item. This is the longest
@@ -47,11 +47,6 @@ extern "C" {
 #define BLE_LL_SCHED_DIRECT_ADV_MAX_USECS   (502)
 #define BLE_LL_SCHED_MAX_ADV_PDU_USECS      (376)
 
-/* We don't know how big aux packet will be. It depends on adv packet len which
- * can be up to 256. For now we fix it to 128 bytes, but we can optimize it.
- */
-#define BLE_LL_SCHED_AUX_PTR_DFLT_BYTES_NUM  (128)
-
 /*
  * This is the offset from the start of the scheduled item until the actual
  * tx/rx should occur, in ticks.
@@ -72,8 +67,10 @@ extern uint8_t g_ble_ll_sched_offset_ticks;
 #define BLE_LL_SCHED_TYPE_ADV       (1)
 #define BLE_LL_SCHED_TYPE_SCAN      (2)
 #define BLE_LL_SCHED_TYPE_CONN      (3)
-#define BLE_LL_SCHED_TYPE_AUX_SCAN  (4)
 #define BLE_LL_SCHED_TYPE_DTM       (5)
+#define BLE_LL_SCHED_TYPE_PERIODIC  (6)
+#define BLE_LL_SCHED_TYPE_SYNC      (7)
+#define BLE_LL_SCHED_TYPE_SCAN_AUX  (8)
 
 /* Return values for schedule callback. */
 #define BLE_LL_SCHED_STATE_RUNNING  (0)
@@ -161,8 +158,19 @@ typedef void ble_ll_sched_adv_new_cb(struct ble_ll_adv_sm *advsm,
 int ble_ll_sched_adv_new(struct ble_ll_sched_item *sch,
                          ble_ll_sched_adv_new_cb cb, void *arg);
 
+/* Schedule periodic advertising event */
+int ble_ll_sched_periodic_adv(struct ble_ll_sched_item *sch, bool first_event);
+
+int ble_ll_sched_sync_reschedule(struct ble_ll_sched_item *sch,
+                                 uint32_t anchor_point,
+                                 uint8_t anchor_point_usecs,
+                                 uint32_t window_widening, int8_t phy_mode);
+int ble_ll_sched_sync(struct ble_ll_sched_item *sch,
+                      uint32_t beg_cputime, uint32_t rem_usecs, uint32_t offset,
+                      int8_t phy_mode);
+
 /* Reschedule an advertising event */
-int ble_ll_sched_adv_reschedule(struct ble_ll_sched_item *sch, uint32_t *start,
+int ble_ll_sched_adv_reschedule(struct ble_ll_sched_item *sch,
                                 uint32_t max_delay_ticks);
 
 /* Reschedule and advertising pdu */
@@ -190,18 +198,14 @@ int ble_ll_sched_aux_scan(struct ble_mbuf_hdr *ble_hdr,
                           struct ble_ll_scan_sm *scansm,
                           struct ble_ll_aux_data *aux_scan);
 
-int ble_ll_sched_scan_req_over_aux_ptr(uint32_t chan, uint8_t phy_mode);
+int ble_ll_sched_scan_aux(struct ble_ll_sched_item *sch, uint32_t pdu_time,
+                          uint8_t pdu_time_rem, uint32_t offset_us);
 #endif
 
 /* Stop the scheduler */
 void ble_ll_sched_stop(void);
 
-#ifdef BLE_XCVR_RFCLK
-/* Check if RF clock needs to be restarted */
-void ble_ll_sched_rfclk_chk_restart(void);
-#endif
-
-#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 1
+#if MYNEWT_VAL(BLE_LL_DTM)
 int ble_ll_sched_dtm(struct ble_ll_sched_item *sch);
 #endif
 

nimble/controller/include/controller/ble_ll_sync.h

@@ -0,0 +1,74 @@
+/*
+ * 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_SYNC_
+#define H_BLE_LL_SYNC_
+
+#include <stdint.h>
+
+#include "nimble/ble.h"
+#include "controller/ble_ll_hci.h"
+#include "controller/ble_ll_conn.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct ble_ll_scan_addr_data;
+struct ble_ll_sync_sm;
+
+int ble_ll_sync_create(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_sync_cancel(ble_ll_hci_post_cmd_complete_cb *post_cmd_cb);
+int ble_ll_sync_terminate(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_sync_list_add(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_sync_list_remove(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_sync_list_clear(void);
+int ble_ll_sync_list_size(uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_sync_receive_enable(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_sync_transfer(const uint8_t *cmdbuf, uint8_t len,
+                         uint8_t *rspbuf, uint8_t *rsplen);
+
+void ble_ll_sync_periodic_ind(struct ble_ll_conn_sm *connsm,
+                              const uint8_t *sync_ind, bool reports_disabled,
+                              uint16_t max_skip, uint32_t sync_timeout);
+void ble_ll_sync_transfer_disconnected(struct ble_ll_conn_sm *connsm);
+
+void ble_ll_sync_info_event(struct ble_ll_scan_addr_data *addrd, uint8_t sid,
+                            struct ble_mbuf_hdr *rxhdr,
+                            const uint8_t *syncinfo);
+
+int ble_ll_sync_rx_isr_start(uint8_t pdu_type, struct ble_mbuf_hdr *rxhdr);
+int ble_ll_sync_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr);
+void ble_ll_sync_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr);
+void ble_ll_sync_wfr_timer_exp(void);
+void ble_ll_sync_halt(void);
+void ble_ll_sync_rmvd_from_sched(struct ble_ll_sync_sm *sm);
+
+uint32_t ble_ll_sync_get_event_end_time(void);
+
+bool ble_ll_sync_enabled(void);
+
+void ble_ll_sync_reset(void);
+void ble_ll_sync_init(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_LL_SYNC_ */

nimble/controller/include/controller/ble_ll_utils.h

@@ -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.
+ */
+
+#include <stdint.h>
+
+uint32_t ble_ll_utils_calc_access_addr(void);
+uint8_t ble_ll_utils_remapped_channel(uint8_t remap_index, const uint8_t *chanmap);
+uint8_t ble_ll_utils_calc_dci_csa2(uint16_t event_cntr, uint16_t channel_id,
+                                   uint8_t num_used_chans, const uint8_t *chanmap);
+uint8_t ble_ll_utils_calc_num_used_chans(const uint8_t *chanmap);
+uint32_t ble_ll_utils_calc_window_widening(uint32_t anchor_point,
+                                           uint32_t last_anchor_point,
+                                           uint8_t master_sca);

nimble/controller/include/controller/ble_ll_whitelist.h

@@ -31,10 +31,10 @@ int ble_ll_whitelist_clear(void);
 int ble_ll_whitelist_read_size(uint8_t *rspbuf, uint8_t *rsplen);
 
 /* Add a device to the whitelist */
-int ble_ll_whitelist_add(uint8_t *addr, uint8_t addr_type);
+int ble_ll_whitelist_add(const uint8_t *cmdbuf, uint8_t len);
 
 /* Remove a device fromthe whitelist */
-int ble_ll_whitelist_rmv(uint8_t *addr, uint8_t addr_type);
+int ble_ll_whitelist_rmv(const uint8_t *cmdbuf, uint8_t len);
 
 /* Enable whitelisting */
 void ble_ll_whitelist_enable(void);

nimble/controller/include/controller/ble_phy.h

@@ -26,14 +26,6 @@
 extern "C" {
 #endif
 
-#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) && !(MYNEWT_VAL(BSP_NRF52) || MYNEWT_VAL(BSP_NRF52840))
-#error LE 2M PHY can only be enabled on nRF52xxx
-#endif
-
-#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) && !MYNEWT_VAL(BSP_NRF52840)
-#error LE Coded PHY can only be enabled on nRF52840
-#endif
-
 /* Forward declarations */
 struct os_mbuf;
 
@@ -118,9 +110,6 @@ int ble_phy_rx(void);
 /* Copies the received PHY buffer into the allocated pdu */
 void ble_phy_rxpdu_copy(uint8_t *dptr, struct os_mbuf *rxpdu);
 
-/* Get an RSSI reading */
-int ble_phy_rssi_get(void);
-
 /* Set the transmit power */
 int ble_phy_txpwr_set(int dbm);
 
@@ -130,13 +119,15 @@ int ble_phy_txpower_round(int dbm);
 /* Get the transmit power */
 int ble_phy_txpwr_get(void);
 
+/* Set RX path power compensation value rounded to integer dB */
+void ble_phy_set_rx_pwr_compensation(int8_t compensation);
+
 /* Disable the PHY */
 void ble_phy_disable(void);
 
 #define BLE_PHY_WFR_ENABLE_RX       (0)
 #define BLE_PHY_WFR_ENABLE_TXRX     (1)
 
-void ble_phy_stop_usec_timer(void);
 void ble_phy_wfr_enable(int txrx, uint8_t tx_phy_mode, uint32_t wfr_usecs);
 
 /* Starts rf clock */
@@ -188,17 +179,15 @@ void ble_phy_resolv_list_enable(void);
 void ble_phy_resolv_list_disable(void);
 
 /*
- * These definitions are used for the 'phy' parameters in the API listed below.
- * These are numbered in a specific order to save code. The HCI definitions for
- * the PHY modes for 1Mbps and 2Mbps are the same here. For the coded phy
- * they need to be translated from the HCI number to either 0 or 3. This
- * was done in order to save code when translating between the HCI phy value
- * and the phy API.
+ * PHY mode values for 1M, 2M and Coded S=8 are the same as corresponding values
+ * of PHY. This makes conversion between 'phy' and 'phy_mode' easier and it also
+ * means that default coding for Coded will be S=8, unless explicitly translated
+ * to S=2.
  */
+#define BLE_PHY_MODE_CODED_500KBPS  (0)
 #define BLE_PHY_MODE_1M             (1)
 #define BLE_PHY_MODE_2M             (2)
-#define BLE_PHY_MODE_CODED_125KBPS  (0)
-#define BLE_PHY_MODE_CODED_500KBPS  (3)
+#define BLE_PHY_MODE_CODED_125KBPS  (3)
 
 /* The number of different modes */
 #define BLE_PHY_NUM_MODE            (4)
@@ -213,9 +202,14 @@ void ble_phy_resolv_list_disable(void);
 #define BLE_PHY_MASK_2M             (BLE_HCI_LE_PHY_2M_PREF_MASK)
 #define BLE_PHY_MASK_CODED          (BLE_HCI_LE_PHY_CODED_PREF_MASK)
 
+/* PHY indices (for a zero-based array) */
+#define BLE_PHY_IDX_1M              (0)
+#define BLE_PHY_IDX_2M              (1)
+#define BLE_PHY_IDX_CODED           (2)
+
 #if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) || MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY))
 uint32_t ble_phy_mode_pdu_start_off(int phy);
-void ble_phy_mode_set(uint8_t new_phy_mode, uint8_t txtorx_phy_mode);
+void ble_phy_mode_set(uint8_t tx_phy_mode, uint8_t rx_phy_mode);
 #else
 #define ble_phy_mode_pdu_start_off(phy)     (40)
 
@@ -227,23 +221,23 @@ static inline int ble_ll_phy_to_phy_mode(int phy, int phy_options)
     int phy_mode;
 
     /*
-     * Mode values are set in a way that 1M, 2M and Coded(S=2) are equivalent
-     * to 1M, 2M and Coded in HCI. The only conversion is needed for Coded(S=8)
-     * which uses non-HCI value.
+     * 'phy' value can be used as 'phy_mode' value unless S=2 coding is explicitly
+     * required. By default we'll use S=2 for Coded.
      */
-
     phy_mode = phy;
 
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
-    if (phy == BLE_PHY_CODED && phy_options == BLE_HCI_LE_PHY_CODED_S8_PREF) {
-        phy_mode = BLE_PHY_MODE_CODED_125KBPS;
+    if (phy == BLE_PHY_CODED && phy_options == BLE_HCI_LE_PHY_CODED_S2_PREF) {
+        phy_mode = BLE_PHY_MODE_CODED_500KBPS;
     }
+#else
+    (void)phy_options;
 #endif
 
     return phy_mode;
 }
 
-#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE)
+#if MYNEWT_VAL(BLE_LL_DTM)
 void ble_phy_enable_dtm(void);
 void ble_phy_disable_dtm(void);
 #endif

nimble/controller/src/ble_ll.c

@@ -37,14 +37,17 @@
 #include "controller/ble_ll_adv.h"
 #include "controller/ble_ll_sched.h"
 #include "controller/ble_ll_scan.h"
+#include "controller/ble_ll_scan_aux.h"
 #include "controller/ble_ll_hci.h"
 #include "controller/ble_ll_whitelist.h"
 #include "controller/ble_ll_resolv.h"
-#include "controller/ble_ll_xcvr.h"
+#include "controller/ble_ll_rfmgmt.h"
 #include "controller/ble_ll_trace.h"
+#include "controller/ble_ll_sync.h"
 #include "ble_ll_conn_priv.h"
+#include "ble_ll_priv.h"
 
-#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 1
+#if MYNEWT_VAL(BLE_LL_DTM)
 #include "ble_ll_dtm_priv.h"
 #endif
 
@@ -204,9 +207,21 @@ STATS_NAME_START(ble_ll_stats)
     STATS_NAME(ble_ll_stats, aux_scan_rsp_err)
     STATS_NAME(ble_ll_stats, aux_chain_cnt)
     STATS_NAME(ble_ll_stats, aux_chain_err)
+    STATS_NAME(ble_ll_stats, aux_scan_drop)
     STATS_NAME(ble_ll_stats, adv_evt_dropped)
     STATS_NAME(ble_ll_stats, scan_timer_stopped)
     STATS_NAME(ble_ll_stats, scan_timer_restarted)
+    STATS_NAME(ble_ll_stats, periodic_adv_drop_event)
+    STATS_NAME(ble_ll_stats, periodic_chain_drop_event)
+    STATS_NAME(ble_ll_stats, sync_event_failed)
+    STATS_NAME(ble_ll_stats, sync_received)
+    STATS_NAME(ble_ll_stats, sync_chain_failed)
+    STATS_NAME(ble_ll_stats, sync_missed_err)
+    STATS_NAME(ble_ll_stats, sync_crc_err)
+    STATS_NAME(ble_ll_stats, sync_rx_buf_err)
+    STATS_NAME(ble_ll_stats, sync_scheduled)
+    STATS_NAME(ble_ll_stats, sched_state_sync_errs)
+    STATS_NAME(ble_ll_stats, sched_invalid_pdu)
 STATS_NAME_END(ble_ll_stats)
 
 static void ble_ll_event_rx_pkt(struct ble_npl_event *ev);
@@ -217,8 +232,7 @@ static void ble_ll_event_dbuf_overflow(struct ble_npl_event *ev);
 
 /* The BLE LL task data structure */
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
-/* TODO: This is for testing. Check it we really need it */
-#define BLE_LL_STACK_SIZE   (128)
+#define BLE_LL_STACK_SIZE   (120)
 #else
 #define BLE_LL_STACK_SIZE   (90)
 #endif
@@ -284,7 +298,7 @@ ble_ll_count_rx_adv_pdus(uint8_t pdu_type)
     case BLE_ADV_PDU_TYPE_SCAN_RSP:
         STATS_INC(ble_ll_stats, rx_scan_rsps);
         break;
-    case BLE_ADV_PDU_TYPE_CONNECT_REQ:
+    case BLE_ADV_PDU_TYPE_CONNECT_IND:
         STATS_INC(ble_ll_stats, rx_connect_reqs);
         break;
     case BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP:
@@ -298,66 +312,77 @@ ble_ll_count_rx_adv_pdus(uint8_t pdu_type)
     }
 }
 
-/**
- * Allocate a pdu (chain) for reception.
- *
- * @param len
- *
- * @return struct os_mbuf*
- */
 struct os_mbuf *
 ble_ll_rxpdu_alloc(uint16_t len)
 {
-    uint16_t mb_bytes;
-    struct os_mbuf *m;
-    struct os_mbuf *n;
-    struct os_mbuf *p;
+    struct os_mbuf *om_ret;
+    struct os_mbuf *om_next;
+    struct os_mbuf *om;
     struct os_mbuf_pkthdr *pkthdr;
+    uint16_t databuf_len;
+    int rem_len;
 
-    p = os_msys_get_pkthdr(len, sizeof(struct ble_mbuf_hdr));
-    if (!p) {
-        goto rxpdu_alloc_exit;
+    /*
+     * Make sure that data in mbuf are word-aligned with and without packet
+     * header. This is essential for proper and quick copying of received PDUs
+     * into mbufs.
+     */
+    _Static_assert((offsetof(struct os_mbuf, om_data) & 3) == 0,
+                   "Unaligned om_data");
+    _Static_assert(((offsetof(struct os_mbuf, om_data) +
+                     sizeof(struct os_mbuf_pkthdr) +
+                     sizeof(struct ble_mbuf_hdr)) & 3) == 0,
+                   "Unaligned data trailing packet header");
+
+    om_ret = os_msys_get_pkthdr(len, sizeof(struct ble_mbuf_hdr));
+    if (!om_ret) {
+        goto rxpdu_alloc_fail;
     }
 
-    /* Set packet length */
-    pkthdr = OS_MBUF_PKTHDR(p);
+    /* Set complete PDU length in packet header */
+    pkthdr = OS_MBUF_PKTHDR(om_ret);
     pkthdr->omp_len = len;
 
+    rem_len = len;
+
     /*
-     * NOTE: first mbuf in chain will have data pre-pended to it so we adjust
-     * m_data by a word.
+     * Calculate length of data in memory block. We assume length is rounded
+     * down to word size so PHY can do word-size aligned data copy to mbufs
+     * (except for last one) and leave remainder unused.
+     *
+     * Note that there likely won't be any remainder here since all pools have
+     * block size aligned to word size anyway.
      */
-    p->om_data += 4;
-    mb_bytes = (p->om_omp->omp_databuf_len - p->om_pkthdr_len - 4);
-
-    if (mb_bytes < len) {
-        n = p;
-        len -= mb_bytes;
-        while (len) {
-            m = os_msys_get(len, 0);
-            if (!m) {
-                os_mbuf_free_chain(p);
-                p = NULL;
-                goto rxpdu_alloc_exit;
-            }
-            /* Chain new mbuf to existing chain */
-            SLIST_NEXT(n, om_next) = m;
-            n = m;
-            mb_bytes = m->om_omp->omp_databuf_len;
-            if (mb_bytes >= len) {
-                len = 0;
-            } else {
-                len -= mb_bytes;
-            }
+    databuf_len = om_ret->om_omp->omp_databuf_len & ~3;
+
+    /*
+     * First mbuf can store less data due to packet header. Also we reserve one
+     * word for leading space to prepend header when necessary (like for data
+     * PDU before handing over to HCI)
+     */
+    om_ret->om_data += 4;
+    rem_len -= databuf_len - om_ret->om_pkthdr_len - 4;
+
+    /* Allocate and chain mbufs until there's enough space to store complete PDU */
+    om = om_ret;
+    while (rem_len > 0) {
+        om_next = os_msys_get(rem_len, 0);
+        if (!om_next) {
+            os_mbuf_free_chain(om_ret);
+            goto rxpdu_alloc_fail;
         }
-    }
 
+        SLIST_NEXT(om, om_next) = om_next;
+        om = om_next;
 
-rxpdu_alloc_exit:
-    if (!p) {
-        STATS_INC(ble_ll_stats, no_bufs);
+        rem_len -= databuf_len;
     }
-    return p;
+
+    return om_ret;
+
+rxpdu_alloc_fail:
+    STATS_INC(ble_ll_stats, no_bufs);
+    return NULL;
 }
 
 int
@@ -402,7 +427,7 @@ ble_ll_chk_txrx_time(uint16_t time)
  * @return int
  */
 int
-ble_ll_is_rpa(uint8_t *addr, uint8_t addr_type)
+ble_ll_is_rpa(const uint8_t *addr, uint8_t addr_type)
 {
     int rc;
 
@@ -414,9 +439,46 @@ ble_ll_is_rpa(uint8_t *addr, uint8_t addr_type)
     return rc;
 }
 
+int
+ble_ll_addr_is_id(uint8_t *addr, uint8_t addr_type)
+{
+    return !addr_type || ((addr[5] & 0xc0) == 0xc0);
+}
+
+int
+ble_ll_addr_subtype(const uint8_t *addr, uint8_t addr_type)
+{
+    if (!addr_type) {
+        return BLE_LL_ADDR_SUBTYPE_IDENTITY;
+    }
+
+    switch (addr[5] >> 6) {
+    case 0:
+        return BLE_LL_ADDR_SUBTYPE_NRPA; /* NRPA */
+    case 1:
+        return BLE_LL_ADDR_SUBTYPE_RPA; /* RPA */
+    default:
+        return BLE_LL_ADDR_SUBTYPE_IDENTITY; /* static random */
+    }
+}
+
+int
+ble_ll_is_valid_public_addr(const uint8_t *addr)
+{
+    int i;
+
+    for (i = 0; i < BLE_DEV_ADDR_LEN; ++i) {
+        if (addr[i]) {
+            return 1;
+        }
+    }
+
+    return 0;
+}
+
 /* Checks to see that the device is a valid random address */
 int
-ble_ll_is_valid_random_addr(uint8_t *addr)
+ble_ll_is_valid_random_addr(const uint8_t *addr)
 {
     int i;
     int rc;
@@ -457,6 +519,39 @@ ble_ll_is_valid_random_addr(uint8_t *addr)
 
     return rc;
 }
+int
+ble_ll_is_valid_own_addr_type(uint8_t own_addr_type, const uint8_t *random_addr)
+{
+    int rc;
+
+    switch (own_addr_type) {
+    case BLE_HCI_ADV_OWN_ADDR_PUBLIC:
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    case BLE_HCI_ADV_OWN_ADDR_PRIV_PUB:
+#endif
+        rc = ble_ll_is_valid_public_addr(g_dev_addr);
+        break;
+    case BLE_HCI_ADV_OWN_ADDR_RANDOM:
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    case BLE_HCI_ADV_OWN_ADDR_PRIV_RAND:
+#endif
+        rc = ble_ll_is_valid_random_addr(random_addr);
+        break;
+    default:
+        rc = 0;
+        break;
+    }
+
+    return rc;
+}
+
+int
+ble_ll_set_public_addr(const uint8_t *addr)
+{
+    memcpy(g_dev_addr, addr, BLE_DEV_ADDR_LEN);
+
+    return BLE_ERR_SUCCESS;
+}
 
 /**
  * Called from the HCI command parser when the set random address command
@@ -469,24 +564,30 @@ ble_ll_is_valid_random_addr(uint8_t *addr)
  * @return int 0: success
  */
 int
-ble_ll_set_random_addr(uint8_t *addr, bool hci_adv_ext)
+ble_ll_set_random_addr(const uint8_t *cmdbuf, uint8_t len, bool hci_adv_ext)
 {
+    const struct ble_hci_le_set_rand_addr_cp *cmd = (const void *) cmdbuf;
+
+    if (len < sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
     /* If the Host issues this command when scanning or legacy advertising is
      * enabled, the Controller shall return the error code Command Disallowed.
      *
      * Test specification extends this also to initiating.
      */
 
-    if (g_ble_ll_conn_create_sm || ble_ll_scan_enabled() ||
+    if (g_ble_ll_conn_create_sm.connsm || ble_ll_scan_enabled() ||
                                 (!hci_adv_ext && ble_ll_adv_enabled())) {
         return BLE_ERR_CMD_DISALLOWED;
     }
 
-    if (!ble_ll_is_valid_random_addr(addr)) {
+    if (!ble_ll_is_valid_random_addr(cmd->addr)) {
         return BLE_ERR_INV_HCI_CMD_PARMS;
     }
 
-    memcpy(g_random_addr, addr, BLE_DEV_ADDR_LEN);
+    memcpy(g_random_addr, cmd->addr, BLE_DEV_ADDR_LEN);
 
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
     /* For instance 0 we need same address if legacy advertising might be
@@ -494,7 +595,7 @@ ble_ll_set_random_addr(uint8_t *addr, bool hci_adv_ext)
      * affect instance 0.
      */
     if (!hci_adv_ext)
-        ble_ll_adv_set_random_addr(addr, 0);
+        ble_ll_adv_set_random_addr(cmd->addr, 0);
 #endif
 
     return BLE_ERR_SUCCESS;
@@ -577,13 +678,20 @@ ble_ll_wfr_timer_exp(void *arg)
         case BLE_LL_STATE_SCANNING:
             ble_ll_scan_wfr_timer_exp();
             break;
-        case BLE_LL_STATE_INITIATING:
-            ble_ll_conn_init_wfr_timer_exp();
-            break;
-#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 1
+#if MYNEWT_VAL(BLE_LL_DTM)
         case BLE_LL_STATE_DTM:
             ble_ll_dtm_wfr_timer_exp();
             break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+        case BLE_LL_STATE_SYNC:
+            ble_ll_sync_wfr_timer_exp();
+            break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        case BLE_LL_STATE_SCAN_AUX:
+            ble_ll_scan_aux_wfr_timer_exp();
+            break;
 #endif
         default:
             break;
@@ -591,28 +699,6 @@ ble_ll_wfr_timer_exp(void *arg)
     }
 }
 
-/**
- * Enable the wait for response timer.
- *
- * Context: Interrupt.
- *
- * @param cputime
- * @param wfr_cb
- * @param arg
- */
-void
-ble_ll_wfr_enable(uint32_t cputime)
-{
-}
-
-/**
- * Disable the wait for response timer
- */
-void
-ble_ll_wfr_disable(void)
-{
-}
-
 /**
  * ll tx pkt in proc
  *
@@ -629,6 +715,7 @@ ble_ll_tx_pkt_in(void)
     uint16_t pb;
     struct os_mbuf_pkthdr *pkthdr;
     struct os_mbuf *om;
+    os_sr_t sr;
 
     /* Drain all packets off the queue */
     while (STAILQ_FIRST(&g_ble_ll_data.ll_tx_pkt_q)) {
@@ -637,7 +724,9 @@ ble_ll_tx_pkt_in(void)
         om = (struct os_mbuf *)((uint8_t *)pkthdr - sizeof(struct os_mbuf));
 
         /* Remove from queue */
+        OS_ENTER_CRITICAL(sr);
         STAILQ_REMOVE_HEAD(&g_ble_ll_data.ll_tx_pkt_q, omp_next);
+        OS_EXIT_CRITICAL(sr);
 
         /* Strip HCI ACL header to get handle and length */
         handle = get_le16(om->om_data);
@@ -675,9 +764,11 @@ ble_ll_count_rx_stats(struct ble_mbuf_hdr *hdr, uint16_t len, uint8_t pdu_type)
     crcok = BLE_MBUF_HDR_CRC_OK(hdr);
     connection_data = (BLE_MBUF_HDR_RX_STATE(hdr) == BLE_LL_STATE_CONNECTION);
 
-#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 1
+#if MYNEWT_VAL(BLE_LL_DTM)
     /* Reuse connection stats for DTM */
-    connection_data = (BLE_MBUF_HDR_RX_STATE(hdr) == BLE_LL_STATE_DTM);
+    if (!connection_data) {
+        connection_data = (BLE_MBUF_HDR_RX_STATE(hdr) == BLE_LL_STATE_DTM);
+    }
 #endif
 
     if (crcok) {
@@ -749,13 +840,20 @@ ble_ll_rx_pkt_in(void)
         case BLE_LL_STATE_SCANNING:
             ble_ll_scan_rx_pkt_in(pdu_type, m, ble_hdr);
             break;
-        case BLE_LL_STATE_INITIATING:
-            ble_ll_init_rx_pkt_in(pdu_type, rxbuf, ble_hdr);
-            break;
-#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 1
+#if MYNEWT_VAL(BLE_LL_DTM)
         case BLE_LL_STATE_DTM:
             ble_ll_dtm_rx_pkt_in(m, ble_hdr);
             break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+        case BLE_LL_STATE_SYNC:
+            ble_ll_sync_rx_pkt_in(m, ble_hdr);
+            break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+        case BLE_LL_STATE_SCAN_AUX:
+            ble_ll_scan_aux_rx_pkt_in(m, ble_hdr);
+            break;
 #endif
         default:
             /* Any other state should never occur */
@@ -877,16 +975,23 @@ ble_ll_rx_start(uint8_t *rxbuf, uint8_t chan, struct ble_mbuf_hdr *rxhdr)
     case BLE_LL_STATE_ADV:
         rc = ble_ll_adv_rx_isr_start(pdu_type);
         break;
-    case BLE_LL_STATE_INITIATING:
-        rc = ble_ll_init_rx_isr_start(pdu_type, rxhdr);
-        break;
     case BLE_LL_STATE_SCANNING:
         rc = ble_ll_scan_rx_isr_start(pdu_type, &rxhdr->rxinfo.flags);
         break;
-#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 1
+#if MYNEWT_VAL(BLE_LL_DTM)
     case BLE_LL_STATE_DTM:
         rc = ble_ll_dtm_rx_isr_start(rxhdr, ble_phy_access_addr_get());
         break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    case BLE_LL_STATE_SYNC:
+        rc = ble_ll_sync_rx_isr_start(pdu_type, rxhdr);
+        break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    case BLE_LL_STATE_SCAN_AUX:
+        rc = ble_ll_scan_aux_rx_isr_start(pdu_type, rxhdr);
+        break;
 #endif
     default:
         /* Should not be in this state! */
@@ -931,7 +1036,7 @@ ble_ll_rx_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr)
     ble_ll_trace_u32x3(BLE_LL_TRACE_ID_RX_END, pdu_type, len,
                        rxhdr->rxinfo.flags);
 
-#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 1
+#if MYNEWT_VAL(BLE_LL_DTM)
     if (BLE_MBUF_HDR_RX_STATE(rxhdr) == BLE_LL_STATE_DTM) {
         rc = ble_ll_dtm_rx_isr_end(rxbuf, rxhdr);
         return rc;
@@ -943,6 +1048,13 @@ ble_ll_rx_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr)
         return rc;
     }
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    if (BLE_MBUF_HDR_RX_STATE(rxhdr) == BLE_LL_STATE_SYNC) {
+        rc = ble_ll_sync_rx_isr_end(rxbuf, rxhdr);
+        return rc;
+    }
+#endif
+
     /* If the CRC checks, make sure lengths check! */
     badpkt = 0;
     if (crcok) {
@@ -965,7 +1077,7 @@ ble_ll_rx_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr)
             break;
         case BLE_ADV_PDU_TYPE_ADV_EXT_IND:
             break;
-        case BLE_ADV_PDU_TYPE_CONNECT_REQ:
+        case BLE_ADV_PDU_TYPE_CONNECT_IND:
             if (len != BLE_CONNECT_REQ_LEN) {
                 badpkt = 1;
             }
@@ -1002,9 +1114,17 @@ ble_ll_rx_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr)
         }
         rc = ble_ll_scan_rx_isr_end(rxpdu, crcok);
         break;
-    case BLE_LL_STATE_INITIATING:
-        rc = ble_ll_init_rx_isr_end(rxbuf, crcok, rxhdr);
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    case BLE_LL_STATE_SCAN_AUX:
+        if (!badpkt) {
+            rxpdu = ble_ll_rxpdu_alloc(len + BLE_LL_PDU_HDR_LEN);
+            if (rxpdu) {
+                ble_phy_rxpdu_copy(rxbuf, rxpdu);
+            }
+        }
+        rc = ble_ll_scan_aux_rx_isr_end(rxpdu, crcok);
         break;
+#endif
     default:
         rc = -1;
         STATS_INC(ble_ll_stats, bad_ll_state);
@@ -1090,28 +1210,18 @@ ble_ll_task(void *arg)
 {
     struct ble_npl_event *ev;
 
-    /*
-     * XXX RIOT ties event queue to a thread which initialized it so we need to
-     * create event queue in LL task, not in general init function. This can
-     * lead to some races between host and LL so for now let us have it as a
-     * hack for RIOT where races can be avoided by proper initialization inside
-     * package.
-     */
-#ifdef RIOT_VERSION
-    ble_npl_eventq_init(&g_ble_ll_data.ll_evq);
-#endif
-
     /* Init ble phy */
     ble_phy_init();
 
     /* Set output power to 1mW (0 dBm) */
     ble_phy_txpwr_set(MYNEWT_VAL(BLE_LL_TX_PWR_DBM));
 
+    /* Register callback for transport */
+    ble_hci_trans_cfg_ll(ble_ll_hci_cmd_rx, NULL, ble_ll_hci_acl_rx, NULL);
+
     /* Tell the host that we are ready to receive packets */
     ble_ll_hci_send_noop();
 
-    ble_ll_rand_start();
-
     while (1) {
         ev = ble_npl_eventq_get(&g_ble_ll_data.ll_evq, BLE_NPL_TIME_FOREVER);
         assert(ev);
@@ -1132,6 +1242,10 @@ void
 ble_ll_state_set(uint8_t ll_state)
 {
     g_ble_ll_data.ll_state = ll_state;
+
+    if (ll_state == BLE_LL_STATE_STANDBY) {
+        BLE_LL_DEBUG_GPIO(SCHED_ITEM, 0);
+    }
 }
 
 /**
@@ -1176,14 +1290,50 @@ ble_ll_read_supp_states(void)
 /**
  * Returns the features supported by the link layer
  *
- * @return uint32_t bitmask of supported features.
+ * @return uint64_t bitmask of supported features.
  */
-uint32_t
+uint64_t
 ble_ll_read_supp_features(void)
 {
     return g_ble_ll_data.ll_supp_features;
 }
 
+/**
+ * Sets the features controlled by the host.
+ *
+ * @return HCI command status
+ */
+int
+ble_ll_set_host_feat(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_set_host_feat_cp *cmd = (const void *) cmdbuf;
+    uint64_t mask;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (!SLIST_EMPTY(&g_ble_ll_conn_active_list)) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    if ((cmd->bit_num > 0x3F) || (cmd->val > 1)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    mask = (uint64_t)1 << (cmd->bit_num);
+    if (!(mask & BLE_LL_HOST_CONTROLLED_FEATURES)) {
+        return BLE_ERR_UNSUPPORTED;
+    }
+
+    if (cmd->val == 0) {
+        g_ble_ll_data.ll_supp_features &= ~(mask);
+    } else {
+        g_ble_ll_data.ll_supp_features |= mask;
+    }
+
+    return BLE_ERR_SUCCESS;
+}
 /**
  * Flush a link layer packet queue.
  *
@@ -1236,6 +1386,20 @@ ble_ll_mbuf_init(struct os_mbuf *m, uint8_t pdulen, uint8_t hdr)
     ble_hdr->txinfo.hdr_byte = hdr;
 }
 
+static void
+ble_ll_validate_task(void)
+{
+#ifdef MYNEWT
+#ifndef NDEBUG
+    struct os_task_info oti;
+
+    os_task_info_get(&g_ble_ll_task, &oti);
+
+    BLE_LL_ASSERT(oti.oti_stkusage < oti.oti_stksize);
+#endif
+#endif
+}
+
 /**
  * Called to reset the controller. This performs a "software reset" of the link
  * layer; it does not perform a HW reset of the controller nor does it reset
@@ -1252,25 +1416,28 @@ ble_ll_reset(void)
     int rc;
     os_sr_t sr;
 
-    /* Stop the phy */
-    ble_phy_disable();
+    /* do sanity check on LL task stack */
+    ble_ll_validate_task();
 
-    /* Stop any wait for response timer */
     OS_ENTER_CRITICAL(sr);
-    ble_ll_wfr_disable();
+    ble_phy_disable();
     ble_ll_sched_stop();
-    OS_EXIT_CRITICAL(sr);
-
-    /* Stop any scanning */
     ble_ll_scan_reset();
+    ble_ll_rfmgmt_reset();
+    OS_EXIT_CRITICAL(sr);
 
     /* Stop any advertising */
     ble_ll_adv_reset();
 
-#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE)
+#if MYNEWT_VAL(BLE_LL_DTM)
     ble_ll_dtm_reset();
 #endif
 
+    /* Stop sync */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    ble_ll_sync_reset();
+#endif
+
     /* FLush all packets from Link layer queues */
     ble_ll_flush_pkt_queue(&g_ble_ll_data.ll_tx_pkt_q);
     ble_ll_flush_pkt_queue(&g_ble_ll_data.ll_rx_pkt_q);
@@ -1294,11 +1461,6 @@ ble_ll_reset(void)
     /* Set state to standby */
     ble_ll_state_set(BLE_LL_STATE_STANDBY);
 
-#ifdef BLE_XCVR_RFCLK
-    /* Stops rf clock and rfclock timer */
-    ble_ll_xcvr_rfclk_stop();
-#endif
-
     /* Reset our random address */
     memset(g_random_addr, 0, BLE_DEV_ADDR_LEN);
 
@@ -1306,7 +1468,7 @@ ble_ll_reset(void)
     ble_ll_whitelist_clear();
 
     /* Reset resolving list */
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
     ble_ll_resolv_list_reset();
 #endif
 
@@ -1316,23 +1478,6 @@ ble_ll_reset(void)
     return rc;
 }
 
-static void
-ble_ll_seed_prng(void)
-{
-    uint32_t seed;
-    int i;
-
-    /* Seed random number generator with least significant bytes of device
-     * address.
-     */
-    seed = 0;
-    for (i = 0; i < 4; ++i) {
-        seed |= g_dev_addr[i];
-        seed <<= 8;
-    }
-    srand(seed);
-}
-
 uint32_t
 ble_ll_pdu_tx_time_get(uint16_t payload_len, int phy_mode)
 {
@@ -1368,7 +1513,7 @@ uint16_t
 ble_ll_pdu_max_tx_octets_get(uint32_t usecs, int phy_mode)
 {
     uint32_t header_tx_time;
-    uint16_t octets;
+    uint16_t octets = 0;
 
     BLE_LL_ASSERT(phy_mode < BLE_PHY_NUM_MODE);
 
@@ -1412,6 +1557,12 @@ ble_ll_pdu_max_tx_octets_get(uint32_t usecs, int phy_mode)
     return max(27, octets);
 }
 
+static inline bool
+ble_ll_is_addr_empty(const uint8_t *addr)
+{
+    return memcmp(addr, BLE_ADDR_ANY, BLE_DEV_ADDR_LEN) == 0;
+}
+
 /**
  * Initialize the Link Layer. Should be called only once
  *
@@ -1421,10 +1572,7 @@ void
 ble_ll_init(void)
 {
     int rc;
-    uint32_t features;
-#ifdef BLE_XCVR_RFCLK
-    uint32_t xtal_ticks;
-#endif
+    uint64_t features;
     ble_addr_t addr;
     struct ble_ll_obj *lldata;
 
@@ -1434,29 +1582,20 @@ ble_ll_init(void)
     ble_ll_trace_init();
     ble_phy_trace_init();
 
-    /* Retrieve the public device address if not set by syscfg */
-    memcpy(&addr.val[0], MYNEWT_VAL_BLE_PUBLIC_DEV_ADDR, BLE_DEV_ADDR_LEN);
-    if (!memcmp(&addr.val[0], ((ble_addr_t *)BLE_ADDR_ANY)->val,
-                BLE_DEV_ADDR_LEN)) {
-        rc = ble_hw_get_public_addr(&addr);
-        if (!rc) {
-            memcpy(g_dev_addr, &addr.val[0], BLE_DEV_ADDR_LEN);
+    /* Set public device address if not already set */
+    if (ble_ll_is_addr_empty(g_dev_addr)) {
+        /* Use sycfg address if configured, otherwise try to read from HW */
+        if (!ble_ll_is_addr_empty(MYNEWT_VAL(BLE_PUBLIC_DEV_ADDR))) {
+            memcpy(g_dev_addr, MYNEWT_VAL(BLE_PUBLIC_DEV_ADDR), BLE_DEV_ADDR_LEN);
+        } else {
+            rc = ble_hw_get_public_addr(&addr);
+            if (!rc) {
+                memcpy(g_dev_addr, &addr.val[0], BLE_DEV_ADDR_LEN);
+            }
         }
-    } else {
-        memcpy(g_dev_addr, &addr.val[0], BLE_DEV_ADDR_LEN);
     }
 
-#ifdef BLE_XCVR_RFCLK
-    /* Settling time of crystal, in ticks */
-    xtal_ticks = MYNEWT_VAL(BLE_XTAL_SETTLE_TIME);
-    BLE_LL_ASSERT(xtal_ticks != 0);
-    g_ble_ll_data.ll_xtal_ticks = os_cputime_usecs_to_ticks(xtal_ticks);
-
-    /* Initialize rf clock timer */
-    os_cputime_timer_init(&g_ble_ll_data.ll_rfclk_timer,
-                          ble_ll_xcvr_rfclk_timer_exp, NULL);
-
-#endif
+    ble_ll_rfmgmt_init();
 
     /* Get pointer to global data object */
     lldata = &g_ble_ll_data;
@@ -1465,17 +1604,8 @@ ble_ll_init(void)
     lldata->ll_num_acl_pkts = MYNEWT_VAL(BLE_ACL_BUF_COUNT);
     lldata->ll_acl_pkt_size = MYNEWT_VAL(BLE_ACL_BUF_SIZE);
 
-    /*
-     * XXX RIOT ties event queue to a thread which initialized it so we need to
-     * create event queue in LL task, not in general init function. This can
-     * lead to some races between host and LL so for now let us have it as a
-     * hack for RIOT where races can be avoided by proper initialization inside
-     * package.
-     */
-#ifndef RIOT_VERSION
     /* Initialize eventq */
     ble_npl_eventq_init(&lldata->ll_evq);
-#endif
 
     /* Initialize the transmit (from host) and receive (from phy) queues */
     STAILQ_INIT(&lldata->ll_tx_pkt_q);
@@ -1513,54 +1643,82 @@ ble_ll_init(void)
     /* Set the supported features. NOTE: we always support extended reject. */
     features = BLE_LL_FEAT_EXTENDED_REJ;
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_DATA_LEN_EXT) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_DATA_LEN_EXT)
     features |= BLE_LL_FEAT_DATA_LEN_EXT;
 #endif
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_CONN_PARAM_REQ) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_CONN_PARAM_REQ)
     features |= BLE_LL_FEAT_CONN_PARM_REQ;
 #endif
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_SLAVE_INIT_FEAT_XCHG) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_SLAVE_INIT_FEAT_XCHG)
     features |= BLE_LL_FEAT_SLAVE_INIT;
 #endif
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
     features |= BLE_LL_FEAT_LE_ENCRYPTION;
 #endif
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
     features |= (BLE_LL_FEAT_LL_PRIVACY | BLE_LL_FEAT_EXT_SCAN_FILT);
     ble_ll_resolv_init();
 #endif
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING)
     features |= BLE_LL_FEAT_LE_PING;
 #endif
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
     features |= BLE_LL_FEAT_EXT_ADV;
 #endif
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2)
     /* CSA2 */
     features |= BLE_LL_FEAT_CSA2;
 #endif
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY)
     features |= BLE_LL_FEAT_LE_2M_PHY;
 #endif
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
     features |= BLE_LL_FEAT_LE_CODED_PHY;
 #endif
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    features |= BLE_LL_FEAT_PERIODIC_ADV;
+    ble_ll_sync_init();
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+    features |= BLE_LL_FEAT_SYNC_TRANS_RECV;
+    features |= BLE_LL_FEAT_SYNC_TRANS_SEND;
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_SCA_UPDATE)
+    features |= BLE_LL_FEAT_SCA_UPDATE;
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_ISO)
+    features |= BLE_LL_FEAT_CIS_MASTER;
+    features |= BLE_LL_FEAT_CIS_SLAVE;
+    features |= BLE_LL_FEAT_ISO_BROADCASTER;
+    features |= BLE_LL_FEAT_ISO_HOST_SUPPORT;
+#endif
+
+    lldata->ll_supp_features = features;
+
     /* Initialize random number generation */
     ble_ll_rand_init();
+    /* Start the random number generator */
+    ble_ll_rand_start();
 
-    /* XXX: This really doesn't belong here, as the address probably has not
-     * been set yet.
-     */
-    ble_ll_seed_prng();
+    rc = stats_init_and_reg(STATS_HDR(ble_ll_stats),
+                            STATS_SIZE_INIT_PARMS(ble_ll_stats, STATS_SIZE_32),
+                            STATS_NAME_INIT_PARMS(ble_ll_stats),
+                            "ble_ll");
+    SYSINIT_PANIC_ASSERT(rc == 0);
 
-    lldata->ll_supp_features = features;
+#if MYNEWT_VAL(BLE_LL_DTM)
+    ble_ll_dtm_init();
+#endif
 
 #if MYNEWT
     /* Initialize the LL task */
@@ -1574,17 +1732,5 @@ ble_ll_init(void)
  * routine which is wrapped by nimble_port_ll_task_func().
  */
 
-#endif
-
-    rc = stats_init_and_reg(STATS_HDR(ble_ll_stats),
-                            STATS_SIZE_INIT_PARMS(ble_ll_stats, STATS_SIZE_32),
-                            STATS_NAME_INIT_PARMS(ble_ll_stats),
-                            "ble_ll");
-    SYSINIT_PANIC_ASSERT(rc == 0);
-
-    ble_hci_trans_cfg_ll(ble_ll_hci_cmd_rx, NULL, ble_ll_hci_acl_rx, NULL);
-
-#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE)
-    ble_ll_dtm_init();
 #endif
 }

nimble/controller/src/ble_ll_conn_priv.h

@@ -21,6 +21,7 @@
 #define H_BLE_LL_CONN_PRIV_
 
 #include "controller/ble_ll_conn.h"
+#include "controller/ble_ll_hci.h"
 
 #ifdef __cplusplus
 extern "C" {
@@ -58,9 +59,6 @@ extern "C" {
 #define BLE_LL_CONN_DEF_AUTH_PYLD_TMO       (3000)
 #define BLE_LL_CONN_AUTH_PYLD_OS_TMO(x)     ble_npl_time_ms_to_ticks32((x) * 10)
 
-
-typedef void (*ble_ll_hci_post_cmd_complete_cb)(void);
-
 /* Global Link Layer connection parameters */
 struct ble_ll_conn_global_params
 {
@@ -79,40 +77,87 @@ struct ble_ll_conn_global_params
 };
 extern struct ble_ll_conn_global_params g_ble_ll_conn_params;
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+struct ble_ll_conn_sync_transfer_params
+{
+    uint32_t sync_timeout_us;
+    uint16_t max_skip;
+    uint8_t  mode;
+};
+extern struct ble_ll_conn_sync_transfer_params g_ble_ll_conn_sync_transfer_params;
+#endif
+
 /* Some data structures used by other LL routines */
 SLIST_HEAD(ble_ll_conn_active_list, ble_ll_conn_sm);
 STAILQ_HEAD(ble_ll_conn_free_list, ble_ll_conn_sm);
 extern struct ble_ll_conn_active_list g_ble_ll_conn_active_list;
 extern struct ble_ll_conn_free_list g_ble_ll_conn_free_list;
 
-/* Pointer to connection state machine we are trying to create */
-extern struct ble_ll_conn_sm *g_ble_ll_conn_create_sm;
+struct ble_ll_conn_create_scan {
+    uint8_t filter_policy;
+    uint8_t own_addr_type;
+    uint8_t peer_addr_type;
+    uint8_t peer_addr[BLE_DEV_ADDR_LEN];
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    uint8_t init_phy_mask;
+#endif
+    struct {
+        uint16_t itvl;
+        uint16_t window;
+    } scan_params[2];
+};
+
+struct ble_ll_conn_create_params {
+    uint32_t conn_itvl;
+    uint32_t conn_itvl_ticks;
+    uint8_t conn_itvl_usecs;
+    uint16_t conn_latency;
+    uint16_t supervision_timeout;
+    uint16_t min_ce_len;
+    uint16_t max_ce_len;
+};
+
+struct ble_ll_conn_create_sm {
+    struct ble_ll_conn_sm *connsm;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    struct ble_ll_conn_create_params params[3];
+#endif
+};
+
+extern struct ble_ll_conn_create_sm g_ble_ll_conn_create_sm;
 
 /* Generic interface */
 struct ble_ll_len_req;
-struct hci_create_conn;
 struct ble_mbuf_hdr;
 struct ble_ll_adv_sm;
 
+struct hci_create_conn
+{
+    uint16_t scan_itvl;
+    uint16_t scan_window;
+    uint8_t filter_policy;
+    uint8_t peer_addr_type;
+    uint8_t peer_addr[BLE_DEV_ADDR_LEN];
+    uint8_t own_addr_type;
+    uint16_t conn_itvl_min;
+    uint16_t conn_itvl_max;
+    uint16_t conn_latency;
+    uint16_t supervision_timeout;
+    uint16_t min_ce_len;
+    uint16_t max_ce_len;
+};
+
 void ble_ll_conn_sm_new(struct ble_ll_conn_sm *connsm);
 void ble_ll_conn_end(struct ble_ll_conn_sm *connsm, uint8_t ble_err);
 void ble_ll_conn_enqueue_pkt(struct ble_ll_conn_sm *connsm, struct os_mbuf *om,
-                             uint8_t hdr_byte, uint8_t length);
+                             uint8_t hdr_byte, uint16_t length);
 struct ble_ll_conn_sm *ble_ll_conn_sm_get(void);
 void ble_ll_conn_master_init(struct ble_ll_conn_sm *connsm,
-                             struct hci_create_conn *hcc);
-#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
-void ble_ll_conn_ext_master_init(struct ble_ll_conn_sm *connsm,
-                                 struct hci_ext_create_conn *hcc);
-
-void ble_ll_conn_ext_set_params(struct ble_ll_conn_sm *connsm,
-                                struct hci_ext_conn_params *hcc_params,
-                                int phy);
-#endif
+                             struct ble_ll_conn_create_scan *cc_scan,
+                             struct ble_ll_conn_create_params *cc_params);
 
 struct ble_ll_conn_sm *ble_ll_conn_find_active_conn(uint16_t handle);
-void ble_ll_conn_datalen_update(struct ble_ll_conn_sm *connsm,
-                                struct ble_ll_len_req *req);
+void ble_ll_conn_update_eff_data_len(struct ble_ll_conn_sm *connsm);
 
 /* Advertising interface */
 int ble_ll_conn_slave_start(uint8_t *rxbuf, uint8_t pat,
@@ -120,36 +165,29 @@ int ble_ll_conn_slave_start(uint8_t *rxbuf, uint8_t pat,
 
 /* Link Layer interface */
 void ble_ll_conn_module_init(void);
-void ble_ll_conn_set_global_chanmap(uint8_t num_used_chans, uint8_t *chanmap);
+void ble_ll_conn_set_global_chanmap(uint8_t num_used_chans, const uint8_t *chanmap);
 void ble_ll_conn_module_reset(void);
 void ble_ll_conn_tx_pkt_in(struct os_mbuf *om, uint16_t handle, uint16_t len);
 int ble_ll_conn_rx_isr_start(struct ble_mbuf_hdr *rxhdr, uint32_t aa);
 int ble_ll_conn_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr);
 void ble_ll_conn_rx_data_pdu(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr);
-void ble_ll_init_rx_pkt_in(uint8_t pdu_type, uint8_t *rxbuf,
-                           struct ble_mbuf_hdr *ble_hdr);
-int ble_ll_init_rx_isr_start(uint8_t pdu_type, struct ble_mbuf_hdr *ble_hdr);
-int ble_ll_init_rx_isr_end(uint8_t *rxbuf, uint8_t crcok,
-                           struct ble_mbuf_hdr *ble_hdr);
 void ble_ll_conn_wfr_timer_exp(void);
-void ble_ll_conn_init_wfr_timer_exp(void);
 int ble_ll_conn_is_lru(struct ble_ll_conn_sm *s1, struct ble_ll_conn_sm *s2);
 uint32_t ble_ll_conn_get_ce_end_time(void);
 void ble_ll_conn_event_halt(void);
-uint8_t ble_ll_conn_calc_used_chans(uint8_t *chmap);
-void ble_ll_conn_reset_pending_aux_conn_rsp(void);
-bool ble_ll_conn_init_pending_aux_conn_rsp(void);
 /* HCI */
 void ble_ll_disconn_comp_event_send(struct ble_ll_conn_sm *connsm,
                                     uint8_t reason);
 void ble_ll_auth_pyld_tmo_event_send(struct ble_ll_conn_sm *connsm);
-int ble_ll_conn_hci_disconnect_cmd(uint8_t *cmdbuf);
-int ble_ll_conn_hci_rd_rem_ver_cmd(uint8_t *cmdbuf);
-int ble_ll_conn_create(uint8_t *cmdbuf);
-int ble_ll_conn_hci_update(uint8_t *cmdbuf);
-int ble_ll_conn_hci_set_chan_class(uint8_t *cmdbuf);
-int ble_ll_conn_hci_param_reply(uint8_t *cmdbuf, int negative_reply,
-                                uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_conn_hci_disconnect_cmd(const struct ble_hci_lc_disconnect_cp *cmd);
+int ble_ll_conn_hci_rd_rem_ver_cmd(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_conn_hci_create(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_conn_hci_update(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_conn_hci_set_chan_class(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_conn_hci_param_rr(const uint8_t *cmdbuf, uint8_t len,
+                             uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_conn_hci_param_nrr(const uint8_t *cmdbuf, uint8_t len,
+                             uint8_t *rspbuf, uint8_t *rsplen);
 int ble_ll_conn_create_cancel(ble_ll_hci_post_cmd_complete_cb *post_cmd_cb);
 void ble_ll_conn_num_comp_pkts_event_send(struct ble_ll_conn_sm *connsm);
 void ble_ll_conn_comp_event_send(struct ble_ll_conn_sm *connsm, uint8_t status,
@@ -157,37 +195,59 @@ void ble_ll_conn_comp_event_send(struct ble_ll_conn_sm *connsm, uint8_t status,
 void ble_ll_conn_timeout(struct ble_ll_conn_sm *connsm, uint8_t ble_err);
 int ble_ll_conn_hci_chk_conn_params(uint16_t itvl_min, uint16_t itvl_max,
                                     uint16_t latency, uint16_t spvn_tmo);
-int ble_ll_conn_hci_read_rem_features(uint8_t *cmdbuf);
-int ble_ll_conn_hci_read_rem_features_complete(void);
-int ble_ll_conn_hci_rd_rssi(uint8_t *cmdbuf, uint8_t *rspbuf, uint8_t *rsplen);
-int ble_ll_conn_hci_rd_chan_map(uint8_t *cmdbuf, uint8_t *rspbuf,
-                                uint8_t *rsplen);
-int ble_ll_conn_hci_set_data_len(uint8_t *cmdbuf, uint8_t *rspbuf,
-                                 uint8_t *rsplen);
-int ble_ll_conn_hci_le_start_encrypt(uint8_t *cmdbuf);
-int ble_ll_conn_hci_le_ltk_reply(uint8_t *cmdbuf, uint8_t *rspbuf,
-                                 uint8_t *rsplen);
-int ble_ll_conn_hci_le_ltk_neg_reply(uint8_t *cmdbuf, uint8_t *rspbuf,
-                                     uint8_t *rsplen);
-int ble_ll_conn_hci_wr_auth_pyld_tmo(uint8_t *cmdbuf, uint8_t *rsp,
-                                     uint8_t *rsplen);
-int ble_ll_conn_hci_rd_auth_pyld_tmo(uint8_t *cmdbuf, uint8_t *rsp,
-                                     uint8_t *rsplen);
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING) == 1)
+int ble_ll_conn_hci_read_rem_features(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_conn_hci_rd_rssi(const uint8_t *cmdbuf, uint8_t len, uint8_t *rspbuf,
+                            uint8_t *rsplen);
+int ble_ll_conn_hci_rd_chan_map(const uint8_t *cmdbuf, uint8_t len,
+                                uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_conn_hci_set_data_len(const uint8_t *cmdbuf, uint8_t len,
+                                 uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_conn_hci_le_start_encrypt(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_conn_hci_le_ltk_reply(const uint8_t *cmdbuf, uint8_t len,
+                                 uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_conn_hci_le_ltk_neg_reply(const uint8_t *cmdbuf, uint8_t len,
+                                     uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_conn_hci_wr_auth_pyld_tmo(const uint8_t *cmdbuf, uint8_t len,
+                                     uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_conn_hci_rd_auth_pyld_tmo(const uint8_t *cmdbuf, uint8_t len,
+                                     uint8_t *rspbuf, uint8_t *rsplen);
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_SCA_UPDATE)
+int ble_ll_conn_req_peer_sca(const uint8_t *cmdbuf, uint8_t len,
+                             uint8_t *rspbuf, uint8_t *rsplen);
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING)
 void ble_ll_conn_auth_pyld_timer_start(struct ble_ll_conn_sm *connsm);
 #else
 #define ble_ll_conn_auth_pyld_timer_start(x)
 #endif
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_CTRL_TO_HOST_FLOW_CONTROL)
+void ble_ll_conn_cth_flow_set_buffers(uint16_t num_buffers);
+bool ble_ll_conn_cth_flow_enable(bool enabled);
+void ble_ll_conn_cth_flow_process_cmd(const uint8_t *cmdbuf);
+#endif
+
+void ble_ll_conn_itvl_to_ticks(uint32_t itvl,
+                               uint32_t *itvl_ticks, uint8_t *itvl_usecs);
+
 int ble_ll_hci_cmd_rx(uint8_t *cmd, void *arg);
 int ble_ll_hci_acl_rx(struct os_mbuf *om, void *arg);
 
-int ble_ll_conn_hci_le_rd_phy(uint8_t *cmdbuf, uint8_t *rsp, uint8_t *rsplen);
-int ble_ll_conn_hci_le_set_phy(uint8_t *cmdbuf);
+int ble_ll_conn_hci_le_rd_phy(const uint8_t *cmdbuf, uint8_t len,
+                              uint8_t *rsp, uint8_t *rsplen);
+int ble_ll_conn_hci_le_set_phy(const uint8_t *cmdbuf, uint8_t len);
 int ble_ll_conn_chk_phy_upd_start(struct ble_ll_conn_sm *connsm);
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
-int ble_ll_ext_conn_create(uint8_t *cmdbuf, uint8_t cmdlen);
+int ble_ll_conn_hci_ext_create(const uint8_t *cmdbuf, uint8_t len);
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+int ble_ll_set_sync_transfer_params(const uint8_t *cmdbuf, uint8_t len,
+                                    uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_set_default_sync_transfer_params(const uint8_t *cmdbuf, uint8_t len);
 #endif
+
 #ifdef __cplusplus
 }
 #endif

nimble/controller/src/ble_ll_ctrl.c

@@ -28,6 +28,7 @@
 #include "controller/ble_ll_ctrl.h"
 #include "controller/ble_ll_trace.h"
 #include "controller/ble_hw.h"
+#include "controller/ble_ll_sync.h"
 #include "ble_ll_conn_priv.h"
 
 /* To use spec sample data for testing */
@@ -105,7 +106,16 @@ const uint8_t g_ble_ll_ctrl_pkt_lengths[BLE_LL_CTRL_OPCODES] =
     BLE_LL_CTRL_PHY_REQ_LEN,
     BLE_LL_CTRL_PHY_RSP_LEN,
     BLE_LL_CTRL_PHY_UPD_IND_LEN,
-    BLE_LL_CTRL_MIN_USED_CHAN_LEN
+    BLE_LL_CTRL_MIN_USED_CHAN_LEN,
+    BLE_LL_CTRL_CTE_REQ_LEN,
+    BLE_LL_CTRL_CTE_RSP_LEN,
+    BLE_LL_CTRL_PERIODIC_SYNC_IND_LEN,
+    BLE_LL_CTRL_CLOCK_ACCURACY_REQ_LEN,
+    BLE_LL_CTRL_CLOCK_ACCURACY_RSP_LEN,
+    BLE_LL_CTRL_CIS_REQ_LEN,
+    BLE_LL_CTRL_CIS_RSP_LEN,
+    BLE_LL_CTRL_CIS_IND_LEN,
+    BLE_LL_CTRL_CIS_TERMINATE_LEN
 };
 
 /**
@@ -204,8 +214,14 @@ ble_ll_ctrl_len_proc(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
         (ctrl_req.max_tx_time < BLE_LL_CONN_SUPP_TIME_MIN)) {
         rc = 1;
     } else {
-        /* Update the connection with the new parameters */
-        ble_ll_conn_datalen_update(connsm, &ctrl_req);
+        /* Update parameters */
+        connsm->rem_max_rx_time = ctrl_req.max_rx_time;
+        connsm->rem_max_tx_time = ctrl_req.max_tx_time;
+        connsm->rem_max_rx_octets = ctrl_req.max_rx_bytes;
+        connsm->rem_max_tx_octets = ctrl_req.max_tx_bytes;
+
+        /* Recalculate effective connection parameters */
+        ble_ll_conn_update_eff_data_len(connsm);
         rc = 0;
     }
 
@@ -493,6 +509,12 @@ ble_ll_ctrl_proc_unk_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *
         ble_ll_ctrl_phy_update_cancel(connsm, BLE_ERR_UNSUPP_REM_FEATURE);
         ctrl_proc = BLE_LL_CTRL_PROC_PHY_UPDATE;
         break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_SCA_UPDATE)
+    case BLE_LL_CTRL_CLOCK_ACCURACY_REQ:
+        ble_ll_hci_ev_sca_update(connsm, BLE_ERR_UNSUPPORTED, 0);
+        ctrl_proc = BLE_LL_CTRL_PROC_SCA_UPDATE;
+        break;
 #endif
     default:
         ctrl_proc = BLE_LL_CTRL_PROC_NUM;
@@ -547,47 +569,6 @@ ble_ll_ctrl_start_rsp_timer(struct ble_ll_conn_sm *connsm)
                      ble_npl_time_ms_to_ticks32(BLE_LL_CTRL_PROC_TIMEOUT_MS));
 }
 
-#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
-void
-ble_ll_ctrl_phy_update_proc_complete(struct ble_ll_conn_sm *connsm)
-{
-    int chk_proc_stop;
-    int chk_host_phy;
-
-    chk_proc_stop = 1;
-    chk_host_phy = 1;
-
-    connsm->phy_tx_transition = BLE_PHY_TRANSITION_INVALID;
-
-    if (CONN_F_PEER_PHY_UPDATE(connsm)) {
-        CONN_F_PEER_PHY_UPDATE(connsm) = 0;
-    } else if (CONN_F_CTRLR_PHY_UPDATE(connsm)) {
-        CONN_F_CTRLR_PHY_UPDATE(connsm) = 0;
-    } else {
-        /* Must be a host-initiated update */
-        CONN_F_HOST_PHY_UPDATE(connsm) = 0;
-        chk_host_phy = 0;
-        if (CONN_F_PHY_UPDATE_EVENT(connsm) == 0) {
-            ble_ll_hci_ev_phy_update(connsm, BLE_ERR_SUCCESS);
-        }
-    }
-
-    /* Must check if we need to start host procedure */
-    if (chk_host_phy) {
-        if (CONN_F_HOST_PHY_UPDATE(connsm)) {
-            if (ble_ll_conn_chk_phy_upd_start(connsm)) {
-                CONN_F_HOST_PHY_UPDATE(connsm) = 0;
-            } else {
-                chk_proc_stop = 0;
-            }
-        }
-    }
-
-    if (chk_proc_stop) {
-        ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_PHY_UPDATE);
-    }
-}
-
 /**
  * Convert a phy mask to a numeric phy value.
  *
@@ -602,7 +583,7 @@ ble_ll_ctrl_phy_update_proc_complete(struct ble_ll_conn_sm *connsm)
  * BLE_HCI_LE_PHY_2M                    (2)
  * BLE_HCI_LE_PHY_CODED                 (3)
  */
-static uint8_t
+uint8_t
 ble_ll_ctrl_phy_from_phy_mask(uint8_t phy_mask)
 {
     uint8_t phy;
@@ -638,6 +619,66 @@ ble_ll_ctrl_phy_from_phy_mask(uint8_t phy_mask)
     return phy;
 }
 
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+uint8_t
+ble_ll_ctrl_phy_tx_transition_get(uint8_t phy_mask)
+{
+    /*
+     * Evaluate PHYs in transition starting from the one with longest TX time
+     * so we select the one that allows shortest payload to be sent. This is
+     * to make sure we do not violate timing restriction on new PHY.
+     */
+    if (phy_mask & BLE_PHY_MASK_CODED) {
+        return BLE_PHY_CODED;
+    } else if (phy_mask & BLE_PHY_MASK_1M) {
+        return BLE_PHY_1M;
+    } else if (phy_mask & BLE_PHY_MASK_2M) {
+        return BLE_PHY_2M;
+    }
+
+    return 0;
+}
+
+void
+ble_ll_ctrl_phy_update_proc_complete(struct ble_ll_conn_sm *connsm)
+{
+    int chk_proc_stop;
+    int chk_host_phy;
+
+    chk_proc_stop = 1;
+    chk_host_phy = 1;
+
+    connsm->phy_tx_transition = 0;
+
+    if (CONN_F_PEER_PHY_UPDATE(connsm)) {
+        CONN_F_PEER_PHY_UPDATE(connsm) = 0;
+    } else if (CONN_F_CTRLR_PHY_UPDATE(connsm)) {
+        CONN_F_CTRLR_PHY_UPDATE(connsm) = 0;
+    } else {
+        /* Must be a host-initiated update */
+        CONN_F_HOST_PHY_UPDATE(connsm) = 0;
+        chk_host_phy = 0;
+        if (CONN_F_PHY_UPDATE_EVENT(connsm) == 0) {
+            ble_ll_hci_ev_phy_update(connsm, BLE_ERR_SUCCESS);
+        }
+    }
+
+    /* Must check if we need to start host procedure */
+    if (chk_host_phy) {
+        if (CONN_F_HOST_PHY_UPDATE(connsm)) {
+            if (ble_ll_conn_chk_phy_upd_start(connsm)) {
+                CONN_F_HOST_PHY_UPDATE(connsm) = 0;
+            } else {
+                chk_proc_stop = 0;
+            }
+        }
+    }
+
+    if (chk_proc_stop) {
+        ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_PHY_UPDATE);
+    }
+}
+
 /**
  *
  *  There is probably a better way for the controller to choose which PHY use.
@@ -686,11 +727,18 @@ ble_ll_ctrl_phy_update_ind_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
     uint8_t tx_phys;
     uint8_t rx_phys;
     uint16_t instant;
+    uint8_t is_slave_sym = 0;
 
     /* Get preferences from PDU */
     tx_phys = dptr[0];
     rx_phys = dptr[1];
 
+    /* If we are master, check if slave requested symmetric PHY */
+    if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+        is_slave_sym = tx_phys == rx_phys;
+        is_slave_sym &= __builtin_popcount(tx_phys) == 1;
+    }
+
     /* Get m_to_s and s_to_m masks */
     if (slave_req) {
         m_to_s = connsm->phy_data.host_pref_tx_phys_mask & rx_phys;
@@ -700,13 +748,34 @@ ble_ll_ctrl_phy_update_ind_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
         s_to_m = connsm->phy_data.req_pref_rx_phys_mask & tx_phys;
     }
 
-    /* Find new phys. If not different than current, set to 0 */
+    if (is_slave_sym) {
+        /*
+         * If either s_to_m or m_to_s is 0, it means for at least one direction
+         * requested PHY is not our preferred one so make sure we keep current
+         * PHY in both directions
+         *
+         * Core 5.2, Vol 6, PartB, 5.1.10
+         *     If the slave specified a single PHY in both the TX_PHYS and
+         *     RX_PHYS fields and both fields are the same, the master shall
+         *     either select the PHY specified by the slave for both directions
+         *     or shall leave both directions unchanged.
+         */
+        if ((s_to_m == 0) || (m_to_s == 0)) {
+            s_to_m = 0;
+            m_to_s = 0;
+        } else {
+            BLE_LL_ASSERT(s_to_m == m_to_s);
+        }
+    }
+
+    /* Calculate new PHYs to use */
     m_to_s = ble_ll_ctrl_find_new_phy(m_to_s);
+    s_to_m = ble_ll_ctrl_find_new_phy(s_to_m);
+
+    /* Make sure we do not indicate PHY change if the same as current one */
     if (m_to_s == connsm->phy_data.cur_tx_phy) {
         m_to_s = 0;
     }
-
-    s_to_m = ble_ll_ctrl_find_new_phy(s_to_m);
     if (s_to_m == connsm->phy_data.cur_rx_phy) {
         s_to_m = 0;
     }
@@ -777,6 +846,20 @@ ble_ll_ctrl_phy_req_rsp_make(struct ble_ll_conn_sm *connsm, uint8_t *ctrdata)
     }
 }
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_SCA_UPDATE)
+/**
+ * Create a LL_CLOCK_ACCURACY_REQ or LL_CLOCK_ACCURACY_RSP pdu
+ *
+ * @param connsm Pointer to connection state machine
+ * @param ctrdata: Pointer to where CtrData starts in pdu
+ */
+static void
+ble_ll_ctrl_sca_req_rsp_make(struct ble_ll_conn_sm *connsm, uint8_t *ctrdata)
+{
+    ctrdata[0] = BLE_LL_SCA_ENUM;
+}
+#endif
+
 static uint8_t
 ble_ll_ctrl_rx_phy_req(struct ble_ll_conn_sm *connsm, uint8_t *req,
                        uint8_t *rsp)
@@ -830,13 +913,7 @@ ble_ll_ctrl_rx_phy_req(struct ble_ll_conn_sm *connsm, uint8_t *req,
         ble_ll_ctrl_phy_req_rsp_make(connsm, rsp);
         rsp_opcode = BLE_LL_CTRL_PHY_RSP;
 
-        if (rsp[0] & BLE_PHY_MASK_1M) {
-            connsm->phy_tx_transition = BLE_PHY_1M;
-        } else if (rsp[0] & BLE_PHY_MASK_2M) {
-            connsm->phy_tx_transition = BLE_PHY_2M;
-        } else if (rsp[0] & BLE_PHY_MASK_CODED) {
-            connsm->phy_tx_transition = BLE_PHY_CODED;
-        }
+        connsm->phy_tx_transition = ble_ll_ctrl_phy_tx_transition_get(req[1] | rsp[0]);
 
         /* Start response timer */
         connsm->cur_ctrl_proc = BLE_LL_CTRL_PROC_PHY_UPDATE;
@@ -970,6 +1047,87 @@ ble_ll_ctrl_rx_phy_update_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
 }
 #endif
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+/**
+ * Called when a BLE_LL_CTRL_PERIODIC_SYNC_IND PDU is received
+ *
+ * @param connsm
+ * @param dptr
+ *
+ * @return uint8_t
+ */
+static uint8_t
+ble_ll_ctrl_rx_periodic_sync_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    if (connsm->sync_transfer_mode) {
+        ble_ll_sync_periodic_ind(connsm, dptr, connsm->sync_transfer_mode == 1,
+                                 connsm->sync_transfer_skip,
+                                 connsm->sync_transfer_sync_timeout);
+    }
+    return BLE_ERR_MAX;
+}
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_SCA_UPDATE)
+/**
+ * Called when a BLE_LL_CTRL_CLOCK_ACCURACY_REQ PDU is received
+ *
+ * @param connsm
+ * @param dptr
+ * @param rsp Pointer to CtrData of BLE_LL_CTRL_CLOCK_ACCURACY_RSP.
+ *
+ * @return uint8_t
+ */
+static uint8_t
+ble_ll_ctrl_rx_sca_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+                       uint8_t *rsp)
+{
+    ble_ll_ctrl_sca_req_rsp_make(connsm, rsp);
+    return BLE_LL_CTRL_CLOCK_ACCURACY_RSP;
+}
+
+/**
+ * Called when a BLE_LL_CTRL_CLOCK_ACCURACY_RSP PDU is received
+ *
+ * @param connsm
+ * @param dptr
+ *
+ * @return uint8_t
+ */
+static uint8_t
+ble_ll_ctrl_rx_sca_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    if (connsm->cur_ctrl_proc != BLE_LL_CTRL_PROC_SCA_UPDATE) {
+        return BLE_LL_CTRL_UNKNOWN_RSP;
+    }
+    ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_SCA_UPDATE);
+    ble_ll_hci_ev_sca_update(connsm, BLE_ERR_SUCCESS, dptr[0]);
+    return BLE_ERR_MAX;
+}
+
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_ISO)
+static uint8_t
+ble_ll_ctrl_rx_cis_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+                       uint8_t *rspdata)
+{
+    return BLE_LL_CTRL_UNKNOWN_RSP;
+}
+
+static uint8_t
+ble_ll_ctrl_rx_cis_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+                       uint8_t *rspdata)
+{
+    return BLE_LL_CTRL_UNKNOWN_RSP;
+}
+
+static uint8_t
+ble_ll_ctrl_rx_cis_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    return BLE_LL_CTRL_UNKNOWN_RSP;
+}
+#endif
 /**
  * Create a link layer length request or length response PDU.
  *
@@ -988,7 +1146,7 @@ ble_ll_ctrl_datalen_upd_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
     put_le16(dptr + 7, connsm->max_tx_time);
 }
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
 void
 ble_ll_calc_session_key(struct ble_ll_conn_sm *connsm)
 {
@@ -1160,9 +1318,13 @@ ble_ll_ctrl_start_enc_send(struct ble_ll_conn_sm *connsm)
         om->om_data[0] = BLE_LL_CTRL_START_ENC_REQ;
         ble_ll_conn_enqueue_pkt(connsm, om, BLE_LL_LLID_CTRL, 1);
 
-        /* Wait for LL_START_ENC_RSP */
-        connsm->cur_ctrl_proc = BLE_LL_CTRL_PROC_ENCRYPT;
-        ble_ll_ctrl_start_rsp_timer(connsm);
+        /* Wait for LL_START_ENC_RSP. If there is already procedure in progress,
+         * LL response timer is already running.
+         */
+        if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_IDLE) {
+            connsm->cur_ctrl_proc = BLE_LL_CTRL_PROC_ENCRYPT;
+            ble_ll_ctrl_start_rsp_timer(connsm);
+        }
 
         rc = 0;
     } else {
@@ -1171,6 +1333,15 @@ ble_ll_ctrl_start_enc_send(struct ble_ll_conn_sm *connsm)
     return rc;
 }
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_ISO)
+static void
+ble_ll_ctrl_cis_create(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    /* TODO Implement */
+    return;
+}
+#endif
+
 /**
  * Create a link layer control "encrypt request" PDU.
  *
@@ -1272,6 +1443,8 @@ ble_ll_ctrl_rx_enc_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
         return BLE_LL_CTRL_UNKNOWN_RSP;
     }
 
+    connsm->enc_data.enc_state = CONN_ENC_S_ENC_RSP_TO_BE_SENT;
+
     /* In case we were already encrypted we need to reset packet counters */
     connsm->enc_data.rx_pkt_cntr = 0;
     connsm->enc_data.tx_pkt_cntr = 0;
@@ -1359,6 +1532,12 @@ ble_ll_ctrl_rx_pause_enc_rsp(struct ble_ll_conn_sm *connsm)
 
     if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
         rc = BLE_LL_CTRL_PAUSE_ENC_RSP;
+    } else if (connsm->enc_data.enc_state == CONN_ENC_S_PAUSE_ENC_RSP_WAIT) {
+        /* Master sends back unencrypted LL_PAUSE_ENC_RSP.
+         * From this moment encryption is paused.
+         */
+        rc = BLE_ERR_MAX;
+        connsm->enc_data.enc_state = CONN_ENC_S_PAUSED;
     } else {
         rc = BLE_LL_CTRL_UNKNOWN_RSP;
     }
@@ -1385,19 +1564,25 @@ ble_ll_ctrl_rx_start_enc_rsp(struct ble_ll_conn_sm *connsm)
         return BLE_ERR_MAX;
     }
 
-    ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_ENCRYPT);
-
     /* If master, we are done. Stop control procedure and sent event to host */
     if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+
+        ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_ENCRYPT);
+
         /* We are encrypted */
         connsm->enc_data.enc_state = CONN_ENC_S_ENCRYPTED;
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING)
         ble_ll_conn_auth_pyld_timer_start(connsm);
 #endif
         rc = BLE_ERR_MAX;
     } else {
         /* Procedure has completed but slave needs to send START_ENC_RSP */
         rc = BLE_LL_CTRL_START_ENC_RSP;
+
+        /* Stop timer if it was started when sending START_ENC_REQ */
+        if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_ENCRYPT) {
+            ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_ENCRYPT);
+        }
     }
 
     /*
@@ -1470,7 +1655,7 @@ ble_ll_ctrl_version_ind_make(struct ble_ll_conn_sm *connsm, uint8_t *pyld)
     connsm->csmflags.cfbit.version_ind_sent = 1;
 
     /* Fill out response */
-    pyld[0] = BLE_HCI_VER_BCS_5_0;
+    pyld[0] = BLE_HCI_VER_BCS;
     put_le16(pyld + 1, MYNEWT_VAL(BLE_LL_MFRG_ID));
     put_le16(pyld + 3, BLE_LL_SUB_VERS_NR);
 }
@@ -1563,7 +1748,7 @@ ble_ll_ctrl_rx_reject_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
             }
         }
         break;
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
     case BLE_LL_CTRL_PROC_ENCRYPT:
         ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_ENCRYPT);
         ble_ll_hci_ev_encrypt_chg(connsm, ble_error);
@@ -1583,6 +1768,12 @@ 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_DATA_LEN_UPD);
         break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_SCA_UPDATE)
+    case BLE_LL_CTRL_PROC_SCA_UPDATE:
+        ble_ll_hci_ev_sca_update(connsm, ble_error, 0);
+        ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_SCA_UPDATE);
+        break;
+#endif
     default:
         break;
     }
@@ -1670,6 +1861,38 @@ ble_ll_ctrl_initiate_dle(struct ble_ll_conn_sm *connsm)
     ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_DATA_LEN_UPD);
 }
 
+static void
+ble_ll_ctrl_update_features(struct ble_ll_conn_sm *connsm, uint8_t *feat)
+{
+    connsm->conn_features = feat[0];
+    memcpy(connsm->remote_features, feat + 1, 7);
+
+    /* If we received peer's features for the 1st time, we should try DLE */
+    if (!connsm->csmflags.cfbit.rxd_features) {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+        /*
+         * If connection was established on uncoded PHY, by default we use
+         * MaxTxTime and MaxRxTime applicable for that PHY since we are not
+         * allowed to indicate longer supported time if peer does not support
+         * LE Coded PHY. However, once we know that peer does support it we can
+         * update those values to ones applicable for coded PHY.
+         */
+        if (connsm->remote_features[0] & (BLE_LL_FEAT_LE_CODED_PHY >> 8)) {
+            if (connsm->host_req_max_tx_time) {
+                connsm->max_tx_time = max(connsm->max_tx_time,
+                                          connsm->host_req_max_tx_time);
+            } else {
+                connsm->max_tx_time = g_ble_ll_conn_params.conn_init_max_tx_time_coded;
+            }
+            connsm->max_rx_time = BLE_LL_CONN_SUPP_TIME_MAX_CODED;
+        }
+#endif
+
+        connsm->csmflags.cfbit.pending_initiate_dle = 1;
+        connsm->csmflags.cfbit.rxd_features = 1;
+    }
+}
+
 /**
  * Called when we receive a feature request or a slave initiated feature
  * request.
@@ -1679,6 +1902,7 @@ ble_ll_ctrl_initiate_dle(struct ble_ll_conn_sm *connsm)
  * @param dptr
  * @param rspbuf
  * @param opcode
+ * @param new_features
  *
  * @return int
  */
@@ -1687,7 +1911,7 @@ ble_ll_ctrl_rx_feature_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
                            uint8_t *rspbuf, uint8_t opcode)
 {
     uint8_t rsp_opcode;
-    uint32_t our_feat;
+    uint64_t our_feat;
 
     /*
      * Only accept slave feature requests if we are a master and feature
@@ -1708,6 +1932,8 @@ ble_ll_ctrl_rx_feature_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
 
     rsp_opcode = BLE_LL_CTRL_FEATURE_RSP;
 
+    ble_ll_ctrl_update_features(connsm, dptr);
+
     /*
      * 1st octet of features should be common features of local and remote
      * controller - we call this 'connection features'
@@ -1716,19 +1942,37 @@ ble_ll_ctrl_rx_feature_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
      *
      * See: Vol 6, Part B, section 2.4.2.10
      */
+    connsm->conn_features &= our_feat;
 
-    connsm->conn_features = dptr[0] & our_feat;
-    memset(rspbuf + 1, 0, 8);
-    put_le32(rspbuf + 1, our_feat);
+    put_le64(rspbuf + 1, our_feat);
     rspbuf[1] = connsm->conn_features;
 
-    /* If this is the first time we received remote features, try to start DLE */
-    if (!connsm->csmflags.cfbit.rxd_features) {
-        ble_ll_ctrl_initiate_dle(connsm);
-        connsm->csmflags.cfbit.rxd_features = 1;
+    return rsp_opcode;
+}
+
+/**
+ * Called when we receive a feature response
+ *
+ * @param connsm
+ * @param dptr
+ * @param new_features
+ *
+ */
+static void
+ble_ll_ctrl_rx_feature_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+    ble_ll_ctrl_update_features(connsm, dptr);
+
+    /* Stop the control procedure */
+    if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_FEATURE_XCHG)) {
+        ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_FEATURE_XCHG);
     }
 
-    return rsp_opcode;
+    /* Send event to host if pending features read */
+    if (connsm->csmflags.cfbit.pending_hci_rd_features) {
+        ble_ll_hci_ev_rd_rem_used_feat(connsm, BLE_ERR_SUCCESS);
+        connsm->csmflags.cfbit.pending_hci_rd_features = 0;
+    }
 }
 
 /**
@@ -1954,8 +2198,7 @@ ble_ll_ctrl_proc_init(struct ble_ll_conn_sm *connsm, int ctrl_proc)
             } else {
                 opcode = BLE_LL_CTRL_SLAVE_FEATURE_REQ;
             }
-            memset(ctrdata, 0, BLE_LL_CTRL_FEATURE_LEN);
-            put_le32(ctrdata, ble_ll_read_supp_features());
+            put_le64(ctrdata, ble_ll_read_supp_features());
             break;
         case BLE_LL_CTRL_PROC_VERSION_XCHG:
             opcode = BLE_LL_CTRL_VERSION_IND;
@@ -1976,7 +2219,7 @@ ble_ll_ctrl_proc_init(struct ble_ll_conn_sm *connsm, int ctrl_proc)
             opcode = BLE_LL_CTRL_LENGTH_REQ;
             ble_ll_ctrl_datalen_upd_make(connsm, dptr);
             break;
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
         /* XXX: deal with already encrypted connection.*/
         case BLE_LL_CTRL_PROC_ENCRYPT:
             /* If we are already encrypted we do pause procedure */
@@ -1993,6 +2236,18 @@ ble_ll_ctrl_proc_init(struct ble_ll_conn_sm *connsm, int ctrl_proc)
             opcode = BLE_LL_CTRL_PHY_REQ;
             ble_ll_ctrl_phy_req_rsp_make(connsm, ctrdata);
             break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_SCA_UPDATE)
+        case BLE_LL_CTRL_PROC_SCA_UPDATE:
+            opcode = BLE_LL_CTRL_CLOCK_ACCURACY_REQ;
+            ble_ll_ctrl_sca_req_rsp_make(connsm, ctrdata);
+            break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_ISO)
+        case BLE_LL_CTRL_PROC_CIS_CREATE:
+            opcode = BLE_LL_CTRL_CIS_REQ;
+            ble_ll_ctrl_cis_create(connsm, ctrdata);
+            break;
 #endif
         default:
             BLE_LL_ASSERT(0);
@@ -2199,7 +2454,7 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om)
     uint8_t *dptr;
     uint8_t *rspbuf;
     uint8_t *rspdata;
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
     int restart_encryption;
 #endif
     int rc = 0;
@@ -2218,6 +2473,11 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om)
     len = dptr[1];
     opcode = dptr[2];
 
+#if MYNEWT_VAL(BLE_LL_HCI_LLCP_TRACE)
+    ble_ll_hci_ev_send_llcp_trace(0x03, connsm->conn_handle, connsm->event_cntr,
+                                  &dptr[2], len);
+#endif
+
     /*
      * rspbuf points to first byte of response. The response buffer does not
      * contain the Data Channel PDU. Thus, the first byte of rspbuf is the
@@ -2239,7 +2499,7 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om)
 
     ble_ll_trace_u32x2(BLE_LL_TRACE_ID_CTRL_RX, opcode, len);
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
     restart_encryption = 0;
 #endif
 
@@ -2279,6 +2539,9 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om)
     case BLE_LL_CTRL_MIN_USED_CHAN_IND:
         feature = BLE_LL_FEAT_MIN_USED_CHAN;
         break;
+    case BLE_LL_CTRL_PERIODIC_SYNC_IND:
+        feature = BLE_LL_FEAT_SYNC_TRANS_RECV;
+        break;
     default:
         feature = 0;
         break;
@@ -2360,22 +2623,7 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om)
         break;
     /* XXX: check to see if ctrl procedure was running? Do we care? */
     case BLE_LL_CTRL_FEATURE_RSP:
-        connsm->conn_features = dptr[0];
-        memcpy(connsm->remote_features, dptr + 1, 7);
-        /* If this is the first time we received remote features, try to start DLE */
-        if (!connsm->csmflags.cfbit.rxd_features) {
-            ble_ll_ctrl_initiate_dle(connsm);
-            connsm->csmflags.cfbit.rxd_features = 1;
-        }
-        /* Stop the control procedure */
-        if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_FEATURE_XCHG)) {
-            ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_FEATURE_XCHG);
-        }
-        /* Send event to host if pending features read */
-        if (connsm->csmflags.cfbit.pending_hci_rd_features) {
-            ble_ll_hci_ev_rd_rem_used_feat(connsm, BLE_ERR_SUCCESS);
-            connsm->csmflags.cfbit.pending_hci_rd_features = 0;
-        }
+        ble_ll_ctrl_rx_feature_rsp(connsm, dptr);
         break;
     case BLE_LL_CTRL_VERSION_IND:
         rsp_opcode = ble_ll_ctrl_rx_version_ind(connsm, dptr, rspdata);
@@ -2383,7 +2631,7 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om)
     case BLE_LL_CTRL_SLAVE_FEATURE_REQ:
         rsp_opcode = ble_ll_ctrl_rx_feature_req(connsm, dptr, rspbuf, opcode);
         break;
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
     case BLE_LL_CTRL_ENC_REQ:
         rsp_opcode = ble_ll_ctrl_rx_enc_req(connsm, dptr, rspdata);
         break;
@@ -2434,6 +2682,31 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om)
     case BLE_LL_CTRL_PHY_UPDATE_IND:
         rsp_opcode = ble_ll_ctrl_rx_phy_update_ind(connsm, dptr);
         break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_SCA_UPDATE)
+    case BLE_LL_CTRL_CLOCK_ACCURACY_REQ:
+        rsp_opcode = ble_ll_ctrl_rx_sca_req(connsm, dptr, rspdata);
+        break;
+    case BLE_LL_CTRL_CLOCK_ACCURACY_RSP:
+        rsp_opcode = ble_ll_ctrl_rx_sca_rsp(connsm, dptr);
+        break;
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_ISO)
+    case BLE_LL_CTRL_CIS_REQ:
+        rsp_opcode = ble_ll_ctrl_rx_cis_req(connsm, dptr, rspdata);
+        break;
+    case BLE_LL_CTRL_CIS_RSP:
+        rsp_opcode = ble_ll_ctrl_rx_cis_rsp(connsm, dptr, rspdata);
+        break;
+    case BLE_LL_CTRL_CIS_IND:
+        rsp_opcode = ble_ll_ctrl_rx_cis_ind(connsm, dptr);
+        break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+    case BLE_LL_CTRL_PERIODIC_SYNC_IND:
+        rsp_opcode = ble_ll_ctrl_rx_periodic_sync_ind(connsm, dptr);
+        break;
 #endif
     default:
         /* Nothing to do here */
@@ -2455,7 +2728,7 @@ ll_ctrl_send_rsp:
         }
         len = g_ble_ll_ctrl_pkt_lengths[rsp_opcode] + 1;
         ble_ll_conn_enqueue_pkt(connsm, om, BLE_LL_LLID_CTRL, len);
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
         if (restart_encryption) {
             /* XXX: what happens if this fails? Meaning we cant allocate
                mbuf? */
@@ -2463,6 +2736,12 @@ ll_ctrl_send_rsp:
         }
 #endif
     }
+
+    if (connsm->csmflags.cfbit.pending_initiate_dle) {
+        connsm->csmflags.cfbit.pending_initiate_dle = 0;
+        ble_ll_ctrl_initiate_dle(connsm);
+    }
+
     return rc;
 }
 
@@ -2528,6 +2807,11 @@ ble_ll_ctrl_tx_done(struct os_mbuf *txpdu, struct ble_ll_conn_sm *connsm)
     int rc;
     uint8_t opcode;
 
+#if MYNEWT_VAL(BLE_LL_HCI_LLCP_TRACE)
+    ble_ll_hci_ev_send_llcp_trace(0x04, connsm->conn_handle, connsm->event_cntr,
+                                  txpdu->om_data, txpdu->om_len);
+#endif
+
     rc = 0;
     opcode = txpdu->om_data[0];
     switch (opcode) {
@@ -2547,18 +2831,18 @@ ble_ll_ctrl_tx_done(struct os_mbuf *txpdu, struct ble_ll_conn_sm *connsm)
                 connsm->csmflags.cfbit.host_expects_upd_event = 1;
             }
         }
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
         if (connsm->enc_data.enc_state > CONN_ENC_S_ENCRYPTED) {
             connsm->enc_data.enc_state = CONN_ENC_S_UNENCRYPTED;
         }
 #endif
         break;
     case BLE_LL_CTRL_REJECT_IND:
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
         connsm->enc_data.enc_state = CONN_ENC_S_UNENCRYPTED;
 #endif
         break;
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
     case BLE_LL_CTRL_PAUSE_ENC_REQ:
         /* note: fall-through intentional */
     case BLE_LL_CTRL_ENC_REQ:
@@ -2585,15 +2869,15 @@ ble_ll_ctrl_tx_done(struct os_mbuf *txpdu, struct ble_ll_conn_sm *connsm)
 #if (BLE_LL_BT5_PHY_SUPPORTED == 1)
     case BLE_LL_CTRL_PHY_REQ:
         if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) {
-            if (connsm->phy_data.req_pref_tx_phys_mask & BLE_PHY_MASK_1M) {
-                connsm->phy_tx_transition = BLE_PHY_1M;
-            } else if (connsm->phy_data.req_pref_tx_phys_mask & BLE_PHY_MASK_2M) {
-                connsm->phy_tx_transition = BLE_PHY_2M;
-            } else if (connsm->phy_data.req_pref_tx_phys_mask & BLE_PHY_MASK_CODED) {
-                connsm->phy_tx_transition = BLE_PHY_CODED;
-            }
+            connsm->phy_tx_transition =
+                    ble_ll_ctrl_phy_tx_transition_get(
+                            connsm->phy_data.req_pref_tx_phys_mask);
         }
         break;
+    case BLE_LL_CTRL_PHY_UPDATE_IND:
+        connsm->phy_tx_transition =
+                    ble_ll_ctrl_phy_tx_transition_get(txpdu->om_data[2]);
+        break;
 #endif
     default:
         break;

nimble/controller/src/ble_ll_dtm.c

@@ -20,7 +20,7 @@
 #include "syscfg/syscfg.h"
 #include "sysinit/sysinit.h"
 
-#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 1
+#if MYNEWT_VAL(BLE_LL_DTM)
 
 #include <assert.h>
 #include "os/os.h"
@@ -28,7 +28,7 @@
 #include "controller/ble_ll.h"
 #include "controller/ble_phy.h"
 #include "controller/ble_ll_sched.h"
-#include "controller/ble_ll_xcvr.h"
+#include "controller/ble_ll_rfmgmt.h"
 #include "ble_ll_dtm_priv.h"
 
 STATS_SECT_START(ble_ll_dtm_stats)
@@ -49,12 +49,17 @@ struct dtm_ctx {
     uint8_t itvl_rem_usec;
     uint16_t num_of_packets;
     uint32_t itvl_ticks;
+#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS)
+    uint16_t num_of_packets_max;
+#endif
     int active;
     uint8_t rf_channel;
     uint8_t phy_mode;
     struct os_mbuf *om;
     struct ble_npl_event evt;
     struct ble_ll_sched_item sch;
+    uint32_t pdu_start_ticks;
+    uint8_t pdu_start_usecs;
 };
 
 static struct dtm_ctx g_ble_ll_dtm_ctx;
@@ -140,18 +145,23 @@ static const uint8_t channel_rf_to_index[] = {
 #define BLE_DTM_SYNC_WORD          (0x71764129)
 #define BLE_DTM_CRC                (0x555555)
 
+static void ble_ll_dtm_ctx_free(struct dtm_ctx * ctx);
+
 static void
 ble_ll_dtm_set_next(struct dtm_ctx *ctx)
 {
     struct ble_ll_sched_item *sch = &ctx->sch;
 
-    sch->start_time += ctx->itvl_ticks;
-    sch->remainder += ctx->itvl_rem_usec;
-    if (sch->remainder > 30) {
-       sch->start_time++;
-       sch->remainder -= 30;
+    ctx->pdu_start_ticks += ctx->itvl_ticks;
+    ctx->pdu_start_usecs += ctx->itvl_rem_usec;
+    if (ctx->pdu_start_usecs >= 31) {
+       ctx->pdu_start_ticks++;
+       ctx->pdu_start_usecs -= 31;
     }
 
+    sch->start_time = ctx->pdu_start_ticks;
+    sch->remainder = ctx->pdu_start_usecs;
+
     sch->start_time -= g_ble_ll_sched_offset_ticks;
 }
 
@@ -169,6 +179,18 @@ ble_ll_dtm_ev_tx_resched_cb(struct ble_npl_event *evt) {
     }
     OS_EXIT_CRITICAL(sr);
 
+#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS)
+    if (g_ble_ll_dtm_ctx.num_of_packets_max &&
+        (g_ble_ll_dtm_ctx.num_of_packets == g_ble_ll_dtm_ctx.num_of_packets_max)) {
+        /*
+         * XXX do not send more packets, but also do not stop DTM - it shall be
+         * stopped as usual by HCI command since there is no standard way to
+         * signal end of test to host.
+         */
+        return;
+    }
+#endif
+
     ble_ll_dtm_set_next(ctx);
     rc = ble_ll_sched_dtm(&ctx->sch);
     BLE_LL_ASSERT(rc == 0);
@@ -194,6 +216,8 @@ ble_ll_dtm_tx_done(void *arg)
         return;
     }
 
+    g_ble_ll_dtm_ctx.num_of_packets++;
+
     /* Reschedule event in LL context */
     ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &ctx->evt);
 
@@ -237,7 +261,7 @@ ble_ll_dtm_tx_sched_cb(struct ble_ll_sched_item *sch)
 
     ble_ll_state_set(BLE_LL_STATE_DTM);
 
-    return BLE_LL_SCHED_STATE_DONE;
+    return BLE_LL_SCHED_STATE_RUNNING;
 
 resched:
     /* Reschedule from LL task if late for this PDU */
@@ -249,7 +273,8 @@ resched:
 }
 
 static void
-ble_ll_dtm_calculate_itvl(struct dtm_ctx *ctx, uint8_t len, int phy_mode)
+ble_ll_dtm_calculate_itvl(struct dtm_ctx *ctx, uint8_t len,
+                          uint16_t cmd_interval, int phy_mode)
 {
     uint32_t l;
     uint32_t itvl_usec;
@@ -259,6 +284,12 @@ ble_ll_dtm_calculate_itvl(struct dtm_ctx *ctx, uint8_t len, int phy_mode)
     l = ble_ll_pdu_tx_time_get(len + BLE_LL_PDU_HDR_LEN, phy_mode);
     itvl_usec = ((l + 249 + 624) / 625) * 625;
 
+#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS)
+    if (cmd_interval > itvl_usec) {
+        itvl_usec = cmd_interval;
+    }
+#endif
+
     itvl_ticks = os_cputime_usecs_to_ticks(itvl_usec);
     ctx->itvl_rem_usec = (itvl_usec - os_cputime_ticks_to_usecs(itvl_ticks));
     if (ctx->itvl_rem_usec == 31) {
@@ -270,21 +301,27 @@ ble_ll_dtm_calculate_itvl(struct dtm_ctx *ctx, uint8_t len, int phy_mode)
 
 static int
 ble_ll_dtm_tx_create_ctx(uint8_t packet_payload, uint8_t len,
-                         uint8_t rf_channel, uint8_t phy_mode)
+                         uint8_t rf_channel, uint8_t phy_mode,
+                         uint16_t cmd_interval, uint16_t cmd_pkt_count)
 {
     int rc = 0;
     uint8_t byte_pattern;
     struct ble_mbuf_hdr *ble_hdr;
     struct os_mbuf *m;
-    struct ble_ll_sched_item *sch = &g_ble_ll_dtm_ctx.sch;
+    struct dtm_ctx *ctx = &g_ble_ll_dtm_ctx;
+    struct ble_ll_sched_item *sch = &ctx->sch;
 
     /* MSYS is big enough to get continues memory */
     m = os_msys_get_pkthdr(len, sizeof(struct ble_mbuf_hdr));
-    g_ble_ll_dtm_ctx.om = m;
+    ctx->om = m;
     BLE_LL_ASSERT(g_ble_ll_dtm_ctx.om);
 
-    g_ble_ll_dtm_ctx.phy_mode = phy_mode;
-    g_ble_ll_dtm_ctx.rf_channel = rf_channel;
+    ctx->phy_mode = phy_mode;
+    ctx->rf_channel = rf_channel;
+    ctx->num_of_packets = 0;
+#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS)
+    ctx->num_of_packets_max = cmd_pkt_count;
+#endif
 
     /* Set BLE transmit header */
     ble_hdr = BLE_MBUF_HDR_PTR(m);
@@ -333,25 +370,25 @@ ble_ll_dtm_tx_create_ctx(uint8_t packet_payload, uint8_t len,
     }
 
 schedule:
+    ble_phy_enable_dtm();
 
     sch->sched_cb = ble_ll_dtm_tx_sched_cb;
-    sch->cb_arg = &g_ble_ll_dtm_ctx;
+    sch->cb_arg = ctx;
     sch->sched_type = BLE_LL_SCHED_TYPE_DTM;
-    sch->start_time =  os_cputime_get32() +
-                                       os_cputime_usecs_to_ticks(5000);
 
     /* Prepare os_event */
-    ble_npl_event_init(&g_ble_ll_dtm_ctx.evt, ble_ll_dtm_ev_tx_resched_cb,
-                       &g_ble_ll_dtm_ctx);
+    ble_npl_event_init(&ctx->evt, ble_ll_dtm_ev_tx_resched_cb, ctx);
 
-    ble_ll_dtm_calculate_itvl(&g_ble_ll_dtm_ctx, len, phy_mode);
+    ble_ll_dtm_calculate_itvl(ctx, len, cmd_interval, phy_mode);
+
+    ctx->pdu_start_ticks = ble_ll_rfmgmt_enable_now();
+    ctx->pdu_start_usecs = 0;
+    ble_ll_dtm_set_next(ctx);
 
     /* Set some start point for TX packets */
     rc = ble_ll_sched_dtm(sch);
     BLE_LL_ASSERT(rc == 0);
 
-    ble_phy_enable_dtm();
-
     g_ble_ll_dtm_ctx.active = 1;
     return 0;
 }
@@ -373,8 +410,7 @@ ble_ll_dtm_rx_start(void)
 #endif
 
     OS_ENTER_CRITICAL(sr);
-    rc = ble_phy_rx_set_start_time(os_cputime_get32() +
-                                   g_ble_ll_sched_offset_ticks, 0);
+    rc = ble_phy_rx_set_start_time(os_cputime_get32(), 0);
     OS_EXIT_CRITICAL(sr);
     if (rc && rc != BLE_PHY_ERR_RX_LATE) {
         return rc;
@@ -382,33 +418,46 @@ ble_ll_dtm_rx_start(void)
 
     ble_ll_state_set(BLE_LL_STATE_DTM);
 
-#ifdef BLE_XCVR_RFCLK
-    if (ble_ll_xcvr_rfclk_state() == BLE_RFCLK_STATE_OFF) {
-        ble_ll_xcvr_rfclk_start_now(os_cputime_get32());
+    return 0;
+}
+
+static int
+ble_ll_dtm_rx_sched_cb(struct ble_ll_sched_item *sch)
+{
+    if (ble_ll_dtm_rx_start() != 0) {
+        ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &g_ble_ll_dtm_ctx.evt);
+        STATS_INC(ble_ll_dtm_stats, rx_failed);
+        return BLE_LL_SCHED_STATE_DONE;
     }
-#endif
 
-    return 0;
+    return BLE_LL_SCHED_STATE_RUNNING;
 }
 
 static int
 ble_ll_dtm_rx_create_ctx(uint8_t rf_channel, uint8_t phy_mode)
 {
+    struct ble_ll_sched_item *sch = &g_ble_ll_dtm_ctx.sch;
+    int rc;
+
     g_ble_ll_dtm_ctx.phy_mode = phy_mode;
     g_ble_ll_dtm_ctx.rf_channel = rf_channel;
-    g_ble_ll_dtm_ctx.active = 1;
 
     STATS_CLEAR(ble_ll_dtm_stats, rx_count);
 
     ble_npl_event_init(&g_ble_ll_dtm_ctx.evt, ble_ll_dtm_ev_rx_restart_cb,
                        NULL);
 
-    if (ble_ll_dtm_rx_start() != 0) {
-        return 1;
-    }
+    sch->sched_cb = ble_ll_dtm_rx_sched_cb;
+    sch->cb_arg = &g_ble_ll_dtm_ctx;
+    sch->sched_type = BLE_LL_SCHED_TYPE_DTM;
+    sch->start_time =  ble_ll_rfmgmt_enable_now();
+
+    rc = ble_ll_sched_dtm(sch);
+    BLE_LL_ASSERT(rc == 0);
 
     ble_phy_enable_dtm();
 
+    g_ble_ll_dtm_ctx.active = 1;
     return 0;
 }
 
@@ -422,96 +471,148 @@ ble_ll_dtm_ctx_free(struct dtm_ctx * ctx)
         OS_EXIT_CRITICAL(sr);
         return;
     }
-    OS_EXIT_CRITICAL(sr);
 
     ble_ll_sched_rmv_elem(&ctx->sch);
+    ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &g_ble_ll_dtm_ctx.evt);
 
     ble_phy_disable();
     ble_phy_disable_dtm();
     ble_ll_state_set(BLE_LL_STATE_STANDBY);
-#ifdef BLE_XCVR_RFCLK
-    ble_ll_xcvr_rfclk_stop();
-#endif
+    ble_ll_rfmgmt_release();
 
     os_mbuf_free_chain(ctx->om);
     memset(ctx, 0, sizeof(*ctx));
+    OS_EXIT_CRITICAL(sr);
 }
 
-int
-ble_ll_dtm_tx_test(uint8_t *cmdbuf, bool enhanced)
+static int
+ble_ll_dtm_tx_test(uint8_t tx_chan, uint8_t len, uint8_t packet_payload,
+                   uint8_t hci_phy, uint16_t interval, uint16_t pkt_count)
 {
-    uint8_t tx_chan = cmdbuf[0];
-    uint8_t len = cmdbuf[1];
-    uint8_t packet_payload = cmdbuf[2];
-    uint8_t phy_mode = BLE_PHY_MODE_1M;
+    uint8_t phy_mode;
 
     if (g_ble_ll_dtm_ctx.active) {
         return BLE_ERR_CTLR_BUSY;
     }
 
-    if (enhanced) {
-        switch (cmdbuf[3]) {
-        case BLE_HCI_LE_PHY_1M:
-            phy_mode = BLE_PHY_MODE_1M;
-            break;
+    switch (hci_phy) {
+    case BLE_HCI_LE_PHY_1M:
+        phy_mode = BLE_PHY_MODE_1M;
+        break;
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY)
-        case BLE_HCI_LE_PHY_2M:
-            phy_mode = BLE_PHY_MODE_2M;
-            break;
+    case BLE_HCI_LE_PHY_2M:
+        phy_mode = BLE_PHY_MODE_2M;
+        break;
 #endif
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
-        case BLE_HCI_LE_PHY_CODED_S8:
-            phy_mode = BLE_PHY_MODE_CODED_125KBPS;
-            break;
-        case BLE_HCI_LE_PHY_CODED_S2:
-            phy_mode = BLE_PHY_MODE_CODED_500KBPS;
-            break;
+    case BLE_HCI_LE_PHY_CODED_S8:
+        phy_mode = BLE_PHY_MODE_CODED_125KBPS;
+        break;
+    case BLE_HCI_LE_PHY_CODED_S2:
+        phy_mode = BLE_PHY_MODE_CODED_500KBPS;
+        break;
 #endif
-        default:
-            return BLE_ERR_INV_HCI_CMD_PARMS;
-        }
+    default:
+        return BLE_ERR_INV_HCI_CMD_PARMS;
     }
 
     if (tx_chan > 0x27 || packet_payload > 0x07) {
         return BLE_ERR_INV_HCI_CMD_PARMS;
     }
 
-    if (ble_ll_dtm_tx_create_ctx(packet_payload, len, tx_chan, phy_mode)) {
+    if (ble_ll_dtm_tx_create_ctx(packet_payload, len, tx_chan, phy_mode,
+                                 interval, pkt_count)) {
         return BLE_ERR_UNSPECIFIED;
     }
 
     return BLE_ERR_SUCCESS;
 }
 
-int ble_ll_dtm_rx_test(uint8_t *cmdbuf, bool enhanced)
+#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS)
+static int
+ble_ll_hci_dtm_tx_test_ext(const uint8_t *cmdbuf)
+{
+    const struct ble_hci_le_tx_test_ext_cp *cmd = (const void *) cmdbuf;
+
+    return ble_ll_dtm_tx_test(cmd->tx_chan, cmd->test_data_len, cmd->payload,
+                              BLE_HCI_LE_PHY_1M, le16toh(cmd->interval),
+                              le16toh(cmd->pkt_count));
+}
+
+static int
+ble_ll_hci_dtm_tx_test_v2_ext(const uint8_t *cmdbuf)
+{
+    const struct ble_hci_le_tx_test_v2_ext_cp *cmd = (const void *) cmdbuf;
+
+    return ble_ll_dtm_tx_test(cmd->tx_chan, cmd->test_data_len, cmd->payload,
+                              cmd->phy, le16toh(cmd->interval),
+                              le16toh(cmd->pkt_count));
+}
+#endif
+
+int
+ble_ll_hci_dtm_tx_test(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_tx_test_cp *cmd = (const void *) cmdbuf;
+
+#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS)
+    if (len == sizeof(struct ble_hci_le_tx_test_ext_cp)) {
+        return ble_ll_hci_dtm_tx_test_ext(cmdbuf);
+    }
+#endif
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    return ble_ll_dtm_tx_test(cmd->tx_chan, cmd->test_data_len, cmd->payload,
+                              BLE_HCI_LE_PHY_1M, 0, 0);
+}
+
+int
+ble_ll_hci_dtm_tx_test_v2(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_tx_test_v2_cp *cmd = (const void *) cmdbuf;
+
+#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS)
+    if (len == sizeof(struct ble_hci_le_tx_test_v2_ext_cp)) {
+        return ble_ll_hci_dtm_tx_test_v2_ext(cmdbuf);
+    }
+#endif
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    return ble_ll_dtm_tx_test(cmd->tx_chan, cmd->test_data_len, cmd->payload,
+                              cmd->phy, 0, 0);
+}
+
+static int
+ble_ll_dtm_rx_test(uint8_t rx_chan, uint8_t hci_phy)
 {
-    uint8_t rx_chan = cmdbuf[0];
-    uint8_t phy_mode = BLE_PHY_MODE_1M;
+    uint8_t phy_mode;
 
     if (g_ble_ll_dtm_ctx.active) {
         return BLE_ERR_CTLR_BUSY;
     }
 
-    /*XXX What to do with modulation cmdbuf[2]? */
-
-    if (enhanced) {
-        switch (cmdbuf[1]) {
-        case BLE_HCI_LE_PHY_1M:
-            phy_mode = BLE_PHY_MODE_1M;
-            break;
+    switch (hci_phy) {
+    case BLE_HCI_LE_PHY_1M:
+        phy_mode = BLE_PHY_MODE_1M;
+        break;
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY)
-        case BLE_HCI_LE_PHY_2M:
-            phy_mode = BLE_PHY_MODE_2M;
-            break;
+    case BLE_HCI_LE_PHY_2M:
+        phy_mode = BLE_PHY_MODE_2M;
+        break;
 #endif
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
-        case BLE_HCI_LE_PHY_CODED:
-            phy_mode = BLE_PHY_MODE_CODED_500KBPS;
-            break;
+    case BLE_HCI_LE_PHY_CODED:
+        phy_mode = BLE_PHY_MODE_CODED_500KBPS;
+        break;
 #endif
-        default:
-            return BLE_ERR_INV_HCI_CMD_PARMS;
-        }
+    default:
+        return BLE_ERR_INV_HCI_CMD_PARMS;
     }
 
     if (rx_chan > 0x27) {
@@ -525,6 +626,30 @@ int ble_ll_dtm_rx_test(uint8_t *cmdbuf, bool enhanced)
     return BLE_ERR_SUCCESS;
 }
 
+int ble_ll_hci_dtm_rx_test(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_rx_test_cp *cmd = (const void *) cmdbuf;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    return ble_ll_dtm_rx_test(cmd->rx_chan, BLE_HCI_LE_PHY_1M);
+}
+
+int ble_ll_hci_dtm_rx_test_v2(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_rx_test_v2_cp *cmd = (const void *) cmdbuf;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    /* TODO ignoring modulation index */
+
+    return ble_ll_dtm_rx_test(cmd->rx_chan, cmd->phy);
+}
+
 int ble_ll_dtm_end_test(uint8_t *rsp, uint8_t *rsplen)
 {
     put_le16(rsp, g_ble_ll_dtm_ctx. num_of_packets);

nimble/controller/src/ble_ll_dtm_priv.h

@@ -24,8 +24,12 @@
 #include <stdbool.h>
 #include "nimble/ble.h"
 
-int ble_ll_dtm_tx_test(uint8_t *cmdbuf, bool enhanced);
-int ble_ll_dtm_rx_test(uint8_t *cmdbuf, bool enhanced);
+int ble_ll_hci_dtm_tx_test(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_hci_dtm_tx_test_v2(const uint8_t *cmdbuf, uint8_t len);
+
+int ble_ll_hci_dtm_rx_test(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_hci_dtm_rx_test_v2(const uint8_t *cmdbuf, uint8_t len);
+
 int ble_ll_dtm_end_test(uint8_t *rsp, uint8_t *rsplen);
 
 int ble_ll_dtm_rx_isr_start(struct ble_mbuf_hdr *rxhdr, uint32_t aa);

nimble/controller/src/ble_ll_hci_ev.c

@@ -40,20 +40,25 @@ extern void bletest_ltk_req_reply(uint16_t handle);
 void
 ble_ll_hci_ev_datalen_chg(struct ble_ll_conn_sm *connsm)
 {
-    uint8_t *evbuf;
+    struct ble_hci_ev_le_subev_data_len_chg *ev;
+    struct ble_hci_ev *hci_ev;
 
     if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_DATA_LEN_CHG)) {
-        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
-        if (evbuf) {
-            evbuf[0] = BLE_HCI_EVCODE_LE_META;
-            evbuf[1] = BLE_HCI_LE_DATA_LEN_CHG_LEN;
-            evbuf[2] = BLE_HCI_LE_SUBEV_DATA_LEN_CHG;
-            put_le16(evbuf + 3, connsm->conn_handle);
-            put_le16(evbuf + 5, connsm->eff_max_tx_octets);
-            put_le16(evbuf + 7, connsm->eff_max_tx_time);
-            put_le16(evbuf + 9, connsm->eff_max_rx_octets);
-            put_le16(evbuf + 11, connsm->eff_max_rx_time);
-            ble_ll_hci_event_send(evbuf);
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        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_DATA_LEN_CHG;
+            ev->conn_handle = htole16(connsm->conn_handle);
+
+            ev->max_tx_octets = htole16(connsm->eff_max_tx_octets);
+            ev->max_tx_time = htole16(connsm->eff_max_tx_time);
+            ev->max_rx_octets = htole16(connsm->eff_max_rx_octets);
+            ev->max_rx_time = htole16(connsm->eff_max_rx_time);
+
+            ble_ll_hci_event_send(hci_ev);
         }
     }
 }
@@ -67,20 +72,24 @@ void
 ble_ll_hci_ev_rem_conn_parm_req(struct ble_ll_conn_sm *connsm,
                                 struct ble_ll_conn_params *cp)
 {
-    uint8_t *evbuf;
+    struct ble_hci_ev_le_subev_rem_conn_param_req *ev;
+    struct ble_hci_ev *hci_ev;
 
     if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_REM_CONN_PARM_REQ)) {
-        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
-        if (evbuf) {
-            evbuf[0] = BLE_HCI_EVCODE_LE_META;
-            evbuf[1] = BLE_HCI_LE_REM_CONN_PARM_REQ_LEN;
-            evbuf[2] = BLE_HCI_LE_SUBEV_REM_CONN_PARM_REQ;
-            put_le16(evbuf + 3, connsm->conn_handle);
-            put_le16(evbuf + 5, cp->interval_min);
-            put_le16(evbuf + 7, cp->interval_max);
-            put_le16(evbuf + 9, cp->latency);
-            put_le16(evbuf + 11, cp->timeout);
-            ble_ll_hci_event_send(evbuf);
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        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_REM_CONN_PARM_REQ;
+            ev->conn_handle = htole16(connsm->conn_handle);
+            ev->min_interval = htole16(cp->interval_min);
+            ev->max_interval = htole16(cp->interval_max);
+            ev->latency = htole16(cp->latency);
+            ev->timeout = htole16(cp->timeout);
+
+            ble_ll_hci_event_send(hci_ev);
         }
     }
 }
@@ -94,58 +103,69 @@ ble_ll_hci_ev_rem_conn_parm_req(struct ble_ll_conn_sm *connsm,
 void
 ble_ll_hci_ev_conn_update(struct ble_ll_conn_sm *connsm, uint8_t status)
 {
-    uint8_t *evbuf;
+    struct ble_hci_ev_le_subev_conn_upd_complete *ev;
+    struct ble_hci_ev *hci_ev;
 
     if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_CONN_UPD_COMPLETE)) {
-        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
-        if (evbuf) {
-            evbuf[0] = BLE_HCI_EVCODE_LE_META;
-            evbuf[1] = BLE_HCI_LE_CONN_UPD_LEN;
-            evbuf[2] = BLE_HCI_LE_SUBEV_CONN_UPD_COMPLETE;
-            evbuf[3] = status;
-            put_le16(evbuf + 4, connsm->conn_handle);
-            put_le16(evbuf + 6, connsm->conn_itvl);
-            put_le16(evbuf + 8, connsm->slave_latency);
-            put_le16(evbuf + 10, connsm->supervision_tmo);
-            ble_ll_hci_event_send(evbuf);
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        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_CONN_UPD_COMPLETE;
+            ev->status = status;
+            ev->conn_handle = htole16(connsm->conn_handle);
+            ev->conn_itvl = htole16(connsm->conn_itvl);
+            ev->conn_latency = htole16(connsm->slave_latency);
+            ev->supervision_timeout = htole16(connsm->supervision_tmo);
+
+            ble_ll_hci_event_send(hci_ev);
         }
     }
 }
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
 void
 ble_ll_hci_ev_encrypt_chg(struct ble_ll_conn_sm *connsm, uint8_t status)
 {
-    uint8_t evcode;
-    uint8_t *evbuf;
-    uint8_t evlen;
+    struct ble_hci_ev_enc_key_refresh *ev_key_refresh;
+    struct ble_hci_ev_enrypt_chg *ev_enc_chf;
+    struct ble_hci_ev *hci_ev;
 
     if (CONN_F_ENC_CHANGE_SENT(connsm) == 0) {
-        evcode = BLE_HCI_EVCODE_ENCRYPT_CHG;
-        evlen = BLE_HCI_EVENT_ENCRYPT_CHG_LEN;
-    } else {
-        evcode = BLE_HCI_EVCODE_ENC_KEY_REFRESH;
-        evlen = BLE_HCI_EVENT_ENC_KEY_REFRESH_LEN;
+        if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_ENCRYPT_CHG)) {
+            hci_ev = (void *)ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+            if (hci_ev) {
+                hci_ev->opcode = BLE_HCI_EVCODE_ENCRYPT_CHG;
+                hci_ev->length = sizeof(*ev_enc_chf);
+                ev_enc_chf = (void *) hci_ev->data;
+
+                ev_enc_chf->status = status;
+                ev_enc_chf->connection_handle = htole16(connsm->conn_handle);
+                ev_enc_chf->enabled = (status == BLE_ERR_SUCCESS) ? 0x01 : 0x00;
+
+                ble_ll_hci_event_send(hci_ev);
+            }
+        }
+
+        CONN_F_ENC_CHANGE_SENT(connsm) = 1;
+        return;
     }
 
-    if (ble_ll_hci_is_event_enabled(evcode)) {
-        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
-        if (evbuf) {
-            evbuf[0] = evcode;
-            evbuf[1] = evlen;
-            evbuf[2] = status;
-            put_le16(evbuf + 3, connsm->conn_handle);
-            if (evcode == BLE_HCI_EVCODE_ENCRYPT_CHG) {
-                if (status == BLE_ERR_SUCCESS) {
-                    evbuf[5] = 0x01;
-                } else {
-                    evbuf[5] = 0;
-                }
-            }
-            ble_ll_hci_event_send(evbuf);
+    if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_ENC_KEY_REFRESH)) {
+        hci_ev = (void *)ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_ENC_KEY_REFRESH;
+            hci_ev->length = sizeof(*ev_key_refresh);
+            ev_key_refresh = (void *) hci_ev->data;
+
+            ev_key_refresh->status = status;
+            ev_key_refresh->conn_handle = htole16(connsm->conn_handle);
+
+            ble_ll_hci_event_send(hci_ev);
         }
     }
-    CONN_F_ENC_CHANGE_SENT(connsm) = 1;
 }
 
 /**
@@ -156,19 +176,23 @@ ble_ll_hci_ev_encrypt_chg(struct ble_ll_conn_sm *connsm, uint8_t status)
 int
 ble_ll_hci_ev_ltk_req(struct ble_ll_conn_sm *connsm)
 {
+    struct ble_hci_ev_le_subev_lt_key_req *ev;
+    struct ble_hci_ev *hci_ev;
     int rc;
-    uint8_t *evbuf;
 
     if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_LT_KEY_REQ)) {
-        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
-        if (evbuf) {
-            evbuf[0] = BLE_HCI_EVCODE_LE_META;
-            evbuf[1] = BLE_HCI_LE_LT_KEY_REQ_LEN;
-            evbuf[2] = BLE_HCI_LE_SUBEV_LT_KEY_REQ;
-            put_le16(evbuf + 3, connsm->conn_handle);
-            put_le64(evbuf + 5, connsm->enc_data.host_rand_num);
-            put_le16(evbuf + 13, connsm->enc_data.enc_div);
-            ble_ll_hci_event_send(evbuf);
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        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_LT_KEY_REQ;
+            ev->conn_handle = htole16(connsm->conn_handle);
+            ev->rand = htole64(connsm->enc_data.host_rand_num);
+            ev->div = htole16(connsm->enc_data.enc_div);
+
+            ble_ll_hci_event_send(hci_ev);
         }
         rc = 0;
     } else {
@@ -187,20 +211,23 @@ ble_ll_hci_ev_ltk_req(struct ble_ll_conn_sm *connsm)
 void
 ble_ll_hci_ev_rd_rem_used_feat(struct ble_ll_conn_sm *connsm, uint8_t status)
 {
-    uint8_t *evbuf;
+    struct ble_hci_ev_le_subev_rd_rem_used_feat *ev;
+    struct ble_hci_ev *hci_ev;
 
     if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_RD_REM_USED_FEAT)) {
-        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
-        if (evbuf) {
-            evbuf[0] = BLE_HCI_EVCODE_LE_META;
-            evbuf[1] = BLE_HCI_LE_RD_REM_USED_FEAT_LEN;
-            evbuf[2] = BLE_HCI_LE_SUBEV_RD_REM_USED_FEAT;
-            evbuf[3] = status;
-            put_le16(evbuf + 4, connsm->conn_handle);
-            memset(evbuf + 6, 0, BLE_HCI_RD_LOC_SUPP_FEAT_RSPLEN);
-            evbuf[6] = connsm->conn_features;
-            memcpy(evbuf + 7, connsm->remote_features, 7);
-            ble_ll_hci_event_send(evbuf);
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        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_RD_REM_USED_FEAT;
+            ev->status = status;
+            ev->conn_handle = htole16(connsm->conn_handle);
+            ev->features[0] = connsm->conn_features;
+            memcpy(ev->features + 1, connsm->remote_features, 7);
+
+            ble_ll_hci_event_send(hci_ev);
         }
     }
 }
@@ -208,19 +235,23 @@ ble_ll_hci_ev_rd_rem_used_feat(struct ble_ll_conn_sm *connsm, uint8_t status)
 void
 ble_ll_hci_ev_rd_rem_ver(struct ble_ll_conn_sm *connsm, uint8_t status)
 {
-    uint8_t *evbuf;
+    struct ble_hci_ev_rd_rem_ver_info_cmp *ev;
+    struct ble_hci_ev *hci_ev;
 
     if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_RD_REM_VER_INFO_CMP)) {
-        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
-        if (evbuf) {
-            evbuf[0] = BLE_HCI_EVCODE_RD_REM_VER_INFO_CMP;
-            evbuf[1] = BLE_HCI_EVENT_RD_RM_VER_LEN;
-            evbuf[2] = status;
-            put_le16(evbuf + 3, connsm->conn_handle);
-            evbuf[5] = connsm->vers_nr;
-            put_le16(evbuf + 6, connsm->comp_id);
-            put_le16(evbuf + 8, connsm->sub_vers_nr);
-            ble_ll_hci_event_send(evbuf);
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_RD_REM_VER_INFO_CMP;
+            hci_ev->length = sizeof(*ev);
+            ev = (void *) hci_ev->data;
+
+            ev->status = status;
+            ev->conn_handle = htole16(connsm->conn_handle);
+            ev->version = connsm->vers_nr;
+            ev->manufacturer = htole16(connsm->comp_id);
+            ev->subversion = htole16(connsm->sub_vers_nr);
+
+            ble_ll_hci_event_send(hci_ev);
         }
     }
 }
@@ -235,17 +266,21 @@ ble_ll_hci_ev_rd_rem_ver(struct ble_ll_conn_sm *connsm, uint8_t status)
 int
 ble_ll_hci_ev_hw_err(uint8_t hw_err)
 {
+    struct ble_hci_ev_hw_error *ev;
+    struct ble_hci_ev *hci_ev;
     int rc;
-    uint8_t *evbuf;
 
     rc = 0;
     if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_HW_ERROR)) {
-        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
-        if (evbuf) {
-            evbuf[0] = BLE_HCI_EVCODE_HW_ERROR;
-            evbuf[1] = BLE_HCI_EVENT_HW_ERROR_LEN;
-            evbuf[2] = hw_err;
-            ble_ll_hci_event_send(evbuf);
+        hci_ev = (void *)ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_HW_ERROR;
+            hci_ev->length = sizeof(*ev);
+            ev = (void *) hci_ev->data;
+
+            ev->hw_code = hw_err;
+
+            ble_ll_hci_event_send(hci_ev);
         } else {
             rc = -1;
         }
@@ -256,15 +291,19 @@ ble_ll_hci_ev_hw_err(uint8_t hw_err)
 void
 ble_ll_hci_ev_databuf_overflow(void)
 {
-    uint8_t *evbuf;
+    struct ble_hci_ev_data_buf_overflow *ev;
+    struct ble_hci_ev *hci_ev;
 
     if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_DATA_BUF_OVERFLOW)) {
-        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
-        if (evbuf) {
-            evbuf[0] = BLE_HCI_EVCODE_DATA_BUF_OVERFLOW;
-            evbuf[1] = BLE_HCI_EVENT_DATABUF_OVERFLOW_LEN;
-            evbuf[2] = BLE_HCI_EVENT_ACL_BUF_OVERFLOW;
-            ble_ll_hci_event_send(evbuf);
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        if (hci_ev) {
+            hci_ev->opcode = BLE_HCI_EVCODE_DATA_BUF_OVERFLOW;
+            hci_ev->length = sizeof(*ev);
+            ev = (void *) hci_ev->data;
+
+            ev->link_type = BLE_HCI_EVENT_ACL_BUF_OVERFLOW;
+
+            ble_ll_hci_event_send(hci_ev);
         }
     }
 }
@@ -274,21 +313,25 @@ ble_ll_hci_ev_databuf_overflow(void)
  *
  * @param connsm Pointer to connection state machine
  */
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2)
 void
 ble_ll_hci_ev_le_csa(struct ble_ll_conn_sm *connsm)
 {
-    uint8_t *evbuf;
+    struct ble_hci_ev_le_subev_chan_sel_alg *ev;
+    struct ble_hci_ev *hci_ev;
 
     if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_CHAN_SEL_ALG)) {
-        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
-        if (evbuf) {
-            evbuf[0] = BLE_HCI_EVCODE_LE_META;
-            evbuf[1] = BLE_HCI_LE_SUBEV_CHAN_SEL_ALG_LEN;
-            evbuf[2] = BLE_HCI_LE_SUBEV_CHAN_SEL_ALG;
-            put_le16(evbuf + 3, connsm->conn_handle);
-            evbuf[5] = connsm->csmflags.cfbit.csa2_supp ? 0x01 : 0x00;
-            ble_ll_hci_event_send(evbuf);
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        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_CHAN_SEL_ALG;
+            ev->conn_handle = htole16(connsm->conn_handle);
+            ev->csa = connsm->csmflags.cfbit.csa2_supp ? 0x01 : 0x00;
+
+            ble_ll_hci_event_send(hci_ev);
         }
     }
 }
@@ -303,18 +346,22 @@ void
 ble_ll_hci_ev_send_scan_req_recv(uint8_t adv_handle, const uint8_t *peer,
                                  uint8_t peer_addr_type)
 {
-    uint8_t *evbuf;
+    struct ble_hci_ev_le_subev_scan_req_rcvd *ev;
+    struct ble_hci_ev *hci_ev;
 
     if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_SCAN_REQ_RCVD)) {
-        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
-        if (evbuf) {
-            evbuf[0] = BLE_HCI_EVCODE_LE_META;
-            evbuf[1] = BLE_HCI_LE_SUBEV_SCAN_REQ_RCVD_LEN;
-            evbuf[2] = BLE_HCI_LE_SUBEV_SCAN_REQ_RCVD;
-            evbuf[3] = adv_handle;
-            evbuf[4] = peer_addr_type;
-            memcpy(&evbuf[5], peer, BLE_DEV_ADDR_LEN);
-            ble_ll_hci_event_send(evbuf);
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        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_SCAN_REQ_RCVD;
+            ev->adv_handle = adv_handle;
+            ev->peer_addr_type = peer_addr_type;
+            memcpy(ev->peer_addr, peer, BLE_DEV_ADDR_LEN);
+
+            ble_ll_hci_event_send(hci_ev);
         }
     }
 }
@@ -328,15 +375,19 @@ ble_ll_hci_ev_send_scan_req_recv(uint8_t adv_handle, const uint8_t *peer,
 void
 ble_ll_hci_ev_send_scan_timeout(void)
 {
-    uint8_t *evbuf;
+    struct ble_hci_ev_le_subev_scan_timeout *ev;
+    struct ble_hci_ev *hci_ev;
 
     if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_SCAN_TIMEOUT)) {
-        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
-        if (evbuf) {
-            evbuf[0] = BLE_HCI_EVCODE_LE_META;
-            evbuf[1] = BLE_HCI_LE_SUBEV_SCAN_TIMEOUT_LEN;
-            evbuf[2] = BLE_HCI_LE_SUBEV_SCAN_TIMEOUT;
-            ble_ll_hci_event_send(evbuf);
+        hci_ev = (void *)ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        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_SCAN_TIMEOUT;
+
+            ble_ll_hci_event_send(hci_ev);
         }
     }
 }
@@ -351,19 +402,23 @@ void
 ble_ll_hci_ev_send_adv_set_terminated(uint8_t status, uint8_t adv_handle,
                                       uint16_t conn_handle, uint8_t events)
 {
-    uint8_t *evbuf;
+    struct ble_hci_ev_le_subev_adv_set_terminated *ev;
+    struct ble_hci_ev *hci_ev;
 
     if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_ADV_SET_TERMINATED)) {
-        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
-        if (evbuf) {
-            evbuf[0] = BLE_HCI_EVCODE_LE_META;
-            evbuf[1] = BLE_HCI_LE_SUBEV_ADV_SET_TERMINATED_LEN;
-            evbuf[2] = BLE_HCI_LE_SUBEV_ADV_SET_TERMINATED;
-            evbuf[3] = status;
-            evbuf[4] = adv_handle;
-            put_le16(evbuf + 5, conn_handle);
-            evbuf[7] = events;
-            ble_ll_hci_event_send(evbuf);
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        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_ADV_SET_TERMINATED;
+            ev->status = status;
+            ev->adv_handle = adv_handle;
+            ev->conn_handle = htole16(conn_handle);
+            ev->num_events = events;
+
+            ble_ll_hci_event_send(hci_ev);
         }
     }
 }
@@ -379,21 +434,25 @@ ble_ll_hci_ev_send_adv_set_terminated(uint8_t status, uint8_t adv_handle,
 int
 ble_ll_hci_ev_phy_update(struct ble_ll_conn_sm *connsm, uint8_t status)
 {
+    struct ble_hci_ev_le_subev_phy_update_complete *ev;
+    struct ble_hci_ev *hci_ev;
     int rc;
-    uint8_t *evbuf;
 
     rc = 0;
     if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PHY_UPDATE_COMPLETE)) {
-        evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
-        if (evbuf) {
-            evbuf[0] = BLE_HCI_EVCODE_LE_META;
-            evbuf[1] = BLE_HCI_LE_PHY_UPD_LEN;
-            evbuf[2] = BLE_HCI_LE_SUBEV_PHY_UPDATE_COMPLETE;
-            evbuf[3] = status;
-            put_le16(evbuf + 4, connsm->conn_handle);
-            evbuf[6] = connsm->phy_data.cur_tx_phy;
-            evbuf[7] = connsm->phy_data.cur_rx_phy;
-            ble_ll_hci_event_send(evbuf);
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        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_PHY_UPDATE_COMPLETE;
+            ev->status = status;
+            ev->conn_handle = htole16(connsm->conn_handle);
+            ev->tx_phy = connsm->phy_data.cur_tx_phy;
+            ev->rx_phy = connsm->phy_data.cur_rx_phy;
+
+            ble_ll_hci_event_send(hci_ev);
         } else {
             rc = BLE_ERR_MEM_CAPACITY;
         }
@@ -402,22 +461,121 @@ ble_ll_hci_ev_phy_update(struct ble_ll_conn_sm *connsm, uint8_t status)
 }
 #endif
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_SCA_UPDATE)
 void
-ble_ll_hci_ev_send_vendor_err(char *file, uint32_t line)
+ble_ll_hci_ev_sca_update(struct ble_ll_conn_sm *connsm, uint8_t status,
+                         uint8_t peer_sca)
 {
-    uint8_t *evbuf;
-    uint8_t file_len = strlen(file);
+    struct ble_hci_ev_le_subev_peer_sca_complete *ev;
+    struct ble_hci_ev *hci_ev;
 
-    evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
-    if (!evbuf) {
+    if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_REQ_PEER_SCA_COMP)) {
         return;
     }
 
-    evbuf[0] = BLE_HCI_EVCODE_VENDOR_DEBUG;
-    evbuf[1] = file_len + sizeof(line) + 1;
-    /* Debug id for future use */
-    evbuf[2] = 0x00;
-    memcpy(&evbuf[3], file, file_len);
-    put_le32(&evbuf[3] + file_len, line);
-    ble_ll_hci_event_send(evbuf);
+    hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+    if (!hci_ev) {
+        return;
+    }
+
+    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_REQ_PEER_SCA_COMP;
+    ev->status = status;
+    ev->conn_handle = htole16(connsm->conn_handle);
+    ev->sca = peer_sca;
+
+    ble_ll_hci_event_send(hci_ev);
+}
+
+#endif
+
+void
+ble_ll_hci_ev_send_vendor_err(const char *file, uint32_t line)
+{
+    struct ble_hci_ev_vendor_debug *ev;
+    struct ble_hci_ev *hci_ev;
+    unsigned int str_len;
+    bool skip = true;
+    uint8_t digit;
+    int max_len;
+    int i;
+
+    /* 6 is for line number ":00000" , we assume files have no more than 64k of
+     * lines
+     */
+    max_len = BLE_HCI_MAX_DATA_LEN - sizeof(*ev) - 6;
+
+    hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+    if (hci_ev) {
+        hci_ev->opcode = BLE_HCI_EVCODE_VENDOR_DEBUG;
+        hci_ev->length = sizeof(*ev);
+        ev = (void *) hci_ev->data;
+
+        /* Debug id for future use */
+        ev->id = 0x00;
+
+        /* snprintf would be nicer but this is heavy on flash
+         * len = snprintf((char *) ev->data, max_len, "%s:%u", file, line);
+         * if (len < 0) {
+         *     len = 0;
+         * } else if (len > max_len) {
+         *     len = max_len;
+         * }
+         *
+         *  hci_ev->length += len;
+         */
+        str_len = strlen(file);
+        if (str_len > max_len) {
+            str_len = max_len;
+        }
+
+        memcpy(ev->data, file, str_len);
+        ev->data[str_len++] = ':';
+
+        for (i = 100000; i >= 10; i /= 10) {
+            digit = (line % i) / (i/10);
+
+            if (!digit && skip) {
+                continue;
+            }
+
+            skip = false;
+            ev->data[str_len++] = '0' + digit;
+        }
+
+        hci_ev->length += str_len;
+
+        ble_ll_hci_event_send(hci_ev);
+    }
+}
+
+#if MYNEWT_VAL(BLE_LL_HCI_LLCP_TRACE)
+void
+ble_ll_hci_ev_send_llcp_trace(uint8_t type, uint16_t handle, uint16_t count,
+                              void *pdu, size_t length)
+{
+    struct ble_hci_ev_vendor_debug *ev;
+    struct ble_hci_ev *hci_ev;
+
+    hci_ev = (void *)ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO);
+    if (hci_ev) {
+        hci_ev->opcode = BLE_HCI_EVCODE_VENDOR_DEBUG;
+        hci_ev->length = sizeof(*ev) + 8 + length;
+        ev = (void *) hci_ev->data;
+
+        ev->id = 0x17;
+        ev->data[0] = type;
+        put_le16(&ev->data[1], handle);
+        put_le16(&ev->data[3], count);
+        ev->data[5] = 0;
+        ev->data[6] = 0;
+        ev->data[7] = 0;
+        memcpy(&ev->data[8], pdu, length);
+
+        ble_ll_hci_event_send(hci_ev);
+    }
 }
+#endif

nimble/controller/src/ble_ll_iso.c

@@ -0,0 +1,146 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stdint.h>
+#include "syscfg/syscfg.h"
+#include "nimble/ble.h"
+#include "nimble/hci_common.h"
+#include "controller/ble_ll_iso.h"
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_ISO)
+
+int
+ble_ll_iso_read_tx_sync(const uint8_t *cmdbuf, uint8_t len)
+{
+    return BLE_ERR_UNSUPPORTED;
+}
+
+int
+ble_ll_iso_set_cig_param(const uint8_t *cmdbuf, uint8_t len,
+                         uint8_t *rspbuf, uint8_t *rsplen)
+{
+    return BLE_ERR_UNSUPPORTED;
+}
+
+int
+ble_ll_iso_create_cis(const uint8_t *cmdbuf, uint8_t len)
+{
+    return BLE_ERR_UNSUPPORTED;
+}
+
+int
+ble_ll_iso_disconnect_cmd(const struct ble_hci_lc_disconnect_cp *cmd)
+{
+    return BLE_ERR_UNSUPPORTED;
+}
+
+int
+ble_ll_iso_remove_cig(const uint8_t *cmdbuf, uint8_t len,
+                      uint8_t *rspbuf, uint8_t *rsplen)
+{
+    return BLE_ERR_UNSUPPORTED;
+}
+
+int
+ble_ll_iso_accept_cis_req(const uint8_t *cmdbuf, uint8_t len)
+{
+    return BLE_ERR_UNSUPPORTED;
+}
+
+int
+ble_ll_iso_reject_cis_req(const uint8_t *cmdbuf, uint8_t len)
+{
+    return BLE_ERR_UNSUPPORTED;
+}
+
+int
+ble_ll_iso_setup_iso_data_path(const uint8_t *cmdbuf, uint8_t len)
+{
+    return BLE_ERR_UNSUPPORTED;
+}
+
+int
+ble_ll_iso_remove_iso_data_path(const uint8_t *cmdbuf, uint8_t len)
+{
+    /* Nothing to do here for now when HCI is supported */
+    return 0;
+}
+int
+ble_ll_iso_create_big(const uint8_t *cmdbuf, uint8_t len)
+{
+    return BLE_ERR_UNSUPPORTED;
+}
+
+int
+ble_ll_iso_terminate_big(const uint8_t *cmdbuf, uint8_t len)
+{
+    return BLE_ERR_UNSUPPORTED;
+}
+
+int
+ble_ll_iso_big_create_sync(const uint8_t *cmdbuf, uint8_t len)
+{
+    return BLE_ERR_UNSUPPORTED;
+}
+
+int
+ble_ll_iso_big_terminate_sync(const uint8_t *cmdbuf, uint8_t len)
+{
+    return BLE_ERR_UNSUPPORTED;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_ISO_TEST)
+int
+ble_ll_iso_set_cig_param_test(const uint8_t *cmdbuf, uint8_t len,
+                              uint8_t *rspbuf, uint8_t *rsplen)
+{
+    return BLE_ERR_UNSUPPORTED;
+}
+
+int
+ble_ll_iso_create_big_test(const uint8_t *cmdbuf, uint8_t len)
+{
+    return BLE_ERR_UNSUPPORTED;
+}
+
+int
+ble_ll_iso_transmit_test(const uint8_t *cmdbuf, uint8_t len)
+{
+    return BLE_ERR_UNSUPPORTED;
+}
+
+int
+ble_ll_iso_receive_test(const uint8_t *cmdbuf, uint8_t len)
+{
+    return BLE_ERR_UNSUPPORTED;
+}
+
+int
+ble_ll_iso_read_counters_test(const uint8_t *cmdbuf, uint8_t len)
+{
+    return BLE_ERR_UNSUPPORTED;
+}
+
+int
+ble_ll_iso_end_test(const uint8_t *cmdbuf, uint8_t len)
+{
+    return BLE_ERR_UNSUPPORTED;
+}
+#endif
+#endif

nimble/controller/src/ble_ll_priv.h

@@ -0,0 +1,51 @@
+/*
+ * 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_PRIV_
+#define H_BLE_LL_PRIV_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifdef MYNEWT
+
+#include "syscfg/syscfg.h"
+#include "hal/hal_gpio.h"
+
+#define BLE_LL_DEBUG_GPIO_INIT(_name)                                       \
+    if (MYNEWT_VAL(BLE_LL_DEBUG_GPIO_ ## _name) >= 0) {                     \
+        hal_gpio_init_out(MYNEWT_VAL(BLE_LL_DEBUG_GPIO_ ## _name), 0);      \
+    }
+
+#define BLE_LL_DEBUG_GPIO(_name, _val)                                      \
+    if (MYNEWT_VAL(BLE_LL_DEBUG_GPIO_ ## _name) >= 0) {                     \
+        hal_gpio_write(MYNEWT_VAL(BLE_LL_DEBUG_GPIO_ ## _name), !!(_val));  \
+    }
+
+#else
+#define BLE_LL_DEBUG_GPIO_INIT(_name)       (void)(0)
+#define BLE_LL_DEBUG_GPIO(_name, _val)      (void)(0)
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_LL_PRIV_ */

nimble/controller/src/ble_ll_rand.c

@@ -17,6 +17,9 @@
  * under the License.
  */
 
+/* for jrand48 */
+#define _XOPEN_SOURCE
+#include <stdlib.h>
 #include <stdint.h>
 #include <assert.h>
 #include <string.h>
@@ -120,6 +123,21 @@ ble_ll_rand_data_get(uint8_t *buf, uint8_t len)
     return BLE_ERR_SUCCESS;
 }
 
+/* Simple wrapper to allow easy replacement of rand() */
+uint32_t
+ble_ll_rand(void)
+{
+    static unsigned short xsubi[3];
+    static bool init = true;
+
+    if (init) {
+        init = false;
+        ble_ll_rand_data_get((uint8_t *)xsubi, sizeof(xsubi));
+    }
+
+    return (uint32_t) jrand48(xsubi);
+}
+
 /**
  * Called to obtain a "prand" as defined in core V4.2 Vol 6 Part B 1.3.2.2
  *

nimble/controller/src/ble_ll_resolv.c

@@ -28,27 +28,36 @@
 #include "controller/ble_ll_hci.h"
 #include "controller/ble_ll_scan.h"
 #include "controller/ble_ll_adv.h"
+#include "controller/ble_ll_sync.h"
 #include "controller/ble_hw.h"
 #include "ble_ll_conn_priv.h"
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
 struct ble_ll_resolv_data
 {
     uint8_t addr_res_enabled;
     uint8_t rl_size;
+    uint8_t rl_cnt_hw;
     uint8_t rl_cnt;
-    uint32_t rpa_tmo;
+    ble_npl_time_t rpa_tmo;
     struct ble_npl_callout rpa_timer;
 };
 struct ble_ll_resolv_data g_ble_ll_resolv_data;
 
+__attribute__((aligned(4)))
 struct ble_ll_resolv_entry g_ble_ll_resolv_list[MYNEWT_VAL(BLE_LL_RESOLV_LIST_SIZE)];
 
 static int
 ble_ll_is_controller_busy(void)
 {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    if (ble_ll_sync_enabled()) {
+        return 1;
+    }
+#endif
+
     return ble_ll_adv_enabled() || ble_ll_scan_enabled() ||
-           g_ble_ll_conn_create_sm;
+           g_ble_ll_conn_create_sm.connsm;
 }
 /**
  * Called to determine if a change is allowed to the resolving list at this
@@ -129,17 +138,22 @@ ble_ll_resolv_rpa_timer_cb(struct ble_npl_event *ev)
 
     rl = &g_ble_ll_resolv_list[0];
     for (i = 0; i < g_ble_ll_resolv_data.rl_cnt; ++i) {
-        OS_ENTER_CRITICAL(sr);
-        ble_ll_resolv_gen_priv_addr(rl, 1);
-        OS_EXIT_CRITICAL(sr);
+        if (rl->rl_has_local) {
+            OS_ENTER_CRITICAL(sr);
+            ble_ll_resolv_gen_priv_addr(rl, 1);
+            OS_EXIT_CRITICAL(sr);
+        }
 
-        OS_ENTER_CRITICAL(sr);
-        ble_ll_resolv_gen_priv_addr(rl, 0);
-        OS_EXIT_CRITICAL(sr);
+        if (rl->rl_has_peer) {
+            OS_ENTER_CRITICAL(sr);
+            ble_ll_resolv_gen_priv_addr(rl, 0);
+            OS_EXIT_CRITICAL(sr);
+        }
         ++rl;
     }
+
     ble_npl_callout_reset(&g_ble_ll_resolv_data.rpa_timer,
-                     (int32_t)g_ble_ll_resolv_data.rpa_tmo);
+                          g_ble_ll_resolv_data.rpa_tmo);
 
     ble_ll_adv_rpa_timeout();
 }
@@ -151,8 +165,8 @@ ble_ll_resolv_rpa_timer_cb(struct ble_npl_event *ev)
  *
  * @return int 0: IRK is zero . 1: IRK has non-zero value.
  */
-int
-ble_ll_resolv_irk_nonzero(uint8_t *irk)
+static int
+ble_ll_resolv_irk_nonzero(const uint8_t *irk)
 {
     int i;
     int rc;
@@ -183,9 +197,13 @@ ble_ll_resolv_list_clr(void)
     }
 
     /* Sets total on list to 0. Clears HW resolve list */
+    g_ble_ll_resolv_data.rl_cnt_hw = 0;
     g_ble_ll_resolv_data.rl_cnt = 0;
     ble_hw_resolv_list_clear();
 
+    /* stop RPA timer when clearing RL */
+    ble_npl_callout_stop(&g_ble_ll_resolv_data.rpa_timer);
+
     return BLE_ERR_SUCCESS;
 }
 
@@ -200,8 +218,11 @@ ble_ll_resolv_list_clr(void)
 int
 ble_ll_resolv_list_read_size(uint8_t *rspbuf, uint8_t *rsplen)
 {
-    rspbuf[0] = g_ble_ll_resolv_data.rl_size;
-    *rsplen = 1;
+    struct ble_hci_le_rd_resolv_list_size_rp *rsp = (void *) rspbuf;
+
+    rsp->size = g_ble_ll_resolv_data.rl_size;
+
+    *rsplen = sizeof(*rsp);
     return BLE_ERR_SUCCESS;
 }
 
@@ -216,7 +237,7 @@ ble_ll_resolv_list_read_size(uint8_t *rspbuf, uint8_t *rsplen)
  * element plus 1).
  */
 static int
-ble_ll_is_on_resolv_list(uint8_t *addr, uint8_t addr_type)
+ble_ll_is_on_resolv_list(const uint8_t *addr, uint8_t addr_type)
 {
     int i;
     struct ble_ll_resolv_entry *rl;
@@ -242,7 +263,7 @@ ble_ll_is_on_resolv_list(uint8_t *addr, uint8_t addr_type)
  * @return Pointer to resolving list entry or NULL if no entry found.
  */
 struct ble_ll_resolv_entry *
-ble_ll_resolv_list_find(uint8_t *addr, uint8_t addr_type)
+ble_ll_resolv_list_find(const uint8_t *addr, uint8_t addr_type)
 {
     int i;
     struct ble_ll_resolv_entry *rl;
@@ -265,12 +286,15 @@ ble_ll_resolv_list_find(uint8_t *addr, uint8_t addr_type)
  * @return int
  */
 int
-ble_ll_resolv_list_add(uint8_t *cmdbuf)
+ble_ll_resolv_list_add(const uint8_t *cmdbuf, uint8_t len)
 {
-    int rc;
-    uint8_t addr_type;
-    uint8_t *ident_addr;
+    const struct ble_hci_le_add_resolv_list_cp *cmd = (const void *) cmdbuf;
     struct ble_ll_resolv_entry *rl;
+    int rc = BLE_ERR_SUCCESS;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
 
     /* Must be in proper state */
     if (!ble_ll_resolv_list_chg_allowed()) {
@@ -282,39 +306,67 @@ ble_ll_resolv_list_add(uint8_t *cmdbuf)
         return BLE_ERR_MEM_CAPACITY;
     }
 
-    addr_type = cmdbuf[0];
-    ident_addr = cmdbuf + 1;
-
     /* spec is not clear on how to handle this but make sure host is aware
      * that new keys are not used in that case
      */
-    if (ble_ll_is_on_resolv_list(ident_addr, addr_type)) {
+    if (ble_ll_is_on_resolv_list(cmd->peer_id_addr, cmd->peer_addr_type)) {
         return BLE_ERR_INV_HCI_CMD_PARMS;
     }
 
-    rl = &g_ble_ll_resolv_list[g_ble_ll_resolv_data.rl_cnt];
+    /* we keep this sorted in a way that entries with peer_irk are first */
+    if (ble_ll_resolv_irk_nonzero(cmd->peer_irk)) {
+        memmove(&g_ble_ll_resolv_list[g_ble_ll_resolv_data.rl_cnt_hw + 1],
+                &g_ble_ll_resolv_list[g_ble_ll_resolv_data.rl_cnt_hw],
+                (g_ble_ll_resolv_data.rl_cnt - g_ble_ll_resolv_data.rl_cnt_hw) *
+                sizeof(g_ble_ll_resolv_list[0]));
+        rl = &g_ble_ll_resolv_list[g_ble_ll_resolv_data.rl_cnt_hw];
+    } else {
+        rl = &g_ble_ll_resolv_list[g_ble_ll_resolv_data.rl_cnt];
+    }
+
     memset (rl, 0, sizeof(*rl));
+    rl->rl_addr_type = cmd->peer_addr_type;
+    memcpy(rl->rl_identity_addr, cmd->peer_id_addr, BLE_DEV_ADDR_LEN);
+
+    if (ble_ll_resolv_irk_nonzero(cmd->peer_irk)) {
+        swap_buf(rl->rl_peer_irk, cmd->peer_irk, 16);
+        rl->rl_has_peer = 1;
+
+        /* generate peer RPA now, those will be updated by timer when
+         * resolution is enabled
+         */
+        ble_ll_resolv_gen_priv_addr(rl, 0);
+    }
+
+    if (ble_ll_resolv_irk_nonzero(cmd->local_irk)) {
+        swap_buf(rl->rl_local_irk, cmd->local_irk, 16);
+        rl->rl_has_local = 1;
 
-    rl->rl_addr_type = addr_type;
-    memcpy(&rl->rl_identity_addr[0], ident_addr, BLE_DEV_ADDR_LEN);
-    swap_buf(rl->rl_peer_irk, cmdbuf + 7, 16);
-    swap_buf(rl->rl_local_irk, cmdbuf + 23, 16);
+        /* generate local RPA now, those will be updated by timer when
+         * resolution is enabled
+         */
+        ble_ll_resolv_gen_priv_addr(rl, 1);
+    }
 
     /* By default use privacy network mode */
     rl->rl_priv_mode = BLE_HCI_PRIVACY_NETWORK;
 
-    /* Add peer IRK to HW resolving list. Should always succeed since we
+    /* Add peers IRKs to HW resolving list. Should always succeed since we
      * already checked if there is room for it.
      */
-    rc = ble_hw_resolv_list_add(rl->rl_peer_irk);
-    BLE_LL_ASSERT (rc == BLE_ERR_SUCCESS);
+    if (rl->rl_has_peer) {
+        rc = ble_hw_resolv_list_add(rl->rl_peer_irk);
+        BLE_LL_ASSERT(rc == BLE_ERR_SUCCESS);
+        g_ble_ll_resolv_data.rl_cnt_hw++;
+    }
 
-    /* generate a local and peer RPAs now, those will be updated by timer
-     * when resolution is enabled
-     */
-    ble_ll_resolv_gen_priv_addr(rl, 1);
-    ble_ll_resolv_gen_priv_addr(rl, 0);
-    ++g_ble_ll_resolv_data.rl_cnt;
+    g_ble_ll_resolv_data.rl_cnt++;
+
+    /* start RPA timer if this was first element added to RL */
+    if (g_ble_ll_resolv_data.rl_cnt == 1) {
+        ble_npl_callout_reset(&g_ble_ll_resolv_data.rpa_timer,
+                              g_ble_ll_resolv_data.rpa_tmo);
+    }
 
     return rc;
 }
@@ -327,32 +379,42 @@ ble_ll_resolv_list_add(uint8_t *cmdbuf)
  * @return int 0: success, BLE error code otherwise
  */
 int
-ble_ll_resolv_list_rmv(uint8_t *cmdbuf)
+ble_ll_resolv_list_rmv(const uint8_t *cmdbuf, uint8_t len)
 {
+    const struct ble_hci_le_rmv_resolve_list_cp *cmd = (const void *) cmdbuf;
     int position;
-    uint8_t addr_type;
-    uint8_t *ident_addr;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
 
     /* Must be in proper state */
     if (!ble_ll_resolv_list_chg_allowed()) {
         return BLE_ERR_CMD_DISALLOWED;
     }
 
-    addr_type = cmdbuf[0];
-    ident_addr = cmdbuf + 1;
-
     /* Remove from IRK records */
-    position = ble_ll_is_on_resolv_list(ident_addr, addr_type);
+    position = ble_ll_is_on_resolv_list(cmd->peer_id_addr, cmd->peer_addr_type);
     if (position) {
         BLE_LL_ASSERT(position <= g_ble_ll_resolv_data.rl_cnt);
 
         memmove(&g_ble_ll_resolv_list[position - 1],
                 &g_ble_ll_resolv_list[position],
-                g_ble_ll_resolv_data.rl_cnt - position);
-        --g_ble_ll_resolv_data.rl_cnt;
+                (g_ble_ll_resolv_data.rl_cnt - position) *
+                sizeof(g_ble_ll_resolv_list[0]));
+        g_ble_ll_resolv_data.rl_cnt--;
 
         /* Remove from HW list */
-        ble_hw_resolv_list_rmv(position - 1);
+        if (position <= g_ble_ll_resolv_data.rl_cnt_hw) {
+            ble_hw_resolv_list_rmv(position - 1);
+            g_ble_ll_resolv_data.rl_cnt_hw--;
+        }
+
+        /* stop RPA timer if list is empty */
+        if (g_ble_ll_resolv_data.rl_cnt == 0) {
+            ble_npl_callout_stop(&g_ble_ll_resolv_data.rpa_timer);
+        }
+
         return BLE_ERR_SUCCESS;
     }
 
@@ -367,81 +429,77 @@ ble_ll_resolv_list_rmv(uint8_t *cmdbuf)
  * @return int
  */
 int
-ble_ll_resolv_enable_cmd(uint8_t *cmdbuf)
+ble_ll_resolv_enable_cmd(const uint8_t *cmdbuf, uint8_t len)
 {
-    int rc;
-    int32_t tmo;
-    uint8_t enabled;
+    const struct ble_hci_le_set_addr_res_en_cp *cmd = (const void *) cmdbuf;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
 
     if (ble_ll_is_controller_busy()) {
-        rc = BLE_ERR_CMD_DISALLOWED;
-    } else {
-        enabled = cmdbuf[0];
-        if (enabled <= 1) {
-            /* If we change state, we need to disable/enable the RPA timer */
-            if ((enabled ^ g_ble_ll_resolv_data.addr_res_enabled) != 0) {
-                if (enabled) {
-                    tmo = (int32_t)g_ble_ll_resolv_data.rpa_tmo;
-                    ble_npl_callout_reset(&g_ble_ll_resolv_data.rpa_timer, tmo);
-                } else {
-                    ble_npl_callout_stop(&g_ble_ll_resolv_data.rpa_timer);
-                }
-                g_ble_ll_resolv_data.addr_res_enabled = enabled;
-            }
-            rc = BLE_ERR_SUCCESS;
-        } else {
-            rc = BLE_ERR_INV_HCI_CMD_PARMS;
-        }
+        return  BLE_ERR_CMD_DISALLOWED;
+
     }
 
-    return rc;
+    if (cmd->enable > 1) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    g_ble_ll_resolv_data.addr_res_enabled = cmd->enable;
+
+    return BLE_ERR_SUCCESS;
 }
 
 int
-ble_ll_resolv_peer_addr_rd(uint8_t *cmdbuf, uint8_t *rspbuf, uint8_t *rsplen)
+ble_ll_resolv_peer_addr_rd(const uint8_t *cmdbuf, uint8_t len,
+                           uint8_t *rspbuf, uint8_t *rsplen)
 {
+    const struct ble_hci_le_rd_peer_recolv_addr_cp *cmd = (const void *) cmdbuf;
+    struct ble_hci_le_rd_peer_recolv_addr_rp *rsp = (void *) rspbuf;
     struct ble_ll_resolv_entry *rl;
-    uint8_t addr_type;
-    uint8_t *ident_addr;
     int rc;
 
-    addr_type = cmdbuf[0];
-    ident_addr = cmdbuf + 1;
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
 
-    rl = ble_ll_resolv_list_find(ident_addr, addr_type);
+    rl = ble_ll_resolv_list_find(cmd->peer_id_addr, cmd->peer_addr_type);
     if (rl) {
-        memcpy(rspbuf, rl->rl_peer_rpa, BLE_DEV_ADDR_LEN);
+        memcpy(rsp->rpa, rl->rl_peer_rpa, BLE_DEV_ADDR_LEN);
         rc = BLE_ERR_SUCCESS;
     } else {
-        memset(rspbuf, 0, BLE_DEV_ADDR_LEN);
+        memset(rsp->rpa, 0, BLE_DEV_ADDR_LEN);
         rc = BLE_ERR_UNK_CONN_ID;
     }
 
-    *rsplen = BLE_DEV_ADDR_LEN;
+    *rsplen = sizeof(*rsp);
     return rc;
 }
 
 int
-ble_ll_resolv_local_addr_rd(uint8_t *cmdbuf, uint8_t *rspbuf, uint8_t *rsplen)
+ble_ll_resolv_local_addr_rd(const uint8_t *cmdbuf, uint8_t len,
+                            uint8_t *rspbuf, uint8_t *rsplen)
 {
+    const struct ble_hci_le_rd_local_recolv_addr_cp *cmd = (const void *) cmdbuf;
+    struct ble_hci_le_rd_local_recolv_addr_rp *rsp = (void *) rspbuf;
     struct ble_ll_resolv_entry *rl;
-    uint8_t addr_type;
-    uint8_t *ident_addr;
     int rc;
 
-    addr_type = cmdbuf[0];
-    ident_addr = cmdbuf + 1;
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
 
-    rl = ble_ll_resolv_list_find(ident_addr, addr_type);
+    rl = ble_ll_resolv_list_find(cmd->peer_id_addr, cmd->peer_addr_type);
     if (rl) {
-        memcpy(rspbuf, rl->rl_local_rpa, BLE_DEV_ADDR_LEN);
+        memcpy(rsp->rpa, rl->rl_local_rpa, BLE_DEV_ADDR_LEN);
         rc = BLE_ERR_SUCCESS;
     } else {
-        memset(rspbuf, 0, BLE_DEV_ADDR_LEN);
+        memset(rsp->rpa, 0, BLE_DEV_ADDR_LEN);
         rc = BLE_ERR_UNK_CONN_ID;
     }
 
-    *rsplen = BLE_DEV_ADDR_LEN;
+    *rsplen = sizeof(*rsp);
     return rc;
 }
 
@@ -453,50 +511,57 @@ ble_ll_resolv_local_addr_rd(uint8_t *cmdbuf, uint8_t *rspbuf, uint8_t *rsplen)
  * @return int
  */
 int
-ble_ll_resolv_set_rpa_tmo(uint8_t *cmdbuf)
+ble_ll_resolv_set_rpa_tmo(const uint8_t *cmdbuf, uint8_t len)
 {
+    const struct ble_hci_le_set_rpa_tmo_cp *cmd = (const void *)cmdbuf;
     uint16_t tmo_secs;
 
-    tmo_secs = get_le16(cmdbuf);
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    tmo_secs = le16toh(cmd->rpa_timeout);
     if (!((tmo_secs > 0) && (tmo_secs <= 0xA1B8))) {
         return BLE_ERR_INV_HCI_CMD_PARMS;
     }
 
     g_ble_ll_resolv_data.rpa_tmo = ble_npl_time_ms_to_ticks32(tmo_secs * 1000);
 
-    /* If resolving is not enabled, we are done here. */
-    if (!ble_ll_resolv_enabled()) {
-        return BLE_ERR_SUCCESS;
+    /* restart timer if there is something on RL */
+    if (g_ble_ll_resolv_data.rl_cnt) {
+        ble_npl_callout_reset(&g_ble_ll_resolv_data.rpa_timer,
+                              g_ble_ll_resolv_data.rpa_tmo);
     }
 
-    /* Reset timeout if resolving is enabled */
-    ble_npl_callout_reset(&g_ble_ll_resolv_data.rpa_timer,
-                     (int32_t)g_ble_ll_resolv_data.rpa_tmo);
-
     return BLE_ERR_SUCCESS;
 }
 
 int
-ble_ll_resolve_set_priv_mode(uint8_t *cmdbuf)
+ble_ll_resolve_set_priv_mode(const uint8_t *cmdbuf, uint8_t len)
 {
+    const struct ble_hci_le_set_privacy_mode_cp *cmd = (const void *) cmdbuf;
     struct ble_ll_resolv_entry *rl;
 
     if (ble_ll_is_controller_busy()) {
         return BLE_ERR_CMD_DISALLOWED;
     }
 
-    /* cmdbuf = addr_type(0) | addr(6) | priv_mode(1) */
-    rl = ble_ll_resolv_list_find(&cmdbuf[1], cmdbuf[0]);
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    rl = ble_ll_resolv_list_find(cmd->peer_id_addr, cmd->peer_id_addr_type);
     if (!rl) {
         return BLE_ERR_UNK_CONN_ID;
     }
 
-    if (cmdbuf[7] > BLE_HCI_PRIVACY_DEVICE) {
+    if (cmd->mode > BLE_HCI_PRIVACY_DEVICE) {
         return BLE_ERR_INV_HCI_CMD_PARMS;
     }
 
-    rl->rl_priv_mode = cmdbuf[7];
-    return 0;
+    rl->rl_priv_mode = cmd->mode;
+
+    return BLE_ERR_SUCCESS;
 }
 
 /**
@@ -522,8 +587,10 @@ ble_ll_resolv_get_priv_addr(struct ble_ll_resolv_entry *rl, int local,
 
     OS_ENTER_CRITICAL(sr);
     if (local) {
+        BLE_LL_ASSERT(rl->rl_has_local);
         memcpy(addr, rl->rl_local_rpa, BLE_DEV_ADDR_LEN);
     } else {
+        BLE_LL_ASSERT(rl->rl_has_peer);
         memcpy(addr, rl->rl_peer_rpa, BLE_DEV_ADDR_LEN);
     }
 
@@ -542,6 +609,18 @@ ble_ll_resolv_set_peer_rpa(int index, uint8_t *rpa)
     OS_EXIT_CRITICAL(sr);
 }
 
+void
+ble_ll_resolv_set_local_rpa(int index, uint8_t *rpa)
+{
+    os_sr_t sr;
+    struct ble_ll_resolv_entry *rl;
+
+    OS_ENTER_CRITICAL(sr);
+    rl = &g_ble_ll_resolv_list[index];
+    memcpy(rl->rl_local_rpa, rpa, BLE_DEV_ADDR_LEN);
+    OS_EXIT_CRITICAL(sr);
+}
+
 /**
  * Generate a resolvable private address.
  *
@@ -554,25 +633,17 @@ ble_ll_resolv_set_peer_rpa(int index, uint8_t *rpa)
 int
 ble_ll_resolv_gen_rpa(uint8_t *addr, uint8_t addr_type, uint8_t *rpa, int local)
 {
-    int rc;
-    uint8_t *irk;
     struct ble_ll_resolv_entry *rl;
 
-    rc = 0;
     rl = ble_ll_resolv_list_find(addr, addr_type);
     if (rl) {
-        if (local) {
-            irk = rl->rl_local_irk;
-        } else {
-            irk = rl->rl_peer_irk;
-        }
-        if (ble_ll_resolv_irk_nonzero(irk)) {
+        if ((local && rl->rl_has_local) || (!local && rl->rl_has_peer)) {
             ble_ll_resolv_get_priv_addr(rl, local, rpa);
-            rc = 1;
+            return 1;
         }
     }
 
-    return rc;
+    return 0;
 }
 
 /**
@@ -584,15 +655,15 @@ ble_ll_resolv_gen_rpa(uint8_t *addr, uint8_t addr_type, uint8_t *rpa, int local)
  * @return int
  */
 int
-ble_ll_resolv_rpa(uint8_t *rpa, uint8_t *irk)
+ble_ll_resolv_rpa(const uint8_t *rpa, const uint8_t *irk)
 {
     int rc;
-    uint32_t *irk32;
+    const uint32_t *irk32;
     uint32_t *key32;
     uint32_t *pt32;
     struct ble_encryption_block ecb;
 
-    irk32 = (uint32_t *)irk;
+    irk32 = (const uint32_t *)irk;
     key32 = (uint32_t *)&ecb.key[0];
 
     key32[0] = irk32[0];
@@ -621,6 +692,20 @@ ble_ll_resolv_rpa(uint8_t *rpa, uint8_t *irk)
     return rc;
 }
 
+int
+ble_ll_resolv_peer_rpa_any(const uint8_t *rpa)
+{
+    int i;
+
+    for (i = 0; i < g_ble_ll_resolv_data.rl_cnt_hw; i++) {
+        if (ble_ll_resolv_rpa(rpa, g_ble_ll_resolv_list[i].rl_peer_irk)) {
+            return i;
+        }
+    }
+
+    return -1;
+}
+
 /**
  * Returns whether or not address resolution is enabled.
  *
@@ -664,5 +749,5 @@ ble_ll_resolv_init(void)
                          NULL);
 }
 
-#endif  /* if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1 */
+#endif  /* if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) */
 

nimble/controller/src/ble_ll_rfmgmt.c

@@ -0,0 +1,351 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stdint.h>
+#include <stddef.h>
+#include <assert.h>
+#include <stddef.h>
+#include "syscfg/syscfg.h"
+#include "os/os_cputime.h"
+#include "controller/ble_phy.h"
+#include "controller/ble_ll.h"
+#include "controller/ble_ll_sched.h"
+#include "controller/ble_ll_rfmgmt.h"
+#include "ble_ll_priv.h"
+
+#if MYNEWT_VAL(BLE_LL_RFMGMT_ENABLE_TIME) > 0
+
+enum ble_ll_rfmgmt_state {
+    RFMGMT_STATE_OFF = 0,
+    RFMGMT_STATE_ENABLING = 1,
+    RFMGMT_STATE_ENABLED = 2,
+};
+
+struct ble_ll_rfmgmt_data {
+    enum ble_ll_rfmgmt_state state;
+    uint16_t ticks_to_enabled;
+
+    struct hal_timer timer;
+    bool timer_scheduled;
+    uint32_t timer_scheduled_at;
+
+    bool enable_scan;
+    bool enable_sched;
+    uint32_t enable_scan_at;
+    uint32_t enable_sched_at;
+
+    uint32_t enabled_at;
+
+    struct ble_npl_event release_ev;
+};
+
+static struct ble_ll_rfmgmt_data g_ble_ll_rfmgmt_data;
+
+static void
+ble_ll_rfmgmt_enable(void)
+{
+    OS_ASSERT_CRITICAL();
+
+    if (g_ble_ll_rfmgmt_data.state == RFMGMT_STATE_OFF) {
+        g_ble_ll_rfmgmt_data.state = RFMGMT_STATE_ENABLING;
+        g_ble_ll_rfmgmt_data.enabled_at = os_cputime_get32();
+        ble_phy_rfclk_enable();
+        BLE_LL_DEBUG_GPIO(RFMGMT, 1);
+    }
+}
+
+static void
+ble_ll_rfmgmt_disable(void)
+{
+    OS_ASSERT_CRITICAL();
+
+    if (g_ble_ll_rfmgmt_data.state != RFMGMT_STATE_OFF) {
+        BLE_LL_DEBUG_GPIO(RFMGMT, 0);
+        ble_phy_rfclk_disable();
+        g_ble_ll_rfmgmt_data.state = RFMGMT_STATE_OFF;
+    }
+}
+
+static void
+ble_ll_rfmgmt_timer_reschedule(void)
+{
+    struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data;
+    uint32_t enable_at;
+
+    /* Figure out when we need to enable RF */
+    if (rfmgmt->enable_scan && rfmgmt->enable_sched) {
+        if (CPUTIME_LT(rfmgmt->enable_scan_at, rfmgmt->enable_sched_at)) {
+            enable_at = rfmgmt->enable_scan_at;
+        } else {
+            enable_at = rfmgmt->enable_sched_at;
+        }
+    } else if (rfmgmt->enable_scan) {
+        enable_at = rfmgmt->enable_scan_at;
+    } else if (rfmgmt->enable_sched) {
+        enable_at = rfmgmt->enable_sched_at;
+    } else {
+        rfmgmt->timer_scheduled = false;
+        os_cputime_timer_stop(&rfmgmt->timer);
+        return;
+    }
+
+    if (rfmgmt->timer_scheduled) {
+        /*
+         * If there is timer already scheduled at the same time we do not need
+         * to do anything. Otherwise we need to stop timer and schedule it again
+         * regardless if it's earlier or later to make sure it fires at the time
+         * something expects it.
+         */
+
+        if (rfmgmt->timer_scheduled_at == enable_at) {
+            return;
+        }
+
+        rfmgmt->timer_scheduled = false;
+        os_cputime_timer_stop(&rfmgmt->timer);
+    }
+
+    /*
+     * In case timer was requested to be enabled before current time, just make
+     * sure it's enabled and assume caller can deal with this. This will happen
+     * if something is scheduled "now" since "enable_at" is in the past, but in
+     * such case it's absolutely harmless since we already have clock enabled
+     * and this will do nothing.
+     */
+    if (CPUTIME_LEQ(enable_at, os_cputime_get32())) {
+        ble_ll_rfmgmt_enable();
+        return;
+    }
+
+    rfmgmt->timer_scheduled = true;
+    rfmgmt->timer_scheduled_at = enable_at;
+    os_cputime_timer_start(&rfmgmt->timer, enable_at);
+}
+
+static void
+ble_ll_rfmgmt_timer_exp(void *arg)
+{
+    g_ble_ll_rfmgmt_data.timer_scheduled = false;
+    ble_ll_rfmgmt_enable();
+}
+
+static void
+ble_ll_rfmgmt_release_ev(struct ble_npl_event *ev)
+{
+    struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data;
+    uint32_t now;
+    bool can_disable;
+    uint8_t lls;
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+
+    now = os_cputime_get32();
+
+    can_disable = true;
+    lls = ble_ll_state_get();
+
+    if (rfmgmt->enable_scan && CPUTIME_GEQ(now, rfmgmt->enable_scan_at)) {
+        /* Blocked by scan */
+        can_disable = false;
+    } else if (rfmgmt->enable_sched && CPUTIME_GEQ(now, rfmgmt->enable_sched_at)) {
+        /* Blocked by scheduler item */
+        can_disable = false;
+    } else if (lls != BLE_LL_STATE_STANDBY) {
+        /* Blocked by LL state */
+        can_disable = false;
+    }
+
+    if (can_disable) {
+        ble_ll_rfmgmt_disable();
+    }
+
+    OS_EXIT_CRITICAL(sr);
+}
+
+static uint32_t
+ble_ll_rfmgmt_ticks_to_enabled(void)
+{
+    struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data;
+    uint32_t rem_ticks;
+    uint32_t now;
+
+    switch (rfmgmt->state) {
+    case RFMGMT_STATE_OFF:
+        rem_ticks = rfmgmt->ticks_to_enabled;
+        break;
+    case RFMGMT_STATE_ENABLING:
+        now = os_cputime_get32();
+        if (CPUTIME_LT(now, rfmgmt->enabled_at + rfmgmt->ticks_to_enabled)) {
+            rem_ticks = rfmgmt->enabled_at + rfmgmt->ticks_to_enabled - now;
+            break;
+        }
+        rfmgmt->state = RFMGMT_STATE_ENABLED;
+        /* no break */
+    case RFMGMT_STATE_ENABLED:
+        rem_ticks = 0;
+        break;
+    default:
+        BLE_LL_ASSERT(0);
+        rem_ticks = 0;
+        break;
+    }
+
+    return rem_ticks;
+}
+
+void
+ble_ll_rfmgmt_init(void)
+{
+    struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data;
+
+    BLE_LL_DEBUG_GPIO_INIT(RFMGMT);
+
+    rfmgmt->state = RFMGMT_STATE_OFF;
+
+    rfmgmt->ticks_to_enabled =
+            ble_ll_usecs_to_ticks_round_up(MYNEWT_VAL(BLE_LL_RFMGMT_ENABLE_TIME));
+
+    rfmgmt->timer_scheduled = false;
+    os_cputime_timer_init(&rfmgmt->timer, ble_ll_rfmgmt_timer_exp, NULL);
+
+    ble_npl_event_init(&rfmgmt->release_ev, ble_ll_rfmgmt_release_ev, NULL);
+}
+
+void
+ble_ll_rfmgmt_reset(void)
+{
+    struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data;
+
+    rfmgmt->timer_scheduled = false;
+    rfmgmt->timer_scheduled_at = 0;
+    os_cputime_timer_stop(&rfmgmt->timer);
+
+    ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &rfmgmt->release_ev);
+
+    ble_ll_rfmgmt_disable();
+
+    rfmgmt->enable_scan = false;
+    rfmgmt->enable_scan_at = 0;
+    rfmgmt->enable_sched = false;
+    rfmgmt->enable_sched_at = 0;
+
+    rfmgmt->enabled_at = 0;
+}
+
+void
+ble_ll_rfmgmt_scan_changed(bool enabled, uint32_t next_window)
+{
+    struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data;
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+
+    rfmgmt->enable_scan = enabled;
+    rfmgmt->enable_scan_at = next_window - rfmgmt->ticks_to_enabled;
+
+    ble_ll_rfmgmt_timer_reschedule();
+
+    OS_EXIT_CRITICAL(sr);
+}
+
+void
+ble_ll_rfmgmt_sched_changed(struct ble_ll_sched_item *first)
+{
+    struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data;
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+
+    rfmgmt->enable_sched = (first != NULL);
+    if (first) {
+        rfmgmt->enable_sched_at = first->start_time - rfmgmt->ticks_to_enabled;
+    }
+
+    ble_ll_rfmgmt_timer_reschedule();
+
+    OS_EXIT_CRITICAL(sr);
+}
+
+void
+ble_ll_rfmgmt_release(void)
+{
+    struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data;
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+
+    ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &rfmgmt->release_ev);
+
+    if (g_ble_ll_rfmgmt_data.state != RFMGMT_STATE_OFF) {
+        ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &rfmgmt->release_ev);
+    }
+
+    OS_EXIT_CRITICAL(sr);
+}
+
+uint32_t
+ble_ll_rfmgmt_enable_now(void)
+{
+    struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data;
+    uint32_t enabled_at;
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+
+    ble_ll_rfmgmt_enable();
+
+    if (rfmgmt->state == RFMGMT_STATE_ENABLED) {
+        enabled_at = os_cputime_get32();
+    } else {
+        enabled_at = rfmgmt->enabled_at + rfmgmt->ticks_to_enabled + 1;
+    }
+
+    OS_EXIT_CRITICAL(sr);
+
+    return enabled_at;
+}
+
+bool
+ble_ll_rfmgmt_is_enabled(void)
+{
+    bool ret;
+
+    OS_ASSERT_CRITICAL();
+
+    ret = ble_ll_rfmgmt_ticks_to_enabled() == 0;
+
+    return ret;
+}
+
+#else
+
+void
+ble_ll_rfmgmt_init(void)
+{
+    static bool enabled = false;
+
+    if (!enabled) {
+        ble_phy_rfclk_enable();
+    }
+
+    enabled = true;
+}
+
+#endif

nimble/controller/src/ble_ll_scan_aux.c

@@ -0,0 +1,1745 @@
+/*
+ * 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 <syscfg/syscfg.h>
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+#include <assert.h>
+#include "os/os.h"
+#include "nimble/ble.h"
+#include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
+#include "controller/ble_phy.h"
+#include "controller/ble_hw.h"
+#include "controller/ble_ll.h"
+#include "controller/ble_ll_sched.h"
+#include "controller/ble_ll_scan.h"
+#include "controller/ble_ll_scan_aux.h"
+#include "controller/ble_ll_hci.h"
+#include "controller/ble_ll_whitelist.h"
+#include "controller/ble_ll_resolv.h"
+#include "controller/ble_ll_sync.h"
+
+#define BLE_LL_SCAN_AUX_F_AUX_ADV           0x0001
+#define BLE_LL_SCAN_AUX_F_AUX_CHAIN         0x0002
+#define BLE_LL_SCAN_AUX_F_MATCHED           0x0004
+#define BLE_LL_SCAN_AUX_F_W4_SCAN_RSP       0x0008
+#define BLE_LL_SCAN_AUX_F_SCANNED           0x0010
+#define BLE_LL_SCAN_AUX_F_HAS_ADVA          0x0020
+#define BLE_LL_SCAN_AUX_F_HAS_TARGETA       0x0040
+#define BLE_LL_SCAN_AUX_F_HAS_ADI           0x0080
+#define BLE_LL_SCAN_AUX_F_RESOLVED_ADVA     0x0100
+#define BLE_LL_SCAN_AUX_F_RESOLVED_TARGETA  0x0200
+#define BLE_LL_SCAN_AUX_F_CONNECTABLE       0x0400
+#define BLE_LL_SCAN_AUX_F_W4_CONNECT_RSP    0x0800
+
+#define BLE_LL_SCAN_AUX_H_SENT_ANY          0x01
+#define BLE_LL_SCAN_AUX_H_DONE              0x02
+#define BLE_LL_SCAN_AUX_H_TRUNCATED         0x04
+
+struct ble_ll_scan_aux_data {
+    uint16_t flags;
+    uint8_t hci_state;
+
+    uint8_t scan_type;
+
+    uint8_t pri_phy;
+    uint8_t sec_phy;
+    uint8_t chan;
+    uint8_t offset_unit : 1;
+    uint32_t aux_ptr;
+    struct ble_ll_sched_item sch;
+    struct ble_npl_event break_ev;
+    struct ble_hci_ev *hci_ev;
+
+    uint16_t adi;
+
+    uint8_t adva[6];
+    uint8_t targeta[6];
+    uint8_t adva_type : 1;
+    uint8_t targeta_type : 1;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    int8_t rpa_index;
+#endif
+};
+
+#define AUX_MEMPOOL_SIZE    (OS_MEMPOOL_SIZE( \
+                                MYNEWT_VAL(BLE_LL_SCAN_AUX_SEGMENT_CNT), \
+                                sizeof(struct ble_ll_scan_aux_data)))
+
+static os_membuf_t aux_data_mem[AUX_MEMPOOL_SIZE];
+static struct os_mempool aux_data_pool;
+
+static struct ble_ll_scan_aux_data *aux_data_current;
+
+static void ble_ll_hci_ev_send_ext_adv_truncated_report(struct ble_ll_scan_aux_data *aux);
+
+static inline uint8_t *
+ble_ll_scan_aux_get_own_addr(void)
+{
+    uint8_t own_addr_type;
+
+    own_addr_type = ble_ll_scan_get_own_addr_type() & 1;
+
+    return ble_ll_get_our_devaddr(own_addr_type);
+}
+
+static int
+ble_ll_scan_aux_sched_cb(struct ble_ll_sched_item *sch)
+{
+    struct ble_ll_scan_aux_data *aux = sch->cb_arg;
+    uint32_t wfr_us;
+#if BLE_LL_BT5_PHY_SUPPORTED
+    uint8_t phy_mode;
+#endif
+    uint8_t lls;
+    int rc;
+
+    BLE_LL_ASSERT(aux);
+
+    lls = ble_ll_state_get();
+    BLE_LL_ASSERT(lls == BLE_LL_STATE_STANDBY);
+
+    rc = ble_phy_setchan(aux->chan, BLE_ACCESS_ADDR_ADV, BLE_LL_CRCINIT_ADV);
+    BLE_LL_ASSERT(rc == 0);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    ble_phy_encrypt_disable();
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    if (ble_ll_resolv_enabled()) {
+        ble_phy_resolv_list_enable();
+    } else {
+        ble_phy_resolv_list_disable();
+    }
+#endif
+
+#if BLE_LL_BT5_PHY_SUPPORTED
+    phy_mode = ble_ll_phy_to_phy_mode(aux->sec_phy, BLE_HCI_LE_PHY_CODED_ANY);
+    ble_phy_mode_set(phy_mode, phy_mode);
+#endif
+
+    rc = ble_phy_rx_set_start_time(sch->start_time + g_ble_ll_sched_offset_ticks,
+                                   sch->remainder);
+    if (rc != 0 && rc != BLE_PHY_ERR_RX_LATE) {
+        ble_ll_scan_aux_break(aux);
+        return BLE_LL_SCHED_STATE_DONE;
+    }
+
+    /* Keep listening even if we are late, we may still receive something */
+
+    if (ble_ll_scan_get_filt_policy() & 1) {
+        ble_ll_whitelist_enable();
+    } else {
+        ble_ll_whitelist_disable();
+    }
+
+    wfr_us = aux->offset_unit ? 300 : 30;
+    ble_phy_wfr_enable(BLE_PHY_WFR_ENABLE_RX, 0, wfr_us);
+
+    aux_data_current = aux;
+
+    ble_ll_state_set(BLE_LL_STATE_SCAN_AUX);
+
+    return BLE_LL_SCHED_STATE_RUNNING;
+}
+
+static struct ble_ll_scan_aux_data *
+ble_ll_scan_aux_alloc(void)
+{
+    struct ble_ll_scan_aux_data *aux;
+
+    aux = os_memblock_get(&aux_data_pool);
+    if (!aux) {
+        return NULL;
+    }
+
+    memset(aux, 0, sizeof(*aux));
+
+    aux->sch.sched_cb = ble_ll_scan_aux_sched_cb;
+    aux->sch.sched_type = BLE_LL_SCHED_TYPE_SCAN_AUX;
+    aux->sch.cb_arg = aux;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    aux->rpa_index = -1;
+#endif
+
+    return aux;
+}
+
+static void
+ble_ll_scan_aux_free(struct ble_ll_scan_aux_data *aux)
+{
+    BLE_LL_ASSERT(!aux->sch.enqueued);
+    BLE_LL_ASSERT(aux->hci_ev == NULL);
+    BLE_LL_ASSERT((aux->hci_state & BLE_LL_SCAN_AUX_H_DONE) ||
+                  !(aux->hci_state & BLE_LL_SCAN_AUX_H_SENT_ANY));
+
+    os_memblock_put(&aux_data_pool, aux);
+}
+
+static void
+ble_ll_scan_aux_update_scan_backoff(struct ble_ll_scan_aux_data *aux)
+{
+    if (!(aux->flags & BLE_LL_SCAN_AUX_F_W4_SCAN_RSP) &&
+        !(aux->flags & BLE_LL_SCAN_AUX_F_SCANNED)) {
+        return;
+    }
+
+    if ((aux->hci_state & BLE_LL_SCAN_AUX_H_DONE) &&
+        !(aux->hci_state & BLE_LL_SCAN_AUX_H_TRUNCATED)) {
+        ble_ll_scan_backoff_update(1);
+    } else {
+        ble_ll_scan_backoff_update(0);
+    }
+}
+
+static inline bool
+ble_ll_scan_aux_need_truncation(struct ble_ll_scan_aux_data *aux)
+{
+    return (aux->hci_state & BLE_LL_SCAN_AUX_H_SENT_ANY) &&
+           !(aux->hci_state & BLE_LL_SCAN_AUX_H_DONE);
+}
+
+static struct ble_hci_ev *
+ble_ll_hci_ev_alloc_ext_adv_report_for_aux(struct ble_ll_scan_addr_data *addrd,
+                                           struct ble_ll_scan_aux_data *aux)
+{
+    struct ble_hci_ev_le_subev_ext_adv_rpt *hci_subev;
+    struct ext_adv_report *report;
+    struct ble_hci_ev *hci_ev;
+
+    hci_ev = (void *)ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO);
+    if (!hci_ev) {
+        return NULL;
+    }
+
+    hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+    hci_ev->length = sizeof(*hci_subev) + sizeof(*report);
+
+    hci_subev = (void *)hci_ev->data;
+    hci_subev->subev_code = BLE_HCI_LE_SUBEV_EXT_ADV_RPT;
+    hci_subev->num_reports = 1;
+
+    report = hci_subev->reports;
+
+    memset(report, 0, sizeof(*report));
+
+    report->evt_type = 0;
+    if (addrd->adva) {
+        report->addr_type = addrd->adv_addr_type;
+        memcpy(report->addr, addrd->adv_addr, 6);
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+        if (addrd->adva_resolved) {
+            report->addr_type += 2;
+        }
+#endif
+    } else {
+        report->addr_type = 0xff;
+    }
+    report->pri_phy = aux->pri_phy;
+    report->sec_phy = aux->sec_phy;
+    report->sid = aux->adi >> 12;
+    report->tx_power = 0x7f;
+    report->rssi = 0x7f;
+    report->periodic_itvl = 0;
+    if (addrd->targeta) {
+        report->evt_type |= BLE_HCI_ADV_DIRECT_MASK;
+        report->dir_addr_type = addrd->targeta_type;
+        memcpy(report->dir_addr, addrd->targeta, 6);
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+        if (addrd->targeta_resolved) {
+            report->dir_addr_type += 2;
+        }
+#endif
+    }
+    report->data_len = 0;
+
+    return hci_ev;
+}
+
+static struct ble_hci_ev *
+ble_ll_hci_ev_dup_ext_adv_report(struct ble_hci_ev *hci_ev_src)
+{
+    struct ble_hci_ev_le_subev_ext_adv_rpt *hci_subev;
+    struct ext_adv_report *report;
+    struct ble_hci_ev *hci_ev;
+
+    hci_ev = (void *)ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO);
+    if (!hci_ev) {
+        return NULL;
+    }
+
+    memcpy(hci_ev, hci_ev_src, sizeof(*hci_ev) + sizeof(*hci_subev) +
+                               sizeof(*report));
+    hci_ev->length = sizeof(*hci_subev) + sizeof(*report);
+
+    hci_subev = (void *)hci_ev->data;
+
+    report = hci_subev->reports;
+    report->data_len = 0;
+
+    return hci_ev;
+}
+
+static void
+ble_ll_hci_ev_update_ext_adv_report_from_aux(struct ble_hci_ev *hci_ev,
+                                             struct os_mbuf *rxpdu,
+                                             struct ble_mbuf_hdr *rxhdr)
+{
+    struct ble_hci_ev_le_subev_ext_adv_rpt *hci_subev;
+    struct ext_adv_report *report;
+    uint8_t adv_mode;
+    uint8_t eh_len;
+    uint8_t eh_flags;
+    uint8_t *eh_data;
+    uint8_t *rxbuf;
+
+    hci_subev = (void *)hci_ev->data;
+    report = hci_subev->reports;
+    rxbuf = rxpdu->om_data;
+
+    adv_mode = rxbuf[2] >> 6;
+    eh_len = rxbuf[2] & 0x3f;
+    eh_flags = rxbuf[3];
+    eh_data = &rxbuf[4];
+
+    report->evt_type |= adv_mode;
+    if (rxhdr->rxinfo.flags & BLE_MBUF_HDR_F_SCAN_RSP_RXD) {
+        report->evt_type |= BLE_HCI_ADV_SCAN_MASK | BLE_HCI_ADV_SCAN_RSP_MASK;
+    }
+    report->sec_phy = rxhdr->rxinfo.phy;
+
+    /* Strip PDU header and ext header, leave only AD */
+    os_mbuf_adj(rxpdu, 3 + eh_len);
+
+    /*
+     * We only care about SyncInfo and TxPower so don't bother parsing if they
+     * are not present, just set to 'unknown' values.
+     */
+    if ((eh_len == 0) || !(eh_flags & ((1 << BLE_LL_EXT_ADV_SYNC_INFO_BIT) |
+                                       (1 << BLE_LL_EXT_ADV_TX_POWER_BIT)))) {
+        report->periodic_itvl = 0;
+        report->tx_power = 0x7f;
+        return;
+    }
+    /* Now parse extended header... */
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_ADVA_BIT)) {
+        eh_data += BLE_LL_EXT_ADV_ADVA_SIZE;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_TARGETA_BIT)) {
+        eh_data += BLE_LL_EXT_ADV_TARGETA_SIZE;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_CTE_INFO_BIT)) {
+        eh_data += BLE_LL_EXT_ADV_CTE_INFO_SIZE;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)) {
+        eh_data += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) {
+        eh_data += BLE_LL_EXT_ADV_AUX_PTR_SIZE;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_SYNC_INFO_BIT)) {
+        report->periodic_itvl = get_le16(eh_data + 2);
+        eh_data += BLE_LL_EXT_ADV_SYNC_INFO_SIZE;
+    } else {
+        report->periodic_itvl = 0;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_TX_POWER_BIT)) {
+        report->tx_power = *eh_data;
+    } else {
+        report->tx_power = 0x7f;
+    }
+}
+
+static void
+ble_ll_hci_ev_update_ext_adv_report_from_ext(struct ble_hci_ev *hci_ev,
+                                             struct os_mbuf *rxpdu,
+                                             struct ble_mbuf_hdr *rxhdr)
+{
+    struct ble_mbuf_hdr_rxinfo *rxinfo = &rxhdr->rxinfo;
+    struct ble_hci_ev_le_subev_ext_adv_rpt *hci_subev;
+    struct ext_adv_report *report;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    struct ble_ll_resolv_entry *rl;
+#endif
+    uint8_t pdu_hdr;
+    uint8_t adv_mode;
+    uint8_t eh_len;
+    uint8_t eh_flags;
+    uint8_t *eh_data;
+    uint8_t *rxbuf;
+
+    rxbuf = rxpdu->om_data;
+
+    pdu_hdr = rxbuf[0];
+    adv_mode = rxbuf[2] >> 6;
+    eh_len = rxbuf[2] & 0x3f;
+    eh_flags = rxbuf[3];
+    eh_data = &rxbuf[4];
+
+    hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+    hci_ev->length = sizeof(*hci_subev) + sizeof(*report);
+
+    hci_subev = (void *)hci_ev->data;
+    hci_subev->subev_code = BLE_HCI_LE_SUBEV_EXT_ADV_RPT;
+    hci_subev->num_reports = 1;
+
+    report = hci_subev->reports;
+
+    memset(report, 0, sizeof(*report));
+
+    report->evt_type = adv_mode;
+
+    report->pri_phy = rxinfo->phy;
+    report->sec_phy = 0;
+    report->sid = 0xff;
+    report->rssi = rxhdr->rxinfo.rssi;
+    report->periodic_itvl = 0;
+    report->data_len = 0;
+
+    /* Now parse extended header... */
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_ADVA_BIT)) {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+        if (rxinfo->rpa_index >= 0) {
+            rl = &g_ble_ll_resolv_list[rxinfo->rpa_index];
+            report->addr_type = rl->rl_addr_type + 2;
+            memcpy(report->addr, rl->rl_identity_addr, 6);
+        } else {
+            report->addr_type = !!(pdu_hdr & BLE_ADV_PDU_HDR_TXADD_MASK);
+            memcpy(report->addr, eh_data, 6);
+        }
+#else
+        report->addr_type = !!(pdu_hdr & BLE_ADV_PDU_HDR_TXADD_MASK);
+        memcpy(report->addr, eh_data, 6);
+#endif
+        eh_data += BLE_LL_EXT_ADV_ADVA_SIZE;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_TARGETA_BIT)) {
+        report->evt_type |= BLE_HCI_ADV_DIRECT_MASK;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+        if (rxinfo->flags & BLE_MBUF_HDR_F_TARGETA_RESOLVED) {
+            report->dir_addr_type = (ble_ll_scan_get_own_addr_type() & 1) + 2;
+            memcpy(report->dir_addr, ble_ll_scan_aux_get_own_addr(), 6);
+        } else {
+            report->dir_addr_type = !!(pdu_hdr & BLE_ADV_PDU_HDR_RXADD_MASK);
+            memcpy(report->dir_addr, eh_data, 6);
+        }
+#else
+        report->dir_addr_type = !!(pdu_hdr & BLE_ADV_PDU_HDR_RXADD_MASK);
+        memcpy(report->dir_addr, eh_data, 6);
+#endif
+        eh_data += BLE_LL_EXT_ADV_TARGETA_SIZE;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_CTE_INFO_BIT)) {
+        eh_data += BLE_LL_EXT_ADV_CTE_INFO_SIZE;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)) {
+        eh_data += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) {
+        eh_data += BLE_LL_EXT_ADV_AUX_PTR_SIZE;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_SYNC_INFO_BIT)) {
+        eh_data += BLE_LL_EXT_ADV_SYNC_INFO_SIZE;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_TX_POWER_BIT)) {
+        report->tx_power = *eh_data;
+    } else {
+        report->tx_power = 0x7f;
+    }
+
+    /* Strip PDU header and ext header, leave only AD */
+    os_mbuf_adj(rxpdu, 3 + eh_len);
+
+}
+
+static void
+ble_ll_hci_ev_send_ext_adv_truncated_report(struct ble_ll_scan_aux_data *aux)
+{
+    struct ble_hci_ev_le_subev_ext_adv_rpt *hci_subev;
+    struct ext_adv_report *report;
+    struct ble_hci_ev *hci_ev;
+
+    if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_EXT_ADV_RPT)) {
+        return;
+    }
+
+    hci_ev = aux->hci_ev;
+    aux->hci_ev = NULL;
+
+    BLE_LL_ASSERT(hci_ev);
+
+    hci_subev = (void *)hci_ev->data;
+    report = hci_subev->reports;
+    report->evt_type |= BLE_HCI_ADV_DATA_STATUS_TRUNCATED;
+
+    ble_ll_hci_event_send(hci_ev);
+
+    aux->hci_state |= BLE_LL_SCAN_AUX_H_DONE | BLE_LL_SCAN_AUX_H_TRUNCATED;
+}
+
+static int
+ble_ll_hci_ev_send_ext_adv_report(struct os_mbuf *rxpdu,
+                                  struct ble_mbuf_hdr *rxhdr,
+                                  struct ble_hci_ev **hci_ev)
+{
+    struct ble_mbuf_hdr_rxinfo *rxinfo = &rxhdr->rxinfo;
+    struct ble_hci_ev_le_subev_ext_adv_rpt *hci_subev;
+    struct ble_hci_ev *hci_ev_next;
+    struct ext_adv_report *report;
+    bool truncated = false;
+    int max_data_len;
+    int data_len;
+    int offset;
+
+    data_len = OS_MBUF_PKTLEN(rxpdu);
+    max_data_len = BLE_LL_MAX_EVT_LEN -
+                   sizeof(**hci_ev) - sizeof(*hci_subev) - sizeof(*report);
+    offset = 0;
+    hci_ev_next = NULL;
+
+    do {
+        hci_subev = (void *)(*hci_ev)->data;
+        report = hci_subev->reports;
+
+        report->rssi = rxinfo->rssi;
+
+        report->data_len = min(max_data_len, data_len - offset);
+        os_mbuf_copydata(rxpdu, offset, report->data_len, report->data);
+        (*hci_ev)->length += report->data_len;
+
+        offset += report->data_len;
+
+        /*
+         * We need another event if either there are still some data left in
+         * this PDU or scan for next aux is scheduled.
+         */
+        if ((offset < data_len) ||
+            (rxinfo->flags & BLE_MBUF_HDR_F_AUX_PTR_WAIT)) {
+            hci_ev_next = ble_ll_hci_ev_dup_ext_adv_report(*hci_ev);
+            if (hci_ev_next) {
+                report->evt_type |= BLE_HCI_ADV_DATA_STATUS_INCOMPLETE;
+            } else {
+                report->evt_type |= BLE_HCI_ADV_DATA_STATUS_TRUNCATED;
+            }
+        }
+
+        switch (report->evt_type & BLE_HCI_ADV_DATA_STATUS_MASK) {
+        case BLE_HCI_ADV_DATA_STATUS_TRUNCATED:
+            truncated = true;
+            /* no break */
+        case BLE_HCI_ADV_DATA_STATUS_COMPLETE:
+            BLE_LL_ASSERT(!hci_ev_next);
+            break;
+        case BLE_HCI_ADV_DATA_STATUS_INCOMPLETE:
+            BLE_LL_ASSERT(hci_ev_next);
+            break;
+        default:
+            BLE_LL_ASSERT(0);
+        }
+
+        ble_ll_hci_event_send(*hci_ev);
+
+        *hci_ev = hci_ev_next;
+    } while ((offset < data_len) && *hci_ev);
+
+    return truncated ? -1 : 0;
+}
+
+
+static int
+ble_ll_hci_ev_send_ext_adv_report_for_aux(struct os_mbuf *rxpdu,
+                                          struct ble_mbuf_hdr *rxhdr,
+                                          struct ble_ll_scan_aux_data *aux,
+                                          struct ble_ll_scan_addr_data *addrd)
+{
+    struct ble_hci_ev *hci_ev;
+    int rc;
+
+    if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_EXT_ADV_RPT)) {
+        return -1;
+    }
+
+    /*
+     * We need to always keep one event allocated in aux to be able to truncate
+     * data properly in case of an error. If there is no event in aux it means
+     * this is first event and we can silently ignore in case of an error.
+     */
+    if (aux->hci_ev) {
+        hci_ev = aux->hci_ev;
+        aux->hci_ev = NULL;
+    } else {
+        hci_ev = ble_ll_hci_ev_alloc_ext_adv_report_for_aux(addrd, aux);
+        if (!hci_ev) {
+            return -1;
+        }
+    }
+
+    ble_ll_hci_ev_update_ext_adv_report_from_aux(hci_ev, rxpdu, rxhdr);
+
+    rc = ble_ll_hci_ev_send_ext_adv_report(rxpdu, rxhdr, &hci_ev);
+    if (rc < 0) {
+        BLE_LL_ASSERT(!hci_ev);
+        aux->hci_state = BLE_LL_SCAN_AUX_H_DONE | BLE_LL_SCAN_AUX_H_TRUNCATED;
+    } else if (hci_ev) {
+        aux->hci_state = BLE_LL_SCAN_AUX_H_SENT_ANY;
+    } else {
+        aux->hci_state = BLE_LL_SCAN_AUX_H_DONE;
+    }
+
+    aux->hci_ev = hci_ev;
+
+    return rc;
+}
+
+static void
+ble_ll_hci_ev_send_ext_adv_report_for_ext(struct os_mbuf *rxpdu,
+                                          struct ble_mbuf_hdr *rxhdr)
+{
+    struct ble_hci_ev *hci_ev;
+
+    if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_EXT_ADV_RPT)) {
+        return;
+    }
+
+    hci_ev = (void *)ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO);
+    if (!hci_ev) {
+        return;
+    }
+
+    ble_ll_hci_ev_update_ext_adv_report_from_ext(hci_ev, rxpdu, rxhdr);
+
+    ble_ll_hci_ev_send_ext_adv_report(rxpdu, rxhdr, &hci_ev);
+
+    BLE_LL_ASSERT(!hci_ev);
+}
+
+static void
+ble_ll_scan_aux_break_ev(struct ble_npl_event *ev)
+{
+    struct ble_ll_scan_aux_data *aux = ble_npl_event_get_arg(ev);
+
+    BLE_LL_ASSERT(aux);
+
+    if (ble_ll_scan_aux_need_truncation(aux)) {
+        ble_ll_hci_ev_send_ext_adv_truncated_report(aux);
+    }
+
+    ble_ll_scan_aux_update_scan_backoff(aux);
+
+    ble_ll_scan_aux_free(aux);
+    ble_ll_scan_chk_resume();
+}
+
+void
+ble_ll_scan_aux_break(struct ble_ll_scan_aux_data *aux)
+{
+    if (aux->flags & BLE_LL_SCAN_AUX_F_W4_CONNECT_RSP) {
+        ble_ll_conn_send_connect_req_cancel();
+    }
+
+    ble_npl_event_init(&aux->break_ev, ble_ll_scan_aux_break_ev, aux);
+    ble_ll_event_send(&aux->break_ev);
+}
+
+static int
+ble_ll_scan_aux_parse_aux_ptr(struct ble_ll_scan_aux_data *aux,
+                              uint32_t aux_ptr, uint32_t *offset_us)
+{
+    uint8_t offset_unit;
+    uint32_t offset;
+    uint8_t chan;
+    uint8_t phy;
+
+    phy = (aux_ptr >> 21) & 0x07;
+    switch (phy) {
+    case 0:
+        phy = BLE_PHY_1M;
+        break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY)
+    case 1:
+        phy = BLE_PHY_2M;
+        break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    case 2:
+        phy = BLE_PHY_CODED;
+        break;
+#endif
+    default:
+        return -1;
+    }
+
+    chan = aux_ptr & 0x3f;
+    if (chan >= BLE_PHY_NUM_DATA_CHANS) {
+        return -1;
+    }
+
+    offset = 30 * ((aux_ptr >> 8) & 0x1fff);
+    if ((aux_ptr >> 7) & 0x01) {
+        offset *= 10;
+        offset_unit = 1;
+    } else {
+        offset_unit = 0;
+    }
+
+    if (offset < BLE_LL_MAFS) {
+        return -1;
+    }
+
+    aux->sec_phy = phy;
+    aux->chan = chan;
+    aux->offset_unit = offset_unit;
+
+    *offset_us = offset;
+
+    return 0;
+}
+
+int
+ble_ll_scan_aux_sched(struct ble_ll_scan_aux_data *aux, uint32_t pdu_time,
+                      uint8_t pdu_time_rem, uint32_t aux_ptr)
+{
+    uint32_t offset_us;
+    int rc;
+
+    rc = ble_ll_scan_aux_parse_aux_ptr(aux, aux_ptr, &offset_us);
+    if (rc < 0) {
+        return -1;
+    }
+
+    rc = ble_ll_sched_scan_aux(&aux->sch, pdu_time, pdu_time_rem, offset_us);
+    if (rc < 0) {
+        return -1;
+    }
+
+    return 0;
+}
+
+int
+ble_ll_scan_aux_rx_isr_start(uint8_t pdu_type, struct ble_mbuf_hdr *rxhdr)
+{
+    struct ble_ll_scan_aux_data *aux;
+
+    BLE_LL_ASSERT(aux_data_current);
+    aux = aux_data_current;
+
+    if (aux->flags & BLE_LL_SCAN_AUX_F_W4_CONNECT_RSP) {
+        if (pdu_type != BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP) {
+            aux_data_current = NULL;
+            ble_ll_scan_aux_break(aux);
+            return -1;
+        }
+        return 0;
+    }
+
+    if (pdu_type != BLE_ADV_PDU_TYPE_ADV_EXT_IND) {
+        aux_data_current = NULL;
+        ble_ll_scan_aux_break(aux);
+        return -1;
+    }
+
+    /*
+     * Prepare TX transition when doing active scanning and receiving 1st PDU
+     * since we may want to send AUX_SCAN_REQ.
+     */
+    if ((aux->scan_type != BLE_SCAN_TYPE_PASSIVE) &&
+        !(aux->flags & BLE_LL_SCAN_AUX_F_AUX_ADV)) {
+        return 1;
+    }
+
+    return 0;
+}
+
+static int
+ble_ll_scan_aux_parse_to_aux_data(struct ble_ll_scan_aux_data *aux,
+                                  uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr)
+{
+    uint8_t pdu_hdr;
+    uint8_t pdu_len;
+    uint8_t adv_mode;
+    uint8_t eh_len;
+    uint8_t eh_flags;
+    uint8_t *eh_data;
+
+    pdu_hdr = rxbuf[0];
+    pdu_len = rxbuf[1];
+
+    /* PDU without at least Extended Header Length is invalid */
+    if (pdu_len == 0) {
+        return -1;
+    }
+
+    /* Mark as AUX received on 1st PDU, then as CHAIN received on subsequent */
+    if (aux->flags & BLE_LL_SCAN_AUX_F_AUX_ADV) {
+        aux->flags |= BLE_LL_SCAN_AUX_F_AUX_CHAIN;
+    } else {
+        aux->flags |= BLE_LL_SCAN_AUX_F_AUX_ADV;
+    }
+
+    adv_mode = rxbuf[2] >> 6;
+    eh_len = rxbuf[2] & 0x3f;
+
+    /* Only AUX_CHAIN_IND is valid without Extended Header */
+    if (eh_len == 0) {
+        if (!(aux->flags & BLE_LL_SCAN_AUX_F_AUX_CHAIN) || adv_mode) {
+            return -1;
+        }
+        return 0;
+    }
+
+    eh_flags = rxbuf[3];
+    eh_data = &rxbuf[4];
+
+    /* Now parse extended header... */
+
+    /* AdvA is only valid in 1st PDU, ignore in AUX_CHAIN_IND */
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_ADVA_BIT)) {
+        if (!(aux->flags & BLE_LL_SCAN_AUX_F_AUX_CHAIN)) {
+            memcpy(aux->adva, eh_data, 6);
+            aux->adva_type = !!(pdu_hdr & BLE_ADV_PDU_HDR_TXADD_MASK);
+            aux->flags |= BLE_LL_SCAN_AUX_F_HAS_ADVA;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+            aux->rpa_index = ble_hw_resolv_list_match();
+#endif
+        }
+        eh_data += BLE_LL_EXT_ADV_ADVA_SIZE;
+    }
+
+    /* TargetA is only valid in 1st PDU, ignore in AUX_CHAIN_IND */
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_TARGETA_BIT)) {
+        if (!(aux->flags & BLE_LL_SCAN_AUX_F_AUX_CHAIN)) {
+            memcpy(aux->targeta, eh_data, 6);
+            aux->targeta_type = !!(pdu_hdr & BLE_ADV_PDU_HDR_RXADD_MASK);
+            aux->flags |= BLE_LL_SCAN_AUX_F_HAS_TARGETA;
+        }
+        eh_data += BLE_LL_EXT_ADV_TARGETA_SIZE;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_CTE_INFO_BIT)) {
+        eh_data += BLE_LL_EXT_ADV_CTE_INFO_SIZE;
+    }
+
+    /*
+     * ADI handling is a bit convoluted....
+     * ADI is mandatory in ADV_EXT_IND with AuxPtr and is also mandatory in PDU
+     * if included in superior PDU. This implies that each AUX_CHAIN shall have
+     * ADI. However... AUX_SCAN_RSP does not need to have ADI, so if there's no
+     * ADI in AUX_SCAN_RSP we allow it and clear corresponding flag to skip ADI
+     * checks on subsequent PDUs.
+     */
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)) {
+        if (aux->flags & BLE_LL_SCAN_AUX_F_HAS_ADI) {
+            if (get_le16(eh_data) != aux->adi) {
+                return -1;
+            }
+        }
+        eh_data += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+    } else if (aux->flags & BLE_LL_SCAN_AUX_F_HAS_ADI) {
+        if (rxhdr->rxinfo.flags & BLE_MBUF_HDR_F_SCAN_RSP_RXD) {
+            aux->flags &= ~BLE_LL_SCAN_AUX_F_HAS_ADI;
+        } else {
+            return -1;
+        }
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) {
+        aux->aux_ptr = get_le24(eh_data);
+        rxhdr->rxinfo.flags |= BLE_MBUF_HDR_F_AUX_PTR_WAIT;
+    }
+
+    return 0;
+}
+
+static int
+ble_ll_scan_aux_ext_parse(uint8_t *rxbuf, struct ble_ll_scan_addr_data *addrd,
+                          uint16_t *adi, uint32_t *aux_ptr)
+{
+    uint8_t pdu_hdr;
+    uint8_t pdu_len;
+    uint8_t adv_mode;
+    uint8_t eh_len;
+    uint8_t eh_flags;
+    uint8_t *eh_data;
+
+    pdu_hdr = rxbuf[0];
+    pdu_len = rxbuf[1];
+
+    if (pdu_len == 0) {
+        return -1;
+    }
+
+    adv_mode = rxbuf[2] >> 6;
+    eh_len = rxbuf[2] & 0x3f;
+
+    if ((adv_mode == 3) || (eh_len == 0)) {
+        return -1;
+    }
+
+    eh_flags = rxbuf[3];
+    eh_data = &rxbuf[4];
+
+    /* ADV_EXT_IND with AuxPtr but without ADI is invalid */
+    if ((eh_flags & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) &&
+        !(eh_flags & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT))) {
+        return -1;
+    }
+
+    /* ADV_EXT_IND without either AdvA or AuxPtr is not valid */
+    if (!(eh_flags & ((1 << BLE_LL_EXT_ADV_ADVA_BIT) |
+                      (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)))) {
+        return -1;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_ADVA_BIT)) {
+        addrd->adva = eh_data;
+        addrd->adva_type = !!(pdu_hdr & BLE_ADV_PDU_HDR_TXADD_RAND);
+        eh_data += BLE_LL_EXT_ADV_ADVA_SIZE;
+    } else {
+        addrd->adva = NULL;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_TARGETA_BIT)) {
+        addrd->targeta = eh_data;
+        addrd->targeta_type = !!(pdu_hdr & BLE_ADV_PDU_HDR_RXADD_RAND);
+        eh_data += BLE_LL_EXT_ADV_TARGETA_SIZE;
+    } else {
+        addrd->targeta = NULL;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_CTE_INFO_BIT)) {
+        eh_data += BLE_LL_EXT_ADV_CTE_INFO_SIZE;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)) {
+        *adi = get_le16(eh_data);
+        eh_data += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+    } else {
+        *adi = 0;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) {
+        *aux_ptr = get_le24(eh_data);
+    } else {
+        *aux_ptr = 0;
+    }
+
+    return 0;
+}
+
+static void
+ble_ll_scan_aux_update_rxinfo_from_addrd(struct ble_ll_scan_addr_data *addrd,
+                                         struct ble_mbuf_hdr_rxinfo *rxinfo)
+{
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    rxinfo->rpa_index = addrd->rpa_index;
+    if (addrd->adva_resolved) {
+        rxinfo->flags |= BLE_MBUF_HDR_F_RESOLVED;
+    }
+    if (addrd->targeta_resolved) {
+        rxinfo->flags |= BLE_MBUF_HDR_F_TARGETA_RESOLVED;
+    }
+#endif
+}
+
+static void
+ble_ll_scan_aux_update_aux_data_from_addrd(struct ble_ll_scan_addr_data *addrd,
+                                           struct ble_ll_scan_aux_data *aux)
+{
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    aux->rpa_index = addrd->rpa_index;
+    if (addrd->adva_resolved) {
+        aux->flags |= BLE_LL_SCAN_AUX_F_RESOLVED_ADVA;
+    }
+    if (addrd->targeta_resolved) {
+        aux->flags |= BLE_LL_SCAN_AUX_F_RESOLVED_TARGETA;
+    }
+#endif
+}
+
+int
+ble_ll_scan_aux_rx_isr_end_on_ext(struct ble_ll_scan_sm *scansm,
+                                  struct os_mbuf *rxpdu)
+{
+    struct ble_mbuf_hdr *rxhdr = BLE_MBUF_HDR_PTR(rxpdu);
+    struct ble_mbuf_hdr_rxinfo *rxinfo = &rxhdr->rxinfo;
+    struct ble_ll_scan_addr_data addrd;
+    struct ble_ll_scan_aux_data *aux;
+    uint8_t *rxbuf;
+    uint32_t aux_ptr;
+    uint16_t adi;
+    bool do_match;
+    int rc;
+
+    rxbuf = rxpdu->om_data;
+
+    rc = ble_ll_scan_aux_ext_parse(rxbuf, &addrd, &adi, &aux_ptr);
+    if (rc < 0) {
+        return -1;
+    }
+
+    /* We can filter based on ext PDU alone if both AdvA and TargetA are present
+     * or there's no AuxPtr. Otherwise, we need to wait for aux PDU to filter.
+     */
+    do_match = !aux_ptr || (addrd.adva && addrd.targeta);
+    if (do_match) {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+        addrd.rpa_index = ble_hw_resolv_list_match();
+#endif
+        rc = ble_ll_scan_rx_filter(ble_ll_scan_get_own_addr_type(),
+                                   ble_ll_scan_get_filt_policy(),
+                                   &addrd, NULL);
+        if (rc < 0) {
+            return -1;
+        }
+
+        /* Don't care about initiator here, there are no AdvA and TargetA
+         * in connectable ADV_EXT_IND so no filtering to do.
+         */
+
+        if (!aux_ptr) {
+            /* We do not allocate aux_data for ADV_EXT_IND without AuxPtr so
+             * need to pass match data in rxinfo.
+             */
+            ble_ll_scan_aux_update_rxinfo_from_addrd(&addrd, rxinfo);
+        }
+    }
+
+    if (aux_ptr) {
+        aux = ble_ll_scan_aux_alloc();
+        if (!aux) {
+            return -1;
+        }
+
+        aux->scan_type = scansm->scanp->scan_type;
+
+        aux->pri_phy = rxinfo->phy;
+        aux->aux_ptr = aux_ptr;
+
+        if (addrd.adva) {
+            memcpy(aux->adva, addrd.adva, 6);
+            aux->adva_type = addrd.adva_type;
+            aux->flags |= BLE_LL_SCAN_AUX_F_HAS_ADVA;
+        }
+
+        if (addrd.targeta) {
+            memcpy(aux->targeta, addrd.targeta, 6);
+            aux->targeta_type = addrd.targeta_type;
+            aux->flags |= BLE_LL_SCAN_AUX_F_HAS_TARGETA;
+        }
+
+        aux->adi = adi;
+        aux->flags |= BLE_LL_SCAN_AUX_F_HAS_ADI;
+
+        if (aux->scan_type == BLE_SCAN_TYPE_INITIATE) {
+            aux->flags |= BLE_LL_SCAN_AUX_F_CONNECTABLE;
+        }
+
+        if (do_match) {
+            aux->flags |= BLE_LL_SCAN_AUX_F_MATCHED;
+            ble_ll_scan_aux_update_aux_data_from_addrd(&addrd, aux);
+        } else if (addrd.adva) {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+            /* If ext PDU has AdvA, we need to store rpa_index to be able to
+             * reuse it for filtering when done on aux PDU.
+             */
+            aux->rpa_index = ble_hw_resolv_list_match();
+#endif
+        }
+
+        rxinfo->user_data = aux;
+    }
+
+    return 0;
+}
+
+void
+ble_ll_scan_aux_pkt_in_on_ext(struct os_mbuf *rxpdu,
+                              struct ble_mbuf_hdr *rxhdr)
+{
+    struct ble_mbuf_hdr_rxinfo *rxinfo = &rxhdr->rxinfo;
+    struct ble_ll_scan_aux_data *aux = rxinfo->user_data;
+    int rc;
+
+    if (rxinfo->flags & BLE_MBUF_HDR_F_IGNORED) {
+        BLE_LL_ASSERT(!aux);
+        return;
+    }
+
+    if (!aux) {
+        ble_ll_hci_ev_send_ext_adv_report_for_ext(rxpdu, rxhdr);
+        return;
+    }
+
+    BLE_LL_ASSERT(aux->aux_ptr);
+
+    rc = ble_ll_scan_aux_sched(aux, rxhdr->beg_cputime, rxhdr->rem_usecs,
+                               aux->aux_ptr);
+    if (rc < 0) {
+        ble_ll_scan_aux_free(aux);
+    }
+}
+
+static uint8_t
+ble_ll_scan_aux_scan_req_tx_pdu_cb(uint8_t *dptr, void *arg, uint8_t *hdr_byte)
+{
+    struct ble_ll_scan_aux_data *aux = arg;
+    uint8_t *scana;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    struct ble_ll_resolv_entry *rl;
+    uint8_t rpa[BLE_DEV_ADDR_LEN];
+#endif
+    uint8_t hb;
+
+    hb = BLE_ADV_PDU_TYPE_SCAN_REQ;
+
+    /* ScanA */
+    if (ble_ll_scan_get_own_addr_type() & 0x01) {
+        hb |= BLE_ADV_PDU_HDR_TXADD_RAND;
+        scana = g_random_addr;
+    } else {
+        scana = g_dev_addr;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    if (ble_ll_scan_get_own_addr_type() & 0x02) {
+        if (aux->rpa_index >=0) {
+            rl = &g_ble_ll_resolv_list[aux->rpa_index];
+        } else {
+            rl = NULL;
+        }
+
+        /*
+         * If device is on RL and we have local IRK, we use RPA generated using
+         * that IRK as ScanA. Otherwise we use NRPA as ScanA to prevent our
+         * device from being tracked when doing an active scan
+         * ref: Core 5.2, Vol 6, Part B, section 6.3)
+         */
+        if (rl && rl->rl_has_local) {
+            ble_ll_resolv_get_priv_addr(rl, 1, rpa);
+            scana = rpa;
+        } else {
+            scana = ble_ll_get_scan_nrpa();
+        }
+
+        hb |= BLE_ADV_PDU_HDR_TXADD_RAND;
+    }
+#endif
+    memcpy(dptr, scana, BLE_DEV_ADDR_LEN);
+
+    /* AdvA */
+    if (aux->adva_type) {
+        hb |= BLE_ADV_PDU_HDR_RXADD_RAND;
+    }
+    memcpy(dptr + BLE_DEV_ADDR_LEN, aux->adva, BLE_DEV_ADDR_LEN);
+
+    *hdr_byte = hb;
+
+    return BLE_DEV_ADDR_LEN * 2;
+}
+
+static bool
+ble_ll_scan_aux_send_scan_req(struct ble_ll_scan_aux_data *aux,
+                              struct ble_ll_scan_addr_data *addrd)
+{
+    int rc;
+
+    /* Check if we already scanned this device successfully */
+    if (ble_ll_scan_have_rxd_scan_rsp(addrd->adv_addr, addrd->adv_addr_type,
+                                      true, aux->adi)) {
+        return false;
+    }
+
+    /* We want to send a request, see if backoff allows us */
+    if (ble_ll_scan_backoff_kick() != 0) {
+        return false;
+    }
+
+    /* TODO perhaps we should check if scan req+rsp won't overlap with scheduler
+     *      item (old code did it), but for now let's just scan and we will be
+     *      interrupted if scheduler kicks in.
+     */
+
+    rc = ble_phy_tx(ble_ll_scan_aux_scan_req_tx_pdu_cb, aux,
+                    BLE_PHY_TRANSITION_TX_RX);
+    if (rc) {
+        return false;
+    }
+
+    return true;
+}
+
+int
+ble_ll_scan_aux_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok)
+{
+    struct ble_ll_scan_addr_data addrd;
+    struct ble_mbuf_hdr_rxinfo *rxinfo;
+    struct ble_ll_scan_aux_data *aux;
+    struct ble_mbuf_hdr *rxhdr;
+    uint8_t scan_filt_policy;
+    uint8_t scan_ok;
+    uint8_t adv_mode;
+    uint8_t *rxbuf;
+    int rc;
+
+    BLE_LL_ASSERT(aux_data_current);
+    aux = aux_data_current;
+    aux_data_current = NULL;
+
+    if (rxpdu == NULL) {
+        ble_ll_scan_aux_break(aux);
+        goto done;
+    }
+
+    rxhdr = BLE_MBUF_HDR_PTR(rxpdu);
+    rxinfo = &rxhdr->rxinfo;
+    rxinfo->user_data = aux;
+
+    if (!crcok) {
+        rxinfo->flags |= BLE_MBUF_HDR_F_IGNORED;
+        goto done;
+    }
+
+    rxbuf = rxpdu->om_data;
+
+    if (aux->flags & BLE_LL_SCAN_AUX_F_W4_CONNECT_RSP) {
+        aux->flags &= ~BLE_LL_SCAN_AUX_F_W4_CONNECT_RSP;
+        rxinfo->flags |= BLE_MBUF_HDR_F_CONNECT_RSP_RXD;
+        goto done;
+    }
+
+    if (aux->flags & BLE_LL_SCAN_AUX_F_W4_SCAN_RSP) {
+        aux->flags &= ~BLE_LL_SCAN_AUX_F_W4_SCAN_RSP;
+        aux->flags |= BLE_LL_SCAN_AUX_F_SCANNED;
+        rxinfo->flags |= BLE_MBUF_HDR_F_SCAN_RSP_RXD;
+    }
+
+    rc = ble_ll_scan_aux_parse_to_aux_data(aux, rxbuf, rxhdr);
+    if (rc < 0) {
+        rxinfo->flags |= BLE_MBUF_HDR_F_IGNORED;
+        goto done;
+    }
+
+    if (aux->flags & BLE_LL_SCAN_AUX_F_MATCHED) {
+        goto done;
+    }
+
+    /* We do filtering on either ADV_EXT_IND or AUX_ADV_IND so we should not be
+     * here when processing AUX_CHAIN_IND.
+     */
+    BLE_LL_ASSERT(!(aux->flags & BLE_LL_SCAN_AUX_F_AUX_CHAIN));
+
+    if (aux->flags & BLE_LL_SCAN_AUX_F_HAS_ADVA) {
+        addrd.adva = aux->adva;
+        addrd.adva_type = aux->adva_type;
+    } else {
+        addrd.adva = NULL;
+    }
+
+    if (aux->flags & BLE_LL_SCAN_AUX_F_HAS_TARGETA) {
+        addrd.targeta = aux->targeta;
+        addrd.targeta_type = aux->targeta_type;
+    } else {
+        addrd.targeta = NULL;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    addrd.rpa_index = aux->rpa_index;
+#endif
+
+    scan_filt_policy = ble_ll_scan_get_filt_policy();
+
+    rc = ble_ll_scan_rx_filter(ble_ll_scan_get_own_addr_type(),
+                               scan_filt_policy, &addrd, &scan_ok);
+    if (rc < 0) {
+        rxinfo->flags |= BLE_MBUF_HDR_F_IGNORED;
+        /*
+         * XXX hack warning
+         * Even if PDU was not allowed by current scan filter policy, we should
+         * still allow it to sync if SyncInfo is present. Since we do not use
+         * F_DEVMATCH in aux code for its intended purpose, let's use it here to
+         * indicate no match due to scan filter policy.
+         */
+        if (rc == -2) {
+            rxinfo->flags |= BLE_MBUF_HDR_F_DEVMATCH;
+        }
+        goto done;
+    }
+
+    if ((aux->scan_type == BLE_SCAN_TYPE_INITIATE) &&
+        !(scan_filt_policy & 0x01)) {
+        rc = ble_ll_scan_rx_check_init(&addrd);
+        if (rc < 0) {
+            goto done;
+        }
+    }
+
+    aux->flags |= BLE_LL_SCAN_AUX_F_MATCHED;
+
+    ble_ll_scan_aux_update_aux_data_from_addrd(&addrd, aux);
+
+    adv_mode = rxbuf[2] >> 6;
+
+    switch (aux->scan_type) {
+    case BLE_SCAN_TYPE_ACTIVE:
+        if ((adv_mode == BLE_LL_EXT_ADV_MODE_SCAN) && scan_ok &&
+            ble_ll_scan_aux_send_scan_req(aux, &addrd)) {
+            /* AUX_SCAN_REQ sent, keep PHY enabled to continue */
+            aux->flags |= BLE_LL_SCAN_AUX_F_W4_SCAN_RSP;
+            rxinfo->flags |= BLE_MBUF_HDR_F_SCAN_REQ_TXD;
+            aux_data_current = aux;
+            return 0;
+        }
+        break;
+    case BLE_SCAN_TYPE_INITIATE:
+        if (ble_ll_conn_send_connect_req(rxpdu, &addrd, 1) == 0) {
+            /* AUX_CONNECT_REQ sent, keep PHY enabled to continue */
+            aux->flags |= BLE_LL_SCAN_AUX_F_W4_CONNECT_RSP;
+            rxinfo->flags |= BLE_MBUF_HDR_F_CONNECT_REQ_TXD;
+            aux_data_current = aux;
+            return 0;
+        } else {
+            rxinfo->flags |= BLE_MBUF_HDR_F_IGNORED;
+        }
+        break;
+    }
+
+done:
+    /* We are done with this PDU so go to standby and let LL resume if needed */
+    ble_ll_state_set(BLE_LL_STATE_STANDBY);
+    return -1;
+}
+
+static void
+ble_ll_scan_aux_init_addrd_from_aux_data(struct ble_ll_scan_aux_data *aux,
+                                         struct ble_ll_scan_addr_data *addrd)
+{
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    struct ble_ll_resolv_entry *rl;
+#endif
+
+    if (aux->flags & BLE_LL_SCAN_AUX_F_HAS_ADVA) {
+        addrd->adva = aux->adva;
+        addrd->adva_type = aux->adva_type;
+    } else {
+        addrd->adva = NULL;
+        addrd->adva_type = 0;
+    }
+
+    if (aux->flags & BLE_LL_SCAN_AUX_F_HAS_TARGETA) {
+        addrd->targeta = aux->targeta;
+        addrd->targeta_type = aux->targeta_type;
+    } else {
+        addrd->targeta = NULL;
+        addrd->targeta_type = 0;
+    }
+
+    addrd->adv_addr = addrd->adva;
+    addrd->adv_addr_type = addrd->adva_type;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    addrd->rpa_index = aux->rpa_index;
+
+    if (aux->flags & BLE_LL_SCAN_AUX_F_RESOLVED_ADVA) {
+        BLE_LL_ASSERT(aux->rpa_index >= 0);
+        rl = &g_ble_ll_resolv_list[aux->rpa_index];
+        addrd->adv_addr = rl->rl_identity_addr;
+        addrd->adv_addr_type = rl->rl_addr_type;
+        addrd->adva_resolved = 1;
+    } else {
+        addrd->adva_resolved = 0;
+    }
+
+    if (aux->flags & BLE_LL_SCAN_AUX_F_RESOLVED_TARGETA) {
+        addrd->targeta = ble_ll_scan_aux_get_own_addr();
+        addrd->targeta_type = ble_ll_scan_get_own_addr_type() & 1;
+        addrd->targeta_resolved = 1;
+    } else {
+        addrd->targeta_resolved = 0;
+    }
+#endif
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+static void
+ble_ll_scan_aux_sync_check(struct os_mbuf *rxpdu,
+                           struct ble_ll_scan_addr_data *addrd)
+{
+    struct ble_mbuf_hdr *rxhdr = BLE_MBUF_HDR_PTR(rxpdu);
+    uint8_t *rxbuf = rxpdu->om_data;
+    uint8_t adv_mode;
+    uint8_t eh_len;
+    uint8_t eh_flags;
+    uint8_t *eh_data;
+    uint8_t sid;
+
+    adv_mode = rxbuf[2] >> 6;
+
+    if (adv_mode != BLE_LL_EXT_ADV_MODE_NON_CONN) {
+        return;
+    }
+
+    eh_len = rxbuf[2] & 0x3f;
+
+    if (eh_len == 0) {
+        return;
+    }
+
+    eh_flags = rxbuf[3];
+    eh_data = &rxbuf[4];
+
+    /* Need ADI and SyncInfo */
+    if (!(eh_flags & ((1 << BLE_LL_EXT_ADV_SYNC_INFO_BIT) |
+                      (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)))) {
+        return;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_ADVA_BIT)) {
+        eh_data += BLE_LL_EXT_ADV_ADVA_SIZE;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_TARGETA_BIT)) {
+        eh_data += BLE_LL_EXT_ADV_TARGETA_SIZE;
+    }
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_CTE_INFO_BIT)) {
+        eh_data += 1;
+    }
+
+    sid = get_le16(eh_data) >> 12;
+    eh_data += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+
+    if (eh_flags & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) {
+        eh_data += BLE_LL_EXT_ADV_AUX_PTR_SIZE;
+    }
+
+    ble_ll_sync_info_event(addrd, sid, rxhdr, eh_data);
+}
+#endif
+
+
+static int
+ble_ll_scan_aux_check_connect_rsp(uint8_t *rxbuf,
+                                  struct ble_ll_scan_pdu_data *pdu_data,
+                                  struct ble_ll_scan_addr_data *addrd)
+{
+    uint8_t pdu_hdr;
+    uint8_t pdu_len;
+    uint8_t adv_mode;
+    uint8_t eh_len;
+    uint8_t eh_flags;
+    uint8_t *eh_data;
+    uint8_t adva_type;
+    uint8_t *adva;
+    uint8_t targeta_type;
+    uint8_t *targeta;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    struct ble_ll_resolv_entry *rl = NULL;
+#endif
+
+    pdu_hdr = rxbuf[0];
+    pdu_len = rxbuf[1];
+
+    if (pdu_len == 0) {
+        return -1;
+    }
+
+    adv_mode = rxbuf[2] >> 6;
+    eh_len = rxbuf[2] & 0x3f;
+
+    if ((adv_mode != 0) || (eh_len == 0)) {
+        return -1;
+    }
+
+    eh_flags = rxbuf[3];
+    eh_data = &rxbuf[4];
+
+    /* AUX_CONNECT_RSP without AdvA or TargetA is not valid */
+    if (!(eh_flags & ((1 << BLE_LL_EXT_ADV_ADVA_BIT) |
+                      (1 << BLE_LL_EXT_ADV_TARGETA_BIT)))) {
+        return -1;
+    }
+
+    adva = eh_data;
+    adva_type = !!(pdu_hdr & BLE_ADV_PDU_HDR_TXADD_RAND);
+    eh_data += BLE_LL_EXT_ADV_ADVA_SIZE;
+
+    targeta = eh_data;
+    targeta_type = !!(pdu_hdr & BLE_ADV_PDU_HDR_RXADD_RAND);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    /* If AdvA was initially resolved, we need to check if current AdvA also
+     * resolved with the same IRK since it may have changes due to RPA timeout.
+     * Otherwise it shall be the same as in AUX_CONNECT_REQ.
+     */
+    if (addrd->adva_resolved) {
+        if (!adva_type) {
+            return -1;
+        }
+
+        BLE_LL_ASSERT(addrd->rpa_index >= 0);
+        rl = &g_ble_ll_resolv_list[addrd->rpa_index];
+
+        if (!ble_ll_resolv_rpa(adva, rl->rl_peer_irk)) {
+            return -1;
+        }
+
+        addrd->adva = adva;
+    } else if ((adva_type !=
+                !!(pdu_data->hdr_byte & BLE_ADV_PDU_HDR_RXADD_MASK)) ||
+               (memcmp(adva, pdu_data->adva, 6) != 0)) {
+            return -1;
+    }
+#else
+    if ((adva_type != !!(pdu_data->hdr_byte & BLE_ADV_PDU_HDR_RXADD_MASK)) ||
+        (memcmp(adva, pdu_data->adva, 6) != 0)) {
+            return -1;
+    }
+#endif /* BLE_LL_CFG_FEAT_LL_PRIVACY */
+
+    if ((targeta_type != !!(pdu_data->hdr_byte & BLE_ADV_PDU_HDR_RXADD_MASK)) ||
+        (memcmp(targeta, pdu_data->inita, 6) != 0)) {
+        return -1;
+    }
+
+    return 0;
+}
+
+static void
+ble_ll_scan_aux_rx_pkt_in_for_initiator(struct os_mbuf *rxpdu,
+                                        struct ble_mbuf_hdr *rxhdr)
+{
+    struct ble_ll_scan_addr_data addrd;
+    struct ble_mbuf_hdr_rxinfo *rxinfo;
+    struct ble_ll_scan_aux_data *aux;
+
+    rxinfo = &rxhdr->rxinfo;
+    aux = rxinfo->user_data;
+
+    if (rxinfo->flags & BLE_MBUF_HDR_F_IGNORED) {
+        ble_ll_scan_chk_resume();
+        goto done;
+    }
+
+    if (!(rxinfo->flags & BLE_MBUF_HDR_F_CONNECT_RSP_RXD)) {
+        BLE_LL_ASSERT(rxinfo->flags & BLE_MBUF_HDR_F_CONNECT_REQ_TXD);
+        /* Waiting for AUX_CONNECT_RSP, do nothing */
+        return;
+    }
+
+    ble_ll_scan_aux_init_addrd_from_aux_data(aux, &addrd);
+
+    if (ble_ll_scan_aux_check_connect_rsp(rxpdu->om_data,
+                                          ble_ll_scan_get_pdu_data(),
+                                          &addrd) < 0) {
+        ble_ll_scan_chk_resume();
+        goto done;
+    }
+
+    aux->flags &= ~BLE_LL_SCAN_AUX_F_W4_CONNECT_RSP;
+
+    ble_ll_scan_sm_stop(0);
+    ble_ll_conn_created_on_aux(rxpdu, &addrd, aux->targeta);
+
+done:
+    if (aux->flags & BLE_LL_SCAN_AUX_F_W4_CONNECT_RSP) {
+        ble_ll_conn_send_connect_req_cancel();
+    }
+    ble_ll_scan_aux_free(aux);
+}
+
+void
+ble_ll_scan_aux_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *rxhdr)
+{
+    struct ble_ll_scan_addr_data addrd;
+    struct ble_mbuf_hdr_rxinfo *rxinfo;
+    struct ble_ll_scan_aux_data *aux;
+    bool scan_duplicate = false;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    bool sync_check = false;
+#endif
+    int rc;
+
+    rxinfo = &rxhdr->rxinfo;
+    aux = rxinfo->user_data;
+
+    BLE_LL_ASSERT(aux);
+
+    if (aux->scan_type == BLE_SCAN_TYPE_INITIATE) {
+        ble_ll_scan_aux_rx_pkt_in_for_initiator(rxpdu, rxhdr);
+        return;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    sync_check = ble_ll_sync_enabled() &&
+                 !(aux->flags & BLE_LL_SCAN_AUX_F_AUX_CHAIN);
+
+    /* PDU was not allowed due to scan filter policy, but we can still try to
+     * sync since separate filter policy is used for this purpose.
+     */
+    if ((rxinfo->flags & BLE_MBUF_HDR_F_DEVMATCH) && sync_check) {
+        ble_ll_scan_aux_init_addrd_from_aux_data(aux, &addrd);
+        ble_ll_scan_aux_sync_check(rxpdu, &addrd);
+    }
+#endif
+
+    if (rxinfo->flags & BLE_MBUF_HDR_F_IGNORED) {
+        if (ble_ll_scan_aux_need_truncation(aux)) {
+            ble_ll_hci_ev_send_ext_adv_truncated_report(aux);
+        } else {
+            aux->hci_state |= BLE_LL_SCAN_AUX_H_DONE;
+        }
+
+        ble_ll_scan_aux_update_scan_backoff(aux);
+    }
+
+    if (aux->hci_state & BLE_LL_SCAN_AUX_H_DONE) {
+        ble_ll_scan_aux_free(aux);
+        ble_ll_scan_chk_resume();
+        return;
+    }
+
+    /* Try to schedule scan for subsequent aux asap, if needed */
+    if (rxinfo->flags & BLE_MBUF_HDR_F_AUX_PTR_WAIT) {
+        rc = ble_ll_scan_aux_sched(aux, rxhdr->beg_cputime, rxhdr->rem_usecs,
+                                   aux->aux_ptr);
+        if (rc < 0) {
+            rxinfo->flags &= ~BLE_MBUF_HDR_F_AUX_PTR_WAIT;
+        }
+    }
+
+    ble_ll_scan_aux_init_addrd_from_aux_data(aux, &addrd);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+    if (sync_check) {
+        ble_ll_scan_aux_sync_check(rxpdu, &addrd);
+    }
+#endif
+
+    scan_duplicate = ble_ll_scan_get_filt_dups() &&
+                     ble_ll_scan_dup_check_ext(addrd.adv_addr_type,
+                                               addrd.adv_addr, true, aux->adi);
+    if (!scan_duplicate) {
+        rc = ble_ll_hci_ev_send_ext_adv_report_for_aux(rxpdu, rxhdr, aux,
+                                                       &addrd);
+
+        /*
+         * Update duplicates list if report was sent successfully and we are
+         * done with this chain. On error, status is already set to 'done' so
+         * we will cancel aux scan (if any) and stop further processing.
+         * However, if we send AUX_SCAN_REQ for this PDU then need to remove
+         * 'done' as we should continue with scanning after AUX_SCAN_RSP.
+         */
+
+        if (rc == 0) {
+            if (rxinfo->flags & BLE_MBUF_HDR_F_SCAN_REQ_TXD) {
+                aux->hci_state &= ~BLE_LL_SCAN_AUX_H_DONE;
+            } else if (aux->hci_state & BLE_LL_SCAN_AUX_H_DONE) {
+                BLE_LL_ASSERT(!(rxinfo->flags & BLE_MBUF_HDR_F_AUX_PTR_WAIT));
+                if (ble_ll_scan_get_filt_dups()) {
+                    ble_ll_scan_dup_update_ext(addrd.adv_addr_type,
+                                               addrd.adv_addr, true, aux->adi);
+                }
+                if (aux->flags & BLE_LL_SCAN_AUX_F_SCANNED) {
+                    ble_ll_scan_add_scan_rsp_adv(addrd.adv_addr,
+                                                 addrd.adv_addr_type,
+                                                 1, aux->adi);
+                }
+            }
+        }
+    } else {
+        /* This is a duplicate, we don't want to scan it anymore */
+        aux->hci_state |= BLE_LL_SCAN_AUX_H_DONE;
+    }
+
+    /*
+     * If we are done processing this chain and aux scan was not scheduled or
+     * we removed it from scheduler, we can remove aux_data now. Otherwise we
+     * will remove on next pkt_in.
+     */
+    if ((aux->hci_state & BLE_LL_SCAN_AUX_H_DONE) &&
+        (!(rxinfo->flags & BLE_MBUF_HDR_F_AUX_PTR_WAIT) ||
+         (ble_ll_sched_rmv_elem(&aux->sch) == 0))) {
+        ble_ll_scan_aux_update_scan_backoff(aux);
+        ble_ll_scan_aux_free(aux);
+    }
+
+    ble_ll_scan_chk_resume();
+}
+
+void
+ble_ll_scan_aux_wfr_timer_exp(void)
+{
+    ble_phy_disable();
+    ble_ll_scan_aux_halt();
+}
+
+void
+ble_ll_scan_aux_halt(void)
+{
+    struct ble_ll_scan_aux_data *aux = aux_data_current;
+
+    BLE_LL_ASSERT(aux);
+
+    aux_data_current = NULL;
+
+    ble_ll_state_set(BLE_LL_STATE_STANDBY);
+
+    ble_ll_scan_aux_break(aux);
+}
+
+void
+ble_ll_scan_aux_sched_remove(struct ble_ll_sched_item *sch)
+{
+    ble_ll_scan_aux_break(sch->cb_arg);
+}
+
+void
+ble_ll_scan_aux_init(void)
+{
+    os_error_t err;
+
+    err = os_mempool_init(&aux_data_pool,
+                          MYNEWT_VAL(BLE_LL_SCAN_AUX_SEGMENT_CNT),
+                          sizeof(struct ble_ll_scan_aux_data),
+                          aux_data_mem, "ble_ll_scan_aux_data_pool");
+    BLE_LL_ASSERT(err == 0);
+}
+
+#endif /* BLE_LL_CFG_FEAT_LL_EXT_ADV */

nimble/controller/src/ble_ll_supp_cmd.c

@@ -36,7 +36,22 @@
 
 /* Octet 10 */
 #define BLE_SUPP_CMD_RD_TX_PWR              (0 << 2)
-#define BLE_LL_SUPP_CMD_OCTET_10            (BLE_SUPP_CMD_RD_TX_PWR)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_CTRL_TO_HOST_FLOW_CONTROL)
+#define BLE_SUPP_CMD_SET_CTRL_TO_HOST_FLOW  (1 << 5)
+#define BLE_SUPP_CMD_HOST_BUFFER_SIZE       (1 << 6)
+#define BLE_SUPP_CMD_HOST_NUM_COMP_PACKETS  (1 << 7)
+#else
+#define BLE_SUPP_CMD_SET_CTRL_TO_HOST_FLOW  (0 << 5)
+#define BLE_SUPP_CMD_HOST_BUFFER_SIZE       (0 << 6)
+#define BLE_SUPP_CMD_HOST_NUM_COMP_PACKETS  (0 << 7)
+#endif
+#define BLE_LL_SUPP_CMD_OCTET_10            \
+(                                           \
+    BLE_SUPP_CMD_RD_TX_PWR              |   \
+    BLE_SUPP_CMD_SET_CTRL_TO_HOST_FLOW  |   \
+    BLE_SUPP_CMD_HOST_BUFFER_SIZE       |   \
+    BLE_SUPP_CMD_HOST_NUM_COMP_PACKETS      \
+)
 
 /* Octet 14 */
 #define BLE_SUPP_CMD_RD_LOC_VER             (1 << 3)
@@ -106,7 +121,7 @@
 #define BLE_SUPP_CMD_LE_SET_HOST_CHAN_CLASS (1 << 3)
 #define BLE_SUPP_CMD_LE_RD_CHAN_MAP         (1 << 4)
 #define BLE_SUPP_CMD_LE_RD_REM_USED_FEAT    (1 << 5)
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
 #define BLE_SUPP_CMD_LE_ENCRYPT             (1 << 6)
 #else
 #define BLE_SUPP_CMD_LE_ENCRYPT             (0 << 6)
@@ -126,7 +141,7 @@
 )
 
 /* Octet 28 */
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
 #define BLE_SUPP_CMD_LE_START_ENCRYPT       (1 << 0)
 #define BLE_SUPP_CMD_LE_LTK_REQ_REPLY       (1 << 1)
 #define BLE_SUPP_CMD_LE_LTK_REQ_NEG_REPLY   (1 << 2)
@@ -137,14 +152,15 @@
 #endif
 #define BLE_SUPP_CMD_LE_READ_SUPP_STATES    (1 << 3)
 
-#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 0
-#define BLE_SUPP_CMD_LE_RX_TEST             (0 << 4)
-#define BLE_SUPP_CMD_LE_TX_TEST             (0 << 5)
-#define BLE_SUPP_CMD_LE_TEST_END            (0 << 6)
-#else
+#if MYNEWT_VAL(BLE_LL_DTM)
 #define BLE_SUPP_CMD_LE_RX_TEST             (1 << 4)
 #define BLE_SUPP_CMD_LE_TX_TEST             (1 << 5)
 #define BLE_SUPP_CMD_LE_TEST_END            (1 << 6)
+
+#else
+#define BLE_SUPP_CMD_LE_RX_TEST             (0 << 4)
+#define BLE_SUPP_CMD_LE_TX_TEST             (0 << 5)
+#define BLE_SUPP_CMD_LE_TEST_END            (0 << 6)
 #endif
 
 #define BLE_LL_SUPP_CMD_OCTET_28            \
@@ -161,7 +177,7 @@
 /* Octet 33 */
 #define BLE_SUPP_CMD_LE_REM_CONN_PRR        (1 << 4)
 #define BLE_SUPP_CMD_LE_REM_CONN_PRNR       (1 << 5)
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_DATA_LEN_EXT) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_DATA_LEN_EXT)
 #define BLE_SUPP_CMD_LE_SET_DATALEN         (1 << 6)
 #define BLE_SUPP_CMD_LE_RD_SUGG_DATALEN     (1 << 7)
 #else
@@ -178,14 +194,14 @@
 )
 
 /* Octet 34 */
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_DATA_LEN_EXT) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_DATA_LEN_EXT)
 #define BLE_SUPP_CMD_LE_WR_SUGG_DATALEN     (1 << 0)
 #else
 #define BLE_SUPP_CMD_LE_WR_SUGG_DATALEN     (0 << 0)
 #endif
 #define BLE_SUPP_CMD_LE_READ_LOCAL_P256_PK  (0 << 1)
 #define BLE_SUPP_CMD_LE_GENERATE_DH_KEY     (0 << 2)
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
 #define BLE_SUPP_CMD_LE_ADD_RESOLV_LIST     (1 << 3)
 #define BLE_SUPP_CMD_LE_REMOVE_RESOLV_LIST  (1 << 4)
 #define BLE_SUPP_CMD_LE_CLEAR_RESOLV_LIST   (1 << 5)
@@ -212,7 +228,7 @@
 )
 
 /* Octet 35 */
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
 #define BLE_SUPP_CMD_LE_RD_LOCAL_RESV_ADDR  (1 << 0)
 #define BLE_SUPP_CMD_LE_SET_ADDR_RES_EN     (1 << 1)
 #define BLE_SUPP_CMD_LE_SET_RESV_ADDR_TMO   (1 << 2)
@@ -232,10 +248,10 @@
 #define BLE_SUPP_CMD_LE_SET_PHY             (0 << 6)
 #endif
 
-#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 0
-#define BLE_SUPP_CMD_LE_ENHANCED_RX_TEST    (0 << 7)
-#else
+#if MYNEWT_VAL(BLE_LL_DTM)
 #define BLE_SUPP_CMD_LE_ENHANCED_RX_TEST    (1 << 7)
+#else
+#define BLE_SUPP_CMD_LE_ENHANCED_RX_TEST    (0 << 7)
 #endif
 
 #define BLE_LL_SUPP_CMD_OCTET_35            \
@@ -251,13 +267,13 @@
 )
 
 /* Octet 36 */
-#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 0
-#define BLE_SUPP_CMD_LE_ENHANCED_TX_TEST    (0 << 0)
-#else
+#if MYNEWT_VAL(BLE_LL_DTM)
 #define BLE_SUPP_CMD_LE_ENHANCED_TX_TEST    (1 << 0)
+#else
+#define BLE_SUPP_CMD_LE_ENHANCED_TX_TEST    (0 << 0)
 #endif
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
 #define BLE_SUPP_CMD_LE_SET_ADVS_RAND_ADDR  (1 << 1)
 #define BLE_SUPP_CMD_LE_SET_EXT_ADV_PARAM   (1 << 2)
 #define BLE_SUPP_CMD_LE_SET_EXT_ADV_DATA    (1 << 3)
@@ -288,17 +304,23 @@
 )
 
 /* Octet 37 */
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
 #define BLE_SUPP_CMD_LE_REMOVE_ADVS         (1 << 0)
 #define BLE_SUPP_CMD_LE_CLEAR_ADVS          (1 << 1)
 #else
 #define BLE_SUPP_CMD_LE_REMOVE_ADVS         (0 << 0)
 #define BLE_SUPP_CMD_LE_CLEAR_ADVS          (0 << 1)
 #endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+#define BLE_SUPP_CMD_LE_SET_PADV_PARAM      (1 << 2)
+#define BLE_SUPP_CMD_LE_SET_PADV_DATA       (1 << 3)
+#define BLE_SUPP_CMD_LE_SET_PADV_ENABLE     (1 << 4)
+#else
 #define BLE_SUPP_CMD_LE_SET_PADV_PARAM      (0 << 2)
 #define BLE_SUPP_CMD_LE_SET_PADV_DATA       (0 << 3)
 #define BLE_SUPP_CMD_LE_SET_PADV_ENABLE     (0 << 4)
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) == 1)
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
 #define BLE_SUPP_CMD_LE_SET_EXT_SCAN_PARAM  (1 << 5)
 #define BLE_SUPP_CMD_LE_SET_EXT_SCAN_ENABLE (1 << 6)
 #define BLE_SUPP_CMD_LE_EXT_CREATE_CONN     (1 << 7)
@@ -321,6 +343,15 @@
 )
 
 /* Octet 38 */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+#define BLE_SUPP_CMD_LE_PADV_CREATE_SYNC    (1 << 0)
+#define BLE_SUPP_CMD_LE_PADV_CREATE_SYNC_C  (1 << 1)
+#define BLE_SUPP_CMD_LE_PADV_TERMINATE_SYNC (1 << 2)
+#define BLE_SUPP_CMD_LE_ADD_PADV_LIST       (1 << 3)
+#define BLE_SUPP_CMD_LE_REMOVE_PADV_LIST    (1 << 4)
+#define BLE_SUPP_CMD_LE_CLEAR_PADV_LIST     (1 << 5)
+#define BLE_SUPP_CMD_LE_RD_PADV_LIST_SIZE   (1 << 6)
+#else
 #define BLE_SUPP_CMD_LE_PADV_CREATE_SYNC    (0 << 0)
 #define BLE_SUPP_CMD_LE_PADV_CREATE_SYNC_C  (0 << 1)
 #define BLE_SUPP_CMD_LE_PADV_TERMINATE_SYNC (0 << 2)
@@ -328,7 +359,8 @@
 #define BLE_SUPP_CMD_LE_REMOVE_PADV_LIST    (0 << 4)
 #define BLE_SUPP_CMD_LE_CLEAR_PADV_LIST     (0 << 5)
 #define BLE_SUPP_CMD_LE_RD_PADV_LIST_SIZE   (0 << 6)
-#define BLE_SUPP_CMD_LE_RD_TX_POWER         (0 << 7)
+#endif
+#define BLE_SUPP_CMD_LE_RD_TX_POWER         (1 << 7)
 
 #define BLE_LL_SUPP_CMD_OCTET_38            \
 (                                           \
@@ -343,9 +375,9 @@
 )
 
 /* Octet 39 */
-#define BLE_SUPP_CMD_LE_RD_RF_PATH_COMP     (0 << 0)
-#define BLE_SUPP_CMD_LE_WR_RF_PATH_COMP     (0 << 1)
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
+#define BLE_SUPP_CMD_LE_RD_RF_PATH_COMP     (1 << 0)
+#define BLE_SUPP_CMD_LE_WR_RF_PATH_COMP     (1 << 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
 #define BLE_SUPP_CMD_LE_SET_PRIVACY_MODE    (1 << 2)
 #else
 #define BLE_SUPP_CMD_LE_SET_PRIVACY_MODE    (0 << 2)
@@ -358,6 +390,108 @@
     BLE_SUPP_CMD_LE_SET_PRIVACY_MODE        \
 )
 
+/* Octet 40 */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) && MYNEWT_VAL(BLE_VERSION) >= 51
+#define BLE_SUPP_CMD_LE_PADV_RECV_ENABLE (1 << 5)
+#else
+#define BLE_SUPP_CMD_LE_PADV_RECV_ENABLE (0 << 5)
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+#define BLE_SUPP_CMD_LE_PADV_SYNC_TRANSFER (1 << 6)
+#define BLE_SUPP_CMD_LE_PADV_SET_INFO_TRANSFER (1 << 7)
+#else
+#define BLE_SUPP_CMD_LE_PADV_SYNC_TRANSFER (0 << 6)
+#define BLE_SUPP_CMD_LE_PADV_SET_INFO_TRANSFER (0 << 7)
+#endif
+
+#define BLE_LL_SUPP_CMD_OCTET_40             \
+(                                            \
+    BLE_SUPP_CMD_LE_PADV_RECV_ENABLE       | \
+    BLE_SUPP_CMD_LE_PADV_SYNC_TRANSFER     | \
+    BLE_SUPP_CMD_LE_PADV_SET_INFO_TRANSFER   \
+)
+
+/* Octet 41 */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+#define BLE_SUPP_CMD_LE_PADV_SYNC_TRANSFER_PARAMS (1 << 0)
+#define BLE_SUPP_CMD_LE_PADV_DEFAULT_SYNC_TRANSFER_PARAMS (1 << 1)
+#else
+#define BLE_SUPP_CMD_LE_PADV_SYNC_TRANSFER_PARAMS (0 << 0)
+#define BLE_SUPP_CMD_LE_PADV_DEFAULT_SYNC_TRANSFER_PARAMS (0 << 1)
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_ISO)
+#define BLE_SUPP_CMD_LE_READ_BUF_SIZE_V2    (1 << 5)
+#define BLE_SUPP_CMD_LE_READ_ISO_TX_SYNC    (1 << 6)
+#define BLE_SUPP_CMD_LE_SET_CIG_PARAM       (1 << 7)
+#else
+#define BLE_SUPP_CMD_LE_READ_BUF_SIZE_V2    (0 << 5)
+#define BLE_SUPP_CMD_LE_READ_ISO_TX_SYNC    (0 << 6)
+#define BLE_SUPP_CMD_LE_SET_CIG_PARAM       (0 << 7)
+#endif
+
+#define BLE_LL_SUPP_CMD_OCTET_41                        \
+(                                                       \
+    BLE_SUPP_CMD_LE_PADV_SYNC_TRANSFER_PARAMS         | \
+    BLE_SUPP_CMD_LE_PADV_DEFAULT_SYNC_TRANSFER_PARAMS | \
+    BLE_SUPP_CMD_LE_READ_BUF_SIZE_V2                  | \
+    BLE_SUPP_CMD_LE_READ_ISO_TX_SYNC                  | \
+    BLE_SUPP_CMD_LE_SET_CIG_PARAM                       \
+)
+
+/* Octet 42 */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_ISO)
+#define BLE_SUPP_CMD_LE_SET_CIG_PARAM_TEST  (1 << 0)
+#define BLE_SUPP_CMD_LE_CREATE_CIS          (1 << 1)
+#define BLE_SUPP_CMD_LE_REMOVE_CIG          (1 << 2)
+#define BLE_SUPP_CMD_LE_ACCEPT_CIS_REQ      (1 << 3)
+#define BLE_SUPP_CMD_LE_REJECT_CIS_REQ      (1 << 4)
+#define BLE_SUPP_CMD_LE_CREATE_BIG          (1 << 5)
+#define BLE_SUPP_CMD_LE_CREATE_BIG_TEST     (1 << 6)
+#define BLE_SUPP_CMD_LE_TERMINATE_BIG       (1 << 7)
+#else
+#define BLE_SUPP_CMD_LE_SET_CIG_PARAM_TEST  (0 << 0)
+#define BLE_SUPP_CMD_LE_CREATE_CIS          (0 << 1)
+#define BLE_SUPP_CMD_LE_REMOVE_CIG          (0 << 2)
+#define BLE_SUPP_CMD_LE_ACCEPT_CIS_REQ      (0 << 3)
+#define BLE_SUPP_CMD_LE_REJECT_CIS_REQ      (0 << 4)
+#define BLE_SUPP_CMD_LE_CREATE_BIG          (0 << 5)
+#define BLE_SUPP_CMD_LE_CREATE_BIG_TEST     (0 << 6)
+#define BLE_SUPP_CMD_LE_TERMINATE_BIG       (0 << 7)
+#endif
+#define BLE_LL_SUPP_CMD_OCTET_42         \
+(                                        \
+    BLE_SUPP_CMD_LE_SET_CIG_PARAM_TEST | \
+    BLE_SUPP_CMD_LE_CREATE_CIS         | \
+    BLE_SUPP_CMD_LE_REMOVE_CIG         | \
+    BLE_SUPP_CMD_LE_ACCEPT_CIS_REQ     | \
+    BLE_SUPP_CMD_LE_REJECT_CIS_REQ     | \
+    BLE_SUPP_CMD_LE_CREATE_BIG         | \
+    BLE_SUPP_CMD_LE_CREATE_BIG_TEST    | \
+    BLE_SUPP_CMD_LE_TERMINATE_BIG        \
+)
+
+/* Octet 43 */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_SCA_UPDATE)
+#define BLE_SUPP_CMD_LE_REQUEST_PEER_SCA (1 << 2)
+#else
+#define BLE_SUPP_CMD_LE_REQUEST_PEER_SCA (0 << 0)
+#endif
+#define BLE_LL_SUPP_CMD_OCTET_43                        \
+(                                                       \
+    BLE_SUPP_CMD_LE_REQUEST_PEER_SCA  \
+)
+
+/* Octet 44 */
+#if MYNEWT_VAL(BLE_VERSION) >= 52
+#define BLE_SUPP_CMD_LE_SET_HOST_FEATURE (1 << 0)
+#else
+#define BLE_SUPP_CMD_LE_SET_HOST_FEATURE (0 << 0)
+#endif
+#define BLE_LL_SUPP_CMD_OCTET_44                        \
+(                                                       \
+    BLE_SUPP_CMD_LE_SET_HOST_FEATURE  \
+)
+
 /* Defines the array of supported commands */
 const uint8_t g_ble_ll_supp_cmds[BLE_LL_SUPP_CMD_LEN] =
 {
@@ -401,4 +535,9 @@ const uint8_t g_ble_ll_supp_cmds[BLE_LL_SUPP_CMD_LEN] =
     BLE_LL_SUPP_CMD_OCTET_37,
     BLE_LL_SUPP_CMD_OCTET_38,
     BLE_LL_SUPP_CMD_OCTET_39,
+    BLE_LL_SUPP_CMD_OCTET_40,           /* Octet 40 */
+    BLE_LL_SUPP_CMD_OCTET_41,
+    BLE_LL_SUPP_CMD_OCTET_42,
+    BLE_LL_SUPP_CMD_OCTET_43,
+    BLE_LL_SUPP_CMD_OCTET_44,
 };

nimble/controller/src/ble_ll_sync.c

@@ -0,0 +1,2283 @@
+/*
+ * 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 <stdbool.h>
+#include <stdint.h>
+#include <assert.h>
+
+#include "syscfg/syscfg.h"
+
+#include "controller/ble_ll.h"
+#include "controller/ble_ll_hci.h"
+#include "controller/ble_ll_sync.h"
+#include "controller/ble_ll_utils.h"
+#include "controller/ble_ll_sched.h"
+#include "controller/ble_ll_whitelist.h"
+#include "controller/ble_ll_scan.h"
+#include "controller/ble_ll_resolv.h"
+#include "controller/ble_ll_rfmgmt.h"
+
+#include "nimble/ble.h"
+#include "nimble/hci_common.h"
+#include "nimble/ble_hci_trans.h"
+
+#include "ble_ll_conn_priv.h"
+
+#include "stats/stats.h"
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
+
+/* defines number of events that can be lost during sync establishment
+ * before failed to be established error is reported
+ */
+#define BLE_LL_SYNC_ESTABLISH_CNT 6
+
+#define BLE_LL_SYNC_CNT MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_CNT)
+#define BLE_LL_SYNC_LIST_CNT MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_LIST_CNT)
+
+#define BLE_LL_SYNC_SM_FLAG_RESERVED        0x0001
+#define BLE_LL_SYNC_SM_FLAG_ESTABLISHING    0x0002
+#define BLE_LL_SYNC_SM_FLAG_ESTABLISHED     0x0004
+#define BLE_LL_SYNC_SM_FLAG_SET_ANCHOR      0x0008
+#define BLE_LL_SYNC_SM_FLAG_OFFSET_300      0x0010
+#define BLE_LL_SYNC_SM_FLAG_SYNC_INFO       0x0020
+#define BLE_LL_SYNC_SM_FLAG_DISABLED        0x0040
+#define BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED   0x0080
+#define BLE_LL_SYNC_SM_FLAG_HCI_TRUNCATED   0x0100
+#define BLE_LL_SYNC_SM_FLAG_NEW_CHANMAP     0x0200
+#define BLE_LL_SYNC_SM_FLAG_CHAIN           0x0400
+
+#define BLE_LL_SYNC_CHMAP_LEN               5
+#define BLE_LL_SYNC_ITVL_USECS              1250
+
+struct ble_ll_sync_sm {
+    uint16_t flags;
+
+    uint8_t adv_sid;
+    uint8_t adv_addr[BLE_DEV_ADDR_LEN];
+    uint8_t adv_addr_type;
+
+    uint8_t sca;
+    uint8_t chanmap[BLE_LL_SYNC_CHMAP_LEN];
+    uint8_t num_used_chans;
+
+    uint8_t chanmap_new[BLE_LL_SYNC_CHMAP_LEN];
+    uint16_t chanmap_new_instant;
+
+    uint8_t chan_index;
+    uint8_t chan_chain;
+
+    uint8_t phy_mode;
+
+    uint8_t sync_pending_cnt;
+
+    uint32_t timeout;
+    uint16_t skip;
+
+    uint16_t itvl;
+    uint8_t  itvl_usecs;
+    uint32_t itvl_ticks;
+
+    uint32_t crcinit; /* only 3 bytes are used */
+    uint32_t access_addr;
+    uint16_t event_cntr;
+    uint16_t channel_id;
+
+    uint32_t window_widening;
+    uint32_t last_anchor_point;
+    uint32_t anchor_point;
+    uint8_t anchor_point_usecs;
+
+    struct ble_ll_sched_item sch;
+
+    struct ble_npl_event sync_ev_end;
+
+    uint8_t *next_report;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+    struct ble_ll_conn_sm *transfer_conn;
+    uint8_t *transfer_received_ev;
+    uint16_t transfer_id;
+    uint16_t event_cntr_last_received;
+    uint8_t adv_addr_rpa[6];
+#endif
+};
+
+static struct ble_ll_sync_sm g_ble_ll_sync_sm[BLE_LL_SYNC_CNT];
+
+static struct {
+    uint8_t adv_sid;
+    uint8_t adv_addr[BLE_DEV_ADDR_LEN];
+    uint8_t adv_addr_type;
+} g_ble_ll_sync_adv_list[BLE_LL_SYNC_LIST_CNT];
+
+static struct {
+    uint32_t timeout;
+    uint16_t max_skip;
+    uint16_t options;
+} g_ble_ll_sync_create_params;
+
+/* if this is set HCI LE Sync Create is pending */
+static uint8_t *g_ble_ll_sync_create_comp_ev;
+
+static struct ble_ll_sync_sm *g_ble_ll_sync_sm_current;
+
+static int ble_ll_sync_event_start_cb(struct ble_ll_sched_item *sch);
+
+static int
+ble_ll_sync_on_list(const uint8_t *addr, uint8_t addr_type, uint8_t sid)
+{
+    int i;
+
+    for (i = 0; i < ARRAY_SIZE(g_ble_ll_sync_adv_list); i++) {
+        if ((g_ble_ll_sync_adv_list[i].adv_sid == sid) &&
+                (g_ble_ll_sync_adv_list[i].adv_addr_type == addr_type) &&
+                !memcmp(g_ble_ll_sync_adv_list[i].adv_addr, addr, BLE_DEV_ADDR_LEN)) {
+            return i;
+        }
+    }
+
+    return -1;
+}
+
+static int
+ble_ll_sync_list_get_free(void)
+{
+    int i;
+
+    for (i = 0; i < ARRAY_SIZE(g_ble_ll_sync_adv_list); i++) {
+        if (g_ble_ll_sync_adv_list[i].adv_sid == 0xff) {
+            return i;
+        }
+    }
+
+    return -1;
+}
+
+static bool
+ble_ll_sync_list_empty(void) {
+    int i;
+
+    for (i = 0; i < ARRAY_SIZE(g_ble_ll_sync_adv_list); i++) {
+        if (g_ble_ll_sync_adv_list[i].adv_sid != 0xff) {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+static uint8_t
+ble_ll_sync_get_handle(struct ble_ll_sync_sm *sm)
+{
+    /* handle number is offset in global array */
+    return sm - g_ble_ll_sync_sm;
+}
+
+static void
+ble_ll_sync_sm_clear(struct ble_ll_sync_sm *sm)
+{
+    if (sm->flags & (BLE_LL_SYNC_SM_FLAG_ESTABLISHING |
+                     BLE_LL_SYNC_SM_FLAG_ESTABLISHED)) {
+        ble_ll_sched_rmv_elem(&sm->sch);
+        ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &sm->sync_ev_end);
+    }
+
+    if (sm->next_report) {
+        ble_hci_trans_buf_free(sm->next_report);
+    }
+
+    if (g_ble_ll_sync_sm_current == sm) {
+        ble_phy_disable();
+        ble_ll_state_set(BLE_LL_STATE_STANDBY);
+        g_ble_ll_sync_sm_current = NULL;
+        ble_ll_scan_chk_resume();
+    }
+
+    ble_ll_rfmgmt_release();
+
+    BLE_LL_ASSERT(sm->sync_ev_end.ev.ev_queued == 0);
+    BLE_LL_ASSERT(sm->sch.enqueued == 0);
+
+    memset(sm, 0, sizeof(*sm));
+
+    sm->sch.sched_cb = ble_ll_sync_event_start_cb;
+    sm->sch.cb_arg = sm;
+    sm->sch.sched_type = BLE_LL_SCHED_TYPE_SYNC;
+}
+
+static uint8_t
+ble_ll_sync_phy_mode_to_hci(int8_t phy_mode)
+{
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    switch (phy_mode) {
+    case BLE_PHY_MODE_1M:
+        return BLE_HCI_LE_PHY_1M;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY)
+    case BLE_PHY_MODE_2M:
+        return BLE_HCI_LE_PHY_2M;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    case BLE_PHY_MODE_CODED_125KBPS:
+    case BLE_PHY_MODE_CODED_500KBPS:
+        return BLE_HCI_LE_PHY_CODED;
+#endif
+    default:
+        BLE_LL_ASSERT(false);
+        return BLE_PHY_MODE_1M;
+    }
+#else
+    return BLE_PHY_MODE_1M;
+#endif
+}
+
+static struct ble_ll_sync_sm *
+ble_ll_sync_find(const uint8_t *addr, uint8_t addr_type, uint8_t sid)
+{
+    struct ble_ll_sync_sm *sm;
+    int i;
+
+    for (i = 0; i < BLE_LL_SYNC_CNT; i++) {
+        sm = &g_ble_ll_sync_sm[i];
+
+        if (!sm->flags) {
+            continue;
+        }
+        if ((sm->adv_sid == sid) && (sm->adv_addr_type == addr_type) &&
+                !memcmp(&sm->adv_addr, addr, BLE_DEV_ADDR_LEN)) {
+            return sm;
+        }
+    }
+
+    return NULL;
+}
+
+static uint16_t
+get_max_skip(uint32_t interval_us, uint32_t timeout_us)
+{
+    uint16_t max_skip;
+
+    BLE_LL_ASSERT(interval_us);
+    BLE_LL_ASSERT(timeout_us);
+
+    if (timeout_us <= interval_us) {
+        return 0;
+    }
+
+    /*
+     * Calculate max allowed skip to receive something before timeout. We adjust
+     * current skip value to be no more than max_skip-6 so we have at least few
+     * attempts to receive an event (so we don't timeout immediately after just
+     * one missed event).
+     */
+
+    max_skip = (timeout_us / interval_us) - 1;
+
+    if (max_skip < 6) {
+        return 0;
+    }
+
+    return max_skip - 6;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+static void
+ble_ll_sync_transfer_received(struct ble_ll_sync_sm *sm, uint8_t status)
+{
+    struct ble_hci_ev_le_subev_periodic_adv_sync_transfer *ev;
+    struct ble_hci_ev *hci_ev;
+
+    BLE_LL_ASSERT(sm->transfer_received_ev);
+
+    if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_TRANSFER)) {
+        hci_ev = (void *) sm->transfer_received_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_PERIODIC_ADV_SYNC_TRANSFER;
+
+        ev->status = status;
+        ev->conn_handle = htole16(sm->transfer_conn->conn_handle);
+        ev->service_data = htole16(sm->transfer_id);
+
+        /* this is ignored by host on error */
+        ev->sync_handle = htole16(ble_ll_sync_get_handle(sm));
+        ev->sid = sm->adv_sid;
+        ev->peer_addr_type = sm->adv_addr_type;
+        if (sm->flags & BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED) {
+            ev->peer_addr_type += 2;
+        }
+        memcpy(ev->peer_addr, sm->adv_addr, BLE_DEV_ADDR_LEN);
+        ev->phy = ble_ll_sync_phy_mode_to_hci(sm->phy_mode);
+        ev->interval = htole16(sm->itvl);
+        ev->aca = sm->sca;
+
+        ble_ll_hci_event_send(hci_ev);
+    } else {
+        ble_hci_trans_buf_free(sm->transfer_received_ev);
+    }
+
+    sm->transfer_received_ev = NULL;
+    sm->transfer_conn = NULL;
+}
+#endif
+
+static void
+ble_ll_sync_est_event_success(struct ble_ll_sync_sm *sm)
+{
+    struct ble_hci_ev_le_subev_periodic_adv_sync_estab *ev;
+    struct ble_hci_ev *hci_ev;
+
+    BLE_LL_ASSERT(g_ble_ll_sync_create_comp_ev);
+
+    if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_ESTAB)) {
+        hci_ev = (void *) g_ble_ll_sync_create_comp_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_PERIODIC_ADV_SYNC_ESTAB;
+        ev->status = BLE_ERR_SUCCESS;
+        ev->sync_handle = htole16(ble_ll_sync_get_handle(sm));
+        ev->sid = sm->adv_sid;
+        ev->peer_addr_type = sm->adv_addr_type;
+        if (sm->flags & BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED) {
+            ev->peer_addr_type += 2;
+        }
+        memcpy(ev->peer_addr, sm->adv_addr, BLE_DEV_ADDR_LEN);
+        ev->phy = ble_ll_sync_phy_mode_to_hci(sm->phy_mode);
+        ev->interval = htole16(sm->itvl);
+        ev->aca = sm->sca;
+
+        ble_ll_hci_event_send(hci_ev);
+    } else {
+        ble_hci_trans_buf_free(g_ble_ll_sync_create_comp_ev);
+    }
+
+    g_ble_ll_sync_create_comp_ev = NULL;
+}
+
+static void
+ble_ll_sync_est_event_failed(uint8_t status)
+{
+    struct ble_hci_ev_le_subev_periodic_adv_sync_estab *ev;
+    struct ble_hci_ev *hci_ev;
+
+    BLE_LL_ASSERT(g_ble_ll_sync_create_comp_ev);
+
+    if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_ESTAB)) {
+        hci_ev = (void *) g_ble_ll_sync_create_comp_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_PERIODIC_ADV_SYNC_ESTAB;
+        ev->status = status;
+
+        ble_ll_hci_event_send(hci_ev);
+    } else {
+        ble_hci_trans_buf_free(g_ble_ll_sync_create_comp_ev);
+    }
+
+    g_ble_ll_sync_create_comp_ev = NULL;
+}
+
+static void
+ble_ll_sync_lost_event(struct ble_ll_sync_sm *sm)
+{
+    struct ble_hci_ev_le_subev_periodic_adv_sync_lost *ev;
+    struct ble_hci_ev *hci_ev;
+
+    if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_LOST)) {
+        hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+        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_PERIODIC_ADV_SYNC_LOST;
+            ev->sync_handle = htole16(ble_ll_sync_get_handle(sm));
+
+            ble_ll_hci_event_send(hci_ev);
+        }
+    }
+}
+
+static void
+ble_ll_sync_current_sm_over(void)
+{
+    /* Disable the PHY */
+    ble_phy_disable();
+
+    /* Link-layer is in standby state now */
+    ble_ll_state_set(BLE_LL_STATE_STANDBY);
+
+    /* Set current LL sync to NULL */
+    g_ble_ll_sync_sm_current = NULL;
+}
+
+static int
+ble_ll_sync_event_start_cb(struct ble_ll_sched_item *sch)
+{
+    struct ble_ll_sync_sm *sm;
+    uint32_t wfr_usecs;
+    uint32_t start;
+    uint8_t chan;
+    int rc;
+
+    /* Set current connection state machine */
+    sm = sch->cb_arg;
+    BLE_LL_ASSERT(sm);
+
+    g_ble_ll_sync_sm_current = sm;
+
+    /* Disable whitelisting */
+    ble_ll_whitelist_disable();
+
+    /* Set LL state */
+    ble_ll_state_set(BLE_LL_STATE_SYNC);
+
+    /* Set channel */
+    chan = sm->flags & BLE_LL_SYNC_SM_FLAG_CHAIN ? sm->chan_chain :
+                                                   sm->chan_index;
+    ble_phy_setchan(chan, sm->access_addr, sm->crcinit);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    ble_phy_resolv_list_disable();
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    ble_phy_encrypt_disable();
+#endif
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    ble_phy_mode_set(sm->phy_mode, sm->phy_mode);
+#endif
+
+    start = sch->start_time + g_ble_ll_sched_offset_ticks;
+    rc = ble_phy_rx_set_start_time(start, sch->remainder);
+    if (rc && rc != BLE_PHY_ERR_RX_LATE) {
+        STATS_INC(ble_ll_stats, sync_event_failed);
+        ble_ll_event_send(&sm->sync_ev_end);
+        ble_ll_sync_current_sm_over();
+        rc = BLE_LL_SCHED_STATE_DONE;
+    } else {
+        if (sm->flags & BLE_LL_SYNC_SM_FLAG_CHAIN) {
+            wfr_usecs = (sm->flags & BLE_LL_SYNC_SM_FLAG_OFFSET_300) ? 300 : 30;
+        } else {
+            /* Set flag that tells to set last anchor point if a packet
+             * has been received.
+             */
+            sm->flags |= BLE_LL_SYNC_SM_FLAG_SET_ANCHOR;
+
+            /* Set WFR timer.
+             * If establishing we always adjust with offset unit.
+             * If this is first packet of sync (one that was pointed by from
+             * SyncInfo we don't adjust WFR with window widening.
+             */
+            if (sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) {
+                wfr_usecs = (sm->flags & BLE_LL_SYNC_SM_FLAG_OFFSET_300) ? 300
+                                                                         : 30;
+                if (!(sm->flags & BLE_LL_SYNC_SM_FLAG_SYNC_INFO)) {
+                    wfr_usecs += 2 * sm->window_widening;
+                }
+            } else {
+                wfr_usecs = 2 * sm->window_widening;
+            }
+        }
+
+        ble_phy_wfr_enable(BLE_PHY_WFR_ENABLE_RX, 0, wfr_usecs);
+        rc = BLE_LL_SCHED_STATE_RUNNING;
+    }
+
+    sm->flags &= ~BLE_LL_SYNC_SM_FLAG_SYNC_INFO;
+
+    return rc;
+}
+
+/**
+ * Called when a receive PDU has started.
+ *
+ * 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_sync_rx_isr_start(uint8_t pdu_type, struct ble_mbuf_hdr *rxhdr)
+{
+    BLE_LL_ASSERT(g_ble_ll_sync_sm_current);
+
+    /* this also handles chains as those have same PDU type */
+    if (pdu_type != BLE_ADV_PDU_TYPE_AUX_SYNC_IND) {
+        ble_ll_event_send(&g_ble_ll_sync_sm_current->sync_ev_end);
+        ble_ll_sync_current_sm_over();
+        STATS_INC(ble_ll_stats, sched_invalid_pdu);
+        return -1;
+    }
+
+    STATS_INC(ble_ll_stats, sync_received);
+    return 0;
+}
+
+static int
+ble_ll_sync_parse_ext_hdr(struct os_mbuf *om, uint8_t **aux, int8_t *tx_power,
+                          uint8_t **acad, uint8_t *acad_len)
+{
+    uint8_t *rxbuf = om->om_data;
+    uint8_t ext_hdr_flags;
+    uint8_t ext_hdr_len;
+    uint8_t *ext_hdr;
+    uint8_t pdu_len;
+    int i;
+
+    pdu_len = rxbuf[1];
+    if (pdu_len == 0) {
+        return -1;
+    }
+    ext_hdr_len = rxbuf[2] & 0x3F;
+    if (ext_hdr_len > (pdu_len - 1)) {
+        return -1;
+    }
+
+    if (ext_hdr_len) {
+        ext_hdr_flags = rxbuf[3];
+        ext_hdr = &rxbuf[4];
+
+        i = 0;
+
+        /* there should be no AdvA in Sync or chain, skip it */
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_ADVA_BIT)) {
+            i += BLE_LL_EXT_ADV_ADVA_SIZE;
+        }
+
+        /* there should be no TargetA in Sync or chain, skip it */
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_TARGETA_BIT)) {
+            i += BLE_LL_EXT_ADV_TARGETA_SIZE;
+        }
+
+        /* Ignore CTE for now */
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_CTE_INFO_BIT)) {
+            i += 1;
+        }
+
+        /* there should be no ADI in Sync or chain, skip it */
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)) {
+            i += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+        }
+
+        /* get AuXPTR if present */
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) {
+            *aux = ext_hdr + i;
+            i += BLE_LL_EXT_ADV_AUX_PTR_SIZE;
+        }
+
+        /* there should be no SyncInfo in Sync or chain, skip it */
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_SYNC_INFO_BIT)) {
+            i += BLE_LL_EXT_ADV_SYNC_INFO_SIZE;
+        }
+
+        if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_TX_POWER_BIT)) {
+            *tx_power = *(ext_hdr + i);
+            i += BLE_LL_EXT_ADV_TX_POWER_SIZE;
+        }
+
+        /* sanity check */
+        if (i > ext_hdr_len) {
+            return -1;
+        }
+
+        /* ACAD */
+        if (ext_hdr_len > (i + 1)) {
+            *acad = ext_hdr + i;
+            *acad_len = ext_hdr_len - i - 1;
+        }
+
+    }
+
+    return pdu_len - ext_hdr_len - 1;
+}
+
+static void
+ble_ll_sync_adjust_ext_hdr(struct os_mbuf *om)
+{
+    uint8_t *rxbuf = om->om_data;
+    uint8_t ext_hdr_len;
+
+    /* this was already verified in ble_ll_sync_parse_ext_hdr() */
+    ext_hdr_len = rxbuf[2] & 0x3F;
+
+    os_mbuf_adj(om, 3 + ext_hdr_len);
+}
+
+static void
+ble_ll_sync_send_truncated_per_adv_rpt(struct ble_ll_sync_sm *sm, uint8_t *evbuf)
+{
+    struct ble_hci_ev_le_subev_periodic_adv_rpt *ev;
+    struct ble_hci_ev *hci_ev;
+
+    if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PERIODIC_ADV_RPT) ||
+        (sm->flags & BLE_LL_SYNC_SM_FLAG_DISABLED)) {
+        ble_hci_trans_buf_free(evbuf);
+        return;
+    }
+
+    hci_ev = (void *) evbuf;
+
+    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_PERIODIC_ADV_RPT;
+    ev->sync_handle = htole16(ble_ll_sync_get_handle(sm));
+    ev->tx_power = 127; /* not available */
+    ev->rssi = 127; /* not available */
+    ev->cte_type = 0xff;
+    ev->data_status = BLE_HCI_PERIODIC_DATA_STATUS_TRUNCATED;
+    ev->data_len = 0;
+
+    ble_ll_hci_event_send(hci_ev);
+}
+
+static void
+ble_ll_sync_send_per_adv_rpt(struct ble_ll_sync_sm *sm, struct os_mbuf *rxpdu,
+                             int8_t rssi, int8_t tx_power, int datalen,
+                             uint8_t *aux, bool aux_scheduled)
+{
+    struct ble_hci_ev_le_subev_periodic_adv_rpt *ev;
+    struct ble_hci_ev *hci_ev;
+    struct ble_hci_ev *hci_ev_next = NULL;
+    uint8_t max_data_len;
+    int offset;
+
+    /* use next report buffer if present, this means we are chaining */
+    if (sm->next_report) {
+        hci_ev = (void *) sm->next_report;
+        sm->next_report = NULL;
+    } else {
+        hci_ev = (void * )ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO);
+        if (!hci_ev) {
+            goto done;
+        }
+    }
+
+    max_data_len = BLE_LL_MAX_EVT_LEN - sizeof(*hci_ev) - sizeof(*ev);
+    offset = 0;
+
+    do {
+        if (hci_ev_next) {
+            hci_ev = hci_ev_next;
+            hci_ev_next = NULL;
+        }
+
+        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_PERIODIC_ADV_RPT;
+        ev->sync_handle = htole16(ble_ll_sync_get_handle(sm));
+        ev->tx_power = tx_power;
+        ev->rssi = rssi;
+        ev->cte_type = 0xff;
+
+        ev->data_len = min(max_data_len, datalen - offset);
+        /* adjust event length */
+        hci_ev->length += ev->data_len;
+
+        os_mbuf_copydata(rxpdu, offset, ev->data_len, ev->data);
+        offset += ev->data_len;
+
+        /* Need another event for next fragment of this PDU */
+        if (offset < datalen) {
+            hci_ev_next = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO);
+            if (hci_ev_next) {
+                ev->data_status = BLE_HCI_PERIODIC_DATA_STATUS_INCOMPLETE;
+            } else {
+                ev->data_status = BLE_HCI_PERIODIC_DATA_STATUS_TRUNCATED;
+            }
+        } else {
+            /* last report of this PDU */
+            if (aux) {
+                if (aux_scheduled) {
+                    /* if we scheduled aux, we need buffer for next report */
+                    hci_ev_next = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO);
+                    if (hci_ev_next) {
+                        ev->data_status = BLE_HCI_PERIODIC_DATA_STATUS_INCOMPLETE;
+                    } else {
+                        ev->data_status = BLE_HCI_PERIODIC_DATA_STATUS_TRUNCATED;
+                    }
+                } else {
+                    ev->data_status = BLE_HCI_PERIODIC_DATA_STATUS_TRUNCATED;
+                }
+            } else {
+                ev->data_status = BLE_HCI_PERIODIC_DATA_STATUS_COMPLETE;
+            }
+        }
+        ble_ll_hci_event_send(hci_ev);
+    } while ((offset < datalen) && hci_ev_next);
+
+done:
+    /* this means that we already truncated data (or didn't sent first at all)
+     * in HCI report but has scheduled for next PDU in chain. In that case mark
+     * it so that we end event properly when next PDU is received.
+     * */
+    if (aux_scheduled && !hci_ev_next) {
+        sm->flags |= BLE_LL_SYNC_SM_FLAG_HCI_TRUNCATED;
+    }
+
+    /* store for chain */
+    sm->next_report = (void *) hci_ev_next;
+}
+
+/**
+ * Called when a receive PDU has ended.
+ *
+ * Context: Interrupt
+ *
+ * @param rxpdu
+ *
+ * @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_sync_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr)
+{
+    struct ble_mbuf_hdr *ble_hdr;
+    struct os_mbuf *rxpdu;
+
+    BLE_LL_ASSERT(g_ble_ll_sync_sm_current);
+
+    /* type was verified in isr_start */
+
+    rxpdu = ble_ll_rxpdu_alloc(rxbuf[1] + BLE_LL_PDU_HDR_LEN);
+    if (rxpdu) {
+        ble_phy_rxpdu_copy(rxbuf, rxpdu);
+
+        ble_hdr = BLE_MBUF_HDR_PTR(rxpdu);
+        ble_hdr->rxinfo.user_data = g_ble_ll_sync_sm_current;
+
+        ble_ll_rx_pdu_in(rxpdu);
+    } else {
+        STATS_INC(ble_ll_stats, sync_rx_buf_err);
+        ble_ll_event_send(&g_ble_ll_sync_sm_current->sync_ev_end);
+    }
+
+    /* PHY is disabled here */
+    ble_ll_sync_current_sm_over();
+
+    return 1;
+}
+
+/**
+ * Called when the wait for response timer expires while in the sync state.
+ *
+ * Context: Interrupt.
+ */
+void
+ble_ll_sync_wfr_timer_exp(void)
+{
+    struct ble_ll_sync_sm *sm = g_ble_ll_sync_sm_current;
+
+    BLE_LL_ASSERT(g_ble_ll_sync_sm_current);
+    STATS_INC(ble_ll_stats, sync_missed_err);
+
+    ble_ll_sync_current_sm_over();
+    ble_ll_event_send(&sm->sync_ev_end);
+}
+
+/**
+ * Called when sync 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_sync_halt(void)
+{
+    struct ble_ll_sync_sm *sm = g_ble_ll_sync_sm_current;
+
+    ble_ll_sync_current_sm_over();
+
+    if (sm) {
+        ble_ll_event_send(&sm->sync_ev_end);
+    }
+}
+
+uint32_t
+ble_ll_sync_get_event_end_time(void)
+{
+    uint32_t end_time;
+
+    if (g_ble_ll_sync_sm_current) {
+        end_time = g_ble_ll_sync_sm_current->sch.end_time;
+    } else {
+        end_time = os_cputime_get32();
+    }
+    return end_time;
+}
+
+static uint8_t
+ble_ll_sync_phy_mode_to_aux_phy(uint8_t phy_mode)
+{
+    switch (phy_mode) {
+    case BLE_PHY_MODE_1M:
+        return 0x00;
+    case BLE_PHY_MODE_2M:
+        return 0x01;
+    case BLE_PHY_MODE_CODED_125KBPS:
+    case BLE_PHY_MODE_CODED_500KBPS:
+        return 0x02;
+    default:
+        BLE_LL_ASSERT(false);
+        return 0x00;
+    }
+}
+
+static void
+ble_ll_sync_parse_aux_ptr(const uint8_t *buf, uint8_t *chan, uint32_t *offset,
+                          uint8_t *offset_units, uint8_t *phy)
+{
+    uint32_t aux_ptr_field = get_le32(buf) & 0x00FFFFFF;
+
+    *chan = aux_ptr_field & 0x3F;
+
+    /* TODO use CA aux_ptr_field >> 6 */
+
+    if ((aux_ptr_field >> 7) & 0x01) {
+        *offset = 300 * ((aux_ptr_field >> 8) & 0x1FFF);
+        *offset_units = 1;
+    } else {
+        *offset = 30 * ((aux_ptr_field >> 8) & 0x1FFF);
+        *offset_units = 0;
+    }
+
+    *phy = (aux_ptr_field >> 21) & 0x07;
+}
+
+static int
+ble_ll_sync_schedule_chain(struct ble_ll_sync_sm *sm, struct ble_mbuf_hdr *hdr,
+                           const uint8_t *aux)
+{
+    uint8_t offset_units;
+    uint32_t offset;
+    uint8_t chan;
+    uint8_t phy;
+
+    ble_ll_sync_parse_aux_ptr(aux, &chan, &offset, &offset_units, &phy);
+
+    if (chan >= BLE_PHY_NUM_DATA_CHANS) {
+        return -1;
+    }
+
+    if (offset < BLE_LL_MAFS) {
+        return -1;
+    }
+
+    /* chain should use same PHY as master PDU */
+    if (phy != ble_ll_sync_phy_mode_to_aux_phy(sm->phy_mode)) {
+        return -1;
+    }
+
+    if (offset_units) {
+        sm->flags |= BLE_LL_SYNC_SM_FLAG_OFFSET_300;
+    } else {
+        sm->flags &= ~BLE_LL_SYNC_SM_FLAG_OFFSET_300;
+    }
+
+    sm->chan_chain = chan;
+
+    sm->flags |= BLE_LL_SYNC_SM_FLAG_CHAIN;
+
+    return ble_ll_sched_sync(&sm->sch, hdr->beg_cputime, hdr->rem_usecs,
+                             offset, sm->phy_mode);
+}
+
+static void
+ble_ll_sync_established(struct ble_ll_sync_sm *sm)
+{
+    BLE_LL_ASSERT(sm->sync_pending_cnt);
+
+    /* mark as established */
+
+    sm->flags |= BLE_LL_SYNC_SM_FLAG_ESTABLISHED;
+    sm->flags &= ~BLE_LL_SYNC_SM_FLAG_ESTABLISHING;
+
+    sm->sync_pending_cnt = 0;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+    if (sm->transfer_conn) {
+        ble_ll_sync_transfer_received(sm, BLE_ERR_SUCCESS);
+        return;
+    }
+#endif
+
+    ble_ll_sync_est_event_success(sm);
+}
+
+static void
+ble_ll_sync_check_failed(struct ble_ll_sync_sm *sm)
+{
+    BLE_LL_ASSERT(sm->sync_pending_cnt);
+
+    /* if we can retry on next event */
+    if (--sm->sync_pending_cnt) {
+        return;
+    }
+
+    sm->flags &= ~BLE_LL_SYNC_SM_FLAG_ESTABLISHING;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+    if (sm->transfer_conn) {
+        ble_ll_sync_transfer_received(sm, BLE_ERR_CONN_ESTABLISHMENT);
+        return;
+    }
+#endif
+
+    ble_ll_sync_est_event_failed(BLE_ERR_CONN_ESTABLISHMENT);
+}
+
+static bool
+ble_ll_sync_check_acad(struct ble_ll_sync_sm *sm,
+                       const uint8_t *acad, uint8_t acad_len)
+{
+    const struct ble_ll_acad_channel_map_update_ind *chmu;
+    unsigned int ad_len;
+    uint8_t ad_type;
+
+    /* assume no empty fields */
+    while (acad_len > 2) {
+        ad_len = acad[0];
+        ad_type = acad[1];
+
+        /* early termination should not happen in ACAD */
+        if (ad_len == 0) {
+            return false;
+        }
+
+        /* check if not passing pass acad data */
+        if (ad_len + 1 > acad_len) {
+            return false;
+        }
+
+        switch (ad_type) {
+        case BLE_LL_ACAD_CHANNEL_MAP_UPDATE_IND:
+            chmu = (const void *)&acad[2];
+
+            if (ad_len - 1 != sizeof(*chmu)) {
+                return false;
+            }
+
+            /* Channels Mask (37 bits)
+             * TODO should we check this?
+             */
+            sm->chanmap_new[0] = chmu->map[0];
+            sm->chanmap_new[1] = chmu->map[1];
+            sm->chanmap_new[2] = chmu->map[2];
+            sm->chanmap_new[3] = chmu->map[3];
+            sm->chanmap_new[4] = chmu->map[4] & 0x1f;
+
+            /* drop if channel map is invalid */
+            if (ble_ll_utils_calc_num_used_chans(sm->chanmap_new) == 0) {
+                return false;
+            }
+
+            sm->chanmap_new_instant = le16toh(chmu->instant);
+            sm->flags |= BLE_LL_SYNC_SM_FLAG_NEW_CHANMAP;
+            break;
+        default:
+            break;
+        }
+
+        acad += ad_len + 1;
+        acad_len -= ad_len + 1;
+    }
+
+    /* should have no trailing zeros */
+    if (acad_len) {
+        return false;
+    }
+
+    return true;
+}
+
+void
+ble_ll_sync_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr)
+{
+    struct ble_ll_sync_sm *sm = hdr->rxinfo.user_data;
+    bool aux_scheduled = false;
+    int8_t tx_power = 127; /* defaults to not available */
+    uint8_t *aux = NULL;
+    uint8_t *acad = NULL;
+    uint8_t acad_len;
+    int datalen;
+    bool reports_enabled;
+
+    BLE_LL_ASSERT(sm);
+
+    /* this could happen if sync was cancelled or terminated while pkt_in was
+     * already in LL queue, just drop in that case
+     */
+    if (!sm->flags) {
+        ble_ll_scan_chk_resume();
+        ble_ll_rfmgmt_release();
+        return;
+    }
+
+    /* Set anchor point (and last) if 1st rxd frame in sync event.
+     * According to spec this should be done even if CRC is not valid so we
+     * can store it here
+     */
+    if (sm->flags & BLE_LL_SYNC_SM_FLAG_SET_ANCHOR) {
+        sm->flags &= ~BLE_LL_SYNC_SM_FLAG_SET_ANCHOR;
+
+        sm->anchor_point = hdr->beg_cputime;
+        sm->anchor_point_usecs = hdr->rem_usecs;
+        sm->last_anchor_point = sm->anchor_point;
+    }
+
+    /* CRC error, end event */
+    if (!BLE_MBUF_HDR_CRC_OK(hdr)) {
+        STATS_INC(ble_ll_stats, sync_crc_err);
+        goto end_event;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+    /* save last pa counter */
+    sm->event_cntr_last_received = sm->event_cntr;
+#endif
+
+    /* this means we are chaining but due to low buffers already sent data
+     * truncated report to host (or didn't sent any at all). If this happens
+     * next_buf should be already set to NULL and we just end event.
+     */
+    if (sm->flags & BLE_LL_SYNC_SM_FLAG_HCI_TRUNCATED) {
+        BLE_LL_ASSERT(!sm->next_report);
+        goto end_event;
+    }
+
+    /* get ext header data */
+    datalen = ble_ll_sync_parse_ext_hdr(rxpdu, &aux, &tx_power, &acad, &acad_len);
+    if (datalen < 0) {
+        /* we got bad packet, end event */
+        goto end_event;
+    }
+
+    reports_enabled = ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PERIODIC_ADV_RPT) &&
+                      !(sm->flags & BLE_LL_SYNC_SM_FLAG_DISABLED);
+
+    /* no need to schedule for chain if reporting is disabled */
+    if (reports_enabled) {
+        /* if aux is present, we need to schedule ASAP */
+        if (aux && (ble_ll_sync_schedule_chain(sm, hdr, aux) == 0)) {
+            aux_scheduled = true;
+        }
+    }
+
+    /* check ACAD, needs to be done before rxpdu is adjusted for ADV data  */
+    if (acad && !ble_ll_sync_check_acad(sm, acad, acad_len)) {
+        /* we got bad packet (bad ACAD data), end event */
+        goto end_event;
+    }
+
+    /* we need to establish link even if reporting was disabled */
+    if (sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) {
+        ble_ll_sync_established(sm);
+    }
+
+    /* only if reporting is enabled */
+    if (reports_enabled) {
+        /* Adjust rxpdu to contain advertising data only */
+        ble_ll_sync_adjust_ext_hdr(rxpdu);
+
+        /* send reports from this PDU */
+        ble_ll_sync_send_per_adv_rpt(sm, rxpdu, hdr->rxinfo.rssi, tx_power,
+                                     datalen, aux, aux_scheduled);
+    }
+
+    /* if chain was scheduled we don't end event yet */
+    /* TODO should we check resume only if offset is high? */
+    if (aux_scheduled) {
+        ble_ll_scan_chk_resume();
+        ble_ll_rfmgmt_release();
+        return;
+    }
+
+end_event:
+    ble_ll_event_send(&sm->sync_ev_end);
+    ble_ll_rfmgmt_release();
+}
+
+static int
+ble_ll_sync_next_event(struct ble_ll_sync_sm *sm, uint32_t cur_ww_adjust)
+{
+    uint32_t cur_ww;
+    uint32_t max_ww;
+    uint32_t ticks;
+    uint32_t itvl;
+    uint8_t usecs;
+    uint16_t skip = sm->skip;
+
+    /* don't skip if are establishing sync or we missed last event */
+    if (skip && ((sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) ||
+                  CPUTIME_LT(sm->last_anchor_point, sm->anchor_point))) {
+        skip = 0;
+    }
+
+    /* Set next event start time, we can use pre-calculated values for one
+     * interval if not skipping
+     */
+    if (skip == 0) {
+        ticks = sm->itvl_ticks;
+        usecs = sm->itvl_usecs;
+    } else {
+        itvl = sm->itvl * BLE_LL_SYNC_ITVL_USECS * (1 + skip);
+        ticks = os_cputime_usecs_to_ticks(itvl);
+        usecs = itvl - os_cputime_ticks_to_usecs(ticks);
+    }
+
+    sm->anchor_point += ticks;
+    sm->anchor_point_usecs += usecs;
+    if (sm->anchor_point_usecs >= 31) {
+        sm->anchor_point++;
+        sm->anchor_point_usecs -= 31;
+    }
+
+    /* Set event counter to the next event */
+    sm->event_cntr += 1 + skip;
+
+    /* update channel map if needed */
+    if (sm->flags & BLE_LL_SYNC_SM_FLAG_NEW_CHANMAP) {
+        if (((int16_t)(sm->event_cntr - sm->chanmap_new_instant)) >= 0) {
+            /* map was verified on reception */
+            sm->chanmap[0] = sm->chanmap_new[0];
+            sm->chanmap[1] = sm->chanmap_new[1];
+            sm->chanmap[2] = sm->chanmap_new[2];
+            sm->chanmap[3] = sm->chanmap_new[3];
+            sm->chanmap[4] = sm->chanmap_new[4];
+            sm->num_used_chans = ble_ll_utils_calc_num_used_chans(sm->chanmap);
+            sm->flags &= ~BLE_LL_SYNC_SM_FLAG_NEW_CHANMAP;
+        }
+    }
+
+    /* Calculate channel index of next event */
+    sm->chan_index = ble_ll_utils_calc_dci_csa2(sm->event_cntr, sm->channel_id,
+                                                sm->num_used_chans, sm->chanmap);
+
+    cur_ww = ble_ll_utils_calc_window_widening(sm->anchor_point,
+                                               sm->last_anchor_point,
+                                               sm->sca);
+
+    cur_ww += cur_ww_adjust;
+
+    max_ww = (sm->itvl * (BLE_LL_SYNC_ITVL_USECS / 2)) - BLE_LL_IFS;
+    if (cur_ww >= max_ww) {
+        return -1;
+    }
+
+    cur_ww += BLE_LL_JITTER_USECS;
+
+    /* if updated anchor is pass last anchor + timeout it means we will not be
+     * able to get it in time and hit sync timeout
+     *
+     * note that this may result in sync timeout being sent before real
+     * timeout but we won't be able to fit in time anyway..
+     *
+     * We don't do that when establishing since we try up to
+     * BLE_LL_SYNC_ESTABLISH_CNT events before failing regardless of timeout
+     */
+    if (!(sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING)) {
+        if (CPUTIME_GT(sm->anchor_point - os_cputime_usecs_to_ticks(cur_ww),
+                       sm->last_anchor_point + sm->timeout )) {
+            return -1;
+        }
+    }
+
+    sm->window_widening = cur_ww;
+
+    return 0;
+}
+
+static void
+ble_ll_sync_event_end(struct ble_npl_event *ev)
+{
+    struct ble_ll_sync_sm *sm;
+
+    /* Better be a connection state machine! */
+    sm = ble_npl_event_get_arg(ev);
+    BLE_LL_ASSERT(sm);
+
+    ble_ll_rfmgmt_release();
+
+    if (sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) {
+        ble_ll_sync_check_failed(sm);
+    }
+
+    /* Check if we need to resume scanning */
+    ble_ll_scan_chk_resume();
+
+    /* Remove any end events that might be enqueued */
+    ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &sm->sync_ev_end);
+
+    /* don't schedule next event if sync is not established nor establishing
+     * at this point SM is no longer valid
+     */
+    if (!(sm->flags & (BLE_LL_SYNC_SM_FLAG_ESTABLISHED |
+                       BLE_LL_SYNC_SM_FLAG_ESTABLISHING))) {
+        ble_ll_sync_sm_clear(sm);
+        return;
+    }
+
+    /* if we had prepared buffer for next even it means we were chaining and
+     * must send truncated report to host
+     */
+    if (sm->next_report) {
+        BLE_LL_ASSERT(!(sm->flags & BLE_LL_SYNC_SM_FLAG_HCI_TRUNCATED));
+        ble_ll_sync_send_truncated_per_adv_rpt(sm, sm->next_report);
+        sm->next_report = NULL;
+    }
+
+    /* Event ended so we are no longer chaining */
+    sm->flags &= ~BLE_LL_SYNC_SM_FLAG_HCI_TRUNCATED;
+    sm->flags &= ~BLE_LL_SYNC_SM_FLAG_CHAIN;
+
+    do {
+        if (ble_ll_sync_next_event(sm, 0) < 0) {
+            if (sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) {
+                /* don't allow any retry if this failed */
+                sm->sync_pending_cnt = 1;
+                ble_ll_sync_check_failed(sm);
+            } else {
+                ble_ll_sync_lost_event(sm);
+            }
+
+            /* at this point SM is no longer valid */
+            ble_ll_sync_sm_clear(sm);
+            return;
+        }
+    } while (ble_ll_sched_sync_reschedule(&sm->sch, sm->anchor_point,
+                                          sm->anchor_point_usecs,
+                                          sm->window_widening, sm->phy_mode));
+}
+
+void
+ble_ll_sync_info_event(struct ble_ll_scan_addr_data *addrd,
+                       uint8_t sid, struct ble_mbuf_hdr *rxhdr,
+                       const uint8_t *syncinfo)
+{
+    struct ble_ll_sync_sm *sm = NULL;
+    uint16_t max_skip;
+    uint32_t offset;
+    uint32_t usecs;
+    uint16_t itvl;
+    int i;
+
+    /* ignore if not synchronizing */
+    if (!g_ble_ll_sync_create_comp_ev) {
+        return;
+    }
+
+    /* get reserved SM */
+    for (i = 0; i < BLE_LL_SYNC_CNT; i++) {
+        if (g_ble_ll_sync_sm[i].flags & BLE_LL_SYNC_SM_FLAG_RESERVED) {
+            sm = &g_ble_ll_sync_sm[i];
+            break;
+        }
+    }
+
+    /* this means we already got sync info event and pending sync */
+    if (!sm) {
+        return;
+    }
+
+    /* check peer */
+    if (g_ble_ll_sync_create_params.options & BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_FILTER) {
+        if (ble_ll_sync_on_list(addrd->adv_addr,
+                                addrd->adv_addr_type, sid) < 0) {
+            return;
+        }
+
+        /* set addr and sid in sm */
+        sm->adv_sid = sid;
+        sm->adv_addr_type = addrd->adv_addr_type;
+        memcpy(sm->adv_addr, addrd->adv_addr, BLE_DEV_ADDR_LEN);
+    } else {
+        if ((sm->adv_sid != sid) ||
+                (sm->adv_addr_type != addrd->adv_addr_type) ||
+                memcmp(sm->adv_addr, addrd->adv_addr, BLE_DEV_ADDR_LEN)) {
+            return;
+        }
+    }
+
+    /* Sync Packet Offset (13 bits), Offset Units (1 bit), RFU (2 bits) */
+    offset = syncinfo[0];
+    offset |= (uint16_t)(syncinfo[1] & 0x1f) << 8;
+
+    /* ignore if offset is not valid */
+    if (!offset) {
+        return;
+    }
+
+    /* Interval (2 bytes), ignore if invalid */
+    itvl = get_le16(&syncinfo[2]);
+    if (itvl < 6) {
+        return;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    if (addrd->adva_resolved) {
+        sm->flags |= BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+        memcpy(sm->adv_addr_rpa, addrd->adva, BLE_DEV_ADDR_LEN);
+#endif
+    }
+#endif
+
+    /* set params from HCI LE Create Periodic Sync */
+    sm->timeout = g_ble_ll_sync_create_params.timeout;
+    sm->skip = g_ble_ll_sync_create_params.max_skip;
+    sm->sync_pending_cnt = BLE_LL_SYNC_ESTABLISH_CNT;
+
+    if (syncinfo[1] & 0x20) {
+        offset *= 300;
+        sm->flags |= BLE_LL_SYNC_SM_FLAG_OFFSET_300;
+    } else {
+        offset *= 30;
+        sm->flags &= ~BLE_LL_SYNC_SM_FLAG_OFFSET_300;
+    }
+
+    /* sync end event */
+    ble_npl_event_init(&sm->sync_ev_end, ble_ll_sync_event_end, sm);
+
+    sm->itvl = itvl;
+
+    /* precalculate interval ticks and usecs */
+    usecs = sm->itvl * BLE_LL_SYNC_ITVL_USECS;
+    sm->itvl_ticks = os_cputime_usecs_to_ticks(usecs);
+    sm->itvl_usecs = (uint8_t)(usecs -
+                               os_cputime_ticks_to_usecs(sm->itvl_ticks));
+    if (sm->itvl_usecs == 31) {
+        sm->itvl_usecs = 0;
+        sm->itvl_ticks++;
+    }
+
+    /* Channels Mask (37 bits) */
+    sm->chanmap[0] = syncinfo[4];
+    sm->chanmap[1] = syncinfo[5];
+    sm->chanmap[2] = syncinfo[6];
+    sm->chanmap[3] = syncinfo[7];
+    sm->chanmap[4] = syncinfo[8] & 0x1f;
+    sm->num_used_chans = ble_ll_utils_calc_num_used_chans(sm->chanmap);
+
+    /* SCA (3 bits) */
+    sm->sca = syncinfo[8] >> 5;
+
+    /* AA (4 bytes) */
+    sm->access_addr = get_le32(&syncinfo[9]);
+    sm->channel_id = ((sm->access_addr & 0xffff0000) >> 16) ^
+                      (sm->access_addr & 0x0000ffff);
+
+    /* CRCInit (3 bytes) */
+    sm->crcinit = syncinfo[15];
+    sm->crcinit = (sm->crcinit << 8) | syncinfo[14];
+    sm->crcinit = (sm->crcinit << 8) | syncinfo[13];
+
+    /* Event Counter (2 bytes) */
+    sm->event_cntr = get_le16(&syncinfo[16]);
+
+    /* adjust skip if pass timeout */
+    max_skip = get_max_skip(sm->itvl * BLE_LL_SYNC_ITVL_USECS, sm->timeout);
+    if (sm->skip > max_skip) {
+        sm->skip = max_skip;
+    }
+
+    /* from now on we only need timeout in ticks */
+    sm->timeout = os_cputime_usecs_to_ticks(sm->timeout);
+
+    sm->phy_mode = rxhdr->rxinfo.phy_mode;
+    sm->window_widening = BLE_LL_JITTER_USECS;
+
+    /* Calculate channel index of first event */
+    sm->chan_index = ble_ll_utils_calc_dci_csa2(sm->event_cntr, sm->channel_id,
+                                                sm->num_used_chans, sm->chanmap);
+
+    if (ble_ll_sched_sync(&sm->sch, rxhdr->beg_cputime, rxhdr->rem_usecs,
+                          offset, sm->phy_mode)) {
+        return;
+    }
+
+    sm->anchor_point = sm->sch.start_time + g_ble_ll_sched_offset_ticks;
+    sm->anchor_point_usecs = sm->sch.remainder;
+    sm->last_anchor_point = sm->anchor_point;
+
+#if MYNEWT_VAL(BLE_VERSION) >= 51
+    if (g_ble_ll_sync_create_params.options & BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_DISABLED) {
+        sm->flags |= BLE_LL_SYNC_SM_FLAG_DISABLED;
+    }
+#endif
+
+    sm->flags &= ~BLE_LL_SYNC_SM_FLAG_RESERVED;
+    sm->flags |= BLE_LL_SYNC_SM_FLAG_ESTABLISHING;
+    sm->flags |= BLE_LL_SYNC_SM_FLAG_SYNC_INFO;
+}
+
+static struct ble_ll_sync_sm *
+ble_ll_sync_reserve(void)
+{
+    struct ble_ll_sync_sm *sm;
+    int i;
+
+    for (i = 0; i < BLE_LL_SYNC_CNT; i++) {
+        sm = &g_ble_ll_sync_sm[i];
+
+        if (!sm->flags) {
+            sm->flags |= BLE_LL_SYNC_SM_FLAG_RESERVED;
+            return sm;
+        }
+    }
+
+    return NULL;
+}
+
+int
+ble_ll_sync_create(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_periodic_adv_create_sync_cp *cmd = (const void *) cmdbuf;
+    struct ble_ll_sync_sm *sm;
+    uint16_t timeout;
+    os_sr_t sr;
+
+    if (g_ble_ll_sync_create_comp_ev) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+#if MYNEWT_VAL(BLE_VERSION) >= 51
+    if (cmd->options > BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_DISABLED) {
+#else
+    if (cmd->options > BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_FILTER) {
+#endif
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (cmd->skip > 0x01f3) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    timeout = le16toh(cmd->sync_timeout);
+    if (timeout < 0x000a || timeout > 0x4000) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+#if MYNEWT_VAL(BLE_VERSION) >= 51
+    /* we don't support any CTE yet */
+    if (cmd->sync_cte_type) {
+        if (cmd->sync_cte_type > 4) {
+           return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        return BLE_ERR_UNSUPPORTED;
+    }
+#endif
+
+    if (cmd->options & BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_FILTER) {
+        if (ble_ll_sync_list_empty()) {
+            return BLE_ERR_CMD_DISALLOWED;
+        }
+    } else {
+        if (cmd->sid > 0x0f) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        if (cmd->peer_addr_type > BLE_HCI_ADV_PEER_ADDR_MAX) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
+        OS_ENTER_CRITICAL(sr);
+        sm = ble_ll_sync_find(cmd->peer_addr, cmd->peer_addr_type, cmd->sid);
+        OS_EXIT_CRITICAL(sr);
+
+        if (sm) {
+            return BLE_ERR_ACL_CONN_EXISTS;
+        }
+    }
+
+    /* reserve buffer for sync complete event */
+    g_ble_ll_sync_create_comp_ev = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+    if (!g_ble_ll_sync_create_comp_ev) {
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    OS_ENTER_CRITICAL(sr);
+
+    /* reserve 1 SM for created sync */
+    sm = ble_ll_sync_reserve();
+    if (!sm) {
+        ble_hci_trans_buf_free(g_ble_ll_sync_create_comp_ev);
+        g_ble_ll_sync_create_comp_ev = NULL;
+        OS_EXIT_CRITICAL(sr);
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    /* if we don't use list, store expected address in reserved SM */
+    if (!(cmd->options & BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_FILTER)) {
+        sm->adv_sid = cmd->sid;
+        sm->adv_addr_type = cmd->peer_addr_type;
+        memcpy(&sm->adv_addr, cmd->peer_addr, BLE_DEV_ADDR_LEN);
+    }
+
+    g_ble_ll_sync_create_params.timeout = timeout * 10000; /* 10ms units, store in us */;
+    g_ble_ll_sync_create_params.max_skip = cmd->skip;
+    g_ble_ll_sync_create_params.options = cmd->options;
+
+    OS_EXIT_CRITICAL(sr);
+    return BLE_ERR_SUCCESS;
+}
+
+static void
+ble_ll_sync_cancel_complete_event(void)
+{
+    ble_ll_sync_est_event_failed(BLE_ERR_OPERATION_CANCELLED);
+}
+
+int
+ble_ll_sync_cancel(ble_ll_hci_post_cmd_complete_cb *post_cmd_cb)
+{
+    struct ble_ll_sync_sm *sm;
+    os_sr_t sr;
+    int i;
+
+    if (!g_ble_ll_sync_create_comp_ev) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    OS_ENTER_CRITICAL(sr);
+
+    for (i = 0; i < BLE_LL_SYNC_CNT; i++) {
+        sm = &g_ble_ll_sync_sm[i];
+
+        /* cancelled before fist sync info packet */
+        if (sm->flags & BLE_LL_SYNC_SM_FLAG_RESERVED) {
+            memset(sm, 0, sizeof(*sm));
+            break;
+        }
+
+        /* cancelled while pending sync */
+        if (sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) {
+            ble_ll_sync_sm_clear(sm);
+            break;
+        }
+    }
+
+    OS_EXIT_CRITICAL(sr);
+
+    /* g_ble_ll_sync_create_comp_ev will be cleared by this callback */
+    *post_cmd_cb = ble_ll_sync_cancel_complete_event;
+
+    return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_sync_terminate(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_periodic_adv_term_sync_cp *cmd = (const void *) cmdbuf;
+    struct ble_ll_sync_sm *sm;
+    uint16_t handle;
+    os_sr_t sr;
+
+    if (g_ble_ll_sync_create_comp_ev) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    handle = le16toh(cmd->sync_handle);
+    if (handle > 0xeff) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (handle >= BLE_LL_SYNC_CNT) {
+        return BLE_ERR_UNK_ADV_INDENT;
+    }
+
+    sm = &g_ble_ll_sync_sm[handle];
+
+    OS_ENTER_CRITICAL(sr);
+
+    if (!(sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHED)) {
+        OS_EXIT_CRITICAL(sr);
+        return BLE_ERR_UNK_ADV_INDENT;
+    }
+
+    ble_ll_sync_sm_clear(sm);
+
+    OS_EXIT_CRITICAL(sr);
+
+    return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_sync_list_add(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_add_dev_to_periodic_adv_list_cp *cmd = (const void *)cmdbuf;
+    int i;
+
+    if (g_ble_ll_sync_create_comp_ev) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (cmd->peer_addr_type > BLE_HCI_ADV_PEER_ADDR_MAX) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+    if (cmd->sid > 0x0f) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    i = ble_ll_sync_on_list(cmd->peer_addr, cmd->peer_addr_type, cmd->sid);
+    if (i >= 0) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    i = ble_ll_sync_list_get_free();
+    if (i < 0) {
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    g_ble_ll_sync_adv_list[i].adv_sid = cmd->sid;
+    g_ble_ll_sync_adv_list[i].adv_addr_type = cmd->peer_addr_type;
+    memcpy(&g_ble_ll_sync_adv_list[i].adv_addr, cmd->peer_addr, BLE_DEV_ADDR_LEN);
+
+    return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_sync_list_remove(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_rem_dev_from_periodic_adv_list_cp *cmd = (const void *)cmdbuf;
+    int i;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (g_ble_ll_sync_create_comp_ev) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    if (cmd->peer_addr_type > BLE_HCI_ADV_PEER_ADDR_MAX) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (cmd->sid > 0x0f) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    i = ble_ll_sync_on_list(cmd->peer_addr, cmd->peer_addr_type, cmd->sid);
+    if (i < 0) {
+        return BLE_ERR_UNK_ADV_INDENT;
+    }
+
+    memset(&g_ble_ll_sync_adv_list[i], 0, sizeof(g_ble_ll_sync_adv_list[i]));
+    g_ble_ll_sync_adv_list[i].adv_sid = 0xff;
+
+    return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_sync_list_clear(void)
+{
+    int i;
+
+    if (g_ble_ll_sync_create_comp_ev) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    for (i = 0; i < ARRAY_SIZE(g_ble_ll_sync_adv_list); i++) {
+        memset(&g_ble_ll_sync_adv_list[i], 0, sizeof(g_ble_ll_sync_adv_list[i]));
+        g_ble_ll_sync_adv_list[i].adv_sid = 0xff;
+    }
+
+    return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_sync_list_size(uint8_t *rspbuf, uint8_t *rsplen)
+{
+    struct ble_hci_le_rd_periodic_adv_list_size_rp *rsp = (void *) rspbuf;
+
+    rsp->list_size = ARRAY_SIZE(g_ble_ll_sync_adv_list);
+
+    *rsplen = sizeof(*rsp);
+    return BLE_ERR_SUCCESS;
+}
+
+#if MYNEWT_VAL(BLE_VERSION) >= 51
+int
+ble_ll_sync_receive_enable(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_periodic_adv_receive_enable_cp *cmd = (const void *)cmdbuf;
+    struct ble_ll_sync_sm *sm;
+    uint16_t handle;
+    os_sr_t sr;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (cmd->enable > 0x01) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    handle = le16toh(cmd->sync_handle);
+    if (handle > 0xeff) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (handle >= BLE_LL_SYNC_CNT) {
+        return BLE_ERR_UNK_ADV_INDENT;
+    }
+
+    sm = &g_ble_ll_sync_sm[handle];
+
+    OS_ENTER_CRITICAL(sr);
+
+    if (!(sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHED)) {
+        OS_EXIT_CRITICAL(sr);
+        return BLE_ERR_UNK_ADV_INDENT;
+    }
+
+    if (cmd->enable) {
+        sm->flags &= ~BLE_LL_SYNC_SM_FLAG_DISABLED;
+    } else {
+        sm->flags |= BLE_LL_SYNC_SM_FLAG_DISABLED;
+    }
+
+    OS_EXIT_CRITICAL(sr);
+    return BLE_ERR_SUCCESS;
+}
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
+static struct ble_ll_sync_sm *
+ble_ll_sync_transfer_get(const uint8_t *addr, uint8_t addr_type, uint8_t sid)
+{
+    struct ble_ll_sync_sm *sm;
+    int i;
+
+    for (i = 0; i < BLE_LL_SYNC_CNT; i++) {
+        sm = &g_ble_ll_sync_sm[i];
+
+        if (!sm->flags) {
+            /* allocate event for transfer received event */
+            sm->transfer_received_ev = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI);
+            if (!sm->transfer_received_ev) {
+                break;
+            }
+
+            sm->adv_sid = sid;
+            sm->adv_addr_type = addr_type;
+            memcpy(&sm->adv_addr, addr, BLE_DEV_ADDR_LEN);
+
+            sm->flags |= BLE_LL_SYNC_SM_FLAG_ESTABLISHING;
+            return sm;
+        }
+    }
+
+    return NULL;
+}
+
+void
+ble_ll_sync_periodic_ind(struct ble_ll_conn_sm *connsm,
+                         const uint8_t *sync_ind, bool reports_disabled,
+                         uint16_t max_skip, uint32_t sync_timeout)
+{
+    const uint8_t *syncinfo = sync_ind + 2;
+    uint16_t sync_conn_event_count;
+    uint16_t last_pa_event_count;
+    struct ble_ll_sync_sm *sm;
+    uint16_t conn_event_count;
+    uint8_t sync_anchor_usecs;
+    const uint8_t *rpa = NULL;
+    int last_pa_diff;
+    uint32_t sync_anchor;
+    const uint8_t *addr;
+    uint16_t event_cntr;
+    uint32_t itvl_usecs;
+    uint32_t ww_adjust;
+    uint8_t addr_type;
+    uint8_t phy_mode;
+    uint32_t offset;
+    uint32_t future;
+    uint16_t itvl;
+    int rpa_index;
+    uint8_t sid;
+    uint8_t sca;
+    os_sr_t sr;
+
+    phy_mode = ble_ll_ctrl_phy_from_phy_mask(sync_ind[25]);
+    itvl = get_le16(syncinfo + 2);
+    /* ignore if sync params are not valid */
+    if ((phy_mode == 0) || (itvl < 6)) {
+        return;
+    }
+
+    last_pa_event_count = get_le16(sync_ind + 22);
+    event_cntr = get_le16(syncinfo + 16);
+    itvl_usecs = itvl * BLE_LL_SYNC_ITVL_USECS;
+
+    last_pa_diff = abs((int16_t)(event_cntr - last_pa_event_count));
+    /* check if not 5 seconds apart, if so ignore sync transfer */
+    if ((last_pa_diff * itvl_usecs) > 5000000) {
+        return;
+    }
+
+    sid = (sync_ind[24] & 0x0f);
+    addr_type = (sync_ind[24] & 0x10) ? BLE_ADDR_RANDOM : BLE_ADDR_PUBLIC;
+    addr = sync_ind + 26;
+
+    rpa_index = -1;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    /* check if need to resolve */
+    if (ble_ll_is_rpa(addr, addr_type)) {
+        rpa_index = ble_ll_resolv_peer_rpa_any(addr);
+        if (rpa_index >= 0) {
+            rpa = addr;
+            addr = g_ble_ll_resolv_list[rpa_index].rl_identity_addr;
+            addr_type = g_ble_ll_resolv_list[rpa_index].rl_addr_type;
+        }
+    }
+#endif
+
+    OS_ENTER_CRITICAL(sr);
+    /* check if already synchronized with this peer */
+    sm = ble_ll_sync_find(addr, addr_type, sid);
+    if (sm) {
+        OS_EXIT_CRITICAL(sr);
+        return;
+    }
+
+    /* ignore if no memory for new sync */
+    sm = ble_ll_sync_transfer_get(addr, addr_type, sid);
+    if (!sm) {
+        OS_EXIT_CRITICAL(sr);
+        return;
+    }
+
+    OS_EXIT_CRITICAL(sr);
+
+    if (rpa_index >= 0) {
+        sm->flags |= BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED;
+        memcpy(sm->adv_addr_rpa, rpa, BLE_DEV_ADDR_LEN);
+    }
+
+    /* set params from transfer */
+    sm->timeout = os_cputime_usecs_to_ticks(sync_timeout);
+    sm->skip = max_skip;
+    sm->sync_pending_cnt = BLE_LL_SYNC_ESTABLISH_CNT;
+    sm->transfer_id = get_le16(sync_ind); /* first two bytes */
+    sm->transfer_conn = connsm;
+
+    /* Sync Packet Offset (13 bits), Offset Units (1 bit), Offset Adjust (1 bit),
+     * RFU (1 bit)
+     */
+    offset = syncinfo[0];
+    offset |= (uint16_t)(syncinfo[1] & 0x1f) << 8;
+
+    if (syncinfo[1] & 0x20) {
+        if (syncinfo[1] & 0x40) {
+            offset += 0x2000;
+        }
+
+        offset *= 300;
+        sm->flags |= BLE_LL_SYNC_SM_FLAG_OFFSET_300;
+    } else {
+        offset *= 30;
+        sm->flags &= ~BLE_LL_SYNC_SM_FLAG_OFFSET_300;
+    }
+
+    /* sync end event */
+    ble_npl_event_init(&sm->sync_ev_end, ble_ll_sync_event_end, sm);
+
+    sm->itvl = itvl;
+
+    /* precalculate interval ticks and usecs */
+    sm->itvl_ticks = os_cputime_usecs_to_ticks(itvl_usecs);
+    sm->itvl_usecs = (uint8_t)(itvl_usecs -
+                               os_cputime_ticks_to_usecs(sm->itvl_ticks));
+    if (sm->itvl_usecs == 31) {
+        sm->itvl_usecs = 0;
+        sm->itvl_ticks++;
+    }
+
+    /* Channels Mask (37 bits) */
+    sm->chanmap[0] = syncinfo[4];
+    sm->chanmap[1] = syncinfo[5];
+    sm->chanmap[2] = syncinfo[6];
+    sm->chanmap[3] = syncinfo[7];
+    sm->chanmap[4] = syncinfo[8] & 0x1f;
+    sm->num_used_chans = ble_ll_utils_calc_num_used_chans(sm->chanmap);
+
+    /* SCA (3 bits) */
+    sm->sca = syncinfo[8] >> 5;
+
+    /* AA (4 bytes) */
+    sm->access_addr = get_le32(syncinfo + 9);
+    sm->channel_id = ((sm->access_addr & 0xffff0000) >> 16) ^
+                      (sm->access_addr & 0x0000ffff);
+
+    /* CRCInit (3 bytes) */
+    sm->crcinit = syncinfo[13];
+    sm->crcinit |= syncinfo[14] << 8;
+    sm->crcinit |= syncinfo[15] << 16;
+
+    /* Event Counter (2 bytes) */
+    sm->event_cntr = event_cntr;
+
+    /* adjust skip if pass timeout */
+    max_skip = get_max_skip(sm->itvl * BLE_LL_SYNC_ITVL_USECS, sync_timeout);
+    if (sm->skip > max_skip) {
+        sm->skip = max_skip;
+    }
+
+    sm->phy_mode = phy_mode;
+
+    /* Calculate channel index of first event */
+    sm->chan_index = ble_ll_utils_calc_dci_csa2(sm->event_cntr, sm->channel_id,
+                                                sm->num_used_chans, sm->chanmap);
+
+    /* get anchor for specified conn event */
+    conn_event_count = get_le16(sync_ind + 20);
+    ble_ll_conn_get_anchor(connsm, conn_event_count, &sm->anchor_point,
+                           &sm->anchor_point_usecs);
+
+    /* Set last anchor point */
+    sm->last_anchor_point = sm->anchor_point - (last_pa_diff * sm->itvl_ticks);
+
+    /* calculate extra window widening */
+    sync_conn_event_count = get_le16(sync_ind + 32);
+    sca = sync_ind[24] >> 5;
+    ble_ll_conn_get_anchor(connsm, sync_conn_event_count, &sync_anchor,
+                           &sync_anchor_usecs);
+    ww_adjust = ble_ll_utils_calc_window_widening(connsm->anchor_point,
+                                                  sync_anchor, sca);
+
+    /* spin until we get anchor in future */
+    future = os_cputime_get32() + g_ble_ll_sched_offset_ticks;
+    while (CPUTIME_LT(sm->anchor_point, future)) {
+        if (ble_ll_sync_next_event(sm, ww_adjust) < 0) {
+            /* release SM if this failed */
+            ble_ll_sync_transfer_received(sm, BLE_ERR_CONN_ESTABLISHMENT);
+            memset(sm, 0, sizeof(*sm));
+            return;
+        }
+    }
+
+    if (ble_ll_sched_sync(&sm->sch, sm->anchor_point, sm->anchor_point_usecs,
+                          offset, sm->phy_mode)) {
+        /* release SM if this failed */
+        ble_ll_sync_transfer_received(sm, BLE_ERR_CONN_ESTABLISHMENT);
+        memset(sm, 0, sizeof(*sm));
+        return;
+    }
+
+    /* Set new anchor point */
+    sm->anchor_point = sm->sch.start_time + g_ble_ll_sched_offset_ticks;
+    sm->anchor_point_usecs = sm->sch.remainder;
+
+    if (reports_disabled) {
+        sm->flags |= BLE_LL_SYNC_SM_FLAG_DISABLED;
+    }
+}
+
+static void
+ble_ll_sync_put_syncinfo(struct ble_ll_sync_sm *syncsm,
+                         struct ble_ll_conn_sm *connsm, uint8_t *conn_event_cnt,
+                         uint8_t *dptr)
+{
+    uint8_t anchor_usecs;
+    uint16_t conn_cnt;
+    uint32_t offset;
+    uint32_t anchor;
+    uint8_t units;
+
+    anchor = connsm->anchor_point;
+    anchor_usecs = connsm->anchor_point_usecs;
+    conn_cnt = connsm->event_cntr;
+
+    /* get anchor for conn event that is before periodic_adv_event_start_time */
+    while (CPUTIME_GT(anchor, syncsm->anchor_point)) {
+        ble_ll_conn_get_anchor(connsm, --conn_cnt, &anchor, &anchor_usecs);
+    }
+
+    offset = os_cputime_ticks_to_usecs(syncsm->anchor_point - anchor);
+    offset -= anchor_usecs;
+    offset += syncsm->anchor_point_usecs;
+
+    /* connEventCount */
+    put_le16(conn_event_cnt, conn_cnt);
+
+    /* Sync Packet Offset (13 bits), Offset Units (1 bit), Offset Adjust (1 bit),
+     * RFU (1 bit)
+     */
+    if (offset > 245700) {
+        units = 0x20;
+
+        if (offset >= 0x2000) {
+            offset -= 0x2000;
+            units |= 0x40;
+        }
+
+        offset = offset / 300;
+    } else {
+        units = 0x00;
+        offset = offset / 30;
+    }
+
+    dptr[0] = (offset & 0x000000ff);
+    dptr[1] = ((offset >> 8) & 0x0000001f) | units;
+
+    /* Interval (2 bytes) */
+    put_le16(&dptr[2], syncsm->itvl);
+
+    /* Channels Mask (37 bits) */
+    dptr[4] = syncsm->chanmap[0];
+    dptr[5] = syncsm->chanmap[1];
+    dptr[6] = syncsm->chanmap[2];
+    dptr[7] = syncsm->chanmap[3];
+    dptr[8] = syncsm->chanmap[4] & 0x1f;
+
+    /* SCA (3 bits) */
+    dptr[8] |= syncsm->sca << 5;
+
+    /* AA (4 bytes) */
+    put_le32(&dptr[9], syncsm->access_addr);
+
+    /* CRCInit (3 bytes) */
+    dptr[13] = (uint8_t)syncsm->crcinit;
+    dptr[14] = (uint8_t)(syncsm->crcinit >> 8);
+    dptr[15] = (uint8_t)(syncsm->crcinit >> 16);
+
+    /* Event Counter (2 bytes) */
+    put_le16(&dptr[16], syncsm->event_cntr);
+}
+
+static int
+ble_ll_sync_send_sync_ind(struct ble_ll_sync_sm *syncsm,
+                          struct ble_ll_conn_sm *connsm, uint16_t service_data)
+{
+    struct os_mbuf *om;
+    uint8_t *sync_ind;
+
+    om = os_msys_get_pkthdr(BLE_LL_CTRL_MAX_PDU_LEN,
+                            sizeof(struct ble_mbuf_hdr));
+    if (!om) {
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    om->om_data[0] = BLE_LL_CTRL_PERIODIC_SYNC_IND;
+
+    sync_ind = om->om_data + 1;
+
+    /* ID (service_data), already in LE order */
+    memcpy(sync_ind, &service_data, sizeof(service_data));
+
+    /* fill in syncinfo */
+    ble_ll_sync_put_syncinfo(syncsm, connsm, sync_ind + 20, sync_ind + 2);
+
+    /* lastPaEventCounter */
+    put_le16(sync_ind + 22, syncsm->event_cntr_last_received);
+
+    /* SID, AType, SCA */
+    sync_ind[24] = syncsm->adv_sid;
+
+    if (syncsm->flags & BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED) {
+        sync_ind[24] |= 1 << 4;
+    } else {
+        sync_ind[24] |= (syncsm->adv_addr_type == BLE_ADDR_RANDOM) << 4;
+    }
+
+    sync_ind[24] |= BLE_LL_SCA_ENUM << 5;
+
+    /* PHY */
+    sync_ind[25] = (0x01 << (ble_ll_sync_phy_mode_to_hci(syncsm->phy_mode) - 1));
+
+    /* AdvA */
+    if (syncsm->flags & BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED) {
+        memcpy(sync_ind + 26, syncsm->adv_addr_rpa, BLE_DEV_ADDR_LEN);
+    } else {
+        memcpy(sync_ind + 26, syncsm->adv_addr, BLE_DEV_ADDR_LEN);
+    }
+
+    /* syncConnEventCount */
+    put_le16(sync_ind + 32, connsm->event_cntr);
+
+    ble_ll_conn_enqueue_pkt(connsm, om, BLE_LL_LLID_CTRL,
+                            BLE_LL_CTRL_PERIODIC_SYNC_IND_LEN + 1);
+
+    return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_sync_transfer(const uint8_t *cmdbuf, uint8_t len,
+                     uint8_t *rspbuf, uint8_t *rsplen)
+{
+    const struct ble_hci_le_periodic_adv_sync_transfer_cp *cmd = (const void *)cmdbuf;
+    struct ble_hci_le_periodic_adv_sync_transfer_rp *rsp = (void *) rspbuf;
+    struct ble_ll_conn_sm *connsm;
+    struct ble_ll_sync_sm *sm;
+    uint16_t handle;
+    os_sr_t sr;
+    int rc;
+
+    if (len != sizeof(*cmd)) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        goto done;
+    }
+
+    handle = le16toh(cmd->sync_handle);
+    if (handle > 0xeff) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        goto done;
+    }
+
+    if (handle >= BLE_LL_SYNC_CNT) {
+        rc = BLE_ERR_UNK_ADV_INDENT;
+        goto done;
+    }
+
+    sm = &g_ble_ll_sync_sm[handle];
+
+    OS_ENTER_CRITICAL(sr);
+
+    if (!(sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHED)) {
+        rc = BLE_ERR_UNK_ADV_INDENT;
+        OS_EXIT_CRITICAL(sr);
+        goto done;
+    }
+
+    handle = le16toh(cmd->conn_handle);
+    if (handle > 0xeff) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        OS_EXIT_CRITICAL(sr);
+        goto done;
+    }
+
+    connsm = ble_ll_conn_find_active_conn(handle);
+    if (!connsm) {
+        rc = BLE_ERR_UNK_CONN_ID;
+        OS_EXIT_CRITICAL(sr);
+        goto done;
+    }
+
+    /* TODO should not need to shift
+     * byte 3 (0 byte is conn_feature) , bit 1
+     *
+     * Allow initiate LL procedure only if remote supports it.
+     */
+    if (!(connsm->remote_features[2] & (BLE_LL_FEAT_SYNC_TRANS_RECV >> (8 * 3)))) {
+        rc = BLE_ERR_UNSUPP_REM_FEATURE;
+        goto done;
+    }
+
+    rc = ble_ll_sync_send_sync_ind(sm, connsm, cmd->service_data);
+
+    OS_EXIT_CRITICAL(sr);
+done:
+    rsp->conn_handle = cmd->conn_handle;
+    *rsplen = sizeof(*rsp);
+    return rc;
+}
+#endif
+
+/*
+ * Called when a sync scan event has been removed from the scheduler
+ * without being run.
+ */
+void
+ble_ll_sync_rmvd_from_sched(struct ble_ll_sync_sm *sm)
+{
+    ble_ll_event_send(&sm->sync_ev_end);
+}
+
+bool
+ble_ll_sync_enabled(void)
+{
+    return g_ble_ll_sync_create_comp_ev != NULL;
+}
+
+/**
+ * Called to reset the sync module. When this function is called the
+ * scheduler has been stopped and the phy has been disabled. The LL should
+ * be in the standby state.
+ */
+void
+ble_ll_sync_reset(void)
+{
+    int i;
+
+    for (i = 0; i < BLE_LL_SYNC_CNT; i++) {
+        ble_ll_sync_sm_clear(&g_ble_ll_sync_sm[i]);
+    }
+
+    for (i = 0; i < ARRAY_SIZE(g_ble_ll_sync_adv_list); i++) {
+        memset(&g_ble_ll_sync_adv_list[i], 0, sizeof(g_ble_ll_sync_adv_list[i]));
+        g_ble_ll_sync_adv_list[i].adv_sid = 0xff;
+    }
+
+    g_ble_ll_sync_create_params.timeout = 0;
+    g_ble_ll_sync_create_params.max_skip = 0;
+    g_ble_ll_sync_create_params.options = 0;
+
+    g_ble_ll_sync_sm_current = NULL;
+
+    if (g_ble_ll_sync_create_comp_ev) {
+        ble_hci_trans_buf_free(g_ble_ll_sync_create_comp_ev);
+        g_ble_ll_sync_create_comp_ev = NULL;
+    }
+}
+
+void
+ble_ll_sync_init(void)
+{
+    int i;
+
+    for (i = 0; i < ARRAY_SIZE(g_ble_ll_sync_adv_list); i++) {
+        g_ble_ll_sync_adv_list[i].adv_sid = 0xff;
+    }
+}
+#endif

nimble/controller/src/ble_ll_utils.c

@@ -0,0 +1,300 @@
+/*
+ * 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 <assert.h>
+#include <stdlib.h>
+#include "nimble/ble.h"
+#include "controller/ble_ll.h"
+#include "controller/ble_ll_utils.h"
+
+/* 37 bits require 5 bytes */
+#define BLE_LL_CHMAP_LEN (5)
+
+/* Sleep clock accuracy table (in ppm) */
+static const uint16_t g_ble_sca_ppm_tbl[8] = {
+    500, 250, 150, 100, 75, 50, 30, 20
+};
+
+uint32_t
+ble_ll_utils_calc_access_addr(void)
+{
+    uint32_t aa;
+    uint16_t aa_low;
+    uint16_t aa_high;
+    uint32_t temp;
+    uint32_t mask;
+    uint32_t prev_bit;
+    uint8_t bits_diff;
+    uint8_t consecutive;
+    uint8_t transitions;
+    uint8_t ones;
+    int tmp;
+
+    /* Calculate a random access address */
+    aa = 0;
+    while (1) {
+        /* Get two, 16-bit random numbers */
+        aa_low = ble_ll_rand() & 0xFFFF;
+        aa_high = ble_ll_rand() & 0xFFFF;
+
+        /* All four bytes cannot be equal */
+        if (aa_low == aa_high) {
+            continue;
+        }
+
+        /* Upper 6 bits must have 2 transitions */
+        tmp = (int16_t)aa_high >> 10;
+        if (__builtin_popcount(tmp ^ (tmp >> 1)) < 2) {
+            continue;
+        }
+
+        /* Cannot be access address or be 1 bit different */
+        aa = aa_high;
+        aa = (aa << 16) | aa_low;
+        bits_diff = 0;
+        temp = aa ^ BLE_ACCESS_ADDR_ADV;
+        for (mask = 0x00000001; mask != 0; mask <<= 1) {
+            if (mask & temp) {
+                ++bits_diff;
+                if (bits_diff > 1) {
+                    break;
+                }
+            }
+        }
+        if (bits_diff <= 1) {
+            continue;
+        }
+
+        /* Cannot have more than 24 transitions */
+        transitions = 0;
+        consecutive = 1;
+        ones = 0;
+        mask = 0x00000001;
+        while (mask < 0x80000000) {
+            prev_bit = aa & mask;
+            mask <<= 1;
+            if (mask & aa) {
+                if (prev_bit == 0) {
+                    ++transitions;
+                    consecutive = 1;
+                } else {
+                    ++consecutive;
+                }
+            } else {
+                if (prev_bit == 0) {
+                    ++consecutive;
+                } else {
+                    ++transitions;
+                    consecutive = 1;
+                }
+            }
+
+            if (prev_bit) {
+                ones++;
+            }
+
+            /* 8 lsb should have at least three 1 */
+            if (mask == 0x00000100 && ones < 3) {
+                break;
+            }
+
+            /* 16 lsb should have no more than 11 transitions */
+            if (mask == 0x00010000 && transitions > 11) {
+                break;
+            }
+
+            /* This is invalid! */
+            if (consecutive > 6) {
+                /* Make sure we always detect invalid sequence below */
+                mask = 0;
+                break;
+            }
+        }
+
+        /* Invalid sequence found */
+        if (mask != 0x80000000) {
+            continue;
+        }
+
+        /* Cannot be more than 24 transitions */
+        if (transitions > 24) {
+            continue;
+        }
+
+        /* We have a valid access address */
+        break;
+    }
+    return aa;
+}
+
+uint8_t
+ble_ll_utils_remapped_channel(uint8_t remap_index, const uint8_t *chanmap)
+{
+    uint8_t cntr;
+    uint8_t mask;
+    uint8_t usable_chans;
+    uint8_t chan;
+    int i, j;
+
+    /* NOTE: possible to build a map but this would use memory. For now,
+     * we just calculate
+     * Iterate through channel map to find this channel
+     */
+    chan = 0;
+    cntr = 0;
+    for (i = 0; i < BLE_LL_CHMAP_LEN; i++) {
+        usable_chans = chanmap[i];
+        if (usable_chans != 0) {
+            mask = 0x01;
+            for (j = 0; j < 8; j++) {
+                if (usable_chans & mask) {
+                    if (cntr == remap_index) {
+                        return (chan + j);
+                    }
+                    ++cntr;
+                }
+                mask <<= 1;
+            }
+        }
+        chan += 8;
+    }
+
+    /* we should never reach here */
+    BLE_LL_ASSERT(0);
+    return 0;
+}
+
+uint8_t
+ble_ll_utils_calc_num_used_chans(const uint8_t *chmap)
+{
+    int i;
+    int j;
+    uint8_t mask;
+    uint8_t chanbyte;
+    uint8_t used_channels;
+
+    used_channels = 0;
+    for (i = 0; i < BLE_LL_CHMAP_LEN; ++i) {
+        chanbyte = chmap[i];
+        if (chanbyte) {
+            if (chanbyte == 0xff) {
+                used_channels += 8;
+            } else {
+                mask = 0x01;
+                for (j = 0; j < 8; ++j) {
+                    if (chanbyte & mask) {
+                        ++used_channels;
+                    }
+                    mask <<= 1;
+                }
+            }
+        }
+    }
+    return used_channels;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2)
+static uint16_t
+ble_ll_utils_csa2_perm(uint16_t in)
+{
+    uint16_t out = 0;
+    int i;
+
+    for (i = 0; i < 8; i++) {
+        out |= ((in >> i) & 0x00000001) << (7 - i);
+    }
+
+    for (i = 8; i < 16; i++) {
+        out |= ((in >> i) & 0x00000001) << (15 + 8 - i);
+    }
+
+    return out;
+}
+
+static uint16_t
+ble_ll_utils_csa2_prng(uint16_t counter, uint16_t ch_id)
+{
+    uint16_t prn_e;
+
+    prn_e = counter ^ ch_id;
+
+    prn_e = ble_ll_utils_csa2_perm(prn_e);
+    prn_e = (prn_e * 17) + ch_id;
+
+    prn_e = ble_ll_utils_csa2_perm(prn_e);
+    prn_e = (prn_e * 17) + ch_id;
+
+    prn_e = ble_ll_utils_csa2_perm(prn_e);
+    prn_e = (prn_e * 17) + ch_id;
+
+    prn_e = prn_e ^ ch_id;
+
+    return prn_e;
+}
+
+uint8_t
+ble_ll_utils_calc_dci_csa2(uint16_t event_cntr, uint16_t channel_id,
+                           uint8_t num_used_chans, const uint8_t *chanmap)
+{
+    uint16_t channel_unmapped;
+    uint8_t remap_index;
+
+    uint16_t prn_e;
+    uint8_t bitpos;
+
+    prn_e = ble_ll_utils_csa2_prng(event_cntr, channel_id);
+
+    channel_unmapped = prn_e % 37;
+
+    /*
+     * If unmapped channel is the channel index of a used channel it is used
+     * as channel index.
+     */
+    bitpos = 1 << (channel_unmapped & 0x07);
+    if (chanmap[channel_unmapped >> 3] & bitpos) {
+        return channel_unmapped;
+    }
+
+    remap_index = (num_used_chans * prn_e) / 0x10000;
+
+    return ble_ll_utils_remapped_channel(remap_index, chanmap);
+}
+#endif
+
+uint32_t
+ble_ll_utils_calc_window_widening(uint32_t anchor_point,
+                                  uint32_t last_anchor_point,
+                                  uint8_t master_sca)
+{
+    uint32_t total_sca_ppm;
+    uint32_t window_widening;
+    int32_t time_since_last_anchor;
+    uint32_t delta_msec;
+
+    window_widening = 0;
+
+    time_since_last_anchor = (int32_t)(anchor_point - last_anchor_point);
+    if (time_since_last_anchor > 0) {
+        delta_msec = os_cputime_ticks_to_usecs(time_since_last_anchor) / 1000;
+        total_sca_ppm = g_ble_sca_ppm_tbl[master_sca] + MYNEWT_VAL(BLE_LL_SCA);
+        window_widening = (total_sca_ppm * delta_msec) / 1000;
+    }
+
+    return window_widening;
+}

nimble/controller/src/ble_ll_whitelist.c

@@ -105,8 +105,12 @@ ble_ll_whitelist_clear(void)
 int
 ble_ll_whitelist_read_size(uint8_t *rspbuf, uint8_t *rsplen)
 {
-    rspbuf[0] = BLE_LL_WHITELIST_SIZE;
-    *rsplen = 1;
+    struct ble_hci_le_rd_white_list_rp *rsp = (void *) rspbuf;
+
+    rsp->size = BLE_LL_WHITELIST_SIZE;
+
+    *rsplen = sizeof(*rsp);
+
     return BLE_ERR_SUCCESS;
 }
 
@@ -123,7 +127,7 @@ ble_ll_whitelist_read_size(uint8_t *rspbuf, uint8_t *rsplen)
  * plus 1).
  */
 static int
-ble_ll_whitelist_search(uint8_t *addr, uint8_t addr_type)
+ble_ll_whitelist_search(const uint8_t *addr, uint8_t addr_type)
 {
     int i;
     struct ble_ll_whitelist_entry *wl;
@@ -180,11 +184,16 @@ ble_ll_whitelist_match(uint8_t *addr, uint8_t addr_type, int is_ident)
  * @return int
  */
 int
-ble_ll_whitelist_add(uint8_t *addr, uint8_t addr_type)
+ble_ll_whitelist_add(const uint8_t *cmdbuf, uint8_t len)
 {
-    int i;
-    int rc;
+    const struct ble_hci_le_add_whte_list_cp *cmd = (const void *) cmdbuf;
     struct ble_ll_whitelist_entry *wl;
+    int rc;
+    int i;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
 
     /* Must be in proper state */
     if (!ble_ll_whitelist_chg_allowed()) {
@@ -193,12 +202,12 @@ ble_ll_whitelist_add(uint8_t *addr, uint8_t addr_type)
 
     /* Check if we have any open entries */
     rc = BLE_ERR_SUCCESS;
-    if (!ble_ll_whitelist_search(addr, addr_type)) {
+    if (!ble_ll_whitelist_search(cmd->addr, cmd->addr_type)) {
         wl = &g_ble_ll_whitelist[0];
         for (i = 0; i < BLE_LL_WHITELIST_SIZE; ++i) {
             if (wl->wl_valid == 0) {
-                memcpy(&wl->wl_dev_addr[0], addr, BLE_DEV_ADDR_LEN);
-                wl->wl_addr_type = addr_type;
+                memcpy(&wl->wl_dev_addr[0], cmd->addr, BLE_DEV_ADDR_LEN);
+                wl->wl_addr_type = cmd->addr_type;
                 wl->wl_valid = 1;
                 break;
             }
@@ -209,7 +218,7 @@ ble_ll_whitelist_add(uint8_t *addr, uint8_t addr_type)
             rc = BLE_ERR_MEM_CAPACITY;
         } else {
 #if (BLE_USES_HW_WHITELIST == 1)
-            rc = ble_hw_whitelist_add(addr, addr_type);
+            rc = ble_hw_whitelist_add(cmd->addr, cmd->addr_type);
 #endif
         }
     }
@@ -225,22 +234,27 @@ ble_ll_whitelist_add(uint8_t *addr, uint8_t addr_type)
  * @return int 0: success, BLE error code otherwise
  */
 int
-ble_ll_whitelist_rmv(uint8_t *addr, uint8_t addr_type)
+ble_ll_whitelist_rmv(const uint8_t *cmdbuf, uint8_t len)
 {
+    const struct ble_hci_le_rmv_white_list_cp *cmd = (const void *) cmdbuf;
     int position;
 
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
     /* Must be in proper state */
     if (!ble_ll_whitelist_chg_allowed()) {
         return BLE_ERR_CMD_DISALLOWED;
     }
 
-    position = ble_ll_whitelist_search(addr, addr_type);
+    position = ble_ll_whitelist_search(cmd->addr, cmd->addr_type);
     if (position) {
         g_ble_ll_whitelist[position - 1].wl_valid = 0;
     }
 
 #if (BLE_USES_HW_WHITELIST == 1)
-    ble_hw_whitelist_rmv(addr, addr_type);
+    ble_hw_whitelist_rmv(cmd->addr, cmd->addr_type);
 #endif
 
     return BLE_ERR_SUCCESS;

nimble/controller/src/ble_ll_xcvr.c

@@ -1,155 +0,0 @@
-/*
- * Licensed to the Apache Software Foundation (ASF) under one
- * or more contributor license agreements.  See the NOTICE file
- * distributed with this work for additional information
- * regarding copyright ownership.  The ASF licenses this file
- * to you under the Apache License, Version 2.0 (the
- * "License"); you may not use this file except in compliance
- * with the License.  You may obtain a copy of the License at
- *
- *  http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing,
- * software distributed under the License is distributed on an
- * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
- * KIND, either express or implied.  See the License for the
- * specific language governing permissions and limitations
- * under the License.
- */
-
-#include <stdint.h>
-#include <stddef.h>
-#include <assert.h>
-#include <stddef.h>
-#include "syscfg/syscfg.h"
-#include "os/os_cputime.h"
-#include "controller/ble_phy.h"
-#include "controller/ble_ll.h"
-#include "controller/ble_ll_xcvr.h"
-
-#ifdef BLE_XCVR_RFCLK
-int
-ble_ll_xcvr_rfclk_state(void)
-{
-    uint32_t expiry;
-
-    if (g_ble_ll_data.ll_rfclk_state == BLE_RFCLK_STATE_ON) {
-        expiry = g_ble_ll_data.ll_rfclk_start_time;
-        if ((int32_t)(os_cputime_get32() - expiry) >
-                g_ble_ll_data.ll_xtal_ticks) {
-            g_ble_ll_data.ll_rfclk_state = BLE_RFCLK_STATE_SETTLED;
-        }
-    }
-    return g_ble_ll_data.ll_rfclk_state;
-}
-
-void
-ble_ll_xcvr_rfclk_enable(void)
-{
-    if (g_ble_ll_data.ll_rfclk_state == BLE_RFCLK_STATE_OFF) {
-        g_ble_ll_data.ll_rfclk_state = BLE_RFCLK_STATE_ON;
-        ble_phy_rfclk_enable();
-    }
-}
-
-void
-ble_ll_xcvr_rfclk_disable(void)
-{
-    if (g_ble_ll_data.ll_rfclk_state != BLE_RFCLK_STATE_OFF) {
-        ble_phy_rfclk_disable();
-        g_ble_ll_data.ll_rfclk_state = BLE_RFCLK_STATE_OFF;
-    }
-}
-
-void
-ble_ll_xcvr_rfclk_stop(void)
-{
-    os_cputime_timer_stop(&g_ble_ll_data.ll_rfclk_timer);
-    ble_ll_xcvr_rfclk_disable();
-}
-
-uint32_t
-ble_ll_xcvr_rfclk_time_till_settled(void)
-{
-    int32_t dt;
-    uint32_t rc;
-
-    rc = 0;
-    if (g_ble_ll_data.ll_rfclk_state == BLE_RFCLK_STATE_ON) {
-        dt = (int32_t)(os_cputime_get32() - g_ble_ll_data.ll_rfclk_start_time);
-        BLE_LL_ASSERT(dt >= 0);
-        if (dt < g_ble_ll_data.ll_xtal_ticks) {
-            rc = g_ble_ll_data.ll_xtal_ticks - (uint32_t)dt;
-        }
-    }
-
-    return rc;
-}
-
-/**
- * Called when the timer to turn on the RF CLOCK expires. This function checks
- * the state of the clock. If the clock is off, the clock is turned on.
- * Otherwise, we just exit.
- *
- * Context: Interrupt
- *
- * @param arg
- */
-void
-ble_ll_xcvr_rfclk_timer_exp(void *arg)
-{
-    if (g_ble_ll_data.ll_rfclk_state == BLE_RFCLK_STATE_OFF) {
-        ble_ll_xcvr_rfclk_start_now(os_cputime_get32());
-    }
-}
-
-/**
- * This API is used to turn on the rfclock without setting the cputime timer to
- * start the clock at some later point.
- *
- * NOTE: presumes that the state of the rf clock was checked prior to calling.
- *
- * @param now
- */
-void
-ble_ll_xcvr_rfclk_start_now(uint32_t now)
-{
-    ble_ll_xcvr_rfclk_enable();
-    g_ble_ll_data.ll_rfclk_start_time = now;
-}
-
-/**
- * Starts the timer that will turn the rf clock on. The 'cputime' is
- * the time at which the clock needs to be settled.
- *
- * @param cputime   Time at which rfclock should be on and settled.
- */
-void
-ble_ll_xcvr_rfclk_timer_start(uint32_t cputime)
-{
-    /*
-     * If we are currently in an advertising event or a connection event,
-     * no need to start the cputime timer
-     */
-    if ((g_ble_ll_data.ll_state == BLE_LL_STATE_ADV) ||
-        (g_ble_ll_data.ll_state == BLE_LL_STATE_CONNECTION)) {
-        return;
-    }
-
-    /* Account for the settling time */
-    cputime -= g_ble_ll_data.ll_xtal_ticks;
-
-    /*
-     * If the timer is on the list, we need to see if its expiry is before
-     * 'cputime'. If the expiry is before, no need to do anything. If it
-     * is after, we need to stop the timer and start at new time.
-     */
-    if (g_ble_ll_data.ll_rfclk_timer.link.tqe_prev != NULL) {
-        if ((int32_t)(cputime - g_ble_ll_data.ll_rfclk_timer.expiry) >= 0) {
-            return;
-        }
-        os_cputime_timer_stop(&g_ble_ll_data.ll_rfclk_timer);
-    }
-    os_cputime_timer_start(&g_ble_ll_data.ll_rfclk_timer, cputime);
-}
-#endif

nimble/controller/syscfg.yml

@@ -17,15 +17,10 @@
 #
 
 syscfg.defs:
-    BLE_DEVICE:
-        description: >
-            Used by package management system to include BLE hardware
-            drivers.
-        value: 1
-
-    BLE_LP_CLOCK:
+    BLE_CONTROLLER:
         description: >
-            Used by BSP packages to configure LP clock for controller.
+            Indicates that NimBLE controller is present. The default value for
+            this setting shall not be overriden.
         value: 1
 
     BLE_HW_WHITELIST_ENABLE:
@@ -43,46 +38,20 @@ syscfg.defs:
         type: 'task_priority'
         value: 0
 
-    # Sleep clock accuracy (sca). This is the amount of drift in the system
-    # during when the device is sleeping (in parts per million).
-    #
-    # NOTE: 'the' master sca is an enumerated value based on the sca. Rather
-    # than have a piece of code calculate this value, the developer must set
-    # this value based on the value of the SCA using the following table:
-    #
-    #  SCA between 251 and 500 ppm (inclusive); master sca = 0
-    #  SCA between 151 and 250 ppm (inclusive); master sca = 1
-    #  SCA between 101 and 150 ppm (inclusive); master sca = 2
-    #  SCA between 76 and 100 ppm (inclusive); master sca = 3
-    #  SCA between 51 and 75 ppm (inclusive); master sca = 4
-    #  SCA between 31 and 50 ppm (inclusive); master sca = 5
-    #  SCA between 21 and 30 ppm (inclusive); master sca = 6
-    #  SCA between 0 and 20 ppm (inclusive); master sca = 7
-    #
-    #  For example:
-    #      if your clock drift is 101 ppm, your master should be set to 2.
-    #      if your clock drift is 20, your master sca should be set to 7.
-    #
-    #  The values provided below are merely meant to be an example and should
-    #  be replaced by values appropriate for your platform.
-    BLE_LL_OUR_SCA:
-        description: 'The system clock accuracy of the device.'
-        value: '60'    # in ppm
-
-    BLE_LL_MASTER_SCA:
-        description: 'Enumerated value based on our sca'
-        value: '4'
+    BLE_LL_SCA:
+        description: Sleep clock accuracy of our device (in ppm)
+        value: MYNEWT_VAL(BLE_LL_OUR_SCA)
+        range: 0..500
 
     BLE_LL_TX_PWR_DBM:
         description: 'Transmit power level.'
         value: '0'
 
-    BLE_NUM_COMP_PKT_RATE:
+    BLE_LL_NUM_COMP_PKT_ITVL_MS:
         description: >
-            Determines the maximum rate at which the controller will send the
-            number of completed packets event to the host. Rate is in os time
-            ticks.
-        value: '(2 * OS_TICKS_PER_SEC)'
+            Determines the interval at which the controller will send the
+            number of completed packets event to the host. Rate is in milliseconds.
+        value: 2000
 
     BLE_LL_MFRG_ID:
         description: >
@@ -116,10 +85,10 @@ syscfg.defs:
         value: '251'
     BLE_LL_SUPP_MAX_RX_BYTES:
         description: 'The maximum supported received PDU size'
-        value: 'MYNEWT_VAL_BLE_LL_MAX_PKT_SIZE'
+        value: MYNEWT_VAL(BLE_LL_MAX_PKT_SIZE)
     BLE_LL_SUPP_MAX_TX_BYTES:
         description: 'The maximum supported transmit PDU size'
-        value: 'MYNEWT_VAL_BLE_LL_MAX_PKT_SIZE'
+        value: MYNEWT_VAL(BLE_LL_MAX_PKT_SIZE)
     BLE_LL_CONN_INIT_MAX_TX_BYTES:
         description: >
             Used to set the initial maximum transmit PDU size in a
@@ -181,12 +150,22 @@ syscfg.defs:
             material often.
         value: '32'
 
-    # Crystal setting time
-    BLE_XTAL_SETTLE_TIME:
+    BLE_LL_RFMGMT_ENABLE_TIME:
         description: >
-            The settling time of the high-frequency oscillator. This is
-            used to turn on/off the clock used for the radio (assuming
-            the HW supports this). This value is in microseconds.
+            Time required for radio and/or related components to be fully
+            enabled before any request from LL is sent. This value is used
+            by rfmgmt to enable PHY in advance, before request from LL is
+            made. It depends on radio driver selected and may also depend
+            on hardware used:
+            - nrf51 - time required for XTAL to settle
+            - nrf52 - time required for XTAL to settle
+            Value is specified in microseconds. If set to 0, rfmgmt keeps
+            PHY enabled all the time.
+        value: MYNEWT_VAL(BLE_XTAL_SETTLE_TIME)
+
+    BLE_LL_HCI_LLCP_TRACE:
+        description: >
+            Enables LLCP tracing using HCI vendor-specific events.
         value: '0'
 
     # Configuration for LL supported features.
@@ -241,13 +220,6 @@ syscfg.defs:
             This option is used to enable/disable LL privacy.
         value: '1'
 
-    BLE_LL_CFG_FEAT_EXT_SCAN_FILT:
-        description: >
-            This option is used to enable/disable the extended scanner filter
-            policy feature. Currently, this feature is not supported by the
-            nimble controller.
-        value: '0'
-
     BLE_LL_CFG_FEAT_LE_CSA2:
         description: >
             This option is used to enable/disable support for LE Channel
@@ -269,7 +241,66 @@ syscfg.defs:
             This option is used to enable/disable support for Extended
             Advertising Feature. That means extended scanner, advertiser
             and connect.
-        value: MYNEWT_VAL_BLE_EXT_ADV
+        value: MYNEWT_VAL(BLE_EXT_ADV)
+
+    BLE_LL_CFG_FEAT_LL_PERIODIC_ADV:
+        description: >
+            This option is used to enable/disable support for Periodic
+            Advertising Feature.
+        value: MYNEWT_VAL(BLE_PERIODIC_ADV)
+        restrictions:
+            - 'BLE_LL_CFG_FEAT_LL_EXT_ADV if 1'
+
+    BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_CNT:
+        description: >
+            This option is used to configure number of supported periodic syncs.
+        value: MYNEWT_VAL(BLE_MAX_PERIODIC_SYNCS)
+
+    BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_LIST_CNT:
+        description: >
+            Size of Periodic Advertiser sync list.
+        value: MYNEWT_VAL(BLE_MAX_PERIODIC_SYNCS)
+
+    BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER:
+        description: >
+            This option is use to enable/disable support for Periodic
+            Advertising Sync Transfer Feature.
+        value: MYNEWT_VAL(BLE_PERIODIC_ADV_SYNC_TRANSFER)
+
+    BLE_LL_CFG_FEAT_CTRL_TO_HOST_FLOW_CONTROL:
+        description: >
+            Enable controller-to-host flow control support. This allows host to
+            limit number of ACL packets sent at once from controller to avoid
+            congestion on HCI transport if feature is also supported by host.
+        value: 0
+
+    BLE_LL_CFG_FEAT_LL_SCA_UPDATE:
+        description: >
+            This option is used to enable/disable support for SCA update procedure
+        value: 0
+        restrictions:
+            - '(BLE_VERSION >= 52) if 1'
+
+    BLE_LL_CFG_FEAT_LL_ISO:
+        description: >
+            This option is used to enable/disable support for LE Isochronous Channels
+            as per Bluetooth v5.2 channels
+        value: MYNEWT_VAL(BLE_ISO)
+        restrictions:
+            - '(BLE_VERSION >= 52) if 1'
+
+    BLE_LL_CFG_FEAT_LL_ISO_TEST:
+        description: >
+            This option is used to enable/disbale test commands for ISO support
+        value: MYNEWT_VAL(BLE_ISO_TEST)
+        restrictions:
+            - 'BLE_LL_CFG_FEAT_LL_ISO if 1'
+
+    BLE_LL_SCAN_AUX_SEGMENT_CNT:
+         description: >
+            Number of auxiliary advertising segments that can be scanned
+            concurrently (Core 5.2, Vol 6, Part B, 4.4.2.2.2).
+         value: 8
 
     BLE_LL_EXT_ADV_AUX_PTR_CNT:
          description: >
@@ -286,9 +317,27 @@ syscfg.defs:
             be used.
         value: "(uint8_t[6]){0x00, 0x00, 0x00, 0x00, 0x00, 0x00}"
 
-    BLE_LL_DIRECT_TEST_MODE:
+    BLE_LL_DTM:
         description: >
              Enables HCI Test commands needed for Bluetooth SIG certification
+        value: MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE)
+    BLE_LL_DTM_EXTENSIONS:
+        description: >
+             Enables non-standard extensions to HCI test commands. Once enabled,
+             HCI_LE_Transmitter_Test accepts extra parameters in addition to
+             those defined in Core specification
+               interval (2 octets)   interval between packets (usecs), overrides
+                                     standard interval
+               pkt_count (2 octets)  number of packets to transmit, controller
+                                     will automatically stop sending packets
+                                     after given number of packets was sent
+             Setting either of these parameters to 0 will configure for default
+             behavior, as per Core specification.
+             If specified interval is shorter then allowed by specification it
+             will be ignored.
+             Extended parameters shall immediately follow standard parameters.
+             Controller can accept both standard and extended version of command
+             depending on specified HCI command length.
         value: 0
 
     BLE_LL_VND_EVENT_ON_ASSERT:
@@ -303,6 +352,104 @@ syscfg.defs:
             Sysinit stage for the NimBLE controller.
         value: 250
 
+    BLE_LL_DEBUG_GPIO_HCI_CMD:
+        description: >
+            GPIO pin number to debug HCI commands flow. Pin is set to high state
+            when HCI command is being processed.
+        value: -1
+    BLE_LL_DEBUG_GPIO_HCI_EV:
+        description: >
+            GPIO pin number to debug HCI events flow. Pin is set to high state
+            when HCI event is being sent.
+        value: -1
+    BLE_LL_DEBUG_GPIO_SCHED_RUN:
+        description: >
+            GPIO pin number to debug scheduler running (on timer). Pin is set
+            to high state while scheduler is running.
+        value: -1
+    BLE_LL_DEBUG_GPIO_SCHED_ITEM:
+        description: >
+            GPIO pin number to debug scheduler item execution times. Pin is set
+            to high state while item is active.
+        value: -1
+    BLE_LL_DEBUG_GPIO_RFMGMT:
+        description: >
+            GPIO pin number to debug rfmgmt activity. Pin is set to high state
+            while rfmgmt is active.
+        value: -1
+
+# Below settings allow to change scheduler timings. These should be left at
+# default values unless you know what you are doing!
+    BLE_LL_SCHED_AUX_MAFS_DELAY:
+        description: >
+            Additional delay [us] between last ADV_EXT_IND and AUX_ADV_IND PDUs
+            when scheduling extended advertising event. This extends T_MAFS.
+        value: 0
+    BLE_LL_SCHED_AUX_CHAIN_MAFS_DELAY:
+        description: >
+            Additional delay [us] between consecutive AUX_CHAIN_IND PDUs
+            when scheduling extended or periodic advertising event. This extends
+            T_MAFS.
+        value: 0
+    BLE_LL_SCHED_SCAN_AUX_PDU_LEN:
+        description: >
+            This is expected PDU len for AUX_ADV_IND and subsequent
+            AUX_CHAIN_IND. When scheduling scan scheduler will reserve time for
+            receiving this amount of time. Setting this to high value improves
+            reception of large PDUs but results in wasting scheduler space when
+            receiving small PDUs only. On the other hand too low value can
+            result in not being able to scan whole PDU due to being preempted
+            by next scheduled item. By default size matching legacy ADV_IND PDU
+            payload is used: ExtHeader (Flags, AdvA, ADI) + 31 bytes of data.
+        range: 1..257
+        value: 41
+
+    BLE_LL_SCHED_SCAN_SYNC_PDU_LEN:
+        description: >
+            This is expected PDU len for AUX_SYNC_IND and subsequent
+            AUX_CHAIN_IND. When scheduling scan scheduler will reserve time for
+            receiving this amount of time. Setting this to high value improves
+            reception of large PDUs but results in wasting scheduler space when
+            receiving small PDUs only. On the other hand too low value can
+            result in not being able to scan whole PDU due to being preempted
+            by next scheduled item. By default size matching PDU with legacy
+            data size is used: ExtHeader + 31 bytes of data.
+        range: 1..257
+        value: 32
+
+# deprecated settings (to be defunct/removed eventually)
+    BLE_LL_DIRECT_TEST_MODE:
+        description: use BLE_LL_DTM instead
+        value: 0
+        deprecated: 1
+    BLE_XTAL_SETTLE_TIME:
+        description: use BLE_LL_RFMGMT_ENABLE_TIME instead
+        value: 0
+        deprecated: 1
+    BLE_LL_OUR_SCA:
+        description: use BLE_LL_SCA instead
+        value: 60
+        deprecated: 1
+
+# defunct settings (to be removed eventually)
+    BLE_DEVICE:
+        description: Superseded by BLE_CONTROLLER
+        value: 1
+        defunct: 1
+    BLE_LP_CLOCK:
+        description: Superseded by BLE_CONTROLLER
+        value: 1
+        defunct: 1
+    BLE_NUM_COMP_PKT_RATE:
+        description: Superseded by BLE_LL_NUM_COMP_PKT_ITVL_MS
+        value: '(2 * OS_TICKS_PER_SEC)'
+        defunct: 1
+    BLE_LL_MASTER_SCA:
+        description: use BLE_LL_SCA instead
+        value: 4
+        defunct: 1
+
+
 syscfg.vals.BLE_LL_CFG_FEAT_LL_EXT_ADV:
     BLE_LL_CFG_FEAT_LE_CSA2: 1
     BLE_HW_WHITELIST_ENABLE: 0
@@ -312,3 +459,6 @@ syscfg.vals.BLE_LL_CFG_FEAT_LL_EXT_ADV:
 # controller code visible when connected to external host
 syscfg.vals.!BLE_HOST:
     BLE_LL_VND_EVENT_ON_ASSERT: 1
+
+syscfg.restrictions:
+    - OS_CPUTIME_FREQ == 32768

nimble/controller/test/pkg.yml

@@ -30,4 +30,5 @@ pkg.deps.SELFTEST:
     - "@apache-mynewt-core/sys/console/stub"
     - "@apache-mynewt-core/sys/log/full"
     - "@apache-mynewt-core/sys/stats/stub"
+    - nimble/drivers/native
     - nimble/transport/ram

nimble/controller/test/src/ble_ll_csa2_test.c

@@ -22,8 +22,9 @@
 #include "testutil/testutil.h"
 #include "controller/ble_ll_test.h"
 #include "controller/ble_ll_conn.h"
+#include "ble_ll_csa2_test.h"
 
-TEST_CASE(ble_ll_csa2_test_1)
+TEST_CASE_SELF(ble_ll_csa2_test_1)
 {
     struct ble_ll_conn_sm conn;
     uint8_t rc;
@@ -65,7 +66,7 @@ TEST_CASE(ble_ll_csa2_test_1)
     TEST_ASSERT(rc == 21);
 }
 
-TEST_CASE(ble_ll_csa2_test_2)
+TEST_CASE_SELF(ble_ll_csa2_test_2)
 {
     struct ble_ll_conn_sm conn;
     uint8_t rc;
@@ -112,12 +113,3 @@ TEST_SUITE(ble_ll_csa2_test_suite)
     ble_ll_csa2_test_1();
     ble_ll_csa2_test_2();
 }
-
-int
-ble_ll_csa2_test_all(void)
-{
-    ble_ll_csa2_test_1();
-    ble_ll_csa2_test_2();
-
-    return tu_any_failed;
-}

nimble/controller/test/src/ble_ll_csa2_test.h

@@ -0,0 +1,27 @@
+/*
+ * 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_CSA2_TEST_
+#define H_BLE_LL_CSA2_TEST_
+
+#include "testutil/testutil.h"
+
+TEST_SUITE_DECL(ble_ll_csa2_test_suite);
+
+#endif

nimble/controller/test/src/ble_ll_test.c

@@ -22,16 +22,14 @@
 #include "controller/ble_ll_test.h"
 #include "os/os.h"
 #include "testutil/testutil.h"
+#include "ble_ll_csa2_test.h"
 
 #if MYNEWT_VAL(SELFTEST)
 
 int
 main(int argc, char **argv)
 {
-    sysinit();
-
-    ble_ll_csa2_test_all();
-
+    ble_ll_csa2_test_suite();
     return tu_any_failed;
 }
 

nimble/controller/test/syscfg.yml

@@ -20,4 +20,6 @@ syscfg.vals:
     BLE_LL_CFG_FEAT_LE_CSA2: 1
 
     # Prevent priority conflict with controller task.
-    MCU_UART_POLLER_PRIO: 16
+    MCU_TIMER_POLLER_PRIO: 1
+    MCU_UART_POLLER_PRIO: 2
+    NATIVE_SOCKETS_PRIO: 3

nimble/drivers/dialog_cmac/README.md

@@ -0,0 +1,68 @@
+<!--
+#
+# 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.
+#
+-->
+
+## How to run NimBLE controller on Dialog DA1469x
+
+Dialog DA1469x has separate Cortex-M0+ core inside CMAC hw block which can run
+NimBLE controller. This means DA1469x can run full NimBLE stack: host is running
+on M33 core while controller is running on M0+ core. Both communicate using
+standard HCI H4 protocol exchanged over mailboxes located in shared memory.
+
+### Basic setup
+
+In order to run full NimBLE stack on DA1469x you will need two newt targets: one
+for M33 (e.g. `dialog_da1469x-dk-pro` BSP) and one for M0+ (`dialog_cmac` BSP).
+
+Once everything is configured properly, you only need to build target for M33.
+Target configured for M0+ will be build automatically and image is linked with
+M33 image so everything can be flashed at once just as if there is only single
+target used.
+
+Target for M33 should be set and configured as any other BLE application. In
+order to use NimBLE controller on CMAC, set proper HCI transport via syscfg:
+
+    BLE_HCI_TRANSPORT: dialog_cmac
+
+This will include proper transport, driver and add M0+ target to build process.
+
+For M0+, there is sample target provided in `targets/dialog_cmac` and it's used
+by default unless overrided by syscfg in M33 target:
+
+    CMAC_IMAGE_TARGET_NAME: "@apache-mynewt-nimble/targets/dialog_cmac"
+
+If you wish to create own target for M0+, make sure your target is set the same
+way (`app`, `bsp` and `build_profile`) as sample. Also it is recommended to use
+syscfg settings from sample target in new target.
+
+### NimBLE configuration
+
+Since host and controller are running on different cores, they both use separate
+configuration: host configuration is in M33 target, controller configuration is
+in M0+ target. There is currently no way to automatically synchronize both, so
+care needs to be taken when enabling features in either of targets.
+
+A possible workaround is to use separate `.yml` file with all the NimBLE syscfg
+values settings and include it in both targets using `$import` directive which
+is supported by recent versions of `newt` tool.
+
+### Advanced settings
+
+(tbd)

nimble/controller/include/controller/ble_ll_xcvr.h → nimble/drivers/dialog_cmac/include/ble/xcvr.h

@@ -17,32 +17,23 @@
  * under the License.
  */
 
-#ifndef H_BLE_LL_XCVR_
-#define H_BLE_LL_XCVR_
+#ifndef H_BLE_XCVR_
+#define H_BLE_XCVR_
 
 #ifdef __cplusplus
 extern "C" {
 #endif
 
-#ifdef BLE_XCVR_RFCLK
+#define XCVR_TX_SCHED_DELAY_USECS       (250)
 
-/* RF clock states */
-#define BLE_RFCLK_STATE_OFF     (0)
-#define BLE_RFCLK_STATE_ON      (1)
-#define BLE_RFCLK_STATE_SETTLED (2)
-
-int ble_ll_xcvr_rfclk_state(void);
-void ble_ll_xcvr_rfclk_start_now(uint32_t now);
-void ble_ll_xcvr_rfclk_stop(void);
-void ble_ll_xcvr_rfclk_enable(void);
-void ble_ll_xcvr_rfclk_disable(void);
-uint32_t ble_ll_xcvr_rfclk_time_till_settled(void);
-void ble_ll_xcvr_rfclk_timer_exp(void *arg);
-void ble_ll_xcvr_rfclk_timer_start(uint32_t cputime);
-#endif
+/*
+ * Define HW whitelist size. This is the total possible whitelist size;
+ * not necessarily the size that will be used (may be smaller)
+ */
+#define BLE_HW_WHITE_LIST_SIZE        (8)
 
 #ifdef __cplusplus
 }
 #endif
 
-#endif /* H_LL_ */
+#endif /* H_BLE_XCVR_ */

nimble/drivers/dialog_cmac/pkg.yml

@@ -0,0 +1,33 @@
+#
+# 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: nimble/drivers/dialog_cmac
+pkg.description: BLE driver for Dialog CMAC
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+pkg.deps:
+    - "@apache-mynewt-nimble/nimble/controller"
+    - "@apache-mynewt-core/crypto/tinycrypt"
+pkg.apis:
+    - ble_driver
+pkg.req_apis:
+    - ble_transport

nimble/drivers/dialog_cmac/src/ble_hw.c

@@ -0,0 +1,340 @@
+/*
+ * 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 <assert.h>
+#include <stdint.h>
+#include "mcu/mcu.h"
+#include "nimble/ble.h"
+#include "controller/ble_hw.h"
+#include "CMAC.h"
+#include "cmac_driver/cmac_shared.h"
+#include "mcu/mcu.h"
+#include "tinycrypt/aes.h"
+
+static struct tc_aes_key_sched_struct g_ctx;
+
+int
+ble_hw_rng_init(ble_rng_isr_cb_t cb, int bias)
+{
+    cmac_rand_set_isr_cb(cb);
+    return 0;
+}
+
+int
+ble_hw_rng_start(void)
+{
+    /* Chime the M33 in case we need random numbers generated */
+    cmac_rand_start();
+    CMAC->CM_EV_SET_REG = CMAC_CM_EV_SET_REG_EV1C_CMAC2SYS_IRQ_SET_Msk;
+    return 0;
+}
+
+int
+ble_hw_rng_stop(void)
+{
+    cmac_rand_stop();
+    return 0;
+}
+
+#define BLE_HW_RESOLV_LIST_SIZE     (MYNEWT_VAL(BLE_LL_RESOLV_LIST_SIZE))
+
+struct ble_hw_resolv_irk {
+    uint32_t key[4];
+};
+
+struct ble_hw_resolv_list {
+    uint8_t count;
+    struct ble_hw_resolv_irk irk[BLE_HW_RESOLV_LIST_SIZE];
+};
+
+struct ble_hw_resolv_proc {
+    uint32_t hash;
+    uint8_t f_configured;
+    uint8_t f_active;
+    uint8_t f_match;
+    uint8_t f_done;
+    struct ble_hw_resolv_irk *irk;
+    struct ble_hw_resolv_irk *irk_end;
+    uint32_t crypto_prand_in[4];
+    uint32_t crypto_e_out[4];
+};
+
+static struct ble_hw_resolv_list g_ble_hw_resolv_list;
+static struct ble_hw_resolv_proc g_ble_hw_resolv_proc;
+
+int
+ble_hw_get_public_addr(ble_addr_t *addr)
+{
+    return -1;
+}
+
+int
+ble_hw_get_static_addr(ble_addr_t *addr)
+{
+    return -1;
+}
+
+void
+ble_hw_whitelist_clear(void)
+{
+}
+
+int
+ble_hw_whitelist_add(const uint8_t *addr, uint8_t addr_type)
+{
+    return 0;
+}
+
+void
+ble_hw_whitelist_rmv(const uint8_t *addr, uint8_t addr_type)
+{
+}
+
+uint8_t
+ble_hw_whitelist_size(void)
+{
+    return 0;
+}
+
+void
+ble_hw_whitelist_enable(void)
+{
+}
+
+
+void
+ble_hw_whitelist_disable(void)
+{
+}
+
+int
+ble_hw_whitelist_match(void)
+{
+    return 0;
+}
+
+int
+ble_hw_encrypt_block(struct ble_encryption_block *ecb)
+{
+    uint32_t in_addr;
+    uint32_t out_addr;
+
+    /*
+     * The following code bears some explanation. This function is called by
+     * the LL task to encrypt blocks and calculate session keys. Address
+     * resolution also calls this function. Furthermore, during connections,
+     * the M0 crypto accelerator is used but this function is not called when
+     * using it. During the entire connection event, the M0 crypto block cannot
+     * be used as the crypto state (some of it) needs to remain un-changed.
+     * Note that this is also true when address resolution is enabled: the
+     * HW crypto block is set up and cannot be modified.
+     *
+     * Rather than attempt to share the M0 crypto block between the various
+     * controller features which require it, we decided to use software to
+     * perform the encryption task for anything being done at the link-layer
+     * (outside of an ISR). If this function is called inside an ISR, and it
+     * is when resolving addresses, the crypto accelerator is not being used
+     * by a connection event. Thus, we check to see if we are inside of an ISR.
+     * If so, we use the M0 crypto block. If outside of an ISR, we use the M33
+     */
+    if (!os_arch_in_isr()) {
+        tc_aes128_set_encrypt_key(&g_ctx, ecb->key);
+        tc_aes_encrypt(ecb->cipher_text, ecb->plain_text, &g_ctx);
+        return 0;
+    }
+
+    /* Need to retain state of in/out pointers */
+    in_addr = CMAC->CM_CRYPTO_IN_ADR2_REG;
+    out_addr = CMAC->CM_CRYPTO_OUT_ADR_REG;
+
+    while (CMAC->CM_CRYPTO_STAT_REG & CMAC_CM_CRYPTO_STAT_REG_CM_CRYPTO_BUSY_Msk);
+
+    /* RECB, memory in/out, encryption */
+    CMAC->CM_CRYPTO_CTRL_REG = CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_ECB_ENC_EN_Msk |
+                               CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_IN_SEL_Msk |
+                               CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_OUT_SEL_Msk |
+                               CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_ENC_DECN_Msk;
+
+    CMAC->CM_CRYPTO_KEY_31_0_REG = get_le32(&ecb->key[0]);
+    CMAC->CM_CRYPTO_KEY_63_32_REG = get_le32(&ecb->key[4]);
+    CMAC->CM_CRYPTO_KEY_95_64_REG = get_le32(&ecb->key[8]);
+    CMAC->CM_CRYPTO_KEY_127_96_REG = get_le32(&ecb->key[12]);
+    CMAC->CM_CRYPTO_IN_ADR2_REG = (uint32_t)ecb->plain_text;
+    CMAC->CM_CRYPTO_OUT_ADR_REG = (uint32_t)ecb->cipher_text;
+
+    CMAC->CM_EXC_STAT_REG = CMAC_CM_EXC_STAT_REG_EXC_CRYPTO_Msk;
+    CMAC->CM_EV_SET_REG = CMAC_CM_EV_SET_REG_EV_CRYPTO_START_Msk;
+    while (!(CMAC->CM_EXC_STAT_REG & CMAC_CM_EXC_STAT_REG_EXC_CRYPTO_Msk));
+    CMAC->CM_EXC_STAT_REG = CMAC_CM_EXC_STAT_REG_EXC_CRYPTO_Msk;
+
+    CMAC->CM_CRYPTO_IN_ADR2_REG = in_addr;
+    CMAC->CM_CRYPTO_OUT_ADR_REG = out_addr;
+
+    return 0;
+}
+
+void
+ble_hw_resolv_list_clear(void)
+{
+    g_ble_hw_resolv_list.count = 0;
+}
+
+int
+ble_hw_resolv_list_add(uint8_t *irk)
+{
+    struct ble_hw_resolv_irk *e;
+
+    if (g_ble_hw_resolv_list.count == BLE_HW_RESOLV_LIST_SIZE) {
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    e = &g_ble_hw_resolv_list.irk[g_ble_hw_resolv_list.count];
+    /* Prepare key here so we do not need to do it during resolving */
+    e->key[0] = get_le32(&irk[0]);
+    e->key[1] = get_le32(&irk[4]);
+    e->key[2] = get_le32(&irk[8]);
+    e->key[3] = get_le32(&irk[12]);
+
+    g_ble_hw_resolv_list.count++;
+
+    return BLE_ERR_SUCCESS;
+}
+
+void
+ble_hw_resolv_list_rmv(int index)
+{
+    struct ble_hw_resolv_irk *e;
+
+    if (index < g_ble_hw_resolv_list.count) {
+        g_ble_hw_resolv_list.count--;
+
+        e = &g_ble_hw_resolv_list.irk[index];
+        memmove(e, e + 1, (g_ble_hw_resolv_list.count - index) * sizeof(e->key));
+    }
+}
+
+uint8_t
+ble_hw_resolv_list_size(void)
+{
+    return BLE_HW_RESOLV_LIST_SIZE;
+}
+
+int
+ble_hw_resolv_list_match(void)
+{
+    return g_ble_hw_resolv_proc.f_match ?
+           g_ble_hw_resolv_proc.irk - g_ble_hw_resolv_list.irk : -1;
+}
+
+static void
+ble_hw_resolv_proc_next(void)
+{
+    void *src = &g_ble_hw_resolv_proc.irk->key;
+
+    if (g_ble_hw_resolv_proc.irk == g_ble_hw_resolv_proc.irk_end) {
+        g_ble_hw_resolv_proc.f_done = 1;
+        g_ble_hw_resolv_proc.f_active = 0;
+    } else {
+        __asm__ volatile (".syntax unified                      \n"
+                          "   ldm  %[ptr]!, {r1, r2, r3, r4}    \n"
+                          "   ldr  %[ptr], =%[reg]              \n"
+                          "   stm  %[ptr]!, {r1, r2, r3, r4}    \n"
+                          : [ptr] "+l" (src)
+                          : [reg] "i" (&CMAC->CM_CRYPTO_KEY_31_0_REG)
+                          : "r1", "r2", "r3", "r4", "memory");
+
+        CMAC->CM_EV_SET_REG = CMAC_CM_EV_SET_REG_EV_CRYPTO_START_Msk;
+    }
+}
+
+void
+ble_hw_resolv_proc_enable(void)
+{
+    assert(!g_ble_hw_resolv_proc.f_active);
+
+    CMAC->CM_CRYPTO_CTRL_REG = CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_SW_REQ_ABORT_Msk;
+
+    CMAC->CM_CRYPTO_CTRL_REG = CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_ECB_ENC_EN_Msk |
+                               CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_IN_SEL_Msk |
+                               CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_OUT_SEL_Msk |
+                               CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_ENC_DECN_Msk;
+
+    CMAC->CM_CRYPTO_IN_ADR2_REG = (uint32_t)g_ble_hw_resolv_proc.crypto_prand_in;
+    CMAC->CM_CRYPTO_OUT_ADR_REG = (uint32_t)g_ble_hw_resolv_proc.crypto_e_out;
+
+    g_ble_hw_resolv_proc.irk = g_ble_hw_resolv_list.irk;
+    g_ble_hw_resolv_proc.irk_end = g_ble_hw_resolv_list.irk +
+                                   g_ble_hw_resolv_list.count;
+    g_ble_hw_resolv_proc.f_configured = 1;
+    g_ble_hw_resolv_proc.f_active = 0;
+
+    /*
+     * It would be better to enable IRQ in ble_hw_resolv_proc_start, but this
+     * would introduce a bit of latency when starting resolving procedure and
+     * we need to save every us possible there in order to be able to resolve
+     * RPA on time.
+     */
+    NVIC_ClearPendingIRQ(CRYPTO_IRQn);
+    NVIC_EnableIRQ(CRYPTO_IRQn);
+}
+
+void
+ble_hw_resolv_proc_disable(void)
+{
+    g_ble_hw_resolv_proc.f_configured = 0;
+    g_ble_hw_resolv_proc.f_active = 0;
+    g_ble_hw_resolv_proc.f_match = 0;
+    g_ble_hw_resolv_proc.f_done = 1;
+
+    NVIC_DisableIRQ(CRYPTO_IRQn);
+}
+
+void
+ble_hw_resolv_proc_start(const uint8_t *addr)
+{
+    assert(g_ble_hw_resolv_proc.f_configured);
+
+    /* crypto_prand_in is already zeroed so prand is properly padded */
+    g_ble_hw_resolv_proc.crypto_prand_in[3] = get_be24(&addr[3]) << 8;
+    g_ble_hw_resolv_proc.hash = get_be24(&addr[0]);
+
+    g_ble_hw_resolv_proc.f_match = 0;
+    g_ble_hw_resolv_proc.f_done = 0;
+    g_ble_hw_resolv_proc.f_active = 1;
+
+    ble_hw_resolv_proc_next();
+}
+
+void
+CRYPTO_IRQHandler(void)
+{
+    uint32_t hash;
+
+    CMAC->CM_EXC_STAT_REG = CMAC_CM_EXC_STAT_REG_EXC_CRYPTO_Msk;
+
+    hash = g_ble_hw_resolv_proc.crypto_e_out[3] >> 8;
+    if (g_ble_hw_resolv_proc.hash == hash) {
+        g_ble_hw_resolv_proc.f_active = 0;
+        g_ble_hw_resolv_proc.f_match = 1;
+        g_ble_hw_resolv_proc.f_done = 1;
+    } else {
+        g_ble_hw_resolv_proc.irk++;
+        ble_hw_resolv_proc_next();
+    }
+}

nimble/drivers/dialog_cmac/src/ble_hw_priv.h

@@ -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.
+ */
+
+#ifndef _BLE_HW_PRIV_H_
+#define _BLE_HW_PRIV_H_
+
+#include <stdint.h>
+
+void ble_hw_resolv_proc_enable(void);
+void ble_hw_resolv_proc_disable(void);
+void ble_hw_resolv_proc_start(const uint8_t *addr);
+
+#endif /* _BLE_HW_PRIV_H_ */

nimble/drivers/dialog_cmac/src/ble_phy.c

@@ -0,0 +1,1817 @@
+/*
+ * 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 "syscfg/syscfg.h"
+#if !MYNEWT_VAL(BLE_PHY_DEBUG_DSER)
+#define MCU_DIAG_SER_DISABLE
+#endif
+
+#include <assert.h>
+#include <stdint.h>
+#include <assert.h>
+#include "nimble/ble.h"
+#include "mcu/mcu.h"
+#include "mcu/cmac_timer.h"
+#include "cmac_driver/cmac_shared.h"
+#include "controller/ble_phy.h"
+#include "controller/ble_ll.h"
+#include "stats/stats.h"
+#include "CMAC.h"
+#include "ble_hw_priv.h"
+#include "ble_rf_priv.h"
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+#error LE Coded PHY cannot be enabled on DA1469x
+#endif
+
+/* Statistics */
+STATS_SECT_START(ble_phy_stats)
+    STATS_SECT_ENTRY(phy_isrs)
+    STATS_SECT_ENTRY(tx_good)
+    STATS_SECT_ENTRY(tx_fail)
+    STATS_SECT_ENTRY(tx_late)
+    STATS_SECT_ENTRY(tx_late_sched)
+    STATS_SECT_ENTRY(tx_late_frame)
+    STATS_SECT_ENTRY(tx_late_field)
+    STATS_SECT_ENTRY(tx_bytes)
+    STATS_SECT_ENTRY(rx_starts)
+    STATS_SECT_ENTRY(rx_aborts)
+    STATS_SECT_ENTRY(rx_valid)
+    STATS_SECT_ENTRY(rx_crc_err)
+    STATS_SECT_ENTRY(rx_late)
+    STATS_SECT_ENTRY(radio_state_errs)
+    STATS_SECT_ENTRY(rx_hw_err)
+    STATS_SECT_ENTRY(tx_hw_err)
+STATS_SECT_END
+STATS_SECT_DECL(ble_phy_stats) ble_phy_stats;
+
+STATS_NAME_START(ble_phy_stats)
+    STATS_NAME(ble_phy_stats, phy_isrs)
+    STATS_NAME(ble_phy_stats, tx_good)
+    STATS_NAME(ble_phy_stats, tx_fail)
+    STATS_NAME(ble_phy_stats, tx_late)
+    STATS_NAME(ble_phy_stats, tx_late_sched)
+    STATS_NAME(ble_phy_stats, tx_late_frame)
+    STATS_NAME(ble_phy_stats, tx_late_field)
+    STATS_NAME(ble_phy_stats, tx_bytes)
+    STATS_NAME(ble_phy_stats, rx_starts)
+    STATS_NAME(ble_phy_stats, rx_aborts)
+    STATS_NAME(ble_phy_stats, rx_valid)
+    STATS_NAME(ble_phy_stats, rx_crc_err)
+    STATS_NAME(ble_phy_stats, rx_late)
+    STATS_NAME(ble_phy_stats, radio_state_errs)
+    STATS_NAME(ble_phy_stats, rx_hw_err)
+    STATS_NAME(ble_phy_stats, tx_hw_err)
+STATS_NAME_END(ble_phy_stats)
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+#error LE Coded PHY is not supported
+#endif
+
+/* An easy way to get and set bit field value in CMAC registers */
+#define CMAC_SETREGF(_reg, _field, _val)                                    \
+    CMAC->_reg = (CMAC->_reg & ~(CMAC_ ## _reg ## _ ## _field ## _Msk)) |   \
+                 ((_val) << (CMAC_ ## _reg ## _ ## _field ## _Pos));
+#define CMAC_GETREGF(_reg, _field)                                          \
+    (CMAC->_reg & (CMAC_ ## _reg ## _ ## _field ## _Msk)) >>                \
+    (CMAC_ ## _reg ## _ ## _field ## _Pos)
+
+/* Definitions for fields queue */
+#define FIELD_DATA_REG_DMA_TX(_offset, _len) \
+    ((uint32_t)&g_ble_phy_tx_buf[(_offset)] & 0x3ffff) | ((_len) << 20)
+#define FIELD_DATA_REG_DMA_RX(_offset, _len) \
+    ((uint32_t)&g_ble_phy_rx_buf[(_offset)] & 0x3ffff) | ((_len) << 20)
+
+#if MYNEWT_VAL(BLE_LL_DTM)
+#define PHY_WHITENING   (g_ble_phy_data.phy_whitening)
+#else
+#define PHY_WHITENING   (1)
+#endif
+
+#define FIELD_CTRL_REG_TX_PREAMBLE                                  \
+    (0 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_CRC_Pos) |           \
+    (0 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_WHITENING_Pos) |     \
+    (0 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_TX_DATA_SRC_Pos) |      \
+    (g_ble_phy_data.phy_mode_evpsym <<                              \
+     CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_EVPSYMBOL_LUT_Pos) |         \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_VALID_Pos) |            \
+    (g_ble_phy_data.phy_mode_pre_len <<                             \
+     CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_SIZE_M1_Pos)
+#define FIELD_CTRL_REG_TX_ACCESS_ADDR                               \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_EXC_ON_EXP_Pos) |       \
+    (0 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_CRC_Pos) |           \
+    (0 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_WHITENING_Pos) |     \
+    (0 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_TX_DATA_SRC_Pos) |      \
+    (g_ble_phy_data.phy_mode_evpsym <<                              \
+     CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_EVPSYMBOL_LUT_Pos) |         \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_VALID_Pos) |            \
+    (31 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_SIZE_M1_Pos)
+#define FIELD_CTRL_REG_TX_PAYLOAD                                   \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_CRC_Pos) |           \
+    (PHY_WHITENING <<                                               \
+     CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_WHITENING_Pos) |          \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_DMA_MEM_Pos) |       \
+    (g_ble_phy_data.phy_mode_evpsym <<                              \
+     CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_EVPSYMBOL_LUT_Pos) |         \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_VALID_Pos);
+#define FIELD_CTRL_REG_TX_ENC_PAYLOAD                               \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_CRC_Pos) |           \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_WHITENING_Pos) |     \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_DMA_CRYPTO_Pos) |    \
+    (g_ble_phy_data.phy_mode_evpsym <<                              \
+     CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_EVPSYMBOL_LUT_Pos) |         \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_VALID_Pos)
+#define FIELD_CTRL_REG_TX_MIC                                       \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_CRC_Pos) |           \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_WHITENING_Pos) |     \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_DMA_CRYPTO_Pos) |    \
+    (g_ble_phy_data.phy_mode_evpsym <<                              \
+     CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_EVPSYMBOL_LUT_Pos) |         \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_VALID_Pos)
+#define FIELD_CTRL_REG_TX_CRC                                       \
+    (0 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_CRC_Pos) |           \
+    (PHY_WHITENING <<                                               \
+     CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_WHITENING_Pos) |          \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_TX_DATA_SRC_Pos) |      \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_LAST_Pos) |             \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_MSB_FIRST_Pos) |        \
+    (g_ble_phy_data.phy_mode_evpsym <<                              \
+     CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_EVPSYMBOL_LUT_Pos) |         \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_VALID_Pos) |            \
+    (23 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_SIZE_M1_Pos)
+#define FIELD_CTRL_REG_RX_ACCESS_ADDR \
+    (0 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_CRC_Pos) |           \
+    (0 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_WHITENING_Pos) |     \
+    (0 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_TX_DATA_SRC_Pos) |      \
+    (g_ble_phy_data.phy_mode_evpsym <<                              \
+     CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_EVPSYMBOL_LUT_Pos) |         \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_CORR_Pos) |          \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_VALID_Pos) |            \
+    (31 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_SIZE_M1_Pos)
+#define FIELD_CTRL_REG_RX_HEADER \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_EXC_ON_EXP_Pos) |       \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_CRC_Pos) |           \
+    (PHY_WHITENING <<                                               \
+     CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_WHITENING_Pos) |          \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_DMA_MEM_Pos) |       \
+    (g_ble_phy_data.phy_mode_evpsym <<                              \
+     CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_EVPSYMBOL_LUT_Pos) |         \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_VALID_Pos)
+#define FIELD_CTRL_REG_RX_CRC                                       \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_CRC_Pos) |           \
+    (PHY_WHITENING <<                                               \
+     CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_WHITENING_Pos) |          \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_LAST_Pos) |             \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_MSB_FIRST_Pos) |        \
+    (g_ble_phy_data.phy_mode_evpsym <<                              \
+     CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_EVPSYMBOL_LUT_Pos) |         \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_VALID_Pos) |            \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_DMA_MEM_Pos)
+#define FIELD_CTRL_REG_RX_PAYLOAD                                   \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_CRC_Pos) |           \
+    (PHY_WHITENING <<                                               \
+     CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_WHITENING_Pos) |          \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_DMA_MEM_Pos) |       \
+    (g_ble_phy_data.phy_mode_evpsym <<                              \
+     CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_EVPSYMBOL_LUT_Pos) |         \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_VALID_Pos)
+#define FIELD_CTRL_REG_RX_PAYLOAD_WITH_EXC \
+    FIELD_CTRL_REG_RX_PAYLOAD |                                     \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_EXC_ON_EXP_Pos)
+#define FIELD_CTRL_REG_RX_ENC_PAYLOAD                               \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_CRC_Pos) |           \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_WHITENING_Pos) |     \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_XL_DMA_CRYPTO_Pos) |    \
+    (g_ble_phy_data.phy_mode_evpsym <<                              \
+     CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_EVPSYMBOL_LUT_Pos) |         \
+    (1 << CMAC_CM_FIELD_PUSH_CTRL_REG_FIELD_VALID_Pos)
+
+/* RF power up/down delays */
+#define PHY_DELAY_POWER_DN_RX   (23)
+#define PHY_DELAY_POWER_DN_TX   (23)
+#define PHY_DELAY_POWER_UP_RX   (90)
+#define PHY_DELAY_POWER_UP_TX   (75)
+#define PHY_DELAY_TX_RX         ((PHY_DELAY_POWER_DN_TX) + (PHY_DELAY_POWER_UP_RX))
+#define PHY_DELAY_RX_TX         ((PHY_DELAY_POWER_DN_RX) + (PHY_DELAY_POWER_UP_TX))
+
+/* RF TX/RX path delays */
+static const uint8_t g_ble_phy_path_delay_tx[2] = {
+    4, /* 1M = 3.8us */
+    0, /* 2M = 0.2us */
+};
+static const uint8_t g_ble_phy_path_delay_rx[2] = {
+    2, /* 1M = 2.2us */
+    1, /* 2M = 0.8us */
+};
+
+/* Measured and pre-calculated offsets for transitions */
+static const uint8_t g_ble_phy_frame_offset_txrx[4] = {
+    ((BLE_LL_IFS) - (PHY_DELAY_TX_RX) + (4)), /* 2M/1M */
+    ((BLE_LL_IFS) - (PHY_DELAY_TX_RX) + (5)), /* 1M/1M */
+    ((BLE_LL_IFS) - (PHY_DELAY_TX_RX) + (4)), /* 2M/2M */
+    ((BLE_LL_IFS) - (PHY_DELAY_TX_RX) + (5)), /* 1M/2M */
+};
+static const uint8_t g_ble_phy_frame_offset_rxtx[4] = {
+    ((BLE_LL_IFS) - (PHY_DELAY_RX_TX) - (5)), /* 2M/1M */
+    ((BLE_LL_IFS) - (PHY_DELAY_RX_TX) - (6)), /* 1M/1M */
+    ((BLE_LL_IFS) - (PHY_DELAY_RX_TX) - (3)), /* 2M/2M */
+    ((BLE_LL_IFS) - (PHY_DELAY_RX_TX) - (5)), /* 1M/2M */
+};
+
+/* packet start offsets (in usecs) */
+static const uint16_t g_ble_phy_mode_pkt_start_off[BLE_PHY_NUM_MODE] = { 376, 40, 24, 376 };
+
+struct ble_phy_data {
+    uint8_t phy_state;          /* Current state */
+    uint8_t channel;            /* Current PHY channel */
+    uint8_t phy_mode_cur;       /* Current PHY mode */
+    uint8_t phy_mode_tx;        /* TX PHY mode */
+    uint8_t phy_mode_rx;        /* RX PHY mode */
+    uint8_t phy_mode_pre_len;   /* Preamble length - 1 */
+    uint8_t phy_mode_evpsym;    /* EVPSYMBOL_LUT value for fields */
+    uint8_t end_transition;     /* Scheduled transition */
+    uint8_t path_delay_tx;
+    uint8_t path_delay_rx;
+    uint8_t frame_offset_txrx;
+    uint8_t frame_offset_rxtx;
+    uint8_t phy_rx_started;
+    uint8_t phy_encrypted;
+    uint8_t phy_privacy;
+#if MYNEWT_VAL(BLE_LL_DTM)
+    uint8_t phy_whitening;      /* Whitening state (disabled for DTM) */
+#endif
+    uint32_t access_addr;       /* Current access address */
+    uint32_t crc_init;
+    uint32_t llt_at_cputime;
+    uint32_t cputime_at_llt;
+    uint64_t start_llt;
+    struct ble_mbuf_hdr rxhdr;
+    ble_phy_tx_end_func txend_cb;
+    void *txend_arg;
+};
+
+static struct ble_phy_data g_ble_phy_data;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+/* Encryption related variables */
+struct ble_phy_encrypt_obj {
+    uint8_t key[16];
+    uint8_t b0[16];
+    uint8_t b1[16];
+    uint8_t ai[16];
+};
+
+struct ble_phy_encrypt_obj g_ble_phy_encrypt_data;
+
+static void ble_phy_tx_enc_start(void);
+static void ble_phy_rx_enc_start(uint8_t len);
+#endif
+
+#define SW_MAC_EXC_NONE             (0)
+#define SW_MAC_EXC_LL_RX_END        (1)
+#define SW_MAC_EXC_TXEND_CB         (2)
+#define SW_MAC_EXC_LL_RX_START      (3)
+#define SW_MAC_EXC_WFR_TIMER_EXP    (4)
+
+static volatile uint8_t g_sw_mac_exc;
+
+/* Channel index to RF channel mapping */
+static const uint8_t g_ble_phy_chan_to_rf[BLE_PHY_NUM_CHANS] = {
+     1,  2,  3,  4,  5,  6,  7,  8,  9, 10, /* 0-9 */
+    11, 13, 14, 15, 16, 17, 18, 19, 20, 21, /* 10-19 */
+    22, 23, 24, 25, 26, 27, 28, 29, 30, 31, /* 20-29 */
+    32, 33, 34, 35, 36, 37, 38,  0, 12, 39, /* 30-39 */
+};
+
+__attribute__((aligned(4)))
+static uint8_t g_ble_phy_tx_buf[BLE_PHY_MAX_PDU_LEN + 3];
+__attribute__((aligned(4)))
+static uint8_t g_ble_phy_rx_buf[BLE_PHY_MAX_PDU_LEN + 3];
+
+static void ble_phy_irq_field_tx(void);
+static void ble_phy_irq_field_rx(void);
+static void ble_phy_irq_frame_tx(void);
+static void ble_phy_irq_frame_rx(void);
+static bool ble_phy_rx_start_isr(void);
+static void ble_phy_rx_setup_fields(void);
+static void ble_phy_rx_setup_xcvr(void);
+static void ble_phy_mode_apply(uint8_t phy_mode);
+
+void
+FIELD_IRQHandler(void)
+{
+    MCU_DIAG_SER('E');
+
+    switch (g_ble_phy_data.phy_state) {
+    case BLE_PHY_STATE_TX:
+        ble_phy_irq_field_tx();
+        break;
+    case BLE_PHY_STATE_RX:
+        ble_phy_irq_field_rx();
+        break;
+    default:
+        STATS_INC(ble_phy_stats, radio_state_errs);
+        CMAC->CM_EXC_STAT_REG = 0xfffffffe;
+        break;
+    }
+
+    MCU_DIAG_SER('e');
+}
+
+void
+CALLBACK_IRQHandler(void)
+{
+    MCU_DIAG_SER('C');
+
+    /* XXX: clear these for now. */
+    (void)CMAC->CM_BS_SMPL_D_REG;
+
+    /* Clear IRQ*/
+    CMAC->CM_EV_SET_REG = CMAC_CM_EV_SET_REG_EV1C_CALLBACK_VALID_CLR_Msk;
+    CMAC->CM_EXC_STAT_REG = CMAC_CM_EXC_STAT_REG_EXC_FIELD_ON_THR_EXP_Msk;
+
+    /*
+     * Program next frame for transition to TX. CM_EV_LINKUP_REG register to
+     * enable actual transition can be set later, we just need to make sure 2nd
+     * frame is already set before current frame is finished - this guarantees
+     * that frame for transition will be moved to 1st frame once current frame
+     * is popped off the queue.
+     */
+    CMAC->CM_FRAME_2_REG = CMAC_CM_FRAME_2_REG_FRAME_VALID_Msk |
+                           CMAC_CM_FRAME_2_REG_FRAME_TX_Msk |
+                           CMAC_CM_FRAME_2_REG_FRAME_EXC_ON_BS_START_Msk |
+                           ((g_ble_phy_data.frame_offset_rxtx) <<
+                            CMAC_CM_FRAME_2_REG_FRAME_START_OFFSET_Pos);
+
+    /*
+     * We just got an access address match so do this as early as possible
+     * to save time in the field rx isr.
+     */
+    ble_phy_rx_start_isr();
+
+    MCU_DIAG_SER('c');
+}
+
+void
+FRAME_IRQHandler(void)
+{
+    MCU_DIAG_SER('F');
+
+    switch (g_ble_phy_data.phy_state) {
+    case BLE_PHY_STATE_TX:
+        ble_phy_irq_frame_tx();
+        break;
+    case BLE_PHY_STATE_RX:
+        ble_phy_irq_frame_rx();
+        break;
+    default:
+        STATS_INC(ble_phy_stats, radio_state_errs);
+        CMAC->CM_EXC_STAT_REG = 0xfffffffe;
+        break;
+    }
+
+    MCU_DIAG_SER('f');
+}
+
+void
+SW_MAC_IRQHandler(void)
+{
+    uint8_t exc;
+    int rc;
+
+    MCU_DIAG_SER('S');
+
+    CMAC->CM_EXC_STAT_REG = CMAC_CM_EXC_STAT_REG_EXC_SW_MAC_Msk;
+    assert(g_sw_mac_exc);
+
+    exc = g_sw_mac_exc;
+    g_sw_mac_exc = 0;
+
+    MCU_DIAG_SER('0' + exc);
+
+    /* Next SW_MAC handover can now be queued */
+    os_arch_cmac_bs_ctrl_irq_unblock();
+
+    switch (exc) {
+    case SW_MAC_EXC_TXEND_CB:
+        assert(g_ble_phy_data.txend_cb);
+        g_ble_phy_data.txend_cb(g_ble_phy_data.txend_arg);
+        break;
+    case SW_MAC_EXC_WFR_TIMER_EXP:
+        ble_ll_wfr_timer_exp(NULL);
+        break;
+    case SW_MAC_EXC_LL_RX_START:
+        /* Call Link Layer receive start function */
+        rc = ble_ll_rx_start(&g_ble_phy_rx_buf[0], g_ble_phy_data.channel,
+                             &g_ble_phy_data.rxhdr);
+        if (rc == 0) {
+            /* Set rx started flag and enable rx end ISR */
+            g_ble_phy_data.phy_rx_started = 1;
+
+            /* No transition */
+            CMAC->CM_EV_LINKUP_REG = CMAC_CM_EV_LINKUP_REG_LU_PHY_TO_IDLE_2_NONE_Msk |
+                                     CMAC_CM_EV_LINKUP_REG_LU_FRAME_START_2_NONE_Msk;
+        } else if (rc > 0) {
+            g_ble_phy_data.phy_rx_started = 1;
+
+            /* Setup transition */
+            CMAC->CM_EV_LINKUP_REG = CMAC_CM_EV_LINKUP_REG_LU_PHY_TO_IDLE_2_EXC_Msk |
+                                     CMAC_CM_EV_LINKUP_REG_LU_FRAME_START_2_PHY_TO_IDLE_Msk;
+        } else {
+            /* Disable PHY */
+            ble_phy_disable();
+            STATS_INC(ble_phy_stats, rx_aborts);
+        }
+        break;
+    case SW_MAC_EXC_LL_RX_END:
+        /* Call LL end processing */
+        rc = ble_ll_rx_end(&g_ble_phy_rx_buf[0], &g_ble_phy_data.rxhdr);
+        if (rc < 0) {
+            ble_phy_disable();
+        }
+        break;
+    default:
+        assert(0);
+        break;
+    }
+
+    MCU_DIAG_SER('s');
+}
+
+static inline uint32_t
+ble_phy_convert_and_record_start_time(uint32_t cputime, uint8_t rem_usecs)
+{
+    uint64_t ll_val;
+
+    ll_val = cmac_timer_convert_hal2llt(cputime);
+
+    /*
+     * Since we just converted cputime to the LL timer, record both these
+     * values as they will be used to calculate packet reception start time.
+     */
+    g_ble_phy_data.cputime_at_llt = cputime;
+    g_ble_phy_data.llt_at_cputime = ll_val;
+    g_ble_phy_data.start_llt = ll_val + rem_usecs;
+
+    return ll_val;
+}
+
+static inline void
+ble_phy_sw_mac_handover(uint8_t exc)
+{
+    assert(!g_sw_mac_exc);
+
+    g_sw_mac_exc = exc;
+
+    CMAC->CM_EV_SET_REG = CMAC_CM_EV_SET_REG_EV1C_SW_MAC_Msk;
+
+    /*
+     * We want SW_MAC to be fired just after BS_CTRL interrupt so we block
+     * BS_CTRL temporarily and SW_MAC is next in order of interrupts priority.
+     */
+    os_arch_cmac_bs_ctrl_irq_block();
+}
+
+static void
+ble_phy_rx_end_isr(void)
+{
+    struct ble_mbuf_hdr *ble_hdr;
+
+    /* XXX just clear captured timer for now. Handle rx end time */
+    (void)CMAC->CM_TS1_REG;
+
+    /* Set RSSI and CRC status flag in header */
+    ble_hdr = &g_ble_phy_data.rxhdr;
+
+    /* Count PHY crc errors and valid packets */
+    if (CMAC->CM_CRC_REG != 0) {
+        STATS_INC(ble_phy_stats, rx_crc_err);
+    } else {
+        STATS_INC(ble_phy_stats, rx_valid);
+        ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_CRC_OK;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+        if (g_ble_phy_data.phy_encrypted) {
+            /* Only set MIC failure flag if frame is not zero length */
+            if (g_ble_phy_rx_buf[1] != 0) {
+                if (CMAC->CM_CRYPTO_STAT_REG != 0) {
+                    ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_MIC_FAILURE;
+                } else {
+                    g_ble_phy_rx_buf[1] = g_ble_phy_rx_buf[1] - 4;
+                }
+            }
+        }
+#endif
+    }
+
+    ble_phy_sw_mac_handover(SW_MAC_EXC_LL_RX_END);
+}
+
+static bool
+ble_phy_rx_start_isr(void)
+{
+    uint32_t llt32;
+    uint32_t llt_10_0;
+    uint32_t llt_10_0_mask;
+    uint32_t timestamp;
+    uint32_t ticks;
+    uint32_t usecs;
+    struct ble_mbuf_hdr *ble_hdr;
+
+    /* Initialize the ble mbuf header */
+    ble_hdr = &g_ble_phy_data.rxhdr;
+    ble_hdr->rxinfo.flags = ble_ll_state_get();
+    ble_hdr->rxinfo.channel = g_ble_phy_data.channel;
+    ble_hdr->rxinfo.handle = 0;
+    ble_hdr->rxinfo.phy = ble_phy_get_cur_phy();
+    ble_hdr->rxinfo.phy_mode = g_ble_phy_data.phy_mode_rx;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    ble_hdr->rxinfo.user_data = NULL;
+#endif
+
+    /* Read the latched RSSI value */
+    ble_hdr->rxinfo.rssi = ble_rf_get_rssi();
+#if MYNEWT_VAL(CMAC_DEBUG_DATA_ENABLE)
+    g_cmac_shared_data.debug.last_rx_rssi = ble_hdr->rxinfo.rssi;
+#endif
+
+    /* Count rx starts */
+    STATS_INC(ble_phy_stats, rx_starts);
+
+    /*
+     * Calculate packet start time. Note that we have only received the
+     * access address at this point but we should have the 1st symbol and
+     * thus the timestamp should be set (this is based on looking at the diag
+     * signals). For now, lets make sure that the dirty bit is set. The
+     * dirty bit means that the timestamp was set since the last time cleared.
+     * Note that we need to read the timestamp first to guarantee it was set
+     * before reading the LL timer.
+     */
+    timestamp = CMAC->CM_TS1_REG;
+    assert((timestamp & CMAC_CM_TS1_REG_TS1_DIRTY_Msk) != 0);
+
+    /* Get the LL timer (only need 32 bits) */
+    llt32 = cmac_timer_read32();
+
+    /*
+     * We assume that the timestamp was set within 11 bits, or 2047 usecs, of
+     * when we read the ll timer. We assume this because we need to calculate
+     * the LL timer value at the timestamp. If the low 11 bits of the LL timer
+     * are greater than the timestamp, it means that the upper bits of the
+     * timestamp are correct. If the timestamp value is greater, it means the
+     * timer wrapped the 11 bits and we need to adjust the LL timer value.
+     */
+    llt_10_0_mask = (CMAC_CM_TS1_REG_TS1_TIMER1_9_0_Msk |
+                     CMAC_CM_TS1_REG_TS1_TIMER1_10_Msk);
+    timestamp &= llt_10_0_mask;
+    llt_10_0 = llt32 & llt_10_0_mask;
+    llt32 &= ~llt_10_0_mask;
+    if (timestamp > llt_10_0) {
+        llt32 -= 2048;
+    }
+    llt32 |= timestamp;
+
+    /* Actual RX start time needs to account for preamble and access address */
+    llt32 -= g_ble_phy_mode_pkt_start_off[g_ble_phy_data.phy_mode_rx] +
+             g_ble_phy_data.path_delay_rx;
+
+    if (llt32 < g_ble_phy_data.llt_at_cputime) {
+        g_ble_phy_data.llt_at_cputime -= 31;
+        g_ble_phy_data.cputime_at_llt--;
+    }
+
+    /*
+     * We now have the LL timer when the packet was received. Get the cputime
+     * and the leftover usecs.
+     */
+    usecs = llt32 - g_ble_phy_data.llt_at_cputime;
+    ticks = os_cputime_usecs_to_ticks(usecs);
+    ble_hdr->beg_cputime = g_ble_phy_data.cputime_at_llt + ticks;
+    ble_hdr->rem_usecs = usecs - os_cputime_ticks_to_usecs(ticks);
+
+    return true;
+}
+
+static void
+ble_phy_irq_field_tx_exc_bs_start_4this(void)
+{
+    CMAC->CM_EXC_STAT_REG = CMAC_CM_EXC_STAT_REG_EXC_BS_START_4THIS_Msk;
+
+    if (g_ble_phy_data.end_transition == BLE_PHY_TRANSITION_TX_RX) {
+        /*
+         * Setup 2nd frame that will start after current one.
+         * -2us offset to adjust for allowed active clock accuracy.
+         */
+        CMAC->CM_FRAME_2_REG = CMAC_CM_FRAME_2_REG_FRAME_VALID_Msk |
+                               (((g_ble_phy_data.frame_offset_txrx) - 2) <<
+                                CMAC_CM_FRAME_2_REG_FRAME_START_OFFSET_Pos);
+
+        /* Next frame starts automatically on phy2idle */
+        CMAC->CM_EV_LINKUP_REG = CMAC_CM_EV_LINKUP_REG_LU_PHY_TO_IDLE_2_EXC_Msk |
+                                 CMAC_CM_EV_LINKUP_REG_LU_FRAME_START_2_PHY_TO_IDLE_Msk;
+
+        ble_phy_wfr_enable(BLE_PHY_WFR_ENABLE_TXRX, g_ble_phy_data.phy_mode_rx, 0);
+    } else {
+        CMAC->CM_EV_LINKUP_REG = CMAC_CM_EV_LINKUP_REG_LU_PHY_TO_IDLE_2_EXC_Msk |
+                                 CMAC_CM_EV_LINKUP_REG_LU_FRAME_START_2_NONE_Msk;
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    if (g_ble_phy_data.phy_encrypted && (g_ble_phy_tx_buf[1] != 0)) {
+        ble_phy_tx_enc_start();
+    }
+#endif
+}
+
+static void
+ble_phy_irq_field_tx_exc_field_on_thr_exp(void)
+{
+    CMAC->CM_EXC_STAT_REG = CMAC_CM_EXC_STAT_REG_EXC_FIELD_ON_THR_EXP_Msk;
+    (void)CMAC->CM_TS1_REG;
+
+    /*  Set up remaining field (CRC) */
+    CMAC->CM_FIELD_PUSH_CTRL_REG = FIELD_CTRL_REG_TX_CRC;
+}
+
+static void
+ble_phy_irq_field_tx(void)
+{
+    uint32_t stat;
+
+    stat = CMAC->CM_EXC_STAT_REG;
+
+    if (stat & CMAC_CM_EXC_STAT_REG_EXC_BS_START_4THIS_Msk) {
+        MCU_DIAG_SER('6');
+        ble_phy_irq_field_tx_exc_bs_start_4this();
+    }
+
+    if (stat & CMAC_CM_EXC_STAT_REG_EXC_FIELD_ON_THR_EXP_Msk) {
+        MCU_DIAG_SER('7');
+        ble_phy_irq_field_tx_exc_field_on_thr_exp();
+    }
+}
+
+static void
+ble_phy_irq_frame_tx_exc_bs_stop(void)
+{
+    CMAC->CM_EXC_STAT_REG = CMAC_CM_EXC_STAT_REG_EXC_BS_STOP_Msk;
+
+    /* Clear latched timestamp so we do not have error on next frame */
+    (void)CMAC->CM_TS1_REG;
+
+    if (g_ble_phy_data.end_transition == BLE_PHY_TRANSITION_TX_RX) {
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+        ble_phy_mode_apply(g_ble_phy_data.phy_mode_rx);
+#endif
+        ble_phy_rx_setup_fields();
+    } else {
+        CMAC->CM_EV_LINKUP_REG = CMAC_CM_EV_LINKUP_REG_LU_PHY_TO_IDLE_2_EXC_Msk |
+                                 CMAC_CM_EV_LINKUP_REG_LU_FRAME_START_2_NONE_Msk;
+    }
+
+    if (g_ble_phy_data.txend_cb) {
+        ble_phy_sw_mac_handover(SW_MAC_EXC_TXEND_CB);
+        return;
+    }
+}
+
+static void
+ble_phy_irq_frame_tx_exc_phy_to_idle_4this(void)
+{
+    CMAC->CM_EXC_STAT_REG = CMAC_CM_EXC_STAT_REG_EXC_PHY_TO_IDLE_4THIS_Msk;
+
+    if (g_ble_phy_data.end_transition == BLE_PHY_TRANSITION_TX_RX) {
+        ble_phy_rx_setup_xcvr();
+
+        g_ble_phy_data.phy_state = BLE_PHY_STATE_RX;
+    } else {
+        /*
+         * Disable explicitly in case RX-TX was done (we cannot setup for auto
+         * disable in such case) */
+        ble_rf_stop();
+
+        g_ble_phy_data.phy_state = BLE_PHY_STATE_IDLE;
+    }
+
+    g_ble_phy_data.end_transition = BLE_PHY_TRANSITION_NONE;
+}
+
+static void
+ble_phy_irq_frame_tx(void)
+{
+    uint32_t stat;
+
+    stat = CMAC->CM_EXC_STAT_REG;
+
+    /*
+     * In case of phy2idle this should be first and only exception we handle
+     * here. This is because in case of TX-RX transition frame_start will occur
+     * at the same as phy2idle so we will have 2 exceptions here. To handle this
+     * properly we first need to handle phy2idle in TX state and keep frame_start
+     * pending so it will be called again in RX state.
+     */
+    if (stat & CMAC_CM_EXC_STAT_REG_EXC_PHY_TO_IDLE_4THIS_Msk) {
+        MCU_DIAG_SER('6');
+        ble_phy_irq_frame_tx_exc_phy_to_idle_4this();
+        return;
+    }
+
+    if (stat & CMAC_CM_EXC_STAT_REG_EXC_FRAME_START_Msk) {
+        MCU_DIAG_SER('7');
+        CMAC->CM_EXC_STAT_REG = CMAC_CM_EXC_STAT_REG_EXC_FRAME_START_Msk;
+    }
+
+    if (stat & CMAC_CM_EXC_STAT_REG_EXC_BS_STOP_Msk) {
+        MCU_DIAG_SER('8');
+        ble_phy_irq_frame_tx_exc_bs_stop();
+    }
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+static void
+ble_phy_field_rx_encrypted(uint32_t len)
+{
+    if (len) {
+        /*
+         * An encrypted frame should have a minimum length of 5
+         * bytes (at least one for payload and 4 for MIC). If the
+         * length is less than 5 this frame is bogus and will most
+         * likely fail CRC. We still need to process this frame
+         * though as we need to call the handover function with
+         * the frame. If this happens we will not bother to
+         * run the remaining bytes through the accelerator; just
+         * process them like normal and generate (a hopefully
+         * incorrect) CRC.
+         */
+        if (len >= 5) {
+            /* Start the crypto accelerator */
+            ble_phy_rx_enc_start(len);
+
+            /*
+             * We have already processed one byte; process remaining
+             * payload and MIC. Note: length contains MIC.
+             */
+            len -= 2;
+            CMAC->CM_FIELD_PUSH_DATA_REG = FIELD_DATA_REG_DMA_RX(4, len);
+            CMAC->CM_FIELD_PUSH_CTRL_REG = FIELD_CTRL_REG_RX_ENC_PAYLOAD;
+
+            /* CRC */
+            CMAC->CM_FIELD_PUSH_DATA_REG = FIELD_DATA_REG_DMA_RX(4 + len, 3);
+            CMAC->CM_FIELD_PUSH_CTRL_REG = FIELD_CTRL_REG_RX_CRC;
+        } else {
+            /* We have processed one byte so far. Send remaining
+               payload bytes to normal rx payload processing */
+            len -= 2;
+            if (len) {
+                CMAC->CM_FIELD_PUSH_DATA_REG = FIELD_DATA_REG_DMA_RX(4, len);
+                CMAC->CM_FIELD_PUSH_CTRL_REG = FIELD_CTRL_REG_RX_PAYLOAD;
+            }
+
+            CMAC->CM_FIELD_PUSH_DATA_REG = FIELD_DATA_REG_DMA_RX(4 + len, 3);
+            CMAC->CM_FIELD_PUSH_CTRL_REG = FIELD_CTRL_REG_RX_CRC;
+
+            /* Clear crypto pre-buffer */
+            CMAC->CM_CRYPTO_CTRL_REG = CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_SW_REQ_PBUF_CLR_Msk;
+        }
+    } else {
+        /* We programmed one byte, so get next two bytes for CRC */
+        CMAC->CM_FIELD_PUSH_DATA_REG = FIELD_DATA_REG_DMA_RX(4, 1);
+        CMAC->CM_FIELD_PUSH_CTRL_REG = FIELD_CTRL_REG_RX_CRC;
+    }
+}
+#endif
+
+static void
+ble_phy_field_rx_unencrypted(uint32_t len)
+{
+    uint8_t pduhdr;
+    uint8_t adva_thr;
+
+    if (len) {
+        pduhdr = g_ble_phy_rx_buf[0];
+        adva_thr = 0;
+
+        /*
+         * Setup interrupt after AdvA to start address resolving if
+         * privacy is enabled and TxAdd bit is set.
+         */
+        if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) &&
+            g_ble_phy_data.phy_privacy && (pduhdr & 0x40)) {
+
+            /*
+             * For legacy advertising AdvA ends at 6th byte.
+             * For extended advertising AdvA ends at 8th byte.
+             * We already programmed 2 bytes of payload so need
+             * to adjust threshold accordingly or just reset it
+             * in case there is not enough bytes in PDU to fit AdvA.
+             */
+            adva_thr = (pduhdr & 0x0f) == 0x07 ? 6 : 4;
+            if (len >= adva_thr + 2) {
+                CMAC->CM_FIELD_PUSH_DATA_REG = FIELD_DATA_REG_DMA_RX(4, adva_thr);
+                CMAC->CM_FIELD_PUSH_CTRL_REG = FIELD_CTRL_REG_RX_PAYLOAD_WITH_EXC;
+            } else {
+                adva_thr = 0;
+            }
+        }
+
+        CMAC->CM_FIELD_PUSH_DATA_REG = FIELD_DATA_REG_DMA_RX(4 + adva_thr,
+                                                             len - adva_thr);
+        CMAC->CM_FIELD_PUSH_CTRL_REG = FIELD_CTRL_REG_RX_PAYLOAD;
+    }
+
+    CMAC->CM_FIELD_PUSH_DATA_REG = FIELD_DATA_REG_DMA_RX(4 + len, 1);
+    CMAC->CM_FIELD_PUSH_CTRL_REG = FIELD_CTRL_REG_RX_CRC;
+}
+
+static void
+ble_phy_irq_field_rx_exc_field_on_thr_exp(void)
+{
+    uint32_t len;
+    uint32_t smpl;
+
+    CMAC->CM_EXC_STAT_REG = CMAC_CM_EXC_STAT_REG_EXC_FIELD_ON_THR_EXP_Msk;
+
+    smpl = CMAC->CM_BS_SMPL_ST_REG;
+
+    if ((smpl & CMAC_CM_BS_SMPL_ST_REG_FIELD_CNT_LATCHED_Msk) == 1) {
+        assert(g_ble_phy_data.phy_rx_started == 0);
+
+        /* Clear this */
+        (void)CMAC->CM_TS1_REG;
+
+        /* Read length of frame */
+        len = CMAC->CM_BS_SMPL_D_REG;
+        len = len & 0xFF;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+        if (g_ble_phy_data.phy_encrypted) {
+            ble_phy_field_rx_encrypted(len);
+        } else {
+            ble_phy_field_rx_unencrypted(len);
+        }
+#else
+        ble_phy_field_rx_unencrypted(len);
+#endif
+
+        ble_phy_sw_mac_handover(SW_MAC_EXC_LL_RX_START);
+    } else if ((smpl & CMAC_CM_BS_SMPL_ST_REG_FIELD_CNT_LATCHED_Msk) == 3) {
+        (void)CMAC->CM_BS_SMPL_D_REG;
+
+        assert(g_ble_phy_data.phy_privacy);
+
+        /*
+         * Resolve only if RPA is received. AdvA is at different offset
+         * in ExtAdv PDU. TxAdd was already checked before programming
+         * field threshold.
+         */
+        if ((g_ble_phy_rx_buf[0] & 0x0f) == 0x07) {
+            if ((g_ble_phy_rx_buf[9] & 0xc0) == 0x40) {
+                ble_hw_resolv_proc_start(&g_ble_phy_rx_buf[4]);
+            }
+        } else {
+            if ((g_ble_phy_rx_buf[7] & 0xc0) == 0x40) {
+                ble_hw_resolv_proc_start(&g_ble_phy_rx_buf[2]);
+            }
+        }
+    } else {
+        assert(0);
+    }
+}
+
+static void
+ble_phy_irq_field_rx_exc_stat_reg_exc_corr_timeout(void)
+{
+    CMAC->CM_EXC_STAT_REG = CMAC_CM_EXC_STAT_REG_EXC_CORR_TIMEOUT_Msk;
+    CMAC->CM_EXC_STAT_REG = CMAC_CM_EXC_STAT_REG_EXC_BS_STOP_Msk;
+    CMAC->CM_EV_LINKUP_REG = CMAC_CM_EV_LINKUP_REG_LU_FRAME_START_2_NONE_Msk;
+
+    ble_phy_sw_mac_handover(SW_MAC_EXC_WFR_TIMER_EXP);
+}
+
+static void
+ble_phy_irq_field_rx(void)
+{
+    uint32_t stat;
+
+    stat = CMAC->CM_EXC_STAT_REG;
+
+    if (stat & CMAC_CM_EXC_STAT_REG_EXC_FIELD_ON_THR_EXP_Msk) {
+        MCU_DIAG_SER('1');
+        ble_phy_irq_field_rx_exc_field_on_thr_exp();
+    }
+
+    if (stat & CMAC_CM_EXC_STAT_REG_EXC_CORR_TIMEOUT_Msk) {
+        MCU_DIAG_SER('2');
+        ble_phy_irq_field_rx_exc_stat_reg_exc_corr_timeout();
+    }
+}
+
+static void
+ble_phy_irq_frame_rx_exc_phy_to_idle_4this(void)
+{
+    uint8_t rf_chan;
+
+    CMAC->CM_EXC_STAT_REG = CMAC_CM_EXC_STAT_REG_EXC_PHY_TO_IDLE_4THIS_Msk;
+
+    /* We are here only on transition, so switch to TX */
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    ble_phy_mode_apply(g_ble_phy_data.phy_mode_tx);
+#endif
+    rf_chan = g_ble_phy_chan_to_rf[g_ble_phy_data.channel];
+    ble_rf_setup_tx(rf_chan, g_ble_phy_data.phy_mode_tx);
+    g_ble_phy_data.phy_state = BLE_PHY_STATE_TX;
+
+    /* We do not want FIELD/FRAME interrupts until ble_phy_tx() has pushed all
+     * fields.
+     */
+    NVIC_DisableIRQ(FRAME_IRQn);
+    NVIC_DisableIRQ(FIELD_IRQn);
+}
+
+static void
+ble_phy_irq_frame_rx_exc_frame_start(void)
+{
+    CMAC->CM_EXC_STAT_REG = CMAC_CM_EXC_STAT_REG_EXC_FRAME_START_Msk;
+}
+
+static void
+ble_phy_irq_frame_rx_exc_bs_stop(void)
+{
+    CMAC->CM_EXC_STAT_REG = CMAC_CM_EXC_STAT_REG_EXC_BS_STOP_Msk;
+    ble_phy_rx_end_isr();
+}
+
+static void
+ble_phy_irq_frame_rx(void)
+{
+    uint32_t stat;
+
+    stat = CMAC->CM_EXC_STAT_REG;
+
+    if (stat & CMAC_CM_EXC_STAT_REG_EXC_PHY_TO_IDLE_4THIS_Msk) {
+        MCU_DIAG_SER('3');
+        ble_phy_irq_frame_rx_exc_phy_to_idle_4this();
+        return;
+    }
+
+    if (stat & CMAC_CM_EXC_STAT_REG_EXC_FRAME_START_Msk) {
+        MCU_DIAG_SER('1');
+        ble_phy_irq_frame_rx_exc_frame_start();
+    }
+
+    if (stat & CMAC_CM_EXC_STAT_REG_EXC_BS_STOP_Msk) {
+        MCU_DIAG_SER('2');
+        ble_phy_irq_frame_rx_exc_bs_stop();
+    }
+}
+
+static void
+ble_phy_mode_apply(uint8_t phy_mode)
+{
+    if (phy_mode == g_ble_phy_data.phy_mode_cur) {
+        return;
+    }
+
+    switch (phy_mode) {
+    case BLE_PHY_MODE_1M:
+#if MYNEWT_VAL(BLE_PHY_RF_HP_MODE)
+        CMAC_SETREGF(CM_PHY_CTRL2_REG, CORR_CLK_MODE, 3);
+#endif
+        CMAC_SETREGF(CM_PHY_CTRL2_REG, PHY_MODE, 1);
+        g_ble_phy_data.phy_mode_evpsym = 1; /* 1000 ns per symbol */
+        g_ble_phy_data.phy_mode_pre_len = 7;
+        break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY)
+    case BLE_PHY_MODE_2M:
+#if MYNEWT_VAL(BLE_PHY_RF_HP_MODE)
+        CMAC_SETREGF(CM_PHY_CTRL2_REG, CORR_CLK_MODE, 2);
+#endif
+        CMAC_SETREGF(CM_PHY_CTRL2_REG, PHY_MODE, 0);
+        g_ble_phy_data.phy_mode_evpsym = 0; /* 500 ns per symbol */
+        g_ble_phy_data.phy_mode_pre_len = 15;
+        break;
+#endif
+    default:
+        assert(0);
+        return;
+    }
+
+    g_ble_phy_data.phy_mode_cur = phy_mode;
+}
+
+void
+ble_phy_mode_set(uint8_t tx_phy_mode, uint8_t rx_phy_mode)
+{
+    uint8_t txrx;
+    uint8_t rxtx;
+
+    g_ble_phy_data.phy_mode_tx = tx_phy_mode;
+    g_ble_phy_data.phy_mode_rx = rx_phy_mode;
+
+    g_ble_phy_data.path_delay_tx = g_ble_phy_path_delay_tx[tx_phy_mode - 1];
+    g_ble_phy_data.path_delay_rx = g_ble_phy_path_delay_rx[rx_phy_mode - 1];
+
+    /*
+     * Calculate index of transition in frame offset array without tons of
+     * branches. Note that transitions have to be in specific order in array.
+     *
+     * phy_mode 1M = 01b
+     * phy_mode 2M = 10b
+     *
+     * 1M/1M = 01b | 00b = 01b
+     * 1M/2M = 01b | 10b = 11b
+     * 2M/1M = 00b | 00b = 00b
+     * 2M/2M = 00b | 10b = 10b
+     */
+    txrx = (tx_phy_mode & 0x01) | (rx_phy_mode & 0x02);
+    rxtx = (rx_phy_mode & 0x01) | (tx_phy_mode & 0x02);
+    g_ble_phy_data.frame_offset_txrx = g_ble_phy_frame_offset_txrx[txrx];
+    g_ble_phy_data.frame_offset_rxtx = g_ble_phy_frame_offset_rxtx[rxtx];
+}
+
+int
+ble_phy_get_cur_phy(void)
+{
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    switch (g_ble_phy_data.phy_mode_cur) {
+    case BLE_PHY_MODE_1M:
+        return BLE_PHY_1M;
+    case BLE_PHY_MODE_2M:
+        return BLE_PHY_2M;
+    default:
+        assert(0);
+        return -1;
+    }
+#else
+    return BLE_PHY_1M;
+#endif
+}
+
+/**
+ * Copies the data from the phy receive buffer into a mbuf chain.
+ *
+ * @param dptr Pointer to receive buffer
+ * @param rxpdu Pointer to already allocated mbuf chain
+ *
+ * NOTE: the packet header already has the total mbuf length in it. The
+ * lengths of the individual mbufs are not set prior to calling.
+ *
+ */
+void
+ble_phy_rxpdu_copy(uint8_t *dptr, struct os_mbuf *rxpdu)
+{
+    uint32_t rem_len;
+    uint32_t copy_len;
+    uint32_t block_len;
+    uint32_t block_rem_len;
+    void *dst;
+    void *src;
+    struct os_mbuf * om;
+
+    /* Better be aligned */
+    assert(((uint32_t)dptr & 3) == 0);
+
+    block_len = rxpdu->om_omp->omp_databuf_len;
+    rem_len = OS_MBUF_PKTHDR(rxpdu)->omp_len;
+    src = dptr;
+
+    /*
+     * Setup for copying from first mbuf which is shorter due to packet header
+     * and extra leading space
+     */
+    copy_len = block_len - rxpdu->om_pkthdr_len - 4;
+    om = rxpdu;
+    dst = om->om_data;
+
+    while (om) {
+        /*
+         * Always copy blocks of length aligned to word size, only last mbuf
+         * will have remaining non-word size bytes appended.
+         */
+        block_rem_len = copy_len;
+        copy_len = min(copy_len, rem_len);
+        copy_len &= ~3;
+
+        dst = om->om_data;
+        om->om_len = copy_len;
+        rem_len -= copy_len;
+        block_rem_len -= copy_len;
+
+        __asm__ volatile (".syntax unified              \n"
+                          "   mov  r4, %[len]           \n"
+                          "   b    2f                   \n"
+                          "1: ldr  r3, [%[src], %[len]] \n"
+                          "   str  r3, [%[dst], %[len]] \n"
+                          "2: subs %[len], #4           \n"
+                          "   bpl  1b                   \n"
+                          "   adds %[src], %[src], r4   \n"
+                          "   adds %[dst], %[dst], r4   \n"
+                          : [dst] "+l" (dst), [src] "+l" (src),
+                            [len] "+l" (copy_len)
+                          :
+                          : "r3", "r4", "memory");
+
+        if ((rem_len < 4) && (block_rem_len >= rem_len)) {
+            break;
+        }
+
+        /* Move to next mbuf */
+        om = SLIST_NEXT(om, om_next);
+        copy_len = block_len;
+    }
+
+    /* Copy remaining bytes, if any, to last mbuf */
+    om->om_len += rem_len;
+    __asm__ volatile (".syntax unified              \n"
+                      "   b    2f                   \n"
+                      "1: ldrb r3, [%[src], %[len]] \n"
+                      "   strb r3, [%[dst], %[len]] \n"
+                      "2: subs %[len], #1           \n"
+                      "   bpl  1b                   \n"
+                      : [len] "+l" (rem_len)
+                      : [dst] "l" (dst), [src] "l" (src)
+                      : "r3", "memory");
+
+    /* Copy header */
+    memcpy(BLE_MBUF_HDR_PTR(rxpdu), &g_ble_phy_data.rxhdr,
+           sizeof(struct ble_mbuf_hdr));
+}
+
+void
+ble_phy_wfr_enable(int txrx, uint8_t tx_phy_mode, uint32_t wfr_usecs)
+{
+    uint64_t llt;
+    uint32_t corr_window;
+    uint32_t llt_z_ticks;
+    uint32_t aa_time;
+
+    /*
+     * RX is started 2us earlier due to allowed clock accuracy and it should end
+     * 2us later for the same reason. Preamble is always 8us (8 symbols on 1M,
+     * 16 symbols on 2M) and Access Address is 32us on 1M and 16us on 2M. Add
+     * 1us just in case...
+     */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY)
+    aa_time = 2 + (g_ble_phy_data.phy_mode_rx == BLE_PHY_MODE_1M ? 40 : 24) + 2 + 1;
+#else
+    aa_time = 45;
+#endif
+
+    if (txrx == BLE_PHY_WFR_ENABLE_TXRX) {
+        CMAC_SETREGF(CM_PHY_CTRL2_REG, CORR_WINDOW, aa_time);
+        CMAC->CM_EV_LINKUP_REG = CMAC_CM_EV_LINKUP_REG_LU_CORR_TMR_LD_2_CORR_START_Msk;
+    } else if (wfr_usecs < 16384) {
+        CMAC_SETREGF(CM_PHY_CTRL2_REG, CORR_WINDOW, wfr_usecs + aa_time);
+        CMAC->CM_EV_LINKUP_REG = CMAC_CM_EV_LINKUP_REG_LU_CORR_TMR_LD_2_CORR_START_Msk;
+    } else {
+        wfr_usecs += aa_time;
+        llt = g_ble_phy_data.start_llt;
+
+        /*
+         * wfr is outside range of CORR_WINDOW so we need to use LLT to start
+         * correlator timeout with some delay. Let's use ~10ms as new CORR_WINDOW
+         * value (does not really matter, just had to pick something) so need to
+         * calculate how many hi-Z ticks of delay we need.
+         */
+        llt_z_ticks = (wfr_usecs - 10000) / 1024;
+
+        /* New CORR_WINDOW is wfr adjusted by hi-Z ticks and remainder of 1st tick. */
+        corr_window = wfr_usecs;
+        corr_window -= llt_z_ticks * 1024;
+        corr_window -= 1024 - (llt & 0x3ff);
+
+        CMAC->CM_LL_TIMER1_36_10_EQ_Z_REG = (llt >> 10) + llt_z_ticks;
+        CMAC_SETREGF(CM_PHY_CTRL2_REG, CORR_WINDOW, corr_window);
+        CMAC->CM_EV_LINKUP_REG = CMAC_CM_EV_LINKUP_REG_LU_CORR_TMR_LD_2_TMR1_36_10_EQ_Z_Msk;
+    }
+}
+
+int
+ble_phy_init(void)
+{
+    g_ble_phy_data.phy_state = BLE_PHY_STATE_IDLE;
+#if MYNEWT_VAL(BLE_LL_DTM)
+    g_ble_phy_data.phy_whitening = 1;
+#endif
+
+    ble_rf_init();
+
+    /*
+     * 9_0_EQ_X can be linked to start RX/TX so we'll use this one for
+     * scheduling TX/RX start - make sure it's not linked to LL_TIMER2LLC
+     */
+    CMAC->CM_LL_INT_SEL_REG &= ~CMAC_CM_LL_INT_SEL_REG_LL_TIMER1_9_0_EQ_X_SEL_Msk;
+
+    CMAC->CM_PHY_CTRL_REG = ((PHY_DELAY_POWER_DN_RX - 1) << 24) |
+                            ((PHY_DELAY_POWER_DN_TX - 1) << 16) |
+                            ((PHY_DELAY_POWER_UP_RX - 1) << 8) |
+                            ((PHY_DELAY_POWER_UP_TX - 1));
+
+#if MYNEWT_VAL(BLE_PHY_RF_HP_MODE)
+    CMAC->CM_PHY_CTRL2_REG = CMAC_CM_PHY_CTRL2_REG_PHY_MODE_Msk |
+                             (3 << CMAC_CM_PHY_CTRL2_REG_CORR_CLK_MODE_Pos);
+#else
+    CMAC->CM_PHY_CTRL2_REG = CMAC_CM_PHY_CTRL2_REG_PHY_MODE_Msk |
+                             (2 << CMAC_CM_PHY_CTRL2_REG_CORR_CLK_MODE_Pos);
+#endif
+
+    CMAC_SETREGF(CM_CTRL2_REG, WHITENING_MODE, 0);
+    CMAC_SETREGF(CM_CTRL2_REG, CRC_MODE, 0);
+
+    /* Setup for 1M by default */
+    ble_phy_mode_set(BLE_PHY_MODE_1M, BLE_PHY_MODE_1M);
+    ble_phy_mode_apply(BLE_PHY_MODE_1M);
+
+    NVIC_SetPriority(FIELD_IRQn, 0);
+    NVIC_SetPriority(CALLBACK_IRQn, 0);
+    NVIC_SetPriority(FRAME_IRQn, 0);
+    NVIC_SetPriority(CRYPTO_IRQn, 1);
+    NVIC_SetPriority(SW_MAC_IRQn, 1);
+    NVIC_EnableIRQ(FIELD_IRQn);
+    NVIC_EnableIRQ(CALLBACK_IRQn);
+    NVIC_EnableIRQ(FRAME_IRQn);
+    NVIC_EnableIRQ(SW_MAC_IRQn);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    /* Initialize non-zero fixed values in CCM blocks */
+    g_ble_phy_encrypt_data.b0[0] = 0x49;
+    g_ble_phy_encrypt_data.b1[1] = 0x01;
+    g_ble_phy_encrypt_data.ai[0] = 0x01;
+#endif
+
+    /*
+     * Disable FIELD1, FIELD2 and FRAME errors since they can happen
+     * sometimes if we are too late on scheduling and trigger CMAC
+     * error. We can detect if tx_late happened and recover properly.
+     */
+    CMAC->CM_ERROR_DIS_REG |= CMAC_CM_ERROR_DIS_REG_CM_FIELD1_ERR_Msk |
+                              CMAC_CM_ERROR_DIS_REG_CM_FIELD2_ERR_Msk |
+                              CMAC_CM_ERROR_DIS_REG_CM_FRAME_ERR_Msk;
+
+    return 0;
+}
+
+void
+ble_phy_disable(void)
+{
+    MCU_DIAG_SER('D');
+
+    __disable_irq();
+
+    CMAC->CM_EV_SET_REG = CMAC_CM_EV_SET_REG_EV1C_BS_CLEAR_Msk;
+
+    __NOP();
+    __NOP();
+    __NOP();
+    __NOP();
+    __NOP();
+
+    CMAC->CM_EXC_STAT_REG = CMAC_CM_EXC_STAT_REG_EXC_FIELD_ON_THR_EXP_Msk |
+                            CMAC_CM_EXC_STAT_REG_EXC_BS_START_4THIS_Msk |
+                            CMAC_CM_EXC_STAT_REG_EXC_CORR_TIMEOUT_Msk |
+                            CMAC_CM_EXC_STAT_REG_EXC_BS_STOP_Msk |
+                            CMAC_CM_EXC_STAT_REG_EXC_FRAME_START_Msk |
+                            CMAC_CM_EXC_STAT_REG_EXC_PHY_TO_IDLE_4THIS_Msk |
+                            CMAC_CM_EXC_STAT_REG_EXC_SW_MAC_Msk;
+
+    NVIC->ICPR[0] = (1 << FIELD_IRQn) | (1 << CALLBACK_IRQn) |
+                    (1 << FRAME_IRQn) | (1 << SW_MAC_IRQn);
+
+    os_arch_cmac_bs_ctrl_irq_unblock();
+    g_sw_mac_exc = 0;
+
+    ble_rf_stop();
+
+    /*
+     * If ble_phy_disable is called precisely when access address was matched,
+     * ts1_dirty may not be cleared properly. This is because bs_clear will
+     * cause bitstream controller to be stopped and we won't get callback_irq,
+     * but seems like demodulator is still active for a while and will trigger
+     * ev1c_ts1_trigger on 1st symbol which will set ts1_dirty. We do not expect
+     * ts1_dirty to be set after bs_clear so we won't clear it. To workaround
+     * his, we can just clear it explicitly here after everything is already
+     * disabled.
+     */
+    (void)CMAC->CM_TS1_REG;
+
+    __enable_irq();
+
+    NVIC_EnableIRQ(FRAME_IRQn);
+    NVIC_EnableIRQ(FIELD_IRQn);
+
+    g_ble_phy_data.phy_state = BLE_PHY_STATE_IDLE;
+}
+
+static void
+ble_phy_rx_setup_fields(void)
+{
+    /* Make sure CRC LFSR initial value is set */
+    CMAC_SETREGF(CM_CRC_REG, CRC_INIT_VAL, g_ble_phy_data.crc_init);
+
+    CMAC->CM_FIELD_PUSH_DATA_REG = g_ble_phy_data.access_addr;
+    CMAC->CM_FIELD_PUSH_CTRL_REG = FIELD_CTRL_REG_RX_ACCESS_ADDR;
+    CMAC->CM_FIELD_PUSH_DATA_REG = FIELD_DATA_REG_DMA_RX(0, 2);
+    CMAC->CM_FIELD_PUSH_CTRL_REG = FIELD_CTRL_REG_RX_HEADER;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    if (g_ble_phy_data.phy_encrypted) {
+        /* Only program one byte for encrypted payloads */
+        CMAC->CM_FIELD_PUSH_DATA_REG = FIELD_DATA_REG_DMA_RX(2, 2);
+        CMAC->CM_FIELD_PUSH_CTRL_REG = FIELD_CTRL_REG_RX_ENC_PAYLOAD;
+    } else {
+        CMAC->CM_FIELD_PUSH_DATA_REG = FIELD_DATA_REG_DMA_RX(2, 2);
+        CMAC->CM_FIELD_PUSH_CTRL_REG = FIELD_CTRL_REG_RX_PAYLOAD;
+    }
+#else
+    CMAC->CM_FIELD_PUSH_DATA_REG = FIELD_DATA_REG_DMA_RX(2, 2);
+    CMAC->CM_FIELD_PUSH_CTRL_REG = FIELD_CTRL_REG_RX_PAYLOAD;
+#endif
+}
+
+static void
+ble_phy_rx_setup_xcvr(void)
+{
+    uint8_t rf_chan = g_ble_phy_chan_to_rf[g_ble_phy_data.channel];
+
+    CMAC->CM_EV_SET_REG = CMAC_CM_EV_SET_REG_EV1C_CALLBACK_VALID_SET_Msk;
+
+    ble_rf_setup_rx(rf_chan, g_ble_phy_data.phy_mode_rx);
+
+    g_ble_phy_data.phy_rx_started = 0;
+}
+
+int
+ble_phy_rx(void)
+{
+    MCU_DIAG_SER('R');
+
+    ble_rf_configure();
+    ble_phy_rx_setup_xcvr();
+
+    CMAC->CM_FRAME_1_REG = CMAC_CM_FRAME_1_REG_FRAME_VALID_Msk;
+
+    CMAC_SETREGF(CM_PHY_CTRL2_REG, CORR_WINDOW, 0);
+
+    ble_phy_rx_setup_fields();
+
+    g_ble_phy_data.phy_state = BLE_PHY_STATE_RX;
+
+    return 0;
+}
+
+int
+ble_phy_rx_set_start_time(uint32_t cputime, uint8_t rem_usecs)
+{
+    uint32_t ll_val32;
+    int32_t time_till_start;
+    int rc = 0;
+
+    MCU_DIAG_SER('r');
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    ble_phy_mode_apply(g_ble_phy_data.phy_mode_rx);
+#endif
+
+    assert(ble_rf_is_enabled());
+
+    ble_phy_rx();
+
+    /* Get LL timer at cputime */
+    ll_val32 = ble_phy_convert_and_record_start_time(cputime, rem_usecs);
+
+    /* Add remaining usecs to get exact receive start time */
+    ll_val32 += rem_usecs;
+
+    /* Adjust start time for rx delays */
+    ll_val32 -= PHY_DELAY_POWER_UP_RX - g_ble_phy_data.path_delay_rx;
+
+    __disable_irq();
+    CMAC->CM_LL_TIMER1_9_0_EQ_X_REG = ll_val32;
+    CMAC->CM_EV_LINKUP_REG =
+        CMAC_CM_EV_LINKUP_REG_LU_FRAME_START_2_TMR1_9_0_EQ_X_Msk;
+
+    time_till_start = (int32_t)(ll_val32 - cmac_timer_read32());
+    if (time_till_start <= 0) {
+        /*
+         * Possible we missed the frame start! If we have, we need to start
+         * ASAP.
+         */
+        if ((CMAC->CM_EXC_STAT_REG & CMAC_CM_EXC_STAT_REG_EXC_FRAME_START_Msk) == 0) {
+            /* We missed start. Start now */
+            CMAC->CM_EV_LINKUP_REG =
+                CMAC_CM_EV_LINKUP_REG_LU_FRAME_START_2_ASAP_Msk;
+            rc = BLE_PHY_ERR_RX_LATE;
+        }
+    }
+    __enable_irq();
+
+    return rc;
+}
+
+int
+ble_phy_tx(ble_phy_tx_pducb_t pducb, void *pducb_arg, uint8_t end_trans)
+{
+    uint8_t *txbuf = g_ble_phy_tx_buf;
+    int rc;
+
+    MCU_DIAG_SER('T');
+
+    assert(CMAC->CM_FRAME_1_REG & CMAC_CM_FRAME_1_REG_FRAME_TX_Msk);
+
+    g_ble_phy_data.end_transition = end_trans;
+
+    /*
+     * Program required fields now so in worst case TX can continue while we
+     * are still preparing header and payload.
+     */
+    CMAC->CM_FIELD_PUSH_DATA_REG = g_ble_phy_data.access_addr & 1 ? 0x5555 : 0xaaaa;
+    CMAC->CM_FIELD_PUSH_CTRL_REG = FIELD_CTRL_REG_TX_PREAMBLE;
+    CMAC->CM_FIELD_PUSH_DATA_REG = g_ble_phy_data.access_addr;
+    CMAC->CM_FIELD_PUSH_CTRL_REG = FIELD_CTRL_REG_TX_ACCESS_ADDR;
+
+    /* Make sure CRC LFSR initial value is set */
+    CMAC_SETREGF(CM_CRC_REG, CRC_INIT_VAL, g_ble_phy_data.crc_init);
+
+    /* txbuf[0] is hdr_byte, txbuf[1] is pkt_len */
+    txbuf[1] = pducb(&txbuf[2], pducb_arg, &txbuf[0]);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    if (g_ble_phy_data.phy_encrypted && (txbuf[1] != 0)) {
+        /* We have to add the MIC to the length */
+        txbuf[1] += BLE_LL_DATA_MIC_LEN;
+
+        /* Program header field */
+        CMAC->CM_FIELD_PUSH_DATA_REG = FIELD_DATA_REG_DMA_TX(0, 2);
+        CMAC->CM_FIELD_PUSH_CTRL_REG = FIELD_CTRL_REG_TX_PAYLOAD;
+
+        /* Program payload (and MIC) */
+        CMAC->CM_FIELD_PUSH_DATA_REG = FIELD_DATA_REG_DMA_TX(2, txbuf[1]);
+        CMAC->CM_FIELD_PUSH_CTRL_REG = FIELD_CTRL_REG_TX_ENC_PAYLOAD;
+    } else {
+        /* Program header and payload fields */
+        CMAC->CM_FIELD_PUSH_DATA_REG = FIELD_DATA_REG_DMA_TX(0, txbuf[1] + 2);
+        CMAC->CM_FIELD_PUSH_CTRL_REG = FIELD_CTRL_REG_TX_PAYLOAD;
+    }
+#else
+    /* Program header and payload fields */
+    CMAC->CM_FIELD_PUSH_DATA_REG = FIELD_DATA_REG_DMA_TX(0, txbuf[1] + 2);
+    CMAC->CM_FIELD_PUSH_CTRL_REG = FIELD_CTRL_REG_TX_PAYLOAD;
+#endif
+
+    /*
+     * If there was FIELD_ON_THR exception it means access address was already
+     * sent and we are likely too late here - abort.
+     */
+    if (CMAC->CM_EXC_STAT_REG & CMAC_CM_EXC_STAT_REG_EXC_FIELD_ON_THR_EXP_Msk) {
+        ble_phy_disable();
+        g_ble_phy_data.end_transition = BLE_PHY_TRANSITION_NONE;
+        STATS_INC(ble_phy_stats, tx_late_field);
+        STATS_INC(ble_phy_stats, tx_late);
+        rc = BLE_PHY_ERR_RADIO_STATE;
+    } else {
+        if (g_ble_phy_data.phy_state == BLE_PHY_STATE_IDLE) {
+            g_ble_phy_data.phy_state = BLE_PHY_STATE_TX;
+        }
+        STATS_INC(ble_phy_stats, tx_good);
+        STATS_INCN(ble_phy_stats, tx_bytes, txbuf[1] + 2);
+        rc = BLE_ERR_SUCCESS;
+    }
+
+    /* Now we can handle BS_CTRL */
+    NVIC_EnableIRQ(FRAME_IRQn);
+    NVIC_EnableIRQ(FIELD_IRQn);
+
+    return rc;
+}
+
+int
+ble_phy_tx_set_start_time(uint32_t cputime, uint8_t rem_usecs)
+{
+    uint8_t rf_chan = g_ble_phy_chan_to_rf[g_ble_phy_data.channel];
+    uint32_t ll_val32;
+    int rc;
+
+    MCU_DIAG_SER('t');
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    ble_phy_mode_apply(g_ble_phy_data.phy_mode_tx);
+#endif
+
+    assert(ble_rf_is_enabled());
+
+    ble_rf_configure();
+    ble_rf_setup_tx(rf_chan, g_ble_phy_data.phy_mode_tx);
+
+    ll_val32 = ble_phy_convert_and_record_start_time(cputime, rem_usecs);
+    ll_val32 += rem_usecs;
+    ll_val32 -= PHY_DELAY_POWER_UP_TX + g_ble_phy_data.path_delay_tx;
+    /* we can schedule TX only up to 1023us in advance */
+    assert((int32_t)(ll_val32 - cmac_timer_read32()) < 1024);
+
+    /*
+     * We do not want FIELD/FRAME interrupts until ble_phy_tx() has
+     * pushed all fields.
+     */
+    NVIC_DisableIRQ(FRAME_IRQn);
+    NVIC_DisableIRQ(FIELD_IRQn);
+
+    CMAC->CM_LL_TIMER1_9_0_EQ_X_REG = ll_val32;
+    CMAC->CM_EV_LINKUP_REG = CMAC_CM_EV_LINKUP_REG_LU_FRAME_START_2_TMR1_9_0_EQ_X_Msk;
+
+    if ((int32_t)(ll_val32 - cmac_timer_read32()) < 0) {
+        STATS_INC(ble_phy_stats, tx_late_sched);
+        goto tx_late;
+    }
+
+    /*
+     * Program frame now since it needs to be ready for FRAME_START, we can
+     * push fields later
+     */
+    CMAC->CM_FRAME_1_REG = CMAC_CM_FRAME_1_REG_FRAME_VALID_Msk |
+                           CMAC_CM_FRAME_1_REG_FRAME_TX_Msk |
+                           CMAC_CM_FRAME_1_REG_FRAME_EXC_ON_BS_START_Msk;
+
+    /*
+     * There should be no EXC_FRAME_START here so if it already happened we
+     * need to assume tx_late and abort.
+     */
+    if (CMAC->CM_EXC_STAT_REG & CMAC_CM_EXC_STAT_REG_EXC_FRAME_START_Msk) {
+        STATS_INC(ble_phy_stats, tx_late_frame);
+        goto tx_late;
+    }
+
+    rc = 0;
+
+    goto done;
+
+tx_late:
+    STATS_INC(ble_phy_stats, tx_late);
+    ble_phy_disable();
+    rc = BLE_PHY_ERR_TX_LATE;
+
+done:
+    return rc;
+}
+
+void
+ble_phy_set_txend_cb(ble_phy_tx_end_func txend_cb, void *arg)
+{
+    g_ble_phy_data.txend_cb = txend_cb;
+    g_ble_phy_data.txend_arg = arg;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+void
+ble_phy_tx_enc_start(void)
+{
+    struct ble_phy_encrypt_obj *enc;
+
+    enc = &g_ble_phy_encrypt_data;
+    enc->b0[15] = g_ble_phy_tx_buf[1] - 4;
+    enc->b1[2] = g_ble_phy_tx_buf[0] & BLE_LL_DATA_HDR_LLID_MASK;
+
+    /* XXX: should we check for busy? */
+    /* XXX: might not be needed, but for now terminate any crypto operations. */
+    CMAC->CM_CRYPTO_CTRL_REG = CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_SW_REQ_ABORT_Msk;
+
+    CMAC->CM_CRYPTO_CTRL_REG = CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_IN_SEL_Msk |
+                               CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_CTR_MAC_EN_Msk |
+                               CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_CTR_PLD_EN_Msk |
+                               CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_AUTH_EN_Msk |
+                               CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_ENC_DECN_Msk;
+
+    /* Start crypto */
+    CMAC->CM_EV_SET_REG = CMAC_CM_EV_SET_REG_EV_CRYPTO_START_Msk;
+}
+
+void
+ble_phy_rx_enc_start(uint8_t len)
+{
+    struct ble_phy_encrypt_obj *enc;
+
+    enc = &g_ble_phy_encrypt_data;
+    enc->b0[15] = len - 4; /* length without MIC as length includes MIC */
+    enc->b1[2] = g_ble_phy_rx_buf[0] & BLE_LL_DATA_HDR_LLID_MASK;
+
+    /* XXX: should we check for busy? */
+    /* XXX: might not be needed, but for now terminate any crypto operations. */
+    //CMAC->CM_CRYPTO_CTRL_REG = CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_SW_REQ_ABORT_Msk;
+
+    CMAC->CM_CRYPTO_CTRL_REG = CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_OUT_SEL_Msk |
+        CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_CTR_MAC_EN_Msk |
+        CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_CTR_PLD_EN_Msk |
+        CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_AUTH_EN_Msk;
+
+    /* Start crypto */
+    CMAC->CM_EV_SET_REG = CMAC_CM_EV_SET_REG_EV_CRYPTO_START_Msk;
+}
+
+void
+ble_phy_encrypt_enable(uint64_t pkt_counter, uint8_t *iv, uint8_t *key,
+                       uint8_t is_master)
+{
+    struct ble_phy_encrypt_obj *enc;
+
+    enc = &g_ble_phy_encrypt_data;
+    memcpy(enc->key, key, 16);
+    memcpy(&enc->b0[6], iv, 8);
+    put_le32(&enc->b0[1], pkt_counter);
+    enc->b0[5] = is_master ? 0x80 : 0;
+    memcpy(&enc->ai[6], iv, 8);
+    put_le32(&enc->ai[1], pkt_counter);
+    enc->ai[5] = enc->b0[5];
+
+    g_ble_phy_data.phy_encrypted = 1;
+
+    /* Program key registers */
+    CMAC->CM_CRYPTO_KEY_31_0_REG = get_le32(&enc->key[0]);
+    CMAC->CM_CRYPTO_KEY_63_32_REG = get_le32(&enc->key[4]);
+    CMAC->CM_CRYPTO_KEY_95_64_REG = get_le32(&enc->key[8]);
+    CMAC->CM_CRYPTO_KEY_127_96_REG = get_le32(&enc->key[12]);
+
+    /* Program ADRx registers */
+    CMAC->CM_CRYPTO_IN_ADR0_REG = (uint32_t)enc->b1;
+    CMAC->CM_CRYPTO_IN_ADR1_REG = (uint32_t)enc->b0;
+    CMAC->CM_CRYPTO_IN_ADR2_REG = (uint32_t)&g_ble_phy_tx_buf[2];
+    CMAC->CM_CRYPTO_IN_ADR3_REG = (uint32_t)enc->ai;
+    CMAC->CM_CRYPTO_OUT_ADR_REG = (uint32_t)&g_ble_phy_rx_buf[2];
+
+    CMAC->CM_CRYPTO_CTRL_REG = CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_SW_REQ_PBUF_CLR_Msk;
+}
+
+void
+ble_phy_encrypt_set_pkt_cntr(uint64_t pkt_counter, int dir)
+{
+    struct ble_phy_encrypt_obj *enc;
+
+    enc = &g_ble_phy_encrypt_data;
+    put_le32(&enc->b0[1], pkt_counter);
+    enc->b0[5] = dir ? 0x80 : 0;
+    put_le32(&enc->ai[1], pkt_counter);
+    enc->ai[5] = enc->b0[5];
+
+    CMAC->CM_CRYPTO_CTRL_REG = CMAC_CM_CRYPTO_CTRL_REG_CM_CRYPTO_SW_REQ_PBUF_CLR_Msk;
+}
+
+void
+ble_phy_encrypt_disable(void)
+{
+    g_ble_phy_data.phy_encrypted = 0;
+}
+#endif
+
+int
+ble_phy_txpwr_set(int dbm)
+{
+#if MYNEWT_VAL(CMAC_DEBUG_DATA_ENABLE)
+    if (g_cmac_shared_data.debug.tx_power_override != INT8_MAX) {
+        ble_rf_set_tx_power(g_cmac_shared_data.debug.tx_power_override);
+    } else {
+        ble_rf_set_tx_power(dbm);
+    }
+#else
+    ble_rf_set_tx_power(dbm);
+#endif
+
+    return 0;
+}
+
+int
+ble_phy_txpower_round(int dbm)
+{
+    return 0;
+}
+
+void
+ble_phy_set_rx_pwr_compensation(int8_t compensation)
+{
+}
+
+int
+ble_phy_setchan(uint8_t chan, uint32_t access_addr, uint32_t crc_init)
+{
+    uint8_t rf_chan = g_ble_phy_chan_to_rf[chan];
+
+    assert(chan < BLE_PHY_NUM_CHANS);
+
+    if (chan >= BLE_PHY_NUM_CHANS) {
+        return BLE_PHY_ERR_INV_PARAM;
+    }
+
+    g_ble_phy_data.channel = chan;
+    g_ble_phy_data.access_addr = access_addr;
+    g_ble_phy_data.crc_init = crc_init;
+
+    CMAC_SETREGF(CM_PHY_CTRL2_REG, PHY_RF_CHANNEL, rf_chan);
+
+    return 0;
+}
+
+void
+ble_phy_restart_rx(void)
+{
+    /* XXX: for now, we will disable the phy using ble_phy_disable and then
+       re-enable it
+     */
+    ble_phy_disable();
+
+    /* Apply mode before starting RX */
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    ble_phy_mode_apply(g_ble_phy_data.phy_mode_rx);
+#endif
+
+    /* Setup phy to rx again */
+    ble_phy_rx();
+
+    /* Start reception now */
+    CMAC->CM_EV_LINKUP_REG = CMAC_CM_EV_LINKUP_REG_LU_FRAME_START_2_ASAP_Msk;
+}
+
+uint32_t
+ble_phy_access_addr_get(void)
+{
+    return g_ble_phy_data.access_addr;
+}
+
+int
+ble_phy_state_get(void)
+{
+    return g_ble_phy_data.phy_state;
+}
+
+int
+ble_phy_rx_started(void)
+{
+    return g_ble_phy_data.phy_rx_started;
+}
+
+uint8_t
+ble_phy_xcvr_state_get(void)
+{
+    return ble_rf_is_enabled();
+}
+
+uint8_t
+ble_phy_max_data_pdu_pyld(void)
+{
+    return BLE_LL_DATA_PDU_MAX_PYLD;
+}
+
+void
+ble_phy_resolv_list_enable(void)
+{
+    g_ble_phy_data.phy_privacy = 1;
+
+    ble_hw_resolv_proc_enable();
+}
+
+void
+ble_phy_resolv_list_disable(void)
+{
+    ble_hw_resolv_proc_disable();
+
+    g_ble_phy_data.phy_privacy = 0;
+}
+
+void
+ble_phy_rfclk_enable(void)
+{
+    ble_rf_enable();
+}
+
+void
+ble_phy_rfclk_disable(void)
+{
+    /* XXX We can't disable RF while PHY_BUSY is asserted so let's wait a bit */
+    while (CMAC->CM_DIAG_WORD2_REG & CMAC_CM_DIAG_WORD2_REG_DIAG2_PHY_BUSY_BUF_Msk);
+
+    ble_rf_disable();
+}
+
+#if MYNEWT_VAL(BLE_LL_DTM)
+void
+ble_phy_enable_dtm(void)
+{
+    g_ble_phy_data.phy_whitening = 0;
+}
+
+void
+ble_phy_disable_dtm(void)
+{
+    g_ble_phy_data.phy_whitening = 1;
+}
+#endif

nimble/drivers/dialog_cmac/src/ble_rf.c

@@ -0,0 +1,747 @@
+/*
+ * 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 <assert.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include "mcu/mcu.h"
+#include "mcu/cmac_timer.h"
+#include "controller/ble_phy.h"
+#include "cmac_driver/cmac_shared.h"
+#include "ble_rf_priv.h"
+
+#define RF_CALIBRATION_0        (0x01)
+#define RF_CALIBRATION_1        (0x02)
+#define RF_CALIBRATION_2        (0x04)
+
+static const int8_t g_ble_rf_power_lvls[] = {
+    -18, -12, -8, -6, -3, -2, -1, 0, 1, 2, 3, 4, 4, 5, 6
+};
+
+struct ble_phy_rf_data {
+    uint8_t tx_power_cfg0;
+    uint8_t tx_power_cfg1;
+    uint8_t tx_power_cfg2;
+    uint8_t tx_power_cfg3;
+    uint32_t cal_res_1;
+    uint32_t cal_res_2;
+    uint32_t trim_val1_tx_1;
+    uint32_t trim_val1_tx_2;
+    uint32_t trim_val2_tx;
+    uint32_t trim_val2_rx;
+    uint8_t calibrate_req;
+};
+
+static struct ble_phy_rf_data g_ble_phy_rf_data;
+
+static inline uint32_t
+get_reg32(uint32_t addr)
+{
+    volatile uint32_t *reg = (volatile uint32_t *)addr;
+
+    return *reg;
+}
+
+static inline uint32_t
+get_reg32_bits(uint32_t addr, uint32_t mask)
+{
+    volatile uint32_t *reg = (volatile uint32_t *)addr;
+
+    return (*reg & mask) >> __builtin_ctz(mask);
+}
+
+static inline void
+set_reg8(uint32_t addr, uint8_t val)
+{
+    volatile uint8_t *reg = (volatile uint8_t *)addr;
+
+    *reg = val;
+}
+
+static inline void
+set_reg16(uint32_t addr, uint16_t val)
+{
+    volatile uint16_t *reg = (volatile uint16_t *)addr;
+
+    *reg = val;
+}
+
+static inline void
+set_reg32(uint32_t addr, uint32_t val)
+{
+    volatile uint32_t *reg = (volatile uint32_t *)addr;
+
+    *reg = val;
+}
+
+static inline void
+set_reg32_bits(uint32_t addr, uint32_t mask, uint32_t val)
+{
+    volatile uint32_t *reg = (volatile uint32_t *)addr;
+
+    *reg = (*reg & (~mask)) | (val << __builtin_ctz(mask));
+}
+
+static inline void
+set_reg32_mask(uint32_t addr, uint32_t mask, uint32_t val)
+{
+    volatile uint32_t *reg = (volatile uint32_t *)addr;
+
+    *reg = (*reg & (~mask)) | (val & mask);
+}
+
+static inline void
+set_reg16_mask(uint32_t addr, uint16_t mask, uint16_t val)
+{
+    volatile uint16_t *reg = (volatile uint16_t *)addr;
+
+    *reg = (*reg & (~mask)) | (val & mask);
+}
+
+static void
+delay_us(uint32_t delay_us)
+{
+    while (delay_us--) {
+        __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
+        __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
+        __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
+        __NOP(); __NOP(); __NOP(); __NOP();
+    }
+}
+
+static void
+ble_rf_apply_trim(volatile uint32_t *tv, unsigned len)
+{
+    while (len) {
+        *(volatile uint32_t *)tv[0] = tv[1];
+        len -= 2;
+        tv += 2;
+    }
+}
+
+static void
+ble_rf_apply_calibration(void)
+{
+    set_reg32(0x40020094, g_ble_phy_rf_data.cal_res_1);
+    if (g_ble_phy_rf_data.cal_res_2) {
+        set_reg32_bits(0x40022018, 0xff800000, g_ble_phy_rf_data.cal_res_2);
+        set_reg32_bits(0x40022018, 0x00007fc0, g_ble_phy_rf_data.cal_res_2);
+    }
+}
+
+static inline void
+ble_rf_ldo_on(void)
+{
+    set_reg8(0x40020004, 9);
+}
+
+static inline void
+ble_rf_ldo_off(void)
+{
+    set_reg8(0x40020004, 0);
+}
+
+static inline void
+ble_rf_rfcu_enable(void)
+{
+    set_reg32_bits(0x50000010, 0x00000020, 1);
+}
+
+static inline void
+ble_rf_rfcu_disable(void)
+{
+    set_reg32_bits(0x50000010, 0x00000020, 0);
+}
+
+static void
+ble_rf_rfcu_apply_recommended_settings(void)
+{
+    set_reg16_mask(0x400200a0, 0x0001, 0x0001);
+    set_reg16_mask(0x40021020, 0x03f0, 0x02f5);
+    set_reg32_mask(0x40021018, 0x001fffff, 0x005a5809);
+    set_reg32_mask(0x4002101c, 0x00001e01, 0x0040128c);
+    set_reg32_mask(0x40021004, 0xffffff1f, 0x64442404);
+    set_reg32_mask(0x40021008, 0xfcfcffff, 0x6b676665);
+    set_reg32_mask(0x4002100c, 0x00fcfcfc, 0x9793736f);
+    set_reg32_mask(0x40021010, 0x1f1f1c1f, 0x04072646);
+    set_reg32_mask(0x40020000, 0x001ff000, 0x0f099820);
+    set_reg16_mask(0x40020348, 0x00ff, 0x0855);
+    set_reg16(0x40020350, 0x0234);
+    set_reg16(0x40020354, 0x0a34);
+    set_reg16(0x40020358, 0x0851);
+    set_reg16(0x4002035c, 0x0a26);
+    set_reg16(0x40020360, 0x0858);
+    set_reg16(0x4002102c, 0xdfe7);
+    set_reg32_mask(0x4002103c, 0x00c00000, 0x0024a19f);
+    set_reg16_mask(0x40021000, 0x0008, 0x000b);
+    set_reg16_mask(0x40020238, 0x03e0, 0x02c0);
+    set_reg16_mask(0x4002023c, 0x03e0, 0x02c0);
+    set_reg16_mask(0x40020244, 0x03e0, 0x0250);
+    set_reg16_mask(0x40020248, 0x03e0, 0x02a0);
+    set_reg16_mask(0x4002024c, 0x03e0, 0x02c0);
+    set_reg16_mask(0x40020288, 0x03e0, 0x0300);
+    set_reg16_mask(0x4002029c, 0x001f, 0x0019);
+    set_reg16_mask(0x4002003c, 0x6000, 0x0788);
+    set_reg16_mask(0x40020074, 0x7f00, 0x2007);
+    set_reg32_mask(0x40020080, 0x00333330, 0x00222224);
+    set_reg32_mask(0x40020068, 0x00000f0f, 0x00000f0d);
+}
+
+static void
+ble_rf_rfcu_apply_settings(void)
+{
+    ble_rf_apply_trim(g_cmac_shared_data.trim.rfcu,
+                      g_cmac_shared_data.trim.rfcu_len);
+    ble_rf_rfcu_apply_recommended_settings();
+}
+
+static inline void
+ble_rf_synth_enable(void)
+{
+    set_reg8(0x40020005, 3);
+}
+
+static inline void
+ble_rf_synth_disable(void)
+{
+    set_reg8(0x40020005, 0);
+    __NOP();
+    __NOP();
+}
+
+static bool
+ble_rf_synth_is_enabled(void)
+{
+    return get_reg32_bits(0x40020004, 256);
+}
+
+static void
+ble_rf_synth_apply_recommended_settings(void)
+{
+    set_reg32_mask(0x40022048, 0x0000000c, 0x000000d5);
+    set_reg32_mask(0x40022050, 0x00000300, 0x00000300);
+    set_reg16_mask(0x40022024, 0x0001, 0x0001);
+}
+
+static void
+ble_rf_synth_apply_settings(void)
+{
+    ble_rf_apply_trim(g_cmac_shared_data.trim.synth,
+                      g_cmac_shared_data.trim.synth_len);
+    ble_rf_synth_apply_recommended_settings();
+}
+
+static void
+ble_rf_calibration_0(void)
+{
+    uint32_t bkp[10];
+
+    bkp[0] = get_reg32(0x40020208);
+    bkp[1] = get_reg32(0x40020250);
+    bkp[2] = get_reg32(0x40020254);
+    bkp[3] = get_reg32(0x40021028);
+    bkp[4] = get_reg32(0x40020020);
+    bkp[5] = get_reg32(0x40020294);
+    bkp[6] = get_reg32(0x4002103C);
+    bkp[7] = get_reg32(0x400200A8);
+    bkp[8] = get_reg32(0x40020000);
+    bkp[9] = get_reg32(0x40022000);
+
+    set_reg32_bits(0x40020000, 0x00000002, 0);
+    set_reg32_bits(0x40022000, 0x00000001, 0);
+    set_reg32_mask(0x4002103C, 0x00201c00, 0x00001c00);
+    set_reg32_bits(0x400200A8, 0x00000001, 1);
+    set_reg8(0x40020006, 1);
+    set_reg32(0x40020208, 0);
+    set_reg32(0x40020250, 0);
+    set_reg32(0x40020254, 0);
+    set_reg32(0x40021028, 0x00F8A494);
+    set_reg32(0x40020020, 8);
+    set_reg32(0x40020294, 0);
+    set_reg32(0x40020024, 0);
+
+    delay_us(5);
+    if (get_reg32_bits(0x40020020, 0x00000002)) {
+        goto done;
+    }
+
+    set_reg32_bits(0x40020020, 0x00000001, 1);
+    delay_us(15);
+    if (!get_reg32_bits(0x40020020, 0x00000001)) {
+        goto done;
+    }
+
+    delay_us(300);
+    if (get_reg32_bits(0x40020020, 0x00000001)) {
+        goto done;
+    }
+
+done:
+    set_reg32(0x40020024, 0);
+    set_reg32(0x40020208, bkp[0]);
+    set_reg32(0x40020250, bkp[1]);
+    set_reg32(0x40020254, bkp[2]);
+    set_reg32(0x40021028, bkp[3]);
+    set_reg32(0x40020020, bkp[4]);
+    set_reg32(0x40020294, bkp[5]);
+    set_reg32(0x4002103C, bkp[6]);
+    set_reg32(0x400200A8, bkp[7]);
+    set_reg32(0x40020000, bkp[8]);
+    set_reg32(0x40022000, bkp[9]);
+}
+
+static void
+ble_rf_calibration_1(void)
+{
+    uint32_t bkp[12];
+    uint32_t val;
+
+    bkp[0] = get_reg32(0x40020020);
+    bkp[1] = get_reg32(0x40020208);
+    bkp[2] = get_reg32(0x40020250);
+    bkp[3] = get_reg32(0x40020254);
+    bkp[4] = get_reg32(0x40020218);
+    bkp[5] = get_reg32(0x4002021c);
+    bkp[6] = get_reg32(0x40020220);
+    bkp[7] = get_reg32(0x40020270);
+    bkp[8] = get_reg32(0x4002027c);
+    bkp[9] = get_reg32(0x4002101c);
+    bkp[10] = get_reg32(0x40020000);
+    bkp[11] = get_reg32(0x40022000);
+
+    set_reg32(0x4002103c, 0x0124a21f);
+    set_reg32(0x40020208, 0);
+    set_reg32(0x40020250, 0);
+    set_reg32(0x40020254, 0);
+    set_reg32(0x40020218, 0);
+    set_reg32(0x4002021c, 0);
+    set_reg32(0x40020220, 0);
+    set_reg32(0x40020270, 0);
+    set_reg32(0x4002027c, 0);
+    set_reg32(0x40020000, 0x0f168820);
+    set_reg32_bits(0x40022000, 0x00000001, 0);
+    set_reg32_bits(0x4002101c, 0x00001e00, 0);
+    set_reg32_bits(0x4002001c, 0x0000003f, 47);
+    set_reg8(0x40020006, 1);
+    set_reg32(0x40020020, 16);
+    set_reg32_bits(0x4002003c, 0x00000800, 1);
+    set_reg32(0x40020024, 0);
+
+    delay_us(5);
+    if (get_reg32_bits(0x40020020, 0x00000002)) {
+        goto done;
+    }
+
+    set_reg32_bits(0x40020020, 0x00000001, 1);
+    delay_us(15);
+    if (!get_reg32_bits(0x40020020, 0x00000001)) {
+        goto done;
+    }
+
+    delay_us(300);
+    if (get_reg32_bits(0x40020020, 0x00000001)) {
+        goto done;
+    }
+
+    val = get_reg32(0x40020090);
+    set_reg32_bits(0x40020094, 0x0000000f, val);
+    set_reg32_bits(0x40020094, 0x00000f00, val);
+    set_reg32_bits(0x40020094, 0x000f0000, val);
+    set_reg32_bits(0x40020094, 0x0f000000, val);
+    g_ble_phy_rf_data.cal_res_1 = get_reg32(0x40020094);
+
+done:
+    set_reg32(0x40020024, 0);
+    set_reg32(0x40020020, bkp[0]);
+    set_reg32(0x40020208, bkp[1]);
+    set_reg32(0x40020250, bkp[2]);
+    set_reg32(0x40020254, bkp[3]);
+    set_reg32(0x40020218, bkp[4]);
+    set_reg32(0x4002021c, bkp[5]);
+    set_reg32(0x40020220, bkp[6]);
+    set_reg32(0x40020270, bkp[7]);
+    set_reg32(0x4002027c, bkp[8]);
+    set_reg32(0x4002101c, bkp[9]);
+    set_reg32(0x40020000, bkp[10]);
+    set_reg32(0x40022000, bkp[11]);
+    set_reg32_bits(0x4002003c, 0x00000800, 0);
+}
+
+static void
+ble_rf_calibration_2(void)
+{
+    uint32_t bkp[2];
+    uint32_t k1;
+
+    set_reg8(0x40020005, 3);
+    set_reg32(0x40022000, 0x00000300);
+    set_reg32_bits(0x40022004, 0x0000007f, 20);
+    bkp[0] = get_reg32(0x40022040);
+    set_reg32(0x40022040, 0xffffffff);
+    set_reg32_bits(0x40022018, 0x0000003f, 0);
+    set_reg32_bits(0x40022018, 0x00008000, 0);
+    set_reg32_bits(0x4002201c, 0x00000600, 2);
+    set_reg32_bits(0x4002201c, 0x00000070, 4);
+    set_reg32_bits(0x40022030, 0x3f000000, 22);
+    set_reg32_bits(0x40022030, 0x00000fc0, 24);
+    set_reg32_bits(0x40022030, 0x0000003f, 24);
+    set_reg8(0x4002201c, 0x43);
+    set_reg8(0x40020006, 2);
+    delay_us(2);
+    bkp[1] = get_reg32_bits(0x4002024c, 0x000003e0);
+    set_reg32_bits(0x4002024c, 0x000003e0, 0);
+    set_reg8(0x40020006, 1);
+    set_reg32_bits(0x400200ac, 0x00000003, 3);
+    delay_us(30);
+    delay_us(100);
+    set_reg8(0x40020005, 3);
+    k1 = get_reg32_bits(0x40022088, 0x000001ff);
+    set_reg32(0x400200ac, 0);
+    delay_us(20);
+    set_reg32_bits(0x4002024c, 0x000003e0, bkp[1]);
+    delay_us(10);
+
+    set_reg32_bits(0x40022018, 0xff800000, k1);
+    set_reg32_bits(0x40022018, 0x00007fc0, k1);
+    set_reg8(0x4002201c, 0x41);
+    set_reg32_bits(0x4002201c, 0x00000600, 2);
+    set_reg8(0x40020006, 2);
+    delay_us(2);
+    bkp[1] = get_reg32_bits(0x4002024c, 0x000003e0);
+    set_reg32_bits(0x4002024c, 0x000003e0, 0);
+    set_reg8(0x40020006, 1);
+    set_reg32_bits(0x400200ac, 0x00000003, 3);
+    delay_us(30);
+    delay_us(100);
+    set_reg8(0x40020005, 3);
+    k1 = get_reg32_bits(0x40022088, 0x1ff);
+    set_reg32(0x400200ac, 0);
+    delay_us(20);
+    set_reg32_bits(0x4002024c, 0x000003e0, bkp[1]);
+    delay_us(10);
+
+    set_reg32_bits(0x40022018, 0xff800000, k1);
+    set_reg32_bits(0x40022018, 0x00007fc0, k1);
+    set_reg8(0x4002201c, 0x41);
+    set_reg32_bits(0x4002201c, 0x00000600, 2);
+    set_reg8(0x40020006, 2);
+    delay_us(2);
+    bkp[1] = get_reg32_bits(0x4002024c, 0x000003e0);
+    set_reg32_bits(0x4002024c, 0x000003e0, 0);
+    set_reg8(0x40020006, 1);
+    set_reg32_bits(0x400200ac, 0x00000003, 3);
+    delay_us(30);
+    delay_us(100);
+    set_reg8(0x40020005, 3);
+    k1 = get_reg32_bits(0x40022088, 0x000001ff);
+    set_reg32_bits(0x40022018, 0xff800000, k1);
+    set_reg32_bits(0x40022018, 0x00007fc0, k1);
+    set_reg32_bits(0x4002201c, 0x00000001, 0);
+    set_reg32(0x40022040, bkp[0]);
+    set_reg32_bits(0x40022018, 0x0000003f, 0x1c);
+    set_reg32_bits(0x40022018, 0x00008000, 0);
+    set_reg32_bits(0x40022030, 0x3f000000, 28);
+    set_reg32_bits(0x40022030, 0x00000fc0, 30);
+    set_reg32_bits(0x40022030, 0x0000003f, 30);
+    set_reg32(0x400200ac, 0);
+    delay_us(20);
+    set_reg32_bits(0x4002024c, 0x000003e0, bkp[1]);
+    delay_us(10);
+
+    g_ble_phy_rf_data.cal_res_2 = k1;
+}
+
+static void
+ble_rf_calibrate_int(uint8_t mask)
+{
+    __disable_irq();
+
+    ble_rf_enable();
+    delay_us(20);
+
+    ble_rf_synth_disable();
+    ble_rf_synth_enable();
+    ble_rf_synth_apply_settings();
+    set_reg8(0x40020005, 1);
+
+    if (mask & RF_CALIBRATION_0) {
+        ble_rf_calibration_0();
+    }
+    if (mask & RF_CALIBRATION_1) {
+        ble_rf_calibration_1();
+    }
+    if (mask & RF_CALIBRATION_2) {
+        ble_rf_calibration_2();
+    }
+
+    ble_rf_disable();
+
+    __enable_irq();
+
+#if MYNEWT_VAL(CMAC_DEBUG_DATA_ENABLE)
+    g_cmac_shared_data.debug.cal_res_1 = g_ble_phy_rf_data.cal_res_1;
+    g_cmac_shared_data.debug.cal_res_2 = g_ble_phy_rf_data.cal_res_2;
+#endif
+}
+
+bool
+ble_rf_try_recalibrate(uint32_t idle_time_us)
+{
+    /* Run recalibration if we have at least 1ms of time to spare and RF is
+     * currently disabled. Calibration is much shorter than 1ms, but that gives
+     * us good margin to make sure we can finish before next event.
+     */
+    if (!g_ble_phy_rf_data.calibrate_req || (idle_time_us < 1000) ||
+        ble_rf_is_enabled()) {
+        return false;
+    }
+
+    ble_rf_calibrate_int(RF_CALIBRATION_2);
+
+    g_ble_phy_rf_data.calibrate_req = 0;
+
+    return true;
+}
+
+static uint32_t
+ble_rf_find_trim_reg(volatile uint32_t *tv, unsigned len, uint32_t reg)
+{
+    while (len) {
+        if (tv[0] == reg) {
+            return tv[1];
+        }
+        len -= 2;
+        tv += 2;
+    }
+
+    return 0;
+}
+
+void
+ble_rf_init(void)
+{
+    static bool done = false;
+    uint32_t val;
+
+    ble_rf_disable();
+
+    if (done) {
+        return;
+    }
+
+    val = ble_rf_find_trim_reg(g_cmac_shared_data.trim.rfcu_mode1,
+                               g_cmac_shared_data.trim.rfcu_mode1_len,
+                               0x4002004c);
+    g_ble_phy_rf_data.trim_val1_tx_1 = val;
+
+    val = ble_rf_find_trim_reg(g_cmac_shared_data.trim.rfcu_mode2,
+                               g_cmac_shared_data.trim.rfcu_mode2_len,
+                               0x4002004c);
+    g_ble_phy_rf_data.trim_val1_tx_2 = val;
+
+    if (!g_ble_phy_rf_data.trim_val1_tx_1 || !g_ble_phy_rf_data.trim_val1_tx_2) {
+        val = ble_rf_find_trim_reg(g_cmac_shared_data.trim.rfcu,
+                                   g_cmac_shared_data.trim.rfcu_len,
+                                   0x4002004c);
+        if (!val) {
+            val = 0x0300;
+        }
+        g_ble_phy_rf_data.trim_val1_tx_1 = val;
+        g_ble_phy_rf_data.trim_val1_tx_2 = val;
+    }
+
+    val = ble_rf_find_trim_reg(g_cmac_shared_data.trim.synth,
+                               g_cmac_shared_data.trim.synth_len,
+                               0x40022038);
+    if (!val) {
+        val = 0x0198ff03;
+    }
+    g_ble_phy_rf_data.trim_val2_rx = val;
+    g_ble_phy_rf_data.trim_val2_tx = val;
+    set_reg32_bits((uint32_t)&g_ble_phy_rf_data.trim_val2_tx, 0x0001ff00, 0x87);
+
+#if MYNEWT_VAL(CMAC_DEBUG_DATA_ENABLE)
+    g_cmac_shared_data.debug.trim_val1_tx_1 = g_ble_phy_rf_data.trim_val1_tx_1;
+    g_cmac_shared_data.debug.trim_val1_tx_2 = g_ble_phy_rf_data.trim_val1_tx_2;
+    g_cmac_shared_data.debug.trim_val2_tx = g_ble_phy_rf_data.trim_val2_tx;
+    g_cmac_shared_data.debug.trim_val2_rx = g_ble_phy_rf_data.trim_val2_rx;
+#endif
+
+    ble_rf_rfcu_enable();
+    ble_rf_rfcu_apply_settings();
+    g_ble_phy_rf_data.tx_power_cfg1 = get_reg32_bits(0x500000a4, 0xf0);
+    g_ble_phy_rf_data.tx_power_cfg2 = get_reg32_bits(0x40020238, 0x000003e0);
+    g_ble_phy_rf_data.tx_power_cfg3 = 0;
+    ble_rf_rfcu_disable();
+
+    ble_rf_calibrate_int(RF_CALIBRATION_0 | RF_CALIBRATION_1 | RF_CALIBRATION_2);
+
+    done = true;
+}
+
+void
+ble_rf_enable(void)
+{
+    if (ble_rf_is_enabled()) {
+        return;
+    }
+
+    ble_rf_rfcu_enable();
+    ble_rf_rfcu_apply_settings();
+    ble_rf_ldo_on();
+}
+
+void
+ble_rf_configure(void)
+{
+    if (ble_rf_synth_is_enabled()) {
+        return;
+    }
+
+    ble_rf_synth_enable();
+    ble_rf_synth_apply_settings();
+}
+
+void
+ble_rf_stop(void)
+{
+    ble_rf_synth_disable();
+    set_reg8(0x40020006, 0);
+}
+
+void
+ble_rf_disable(void)
+{
+    ble_rf_stop();
+    ble_rf_ldo_off();
+    ble_rf_rfcu_disable();
+}
+
+bool
+ble_rf_is_enabled(void)
+{
+    return get_reg32_bits(0x40020008, 5) == 5;
+}
+
+void
+ble_rf_calibrate_req(void)
+{
+    g_ble_phy_rf_data.calibrate_req = 1;
+}
+
+void
+ble_rf_setup_tx(uint8_t rf_chan, uint8_t phy_mode)
+{
+    set_reg32_bits(0x40020000, 0x0f000000, g_ble_phy_rf_data.tx_power_cfg0);
+    set_reg32_bits(0x500000a4, 0x000000f0, g_ble_phy_rf_data.tx_power_cfg1);
+    set_reg32_bits(0x40020238, 0x000003e0, g_ble_phy_rf_data.tx_power_cfg2);
+    set_reg32_bits(0x40020234, 0x000003e0, g_ble_phy_rf_data.tx_power_cfg3);
+
+    if (g_ble_phy_rf_data.tx_power_cfg0 < 13) {
+        set_reg32(0x4002004c, g_ble_phy_rf_data.trim_val1_tx_1);
+    } else {
+        set_reg32(0x4002004c, g_ble_phy_rf_data.trim_val1_tx_2);
+    }
+    set_reg8(0x40020005, 3);
+    set_reg8(0x40022004, rf_chan);
+    if (phy_mode == BLE_PHY_MODE_2M) {
+#if MYNEWT_VAL(BLE_PHY_RF_HP_MODE)
+        set_reg32(0x40022000, 0x00000303);
+#else
+        set_reg32(0x40022000, 0x00000003);
+#endif
+    } else {
+#if MYNEWT_VAL(BLE_PHY_RF_HP_MODE)
+        set_reg32(0x40022000, 0x00000300);
+#else
+        set_reg32(0x40022000, 0x00000000);
+#endif
+    }
+
+    ble_rf_apply_calibration();
+
+    set_reg32_bits(0x40022050, 0x00000200, 1);
+    set_reg32_bits(0x40022050, 0x00000100, 0);
+    set_reg32_bits(0x40022048, 0x01ffff00, 0x7700);
+    set_reg32(0x40022038, g_ble_phy_rf_data.trim_val2_tx);
+
+    set_reg8(0x40020006, 3);
+}
+
+void
+ble_rf_setup_rx(uint8_t rf_chan, uint8_t phy_mode)
+{
+    set_reg32_bits(0x500000a4, 0x000000f0, g_ble_phy_rf_data.tx_power_cfg1);
+    set_reg8(0x40020005, 3);
+    set_reg8(0x40022004, rf_chan);
+    if (phy_mode == BLE_PHY_MODE_2M) {
+#if MYNEWT_VAL(BLE_PHY_RF_HP_MODE)
+        set_reg32(0x40022000, 0x00000303);
+        set_reg32(0x40020000, 0x0f11b823);
+        set_reg32(0x4002103c, 0x0125261b);
+#else
+        set_reg32(0x40022000, 0x00000003);
+        set_reg32(0x40020000, 0x0f0c2803);
+        set_reg32(0x4002103c, 0x0125a61b);
+#endif
+        set_reg32(0x40021020, 0x000002f5);
+        set_reg32(0x4002102c, 0x0000d1d5);
+    } else {
+#if MYNEWT_VAL(BLE_PHY_RF_HP_MODE)
+        set_reg32(0x40022000, 0x00000300);
+        set_reg32(0x40020000, 0x0f099820);
+        set_reg32(0x4002103c, 0x0124a21f);
+#else
+        set_reg32(0x40022000, 0x00000000);
+        set_reg32(0x40020000, 0x0f062800);
+        set_reg32(0x4002103c, 0x01051e1f);
+#endif
+        set_reg32(0x40021020, 0x000002f5);
+        set_reg32(0x4002102c, 0x0000dfe7);
+    }
+
+    ble_rf_apply_calibration();
+
+    set_reg32_bits(0x40022050, 0x00000200, 1);
+    set_reg32_bits(0x40022050, 0x00000100, 1);
+    set_reg32_bits(0x40022048, 0x01ffff00, 0);
+    set_reg32(0x40022038, g_ble_phy_rf_data.trim_val2_rx);
+
+    set_reg8(0x40020006, 3);
+}
+
+void
+ble_rf_set_tx_power(int dbm)
+{
+    int i;
+
+    for (i = 0; i < ARRAY_SIZE(g_ble_rf_power_lvls); i++) {
+        if (g_ble_rf_power_lvls[i] >= dbm) {
+            break;
+        }
+    }
+
+    g_ble_phy_rf_data.tx_power_cfg0 = i + 1;
+}
+
+int8_t
+ble_rf_get_rssi(void)
+{
+    return (501 * get_reg32_bits(0x40021038, 0x000003ff) - 493000) / 4096;
+}

nimble/drivers/dialog_cmac/src/ble_rf_priv.h

@@ -0,0 +1,38 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#ifndef _BLE_RF_PRIV_H_
+#define _BLE_RF_PRIV_H_
+
+void ble_rf_init(void);
+void ble_rf_enable(void);
+void ble_rf_stop(void);
+void ble_rf_disable(void);
+bool ble_rf_is_enabled(void);
+void ble_rf_configure(void);
+
+void ble_rf_calibrate(void);
+
+void ble_rf_setup_tx(uint8_t rf_chan, uint8_t mode);
+void ble_rf_setup_rx(uint8_t rf_chan, uint8_t mode);
+
+void ble_rf_set_tx_power(int dbm);
+int8_t ble_rf_get_rssi(void);
+
+#endif /* _BLE_RF_PRIV_H_ */

nimble/drivers/dialog_cmac/syscfg.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.
+#
+
+syscfg.defs:
+    BLE_PHY_RF_HP_MODE:
+        description: Enable high-performance RF mode.
+        value: 1
+
+    BLE_PHY_DEBUG_DSER:
+        description: Enable DSER output from PHY
+        value: 0
+
+syscfg.restrictions:
+    - BLE_LL_RFMGMT_ENABLE_TIME == 0 || BLE_LL_RFMGMT_ENABLE_TIME >= 20

nimble/drivers/native/src/ble_hw.c

@@ -20,6 +20,8 @@
 #include <stdint.h>
 #include <assert.h>
 #include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
 #include "syscfg/syscfg.h"
 #include "os/os.h"
 #include "nimble/ble.h"
@@ -32,6 +34,9 @@
 /* We use this to keep track of which entries are set to valid addresses */
 static uint8_t g_ble_hw_whitelist_mask;
 
+static ble_rng_isr_cb_t rng_cb;
+static bool rng_started;
+
 /* Returns public device address or -1 if not present */
 int
 ble_hw_get_public_addr(ble_addr_t *addr)
@@ -66,7 +71,7 @@ ble_hw_whitelist_clear(void)
  * @return int 0: success, BLE error code otherwise
  */
 int
-ble_hw_whitelist_add(uint8_t *addr, uint8_t addr_type)
+ble_hw_whitelist_add(const uint8_t *addr, uint8_t addr_type)
 {
     return BLE_ERR_MEM_CAPACITY;
 }
@@ -79,7 +84,7 @@ ble_hw_whitelist_add(uint8_t *addr, uint8_t addr_type)
  *
  */
 void
-ble_hw_whitelist_rmv(uint8_t *addr, uint8_t addr_type)
+ble_hw_whitelist_rmv(const uint8_t *addr, uint8_t addr_type)
 {
     return;
 }
@@ -143,7 +148,8 @@ ble_hw_encrypt_block(struct ble_encryption_block *ecb)
 int
 ble_hw_rng_init(ble_rng_isr_cb_t cb, int bias)
 {
-    return -1;
+    rng_cb = cb;
+    return 0;
 }
 
 /**
@@ -154,7 +160,15 @@ ble_hw_rng_init(ble_rng_isr_cb_t cb, int bias)
 int
 ble_hw_rng_start(void)
 {
-    return -1;
+    rng_started = true;
+
+    if (rng_cb) {
+        while (rng_started) {
+            rng_cb(rand());
+        }
+    }
+
+    return 0;
 }
 
 /**
@@ -165,7 +179,8 @@ ble_hw_rng_start(void)
 int
 ble_hw_rng_stop(void)
 {
-    return -1;
+    rng_started = false;
+    return 0;
 }
 
 /**
@@ -176,10 +191,10 @@ ble_hw_rng_stop(void)
 uint8_t
 ble_hw_rng_read(void)
 {
-    return 0;
+    return rand();
 }
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
 /**
  * Clear the resolving list
  *

nimble/drivers/native/src/ble_phy.c

@@ -33,6 +33,7 @@ struct ble_phy_obj
 {
     uint8_t phy_stats_initialized;
     int8_t  phy_txpwr_dbm;
+    int16_t rx_pwr_compensation;
     uint8_t phy_chan;
     uint8_t phy_state;
     uint8_t phy_transition;
@@ -275,7 +276,8 @@ ble_phy_isr(void)
         /* Construct BLE header before handing up */
         ble_hdr = &g_ble_phy_data.rxhdr;
         ble_hdr->rxinfo.flags = 0;
-        ble_hdr->rxinfo.rssi = -77;    /* XXX: dummy rssi */
+        /* XXX: dummy rssi */
+        ble_hdr->rxinfo.rssi = -77 + g_ble_phy_data.rx_pwr_compensation;
         ble_hdr->rxinfo.channel = g_ble_phy_data.phy_chan;
         ble_hdr->rxinfo.phy = BLE_PHY_1M;
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
@@ -312,6 +314,8 @@ ble_phy_init(void)
     g_ble_phy_data.phy_state = BLE_PHY_STATE_IDLE;
     g_ble_phy_data.phy_chan = BLE_PHY_NUM_CHANS;
 
+    g_ble_phy_data.rx_pwr_compensation = 0;
+
     /* XXX: emulate ISR? */
 
     return 0;
@@ -337,7 +341,7 @@ ble_phy_restart_rx(void)
 {
 }
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
 /**
  * Called to enable encryption at the PHY. Note that this state will persist
  * in the PHY; in other words, if you call this function you have to call
@@ -515,6 +519,12 @@ ble_phy_txpwr_get(void)
     return g_ble_phy_data.phy_txpwr_dbm;
 }
 
+void
+ble_phy_set_rx_pwr_compensation(int8_t compensation)
+{
+    g_ble_phy_data.rx_pwr_compensation = compensation;
+}
+
 /**
  * ble phy setchan
  *
@@ -600,7 +610,7 @@ ble_phy_max_data_pdu_pyld(void)
     return BLE_LL_DATA_PDU_MAX_PYLD;
 }
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
 void
 ble_phy_resolv_list_enable(void)
 {
@@ -630,3 +640,13 @@ void
 ble_phy_wfr_enable(int txrx, uint8_t tx_phy_mode, uint32_t wfr_usecs)
 {
 }
+
+void
+ble_phy_rfclk_enable(void)
+{
+}
+
+void
+ble_phy_rfclk_disable(void)
+{
+}

nimble/drivers/nrf51/src/ble_hw.c

@@ -27,6 +27,12 @@
 #include "nimble/nimble_opt.h"
 #include "nrfx.h"
 #include "controller/ble_hw.h"
+#if MYNEWT
+#include "mcu/cmsis_nvic.h"
+#else
+#include "core_cm0.h"
+#include <nimble/nimble_npl_os.h>
+#endif
 #include "os/os_trace_api.h"
 
 /* Total number of resolving list elements */
@@ -39,7 +45,7 @@ static uint8_t g_ble_hw_whitelist_mask;
 ble_rng_isr_cb_t g_ble_rng_isr_cb;
 
 /* If LL privacy is enabled, allocate memory for AAR */
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
 
 /* The NRF51 supports up to 16 IRK entries */
 #if (MYNEWT_VAL(BLE_LL_RESOLV_LIST_SIZE) < 16)
@@ -60,33 +66,22 @@ uint8_t g_nrf_num_irks;
 int
 ble_hw_get_public_addr(ble_addr_t *addr)
 {
-    int rc;
     uint32_t addr_high;
     uint32_t addr_low;
 
     /* Does FICR have a public address */
-    rc = -1;
-    if ((NRF_FICR->DEVICEADDRTYPE & 1) == 0) {
-        addr_low = NRF_FICR->DEVICEADDR[0];
-        addr_high = NRF_FICR->DEVICEADDR[1];
-        rc = 0;
-    } else {
-        /* See if programmed in UICR. Upper 16 bits must all be zero */
-        addr_high = NRF_UICR->CUSTOMER[1];
-        if (addr_high < 65536) {
-            addr_low = NRF_UICR->CUSTOMER[0];
-            rc = 0;
-        }
+    if ((NRF_FICR->DEVICEADDRTYPE & 1) != 0) {
+        return -1;
     }
 
-    if (!rc) {
-        /* Copy into device address. We can do this because we know platform */
-        memcpy(addr->val, &addr_low, 4);
-        memcpy(&addr->val[4], &addr_high, 2);
-        addr->type = BLE_ADDR_PUBLIC;
-    }
+    /* Copy into device address. We can do this because we know platform */
+    addr_low = NRF_FICR->DEVICEADDR[0];
+    addr_high = NRF_FICR->DEVICEADDR[1];
+    memcpy(addr->val, &addr_low, 4);
+    memcpy(&addr->val[4], &addr_high, 2);
+    addr->type = BLE_ADDR_PUBLIC;
 
-    return rc;
+    return 0;
 }
 
 /* Returns random static address or -1 if not present */
@@ -129,7 +124,7 @@ ble_hw_whitelist_clear(void)
  * @return int 0: success, BLE error code otherwise
  */
 int
-ble_hw_whitelist_add(uint8_t *addr, uint8_t addr_type)
+ble_hw_whitelist_add(const uint8_t *addr, uint8_t addr_type)
 {
     int i;
     uint32_t mask;
@@ -160,7 +155,7 @@ ble_hw_whitelist_add(uint8_t *addr, uint8_t addr_type)
  *
  */
 void
-ble_hw_whitelist_rmv(uint8_t *addr, uint8_t addr_type)
+ble_hw_whitelist_rmv(const uint8_t *addr, uint8_t addr_type)
 {
     int i;
     uint8_t cfg_addr;
@@ -324,8 +319,14 @@ ble_hw_rng_init(ble_rng_isr_cb_t cb, int bias)
 
     /* If we were passed a function pointer we need to enable the interrupt */
     if (cb != NULL) {
+#ifndef RIOT_VERSION
         NVIC_SetPriority(RNG_IRQn, (1 << __NVIC_PRIO_BITS) - 1);
+#endif
+#if MYNEWT
+        NVIC_SetVector(RNG_IRQn, (uint32_t)ble_rng_isr);
+#else
         ble_npl_hw_set_isr(RNG_IRQn, ble_rng_isr);
+#endif
         NVIC_EnableIRQ(RNG_IRQn);
         g_ble_rng_isr_cb = cb;
     }
@@ -395,7 +396,7 @@ ble_hw_rng_read(void)
     return rnum;
 }
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY))
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
 /**
  * Clear the resolving list
  *
@@ -447,7 +448,7 @@ ble_hw_resolv_list_rmv(int index)
         --g_nrf_num_irks;
         irk_entry = &g_nrf_irk_list[index];
         if (g_nrf_num_irks > index) {
-            memmove(irk_entry, irk_entry + 4, g_nrf_num_irks - index);
+            memmove(irk_entry, irk_entry + 4, 16 * (g_nrf_num_irks - index));
         }
     }
 }
@@ -473,13 +474,8 @@ ble_hw_resolv_list_size(void)
 int
 ble_hw_resolv_list_match(void)
 {
-    uint32_t index;
-
-    if (NRF_AAR->EVENTS_END) {
-        if (NRF_AAR->EVENTS_RESOLVED) {
-            index = NRF_AAR->STATUS;
-            return (int)index;
-        }
+    if (NRF_AAR->ENABLE && NRF_AAR->EVENTS_END && NRF_AAR->EVENTS_RESOLVED) {
+        return (int)NRF_AAR->STATUS;
     }
 
     return -1;

nimble/drivers/nrf51/src/ble_phy.c

@@ -29,10 +29,20 @@
 #include "controller/ble_phy_trace.h"
 #include "controller/ble_ll.h"
 #include "nrfx.h"
-#include "os/os_trace_api.h"
 
 #if MYNEWT
 #include "mcu/nrf51_clock.h"
+#include "mcu/cmsis_nvic.h"
+#else
+#include "core_cm0.h"
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY)
+#error LE 2M PHY cannot be enabled on nRF51
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+#error LE Coded PHY cannot be enabled on nRF51
 #endif
 
 /* XXX: 4) Make sure RF is higher priority interrupt than schedule */
@@ -84,6 +94,7 @@ struct ble_phy_obj
     uint8_t phy_privacy;
     uint8_t phy_tx_pyld_len;
     uint8_t *rxdptr;
+    int8_t  rx_pwr_compensation;
     uint32_t phy_aar_scratch;
     uint32_t phy_access_address;
     struct ble_mbuf_hdr rxhdr;
@@ -98,7 +109,7 @@ struct ble_phy_obj g_ble_phy_data;
 static uint32_t g_ble_phy_tx_buf[(BLE_PHY_MAX_PDU_LEN + 3) / 4];
 static uint32_t g_ble_phy_rx_buf[(BLE_PHY_MAX_PDU_LEN + 3) / 4];
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
 /* Make sure word-aligned for faster copies */
 static uint32_t g_ble_phy_enc_buf[(NRF_ENC_BUF_SIZE + 3) / 4];
 #endif
@@ -174,7 +185,7 @@ STATS_NAME_END(ble_phy_stats)
  *  bit in the NVIC just to be sure when we disable the PHY.
  */
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
 /* Per nordic, the number of bytes needed for scratch is 16 + MAX_PKT_SIZE. */
 #define NRF_ENC_SCRATCH_WORDS   (((NRF_MAX_ENCRYPTED_PYLD_LEN + 16) + 3) / 4)
 
@@ -204,69 +215,83 @@ struct nrf_ccm_data g_nrf_ccm_data;
 void
 ble_phy_rxpdu_copy(uint8_t *dptr, struct os_mbuf *rxpdu)
 {
-    uint16_t rem_bytes;
-    uint16_t mb_bytes;
-    uint16_t copylen;
-    uint32_t *dst;
-    uint32_t *src;
-    struct os_mbuf *m;
-    struct ble_mbuf_hdr *ble_hdr;
-    struct os_mbuf_pkthdr *pkthdr;
+    uint32_t rem_len;
+    uint32_t copy_len;
+    uint32_t block_len;
+    uint32_t block_rem_len;
+    void *dst;
+    void *src;
+    struct os_mbuf * om;
 
     /* Better be aligned */
     assert(((uint32_t)dptr & 3) == 0);
 
-    pkthdr = OS_MBUF_PKTHDR(rxpdu);
-    rem_bytes = pkthdr->omp_len;
-
-    /* Fill in the mbuf pkthdr first. */
-    dst = (uint32_t *)(rxpdu->om_data);
-    src = (uint32_t *)dptr;
-
-    mb_bytes = (rxpdu->om_omp->omp_databuf_len - rxpdu->om_pkthdr_len - 4);
-    copylen = min(mb_bytes, rem_bytes);
-    copylen &= 0xFFFC;
-    rem_bytes -= copylen;
-    mb_bytes -= copylen;
-    rxpdu->om_len = copylen;
-    while (copylen > 0) {
-        *dst = *src;
-        ++dst;
-        ++src;
-        copylen -= 4;
-    }
+    block_len = rxpdu->om_omp->omp_databuf_len;
+    rem_len = OS_MBUF_PKTHDR(rxpdu)->omp_len;
+    src = dptr;
+
+    /*
+     * Setup for copying from first mbuf which is shorter due to packet header
+     * and extra leading space
+     */
+    copy_len = block_len - rxpdu->om_pkthdr_len - 4;
+    om = rxpdu;
+    dst = om->om_data;
 
-    /* Copy remaining bytes */
-    m = rxpdu;
-    while (rem_bytes > 0) {
-        /* If there are enough bytes in the mbuf, copy them and leave */
-        if (rem_bytes <= mb_bytes) {
-            memcpy(m->om_data + m->om_len, src, rem_bytes);
-            m->om_len += rem_bytes;
+    while (true) {
+        /*
+         * Always copy blocks of length aligned to word size, only last mbuf
+         * will have remaining non-word size bytes appended.
+         */
+        block_rem_len = copy_len;
+        copy_len = min(copy_len, rem_len);
+        copy_len &= ~3;
+
+        dst = om->om_data;
+        om->om_len = copy_len;
+        rem_len -= copy_len;
+        block_rem_len -= copy_len;
+
+        __asm__ volatile (".syntax unified              \n"
+                          "   mov  r4, %[len]           \n"
+                          "   b    2f                   \n"
+                          "1: ldr  r3, [%[src], %[len]] \n"
+                          "   str  r3, [%[dst], %[len]] \n"
+                          "2: subs %[len], #4           \n"
+                          "   bpl  1b                   \n"
+                          "   adds %[src], %[src], r4   \n"
+                          "   adds %[dst], %[dst], r4   \n"
+                          : [dst] "+l" (dst), [src] "+l" (src),
+                            [len] "+l" (copy_len)
+                          :
+                          : "r3", "r4", "memory"
+                         );
+
+        if ((rem_len < 4) && (block_rem_len >= rem_len)) {
             break;
         }
 
-        m = SLIST_NEXT(m, om_next);
-        assert(m != NULL);
-
-        mb_bytes = m->om_omp->omp_databuf_len;
-        copylen = min(mb_bytes, rem_bytes);
-        copylen &= 0xFFFC;
-        rem_bytes -= copylen;
-        mb_bytes -= copylen;
-        m->om_len = copylen;
-        dst = (uint32_t *)m->om_data;
-        while (copylen > 0) {
-            *dst = *src;
-            ++dst;
-            ++src;
-            copylen -= 4;
-        }
+        /* Move to next mbuf */
+        om = SLIST_NEXT(om, om_next);
+        copy_len = block_len;
     }
 
-    /* Copy ble header */
-    ble_hdr = BLE_MBUF_HDR_PTR(rxpdu);
-    memcpy(ble_hdr, &g_ble_phy_data.rxhdr, sizeof(struct ble_mbuf_hdr));
+    /* Copy remaining bytes, if any, to last mbuf */
+    om->om_len += rem_len;
+    __asm__ volatile (".syntax unified              \n"
+                      "   b    2f                   \n"
+                      "1: ldrb r3, [%[src], %[len]] \n"
+                      "   strb r3, [%[dst], %[len]] \n"
+                      "2: subs %[len], #1           \n"
+                      "   bpl  1b                   \n"
+                      : [len] "+l" (rem_len)
+                      : [dst] "l" (dst), [src] "l" (src)
+                      : "r3", "memory"
+                     );
+
+    /* Copy header */
+    memcpy(BLE_MBUF_HDR_PTR(rxpdu), &g_ble_phy_data.rxhdr,
+           sizeof(struct ble_mbuf_hdr));
 }
 
 /**
@@ -383,7 +408,7 @@ ble_phy_set_start_time(uint32_t cputime, uint8_t rem_usecs)
 /**
  * Function is used to set PPI so that we can time out waiting for a reception
  * to occur. This happens for two reasons: we have sent a packet and we are
- * waiting for a respons (txrx should be set to ENABLE_TXRX) or we are
+ * waiting for a response (txrx should be set to ENABLE_TXRX) or we are
  * starting a connection event and we are a slave and we are waiting for the
  * master to send us a packet (txrx should be set to ENABLE_RX).
  *
@@ -530,7 +555,7 @@ ble_phy_tx_end_isr(void)
     wfr_time = NRF_RADIO->SHORTS;
     (void)wfr_time;
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
     /*
      * XXX: not sure what to do. We had a HW error during transmission.
      * For now I just count a stat but continue on like all is good.
@@ -593,7 +618,8 @@ ble_phy_rx_end_isr(void)
     /* Set RSSI and CRC status flag in header */
     ble_hdr = &g_ble_phy_data.rxhdr;
     assert(NRF_RADIO->EVENTS_RSSIEND != 0);
-    ble_hdr->rxinfo.rssi = -1 * NRF_RADIO->RSSISAMPLE;
+    ble_hdr->rxinfo.rssi = (-1 * NRF_RADIO->RSSISAMPLE) +
+                            g_ble_phy_data.rx_pwr_compensation;
 
     dptr = g_ble_phy_data.rxdptr;
 
@@ -661,7 +687,7 @@ ble_phy_rx_start_isr(void)
     uint32_t usecs;
     uint32_t ticks;
     struct ble_mbuf_hdr *ble_hdr;
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
     uint8_t *dptr;
 
     dptr = (uint8_t *)&g_ble_phy_rx_buf[0];
@@ -725,7 +751,7 @@ ble_phy_rx_start_isr(void)
         g_ble_phy_data.phy_rx_started = 1;
         NRF_RADIO->INTENSET = RADIO_INTENSET_END_Msk;
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
         /* Must start aar if we need to  */
         if (g_ble_phy_data.phy_privacy) {
             NRF_RADIO->EVENTS_BCMATCH = 0;
@@ -818,20 +844,11 @@ ble_phy_init(void)
 {
     int rc;
 
-#if !defined(BLE_XCVR_RFCLK)
-    /* BLE wants the HFXO on all the time in this case */
-    ble_phy_rfclk_enable();
-
-    /*
-     * XXX: I do not think we need to wait for settling time here since
-     * we will probably not use the radio for longer than the settling time
-     * and it will only degrade performance. Might want to wait here though.
-     */
-#endif
-
     /* Set phy channel to an invalid channel so first set channel works */
     g_ble_phy_data.phy_chan = BLE_PHY_NUM_CHANS;
 
+    g_ble_phy_data.rx_pwr_compensation = 0;
+
     /* Toggle peripheral power to reset (just in case) */
     NRF_RADIO->POWER = 0;
     NRF_RADIO->POWER = 1;
@@ -865,14 +882,14 @@ ble_phy_init(void)
     /* Captures tx/rx start in timer0 cc 1 and tx/rx end in timer0 cc 2 */
     NRF_PPI->CHENSET = PPI_CHEN_CH26_Msk | PPI_CHEN_CH27_Msk;
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
     NRF_CCM->INTENCLR = 0xffffffff;
     NRF_CCM->SHORTS = CCM_SHORTS_ENDKSGEN_CRYPT_Msk;
     NRF_CCM->EVENTS_ERROR = 0;
     memset(g_nrf_encrypt_scratchpad, 0, sizeof(g_nrf_encrypt_scratchpad));
 #endif
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
     g_ble_phy_data.phy_aar_scratch = 0;
     NRF_AAR->IRKPTR = (uint32_t)&g_nrf_irk_list[0];
     NRF_AAR->INTENCLR = 0xffffffff;
@@ -902,8 +919,14 @@ ble_phy_init(void)
     NRF_PPI->CH[5].TEP = (uint32_t)&(NRF_RADIO->TASKS_DISABLE);
 
     /* Set isr in vector table and enable interrupt */
+#ifndef RIOT_VERSION
     NVIC_SetPriority(RADIO_IRQn, 0);
+#endif
+#if MYNEWT
+    NVIC_SetVector(RADIO_IRQn, (uint32_t)ble_phy_isr);
+#else
     ble_npl_hw_set_isr(RADIO_IRQn, ble_phy_isr);
+#endif
     NVIC_EnableIRQ(RADIO_IRQn);
 
     /* Register phy statistics */
@@ -955,7 +978,7 @@ ble_phy_rx(void)
     return 0;
 }
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
 /**
  * Called to enable encryption at the PHY. Note that this state will persist
  * in the PHY; in other words, if you call this function you have to call
@@ -1130,7 +1153,7 @@ ble_phy_tx(ble_phy_tx_pducb_t pducb, void *pducb_arg, uint8_t end_trans)
     NRF_PPI->CHENCLR = PPI_CHEN_CH4_Msk | PPI_CHEN_CH5_Msk | PPI_CHEN_CH23_Msk |
                        PPI_CHEN_CH25_Msk;
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
     if (g_ble_phy_data.phy_encrypted) {
         /* RAM representation has S0, LENGTH and S1 fields. (3 bytes) */
         dptr = (uint8_t *)&g_ble_phy_enc_buf[0];
@@ -1145,7 +1168,7 @@ ble_phy_tx(ble_phy_tx_pducb_t pducb, void *pducb_arg, uint8_t end_trans)
         NRF_CCM->CNFPTR = (uint32_t)&g_nrf_ccm_data;
         NRF_PPI->CHENSET = PPI_CHEN_CH24_Msk;
     } else {
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
         /* Reconfigure PCNF0 */
         NRF_RADIO->PCNF0 = (NRF_LFLEN_BITS << RADIO_PCNF0_LFLEN_Pos) |
                            (NRF_S0_LEN << RADIO_PCNF0_S0LEN_Pos);
@@ -1157,7 +1180,7 @@ ble_phy_tx(ble_phy_tx_pducb_t pducb, void *pducb_arg, uint8_t end_trans)
     }
 #else
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
     /* Reconfigure PCNF0 */
     NRF_RADIO->PCNF0 = (NRF_LFLEN_BITS << RADIO_PCNF0_LFLEN_Pos) |
                        (NRF_S0_LEN << RADIO_PCNF0_S0LEN_Pos);
@@ -1287,6 +1310,12 @@ ble_phy_txpwr_get(void)
     return g_ble_phy_data.phy_txpwr_dbm;
 }
 
+void
+ble_phy_set_rx_pwr_compensation(int8_t compensation)
+{
+    g_ble_phy_data.rx_pwr_compensation = compensation;
+}
+
 /**
  * ble phy setchan
  *
@@ -1328,7 +1357,7 @@ ble_phy_setchan(uint8_t chan, uint32_t access_addr, uint32_t crcinit)
         NRF_RADIO->RXADDRESSES = (1 << 1);
         NRF_RADIO->CRCINIT = crcinit;
     } else {
-        /* Logical adddress 0 preconfigured */
+        /* Logical address 0 preconfigured */
         NRF_RADIO->TXADDRESS = 0;
         NRF_RADIO->RXADDRESSES = (1 << 0);
         NRF_RADIO->CRCINIT = BLE_LL_CRCINIT_ADV;
@@ -1348,7 +1377,7 @@ ble_phy_setchan(uint8_t chan, uint32_t access_addr, uint32_t crcinit)
 /**
  * Stop the timer used to count microseconds when using RTC for cputime
  */
-void
+static void
 ble_phy_stop_usec_timer(void)
 {
     NRF_TIMER0->TASKS_STOP = 1;
@@ -1364,7 +1393,7 @@ ble_phy_stop_usec_timer(void)
  * restarted in receive mode. Generally, the disable routine is called to stop
  * the phy.
  */
-void
+static void
 ble_phy_disable_irq_and_ppi(void)
 {
     NRF_RADIO->INTENCLR = NRF_RADIO_IRQ_MASK_ALL;
@@ -1380,6 +1409,7 @@ ble_phy_disable_irq_and_ppi(void)
 void
 ble_phy_restart_rx(void)
 {
+    ble_phy_stop_usec_timer();
     ble_phy_disable_irq_and_ppi();
     ble_phy_rx();
 }
@@ -1451,14 +1481,14 @@ ble_phy_xcvr_state_get(void)
 uint8_t
 ble_phy_max_data_pdu_pyld(void)
 {
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
     return NRF_MAX_ENCRYPTED_PYLD_LEN;
 #else
     return BLE_LL_DATA_PDU_MAX_PYLD;
 #endif
 }
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
 void
 ble_phy_resolv_list_enable(void)
 {

nimble/drivers/nrf52/src/ble_hw.c

@@ -45,7 +45,7 @@ static uint8_t g_ble_hw_whitelist_mask;
 ble_rng_isr_cb_t g_ble_rng_isr_cb;
 
 /* If LL privacy is enabled, allocate memory for AAR */
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
 
 /* The NRF51 supports up to 16 IRK entries */
 #if (MYNEWT_VAL(BLE_LL_RESOLV_LIST_SIZE) < 16)
@@ -66,33 +66,22 @@ uint8_t g_nrf_num_irks;
 int
 ble_hw_get_public_addr(ble_addr_t *addr)
 {
-    int rc;
     uint32_t addr_high;
     uint32_t addr_low;
 
     /* Does FICR have a public address */
-    rc = -1;
-    if ((NRF_FICR->DEVICEADDRTYPE & 1) == 0) {
-        addr_low = NRF_FICR->DEVICEADDR[0];
-        addr_high = NRF_FICR->DEVICEADDR[1];
-        rc = 0;
-    } else {
-        /* See if programmed in UICR. Upper 16 bits must all be zero */
-        addr_high = NRF_UICR->CUSTOMER[1];
-        if (addr_high < 65536) {
-            addr_low = NRF_UICR->CUSTOMER[0];
-            rc = 0;
-        }
+    if ((NRF_FICR->DEVICEADDRTYPE & 1) != 0) {
+        return -1;
     }
 
-    if (!rc) {
-        /* Copy into device address. We can do this because we know platform */
-        memcpy(addr->val, &addr_low, 4);
-        memcpy(&addr->val[4], &addr_high, 2);
-        addr->type = BLE_ADDR_PUBLIC;
-    }
+    /* Copy into device address. We can do this because we know platform */
+    addr_low = NRF_FICR->DEVICEADDR[0];
+    addr_high = NRF_FICR->DEVICEADDR[1];
+    memcpy(addr->val, &addr_low, 4);
+    memcpy(&addr->val[4], &addr_high, 2);
+    addr->type = BLE_ADDR_PUBLIC;
 
-    return rc;
+    return 0;
 }
 
 /* Returns random static address or -1 if not present */
@@ -135,7 +124,7 @@ ble_hw_whitelist_clear(void)
  * @return int 0: success, BLE error code otherwise
  */
 int
-ble_hw_whitelist_add(uint8_t *addr, uint8_t addr_type)
+ble_hw_whitelist_add(const uint8_t *addr, uint8_t addr_type)
 {
     int i;
     uint32_t mask;
@@ -166,7 +155,7 @@ ble_hw_whitelist_add(uint8_t *addr, uint8_t addr_type)
  *
  */
 void
-ble_hw_whitelist_rmv(uint8_t *addr, uint8_t addr_type)
+ble_hw_whitelist_rmv(const uint8_t *addr, uint8_t addr_type)
 {
     int i;
     uint8_t cfg_addr;
@@ -330,7 +319,9 @@ ble_hw_rng_init(ble_rng_isr_cb_t cb, int bias)
 
     /* If we were passed a function pointer we need to enable the interrupt */
     if (cb != NULL) {
+#ifndef RIOT_VERSION
         NVIC_SetPriority(RNG_IRQn, (1 << __NVIC_PRIO_BITS) - 1);
+#endif
 #if MYNEWT
         NVIC_SetVector(RNG_IRQn, (uint32_t)ble_rng_isr);
 #else
@@ -405,7 +396,7 @@ ble_hw_rng_read(void)
     return rnum;
 }
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY))
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
 /**
  * Clear the resolving list
  *
@@ -457,7 +448,7 @@ ble_hw_resolv_list_rmv(int index)
         --g_nrf_num_irks;
         irk_entry = &g_nrf_irk_list[index];
         if (g_nrf_num_irks > index) {
-            memmove(irk_entry, irk_entry + 4, g_nrf_num_irks - index);
+            memmove(irk_entry, irk_entry + 4, 16 * (g_nrf_num_irks - index));
         }
     }
 }
@@ -483,13 +474,8 @@ ble_hw_resolv_list_size(void)
 int
 ble_hw_resolv_list_match(void)
 {
-    uint32_t index;
-
-    if (NRF_AAR->EVENTS_END) {
-        if (NRF_AAR->EVENTS_RESOLVED) {
-            index = NRF_AAR->STATUS;
-            return (int)index;
-        }
+    if (NRF_AAR->ENABLE && NRF_AAR->EVENTS_END && NRF_AAR->EVENTS_RESOLVED) {
+        return (int)NRF_AAR->STATUS;
     }
 
     return -1;

nimble/drivers/nrf52/src/ble_phy.c

@@ -38,6 +38,12 @@
 #include "core_cm4.h"
 #endif
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+#if !MYNEWT_VAL_CHOICE(MCU_TARGET, nRF52840) && !MYNEWT_VAL_CHOICE(MCU_TARGET, nRF52811)
+#error LE Coded PHY can only be enabled on nRF52811 or nRF52840
+#endif
+#endif
+
 /*
  * NOTE: This code uses a couple of PPI channels so care should be taken when
  *       using PPI somewhere else.
@@ -81,10 +87,6 @@ extern uint32_t g_nrf_irk_list[];
 #define NRF_MAXLEN              (255)
 #define NRF_BALEN               (3)     /* For base address of 3 bytes */
 
-/* Maximum tx power */
-#define NRF_TX_PWR_MAX_DBM      (4)
-#define NRF_TX_PWR_MIN_DBM      (-40)
-
 /* NRF_RADIO->PCNF0 configuration values */
 #define NRF_PCNF0               (NRF_LFLEN_BITS << RADIO_PCNF0_LFLEN_Pos) | \
                                 (RADIO_PCNF0_S1INCL_Msk) | \
@@ -112,9 +114,11 @@ struct ble_phy_obj
     uint8_t phy_encrypted;
     uint8_t phy_privacy;
     uint8_t phy_tx_pyld_len;
-    uint8_t phy_txtorx_phy_mode;
     uint8_t phy_cur_phy_mode;
+    uint8_t phy_tx_phy_mode;
+    uint8_t phy_rx_phy_mode;
     uint8_t phy_bcc_offset;
+    int8_t  rx_pwr_compensation;
     uint32_t phy_aar_scratch;
     uint32_t phy_access_address;
     struct ble_mbuf_hdr rxhdr;
@@ -129,7 +133,7 @@ struct ble_phy_obj g_ble_phy_data;
 static uint32_t g_ble_phy_tx_buf[(BLE_PHY_MAX_PDU_LEN + 3) / 4];
 static uint32_t g_ble_phy_rx_buf[(BLE_PHY_MAX_PDU_LEN + 3) / 4];
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
 /* Make sure word-aligned for faster copies */
 static uint32_t g_ble_phy_enc_buf[(BLE_PHY_MAX_PDU_LEN + 3) / 4];
 #endif
@@ -144,7 +148,12 @@ static const uint8_t g_ble_phy_chan_freq[BLE_PHY_NUM_CHANS] = {
 
 #if (BLE_LL_BT5_PHY_SUPPORTED == 1)
 /* packet start offsets (in usecs) */
-static const uint16_t g_ble_phy_mode_pkt_start_off[BLE_PHY_NUM_MODE] = { 376, 40, 24, 376 };
+static const uint16_t g_ble_phy_mode_pkt_start_off[BLE_PHY_NUM_MODE] = {
+    [BLE_PHY_MODE_1M] = 40,
+    [BLE_PHY_MODE_2M] = 24,
+    [BLE_PHY_MODE_CODED_125KBPS] = 376,
+    [BLE_PHY_MODE_CODED_500KBPS] = 376
+};
 #endif
 
 /* Various radio timings */
@@ -152,13 +161,33 @@ static const uint16_t g_ble_phy_mode_pkt_start_off[BLE_PHY_NUM_MODE] = { 376, 40
 #define BLE_PHY_T_TXENFAST      (XCVR_TX_RADIO_RAMPUP_USECS)
 #define BLE_PHY_T_RXENFAST      (XCVR_RX_RADIO_RAMPUP_USECS)
 /* delay between EVENTS_READY and start of tx */
-static const uint8_t g_ble_phy_t_txdelay[BLE_PHY_NUM_MODE] = { 5, 4, 3, 5 };
+static const uint8_t g_ble_phy_t_txdelay[BLE_PHY_NUM_MODE] = {
+    [BLE_PHY_MODE_1M] = 4,
+    [BLE_PHY_MODE_2M] = 3,
+    [BLE_PHY_MODE_CODED_125KBPS] = 5,
+    [BLE_PHY_MODE_CODED_500KBPS] = 5
+};
 /* delay between EVENTS_END and end of txd packet */
-static const uint8_t g_ble_phy_t_txenddelay[BLE_PHY_NUM_MODE] = { 9, 4, 3, 3 };
+static const uint8_t g_ble_phy_t_txenddelay[BLE_PHY_NUM_MODE] = {
+    [BLE_PHY_MODE_1M] = 4,
+    [BLE_PHY_MODE_2M] = 3,
+    [BLE_PHY_MODE_CODED_125KBPS] = 9,
+    [BLE_PHY_MODE_CODED_500KBPS] = 3
+};
 /* delay between rxd access address (w/ TERM1 for coded) and EVENTS_ADDRESS */
-static const uint8_t g_ble_phy_t_rxaddrdelay[BLE_PHY_NUM_MODE] = { 17, 6, 2, 17 };
+static const uint8_t g_ble_phy_t_rxaddrdelay[BLE_PHY_NUM_MODE] = {
+    [BLE_PHY_MODE_1M] = 6,
+    [BLE_PHY_MODE_2M] = 2,
+    [BLE_PHY_MODE_CODED_125KBPS] = 17,
+    [BLE_PHY_MODE_CODED_500KBPS] = 17
+};
 /* delay between end of rxd packet and EVENTS_END */
-static const uint8_t g_ble_phy_t_rxenddelay[BLE_PHY_NUM_MODE] = { 27, 6, 2, 22 };
+static const uint8_t g_ble_phy_t_rxenddelay[BLE_PHY_NUM_MODE] = {
+    [BLE_PHY_MODE_1M] = 6,
+    [BLE_PHY_MODE_2M] = 2,
+    [BLE_PHY_MODE_CODED_125KBPS] = 27,
+    [BLE_PHY_MODE_CODED_500KBPS] = 22
+};
 
 /* Statistics */
 STATS_SECT_START(ble_phy_stats)
@@ -223,7 +252,7 @@ STATS_NAME_END(ble_phy_stats)
  *  bit in the NVIC just to be sure when we disable the PHY.
  */
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
 
 /*
  * Per nordic, the number of bytes needed for scratch is 16 + MAX_PKT_SIZE.
@@ -248,7 +277,6 @@ struct nrf_ccm_data
 struct nrf_ccm_data g_nrf_ccm_data;
 #endif
 
-#ifdef NRF52
 static void
 ble_phy_apply_errata_102_106_107(void)
 {
@@ -259,7 +287,6 @@ ble_phy_apply_errata_102_106_107(void)
     *(volatile uint32_t *)0x40001774 = ((*(volatile uint32_t *)0x40001774) &
                          0xfffffffe) | 0x01000000;
 }
-#endif
 
 #if (BLE_LL_BT5_PHY_SUPPORTED == 1)
 
@@ -326,19 +353,18 @@ ble_phy_apply_nrf52840_errata(uint8_t new_phy_mode)
 }
 #endif
 
-void
-ble_phy_mode_set(uint8_t new_phy_mode, uint8_t txtorx_phy_mode)
+static void
+ble_phy_mode_apply(uint8_t phy_mode)
 {
-    if (new_phy_mode == g_ble_phy_data.phy_cur_phy_mode) {
-        g_ble_phy_data.phy_txtorx_phy_mode = txtorx_phy_mode;
+    if (phy_mode == g_ble_phy_data.phy_cur_phy_mode) {
         return;
     }
 
 #if NRF52840_XXAA
-    ble_phy_apply_nrf52840_errata(new_phy_mode);
+    ble_phy_apply_nrf52840_errata(phy_mode);
 #endif
 
-    switch (new_phy_mode) {
+    switch (phy_mode) {
     case BLE_PHY_MODE_1M:
         NRF_RADIO->MODE = RADIO_MODE_MODE_Ble_1Mbit;
         NRF_RADIO->PCNF0 = NRF_PCNF0_1M;
@@ -363,8 +389,14 @@ ble_phy_mode_set(uint8_t new_phy_mode, uint8_t txtorx_phy_mode)
         assert(0);
     }
 
-    g_ble_phy_data.phy_cur_phy_mode = new_phy_mode;
-    g_ble_phy_data.phy_txtorx_phy_mode = txtorx_phy_mode;
+    g_ble_phy_data.phy_cur_phy_mode = phy_mode;
+}
+
+void
+ble_phy_mode_set(uint8_t tx_phy_mode, uint8_t rx_phy_mode)
+{
+    g_ble_phy_data.phy_tx_phy_mode = tx_phy_mode;
+    g_ble_phy_data.phy_rx_phy_mode = rx_phy_mode;
 }
 #endif
 
@@ -402,69 +434,83 @@ ble_phy_get_cur_phy(void)
 void
 ble_phy_rxpdu_copy(uint8_t *dptr, struct os_mbuf *rxpdu)
 {
-    uint16_t rem_bytes;
-    uint16_t mb_bytes;
-    uint16_t copylen;
-    uint32_t *dst;
-    uint32_t *src;
-    struct os_mbuf *m;
-    struct ble_mbuf_hdr *ble_hdr;
-    struct os_mbuf_pkthdr *pkthdr;
+    uint32_t rem_len;
+    uint32_t copy_len;
+    uint32_t block_len;
+    uint32_t block_rem_len;
+    void *dst;
+    void *src;
+    struct os_mbuf * om;
 
     /* Better be aligned */
     assert(((uint32_t)dptr & 3) == 0);
 
-    pkthdr = OS_MBUF_PKTHDR(rxpdu);
-    rem_bytes = pkthdr->omp_len;
-
-    /* Fill in the mbuf pkthdr first. */
-    dst = (uint32_t *)(rxpdu->om_data);
-    src = (uint32_t *)dptr;
-
-    mb_bytes = (rxpdu->om_omp->omp_databuf_len - rxpdu->om_pkthdr_len - 4);
-    copylen = min(mb_bytes, rem_bytes);
-    copylen &= 0xFFFC;
-    rem_bytes -= copylen;
-    mb_bytes -= copylen;
-    rxpdu->om_len = copylen;
-    while (copylen > 0) {
-        *dst = *src;
-        ++dst;
-        ++src;
-        copylen -= 4;
-    }
+    block_len = rxpdu->om_omp->omp_databuf_len;
+    rem_len = OS_MBUF_PKTHDR(rxpdu)->omp_len;
+    src = dptr;
+
+    /*
+     * Setup for copying from first mbuf which is shorter due to packet header
+     * and extra leading space
+     */
+    copy_len = block_len - rxpdu->om_pkthdr_len - 4;
+    om = rxpdu;
+    dst = om->om_data;
 
-    /* Copy remaining bytes */
-    m = rxpdu;
-    while (rem_bytes > 0) {
-        /* If there are enough bytes in the mbuf, copy them and leave */
-        if (rem_bytes <= mb_bytes) {
-            memcpy(m->om_data + m->om_len, src, rem_bytes);
-            m->om_len += rem_bytes;
+    while (true) {
+        /*
+         * Always copy blocks of length aligned to word size, only last mbuf
+         * will have remaining non-word size bytes appended.
+         */
+        block_rem_len = copy_len;
+        copy_len = min(copy_len, rem_len);
+        copy_len &= ~3;
+
+        dst = om->om_data;
+        om->om_len = copy_len;
+        rem_len -= copy_len;
+        block_rem_len -= copy_len;
+
+        __asm__ volatile (".syntax unified              \n"
+                          "   mov  r4, %[len]           \n"
+                          "   b    2f                   \n"
+                          "1: ldr  r3, [%[src], %[len]] \n"
+                          "   str  r3, [%[dst], %[len]] \n"
+                          "2: subs %[len], #4           \n"
+                          "   bpl  1b                   \n"
+                          "   adds %[src], %[src], r4   \n"
+                          "   adds %[dst], %[dst], r4   \n"
+                          : [dst] "+r" (dst), [src] "+r" (src),
+                            [len] "+r" (copy_len)
+                          :
+                          : "r3", "r4", "memory"
+                         );
+
+        if ((rem_len < 4) && (block_rem_len >= rem_len)) {
             break;
         }
 
-        m = SLIST_NEXT(m, om_next);
-        assert(m != NULL);
-
-        mb_bytes = m->om_omp->omp_databuf_len;
-        copylen = min(mb_bytes, rem_bytes);
-        copylen &= 0xFFFC;
-        rem_bytes -= copylen;
-        mb_bytes -= copylen;
-        m->om_len = copylen;
-        dst = (uint32_t *)m->om_data;
-        while (copylen > 0) {
-            *dst = *src;
-            ++dst;
-            ++src;
-            copylen -= 4;
-        }
+        /* Move to next mbuf */
+        om = SLIST_NEXT(om, om_next);
+        copy_len = block_len;
     }
 
-    /* Copy ble header */
-    ble_hdr = BLE_MBUF_HDR_PTR(rxpdu);
-    memcpy(ble_hdr, &g_ble_phy_data.rxhdr, sizeof(struct ble_mbuf_hdr));
+    /* Copy remaining bytes, if any, to last mbuf */
+    om->om_len += rem_len;
+    __asm__ volatile (".syntax unified              \n"
+                      "   b    2f                   \n"
+                      "1: ldrb r3, [%[src], %[len]] \n"
+                      "   strb r3, [%[dst], %[len]] \n"
+                      "2: subs %[len], #1           \n"
+                      "   bpl  1b                   \n"
+                      : [len] "+r" (rem_len)
+                      : [dst] "r" (dst), [src] "r" (src)
+                      : "r3", "memory"
+                     );
+
+    /* Copy header */
+    memcpy(BLE_MBUF_HDR_PTR(rxpdu), &g_ble_phy_data.rxhdr,
+           sizeof(struct ble_mbuf_hdr));
 }
 
 /**
@@ -577,10 +623,10 @@ ble_phy_set_start_time(uint32_t cputime, uint8_t rem_usecs, bool tx)
 static int
 ble_phy_set_start_now(void)
 {
-    uint32_t cntr;
+    os_sr_t sr;
+    uint32_t now;
 
-    /* Read current RTC0 state */
-    cntr = NRF_RTC0->COUNTER;
+    OS_ENTER_CRITICAL(sr);
 
     /*
      * Set TIMER0 to fire immediately. We can't set CC to 0 as compare will not
@@ -596,8 +642,9 @@ ble_phy_set_start_now(void)
      * it to N+3 to account for possible extra tick on RTC0 during these
      * operations.
      */
+    now = os_cputime_get32();
     NRF_RTC0->EVENTS_COMPARE[0] = 0;
-    NRF_RTC0->CC[0] = cntr + 3;
+    NRF_RTC0->CC[0] = now + 3;
     NRF_RTC0->EVTENSET = RTC_EVTENSET_COMPARE0_Msk;
 
     /* Enable PPI */
@@ -612,7 +659,9 @@ ble_phy_set_start_now(void)
      * to be scheduled 1 or 2 ticks too late so we'll miss it - it's acceptable
      * for now.
      */
-    g_ble_phy_data.phy_start_cputime = cntr + 3;
+    g_ble_phy_data.phy_start_cputime = now + 3;
+
+    OS_EXIT_CRITICAL(sr);
 
     return 0;
 }
@@ -620,7 +669,7 @@ ble_phy_set_start_now(void)
 /**
  * Function is used to set PPI so that we can time out waiting for a reception
  * to occur. This happens for two reasons: we have sent a packet and we are
- * waiting for a respons (txrx should be set to ENABLE_TXRX) or we are
+ * waiting for a response (txrx should be set to ENABLE_TXRX) or we are
  * starting a connection event and we are a slave and we are waiting for the
  * master to send us a packet (txrx should be set to ENABLE_RX).
  *
@@ -772,7 +821,7 @@ ble_phy_rx_xcvr_setup(void)
     NRF_RADIO->PACKETPTR = (uint32_t)dptr;
 #endif
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
     if (g_ble_phy_data.phy_privacy) {
         NRF_AAR->ENABLE = AAR_ENABLE_ENABLE_Enabled;
         NRF_AAR->IRKPTR = (uint32_t)&g_nrf_irk_list[0];
@@ -832,9 +881,6 @@ ble_phy_rx_xcvr_setup(void)
 static void
 ble_phy_tx_end_isr(void)
 {
-#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
-    int phy;
-#endif
     uint8_t tx_phy_mode;
     uint8_t was_encrypted;
     uint8_t transition;
@@ -858,7 +904,7 @@ ble_phy_tx_end_isr(void)
     wfr_time = NRF_RADIO->SHORTS;
     (void)wfr_time;
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
     /*
      * XXX: not sure what to do. We had a HW error during transmission.
      * For now I just count a stat but continue on like all is good.
@@ -880,11 +926,7 @@ ble_phy_tx_end_isr(void)
     if (transition == BLE_PHY_TRANSITION_TX_RX) {
 
 #if (BLE_LL_BT5_PHY_SUPPORTED == 1)
-        /* See if a new phy has been specified for tx to rx transition */
-        phy = g_ble_phy_data.phy_txtorx_phy_mode;
-        if (phy != g_ble_phy_data.phy_cur_phy_mode) {
-            ble_phy_mode_set(phy, phy);
-        }
+        ble_phy_mode_apply(g_ble_phy_data.phy_rx_phy_mode);
 #endif
 
         /* Packet pointer needs to be reset. */
@@ -961,7 +1003,8 @@ ble_phy_rx_end_isr(void)
     /* Set RSSI and CRC status flag in header */
     ble_hdr = &g_ble_phy_data.rxhdr;
     assert(NRF_RADIO->EVENTS_RSSIEND != 0);
-    ble_hdr->rxinfo.rssi = -1 * NRF_RADIO->RSSISAMPLE;
+    ble_hdr->rxinfo.rssi = (-1 * NRF_RADIO->RSSISAMPLE) +
+                           g_ble_phy_data.rx_pwr_compensation;
 
     dptr = (uint8_t *)&g_ble_phy_rx_buf[0];
     dptr += 3;
@@ -973,7 +1016,7 @@ ble_phy_rx_end_isr(void)
     } else {
         STATS_INC(ble_phy_stats, rx_valid);
         ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_CRC_OK;
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
         if (g_ble_phy_data.phy_encrypted) {
             /* Only set MIC failure flag if frame is not zero length */
             if ((dptr[1] != 0) && (NRF_CCM->MICSTATUS == 0)) {
@@ -1004,6 +1047,10 @@ ble_phy_rx_end_isr(void)
 #endif
     }
 
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    ble_phy_mode_apply(g_ble_phy_data.phy_tx_phy_mode);
+#endif
+
     /*
      * Let's schedule TX now and we will just cancel it after processing RXed
      * packet if we don't need TX.
@@ -1025,7 +1072,6 @@ ble_phy_rx_end_isr(void)
     /* Adjust for radio ramp-up */
     tx_time -= BLE_PHY_T_TXENFAST;
     /* Adjust for delay between EVENT_READY and actual TX start time */
-    /* XXX: we may have asymmetric phy so next phy may be different... */
     tx_time -= g_ble_phy_t_txdelay[g_ble_phy_data.phy_cur_phy_mode];
 
     NRF_TIMER0->CC[0] = tx_time;
@@ -1073,6 +1119,9 @@ ble_phy_rx_start_isr(void)
     uint32_t ticks;
     struct ble_mbuf_hdr *ble_hdr;
     uint8_t *dptr;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    int adva_offset;
+#endif
 
     dptr = (uint8_t *)&g_ble_phy_rx_buf[0];
 
@@ -1143,6 +1192,28 @@ ble_phy_rx_start_isr(void)
         }
     }
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    /*
+     * If privacy is enabled and received PDU has TxAdd bit set (i.e. random
+     * address) we try to resolve address using AAR.
+     */
+    if (g_ble_phy_data.phy_privacy && (dptr[3] & 0x40)) {
+        /*
+         * AdvA is located at 4th octet in RX buffer (after S0, length an S1
+         * fields). In case of extended advertising PDU we need to add 2 more
+         * octets for extended header.
+         */
+        adva_offset = (dptr[3] & 0x0f) == 0x07 ? 2 : 0;
+        NRF_AAR->ADDRPTR = (uint32_t)(dptr + 3 + adva_offset);
+
+        /* Trigger AAR after last bit of AdvA is received */
+        NRF_RADIO->EVENTS_BCMATCH = 0;
+        NRF_PPI->CHENSET = PPI_CHEN_CH23_Msk;
+        NRF_RADIO->BCC = (BLE_LL_PDU_HDR_LEN + adva_offset + BLE_DEV_ADDR_LEN) * 8 +
+                         g_ble_phy_data.phy_bcc_offset;
+    }
+#endif
+
     /* Call Link Layer receive start function */
     rc = ble_ll_rx_start(dptr + 3,
                          g_ble_phy_data.phy_chan,
@@ -1151,34 +1222,6 @@ ble_phy_rx_start_isr(void)
         /* Set rx started flag and enable rx end ISR */
         g_ble_phy_data.phy_rx_started = 1;
         NRF_RADIO->INTENSET = RADIO_INTENSET_END_Msk;
-
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
-        /* Must start aar if we need to  */
-        if (g_ble_phy_data.phy_privacy) {
-            NRF_RADIO->EVENTS_BCMATCH = 0;
-            NRF_PPI->CHENSET = PPI_CHEN_CH23_Msk;
-
-            /*
-             * Setup AAR to resolve AdvA and trigger it after complete address
-             * is received, i.e. after PDU header and AdvA is received.
-             *
-             * AdvA starts at 4th octet in receive buffer, after S0, len and S1
-             * fields.
-             *
-             * In case of extended advertising AdvA is located after extended
-             * header (+2 octets).
-             */
-            if (BLE_MBUF_HDR_EXT_ADV(&g_ble_phy_data.rxhdr)) {
-                NRF_AAR->ADDRPTR = (uint32_t)(dptr + 5);
-                NRF_RADIO->BCC = (BLE_DEV_ADDR_LEN + BLE_LL_PDU_HDR_LEN + 2) * 8 +
-                                 g_ble_phy_data.phy_bcc_offset;
-            } else {
-                NRF_AAR->ADDRPTR = (uint32_t)(dptr + 3);
-                NRF_RADIO->BCC = (BLE_DEV_ADDR_LEN + BLE_LL_PDU_HDR_LEN) * 8 +
-                                 g_ble_phy_data.phy_bcc_offset;
-            }
-        }
-#endif
     } else {
         /* Disable PHY */
         ble_phy_disable();
@@ -1349,18 +1392,10 @@ ble_phy_init(void)
 
     /* Default phy to use is 1M */
     g_ble_phy_data.phy_cur_phy_mode = BLE_PHY_MODE_1M;
-    g_ble_phy_data.phy_txtorx_phy_mode = BLE_PHY_MODE_1M;
-
-#if !defined(BLE_XCVR_RFCLK)
-    /* BLE wants the HFXO on all the time in this case */
-    ble_phy_rfclk_enable();
+    g_ble_phy_data.phy_tx_phy_mode = BLE_PHY_MODE_1M;
+    g_ble_phy_data.phy_rx_phy_mode = BLE_PHY_MODE_1M;
 
-    /*
-     * XXX: I do not think we need to wait for settling time here since
-     * we will probably not use the radio for longer than the settling time
-     * and it will only degrade performance. Might want to wait here though.
-     */
-#endif
+    g_ble_phy_data.rx_pwr_compensation = 0;
 
     /* Set phy channel to an invalid channel so first set channel works */
     g_ble_phy_data.phy_chan = BLE_PHY_NUM_CHANS;
@@ -1402,14 +1437,14 @@ ble_phy_init(void)
     /* Captures tx/rx start in timer0 cc 1 and tx/rx end in timer0 cc 2 */
     NRF_PPI->CHENSET = PPI_CHEN_CH26_Msk | PPI_CHEN_CH27_Msk;
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
     NRF_CCM->INTENCLR = 0xffffffff;
     NRF_CCM->SHORTS = CCM_SHORTS_ENDKSGEN_CRYPT_Msk;
     NRF_CCM->EVENTS_ERROR = 0;
     memset(g_nrf_encrypt_scratchpad, 0, sizeof(g_nrf_encrypt_scratchpad));
 #endif
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
     g_ble_phy_data.phy_aar_scratch = 0;
     NRF_AAR->IRKPTR = (uint32_t)&g_nrf_irk_list[0];
     NRF_AAR->INTENCLR = 0xffffffff;
@@ -1439,7 +1474,9 @@ ble_phy_init(void)
     NRF_PPI->CH[5].TEP = (uint32_t)&(NRF_RADIO->TASKS_DISABLE);
 
     /* Set isr in vector table and enable interrupt */
+#ifndef RIOT_VERSION
     NVIC_SetPriority(RADIO_IRQn, 0);
+#endif
 #if MYNEWT
     NVIC_SetVector(RADIO_IRQn, (uint32_t)ble_phy_isr);
 #else
@@ -1506,7 +1543,7 @@ ble_phy_rx(void)
     return 0;
 }
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
 /**
  * Called to enable encryption at the PHY. Note that this state will persist
  * in the PHY; in other words, if you call this function you have to call
@@ -1582,6 +1619,10 @@ ble_phy_tx_set_start_time(uint32_t cputime, uint8_t rem_usecs)
 
     ble_phy_trace_u32x2(BLE_PHY_TRACE_ID_START_TX, cputime, rem_usecs);
 
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    ble_phy_mode_apply(g_ble_phy_data.phy_tx_phy_mode);
+#endif
+
     /* XXX: This should not be necessary, but paranoia is good! */
     /* Clear timer0 compare to RXEN since we are transmitting */
     NRF_PPI->CHENCLR = PPI_CHEN_CH21_Msk;
@@ -1619,6 +1660,10 @@ ble_phy_rx_set_start_time(uint32_t cputime, uint8_t rem_usecs)
 
     ble_phy_trace_u32x2(BLE_PHY_TRACE_ID_START_RX, cputime, rem_usecs);
 
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    ble_phy_mode_apply(g_ble_phy_data.phy_rx_phy_mode);
+#endif
+
     /* XXX: This should not be necessary, but paranoia is good! */
     /* Clear timer0 compare to TXEN since we are transmitting */
     NRF_PPI->CHENCLR = PPI_CHEN_CH20_Msk;
@@ -1682,7 +1727,7 @@ ble_phy_tx(ble_phy_tx_pducb_t pducb, void *pducb_arg, uint8_t end_trans)
     NRF_PPI->CHENCLR = PPI_CHEN_CH4_Msk | PPI_CHEN_CH5_Msk | PPI_CHEN_CH23_Msk |
                        PPI_CHEN_CH25_Msk;
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
     if (g_ble_phy_data.phy_encrypted) {
         dptr = (uint8_t *)&g_ble_phy_enc_buf[0];
         pktptr = (uint8_t *)&g_ble_phy_tx_buf[0];
@@ -1694,7 +1739,7 @@ ble_phy_tx(ble_phy_tx_pducb_t pducb, void *pducb_arg, uint8_t end_trans)
         NRF_CCM->MODE = CCM_MODE_LENGTH_Msk | ble_phy_get_ccm_datarate();
         NRF_CCM->CNFPTR = (uint32_t)&g_nrf_ccm_data;
     } else {
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
         NRF_AAR->IRKPTR = (uint32_t)&g_nrf_irk_list[0];
 #endif
         dptr = (uint8_t *)&g_ble_phy_tx_buf[0];
@@ -1713,7 +1758,7 @@ ble_phy_tx(ble_phy_tx_pducb_t pducb, void *pducb_arg, uint8_t end_trans)
     dptr[1] = payload_len;
     dptr[2] = 0;
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
     /* Start key-stream generation and encryption (via short) */
     if (g_ble_phy_data.phy_encrypted) {
         NRF_CCM->TASKS_KSGEN = 1;
@@ -1771,17 +1816,8 @@ ble_phy_tx(ble_phy_tx_pducb_t pducb, void *pducb_arg, uint8_t end_trans)
 int
 ble_phy_txpwr_set(int dbm)
 {
-    /* Check valid range */
-    assert(dbm <= BLE_PHY_MAX_PWR_DBM);
-
     /* "Rail" power level if outside supported range */
-    if (dbm > NRF_TX_PWR_MAX_DBM) {
-        dbm = NRF_TX_PWR_MAX_DBM;
-    } else {
-        if (dbm < NRF_TX_PWR_MIN_DBM) {
-            dbm = NRF_TX_PWR_MIN_DBM;
-        }
-    }
+    dbm = ble_phy_txpower_round(dbm);
 
     NRF_RADIO->TXPOWER = dbm;
     g_ble_phy_data.phy_txpwr_dbm = dbm;
@@ -1800,16 +1836,38 @@ ble_phy_txpwr_set(int dbm)
  */
 int ble_phy_txpower_round(int dbm)
 {
+    /* TODO this should be per nRF52XXX */
+
     /* "Rail" power level if outside supported range */
-    if (dbm > NRF_TX_PWR_MAX_DBM) {
-        dbm = NRF_TX_PWR_MAX_DBM;
-    } else {
-        if (dbm < NRF_TX_PWR_MIN_DBM) {
-            dbm = NRF_TX_PWR_MIN_DBM;
-        }
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Pos4dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Pos4dBm;
     }
 
-    return dbm;
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Pos3dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Pos3dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_0dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_0dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg4dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg4dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg8dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg8dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg12dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg12dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg20dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg20dBm;
+    }
+
+    return (int8_t)RADIO_TXPOWER_TXPOWER_Neg40dBm;
 }
 
 /**
@@ -1821,7 +1879,7 @@ int ble_phy_txpower_round(int dbm)
  *
  * @return int 0: success; PHY error code otherwise
  */
-int
+static int
 ble_phy_set_access_addr(uint32_t access_addr)
 {
     NRF_RADIO->BASE0 = (access_addr << 8);
@@ -1829,9 +1887,7 @@ ble_phy_set_access_addr(uint32_t access_addr)
 
     g_ble_phy_data.phy_access_address = access_addr;
 
-#ifdef NRF52
     ble_phy_apply_errata_102_106_107();
-#endif
 
     return 0;
 }
@@ -1849,6 +1905,12 @@ ble_phy_txpwr_get(void)
     return g_ble_phy_data.phy_txpwr_dbm;
 }
 
+void
+ble_phy_set_rx_pwr_compensation(int8_t compensation)
+{
+    g_ble_phy_data.rx_pwr_compensation = compensation;
+}
+
 /**
  * ble phy setchan
  *
@@ -1890,7 +1952,7 @@ ble_phy_setchan(uint8_t chan, uint32_t access_addr, uint32_t crcinit)
 /**
  * Stop the timer used to count microseconds when using RTC for cputime
  */
-void
+static void
 ble_phy_stop_usec_timer(void)
 {
     NRF_TIMER0->TASKS_STOP = 1;
@@ -1906,7 +1968,7 @@ ble_phy_stop_usec_timer(void)
  * restarted in receive mode. Generally, the disable routine is called to stop
  * the phy.
  */
-void
+static void
 ble_phy_disable_irq_and_ppi(void)
 {
     NRF_RADIO->INTENCLR = NRF_RADIO_IRQ_MASK_ALL;
@@ -1922,6 +1984,7 @@ ble_phy_disable_irq_and_ppi(void)
 void
 ble_phy_restart_rx(void)
 {
+    ble_phy_stop_usec_timer();
     ble_phy_disable_irq_and_ppi();
 
     ble_phy_set_start_now();
@@ -2001,7 +2064,7 @@ ble_phy_max_data_pdu_pyld(void)
     return BLE_LL_DATA_PDU_MAX_PYLD;
 }
 
-#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
 void
 ble_phy_resolv_list_enable(void)
 {
@@ -2016,7 +2079,7 @@ ble_phy_resolv_list_disable(void)
 }
 #endif
 
-#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 1
+#if MYNEWT_VAL(BLE_LL_DTM)
 void ble_phy_enable_dtm(void)
 {
     /* When DTM is enabled we need to disable whitening as per

nimble/drivers/nrf5340/include/ble/xcvr.h

@@ -0,0 +1,50 @@
+/*
+ * 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_XCVR_
+#define H_BLE_XCVR_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define XCVR_RX_RADIO_RAMPUP_USECS  (40)
+#define XCVR_TX_RADIO_RAMPUP_USECS  (40)
+
+/*
+ * NOTE: we have to account for the RTC output compare issue. We want it to be
+ * 5 ticks.
+ */
+#define XCVR_PROC_DELAY_USECS         (153)
+#define XCVR_RX_START_DELAY_USECS     (XCVR_RX_RADIO_RAMPUP_USECS)
+#define XCVR_TX_START_DELAY_USECS     (XCVR_TX_RADIO_RAMPUP_USECS)
+#define XCVR_TX_SCHED_DELAY_USECS     (XCVR_TX_START_DELAY_USECS + XCVR_PROC_DELAY_USECS)
+#define XCVR_RX_SCHED_DELAY_USECS     (XCVR_RX_START_DELAY_USECS + XCVR_PROC_DELAY_USECS)
+
+/*
+ * Define HW whitelist size. This is the total possible whitelist size;
+ * not necessarily the size that will be used (may be smaller)
+ */
+#define BLE_HW_WHITE_LIST_SIZE        (8)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* H_BLE_XCVR_ */

nimble/drivers/nrf5340/pkg.yml

@@ -0,0 +1,31 @@
+#
+# 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: nimble/drivers/nrf5340
+pkg.description: BLE driver for nRF5340 systems.
+pkg.author: "Apache Mynewt <dev@mynewt.apache.org>"
+pkg.homepage: "http://mynewt.apache.org/"
+pkg.keywords:
+    - ble
+    - bluetooth
+
+pkg.apis: ble_driver
+pkg.deps:
+    - nimble
+    - nimble/controller

nimble/drivers/nrf5340/src/ble_hw.c

@@ -0,0 +1,471 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stdint.h>
+#include <assert.h>
+#include <string.h>
+#include <syscfg/syscfg.h>
+#include <os/os.h>
+#include <nimble/ble.h>
+#include <nimble/nimble_opt.h>
+#include <controller/ble_hw.h>
+
+#include <ble/xcvr.h>
+#include <mcu/cmsis_nvic.h>
+#include <os/os_trace_api.h>
+
+/* Total number of resolving list elements */
+#define BLE_HW_RESOLV_LIST_SIZE (16)
+
+/* We use this to keep track of which entries are set to valid addresses */
+static uint8_t g_ble_hw_whitelist_mask;
+
+/* Random number generator isr callback */
+static ble_rng_isr_cb_t ble_rng_isr_cb;
+
+/* If LL privacy is enabled, allocate memory for AAR */
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+
+/* The NRF5340 supports up to 16 IRK entries */
+#if (MYNEWT_VAL(BLE_LL_RESOLV_LIST_SIZE) < 16)
+#define NRF_IRK_LIST_ENTRIES    (MYNEWT_VAL(BLE_LL_RESOLV_LIST_SIZE))
+#else
+#define NRF_IRK_LIST_ENTRIES    (16)
+#endif
+
+/* NOTE: each entry is 16 bytes long. */
+uint32_t g_nrf_irk_list[NRF_IRK_LIST_ENTRIES * 4];
+
+/* Current number of IRK entries */
+uint8_t g_nrf_num_irks;
+
+#endif
+
+/* Returns public device address or -1 if not present */
+int
+ble_hw_get_public_addr(ble_addr_t *addr)
+{
+    uint32_t addr_high;
+    uint32_t addr_low;
+
+    /* Does FICR have a public address */
+    if ((NRF_FICR_NS->DEVICEADDRTYPE & 1) != 0) {
+        return -1;
+    }
+
+    /* Copy into device address. We can do this because we know platform */
+    addr_low = NRF_FICR_NS->DEVICEADDR[0];
+    addr_high = NRF_FICR_NS->DEVICEADDR[1];
+    memcpy(addr->val, &addr_low, 4);
+    memcpy(&addr->val[4], &addr_high, 2);
+    addr->type = BLE_ADDR_PUBLIC;
+
+    return 0;
+}
+
+/* Returns random static address or -1 if not present */
+int
+ble_hw_get_static_addr(ble_addr_t *addr)
+{
+    int rc;
+
+    if ((NRF_FICR_NS->DEVICEADDRTYPE & 1) == 1) {
+        memcpy(addr->val, (void *)&NRF_FICR_NS->DEVICEADDR[0], 4);
+        memcpy(&addr->val[4], (void *)&NRF_FICR_NS->DEVICEADDR[1], 2);
+        addr->val[5] |= 0xc0;
+        addr->type = BLE_ADDR_RANDOM;
+        rc = 0;
+    } else {
+        rc = -1;
+    }
+
+    return rc;
+}
+
+/**
+ * Clear the whitelist
+ *
+ * @return int
+ */
+void
+ble_hw_whitelist_clear(void)
+{
+    NRF_RADIO_NS->DACNF = 0;
+    g_ble_hw_whitelist_mask = 0;
+}
+
+/**
+ * Add a device to the hw whitelist
+ *
+ * @param addr
+ * @param addr_type
+ *
+ * @return int 0: success, BLE error code otherwise
+ */
+int
+ble_hw_whitelist_add(const uint8_t *addr, uint8_t addr_type)
+{
+    int i;
+    uint32_t mask;
+
+    /* Find first ununsed device address match element */
+    mask = 0x01;
+    for (i = 0; i < BLE_HW_WHITE_LIST_SIZE; ++i) {
+        if ((mask & g_ble_hw_whitelist_mask) == 0) {
+            NRF_RADIO_NS->DAB[i] = get_le32(addr);
+            NRF_RADIO_NS->DAP[i] = get_le16(addr + 4);
+            if (addr_type == BLE_ADDR_RANDOM) {
+                NRF_RADIO_NS->DACNF |= (mask << 8);
+            }
+            g_ble_hw_whitelist_mask |= mask;
+            return BLE_ERR_SUCCESS;
+        }
+        mask <<= 1;
+    }
+
+    return BLE_ERR_MEM_CAPACITY;
+}
+
+/**
+ * Remove a device from the hw whitelist
+ *
+ * @param addr
+ * @param addr_type
+ *
+ */
+void
+ble_hw_whitelist_rmv(const uint8_t *addr, uint8_t addr_type)
+{
+    int i;
+    uint16_t dap;
+    uint16_t txadd;
+    uint32_t dab;
+    uint32_t mask;
+
+    /* Find first ununsed device address match element */
+    dab = get_le32(addr);
+    dap = get_le16(addr + 4);
+    txadd = NRF_RADIO_NS->DACNF >> 8;
+    mask = 0x01;
+    for (i = 0; i < BLE_HW_WHITE_LIST_SIZE; ++i) {
+        if (mask & g_ble_hw_whitelist_mask) {
+            if ((dab == NRF_RADIO_NS->DAB[i]) && (dap == NRF_RADIO_NS->DAP[i])) {
+                if (addr_type == !!(txadd & mask)) {
+                    break;
+                }
+            }
+        }
+        mask <<= 1;
+    }
+
+    if (i < BLE_HW_WHITE_LIST_SIZE) {
+        g_ble_hw_whitelist_mask &= ~mask;
+        NRF_RADIO_NS->DACNF &= ~mask;
+    }
+}
+
+/**
+ * Returns the size of the whitelist in HW
+ *
+ * @return int Number of devices allowed in whitelist
+ */
+uint8_t
+ble_hw_whitelist_size(void)
+{
+    return BLE_HW_WHITE_LIST_SIZE;
+}
+
+/**
+ * Enable the whitelisted devices
+ */
+void
+ble_hw_whitelist_enable(void)
+{
+    /* Enable the configured device addresses */
+    NRF_RADIO_NS->DACNF |= g_ble_hw_whitelist_mask;
+}
+
+/**
+ * Disables the whitelisted devices
+ */
+void
+ble_hw_whitelist_disable(void)
+{
+    /* Disable all whitelist devices */
+    NRF_RADIO_NS->DACNF &= 0x0000ff00;
+}
+
+/**
+ * Boolean function which returns true ('1') if there is a match on the
+ * whitelist.
+ *
+ * @return int
+ */
+int
+ble_hw_whitelist_match(void)
+{
+    return NRF_RADIO_NS->EVENTS_DEVMATCH;
+}
+
+/* Encrypt data */
+int
+ble_hw_encrypt_block(struct ble_encryption_block *ecb)
+{
+    int rc;
+    uint32_t end;
+    uint32_t err;
+
+    /* Stop ECB */
+    NRF_ECB_NS->TASKS_STOPECB = 1;
+    /* XXX: does task stop clear these counters? Anyway to do this quicker? */
+    NRF_ECB_NS->EVENTS_ENDECB = 0;
+    NRF_ECB_NS->EVENTS_ERRORECB = 0;
+    NRF_ECB_NS->ECBDATAPTR = (uint32_t)ecb;
+
+    /* Start ECB */
+    NRF_ECB_NS->TASKS_STARTECB = 1;
+
+    /* Wait till error or done */
+    rc = 0;
+    while (1) {
+        end = NRF_ECB_NS->EVENTS_ENDECB;
+        err = NRF_ECB_NS->EVENTS_ERRORECB;
+        if (end || err) {
+            if (err) {
+                rc = -1;
+            }
+            break;
+        }
+    }
+
+    return rc;
+}
+
+/**
+ * Random number generator ISR.
+ */
+static void
+ble_rng_isr(void)
+{
+    uint8_t rnum;
+
+    os_trace_isr_enter();
+
+    /* No callback? Clear and disable interrupts */
+    if (ble_rng_isr_cb == NULL) {
+        NRF_RNG_NS->INTENCLR = 1;
+        NRF_RNG_NS->EVENTS_VALRDY = 0;
+        (void)NRF_RNG_NS->SHORTS;
+        os_trace_isr_exit();
+        return;
+    }
+
+    /* If there is a value ready grab it */
+    if (NRF_RNG_NS->EVENTS_VALRDY) {
+        NRF_RNG_NS->EVENTS_VALRDY = 0;
+        rnum = (uint8_t)NRF_RNG_NS->VALUE;
+        (*ble_rng_isr_cb)(rnum);
+    }
+
+    os_trace_isr_exit();
+}
+
+/**
+ * Initialize the random number generator
+ *
+ * @param cb
+ * @param bias
+ *
+ * @return int
+ */
+int
+ble_hw_rng_init(ble_rng_isr_cb_t cb, int bias)
+{
+    /* Set bias */
+    if (bias) {
+        NRF_RNG_NS->CONFIG = 1;
+    } else {
+        NRF_RNG_NS->CONFIG = 0;
+    }
+
+    /* If we were passed a function pointer we need to enable the interrupt */
+    if (cb != NULL) {
+#ifndef RIOT_VERSION
+        NVIC_SetPriority(RNG_IRQn, (1 << __NVIC_PRIO_BITS) - 1);
+#endif
+#if MYNEWT
+        NVIC_SetVector(RNG_IRQn, (uint32_t)ble_rng_isr);
+#else
+        ble_npl_hw_set_isr(RNG_IRQn, ble_rng_isr);
+#endif
+        NVIC_EnableIRQ(RNG_IRQn);
+        ble_rng_isr_cb = cb;
+    }
+
+    return 0;
+}
+
+/**
+ * Start the random number generator
+ *
+ * @return int
+ */
+int
+ble_hw_rng_start(void)
+{
+    os_sr_t sr;
+
+    /* No need for interrupt if there is no callback */
+    OS_ENTER_CRITICAL(sr);
+    NRF_RNG_NS->EVENTS_VALRDY = 0;
+    if (ble_rng_isr_cb) {
+        NRF_RNG_NS->INTENSET = 1;
+    }
+    NRF_RNG_NS->TASKS_START = 1;
+    OS_EXIT_CRITICAL(sr);
+
+    return 0;
+}
+
+/**
+ * Stop the random generator
+ *
+ * @return int
+ */
+int
+ble_hw_rng_stop(void)
+{
+    os_sr_t sr;
+
+    /* No need for interrupt if there is no callback */
+    OS_ENTER_CRITICAL(sr);
+    NRF_RNG_NS->INTENCLR = 1;
+    NRF_RNG_NS->TASKS_STOP = 1;
+    NRF_RNG_NS->EVENTS_VALRDY = 0;
+    OS_EXIT_CRITICAL(sr);
+
+    return 0;
+}
+
+/**
+ * Read the random number generator.
+ *
+ * @return uint8_t
+ */
+uint8_t
+ble_hw_rng_read(void)
+{
+    uint8_t rnum;
+
+    /* Wait for a sample */
+    while (NRF_RNG_NS->EVENTS_VALRDY == 0) {
+    }
+
+    NRF_RNG_NS->EVENTS_VALRDY = 0;
+    rnum = (uint8_t)NRF_RNG_NS->VALUE;
+
+    return rnum;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+/**
+ * Clear the resolving list
+ *
+ * @return int
+ */
+void
+ble_hw_resolv_list_clear(void)
+{
+    g_nrf_num_irks = 0;
+}
+
+/**
+ * Add a device to the hw resolving list
+ *
+ * @param irk   Pointer to IRK to add
+ *
+ * @return int 0: success, BLE error code otherwise
+ */
+int
+ble_hw_resolv_list_add(uint8_t *irk)
+{
+    uint32_t *nrf_entry;
+
+    /* Find first ununsed device address match element */
+    if (g_nrf_num_irks == NRF_IRK_LIST_ENTRIES) {
+        return BLE_ERR_MEM_CAPACITY;
+    }
+
+    /* Copy into irk list */
+    nrf_entry = &g_nrf_irk_list[4 * g_nrf_num_irks];
+    memcpy(nrf_entry, irk, 16);
+
+    /* Add to total */
+    ++g_nrf_num_irks;
+    return BLE_ERR_SUCCESS;
+}
+
+/**
+ * Remove a device from the hw resolving list
+ *
+ * @param index Index of IRK to remove
+ */
+void
+ble_hw_resolv_list_rmv(int index)
+{
+    uint32_t *irk_entry;
+
+    if (index < g_nrf_num_irks) {
+        --g_nrf_num_irks;
+        irk_entry = &g_nrf_irk_list[index];
+        if (g_nrf_num_irks > index) {
+            memmove(irk_entry, irk_entry + 4, 16 * (g_nrf_num_irks - index));
+        }
+    }
+}
+
+/**
+ * Returns the size of the resolving list. NOTE: this returns the maximum
+ * allowable entries in the HW. Configuration options may limit this.
+ *
+ * @return int Number of devices allowed in resolving list
+ */
+uint8_t
+ble_hw_resolv_list_size(void)
+{
+    return BLE_HW_RESOLV_LIST_SIZE;
+}
+
+/**
+ * Called to determine if the address received was resolved.
+ *
+ * @return int  Negative values indicate unresolved address; positive values
+ *              indicate index in resolving list of resolved address.
+ */
+int
+ble_hw_resolv_list_match(void)
+{
+    if (NRF_AAR_NS->ENABLE && NRF_AAR_NS->EVENTS_END &&
+        NRF_AAR_NS->EVENTS_RESOLVED) {
+        return (int)NRF_AAR_NS->STATUS;
+    }
+
+    return -1;
+}
+#endif

nimble/drivers/nrf5340/src/ble_phy.c

@@ -0,0 +1,1820 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stdint.h>
+#include <string.h>
+#include <assert.h>
+#include <syscfg/syscfg.h>
+#include <os/os.h>
+#include <nimble/ble.h>
+#include <nimble/nimble_opt.h>
+#include <nimble/nimble_npl.h>
+#include <controller/ble_phy.h>
+
+#include <ble/xcvr.h>
+#include <controller/ble_phy_trace.h>
+#include <controller/ble_ll.h>
+#include <mcu/nrf5340_net_clock.h>
+#include <mcu/cmsis_nvic.h>
+
+/*
+ * NOTE: This code uses 0-5 DPPI channels so care should be taken when using
+ * DPPI somewhere else.
+ * TODO maybe we could reduce number of used channels if we reuse same channel
+ * for mutually exclusive events but for now make it simpler to debug.
+ */
+
+#define DPPI_CH_TIMER0_EVENTS_COMPARE_0         0
+#define DPPI_CH_TIMER0_EVENTS_COMPARE_3         1
+#define DPPI_CH_RADIO_EVENTS_END                2
+#define DPPI_CH_RADIO_EVENTS_BCMATCH            3
+#define DPPI_CH_RADIO_EVENTS_ADDRESS            4
+#define DPPI_CH_RTC0_EVENTS_COMPARE_0           5
+
+#define DPPI_CH_ENABLE_ALL (DPPIC_CHEN_CH0_Msk | DPPIC_CHEN_CH1_Msk | DPPIC_CHEN_CH2_Msk | \
+                            DPPIC_CHEN_CH3_Msk |  DPPIC_CHEN_CH4_Msk | DPPIC_CHEN_CH5_Msk)
+
+#define DPPI_PUBLISH_TIMER0_EVENTS_COMPARE_0    ((DPPI_CH_TIMER0_EVENTS_COMPARE_0 << TIMER_PUBLISH_COMPARE_CHIDX_Pos) | \
+                                                 (TIMER_PUBLISH_COMPARE_EN_Enabled << TIMER_PUBLISH_COMPARE_EN_Pos))
+#define DPPI_PUBLISH_TIMER0_EVENTS_COMPARE_3    ((DPPI_CH_TIMER0_EVENTS_COMPARE_3 << TIMER_PUBLISH_COMPARE_CHIDX_Pos) | \
+                                                 (TIMER_PUBLISH_COMPARE_EN_Enabled << TIMER_PUBLISH_COMPARE_EN_Pos))
+#define DPPI_PUBLISH_RADIO_EVENTS_END           ((DPPI_CH_RADIO_EVENTS_END << RADIO_PUBLISH_END_CHIDX_Pos) | \
+                                                 (RADIO_PUBLISH_END_EN_Enabled << RADIO_PUBLISH_END_EN_Pos))
+#define DPPI_PUBLISH_RADIO_EVENTS_BCMATCH       ((DPPI_CH_RADIO_EVENTS_BCMATCH << RADIO_PUBLISH_BCMATCH_CHIDX_Pos) | \
+                                                 (RADIO_PUBLISH_BCMATCH_EN_Enabled << RADIO_PUBLISH_BCMATCH_EN_Pos))
+#define DPPI_PUBLISH_RADIO_EVENTS_ADDRESS       ((DPPI_CH_RADIO_EVENTS_ADDRESS << RADIO_PUBLISH_ADDRESS_CHIDX_Pos) | \
+                                                 (RADIO_PUBLISH_ADDRESS_EN_Enabled << RADIO_PUBLISH_ADDRESS_EN_Pos))
+#define DPPI_PUBLISH_RTC0_EVENTS_COMPARE_0      ((DPPI_CH_RTC0_EVENTS_COMPARE_0 << RTC_PUBLISH_COMPARE_CHIDX_Pos) | \
+                                                 (RTC_PUBLISH_COMPARE_EN_Enabled << RTC_PUBLISH_COMPARE_EN_Pos))
+
+#define DPPI_SUBSCRIBE_TIMER0_TASKS_START(_enable)     ((DPPI_CH_RTC0_EVENTS_COMPARE_0 << TIMER_SUBSCRIBE_START_CHIDX_Pos) | \
+                                                        ((_enable) << TIMER_SUBSCRIBE_START_EN_Pos))
+#define DPPI_SUBSCRIBE_TIMER0_TASKS_CAPTURE1(_enable)  ((DPPI_CH_RADIO_EVENTS_ADDRESS << TIMER_SUBSCRIBE_CAPTURE_CHIDX_Pos) | \
+                                                        ((_enable) << TIMER_SUBSCRIBE_CAPTURE_EN_Pos))
+#define DPPI_SUBSCRIBE_TIMER0_TASKS_CAPTURE2(_enable)  ((DPPI_CH_RADIO_EVENTS_END << TIMER_SUBSCRIBE_CAPTURE_CHIDX_Pos) | \
+                                                        ((_enable) << TIMER_SUBSCRIBE_CAPTURE_EN_Pos))
+#define DPPI_SUBSCRIBE_TIMER0_TASKS_CAPTURE3(_enable)  ((DPPI_CH_RADIO_EVENTS_ADDRESS << TIMER_SUBSCRIBE_CAPTURE_CHIDX_Pos) | \
+                                                        ((_enable) << TIMER_SUBSCRIBE_CAPTURE_EN_Pos))
+#define DPPI_SUBSCRIBE_RADIO_TASKS_DISABLE(_enable)    ((DPPI_CH_TIMER0_EVENTS_COMPARE_3 << RADIO_SUBSCRIBE_DISABLE_CHIDX_Pos) | \
+                                                        ((_enable) << RADIO_SUBSCRIBE_DISABLE_EN_Pos))
+#define DPPI_SUBSCRIBE_RADIO_TASKS_RXEN(_enable)       ((DPPI_CH_TIMER0_EVENTS_COMPARE_0 << RADIO_SUBSCRIBE_RXEN_CHIDX_Pos) | \
+                                                        ((_enable) << RADIO_SUBSCRIBE_RXEN_EN_Pos))
+#define DPPI_SUBSCRIBE_RADIO_TASKS_TXEN(_enable)       ((DPPI_CH_TIMER0_EVENTS_COMPARE_0 << RADIO_SUBSCRIBE_TXEN_CHIDX_Pos) | \
+                                                        ((_enable) << RADIO_SUBSCRIBE_TXEN_EN_Pos))
+#define DPPI_SUBSCRIBE_AAR_TASKS_START(_enable)        ((DPPI_CH_RADIO_EVENTS_BCMATCH << AAR_SUBSCRIBE_START_CHIDX_Pos) | \
+                                                        ((_enable) << AAR_SUBSCRIBE_START_EN_Pos))
+#define DPPI_SUBSCRIBE_CCM_TASKS_CRYPT(_enable)        ((DPPI_CH_RADIO_EVENTS_ADDRESS << CCM_SUBSCRIBE_CRYPT_CHIDX_Pos) | \
+                                                        ((_enable) << CCM_SUBSCRIBE_CRYPT_EN_Pos))
+
+extern uint8_t g_nrf_num_irks;
+extern uint32_t g_nrf_irk_list[];
+
+/* To disable all radio interrupts */
+#define NRF_RADIO_IRQ_MASK_ALL (0x34FF)
+
+/*
+ * We configure the nrf with a 1 byte S0 field, 8 bit length field, and
+ * zero bit S1 field. The preamble is 8 bits long.
+ */
+#define NRF_LFLEN_BITS      (8)
+#define NRF_S0LEN           (1)
+#define NRF_S1LEN_BITS      (0)
+#define NRF_CILEN_BITS      (2)
+#define NRF_TERMLEN_BITS    (3)
+
+/* Maximum length of frames */
+#define NRF_MAXLEN          (255)
+#define NRF_BALEN           (3)     /* For base address of 3 bytes */
+
+/* NRF_RADIO_NS->PCNF0 configuration values */
+#define NRF_PCNF0           (NRF_LFLEN_BITS << RADIO_PCNF0_LFLEN_Pos) | \
+                            (RADIO_PCNF0_S1INCL_Msk) | \
+                            (NRF_S0LEN << RADIO_PCNF0_S0LEN_Pos) | \
+                            (NRF_S1LEN_BITS << RADIO_PCNF0_S1LEN_Pos)
+#define NRF_PCNF0_1M        (NRF_PCNF0) | \
+                            (RADIO_PCNF0_PLEN_8bit << RADIO_PCNF0_PLEN_Pos)
+#define NRF_PCNF0_2M        (NRF_PCNF0) | \
+                            (RADIO_PCNF0_PLEN_16bit << RADIO_PCNF0_PLEN_Pos)
+#define NRF_PCNF0_CODED     (NRF_PCNF0) | \
+                            (RADIO_PCNF0_PLEN_LongRange << RADIO_PCNF0_PLEN_Pos) | \
+                            (NRF_CILEN_BITS << RADIO_PCNF0_CILEN_Pos) | \
+                            (NRF_TERMLEN_BITS << RADIO_PCNF0_TERMLEN_Pos)
+
+/* BLE PHY data structure */
+struct ble_phy_obj {
+    uint8_t phy_stats_initialized;
+    int8_t phy_txpwr_dbm;
+    uint8_t phy_chan;
+    uint8_t phy_state;
+    uint8_t phy_transition;
+    uint8_t phy_transition_late;
+    uint8_t phy_rx_started;
+    uint8_t phy_encrypted;
+    uint8_t phy_privacy;
+    uint8_t phy_tx_pyld_len;
+    uint8_t phy_cur_phy_mode;
+    uint8_t phy_tx_phy_mode;
+    uint8_t phy_rx_phy_mode;
+    uint8_t phy_bcc_offset;
+    int8_t rx_pwr_compensation;
+    uint32_t phy_aar_scratch;
+    uint32_t phy_access_address;
+    struct ble_mbuf_hdr rxhdr;
+    void *txend_arg;
+    ble_phy_tx_end_func txend_cb;
+    uint32_t phy_start_cputime;
+};
+struct ble_phy_obj g_ble_phy_data;
+
+/* XXX: if 27 byte packets desired we can make this smaller */
+/* Global transmit/receive buffer */
+static uint32_t g_ble_phy_tx_buf[(BLE_PHY_MAX_PDU_LEN + 3) / 4];
+static uint32_t g_ble_phy_rx_buf[(BLE_PHY_MAX_PDU_LEN + 3) / 4];
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+/* Make sure word-aligned for faster copies */
+static uint32_t g_ble_phy_enc_buf[(BLE_PHY_MAX_PDU_LEN + 3) / 4];
+#endif
+
+/* RF center frequency for each channel index (offset from 2400 MHz) */
+static const uint8_t g_ble_phy_chan_freq[BLE_PHY_NUM_CHANS] = {
+    4,  6,  8, 10, 12, 14, 16, 18, 20, 22, /* 0-9 */
+    24, 28, 30, 32, 34, 36, 38, 40, 42, 44, /* 10-19 */
+    46, 48, 50, 52, 54, 56, 58, 60, 62, 64, /* 20-29 */
+    66, 68, 70, 72, 74, 76, 78,  2, 26, 80, /* 30-39 */
+};
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+/* packet start offsets (in usecs) */
+static const uint16_t g_ble_phy_mode_pkt_start_off[BLE_PHY_NUM_MODE] = {
+    [BLE_PHY_MODE_1M] = 40,
+    [BLE_PHY_MODE_2M] = 24,
+    [BLE_PHY_MODE_CODED_125KBPS] = 376,
+    [BLE_PHY_MODE_CODED_500KBPS] = 376
+};
+#endif
+
+/* Various radio timings */
+/* Radio ramp-up times in usecs (fast mode) */
+#define BLE_PHY_T_TXENFAST      (XCVR_TX_RADIO_RAMPUP_USECS)
+#define BLE_PHY_T_RXENFAST      (XCVR_RX_RADIO_RAMPUP_USECS)
+
+/* delay between EVENTS_READY and start of tx */
+static const uint8_t g_ble_phy_t_txdelay[BLE_PHY_NUM_MODE] = {
+    [BLE_PHY_MODE_1M] = 4,
+    [BLE_PHY_MODE_2M] = 3,
+    [BLE_PHY_MODE_CODED_125KBPS] = 5,
+    [BLE_PHY_MODE_CODED_500KBPS] = 5
+};
+/* 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] = 4,
+    [BLE_PHY_MODE_2M] = 3,
+    [BLE_PHY_MODE_CODED_125KBPS] = 9,
+    [BLE_PHY_MODE_CODED_500KBPS] = 3
+};
+/* delay between rxd access address (w/ TERM1 for coded) and EVENTS_ADDRESS */
+static const uint8_t g_ble_phy_t_rxaddrdelay[BLE_PHY_NUM_MODE] = {
+    [BLE_PHY_MODE_1M] = 6,
+    [BLE_PHY_MODE_2M] = 2,
+    [BLE_PHY_MODE_CODED_125KBPS] = 17,
+    [BLE_PHY_MODE_CODED_500KBPS] = 17
+};
+/* delay between end of rxd packet and EVENTS_END */
+static const uint8_t g_ble_phy_t_rxenddelay[BLE_PHY_NUM_MODE] = {
+    [BLE_PHY_MODE_1M] = 6,
+    [BLE_PHY_MODE_2M] = 2,
+    [BLE_PHY_MODE_CODED_125KBPS] = 27,
+    [BLE_PHY_MODE_CODED_500KBPS] = 22
+};
+
+/* Statistics */
+STATS_SECT_START(ble_phy_stats)
+STATS_SECT_ENTRY(phy_isrs)
+STATS_SECT_ENTRY(tx_good)
+STATS_SECT_ENTRY(tx_fail)
+STATS_SECT_ENTRY(tx_late)
+STATS_SECT_ENTRY(tx_bytes)
+STATS_SECT_ENTRY(rx_starts)
+STATS_SECT_ENTRY(rx_aborts)
+STATS_SECT_ENTRY(rx_valid)
+STATS_SECT_ENTRY(rx_crc_err)
+STATS_SECT_ENTRY(rx_late)
+STATS_SECT_ENTRY(radio_state_errs)
+STATS_SECT_ENTRY(rx_hw_err)
+STATS_SECT_ENTRY(tx_hw_err)
+STATS_SECT_END
+STATS_SECT_DECL(ble_phy_stats) ble_phy_stats;
+
+STATS_NAME_START(ble_phy_stats)
+STATS_NAME(ble_phy_stats, phy_isrs)
+STATS_NAME(ble_phy_stats, tx_good)
+STATS_NAME(ble_phy_stats, tx_fail)
+STATS_NAME(ble_phy_stats, tx_late)
+STATS_NAME(ble_phy_stats, tx_bytes)
+STATS_NAME(ble_phy_stats, rx_starts)
+STATS_NAME(ble_phy_stats, rx_aborts)
+STATS_NAME(ble_phy_stats, rx_valid)
+STATS_NAME(ble_phy_stats, rx_crc_err)
+STATS_NAME(ble_phy_stats, rx_late)
+STATS_NAME(ble_phy_stats, radio_state_errs)
+STATS_NAME(ble_phy_stats, rx_hw_err)
+STATS_NAME(ble_phy_stats, tx_hw_err)
+STATS_NAME_END(ble_phy_stats)
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+/*
+ * Per nordic, the number of bytes needed for scratch is 16 + MAX_PKT_SIZE.
+ * However, when I used a smaller size it still overwrote the scratchpad. Until
+ * I figure this out I am just going to allocate 67 words so we have enough
+ * space for 267 bytes of scratch. I used 268 bytes since not sure if this
+ * needs to be aligned and burning a byte is no big deal.
+ *
+ *#define NRF_ENC_SCRATCH_WORDS (((MYNEWT_VAL(BLE_LL_MAX_PKT_SIZE) + 16) + 3) / 4)
+ */
+#define NRF_ENC_SCRATCH_WORDS   (67)
+
+static uint32_t nrf_encrypt_scratchpad[NRF_ENC_SCRATCH_WORDS];
+
+struct nrf_ccm_data {
+    uint8_t key[16];
+    uint64_t pkt_counter;
+    uint8_t dir_bit;
+    uint8_t iv[8];
+} __attribute__((packed));
+
+static struct nrf_ccm_data nrf_ccm_data;
+#endif
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+
+uint32_t
+ble_phy_mode_pdu_start_off(int phy_mode)
+{
+    return g_ble_phy_mode_pkt_start_off[phy_mode];
+}
+
+static void
+ble_phy_mode_apply(uint8_t phy_mode)
+{
+    if (phy_mode == g_ble_phy_data.phy_cur_phy_mode) {
+        return;
+    }
+
+    switch (phy_mode) {
+    case BLE_PHY_MODE_1M:
+        NRF_RADIO_NS->MODE = RADIO_MODE_MODE_Ble_1Mbit;
+        NRF_RADIO_NS->PCNF0 = NRF_PCNF0_1M;
+        break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY)
+    case BLE_PHY_MODE_2M:
+        NRF_RADIO_NS->MODE = RADIO_MODE_MODE_Ble_2Mbit;
+        NRF_RADIO_NS->PCNF0 = NRF_PCNF0_2M;
+        break;
+#endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    case BLE_PHY_MODE_CODED_125KBPS:
+        NRF_RADIO_NS->MODE = RADIO_MODE_MODE_Ble_LR125Kbit;
+        NRF_RADIO_NS->PCNF0 = NRF_PCNF0_CODED;
+        break;
+    case BLE_PHY_MODE_CODED_500KBPS:
+        NRF_RADIO_NS->MODE = RADIO_MODE_MODE_Ble_LR500Kbit;
+        NRF_RADIO_NS->PCNF0 = NRF_PCNF0_CODED;
+        break;
+#endif
+    default:
+        assert(0);
+    }
+
+    g_ble_phy_data.phy_cur_phy_mode = phy_mode;
+}
+#endif
+
+void
+ble_phy_mode_set(uint8_t tx_phy_mode, uint8_t rx_phy_mode)
+{
+    g_ble_phy_data.phy_tx_phy_mode = tx_phy_mode;
+    g_ble_phy_data.phy_rx_phy_mode = rx_phy_mode;
+}
+
+int
+ble_phy_get_cur_phy(void)
+{
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    switch (g_ble_phy_data.phy_cur_phy_mode) {
+    case BLE_PHY_MODE_1M:
+        return BLE_PHY_1M;
+    case BLE_PHY_MODE_2M:
+        return BLE_PHY_2M;
+    case BLE_PHY_MODE_CODED_125KBPS:
+    case BLE_PHY_MODE_CODED_500KBPS:
+        return BLE_PHY_CODED;
+    default:
+        assert(0);
+        return -1;
+    }
+#else
+    return BLE_PHY_1M;
+#endif
+}
+
+void
+ble_phy_rxpdu_copy(uint8_t *dptr, struct os_mbuf *rxpdu)
+{
+    uint32_t rem_len;
+    uint32_t copy_len;
+    uint32_t block_len;
+    uint32_t block_rem_len;
+    void *dst;
+    void *src;
+    struct os_mbuf * om;
+
+    /* Better be aligned */
+    assert(((uint32_t)dptr & 3) == 0);
+
+    block_len = rxpdu->om_omp->omp_databuf_len;
+    rem_len = OS_MBUF_PKTHDR(rxpdu)->omp_len;
+    src = dptr;
+
+    /*
+     * Setup for copying from first mbuf which is shorter due to packet header
+     * and extra leading space
+     */
+    copy_len = block_len - rxpdu->om_pkthdr_len - 4;
+    om = rxpdu;
+    dst = om->om_data;
+
+    while (true) {
+        /*
+         * Always copy blocks of length aligned to word size, only last mbuf
+         * will have remaining non-word size bytes appended.
+         */
+        block_rem_len = copy_len;
+        copy_len = min(copy_len, rem_len);
+        copy_len &= ~3;
+
+        dst = om->om_data;
+        om->om_len = copy_len;
+        rem_len -= copy_len;
+        block_rem_len -= copy_len;
+
+        __asm__ volatile (".syntax unified              \n"
+                          "   mov  r4, %[len]           \n"
+                          "   b    2f                   \n"
+                          "1: ldr  r3, [%[src], %[len]] \n"
+                          "   str  r3, [%[dst], %[len]] \n"
+                          "2: subs %[len], #4           \n"
+                          "   bpl  1b                   \n"
+                          "   adds %[src], %[src], r4   \n"
+                          "   adds %[dst], %[dst], r4   \n"
+                          : [dst] "+r" (dst), [src] "+r" (src),
+                          [len] "+r" (copy_len)
+                          :
+                          : "r3", "r4", "memory"
+                          );
+
+        if ((rem_len < 4) && (block_rem_len >= rem_len)) {
+            break;
+        }
+
+        /* Move to next mbuf */
+        om = SLIST_NEXT(om, om_next);
+        copy_len = block_len;
+    }
+
+    /* Copy remaining bytes, if any, to last mbuf */
+    om->om_len += rem_len;
+    __asm__ volatile (".syntax unified              \n"
+                      "   b    2f                   \n"
+                      "1: ldrb r3, [%[src], %[len]] \n"
+                      "   strb r3, [%[dst], %[len]] \n"
+                      "2: subs %[len], #1           \n"
+                      "   bpl  1b                   \n"
+                      : [len] "+r" (rem_len)
+                      : [dst] "r" (dst), [src] "r" (src)
+                      : "r3", "memory"
+                      );
+
+    /* Copy header */
+    memcpy(BLE_MBUF_HDR_PTR(rxpdu), &g_ble_phy_data.rxhdr,
+           sizeof(struct ble_mbuf_hdr));
+}
+
+/**
+ * Called when we want to wait if the radio is in either the rx or tx
+ * disable states. We want to wait until that state is over before doing
+ * anything to the radio
+ */
+static void
+nrf_wait_disabled(void)
+{
+    uint32_t state;
+
+    state = NRF_RADIO_NS->STATE;
+    if (state != RADIO_STATE_STATE_Disabled) {
+        if ((state == RADIO_STATE_STATE_RxDisable) ||
+            (state == RADIO_STATE_STATE_TxDisable)) {
+            /* This will end within a short time (6 usecs). Just poll */
+            while (NRF_RADIO_NS->STATE == state) {
+                /* If this fails, something is really wrong. Should last
+                 * no more than 6 usecs
+                 */
+            }
+        }
+    }
+}
+
+static int
+ble_phy_set_start_time(uint32_t cputime, uint8_t rem_usecs, bool tx)
+{
+    uint32_t next_cc;
+    uint32_t cur_cc;
+    uint32_t cntr;
+    uint32_t delta;
+
+    /*
+     * We need to adjust start time to include radio ramp-up and TX pipeline
+     * delay (the latter only if applicable, so only for TX).
+     *
+     * Radio ramp-up time is 40 usecs and TX delay is 3 or 5 usecs depending on
+     * phy, thus we'll offset RTC by 2 full ticks (61 usecs) and then compensate
+     * using TIMER0 with 1 usec precision.
+     */
+
+    cputime -= 2;
+    rem_usecs += 61;
+    if (tx) {
+        rem_usecs -= BLE_PHY_T_TXENFAST;
+        rem_usecs -= g_ble_phy_t_txdelay[g_ble_phy_data.phy_cur_phy_mode];
+    } else {
+        rem_usecs -= BLE_PHY_T_RXENFAST;
+    }
+
+    /*
+     * rem_usecs will be no more than 2 ticks, but if it is more than single
+     * tick then we should better count one more low-power tick rather than
+     * 30 high-power usecs. Also make sure we don't set TIMER0 CC to 0 as the
+     * compare won't occur.
+     */
+
+    if (rem_usecs > 30) {
+        cputime++;
+        rem_usecs -= 30;
+    }
+
+    /*
+     * Can we set the RTC compare to start TIMER0? We can do it if:
+     *      a) Current compare value is not N+1 or N+2 ticks from current
+     *      counter.
+     *      b) The value we want to set is not at least N+2 from current
+     *      counter.
+     *
+     * NOTE: since the counter can tick 1 while we do these calculations we
+     * need to account for it.
+     */
+    next_cc = cputime & 0xffffff;
+    cur_cc = NRF_RTC0_NS->CC[0];
+    cntr = NRF_RTC0_NS->COUNTER;
+
+    delta = (cur_cc - cntr) & 0xffffff;
+    if ((delta <= 3) && (delta != 0)) {
+        return -1;
+    }
+    delta = (next_cc - cntr) & 0xffffff;
+    if ((delta & 0x800000) || (delta < 3)) {
+        return -1;
+    }
+
+    /* Clear and set TIMER0 to fire off at proper time */
+    NRF_TIMER0_NS->TASKS_CLEAR = 1;
+    NRF_TIMER0_NS->CC[0] = rem_usecs;
+    NRF_TIMER0_NS->EVENTS_COMPARE[0] = 0;
+
+    /* Set RTC compare to start TIMER0 */
+    NRF_RTC0_NS->EVENTS_COMPARE[0] = 0;
+    NRF_RTC0_NS->CC[0] = next_cc;
+    NRF_RTC0_NS->EVTENSET = RTC_EVTENSET_COMPARE0_Msk;
+
+    /* Enable PPI */
+    NRF_TIMER0_NS->SUBSCRIBE_START = DPPI_SUBSCRIBE_TIMER0_TASKS_START(1);
+
+    /* Store the cputime at which we set the RTC */
+    g_ble_phy_data.phy_start_cputime = cputime;
+
+    return 0;
+}
+
+static int
+ble_phy_set_start_now(void)
+{
+    os_sr_t sr;
+    uint32_t now;
+
+    OS_ENTER_CRITICAL(sr);
+
+    /*
+     * Set TIMER0 to fire immediately. We can't set CC to 0 as compare will not
+     * occur in such case.
+     */
+    NRF_TIMER0_NS->TASKS_CLEAR = 1;
+    NRF_TIMER0_NS->CC[0] = 1;
+    NRF_TIMER0_NS->EVENTS_COMPARE[0] = 0;
+
+    /*
+     * Set RTC compare to start TIMER0. We need to set it to at least N+2 ticks
+     * from current value to guarantee triggering compare event, but let's set
+     * it to N+3 to account for possible extra tick on RTC0 during these
+     * operations.
+     */
+    now = os_cputime_get32();
+    NRF_RTC0_NS->EVENTS_COMPARE[0] = 0;
+    NRF_RTC0_NS->CC[0] = now + 3;
+    NRF_RTC0_NS->EVTENSET = RTC_EVTENSET_COMPARE0_Msk;
+
+    /* Enable PPI */
+    NRF_TIMER0_NS->SUBSCRIBE_START = DPPI_SUBSCRIBE_TIMER0_TASKS_START(1);
+
+    /*
+     * Store the cputime at which we set the RTC
+     *
+     * XXX Compare event may be triggered on previous CC value (if it was set to
+     * less than N+2) so in rare cases actual start time may be 2 ticks earlier
+     * than what we expect. Since this is only used on RX, it may cause AUX scan
+     * to be scheduled 1 or 2 ticks too late so we'll miss it - it's acceptable
+     * for now.
+     */
+    g_ble_phy_data.phy_start_cputime = now + 3;
+
+    OS_EXIT_CRITICAL(sr);
+
+    return 0;
+}
+
+void
+ble_phy_wfr_enable(int txrx, uint8_t tx_phy_mode, uint32_t wfr_usecs)
+{
+    uint32_t end_time;
+    uint8_t phy;
+
+    phy = g_ble_phy_data.phy_cur_phy_mode;
+
+    if (txrx == BLE_PHY_WFR_ENABLE_TXRX) {
+        /* RX shall start exactly T_IFS after TX end captured in CC[2] */
+        end_time = NRF_TIMER0_NS->CC[2] + BLE_LL_IFS;
+        /* Adjust for delay between EVENT_END and actual TX end time */
+        end_time += g_ble_phy_t_txenddelay[tx_phy_mode];
+        /* Wait a bit longer due to allowed active clock accuracy */
+        end_time += 2;
+        /*
+         * It's possible that we'll capture PDU start time at the end of timer
+         * cycle and since wfr expires at the beginning of calculated timer
+         * cycle it can be almost 1 usec too early. Let's compensate for this
+         * by waiting 1 usec more.
+         */
+        end_time += 1;
+    } else {
+        /*
+         * RX shall start no later than wfr_usecs after RX enabled.
+         * CC[0] is the time of RXEN so adjust for radio ram-up.
+         * Do not add jitter since this is already covered by LL.
+         */
+        end_time = NRF_TIMER0_NS->CC[0] + BLE_PHY_T_RXENFAST + wfr_usecs;
+    }
+
+    /*
+     * Note: on LE Coded EVENT_ADDRESS is fired after TERM1 is received, so
+     *       we are actually calculating relative to start of packet payload
+     *       which is fine.
+     */
+
+    /* Adjust for receiving access address since this triggers EVENT_ADDRESS */
+    end_time += ble_phy_mode_pdu_start_off(phy);
+    /* Adjust for delay between actual access address RX and EVENT_ADDRESS */
+    end_time += g_ble_phy_t_rxaddrdelay[phy];
+
+    /* wfr_secs is the time from rxen until timeout */
+    NRF_TIMER0_NS->CC[3] = end_time;
+    NRF_TIMER0_NS->EVENTS_COMPARE[3] = 0;
+
+    /* Subscribe for wait for response events  */
+    NRF_TIMER0_NS->SUBSCRIBE_CAPTURE[3] = DPPI_SUBSCRIBE_TIMER0_TASKS_CAPTURE3(1);
+    NRF_RADIO_NS->SUBSCRIBE_DISABLE = DPPI_SUBSCRIBE_RADIO_TASKS_DISABLE(1);
+
+    /* Enable the disabled interrupt so we time out on events compare */
+    NRF_RADIO_NS->INTENSET = RADIO_INTENSET_DISABLED_Msk;
+
+    /*
+     * It may happen that if CPU is halted for a brief moment (e.g. during flash
+     * erase or write), TIMER0 already counted past CC[3] and thus wfr will not
+     * fire as expected. In case this happened, let's just disable PPIs for wfr
+     * and trigger wfr manually (i.e. disable radio).
+     *
+     * Note that the same applies to RX start time set in CC[0] but since it
+     * should fire earlier than wfr, fixing wfr is enough.
+     *
+     * CC[1] is only used as a reference on RX start, we do not need it here so
+     * it can be used to read TIMER0 counter.
+     */
+    NRF_TIMER0_NS->TASKS_CAPTURE[1] = 1;
+    if (NRF_TIMER0_NS->CC[1] > NRF_TIMER0_NS->CC[3]) {
+        /* Unsubscribe from wfr events */
+        NRF_TIMER0_NS->SUBSCRIBE_CAPTURE[3] = DPPI_SUBSCRIBE_TIMER0_TASKS_CAPTURE3(0);
+        NRF_RADIO_NS->SUBSCRIBE_DISABLE = DPPI_SUBSCRIBE_RADIO_TASKS_DISABLE(0);
+
+        NRF_RADIO_NS->TASKS_DISABLE = 1;
+    }
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+static uint32_t
+ble_phy_get_ccm_datarate(void)
+{
+#if BLE_LL_BT5_PHY_SUPPORTED
+    switch (g_ble_phy_data.phy_cur_phy_mode) {
+    case BLE_PHY_MODE_1M:
+        return CCM_MODE_DATARATE_1Mbit << CCM_MODE_DATARATE_Pos;
+    case BLE_PHY_MODE_2M:
+        return CCM_MODE_DATARATE_2Mbit << CCM_MODE_DATARATE_Pos;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    case BLE_PHY_MODE_CODED_125KBPS:
+        return CCM_MODE_DATARATE_125Kbps << CCM_MODE_DATARATE_Pos;
+    case BLE_PHY_MODE_CODED_500KBPS:
+        return CCM_MODE_DATARATE_500Kbps << CCM_MODE_DATARATE_Pos;
+#endif
+    }
+
+    assert(0);
+    return 0;
+#else
+    return CCM_MODE_DATARATE_1Mbit << CCM_MODE_DATARATE_Pos;
+#endif
+}
+#endif
+
+/**
+ * Setup transceiver for receive.
+ */
+static void
+ble_phy_rx_xcvr_setup(void)
+{
+    uint8_t *dptr;
+
+    dptr = (uint8_t *)&g_ble_phy_rx_buf[0];
+    dptr += 3;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    if (g_ble_phy_data.phy_encrypted) {
+        NRF_RADIO_NS->PACKETPTR = (uint32_t)&g_ble_phy_enc_buf[0];
+        NRF_CCM_NS->INPTR = (uint32_t)&g_ble_phy_enc_buf[0];
+        NRF_CCM_NS->OUTPTR = (uint32_t)dptr;
+        NRF_CCM_NS->SCRATCHPTR = (uint32_t)&nrf_encrypt_scratchpad[0];
+        NRF_CCM_NS->MODE = CCM_MODE_LENGTH_Msk | CCM_MODE_MODE_Decryption |
+                           ble_phy_get_ccm_datarate();
+        NRF_CCM_NS->CNFPTR = (uint32_t)&nrf_ccm_data;
+        NRF_CCM_NS->SHORTS = 0;
+        NRF_CCM_NS->EVENTS_ERROR = 0;
+        NRF_CCM_NS->EVENTS_ENDCRYPT = 0;
+        NRF_CCM_NS->TASKS_KSGEN = 1;
+
+        /* Subscribe to radio address event */
+        NRF_CCM_NS->SUBSCRIBE_CRYPT = DPPI_SUBSCRIBE_CCM_TASKS_CRYPT(1);
+    } else {
+        NRF_RADIO_NS->PACKETPTR = (uint32_t)dptr;
+    }
+#else
+    NRF_RADIO_NS->PACKETPTR = (uint32_t)dptr;
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    if (g_ble_phy_data.phy_privacy) {
+        NRF_AAR_NS->ENABLE = AAR_ENABLE_ENABLE_Enabled;
+        NRF_AAR_NS->IRKPTR = (uint32_t)&g_nrf_irk_list[0];
+        NRF_AAR_NS->SCRATCHPTR = (uint32_t)&g_ble_phy_data.phy_aar_scratch;
+        NRF_AAR_NS->EVENTS_END = 0;
+        NRF_AAR_NS->EVENTS_RESOLVED = 0;
+        NRF_AAR_NS->EVENTS_NOTRESOLVED = 0;
+    } else {
+        if (g_ble_phy_data.phy_encrypted == 0) {
+            NRF_AAR_NS->ENABLE = AAR_ENABLE_ENABLE_Disabled;
+        }
+    }
+#endif
+
+    /* Turn off trigger TXEN on output compare match and AAR on bcmatch */
+    NRF_RADIO_NS->SUBSCRIBE_TXEN = DPPI_SUBSCRIBE_RADIO_TASKS_TXEN(0);
+    NRF_AAR_NS->SUBSCRIBE_START = DPPI_SUBSCRIBE_AAR_TASKS_START(0);
+
+    /* Reset the rx started flag. Used for the wait for response */
+    g_ble_phy_data.phy_rx_started = 0;
+    g_ble_phy_data.phy_state = BLE_PHY_STATE_RX;
+
+#if BLE_LL_BT5_PHY_SUPPORTED
+    /*
+     * On Coded PHY there are CI and TERM1 fields before PDU starts so we need
+     * to take this into account when setting up BCC.
+     */
+    if (g_ble_phy_data.phy_cur_phy_mode == BLE_PHY_MODE_CODED_125KBPS ||
+        g_ble_phy_data.phy_cur_phy_mode == BLE_PHY_MODE_CODED_500KBPS) {
+        g_ble_phy_data.phy_bcc_offset = 5;
+    } else {
+        g_ble_phy_data.phy_bcc_offset = 0;
+    }
+#else
+    g_ble_phy_data.phy_bcc_offset = 0;
+#endif
+
+    /* I want to know when 1st byte received (after address) */
+    NRF_RADIO_NS->BCC = 8 + g_ble_phy_data.phy_bcc_offset; /* in bits */
+    NRF_RADIO_NS->EVENTS_ADDRESS = 0;
+    NRF_RADIO_NS->EVENTS_DEVMATCH = 0;
+    NRF_RADIO_NS->EVENTS_BCMATCH = 0;
+    NRF_RADIO_NS->EVENTS_RSSIEND = 0;
+    NRF_RADIO_NS->EVENTS_CRCOK = 0;
+    NRF_RADIO_NS->SHORTS = RADIO_SHORTS_END_DISABLE_Msk |
+                           RADIO_SHORTS_READY_START_Msk |
+                           RADIO_SHORTS_ADDRESS_BCSTART_Msk |
+                           RADIO_SHORTS_ADDRESS_RSSISTART_Msk |
+                           RADIO_SHORTS_DISABLED_RSSISTOP_Msk;
+
+    NRF_RADIO_NS->INTENSET = RADIO_INTENSET_ADDRESS_Msk;
+}
+
+/**
+ * Called from interrupt context when the transmit ends
+ *
+ */
+static void
+ble_phy_tx_end_isr(void)
+{
+    uint8_t tx_phy_mode;
+    uint8_t was_encrypted;
+    uint8_t transition;
+    uint32_t rx_time;
+    uint32_t wfr_time;
+
+    /* Store PHY on which we've just transmitted smth */
+    tx_phy_mode = g_ble_phy_data.phy_cur_phy_mode;
+
+    /* If this transmission was encrypted we need to remember it */
+    was_encrypted = g_ble_phy_data.phy_encrypted;
+    (void)was_encrypted;
+
+    /* Better be in TX state! */
+    assert(g_ble_phy_data.phy_state == BLE_PHY_STATE_TX);
+
+    /* Clear events and clear interrupt on disabled event */
+    NRF_RADIO_NS->EVENTS_DISABLED = 0;
+    NRF_RADIO_NS->INTENCLR = RADIO_INTENCLR_DISABLED_Msk;
+    NRF_RADIO_NS->EVENTS_END = 0;
+    wfr_time = NRF_RADIO_NS->SHORTS;
+    (void)wfr_time;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    /*
+     * XXX: not sure what to do. We had a HW error during transmission.
+     * For now I just count a stat but continue on like all is good.
+     */
+    if (was_encrypted) {
+        if (NRF_CCM_NS->EVENTS_ERROR) {
+            STATS_INC(ble_phy_stats, tx_hw_err);
+            NRF_CCM_NS->EVENTS_ERROR = 0;
+        }
+    }
+#endif
+
+    /* Call transmit end callback */
+    if (g_ble_phy_data.txend_cb) {
+        g_ble_phy_data.txend_cb(g_ble_phy_data.txend_arg);
+    }
+
+    transition = g_ble_phy_data.phy_transition;
+    if (transition == BLE_PHY_TRANSITION_TX_RX) {
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+        ble_phy_mode_apply(g_ble_phy_data.phy_rx_phy_mode);
+#endif
+
+        /* Packet pointer needs to be reset. */
+        ble_phy_rx_xcvr_setup();
+
+        ble_phy_wfr_enable(BLE_PHY_WFR_ENABLE_TXRX, tx_phy_mode, 0);
+
+        /* Schedule RX exactly T_IFS after TX end captured in CC[2] */
+        rx_time = NRF_TIMER0_NS->CC[2] + BLE_LL_IFS;
+        /* Adjust for delay between EVENT_END and actual TX end time */
+        rx_time += g_ble_phy_t_txenddelay[tx_phy_mode];
+        /* Adjust for radio ramp-up */
+        rx_time -= BLE_PHY_T_RXENFAST;
+        /* Start listening a bit earlier due to allowed active clock accuracy */
+        rx_time -= 2;
+
+        NRF_TIMER0_NS->CC[0] = rx_time;
+        NRF_TIMER0_NS->EVENTS_COMPARE[0] = 0;
+
+        /* Start radio on timer */
+        NRF_RADIO_NS->SUBSCRIBE_RXEN = DPPI_SUBSCRIBE_RADIO_TASKS_RXEN(1);
+
+    } else {
+        NRF_TIMER0_NS->TASKS_STOP = 1;
+        NRF_TIMER0_NS->TASKS_SHUTDOWN = 1;
+
+        NRF_TIMER0_NS->SUBSCRIBE_CAPTURE[3] = DPPI_SUBSCRIBE_TIMER0_TASKS_CAPTURE3(0);
+        NRF_RADIO_NS->SUBSCRIBE_DISABLE = DPPI_SUBSCRIBE_RADIO_TASKS_DISABLE(0);
+        NRF_RADIO_NS->SUBSCRIBE_TXEN = DPPI_SUBSCRIBE_RADIO_TASKS_TXEN(0);
+        NRF_TIMER0_NS->SUBSCRIBE_START = DPPI_SUBSCRIBE_TIMER0_TASKS_START(0);
+
+        assert(transition == BLE_PHY_TRANSITION_NONE);
+    }
+}
+
+static inline uint8_t
+ble_phy_get_cur_rx_phy_mode(void)
+{
+    uint8_t phy;
+
+    phy = g_ble_phy_data.phy_cur_phy_mode;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)
+    /*
+     * For Coded PHY mode can be set to either codings since actual coding is
+     * set in packet header. However, here we need actual coding of received
+     * packet as this determines pipeline delays so need to figure this out
+     * using CI field.
+     */
+    if ((phy == BLE_PHY_MODE_CODED_125KBPS) ||
+        (phy == BLE_PHY_MODE_CODED_500KBPS)) {
+        phy = NRF_RADIO_NS->PDUSTAT & RADIO_PDUSTAT_CISTAT_Msk ?
+              BLE_PHY_MODE_CODED_500KBPS : BLE_PHY_MODE_CODED_125KBPS;
+    }
+#endif
+
+    return phy;
+}
+
+static void
+ble_phy_rx_end_isr(void)
+{
+    int rc;
+    uint8_t *dptr;
+    uint8_t crcok;
+    uint32_t tx_time;
+    struct ble_mbuf_hdr *ble_hdr;
+
+    /* Clear events and clear interrupt */
+    NRF_RADIO_NS->EVENTS_END = 0;
+    NRF_RADIO_NS->INTENCLR = RADIO_INTENCLR_END_Msk;
+
+    /* Disable automatic RXEN */
+    NRF_RADIO_NS->SUBSCRIBE_RXEN = DPPI_SUBSCRIBE_RADIO_TASKS_RXEN(0);
+
+    /* Set RSSI and CRC status flag in header */
+    ble_hdr = &g_ble_phy_data.rxhdr;
+    assert(NRF_RADIO_NS->EVENTS_RSSIEND != 0);
+    ble_hdr->rxinfo.rssi = (-1 * NRF_RADIO_NS->RSSISAMPLE) +
+                           g_ble_phy_data.rx_pwr_compensation;
+
+    dptr = (uint8_t *)&g_ble_phy_rx_buf[0];
+    dptr += 3;
+
+    /* Count PHY crc errors and valid packets */
+    crcok = NRF_RADIO_NS->EVENTS_CRCOK;
+    if (!crcok) {
+        STATS_INC(ble_phy_stats, rx_crc_err);
+    } else {
+        STATS_INC(ble_phy_stats, rx_valid);
+        ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_CRC_OK;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+        if (g_ble_phy_data.phy_encrypted) {
+            /* Only set MIC failure flag if frame is not zero length */
+            if ((dptr[1] != 0) && (NRF_CCM_NS->MICSTATUS == 0)) {
+                ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_MIC_FAILURE;
+            }
+
+            /*
+             * XXX: not sure how to deal with this. This should not
+             * be a MIC failure but we should not hand it up. I guess
+             * this is just some form of rx error and that is how we
+             * handle it? For now, just set CRC error flags
+             */
+            if (NRF_CCM_NS->EVENTS_ERROR) {
+                STATS_INC(ble_phy_stats, rx_hw_err);
+                ble_hdr->rxinfo.flags &= ~BLE_MBUF_HDR_F_CRC_OK;
+            }
+
+            /*
+             * XXX: This is a total hack work-around for now but I dont
+             * know what else to do. If ENDCRYPT is not set and we are
+             * encrypted we need to not trust this frame and drop it.
+             */
+            if (NRF_CCM_NS->EVENTS_ENDCRYPT == 0) {
+                STATS_INC(ble_phy_stats, rx_hw_err);
+                ble_hdr->rxinfo.flags &= ~BLE_MBUF_HDR_F_CRC_OK;
+            }
+        }
+#endif
+    }
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    ble_phy_mode_apply(g_ble_phy_data.phy_tx_phy_mode);
+#endif
+
+    /*
+     * Let's schedule TX now and we will just cancel it after processing RXed
+     * packet if we don't need TX.
+     *
+     * We need this to initiate connection in case AUX_CONNECT_REQ was sent on
+     * LE Coded S8. In this case the time we process RXed packet is roughly the
+     * same as the limit when we need to have TX scheduled (i.e. TIMER0 and PPI
+     * armed) so we may simply miss the slot and set the timer in the past.
+     *
+     * When TX is scheduled in advance, we may event process packet a bit longer
+     * during radio ramp-up - this gives us extra 40 usecs which is more than
+     * enough.
+     */
+
+    /* Schedule TX exactly T_IFS after RX end captured in CC[2] */
+    tx_time = NRF_TIMER0_NS->CC[2] + BLE_LL_IFS;
+    /* Adjust for delay between actual RX end time and EVENT_END */
+    tx_time -= g_ble_phy_t_rxenddelay[ble_hdr->rxinfo.phy_mode];
+    /* Adjust for radio ramp-up */
+    tx_time -= BLE_PHY_T_TXENFAST;
+    /* Adjust for delay between EVENT_READY and actual TX start time */
+    tx_time -= g_ble_phy_t_txdelay[g_ble_phy_data.phy_cur_phy_mode];
+
+    NRF_TIMER0_NS->CC[0] = tx_time;
+    NRF_TIMER0_NS->EVENTS_COMPARE[0] = 0;
+
+    /* Enable automatic TX */
+    NRF_RADIO_NS->SUBSCRIBE_TXEN = DPPI_SUBSCRIBE_RADIO_TASKS_TXEN(1);
+
+    /*
+     * XXX: Hack warning!
+     *
+     * It may happen (during flash erase) that CPU is stopped for a moment and
+     * TIMER0 already counted past CC[0]. In such case we will be stuck waiting
+     * for TX to start since EVENTS_COMPARE[0] will not happen any time soon.
+     * For now let's set a flag denoting that we are late in RX-TX transition so
+     * ble_phy_tx() will fail - this allows everything to cleanup nicely without
+     * the need for extra handling in many places.
+     *
+     * Note: CC[3] is used only for wfr which we do not need here.
+     */
+    NRF_TIMER0_NS->TASKS_CAPTURE[3] = 1;
+    if (NRF_TIMER0_NS->CC[3] > NRF_TIMER0_NS->CC[0]) {
+        NRF_RADIO_NS->SUBSCRIBE_TXEN = DPPI_SUBSCRIBE_RADIO_TASKS_TXEN(0);
+
+        g_ble_phy_data.phy_transition_late = 1;
+    }
+
+    /*
+     * XXX: This is a horrible ugly hack to deal with the RAM S1 byte
+     * that is not sent over the air but is present here. Simply move the
+     * data pointer to deal with it. Fix this later.
+     */
+    dptr[2] = dptr[1];
+    dptr[1] = dptr[0];
+    rc = ble_ll_rx_end(dptr + 1, ble_hdr);
+    if (rc < 0) {
+        ble_phy_disable();
+    }
+}
+
+static bool
+ble_phy_rx_start_isr(void)
+{
+    int rc;
+    uint32_t state;
+    uint32_t usecs;
+    uint32_t pdu_usecs;
+    uint32_t ticks;
+    struct ble_mbuf_hdr *ble_hdr;
+    uint8_t *dptr;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    int adva_offset;
+#endif
+
+    dptr = (uint8_t *)&g_ble_phy_rx_buf[0];
+
+    /* Clear events and clear interrupt */
+    NRF_RADIO_NS->EVENTS_ADDRESS = 0;
+
+    /* Clear wfr timer channels and DISABLED interrupt */
+    NRF_RADIO_NS->INTENCLR = RADIO_INTENCLR_DISABLED_Msk | RADIO_INTENCLR_ADDRESS_Msk;
+    NRF_TIMER0_NS->SUBSCRIBE_CAPTURE[3] = DPPI_SUBSCRIBE_TIMER0_TASKS_CAPTURE3(0);
+    NRF_RADIO_NS->SUBSCRIBE_DISABLE = DPPI_SUBSCRIBE_RADIO_TASKS_DISABLE(0);
+
+    /* Initialize the ble mbuf header */
+    ble_hdr = &g_ble_phy_data.rxhdr;
+    ble_hdr->rxinfo.flags = ble_ll_state_get();
+    ble_hdr->rxinfo.channel = g_ble_phy_data.phy_chan;
+    ble_hdr->rxinfo.handle = 0;
+    ble_hdr->rxinfo.phy = ble_phy_get_cur_phy();
+    ble_hdr->rxinfo.phy_mode = ble_phy_get_cur_rx_phy_mode();
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    ble_hdr->rxinfo.user_data = NULL;
+#endif
+
+    /*
+     * Calculate accurate packets start time (with remainder)
+     *
+     * We may start receiving packet somewhere during preamble in which case
+     * it is possible that actual transmission started before TIMER0 was
+     * running - need to take this into account.
+     */
+    ble_hdr->beg_cputime = g_ble_phy_data.phy_start_cputime;
+
+    usecs = NRF_TIMER0_NS->CC[1];
+    pdu_usecs = ble_phy_mode_pdu_start_off(ble_hdr->rxinfo.phy_mode) +
+                g_ble_phy_t_rxaddrdelay[ble_hdr->rxinfo.phy_mode];
+    if (usecs < pdu_usecs) {
+        g_ble_phy_data.phy_start_cputime--;
+        usecs += 30;
+    }
+    usecs -= pdu_usecs;
+
+    ticks = os_cputime_usecs_to_ticks(usecs);
+    usecs -= os_cputime_ticks_to_usecs(ticks);
+    if (usecs == 31) {
+        usecs = 0;
+        ++ticks;
+    }
+
+    ble_hdr->beg_cputime += ticks;
+    ble_hdr->rem_usecs = usecs;
+
+    /* Wait to get 1st byte of frame */
+    while (1) {
+        state = NRF_RADIO_NS->STATE;
+        if (NRF_RADIO_NS->EVENTS_BCMATCH != 0) {
+            break;
+        }
+
+        /*
+         * If state is disabled, we should have the BCMATCH. If not,
+         * something is wrong!
+         */
+        if (state == RADIO_STATE_STATE_Disabled) {
+            NRF_RADIO_NS->INTENCLR = NRF_RADIO_IRQ_MASK_ALL;
+            NRF_RADIO_NS->SHORTS = 0;
+            return false;
+        }
+    }
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    /*
+     * If privacy is enabled and received PDU has TxAdd bit set (i.e. random
+     * address) we try to resolve address using AAR.
+     */
+    if (g_ble_phy_data.phy_privacy && (dptr[3] & 0x40)) {
+        /*
+         * AdvA is located at 4th octet in RX buffer (after S0, length an S1
+         * fields). In case of extended advertising PDU we need to add 2 more
+         * octets for extended header.
+         */
+        adva_offset = (dptr[3] & 0x0f) == 0x07 ? 2 : 0;
+        NRF_AAR_NS->ADDRPTR = (uint32_t)(dptr + 3 + adva_offset);
+
+        /* Trigger AAR after last bit of AdvA is received */
+        NRF_RADIO_NS->EVENTS_BCMATCH = 0;
+        NRF_AAR_NS->SUBSCRIBE_START = DPPI_SUBSCRIBE_AAR_TASKS_START(1);
+        NRF_RADIO_NS->BCC = (BLE_LL_PDU_HDR_LEN + adva_offset + BLE_DEV_ADDR_LEN) * 8 +
+                            g_ble_phy_data.phy_bcc_offset;
+    }
+#endif
+
+    /* Call Link Layer receive start function */
+    rc = ble_ll_rx_start(dptr + 3,
+                         g_ble_phy_data.phy_chan,
+                         &g_ble_phy_data.rxhdr);
+    if (rc >= 0) {
+        /* Set rx started flag and enable rx end ISR */
+        g_ble_phy_data.phy_rx_started = 1;
+        NRF_RADIO_NS->INTENSET = RADIO_INTENSET_END_Msk;
+    } else {
+        /* Disable PHY */
+        ble_phy_disable();
+        STATS_INC(ble_phy_stats, rx_aborts);
+    }
+
+    /* Count rx starts */
+    STATS_INC(ble_phy_stats, rx_starts);
+
+    return true;
+}
+
+static void
+ble_phy_isr(void)
+{
+    uint32_t irq_en;
+
+    os_trace_isr_enter();
+
+    /* Read irq register to determine which interrupts are enabled */
+    irq_en = NRF_RADIO_NS->INTENCLR;
+
+    /*
+     * NOTE: order of checking is important! Possible, if things get delayed,
+     * we have both an ADDRESS and DISABLED interrupt in rx state. If we get
+     * an address, we disable the DISABLED interrupt.
+     */
+
+    /* We get this if we have started to receive a frame */
+    if ((irq_en & RADIO_INTENCLR_ADDRESS_Msk) && NRF_RADIO_NS->EVENTS_ADDRESS) {
+        /*
+         * wfr timer is calculated to expire at the exact time we should start
+         * receiving a packet (with 1 usec precision) so it is possible  it will
+         * fire at the same time as EVENT_ADDRESS. If this happens, radio will
+         * be disabled while we are waiting for EVENT_BCCMATCH after 1st byte
+         * of payload is received and ble_phy_rx_start_isr() will fail. In this
+         * case we should not clear DISABLED irq mask so it will be handled as
+         * regular radio disabled event below. In other case radio was disabled
+         * on purpose and there's nothing more to handle so we can clear mask.
+         */
+        if (ble_phy_rx_start_isr()) {
+            irq_en &= ~RADIO_INTENCLR_DISABLED_Msk;
+        }
+    }
+
+    /* Check for disabled event. This only happens for transmits now */
+    if ((irq_en & RADIO_INTENCLR_DISABLED_Msk) && NRF_RADIO_NS->EVENTS_DISABLED) {
+        if (g_ble_phy_data.phy_state == BLE_PHY_STATE_RX) {
+            NRF_RADIO_NS->EVENTS_DISABLED = 0;
+            ble_ll_wfr_timer_exp(NULL);
+        } else if (g_ble_phy_data.phy_state == BLE_PHY_STATE_IDLE) {
+            assert(0);
+        } else {
+            ble_phy_tx_end_isr();
+        }
+    }
+
+    /* Receive packet end (we dont enable this for transmit) */
+    if ((irq_en & RADIO_INTENCLR_END_Msk) && NRF_RADIO_NS->EVENTS_END) {
+        ble_phy_rx_end_isr();
+    }
+
+    g_ble_phy_data.phy_transition_late = 0;
+
+    /* Ensures IRQ is cleared */
+    irq_en = NRF_RADIO_NS->SHORTS;
+
+    /* Count # of interrupts */
+    STATS_INC(ble_phy_stats, phy_isrs);
+
+    os_trace_isr_exit();
+}
+
+int
+ble_phy_init(void)
+{
+    int rc;
+
+    /* Default phy to use is 1M */
+    g_ble_phy_data.phy_cur_phy_mode = BLE_PHY_MODE_1M;
+    g_ble_phy_data.phy_tx_phy_mode = BLE_PHY_MODE_1M;
+    g_ble_phy_data.phy_rx_phy_mode = BLE_PHY_MODE_1M;
+
+    g_ble_phy_data.rx_pwr_compensation = 0;
+
+    /* Set phy channel to an invalid channel so first set channel works */
+    g_ble_phy_data.phy_chan = BLE_PHY_NUM_CHANS;
+
+    /* Toggle peripheral power to reset (just in case) */
+    NRF_RADIO_NS->POWER = 0;
+    NRF_RADIO_NS->POWER = 1;
+
+    /* Errata 16 - RADIO: POWER register is not functional
+     * Workaround: Reset all RADIO registers in firmware.
+     */
+    NRF_RADIO_NS->SUBSCRIBE_TXEN = 0;
+    NRF_RADIO_NS->SUBSCRIBE_RXEN = 0;
+    NRF_RADIO_NS->SUBSCRIBE_DISABLE = 0;
+
+    /* Disable all interrupts */
+    NRF_RADIO_NS->INTENCLR = NRF_RADIO_IRQ_MASK_ALL;
+
+    /* Set configuration registers */
+    NRF_RADIO_NS->MODE = RADIO_MODE_MODE_Ble_1Mbit;
+    NRF_RADIO_NS->PCNF0 = NRF_PCNF0;
+
+    /* XXX: should maxlen be 251 for encryption? */
+    NRF_RADIO_NS->PCNF1 = NRF_MAXLEN |
+                          (RADIO_PCNF1_ENDIAN_Little <<  RADIO_PCNF1_ENDIAN_Pos) |
+                          (NRF_BALEN << RADIO_PCNF1_BALEN_Pos) |
+                          RADIO_PCNF1_WHITEEN_Msk;
+
+    /* Enable radio fast ramp-up */
+    NRF_RADIO_NS->MODECNF0 |= (RADIO_MODECNF0_RU_Fast << RADIO_MODECNF0_RU_Pos) & RADIO_MODECNF0_RU_Msk;
+
+    /* Set logical address 1 for TX and RX */
+    NRF_RADIO_NS->TXADDRESS = 0;
+    NRF_RADIO_NS->RXADDRESSES = (1 << 0);
+
+    /* Configure the CRC registers */
+    NRF_RADIO_NS->CRCCNF = (RADIO_CRCCNF_SKIPADDR_Skip << RADIO_CRCCNF_SKIPADDR_Pos) | RADIO_CRCCNF_LEN_Three;
+
+    /* Configure BLE poly */
+    NRF_RADIO_NS->CRCPOLY = 0x0000065B;
+
+    /* Configure IFS */
+    NRF_RADIO_NS->TIFS = BLE_LL_IFS;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    NRF_CCM_NS->INTENCLR = 0xffffffff;
+    NRF_CCM_NS->SHORTS = CCM_SHORTS_ENDKSGEN_CRYPT_Msk;
+    NRF_CCM_NS->EVENTS_ERROR = 0;
+    memset(nrf_encrypt_scratchpad, 0, sizeof(nrf_encrypt_scratchpad));
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+    g_ble_phy_data.phy_aar_scratch = 0;
+    NRF_AAR_NS->IRKPTR = (uint32_t)&g_nrf_irk_list[0];
+    NRF_AAR_NS->INTENCLR = 0xffffffff;
+    NRF_AAR_NS->EVENTS_END = 0;
+    NRF_AAR_NS->EVENTS_RESOLVED = 0;
+    NRF_AAR_NS->EVENTS_NOTRESOLVED = 0;
+    NRF_AAR_NS->NIRK = 0;
+#endif
+
+    /* TIMER0 setup for PHY when using RTC */
+    NRF_TIMER0_NS->TASKS_STOP = 1;
+    NRF_TIMER0_NS->TASKS_SHUTDOWN = 1;
+    NRF_TIMER0_NS->BITMODE = 3;    /* 32-bit timer */
+    NRF_TIMER0_NS->MODE = 0;       /* Timer mode */
+    NRF_TIMER0_NS->PRESCALER = 4;  /* gives us 1 MHz */
+
+    /* Publish events */
+    NRF_TIMER0_NS->PUBLISH_COMPARE[0] = DPPI_PUBLISH_TIMER0_EVENTS_COMPARE_0;
+    NRF_TIMER0_NS->PUBLISH_COMPARE[3] = DPPI_PUBLISH_TIMER0_EVENTS_COMPARE_3;
+    NRF_RADIO_NS->PUBLISH_END = DPPI_PUBLISH_RADIO_EVENTS_END;
+    NRF_RADIO_NS->PUBLISH_BCMATCH = DPPI_PUBLISH_RADIO_EVENTS_BCMATCH;
+    NRF_RADIO_NS->PUBLISH_ADDRESS = DPPI_PUBLISH_RADIO_EVENTS_ADDRESS;
+    NRF_RTC0_NS->PUBLISH_COMPARE[0] = DPPI_PUBLISH_RTC0_EVENTS_COMPARE_0;
+
+    /* Enable channels we publish on */
+    NRF_DPPIC_NS->CHENSET = DPPI_CH_ENABLE_ALL;
+
+    /* Captures tx/rx start in timer0 cc 1 and tx/rx end in timer0 cc 2 */
+    NRF_TIMER0_NS->SUBSCRIBE_CAPTURE[1] = DPPI_SUBSCRIBE_TIMER0_TASKS_CAPTURE1(1);
+    NRF_TIMER0_NS->SUBSCRIBE_CAPTURE[2] = DPPI_SUBSCRIBE_TIMER0_TASKS_CAPTURE2(1);
+
+    /* Set isr in vector table and enable interrupt */
+#ifndef RIOT_VERSION
+    NVIC_SetPriority(RADIO_IRQn, 0);
+#endif
+#if MYNEWT
+    NVIC_SetVector(RADIO_IRQn, (uint32_t)ble_phy_isr);
+#else
+    ble_npl_hw_set_isr(RADIO_IRQn, ble_phy_isr);
+#endif
+    NVIC_EnableIRQ(RADIO_IRQn);
+
+    /* Register phy statistics */
+    if (!g_ble_phy_data.phy_stats_initialized) {
+        rc = stats_init_and_reg(STATS_HDR(ble_phy_stats),
+                                STATS_SIZE_INIT_PARMS(ble_phy_stats,
+                                                      STATS_SIZE_32),
+                                STATS_NAME_INIT_PARMS(ble_phy_stats),
+                                "ble_phy");
+        assert(rc == 0);
+
+        g_ble_phy_data.phy_stats_initialized = 1;
+    }
+
+    return 0;
+}
+
+int
+ble_phy_rx(void)
+{
+    /*
+     * Check radio state.
+     *
+     * In case radio is now disabling we'll wait for it to finish, but if for
+     * any reason it's just in idle state we proceed with RX as usual since
+     * nRF52 radio can ramp-up from idle state as well.
+     *
+     * Note that TX and RX states values are the same except for 3rd bit so we
+     * can make a shortcut here when checking for idle state.
+     */
+    nrf_wait_disabled();
+    if ((NRF_RADIO_NS->STATE != RADIO_STATE_STATE_Disabled) &&
+        ((NRF_RADIO_NS->STATE & 0x07) != RADIO_STATE_STATE_RxIdle)) {
+        ble_phy_disable();
+        STATS_INC(ble_phy_stats, radio_state_errs);
+        return BLE_PHY_ERR_RADIO_STATE;
+    }
+
+    /* Make sure all interrupts are disabled */
+    NRF_RADIO_NS->INTENCLR = NRF_RADIO_IRQ_MASK_ALL;
+
+    /* Clear events prior to enabling receive */
+    NRF_RADIO_NS->EVENTS_END = 0;
+    NRF_RADIO_NS->EVENTS_DISABLED = 0;
+
+    /* Setup for rx */
+    ble_phy_rx_xcvr_setup();
+
+    /* task to start RX should be subscribed here  */
+    assert(NRF_RADIO_NS->SUBSCRIBE_RXEN & DPPI_SUBSCRIBE_RADIO_TASKS_RXEN(1));
+
+    return 0;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+void
+ble_phy_encrypt_enable(uint64_t pkt_counter, uint8_t *iv, uint8_t *key,
+                       uint8_t is_master)
+{
+    memcpy(nrf_ccm_data.key, key, 16);
+    nrf_ccm_data.pkt_counter = pkt_counter;
+    memcpy(nrf_ccm_data.iv, iv, 8);
+    nrf_ccm_data.dir_bit = is_master;
+    g_ble_phy_data.phy_encrypted = 1;
+    /* Enable the module (AAR cannot be on while CCM on) */
+    NRF_AAR_NS->ENABLE = AAR_ENABLE_ENABLE_Disabled;
+    NRF_CCM_NS->ENABLE = CCM_ENABLE_ENABLE_Enabled;
+}
+
+void
+ble_phy_encrypt_set_pkt_cntr(uint64_t pkt_counter, int dir)
+{
+    nrf_ccm_data.pkt_counter = pkt_counter;
+    nrf_ccm_data.dir_bit = dir;
+}
+
+void
+ble_phy_encrypt_disable(void)
+{
+    NRF_CCM_NS->SUBSCRIBE_CRYPT = DPPI_SUBSCRIBE_CCM_TASKS_CRYPT(0);
+    NRF_CCM_NS->TASKS_STOP = 1;
+    NRF_CCM_NS->EVENTS_ERROR = 0;
+    NRF_CCM_NS->ENABLE = CCM_ENABLE_ENABLE_Disabled;
+
+    g_ble_phy_data.phy_encrypted = 0;
+}
+#endif
+
+void
+ble_phy_set_txend_cb(ble_phy_tx_end_func txend_cb, void *arg)
+{
+    /* Set transmit end callback and arg */
+    g_ble_phy_data.txend_cb = txend_cb;
+    g_ble_phy_data.txend_arg = arg;
+}
+
+int
+ble_phy_tx_set_start_time(uint32_t cputime, uint8_t rem_usecs)
+{
+    int rc;
+
+    ble_phy_trace_u32x2(BLE_PHY_TRACE_ID_START_TX, cputime, rem_usecs);
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    ble_phy_mode_apply(g_ble_phy_data.phy_tx_phy_mode);
+#endif
+
+    /* XXX: This should not be necessary, but paranoia is good! */
+    /* Clear timer0 compare to RXEN since we are transmitting */
+    NRF_RADIO_NS->SUBSCRIBE_RXEN = DPPI_SUBSCRIBE_RADIO_TASKS_RXEN(0);
+
+    if (ble_phy_set_start_time(cputime, rem_usecs, true) != 0) {
+        STATS_INC(ble_phy_stats, tx_late);
+        ble_phy_disable();
+        rc = BLE_PHY_ERR_TX_LATE;
+    } else {
+        /* Enable PPI to automatically start TXEN */
+        NRF_RADIO_NS->SUBSCRIBE_TXEN = DPPI_SUBSCRIBE_RADIO_TASKS_TXEN(1);
+        rc = 0;
+    }
+    return rc;
+}
+
+int
+ble_phy_rx_set_start_time(uint32_t cputime, uint8_t rem_usecs)
+{
+    bool late = false;
+    int rc = 0;
+
+    ble_phy_trace_u32x2(BLE_PHY_TRACE_ID_START_RX, cputime, rem_usecs);
+
+#if (BLE_LL_BT5_PHY_SUPPORTED == 1)
+    ble_phy_mode_apply(g_ble_phy_data.phy_rx_phy_mode);
+#endif
+
+    /* XXX: This should not be necessary, but paranoia is good! */
+    /* Clear timer0 compare to TXEN since we are transmitting */
+    NRF_RADIO_NS->SUBSCRIBE_TXEN = DPPI_SUBSCRIBE_RADIO_TASKS_TXEN(0);
+
+    if (ble_phy_set_start_time(cputime, rem_usecs, false) != 0) {
+        STATS_INC(ble_phy_stats, rx_late);
+
+        /* We're late so let's just try to start RX as soon as possible */
+        ble_phy_set_start_now();
+
+        late = true;
+    }
+
+    /* Enable PPI to automatically start RXEN */
+    NRF_RADIO_NS->SUBSCRIBE_RXEN = DPPI_SUBSCRIBE_RADIO_TASKS_RXEN(1);
+
+    /* Start rx */
+    rc = ble_phy_rx();
+
+    /*
+     * If we enabled receiver but were late, let's return proper error code so
+     * caller can handle this.
+     */
+    if (!rc && late) {
+        rc = BLE_PHY_ERR_RX_LATE;
+    }
+
+    return rc;
+}
+
+int
+ble_phy_tx(ble_phy_tx_pducb_t pducb, void *pducb_arg, uint8_t end_trans)
+{
+    int rc;
+    uint8_t *dptr;
+    uint8_t *pktptr;
+    uint8_t payload_len;
+    uint8_t hdr_byte;
+    uint32_t state;
+    uint32_t shortcuts;
+
+    if (g_ble_phy_data.phy_transition_late) {
+        ble_phy_disable();
+        STATS_INC(ble_phy_stats, tx_late);
+        return BLE_PHY_ERR_TX_LATE;
+    }
+
+    /*
+     * This check is to make sure that the radio is not in a state where
+     * it is moving to disabled state. If so, let it get there.
+     */
+    nrf_wait_disabled();
+
+    /*
+     * XXX: Although we may not have to do this here, I clear all the PPI
+     * that should not be used when transmitting. Some of them are only enabled
+     * if encryption and/or privacy is on, but I dont care. Better to be
+     * paranoid, and if you are going to clear one, might as well clear them
+     * all.
+     */
+    NRF_TIMER0_NS->SUBSCRIBE_CAPTURE[3] = DPPI_SUBSCRIBE_TIMER0_TASKS_CAPTURE3(0);
+    NRF_RADIO_NS->SUBSCRIBE_DISABLE = DPPI_SUBSCRIBE_RADIO_TASKS_DISABLE(0);
+    NRF_AAR_NS->SUBSCRIBE_START = DPPI_SUBSCRIBE_AAR_TASKS_START(0);
+    NRF_CCM_NS->SUBSCRIBE_CRYPT = DPPI_SUBSCRIBE_CCM_TASKS_CRYPT(0);
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    if (g_ble_phy_data.phy_encrypted) {
+        dptr = (uint8_t *)&g_ble_phy_enc_buf[0];
+        pktptr = (uint8_t *)&g_ble_phy_tx_buf[0];
+        NRF_CCM_NS->SHORTS = CCM_SHORTS_ENDKSGEN_CRYPT_Msk;
+        NRF_CCM_NS->INPTR = (uint32_t)dptr;
+        NRF_CCM_NS->OUTPTR = (uint32_t)pktptr;
+        NRF_CCM_NS->SCRATCHPTR = (uint32_t)&nrf_encrypt_scratchpad[0];
+        NRF_CCM_NS->EVENTS_ERROR = 0;
+        NRF_CCM_NS->MODE = CCM_MODE_LENGTH_Msk | ble_phy_get_ccm_datarate();
+        NRF_CCM_NS->CNFPTR = (uint32_t)&nrf_ccm_data;
+    } else {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+        NRF_AAR_NS->IRKPTR = (uint32_t)&g_nrf_irk_list[0];
+#endif
+        dptr = (uint8_t *)&g_ble_phy_tx_buf[0];
+        pktptr = dptr;
+    }
+#else
+    dptr = (uint8_t *)&g_ble_phy_tx_buf[0];
+    pktptr = dptr;
+#endif
+
+    /* Set PDU payload */
+    payload_len = pducb(&dptr[3], pducb_arg, &hdr_byte);
+
+    /* RAM representation has S0, LENGTH and S1 fields. (3 bytes) */
+    dptr[0] = hdr_byte;
+    dptr[1] = payload_len;
+    dptr[2] = 0;
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+    /* Start key-stream generation and encryption (via short) */
+    if (g_ble_phy_data.phy_encrypted) {
+        NRF_CCM_NS->TASKS_KSGEN = 1;
+    }
+#endif
+
+    NRF_RADIO_NS->PACKETPTR = (uint32_t)pktptr;
+
+    /* Clear the ready, end and disabled events */
+    NRF_RADIO_NS->EVENTS_READY = 0;
+    NRF_RADIO_NS->EVENTS_END = 0;
+    NRF_RADIO_NS->EVENTS_DISABLED = 0;
+
+    /* Enable shortcuts for transmit start/end. */
+    shortcuts = RADIO_SHORTS_END_DISABLE_Msk | RADIO_SHORTS_READY_START_Msk;
+    NRF_RADIO_NS->SHORTS = shortcuts;
+    NRF_RADIO_NS->INTENSET = RADIO_INTENSET_DISABLED_Msk;
+
+    /* Set the PHY transition */
+    g_ble_phy_data.phy_transition = end_trans;
+
+    /* Set transmitted payload length */
+    g_ble_phy_data.phy_tx_pyld_len = payload_len;
+
+    /* If we already started transmitting, abort it! */
+    state = NRF_RADIO_NS->STATE;
+    if (state != RADIO_STATE_STATE_Tx) {
+        /* Set phy state to transmitting and count packet statistics */
+        g_ble_phy_data.phy_state = BLE_PHY_STATE_TX;
+        STATS_INC(ble_phy_stats, tx_good);
+        STATS_INCN(ble_phy_stats, tx_bytes, payload_len + BLE_LL_PDU_HDR_LEN);
+        rc = BLE_ERR_SUCCESS;
+    } else {
+        ble_phy_disable();
+        STATS_INC(ble_phy_stats, tx_late);
+        rc = BLE_PHY_ERR_RADIO_STATE;
+    }
+
+    return rc;
+}
+
+int
+ble_phy_txpwr_set(int dbm)
+{
+    /* "Rail" power level if outside supported range */
+    dbm = ble_phy_txpower_round(dbm);
+
+    NRF_RADIO_NS->TXPOWER = dbm;
+    g_ble_phy_data.phy_txpwr_dbm = dbm;
+
+    return 0;
+}
+
+int
+ble_phy_txpower_round(int dbm)
+{
+    /* "Rail" power level if outside supported range */
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_0dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_0dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg1dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg1dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg2dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg2dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg3dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg4dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg4dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg4dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg5dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg5dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg6dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg6dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg7dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg7dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg8dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg8dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg12dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg12dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg16dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg16dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg20dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg20dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg40dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg40dBm;
+    }
+
+    return (int8_t)RADIO_TXPOWER_TXPOWER_Neg40dBm;
+}
+
+static int
+ble_phy_set_access_addr(uint32_t access_addr)
+{
+    NRF_RADIO_NS->BASE0 = (access_addr << 8);
+    NRF_RADIO_NS->PREFIX0 = (NRF_RADIO_NS->PREFIX0 & 0xFFFFFF00) | (access_addr >> 24);
+
+    g_ble_phy_data.phy_access_address = access_addr;
+
+    return 0;
+}
+
+int
+ble_phy_txpwr_get(void)
+{
+    return g_ble_phy_data.phy_txpwr_dbm;
+}
+
+void
+ble_phy_set_rx_pwr_compensation(int8_t compensation)
+{
+    g_ble_phy_data.rx_pwr_compensation = compensation;
+}
+
+int
+ble_phy_setchan(uint8_t chan, uint32_t access_addr, uint32_t crcinit)
+{
+    assert(chan < BLE_PHY_NUM_CHANS);
+
+    /* Check for valid channel range */
+    if (chan >= BLE_PHY_NUM_CHANS) {
+        return BLE_PHY_ERR_INV_PARAM;
+    }
+
+    /* Set current access address */
+    ble_phy_set_access_addr(access_addr);
+
+    /* Configure crcinit */
+    NRF_RADIO_NS->CRCINIT = crcinit;
+
+    /* Set the frequency and the data whitening initial value */
+    g_ble_phy_data.phy_chan = chan;
+    NRF_RADIO_NS->FREQUENCY = g_ble_phy_chan_freq[chan];
+    NRF_RADIO_NS->DATAWHITEIV = chan;
+
+    return 0;
+}
+
+/**
+ * Stop the timer used to count microseconds when using RTC for cputime
+ */
+static void
+ble_phy_stop_usec_timer(void)
+{
+    NRF_TIMER0_NS->TASKS_STOP = 1;
+    NRF_TIMER0_NS->TASKS_SHUTDOWN = 1;
+    NRF_RTC0_NS->EVTENCLR = RTC_EVTENSET_COMPARE0_Msk;
+}
+
+/**
+ * ble phy disable irq and ppi
+ *
+ * This routine is to be called when reception was stopped due to either a
+ * wait for response timeout or a packet being received and the phy is to be
+ * restarted in receive mode. Generally, the disable routine is called to stop
+ * the phy.
+ */
+static void
+ble_phy_disable_irq_and_ppi(void)
+{
+    NRF_RADIO_NS->INTENCLR = NRF_RADIO_IRQ_MASK_ALL;
+    NRF_RADIO_NS->SHORTS = 0;
+    NRF_RADIO_NS->TASKS_DISABLE = 1;
+
+    NRF_TIMER0_NS->SUBSCRIBE_START = DPPI_SUBSCRIBE_TIMER0_TASKS_START(0);
+    NRF_TIMER0_NS->SUBSCRIBE_CAPTURE[3] = DPPI_SUBSCRIBE_TIMER0_TASKS_CAPTURE3(0);
+    NRF_RADIO_NS->SUBSCRIBE_DISABLE = DPPI_SUBSCRIBE_RADIO_TASKS_DISABLE(0);
+    NRF_RADIO_NS->SUBSCRIBE_TXEN = DPPI_SUBSCRIBE_RADIO_TASKS_TXEN(0);
+    NRF_RADIO_NS->SUBSCRIBE_RXEN = DPPI_SUBSCRIBE_RADIO_TASKS_RXEN(0);
+    NRF_AAR_NS->SUBSCRIBE_START = DPPI_SUBSCRIBE_AAR_TASKS_START(0);
+    NRF_CCM_NS->SUBSCRIBE_CRYPT = DPPI_SUBSCRIBE_CCM_TASKS_CRYPT(0);
+
+    NVIC_ClearPendingIRQ(RADIO_IRQn);
+    g_ble_phy_data.phy_state = BLE_PHY_STATE_IDLE;
+}
+
+void
+ble_phy_restart_rx(void)
+{
+    ble_phy_stop_usec_timer();
+    ble_phy_disable_irq_and_ppi();
+
+    ble_phy_set_start_now();
+    /* Enable PPI to automatically start RXEN */
+    NRF_RADIO_NS->SUBSCRIBE_RXEN = DPPI_SUBSCRIBE_RADIO_TASKS_RXEN(1);
+
+    ble_phy_rx();
+}
+
+void
+ble_phy_disable(void)
+{
+    ble_phy_trace_void(BLE_PHY_TRACE_ID_DISABLE);
+
+    ble_phy_stop_usec_timer();
+    ble_phy_disable_irq_and_ppi();
+}
+
+uint32_t
+ble_phy_access_addr_get(void)
+{
+    return g_ble_phy_data.phy_access_address;
+}
+
+int
+ble_phy_state_get(void)
+{
+    return g_ble_phy_data.phy_state;
+}
+
+int
+ble_phy_rx_started(void)
+{
+    return g_ble_phy_data.phy_rx_started;
+}
+
+uint8_t
+ble_phy_xcvr_state_get(void)
+{
+    uint32_t state;
+    state = NRF_RADIO_NS->STATE;
+    return (uint8_t)state;
+}
+
+uint8_t
+ble_phy_max_data_pdu_pyld(void)
+{
+    return BLE_LL_DATA_PDU_MAX_PYLD;
+}
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
+void
+ble_phy_resolv_list_enable(void)
+{
+    NRF_AAR_NS->NIRK = (uint32_t)g_nrf_num_irks;
+    g_ble_phy_data.phy_privacy = 1;
+}
+
+void
+ble_phy_resolv_list_disable(void)
+{
+    g_ble_phy_data.phy_privacy = 0;
+}
+#endif
+
+#if MYNEWT_VAL(BLE_LL_DTM)
+void
+ble_phy_enable_dtm(void)
+{
+    /* When DTM is enabled we need to disable whitening as per
+     * Bluetooth v5.0 Vol 6. Part F. 4.1.1
+     */
+    NRF_RADIO_NS->PCNF1 &= ~RADIO_PCNF1_WHITEEN_Msk;
+}
+
+void
+ble_phy_disable_dtm(void)
+{
+    /* Enable whitening */
+    NRF_RADIO_NS->PCNF1 |= RADIO_PCNF1_WHITEEN_Msk;
+}
+#endif
+
+void
+ble_phy_rfclk_enable(void)
+{
+#if MYNEWT
+    nrf5340_net_clock_hfxo_request();
+#else
+    NRF_CLOCK_NS->TASKS_HFCLKSTART = 1;
+#endif
+}
+
+void
+ble_phy_rfclk_disable(void)
+{
+#if MYNEWT
+    nrf5340_net_clock_hfxo_release();
+#else
+    NRF_CLOCK_NS->TASKS_HFCLKSTOP = 1;
+#endif
+}

nimble/drivers/nrf5340/src/ble_phy_trace.c

@@ -0,0 +1,44 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <stdint.h>
+#include <syscfg/syscfg.h>
+#include <os/os_trace_api.h>
+
+#if MYNEWT_VAL(BLE_PHY_SYSVIEW)
+
+static os_trace_module_t g_ble_phy_trace_mod;
+uint32_t ble_phy_trace_off;
+
+static void
+ble_phy_trace_module_send_desc(void)
+{
+    os_trace_module_desc(&g_ble_phy_trace_mod, "0 phy_set_tx cputime=%u usecs=%u");
+    os_trace_module_desc(&g_ble_phy_trace_mod, "1 phy_set_rx cputime=%u usecs=%u");
+    os_trace_module_desc(&g_ble_phy_trace_mod, "2 phy_disable");
+}
+
+void
+ble_phy_trace_init(void)
+{
+    ble_phy_trace_off =
+        os_trace_module_register(&g_ble_phy_trace_mod, "ble_phy", 3,
+                                 ble_phy_trace_module_send_desc);
+}
+#endif

nimble/drivers/nrf5340/syscfg.yml

@@ -0,0 +1,23 @@
+# 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:
+    BLE_PHY_SYSVIEW:
+        description: >
+            Enable SystemView tracing module for radio driver.
+        value: 0

nimble/host/include/host/ble_att.h

@@ -112,7 +112,7 @@ struct os_mbuf;
 
 /**
  * Reads a locally registered attribute.  If the specified attribute handle
- * coresponds to a GATT characteristic value or descriptor, the read is
+ * corresponds to a GATT characteristic value or descriptor, the read is
  * performed by calling the registered GATT access callback.
  *
  * @param attr_handle           The 16-bit handle of the attribute to read.
@@ -131,7 +131,7 @@ int ble_att_svr_read_local(uint16_t attr_handle, struct os_mbuf **out_om);
 /**
  * Writes a locally registered attribute.  This function consumes the supplied
  * mbuf regardless of the outcome.  If the specified attribute handle
- * coresponds to a GATT characteristic value or descriptor, the write is
+ * corresponds to a GATT characteristic value or descriptor, the write is
  * performed by calling the registered GATT access callback.
  *
  * @param attr_handle           The 16-bit handle of the attribute to write.
@@ -167,7 +167,7 @@ uint16_t ble_att_preferred_mtu(void);
 
 /**
  * Sets the preferred ATT MTU; the device will indicate this value in all
- * subseqeunt ATT MTU exchanges.  The ATT MTU of a connection is equal to the
+ * subsequent ATT MTU exchanges.  The ATT MTU of a connection is equal to the
  * lower of the two peers' preferred MTU values.  The ATT MTU is what dictates
  * the maximum size of any message sent during a GATT procedure.
  *
@@ -178,7 +178,7 @@ uint16_t ble_att_preferred_mtu(void);
  * @param mtu                   The preferred ATT MTU.
  *
  * @return                      0 on success;
- *                              BLE_HS_EINVAL if the specifeid value is not
+ *                              BLE_HS_EINVAL if the specified value is not
  *                                  within the allowed range.
  */
 int ble_att_set_preferred_mtu(uint16_t mtu);

nimble/host/include/host/ble_gatt.h

@@ -143,7 +143,7 @@ typedef int ble_gatt_chr_fn(uint16_t conn_handle,
 
 typedef int ble_gatt_dsc_fn(uint16_t conn_handle,
                             const struct ble_gatt_error *error,
-                            uint16_t chr_def_handle,
+                            uint16_t chr_val_handle,
                             const struct ble_gatt_dsc *dsc,
                             void *arg);
 
@@ -790,9 +790,9 @@ int ble_gatts_count_cfg(const struct ble_gatt_svc_def *defs);
  * Send notification (or indication) to any connected devices that have
  * subscribed for notification (or indication) for specified characteristic.
  *
- * @param chr_def_handle        Characteristic definition handle
+ * @param chr_val_handle        Characteristic value handle
  */
-void ble_gatts_chr_updated(uint16_t chr_def_handle);
+void ble_gatts_chr_updated(uint16_t chr_val_handle);
 
 /**
  * Retrieves the attribute handle associated with a local GATT service.
@@ -835,7 +835,7 @@ int ble_gatts_find_chr(const ble_uuid_t *svc_uuid, const ble_uuid_t *chr_uuid,
  * @param chr_uuid              The UUID of the parent characteristic.
  * @param dsc_uuid              The UUID of the descriptor ro look up.
  * @param out_handle            On success, populated with the handle
- *                                  of the descripytor attribute.  Pass null if
+ *                                  of the descriptor attribute.  Pass null if
  *                                  you don't need this value.
  *
  * @return                      0 on success;
@@ -848,7 +848,8 @@ int ble_gatts_find_dsc(const ble_uuid_t *svc_uuid, const ble_uuid_t *chr_uuid,
 
 typedef void (*ble_gatt_svc_foreach_fn)(const struct ble_gatt_svc_def *svc,
                                         uint16_t handle,
-                                        uint16_t end_group_handle);
+                                        uint16_t end_group_handle,
+                                        void *arg);
 
 /**
  * Prints dump of local GATT database. This is useful to log local state of

nimble/host/include/host/ble_hs.h

@@ -36,7 +36,6 @@
 #include "host/ble_hs_id.h"
 #include "host/ble_hs_hci.h"
 #include "host/ble_hs_log.h"
-#include "host/ble_hs_test.h"
 #include "host/ble_hs_mbuf.h"
 #include "host/ble_hs_stop.h"
 #include "host/ble_ibeacon.h"
@@ -95,6 +94,7 @@ extern "C" {
 #define BLE_HS_ESTORE_FAIL          28
 #define BLE_HS_EPREEMPTED           29
 #define BLE_HS_EDISABLED            30
+#define BLE_HS_ESTALLED             31
 
 /** Error base for ATT errors */
 #define BLE_HS_ERR_ATT_BASE         0x100

nimble/host/include/host/ble_hs_adv.h

@@ -35,7 +35,7 @@ extern "C" {
 struct ble_hs_adv_field {
     uint8_t length;
     uint8_t type;
-    uint8_t value[];
+    uint8_t value[0];
 };
 
 typedef int (* ble_hs_adv_parse_func_t) (const struct ble_hs_adv_field *,
@@ -46,22 +46,22 @@ struct ble_hs_adv_fields {
     uint8_t flags;
 
     /*** 0x02,0x03 - 16-bit service class UUIDs. */
-    ble_uuid16_t *uuids16;
+    const ble_uuid16_t *uuids16;
     uint8_t num_uuids16;
     unsigned uuids16_is_complete:1;
 
     /*** 0x04,0x05 - 32-bit service class UUIDs. */
-    ble_uuid32_t *uuids32;
+    const ble_uuid32_t *uuids32;
     uint8_t num_uuids32;
     unsigned uuids32_is_complete:1;
 
     /*** 0x06,0x07 - 128-bit service class UUIDs. */
-    ble_uuid128_t *uuids128;
+    const ble_uuid128_t *uuids128;
     uint8_t num_uuids128;
     unsigned uuids128_is_complete:1;
 
     /*** 0x08,0x09 - Local name. */
-    uint8_t *name;
+    const uint8_t *name;
     uint8_t name_len;
     unsigned name_is_complete:1;
 
@@ -70,14 +70,14 @@ struct ble_hs_adv_fields {
     unsigned tx_pwr_lvl_is_present:1;
 
     /*** 0x0d - Slave connection interval range. */
-    uint8_t *slave_itvl_range;
+    const uint8_t *slave_itvl_range;
 
     /*** 0x16 - Service data - 16-bit UUID. */
-    uint8_t *svc_data_uuid16;
+    const uint8_t *svc_data_uuid16;
     uint8_t svc_data_uuid16_len;
 
     /*** 0x17 - Public target address. */
-    uint8_t *public_tgt_addr;
+    const uint8_t *public_tgt_addr;
     uint8_t num_public_tgt_addrs;
 
     /*** 0x19 - Appearance. */
@@ -89,19 +89,19 @@ struct ble_hs_adv_fields {
     unsigned adv_itvl_is_present:1;
 
     /*** 0x20 - Service data - 32-bit UUID. */
-    uint8_t *svc_data_uuid32;
+    const uint8_t *svc_data_uuid32;
     uint8_t svc_data_uuid32_len;
 
     /*** 0x21 - Service data - 128-bit UUID. */
-    uint8_t *svc_data_uuid128;
+    const uint8_t *svc_data_uuid128;
     uint8_t svc_data_uuid128_len;
 
     /*** 0x24 - URI. */
-    uint8_t *uri;
+    const uint8_t *uri;
     uint8_t uri_len;
 
     /*** 0xff - Manufacturer specific data. */
-    uint8_t *mfg_data;
+    const uint8_t *mfg_data;
     uint8_t mfg_data_len;
 };
 
@@ -164,8 +164,8 @@ int ble_hs_adv_set_fields_mbuf(const struct ble_hs_adv_fields *adv_fields,
 int ble_hs_adv_set_fields(const struct ble_hs_adv_fields *adv_fields,
                           uint8_t *dst, uint8_t *dst_len, uint8_t max_len);
 
-int ble_hs_adv_parse_fields(struct ble_hs_adv_fields *adv_fields, uint8_t *src,
-                            uint8_t src_len);
+int ble_hs_adv_parse_fields(struct ble_hs_adv_fields *adv_fields,
+                            const uint8_t *src, uint8_t src_len);
 
 int ble_hs_adv_parse(const uint8_t *data, uint8_t length,
                      ble_hs_adv_parse_func_t func, void *user_data);

nimble/host/include/host/ble_hs_log.h

@@ -21,6 +21,12 @@
 #define H_BLE_HS_LOG_
 
 #include "modlog/modlog.h"
+#include "log/log.h"
+
+/* Only include the logcfg header if this version of newt can generate it. */
+#if MYNEWT_VAL(NEWT_FEATURE_LOGCFG)
+#include "logcfg/logcfg.h"
+#endif
 
 #ifdef __cplusplus
 extern "C" {
@@ -29,12 +35,13 @@ extern "C" {
 struct os_mbuf;
 
 #define BLE_HS_LOG(lvl, ...) \
-    MODLOG_ ## lvl(LOG_MODULE_NIMBLE_HOST, __VA_ARGS__)
+    BLE_HS_LOG_ ## lvl(__VA_ARGS__)
 
 #define BLE_HS_LOG_ADDR(lvl, addr)                      \
-    BLE_HS_LOG(lvl, "%02x:%02x:%02x:%02x:%02x:%02x",    \
-               (addr)[5], (addr)[4], (addr)[3],         \
-               (addr)[2], (addr)[1], (addr)[0])
+    BLE_HS_LOG_ ## lvl("%02x:%02x:%02x:%02x:%02x:%02x", \
+                       (addr)[5], (addr)[4], (addr)[3], \
+                       (addr)[2], (addr)[1], (addr)[0])
+
 
 void ble_hs_log_mbuf(const struct os_mbuf *om);
 void ble_hs_log_flat_buf(const void *data, int len);

nimble/host/include/host/ble_hs_mbuf.h

@@ -41,7 +41,7 @@ struct os_mbuf;
  *  - L2CAP B-frame header
  *  - Largest ATT command base (prepare write request / response).
  *
- * @return An empty mbuf on success, NULLl on error.
+ * @return An empty mbuf on success, NULL on error.
  */
 struct os_mbuf *ble_hs_mbuf_att_pkt(void);
 

nimble/host/include/host/ble_hs_test.h

@@ -1,61 +0,0 @@
-/*
- * 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_HOST_TEST_
-#define H_HOST_TEST_
-
-#include <inttypes.h>
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-struct os_mbuf;
-
-int ble_att_clt_test_all(void);
-int ble_att_svr_test_all(void);
-int ble_gap_test_all(void);
-int ble_gatt_conn_test_all(void);
-int ble_gatt_disc_c_test_all(void);
-int ble_gatt_disc_d_test_all(void);
-int ble_gatt_disc_s_test_all(void);
-int ble_gatt_find_s_test_all(void);
-int ble_gatt_read_test_all(void);
-int ble_gatt_write_test_all(void);
-int ble_gatts_notify_test_all(void);
-int ble_gatts_read_test_suite(void);
-int ble_gatts_reg_test_all(void);
-int ble_hs_adv_test_all(void);
-int ble_hs_conn_test_all(void);
-int ble_hs_hci_test_all(void);
-int ble_hs_id_test_all(void);
-int ble_l2cap_test_all(void);
-int ble_os_test_all(void);
-int ble_hs_pvcy_test_all(void);
-int ble_sm_lgcy_test_suite(void);
-int ble_sm_sc_test_suite(void);
-int ble_sm_test_all(void);
-int ble_stop_test_all(void);
-int ble_store_test_all(void);
-int ble_uuid_test_all(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif

nimble/host/include/host/ble_l2cap.h

@@ -51,10 +51,14 @@ struct ble_hs_conn;
 #define BLE_L2CAP_SIG_OP_MOVE_CHAN_CONF_RSP     0x11
 #define BLE_L2CAP_SIG_OP_UPDATE_REQ             0x12
 #define BLE_L2CAP_SIG_OP_UPDATE_RSP             0x13
-#define BLE_L2CAP_SIG_OP_CREDIT_CONNECT_REQ     0x14
-#define BLE_L2CAP_SIG_OP_CREDIT_CONNECT_RSP     0x15
+#define BLE_L2CAP_SIG_OP_LE_CREDIT_CONNECT_REQ  0x14
+#define BLE_L2CAP_SIG_OP_LE_CREDIT_CONNECT_RSP  0x15
 #define BLE_L2CAP_SIG_OP_FLOW_CTRL_CREDIT       0x16
-#define BLE_L2CAP_SIG_OP_MAX                    0x17
+#define BLE_L2CAP_SIG_OP_CREDIT_CONNECT_REQ     0x17
+#define BLE_L2CAP_SIG_OP_CREDIT_CONNECT_RSP     0x18
+#define BLE_L2CAP_SIG_OP_CREDIT_RECONFIG_REQ    0x19
+#define BLE_L2CAP_SIG_OP_CREDIT_RECONFIG_RSP    0x1A
+#define BLE_L2CAP_SIG_OP_MAX                    0x1B
 
 #define BLE_L2CAP_SIG_ERR_CMD_NOT_UNDERSTOOD    0x0000
 #define BLE_L2CAP_SIG_ERR_MTU_EXCEEDED          0x0001
@@ -70,11 +74,21 @@ struct ble_hs_conn;
 #define BLE_L2CAP_COC_ERR_INVALID_SOURCE_CID        0x0009
 #define BLE_L2CAP_COC_ERR_SOURCE_CID_ALREADY_USED   0x000A
 #define BLE_L2CAP_COC_ERR_UNACCEPTABLE_PARAMETERS   0x000B
+#define BLE_L2CAP_COC_ERR_INVALID_PARAMETERS        0x000C
+
+#define BLE_L2CAP_ERR_RECONFIG_SUCCEED                       0x0000
+#define BLE_L2CAP_ERR_RECONFIG_REDUCTION_MTU_NOT_ALLOWED     0x0001
+#define BLE_L2CAP_ERR_RECONFIG_REDUCTION_MPS_NOT_ALLOWED     0x0002
+#define BLE_L2CAP_ERR_RECONFIG_INVALID_DCID                  0x0003
+#define BLE_L2CAP_ERR_RECONFIG_UNACCAPTED_PARAM              0x0004
 
 #define BLE_L2CAP_EVENT_COC_CONNECTED                 0
 #define BLE_L2CAP_EVENT_COC_DISCONNECTED              1
 #define BLE_L2CAP_EVENT_COC_ACCEPT                    2
 #define BLE_L2CAP_EVENT_COC_DATA_RECEIVED             3
+#define BLE_L2CAP_EVENT_COC_TX_UNSTALLED              4
+#define BLE_L2CAP_EVENT_COC_RECONFIG_COMPLETED        5
+#define BLE_L2CAP_EVENT_COC_PEER_RECONFIGURED         6
 
 typedef void ble_l2cap_sig_update_fn(uint16_t conn_handle, int status,
                                      void *arg);
@@ -173,11 +187,68 @@ struct ble_l2cap_event {
             /** The mbuf with received SDU. */
             struct os_mbuf *sdu_rx;
         } receive;
+
+        /**
+         * Represents tx_unstalled data. Valid for the following event
+         * types:
+         *     o BLE_L2CAP_EVENT_COC_TX_UNSTALLED
+         */
+        struct {
+            /** Connection handle of the relevant connection */
+            uint16_t conn_handle;
+
+            /** The L2CAP channel of the relevant L2CAP connection. */
+            struct ble_l2cap_chan *chan;
+
+            /**
+             * The status of the send attempt which was stalled due to
+             * lack of credits; This can be non zero only if there
+             * is an issue with memory allocation for following SDU fragments.
+             * In such a case last SDU has been partially sent to peer device
+             * and it is up to application to decide how to handle it.
+             */
+            int status;
+        } tx_unstalled;
+
+        /**
+         * Represents reconfiguration done. Valid for the following event
+         * types:
+         *      o BLE_L2CAP_EVENT_COC_RECONFIG_COMPLETED
+         *      o BLE_L2CAP_EVENT_COC_PEER_RECONFIGURED
+         */
+        struct {
+            /**
+             * The status of the reconfiguration attempt;
+             *     o 0: the reconfiguration was successfully done.
+             *     o BLE host error code: the reconfiguration attempt failed for
+             *       the specified reason.
+             */
+            int status;
+
+            /** Connection handle of the relevant connection */
+            uint16_t conn_handle;
+
+            /** The L2CAP channel of the relevant L2CAP connection. */
+            struct ble_l2cap_chan *chan;
+        } reconfigured;
     };
 };
 
+struct ble_l2cap_chan_info {
+    uint16_t scid;
+    uint16_t dcid;
+    uint16_t our_l2cap_mtu;
+    uint16_t peer_l2cap_mtu;
+    uint16_t psm;
+    uint16_t our_coc_mtu;
+    uint16_t peer_coc_mtu;
+};
+
 typedef int ble_l2cap_event_fn(struct ble_l2cap_event *event, void *arg);
 
+typedef void ble_l2cap_ping_fn(uint16_t conn_handle, uint32_t rtt_ms,
+                               struct os_mbuf *om);
+
 uint16_t ble_l2cap_get_conn_handle(struct ble_l2cap_chan *chan);
 int ble_l2cap_create_server(uint16_t psm, uint16_t mtu,
                             ble_l2cap_event_fn *cb, void *cb_arg);
@@ -187,11 +258,28 @@ int ble_l2cap_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu,
                       ble_l2cap_event_fn *cb, void *cb_arg);
 int ble_l2cap_disconnect(struct ble_l2cap_chan *chan);
 int ble_l2cap_send(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_tx);
-void ble_l2cap_recv_ready(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_rx);
-int ble_l2cap_get_scid(struct ble_l2cap_chan *chan);
-int ble_l2cap_get_dcid(struct ble_l2cap_chan *chan);
-int ble_l2cap_get_our_mtu(struct ble_l2cap_chan *chan);
-int ble_l2cap_get_peer_mtu(struct ble_l2cap_chan *chan);
+int ble_l2cap_recv_ready(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_rx);
+int ble_l2cap_get_chan_info(struct ble_l2cap_chan *chan, struct ble_l2cap_chan_info *chan_info);
+
+/**
+ * Send an ECHO_REQ packet over the L2CAP signalling channel for the given
+ * connection
+ *
+ * @param conn_handle   Connection handle
+ * @param cb            Function called once the corresponding ECHO_RSP is
+ *                      received. May be NULL.
+ * @param data          User payload appended to the ECHO_REQ packet, may be
+ *                      NULL
+ * @param data_len      Length of @p data in bytes. Set to 0 to omit any user
+ *                      payload
+ *
+ * @return              0 on success
+ *                      BLE_HS_EBADDATA if given payload is invalid
+ *                      BLE_HS_ENOMEM if request packet cannot be allocated
+ *                      BLE_HS_ENOTCONN if not connected
+ */
+int ble_l2cap_ping(uint16_t conn_handle, ble_l2cap_ping_fn cb,
+                   const void *data, uint16_t data_len);
 
 #ifdef __cplusplus
 }

nimble/host/include/host/ble_sm.h

@@ -84,7 +84,16 @@ extern "C" {
 #define BLE_SM_IOACT_INPUT                      2
 #define BLE_SM_IOACT_DISP                       3
 #define BLE_SM_IOACT_NUMCMP                     4
-#define BLE_SM_IOACT_MAX_PLUS_ONE               5
+#define BLE_SM_IOACT_OOB_SC                     5
+#define BLE_SM_IOACT_MAX_PLUS_ONE               6
+
+struct ble_sm_sc_oob_data {
+    /** Random Number. */
+    uint8_t r[16];
+
+    /** Confirm Value. */
+    uint8_t c[16];
+};
 
 struct ble_sm_io {
     uint8_t action;
@@ -92,9 +101,15 @@ struct ble_sm_io {
         uint32_t passkey;
         uint8_t  oob[16];
         uint8_t  numcmp_accept;
+        struct {
+            struct ble_sm_sc_oob_data *local;
+            struct ble_sm_sc_oob_data *remote;
+        } oob_sc_data;
     };
 };
 
+int ble_sm_sc_oob_generate_data(struct ble_sm_sc_oob_data *oob_data);
+
 #if NIMBLE_BLE_SM
 int ble_sm_inject_io(uint16_t conn_handle, struct ble_sm_io *pkey);
 #else

nimble/host/include/host/ble_store.h

@@ -164,7 +164,7 @@ struct ble_store_status_event {
         } overflow;
 
         /**
-         * Represents the possiblity that a scheduled write will fail due to
+         * Represents the possibility that a scheduled write will fail due to
          * storage exhaustion.  Valid for the following event types:
          *     o BLE_STORE_EVENT_FULL
          */
@@ -222,7 +222,7 @@ typedef int ble_store_write_fn(int obj_type, const union ble_store_value *val);
  * @param key                   Specifies properties of the object to search
  *                                  for.  An object is deleted if it matches
  *                                  these criteria.
- * @return                      0 if an object was successfully retreived;
+ * @return                      0 if an object was successfully retrieved;
  *                              BLE_HS_ENOENT if no matching object was found;
  *                              Other nonzero on error.
  */

nimble/host/mesh/include/mesh/access.h

@@ -17,6 +17,10 @@
  * @{
  */
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #define BT_MESH_ADDR_UNASSIGNED   0x0000
 #define BT_MESH_ADDR_ALL_NODES    0xffff
 #define BT_MESH_ADDR_PROXIES      0xfffc
@@ -24,8 +28,23 @@
 #define BT_MESH_ADDR_RELAYS       0xfffe
 
 #define BT_MESH_KEY_UNUSED        0xffff
+#define BT_MESH_KEY_ANY           0xffff
 #define BT_MESH_KEY_DEV           0xfffe
+#define BT_MESH_KEY_DEV_LOCAL     BT_MESH_KEY_DEV
+#define BT_MESH_KEY_DEV_REMOTE    0xfffd
+#define BT_MESH_KEY_DEV_ANY       0xfffc
+
+#define BT_MESH_ADDR_IS_UNICAST(addr) ((addr) && (addr) < 0x8000)
+#define BT_MESH_ADDR_IS_GROUP(addr) ((addr) >= 0xc000 && (addr) <= 0xff00)
+#define BT_MESH_ADDR_IS_VIRTUAL(addr) ((addr) >= 0x8000 && (addr) < 0xc000)
+#define BT_MESH_ADDR_IS_RFU(addr) ((addr) >= 0xff00 && (addr) <= 0xfffb)
+
+#define BT_MESH_IS_DEV_KEY(key) (key == BT_MESH_KEY_DEV_LOCAL || \
+				 key == BT_MESH_KEY_DEV_REMOTE)
 
+#define BT_MESH_APP_SEG_SDU_MAX        12
+#define BT_MESH_TX_SDU_MAX (CONFIG_BT_MESH_TX_SEG_MAX * BT_MESH_APP_SEG_SDU_MAX)
+#define BT_MESH_RX_SDU_MAX (CONFIG_BT_MESH_RX_SEG_MAX * BT_MESH_APP_SEG_SDU_MAX)
 /** Helper to define a mesh element within an array.
  *
  *  In case the element has no SIG or Vendor models the helper
@@ -47,13 +66,13 @@
 /** Abstraction that describes a Mesh Element */
 struct bt_mesh_elem {
 	/* Unicast Address. Set at runtime during provisioning. */
-	u16_t addr;
+	uint16_t addr;
 
 	/* Location Descriptor (GATT Bluetooth Namespace Descriptors) */
-	const u16_t loc;
+	const uint16_t loc;
 
-	const u8_t model_count;
-	const u8_t vnd_model_count;
+	const uint8_t model_count;
+	const uint8_t vnd_model_count;
 
 	struct bt_mesh_model * const models;
 	struct bt_mesh_model * const vnd_models;
@@ -122,30 +141,33 @@ struct bt_mesh_elem {
 /** Message sending context. */
 struct bt_mesh_msg_ctx {
 	/** NetKey Index of the subnet to send the message on. */
-	u16_t net_idx;
+	uint16_t net_idx;
 
 	/** AppKey Index to encrypt the message with. */
-	u16_t app_idx;
+	uint16_t app_idx;
 
 	/** Remote address. */
-	u16_t addr;
+	uint16_t addr;
 
 	/** Destination address of a received message. Not used for sending. */
-	u16_t recv_dst;
+	uint16_t recv_dst;
+
+	/** RSSI of received packet. Not used for sending. */
+	int8_t  recv_rssi;
 
 	/** Received TTL value. Not used for sending. */
-	u8_t  recv_ttl:7;
+	uint8_t  recv_ttl;
 
 	/** Force sending reliably by using segment acknowledgement */
-	u8_t  send_rel:1;
+	bool  send_rel;
 
 	/** TTL, or BT_MESH_TTL_DEFAULT for default TTL. */
-	u8_t  send_ttl;
+	uint8_t  send_ttl;
 };
 
 struct bt_mesh_model_op {
 	/* OpCode encoded using the BT_MESH_MODEL_OP_* macros */
-	const u32_t  opcode;
+	const uint32_t  opcode;
 
 	/* Minimum required message length */
 	const size_t min_len;
@@ -167,7 +189,66 @@ struct bt_mesh_model_op {
 /** Helper to define an empty model array */
 #define BT_MESH_MODEL_NONE ((struct bt_mesh_model []){})
 
-#define BT_MESH_MODEL(_id, _op, _pub, _user_data)                            \
+/** Length of a short Mesh MIC. */
+#define BT_MESH_MIC_SHORT 4
+/** Length of a long Mesh MIC. */
+#define BT_MESH_MIC_LONG 8
+
+/** @def BT_MESH_MODEL_OP_LEN
+ *
+ * @brief Helper to determine the length of an opcode.
+ *
+ * @param _op Opcode.
+ */
+#define BT_MESH_MODEL_OP_LEN(_op) ((_op) <= 0xff ? 1 : (_op) <= 0xffff ? 2 : 3)
+
+/** @def BT_MESH_MODEL_BUF_LEN
+ *
+ * @brief Helper for model message buffer length.
+ *
+ * Returns the length of a Mesh model message buffer, including the opcode
+ * length and a short MIC.
+ *
+ * @param _op Opcode of the message.
+ * @param _payload_len Length of the model payload.
+ */
+#define BT_MESH_MODEL_BUF_LEN(_op, _payload_len)                               \
+	(BT_MESH_MODEL_OP_LEN(_op) + (_payload_len) + BT_MESH_MIC_SHORT)
+
+/** @def BT_MESH_MODEL_BUF_LEN_LONG_MIC
+ *
+ * @brief Helper for model message buffer length.
+ *
+ * Returns the length of a Mesh model message buffer, including the opcode
+ * length and a long MIC.
+ *
+ * @param _op Opcode of the message.
+ * @param _payload_len Length of the model payload.
+ */
+#define BT_MESH_MODEL_BUF_LEN_LONG_MIC(_op, _payload_len)                      \
+	(BT_MESH_MODEL_OP_LEN(_op) + (_payload_len) + BT_MESH_MIC_LONG)
+
+/** @def BT_MESH_MODEL_BUF_DEFINE
+ *
+ * @brief Define a Mesh model message buffer using @ref NET_BUF_SIMPLE.
+ *
+ * @param _op Opcode of the message.
+ * @param _payload_len Length of the model message payload.
+ */
+#define BT_MESH_MODEL_BUF(_op, _payload_len)                      \
+	NET_BUF_SIMPLE(BT_MESH_MODEL_BUF_LEN(_op, (_payload_len)))
+
+/** @def BT_MESH_MODEL_CB
+ *
+ * @brief Composition data SIG model entry with callback functions.
+ *
+ * @param _id Model ID.
+ * @param _op Array of model opcode handlers.
+ * @param _pub Model publish parameters.
+ * @param _user_data User data for the model.
+ * @param _cb Callback structure, or NULL to keep no callbacks.
+ */
+#define BT_MESH_MODEL_CB(_id, _op, _pub, _user_data, _cb)                    \
 {                                                                            \
 	.id = (_id),                                                         \
 	.op = _op,                                                           \
@@ -177,9 +258,21 @@ struct bt_mesh_model_op {
 	.groups = { [0 ... (CONFIG_BT_MESH_MODEL_GROUP_COUNT - 1)] =         \
 			BT_MESH_ADDR_UNASSIGNED },                           \
 	.user_data = _user_data,                                             \
+	.cb = _cb,                                                           \
 }
 
-#define BT_MESH_MODEL_VND(_company, _id, _op, _pub, _user_data)              \
+/** @def BT_MESH_MODEL_VND_CB
+ *
+ * @brief Composition data vendor model entry with callback functions.
+ *
+ * @param _company Company ID.
+ * @param _id Model ID.
+ * @param _op Array of model opcode handlers.
+ * @param _pub Model publish parameters.
+ * @param _user_data User data for the model.
+ * @param _cb Callback structure, or NULL to keep no callbacks.
+ */
+#define BT_MESH_MODEL_VND_CB(_company, _id, _op, _pub, _user_data, _cb)      \
 {                                                                            \
 	.vnd.company = (_company),                                           \
 	.vnd.id = (_id),                                                     \
@@ -190,8 +283,35 @@ struct bt_mesh_model_op {
 	.groups = { [0 ... (CONFIG_BT_MESH_MODEL_GROUP_COUNT - 1)] =         \
 			BT_MESH_ADDR_UNASSIGNED },                           \
 	.user_data = _user_data,                                             \
+	.cb = _cb,                                                           \
 }
 
+
+/** @def BT_MESH_MODEL
+ *
+ * @brief Composition data SIG model entry.
+ *
+ * @param _id Model ID.
+ * @param _op Array of model opcode handlers.
+ * @param _pub Model publish parameters.
+ * @param _user_data User data for the model.
+ */
+#define BT_MESH_MODEL(_id, _op, _pub, _user_data)                              \
+	BT_MESH_MODEL_CB(_id, _op, _pub, _user_data, NULL)
+
+/** @def BT_MESH_MODEL_VND
+ *
+ * @brief Composition data vendor model entry.
+ *
+ * @param _company Company ID.
+ * @param _id Model ID.
+ * @param _op Array of model opcode handlers.
+ * @param _pub Model publish parameters.
+ * @param _user_data User data for the model.
+ */
+#define BT_MESH_MODEL_VND(_company, _id, _op, _pub, _user_data)                \
+	BT_MESH_MODEL_VND_CB(_company, _id, _op, _pub, _user_data, NULL)
+
 /** @def BT_MESH_TRANSMIT
  *
  *  @brief Encode transmission count & interval steps.
@@ -213,7 +333,7 @@ struct bt_mesh_model_op {
  *
  *  @return Transmission count (actual transmissions is N + 1).
  */
-#define BT_MESH_TRANSMIT_COUNT(transmit) (((transmit) & (u8_t)BIT_MASK(3)))
+#define BT_MESH_TRANSMIT_COUNT(transmit) (((transmit) & (uint8_t)BIT_MASK(3)))
 
 /** @def BT_MESH_TRANSMIT_INT
  *
@@ -264,22 +384,24 @@ struct bt_mesh_model_pub {
 	/** The model the context belongs to. Initialized by the stack. */
 	struct bt_mesh_model *mod;
 
-	u16_t addr;         /**< Publish Address. */
-	u16_t key;          /**< Publish AppKey Index. */
+	uint16_t addr;         /**< Publish Address. */
+	uint16_t key;          /**< Publish AppKey Index. */
 
-	u8_t  ttl;          /**< Publish Time to Live. */
-	u8_t  retransmit;   /**< Retransmit Count & Interval Steps. */
-	u8_t  period;       /**< Publish Period. */
-	u8_t  period_div:4, /**< Divisor for the Period. */
+	uint8_t  ttl;          /**< Publish Time to Live. */
+	uint8_t  retransmit;   /**< Retransmit Count & Interval Steps. */
+	uint8_t  period;       /**< Publish Period. */
+	uint8_t  period_div:4, /**< Divisor for the Period. */
 	      cred:1,       /**< Friendship Credentials Flag. */
+		  send_rel:1,
+	      fast_period:1,/**< Use FastPeriodDivisor */
 	      count:3;      /**< Retransmissions left. */
 
-	u32_t period_start; /**< Start of the current period. */
+	uint32_t period_start; /**< Start of the current period. */
 
 	/** @brief Publication buffer, containing the publication message.
 	 *
 	 *  The application is expected to initialize this with
-	 *  a valid net_buf_simple pointer, with the help of e.g.
+	 *  a valid os_mbuf pointer, with the help of e.g.
 	 *  the NET_BUF_SIMPLE() macro. The publication buffer must
 	 *  contain a valid publication message before calling the
 	 *  bt_mesh_model_publish() API or after the publication's
@@ -302,6 +424,10 @@ struct bt_mesh_model_pub {
 	 *  will be called periodically and is expected to update
 	 *  @ref bt_mesh_model_pub.msg with a valid publication
 	 *  message.
+	 * 
+	 *  If the callback returns non-zero, the publication is skipped
+	 *  and will resume on the next periodic publishing interval.
+	 *
 	 *
 	 *  @param mod The Model the Publication Context belogs to.
 	 *
@@ -313,42 +439,107 @@ struct bt_mesh_model_pub {
 	struct k_delayed_work timer;
 };
 
+/** Model callback functions. */
+struct bt_mesh_model_cb {
+	/** @brief Set value handler of user data tied to the model.
+	 *
+	 * @sa settings_handler::h_set
+	 *
+	 * @param model Model to set the persistent data of.
+	 * @param name    Name/key of the settings item.
+	 * @param val Data from the backend.
+	 *
+	 * @return 0 on success, error otherwise.
+	 */
+	int (*const settings_set)(struct bt_mesh_model *model,
+							  const char *name, char *val);
+
+	/** @brief Callback called when the mesh is started.
+	 *
+	 *  This handler gets called after the node has been provisioned, or
+	 *  after all mesh data has been loaded from persistent storage.
+	 *
+	 * @sa settings_handler::h_commit
+	 *
+	 * @param model Model this callback belongs to.
+	 *
+	 * @return 0 on success, error otherwise.
+	 */
+	int (*const start)(struct bt_mesh_model *model);
+
+	/** @brief Model init callback.
+	 *
+	 * Called on every model instance during mesh initialization.
+	 *
+	 *
+	 * If any of the model init callbacks return an error, the Mesh
+	 * subsystem initialization will be aborted, and the error will be
+	 * returned to the caller of @ref bt_mesh_init.
+	 *
+	 * @param model Model to be initialized.
+	 *
+	 * @return 0 on success, error otherwise.
+	 */
+	int (*const init)(struct bt_mesh_model *model);
+
+	/** @brief Model reset callback.
+	 *
+	 * Called when the mesh node is reset. All model data is deleted on
+	 * reset, and the model should clear its state.
+	 *
+	 *  @note If the model stores any persistent data, this needs to be
+	 *  erased manually.
+	 * 
+	 * @param model Model this callback belongs to.
+	 */
+	void (*const reset)(struct bt_mesh_model *model);
+};
+
 /** Abstraction that describes a Mesh Model instance */
 struct bt_mesh_model {
 	union {
-		const u16_t id;
+		const uint16_t id;
 		struct {
-			u16_t company;
-			u16_t id;
+			uint16_t company;
+			uint16_t id;
 		} vnd;
 	};
 
 	/* Internal information, mainly for persistent storage */
-	u8_t  elem_idx;   /* Belongs to Nth element */
-	u8_t  mod_idx;    /* Is the Nth model in the element */
-	u16_t flags;      /* Information about what has changed */
+	uint8_t  elem_idx;   /* Belongs to Nth element */
+	uint8_t  mod_idx;    /* Is the Nth model in the element */
+	uint16_t flags;      /* Model flags for internal bookkeeping */
 
 	/* Model Publication */
 	struct bt_mesh_model_pub * const pub;
 
 	/* AppKey List */
-	u16_t keys[CONFIG_BT_MESH_MODEL_KEY_COUNT];
+	uint16_t keys[CONFIG_BT_MESH_MODEL_KEY_COUNT];
 
 	/* Subscription List (group or virtual addresses) */
-	u16_t groups[CONFIG_BT_MESH_MODEL_GROUP_COUNT];
+	uint16_t groups[CONFIG_BT_MESH_MODEL_GROUP_COUNT];
 
 	const struct bt_mesh_model_op * const op;
 
+	/* Model callback structure. */
+	const struct bt_mesh_model_cb * const cb;
+
+#if MYNEWT_VAL(BLE_MESH_MODEL_EXTENSIONS)
+	/* Pointer to the next model in a model extension tree. */
+	struct bt_mesh_model *next;
+	/* Pointer to the first model this model extends. */
+	struct bt_mesh_model *extends;
+#endif
 	/* Model-specific user data */
 	void *user_data;
 };
 
 struct bt_mesh_send_cb {
-	void (*start)(u16_t duration, int err, void *cb_data);
+	void (*start)(uint16_t duration, int err, void *cb_data);
 	void (*end)(int err, void *cb_data);
 };
 
-void bt_mesh_model_msg_init(struct os_mbuf *msg, u32_t opcode);
+void bt_mesh_model_msg_init(struct os_mbuf *msg, uint32_t opcode);
 
 /** Special TTL value to request using configured default TTL */
 #define BT_MESH_TTL_DEFAULT 0xff
@@ -398,16 +589,91 @@ int bt_mesh_model_publish(struct bt_mesh_model *model);
  */
 struct bt_mesh_elem *bt_mesh_model_elem(struct bt_mesh_model *mod);
 
+/** @brief Find a SIG model.
+ *
+ * @param elem Element to search for the model in.
+ * @param id Model ID of the model.
+ *
+ * @return A pointer to the Mesh model matching the given parameters, or NULL
+ * if no SIG model with the given ID exists in the given element.
+ */
+struct bt_mesh_model *bt_mesh_model_find(const struct bt_mesh_elem *elem,
+					 uint16_t id);
+
+/** @brief Find a vendor model.
+ *
+ * @param elem Element to search for the model in.
+ * @param company Company ID of the model.
+ * @param id Model ID of the model.
+ *
+ * @return A pointer to the Mesh model matching the given parameters, or NULL
+ * if no vendor model with the given ID exists in the given element.
+ */
+struct bt_mesh_model *bt_mesh_model_find_vnd(const struct bt_mesh_elem *elem,
+					     uint16_t company, uint16_t id);
+
+/** @brief Get whether the model is in the primary element of the device.
+ *
+ * @param mod Mesh model.
+ *
+ * @return true if the model is on the primary element, false otherwise.
+ */
+static inline bool bt_mesh_model_in_primary(const struct bt_mesh_model *mod)
+{
+	return (mod->elem_idx == 0);
+}
+
+/** @brief Immediately store the model's user data in persistent storage.
+ *
+ * @param mod Mesh model.
+ * @param vnd This is a vendor model.
+ * @param name     Name/key of the settings item.
+ * @param data Model data to store, or NULL to delete any model data.
+ * @param data_len Length of the model data.
+ *
+ * @return 0 on success, or (negative) error code on failure.
+ */
+int bt_mesh_model_data_store(struct bt_mesh_model *mod, bool vnd,
+				 const char *name, const void *data, size_t data_len);
+
+/** @brief Let a model extend another.
+ *
+ * Mesh models may be extended to reuse their functionality, forming a more
+ * complex model. A Mesh model may extend any number of models, in any element.
+ * The extensions may also be nested, ie a model that extends another may itself
+ * be extended. Extensions may not be cyclical, and a model can only be extended
+ * by one other model.
+ *
+ * A set of models that extend each other form a model extension tree.
+ *
+ * All models in an extension tree share one subscription list per element. The
+ * access layer will utilize the combined subscription list of all models in an
+ * extension tree and element, giving the models extended subscription list
+ * capacity.
+ *
+ * @param[in] mod Mesh model.
+ * @param[in] base_mod The model being extended.
+ *
+ * @retval 0 Successfully extended the base_mod model.
+ * @retval -EALREADY The base_mod model is already extended.
+ */
+int bt_mesh_model_extend(struct bt_mesh_model *mod,
+			 struct bt_mesh_model *base_mod);
+
 /** Node Composition */
 struct bt_mesh_comp {
-	u16_t cid;
-	u16_t pid;
-	u16_t vid;
+	uint16_t cid;
+	uint16_t pid;
+	uint16_t vid;
 
 	size_t elem_count;
 	struct bt_mesh_elem *elem;
 };
 
+#ifdef __cplusplus
+}
+#endif
+
 /**
  * @}
  */

nimble/host/mesh/include/mesh/atomic.h

@@ -0,0 +1,409 @@
+/* atomic operations */
+
+/*
+ * Copyright (c) 1997-2015, Wind River Systems, Inc.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef __ATOMIC_H__
+#define __ATOMIC_H__
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+typedef int atomic_t;
+typedef atomic_t atomic_val_t;
+
+/**
+ * @defgroup atomic_apis Atomic Services APIs
+ * @ingroup kernel_apis
+ * @{
+ */
+
+/**
+ * @brief Atomic compare-and-set.
+ *
+ * This routine performs an atomic compare-and-set on @a target. If the current
+ * value of @a target equals @a old_value, @a target is set to @a new_value.
+ * If the current value of @a target does not equal @a old_value, @a target
+ * is left unchanged.
+ *
+ * @param target Address of atomic variable.
+ * @param old_value Original value to compare against.
+ * @param new_value New value to store.
+ * @return 1 if @a new_value is written, 0 otherwise.
+ */
+static inline int atomic_cas(atomic_t *target, atomic_val_t old_value,
+        atomic_val_t new_value)
+{
+    return __atomic_compare_exchange_n(target, &old_value, new_value,
+            0, __ATOMIC_SEQ_CST,
+            __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic addition.
+ *
+ * This routine performs an atomic addition on @a target.
+ *
+ * @param target Address of atomic variable.
+ * @param value Value to add.
+ *
+ * @return Previous value of @a target.
+ */
+static inline atomic_val_t atomic_add(atomic_t *target, atomic_val_t value)
+{
+    return __atomic_fetch_add(target, value, __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic subtraction.
+ *
+ * This routine performs an atomic subtraction on @a target.
+ *
+ * @param target Address of atomic variable.
+ * @param value Value to subtract.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_sub(atomic_t *target, atomic_val_t value)
+{
+    return __atomic_fetch_sub(target, value, __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic increment.
+ *
+ * This routine performs an atomic increment by 1 on @a target.
+ *
+ * @param target Address of atomic variable.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_inc(atomic_t *target)
+{
+    return atomic_add(target, 1);
+}
+
+/**
+ *
+ * @brief Atomic decrement.
+ *
+ * This routine performs an atomic decrement by 1 on @a target.
+ *
+ * @param target Address of atomic variable.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_dec(atomic_t *target)
+{
+    return atomic_sub(target, 1);
+}
+
+/**
+ *
+ * @brief Atomic get.
+ *
+ * This routine performs an atomic read on @a target.
+ *
+ * @param target Address of atomic variable.
+ *
+ * @return Value of @a target.
+ */
+
+static inline atomic_val_t atomic_get(const atomic_t *target)
+{
+    return __atomic_load_n(target, __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic get-and-set.
+ *
+ * This routine atomically sets @a target to @a value and returns
+ * the previous value of @a target.
+ *
+ * @param target Address of atomic variable.
+ * @param value Value to write to @a target.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_set(atomic_t *target, atomic_val_t value)
+{
+    /* This builtin, as described by Intel, is not a traditional
+     * test-and-set operation, but rather an atomic exchange operation. It
+     * writes value into *ptr, and returns the previous contents of *ptr.
+     */
+    return __atomic_exchange_n(target, value, __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic clear.
+ *
+ * This routine atomically sets @a target to zero and returns its previous
+ * value. (Hence, it is equivalent to atomic_set(target, 0).)
+ *
+ * @param target Address of atomic variable.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_clear(atomic_t *target)
+{
+    return atomic_set(target, 0);
+}
+
+/**
+ *
+ * @brief Atomic bitwise inclusive OR.
+ *
+ * This routine atomically sets @a target to the bitwise inclusive OR of
+ * @a target and @a value.
+ *
+ * @param target Address of atomic variable.
+ * @param value Value to OR.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_or(atomic_t *target, atomic_val_t value)
+{
+    return __atomic_fetch_or(target, value, __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic bitwise exclusive OR (XOR).
+ *
+ * This routine atomically sets @a target to the bitwise exclusive OR (XOR) of
+ * @a target and @a value.
+ *
+ * @param target Address of atomic variable.
+ * @param value Value to XOR
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_xor(atomic_t *target, atomic_val_t value)
+{
+    return __atomic_fetch_xor(target, value, __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic bitwise AND.
+ *
+ * This routine atomically sets @a target to the bitwise AND of @a target
+ * and @a value.
+ *
+ * @param target Address of atomic variable.
+ * @param value Value to AND.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_and(atomic_t *target, atomic_val_t value)
+{
+    return __atomic_fetch_and(target, value, __ATOMIC_SEQ_CST);
+}
+
+/**
+ *
+ * @brief Atomic bitwise NAND.
+ *
+ * This routine atomically sets @a target to the bitwise NAND of @a target
+ * and @a value. (This operation is equivalent to target = ~(target & value).)
+ *
+ * @param target Address of atomic variable.
+ * @param value Value to NAND.
+ *
+ * @return Previous value of @a target.
+ */
+
+static inline atomic_val_t atomic_nand(atomic_t *target, atomic_val_t value)
+{
+    return __atomic_fetch_nand(target, value, __ATOMIC_SEQ_CST);
+}
+
+    /**
+     * @brief Initialize an atomic variable.
+     *
+     * This macro can be used to initialize an atomic variable. For example,
+     * @code atomic_t my_var = ATOMIC_INIT(75); @endcode
+     *
+     * @param i Value to assign to atomic variable.
+     */
+#define ATOMIC_INIT(i) (i)
+
+    /**
+     * @cond INTERNAL_HIDDEN
+     */
+
+#define ATOMIC_BITS (sizeof(atomic_val_t) * 8)
+#define ATOMIC_MASK(bit) (1 << ((bit) & (ATOMIC_BITS - 1)))
+#define ATOMIC_ELEM(addr, bit) ((addr) + ((bit) / ATOMIC_BITS))
+
+    /**
+     * INTERNAL_HIDDEN @endcond
+     */
+
+    /**
+     * @brief Define an array of atomic variables.
+     *
+     * This macro defines an array of atomic variables containing at least
+     * @a num_bits bits.
+     *
+     * @note
+     * If used from file scope, the bits of the array are initialized to zero;
+     * if used from within a function, the bits are left uninitialized.
+     *
+     * @param name Name of array of atomic variables.
+     * @param num_bits Number of bits needed.
+     */
+#define ATOMIC_DEFINE(name, num_bits) \
+	atomic_t name[1 + ((num_bits) - 1) / ATOMIC_BITS]
+
+    /**
+     * @brief Atomically test a bit.
+     *
+     * This routine tests whether bit number @a bit of @a target is set or not.
+     * The target may be a single atomic variable or an array of them.
+     *
+     * @param target Address of atomic variable or array.
+     * @param bit Bit number (starting from 0).
+     *
+     * @return 1 if the bit was set, 0 if it wasn't.
+     */
+    static inline int
+    atomic_test_bit(const atomic_t *target, int bit)
+    {
+        atomic_val_t val = atomic_get(ATOMIC_ELEM(target, bit));
+
+        return (1 & (val >> (bit & (ATOMIC_BITS - 1))));
+    }
+
+    /**
+     * @brief Atomically test and clear a bit.
+     *
+     * Atomically clear bit number @a bit of @a target and return its old value.
+     * The target may be a single atomic variable or an array of them.
+     *
+     * @param target Address of atomic variable or array.
+     * @param bit Bit number (starting from 0).
+     *
+     * @return 1 if the bit was set, 0 if it wasn't.
+     */
+    static inline int
+    atomic_test_and_clear_bit(atomic_t *target, int bit)
+    {
+        atomic_val_t mask = ATOMIC_MASK(bit);
+        atomic_val_t old;
+
+        old = atomic_and(ATOMIC_ELEM(target, bit), ~mask);
+
+        return (old & mask) != 0;
+    }
+
+    /**
+     * @brief Atomically set a bit.
+     *
+     * Atomically set bit number @a bit of @a target and return its old value.
+     * The target may be a single atomic variable or an array of them.
+     *
+     * @param target Address of atomic variable or array.
+     * @param bit Bit number (starting from 0).
+     *
+     * @return 1 if the bit was set, 0 if it wasn't.
+     */
+    static inline int
+    atomic_test_and_set_bit(atomic_t *target, int bit)
+    {
+        atomic_val_t mask = ATOMIC_MASK(bit);
+        atomic_val_t old;
+
+        old = atomic_or(ATOMIC_ELEM(target, bit), mask);
+
+        return (old & mask) != 0;
+    }
+
+    /**
+     * @brief Atomically clear a bit.
+     *
+     * Atomically clear bit number @a bit of @a target.
+     * The target may be a single atomic variable or an array of them.
+     *
+     * @param target Address of atomic variable or array.
+     * @param bit Bit number (starting from 0).
+     *
+     * @return N/A
+     */
+    static inline void
+    atomic_clear_bit(atomic_t *target, int bit)
+    {
+        atomic_val_t mask = ATOMIC_MASK(bit);
+
+        atomic_and(ATOMIC_ELEM(target, bit), ~mask);
+    }
+
+    /**
+     * @brief Atomically set a bit.
+     *
+     * Atomically set bit number @a bit of @a target.
+     * The target may be a single atomic variable or an array of them.
+     *
+     * @param target Address of atomic variable or array.
+     * @param bit Bit number (starting from 0).
+     *
+     * @return N/A
+     */
+    static inline void
+    atomic_set_bit(atomic_t *target, int bit)
+    {
+        atomic_val_t mask = ATOMIC_MASK(bit);
+
+        atomic_or(ATOMIC_ELEM(target, bit), mask);
+    }
+
+/**
+* @brief Atomically set a bit to a given value.
+*
+* Atomically set bit number @a bit of @a target to value @a val.
+* The target may be a single atomic variable or an array of them.
+*
+* @param target Address of atomic variable or array.
+* @param bit Bit number (starting from 0).
+* @param val true for 1, false for 0.
+*
+* @return N/A
+*/
+static inline void atomic_set_bit_to(atomic_t *target, int bit, bool val)
+{
+    atomic_val_t mask = ATOMIC_MASK(bit);
+
+    if (val) {
+        (void)atomic_or(ATOMIC_ELEM(target, bit), mask);
+    } else {
+        (void)atomic_and(ATOMIC_ELEM(target, bit), ~mask);
+    }
+}
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __ATOMIC_H__ */

nimble/host/mesh/include/mesh/cdb.h

@@ -0,0 +1,266 @@
+/*
+ * Copyright (c) 2019 Tobias Svehagen
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#ifndef _BLUETOOTH_MESH_CDB_H_
+#define _BLUETOOTH_MESH_CDB_H_
+
+#include "syscfg/syscfg.h"
+
+#if MYNEWT_VAL(BLE_MESH_CDB)
+#define NODE_COUNT    CONFIG_BT_MESH_NODE_COUNT
+#define SUBNET_COUNT  CONFIG_BT_MESH_SUBNET_COUNT
+#define APP_KEY_COUNT CONFIG_BT_MESH_APP_KEY_COUNT
+#else
+#define NODE_COUNT    0
+#define SUBNET_COUNT  0
+#define APP_KEY_COUNT 0
+#endif
+
+#include "atomic.h"
+
+enum {
+	BT_MESH_CDB_NODE_CONFIGURED,
+	BT_MESH_CDB_NODE_BLACKLISTED,
+
+	BT_MESH_CDB_NODE_FLAG_COUNT
+};
+
+struct bt_mesh_cdb_node {
+	uint8_t  uuid[16];
+	uint16_t addr;
+	uint16_t net_idx;
+	uint8_t  num_elem;
+	uint8_t  dev_key[16];
+
+	ATOMIC_DEFINE(flags, BT_MESH_CDB_NODE_FLAG_COUNT);
+};
+
+struct bt_mesh_cdb_subnet {
+	uint16_t net_idx;
+
+	uint8_t kr_phase;
+
+	struct {
+		uint8_t net_key[16];
+	} keys[2];
+};
+
+struct bt_mesh_cdb_app_key {
+	uint16_t net_idx;
+	uint16_t app_idx;
+
+	struct {
+		uint8_t app_key[16];
+	} keys[2];
+};
+
+enum {
+	BT_MESH_CDB_VALID,
+	BT_MESH_CDB_SUBNET_PENDING,
+	BT_MESH_CDB_KEYS_PENDING,
+	BT_MESH_CDB_NODES_PENDING,
+	BT_MESH_CDB_IVU_IN_PROGRESS,
+
+	BT_MESH_CDB_FLAG_COUNT,
+};
+
+struct bt_mesh_cdb {
+	uint32_t iv_index;
+
+	ATOMIC_DEFINE(flags, BT_MESH_CDB_FLAG_COUNT);
+
+	struct bt_mesh_cdb_node nodes[NODE_COUNT];
+	struct bt_mesh_cdb_subnet subnets[SUBNET_COUNT];
+	struct bt_mesh_cdb_app_key app_keys[APP_KEY_COUNT];
+};
+
+extern struct bt_mesh_cdb bt_mesh_cdb;
+
+/** @brief Create the Mesh Configuration Database.
+ *
+ *  Create and initialize the Mesh Configuration Database. A primary subnet,
+ *  ie one with NetIdx 0, will be added and the provided key will be used as
+ *  NetKey for that subnet.
+ *
+ *  @param key The NetKey to be used for the primary subnet.
+ *
+ *  @return 0 on success or negative error code on failure.
+ */
+int bt_mesh_cdb_create(const uint8_t key[16]);
+
+/** @brief Clear the Mesh Configuration Database.
+ *
+ *  Remove all nodes, subnets and app-keys stored in the database and mark
+ *  the database as invalid. The data will be cleared from persistent storage
+ *  if CONFIG_BT_SETTINGS is enabled.
+ */
+void bt_mesh_cdb_clear(void);
+
+/** @brief Set and store the IV Index and IV Update flag.
+ *
+ *  The IV Index stored in the CDB will be the one used during provisioning
+ *  of new nodes. This function is generally only used from inside the stack.
+ *
+ *  This function will store the data to persistent storage if
+ *  CONFIG_BT_SETTINGS is enabled.
+ *
+ *  @param iv_index The new IV Index to use.
+ *  @param iv_update True if there is an ongoing IV Update procedure.
+ */
+void bt_mesh_cdb_iv_update(uint32_t iv_index, bool iv_update);
+
+/** @brief Allocate a node.
+ *
+ *  Allocate a new node in the CDB.
+ *
+ *  @param uuid UUID of the node.
+ *  @param addr Address of the node's primary element. If 0, the lowest
+ *              possible address available will be assigned to the node.
+ *  @param num_elem Number of elements that the node has.
+ *  @param net_idx NetIdx that the node was provisioned to.
+ *
+ *  @return The new node or NULL if it cannot be allocated.
+ */
+struct bt_mesh_cdb_node *bt_mesh_cdb_node_alloc(const uint8_t uuid[16], uint16_t addr,
+						uint8_t num_elem, uint16_t net_idx);
+
+/** @brief Delete a node.
+ *
+ *  Delete a node from the CDB.
+ *
+ *  @param node The node to be deleted.
+ *  @param store If true, the node will be cleared from persistent storage.
+ */
+void bt_mesh_cdb_node_del(struct bt_mesh_cdb_node *node, bool store);
+
+/** @brief Get a node by address.
+ *
+ *  Try to find the node that has the provided address assigned to one of its
+ *  elements.
+ *
+ *  @param addr Address of the element to look for.
+ *
+ *  @return The node that has an element with address addr or NULL if no such
+ *          node exists.
+ */
+struct bt_mesh_cdb_node *bt_mesh_cdb_node_get(uint16_t addr);
+
+/** @brief Store node to persistent storage.
+ *
+ *  @param node Node to be stored.
+ */
+void bt_mesh_cdb_node_store(const struct bt_mesh_cdb_node *node);
+
+enum {
+	BT_MESH_CDB_ITER_STOP = 0,
+	BT_MESH_CDB_ITER_CONTINUE,
+};
+
+/** @typedef bt_mesh_cdb_node_func_t
+ *  @brief Node iterator callback.
+ *
+ *  @param node Node found.
+ *  @param user_data Data given.
+ *
+ *  @return BT_MESH_CDB_ITER_CONTINUE to continue to iterate through the nodes
+ *          or BT_MESH_CDB_ITER_STOP to stop.
+ */
+typedef uint8_t (*bt_mesh_cdb_node_func_t)(struct bt_mesh_cdb_node *node,
+					void *user_data);
+
+/** @brief Node iterator.
+ *
+ *  Iterate nodes in the Mesh Configuration Database. The callback function
+ *  will only be called for valid, ie allocated, nodes.
+ *
+ *  @param func Callback function.
+ *  @param user_data Data to pass to the callback.
+ */
+void bt_mesh_cdb_node_foreach(bt_mesh_cdb_node_func_t func, void *user_data);
+
+/** @brief Allocate a subnet.
+ *
+ *  Allocate a new subnet in the CDB.
+ *
+ *  @param net_idx NetIdx of the subnet.
+ *
+ *  @return The new subnet or NULL if it cannot be allocated.
+ */
+struct bt_mesh_cdb_subnet *bt_mesh_cdb_subnet_alloc(uint16_t net_idx);
+
+/** @brief Delete a subnet.
+ *
+ *  Delete a subnet from the CDB.
+ *
+ *  @param sub The subnet to be deleted.
+ *  @param store If true, the subnet will be cleared from persistent storage.
+ */
+void bt_mesh_cdb_subnet_del(struct bt_mesh_cdb_subnet *sub, bool store);
+
+/** @brief Get a subnet by NetIdx
+ *
+ *  Try to find the subnet with the specified NetIdx.
+ *
+ *  @param net_idx NetIdx of the subnet to look for.
+ *
+ *  @return The subnet with the specified NetIdx or NULL if no such subnet
+ *          exists.
+ */
+struct bt_mesh_cdb_subnet *bt_mesh_cdb_subnet_get(uint16_t net_idx);
+
+/** @brief Store subnet to persistent storage.
+ *
+ *  @param sub Subnet to be stored.
+ */
+void bt_mesh_cdb_subnet_store(const struct bt_mesh_cdb_subnet *sub);
+
+/** @brief Get the flags for a subnet
+ *
+ *  @param sub The subnet to get flags for.
+ *
+ *  @return The flags for the subnet.
+ */
+uint8_t bt_mesh_cdb_subnet_flags(const struct bt_mesh_cdb_subnet *sub);
+
+
+/** @brief Allocate an application key.
+ *
+ *  Allocate a new application key in the CDB.
+ *
+ *  @param net_idx NetIdx of NetKey that the application key is bound to.
+ *  @param app_idx AppIdx of the application key.
+ *
+ *  @return The new application key or NULL if it cannot be allocated.
+ */
+struct bt_mesh_cdb_app_key *bt_mesh_cdb_app_key_alloc(uint16_t net_idx,
+						      uint16_t app_idx);
+
+/** @brief Delete an application key.
+ *
+ *  Delete an application key from the CDB.
+ *
+ *  @param key The application key to be deleted.
+ *  @param store If true, the key will be cleared from persistent storage.
+ */
+void bt_mesh_cdb_app_key_del(struct bt_mesh_cdb_app_key *key, bool store);
+
+/** @brief Get an application key by AppIdx
+ *
+ *  Try to find the application key with the specified AppIdx.
+ *
+ *  @param app_idx AppIdx of the application key to look for.
+ *
+ *  @return The application key with the specified AppIdx or NULL if no such key
+ *          exists.
+ */
+struct bt_mesh_cdb_app_key *bt_mesh_cdb_app_key_get(uint16_t app_idx);
+
+/** @brief Store application key to persistent storage.
+ *
+ *  @param key Application key to be stored.
+ */
+void bt_mesh_cdb_app_key_store(const struct bt_mesh_cdb_app_key *key);
+
+#endif /* ZEPHYR_INCLUDE_BLUETOOTH_MESH_CDB_H_ */

nimble/host/mesh/include/mesh/cfg.h

@@ -0,0 +1,491 @@
+/** @file
+ *  @brief Bluetooth Mesh Runtime Configuration APIs.
+ */
+
+/*
+ * Copyright (c) 2020 Nordic Semiconductor
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#ifndef _BT_MESH_CFG_H_
+#define _BT_MESH_CFG_H_
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <sys/types.h>
+
+/**
+ * @brief Bluetooth Mesh Runtime Configuration API
+ * @defgroup bt_mesh_cfg Bluetooth Mesh Runtime Configuration
+ * @ingroup bt_mesh
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** Bluetooth Mesh Feature states */
+enum bt_mesh_feat_state {
+	/** Feature is supported, but disabled. */
+	BT_MESH_FEATURE_DISABLED,
+	/** Feature is supported and enabled. */
+	BT_MESH_FEATURE_ENABLED,
+	/** Feature is not supported, and cannot be enabled. */
+	BT_MESH_FEATURE_NOT_SUPPORTED,
+};
+
+/* Key Refresh Phase */
+#define BT_MESH_KR_NORMAL                   0x00
+#define BT_MESH_KR_PHASE_1                  0x01
+#define BT_MESH_KR_PHASE_2                  0x02
+#define BT_MESH_KR_PHASE_3                  0x03
+
+/* Legacy feature defines */
+#define BT_MESH_RELAY_DISABLED              BT_MESH_FEATURE_DISABLED
+#define BT_MESH_RELAY_ENABLED               BT_MESH_FEATURE_ENABLED
+#define BT_MESH_RELAY_NOT_SUPPORTED         BT_MESH_FEATURE_NOT_SUPPORTED
+
+#define BT_MESH_BEACON_DISABLED             BT_MESH_FEATURE_DISABLED
+#define BT_MESH_BEACON_ENABLED              BT_MESH_FEATURE_ENABLED
+
+#define BT_MESH_GATT_PROXY_DISABLED         BT_MESH_FEATURE_DISABLED
+#define BT_MESH_GATT_PROXY_ENABLED          BT_MESH_FEATURE_ENABLED
+#define BT_MESH_GATT_PROXY_NOT_SUPPORTED    BT_MESH_FEATURE_NOT_SUPPORTED
+
+#define BT_MESH_FRIEND_DISABLED             BT_MESH_FEATURE_DISABLED
+#define BT_MESH_FRIEND_ENABLED              BT_MESH_FEATURE_ENABLED
+#define BT_MESH_FRIEND_NOT_SUPPORTED        BT_MESH_FEATURE_NOT_SUPPORTED
+
+#define BT_MESH_NODE_IDENTITY_STOPPED       BT_MESH_FEATURE_DISABLED
+#define BT_MESH_NODE_IDENTITY_RUNNING       BT_MESH_FEATURE_ENABLED
+#define BT_MESH_NODE_IDENTITY_NOT_SUPPORTED BT_MESH_FEATURE_NOT_SUPPORTED
+
+/** @brief Enable or disable sending of the Secure Network Beacon.
+ *
+ *  @param beacon New Secure Network Beacon state.
+ */
+void bt_mesh_beacon_set(bool beacon);
+
+/** @brief Get the current Secure Network Beacon state.
+ *
+ *  @returns Whether the Secure Network Beacon feature is enabled.
+ */
+bool bt_mesh_beacon_enabled(void);
+
+/** @brief Set the default TTL value.
+ *
+ *  The default TTL value is used when no explicit TTL value is set. Models will
+ *  use the default TTL value when @ref bt_mesh_msg_ctx::send_ttl is
+ *  @ref BT_MESH_TTL_DEFAULT.
+ *
+ *  @param default_ttl The new default TTL value. Valid values are 0x00 and 0x02
+ *                     to @ref BT_MESH_TTL_MAX.
+ *
+ *  @retval 0       Successfully set the default TTL value.
+ *  @retval -EINVAL Invalid TTL value.
+ */
+int bt_mesh_default_ttl_set(uint8_t default_ttl);
+
+/** @brief Get the current default TTL value.
+ *
+ *  @return The current default TTL value.
+ */
+uint8_t bt_mesh_default_ttl_get(void);
+
+/** @brief Set the Network Transmit parameters.
+ *
+ *  The Network Transmit parameters determine the parameters local messages are
+ *  transmitted with.
+ *
+ *  @see BT_MESH_TRANSMIT
+ *
+ *  @param xmit New Network Transmit parameters. Use @ref BT_MESH_TRANSMIT for
+ *              encoding.
+ */
+void bt_mesh_net_transmit_set(uint8_t xmit);
+
+/** @brief Get the current Network Transmit parameters.
+ *
+ *  The @ref BT_MESH_TRANSMIT_COUNT and @ref BT_MESH_TRANSMIT_INT macros can be
+ *  used to decode the Network Transmit parameters.
+ *
+ *  @return The current Network Transmit parameters.
+ */
+uint8_t bt_mesh_net_transmit_get(void);
+
+/** @brief Configure the Relay feature.
+ *
+ *  Enable or disable the Relay feature, and configure the parameters to
+ *  transmit relayed messages with.
+ *
+ *  Support for the Relay feature must be enabled through the
+ *  @c CONFIG_BT_MESH_RELAY configuration option.
+ *
+ *  @see BT_MESH_TRANSMIT
+ *
+ *  @param relay New Relay feature state. Must be one of
+ *               @ref BT_MESH_FEATURE_ENABLED and
+ *               @ref BT_MESH_FEATURE_DISABLED.
+ *  @param xmit  New Relay retransmit parameters. Use @ref BT_MESH_TRANSMIT for
+ *               encoding.
+ *
+ *  @retval 0         Successfully changed the Relay configuration.
+ *  @retval -ENOTSUP  The Relay feature is not supported.
+ *  @retval -EINVAL   Invalid parameter.
+ *  @retval -EALREADY Already using the given parameters.
+ */
+int bt_mesh_relay_set(enum bt_mesh_feat_state relay, uint8_t xmit);
+
+/** @brief Get the current Relay feature state.
+ *
+ *  @returns The Relay feature state.
+ */
+enum bt_mesh_feat_state bt_mesh_relay_get(void);
+
+/** @brief Get the current Relay Retransmit parameters.
+ *
+ *  The @ref BT_MESH_TRANSMIT_COUNT and @ref BT_MESH_TRANSMIT_INT macros can be
+ *  used to decode the Relay Retransmit parameters.
+ *
+ *  @return The current Relay Retransmit parameters, or 0 if relay is not
+ *          supported.
+ */
+uint8_t bt_mesh_relay_retransmit_get(void);
+
+/** @brief Enable or disable the GATT Proxy feature.
+ *
+ *  Support for the GATT Proxy feature must be enabled through the
+ *  @c CONFIG_BT_MESH_GATT_PROXY configuration option.
+ *
+ *  @note The GATT Proxy feature only controls a Proxy node's ability to relay
+ *        messages to the mesh network. A node that supports GATT Proxy will
+ *        still advertise Connectable Proxy beacons, even if the feature is
+ *        disabled. The Proxy feature can only be fully disabled through compile
+ *        time configuration.
+ *
+ *  @param gatt_proxy New GATT Proxy state. Must be one of
+ *                    @ref BT_MESH_FEATURE_ENABLED and
+ *                    @ref BT_MESH_FEATURE_DISABLED.
+ *
+ *  @retval 0         Successfully changed the GATT Proxy feature state.
+ *  @retval -ENOTSUP  The GATT Proxy feature is not supported.
+ *  @retval -EINVAL   Invalid parameter.
+ *  @retval -EALREADY Already in the given state.
+ */
+int bt_mesh_gatt_proxy_set(enum bt_mesh_feat_state gatt_proxy);
+
+/** @brief Get the current GATT Proxy state.
+ *
+ *  @returns The GATT Proxy feature state.
+ */
+enum bt_mesh_feat_state bt_mesh_gatt_proxy_get(void);
+
+/** @brief Enable or disable the Friend feature.
+ *
+ *  Any active friendships will be terminated immediately if the Friend feature
+ *  is disabled.
+ *
+ *  Support for the Friend feature must be enabled through the
+ *  @c CONFIG_BT_MESH_FRIEND configuration option.
+ *
+ *  @param friendship New Friend feature state. Must be one of
+ *                    @ref BT_MESH_FEATURE_ENABLED and
+ *                    @ref BT_MESH_FEATURE_DISABLED.
+ *
+ *  @retval 0        Successfully changed the Friend feature state.
+ *  @retval -ENOTSUP The Friend feature is not supported.
+ *  @retval -EINVAL  Invalid parameter.
+ *  @retval -EALREADY Already in the given state.
+ */
+int bt_mesh_friend_set(enum bt_mesh_feat_state friendship);
+
+/** @brief Get the current Friend state.
+ *
+ *  @returns The Friend feature state.
+ */
+enum bt_mesh_feat_state bt_mesh_friend_get(void);
+
+/**
+ * @brief Bluetooth Mesh Subnet Configuration
+ * @defgroup bt_mesh_cfg_subnet Bluetooth Mesh Subnet Configuration
+ * @{
+ */
+
+/** @brief Add a Subnet.
+ *
+ *  Adds a subnet with the given network index and network key to the list of
+ *  known Subnets. All messages sent on the given Subnet will be processed by
+ *  this node, and the node may send and receive Network Beacons on the given
+ *  Subnet.
+ *
+ *  @param net_idx Network index.
+ *  @param key     Root network key of the Subnet. All other keys are derived
+ *                 from this.
+ *
+ *  @retval STATUS_SUCCESS The Subnet was successfully added.
+ *  @retval STATUS_INSUFF_RESOURCES No room for this Subnet.
+ *  @retval STATUS_UNSPECIFIED The Subnet couldn't be created for some reason.
+ */
+uint8_t bt_mesh_subnet_add(uint16_t net_idx, const uint8_t key[16]);
+
+/** @brief Update the given Subnet.
+ *
+ *  Starts the Key Refresh procedure for this Subnet by adding a second set of
+ *  encryption keys. The Subnet will continue sending with the old key (but
+ *  receiving messages using both) until the Subnet enters Key Refresh phase 2.
+ *
+ *  This allows a network configurator to replace old network and application
+ *  keys for the entire network, effectively removing access for all nodes that
+ *  aren't given the new keys.
+ *
+ *  @param net_idx Network index.
+ *  @param key     New root network key of the Subnet.
+ *
+ *  @retval STATUS_SUCCESS The Subnet was updated with a second key.
+ *  @retval STATUS_INVALID_NETKEY The NetIdx is unknown.
+ *  @retval STATUS_IDX_ALREADY_STORED The @c key value is the same as the
+ *                                    current key.
+ *  @retval STATUS_CANNOT_UPDATE The Subnet cannot be updated for some reason.
+ */
+uint8_t bt_mesh_subnet_update(uint16_t net_idx, const uint8_t key[16]);
+
+/** @brief Delete a Subnet.
+ *
+ *  Removes the Subnet with the given network index from the node. The node will
+ *  stop sending Network Beacons with the given Subnet, and can no longer
+ *  process messages on this Subnet.
+ *
+ *  All Applications bound to this Subnet are also deleted.
+ *
+ *  @param net_idx Network index.
+ *
+ *  @retval STATUS_SUCCESS The Subnet was deleted.
+ *  @retval STATUS_INVALID_NETKEY The NetIdx is unknown.
+ */
+uint8_t bt_mesh_subnet_del(uint16_t net_idx);
+
+/** @brief Check whether a Subnet is known.
+ *
+ *  @param net_idx Network index
+ *
+ *  @return true if a Subnet with the given index exists, false otherwise.
+ */
+bool bt_mesh_subnet_exists(uint16_t net_idx);
+
+/** @brief Set the Subnet's Key Refresh phase.
+ *
+ *  The Key Refresh procedure is started by updating the Subnet keys through
+ *  @ref bt_mesh_subnet_update. This puts the Subnet in Key Refresh Phase 1.
+ *  Once all nodes have received the new Subnet key, Key Refresh Phase 2 can be
+ *  activated through this function to start transmitting with the new network
+ *  key. Finally, to revoke the old key, set the Key Refresh Phase to 3. This
+ *  removes the old keys from the node, and returns the Subnet back to normal
+ *  single-key operation with the new key set.
+ *
+ *  @param net_idx Network index.
+ *  @param phase   Pointer to the new Key Refresh phase. Will return the actual
+ *                 Key Refresh phase after updating.
+ *
+ *  @retval STATUS_SUCCESS The Key Refresh phase of the Subnet was successfully
+ *                         changed.
+ *  @retval STATUS_INVALID_NETKEY The NetIdx is unknown.
+ *  @retval STATUS_CANNOT_UPDATE The given phase change is invalid.
+ */
+uint8_t bt_mesh_subnet_kr_phase_set(uint16_t net_idx, uint8_t *phase);
+
+/** @brief Get the Subnet's Key Refresh phase.
+ *
+ *  @param net_idx Network index.
+ *  @param phase   Pointer to the Key Refresh variable to fill.
+ *
+ *  @retval STATUS_SUCCESS Successfully populated the @c phase variable.
+ *  @retval STATUS_INVALID_NETKEY The NetIdx is unknown.
+ */
+uint8_t bt_mesh_subnet_kr_phase_get(uint16_t net_idx, uint8_t *phase);
+
+/** @brief Set the Node Identity state of the Subnet.
+ *
+ *  The Node Identity state of a Subnet determines whether the Subnet advertises
+ *  connectable Node Identity beacons for Proxy Clients to connect to.
+ *  Once started, the Node Identity beacon runs for 60 seconds, or until it is
+ *  stopped.
+ *
+ *  This function serves the same purpose as @ref bt_mesh_proxy_identity_enable,
+ *  but only acts on a single Subnet.
+ *
+ *  GATT Proxy support must be enabled through
+ *  @option{CONFIG_BT_MESH_GATT_PROXY}.
+ *
+ *  @param net_idx Network index.
+ *  @param node_id New Node Identity state, must be either @ref
+ *                 BT_MESH_FEATURE_ENABLED or @ref BT_MESH_FEATURE_DISABLED.
+ *
+ *  @retval STATUS_SUCCESS Successfully set the Node Identity state of the
+ *                         Subnet.
+ *  @retval STATUS_INVALID_NETKEY The NetIdx is unknown.
+ *  @retval STATUS_FEAT_NOT_SUPP The Node Identity feature is not supported.
+ *  @retval STATUS_CANNOT_SET Couldn't set the Node Identity state.
+ */
+uint8_t bt_mesh_subnet_node_id_set(uint16_t net_idx,
+				   enum bt_mesh_feat_state node_id);
+
+/** @brief Get the Node Identity state of the Subnet.
+ *
+ *  @param net_idx Network index.
+ *  @param node_id Node Identity variable to fill.
+ *
+ *  @retval STATUS_SUCCESS Successfully populated the @c node_id variable.
+ *  @retval STATUS_INVALID_NETKEY The NetIdx is unknown.
+ */
+uint8_t bt_mesh_subnet_node_id_get(uint16_t net_idx,
+				   enum bt_mesh_feat_state *node_id);
+
+/** @brief Get a list of all known Subnet indexes.
+ *
+ *  Builds a list of all known Subnet indexes in the @c net_idxs array.
+ *  If the @c net_idxs array is smaller than the list of known Subnets, this
+ *  function fills all available entries and returns @c -ENOMEM. In this
+ *  case, the next @c max entries of the list can be read out by calling
+ *  @code
+ *  bt_mesh_subnets_get(list, max, max);
+ *  @endcode
+ *
+ *  Note that any changes to the Subnet list between calls to this function
+ *  could change the order and number of entries in the list.
+ *
+ *  @param net_idxs Array to fill.
+ *  @param max      Max number of indexes to return.
+ *  @param skip     Number of indexes to skip. Enables batched processing of the
+ *                  list.
+ *
+ *  @return The number of indexes added to the @c net_idxs array, or @c -ENOMEM
+ *          if the number of known Subnets exceeds the @c max parameter.
+ */
+ssize_t bt_mesh_subnets_get(uint16_t net_idxs[], size_t max, off_t skip);
+
+/**
+ * @}
+ */
+
+/**
+ * @brief Bluetooth Mesh Application Configuration
+ * @defgroup bt_mesh_cfg_app Bluetooth Mesh Application Configuration
+ * @{
+ */
+
+/** @brief Add an Application key.
+ *
+ *  Adds the Application with the given index to the list of known applications.
+ *  Allows the node to send and receive model messages encrypted with this
+ *  Application key.
+ *
+ *  Every Application is bound to a specific Subnet. The node must know the
+ *  Subnet the Application is bound to before it can add the Application.
+ *
+ *  @param app_idx Application index.
+ *  @param net_idx Network index the Application is bound to.
+ *  @param key     Application key value.
+ *
+ *  @retval STATUS_SUCCESS The Application was successfully added.
+ *  @retval STATUS_INVALID_NETKEY The NetIdx is unknown.
+ *  @retval STATUS_INSUFF_RESOURCES There's no room for storing this
+ *                                  Application.
+ *  @retval STATUS_INVALID_BINDING This AppIdx is already bound to another
+ *                                 Subnet.
+ *  @retval STATUS_IDX_ALREADY_STORED This AppIdx is already stored with a
+ *                                    different key value.
+ *  @retval STATUS_CANNOT_SET Cannot set the Application key for some reason.
+ */
+uint8_t bt_mesh_app_key_add(uint16_t app_idx, uint16_t net_idx,
+			    const uint8_t key[16]);
+
+/** @brief Update an Application key.
+ *
+ *  Update an Application with a second Application key, as part of the
+ *  Key Refresh procedure of the bound Subnet. The node will continue
+ *  transmitting with the old application key (but receiving on both) until the
+ *  Subnet enters Key Refresh phase 2. Once the Subnet enters Key Refresh phase
+ *  3, the old application key will be deleted.
+ *
+ *  @note The Application key can only be updated if the bound Subnet is in Key
+ *        Refresh phase 1.
+ *
+ *  @param app_idx Application index.
+ *  @param net_idx Network index the Application is bound to, or
+ *                 @ref BT_MESH_KEY_ANY to skip the binding check.
+ *  @param key     New key value.
+ *
+ *  @retval STATUS_SUCCESS The Application key was successfully updated.
+ *  @retval STATUS_INVALID_NETKEY The NetIdx is unknown.
+ *  @retval STATUS_INVALID_BINDING This AppIdx is not bound to the given NetIdx.
+ *  @retval STATUS_CANNOT_UPDATE The Application key cannot be updated for some
+ *                               reason.
+ *  @retval STATUS_IDX_ALREADY_STORED This AppIdx is already updated with a
+ *                                    different key value.
+ */
+uint8_t bt_mesh_app_key_update(uint16_t app_idx, uint16_t net_idx,
+			       const uint8_t key[16]);
+
+/** @brief Delete an Application key.
+ *
+ *  All models bound to this application will remove this binding.
+ *  All models publishing with this application will stop publishing.
+ *
+ *  @param app_idx Application index.
+ *  @param net_idx Network index.
+ *
+ *  @retval STATUS_SUCCESS The Application key was successfully deleted.
+ *  @retval STATUS_INVALID_NETKEY The NetIdx is unknown.
+ *  @retval STATUS_INVALID_BINDING This AppIdx is not bound to the given NetIdx.
+ */
+uint8_t bt_mesh_app_key_del(uint16_t app_idx, uint16_t net_idx);
+
+/** @brief Check if an Application key is known.
+ *
+ *  @param app_idx Application index.
+ *
+ *  @return true if the Application is known, false otherwise.
+ */
+bool bt_mesh_app_key_exists(uint16_t app_idx);
+
+/** @brief Get a list of all known Application key indexes.
+ *
+ *  Builds a list of all Application indexes for the given network index in the
+ *  @c app_idxs array. If the @c app_idxs array cannot fit all bound
+ *  Applications, this function fills all available entries and returns @c
+ *  -ENOMEM. In this case, the next @c max entries of the list can be read out
+ *  by calling
+ *  @code
+ *  bt_mesh_app_keys_get(net_idx, list, max, max);
+ *  @endcode
+ *
+ *  Note that any changes to the Application key list between calls to this
+ *  function could change the order and number of entries in the list.
+ *
+ *  @param net_idx  Network Index to get the Applications of, or @ref
+ *                  BT_MESH_KEY_ANY to get all Applications.
+ *  @param app_idxs Array to fill.
+ *  @param max      Max number of indexes to return.
+ *  @param skip     Number of indexes to skip. Enables batched processing of the
+ *                  list.
+ *
+ *  @return The number of indexes added to the @c app_idxs array, or @c -ENOMEM
+ *          if the number of known Applications exceeds the @c max parameter.
+ */
+ssize_t bt_mesh_app_keys_get(uint16_t net_idx, uint16_t app_idxs[], size_t max,
+			     off_t skip);
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+/**
+ * @}
+ */
+
+#endif /* _BT_MESH_CFG_H_ */

nimble/host/mesh/include/mesh/cfg_cli.h

@@ -17,166 +17,261 @@
  * @{
  */
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 /** Mesh Configuration Client Model Context */
 struct bt_mesh_cfg_cli {
 	struct bt_mesh_model *model;
 
 	struct k_sem          op_sync;
-	u32_t                 op_pending;
+	uint32_t              op_pending;
 	void                 *op_param;
+    	uint16_t              op_addr;
 };
 
 extern const struct bt_mesh_model_op bt_mesh_cfg_cli_op[];
+extern const struct bt_mesh_model_cb bt_mesh_cfg_cli_cb;
+
+#define BT_MESH_MODEL_CFG_CLI(cli_data)                                        \
+	BT_MESH_MODEL_CB(BT_MESH_MODEL_ID_CFG_CLI, bt_mesh_cfg_cli_op, NULL,   \
+			 cli_data, &bt_mesh_cfg_cli_cb)
+
+int bt_mesh_cfg_node_reset(uint16_t net_idx, uint16_t addr, bool *status);
+
+int bt_mesh_cfg_comp_data_get(uint16_t net_idx, uint16_t addr, uint8_t page,
+			      uint8_t *status, struct os_mbuf *comp);
+
+int bt_mesh_cfg_beacon_get(uint16_t net_idx, uint16_t addr, uint8_t *status);
+
+int bt_mesh_cfg_beacon_set(uint16_t net_idx, uint16_t addr, uint8_t val, uint8_t *status);
+
+int bt_mesh_cfg_ttl_get(uint16_t net_idx, uint16_t addr, uint8_t *ttl);
+
+int bt_mesh_cfg_ttl_set(uint16_t net_idx, uint16_t addr, uint8_t val, uint8_t *ttl);
+
+int bt_mesh_cfg_friend_get(uint16_t net_idx, uint16_t addr, uint8_t *status);
 
-#define BT_MESH_MODEL_CFG_CLI(cli_data)                                      \
-		BT_MESH_MODEL(BT_MESH_MODEL_ID_CFG_CLI,                      \
-			      bt_mesh_cfg_cli_op, NULL, cli_data)
+int bt_mesh_cfg_friend_set(uint16_t net_idx, uint16_t addr, uint8_t val, uint8_t *status);
 
-int bt_mesh_cfg_comp_data_get(u16_t net_idx, u16_t addr, u8_t page,
-			      u8_t *status, struct os_mbuf *comp);
+int bt_mesh_cfg_gatt_proxy_get(uint16_t net_idx, uint16_t addr, uint8_t *status);
 
-int bt_mesh_cfg_beacon_get(u16_t net_idx, u16_t addr, u8_t *status);
+int bt_mesh_cfg_gatt_proxy_set(uint16_t net_idx, uint16_t addr, uint8_t val,
+			       uint8_t *status);
 
-int bt_mesh_cfg_beacon_set(u16_t net_idx, u16_t addr, u8_t val, u8_t *status);
+int bt_mesh_cfg_net_transmit_get(uint16_t net_idx, uint16_t addr,
+			  uint8_t *transmit);
 
-int bt_mesh_cfg_ttl_get(u16_t net_idx, u16_t addr, u8_t *ttl);
+int bt_mesh_cfg_net_transmit_set(uint16_t net_idx, uint16_t addr,
+		uint8_t val, uint8_t *transmit);
 
-int bt_mesh_cfg_ttl_set(u16_t net_idx, u16_t addr, u8_t val, u8_t *ttl);
+int bt_mesh_cfg_relay_get(uint16_t net_idx, uint16_t addr, uint8_t *status,
+			  uint8_t *transmit);
 
-int bt_mesh_cfg_friend_get(u16_t net_idx, u16_t addr, u8_t *status);
+int bt_mesh_cfg_relay_set(uint16_t net_idx, uint16_t addr, uint8_t new_relay,
+			  uint8_t new_transmit, uint8_t *status, uint8_t *transmit);
 
-int bt_mesh_cfg_friend_set(u16_t net_idx, u16_t addr, u8_t val, u8_t *status);
+int bt_mesh_cfg_net_key_add(uint16_t net_idx, uint16_t addr, uint16_t key_net_idx,
+			    const uint8_t net_key[16], uint8_t *status);
 
-int bt_mesh_cfg_gatt_proxy_get(u16_t net_idx, u16_t addr, u8_t *status);
+int bt_mesh_cfg_net_key_get(uint16_t net_idx, uint16_t addr, uint16_t *keys,
+			    size_t *key_cnt);
 
-int bt_mesh_cfg_gatt_proxy_set(u16_t net_idx, u16_t addr, u8_t val,
-			       u8_t *status);
+int bt_mesh_cfg_net_key_del(uint16_t net_idx, uint16_t addr,
+			    uint16_t key_net_idx, uint8_t *status);
 
-int bt_mesh_cfg_relay_get(u16_t net_idx, u16_t addr, u8_t *status,
-			  u8_t *transmit);
+int bt_mesh_cfg_app_key_add(uint16_t net_idx, uint16_t addr, uint16_t key_net_idx,
+			    uint16_t key_app_idx, const uint8_t app_key[16],
+			    uint8_t *status);
 
-int bt_mesh_cfg_relay_set(u16_t net_idx, u16_t addr, u8_t new_relay,
-			  u8_t new_transmit, u8_t *status, u8_t *transmit);
+int bt_mesh_cfg_app_key_get(uint16_t net_idx, uint16_t addr, uint16_t key_net_idx,
+			    uint8_t *status, uint16_t *keys, size_t *key_cnt);
 
-int bt_mesh_cfg_net_key_add(u16_t net_idx, u16_t addr, u16_t key_net_idx,
-			    const u8_t net_key[16], u8_t *status);
+int bt_mesh_cfg_app_key_del(uint16_t net_idx, uint16_t addr,
+		uint16_t key_net_idx, uint16_t key_app_idx, uint8_t *status);
+
+int bt_mesh_cfg_mod_app_bind(uint16_t net_idx, uint16_t addr, uint16_t elem_addr,
+			     uint16_t mod_app_idx, uint16_t mod_id, uint8_t *status);
+
+int bt_mesh_cfg_mod_app_unbind(uint16_t net_idx, uint16_t addr,
+	uint16_t elem_addr, uint16_t mod_app_idx,
+	uint16_t mod_id, uint8_t *status);
+
+int bt_mesh_cfg_mod_app_bind_vnd(uint16_t net_idx, uint16_t addr, uint16_t elem_addr,
+				 uint16_t mod_app_idx, uint16_t mod_id, uint16_t cid,
+				 uint8_t *status);
+
+int bt_mesh_cfg_mod_app_unbind_vnd(uint16_t net_idx, uint16_t addr,
+	uint16_t elem_addr, uint16_t mod_app_idx, uint16_t mod_id,
+	uint16_t cid, uint8_t *status);
+
+int bt_mesh_cfg_mod_app_get(uint16_t net_idx, uint16_t addr, uint16_t elem_addr,
+			    uint16_t mod_id, uint8_t *status, uint16_t *apps,
+			    size_t *app_cnt);
+
+int bt_mesh_cfg_mod_app_get_vnd(uint16_t net_idx, uint16_t addr, uint16_t elem_addr,
+				uint16_t mod_id, uint16_t cid, uint8_t *status,
+				uint16_t *apps, size_t *app_cnt);
+
+/** @def BT_MESH_PUB_PERIOD_100MS
+ *
+ *  @brief Helper macro to encode model publication period in units of 100ms
+ *
+ *  @param steps Number of 100ms steps.
+ *
+ *  @return Encoded value that can be assigned to bt_mesh_cfg_mod_pub.period
+ */
+#define BT_MESH_PUB_PERIOD_100MS(steps)  ((steps) & BIT_MASK(6))
 
-int bt_mesh_cfg_app_key_add(u16_t net_idx, u16_t addr, u16_t key_net_idx,
-			    u16_t key_app_idx, const u8_t app_key[16],
-			    u8_t *status);
+/** @def BT_MESH_PUB_PERIOD_SEC
+ *
+ *  @brief Helper macro to encode model publication period in units of 1 second
+ *
+ *  @param steps Number of 1 second steps.
+ *
+ *  @return Encoded value that can be assigned to bt_mesh_cfg_mod_pub.period
+ */
+#define BT_MESH_PUB_PERIOD_SEC(steps)   (((steps) & BIT_MASK(6)) | (1 << 6))
 
-int bt_mesh_cfg_mod_app_bind(u16_t net_idx, u16_t addr, u16_t elem_addr,
-			     u16_t mod_app_idx, u16_t mod_id, u8_t *status);
+/** @def BT_MESH_PUB_PERIOD_10SEC
+ *
+ *  @brief Helper macro to encode model publication period in units of 10
+ *  seconds
+ *
+ *  @param steps Number of 10 second steps.
+ *
+ *  @return Encoded value that can be assigned to bt_mesh_cfg_mod_pub.period
+ */
+#define BT_MESH_PUB_PERIOD_10SEC(steps) (((steps) & BIT_MASK(6)) | (2 << 6))
 
-int bt_mesh_cfg_mod_app_bind_vnd(u16_t net_idx, u16_t addr, u16_t elem_addr,
-				 u16_t mod_app_idx, u16_t mod_id, u16_t cid,
-				 u8_t *status);
+/** @def BT_MESH_PUB_PERIOD_10MIN
+ *
+ *  @brief Helper macro to encode model publication period in units of 10
+ *  minutes
+ *
+ *  @param steps Number of 10 minute steps.
+ *
+ *  @return Encoded value that can be assigned to bt_mesh_cfg_mod_pub.period
+ */
+#define BT_MESH_PUB_PERIOD_10MIN(steps) (((steps) & BIT_MASK(6)) | (3 << 6))
 
 struct bt_mesh_cfg_mod_pub {
-	u16_t  addr;
-	u16_t  app_idx;
+	uint16_t  addr;
+	uint16_t  app_idx;
 	bool   cred_flag;
-	u8_t   ttl;
-	u8_t   period;
-	u8_t   transmit;
+	uint8_t   ttl;
+	uint8_t   period;
+	uint8_t   transmit;
 };
 
-int bt_mesh_cfg_mod_pub_get(u16_t net_idx, u16_t addr, u16_t elem_addr,
-			    u16_t mod_id, struct bt_mesh_cfg_mod_pub *pub,
-			    u8_t *status);
+int bt_mesh_cfg_mod_pub_get(uint16_t net_idx, uint16_t addr, uint16_t elem_addr,
+			    uint16_t mod_id, struct bt_mesh_cfg_mod_pub *pub,
+			    uint8_t *status);
+
+int bt_mesh_cfg_mod_pub_get_vnd(uint16_t net_idx, uint16_t addr, uint16_t elem_addr,
+				uint16_t mod_id, uint16_t cid,
+				struct bt_mesh_cfg_mod_pub *pub, uint8_t *status);
+
+int bt_mesh_cfg_mod_pub_set(uint16_t net_idx, uint16_t addr, uint16_t elem_addr,
+			    uint16_t mod_id, struct bt_mesh_cfg_mod_pub *pub,
+			    uint8_t *status);
 
-int bt_mesh_cfg_mod_pub_get_vnd(u16_t net_idx, u16_t addr, u16_t elem_addr,
-				u16_t mod_id, u16_t cid,
-				struct bt_mesh_cfg_mod_pub *pub, u8_t *status);
+int bt_mesh_cfg_mod_pub_set_vnd(uint16_t net_idx, uint16_t addr, uint16_t elem_addr,
+				uint16_t mod_id, uint16_t cid,
+				struct bt_mesh_cfg_mod_pub *pub, uint8_t *status);
 
-int bt_mesh_cfg_mod_pub_set(u16_t net_idx, u16_t addr, u16_t elem_addr,
-			    u16_t mod_id, struct bt_mesh_cfg_mod_pub *pub,
-			    u8_t *status);
+int bt_mesh_cfg_mod_sub_add(uint16_t net_idx, uint16_t addr, uint16_t elem_addr,
+			    uint16_t sub_addr, uint16_t mod_id, uint8_t *status);
 
-int bt_mesh_cfg_mod_pub_set_vnd(u16_t net_idx, u16_t addr, u16_t elem_addr,
-				u16_t mod_id, u16_t cid,
-				struct bt_mesh_cfg_mod_pub *pub, u8_t *status);
+int bt_mesh_cfg_mod_sub_add_vnd(uint16_t net_idx, uint16_t addr, uint16_t elem_addr,
+				 uint16_t sub_addr, uint16_t mod_id, uint16_t cid,
+				 uint8_t *status);
 
-int bt_mesh_cfg_mod_sub_add(u16_t net_idx, u16_t addr, u16_t elem_addr,
-			    u16_t sub_addr, u16_t mod_id, u8_t *status);
+int bt_mesh_cfg_mod_sub_del(uint16_t net_idx, uint16_t addr, uint16_t elem_addr,
+			    uint16_t sub_addr, uint16_t mod_id, uint8_t *status);
 
-int bt_mesh_cfg_mod_sub_add_vnd(u16_t net_idx, u16_t addr, u16_t elem_addr,
-				 u16_t sub_addr, u16_t mod_id, u16_t cid,
-				 u8_t *status);
+int bt_mesh_cfg_mod_sub_del_vnd(uint16_t net_idx, uint16_t addr, uint16_t elem_addr,
+				 uint16_t sub_addr, uint16_t mod_id, uint16_t cid,
+				 uint8_t *status);
 
-int bt_mesh_cfg_mod_sub_del(u16_t net_idx, u16_t addr, u16_t elem_addr,
-			    u16_t sub_addr, u16_t mod_id, u8_t *status);
+int bt_mesh_cfg_mod_sub_overwrite(uint16_t net_idx, uint16_t addr, uint16_t elem_addr,
+				  uint16_t sub_addr, uint16_t mod_id, uint8_t *status);
 
-int bt_mesh_cfg_mod_sub_del_vnd(u16_t net_idx, u16_t addr, u16_t elem_addr,
-				 u16_t sub_addr, u16_t mod_id, u16_t cid,
-				 u8_t *status);
+int bt_mesh_cfg_mod_sub_overwrite_vnd(uint16_t net_idx, uint16_t addr,
+				      uint16_t elem_addr, uint16_t sub_addr,
+				      uint16_t mod_id, uint16_t cid, uint8_t *status);
 
-int bt_mesh_cfg_mod_sub_overwrite(u16_t net_idx, u16_t addr, u16_t elem_addr,
-				  u16_t sub_addr, u16_t mod_id, u8_t *status);
+int bt_mesh_cfg_mod_sub_va_add(uint16_t net_idx, uint16_t addr, uint16_t elem_addr,
+			       const uint8_t label[16], uint16_t mod_id,
+			       uint16_t *virt_addr, uint8_t *status);
 
-int bt_mesh_cfg_mod_sub_overwrite_vnd(u16_t net_idx, u16_t addr,
-				      u16_t elem_addr, u16_t sub_addr,
-				      u16_t mod_id, u16_t cid, u8_t *status);
+int bt_mesh_cfg_mod_sub_va_add_vnd(uint16_t net_idx, uint16_t addr, uint16_t elem_addr,
+				   const uint8_t label[16], uint16_t mod_id,
+				   uint16_t cid, uint16_t *virt_addr, uint8_t *status);
 
-int bt_mesh_cfg_mod_sub_va_add(u16_t net_idx, u16_t addr, u16_t elem_addr,
-			       const u8_t label[16], u16_t mod_id,
-			       u16_t *virt_addr, u8_t *status);
+int bt_mesh_cfg_mod_sub_va_del(uint16_t net_idx, uint16_t addr, uint16_t elem_addr,
+			       const uint8_t label[16], uint16_t mod_id,
+			       uint16_t *virt_addr, uint8_t *status);
 
-int bt_mesh_cfg_mod_sub_va_add_vnd(u16_t net_idx, u16_t addr, u16_t elem_addr,
-				   const u8_t label[16], u16_t mod_id,
-				   u16_t cid, u16_t *virt_addr, u8_t *status);
+int bt_mesh_cfg_mod_sub_va_del_vnd(uint16_t net_idx, uint16_t addr, uint16_t elem_addr,
+				   const uint8_t label[16], uint16_t mod_id,
+				   uint16_t cid, uint16_t *virt_addr, uint8_t *status);
 
-int bt_mesh_cfg_mod_sub_va_del(u16_t net_idx, u16_t addr, u16_t elem_addr,
-			       const u8_t label[16], u16_t mod_id,
-			       u16_t *virt_addr, u8_t *status);
+int bt_mesh_cfg_mod_sub_va_overwrite(uint16_t net_idx, uint16_t addr,
+				     uint16_t elem_addr, const uint8_t label[16],
+				     uint16_t mod_id, uint16_t *virt_addr,
+				     uint8_t *status);
 
-int bt_mesh_cfg_mod_sub_va_del_vnd(u16_t net_idx, u16_t addr, u16_t elem_addr,
-				   const u8_t label[16], u16_t mod_id,
-				   u16_t cid, u16_t *virt_addr, u8_t *status);
+int bt_mesh_cfg_mod_sub_va_overwrite_vnd(uint16_t net_idx, uint16_t addr,
+					 uint16_t elem_addr, const uint8_t label[16],
+					 uint16_t mod_id, uint16_t cid,
+					 uint16_t *virt_addr, uint8_t *status);
 
-int bt_mesh_cfg_mod_sub_va_overwrite(u16_t net_idx, u16_t addr,
-				     u16_t elem_addr, const u8_t label[16],
-				     u16_t mod_id, u16_t *virt_addr,
-				     u8_t *status);
+int bt_mesh_cfg_mod_sub_get(uint16_t net_idx, uint16_t addr, uint16_t elem_addr,
+			    uint16_t mod_id, uint8_t *status, uint16_t *subs,
+			    size_t *sub_cnt);
 
-int bt_mesh_cfg_mod_sub_va_overwrite_vnd(u16_t net_idx, u16_t addr,
-					 u16_t elem_addr, const u8_t label[16],
-					 u16_t mod_id, u16_t cid,
-					 u16_t *virt_addr, u8_t *status);
+int bt_mesh_cfg_mod_sub_get_vnd(uint16_t net_idx, uint16_t addr, uint16_t elem_addr,
+				uint16_t mod_id, uint16_t cid, uint8_t *status,
+				uint16_t *subs, size_t *sub_cnt);
 
 struct bt_mesh_cfg_hb_sub {
-	u16_t src;
-	u16_t dst;
-	u8_t  period;
-	u8_t  count;
-	u8_t  min;
-	u8_t  max;
+	uint16_t src;
+	uint16_t dst;
+	uint8_t  period;
+	uint8_t  count;
+	uint8_t  min;
+	uint8_t  max;
 };
 
-int bt_mesh_cfg_hb_sub_set(u16_t net_idx, u16_t addr,
-			   struct bt_mesh_cfg_hb_sub *sub, u8_t *status);
+int bt_mesh_cfg_hb_sub_set(uint16_t net_idx, uint16_t addr,
+			   struct bt_mesh_cfg_hb_sub *sub, uint8_t *status);
 
-int bt_mesh_cfg_hb_sub_get(u16_t net_idx, u16_t addr,
-			   struct bt_mesh_cfg_hb_sub *sub, u8_t *status);
+int bt_mesh_cfg_hb_sub_get(uint16_t net_idx, uint16_t addr,
+			   struct bt_mesh_cfg_hb_sub *sub, uint8_t *status);
 
 struct bt_mesh_cfg_hb_pub {
-	u16_t dst;
-	u8_t  count;
-	u8_t  period;
-	u8_t  ttl;
-	u16_t feat;
-	u16_t net_idx;
+	uint16_t dst;
+	uint8_t  count;
+	uint8_t  period;
+	uint8_t  ttl;
+	uint16_t feat;
+	uint16_t net_idx;
 };
 
-int bt_mesh_cfg_hb_pub_set(u16_t net_idx, u16_t addr,
-			   const struct bt_mesh_cfg_hb_pub *pub, u8_t *status);
+int bt_mesh_cfg_hb_pub_set(uint16_t net_idx, uint16_t addr,
+			   const struct bt_mesh_cfg_hb_pub *pub, uint8_t *status);
 
-int bt_mesh_cfg_hb_pub_get(u16_t net_idx, u16_t addr,
-			   struct bt_mesh_cfg_hb_pub *pub, u8_t *status);
+int bt_mesh_cfg_hb_pub_get(uint16_t net_idx, uint16_t addr,
+			   struct bt_mesh_cfg_hb_pub *pub, uint8_t *status);
 
-s32_t bt_mesh_cfg_cli_timeout_get(void);
-void bt_mesh_cfg_cli_timeout_set(s32_t timeout);
+int32_t bt_mesh_cfg_cli_timeout_get(void);
+void bt_mesh_cfg_cli_timeout_set(int32_t timeout);
 
+#ifdef __cplusplus
+}
+#endif
 /**
  * @}
  */

nimble/host/mesh/include/mesh/cfg_srv.h

@@ -17,51 +17,21 @@
  * @{
  */
 
-/** Mesh Configuration Server Model Context */
-struct bt_mesh_cfg_srv {
-	struct bt_mesh_model *model;
+#ifdef __cplusplus
+extern "C" {
+#endif
 
-	u8_t net_transmit;         /* Network Transmit state */
-	u8_t relay;                /* Relay Mode state */
-	u8_t relay_retransmit;     /* Relay Retransmit state */
-	u8_t beacon;               /* Secure Network Beacon state */
-	u8_t gatt_proxy;           /* GATT Proxy state */
-	u8_t frnd;                 /* Friend state */
-	u8_t default_ttl;          /* Default TTL */
-
-	/* Heartbeat Publication */
-	struct bt_mesh_hb_pub {
-		struct k_delayed_work timer;
-
-		u16_t dst;
-		u16_t count;
-		u8_t  period;
-		u8_t  ttl;
-		u16_t feat;
-		u16_t net_idx;
-	} hb_pub;
-
-	/* Heartbeat Subscription */
-	struct bt_mesh_hb_sub {
-		s64_t  expiry;
-
-		u16_t src;
-		u16_t dst;
-		u16_t count;
-		u8_t  min_hops;
-		u8_t  max_hops;
-
-		/* Optional subscription tracking function */
-		void (*func)(u8_t hops, u16_t feat);
-	} hb_sub;
-};
 
 extern const struct bt_mesh_model_op bt_mesh_cfg_srv_op[];
+extern const struct bt_mesh_model_cb bt_mesh_cfg_srv_cb;
 
-#define BT_MESH_MODEL_CFG_SRV(srv_data)                                      \
-		BT_MESH_MODEL(BT_MESH_MODEL_ID_CFG_SRV,                      \
-			      bt_mesh_cfg_srv_op, NULL, srv_data)
+#define BT_MESH_MODEL_CFG_SRV                                  \
+	BT_MESH_MODEL_CB(BT_MESH_MODEL_ID_CFG_SRV, bt_mesh_cfg_srv_op, NULL,   \
+			 		 NULL, &bt_mesh_cfg_srv_cb)
 
+#ifdef __cplusplus
+}
+#endif
 
 /**
  * @}

nimble/host/mesh/include/mesh/glue.h

@@ -24,6 +24,8 @@
 #include <errno.h>
 
 #include "syscfg/syscfg.h"
+#include "logcfg/logcfg.h"
+#include "modlog/modlog.h"
 #include "nimble/nimble_npl.h"
 
 #include "os/os_mbuf.h"
@@ -45,14 +47,9 @@
 #include "config/config.h"
 #endif
 
-#define u8_t    uint8_t
-#define s8_t    int8_t
-#define u16_t   uint16_t
-#define s16_t   int16_t
-#define u32_t   uint32_t
-#define u64_t   uint64_t
-#define s64_t   int64_t
-#define s32_t   int32_t
+#ifdef __cplusplus
+extern "C" {
+#endif
 
 /** @brief Helper to declare elements of bt_data arrays
  *
@@ -67,7 +64,7 @@
     { \
         .type = (_type), \
         .data_len = (_data_len), \
-        .data = (const u8_t *)(_data), \
+        .data = (const uint8_t *)(_data), \
     }
 
 /** @brief Helper to declare elements of bt_data arrays
@@ -79,8 +76,8 @@
  *  @param _bytes Variable number of single-byte parameters
  */
 #define BT_DATA_BYTES(_type, _bytes...) \
-    BT_DATA(_type, ((u8_t []) { _bytes }), \
-        sizeof((u8_t []) { _bytes }))
+    BT_DATA(_type, ((uint8_t []) { _bytes }), \
+        sizeof((uint8_t []) { _bytes }))
 
 /* EIR/AD data type definitions */
 #define BT_DATA_FLAGS                   0x01 /* AD flags */
@@ -113,9 +110,13 @@
 
 #define sys_put_be16(a,b) put_be16(b, a)
 #define sys_put_le16(a,b) put_le16(b, a)
+#define sys_put_le24(a,b) put_le24(b, a)
+#define sys_put_be24(a,b) put_be24(b, a)
 #define sys_put_be32(a,b) put_be32(b, a)
 #define sys_get_be16(a) get_be16(a)
+#define sys_get_be24(a) get_be24(a)
 #define sys_get_le16(a) get_le16(a)
+#define sys_get_le24(a) get_le24(a)
 #define sys_get_be32(a) get_be32(a)
 #define sys_cpu_to_be16(a) htobe16(a)
 #define sys_cpu_to_be32(a) htobe32(a)
@@ -123,7 +124,10 @@
 #define sys_be16_to_cpu(a) be16toh(a)
 #define sys_le16_to_cpu(a) le16toh(a)
 
+#ifndef ARRAY_SIZE
 #define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
+#endif
+
 #define CODE_UNREACHABLE __builtin_unreachable()
 #define __ASSERT(code, str) \
     do {                          \
@@ -161,13 +165,19 @@
 #define BT_GAP_ADV_SLOW_INT_MIN                 0x0640  /* 1 s      */
 #define BT_GAP_ADV_SLOW_INT_MAX                 0x0780  /* 1.2 s    */
 
-#define BT_DBG(fmt, ...)    \
-    if (BT_DBG_ENABLED) { \
-        BLE_HS_LOG(DEBUG, "%s: " fmt "\n", __func__, ## __VA_ARGS__); \
-    }
-#define BT_INFO(fmt, ...)   BLE_HS_LOG(INFO, "%s: " fmt "\n", __func__, ## __VA_ARGS__);
-#define BT_WARN(fmt, ...)   BLE_HS_LOG(WARN, "%s: " fmt "\n", __func__, ## __VA_ARGS__);
-#define BT_ERR(fmt, ...)    BLE_HS_LOG(ERROR, "%s: " fmt "\n", __func__, ## __VA_ARGS__);
+#ifndef MESH_LOG_MODULE
+#define MESH_LOG_MODULE BLE_MESH_LOG
+#endif
+
+#define CAT(a, ...) PRIMITIVE_CAT(a, __VA_ARGS__)
+#define PRIMITIVE_CAT(a, ...) a ## __VA_ARGS__
+
+#define BLE_MESH_LOG(lvl, ...) CAT(MESH_LOG_MODULE, CAT(_, lvl))(__VA_ARGS__)
+
+#define BT_DBG(fmt, ...)    BLE_MESH_LOG(DEBUG, "%s: " fmt "\n", __func__, ## __VA_ARGS__);
+#define BT_INFO(fmt, ...)   BLE_MESH_LOG(INFO, "%s: " fmt "\n", __func__, ## __VA_ARGS__);
+#define BT_WARN(fmt, ...)   BLE_MESH_LOG(WARN, "%s: " fmt "\n", __func__, ## __VA_ARGS__);
+#define BT_ERR(fmt, ...)    BLE_MESH_LOG(ERROR, "%s: " fmt "\n", __func__, ## __VA_ARGS__);
 #define BT_GATT_ERR(_att_err)   (-(_att_err))
 
 typedef ble_addr_t bt_addr_le_t;
@@ -181,9 +191,9 @@ typedef ble_addr_t bt_addr_le_t;
 
 struct net_buf_simple_state {
     /** Offset of the data pointer from the beginning of the storage */
-    u16_t offset;
+    uint16_t offset;
     /** Length of data */
-    u16_t len;
+    uint16_t len;
 };
 
 static inline struct os_mbuf * NET_BUF_SIMPLE(uint16_t size)
@@ -199,6 +209,20 @@ static inline struct os_mbuf * NET_BUF_SIMPLE(uint16_t size)
 #define K_NO_WAIT (0)
 #define K_FOREVER (-1)
 
+#if MYNEWT_VAL(BLE_EXT_ADV)
+#define BT_MESH_ADV_INST     (MYNEWT_VAL(BLE_MULTI_ADV_INSTANCES))
+
+#if MYNEWT_VAL(BLE_MESH_PROXY)
+/* Note that BLE_MULTI_ADV_INSTANCES contains number of additional instances.
+ * Instance 0 is always there
+ */
+#if MYNEWT_VAL(BLE_MULTI_ADV_INSTANCES) < 1
+#error "Mesh needs at least BLE_MULTI_ADV_INSTANCES set to 1"
+#endif
+#define BT_MESH_ADV_GATT_INST     (MYNEWT_VAL(BLE_MULTI_ADV_INSTANCES) - 1)
+#endif /* BLE_MESH_PROXY */
+#endif /* BLE_EXT_ADV */
+
 /* This is by purpose */
 static inline void net_buf_simple_init(struct os_mbuf *buf,
                                        size_t reserve_head)
@@ -209,6 +233,14 @@ static inline void net_buf_simple_init(struct os_mbuf *buf,
     buf->om_len = 0;
 }
 
+#define net_buf_simple_init_with_data(buf, data, size)  \
+    os_mbuf_copyinto(buf, 0, data, size);
+
+static inline void net_buf_simple_reset(struct os_mbuf *om)
+{
+    net_buf_simple_init(om, 0);
+}
+
 void net_buf_put(struct ble_npl_eventq *fifo, struct os_mbuf *buf);
 void * net_buf_ref(struct os_mbuf *om);
 void net_buf_unref(struct os_mbuf *om);
@@ -219,17 +251,20 @@ uint32_t net_buf_simple_pull_le32(struct os_mbuf *om);
 uint8_t net_buf_simple_pull_u8(struct os_mbuf *om);
 void net_buf_simple_add_le16(struct os_mbuf *om, uint16_t val);
 void net_buf_simple_add_be16(struct os_mbuf *om, uint16_t val);
+void net_buf_simple_add_le24(struct os_mbuf *om, uint32_t val);
 void net_buf_simple_add_u8(struct os_mbuf *om, uint8_t val);
 void net_buf_simple_add_be32(struct os_mbuf *om, uint32_t val);
 void net_buf_simple_add_le32(struct os_mbuf *om, uint32_t val);
 void net_buf_add_zeros(struct os_mbuf *om, uint8_t len);
 void net_buf_simple_push_le16(struct os_mbuf *om, uint16_t val);
 void net_buf_simple_push_be16(struct os_mbuf *om, uint16_t val);
+void net_buf_simple_push_be24(struct os_mbuf *om, uint32_t val);
 void net_buf_simple_push_u8(struct os_mbuf *om, uint8_t val);
 void *net_buf_simple_pull(struct os_mbuf *om, uint8_t len);
+void *net_buf_simple_pull_mem(struct os_mbuf *om, uint8_t len);
 void *net_buf_simple_add(struct os_mbuf *om, uint8_t len);
 bool k_fifo_is_empty(struct ble_npl_eventq *q);
-void *net_buf_get(struct ble_npl_eventq *fifo,s32_t t);
+void *net_buf_get(struct ble_npl_eventq *fifo,int32_t t);
 uint8_t *net_buf_simple_push(struct os_mbuf *om, uint8_t len);
 void net_buf_reserve(struct os_mbuf *om, size_t reserve);
 
@@ -241,18 +276,24 @@ void net_buf_reserve(struct os_mbuf *om, size_t reserve);
 #define net_buf_clone(a, b) os_mbuf_dup(a)
 #define net_buf_add_be32(a, b) net_buf_simple_add_be32(a, b)
 #define net_buf_add_be16(a, b) net_buf_simple_add_be16(a, b)
+#define net_buf_pull(a, b) net_buf_simple_pull_mem(a, b)
+#define net_buf_pull_mem(a, b) net_buf_simple_pull_mem(a, b)
+#define net_buf_pull_u8(a) net_buf_simple_pull_u8(a)
+#define net_buf_pull_be16(a) net_buf_simple_pull_be16(a)
+#define net_buf_skip(a, b) net_buf_simple_pull_mem(a, b)
 
 #define BT_GATT_CCC_NOTIFY BLE_GATT_CHR_PROP_NOTIFY
-#define bt_gatt_attr ble_gatt_attr
 
+#define MIN(a, b) (((a) < (b)) ? (a) : (b))
+#define MAX(a, b) (((a) > (b)) ? (a) : (b))
 /** Description of different data types that can be encoded into
   * advertising data. Used to form arrays that are passed to the
   * bt_le_adv_start() function.
   */
 struct bt_data {
-    u8_t type;
-    u8_t data_len;
-    const u8_t *data;
+    uint8_t type;
+    uint8_t data_len;
+    const uint8_t *data;
 };
 
 struct bt_pub_key_cb {
@@ -264,18 +305,24 @@ struct bt_pub_key_cb {
      *
      *  @param key The local public key, or NULL in case of no key.
      */
-    void (*func)(const u8_t key[64]);
+    void (*func)(const uint8_t key[64]);
 
     struct bt_pub_key_cb *_next;
 };
 
-typedef void (*bt_dh_key_cb_t)(const u8_t key[32]);
-int bt_dh_key_gen(const u8_t remote_pk[64], bt_dh_key_cb_t cb);
+typedef void (*bt_dh_key_cb_t)(const uint8_t key[32]);
+int bt_dh_key_gen(const uint8_t remote_pk[64], bt_dh_key_cb_t cb);
 int bt_pub_key_gen(struct bt_pub_key_cb *new_cb);
 uint8_t *bt_pub_key_get(void);
 int bt_rand(void *buf, size_t len);
 const char * bt_hex(const void *buf, size_t len);
 int bt_encrypt_be(const uint8_t *key, const uint8_t *plaintext, uint8_t *enc_data);
+int bt_ccm_decrypt(const uint8_t key[16], uint8_t nonce[13], const uint8_t *enc_data,
+		   size_t len, const uint8_t *aad, size_t aad_len,
+		   uint8_t *plaintext, size_t mic_size);
+int bt_ccm_encrypt(const uint8_t key[16], uint8_t nonce[13], const uint8_t *enc_data,
+		   size_t len, const uint8_t *aad, size_t aad_len,
+		   uint8_t *plaintext, size_t mic_size);
 void bt_mesh_register_gatt(void);
 int bt_le_adv_start(const struct ble_gap_adv_params *param,
                     const struct bt_data *ad, size_t ad_len,
@@ -289,9 +336,10 @@ struct k_delayed_work {
 void k_work_init(struct ble_npl_callout *work, ble_npl_event_fn handler);
 void k_delayed_work_init(struct k_delayed_work *w, ble_npl_event_fn *f);
 void k_delayed_work_cancel(struct k_delayed_work *w);
+bool k_delayed_work_pending(struct k_delayed_work *w);
 void k_delayed_work_submit(struct k_delayed_work *w, uint32_t ms);
 int64_t k_uptime_get(void);
-u32_t k_uptime_get_32(void);
+uint32_t k_uptime_get_32(void);
 void k_sleep(int32_t duration);
 void k_work_submit(struct ble_npl_callout *w);
 void k_work_add_arg(struct ble_npl_callout *w, void *arg);
@@ -321,18 +369,18 @@ static inline void sys_memcpy_swap(void *dst, const void *src, size_t length)
     src += length - 1;
 
     for (; length > 0; length--) {
-        *((u8_t *)dst++) = *((u8_t *)src--);
+        *((uint8_t *)dst++) = *((uint8_t *)src--);
     }
 }
 
 #define popcount(x) __builtin_popcount(x)
 
-static inline unsigned int find_lsb_set(u32_t op)
+static inline unsigned int find_lsb_set(uint32_t op)
 {
     return __builtin_ffs(op);
 }
 
-static inline unsigned int find_msb_set(u32_t op)
+static inline unsigned int find_msb_set(uint32_t op)
 {
     if (!op)
         return 0;
@@ -340,40 +388,61 @@ static inline unsigned int find_msb_set(u32_t op)
     return 32 - __builtin_clz(op);
 }
 
-#define CONFIG_BT_MESH_FRIEND               BLE_MESH_FRIEND
-#define CONFIG_BT_MESH_GATT_PROXY           BLE_MESH_GATT_PROXY
-#define CONFIG_BT_MESH_IV_UPDATE_TEST       BLE_MESH_IV_UPDATE_TEST
-#define CONFIG_BT_MESH_LOW_POWER            BLE_MESH_LOW_POWER
-#define CONFIG_BT_MESH_LPN_AUTO             BLE_MESH_LPN_AUTO
-#define CONFIG_BT_MESH_LPN_ESTABLISHMENT    BLE_MESH_LPN_ESTABLISHMENT
-#define CONFIG_BT_MESH_PB_ADV               BLE_MESH_PB_ADV
-#define CONFIG_BT_MESH_PB_GATT              BLE_MESH_PB_GATT
-#define CONFIG_BT_MESH_PROV                 BLE_MESH_PROV
-#define CONFIG_BT_TESTING                   BLE_MESH_TESTING
-#define CONFIG_BT_SETTINGS                  BLE_MESH_SETTINGS
-#define CONFIG_SETTINGS                     BLE_MESH_SETTINGS
-#define BT_SETTINGS                         BLE_MESH_SETTINGS
+#define CONFIG_BT_MESH_FRIEND                 BLE_MESH_FRIEND
+#define CONFIG_BT_MESH_GATT_PROXY             BLE_MESH_GATT_PROXY
+#define CONFIG_BT_MESH_IV_UPDATE_TEST         BLE_MESH_IV_UPDATE_TEST
+#define CONFIG_BT_MESH_LOW_POWER              BLE_MESH_LOW_POWER
+#define CONFIG_BT_MESH_LPN_SUB_ALL_NODES_ADDR BLE_MESH_LPN_SUB_ALL_NODES_ADDR
+#define CONFIG_BT_MESH_LPN_AUTO               BLE_MESH_LPN_AUTO
+#define CONFIG_BT_MESH_LPN_ESTABLISHMENT      BLE_MESH_LPN_ESTABLISHMENT
+#define CONFIG_BT_MESH_PB_ADV                 BLE_MESH_PB_ADV
+#define CONFIG_BT_MESH_PB_GATT                BLE_MESH_PB_GATT
+#define CONFIG_BT_MESH_PROV                   BLE_MESH_PROV
+#define CONFIG_BT_MESH_PROXY                  BLE_MESH_PROXY
+#define CONFIG_BT_TESTING                     BLE_MESH_TESTING
+#define CONFIG_BT_SETTINGS                    BLE_MESH_SETTINGS
+#define CONFIG_SETTINGS                       BLE_MESH_SETTINGS
+#define CONFIG_BT_MESH_PROVISIONER            BLE_MESH_PROVISIONER
+#define CONFIG_BT_MESH_PROV_DEVICE            BLE_MESH_PROV_DEVICE
+#define CONFIG_BT_MESH_CDB                    BLE_MESH_CDB
 
 /* Above flags are used with IS_ENABLED macro */
 #define IS_ENABLED(config) MYNEWT_VAL(config)
 
-#define CONFIG_BT_MESH_LPN_GROUPS           MYNEWT_VAL(BLE_MESH_LPN_GROUPS)
-#define CONFIG_BT_MESH_ADV_BUF_COUNT        MYNEWT_VAL(BLE_MESH_ADV_BUF_COUNT)
-#define CONFIG_BT_MESH_FRIEND_QUEUE_SIZE    MYNEWT_VAL(BLE_MESH_FRIEND_QUEUE_SIZE)
-#define CONFIG_BT_MESH_FRIEND_RECV_WIN      MYNEWT_VAL(BLE_MESH_FRIEND_RECV_WIN)
-#define CONFIG_BT_MESH_LPN_POLL_TIMEOUT     MYNEWT_VAL(BLE_MESH_LPN_POLL_TIMEOUT)
-#define CONFIG_BT_MESH_MODEL_GROUP_COUNT    MYNEWT_VAL(BLE_MESH_MODEL_GROUP_COUNT)
-#define CONFIG_BT_MESH_MODEL_KEY_COUNT      MYNEWT_VAL(BLE_MESH_MODEL_KEY_COUNT)
-#define CONFIG_BT_MESH_NODE_ID_TIMEOUT      MYNEWT_VAL(BLE_MESH_NODE_ID_TIMEOUT)
-#define CONFIG_BT_MAX_CONN                  MYNEWT_VAL(BLE_MAX_CONNECTIONS)
-#define CONFIG_BT_MESH_SEQ_STORE_RATE       MYNEWT_VAL(BLE_MESH_SEQ_STORE_RATE)
-#define CONFIG_BT_MESH_RPL_STORE_TIMEOUT    MYNEWT_VAL(BLE_MESH_RPL_STORE_TIMEOUT)
-#define CONFIG_BT_MESH_APP_KEY_COUNT        MYNEWT_VAL(BLE_MESH_APP_KEY_COUNT)
-#define CONFIG_BT_MESH_SUBNET_COUNT         MYNEWT_VAL(BLE_MESH_SUBNET_COUNT)
-#define CONFIG_BT_MESH_STORE_TIMEOUT        MYNEWT_VAL(BLE_MESH_STORE_TIMEOUT)
-#define CONFIG_BT_MESH_IVU_DIVIDER          MYNEWT_VAL(BLE_MESH_IVU_DIVIDER)
-#define CONFIG_BT_DEVICE_NAME               MYNEWT_VAL(BLE_MESH_DEVICE_NAME)
-#define CONFIG_BT_MESH_TX_SEG_MAX           MYNEWT_VAL(BLE_MESH_TX_SEG_MAX)
+#define CONFIG_BT_MESH_LPN_GROUPS                MYNEWT_VAL(BLE_MESH_LPN_GROUPS)
+#define CONFIG_BT_MESH_ADV_BUF_COUNT             MYNEWT_VAL(BLE_MESH_ADV_BUF_COUNT)
+#define CONFIG_BT_MESH_SEG_BUFS                  MYNEWT_VAL(BLE_MESH_SEG_BUFS)
+#define CONFIG_BT_MESH_FRIEND_QUEUE_SIZE         MYNEWT_VAL(BLE_MESH_FRIEND_QUEUE_SIZE)
+#define CONFIG_BT_MESH_FRIEND_RECV_WIN           MYNEWT_VAL(BLE_MESH_FRIEND_RECV_WIN)
+#define CONFIG_BT_MESH_LPN_POLL_TIMEOUT          MYNEWT_VAL(BLE_MESH_LPN_POLL_TIMEOUT)
+#define CONFIG_BT_MESH_MODEL_GROUP_COUNT         MYNEWT_VAL(BLE_MESH_MODEL_GROUP_COUNT)
+#define CONFIG_BT_MESH_MODEL_KEY_COUNT           MYNEWT_VAL(BLE_MESH_MODEL_KEY_COUNT)
+#define CONFIG_BT_MESH_NODE_ID_TIMEOUT           MYNEWT_VAL(BLE_MESH_NODE_ID_TIMEOUT)
+#define CONFIG_BT_MAX_CONN                       MYNEWT_VAL(BLE_MAX_CONNECTIONS)
+#define CONFIG_BT_MESH_SEQ_STORE_RATE            MYNEWT_VAL(BLE_MESH_SEQ_STORE_RATE)
+#define CONFIG_BT_MESH_RPL_STORE_TIMEOUT         MYNEWT_VAL(BLE_MESH_RPL_STORE_TIMEOUT)
+#define CONFIG_BT_MESH_APP_KEY_COUNT             MYNEWT_VAL(BLE_MESH_APP_KEY_COUNT)
+#define CONFIG_BT_MESH_SUBNET_COUNT              MYNEWT_VAL(BLE_MESH_SUBNET_COUNT)
+#define CONFIG_BT_MESH_STORE_TIMEOUT             MYNEWT_VAL(BLE_MESH_STORE_TIMEOUT)
+#define CONFIG_BT_MESH_IVU_DIVIDER               MYNEWT_VAL(BLE_MESH_IVU_DIVIDER)
+#define CONFIG_BT_DEVICE_NAME                    MYNEWT_VAL(BLE_MESH_DEVICE_NAME)
+#define CONFIG_BT_RX_SEG_MAX                     MYNEWT_VAL(BLE_MESH_RX_SEG_MAX)
+#define CONFIG_BT_MESH_TX_SEG_MAX                MYNEWT_VAL(BLE_MESH_TX_SEG_MAX)
+#define CONFIG_BT_MESH_RX_SEG_MAX                MYNEWT_VAL(BLE_MESH_RX_SEG_MAX)
+#define CONFIG_BT_MESH_RX_SEG_MSG_COUNT          MYNEWT_VAL(BLE_MESH_RX_SEG_MSG_COUNT)
+#define CONFIG_BT_MESH_LABEL_COUNT               MYNEWT_VAL(BLE_MESH_LABEL_COUNT)
+#define CONFIG_BT_MESH_NODE_COUNT                MYNEWT_VAL(BLE_MESH_CDB_NODE_COUNT)
+#define CONFIG_BT_GATT_PROXY_ENABLED             MYNEWT_VAL(BLE_MESH_GATT_PROXY_ENABLED)
+#define CONFIG_BT_MESH_DEFAULT_TTL               MYNEWT_VAL(BLE_MESH_DEFAULT_TTL)
+#define CONFIG_BT_MESH_NETWORK_TRANSMIT_COUNT    MYNEWT_VAL(BLE_MESH_NETWORK_TRANSMIT_COUNT)
+#define CONFIG_BT_MESH_NETWORK_TRANSMIT_INTERVAL MYNEWT_VAL(BLE_MESH_NETWORK_TRANSMIT_INTERVAL)
+#define CONFIG_BT_MESH_RELAY_ENABLED             MYNEWT_VAL(BLE_MESH_RELAY_ENABLED)
+#define CONFIG_BT_MESH_RELAY_RETRANSMIT_INTERVAL MYNEWT_VAL(BLE_MESH_RELAY_RETRANSMIT_INTERVAL)
+#define CONFIG_BT_MESH_BEACON_ENABLED            MYNEWT_VAL(BLE_MESH_BEACON_ENABLED)
+#define CONFIG_BT_MESH_FRIEND_ENABLED            MYNEWT_VAL(BLE_MESH_FRIEND_ENABLED)
+#define CONFIG_BT_MESH_RELAY                     MYNEWT_VAL(BLE_MESH_RELAY)
+#define CONFIG_BT_MESH_RELAY_RETRANSMIT_COUNT    MYNEWT_VAL(BLE_MESH_RELAY_RETRANSMIT_COUNT)
+#define CONFIG_BT_MESH_GATT_PROXY_ENABLED        MYNEWT_VAL(BLE_MESH_GATT_PROXY_ENABLED)
 
 #define printk console_printf
 
@@ -389,7 +458,7 @@ static inline void k_sem_init(struct k_sem *sem, unsigned int initial_count,
 	ble_npl_sem_init(sem, initial_count);
 }
 
-static inline int k_sem_take(struct k_sem *sem, s32_t timeout)
+static inline int k_sem_take(struct k_sem *sem, int32_t timeout)
 {
 	uint32_t ticks;
 
@@ -411,8 +480,8 @@ static inline void k_sem_give(struct k_sem *sem)
 static inline int net_buf_id(struct os_mbuf *buf)
 {
 	struct os_mbuf_pool *pool = buf->om_omp;
-	u8_t *pool_start = (u8_t *)pool->omp_pool->mp_membuf_addr;
-	u8_t *buf_ptr = (u8_t *)buf;
+	uint8_t *pool_start = (uint8_t *)pool->omp_pool->mp_membuf_addr;
+	uint8_t *buf_ptr = (uint8_t *)buf;
 
 	return (buf_ptr - pool_start) / BUF_SIZE(pool);
 }
@@ -439,7 +508,8 @@ void net_buf_slist_merge_slist(struct net_buf_slist_t *list,
 
 #define settings_load conf_load
 int settings_bytes_from_str(char *val_str, void *vp, int *len);
-char *settings_str_from_bytes(void *vp, int vp_len, char *buf, int buf_len);
+char *settings_str_from_bytes(const void *vp, int vp_len,
+			      char *buf, int buf_len);
 
 #define snprintk snprintf
 #define BT_SETTINGS_SIZE(in_size) ((((((in_size) - 1) / 3) * 4) + 4) + 1)
@@ -457,4 +527,48 @@ settings_load(void)
 
 #define BUILD_ASSERT(cond) _Static_assert(cond, "")
 
+
+/* Memory slabs/blocks */
+
+/** Memory slab structure */
+struct k_mem_slab {
+    /**
+     * _wait_q_t is not required now, as we don't implement zephyr timeouts -
+     * if slab couldn't be allocated, we simply return error  
+     */
+    uint32_t num_blocks; /** number of memory blocks available for allocation */
+    size_t block_size; /** size of single block */
+	/**
+     * buffer for blocks - must be alligned to N-byte, where N is a power of 2.
+     * Minimal size of buffer is num_blocks * block_size
+     */
+    char *buffer;
+	char *free_list; /** list of free memory blocks */
+	uint32_t num_used; /** count of used memory blocks */
+};
+
+struct k_mem_block_id {
+	uint32_t pool : 8;
+	uint32_t level : 4;
+	uint32_t block : 20;
+};
+
+struct k_mem_block {
+	void *data;
+	struct k_mem_block_id id;
+};
+
+extern void k_mem_slab_free(struct k_mem_slab *slab, void **mem);
+extern int k_mem_slab_alloc(struct k_mem_slab *slab, void **mem);
+static inline uint32_t k_mem_slab_num_free_get(struct k_mem_slab *slab)
+{
+	return slab->num_blocks - slab->num_used;
+}
+
+int create_free_list(struct k_mem_slab *slab);
+
+#ifdef __cplusplus
+}
 #endif
+
+#endif /* _MESH_GLUE_ */

nimble/host/mesh/include/mesh/health_cli.h

@@ -17,53 +17,60 @@
  * @{
  */
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 /** Mesh Health Client Model Context */
 struct bt_mesh_health_cli {
 	struct bt_mesh_model *model;
 
-	void (*current_status)(struct bt_mesh_health_cli *cli, u16_t addr,
-			       u8_t test_id, u16_t cid, u8_t *faults,
+	void (*current_status)(struct bt_mesh_health_cli *cli, uint16_t addr,
+			       uint8_t test_id, uint16_t cid, uint8_t *faults,
 			       size_t fault_count);
 
 	struct k_sem          op_sync;
-	u32_t                 op_pending;
+	uint32_t                 op_pending;
 	void                 *op_param;
 };
 
 extern const struct bt_mesh_model_op bt_mesh_health_cli_op[];
+extern const struct bt_mesh_model_cb bt_mesh_health_cli_cb;
 
-#define BT_MESH_MODEL_HEALTH_CLI(cli_data)                                   \
-		BT_MESH_MODEL(BT_MESH_MODEL_ID_HEALTH_CLI,                   \
-			      bt_mesh_health_cli_op, NULL, cli_data)
+#define BT_MESH_MODEL_HEALTH_CLI(cli_data)                                     \
+	BT_MESH_MODEL_CB(BT_MESH_MODEL_ID_HEALTH_CLI, bt_mesh_health_cli_op,   \
+			 NULL, cli_data, &bt_mesh_health_cli_cb)
 
 int bt_mesh_health_cli_set(struct bt_mesh_model *model);
 
-int bt_mesh_health_fault_get(u16_t net_idx, u16_t addr, u16_t app_idx,
-			     u16_t cid, u8_t *test_id, u8_t *faults,
-			     size_t *fault_count);
+int bt_mesh_health_fault_get(uint16_t addr, uint16_t app_idx, uint16_t cid,
+							 uint8_t *test_id, uint8_t *faults,
+							 size_t *fault_count);
+
+int bt_mesh_health_fault_clear(uint16_t addr, uint16_t app_idx, uint16_t cid,
+							   uint8_t *test_id, uint8_t *faults,
+							   size_t *fault_count);
 
-int bt_mesh_health_fault_clear(u16_t net_idx, u16_t addr, u16_t app_idx,
-			       u16_t cid, u8_t *test_id, u8_t *faults,
-			       size_t *fault_count);
+int bt_mesh_health_fault_test(uint16_t addr, uint16_t app_idx, uint16_t cid,
+							  uint8_t test_id, uint8_t *faults,
+							  size_t *fault_count);
 
-int bt_mesh_health_fault_test(u16_t net_idx, u16_t addr, u16_t app_idx,
-			      u16_t cid, u8_t test_id, u8_t *faults,
-			      size_t *fault_count);
+int bt_mesh_health_period_get(uint16_t addr, uint16_t app_idx, uint8_t *divisor);
 
-int bt_mesh_health_period_get(u16_t net_idx, u16_t addr, u16_t app_idx,
-			      u8_t *divisor);
+int bt_mesh_health_period_set(uint16_t addr, uint16_t app_idx, uint8_t divisor,
+							  uint8_t *updated_divisor);
 
-int bt_mesh_health_period_set(u16_t net_idx, u16_t addr, u16_t app_idx,
-			      u8_t divisor, u8_t *updated_divisor);
+int bt_mesh_health_attention_get(uint16_t addr, uint16_t app_idx, uint8_t *attention);
 
-int bt_mesh_health_attention_get(u16_t net_idx, u16_t addr, u16_t app_idx,
-				 u8_t *attention);
+int bt_mesh_health_attention_set(uint16_t addr, uint16_t app_idx, uint8_t attention,
+								 uint8_t *updated_attention);
 
-int bt_mesh_health_attention_set(u16_t net_idx, u16_t addr, u16_t app_idx,
-				 u8_t attention, u8_t *updated_attention);
+int32_t bt_mesh_health_cli_timeout_get(void);
+void bt_mesh_health_cli_timeout_set(int32_t timeout);
 
-s32_t bt_mesh_health_cli_timeout_get(void);
-void bt_mesh_health_cli_timeout_set(s32_t timeout);
+#ifdef __cplusplus
+}
+#endif
 
 /**
  * @}

nimble/host/mesh/include/mesh/health_srv.h

@@ -17,23 +17,27 @@
  * @{
  */
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 struct bt_mesh_health_srv_cb {
 	/* Fetch current faults */
-	int (*fault_get_cur)(struct bt_mesh_model *model, u8_t *test_id,
-			     u16_t *company_id, u8_t *faults,
-			     u8_t *fault_count);
+	int (*fault_get_cur)(struct bt_mesh_model *model, uint8_t *test_id,
+			     uint16_t *company_id, uint8_t *faults,
+			     uint8_t *fault_count);
 
 	/* Fetch registered faults */
-	int (*fault_get_reg)(struct bt_mesh_model *model, u16_t company_id,
-			     u8_t *test_id, u8_t *faults,
-			     u8_t *fault_count);
+	int (*fault_get_reg)(struct bt_mesh_model *model, uint16_t company_id,
+			     uint8_t *test_id, uint8_t *faults,
+			     uint8_t *fault_count);
 
 	/* Clear registered faults */
-	int (*fault_clear)(struct bt_mesh_model *model, u16_t company_id);
+	int (*fault_clear)(struct bt_mesh_model *model, uint16_t company_id);
 
 	/* Run a specific test */
-	int (*fault_test)(struct bt_mesh_model *model, u8_t test_id,
-			  u16_t company_id);
+	int (*fault_test)(struct bt_mesh_model *model, uint8_t test_id,
+			  uint16_t company_id);
 
 	/* Attention on */
 	void (*attn_on)(struct bt_mesh_model *model);
@@ -48,7 +52,7 @@ struct bt_mesh_health_srv_cb {
  *
  *  @param max_faults Maximum number of faults the element can have.
  *
- *  @return a New net_buf_simple of the needed size.
+ *  @return a New os_mbuf of the needed size.
  */
 #define BT_MESH_HEALTH_FAULT_MSG(max_faults) \
 	NET_BUF_SIMPLE(1 + 3 + (max_faults))
@@ -67,6 +71,7 @@ struct bt_mesh_health_srv {
 int bt_mesh_fault_update(struct bt_mesh_elem *elem);
 
 extern const struct bt_mesh_model_op bt_mesh_health_srv_op[];
+extern const struct bt_mesh_model_cb bt_mesh_health_srv_cb;
 
 /** @def BT_MESH_MODEL_HEALTH_SRV
  *
@@ -80,9 +85,13 @@ extern const struct bt_mesh_model_op bt_mesh_health_srv_op[];
  *
  *  @return New mesh model instance.
  */
-#define BT_MESH_MODEL_HEALTH_SRV(srv, pub)                                   \
-		BT_MESH_MODEL(BT_MESH_MODEL_ID_HEALTH_SRV,                   \
-			      bt_mesh_health_srv_op, pub, srv)
+#define BT_MESH_MODEL_HEALTH_SRV(srv, pub)                                     \
+	BT_MESH_MODEL_CB(BT_MESH_MODEL_ID_HEALTH_SRV, bt_mesh_health_srv_op,   \
+			 pub, srv, &bt_mesh_health_srv_cb)
+
+#ifdef __cplusplus
+}
+#endif
 
 /**
  * @}

nimble/host/mesh/include/mesh/heartbeat.h

@@ -0,0 +1,123 @@
+/** @file
+ *  @brief Bluetooth Mesh Heartbeat API.
+ */
+
+/*
+ * Copyright (c) 2020 Nordic Semiconductor ASA
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#ifndef _BLUETOOTH_MESH_HEARTBEAT_H_
+#define _BLUETOOTH_MESH_HEARTBEAT_H_
+
+/**
+ * @brief Bluetooth Mesh
+ * @defgroup bt_mesh_heartbeat Bluetooth Mesh Heartbeat
+ * @ingroup bt_mesh
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** Heartbeat Publication parameters */
+struct bt_mesh_hb_pub {
+	/** Destination address. */
+	uint16_t dst;
+	/** Remaining publish count. */
+	uint16_t count;
+	/** Time To Live value. */
+	uint8_t ttl;
+	/**
+	 * Bitmap of features that trigger a Heartbeat publication if
+	 * they change. Legal values are @ref BT_MESH_FEAT_RELAY,
+	 * @ref BT_MESH_FEAT_PROXY, @ref BT_MESH_FEAT_FRIEND and
+	 * @ref BT_MESH_FEAT_LOW_POWER.
+	 */
+	uint16_t feat;
+	/** Network index used for publishing. */
+	uint16_t net_idx;
+	/** Publication period in seconds. */
+	uint32_t period;
+};
+
+/** Heartbeat Subscription parameters. */
+struct bt_mesh_hb_sub {
+	/** Subscription period in seconds. */
+	uint32_t period;
+	/** Remaining subscription time in seconds. */
+	uint32_t remaining;
+	/** Source address to receive Heartbeats from. */
+	uint16_t src;
+	/** Destination address to received Heartbeats on. */
+	uint16_t dst;
+	/** The number of received Heartbeat messages so far. */
+	uint16_t count;
+	/**
+	 * Minimum hops in received messages, ie the shortest registered
+	 * path from the publishing node to the subscribing node. A
+	 * Heartbeat received from an immediate neighbor has hop
+	 * count = 1.
+	 */
+	uint8_t min_hops;
+	/**
+	 * Maximum hops in received messages, ie the longest registered
+	 * path from the publishing node to the subscribing node. A
+	 * Heartbeat received from an immediate neighbor has hop
+	 * count = 1.
+	 */
+	uint8_t max_hops;
+};
+
+/** Heartbeat callback structure */
+struct bt_mesh_hb_cb {
+	/** @brief Receive callback for heartbeats.
+	 *
+	 *  Gets called on every received Heartbeat that matches the current
+	 *  Heartbeat subscription parameters.
+	 *
+	 *  @param sub  Current Heartbeat subscription parameters.
+	 *  @param hops The number of hops the Heartbeat was received
+	 *              with.
+	 *  @param feat The feature set of the publishing node. The
+	 *              value is a bitmap of @ref BT_MESH_FEAT_RELAY,
+	 *              @ref BT_MESH_FEAT_PROXY,
+	 *              @ref BT_MESH_FEAT_FRIEND and
+	 *              @ref BT_MESH_FEAT_LOW_POWER.
+	 */
+	void (*recv)(const struct bt_mesh_hb_sub *sub, uint8_t hops,
+		     uint16_t feat);
+
+	/** @brief Subscription end callback for heartbeats.
+	 *
+	 *  Gets called when the subscription period ends, providing a summary
+	 *  of the received heartbeat messages.
+	 *
+	 *  @param sub Current Heartbeat subscription parameters.
+	 */
+	void (*sub_end)(const struct bt_mesh_hb_sub *sub);
+};
+
+/** @brief Get the current Heartbeat publication parameters.
+ *
+ *  @param get Heartbeat publication parameters return buffer.
+ */
+void bt_mesh_hb_pub_get(struct bt_mesh_hb_pub *get);
+
+/** @brief Get the current Heartbeat subscription parameters.
+ *
+ *  @param get Heartbeat subscription parameters return buffer.
+ */
+void bt_mesh_hb_sub_get(struct bt_mesh_hb_sub *get);
+
+extern struct bt_mesh_hb_cb hb_cb;
+
+#ifdef __cplusplus
+}
+#endif
+/**
+ * @}
+ */
+
+#endif /* _BLUETOOTH_MESH_HEARTBEAT_H_ */

nimble/host/mesh/include/mesh/main.h

@@ -17,6 +17,10 @@
  * @{
  */
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 typedef enum {
 	BT_MESH_NO_OUTPUT       = 0,
 	BT_MESH_BLINK           = BIT(0),
@@ -55,10 +59,37 @@ typedef enum {
 	BT_MESH_PROV_OOB_ON_DEV    = BIT(15),
 } bt_mesh_prov_oob_info_t;
 
+/** Device Capabilities. */
+struct bt_mesh_dev_capabilities {
+	/** Number of elements supported by the device */
+	uint8_t elem_count;
+
+	/** Supported algorithms and other capabilities */
+	uint16_t algorithms;
+
+	/** Supported public key types */
+	uint8_t pub_key_type;
+
+	/** Supported static OOB Types */
+	uint8_t static_oob;
+
+	/** Supported Output OOB Actions */
+	bt_mesh_output_action_t output_actions;
+
+	/** Supported Input OOB Actions */
+	bt_mesh_input_action_t input_actions;
+
+	/** Maximum size of Output OOB supported */
+	uint8_t output_size;
+
+	/** Maximum size in octets of Input OOB supported */
+	uint8_t input_size;
+};
+
 /** Provisioning properties & capabilities. */
 struct bt_mesh_prov {
 	/** The UUID that's used when advertising as unprovisioned */
-	const u8_t *uuid;
+	const uint8_t *uuid;
 
 	/** Optional URI. This will be advertised separately from the
 	 *  unprovisioned beacon, however the unprovisioned beacon will
@@ -71,19 +102,34 @@ struct bt_mesh_prov {
 	bt_mesh_prov_oob_info_t oob_info;
 
 	/** Static OOB value */
-	const u8_t *static_val;
+	const uint8_t *static_val;
 	/** Static OOB value length */
-	u8_t        static_val_len;
+	uint8_t        static_val_len;
 
 	/** Maximum size of Output OOB supported */
-	u8_t        output_size;
+	uint8_t        output_size;
 	/** Supported Output OOB Actions */
-	u16_t       output_actions;
+	uint16_t       output_actions;
 
 	/* Maximum size of Input OOB supported */
-	u8_t        input_size;
+	uint8_t        input_size;
 	/** Supported Input OOB Actions */
-	u16_t       input_actions;
+	uint16_t       input_actions;
+
+	/** @brief Provisioning Capabilities.
+	 *
+	 *  This callback notifies the application that the provisioning capabilities
+	 *  of the unprovisioned device has been received.
+	 *
+	 *  The application can consequently call bt_mesh_auth_method_set_<*> to
+	 *  select suitable provisioning oob authentication method.
+	 *
+	 *  When this callback returns, the provisioner will start authentication with
+	 *  the chosen method.
+	 *
+	 *  @param cap capabilities supported by device.
+	 */
+	void         (*capabilities)(const struct bt_mesh_dev_capabilities *cap);
 
 	/** @brief Output of a number is requested.
 	 *
@@ -95,7 +141,7 @@ struct bt_mesh_prov {
 	 *
 	 *  @return Zero on success or negative error code otherwise
 	 */
-	int         (*output_number)(bt_mesh_output_action_t act, u32_t num);
+	int         (*output_number)(bt_mesh_output_action_t act, uint32_t num);
 
 	/** @brief Output of a string is requested.
 	 *
@@ -122,7 +168,29 @@ struct bt_mesh_prov {
 	 *
 	 *  @return Zero on success or negative error code otherwise
 	 */
-	int         (*input)(bt_mesh_input_action_t act, u8_t size);
+	int         (*input)(bt_mesh_input_action_t act, uint8_t size);
+
+	/** @brief The other device finished their OOB input.
+	 *
+	 * This callback notifies the application that it should stop
+	 * displaying its output OOB value, as the other party finished their
+	 * OOB input.
+	 */
+	void 	    (*input_complete)(void);
+
+	/** @brief Unprovisioned beacon has been received.
+	 *
+	 * This callback notifies the application that an unprovisioned
+	 * beacon has been received.
+	 *
+	 * @param uuid UUID
+	 * @param oob_info OOB Information
+	 * @param uri_hash Pointer to URI Hash value. NULL if no hash was
+	 *                 present in the beacon.
+	 */
+	void        (*unprovisioned_beacon)(uint8_t uuid[16],
+					    bt_mesh_prov_oob_info_t oob_info,
+					    uint32_t *uri_hash);
 
 	/** @brief Provisioning link has been opened.
 	 *
@@ -151,7 +219,21 @@ struct bt_mesh_prov {
 	 *  @param net_idx NetKeyIndex given during provisioning.
 	 *  @param addr Primary element address.
 	 */
-	void        (*complete)(u16_t net_idx, u16_t addr);
+	void        (*complete)(uint16_t net_idx, uint16_t addr);
+
+	/** @brief A new node has been added to the provisioning database.
+	 *
+	 *  This callback notifies the application that provisioning has
+	 *  been successfully completed, and that a node has been assigned
+	 *  the specified NetKeyIndex and primary element address.
+	 *
+	 *  @param net_idx NetKeyIndex given during provisioning.
+	 *  @param uuid     UUID of the added node
+	 *  @param addr Primary element address.
+	 *  @param num_elem Number of elements that this node has.
+	 */
+	void        (*node_added)(uint16_t net_idx, uint8_t uuid[16], uint16_t addr,
+				  uint8_t num_elem);
 
 	/** @brief Node has been reset.
 	 *
@@ -184,7 +266,90 @@ int bt_mesh_input_string(const char *str);
  *
  *  @return Zero on success or (negative) error code otherwise.
  */
-int bt_mesh_input_number(u32_t num);
+int bt_mesh_input_number(uint32_t num);
+
+/** @brief Provide Device public key.
+ *
+ *  @param public_key Device public key.
+ *
+ *  @return Zero on success or (negative) error code otherwise.
+ */
+int bt_mesh_prov_remote_pub_key_set(const uint8_t public_key[64]);
+
+/** @brief Use Input OOB authentication.
+ *
+ *  Provisioner only.
+ *
+ *  Instruct the unprovisioned device to use the specified Input OOB
+ *  authentication action. When using @ref BT_MESH_PUSH, @ref BT_MESH_TWIST or
+ *  @ref BT_MESH_ENTER_NUMBER, the @ref bt_mesh_prov::output_number callback is
+ *  called with a random number that has to be entered on the unprovisioned
+ *  device.
+ *
+ *  When using @ref BT_MESH_ENTER_STRING, the @ref bt_mesh_prov::output_string
+ *  callback is called with a random string that has to be entered on the
+ *  unprovisioned device.
+ *
+ *  @param action Authentication action used by the unprovisioned device.
+ *  @param size Authentication size.
+ *
+ *  @return Zero on success or (negative) error code otherwise.
+ */
+int bt_mesh_auth_method_set_input(bt_mesh_input_action_t action, uint8_t size);
+
+/** @brief Use Output OOB authentication.
+ *
+ *  Provisioner only.
+ *
+ *  Instruct the unprovisioned device to use the specified Output OOB
+ *  authentication action. The @ref bt_mesh_prov::input callback will
+ *  be called.
+ *
+ *  When using @ref BT_MESH_BLINK, @ref BT_MESH_BEEP, @ref BT_MESH_VIBRATE
+ *  or @ref BT_MESH_DISPLAY_NUMBER, and the application has to call
+ *  @ref bt_mesh_input_number with the random number indicated by
+ *  the unprovisioned device.
+ *
+ *  When using @ref BT_MESH_DISPLAY_STRING, the application has to call
+ *  @ref bt_mesh_input_string with the random string displayed by the
+ *  unprovisioned device.
+ *
+ *  @param action Authentication action used by the unprovisioned device.
+ *  @param size Authentication size.
+ *
+ *  @return Zero on success or (negative) error code otherwise.
+ */
+int bt_mesh_auth_method_set_output(bt_mesh_output_action_t action, uint8_t size);
+
+/** @brief Use static OOB authentication.
+ *
+ *  Provisioner only.
+ *
+ *  Instruct the unprovisioned device to use static OOB authentication, and use
+ *  the given static authentication value when provisioning.
+ *
+ *  @param static_val Static OOB value.
+ *  @param size Static OOB value size.
+ *
+ *  @return Zero on success or (negative) error code otherwise.
+ */
+int bt_mesh_auth_method_set_static(const uint8_t *static_val, uint8_t size);
+
+/** @brief Don't use OOB authentication.
+ *
+ *  Provisioner only.
+ *
+ *  Don't use any authentication when provisioning new devices. This is the
+ *  default behavior.
+ *
+ *  @warning Not using any authentication exposes the mesh network to
+ *           impersonation attacks, where attackers can pretend to be the
+ *           unprovisioned device to gain access to the network. Authentication
+ *           is strongly encouraged.
+ *
+ *  @return Zero on success or (negative) error code otherwise.
+ */
+int bt_mesh_auth_method_set_none(void);
 
 /** @brief Enable specific provisioning bearers
  *
@@ -220,25 +385,6 @@ int bt_mesh_prov_disable(bt_mesh_prov_bearer_t bearers);
 /* Primary Network Key index */
 #define BT_MESH_NET_PRIMARY                 0x000
 
-#define BT_MESH_RELAY_DISABLED              0x00
-#define BT_MESH_RELAY_ENABLED               0x01
-#define BT_MESH_RELAY_NOT_SUPPORTED         0x02
-
-#define BT_MESH_BEACON_DISABLED             0x00
-#define BT_MESH_BEACON_ENABLED              0x01
-
-#define BT_MESH_GATT_PROXY_DISABLED         0x00
-#define BT_MESH_GATT_PROXY_ENABLED          0x01
-#define BT_MESH_GATT_PROXY_NOT_SUPPORTED    0x02
-
-#define BT_MESH_FRIEND_DISABLED             0x00
-#define BT_MESH_FRIEND_ENABLED              0x01
-#define BT_MESH_FRIEND_NOT_SUPPORTED        0x02
-
-#define BT_MESH_NODE_IDENTITY_STOPPED       0x00
-#define BT_MESH_NODE_IDENTITY_RUNNING       0x01
-#define BT_MESH_NODE_IDENTITY_NOT_SUPPORTED 0x02
-
 /* Features */
 #define BT_MESH_FEAT_RELAY                  BIT(0)
 #define BT_MESH_FEAT_PROXY                  BIT(1)
@@ -261,7 +407,7 @@ int bt_mesh_prov_disable(bt_mesh_prov_bearer_t bearers);
  *
  *  @return Zero on success or (negative) error code otherwise.
  */
-int bt_mesh_init(u8_t own_addr_type,
+int bt_mesh_init(uint8_t own_addr_type,
 		 const struct bt_mesh_prov *prov,
 		 const struct bt_mesh_comp *comp);
 
@@ -277,6 +423,27 @@ int bt_mesh_init(u8_t own_addr_type,
  */
 void bt_mesh_reset(void);
 
+/** @brief Suspend the Mesh network temporarily.
+ *
+ *  This API can be used for power saving purposes, but the user should be
+ *  aware that leaving the local node suspended for a long period of time
+ *  may cause it to become permanently disconnected from the Mesh network.
+ *  If at all possible, the Friendship feature should be used instead, to
+ *  make the node into a Low Power Node.
+ *
+ *  @return 0 on success, or (negative) error code on failure.
+ */
+int bt_mesh_suspend(void);
+
+/** @brief Resume a suspended Mesh network.
+ *
+ *  This API resumes the local node, after it has been suspended using the
+ *  bt_mesh_suspend() API.
+ *
+ *  @return 0 on success, or (negative) error code on failure.
+ */
+int bt_mesh_resume(void);
+
 /** @brief Provision the local Mesh Node.
  *
  *  This API should normally not be used directly by the application. The
@@ -292,9 +459,22 @@ void bt_mesh_reset(void);
  *
  *  @return Zero on success or (negative) error code otherwise.
  */
-int bt_mesh_provision(const u8_t net_key[16], u16_t net_idx,
-		      u8_t flags, u32_t iv_index, u16_t addr,
-		      const u8_t dev_key[16]);
+int bt_mesh_provision(const uint8_t net_key[16], uint16_t net_idx,
+		      uint8_t flags, uint32_t iv_index, uint16_t addr,
+		      const uint8_t dev_key[16]);
+
+/** @brief Provision a Mesh Node using PB-ADV
+ *
+ * @param uuid    UUID
+ * @param net_idx Network Key Index
+ * @param addr    Address to assign to remote device. If addr is 0, the lowest
+ *                available address will be chosen.
+ * @param attention_duration The attention duration to be send to remote device
+ *
+ * @return Zero on success or (negative) error code otherwise.
+ */
+int bt_mesh_provision_adv(const uint8_t uuid[16], uint16_t net_idx, uint16_t addr,
+			  uint8_t attention_duration);
 
 /** @brief Check if the local node has been provisioned.
  *
@@ -356,7 +536,21 @@ int bt_mesh_lpn_poll(void);
  *
  *  @param cb Function to call when the Friendship status changes.
  */
-void bt_mesh_lpn_set_cb(void (*cb)(u16_t friend_addr, bool established));
+void bt_mesh_lpn_set_cb(void (*cb)(uint16_t friend_addr, bool established));
+
+/** @brief Terminate Friendship.
+ *
+ *  Terminated Friendship for given LPN.
+ *
+ *  @param lpn_addr Low Power Node address.
+ *
+ *  @return Zero on success or (negative) error code otherwise.
+ */
+int bt_mesh_friend_terminate(uint16_t lpn_addr);
+
+#ifdef __cplusplus
+}
+#endif
 
 /**
  * @}

nimble/host/mesh/include/mesh/mesh.h

@@ -17,10 +17,16 @@
 #include "glue.h"
 #include "access.h"
 #include "main.h"
+#include "cfg.h"
 #include "cfg_srv.h"
 #include "health_srv.h"
 #include "cfg_cli.h"
 #include "health_cli.h"
 #include "proxy.h"
+#include "cdb.h"
+#include "cfg.h"
+#include "heartbeat.h"
+#include "../src/app_keys.h"
+#include "../src/net.h"
 
 #endif /* __BT_MESH_H */