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(){ 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(){ 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(){ 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_packet_now_using_packet_buffer(uint8_t packet_type){ 115 return 1; 116 } 117 118 // get addr type and address used in advertisement packets 119 void hci_le_advertisement_address(uint8_t * addr_type, bd_addr_t addr){ 120 *addr_type = 0; 121 uint8_t dummy[] = { 0x00, 0x1b, 0xdc, 0x07, 0x32, 0xef }; 122 memcpy(addr, dummy, 6); 123 } 124 125 int l2cap_can_send_connectionless_packet_now(void){ 126 return packet_buffer_len == 0; 127 } 128 129 int hci_send_cmd(const hci_cmd_t *cmd, ...){ 130 va_list argptr; 131 va_start(argptr, cmd); 132 uint16_t len = hci_create_cmd_internal(packet_buffer, cmd, argptr); 133 va_end(argptr); 134 hci_dump_packet(HCI_COMMAND_DATA_PACKET, 0, packet_buffer, len); 135 dump_packet(HCI_COMMAND_DATA_PACKET, packet_buffer, len); 136 packet_buffer_len = len; 137 138 // track le encrypt and le rand 139 if (cmd->opcode == hci_le_encrypt.opcode){ 140 uint8_t * key_flipped = &packet_buffer[3]; 141 uint8_t key[16]; 142 swap128(key_flipped, key); 143 // printf("le_encrypt key "); 144 // hexdump(key, 16); 145 uint8_t * plaintext_flipped = &packet_buffer[19]; 146 uint8_t plaintext[16]; 147 swap128(plaintext_flipped, plaintext); 148 // printf("le_encrypt txt "); 149 // hexdump(plaintext, 16); 150 aes128_calc_cyphertext(key, plaintext, aes128_cyphertext); 151 // printf("le_encrypt res "); 152 // hexdump(aes128_cyphertext, 16); 153 } 154 return 0; 155 } 156 157 void l2cap_register_fixed_channel(btstack_packet_handler_t packet_handler, uint16_t channel_id) { 158 le_data_handler = packet_handler; 159 } 160 161 void l2cap_register_packet_handler(void (*handler)(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)){ 162 printf("l2cap_register_packet_handler\n"); 163 event_packet_handler = handler; 164 } 165 166 int l2cap_reserve_packet_buffer(void){ 167 printf("l2cap_reserve_packet_buffer\n"); 168 return 1; 169 } 170 171 int l2cap_send_prepared_connectionless(uint16_t handle, uint16_t cid, uint16_t len){ 172 printf("l2cap_send_prepared_connectionless\n"); 173 return 0; 174 } 175 176 int l2cap_send_connectionless(uint16_t handle, uint16_t cid, uint8_t * buffer, uint16_t len){ 177 // printf("l2cap_send_connectionless\n"); 178 179 int pb = hci_non_flushable_packet_boundary_flag_supported() ? 0x00 : 0x02; 180 181 // 0 - Connection handle : PB=pb : BC=00 182 bt_store_16(packet_buffer, 0, handle | (pb << 12) | (0 << 14)); 183 // 2 - ACL length 184 bt_store_16(packet_buffer, 2, len + 4); 185 // 4 - L2CAP packet length 186 bt_store_16(packet_buffer, 4, len + 0); 187 // 6 - L2CAP channel DEST 188 bt_store_16(packet_buffer, 6, cid); 189 190 memcpy(&packet_buffer[8], buffer, len); 191 hci_dump_packet(HCI_ACL_DATA_PACKET, 0, &packet_buffer[0], len + 8); 192 193 dump_packet(HCI_ACL_DATA_PACKET, packet_buffer, len + 8); 194 packet_buffer_len = len + 8; 195 196 return 0; 197 } 198 199 void hci_disconnect_security_block(hci_con_handle_t con_handle){ 200 printf("hci_disconnect_security_block \n"); 201 } 202 203 int hci_non_flushable_packet_boundary_flag_supported(){ 204 return 1; 205 } 206 207 void l2cap_run(void){ 208 } 209