1 /*
2  * SPDX-FileCopyrightText: 2015-2024 Espressif Systems (Shanghai) CO LTD
3  *
4  * SPDX-License-Identifier: Apache-2.0
5  */
6 #include <stddef.h>
7 #include <stdlib.h>
8 #include <stdio.h>
9 #include <string.h>
10 
11 #include "esp_random.h"
12 #include "esp_heap_caps.h"
13 #include "esp_heap_caps_init.h"
14 #include <esp_mac.h>
15 
16 #include "sdkconfig.h"
17 
18 #if CONFIG_BT_NIMBLE_ENABLED
19 #include "nimble/nimble_port.h"
20 #endif // CONFIG_BT_NIMBLE_ENABLED
21 #include "nimble/nimble_port_freertos.h"
22 #include "esp_private/esp_modem_clock.h"
23 
24 #ifdef ESP_PLATFORM
25 #include "esp_log.h"
26 #endif // ESP_PLATFORM
27 
28 #if CONFIG_SW_COEXIST_ENABLE
29 #include "private/esp_coexist_internal.h"
30 #endif // CONFIG_SW_COEXIST_ENABLE
31 
32 #include "nimble/nimble_npl_os.h"
33 #include "esp_hci_transport.h"
34 #include "os/endian.h"
35 
36 #include "esp_bt.h"
37 #include "esp_intr_alloc.h"
38 #include "esp_sleep.h"
39 #include "esp_pm.h"
40 #include "esp_phy_init.h"
41 #include "esp_private/periph_ctrl.h"
42 #include "bt_osi_mem.h"
43 
44 #if SOC_PM_RETENTION_HAS_CLOCK_BUG
45 #include "esp_private/sleep_retention.h"
46 #endif // SOC_PM_RETENTION_HAS_CLOCK_BUG
47 
48 #if CONFIG_FREERTOS_USE_TICKLESS_IDLE
49 #include "esp_private/sleep_modem.h"
50 #endif // CONFIG_FREERTOS_USE_TICKLESS_IDLE
51 
52 #include "freertos/FreeRTOS.h"
53 #include "freertos/task.h"
54 
55 #include "esp_private/periph_ctrl.h"
56 #include "esp_sleep.h"
57 
58 #include "hal/efuse_hal.h"
59 #include "soc/rtc.h"
60 /* Macro definition
61  ************************************************************************
62  */
63 #define NIMBLE_PORT_LOG_TAG          "BLE_INIT"
64 #define OSI_COEX_VERSION              0x00010006
65 #define OSI_COEX_MAGIC_VALUE          0xFADEBEAD
66 
67 #define EXT_FUNC_VERSION             0x20240422
68 #define EXT_FUNC_MAGIC_VALUE         0xA5A5A5A5
69 
70 #define BT_ASSERT_PRINT              ets_printf
71 
72 /* Types definition
73  ************************************************************************
74  */
75 struct osi_coex_funcs_t {
76     uint32_t _magic;
77     uint32_t _version;
78     void (* _coex_wifi_sleep_set)(bool sleep);
79     int (* _coex_core_ble_conn_dyn_prio_get)(bool *low, bool *high);
80     void (* _coex_schm_status_bit_set)(uint32_t type, uint32_t status);
81     void (* _coex_schm_status_bit_clear)(uint32_t type, uint32_t status);
82 };
83 
84 struct ext_funcs_t {
85     uint32_t ext_version;
86     int (*_esp_intr_alloc)(int source, int flags, intr_handler_t handler, void *arg, void **ret_handle);
87     int (*_esp_intr_free)(void **ret_handle);
88     void *(* _malloc)(size_t size);
89     void (*_free)(void *p);
90     int (* _task_create)(void *task_func, const char *name, uint32_t stack_depth, void *param,
91                          uint32_t prio, void *task_handle, uint32_t core_id);
92     void (* _task_delete)(void *task_handle);
93     void (*_osi_assert)(const uint32_t ln, const char *fn, uint32_t param1, uint32_t param2);
94     uint32_t (* _os_random)(void);
95     int (* _ecc_gen_key_pair)(uint8_t *public, uint8_t *priv);
96     int (* _ecc_gen_dh_key)(const uint8_t *remote_pub_key_x, const uint8_t *remote_pub_key_y,
97                             const uint8_t *local_priv_key, uint8_t *dhkey);
98     void (* _esp_reset_rpa_moudle)(void);
99     uint32_t magic;
100 };
101 
102 #if CONFIG_BT_LE_CONTROLLER_LOG_ENABLED
103 typedef void (*interface_func_t) (uint32_t len, const uint8_t*addr, bool end);
104 #endif // CONFIG_BT_LE_CONTROLLER_LOG_ENABLED
105 
106 /* External functions or variables
107  ************************************************************************
108  */
109 extern int ble_osi_coex_funcs_register(struct osi_coex_funcs_t *coex_funcs);
110 extern int r_ble_controller_init(esp_bt_controller_config_t *cfg);
111 #if CONFIG_BT_LE_CONTROLLER_LOG_ENABLED
112 extern int r_ble_log_init_async(interface_func_t bt_controller_log_interface, bool task_create, uint8_t buffers, uint32_t *bufs_size);
113 extern int r_ble_log_deinit_async(void);
114 extern void r_ble_log_async_select_dump_buffers(uint8_t buffers);
115 extern void r_ble_log_async_output_dump_all(bool output);
116 extern void esp_panic_handler_reconfigure_wdts(uint32_t timeout_ms);
117 #endif // CONFIG_BT_LE_CONTROLLER_LOG_ENABLED
118 extern int r_ble_controller_deinit(void);
119 extern int r_ble_controller_enable(uint8_t mode);
120 extern int r_ble_controller_disable(void);
121 extern int esp_register_ext_funcs (struct ext_funcs_t *);
122 extern void esp_unregister_ext_funcs (void);
123 extern int r_esp_ble_ll_set_public_addr(const uint8_t *addr);
124 extern int esp_register_npl_funcs (struct npl_funcs_t *p_npl_func);
125 extern void esp_unregister_npl_funcs (void);
126 extern void npl_freertos_mempool_deinit(void);
127 extern uint32_t r_os_cputime_get32(void);
128 extern uint32_t r_os_cputime_ticks_to_usecs(uint32_t ticks);
129 extern void r_ble_lll_rfmgmt_set_sleep_cb(void *s_cb, void *w_cb, void *s_arg,
130                                           void *w_arg, uint32_t us_to_enabled);
131 extern void r_ble_rtc_wake_up_state_clr(void);
132 extern int os_msys_init(void);
133 extern void os_msys_deinit(void);
134 #if CONFIG_FREERTOS_USE_TICKLESS_IDLE
135 extern sleep_retention_entries_config_t *r_esp_ble_mac_retention_link_get(uint8_t *size, uint8_t extra);
136 extern void r_esp_ble_set_wakeup_overhead(uint32_t overhead);
137 #endif /* CONFIG_FREERTOS_USE_TICKLESS_IDLE */
138 extern void r_esp_ble_change_rtc_freq(uint32_t freq);
139 extern int ble_sm_alg_gen_dhkey(const uint8_t *peer_pub_key_x,
140                                 const uint8_t *peer_pub_key_y,
141                                 const uint8_t *our_priv_key, uint8_t *out_dhkey);
142 extern int ble_sm_alg_gen_key_pair(uint8_t *pub, uint8_t *priv);
143 extern int r_ble_txpwr_set(esp_ble_enhanced_power_type_t power_type, uint16_t handle, int power_level);
144 extern int r_ble_txpwr_get(esp_ble_enhanced_power_type_t power_type, uint16_t handle);
145 extern int r_ble_get_npl_element_info(esp_bt_controller_config_t *cfg, ble_npl_count_info_t * npl_info);
146 extern char *ble_controller_get_compile_version(void);
147 extern int esp_ble_register_bb_funcs(void);
148 extern void esp_ble_unregister_bb_funcs(void);
149 extern uint32_t _bt_bss_start;
150 extern uint32_t _bt_bss_end;
151 extern uint32_t _bt_controller_bss_start;
152 extern uint32_t _bt_controller_bss_end;
153 extern uint32_t _bt_data_start;
154 extern uint32_t _bt_data_end;
155 extern uint32_t _bt_controller_data_start;
156 extern uint32_t _bt_controller_data_end;
157 
158 /* Local Function Declaration
159  *********************************************************************
160  */
161 static void coex_schm_status_bit_set_wrapper(uint32_t type, uint32_t status);
162 static void coex_schm_status_bit_clear_wrapper(uint32_t type, uint32_t status);
163 static int task_create_wrapper(void *task_func, const char *name, uint32_t stack_depth,
164                                void *param, uint32_t prio, void *task_handle, uint32_t core_id);
165 static void task_delete_wrapper(void *task_handle);
166 static int esp_intr_alloc_wrapper(int source, int flags, intr_handler_t handler,
167                                   void *arg, void **ret_handle_in);
168 static int esp_intr_free_wrapper(void **ret_handle);
169 static void osi_assert_wrapper(const uint32_t ln, const char *fn, uint32_t param1, uint32_t param2);
170 static uint32_t osi_random_wrapper(void);
171 static void esp_reset_rpa_moudle(void);
172 static int esp_ecc_gen_key_pair(uint8_t *pub, uint8_t *priv);
173 static int esp_ecc_gen_dh_key(const uint8_t *peer_pub_key_x, const uint8_t *peer_pub_key_y,
174                               const uint8_t *our_priv_key, uint8_t *out_dhkey);
175 #if CONFIG_BT_LE_CONTROLLER_LOG_ENABLED
176 static void esp_bt_controller_log_interface(uint32_t len, const uint8_t *addr, bool end);
177 #if CONFIG_BT_LE_CONTROLLER_LOG_STORAGE_ENABLE
178 static void esp_bt_ctrl_log_partition_get_and_erase_first_block(void);
179 #endif // #if CONFIG_BT_LE_CONTROLLER_LOG_STORAGE_ENABLE
180 #endif // CONFIG_BT_LE_CONTROLLER_LOG_ENABLED
181 /* Local variable definition
182  ***************************************************************************
183  */
184 /* Static variable declare */
185 static DRAM_ATTR esp_bt_controller_status_t ble_controller_status = ESP_BT_CONTROLLER_STATUS_IDLE;
186 #if CONFIG_BT_LE_CONTROLLER_LOG_ENABLED
187 const static uint32_t log_bufs_size[] = {CONFIG_BT_LE_LOG_CTRL_BUF1_SIZE, CONFIG_BT_LE_LOG_HCI_BUF_SIZE, CONFIG_BT_LE_LOG_CTRL_BUF2_SIZE};
188 enum log_out_mode {
189     LOG_DUMP_MEMORY,
190     LOG_ASYNC_OUT,
191     LOG_STORAGE_TO_FLASH,
192 };
193 
194 bool log_is_inited = false;
195 #if CONFIG_BT_LE_CONTROLLER_LOG_DUMP_ONLY
196 uint8_t log_output_mode = LOG_DUMP_MEMORY;
197 #else
198 #if CONFIG_BT_LE_CONTROLLER_LOG_STORAGE_ENABLE
199 uint8_t log_output_mode = LOG_STORAGE_TO_FLASH;
200 #else
201 uint8_t log_output_mode = LOG_ASYNC_OUT;
202 #endif // CONFIG_BT_LE_CONTROLLER_LOG_STORAGE_ENABLE
203 #endif // CONFIG_BT_LE_CONTROLLER_LOG_DUMP_ONLY
204 
esp_bt_log_output_mode_set(uint8_t output_mode)205 void esp_bt_log_output_mode_set(uint8_t output_mode)
206 {
207     log_output_mode = output_mode;
208 }
209 
esp_bt_log_output_mode_get(void)210 uint8_t esp_bt_log_output_mode_get(void)
211 {
212     return log_output_mode;
213 }
214 
esp_bt_controller_log_init(uint8_t log_output_mode)215 esp_err_t esp_bt_controller_log_init(uint8_t log_output_mode)
216 {
217     esp_err_t ret = ESP_OK;
218     interface_func_t bt_controller_log_interface;
219     bt_controller_log_interface = esp_bt_controller_log_interface;
220     bool task_create;
221     uint8_t buffers = 0;
222 
223     if (log_is_inited) {
224         return ret;
225     }
226 
227 #if CONFIG_BT_LE_CONTROLLER_LOG_CTRL_ENABLED
228     buffers |= ESP_BLE_LOG_BUF_CONTROLLER;
229 #endif // CONFIG_BT_LE_CONTROLLER_LOG_CTRL_ENABLED
230 #if CONFIG_BT_LE_CONTROLLER_LOG_HCI_ENABLED
231     buffers |= ESP_BLE_LOG_BUF_HCI;
232 #endif // CONFIG_BT_LE_CONTROLLER_LOG_HCI_ENABLED
233 
234     switch (log_output_mode) {
235         case LOG_DUMP_MEMORY:
236             task_create = false;
237             break;
238         case LOG_ASYNC_OUT:
239         case LOG_STORAGE_TO_FLASH:
240             task_create = true;
241 #if CONFIG_BT_LE_CONTROLLER_LOG_STORAGE_ENABLE
242             if (log_output_mode == LOG_STORAGE_TO_FLASH) {
243                 esp_bt_ctrl_log_partition_get_and_erase_first_block();
244             }
245 #endif // CONFIG_BT_LE_CONTROLLER_LOG_STORAGE_ENABLE
246             break;
247         default:
248             assert(0);
249     }
250 
251     ret = r_ble_log_init_async(bt_controller_log_interface, task_create, buffers, (uint32_t *)log_bufs_size);
252     if (ret == ESP_OK) {
253         log_is_inited = true;
254     }
255 
256     return ret;
257 }
258 
esp_bt_ontroller_log_deinit(void)259 void esp_bt_ontroller_log_deinit(void)
260 {
261     r_ble_log_deinit_async();
262     log_is_inited = false;
263 }
264 
265 #if CONFIG_BT_LE_CONTROLLER_LOG_STORAGE_ENABLE
266 #include "esp_partition.h"
267 #include "hal/wdt_hal.h"
268 
269 #define MAX_STORAGE_SIZE          (CONFIG_BT_LE_CONTROLLER_LOG_PARTITION_SIZE)
270 #define BLOCK_SIZE                (4096)
271 #define THRESHOLD                 (3072)
272 #define PARTITION_NAME            "bt_ctrl_log"
273 
274 static const esp_partition_t *log_partition;
275 static uint32_t write_index = 0;
276 static uint32_t next_erase_index = BLOCK_SIZE;
277 static bool block_erased = false;
278 static bool stop_write = false;
279 static bool is_filled = false;
280 
esp_bt_ctrl_log_partition_get_and_erase_first_block(void)281 static void esp_bt_ctrl_log_partition_get_and_erase_first_block(void)
282 {
283     log_partition = NULL;
284     assert(MAX_STORAGE_SIZE % BLOCK_SIZE == 0);
285     // Find the partition map in the partition table
286     log_partition = esp_partition_find_first(ESP_PARTITION_TYPE_DATA, ESP_PARTITION_SUBTYPE_ANY, PARTITION_NAME);
287     assert(log_partition != NULL);
288     // Prepare data to be read later using the mapped address
289     ESP_ERROR_CHECK(esp_partition_erase_range(log_partition, 0, BLOCK_SIZE));
290     write_index = 0;
291     next_erase_index = BLOCK_SIZE;
292     block_erased = false;
293     is_filled = false;
294     stop_write = false;
295 }
296 
esp_bt_controller_log_storage(uint32_t len,const uint8_t * addr,bool end)297 static int esp_bt_controller_log_storage(uint32_t len, const uint8_t *addr, bool end)
298 {
299     if (len > MAX_STORAGE_SIZE) {
300         return -1;
301     }
302 
303     if (stop_write) {
304         return 0;
305     }
306 
307     if (((write_index) % BLOCK_SIZE) >= THRESHOLD && !block_erased) {
308         // esp_rom_printf("Ers nxt: %d,%d\n", next_erase_index, write_index);
309         esp_partition_erase_range(log_partition, next_erase_index, BLOCK_SIZE);
310         next_erase_index = (next_erase_index + BLOCK_SIZE) % MAX_STORAGE_SIZE;
311         block_erased = true;
312     }
313 
314     if (((write_index + len) / BLOCK_SIZE) >  (write_index / BLOCK_SIZE)) {
315         block_erased = false;
316     }
317 
318     if (write_index + len <= MAX_STORAGE_SIZE) {
319         esp_partition_write(log_partition, write_index, addr, len);
320         write_index = (write_index + len) % MAX_STORAGE_SIZE;
321     } else {
322         uint32_t first_part_len = MAX_STORAGE_SIZE - write_index;
323         esp_partition_write(log_partition, write_index, addr, first_part_len);
324         esp_partition_write(log_partition, 0, addr + first_part_len, len - first_part_len);
325         write_index = len - first_part_len;
326         is_filled = true;
327         // esp_rom_printf("old idx: %d,%d\n",next_erase_index, write_index);
328     }
329 
330     return 0;
331 }
332 
esp_bt_read_ctrl_log_from_flash(bool output)333 void esp_bt_read_ctrl_log_from_flash(bool output)
334 {
335     esp_partition_mmap_handle_t mmap_handle;
336     uint32_t read_index;
337     const void *mapped_ptr;
338     const uint8_t *buffer;
339     uint32_t print_len;
340     uint32_t max_print_len;
341     esp_err_t err;
342 
343     print_len = 0;
344     max_print_len = 4096;
345     err = esp_partition_mmap(log_partition, 0, MAX_STORAGE_SIZE, ESP_PARTITION_MMAP_DATA, &mapped_ptr, &mmap_handle);
346     if (err != ESP_OK) {
347         ESP_LOGE("FLASH", "Mmap failed: %s", esp_err_to_name(err));
348         return;
349     }
350 
351     portMUX_TYPE spinlock = portMUX_INITIALIZER_UNLOCKED;
352     portENTER_CRITICAL_SAFE(&spinlock);
353     esp_panic_handler_reconfigure_wdts(5000);
354     r_ble_log_async_output_dump_all(true);
355     esp_bt_ontroller_log_deinit();
356     stop_write = true;
357 
358     buffer = (const uint8_t *)mapped_ptr;
359     esp_panic_handler_reconfigure_wdts(5000);
360     if (is_filled) {
361         read_index = next_erase_index;
362     } else {
363         read_index = 0;
364     }
365 
366     esp_rom_printf("\r\nREAD_CHECK:%ld,%ld,%d\r\n",read_index, write_index, is_filled);
367     esp_rom_printf("\r\n[DUMP_START:");
368     while (read_index != write_index) {
369         esp_rom_printf("%02x ", buffer[read_index]);
370         if (print_len > max_print_len) {
371             esp_panic_handler_reconfigure_wdts(5000);
372             print_len = 0;
373         }
374 
375         print_len++;
376         read_index = (read_index + 1) % MAX_STORAGE_SIZE;
377     }
378 
379     esp_rom_printf(":DUMP_END]\r\n");
380     portEXIT_CRITICAL_SAFE(&spinlock);
381     esp_partition_munmap(mmap_handle);
382     err = esp_bt_controller_log_init(log_output_mode);
383     assert(err == ESP_OK);
384 }
385 #endif // CONFIG_BT_LE_CONTROLLER_LOG_STORAGE_ENABLE
386 #endif // CONFIG_BT_LE_CONTROLLER_LOG_ENABLED
387 
388 /* This variable tells if BLE is running */
389 static bool s_ble_active = false;
390 #ifdef CONFIG_PM_ENABLE
391 static DRAM_ATTR esp_pm_lock_handle_t s_pm_lock = NULL;
392 #endif // CONFIG_PM_ENABLE
393 static DRAM_ATTR modem_clock_lpclk_src_t s_bt_lpclk_src = MODEM_CLOCK_LPCLK_SRC_INVALID;
394 #define BLE_RTC_DELAY_US_LIGHT_SLEEP        (2500)
395 #define BLE_RTC_DELAY_US_MODEM_SLEEP        (500)
396 
397 #define BLE_CONTROLLER_MALLOC_CAPS          (MALLOC_CAP_INTERNAL|MALLOC_CAP_8BIT|MALLOC_CAP_DMA)
malloc_ble_controller_mem(size_t size)398 void *malloc_ble_controller_mem(size_t size)
399 {
400     return heap_caps_malloc(size, BLE_CONTROLLER_MALLOC_CAPS);
401 }
402 
get_ble_controller_free_heap_size(void)403 uint32_t get_ble_controller_free_heap_size(void)
404 {
405     return heap_caps_get_free_size(BLE_CONTROLLER_MALLOC_CAPS);
406 }
407 
408 static const struct osi_coex_funcs_t s_osi_coex_funcs_ro = {
409     ._magic = OSI_COEX_MAGIC_VALUE,
410     ._version = OSI_COEX_VERSION,
411     ._coex_wifi_sleep_set = NULL,
412     ._coex_core_ble_conn_dyn_prio_get = NULL,
413     ._coex_schm_status_bit_set = coex_schm_status_bit_set_wrapper,
414     ._coex_schm_status_bit_clear = coex_schm_status_bit_clear_wrapper,
415 };
416 
417 struct ext_funcs_t ext_funcs_ro = {
418     .ext_version = EXT_FUNC_VERSION,
419     ._esp_intr_alloc = esp_intr_alloc_wrapper,
420     ._esp_intr_free = esp_intr_free_wrapper,
421     ._malloc = bt_osi_mem_malloc_internal,
422     ._free = bt_osi_mem_free,
423     ._task_create = task_create_wrapper,
424     ._task_delete = task_delete_wrapper,
425     ._osi_assert = osi_assert_wrapper,
426     ._os_random = osi_random_wrapper,
427     ._ecc_gen_key_pair = esp_ecc_gen_key_pair,
428     ._ecc_gen_dh_key = esp_ecc_gen_dh_key,
429     ._esp_reset_rpa_moudle = esp_reset_rpa_moudle,
430     .magic = EXT_FUNC_MAGIC_VALUE,
431 };
432 
esp_reset_rpa_moudle(void)433 static void IRAM_ATTR esp_reset_rpa_moudle(void)
434 {
435 
436 }
437 
osi_assert_wrapper(const uint32_t ln,const char * fn,uint32_t param1,uint32_t param2)438 static void IRAM_ATTR osi_assert_wrapper(const uint32_t ln, const char *fn,
439                                          uint32_t param1, uint32_t param2)
440 {
441     BT_ASSERT_PRINT("BLE assert: line %d in function %s, param: 0x%x, 0x%x", ln, fn, param1, param2);
442 #if CONFIG_BT_LE_CONTROLLER_LOG_ENABLED
443     esp_ble_controller_log_dump_all(true);
444 #endif // CONFIG_BT_LE_CONTROLLER_LOG_ENABLED
445     assert(0);
446 }
447 
osi_random_wrapper(void)448 static uint32_t IRAM_ATTR osi_random_wrapper(void)
449 {
450     return esp_random();
451 }
452 
coex_schm_status_bit_set_wrapper(uint32_t type,uint32_t status)453 static void coex_schm_status_bit_set_wrapper(uint32_t type, uint32_t status)
454 {
455 #if CONFIG_SW_COEXIST_ENABLE
456     coex_schm_status_bit_set(type, status);
457 #endif // CONFIG_SW_COEXIST_ENABLE
458 }
459 
coex_schm_status_bit_clear_wrapper(uint32_t type,uint32_t status)460 static void coex_schm_status_bit_clear_wrapper(uint32_t type, uint32_t status)
461 {
462 #if CONFIG_SW_COEXIST_ENABLE
463     coex_schm_status_bit_clear(type, status);
464 #endif // CONFIG_SW_COEXIST_ENABLE
465 }
466 
task_create_wrapper(void * task_func,const char * name,uint32_t stack_depth,void * param,uint32_t prio,void * task_handle,uint32_t core_id)467 static int task_create_wrapper(void *task_func, const char *name, uint32_t stack_depth,
468                                 void *param, uint32_t prio, void *task_handle, uint32_t core_id)
469 {
470     return (uint32_t)xTaskCreatePinnedToCore(task_func, name, stack_depth, param, prio, task_handle,
471                                              (core_id < portNUM_PROCESSORS ? core_id : tskNO_AFFINITY));
472 }
473 
task_delete_wrapper(void * task_handle)474 static void task_delete_wrapper(void *task_handle)
475 {
476     vTaskDelete(task_handle);
477 }
478 
esp_ecc_gen_key_pair(uint8_t * pub,uint8_t * priv)479 static int esp_ecc_gen_key_pair(uint8_t *pub, uint8_t *priv)
480 {
481     int rc = -1;
482 #if CONFIG_BT_LE_SM_LEGACY || CONFIG_BT_LE_SM_SC
483     rc = ble_sm_alg_gen_key_pair(pub, priv);
484 #endif // CONFIG_BT_LE_SM_LEGACY || CONFIG_BT_LE_SM_SC
485     return rc;
486 }
487 
esp_ecc_gen_dh_key(const uint8_t * peer_pub_key_x,const uint8_t * peer_pub_key_y,const uint8_t * our_priv_key,uint8_t * out_dhkey)488 static int esp_ecc_gen_dh_key(const uint8_t *peer_pub_key_x, const uint8_t *peer_pub_key_y,
489                               const uint8_t *our_priv_key, uint8_t *out_dhkey)
490 {
491     int rc = -1;
492 #if CONFIG_BT_LE_SM_LEGACY || CONFIG_BT_LE_SM_SC
493     rc = ble_sm_alg_gen_dhkey(peer_pub_key_x, peer_pub_key_y, our_priv_key, out_dhkey);
494 #endif // CONFIG_BT_LE_SM_LEGACY || CONFIG_BT_LE_SM_SC
495     return rc;
496 }
497 
esp_intr_alloc_wrapper(int source,int flags,intr_handler_t handler,void * arg,void ** ret_handle_in)498 static int esp_intr_alloc_wrapper(int source, int flags, intr_handler_t handler,
499                                   void *arg, void **ret_handle_in)
500 {
501     int rc = esp_intr_alloc(source, flags | ESP_INTR_FLAG_IRAM, handler,
502                             arg, (intr_handle_t *)ret_handle_in);
503     return rc;
504 }
505 
esp_intr_free_wrapper(void ** ret_handle)506 static int esp_intr_free_wrapper(void **ret_handle)
507 {
508     int rc = 0;
509     rc = esp_intr_free((intr_handle_t) * ret_handle);
510     *ret_handle = NULL;
511     return rc;
512 }
513 
esp_bt_rtc_slow_clk_select(uint8_t slow_clk_src)514 void esp_bt_rtc_slow_clk_select(uint8_t slow_clk_src)
515 {
516     /* Select slow clock source for BT momdule */
517     switch (slow_clk_src) {
518         case MODEM_CLOCK_LPCLK_SRC_MAIN_XTAL:
519             ESP_LOGI(NIMBLE_PORT_LOG_TAG, "Using main XTAL as clock source");
520             uint32_t chip_version = efuse_hal_chip_revision();
521             if (chip_version == 0) {
522                 modem_clock_select_lp_clock_source(PERIPH_BT_MODULE, slow_clk_src, (400 - 1));
523             } else{
524                 modem_clock_select_lp_clock_source(PERIPH_BT_MODULE, slow_clk_src, (5 - 1));
525             }
526             break;
527         case MODEM_CLOCK_LPCLK_SRC_RC_SLOW:
528             ESP_LOGI(NIMBLE_PORT_LOG_TAG, "Using 136 kHz RC as clock source, can only run legacy ADV or SCAN due to low clock accuracy!");
529             modem_clock_select_lp_clock_source(PERIPH_BT_MODULE, slow_clk_src, (5 - 1));
530             break;
531         case MODEM_CLOCK_LPCLK_SRC_XTAL32K:
532             ESP_LOGI(NIMBLE_PORT_LOG_TAG, "Using external 32.768 kHz XTAL as clock source");
533             modem_clock_select_lp_clock_source(PERIPH_BT_MODULE, slow_clk_src, (1 - 1));
534             break;
535         case MODEM_CLOCK_LPCLK_SRC_RC32K:
536             ESP_LOGI(NIMBLE_PORT_LOG_TAG, "Using 32 kHz RC as clock source, can only run legacy ADV or SCAN due to low clock accuracy!");
537             modem_clock_select_lp_clock_source(PERIPH_BT_MODULE, slow_clk_src, (1 - 1));
538             break;
539         case MODEM_CLOCK_LPCLK_SRC_EXT32K:
540             ESP_LOGI(NIMBLE_PORT_LOG_TAG, "Using 32 kHz oscillator as clock source, can only run legacy ADV or SCAN due to low clock accuracy!");
541             modem_clock_select_lp_clock_source(PERIPH_BT_MODULE, slow_clk_src, (1 - 1));
542             break;
543         default:
544     }
545 }
546 
esp_bt_get_lpclk_src(void)547 modem_clock_lpclk_src_t esp_bt_get_lpclk_src(void)
548 {
549     return s_bt_lpclk_src;
550 }
551 
esp_bt_set_lpclk_src(modem_clock_lpclk_src_t clk_src)552 void esp_bt_set_lpclk_src(modem_clock_lpclk_src_t clk_src)
553 {
554     if (clk_src >= MODEM_CLOCK_LPCLK_SRC_MAX) {
555         return;
556     }
557 
558     s_bt_lpclk_src = clk_src;
559 }
560 
controller_sleep_cb(uint32_t enable_tick,void * arg)561 IRAM_ATTR void controller_sleep_cb(uint32_t enable_tick, void *arg)
562 {
563     if (!s_ble_active) {
564         return;
565     }
566 #if CONFIG_FREERTOS_USE_TICKLESS_IDLE
567     r_ble_rtc_wake_up_state_clr();
568 #endif /* CONFIG_FREERTOS_USE_TICKLESS_IDLE */
569     esp_phy_disable(PHY_MODEM_BT);
570 #ifdef CONFIG_PM_ENABLE
571     esp_pm_lock_release(s_pm_lock);
572 #endif // CONFIG_PM_ENABLE
573     s_ble_active = false;
574 }
575 
controller_wakeup_cb(void * arg)576 IRAM_ATTR void controller_wakeup_cb(void *arg)
577 {
578     if (s_ble_active) {
579         return;
580     }
581 #ifdef CONFIG_PM_ENABLE
582     esp_pm_lock_acquire(s_pm_lock);
583     r_ble_rtc_wake_up_state_clr();
584 #endif //CONFIG_PM_ENABLE
585     esp_phy_enable(PHY_MODEM_BT);
586     s_ble_active = true;
587 }
588 
589 #if CONFIG_FREERTOS_USE_TICKLESS_IDLE
sleep_modem_ble_mac_retention_init(void * arg)590 static esp_err_t sleep_modem_ble_mac_retention_init(void *arg)
591 {
592     uint8_t size;
593     int extra = *(int *)arg;
594     sleep_retention_entries_config_t *ble_mac_modem_config = r_esp_ble_mac_retention_link_get(&size, extra);
595     esp_err_t err = sleep_retention_entries_create(ble_mac_modem_config, size, REGDMA_LINK_PRI_BT_MAC_BB, SLEEP_RETENTION_MODULE_BLE_MAC);
596     if (err == ESP_OK) {
597         ESP_LOGI(NIMBLE_PORT_LOG_TAG, "Modem BLE MAC retention initialization");
598     }
599     return err;
600 }
601 
sleep_modem_ble_mac_modem_state_init(uint8_t extra)602 static esp_err_t sleep_modem_ble_mac_modem_state_init(uint8_t extra)
603 {
604     int retention_args = extra;
605     sleep_retention_module_init_param_t init_param = {
606         .cbs     = { .create = { .handle = sleep_modem_ble_mac_retention_init, .arg = &retention_args } },
607         .depends = BIT(SLEEP_RETENTION_MODULE_BT_BB)
608     };
609     esp_err_t err = sleep_retention_module_init(SLEEP_RETENTION_MODULE_BLE_MAC, &init_param);
610     if (err == ESP_OK) {
611         err = sleep_retention_module_allocate(SLEEP_RETENTION_MODULE_BLE_MAC);
612     }
613     return err;
614 }
615 
sleep_modem_ble_mac_modem_state_deinit(void)616 static void sleep_modem_ble_mac_modem_state_deinit(void)
617 {
618     esp_err_t err = sleep_retention_module_free(SLEEP_RETENTION_MODULE_BLE_MAC);
619     if (err == ESP_OK) {
620         err = sleep_retention_module_deinit(SLEEP_RETENTION_MODULE_BLE_MAC);
621         assert(err == ESP_OK);
622     }
623 }
624 
sleep_modem_light_sleep_overhead_set(uint32_t overhead)625 void IRAM_ATTR sleep_modem_light_sleep_overhead_set(uint32_t overhead)
626 {
627     r_esp_ble_set_wakeup_overhead(overhead);
628 }
629 #endif /* CONFIG_FREERTOS_USE_TICKLESS_IDLE */
630 
631 
controller_sleep_init(void)632 esp_err_t controller_sleep_init(void)
633 {
634     esp_err_t rc = 0;
635 
636 #ifdef CONFIG_BT_LE_SLEEP_ENABLE
637     ESP_LOGW(NIMBLE_PORT_LOG_TAG, "BLE modem sleep is enabled");
638 #if CONFIG_FREERTOS_USE_TICKLESS_IDLE
639     r_ble_lll_rfmgmt_set_sleep_cb(controller_sleep_cb, controller_wakeup_cb, 0, 0,
640                                 BLE_RTC_DELAY_US_LIGHT_SLEEP);
641 #else
642     r_ble_lll_rfmgmt_set_sleep_cb(controller_sleep_cb, controller_wakeup_cb, 0, 0,
643                                 BLE_RTC_DELAY_US_MODEM_SLEEP);
644 #endif /* FREERTOS_USE_TICKLESS_IDLE */
645 #endif // CONFIG_BT_LE_SLEEP_ENABLE
646 
647 #ifdef CONFIG_PM_ENABLE
648     rc = esp_pm_lock_create(ESP_PM_CPU_FREQ_MAX, 0, "bt", &s_pm_lock);
649     if (rc != ESP_OK) {
650         goto error;
651     }
652 #endif // CONFIG_PM_ENABLE
653 #if CONFIG_BT_LE_SLEEP_ENABLE && CONFIG_FREERTOS_USE_TICKLESS_IDLE
654 #if SOC_PM_RETENTION_HAS_CLOCK_BUG && !CONFIG_MAC_BB_PD
655 #error "CONFIG_MAC_BB_PD required for BLE light sleep to run properly"
656 #endif // SOC_PM_RETENTION_HAS_CLOCK_BUG && !CONFIG_MAC_BB_PD
657     /* Create a new regdma link for BLE related register restoration */
658     rc = sleep_modem_ble_mac_modem_state_init(1);
659     if (rc != ESP_OK) {
660         goto error;
661     }
662     esp_sleep_enable_bt_wakeup();
663     ESP_LOGW(NIMBLE_PORT_LOG_TAG, "Enable light sleep, the wake up source is BLE timer");
664 
665     rc = esp_pm_register_inform_out_light_sleep_overhead_callback(sleep_modem_light_sleep_overhead_set);
666     if (rc != ESP_OK) {
667         goto error;
668     }
669 
670 #if SOC_PM_RETENTION_HAS_CLOCK_BUG && CONFIG_MAC_BB_PD
671     sleep_modem_register_mac_bb_module_prepare_callback(sleep_modem_mac_bb_power_down_prepare,
672                                                    sleep_modem_mac_bb_power_up_prepare);
673 #endif // SOC_PM_RETENTION_HAS_CLOCK_BUG && CONFIG_MAC_BB_PD
674 #endif /* CONFIG_BT_LE_SLEEP_ENABLE && CONFIG_FREERTOS_USE_TICKLESS_IDLE */
675     return rc;
676 
677 #ifdef CONFIG_PM_ENABLE
678 error:
679 #endif // CONFIG_PM_ENABLE
680 #if CONFIG_BT_LE_SLEEP_ENABLE && CONFIG_FREERTOS_USE_TICKLESS_IDLE
681 #if SOC_PM_RETENTION_HAS_CLOCK_BUG && CONFIG_MAC_BB_PD
682     sleep_modem_unregister_mac_bb_module_prepare_callback(sleep_modem_mac_bb_power_down_prepare,
683                                                      sleep_modem_mac_bb_power_up_prepare);
684 #endif // SOC_PM_RETENTION_HAS_CLOCK_BUG && CONFIG_MAC_BB_PD
685     esp_sleep_disable_bt_wakeup();
686     esp_pm_unregister_inform_out_light_sleep_overhead_callback(sleep_modem_light_sleep_overhead_set);
687 #endif /* CONFIG_BT_LE_SLEEP_ENABLE && CONFIG_FREERTOS_USE_TICKLESS_IDLE */
688 #ifdef CONFIG_PM_ENABLE
689     /*lock should release first and then delete*/
690     if (s_pm_lock != NULL) {
691         esp_pm_lock_delete(s_pm_lock);
692         s_pm_lock = NULL;
693     }
694 #endif // CONFIG_PM_ENABLE
695 
696     return rc;
697 }
698 
controller_sleep_deinit(void)699 void controller_sleep_deinit(void)
700 {
701 #if CONFIG_BT_LE_SLEEP_ENABLE && CONFIG_FREERTOS_USE_TICKLESS_IDLE
702 #if SOC_PM_RETENTION_HAS_CLOCK_BUG && CONFIG_MAC_BB_PD
703     sleep_modem_unregister_mac_bb_module_prepare_callback(sleep_modem_mac_bb_power_down_prepare,
704                                                      sleep_modem_mac_bb_power_up_prepare);
705 #endif // SOC_PM_RETENTION_HAS_CLOCK_BUG && CONFIG_MAC_BB_PD
706     r_ble_rtc_wake_up_state_clr();
707     esp_sleep_disable_bt_wakeup();
708     sleep_modem_ble_mac_modem_state_deinit();
709     esp_pm_unregister_inform_out_light_sleep_overhead_callback(sleep_modem_light_sleep_overhead_set);
710 #endif /* CONFIG_BT_LE_SLEEP_ENABLE && CONFIG_FREERTOS_USE_TICKLESS_IDLE */
711 #ifdef CONFIG_PM_ENABLE
712     /* lock should be released first */
713     esp_pm_lock_delete(s_pm_lock);
714     s_pm_lock = NULL;
715 #endif //CONFIG_PM_ENABLE
716 }
717 
718 typedef enum {
719     FILTER_DUPLICATE_PDUTYPE = BIT(0),
720     FILTER_DUPLICATE_LENGTH  = BIT(1),
721     FILTER_DUPLICATE_ADDRESS = BIT(2),
722     FILTER_DUPLICATE_ADVDATA = BIT(3),
723     FILTER_DUPLICATE_DEFAULT = FILTER_DUPLICATE_PDUTYPE | FILTER_DUPLICATE_ADDRESS,
724     FILTER_DUPLICATE_PDU_ALL = 0xF,
725     FILTER_DUPLICATE_EXCEPTION_FOR_MESH = BIT(4),
726     FILTER_DUPLICATE_AD_TYPE = BIT(5),
727 }disc_duplicate_mode_t;
728 
729 
730 extern void r_filter_duplicate_mode_enable(disc_duplicate_mode_t mode);
731 extern void r_filter_duplicate_mode_disable(disc_duplicate_mode_t mode);
732 extern void r_filter_duplicate_set_ring_list_max_num(uint32_t max_num);
733 extern void r_scan_duplicate_cache_refresh_set_time(uint32_t period_time);
734 
735 int
ble_vhci_disc_duplicate_mode_enable(int mode)736 ble_vhci_disc_duplicate_mode_enable(int mode)
737 {
738     // TODO: use vendor hci to update
739     r_filter_duplicate_mode_enable(mode);
740     return true;
741 }
742 
743 int
ble_vhci_disc_duplicate_mode_disable(int mode)744 ble_vhci_disc_duplicate_mode_disable(int mode)
745 {
746     // TODO: use vendor hci to update
747     r_filter_duplicate_mode_disable(mode);
748     return true;
749 }
750 
ble_vhci_disc_duplicate_set_max_cache_size(int max_cache_size)751 int ble_vhci_disc_duplicate_set_max_cache_size(int max_cache_size){
752     // TODO: use vendor hci to update
753     r_filter_duplicate_set_ring_list_max_num(max_cache_size);
754     return true;
755 }
756 
ble_vhci_disc_duplicate_set_period_refresh_time(int refresh_period_time)757 int ble_vhci_disc_duplicate_set_period_refresh_time(int refresh_period_time){
758     // TODO: use vendor hci to update
759     r_scan_duplicate_cache_refresh_set_time(refresh_period_time);
760     return true;
761 }
762 
763 /**
764  * @brief Config scan duplicate option mode from menuconfig (Adapt to the old configuration method.)
765  */
ble_controller_scan_duplicate_config(void)766 void ble_controller_scan_duplicate_config(void)
767 {
768     uint32_t duplicate_mode = FILTER_DUPLICATE_DEFAULT;
769     uint32_t cache_size = 100;
770 #if CONFIG_BT_LE_SCAN_DUPL == true
771     cache_size = CONFIG_BT_LE_LL_DUP_SCAN_LIST_COUNT;
772     if (CONFIG_BT_LE_SCAN_DUPL_TYPE == 0) {
773         duplicate_mode = FILTER_DUPLICATE_ADDRESS | FILTER_DUPLICATE_PDUTYPE;
774     } else if (CONFIG_BT_LE_SCAN_DUPL_TYPE == 1) {
775         duplicate_mode = FILTER_DUPLICATE_ADVDATA;
776     } else if (CONFIG_BT_LE_SCAN_DUPL_TYPE == 2) {
777         duplicate_mode = FILTER_DUPLICATE_ADDRESS | FILTER_DUPLICATE_ADVDATA;
778     }
779     duplicate_mode |= FILTER_DUPLICATE_EXCEPTION_FOR_MESH;
780 
781     ble_vhci_disc_duplicate_set_period_refresh_time(CONFIG_BT_LE_SCAN_DUPL_CACHE_REFRESH_PERIOD);
782 #endif
783 
784     ble_vhci_disc_duplicate_mode_disable(0xFFFFFFFF);
785     ble_vhci_disc_duplicate_mode_enable(duplicate_mode);
786     ble_vhci_disc_duplicate_set_max_cache_size(cache_size);
787 }
788 
ble_rtc_clk_init(esp_bt_controller_config_t * cfg)789 static void ble_rtc_clk_init(esp_bt_controller_config_t *cfg)
790 {
791     if (s_bt_lpclk_src == MODEM_CLOCK_LPCLK_SRC_INVALID) {
792 #if CONFIG_BT_LE_LP_CLK_SRC_MAIN_XTAL
793         s_bt_lpclk_src = MODEM_CLOCK_LPCLK_SRC_MAIN_XTAL;
794 #else
795 #if CONFIG_RTC_CLK_SRC_INT_RC
796         s_bt_lpclk_src = MODEM_CLOCK_LPCLK_SRC_RC_SLOW;
797 #elif CONFIG_RTC_CLK_SRC_EXT_CRYS
798         if (rtc_clk_slow_src_get() == SOC_RTC_SLOW_CLK_SRC_XTAL32K) {
799             s_bt_lpclk_src = MODEM_CLOCK_LPCLK_SRC_XTAL32K;
800         } else {
801             ESP_LOGW(NIMBLE_PORT_LOG_TAG, "32.768kHz XTAL not detected, fall back to main XTAL as Bluetooth sleep clock");
802             s_bt_lpclk_src = MODEM_CLOCK_LPCLK_SRC_MAIN_XTAL;
803         }
804 #elif CONFIG_RTC_CLK_SRC_INT_RC32K
805         s_bt_lpclk_src = MODEM_CLOCK_LPCLK_SRC_RC32K;
806 #elif CONFIG_RTC_CLK_SRC_EXT_OSC
807         s_bt_lpclk_src = MODEM_CLOCK_LPCLK_SRC_EXT32K;
808 #else
809         ESP_LOGE(NIMBLE_PORT_LOG_TAG, "Unsupported clock source");
810         assert(0);
811 #endif
812 #endif /* CONFIG_BT_LE_LP_CLK_SRC_MAIN_XTAL */
813     }
814 
815     if (s_bt_lpclk_src == MODEM_CLOCK_LPCLK_SRC_MAIN_XTAL) {
816         cfg->rtc_freq = 100000;
817     } else if (s_bt_lpclk_src == MODEM_CLOCK_LPCLK_SRC_XTAL32K) {
818         cfg->rtc_freq = 32768;
819     } else if (s_bt_lpclk_src == MODEM_CLOCK_LPCLK_SRC_RC_SLOW) {
820         cfg->rtc_freq = 30000;
821     } else if (s_bt_lpclk_src == MODEM_CLOCK_LPCLK_SRC_RC32K) {
822         cfg->rtc_freq = 32000;
823     } else if (s_bt_lpclk_src == MODEM_CLOCK_LPCLK_SRC_EXT32K) {
824         cfg->rtc_freq = 32000;
825     }
826     esp_bt_rtc_slow_clk_select(s_bt_lpclk_src);
827 }
828 
esp_bt_controller_init(esp_bt_controller_config_t * cfg)829 esp_err_t esp_bt_controller_init(esp_bt_controller_config_t *cfg)
830 {
831     uint8_t mac[6];
832     esp_err_t ret = ESP_OK;
833     ble_npl_count_info_t npl_info;
834     uint8_t hci_transport_mode;
835 
836     memset(&npl_info, 0, sizeof(ble_npl_count_info_t));
837     if (ble_controller_status != ESP_BT_CONTROLLER_STATUS_IDLE) {
838         ESP_LOGW(NIMBLE_PORT_LOG_TAG, "invalid controller state");
839         return ESP_ERR_INVALID_STATE;
840     }
841 
842     if (!cfg) {
843         ESP_LOGW(NIMBLE_PORT_LOG_TAG, "cfg is NULL");
844         return ESP_ERR_INVALID_ARG;
845     }
846 
847     ret = esp_register_ext_funcs(&ext_funcs_ro);
848     if (ret != ESP_OK) {
849         ESP_LOGW(NIMBLE_PORT_LOG_TAG, "register extend functions failed");
850         return ret;
851     }
852 
853     /* Initialize the function pointers for OS porting */
854     npl_freertos_funcs_init();
855     struct npl_funcs_t *p_npl_funcs = npl_freertos_funcs_get();
856     if (!p_npl_funcs) {
857         ESP_LOGW(NIMBLE_PORT_LOG_TAG, "npl functions get failed");
858         return ESP_ERR_INVALID_ARG;
859     }
860 
861     ret = esp_register_npl_funcs(p_npl_funcs);
862     if (ret != ESP_OK) {
863         ESP_LOGW(NIMBLE_PORT_LOG_TAG, "npl functions register failed");
864         goto free_mem;
865     }
866 
867     r_ble_get_npl_element_info(cfg, &npl_info);
868     npl_freertos_set_controller_npl_info(&npl_info);
869     if (npl_freertos_mempool_init() != 0) {
870         ESP_LOGW(NIMBLE_PORT_LOG_TAG, "npl mempool init failed");
871         ret = ESP_ERR_INVALID_ARG;
872         goto free_mem;
873     }
874 
875 #if CONFIG_BT_NIMBLE_ENABLED
876     /* ble_npl_eventq_init() needs to use npl functions in rom and
877      * must be called after esp_bt_controller_init().
878      */
879     ble_npl_eventq_init(nimble_port_get_dflt_eventq());
880 #endif // CONFIG_BT_NIMBLE_ENABLED
881     /* Enable BT-related clocks */
882     modem_clock_module_enable(PERIPH_BT_MODULE);
883     modem_clock_module_mac_reset(PERIPH_BT_MODULE);
884     /* Select slow clock source for BT momdule */
885     ble_rtc_clk_init(cfg);
886     esp_phy_modem_init();
887 
888     if (ble_osi_coex_funcs_register((struct osi_coex_funcs_t *)&s_osi_coex_funcs_ro) != 0) {
889         ESP_LOGW(NIMBLE_PORT_LOG_TAG, "osi coex funcs reg failed");
890         ret = ESP_ERR_INVALID_ARG;
891         goto modem_deint;
892     }
893 
894 #if CONFIG_SW_COEXIST_ENABLE
895     coex_init();
896 #endif // CONFIG_SW_COEXIST_ENABLE
897 
898 #if CONFIG_BT_LE_CONTROLLER_LOG_ENABLED
899     ret = esp_bt_controller_log_init(log_output_mode);
900     if (ret != ESP_OK) {
901         ESP_LOGW(NIMBLE_PORT_LOG_TAG, "ble_controller_log_init failed %d", ret);
902         goto modem_deint;
903     }
904 #endif // CONFIG_BT_CONTROLLER_LOG_ENABLED
905     ret = esp_ble_register_bb_funcs();
906     if (ret != ESP_OK) {
907         ESP_LOGW(NIMBLE_PORT_LOG_TAG, "esp_ble_register_bb_funcs failed %d", ret);
908         goto modem_deint;
909     }
910 
911     ret = r_ble_controller_init(cfg);
912     if (ret != ESP_OK) {
913         ESP_LOGW(NIMBLE_PORT_LOG_TAG, "r_ble_controller_init failed %d", ret);
914         goto modem_deint;
915     }
916 
917     ESP_LOGI(NIMBLE_PORT_LOG_TAG, "ble controller commit:[%s]", ble_controller_get_compile_version());
918 
919     ble_controller_scan_duplicate_config();
920 
921     ret = os_msys_init();
922     if (ret != ESP_OK) {
923         ESP_LOGW(NIMBLE_PORT_LOG_TAG, "msys_init failed %d", ret);
924         goto free_controller;
925     }
926 
927     ret = controller_sleep_init();
928     if (ret != ESP_OK) {
929         ESP_LOGW(NIMBLE_PORT_LOG_TAG, "controller_sleep_init failed %d", ret);
930         goto free_controller;
931     }
932 
933     ESP_ERROR_CHECK(esp_read_mac((uint8_t *)mac, ESP_MAC_BT));
934     ESP_LOGI(NIMBLE_PORT_LOG_TAG, "Bluetooth MAC: %02x:%02x:%02x:%02x:%02x:%02x",
935              mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
936     swap_in_place(mac, 6);
937     r_esp_ble_ll_set_public_addr(mac);
938 
939     ble_controller_status = ESP_BT_CONTROLLER_STATUS_INITED;
940 
941 #if CONFIG_BT_LE_HCI_INTERFACE_USE_RAM
942     hci_transport_mode = HCI_TRANSPORT_VHCI;
943 #elif CONFIG_BT_LE_HCI_INTERFACE_USE_UART
944     hci_transport_mode = HCI_TRANSPORT_UART_NO_DMA;
945 #if CONFIG_BT_LE_UART_HCI_DMA_MODE
946     hci_transport_mode = HCI_TRANSPORT_UART_UHCI;
947 #endif // CONFIG_BT_LE_UART_HCI_DMA_MODE
948 #endif // CONFIG_BT_LE_HCI_INTERFACE_USE_RAM
949     ret = hci_transport_init(hci_transport_mode);
950     if (ret) {
951         ESP_LOGW(NIMBLE_PORT_LOG_TAG, "hci transport init failed %d", ret);
952         goto free_controller;
953     }
954 
955     return ESP_OK;
956 free_controller:
957     hci_transport_deinit();
958     controller_sleep_deinit();
959     os_msys_deinit();
960     r_ble_controller_deinit();
961 modem_deint:
962     esp_ble_unregister_bb_funcs();
963 #if CONFIG_BT_LE_CONTROLLER_LOG_ENABLED
964     esp_bt_ontroller_log_deinit();
965 #endif // CONFIG_BT_LE_CONTROLLER_LOG_ENABLED
966     esp_phy_modem_deinit();
967     modem_clock_deselect_lp_clock_source(PERIPH_BT_MODULE);
968     modem_clock_module_disable(PERIPH_BT_MODULE);
969 #if CONFIG_BT_NIMBLE_ENABLED
970     ble_npl_eventq_deinit(nimble_port_get_dflt_eventq());
971 #endif // CONFIG_BT_NIMBLE_ENABLED
972 free_mem:
973     npl_freertos_mempool_deinit();
974     esp_unregister_npl_funcs();
975     npl_freertos_funcs_deinit();
976     esp_unregister_ext_funcs();
977     return ret;
978 }
979 
esp_bt_controller_deinit(void)980 esp_err_t esp_bt_controller_deinit(void)
981 {
982     if ((ble_controller_status < ESP_BT_CONTROLLER_STATUS_INITED) ||
983         (ble_controller_status >= ESP_BT_CONTROLLER_STATUS_ENABLED)) {
984         ESP_LOGW(NIMBLE_PORT_LOG_TAG, "invalid controller state");
985         return ESP_FAIL;
986     }
987 
988     hci_transport_deinit();
989     controller_sleep_deinit();
990 
991     os_msys_deinit();
992 
993     esp_phy_modem_deinit();
994     modem_clock_deselect_lp_clock_source(PERIPH_BT_MODULE);
995     modem_clock_module_disable(PERIPH_BT_MODULE);
996 
997     r_ble_controller_deinit();
998     esp_ble_unregister_bb_funcs();
999 #if CONFIG_BT_LE_CONTROLLER_LOG_ENABLED
1000     esp_bt_ontroller_log_deinit();
1001 #endif // CONFIG_BT_LE_CONTROLLER_LOG_ENABLED
1002 
1003 #if CONFIG_BT_NIMBLE_ENABLED
1004     /* De-initialize default event queue */
1005     ble_npl_eventq_deinit(nimble_port_get_dflt_eventq());
1006 #endif // CONFIG_BT_NIMBLE_ENABLED
1007 
1008     esp_unregister_npl_funcs();
1009 
1010     esp_unregister_ext_funcs();
1011 
1012     /* De-initialize npl functions */
1013     npl_freertos_funcs_deinit();
1014 
1015     npl_freertos_mempool_deinit();
1016 
1017     ble_controller_status = ESP_BT_CONTROLLER_STATUS_IDLE;
1018 
1019     return ESP_OK;
1020 }
1021 
esp_bt_controller_enable(esp_bt_mode_t mode)1022 esp_err_t esp_bt_controller_enable(esp_bt_mode_t mode)
1023 {
1024     esp_err_t ret = ESP_OK;
1025 
1026     if (mode != ESP_BT_MODE_BLE) {
1027         ESP_LOGW(NIMBLE_PORT_LOG_TAG, "invalid controller mode");
1028         return ESP_FAIL;
1029     }
1030     if (ble_controller_status != ESP_BT_CONTROLLER_STATUS_INITED) {
1031         ESP_LOGW(NIMBLE_PORT_LOG_TAG, "invalid controller state");
1032         return ESP_FAIL;
1033     }
1034     if (!s_ble_active) {
1035 #if CONFIG_PM_ENABLE
1036         esp_pm_lock_acquire(s_pm_lock);
1037 #endif  // CONFIG_PM_ENABLE
1038         esp_phy_enable(PHY_MODEM_BT);
1039         s_ble_active = true;
1040     }
1041     esp_btbb_enable();
1042 #if CONFIG_SW_COEXIST_ENABLE
1043     coex_enable();
1044 #endif // CONFIG_SW_COEXIST_ENABLE
1045 
1046     if (r_ble_controller_enable(mode) != 0) {
1047         ret = ESP_FAIL;
1048         goto error;
1049     }
1050     ble_controller_status = ESP_BT_CONTROLLER_STATUS_ENABLED;
1051     return ESP_OK;
1052 
1053 error:
1054 #if CONFIG_SW_COEXIST_ENABLE
1055     coex_disable();
1056 #endif
1057     esp_btbb_disable();
1058     if (s_ble_active) {
1059         esp_phy_disable(PHY_MODEM_BT);
1060 #if CONFIG_PM_ENABLE
1061         esp_pm_lock_release(s_pm_lock);
1062 #endif  // CONFIG_PM_ENABLE
1063         s_ble_active = false;
1064     }
1065     return ret;
1066 }
1067 
esp_bt_controller_disable(void)1068 esp_err_t esp_bt_controller_disable(void)
1069 {
1070     if (ble_controller_status < ESP_BT_CONTROLLER_STATUS_ENABLED) {
1071         ESP_LOGW(NIMBLE_PORT_LOG_TAG, "invalid controller state");
1072         return ESP_FAIL;
1073     }
1074     if (r_ble_controller_disable() != 0) {
1075         return ESP_FAIL;
1076     }
1077 #if CONFIG_SW_COEXIST_ENABLE
1078     coex_disable();
1079 #endif
1080     esp_btbb_disable();
1081     if (s_ble_active) {
1082         esp_phy_disable(PHY_MODEM_BT);
1083 #if CONFIG_PM_ENABLE
1084         esp_pm_lock_release(s_pm_lock);
1085 #endif  // CONFIG_PM_ENABLE
1086         s_ble_active = false;
1087     }
1088     ble_controller_status = ESP_BT_CONTROLLER_STATUS_INITED;
1089     return ESP_OK;
1090 }
1091 
esp_bt_controller_mem_release(esp_bt_mode_t mode)1092 esp_err_t esp_bt_controller_mem_release(esp_bt_mode_t mode)
1093 {
1094     ESP_LOGD(NIMBLE_PORT_LOG_TAG, "%s not implemented, return OK", __func__);
1095     return ESP_OK;
1096 }
1097 
try_heap_caps_add_region(intptr_t start,intptr_t end)1098 static esp_err_t try_heap_caps_add_region(intptr_t start, intptr_t end)
1099 {
1100     int ret = heap_caps_add_region(start, end);
1101 
1102     /* heap_caps_add_region() returns ESP_ERR_INVALID_SIZE if the memory region is
1103      * is too small to fit a heap. This cannot be termed as a fatal error and hence
1104      * we replace it by ESP_OK
1105      */
1106     if (ret == ESP_ERR_INVALID_SIZE) {
1107         return ESP_OK;
1108     }
1109     return ret;
1110 }
1111 
1112 
1113 typedef struct {
1114     intptr_t start;
1115     intptr_t end;
1116     const char* name;
1117 } bt_area_t;
1118 
esp_bt_mem_release_area(const bt_area_t * area)1119 static esp_err_t esp_bt_mem_release_area(const bt_area_t *area)
1120 {
1121     esp_err_t ret = ESP_OK;
1122     intptr_t mem_start = area->start;
1123     intptr_t mem_end = area->end;
1124     if (mem_start != mem_end) {
1125         ESP_LOGD(NIMBLE_PORT_LOG_TAG, "Release %s [0x%08x] - [0x%08x], len %d", area->name, mem_start, mem_end, mem_end - mem_start);
1126         ret = try_heap_caps_add_region(mem_start, mem_end);
1127     }
1128     return ret;
1129 }
1130 
esp_bt_mem_release_areas(const bt_area_t * area1,const bt_area_t * area2)1131 static esp_err_t esp_bt_mem_release_areas(const bt_area_t *area1, const bt_area_t *area2)
1132 {
1133     esp_err_t ret = ESP_OK;
1134 
1135     if (area1->end == area2->start) {
1136         bt_area_t merged_area = {
1137             .start = area1->start,
1138             .end = area2->end,
1139             .name = area1->name
1140         };
1141         ret = esp_bt_mem_release_area(&merged_area);
1142     } else {
1143         esp_bt_mem_release_area(area1);
1144         ret = esp_bt_mem_release_area(area2);
1145     }
1146 
1147     return ret;
1148 }
1149 
esp_bt_mem_release(esp_bt_mode_t mode)1150 esp_err_t esp_bt_mem_release(esp_bt_mode_t mode)
1151 {
1152     esp_err_t ret = ESP_OK;
1153 
1154     if (ble_controller_status != ESP_BT_CONTROLLER_STATUS_IDLE) {
1155         return ESP_ERR_INVALID_STATE;
1156     }
1157 
1158     bt_area_t bss = {
1159         .start = (intptr_t)&_bt_bss_start,
1160         .end   = (intptr_t)&_bt_bss_end,
1161         .name  = "BT BSS",
1162     };
1163     bt_area_t cont_bss = {
1164         .start = (intptr_t)&_bt_controller_bss_start,
1165         .end   = (intptr_t)&_bt_controller_bss_end,
1166         .name  = "BT Controller BSS",
1167     };
1168     bt_area_t data = {
1169         .start = (intptr_t)&_bt_data_start,
1170         .end   = (intptr_t)&_bt_data_end,
1171         .name  = "BT Data",
1172     };
1173     bt_area_t cont_data = {
1174         .start = (intptr_t)&_bt_controller_data_start,
1175         .end   = (intptr_t)&_bt_controller_data_end,
1176         .name  = "BT Controller Data"
1177     };
1178 
1179     if (mode & ESP_BT_MODE_BLE) {
1180         /* Start by freeing Bluetooth BSS section */
1181         if (ret == ESP_OK) {
1182             ret = esp_bt_mem_release_areas(&bss, &cont_bss);
1183         }
1184 
1185         /* Do the same thing with the Bluetooth data section */
1186         if (ret == ESP_OK) {
1187             ret = esp_bt_mem_release_areas(&data, &cont_data);
1188         }
1189     }
1190 
1191     return ret;
1192 }
1193 
1194 
esp_bt_controller_get_status(void)1195 esp_bt_controller_status_t esp_bt_controller_get_status(void)
1196 {
1197     return ble_controller_status;
1198 }
1199 
esp_ble_tx_power_set(esp_ble_power_type_t power_type,esp_power_level_t power_level)1200 esp_err_t esp_ble_tx_power_set(esp_ble_power_type_t power_type, esp_power_level_t power_level)
1201 {
1202     esp_err_t stat = ESP_FAIL;
1203 
1204     switch (power_type) {
1205     case ESP_BLE_PWR_TYPE_DEFAULT:
1206     case ESP_BLE_PWR_TYPE_ADV:
1207     case ESP_BLE_PWR_TYPE_SCAN:
1208         if (r_ble_txpwr_set(ESP_BLE_ENHANCED_PWR_TYPE_DEFAULT, 0, power_level) == 0) {
1209             stat = ESP_OK;
1210         }
1211         break;
1212     case ESP_BLE_PWR_TYPE_CONN_HDL0:
1213     case ESP_BLE_PWR_TYPE_CONN_HDL1:
1214     case ESP_BLE_PWR_TYPE_CONN_HDL2:
1215     case ESP_BLE_PWR_TYPE_CONN_HDL3:
1216     case ESP_BLE_PWR_TYPE_CONN_HDL4:
1217     case ESP_BLE_PWR_TYPE_CONN_HDL5:
1218     case ESP_BLE_PWR_TYPE_CONN_HDL6:
1219     case ESP_BLE_PWR_TYPE_CONN_HDL7:
1220     case ESP_BLE_PWR_TYPE_CONN_HDL8:
1221         if (r_ble_txpwr_set(ESP_BLE_ENHANCED_PWR_TYPE_CONN, power_type, power_level) == 0) {
1222             stat = ESP_OK;
1223         }
1224         break;
1225     default:
1226         stat = ESP_ERR_NOT_SUPPORTED;
1227         break;
1228     }
1229 
1230     return stat;
1231 }
1232 
esp_ble_tx_power_set_enhanced(esp_ble_enhanced_power_type_t power_type,uint16_t handle,esp_power_level_t power_level)1233 esp_err_t esp_ble_tx_power_set_enhanced(esp_ble_enhanced_power_type_t power_type, uint16_t handle,
1234                                         esp_power_level_t power_level)
1235 {
1236     esp_err_t stat = ESP_FAIL;
1237     switch (power_type) {
1238     case ESP_BLE_ENHANCED_PWR_TYPE_DEFAULT:
1239     case ESP_BLE_ENHANCED_PWR_TYPE_SCAN:
1240     case ESP_BLE_ENHANCED_PWR_TYPE_INIT:
1241         if (r_ble_txpwr_set(ESP_BLE_ENHANCED_PWR_TYPE_DEFAULT, 0, power_level) == 0) {
1242             stat = ESP_OK;
1243         }
1244         break;
1245     case ESP_BLE_ENHANCED_PWR_TYPE_ADV:
1246     case ESP_BLE_ENHANCED_PWR_TYPE_CONN:
1247         if (r_ble_txpwr_set(power_type, handle, power_level) == 0) {
1248             stat = ESP_OK;
1249         }
1250         break;
1251     default:
1252         stat = ESP_ERR_NOT_SUPPORTED;
1253         break;
1254     }
1255 
1256     return stat;
1257 }
1258 
esp_ble_tx_power_get(esp_ble_power_type_t power_type)1259 esp_power_level_t esp_ble_tx_power_get(esp_ble_power_type_t power_type)
1260 {
1261     int tx_level = 0;
1262 
1263     switch (power_type) {
1264     case ESP_BLE_PWR_TYPE_ADV:
1265     case ESP_BLE_PWR_TYPE_SCAN:
1266     case ESP_BLE_PWR_TYPE_DEFAULT:
1267         tx_level = r_ble_txpwr_get(ESP_BLE_ENHANCED_PWR_TYPE_DEFAULT, 0);
1268         break;
1269     case ESP_BLE_PWR_TYPE_CONN_HDL0:
1270     case ESP_BLE_PWR_TYPE_CONN_HDL1:
1271     case ESP_BLE_PWR_TYPE_CONN_HDL2:
1272     case ESP_BLE_PWR_TYPE_CONN_HDL3:
1273     case ESP_BLE_PWR_TYPE_CONN_HDL4:
1274     case ESP_BLE_PWR_TYPE_CONN_HDL5:
1275     case ESP_BLE_PWR_TYPE_CONN_HDL6:
1276     case ESP_BLE_PWR_TYPE_CONN_HDL7:
1277     case ESP_BLE_PWR_TYPE_CONN_HDL8:
1278         tx_level = r_ble_txpwr_get(ESP_BLE_ENHANCED_PWR_TYPE_CONN, power_type);
1279         break;
1280     default:
1281         return ESP_PWR_LVL_INVALID;
1282     }
1283 
1284     if (tx_level < 0) {
1285         return ESP_PWR_LVL_INVALID;
1286     }
1287 
1288     return (esp_power_level_t)tx_level;
1289 }
1290 
esp_ble_tx_power_get_enhanced(esp_ble_enhanced_power_type_t power_type,uint16_t handle)1291 esp_power_level_t esp_ble_tx_power_get_enhanced(esp_ble_enhanced_power_type_t power_type,
1292                                                 uint16_t handle)
1293 {
1294     int tx_level = 0;
1295 
1296     switch (power_type) {
1297     case ESP_BLE_ENHANCED_PWR_TYPE_DEFAULT:
1298     case ESP_BLE_ENHANCED_PWR_TYPE_SCAN:
1299     case ESP_BLE_ENHANCED_PWR_TYPE_INIT:
1300         tx_level = r_ble_txpwr_get(ESP_BLE_ENHANCED_PWR_TYPE_DEFAULT, 0);
1301         break;
1302     case ESP_BLE_ENHANCED_PWR_TYPE_ADV:
1303     case ESP_BLE_ENHANCED_PWR_TYPE_CONN:
1304         tx_level = r_ble_txpwr_get(power_type, handle);
1305         break;
1306     default:
1307         return ESP_PWR_LVL_INVALID;
1308     }
1309 
1310     if (tx_level < 0) {
1311        return ESP_PWR_LVL_INVALID;
1312     }
1313 
1314     return (esp_power_level_t)tx_level;
1315 }
1316 
1317 #if CONFIG_BT_LE_CONTROLLER_LOG_ENABLED
esp_bt_controller_log_interface(uint32_t len,const uint8_t * addr,bool end)1318 static void esp_bt_controller_log_interface(uint32_t len, const uint8_t *addr, bool end)
1319 {
1320     if (log_output_mode == LOG_STORAGE_TO_FLASH) {
1321 #if CONFIG_BT_LE_CONTROLLER_LOG_STORAGE_ENABLE
1322         esp_bt_controller_log_storage(len, addr, end);
1323 #endif //CONFIG_BT_LE_CONTROLLER_LOG_STORAGE_ENABLE
1324     } else {
1325         portMUX_TYPE spinlock = portMUX_INITIALIZER_UNLOCKED;
1326         portENTER_CRITICAL_SAFE(&spinlock);
1327         esp_panic_handler_reconfigure_wdts(1000);
1328         for (int i = 0; i < len; i++) {
1329             esp_rom_printf("%02x ", addr[i]);
1330         }
1331 
1332         if (end) {
1333             esp_rom_printf("\n");
1334         }
1335         portEXIT_CRITICAL_SAFE(&spinlock);
1336     }
1337 }
1338 
esp_ble_controller_log_dump_all(bool output)1339 void esp_ble_controller_log_dump_all(bool output)
1340 {
1341     if (log_output_mode == LOG_STORAGE_TO_FLASH) {
1342 #if CONFIG_BT_LE_CONTROLLER_LOG_STORAGE_ENABLE
1343         esp_bt_read_ctrl_log_from_flash(output);
1344 #endif // CONFIG_BT_LE_CONTROLLER_LOG_STORAGE_ENABLE
1345     } else {
1346         portMUX_TYPE spinlock = portMUX_INITIALIZER_UNLOCKED;
1347         portENTER_CRITICAL_SAFE(&spinlock);
1348         esp_panic_handler_reconfigure_wdts(5000);
1349         BT_ASSERT_PRINT("\r\n[DUMP_START:");
1350         r_ble_log_async_output_dump_all(output);
1351         BT_ASSERT_PRINT(":DUMP_END]\r\n");
1352         portEXIT_CRITICAL_SAFE(&spinlock);
1353     }
1354 }
1355 #endif // CONFIG_BT_LE_CONTROLLER_LOG_ENABLED
1356 
1357 #if (!CONFIG_BT_NIMBLE_ENABLED) && (CONFIG_BT_CONTROLLER_ENABLED)
1358 #if CONFIG_BT_LE_SM_LEGACY || CONFIG_BT_LE_SM_SC
1359 #define BLE_SM_KEY_ERR 0x17
1360 #if CONFIG_BT_LE_CRYPTO_STACK_MBEDTLS
1361 #include "mbedtls/aes.h"
1362 #if CONFIG_BT_LE_SM_SC
1363 #include "mbedtls/cipher.h"
1364 #include "mbedtls/entropy.h"
1365 #include "mbedtls/ctr_drbg.h"
1366 #include "mbedtls/cmac.h"
1367 #include "mbedtls/ecdh.h"
1368 #include "mbedtls/ecp.h"
1369 
1370 static mbedtls_ecp_keypair keypair;
1371 #endif // CONFIG_BT_LE_SM_SC
1372 
1373 #else
1374 #include "tinycrypt/aes.h"
1375 #include "tinycrypt/constants.h"
1376 #include "tinycrypt/utils.h"
1377 
1378 #if CONFIG_BT_LE_SM_SC
1379 #include "tinycrypt/cmac_mode.h"
1380 #include "tinycrypt/ecc_dh.h"
1381 #endif // CONFIG_BT_LE_SM_SC
1382 #endif // CONFIG_BT_LE_CRYPTO_STACK_MBEDTLS
1383 
1384 /* Based on Core Specification 4.2 Vol 3. Part H 2.3.5.6.1 */
1385 static const uint8_t ble_sm_alg_dbg_priv_key[32] = {
1386     0x3f, 0x49, 0xf6, 0xd4, 0xa3, 0xc5, 0x5f, 0x38, 0x74, 0xc9, 0xb3, 0xe3,
1387     0xd2, 0x10, 0x3f, 0x50, 0x4a, 0xff, 0x60, 0x7b, 0xeb, 0x40, 0xb7, 0x99,
1388     0x58, 0x99, 0xb8, 0xa6, 0xcd, 0x3c, 0x1a, 0xbd
1389 };
1390 
ble_sm_alg_gen_dhkey(const uint8_t * peer_pub_key_x,const uint8_t * peer_pub_key_y,const uint8_t * our_priv_key,uint8_t * out_dhkey)1391 int ble_sm_alg_gen_dhkey(const uint8_t *peer_pub_key_x, const uint8_t *peer_pub_key_y,
1392                          const uint8_t *our_priv_key, uint8_t *out_dhkey)
1393 {
1394     uint8_t dh[32];
1395     uint8_t pk[64];
1396     uint8_t priv[32];
1397     int rc = BLE_SM_KEY_ERR;
1398 
1399     swap_buf(pk, peer_pub_key_x, 32);
1400     swap_buf(&pk[32], peer_pub_key_y, 32);
1401     swap_buf(priv, our_priv_key, 32);
1402 
1403 #if CONFIG_BT_LE_CRYPTO_STACK_MBEDTLS
1404     struct mbedtls_ecp_point pt = {0}, Q = {0};
1405     mbedtls_mpi z = {0}, d = {0};
1406     mbedtls_ctr_drbg_context ctr_drbg = {0};
1407     mbedtls_entropy_context entropy = {0};
1408 
1409     uint8_t pub[65] = {0};
1410     /* Hardcoded first byte of pub key for MBEDTLS_ECP_PF_UNCOMPRESSED */
1411     pub[0] = 0x04;
1412     memcpy(&pub[1], pk, 64);
1413 
1414     /* Initialize the required structures here */
1415     mbedtls_ecp_point_init(&pt);
1416     mbedtls_ecp_point_init(&Q);
1417     mbedtls_ctr_drbg_init(&ctr_drbg);
1418     mbedtls_entropy_init(&entropy);
1419     mbedtls_mpi_init(&d);
1420     mbedtls_mpi_init(&z);
1421 
1422     /* Below 3 steps are to validate public key on curve secp256r1 */
1423     if (mbedtls_ecp_group_load(&keypair.MBEDTLS_PRIVATE(grp), MBEDTLS_ECP_DP_SECP256R1) != 0) {
1424         goto exit;
1425     }
1426 
1427     if (mbedtls_ecp_point_read_binary(&keypair.MBEDTLS_PRIVATE(grp), &pt, pub, 65) != 0) {
1428         goto exit;
1429     }
1430 
1431     if (mbedtls_ecp_check_pubkey(&keypair.MBEDTLS_PRIVATE(grp), &pt) != 0) {
1432         goto exit;
1433     }
1434 
1435     /* Set PRNG */
1436     if ((rc = mbedtls_ctr_drbg_seed(&ctr_drbg, mbedtls_entropy_func, &entropy, NULL, 0)) != 0) {
1437         goto exit;
1438     }
1439 
1440     /* Prepare point Q from pub key */
1441     if (mbedtls_ecp_point_read_binary(&keypair.MBEDTLS_PRIVATE(grp), &Q, pub, 65) != 0) {
1442         goto exit;
1443     }
1444 
1445     if (mbedtls_mpi_read_binary(&d, priv, 32) != 0) {
1446         goto exit;
1447     }
1448 
1449     rc = mbedtls_ecdh_compute_shared(&keypair.MBEDTLS_PRIVATE(grp), &z, &Q, &d,
1450                                      mbedtls_ctr_drbg_random, &ctr_drbg);
1451     if (rc != 0) {
1452         goto exit;
1453     }
1454 
1455     rc = mbedtls_mpi_write_binary(&z, dh, 32);
1456     if (rc != 0) {
1457         goto exit;
1458     }
1459 
1460 exit:
1461     mbedtls_ecp_point_free(&pt);
1462     mbedtls_mpi_free(&z);
1463     mbedtls_mpi_free(&d);
1464     mbedtls_ecp_point_free(&Q);
1465     mbedtls_entropy_free(&entropy);
1466     mbedtls_ctr_drbg_free(&ctr_drbg);
1467     if (rc != 0) {
1468         return BLE_SM_KEY_ERR;
1469     }
1470 
1471 #else
1472     if (uECC_valid_public_key(pk, &curve_secp256r1) < 0) {
1473         return BLE_SM_KEY_ERR;
1474     }
1475 
1476     rc = uECC_shared_secret(pk, priv, dh, &curve_secp256r1);
1477     if (rc == TC_CRYPTO_FAIL) {
1478         return BLE_SM_KEY_ERR;
1479     }
1480 #endif // CONFIG_BT_LE_CRYPTO_STACK_MBEDTLS
1481 
1482     swap_buf(out_dhkey, dh, 32);
1483     return 0;
1484 }
1485 
1486 #if CONFIG_BT_LE_CRYPTO_STACK_MBEDTLS
mbedtls_gen_keypair(uint8_t * public_key,uint8_t * private_key)1487 static int mbedtls_gen_keypair(uint8_t *public_key, uint8_t *private_key)
1488 {
1489     int rc = BLE_SM_KEY_ERR;
1490     mbedtls_entropy_context entropy = {0};
1491     mbedtls_ctr_drbg_context ctr_drbg = {0};
1492 
1493     mbedtls_entropy_init(&entropy);
1494     mbedtls_ctr_drbg_init(&ctr_drbg);
1495     mbedtls_ecp_keypair_init(&keypair);
1496 
1497     if ((rc = mbedtls_ctr_drbg_seed(&ctr_drbg, mbedtls_entropy_func, &entropy,
1498                                     NULL, 0)) != 0) {
1499         goto exit;
1500     }
1501 
1502     if ((rc = mbedtls_ecp_gen_key(MBEDTLS_ECP_DP_SECP256R1, &keypair,
1503                                   mbedtls_ctr_drbg_random, &ctr_drbg)) != 0) {
1504         goto exit;
1505     }
1506 
1507     if ((rc = mbedtls_mpi_write_binary(&keypair.MBEDTLS_PRIVATE(d), private_key, 32)) != 0) {
1508         goto exit;
1509     }
1510 
1511     size_t olen = 0;
1512     uint8_t pub[65] = {0};
1513 
1514     if ((rc = mbedtls_ecp_point_write_binary(&keypair.MBEDTLS_PRIVATE(grp), &keypair.MBEDTLS_PRIVATE(Q), MBEDTLS_ECP_PF_UNCOMPRESSED,
1515               &olen, pub, 65)) != 0) {
1516         goto exit;
1517     }
1518 
1519     memcpy(public_key, &pub[1], 64);
1520 
1521 exit:
1522     mbedtls_ctr_drbg_free(&ctr_drbg);
1523     mbedtls_entropy_free(&entropy);
1524     if (rc != 0) {
1525         mbedtls_ecp_keypair_free(&keypair);
1526         return BLE_SM_KEY_ERR;
1527     }
1528 
1529     return 0;
1530 }
1531 #endif  // CONFIG_BT_LE_CRYPTO_STACK_MBEDTLS
1532 
1533 /**
1534  * pub: 64 bytes
1535  * priv: 32 bytes
1536  */
ble_sm_alg_gen_key_pair(uint8_t * pub,uint8_t * priv)1537 int ble_sm_alg_gen_key_pair(uint8_t *pub, uint8_t *priv)
1538 {
1539 #if CONFIG_BT_LE_SM_SC_DEBUG_KEYS
1540     swap_buf(pub, ble_sm_alg_dbg_pub_key, 32);
1541     swap_buf(&pub[32], &ble_sm_alg_dbg_pub_key[32], 32);
1542     swap_buf(priv, ble_sm_alg_dbg_priv_key, 32);
1543 #else
1544     uint8_t pk[64];
1545 
1546     do {
1547 #if CONFIG_BT_LE_CRYPTO_STACK_MBEDTLS
1548         if (mbedtls_gen_keypair(pk, priv) != 0) {
1549             return BLE_SM_KEY_ERR;
1550         }
1551 #else
1552         if (uECC_make_key(pk, priv, &curve_secp256r1) != TC_CRYPTO_SUCCESS) {
1553             return BLE_SM_KEY_ERR;
1554         }
1555 #endif  // CONFIG_BT_LE_CRYPTO_STACK_MBEDTLS
1556         /* Make sure generated key isn't debug key. */
1557     } while (memcmp(priv, ble_sm_alg_dbg_priv_key, 32) == 0);
1558 
1559     swap_buf(pub, pk, 32);
1560     swap_buf(&pub[32], &pk[32], 32);
1561     swap_in_place(priv, 32);
1562 #endif // CONFIG_BT_LE_SM_SC_DEBUG_KEYS
1563     return 0;
1564 }
1565 
1566 #endif // CONFIG_BT_LE_SM_LEGACY || CONFIG_BT_LE_SM_SC
1567 #endif // (!CONFIG_BT_NIMBLE_ENABLED) && (CONFIG_BT_CONTROLLER_ENABLED)
1568