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