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