1 #include <stdint.h> 2 #include <stdio.h> 3 #include <stdlib.h> 4 #include <string.h> 5 6 #include "ble/att.h" 7 #include "hci.h" 8 #include "hci_dump.h" 9 #include "l2cap.h" 10 #include "rijndael.h" 11 12 13 static btstack_packet_handler_t le_data_handler; 14 static void (*event_packet_handler) (void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) = NULL; 15 16 static uint8_t packet_buffer[256]; 17 static uint16_t packet_buffer_len = 0; 18 19 static uint8_t aes128_cyphertext[16]; 20 21 static hci_connection_t the_connection; 22 static btstack_btstack_linked_list_t connections; 23 24 void mock_init(void){ 25 the_connection.item.next = NULL; 26 connections = (linked_item*) &the_connection; 27 } 28 29 uint8_t * mock_packet_buffer(void){ 30 return packet_buffer; 31 } 32 33 void mock_clear_packet_buffer(void){ 34 packet_buffer_len = 0; 35 memset(packet_buffer, 0, sizeof(packet_buffer)); 36 } 37 38 static void dump_packet(int packet_type, uint8_t * buffer, uint16_t size){ 39 #if 0 40 static int packet_counter = 1; 41 char var_name[80]; 42 sprintf(var_name, "test_%s_packet_%02u", packet_type == HCI_COMMAND_DATA_PACKET ? "command" : "acl", packet_counter); 43 printf("uint8_t %s[] = { ", var_name); 44 for (int i = 0; i < size ; i++){ 45 if ((i % 16) == 0) printf("\n "); 46 printf ("0x%02x, ", buffer[i]); 47 } 48 printf("};\n"); 49 packet_counter++; 50 #endif 51 } 52 53 void aes128_calc_cyphertext(uint8_t key[16], uint8_t plaintext[16], uint8_t cyphertext[16]){ 54 uint32_t rk[RKLENGTH(KEYBITS)]; 55 int nrounds = rijndaelSetupEncrypt(rk, &key[0], KEYBITS); 56 rijndaelEncrypt(rk, nrounds, plaintext, cyphertext); 57 } 58 59 void mock_simulate_hci_event(uint8_t * packet, uint16_t size){ 60 hci_dump_packet(HCI_EVENT_PACKET, 1, packet, size); 61 if (event_packet_handler){ 62 event_packet_handler(NULL, HCI_EVENT_PACKET, NULL, packet, size); 63 } 64 if (le_data_handler){ 65 le_data_handler(HCI_EVENT_PACKET, NULL, packet, size); 66 } 67 } 68 69 void aes128_report_result(void){ 70 uint8_t le_enc_result[22]; 71 uint8_t enc1_data[] = { 0x0e, 0x14, 0x01, 0x17, 0x20, 0x00 }; 72 memcpy (le_enc_result, enc1_data, 6); 73 swap128(aes128_cyphertext, &le_enc_result[6]); 74 mock_simulate_hci_event(&le_enc_result[0], sizeof(le_enc_result)); 75 } 76 77 void mock_simulate_sm_data_packet(uint8_t * packet, uint16_t len){ 78 79 uint16_t handle = 0x40; 80 uint16_t cid = 0x06; 81 82 uint8_t acl_buffer[len + 8]; 83 84 // 0 - Connection handle : PB=10 : BC=00 85 bt_store_16(acl_buffer, 0, handle | (0 << 12) | (0 << 14)); 86 // 2 - ACL length 87 bt_store_16(acl_buffer, 2, len + 4); 88 // 4 - L2CAP packet length 89 bt_store_16(acl_buffer, 4, len + 0); 90 // 6 - L2CAP channel DEST 91 bt_store_16(acl_buffer, 6, cid); 92 93 memcpy(&acl_buffer[8], packet, len); 94 hci_dump_packet(HCI_ACL_DATA_PACKET, 1, &acl_buffer[0], len + 8); 95 96 le_data_handler(SM_DATA_PACKET, handle, packet, len); 97 } 98 99 void mock_simulate_command_complete(const hci_cmd_t *cmd){ 100 uint8_t packet[] = {HCI_EVENT_COMMAND_COMPLETE, 4, 1, cmd->opcode & 0xff, cmd->opcode >> 8, 0}; 101 mock_simulate_hci_event((uint8_t *)&packet, sizeof(packet)); 102 } 103 104 void mock_simulate_hci_state_working(void){ 105 uint8_t packet[] = {BTSTACK_EVENT_STATE, 0, HCI_STATE_WORKING}; 106 mock_simulate_hci_event((uint8_t *)&packet, sizeof(packet)); 107 } 108 109 void mock_simulate_connected(void){ 110 uint8_t packet[] = { 0x3e, 0x13, 0x01, 0x00, 0x40, 0x00, 0x01, 0x01, 0x18, 0x12, 0x5e, 0x68, 0xc9, 0x73, 0x18, 0x00, 0x00, 0x00, 0x48, 0x00, 0x05}; 111 mock_simulate_hci_event((uint8_t *)&packet, sizeof(packet)); 112 } 113 114 void att_init_connection(att_connection_t * att_connection){ 115 att_connection->mtu = 23; 116 att_connection->encryption_key_size = 0; 117 att_connection->authenticated = 0; 118 att_connection->authorized = 0; 119 } 120 121 int hci_can_send_command_packet_now(void){ 122 return 1; 123 } 124 int hci_can_send_packet_now_using_packet_buffer(uint8_t packet_type){ 125 return 1; 126 } 127 128 hci_connection_t * hci_connection_for_bd_addr_and_type(bd_addr_t addr, bd_addr_type_t addr_type){ 129 return &the_connection; 130 } 131 hci_connection_t * hci_connection_for_handle(hci_con_handle_t con_handle){ 132 return &the_connection; 133 } 134 void hci_connections_get_iterator(btstack_linked_list_iterator_t *it){ 135 btstack_linked_list_iterator_init(it, &connections); 136 } 137 138 // get addr type and address used in advertisement packets 139 void hci_le_advertisement_address(uint8_t * addr_type, bd_addr_t addr){ 140 *addr_type = 0; 141 uint8_t dummy[] = { 0x00, 0x1b, 0xdc, 0x07, 0x32, 0xef }; 142 memcpy(addr, dummy, 6); 143 } 144 145 void hci_le_advertisements_set_params(uint16_t adv_int_min, uint16_t adv_int_max, uint8_t adv_type, 146 uint8_t own_address_type, uint8_t direct_address_typ, bd_addr_t direct_address, 147 uint8_t channel_map, uint8_t filter_policy) { 148 } 149 150 int l2cap_can_send_connectionless_packet_now(void){ 151 return packet_buffer_len == 0; 152 } 153 154 int l2cap_can_send_fixed_channel_packet_now(uint16_t handle){ 155 return packet_buffer_len == 0; 156 } 157 158 int hci_send_cmd(const hci_cmd_t *cmd, ...){ 159 va_list argptr; 160 va_start(argptr, cmd); 161 uint16_t len = hci_create_cmd_internal(packet_buffer, cmd, argptr); 162 va_end(argptr); 163 hci_dump_packet(HCI_COMMAND_DATA_PACKET, 0, packet_buffer, len); 164 dump_packet(HCI_COMMAND_DATA_PACKET, packet_buffer, len); 165 packet_buffer_len = len; 166 167 // track le encrypt and le rand 168 if (cmd->opcode == hci_le_encrypt.opcode){ 169 uint8_t * key_flipped = &packet_buffer[3]; 170 uint8_t key[16]; 171 swap128(key_flipped, key); 172 // printf("le_encrypt key "); 173 // hexdump(key, 16); 174 uint8_t * plaintext_flipped = &packet_buffer[19]; 175 uint8_t plaintext[16]; 176 swap128(plaintext_flipped, plaintext); 177 // printf("le_encrypt txt "); 178 // hexdump(plaintext, 16); 179 aes128_calc_cyphertext(key, plaintext, aes128_cyphertext); 180 // printf("le_encrypt res "); 181 // hexdump(aes128_cyphertext, 16); 182 } 183 return 0; 184 } 185 186 void l2cap_register_fixed_channel(btstack_packet_handler_t packet_handler, uint16_t channel_id) { 187 le_data_handler = packet_handler; 188 } 189 190 void l2cap_register_packet_handler(void (*handler)(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)){ 191 printf("l2cap_register_packet_handler\n"); 192 event_packet_handler = handler; 193 } 194 195 int l2cap_reserve_packet_buffer(void){ 196 printf("l2cap_reserve_packet_buffer\n"); 197 return 1; 198 } 199 200 int l2cap_send_prepared_connectionless(uint16_t handle, uint16_t cid, uint16_t len){ 201 printf("l2cap_send_prepared_connectionless\n"); 202 return 0; 203 } 204 205 int l2cap_send_connectionless(uint16_t handle, uint16_t cid, uint8_t * buffer, uint16_t len){ 206 // printf("l2cap_send_connectionless\n"); 207 208 int pb = hci_non_flushable_packet_boundary_flag_supported() ? 0x00 : 0x02; 209 210 // 0 - Connection handle : PB=pb : BC=00 211 bt_store_16(packet_buffer, 0, handle | (pb << 12) | (0 << 14)); 212 // 2 - ACL length 213 bt_store_16(packet_buffer, 2, len + 4); 214 // 4 - L2CAP packet length 215 bt_store_16(packet_buffer, 4, len + 0); 216 // 6 - L2CAP channel DEST 217 bt_store_16(packet_buffer, 6, cid); 218 219 memcpy(&packet_buffer[8], buffer, len); 220 hci_dump_packet(HCI_ACL_DATA_PACKET, 0, &packet_buffer[0], len + 8); 221 222 dump_packet(HCI_ACL_DATA_PACKET, packet_buffer, len + 8); 223 packet_buffer_len = len + 8; 224 225 return 0; 226 } 227 228 void hci_disconnect_security_block(hci_con_handle_t con_handle){ 229 printf("hci_disconnect_security_block \n"); 230 } 231 232 int hci_non_flushable_packet_boundary_flag_supported(void){ 233 return 1; 234 } 235 236 void l2cap_run(void){ 237 } 238