1 #include <stdint.h> 2 #include <stdio.h> 3 #include <stdlib.h> 4 #include <string.h> 5 6 #include "ble/att_db.h" 7 #include "hci.h" 8 #include "hci_dump.h" 9 #include "l2cap.h" 10 #include "rijndael.h" 11 #include "btstack_linked_list.h" 12 #include "btstack_run_loop_embedded.h" 13 #include "btstack_debug.h" 14 15 static btstack_packet_handler_t le_data_handler; 16 17 static uint8_t packet_buffer[256]; 18 static uint16_t packet_buffer_len = 0; 19 20 static uint8_t aes128_cyphertext[16]; 21 22 static hci_connection_t the_connection; 23 static btstack_linked_list_t connections; 24 static btstack_linked_list_t event_packet_handlers; 25 26 void mock_init(void){ 27 the_connection.item.next = NULL; 28 connections = (btstack_linked_item_t*) &the_connection; 29 } 30 31 uint8_t * mock_packet_buffer(void){ 32 return packet_buffer; 33 } 34 35 void mock_clear_packet_buffer(void){ 36 packet_buffer_len = 0; 37 memset(packet_buffer, 0, sizeof(packet_buffer)); 38 } 39 40 static void dump_packet(int packet_type, uint8_t * buffer, uint16_t size){ 41 #if 0 42 static int packet_counter = 1; 43 char var_name[80]; 44 sprintf(var_name, "test_%s_packet_%02u", packet_type == HCI_COMMAND_DATA_PACKET ? "command" : "acl", packet_counter); 45 printf("uint8_t %s[] = { ", var_name); 46 for (int i = 0; i < size ; i++){ 47 if ((i % 16) == 0) printf("\n "); 48 printf ("0x%02x, ", buffer[i]); 49 } 50 printf("};\n"); 51 packet_counter++; 52 #endif 53 } 54 55 void aes128_calc_cyphertext(uint8_t key[16], uint8_t plaintext[16], uint8_t cyphertext[16]){ 56 uint32_t rk[RKLENGTH(KEYBITS)]; 57 int nrounds = rijndaelSetupEncrypt(rk, &key[0], KEYBITS); 58 rijndaelEncrypt(rk, nrounds, plaintext, cyphertext); 59 } 60 61 void mock_simulate_hci_event(uint8_t * packet, uint16_t size){ 62 hci_dump_packet(HCI_EVENT_PACKET, 1, packet, size); 63 btstack_linked_list_iterator_t it; 64 btstack_linked_list_iterator_init(&it ,&event_packet_handlers); 65 while (btstack_linked_list_iterator_has_next(&it)){ 66 btstack_packet_callback_registration_t * item = (btstack_packet_callback_registration_t *) btstack_linked_list_iterator_next(&it); 67 item->callback(HCI_EVENT_PACKET, 0, packet, size); 68 } 69 if (le_data_handler){ 70 le_data_handler(HCI_EVENT_PACKET, 0, packet, size); 71 } 72 } 73 74 void aes128_report_result(void){ 75 uint8_t le_enc_result[22]; 76 uint8_t enc1_data[] = { 0x0e, 0x14, 0x01, 0x17, 0x20, 0x00 }; 77 memcpy (le_enc_result, enc1_data, 6); 78 reverse_128(aes128_cyphertext, &le_enc_result[6]); 79 mock_simulate_hci_event(&le_enc_result[0], sizeof(le_enc_result)); 80 } 81 82 void mock_simulate_sm_data_packet(uint8_t * packet, uint16_t len){ 83 84 uint16_t handle = 0x40; 85 uint16_t cid = 0x06; 86 87 uint8_t acl_buffer[len + 8]; 88 89 // 0 - Connection handle : PB=10 : BC=00 90 little_endian_store_16(acl_buffer, 0, handle | (0 << 12) | (0 << 14)); 91 // 2 - ACL length 92 little_endian_store_16(acl_buffer, 2, len + 4); 93 // 4 - L2CAP packet length 94 little_endian_store_16(acl_buffer, 4, len + 0); 95 // 6 - L2CAP channel DEST 96 little_endian_store_16(acl_buffer, 6, cid); 97 98 memcpy(&acl_buffer[8], packet, len); 99 hci_dump_packet(HCI_ACL_DATA_PACKET, 1, &acl_buffer[0], len + 8); 100 101 le_data_handler(SM_DATA_PACKET, handle, packet, len); 102 103 // process queued callbacks, might become obsolete after queued callback integration in run loop 104 btstack_run_loop_embedded_execute_once(); 105 } 106 107 void mock_simulate_command_complete(const hci_cmd_t *cmd){ 108 uint8_t packet[] = {HCI_EVENT_COMMAND_COMPLETE, 4, 1, (uint8_t) cmd->opcode & 0xff, (uint8_t) cmd->opcode >> 8, 0}; 109 mock_simulate_hci_event((uint8_t *)&packet, sizeof(packet)); 110 } 111 112 void mock_simulate_hci_state_working(void){ 113 uint8_t packet[] = {BTSTACK_EVENT_STATE, 0, HCI_STATE_WORKING}; 114 mock_simulate_hci_event((uint8_t *)&packet, sizeof(packet)); 115 } 116 117 118 static void hci_create_gap_connection_complete_event(const uint8_t * hci_event, uint8_t * gap_event) { 119 gap_event[0] = HCI_EVENT_META_GAP; 120 gap_event[1] = 36 - 2; 121 gap_event[2] = GAP_SUBEVENT_LE_CONNECTION_COMPLETE; 122 switch (hci_event[2]){ 123 case HCI_SUBEVENT_LE_CONNECTION_COMPLETE: 124 memcpy(&gap_event[3], &hci_event[3], 11); 125 memset(&gap_event[14], 0, 12); 126 memcpy(&gap_event[26], &hci_event[14], 7); 127 memset(&gap_event[33], 0xff, 3); 128 break; 129 case HCI_SUBEVENT_LE_ENHANCED_CONNECTION_COMPLETE_V1: 130 memcpy(&gap_event[3], &hci_event[3], 30); 131 memset(&gap_event[33], 0xff, 3); 132 break; 133 case HCI_SUBEVENT_LE_ENHANCED_CONNECTION_COMPLETE_V2: 134 memcpy(&gap_event[3], &hci_event[3], 33); 135 break; 136 default: 137 btstack_unreachable(); 138 break; 139 } 140 } 141 142 void mock_simulate_connected(void){ 143 uint8_t packet[] = { 0x3e, 0x13, 0x01, 0x00, 0x40, 0x00, 0x01, 0x01, 0x18, 0x12, 0x5e, 0x68, 0xc9, 0x73, 0x18, 0x00, 0x00, 0x00, 0x48, 0x00, 0x05}; 144 uint8_t gap_event[36]; 145 hci_create_gap_connection_complete_event(packet, gap_event); 146 mock_simulate_hci_event(gap_event, sizeof(gap_event)); 147 } 148 149 void att_init_connection(att_connection_t * att_connection){ 150 att_connection->mtu = 23; 151 att_connection->encryption_key_size = 0; 152 att_connection->authenticated = 0; 153 att_connection->authorized = 0; 154 } 155 156 void gap_local_bd_addr(bd_addr_t address_buffer){ 157 int i; 158 for (i=0;i<6;i++) { 159 address_buffer[i] = 0x11 * (i+1); 160 } 161 } 162 163 void hci_halting_defer(void){ 164 } 165 166 bool hci_can_send_command_packet_now(void){ 167 return true; 168 } 169 bool hci_can_send_packet_now_using_packet_buffer(uint8_t packet_type){ 170 return true; 171 } 172 173 hci_connection_t * hci_connection_for_bd_addr_and_type(const bd_addr_t addr, bd_addr_type_t addr_type){ 174 return &the_connection; 175 } 176 hci_connection_t * hci_connection_for_handle(hci_con_handle_t con_handle){ 177 return &the_connection; 178 } 179 void hci_connections_get_iterator(btstack_linked_list_iterator_t *it){ 180 btstack_linked_list_iterator_init(it, &connections); 181 } 182 183 // get addr type and address used in different contexts 184 void gap_le_get_own_address(uint8_t * addr_type, bd_addr_t addr){ 185 static uint8_t dummy[] = { 0x00, 0x1b, 0xdc, 0x07, 0x32, 0xef }; 186 *addr_type = 0; 187 memcpy(addr, dummy, 6); 188 } 189 void gap_le_get_own_advertisements_address(uint8_t * addr_type, bd_addr_t addr){ 190 gap_le_get_own_address(addr_type, addr); 191 } 192 void gap_le_get_own_connection_address(uint8_t * addr_type, bd_addr_t addr){ 193 gap_le_get_own_address(addr_type, addr); 194 } 195 196 void hci_le_advertisements_set_params(uint16_t adv_int_min, uint16_t adv_int_max, uint8_t adv_type, 197 uint8_t direct_address_typ, bd_addr_t direct_address, uint8_t channel_map, uint8_t filter_policy) { 198 } 199 200 uint16_t hci_get_manufacturer(void){ 201 return 0xffff; 202 }; 203 204 void hci_le_set_own_address_type(uint8_t own_address){ 205 } 206 207 void hci_le_random_address_set(const bd_addr_t addr){ 208 } 209 210 bool hci_is_le_identity_address_type(bd_addr_type_t address_type) { 211 switch (address_type) { 212 case BD_ADDR_TYPE_LE_PUBLIC_IDENTITY: 213 case BD_ADDR_TYPE_LE_RANDOM_IDENTITY: 214 return true; 215 default: 216 return false; 217 } 218 } 219 220 void l2cap_request_can_send_fix_channel_now_event(hci_con_handle_t con_handle, uint16_t cid){ 221 if (packet_buffer_len) return; 222 uint8_t event[] = { L2CAP_EVENT_CAN_SEND_NOW, 2, 0, 0}; 223 little_endian_store_16(event, 2, cid); 224 le_data_handler(HCI_EVENT_PACKET, 0, event, sizeof(event)); 225 } 226 227 bool l2cap_can_send_connectionless_packet_now(void){ 228 return packet_buffer_len == 0; 229 } 230 231 bool l2cap_can_send_fixed_channel_packet_now(uint16_t handle, uint16_t channel_id){ 232 return packet_buffer_len == 0; 233 } 234 235 uint8_t hci_send_cmd(const hci_cmd_t *cmd, ...){ 236 va_list argptr; 237 va_start(argptr, cmd); 238 uint16_t len = hci_cmd_create_from_template(packet_buffer, cmd, argptr); 239 va_end(argptr); 240 hci_dump_packet(HCI_COMMAND_DATA_PACKET, 0, packet_buffer, len); 241 dump_packet(HCI_COMMAND_DATA_PACKET, packet_buffer, len); 242 packet_buffer_len = len; 243 244 // track le encrypt and le rand 245 if (cmd->opcode == hci_le_encrypt.opcode){ 246 uint8_t * key_flipped = &packet_buffer[3]; 247 uint8_t key[16]; 248 reverse_128(key_flipped, key); 249 // printf("le_encrypt key "); 250 // hexdump(key, 16); 251 uint8_t * plaintext_flipped = &packet_buffer[19]; 252 uint8_t plaintext[16]; 253 reverse_128(plaintext_flipped, plaintext); 254 // printf("le_encrypt txt "); 255 // hexdump(plaintext, 16); 256 aes128_calc_cyphertext(key, plaintext, aes128_cyphertext); 257 // printf("le_encrypt res "); 258 // hexdump(aes128_cyphertext, 16); 259 } 260 return ERROR_CODE_SUCCESS; 261 } 262 263 void l2cap_register_fixed_channel(btstack_packet_handler_t packet_handler, uint16_t channel_id) { 264 le_data_handler = packet_handler; 265 } 266 267 void hci_add_event_handler(btstack_packet_callback_registration_t * callback_handler){ 268 btstack_linked_list_add(&event_packet_handlers, (btstack_linked_item_t *) callback_handler); 269 } 270 271 void l2cap_reserve_packet_buffer(void){ 272 printf("l2cap_reserve_packet_buffer\n"); 273 } 274 275 uint8_t l2cap_send_prepared_connectionless(uint16_t handle, uint16_t cid, uint16_t len){ 276 printf("l2cap_send_prepared_connectionless\n"); 277 return 0; 278 } 279 280 uint8_t l2cap_send_connectionless(uint16_t handle, uint16_t cid, uint8_t * buffer, uint16_t len){ 281 // printf("l2cap_send_connectionless\n"); 282 283 int pb = hci_non_flushable_packet_boundary_flag_supported() ? 0x00 : 0x02; 284 285 // 0 - Connection handle : PB=pb : BC=00 286 little_endian_store_16(packet_buffer, 0, handle | (pb << 12) | (0 << 14)); 287 // 2 - ACL length 288 little_endian_store_16(packet_buffer, 2, len + 4); 289 // 4 - L2CAP packet length 290 little_endian_store_16(packet_buffer, 4, len + 0); 291 // 6 - L2CAP channel DEST 292 little_endian_store_16(packet_buffer, 6, cid); 293 294 memcpy(&packet_buffer[8], buffer, len); 295 hci_dump_packet(HCI_ACL_DATA_PACKET, 0, &packet_buffer[0], len + 8); 296 297 dump_packet(HCI_ACL_DATA_PACKET, packet_buffer, len + 8); 298 packet_buffer_len = len + 8; 299 300 return ERROR_CODE_SUCCESS; 301 } 302 303 void hci_disconnect_security_block(hci_con_handle_t con_handle){ 304 printf("hci_disconnect_security_block \n"); 305 } 306 307 bool hci_non_flushable_packet_boundary_flag_supported(void){ 308 return true; 309 } 310 311 void l2cap_run(void){ 312 } 313 314 HCI_STATE hci_get_state(void){ 315 return HCI_STATE_WORKING; 316 } 317 318 #include "hal_cpu.h" 319 void hal_cpu_disable_irqs(void){} 320 void hal_cpu_enable_irqs(void){} 321 void hal_cpu_enable_irqs_and_sleep(void){} 322 323 #include "hal_time_ms.h" 324 static uint32_t time_ms; 325 uint32_t hal_time_ms(void){ 326 return time_ms++; 327 } 328