xref: /btstack/src/ble/sm.c (revision b3fcedb9c9ccbcc9680da165cd86f394be5d84a8)
1 /*
2  * Copyright (C) 2014 BlueKitchen GmbH
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions
6  * are met:
7  *
8  * 1. Redistributions of source code must retain the above copyright
9  *    notice, this list of conditions and the following disclaimer.
10  * 2. Redistributions in binary form must reproduce the above copyright
11  *    notice, this list of conditions and the following disclaimer in the
12  *    documentation and/or other materials provided with the distribution.
13  * 3. Neither the name of the copyright holders nor the names of
14  *    contributors may be used to endorse or promote products derived
15  *    from this software without specific prior written permission.
16  * 4. Any redistribution, use, or modification is done solely for
17  *    personal benefit and not for any commercial purpose or for
18  *    monetary gain.
19  *
20  * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS
21  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS
24  * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
27  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
28  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
30  * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
31  * SUCH DAMAGE.
32  *
33  * Please inquire about commercial licensing options at
34  * [email protected]
35  *
36  */
37 
38 #include <stdio.h>
39 #include <string.h>
40 #include <inttypes.h>
41 
42 #include "ble/le_device_db.h"
43 #include "ble/core.h"
44 #include "ble/sm.h"
45 #include "btstack_debug.h"
46 #include "btstack_event.h"
47 #include "btstack_linked_list.h"
48 #include "btstack_memory.h"
49 #include "gap.h"
50 #include "hci.h"
51 #include "l2cap.h"
52 
53 //
54 // SM internal types and globals
55 //
56 
57 typedef enum {
58     DKG_W4_WORKING,
59     DKG_CALC_IRK,
60     DKG_W4_IRK,
61     DKG_CALC_DHK,
62     DKG_W4_DHK,
63     DKG_READY
64 } derived_key_generation_t;
65 
66 typedef enum {
67     RAU_W4_WORKING,
68     RAU_IDLE,
69     RAU_GET_RANDOM,
70     RAU_W4_RANDOM,
71     RAU_GET_ENC,
72     RAU_W4_ENC,
73     RAU_SET_ADDRESS,
74 } random_address_update_t;
75 
76 typedef enum {
77     CMAC_IDLE,
78     CMAC_CALC_SUBKEYS,
79     CMAC_W4_SUBKEYS,
80     CMAC_CALC_MI,
81     CMAC_W4_MI,
82     CMAC_CALC_MLAST,
83     CMAC_W4_MLAST
84 } cmac_state_t;
85 
86 typedef enum {
87     JUST_WORKS,
88     PK_RESP_INPUT,  // Initiator displays PK, initiator inputs PK
89     PK_INIT_INPUT,  // Responder displays PK, responder inputs PK
90     OK_BOTH_INPUT,  // Only input on both, both input PK
91     OOB             // OOB available on both sides
92 } stk_generation_method_t;
93 
94 typedef enum {
95     SM_USER_RESPONSE_IDLE,
96     SM_USER_RESPONSE_PENDING,
97     SM_USER_RESPONSE_CONFIRM,
98     SM_USER_RESPONSE_PASSKEY,
99     SM_USER_RESPONSE_DECLINE
100 } sm_user_response_t;
101 
102 typedef enum {
103     SM_AES128_IDLE,
104     SM_AES128_ACTIVE
105 } sm_aes128_state_t;
106 
107 typedef enum {
108     ADDRESS_RESOLUTION_IDLE,
109     ADDRESS_RESOLUTION_GENERAL,
110     ADDRESS_RESOLUTION_FOR_CONNECTION,
111 } address_resolution_mode_t;
112 
113 typedef enum {
114     ADDRESS_RESOLUTION_SUCEEDED,
115     ADDRESS_RESOLUTION_FAILED,
116 } address_resolution_event_t;
117 //
118 // GLOBAL DATA
119 //
120 
121 static uint8_t test_use_fixed_local_csrk;
122 
123 // configuration
124 static uint8_t sm_accepted_stk_generation_methods;
125 static uint8_t sm_max_encryption_key_size;
126 static uint8_t sm_min_encryption_key_size;
127 static uint8_t sm_auth_req = 0;
128 static uint8_t sm_io_capabilities = IO_CAPABILITY_NO_INPUT_NO_OUTPUT;
129 static uint8_t sm_slave_request_security;
130 
131 // Security Manager Master Keys, please use sm_set_er(er) and sm_set_ir(ir) with your own 128 bit random values
132 static sm_key_t sm_persistent_er;
133 static sm_key_t sm_persistent_ir;
134 
135 // derived from sm_persistent_ir
136 static sm_key_t sm_persistent_dhk;
137 static sm_key_t sm_persistent_irk;
138 static uint8_t  sm_persistent_irk_ready = 0;    // used for testing
139 static derived_key_generation_t dkg_state;
140 
141 // derived from sm_persistent_er
142 // ..
143 
144 // random address update
145 static random_address_update_t rau_state;
146 static bd_addr_t sm_random_address;
147 
148 // CMAC calculation
149 static cmac_state_t sm_cmac_state;
150 static sm_key_t     sm_cmac_k;
151 static uint8_t      sm_cmac_header[3];
152 static uint16_t     sm_cmac_message_len;
153 static uint8_t *    sm_cmac_message;
154 static uint8_t      sm_cmac_sign_counter[4];
155 static sm_key_t     sm_cmac_m_last;
156 static sm_key_t     sm_cmac_x;
157 static uint8_t      sm_cmac_block_current;
158 static uint8_t      sm_cmac_block_count;
159 static void (*sm_cmac_done_handler)(uint8_t hash[8]);
160 
161 // resolvable private address lookup / CSRK calculation
162 static int       sm_address_resolution_test;
163 static int       sm_address_resolution_ah_calculation_active;
164 static uint8_t   sm_address_resolution_addr_type;
165 static bd_addr_t sm_address_resolution_address;
166 static void *    sm_address_resolution_context;
167 static address_resolution_mode_t sm_address_resolution_mode;
168 static btstack_linked_list_t sm_address_resolution_general_queue;
169 
170 // aes128 crypto engine. store current sm_connection_t in sm_aes128_context
171 static sm_aes128_state_t  sm_aes128_state;
172 static void *             sm_aes128_context;
173 
174 // random engine. store context (ususally sm_connection_t)
175 static void * sm_random_context;
176 
177 // to receive hci events
178 static btstack_packet_callback_registration_t hci_event_callback_registration;
179 
180 /* to dispatch sm event */
181 static btstack_linked_list_t sm_event_handlers;
182 
183 //
184 // Volume 3, Part H, Chapter 24
185 // "Security shall be initiated by the Security Manager in the device in the master role.
186 // The device in the slave role shall be the responding device."
187 // -> master := initiator, slave := responder
188 //
189 
190 // data needed for security setup
191 typedef struct sm_setup_context {
192 
193     btstack_timer_source_t sm_timeout;
194 
195     // used in all phases
196     uint8_t   sm_pairing_failed_reason;
197 
198     // user response, (Phase 1 and/or 2)
199     uint8_t   sm_user_response;
200 
201     // defines which keys will be send after connection is encrypted - calculated during Phase 1, used Phase 3
202     int       sm_key_distribution_send_set;
203     int       sm_key_distribution_received_set;
204 
205     // Phase 2 (Pairing over SMP)
206     stk_generation_method_t sm_stk_generation_method;
207     sm_key_t  sm_tk;
208 
209     sm_key_t  sm_c1_t3_value;   // c1 calculation
210     sm_pairing_packet_t sm_m_preq; // pairing request - needed only for c1
211     sm_pairing_packet_t sm_s_pres; // pairing response - needed only for c1
212     sm_key_t  sm_local_random;
213     sm_key_t  sm_local_confirm;
214     sm_key_t  sm_peer_random;
215     sm_key_t  sm_peer_confirm;
216     uint8_t   sm_m_addr_type;   // address and type can be removed
217     uint8_t   sm_s_addr_type;   //  ''
218     bd_addr_t sm_m_address;     //  ''
219     bd_addr_t sm_s_address;     //  ''
220     sm_key_t  sm_ltk;
221 
222     // Phase 3
223 
224     // key distribution, we generate
225     uint16_t  sm_local_y;
226     uint16_t  sm_local_div;
227     uint16_t  sm_local_ediv;
228     uint8_t   sm_local_rand[8];
229     sm_key_t  sm_local_ltk;
230     sm_key_t  sm_local_csrk;
231     sm_key_t  sm_local_irk;
232     // sm_local_address/addr_type not needed
233 
234     // key distribution, received from peer
235     uint16_t  sm_peer_y;
236     uint16_t  sm_peer_div;
237     uint16_t  sm_peer_ediv;
238     uint8_t   sm_peer_rand[8];
239     sm_key_t  sm_peer_ltk;
240     sm_key_t  sm_peer_irk;
241     sm_key_t  sm_peer_csrk;
242     uint8_t   sm_peer_addr_type;
243     bd_addr_t sm_peer_address;
244 
245 } sm_setup_context_t;
246 
247 //
248 static sm_setup_context_t the_setup;
249 static sm_setup_context_t * setup = &the_setup;
250 
251 // active connection - the one for which the_setup is used for
252 static uint16_t sm_active_connection = 0;
253 
254 // @returns 1 if oob data is available
255 // stores oob data in provided 16 byte buffer if not null
256 static int (*sm_get_oob_data)(uint8_t addres_type, bd_addr_t addr, uint8_t * oob_data) = NULL;
257 
258 // used to notify applicationss that user interaction is neccessary, see sm_notify_t below
259 static btstack_packet_handler_t sm_client_packet_handler = NULL;
260 
261 // horizontal: initiator capabilities
262 // vertial:    responder capabilities
263 static const stk_generation_method_t stk_generation_method[5][5] = {
264     { JUST_WORKS,      JUST_WORKS,       PK_INIT_INPUT,   JUST_WORKS,    PK_INIT_INPUT },
265     { JUST_WORKS,      JUST_WORKS,       PK_INIT_INPUT,   JUST_WORKS,    PK_INIT_INPUT },
266     { PK_RESP_INPUT,   PK_RESP_INPUT,    OK_BOTH_INPUT,   JUST_WORKS,    PK_RESP_INPUT },
267     { JUST_WORKS,      JUST_WORKS,       JUST_WORKS,      JUST_WORKS,    JUST_WORKS    },
268     { PK_RESP_INPUT,   PK_RESP_INPUT,    PK_INIT_INPUT,   JUST_WORKS,    PK_RESP_INPUT },
269 };
270 
271 static void sm_run(void);
272 static void sm_done_for_handle(hci_con_handle_t con_handle);
273 static sm_connection_t * sm_get_connection_for_handle(hci_con_handle_t con_handle);
274 static inline int sm_calc_actual_encryption_key_size(int other);
275 static int sm_validate_stk_generation_method(void);
276 
277 static void log_info_hex16(const char * name, uint16_t value){
278     log_info("%-6s 0x%04x", name, value);
279 }
280 
281 // @returns 1 if all bytes are 0
282 static int sm_is_null_random(uint8_t random[8]){
283     int i;
284     for (i=0; i < 8 ; i++){
285         if (random[i]) return 0;
286     }
287     return 1;
288 }
289 
290 // Key utils
291 static void sm_reset_tk(void){
292     int i;
293     for (i=0;i<16;i++){
294         setup->sm_tk[i] = 0;
295     }
296 }
297 
298 // "For example, if a 128-bit encryption key is 0x123456789ABCDEF0123456789ABCDEF0
299 // and it is reduced to 7 octets (56 bits), then the resulting key is 0x0000000000000000003456789ABCDEF0.""
300 static void sm_truncate_key(sm_key_t key, int max_encryption_size){
301     int i;
302     for (i = max_encryption_size ; i < 16 ; i++){
303         key[15-i] = 0;
304     }
305 }
306 
307 // SMP Timeout implementation
308 
309 // Upon transmission of the Pairing Request command or reception of the Pairing Request command,
310 // the Security Manager Timer shall be reset and started.
311 //
312 // The Security Manager Timer shall be reset when an L2CAP SMP command is queued for transmission.
313 //
314 // If the Security Manager Timer reaches 30 seconds, the procedure shall be considered to have failed,
315 // and the local higher layer shall be notified. No further SMP commands shall be sent over the L2CAP
316 // Security Manager Channel. A new SM procedure shall only be performed when a new physical link has been
317 // established.
318 
319 static void sm_timeout_handler(btstack_timer_source_t * timer){
320     log_info("SM timeout");
321     sm_connection_t * sm_conn = (sm_connection_t*) btstack_run_loop_get_timer_context(timer);
322     sm_conn->sm_engine_state = SM_GENERAL_TIMEOUT;
323     sm_done_for_handle(sm_conn->sm_handle);
324 
325     // trigger handling of next ready connection
326     sm_run();
327 }
328 static void sm_timeout_start(sm_connection_t * sm_conn){
329     btstack_run_loop_remove_timer(&setup->sm_timeout);
330     btstack_run_loop_set_timer_context(&setup->sm_timeout, sm_conn);
331     btstack_run_loop_set_timer_handler(&setup->sm_timeout, sm_timeout_handler);
332     btstack_run_loop_set_timer(&setup->sm_timeout, 30000); // 30 seconds sm timeout
333     btstack_run_loop_add_timer(&setup->sm_timeout);
334 }
335 static void sm_timeout_stop(void){
336     btstack_run_loop_remove_timer(&setup->sm_timeout);
337 }
338 static void sm_timeout_reset(sm_connection_t * sm_conn){
339     sm_timeout_stop();
340     sm_timeout_start(sm_conn);
341 }
342 
343 // end of sm timeout
344 
345 // GAP Random Address updates
346 static gap_random_address_type_t gap_random_adress_type;
347 static btstack_timer_source_t gap_random_address_update_timer;
348 static uint32_t gap_random_adress_update_period;
349 
350 static void gap_random_address_trigger(void){
351     if (rau_state != RAU_IDLE) return;
352     log_info("gap_random_address_trigger");
353     rau_state = RAU_GET_RANDOM;
354     sm_run();
355 }
356 
357 static void gap_random_address_update_handler(btstack_timer_source_t * timer){
358     log_info("GAP Random Address Update due");
359     btstack_run_loop_set_timer(&gap_random_address_update_timer, gap_random_adress_update_period);
360     btstack_run_loop_add_timer(&gap_random_address_update_timer);
361     gap_random_address_trigger();
362 }
363 
364 static void gap_random_address_update_start(void){
365     btstack_run_loop_set_timer_handler(&gap_random_address_update_timer, gap_random_address_update_handler);
366     btstack_run_loop_set_timer(&gap_random_address_update_timer, gap_random_adress_update_period);
367     btstack_run_loop_add_timer(&gap_random_address_update_timer);
368 }
369 
370 static void gap_random_address_update_stop(void){
371     btstack_run_loop_remove_timer(&gap_random_address_update_timer);
372 }
373 
374 
375 static void sm_random_start(void * context){
376     sm_random_context = context;
377     hci_send_cmd(&hci_le_rand);
378 }
379 
380 // pre: sm_aes128_state != SM_AES128_ACTIVE, hci_can_send_command == 1
381 // context is made availabe to aes128 result handler by this
382 static void sm_aes128_start(sm_key_t key, sm_key_t plaintext, void * context){
383     sm_aes128_state = SM_AES128_ACTIVE;
384     sm_key_t key_flipped, plaintext_flipped;
385     reverse_128(key, key_flipped);
386     reverse_128(plaintext, plaintext_flipped);
387     sm_aes128_context = context;
388     hci_send_cmd(&hci_le_encrypt, key_flipped, plaintext_flipped);
389 }
390 
391 // ah(k,r) helper
392 // r = padding || r
393 // r - 24 bit value
394 static void sm_ah_r_prime(uint8_t r[3], sm_key_t r_prime){
395     // r'= padding || r
396     memset(r_prime, 0, 16);
397     memcpy(&r_prime[13], r, 3);
398 }
399 
400 // d1 helper
401 // d' = padding || r || d
402 // d,r - 16 bit values
403 static void sm_d1_d_prime(uint16_t d, uint16_t r, sm_key_t d1_prime){
404     // d'= padding || r || d
405     memset(d1_prime, 0, 16);
406     big_endian_store_16(d1_prime, 12, r);
407     big_endian_store_16(d1_prime, 14, d);
408 }
409 
410 // dm helper
411 // r’ = padding || r
412 // r - 64 bit value
413 static void sm_dm_r_prime(uint8_t r[8], sm_key_t r_prime){
414     memset(r_prime, 0, 16);
415     memcpy(&r_prime[8], r, 8);
416 }
417 
418 // calculate arguments for first AES128 operation in C1 function
419 static void sm_c1_t1(sm_key_t r, uint8_t preq[7], uint8_t pres[7], uint8_t iat, uint8_t rat, sm_key_t t1){
420 
421     // p1 = pres || preq || rat’ || iat’
422     // "The octet of iat’ becomes the least significant octet of p1 and the most signifi-
423     // cant octet of pres becomes the most significant octet of p1.
424     // For example, if the 8-bit iat’ is 0x01, the 8-bit rat’ is 0x00, the 56-bit preq
425     // is 0x07071000000101 and the 56 bit pres is 0x05000800000302 then
426     // p1 is 0x05000800000302070710000001010001."
427 
428     sm_key_t p1;
429     reverse_56(pres, &p1[0]);
430     reverse_56(preq, &p1[7]);
431     p1[14] = rat;
432     p1[15] = iat;
433     log_info_key("p1", p1);
434     log_info_key("r", r);
435 
436     // t1 = r xor p1
437     int i;
438     for (i=0;i<16;i++){
439         t1[i] = r[i] ^ p1[i];
440     }
441     log_info_key("t1", t1);
442 }
443 
444 // calculate arguments for second AES128 operation in C1 function
445 static void sm_c1_t3(sm_key_t t2, bd_addr_t ia, bd_addr_t ra, sm_key_t t3){
446      // p2 = padding || ia || ra
447     // "The least significant octet of ra becomes the least significant octet of p2 and
448     // the most significant octet of padding becomes the most significant octet of p2.
449     // For example, if 48-bit ia is 0xA1A2A3A4A5A6 and the 48-bit ra is
450     // 0xB1B2B3B4B5B6 then p2 is 0x00000000A1A2A3A4A5A6B1B2B3B4B5B6.
451 
452     sm_key_t p2;
453     memset(p2, 0, 16);
454     memcpy(&p2[4],  ia, 6);
455     memcpy(&p2[10], ra, 6);
456     log_info_key("p2", p2);
457 
458     // c1 = e(k, t2_xor_p2)
459     int i;
460     for (i=0;i<16;i++){
461         t3[i] = t2[i] ^ p2[i];
462     }
463     log_info_key("t3", t3);
464 }
465 
466 static void sm_s1_r_prime(sm_key_t r1, sm_key_t r2, sm_key_t r_prime){
467     log_info_key("r1", r1);
468     log_info_key("r2", r2);
469     memcpy(&r_prime[8], &r2[8], 8);
470     memcpy(&r_prime[0], &r1[8], 8);
471 }
472 
473 static void sm_setup_event_base(uint8_t * event, int event_size, uint8_t type, hci_con_handle_t con_handle, uint8_t addr_type, bd_addr_t address){
474     event[0] = type;
475     event[1] = event_size - 2;
476     little_endian_store_16(event, 2, con_handle);
477     event[4] = addr_type;
478     reverse_bd_addr(address, &event[5]);
479 }
480 
481 static void sm_dispatch_event(uint8_t packet_type, uint16_t channel, uint8_t * packet, uint16_t size){
482     if (sm_client_packet_handler) {
483         sm_client_packet_handler(HCI_EVENT_PACKET, 0, packet, size);
484     }
485     // dispatch to all event handlers
486     btstack_linked_list_iterator_t it;
487     btstack_linked_list_iterator_init(&it, &sm_event_handlers);
488     while (btstack_linked_list_iterator_has_next(&it)){
489         btstack_packet_callback_registration_t * entry = (btstack_packet_callback_registration_t*) btstack_linked_list_iterator_next(&it);
490         entry->callback(packet_type, 0, packet, size);
491     }
492 }
493 
494 static void sm_notify_client_base(uint8_t type, hci_con_handle_t con_handle, uint8_t addr_type, bd_addr_t address){
495     uint8_t event[11];
496     sm_setup_event_base(event, sizeof(event), type, con_handle, addr_type, address);
497     sm_dispatch_event(HCI_EVENT_PACKET, 0, event, sizeof(event));
498 }
499 
500 static void sm_notify_client_passkey(uint8_t type, hci_con_handle_t con_handle, uint8_t addr_type, bd_addr_t address, uint32_t passkey){
501     uint8_t event[15];
502     sm_setup_event_base(event, sizeof(event), type, con_handle, addr_type, address);
503     little_endian_store_32(event, 11, passkey);
504     sm_dispatch_event(HCI_EVENT_PACKET, 0, event, sizeof(event));
505 }
506 
507 static void sm_notify_client_index(uint8_t type, hci_con_handle_t con_handle, uint8_t addr_type, bd_addr_t address, uint16_t index){
508     uint8_t event[13];
509     sm_setup_event_base(event, sizeof(event), type, con_handle, addr_type, address);
510     little_endian_store_16(event, 11, index);
511     sm_dispatch_event(HCI_EVENT_PACKET, 0, event, sizeof(event));
512 }
513 
514 static void sm_notify_client_authorization(uint8_t type, hci_con_handle_t con_handle, uint8_t addr_type, bd_addr_t address, uint8_t result){
515 
516     uint8_t event[18];
517     sm_setup_event_base(event, sizeof(event), type, con_handle, addr_type, address);
518     event[11] = result;
519     sm_dispatch_event(HCI_EVENT_PACKET, 0, (uint8_t*) &event, sizeof(event));
520 }
521 
522 // decide on stk generation based on
523 // - pairing request
524 // - io capabilities
525 // - OOB data availability
526 static void sm_setup_tk(void){
527 
528     // default: just works
529     setup->sm_stk_generation_method = JUST_WORKS;
530 
531     // If both devices have out of band authentication data, then the Authentication
532     // Requirements Flags shall be ignored when selecting the pairing method and the
533     // Out of Band pairing method shall be used.
534     if (setup->sm_m_preq.oob_data_flag && setup->sm_s_pres.oob_data_flag){
535         log_info("SM: have OOB data");
536         log_info_key("OOB", setup->sm_tk);
537         setup->sm_stk_generation_method = OOB;
538         return;
539     }
540 
541     // Reset TK as it has been setup in sm_init_setup
542     sm_reset_tk();
543 
544     // If both devices have not set the MITM option in the Authentication Requirements
545     // Flags, then the IO capabilities shall be ignored and the Just Works association
546     // model shall be used.
547     if ( ((setup->sm_m_preq.auth_req & SM_AUTHREQ_MITM_PROTECTION) == 0x00) && ((setup->sm_s_pres.auth_req & SM_AUTHREQ_MITM_PROTECTION) == 0)){
548         return;
549     }
550 
551     // Also use just works if unknown io capabilites
552     if ((setup->sm_m_preq.io_capability > IO_CAPABILITY_KEYBOARD_DISPLAY) || (setup->sm_m_preq.io_capability > IO_CAPABILITY_KEYBOARD_DISPLAY)){
553         return;
554     }
555 
556     // Otherwise the IO capabilities of the devices shall be used to determine the
557     // pairing method as defined in Table 2.4.
558     setup->sm_stk_generation_method = stk_generation_method[setup->sm_s_pres.io_capability][setup->sm_m_preq.io_capability];
559     log_info("sm_setup_tk: master io cap: %u, slave io cap: %u -> method %u",
560         setup->sm_m_preq.io_capability, setup->sm_s_pres.io_capability, setup->sm_stk_generation_method);
561 }
562 
563 static int sm_key_distribution_flags_for_set(uint8_t key_set){
564     int flags = 0;
565     if (key_set & SM_KEYDIST_ENC_KEY){
566         flags |= SM_KEYDIST_FLAG_ENCRYPTION_INFORMATION;
567         flags |= SM_KEYDIST_FLAG_MASTER_IDENTIFICATION;
568     }
569     if (key_set & SM_KEYDIST_ID_KEY){
570         flags |= SM_KEYDIST_FLAG_IDENTITY_INFORMATION;
571         flags |= SM_KEYDIST_FLAG_IDENTITY_ADDRESS_INFORMATION;
572     }
573     if (key_set & SM_KEYDIST_SIGN){
574         flags |= SM_KEYDIST_FLAG_SIGNING_IDENTIFICATION;
575     }
576     return flags;
577 }
578 
579 static void sm_setup_key_distribution(uint8_t key_set){
580     setup->sm_key_distribution_received_set = 0;
581     setup->sm_key_distribution_send_set = sm_key_distribution_flags_for_set(key_set);
582 }
583 
584 // CSRK Key Lookup
585 
586 
587 static int sm_address_resolution_idle(void){
588     return sm_address_resolution_mode == ADDRESS_RESOLUTION_IDLE;
589 }
590 
591 static void sm_address_resolution_start_lookup(uint8_t addr_type, hci_con_handle_t con_handle, bd_addr_t addr, address_resolution_mode_t mode, void * context){
592     memcpy(sm_address_resolution_address, addr, 6);
593     sm_address_resolution_addr_type = addr_type;
594     sm_address_resolution_test = 0;
595     sm_address_resolution_mode = mode;
596     sm_address_resolution_context = context;
597     sm_notify_client_base(SM_EVENT_IDENTITY_RESOLVING_STARTED, con_handle, addr_type, addr);
598 }
599 
600 int sm_address_resolution_lookup(uint8_t address_type, bd_addr_t address){
601     // check if already in list
602     btstack_linked_list_iterator_t it;
603     sm_lookup_entry_t * entry;
604     btstack_linked_list_iterator_init(&it, &sm_address_resolution_general_queue);
605     while(btstack_linked_list_iterator_has_next(&it)){
606         entry = (sm_lookup_entry_t *) btstack_linked_list_iterator_next(&it);
607         if (entry->address_type != address_type) continue;
608         if (memcmp(entry->address, address, 6))  continue;
609         // already in list
610         return BTSTACK_BUSY;
611     }
612     entry = btstack_memory_sm_lookup_entry_get();
613     if (!entry) return BTSTACK_MEMORY_ALLOC_FAILED;
614     entry->address_type = (bd_addr_type_t) address_type;
615     memcpy(entry->address, address, 6);
616     btstack_linked_list_add(&sm_address_resolution_general_queue, (btstack_linked_item_t *) entry);
617     sm_run();
618     return 0;
619 }
620 
621 // CMAC Implementation using AES128 engine
622 static void sm_shift_left_by_one_bit_inplace(int len, uint8_t * data){
623     int i;
624     int carry = 0;
625     for (i=len-1; i >= 0 ; i--){
626         int new_carry = data[i] >> 7;
627         data[i] = data[i] << 1 | carry;
628         carry = new_carry;
629     }
630 }
631 
632 // while x_state++ for an enum is possible in C, it isn't in C++. we use this helpers to avoid compile errors for now
633 static inline void sm_next_responding_state(sm_connection_t * sm_conn){
634     sm_conn->sm_engine_state = (security_manager_state_t) (((int)sm_conn->sm_engine_state) + 1);
635 }
636 static inline void dkg_next_state(void){
637     dkg_state = (derived_key_generation_t) (((int)dkg_state) + 1);
638 }
639 static inline void rau_next_state(void){
640     rau_state = (random_address_update_t) (((int)rau_state) + 1);
641 }
642 static inline void sm_cmac_next_state(void){
643     sm_cmac_state = (cmac_state_t) (((int)sm_cmac_state) + 1);
644 }
645 static int sm_cmac_last_block_complete(void){
646     if (sm_cmac_message_len == 0) return 0;
647     return (sm_cmac_message_len & 0x0f) == 0;
648 }
649 static inline uint8_t sm_cmac_message_get_byte(int offset){
650     if (offset >= sm_cmac_message_len) {
651         log_error("sm_cmac_message_get_byte. out of bounds, access %u, len %u", offset, sm_cmac_message_len);
652         return 0;
653     }
654 
655     offset = sm_cmac_message_len - 1 - offset;
656 
657     // sm_cmac_header[3] | message[] | sm_cmac_sign_counter[4]
658     if (offset < 3){
659         return sm_cmac_header[offset];
660     }
661     int actual_message_len_incl_header = sm_cmac_message_len - 4;
662     if (offset <  actual_message_len_incl_header){
663         return sm_cmac_message[offset - 3];
664     }
665     return sm_cmac_sign_counter[offset - actual_message_len_incl_header];
666 }
667 
668 void sm_cmac_start(sm_key_t k, uint8_t opcode, hci_con_handle_t con_handle, uint16_t message_len, uint8_t * message, uint32_t sign_counter, void (*done_handler)(uint8_t hash[8])){
669     memcpy(sm_cmac_k, k, 16);
670     sm_cmac_header[0] = opcode;
671     little_endian_store_16(sm_cmac_header, 1, con_handle);
672     little_endian_store_32(sm_cmac_sign_counter, 0, sign_counter);
673     sm_cmac_message_len = 3 + message_len + 4;  // incl. virtually prepended att opcode, handle and appended sign_counter in LE
674     sm_cmac_message = message;
675     sm_cmac_done_handler = done_handler;
676     sm_cmac_block_current = 0;
677     memset(sm_cmac_x, 0, 16);
678 
679     // step 2: n := ceil(len/const_Bsize);
680     sm_cmac_block_count = (sm_cmac_message_len + 15) / 16;
681 
682     // step 3: ..
683     if (sm_cmac_block_count==0){
684         sm_cmac_block_count = 1;
685     }
686 
687     log_info("sm_cmac_start: len %u, block count %u", sm_cmac_message_len, sm_cmac_block_count);
688 
689     // first, we need to compute l for k1, k2, and m_last
690     sm_cmac_state = CMAC_CALC_SUBKEYS;
691 
692     // let's go
693     sm_run();
694 }
695 
696 int sm_cmac_ready(void){
697     return sm_cmac_state == CMAC_IDLE;
698 }
699 
700 static void sm_cmac_handle_aes_engine_ready(void){
701     switch (sm_cmac_state){
702         case CMAC_CALC_SUBKEYS: {
703             sm_key_t const_zero;
704             memset(const_zero, 0, 16);
705             sm_cmac_next_state();
706             sm_aes128_start(sm_cmac_k, const_zero, NULL);
707             break;
708         }
709         case CMAC_CALC_MI: {
710             int j;
711             sm_key_t y;
712             for (j=0;j<16;j++){
713                 y[j] = sm_cmac_x[j] ^ sm_cmac_message_get_byte(sm_cmac_block_current*16 + j);
714             }
715             sm_cmac_block_current++;
716             sm_cmac_next_state();
717             sm_aes128_start(sm_cmac_k, y, NULL);
718             break;
719         }
720         case CMAC_CALC_MLAST: {
721             int i;
722             sm_key_t y;
723             for (i=0;i<16;i++){
724                 y[i] = sm_cmac_x[i] ^ sm_cmac_m_last[i];
725             }
726             log_info_key("Y", y);
727             sm_cmac_block_current++;
728             sm_cmac_next_state();
729             sm_aes128_start(sm_cmac_k, y, NULL);
730             break;
731         }
732         default:
733             log_info("sm_cmac_handle_aes_engine_ready called in state %u", sm_cmac_state);
734             break;
735     }
736 }
737 
738 static void sm_cmac_handle_encryption_result(sm_key_t data){
739     switch (sm_cmac_state){
740         case CMAC_W4_SUBKEYS: {
741             sm_key_t k1;
742             memcpy(k1, data, 16);
743             sm_shift_left_by_one_bit_inplace(16, k1);
744             if (data[0] & 0x80){
745                 k1[15] ^= 0x87;
746             }
747             sm_key_t k2;
748             memcpy(k2, k1, 16);
749             sm_shift_left_by_one_bit_inplace(16, k2);
750             if (k1[0] & 0x80){
751                 k2[15] ^= 0x87;
752             }
753 
754             log_info_key("k", sm_cmac_k);
755             log_info_key("k1", k1);
756             log_info_key("k2", k2);
757 
758             // step 4: set m_last
759             int i;
760             if (sm_cmac_last_block_complete()){
761                 for (i=0;i<16;i++){
762                     sm_cmac_m_last[i] = sm_cmac_message_get_byte(sm_cmac_message_len - 16 + i) ^ k1[i];
763                 }
764             } else {
765                 int valid_octets_in_last_block = sm_cmac_message_len & 0x0f;
766                 for (i=0;i<16;i++){
767                     if (i < valid_octets_in_last_block){
768                         sm_cmac_m_last[i] = sm_cmac_message_get_byte((sm_cmac_message_len & 0xfff0) + i) ^ k2[i];
769                         continue;
770                     }
771                     if (i == valid_octets_in_last_block){
772                         sm_cmac_m_last[i] = 0x80 ^ k2[i];
773                         continue;
774                     }
775                     sm_cmac_m_last[i] = k2[i];
776                 }
777             }
778 
779             // next
780             sm_cmac_state = sm_cmac_block_current < sm_cmac_block_count - 1 ? CMAC_CALC_MI : CMAC_CALC_MLAST;
781             break;
782         }
783         case CMAC_W4_MI:
784             memcpy(sm_cmac_x, data, 16);
785             sm_cmac_state = sm_cmac_block_current < sm_cmac_block_count - 1 ? CMAC_CALC_MI : CMAC_CALC_MLAST;
786             break;
787         case CMAC_W4_MLAST:
788             // done
789             log_info_key("CMAC", data);
790             sm_cmac_done_handler(data);
791             sm_cmac_state = CMAC_IDLE;
792             break;
793         default:
794             log_info("sm_cmac_handle_encryption_result called in state %u", sm_cmac_state);
795             break;
796     }
797 }
798 
799 static void sm_trigger_user_response(sm_connection_t * sm_conn){
800     // notify client for: JUST WORKS confirm, PASSKEY display or input
801     setup->sm_user_response = SM_USER_RESPONSE_IDLE;
802     switch (setup->sm_stk_generation_method){
803         case PK_RESP_INPUT:
804             if (sm_conn->sm_role){
805                 setup->sm_user_response = SM_USER_RESPONSE_PENDING;
806                 sm_notify_client_base(SM_EVENT_PASSKEY_INPUT_NUMBER, sm_conn->sm_handle, sm_conn->sm_peer_addr_type, sm_conn->sm_peer_address);
807             } else {
808                 sm_notify_client_passkey(SM_EVENT_PASSKEY_DISPLAY_NUMBER, sm_conn->sm_handle, sm_conn->sm_peer_addr_type, sm_conn->sm_peer_address, big_endian_read_32(setup->sm_tk, 12));
809             }
810             break;
811         case PK_INIT_INPUT:
812             if (sm_conn->sm_role){
813                 sm_notify_client_passkey(SM_EVENT_PASSKEY_DISPLAY_NUMBER, sm_conn->sm_handle, sm_conn->sm_peer_addr_type, sm_conn->sm_peer_address, big_endian_read_32(setup->sm_tk, 12));
814             } else {
815                 setup->sm_user_response = SM_USER_RESPONSE_PENDING;
816                 sm_notify_client_base(SM_EVENT_PASSKEY_INPUT_NUMBER, sm_conn->sm_handle, sm_conn->sm_peer_addr_type, sm_conn->sm_peer_address);
817             }
818             break;
819         case OK_BOTH_INPUT:
820             setup->sm_user_response = SM_USER_RESPONSE_PENDING;
821             sm_notify_client_base(SM_EVENT_PASSKEY_INPUT_NUMBER, sm_conn->sm_handle, sm_conn->sm_peer_addr_type, sm_conn->sm_peer_address);
822             break;
823         case JUST_WORKS:
824             setup->sm_user_response = SM_USER_RESPONSE_PENDING;
825             sm_notify_client_base(SM_EVENT_JUST_WORKS_REQUEST, sm_conn->sm_handle, sm_conn->sm_peer_addr_type, sm_conn->sm_peer_address);
826             break;
827         case OOB:
828             // client already provided OOB data, let's skip notification.
829             break;
830     }
831 }
832 
833 static int sm_key_distribution_all_received(sm_connection_t * sm_conn){
834     int recv_flags;
835     if (sm_conn->sm_role){
836         // slave / responser
837         recv_flags = sm_key_distribution_flags_for_set(setup->sm_s_pres.initiator_key_distribution);
838     } else {
839         // master / initiator
840         recv_flags = sm_key_distribution_flags_for_set(setup->sm_s_pres.responder_key_distribution);
841     }
842     log_debug("sm_key_distribution_all_received: received 0x%02x, expecting 0x%02x", setup->sm_key_distribution_received_set, recv_flags);
843     return recv_flags == setup->sm_key_distribution_received_set;
844 }
845 
846 static void sm_done_for_handle(hci_con_handle_t con_handle){
847     if (sm_active_connection == con_handle){
848         sm_timeout_stop();
849         sm_active_connection = 0;
850         log_info("sm: connection 0x%x released setup context", con_handle);
851     }
852 }
853 
854 static int sm_key_distribution_flags_for_auth_req(void){
855     int flags = SM_KEYDIST_ID_KEY | SM_KEYDIST_SIGN;
856     if (sm_auth_req & SM_AUTHREQ_BONDING){
857         // encryption information only if bonding requested
858         flags |= SM_KEYDIST_ENC_KEY;
859     }
860     return flags;
861 }
862 
863 static void sm_init_setup(sm_connection_t * sm_conn){
864 
865     // fill in sm setup
866     sm_reset_tk();
867     setup->sm_peer_addr_type = sm_conn->sm_peer_addr_type;
868     memcpy(setup->sm_peer_address, sm_conn->sm_peer_address, 6);
869 
870     // query client for OOB data
871     int have_oob_data = 0;
872     if (sm_get_oob_data) {
873         have_oob_data = (*sm_get_oob_data)(sm_conn->sm_peer_addr_type, sm_conn->sm_peer_address, setup->sm_tk);
874     }
875 
876     sm_pairing_packet_t * local_packet;
877     if (sm_conn->sm_role){
878         // slave
879         local_packet = &setup->sm_s_pres;
880         gap_advertisements_get_address(&setup->sm_s_addr_type, setup->sm_s_address);
881         setup->sm_m_addr_type = sm_conn->sm_peer_addr_type;
882         memcpy(setup->sm_m_address, sm_conn->sm_peer_address, 6);
883     } else {
884         // master
885         local_packet = &setup->sm_m_preq;
886         gap_advertisements_get_address(&setup->sm_m_addr_type, setup->sm_m_address);
887         setup->sm_s_addr_type = sm_conn->sm_peer_addr_type;
888         memcpy(setup->sm_s_address, sm_conn->sm_peer_address, 6);
889 
890         int key_distribution_flags = sm_key_distribution_flags_for_auth_req();
891         setup->sm_m_preq.initiator_key_distribution = key_distribution_flags;
892         setup->sm_m_preq.responder_key_distribution = key_distribution_flags;
893     }
894 
895     local_packet->io_capability = sm_io_capabilities;
896     local_packet->oob_data_flag = have_oob_data;
897     local_packet->auth_req = sm_auth_req;
898     local_packet->max_encryption_key_size = sm_max_encryption_key_size;
899 }
900 
901 static int sm_stk_generation_init(sm_connection_t * sm_conn){
902 
903     sm_pairing_packet_t * remote_packet;
904     int                   remote_key_request;
905     if (sm_conn->sm_role){
906         // slave / responser
907         remote_packet      = &setup->sm_m_preq;
908         remote_key_request = setup->sm_m_preq.responder_key_distribution;
909     } else {
910         // master / initiator
911         remote_packet      = &setup->sm_s_pres;
912         remote_key_request = setup->sm_s_pres.initiator_key_distribution;
913     }
914 
915     // check key size
916     sm_conn->sm_actual_encryption_key_size = sm_calc_actual_encryption_key_size(remote_packet->max_encryption_key_size);
917     if (sm_conn->sm_actual_encryption_key_size == 0) return SM_REASON_ENCRYPTION_KEY_SIZE;
918 
919     // setup key distribution
920     sm_setup_key_distribution(remote_key_request);
921 
922     // identical to responder
923 
924     // decide on STK generation method
925     sm_setup_tk();
926     log_info("SMP: generation method %u", setup->sm_stk_generation_method);
927 
928     // check if STK generation method is acceptable by client
929     if (!sm_validate_stk_generation_method()) return SM_REASON_AUTHENTHICATION_REQUIREMENTS;
930 
931     // JUST WORKS doens't provide authentication
932     sm_conn->sm_connection_authenticated = setup->sm_stk_generation_method == JUST_WORKS ? 0 : 1;
933 
934     return 0;
935 }
936 
937 static void sm_address_resolution_handle_event(address_resolution_event_t event){
938 
939     // cache and reset context
940     int matched_device_id = sm_address_resolution_test;
941     address_resolution_mode_t mode = sm_address_resolution_mode;
942     void * context = sm_address_resolution_context;
943 
944     // reset context
945     sm_address_resolution_mode = ADDRESS_RESOLUTION_IDLE;
946     sm_address_resolution_context = NULL;
947     sm_address_resolution_test = -1;
948     hci_con_handle_t con_handle = 0;
949 
950     sm_connection_t * sm_connection;
951     uint16_t ediv;
952     switch (mode){
953         case ADDRESS_RESOLUTION_GENERAL:
954             break;
955         case ADDRESS_RESOLUTION_FOR_CONNECTION:
956             sm_connection = (sm_connection_t *) context;
957             con_handle = sm_connection->sm_handle;
958             switch (event){
959                 case ADDRESS_RESOLUTION_SUCEEDED:
960                     sm_connection->sm_irk_lookup_state = IRK_LOOKUP_SUCCEEDED;
961                     sm_connection->sm_le_db_index = matched_device_id;
962                     log_info("ADDRESS_RESOLUTION_SUCEEDED, index %d", sm_connection->sm_le_db_index);
963                     if (sm_connection->sm_role) break;
964                     if (!sm_connection->sm_bonding_requested && !sm_connection->sm_security_request_received) break;
965                     sm_connection->sm_security_request_received = 0;
966                     sm_connection->sm_bonding_requested = 0;
967                     le_device_db_encryption_get(sm_connection->sm_le_db_index, &ediv, NULL, NULL, NULL, NULL, NULL);
968                     if (ediv){
969                         sm_connection->sm_engine_state = SM_INITIATOR_PH0_HAS_LTK;
970                     } else {
971                         sm_connection->sm_engine_state = SM_INITIATOR_PH1_W2_SEND_PAIRING_REQUEST;
972                     }
973                     break;
974                 case ADDRESS_RESOLUTION_FAILED:
975                     sm_connection->sm_irk_lookup_state = IRK_LOOKUP_FAILED;
976                     if (sm_connection->sm_role) break;
977                     if (!sm_connection->sm_bonding_requested && !sm_connection->sm_security_request_received) break;
978                     sm_connection->sm_security_request_received = 0;
979                     sm_connection->sm_bonding_requested = 0;
980                     sm_connection->sm_engine_state = SM_INITIATOR_PH1_W2_SEND_PAIRING_REQUEST;
981                     break;
982             }
983             break;
984         default:
985             break;
986     }
987 
988     switch (event){
989         case ADDRESS_RESOLUTION_SUCEEDED:
990             sm_notify_client_index(SM_EVENT_IDENTITY_RESOLVING_SUCCEEDED, con_handle, sm_address_resolution_addr_type, sm_address_resolution_address, matched_device_id);
991             break;
992         case ADDRESS_RESOLUTION_FAILED:
993             sm_notify_client_base(SM_EVENT_IDENTITY_RESOLVING_FAILED, con_handle, sm_address_resolution_addr_type, sm_address_resolution_address);
994             break;
995     }
996 }
997 
998 static void sm_key_distribution_handle_all_received(sm_connection_t * sm_conn){
999 
1000     int le_db_index = -1;
1001 
1002     // lookup device based on IRK
1003     if (setup->sm_key_distribution_received_set & SM_KEYDIST_FLAG_IDENTITY_INFORMATION){
1004         int i;
1005         for (i=0; i < le_device_db_count(); i++){
1006             sm_key_t irk;
1007             bd_addr_t address;
1008             int address_type;
1009             le_device_db_info(i, &address_type, address, irk);
1010             if (memcmp(irk, setup->sm_peer_irk, 16) == 0){
1011                 log_info("sm: device found for IRK, updating");
1012                 le_db_index = i;
1013                 break;
1014             }
1015         }
1016     }
1017 
1018     // if not found, lookup via public address if possible
1019     log_info("sm peer addr type %u, peer addres %s", setup->sm_peer_addr_type, bd_addr_to_str(setup->sm_peer_address));
1020     if (le_db_index < 0 && setup->sm_peer_addr_type == BD_ADDR_TYPE_LE_PUBLIC){
1021         int i;
1022         for (i=0; i < le_device_db_count(); i++){
1023             bd_addr_t address;
1024             int address_type;
1025             le_device_db_info(i, &address_type, address, NULL);
1026             log_info("device %u, sm peer addr type %u, peer addres %s", i, address_type, bd_addr_to_str(address));
1027             if (address_type == BD_ADDR_TYPE_LE_PUBLIC && memcmp(address, setup->sm_peer_address, 6) == 0){
1028                 log_info("sm: device found for public address, updating");
1029                 le_db_index = i;
1030                 break;
1031             }
1032         }
1033     }
1034 
1035     // if not found, add to db
1036     if (le_db_index < 0) {
1037         le_db_index = le_device_db_add(setup->sm_peer_addr_type, setup->sm_peer_address, setup->sm_peer_irk);
1038     }
1039 
1040     if (le_db_index >= 0){
1041         le_device_db_local_counter_set(le_db_index, 0);
1042 
1043         // store local CSRK
1044         if (setup->sm_key_distribution_send_set & SM_KEYDIST_FLAG_SIGNING_IDENTIFICATION){
1045             log_info("sm: store local CSRK");
1046             le_device_db_local_csrk_set(le_db_index, setup->sm_local_csrk);
1047             le_device_db_local_counter_set(le_db_index, 0);
1048         }
1049 
1050         // store remote CSRK
1051         if (setup->sm_key_distribution_received_set & SM_KEYDIST_FLAG_SIGNING_IDENTIFICATION){
1052             log_info("sm: store remote CSRK");
1053             le_device_db_remote_csrk_set(le_db_index, setup->sm_peer_csrk);
1054             le_device_db_remote_counter_set(le_db_index, 0);
1055         }
1056 
1057         // store encryption information
1058         if (setup->sm_key_distribution_received_set & SM_KEYDIST_FLAG_ENCRYPTION_INFORMATION
1059             && setup->sm_key_distribution_received_set &  SM_KEYDIST_FLAG_MASTER_IDENTIFICATION){
1060             log_info("sm: set encryption information (key size %u, authenticatd %u)", sm_conn->sm_actual_encryption_key_size, sm_conn->sm_connection_authenticated);
1061             le_device_db_encryption_set(le_db_index, setup->sm_peer_ediv, setup->sm_peer_rand, setup->sm_peer_ltk,
1062                 sm_conn->sm_actual_encryption_key_size, sm_conn->sm_connection_authenticated, sm_conn->sm_connection_authorization_state == AUTHORIZATION_GRANTED);
1063         }
1064     }
1065 
1066     // keep le_db_index
1067     sm_conn->sm_le_db_index = le_db_index;
1068 }
1069 
1070 static void sm_run(void){
1071 
1072     btstack_linked_list_iterator_t it;
1073 
1074     // assert that we can send at least commands
1075     if (!hci_can_send_command_packet_now()) return;
1076 
1077     //
1078     // non-connection related behaviour
1079     //
1080 
1081     // distributed key generation
1082     switch (dkg_state){
1083         case DKG_CALC_IRK:
1084             // already busy?
1085             if (sm_aes128_state == SM_AES128_IDLE) {
1086                 // IRK = d1(IR, 1, 0)
1087                 sm_key_t d1_prime;
1088                 sm_d1_d_prime(1, 0, d1_prime);  // plaintext
1089                 dkg_next_state();
1090                 sm_aes128_start(sm_persistent_ir, d1_prime, NULL);
1091                 return;
1092             }
1093             break;
1094         case DKG_CALC_DHK:
1095             // already busy?
1096             if (sm_aes128_state == SM_AES128_IDLE) {
1097                 // DHK = d1(IR, 3, 0)
1098                 sm_key_t d1_prime;
1099                 sm_d1_d_prime(3, 0, d1_prime);  // plaintext
1100                 dkg_next_state();
1101                 sm_aes128_start(sm_persistent_ir, d1_prime, NULL);
1102                 return;
1103             }
1104             break;
1105         default:
1106             break;
1107     }
1108 
1109     // random address updates
1110     switch (rau_state){
1111         case RAU_GET_RANDOM:
1112             rau_next_state();
1113             sm_random_start(NULL);
1114             return;
1115         case RAU_GET_ENC:
1116             // already busy?
1117             if (sm_aes128_state == SM_AES128_IDLE) {
1118                 sm_key_t r_prime;
1119                 sm_ah_r_prime(sm_random_address, r_prime);
1120                 rau_next_state();
1121                 sm_aes128_start(sm_persistent_irk, r_prime, NULL);
1122                 return;
1123             }
1124             break;
1125         case RAU_SET_ADDRESS:
1126             log_info("New random address: %s", bd_addr_to_str(sm_random_address));
1127             rau_state = RAU_IDLE;
1128             hci_send_cmd(&hci_le_set_random_address, sm_random_address);
1129             return;
1130         default:
1131             break;
1132     }
1133 
1134     // CMAC
1135     switch (sm_cmac_state){
1136         case CMAC_CALC_SUBKEYS:
1137         case CMAC_CALC_MI:
1138         case CMAC_CALC_MLAST:
1139             // already busy?
1140             if (sm_aes128_state == SM_AES128_ACTIVE) break;
1141             sm_cmac_handle_aes_engine_ready();
1142             return;
1143         default:
1144             break;
1145     }
1146 
1147     // CSRK Lookup
1148     // -- if csrk lookup ready, find connection that require csrk lookup
1149     if (sm_address_resolution_idle()){
1150         hci_connections_get_iterator(&it);
1151         while(btstack_linked_list_iterator_has_next(&it)){
1152             hci_connection_t * hci_connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it);
1153             sm_connection_t  * sm_connection  = &hci_connection->sm_connection;
1154             if (sm_connection->sm_irk_lookup_state == IRK_LOOKUP_W4_READY){
1155                 // and start lookup
1156                 sm_address_resolution_start_lookup(sm_connection->sm_peer_addr_type, sm_connection->sm_handle, sm_connection->sm_peer_address, ADDRESS_RESOLUTION_FOR_CONNECTION, sm_connection);
1157                 sm_connection->sm_irk_lookup_state = IRK_LOOKUP_STARTED;
1158                 break;
1159             }
1160         }
1161     }
1162 
1163     // -- if csrk lookup ready, resolved addresses for received addresses
1164     if (sm_address_resolution_idle()) {
1165         if (!btstack_linked_list_empty(&sm_address_resolution_general_queue)){
1166             sm_lookup_entry_t * entry = (sm_lookup_entry_t *) sm_address_resolution_general_queue;
1167             btstack_linked_list_remove(&sm_address_resolution_general_queue, (btstack_linked_item_t *) entry);
1168             sm_address_resolution_start_lookup(entry->address_type, 0, entry->address, ADDRESS_RESOLUTION_GENERAL, NULL);
1169             btstack_memory_sm_lookup_entry_free(entry);
1170         }
1171     }
1172 
1173     // -- Continue with CSRK device lookup by public or resolvable private address
1174     if (!sm_address_resolution_idle()){
1175         log_info("LE Device Lookup: device %u/%u", sm_address_resolution_test, le_device_db_count());
1176         while (sm_address_resolution_test < le_device_db_count()){
1177             int addr_type;
1178             bd_addr_t addr;
1179             sm_key_t irk;
1180             le_device_db_info(sm_address_resolution_test, &addr_type, addr, irk);
1181             log_info("device type %u, addr: %s", addr_type, bd_addr_to_str(addr));
1182 
1183             if (sm_address_resolution_addr_type == addr_type && memcmp(addr, sm_address_resolution_address, 6) == 0){
1184                 log_info("LE Device Lookup: found CSRK by { addr_type, address} ");
1185                 sm_address_resolution_handle_event(ADDRESS_RESOLUTION_SUCEEDED);
1186                 break;
1187             }
1188 
1189             if (sm_address_resolution_addr_type == 0){
1190                 sm_address_resolution_test++;
1191                 continue;
1192             }
1193 
1194             if (sm_aes128_state == SM_AES128_ACTIVE) break;
1195 
1196             log_info("LE Device Lookup: calculate AH");
1197             log_info_key("IRK", irk);
1198 
1199             sm_key_t r_prime;
1200             sm_ah_r_prime(sm_address_resolution_address, r_prime);
1201             sm_address_resolution_ah_calculation_active = 1;
1202             sm_aes128_start(irk, r_prime, sm_address_resolution_context);   // keep context
1203             return;
1204         }
1205 
1206         if (sm_address_resolution_test >= le_device_db_count()){
1207             log_info("LE Device Lookup: not found");
1208             sm_address_resolution_handle_event(ADDRESS_RESOLUTION_FAILED);
1209         }
1210     }
1211 
1212 
1213     //
1214     // active connection handling
1215     // -- use loop to handle next connection if lock on setup context is released
1216 
1217     while (1) {
1218 
1219         // Find connections that requires setup context and make active if no other is locked
1220         hci_connections_get_iterator(&it);
1221         while(!sm_active_connection && btstack_linked_list_iterator_has_next(&it)){
1222             hci_connection_t * hci_connection = (hci_connection_t *) btstack_linked_list_iterator_next(&it);
1223             sm_connection_t  * sm_connection = &hci_connection->sm_connection;
1224             // - if no connection locked and we're ready/waiting for setup context, fetch it and start
1225             int done = 1;
1226             int err;
1227             int encryption_key_size;
1228             int authenticated;
1229             int authorized;
1230             switch (sm_connection->sm_engine_state) {
1231                 case SM_RESPONDER_SEND_SECURITY_REQUEST:
1232                     // send packet if possible,
1233                     if (l2cap_can_send_fixed_channel_packet_now(sm_connection->sm_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL)){
1234                         const uint8_t buffer[2] = { SM_CODE_SECURITY_REQUEST, SM_AUTHREQ_BONDING};
1235                         sm_connection->sm_engine_state = SM_RESPONDER_PH1_W4_PAIRING_REQUEST;
1236                         l2cap_send_connectionless(sm_connection->sm_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) buffer, sizeof(buffer));
1237                     } else {
1238                         l2cap_request_can_send_fix_channel_now_event(sm_connection->sm_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL);
1239                     }
1240                     // don't lock setup context yet
1241                     done = 0;
1242                     break;
1243                 case SM_RESPONDER_PH1_PAIRING_REQUEST_RECEIVED:
1244                     sm_init_setup(sm_connection);
1245                     // recover pairing request
1246                     memcpy(&setup->sm_m_preq, &sm_connection->sm_m_preq, sizeof(sm_pairing_packet_t));
1247                     err = sm_stk_generation_init(sm_connection);
1248                     if (err){
1249                         setup->sm_pairing_failed_reason = err;
1250                         sm_connection->sm_engine_state = SM_GENERAL_SEND_PAIRING_FAILED;
1251                         break;
1252                     }
1253                     sm_timeout_start(sm_connection);
1254                     // generate random number first, if we need to show passkey
1255                     if (setup->sm_stk_generation_method == PK_INIT_INPUT){
1256                         sm_connection->sm_engine_state = SM_PH2_GET_RANDOM_TK;
1257                         break;
1258                     }
1259                     sm_connection->sm_engine_state = SM_RESPONDER_PH1_SEND_PAIRING_RESPONSE;
1260                     break;
1261                 case SM_INITIATOR_PH0_HAS_LTK:
1262                     // fetch data from device db - incl. authenticated/authorized/key size. Note all sm_connection_X require encryption enabled
1263                     le_device_db_encryption_get(sm_connection->sm_le_db_index, &setup->sm_peer_ediv, setup->sm_peer_rand, setup->sm_peer_ltk,
1264                                                 &encryption_key_size, &authenticated, &authorized);
1265                     log_info("db index %u, key size %u, authenticated %u, authorized %u", sm_connection->sm_le_db_index, encryption_key_size, authenticated, authorized);
1266                     sm_connection->sm_actual_encryption_key_size = encryption_key_size;
1267                     sm_connection->sm_connection_authenticated = authenticated;
1268                     sm_connection->sm_connection_authorization_state = authorized ? AUTHORIZATION_GRANTED : AUTHORIZATION_UNKNOWN;
1269                     sm_connection->sm_engine_state = SM_INITIATOR_PH0_SEND_START_ENCRYPTION;
1270                     break;
1271                 case SM_RESPONDER_PH0_RECEIVED_LTK:
1272                     // re-establish previously used LTK using Rand and EDIV
1273                     memcpy(setup->sm_local_rand, sm_connection->sm_local_rand, 8);
1274                     setup->sm_local_ediv = sm_connection->sm_local_ediv;
1275                     // re-establish used key encryption size
1276                     // no db for encryption size hack: encryption size is stored in lowest nibble of setup->sm_local_rand
1277                     sm_connection->sm_actual_encryption_key_size = (setup->sm_local_rand[7] & 0x0f) + 1;
1278                     // no db for authenticated flag hack: flag is stored in bit 4 of LSB
1279                     sm_connection->sm_connection_authenticated = (setup->sm_local_rand[7] & 0x10) >> 4;
1280                     log_info("sm: received ltk request with key size %u, authenticated %u",
1281                             sm_connection->sm_actual_encryption_key_size, sm_connection->sm_connection_authenticated);
1282                     sm_connection->sm_engine_state = SM_RESPONDER_PH4_Y_GET_ENC;
1283                     break;
1284                 case SM_INITIATOR_PH1_W2_SEND_PAIRING_REQUEST:
1285                     sm_init_setup(sm_connection);
1286                     sm_timeout_start(sm_connection);
1287                     sm_connection->sm_engine_state = SM_INITIATOR_PH1_SEND_PAIRING_REQUEST;
1288                     break;
1289                 default:
1290                     done = 0;
1291                     break;
1292             }
1293             if (done){
1294                 sm_active_connection = sm_connection->sm_handle;
1295                 log_info("sm: connection 0x%04x locked setup context as %s", sm_active_connection, sm_connection->sm_role ? "responder" : "initiator");
1296             }
1297         }
1298 
1299         //
1300         // active connection handling
1301         //
1302 
1303         if (sm_active_connection == 0) return;
1304 
1305         // assert that we could send a SM PDU - not needed for all of the following
1306         if (!l2cap_can_send_fixed_channel_packet_now(sm_active_connection, L2CAP_CID_SECURITY_MANAGER_PROTOCOL)) {
1307             l2cap_request_can_send_fix_channel_now_event(sm_active_connection, L2CAP_CID_SECURITY_MANAGER_PROTOCOL);
1308             return;
1309         }
1310 
1311         sm_connection_t * connection = sm_get_connection_for_handle(sm_active_connection);
1312         if (!connection) return;
1313 
1314         sm_key_t plaintext;
1315         int key_distribution_flags;
1316 
1317         log_info("sm_run: state %u", connection->sm_engine_state);
1318 
1319         // responding state
1320         switch (connection->sm_engine_state){
1321 
1322             // general
1323             case SM_GENERAL_SEND_PAIRING_FAILED: {
1324                 uint8_t buffer[2];
1325                 buffer[0] = SM_CODE_PAIRING_FAILED;
1326                 buffer[1] = setup->sm_pairing_failed_reason;
1327                 connection->sm_engine_state = connection->sm_role ? SM_RESPONDER_IDLE : SM_INITIATOR_CONNECTED;
1328                 l2cap_send_connectionless(connection->sm_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) buffer, sizeof(buffer));
1329                 sm_done_for_handle(connection->sm_handle);
1330                 break;
1331             }
1332 
1333             // initiator side
1334             case SM_INITIATOR_PH0_SEND_START_ENCRYPTION: {
1335                 sm_key_t peer_ltk_flipped;
1336                 reverse_128(setup->sm_peer_ltk, peer_ltk_flipped);
1337                 connection->sm_engine_state = SM_INITIATOR_PH0_W4_CONNECTION_ENCRYPTED;
1338                 log_info("sm: hci_le_start_encryption ediv 0x%04x", setup->sm_peer_ediv);
1339                 uint32_t rand_high = big_endian_read_32(setup->sm_peer_rand, 0);
1340                 uint32_t rand_low  = big_endian_read_32(setup->sm_peer_rand, 4);
1341                 hci_send_cmd(&hci_le_start_encryption, connection->sm_handle,rand_low, rand_high, setup->sm_peer_ediv, peer_ltk_flipped);
1342                 return;
1343             }
1344 
1345             case SM_INITIATOR_PH1_SEND_PAIRING_REQUEST:
1346                 setup->sm_m_preq.code = SM_CODE_PAIRING_REQUEST;
1347                 connection->sm_engine_state = SM_INITIATOR_PH1_W4_PAIRING_RESPONSE;
1348                 l2cap_send_connectionless(connection->sm_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) &setup->sm_m_preq, sizeof(sm_pairing_packet_t));
1349                 sm_timeout_reset(connection);
1350                 break;
1351 
1352             // responder side
1353             case SM_RESPONDER_PH0_SEND_LTK_REQUESTED_NEGATIVE_REPLY:
1354                 connection->sm_engine_state = SM_RESPONDER_IDLE;
1355                 hci_send_cmd(&hci_le_long_term_key_negative_reply, connection->sm_handle);
1356                 return;
1357 
1358             case SM_RESPONDER_PH1_SEND_PAIRING_RESPONSE:
1359                 // echo initiator for now
1360                 setup->sm_s_pres.code = SM_CODE_PAIRING_RESPONSE;
1361                 key_distribution_flags = sm_key_distribution_flags_for_auth_req();
1362                 setup->sm_s_pres.initiator_key_distribution = setup->sm_m_preq.initiator_key_distribution & key_distribution_flags;
1363                 setup->sm_s_pres.responder_key_distribution = setup->sm_m_preq.responder_key_distribution & key_distribution_flags;
1364                 connection->sm_engine_state = SM_RESPONDER_PH1_W4_PAIRING_CONFIRM;
1365                 l2cap_send_connectionless(connection->sm_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) &setup->sm_s_pres, sizeof(sm_pairing_packet_t));
1366                 sm_timeout_reset(connection);
1367                 sm_trigger_user_response(connection);
1368                 return;
1369 
1370             case SM_PH2_SEND_PAIRING_RANDOM: {
1371                 uint8_t buffer[17];
1372                 buffer[0] = SM_CODE_PAIRING_RANDOM;
1373                 reverse_128(setup->sm_local_random, &buffer[1]);
1374                 if (connection->sm_role){
1375                     connection->sm_engine_state = SM_RESPONDER_PH2_W4_LTK_REQUEST;
1376                 } else {
1377                     connection->sm_engine_state = SM_INITIATOR_PH2_W4_PAIRING_RANDOM;
1378                 }
1379                 l2cap_send_connectionless(connection->sm_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) buffer, sizeof(buffer));
1380                 sm_timeout_reset(connection);
1381                 break;
1382             }
1383 
1384             case SM_PH2_GET_RANDOM_TK:
1385             case SM_PH2_C1_GET_RANDOM_A:
1386             case SM_PH2_C1_GET_RANDOM_B:
1387             case SM_PH3_GET_RANDOM:
1388             case SM_PH3_GET_DIV:
1389                 sm_next_responding_state(connection);
1390                 sm_random_start(connection);
1391                 return;
1392 
1393             case SM_PH2_C1_GET_ENC_B:
1394             case SM_PH2_C1_GET_ENC_D:
1395                 // already busy?
1396                 if (sm_aes128_state == SM_AES128_ACTIVE) break;
1397                 sm_next_responding_state(connection);
1398                 sm_aes128_start(setup->sm_tk, setup->sm_c1_t3_value, connection);
1399                 return;
1400 
1401             case SM_PH3_LTK_GET_ENC:
1402             case SM_RESPONDER_PH4_LTK_GET_ENC:
1403                 // already busy?
1404                 if (sm_aes128_state == SM_AES128_IDLE) {
1405                     sm_key_t d_prime;
1406                     sm_d1_d_prime(setup->sm_local_div, 0, d_prime);
1407                     sm_next_responding_state(connection);
1408                     sm_aes128_start(sm_persistent_er, d_prime, connection);
1409                     return;
1410                 }
1411                 break;
1412 
1413             case SM_PH3_CSRK_GET_ENC:
1414                 // already busy?
1415                 if (sm_aes128_state == SM_AES128_IDLE) {
1416                     sm_key_t d_prime;
1417                     sm_d1_d_prime(setup->sm_local_div, 1, d_prime);
1418                     sm_next_responding_state(connection);
1419                     sm_aes128_start(sm_persistent_er, d_prime, connection);
1420                     return;
1421                 }
1422                 break;
1423 
1424             case SM_PH2_C1_GET_ENC_C:
1425                 // already busy?
1426                 if (sm_aes128_state == SM_AES128_ACTIVE) break;
1427                 // calculate m_confirm using aes128 engine - step 1
1428                 sm_c1_t1(setup->sm_peer_random, (uint8_t*) &setup->sm_m_preq, (uint8_t*) &setup->sm_s_pres, setup->sm_m_addr_type, setup->sm_s_addr_type, plaintext);
1429                 sm_next_responding_state(connection);
1430                 sm_aes128_start(setup->sm_tk, plaintext, connection);
1431                 break;
1432             case SM_PH2_C1_GET_ENC_A:
1433                 // already busy?
1434                 if (sm_aes128_state == SM_AES128_ACTIVE) break;
1435                 // calculate confirm using aes128 engine - step 1
1436                 sm_c1_t1(setup->sm_local_random, (uint8_t*) &setup->sm_m_preq, (uint8_t*) &setup->sm_s_pres, setup->sm_m_addr_type, setup->sm_s_addr_type, plaintext);
1437                 sm_next_responding_state(connection);
1438                 sm_aes128_start(setup->sm_tk, plaintext, connection);
1439                 break;
1440             case SM_PH2_CALC_STK:
1441                 // already busy?
1442                 if (sm_aes128_state == SM_AES128_ACTIVE) break;
1443                 // calculate STK
1444                 if (connection->sm_role){
1445                     sm_s1_r_prime(setup->sm_local_random, setup->sm_peer_random, plaintext);
1446                 } else {
1447                     sm_s1_r_prime(setup->sm_peer_random, setup->sm_local_random, plaintext);
1448                 }
1449                 sm_next_responding_state(connection);
1450                 sm_aes128_start(setup->sm_tk, plaintext, connection);
1451                 break;
1452             case SM_PH3_Y_GET_ENC:
1453                 // already busy?
1454                 if (sm_aes128_state == SM_AES128_ACTIVE) break;
1455                 // PH3B2 - calculate Y from      - enc
1456                 // Y = dm(DHK, Rand)
1457                 sm_dm_r_prime(setup->sm_local_rand, plaintext);
1458                 sm_next_responding_state(connection);
1459                 sm_aes128_start(sm_persistent_dhk, plaintext, connection);
1460                 return;
1461             case SM_PH2_C1_SEND_PAIRING_CONFIRM: {
1462                 uint8_t buffer[17];
1463                 buffer[0] = SM_CODE_PAIRING_CONFIRM;
1464                 reverse_128(setup->sm_local_confirm, &buffer[1]);
1465                 if (connection->sm_role){
1466                     connection->sm_engine_state = SM_RESPONDER_PH2_W4_PAIRING_RANDOM;
1467                 } else {
1468                     connection->sm_engine_state = SM_INITIATOR_PH2_W4_PAIRING_CONFIRM;
1469                 }
1470                 l2cap_send_connectionless(connection->sm_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) buffer, sizeof(buffer));
1471                 sm_timeout_reset(connection);
1472                 return;
1473             }
1474             case SM_RESPONDER_PH2_SEND_LTK_REPLY: {
1475                 sm_key_t stk_flipped;
1476                 reverse_128(setup->sm_ltk, stk_flipped);
1477                 connection->sm_engine_state = SM_PH2_W4_CONNECTION_ENCRYPTED;
1478                 hci_send_cmd(&hci_le_long_term_key_request_reply, connection->sm_handle, stk_flipped);
1479                 return;
1480             }
1481             case SM_INITIATOR_PH3_SEND_START_ENCRYPTION: {
1482                 sm_key_t stk_flipped;
1483                 reverse_128(setup->sm_ltk, stk_flipped);
1484                 connection->sm_engine_state = SM_PH2_W4_CONNECTION_ENCRYPTED;
1485                 hci_send_cmd(&hci_le_start_encryption, connection->sm_handle, 0, 0, 0, stk_flipped);
1486                 return;
1487             }
1488             case SM_RESPONDER_PH4_SEND_LTK: {
1489                 sm_key_t ltk_flipped;
1490                 reverse_128(setup->sm_ltk, ltk_flipped);
1491                 connection->sm_engine_state = SM_RESPONDER_IDLE;
1492                 hci_send_cmd(&hci_le_long_term_key_request_reply, connection->sm_handle, ltk_flipped);
1493                 return;
1494             }
1495             case SM_RESPONDER_PH4_Y_GET_ENC:
1496                 // already busy?
1497                 if (sm_aes128_state == SM_AES128_ACTIVE) break;
1498                 log_info("LTK Request: recalculating with ediv 0x%04x", setup->sm_local_ediv);
1499                 // Y = dm(DHK, Rand)
1500                 sm_dm_r_prime(setup->sm_local_rand, plaintext);
1501                 sm_next_responding_state(connection);
1502                 sm_aes128_start(sm_persistent_dhk, plaintext, connection);
1503                 return;
1504 
1505             case SM_PH3_DISTRIBUTE_KEYS:
1506                 if (setup->sm_key_distribution_send_set &   SM_KEYDIST_FLAG_ENCRYPTION_INFORMATION){
1507                     setup->sm_key_distribution_send_set &= ~SM_KEYDIST_FLAG_ENCRYPTION_INFORMATION;
1508                     uint8_t buffer[17];
1509                     buffer[0] = SM_CODE_ENCRYPTION_INFORMATION;
1510                     reverse_128(setup->sm_ltk, &buffer[1]);
1511                     l2cap_send_connectionless(connection->sm_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) buffer, sizeof(buffer));
1512                     sm_timeout_reset(connection);
1513                     return;
1514                 }
1515                 if (setup->sm_key_distribution_send_set &   SM_KEYDIST_FLAG_MASTER_IDENTIFICATION){
1516                     setup->sm_key_distribution_send_set &= ~SM_KEYDIST_FLAG_MASTER_IDENTIFICATION;
1517                     uint8_t buffer[11];
1518                     buffer[0] = SM_CODE_MASTER_IDENTIFICATION;
1519                     little_endian_store_16(buffer, 1, setup->sm_local_ediv);
1520                     reverse_64(setup->sm_local_rand, &buffer[3]);
1521                     l2cap_send_connectionless(connection->sm_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) buffer, sizeof(buffer));
1522                     sm_timeout_reset(connection);
1523                     return;
1524                 }
1525                 if (setup->sm_key_distribution_send_set &   SM_KEYDIST_FLAG_IDENTITY_INFORMATION){
1526                     setup->sm_key_distribution_send_set &= ~SM_KEYDIST_FLAG_IDENTITY_INFORMATION;
1527                     uint8_t buffer[17];
1528                     buffer[0] = SM_CODE_IDENTITY_INFORMATION;
1529                     reverse_128(sm_persistent_irk, &buffer[1]);
1530                     l2cap_send_connectionless(connection->sm_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) buffer, sizeof(buffer));
1531                     sm_timeout_reset(connection);
1532                     return;
1533                 }
1534                 if (setup->sm_key_distribution_send_set &   SM_KEYDIST_FLAG_IDENTITY_ADDRESS_INFORMATION){
1535                     setup->sm_key_distribution_send_set &= ~SM_KEYDIST_FLAG_IDENTITY_ADDRESS_INFORMATION;
1536                     bd_addr_t local_address;
1537                     uint8_t buffer[8];
1538                     buffer[0] = SM_CODE_IDENTITY_ADDRESS_INFORMATION;
1539                     gap_advertisements_get_address(&buffer[1], local_address);
1540                     reverse_bd_addr(local_address, &buffer[2]);
1541                     l2cap_send_connectionless(connection->sm_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) buffer, sizeof(buffer));
1542                     sm_timeout_reset(connection);
1543                     return;
1544                 }
1545                 if (setup->sm_key_distribution_send_set &   SM_KEYDIST_FLAG_SIGNING_IDENTIFICATION){
1546                     setup->sm_key_distribution_send_set &= ~SM_KEYDIST_FLAG_SIGNING_IDENTIFICATION;
1547 
1548                     // hack to reproduce test runs
1549                     if (test_use_fixed_local_csrk){
1550                         memset(setup->sm_local_csrk, 0xcc, 16);
1551                     }
1552 
1553                     uint8_t buffer[17];
1554                     buffer[0] = SM_CODE_SIGNING_INFORMATION;
1555                     reverse_128(setup->sm_local_csrk, &buffer[1]);
1556                     l2cap_send_connectionless(connection->sm_handle, L2CAP_CID_SECURITY_MANAGER_PROTOCOL, (uint8_t*) buffer, sizeof(buffer));
1557                     sm_timeout_reset(connection);
1558                     return;
1559                 }
1560 
1561                 // keys are sent
1562                 if (connection->sm_role){
1563                     // slave -> receive master keys if any
1564                     if (sm_key_distribution_all_received(connection)){
1565                         sm_key_distribution_handle_all_received(connection);
1566                         connection->sm_engine_state = SM_RESPONDER_IDLE;
1567                         sm_done_for_handle(connection->sm_handle);
1568                     } else {
1569                         connection->sm_engine_state = SM_PH3_RECEIVE_KEYS;
1570                     }
1571                 } else {
1572                     // master -> all done
1573                     connection->sm_engine_state = SM_INITIATOR_CONNECTED;
1574                     sm_done_for_handle(connection->sm_handle);
1575                 }
1576                 break;
1577 
1578             default:
1579                 break;
1580         }
1581 
1582         // check again if active connection was released
1583         if (sm_active_connection) break;
1584     }
1585 }
1586 
1587 // note: aes engine is ready as we just got the aes result
1588 static void sm_handle_encryption_result(uint8_t * data){
1589 
1590     sm_aes128_state = SM_AES128_IDLE;
1591 
1592     if (sm_address_resolution_ah_calculation_active){
1593         sm_address_resolution_ah_calculation_active = 0;
1594         // compare calulated address against connecting device
1595         uint8_t hash[3];
1596         reverse_24(data, hash);
1597         if (memcmp(&sm_address_resolution_address[3], hash, 3) == 0){
1598             log_info("LE Device Lookup: matched resolvable private address");
1599             sm_address_resolution_handle_event(ADDRESS_RESOLUTION_SUCEEDED);
1600             return;
1601         }
1602         // no match, try next
1603         sm_address_resolution_test++;
1604         return;
1605     }
1606 
1607     switch (dkg_state){
1608         case DKG_W4_IRK:
1609             reverse_128(data, sm_persistent_irk);
1610             log_info_key("irk", sm_persistent_irk);
1611             dkg_next_state();
1612             return;
1613         case DKG_W4_DHK:
1614             reverse_128(data, sm_persistent_dhk);
1615             log_info_key("dhk", sm_persistent_dhk);
1616             dkg_next_state();
1617             // SM Init Finished
1618             return;
1619         default:
1620             break;
1621     }
1622 
1623     switch (rau_state){
1624         case RAU_W4_ENC:
1625             reverse_24(data, &sm_random_address[3]);
1626             rau_next_state();
1627             return;
1628         default:
1629             break;
1630     }
1631 
1632     switch (sm_cmac_state){
1633         case CMAC_W4_SUBKEYS:
1634         case CMAC_W4_MI:
1635         case CMAC_W4_MLAST:
1636             {
1637             sm_key_t t;
1638             reverse_128(data, t);
1639             sm_cmac_handle_encryption_result(t);
1640             }
1641             return;
1642         default:
1643             break;
1644     }
1645 
1646     // retrieve sm_connection provided to sm_aes128_start_encryption
1647     sm_connection_t * connection = (sm_connection_t*) sm_aes128_context;
1648     if (!connection) return;
1649     switch (connection->sm_engine_state){
1650         case SM_PH2_C1_W4_ENC_A:
1651         case SM_PH2_C1_W4_ENC_C:
1652             {
1653             sm_key_t t2;
1654             reverse_128(data, t2);
1655             sm_c1_t3(t2, setup->sm_m_address, setup->sm_s_address, setup->sm_c1_t3_value);
1656             }
1657             sm_next_responding_state(connection);
1658             return;
1659         case SM_PH2_C1_W4_ENC_B:
1660             reverse_128(data, setup->sm_local_confirm);
1661             log_info_key("c1!", setup->sm_local_confirm);
1662             connection->sm_engine_state = SM_PH2_C1_SEND_PAIRING_CONFIRM;
1663             return;
1664         case SM_PH2_C1_W4_ENC_D:
1665             {
1666             sm_key_t peer_confirm_test;
1667             reverse_128(data, peer_confirm_test);
1668             log_info_key("c1!", peer_confirm_test);
1669             if (memcmp(setup->sm_peer_confirm, peer_confirm_test, 16) != 0){
1670                 setup->sm_pairing_failed_reason = SM_REASON_CONFIRM_VALUE_FAILED;
1671                 connection->sm_engine_state = SM_GENERAL_SEND_PAIRING_FAILED;
1672                 return;
1673             }
1674             if (connection->sm_role){
1675                 connection->sm_engine_state = SM_PH2_SEND_PAIRING_RANDOM;
1676             } else {
1677                 connection->sm_engine_state = SM_PH2_CALC_STK;
1678             }
1679             }
1680             return;
1681         case SM_PH2_W4_STK:
1682             reverse_128(data, setup->sm_ltk);
1683             sm_truncate_key(setup->sm_ltk, connection->sm_actual_encryption_key_size);
1684             log_info_key("stk", setup->sm_ltk);
1685             if (connection->sm_role){
1686                 connection->sm_engine_state = SM_RESPONDER_PH2_SEND_LTK_REPLY;
1687             } else {
1688                 connection->sm_engine_state = SM_INITIATOR_PH3_SEND_START_ENCRYPTION;
1689             }
1690             return;
1691         case SM_PH3_Y_W4_ENC:{
1692             sm_key_t y128;
1693             reverse_128(data, y128);
1694             setup->sm_local_y = big_endian_read_16(y128, 14);
1695             log_info_hex16("y", setup->sm_local_y);
1696             // PH3B3 - calculate EDIV
1697             setup->sm_local_ediv = setup->sm_local_y ^ setup->sm_local_div;
1698             log_info_hex16("ediv", setup->sm_local_ediv);
1699             // PH3B4 - calculate LTK         - enc
1700             // LTK = d1(ER, DIV, 0))
1701             connection->sm_engine_state = SM_PH3_LTK_GET_ENC;
1702             return;
1703         }
1704         case SM_RESPONDER_PH4_Y_W4_ENC:{
1705             sm_key_t y128;
1706             reverse_128(data, y128);
1707             setup->sm_local_y = big_endian_read_16(y128, 14);
1708             log_info_hex16("y", setup->sm_local_y);
1709 
1710             // PH3B3 - calculate DIV
1711             setup->sm_local_div = setup->sm_local_y ^ setup->sm_local_ediv;
1712             log_info_hex16("ediv", setup->sm_local_ediv);
1713             // PH3B4 - calculate LTK         - enc
1714             // LTK = d1(ER, DIV, 0))
1715             connection->sm_engine_state = SM_RESPONDER_PH4_LTK_GET_ENC;
1716             return;
1717         }
1718         case SM_PH3_LTK_W4_ENC:
1719             reverse_128(data, setup->sm_ltk);
1720             log_info_key("ltk", setup->sm_ltk);
1721             // calc CSRK next
1722             connection->sm_engine_state = SM_PH3_CSRK_GET_ENC;
1723             return;
1724         case SM_PH3_CSRK_W4_ENC:
1725             reverse_128(data, setup->sm_local_csrk);
1726             log_info_key("csrk", setup->sm_local_csrk);
1727             if (setup->sm_key_distribution_send_set){
1728                 connection->sm_engine_state = SM_PH3_DISTRIBUTE_KEYS;
1729             } else {
1730                 // no keys to send, just continue
1731                 if (connection->sm_role){
1732                     // slave -> receive master keys
1733                     connection->sm_engine_state = SM_PH3_RECEIVE_KEYS;
1734                 } else {
1735                     // master -> all done
1736                     connection->sm_engine_state = SM_INITIATOR_CONNECTED;
1737                     sm_done_for_handle(connection->sm_handle);
1738                 }
1739             }
1740             return;
1741         case SM_RESPONDER_PH4_LTK_W4_ENC:
1742             reverse_128(data, setup->sm_ltk);
1743             sm_truncate_key(setup->sm_ltk, connection->sm_actual_encryption_key_size);
1744             log_info_key("ltk", setup->sm_ltk);
1745             connection->sm_engine_state = SM_RESPONDER_PH4_SEND_LTK;
1746             return;
1747         default:
1748             break;
1749     }
1750 }
1751 
1752 // note: random generator is ready. this doesn NOT imply that aes engine is unused!
1753 static void sm_handle_random_result(uint8_t * data){
1754 
1755     switch (rau_state){
1756         case RAU_W4_RANDOM:
1757             // non-resolvable vs. resolvable
1758             switch (gap_random_adress_type){
1759                 case GAP_RANDOM_ADDRESS_RESOLVABLE:
1760                     // resolvable: use random as prand and calc address hash
1761                     // "The two most significant bits of prand shall be equal to ‘0’ and ‘1"
1762                     memcpy(sm_random_address, data, 3);
1763                     sm_random_address[0] &= 0x3f;
1764                     sm_random_address[0] |= 0x40;
1765                     rau_state = RAU_GET_ENC;
1766                     break;
1767                 case GAP_RANDOM_ADDRESS_NON_RESOLVABLE:
1768                 default:
1769                     // "The two most significant bits of the address shall be equal to ‘0’""
1770                     memcpy(sm_random_address, data, 6);
1771                     sm_random_address[0] &= 0x3f;
1772                     rau_state = RAU_SET_ADDRESS;
1773                     break;
1774             }
1775             return;
1776         default:
1777             break;
1778     }
1779 
1780     // retrieve sm_connection provided to sm_random_start
1781     sm_connection_t * connection = (sm_connection_t *) sm_random_context;
1782     if (!connection) return;
1783     switch (connection->sm_engine_state){
1784         case SM_PH2_W4_RANDOM_TK:
1785         {
1786             // map random to 0-999999 without speding much cycles on a modulus operation
1787             uint32_t tk = little_endian_read_32(data,0);
1788             tk = tk & 0xfffff;  // 1048575
1789             if (tk >= 999999){
1790                 tk = tk - 999999;
1791             }
1792             sm_reset_tk();
1793             big_endian_store_32(setup->sm_tk, 12, tk);
1794             if (connection->sm_role){
1795                 connection->sm_engine_state = SM_RESPONDER_PH1_SEND_PAIRING_RESPONSE;
1796             } else {
1797                 connection->sm_engine_state = SM_PH1_W4_USER_RESPONSE;
1798                 sm_trigger_user_response(connection);
1799                 // response_idle == nothing <--> sm_trigger_user_response() did not require response
1800                 if (setup->sm_user_response == SM_USER_RESPONSE_IDLE){
1801                     connection->sm_engine_state = SM_PH2_C1_GET_RANDOM_A;
1802                 }
1803             }
1804             return;
1805         }
1806         case SM_PH2_C1_W4_RANDOM_A:
1807             memcpy(&setup->sm_local_random[0], data, 8); // random endinaness
1808             connection->sm_engine_state = SM_PH2_C1_GET_RANDOM_B;
1809             return;
1810         case SM_PH2_C1_W4_RANDOM_B:
1811             memcpy(&setup->sm_local_random[8], data, 8); // random endinaness
1812             connection->sm_engine_state = SM_PH2_C1_GET_ENC_A;
1813             return;
1814         case SM_PH3_W4_RANDOM:
1815             reverse_64(data, setup->sm_local_rand);
1816             // no db for encryption size hack: encryption size is stored in lowest nibble of setup->sm_local_rand
1817             setup->sm_local_rand[7] = (setup->sm_local_rand[7] & 0xf0) + (connection->sm_actual_encryption_key_size - 1);
1818             // no db for authenticated flag hack: store flag in bit 4 of LSB
1819             setup->sm_local_rand[7] = (setup->sm_local_rand[7] & 0xef) + (connection->sm_connection_authenticated << 4);
1820             connection->sm_engine_state = SM_PH3_GET_DIV;
1821             return;
1822         case SM_PH3_W4_DIV:
1823             // use 16 bit from random value as div
1824             setup->sm_local_div = big_endian_read_16(data, 0);
1825             log_info_hex16("div", setup->sm_local_div);
1826             connection->sm_engine_state = SM_PH3_Y_GET_ENC;
1827             return;
1828         default:
1829             break;
1830     }
1831 }
1832 
1833 static void sm_event_packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
1834 
1835     sm_connection_t  * sm_conn;
1836     hci_con_handle_t con_handle;
1837 
1838     switch (packet_type) {
1839 
1840 		case HCI_EVENT_PACKET:
1841 			switch (hci_event_packet_get_type(packet)) {
1842 
1843                 case BTSTACK_EVENT_STATE:
1844 					// bt stack activated, get started
1845 					if (btstack_event_state_get_state(packet) == HCI_STATE_WORKING){
1846                         log_info("HCI Working!");
1847                         dkg_state = sm_persistent_irk_ready ? DKG_CALC_DHK : DKG_CALC_IRK;
1848                         rau_state = RAU_IDLE;
1849                         sm_run();
1850 					}
1851 					break;
1852 
1853                 case HCI_EVENT_LE_META:
1854                     switch (packet[2]) {
1855                         case HCI_SUBEVENT_LE_CONNECTION_COMPLETE:
1856 
1857                             log_info("sm: connected");
1858 
1859                             if (packet[3]) return; // connection failed
1860 
1861                             con_handle = little_endian_read_16(packet, 4);
1862                             sm_conn = sm_get_connection_for_handle(con_handle);
1863                             if (!sm_conn) break;
1864 
1865                             sm_conn->sm_handle = con_handle;
1866                             sm_conn->sm_role = packet[6];
1867                             sm_conn->sm_peer_addr_type = packet[7];
1868                             reverse_bd_addr(&packet[8],
1869                                             sm_conn->sm_peer_address);
1870 
1871                             log_info("New sm_conn, role %s", sm_conn->sm_role ? "slave" : "master");
1872 
1873                             // reset security properties
1874                             sm_conn->sm_connection_encrypted = 0;
1875                             sm_conn->sm_connection_authenticated = 0;
1876                             sm_conn->sm_connection_authorization_state = AUTHORIZATION_UNKNOWN;
1877                             sm_conn->sm_le_db_index = -1;
1878 
1879                             // prepare CSRK lookup (does not involve setup)
1880                             sm_conn->sm_irk_lookup_state = IRK_LOOKUP_W4_READY;
1881 
1882                             // just connected -> everything else happens in sm_run()
1883                             if (sm_conn->sm_role){
1884                                 // slave - state already could be SM_RESPONDER_SEND_SECURITY_REQUEST instead
1885                                 if (sm_conn->sm_engine_state == SM_GENERAL_IDLE){
1886                                     if (sm_slave_request_security) {
1887                                         // request security if requested by app
1888                                         sm_conn->sm_engine_state = SM_RESPONDER_SEND_SECURITY_REQUEST;
1889                                     } else {
1890                                         // otherwise, wait for pairing request
1891                                         sm_conn->sm_engine_state = SM_RESPONDER_IDLE;
1892                                     }
1893                                 }
1894                                 break;
1895                             } else {
1896                                 // master
1897                                 sm_conn->sm_engine_state = SM_INITIATOR_CONNECTED;
1898                             }
1899                             break;
1900 
1901                         case HCI_SUBEVENT_LE_LONG_TERM_KEY_REQUEST:
1902                             con_handle = little_endian_read_16(packet, 3);
1903                             sm_conn = sm_get_connection_for_handle(con_handle);
1904                             if (!sm_conn) break;
1905 
1906                             log_info("LTK Request: state %u", sm_conn->sm_engine_state);
1907                             if (sm_conn->sm_engine_state == SM_RESPONDER_PH2_W4_LTK_REQUEST){
1908                                 sm_conn->sm_engine_state = SM_PH2_CALC_STK;
1909                                 break;
1910                             }
1911 
1912                             // assume that we don't have a LTK for ediv == 0 and random == null
1913                             if (little_endian_read_16(packet, 13) == 0 && sm_is_null_random(&packet[5])){
1914                                 log_info("LTK Request: ediv & random are empty");
1915                                 sm_conn->sm_engine_state = SM_RESPONDER_PH0_SEND_LTK_REQUESTED_NEGATIVE_REPLY;
1916                                 break;
1917                             }
1918 
1919                             // store rand and ediv
1920                             reverse_64(&packet[5], sm_conn->sm_local_rand);
1921                             sm_conn->sm_local_ediv   = little_endian_read_16(packet, 13);
1922                             sm_conn->sm_engine_state = SM_RESPONDER_PH0_RECEIVED_LTK;
1923                             break;
1924 
1925                         default:
1926                             break;
1927                     }
1928                     break;
1929 
1930                 case HCI_EVENT_ENCRYPTION_CHANGE:
1931                     con_handle = little_endian_read_16(packet, 3);
1932                     sm_conn = sm_get_connection_for_handle(con_handle);
1933                     if (!sm_conn) break;
1934 
1935                     sm_conn->sm_connection_encrypted = packet[5];
1936                     log_info("Encryption state change: %u, key size %u", sm_conn->sm_connection_encrypted,
1937                         sm_conn->sm_actual_encryption_key_size);
1938                     log_info("event handler, state %u", sm_conn->sm_engine_state);
1939                     if (!sm_conn->sm_connection_encrypted) break;
1940                     // continue if part of initial pairing
1941                     switch (sm_conn->sm_engine_state){
1942                         case SM_INITIATOR_PH0_W4_CONNECTION_ENCRYPTED:
1943                             sm_conn->sm_engine_state = SM_INITIATOR_CONNECTED;
1944                             sm_done_for_handle(sm_conn->sm_handle);
1945                             break;
1946                         case SM_PH2_W4_CONNECTION_ENCRYPTED:
1947                             if (sm_conn->sm_role){
1948                                 // slave
1949                                 sm_conn->sm_engine_state = SM_PH3_GET_RANDOM;
1950                             } else {
1951                                 // master
1952                                 if (sm_key_distribution_all_received(sm_conn)){
1953                                     // skip receiving keys as there are none
1954                                     sm_key_distribution_handle_all_received(sm_conn);
1955                                     sm_conn->sm_engine_state = SM_PH3_GET_RANDOM;
1956                                 } else {
1957                                     sm_conn->sm_engine_state = SM_PH3_RECEIVE_KEYS;
1958                                 }
1959                             }
1960                             break;
1961                         default:
1962                             break;
1963                     }
1964                     break;
1965 
1966                 case HCI_EVENT_ENCRYPTION_KEY_REFRESH_COMPLETE:
1967                     con_handle = little_endian_read_16(packet, 3);
1968                     sm_conn = sm_get_connection_for_handle(con_handle);
1969                     if (!sm_conn) break;
1970 
1971                     log_info("Encryption key refresh complete, key size %u", sm_conn->sm_actual_encryption_key_size);
1972                     log_info("event handler, state %u", sm_conn->sm_engine_state);
1973                     // continue if part of initial pairing
1974                     switch (sm_conn->sm_engine_state){
1975                         case SM_INITIATOR_PH0_W4_CONNECTION_ENCRYPTED:
1976                             sm_conn->sm_engine_state = SM_INITIATOR_CONNECTED;
1977                             sm_done_for_handle(sm_conn->sm_handle);
1978                             break;
1979                         case SM_PH2_W4_CONNECTION_ENCRYPTED:
1980                             if (sm_conn->sm_role){
1981                                 // slave
1982                                 sm_conn->sm_engine_state = SM_PH3_GET_RANDOM;
1983                             } else {
1984                                 // master
1985                                 sm_conn->sm_engine_state = SM_PH3_RECEIVE_KEYS;
1986                             }
1987                             break;
1988                         default:
1989                             break;
1990                     }
1991                     break;
1992 
1993 
1994                 case HCI_EVENT_DISCONNECTION_COMPLETE:
1995                     con_handle = little_endian_read_16(packet, 3);
1996                     sm_done_for_handle(con_handle);
1997                     sm_conn = sm_get_connection_for_handle(con_handle);
1998                     if (!sm_conn) break;
1999 
2000                     // delete stored bonding on disconnect with authentication failure in ph0
2001                     if (sm_conn->sm_role == 0
2002                         && sm_conn->sm_engine_state == SM_INITIATOR_PH0_W4_CONNECTION_ENCRYPTED
2003                         && packet[2] == ERROR_CODE_AUTHENTICATION_FAILURE){
2004                         le_device_db_remove(sm_conn->sm_le_db_index);
2005                     }
2006 
2007                     sm_conn->sm_engine_state = SM_GENERAL_IDLE;
2008                     sm_conn->sm_handle = 0;
2009                     break;
2010 
2011 				case HCI_EVENT_COMMAND_COMPLETE:
2012                     if (HCI_EVENT_IS_COMMAND_COMPLETE(packet, hci_le_encrypt)){
2013                         sm_handle_encryption_result(&packet[6]);
2014                         break;
2015                     }
2016                     if (HCI_EVENT_IS_COMMAND_COMPLETE(packet, hci_le_rand)){
2017                         sm_handle_random_result(&packet[6]);
2018                         break;
2019                     }
2020                     break;
2021                 default:
2022                     break;
2023 			}
2024             break;
2025         default:
2026             break;
2027 	}
2028 
2029     sm_run();
2030 }
2031 
2032 static inline int sm_calc_actual_encryption_key_size(int other){
2033     if (other < sm_min_encryption_key_size) return 0;
2034     if (other < sm_max_encryption_key_size) return other;
2035     return sm_max_encryption_key_size;
2036 }
2037 
2038 /**
2039  * @return ok
2040  */
2041 static int sm_validate_stk_generation_method(void){
2042     // check if STK generation method is acceptable by client
2043     switch (setup->sm_stk_generation_method){
2044         case JUST_WORKS:
2045             return (sm_accepted_stk_generation_methods & SM_STK_GENERATION_METHOD_JUST_WORKS) != 0;
2046         case PK_RESP_INPUT:
2047         case PK_INIT_INPUT:
2048         case OK_BOTH_INPUT:
2049             return (sm_accepted_stk_generation_methods & SM_STK_GENERATION_METHOD_PASSKEY) != 0;
2050         case OOB:
2051             return (sm_accepted_stk_generation_methods & SM_STK_GENERATION_METHOD_OOB) != 0;
2052         default:
2053             return 0;
2054     }
2055 }
2056 
2057 // helper for sm_pdu_handler, calls sm_run on exit
2058 static void sm_pdu_received_in_wrong_state(sm_connection_t * sm_conn){
2059     setup->sm_pairing_failed_reason = SM_REASON_UNSPECIFIED_REASON;
2060     sm_conn->sm_engine_state = sm_conn->sm_role ? SM_RESPONDER_IDLE : SM_INITIATOR_CONNECTED;
2061     sm_done_for_handle(sm_conn->sm_handle);
2062 }
2063 
2064 static void sm_pdu_handler(uint8_t packet_type, hci_con_handle_t con_handle, uint8_t *packet, uint16_t size){
2065 
2066     if (packet_type == HCI_EVENT_PACKET && packet[0] == L2CAP_EVENT_CAN_SEND_NOW){
2067         sm_run();
2068     }
2069 
2070     if (packet_type != SM_DATA_PACKET) return;
2071 
2072     sm_connection_t * sm_conn = sm_get_connection_for_handle(con_handle);
2073     if (!sm_conn) return;
2074 
2075     if (packet[0] == SM_CODE_PAIRING_FAILED){
2076         sm_conn->sm_engine_state = sm_conn->sm_role ? SM_RESPONDER_IDLE : SM_INITIATOR_CONNECTED;
2077         return;
2078     }
2079 
2080     log_debug("sm_pdu_handler: state %u, pdu 0x%02x", sm_conn->sm_engine_state, packet[0]);
2081 
2082     int err;
2083 
2084     switch (sm_conn->sm_engine_state){
2085 
2086         // a sm timeout requries a new physical connection
2087         case SM_GENERAL_TIMEOUT:
2088             return;
2089 
2090         // Initiator
2091         case SM_INITIATOR_CONNECTED:
2092             if ((packet[0] != SM_CODE_SECURITY_REQUEST) || (sm_conn->sm_role)){
2093                 sm_pdu_received_in_wrong_state(sm_conn);
2094                 break;
2095             }
2096             if (sm_conn->sm_irk_lookup_state == IRK_LOOKUP_FAILED){
2097                 sm_conn->sm_engine_state = SM_INITIATOR_PH1_W2_SEND_PAIRING_REQUEST;
2098                 break;
2099             }
2100             if (sm_conn->sm_irk_lookup_state == IRK_LOOKUP_SUCCEEDED){
2101                 uint16_t ediv;
2102                 le_device_db_encryption_get(sm_conn->sm_le_db_index, &ediv, NULL, NULL, NULL, NULL, NULL);
2103                 if (ediv){
2104                     log_info("sm: Setting up previous ltk/ediv/rand for device index %u", sm_conn->sm_le_db_index);
2105                     sm_conn->sm_engine_state = SM_INITIATOR_PH0_HAS_LTK;
2106                 } else {
2107                     sm_conn->sm_engine_state = SM_INITIATOR_PH1_W2_SEND_PAIRING_REQUEST;
2108                 }
2109                 break;
2110             }
2111             // otherwise, store security request
2112             sm_conn->sm_security_request_received = 1;
2113             break;
2114 
2115         case SM_INITIATOR_PH1_W4_PAIRING_RESPONSE:
2116             if (packet[0] != SM_CODE_PAIRING_RESPONSE){
2117                 sm_pdu_received_in_wrong_state(sm_conn);
2118                 break;
2119             }
2120             // store pairing request
2121             memcpy(&setup->sm_s_pres, packet, sizeof(sm_pairing_packet_t));
2122             err = sm_stk_generation_init(sm_conn);
2123             if (err){
2124                 setup->sm_pairing_failed_reason = err;
2125                 sm_conn->sm_engine_state = SM_GENERAL_SEND_PAIRING_FAILED;
2126                 break;
2127             }
2128             // generate random number first, if we need to show passkey
2129             if (setup->sm_stk_generation_method == PK_RESP_INPUT){
2130                 sm_conn->sm_engine_state = SM_PH2_GET_RANDOM_TK;
2131                 break;
2132             }
2133             sm_conn->sm_engine_state = SM_PH1_W4_USER_RESPONSE;
2134             sm_trigger_user_response(sm_conn);
2135             // response_idle == nothing <--> sm_trigger_user_response() did not require response
2136             if (setup->sm_user_response == SM_USER_RESPONSE_IDLE){
2137                 sm_conn->sm_engine_state = SM_PH2_C1_GET_RANDOM_A;
2138             }
2139             break;
2140 
2141         case SM_INITIATOR_PH2_W4_PAIRING_CONFIRM:
2142             if (packet[0] != SM_CODE_PAIRING_CONFIRM){
2143                 sm_pdu_received_in_wrong_state(sm_conn);
2144                 break;
2145             }
2146 
2147             // store s_confirm
2148             reverse_128(&packet[1], setup->sm_peer_confirm);
2149             sm_conn->sm_engine_state = SM_PH2_SEND_PAIRING_RANDOM;
2150             break;
2151 
2152         case SM_INITIATOR_PH2_W4_PAIRING_RANDOM:
2153             if (packet[0] != SM_CODE_PAIRING_RANDOM){
2154                 sm_pdu_received_in_wrong_state(sm_conn);
2155                 break;;
2156             }
2157 
2158             // received random value
2159             reverse_128(&packet[1], setup->sm_peer_random);
2160             sm_conn->sm_engine_state = SM_PH2_C1_GET_ENC_C;
2161             break;
2162 
2163         // Responder
2164         case SM_RESPONDER_IDLE:
2165         case SM_RESPONDER_SEND_SECURITY_REQUEST:
2166         case SM_RESPONDER_PH1_W4_PAIRING_REQUEST:
2167             if (packet[0] != SM_CODE_PAIRING_REQUEST){
2168                 sm_pdu_received_in_wrong_state(sm_conn);
2169                 break;;
2170             }
2171 
2172             // store pairing request
2173             memcpy(&sm_conn->sm_m_preq, packet, sizeof(sm_pairing_packet_t));
2174             sm_conn->sm_engine_state = SM_RESPONDER_PH1_PAIRING_REQUEST_RECEIVED;
2175             break;
2176 
2177         case SM_RESPONDER_PH1_W4_PAIRING_CONFIRM:
2178             if (packet[0] != SM_CODE_PAIRING_CONFIRM){
2179                 sm_pdu_received_in_wrong_state(sm_conn);
2180                 break;;
2181             }
2182 
2183             // received confirm value
2184             reverse_128(&packet[1], setup->sm_peer_confirm);
2185 
2186             // notify client to hide shown passkey
2187             if (setup->sm_stk_generation_method == PK_INIT_INPUT){
2188                 sm_notify_client_base(SM_EVENT_PASSKEY_DISPLAY_CANCEL, sm_conn->sm_handle, sm_conn->sm_peer_addr_type, sm_conn->sm_peer_address);
2189             }
2190 
2191             // handle user cancel pairing?
2192             if (setup->sm_user_response == SM_USER_RESPONSE_DECLINE){
2193                 setup->sm_pairing_failed_reason = SM_REASON_PASSKEYT_ENTRY_FAILED;
2194                 sm_conn->sm_engine_state = SM_GENERAL_SEND_PAIRING_FAILED;
2195                 break;
2196             }
2197 
2198             // wait for user action?
2199             if (setup->sm_user_response == SM_USER_RESPONSE_PENDING){
2200                 sm_conn->sm_engine_state = SM_PH1_W4_USER_RESPONSE;
2201                 break;
2202             }
2203 
2204             // calculate and send local_confirm
2205             sm_conn->sm_engine_state = SM_PH2_C1_GET_RANDOM_A;
2206             break;
2207 
2208         case SM_RESPONDER_PH2_W4_PAIRING_RANDOM:
2209             if (packet[0] != SM_CODE_PAIRING_RANDOM){
2210                 sm_pdu_received_in_wrong_state(sm_conn);
2211                 break;;
2212             }
2213 
2214             // received random value
2215             reverse_128(&packet[1], setup->sm_peer_random);
2216             sm_conn->sm_engine_state = SM_PH2_C1_GET_ENC_C;
2217             break;
2218 
2219         case SM_PH3_RECEIVE_KEYS:
2220             switch(packet[0]){
2221                 case SM_CODE_ENCRYPTION_INFORMATION:
2222                     setup->sm_key_distribution_received_set |= SM_KEYDIST_FLAG_ENCRYPTION_INFORMATION;
2223                     reverse_128(&packet[1], setup->sm_peer_ltk);
2224                     break;
2225 
2226                 case SM_CODE_MASTER_IDENTIFICATION:
2227                     setup->sm_key_distribution_received_set |= SM_KEYDIST_FLAG_MASTER_IDENTIFICATION;
2228                     setup->sm_peer_ediv = little_endian_read_16(packet, 1);
2229                     reverse_64(&packet[3], setup->sm_peer_rand);
2230                     break;
2231 
2232                 case SM_CODE_IDENTITY_INFORMATION:
2233                     setup->sm_key_distribution_received_set |= SM_KEYDIST_FLAG_IDENTITY_INFORMATION;
2234                     reverse_128(&packet[1], setup->sm_peer_irk);
2235                     break;
2236 
2237                 case SM_CODE_IDENTITY_ADDRESS_INFORMATION:
2238                     setup->sm_key_distribution_received_set |= SM_KEYDIST_FLAG_IDENTITY_ADDRESS_INFORMATION;
2239                     setup->sm_peer_addr_type = packet[1];
2240                     reverse_bd_addr(&packet[2], setup->sm_peer_address);
2241                     break;
2242 
2243                 case SM_CODE_SIGNING_INFORMATION:
2244                     setup->sm_key_distribution_received_set |= SM_KEYDIST_FLAG_SIGNING_IDENTIFICATION;
2245                     reverse_128(&packet[1], setup->sm_peer_csrk);
2246                     break;
2247                 default:
2248                     // Unexpected PDU
2249                     log_info("Unexpected PDU %u in SM_PH3_RECEIVE_KEYS", packet[0]);
2250                     break;
2251             }
2252             // done with key distribution?
2253             if (sm_key_distribution_all_received(sm_conn)){
2254 
2255                 sm_key_distribution_handle_all_received(sm_conn);
2256 
2257                 if (sm_conn->sm_role){
2258                     sm_conn->sm_engine_state = SM_RESPONDER_IDLE;
2259                     sm_done_for_handle(sm_conn->sm_handle);
2260                 } else {
2261                     sm_conn->sm_engine_state = SM_PH3_GET_RANDOM;
2262                 }
2263             }
2264             break;
2265         default:
2266             // Unexpected PDU
2267             log_info("Unexpected PDU %u in state %u", packet[0], sm_conn->sm_engine_state);
2268             break;
2269     }
2270 
2271     // try to send preparared packet
2272     sm_run();
2273 }
2274 
2275 // Security Manager Client API
2276 void sm_register_oob_data_callback( int (*get_oob_data_callback)(uint8_t addres_type, bd_addr_t addr, uint8_t * oob_data)){
2277     sm_get_oob_data = get_oob_data_callback;
2278 }
2279 
2280 void sm_add_event_handler(btstack_packet_callback_registration_t * callback_handler){
2281     btstack_linked_list_add_tail(&sm_event_handlers, (btstack_linked_item_t*) callback_handler);
2282 }
2283 
2284 void sm_set_accepted_stk_generation_methods(uint8_t accepted_stk_generation_methods){
2285     sm_accepted_stk_generation_methods = accepted_stk_generation_methods;
2286 }
2287 
2288 void sm_set_encryption_key_size_range(uint8_t min_size, uint8_t max_size){
2289 	sm_min_encryption_key_size = min_size;
2290 	sm_max_encryption_key_size = max_size;
2291 }
2292 
2293 void sm_set_authentication_requirements(uint8_t auth_req){
2294     sm_auth_req = auth_req;
2295 }
2296 
2297 void sm_set_io_capabilities(io_capability_t io_capability){
2298     sm_io_capabilities = io_capability;
2299 }
2300 
2301 void sm_set_request_security(int enable){
2302     sm_slave_request_security = enable;
2303 }
2304 
2305 void sm_set_er(sm_key_t er){
2306     memcpy(sm_persistent_er, er, 16);
2307 }
2308 
2309 void sm_set_ir(sm_key_t ir){
2310     memcpy(sm_persistent_ir, ir, 16);
2311 }
2312 
2313 // Testing support only
2314 void sm_test_set_irk(sm_key_t irk){
2315     memcpy(sm_persistent_irk, irk, 16);
2316     sm_persistent_irk_ready = 1;
2317 }
2318 
2319 void sm_test_use_fixed_local_csrk(void){
2320     test_use_fixed_local_csrk = 1;
2321 }
2322 
2323 void sm_init(void){
2324     // set some (BTstack default) ER and IR
2325     int i;
2326     sm_key_t er;
2327     sm_key_t ir;
2328     for (i=0;i<16;i++){
2329         er[i] = 0x30 + i;
2330         ir[i] = 0x90 + i;
2331     }
2332     sm_set_er(er);
2333     sm_set_ir(ir);
2334     // defaults
2335     sm_accepted_stk_generation_methods = SM_STK_GENERATION_METHOD_JUST_WORKS
2336                                        | SM_STK_GENERATION_METHOD_OOB
2337                                        | SM_STK_GENERATION_METHOD_PASSKEY;
2338     sm_max_encryption_key_size = 16;
2339     sm_min_encryption_key_size = 7;
2340 
2341     sm_cmac_state  = CMAC_IDLE;
2342     dkg_state = DKG_W4_WORKING;
2343     rau_state = RAU_W4_WORKING;
2344     sm_aes128_state = SM_AES128_IDLE;
2345     sm_address_resolution_test = -1;    // no private address to resolve yet
2346     sm_address_resolution_ah_calculation_active = 0;
2347     sm_address_resolution_mode = ADDRESS_RESOLUTION_IDLE;
2348     sm_address_resolution_general_queue = NULL;
2349 
2350     gap_random_adress_update_period = 15 * 60 * 1000L;
2351 
2352     sm_active_connection = 0;
2353 
2354     test_use_fixed_local_csrk = 0;
2355 
2356     // register for HCI Events from HCI
2357     hci_event_callback_registration.callback = &sm_event_packet_handler;
2358     hci_add_event_handler(&hci_event_callback_registration);
2359 
2360     // and L2CAP PDUs + L2CAP_EVENT_CAN_SEND_NOW
2361     l2cap_register_fixed_channel(sm_pdu_handler, L2CAP_CID_SECURITY_MANAGER_PROTOCOL);
2362 }
2363 
2364 static sm_connection_t * sm_get_connection_for_handle(hci_con_handle_t con_handle){
2365     hci_connection_t * hci_con = hci_connection_for_handle(con_handle);
2366     if (!hci_con) return NULL;
2367     return &hci_con->sm_connection;
2368 }
2369 
2370 // @returns 0 if not encrypted, 7-16 otherwise
2371 int sm_encryption_key_size(hci_con_handle_t con_handle){
2372     sm_connection_t * sm_conn = sm_get_connection_for_handle(con_handle);
2373     if (!sm_conn) return 0;     // wrong connection
2374     if (!sm_conn->sm_connection_encrypted) return 0;
2375     return sm_conn->sm_actual_encryption_key_size;
2376 }
2377 
2378 int sm_authenticated(hci_con_handle_t con_handle){
2379     sm_connection_t * sm_conn = sm_get_connection_for_handle(con_handle);
2380     if (!sm_conn) return 0;     // wrong connection
2381     if (!sm_conn->sm_connection_encrypted) return 0; // unencrypted connection cannot be authenticated
2382     return sm_conn->sm_connection_authenticated;
2383 }
2384 
2385 authorization_state_t sm_authorization_state(hci_con_handle_t con_handle){
2386     sm_connection_t * sm_conn = sm_get_connection_for_handle(con_handle);
2387     if (!sm_conn) return AUTHORIZATION_UNKNOWN;     // wrong connection
2388     if (!sm_conn->sm_connection_encrypted)               return AUTHORIZATION_UNKNOWN; // unencrypted connection cannot be authorized
2389     if (!sm_conn->sm_connection_authenticated)           return AUTHORIZATION_UNKNOWN; // unauthenticatd connection cannot be authorized
2390     return sm_conn->sm_connection_authorization_state;
2391 }
2392 
2393 static void sm_send_security_request_for_connection(sm_connection_t * sm_conn){
2394     switch (sm_conn->sm_engine_state){
2395         case SM_GENERAL_IDLE:
2396         case SM_RESPONDER_IDLE:
2397             sm_conn->sm_engine_state = SM_RESPONDER_SEND_SECURITY_REQUEST;
2398             sm_run();
2399             break;
2400         default:
2401             break;
2402     }
2403 }
2404 
2405 /**
2406  * @brief Trigger Security Request
2407  */
2408 void sm_send_security_request(hci_con_handle_t con_handle){
2409     sm_connection_t * sm_conn = sm_get_connection_for_handle(con_handle);
2410     if (!sm_conn) return;
2411     sm_send_security_request_for_connection(sm_conn);
2412 }
2413 
2414 // request pairing
2415 void sm_request_pairing(hci_con_handle_t con_handle){
2416     sm_connection_t * sm_conn = sm_get_connection_for_handle(con_handle);
2417     if (!sm_conn) return;     // wrong connection
2418 
2419     log_info("sm_request_pairing in role %u, state %u", sm_conn->sm_role, sm_conn->sm_engine_state);
2420     if (sm_conn->sm_role){
2421         sm_send_security_request_for_connection(sm_conn);
2422     } else {
2423         // used as a trigger to start central/master/initiator security procedures
2424             uint16_t ediv;
2425             if (sm_conn->sm_engine_state == SM_INITIATOR_CONNECTED){
2426             switch (sm_conn->sm_irk_lookup_state){
2427                 case IRK_LOOKUP_FAILED:
2428                     sm_conn->sm_engine_state = SM_INITIATOR_PH1_W2_SEND_PAIRING_REQUEST;
2429                     break;
2430                 case IRK_LOOKUP_SUCCEEDED:
2431                         le_device_db_encryption_get(sm_conn->sm_le_db_index, &ediv, NULL, NULL, NULL, NULL, NULL);
2432                         if (ediv){
2433                             log_info("sm: Setting up previous ltk/ediv/rand for device index %u", sm_conn->sm_le_db_index);
2434                             sm_conn->sm_engine_state = SM_INITIATOR_PH0_HAS_LTK;
2435                         } else {
2436                             sm_conn->sm_engine_state = SM_INITIATOR_PH1_W2_SEND_PAIRING_REQUEST;
2437                         }
2438                         break;
2439                 default:
2440                     sm_conn->sm_bonding_requested = 1;
2441                     break;
2442             }
2443         }
2444     }
2445     sm_run();
2446 }
2447 
2448 // called by client app on authorization request
2449 void sm_authorization_decline(hci_con_handle_t con_handle){
2450     sm_connection_t * sm_conn = sm_get_connection_for_handle(con_handle);
2451     if (!sm_conn) return;     // wrong connection
2452     sm_conn->sm_connection_authorization_state = AUTHORIZATION_DECLINED;
2453     sm_notify_client_authorization(SM_EVENT_AUTHORIZATION_RESULT, sm_conn->sm_handle, sm_conn->sm_peer_addr_type, sm_conn->sm_peer_address, 0);
2454 }
2455 
2456 void sm_authorization_grant(hci_con_handle_t con_handle){
2457     sm_connection_t * sm_conn = sm_get_connection_for_handle(con_handle);
2458     if (!sm_conn) return;     // wrong connection
2459     sm_conn->sm_connection_authorization_state = AUTHORIZATION_GRANTED;
2460     sm_notify_client_authorization(SM_EVENT_AUTHORIZATION_RESULT, sm_conn->sm_handle, sm_conn->sm_peer_addr_type, sm_conn->sm_peer_address, 1);
2461 }
2462 
2463 // GAP Bonding API
2464 
2465 void sm_bonding_decline(hci_con_handle_t con_handle){
2466     sm_connection_t * sm_conn = sm_get_connection_for_handle(con_handle);
2467     if (!sm_conn) return;     // wrong connection
2468     setup->sm_user_response = SM_USER_RESPONSE_DECLINE;
2469 
2470     if (sm_conn->sm_engine_state == SM_PH1_W4_USER_RESPONSE){
2471         sm_done_for_handle(sm_conn->sm_handle);
2472         setup->sm_pairing_failed_reason = SM_REASON_PASSKEYT_ENTRY_FAILED;
2473         sm_conn->sm_engine_state = SM_GENERAL_SEND_PAIRING_FAILED;
2474     }
2475     sm_run();
2476 }
2477 
2478 void sm_just_works_confirm(hci_con_handle_t con_handle){
2479     sm_connection_t * sm_conn = sm_get_connection_for_handle(con_handle);
2480     if (!sm_conn) return;     // wrong connection
2481     setup->sm_user_response = SM_USER_RESPONSE_CONFIRM;
2482     if (sm_conn->sm_engine_state == SM_PH1_W4_USER_RESPONSE){
2483         sm_conn->sm_engine_state = SM_PH2_C1_GET_RANDOM_A;
2484     }
2485     sm_run();
2486 }
2487 
2488 void sm_passkey_input(hci_con_handle_t con_handle, uint32_t passkey){
2489     sm_connection_t * sm_conn = sm_get_connection_for_handle(con_handle);
2490     if (!sm_conn) return;     // wrong connection
2491     sm_reset_tk();
2492     big_endian_store_32(setup->sm_tk, 12, passkey);
2493     setup->sm_user_response = SM_USER_RESPONSE_PASSKEY;
2494     if (sm_conn->sm_engine_state == SM_PH1_W4_USER_RESPONSE){
2495         sm_conn->sm_engine_state = SM_PH2_C1_GET_RANDOM_A;
2496     }
2497     sm_run();
2498 }
2499 
2500 /**
2501  * @brief Identify device in LE Device DB
2502  * @param handle
2503  * @returns index from le_device_db or -1 if not found/identified
2504  */
2505 int sm_le_device_index(hci_con_handle_t con_handle ){
2506     sm_connection_t * sm_conn = sm_get_connection_for_handle(con_handle);
2507     if (!sm_conn) return -1;
2508     return sm_conn->sm_le_db_index;
2509 }
2510 
2511 // GAP LE API
2512 void gap_random_address_set_mode(gap_random_address_type_t random_address_type){
2513     gap_random_address_update_stop();
2514     gap_random_adress_type = random_address_type;
2515     if (random_address_type == GAP_RANDOM_ADDRESS_TYPE_OFF) return;
2516     gap_random_address_update_start();
2517     gap_random_address_trigger();
2518 }
2519 
2520 gap_random_address_type_t gap_random_address_get_mode(void){
2521     return gap_random_adress_type;
2522 }
2523 
2524 void gap_random_address_set_update_period(int period_ms){
2525     gap_random_adress_update_period = period_ms;
2526     if (gap_random_adress_type == GAP_RANDOM_ADDRESS_TYPE_OFF) return;
2527     gap_random_address_update_stop();
2528     gap_random_address_update_start();
2529 }
2530 
2531 void gap_random_address_set(bd_addr_t addr){
2532     gap_random_address_set_mode(GAP_RANDOM_ADDRESS_TYPE_OFF);
2533     memcpy(sm_random_address, addr, 6);
2534     rau_state = RAU_SET_ADDRESS;
2535     sm_run();
2536 }
2537 
2538 /*
2539  * @brief Set Advertisement Paramters
2540  * @param adv_int_min
2541  * @param adv_int_max
2542  * @param adv_type
2543  * @param direct_address_type
2544  * @param direct_address
2545  * @param channel_map
2546  * @param filter_policy
2547  *
2548  * @note own_address_type is used from gap_random_address_set_mode
2549  */
2550 void gap_advertisements_set_params(uint16_t adv_int_min, uint16_t adv_int_max, uint8_t adv_type,
2551     uint8_t direct_address_typ, bd_addr_t direct_address, uint8_t channel_map, uint8_t filter_policy){
2552     hci_le_advertisements_set_params(adv_int_min, adv_int_max, adv_type, gap_random_adress_type,
2553         direct_address_typ, direct_address, channel_map, filter_policy);
2554 }
2555 
2556