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 event_packet_handler(NULL, HCI_EVENT_PACKET, NULL, packet, size); 55 } 56 57 void aes128_report_result(){ 58 uint8_t le_enc_result[22]; 59 uint8_t enc1_data[] = { 0x0e, 0x14, 0x01, 0x17, 0x20, 0x00 }; 60 memcpy (le_enc_result, enc1_data, 6); 61 swap128(aes128_cyphertext, &le_enc_result[6]); 62 mock_simulate_hci_event(&le_enc_result[0], sizeof(le_enc_result)); 63 } 64 65 void mock_simulate_sm_data_packet(uint8_t * packet, uint16_t len){ 66 67 uint16_t handle = 0x40; 68 uint16_t cid = 0x06; 69 70 uint8_t acl_buffer[len + 8]; 71 72 // 0 - Connection handle : PB=10 : BC=00 73 bt_store_16(acl_buffer, 0, handle | (2 << 12) | (0 << 14)); 74 // 2 - ACL length 75 bt_store_16(acl_buffer, 2, len + 4); 76 // 4 - L2CAP packet length 77 bt_store_16(acl_buffer, 4, len + 0); 78 // 6 - L2CAP channel DEST 79 bt_store_16(acl_buffer, 6, cid); 80 81 memcpy(&acl_buffer[8], packet, len); 82 hci_dump_packet(HCI_ACL_DATA_PACKET, 1, &acl_buffer[0], len + 8); 83 84 le_data_handler(SM_DATA_PACKET, handle, packet, len); 85 } 86 87 void mock_simulate_command_complete(const hci_cmd_t *cmd){ 88 uint8_t packet[] = {HCI_EVENT_COMMAND_COMPLETE, 4, 1, cmd->opcode & 0xff, cmd->opcode >> 8, 0}; 89 mock_simulate_hci_event((uint8_t *)&packet, sizeof(packet)); 90 } 91 92 void mock_simulate_hci_state_working(){ 93 uint8_t packet[] = {BTSTACK_EVENT_STATE, 0, HCI_STATE_WORKING}; 94 mock_simulate_hci_event((uint8_t *)&packet, sizeof(packet)); 95 } 96 97 void mock_simulate_connected(){ 98 uint8_t packet[] = { 0x3e, 0x13, 0x01, 0x00, 0x40, 0x00, 0x01, 0x01, 0x18, 0x12, 0x5e, 0x68, 0xc9, 0x73, 0x18, 0x00, 0x00, 0x00, 0x48, 0x00, 0x05}; 99 mock_simulate_hci_event((uint8_t *)&packet, sizeof(packet)); 100 } 101 102 void att_init_connection(att_connection_t * att_connection){ 103 att_connection->mtu = 23; 104 att_connection->encryption_key_size = 0; 105 att_connection->authenticated = 0; 106 att_connection->authorized = 0; 107 } 108 109 int hci_can_send_packet_now_using_packet_buffer(uint8_t packet_type){ 110 return 1; 111 } 112 113 // get addr type and address used in advertisement packets 114 void hci_le_advertisement_address(uint8_t * addr_type, bd_addr_t * addr){ 115 *addr_type = 0; 116 uint8_t dummy[] = { 0x00, 0x1b, 0xdc, 0x07, 0x32, 0xef }; 117 memcpy(addr, dummy, 6); 118 } 119 120 int l2cap_can_send_connectionless_packet_now(void){ 121 return packet_buffer_len == 0; 122 } 123 124 int hci_send_cmd(const hci_cmd_t *cmd, ...){ 125 va_list argptr; 126 va_start(argptr, cmd); 127 uint16_t len = hci_create_cmd_internal(packet_buffer, cmd, argptr); 128 va_end(argptr); 129 hci_dump_packet(HCI_COMMAND_DATA_PACKET, 0, packet_buffer, len); 130 dump_packet(HCI_COMMAND_DATA_PACKET, packet_buffer, len); 131 packet_buffer_len = len; 132 133 // track le encrypt and le rand 134 if (cmd->opcode == hci_le_encrypt.opcode){ 135 uint8_t * key_flipped = &packet_buffer[3]; 136 uint8_t key[16]; 137 swap128(key_flipped, key); 138 // printf("le_encrypt key "); 139 // hexdump(key, 16); 140 uint8_t * plaintext_flipped = &packet_buffer[19]; 141 uint8_t plaintext[16]; 142 swap128(plaintext_flipped, plaintext); 143 // printf("le_encrypt txt "); 144 // hexdump(plaintext, 16); 145 aes128_calc_cyphertext(key, plaintext, aes128_cyphertext); 146 // printf("le_encrypt res "); 147 // hexdump(aes128_cyphertext, 16); 148 } 149 return 0; 150 } 151 152 void l2cap_register_fixed_channel(btstack_packet_handler_t packet_handler, uint16_t channel_id) { 153 le_data_handler = packet_handler; 154 } 155 156 void l2cap_register_packet_handler(void (*handler)(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)){ 157 event_packet_handler = handler; 158 } 159 160 int l2cap_reserve_packet_buffer(void){ 161 printf("l2cap_reserve_packet_buffer\n"); 162 return 1; 163 } 164 165 int l2cap_send_prepared_connectionless(uint16_t handle, uint16_t cid, uint16_t len){ 166 printf("l2cap_send_prepared_connectionless\n"); 167 return 0; 168 } 169 170 int l2cap_send_connectionless(uint16_t handle, uint16_t cid, uint8_t * buffer, uint16_t len){ 171 // printf("l2cap_send_connectionless\n"); 172 173 // 0 - Connection handle : PB=10 : BC=00 174 bt_store_16(packet_buffer, 0, handle | (2 << 12) | (0 << 14)); 175 // 2 - ACL length 176 bt_store_16(packet_buffer, 2, len + 4); 177 // 4 - L2CAP packet length 178 bt_store_16(packet_buffer, 4, len + 0); 179 // 6 - L2CAP channel DEST 180 bt_store_16(packet_buffer, 6, cid); 181 182 memcpy(&packet_buffer[8], buffer, len); 183 hci_dump_packet(HCI_ACL_DATA_PACKET, 0, &packet_buffer[0], len + 8); 184 185 dump_packet(HCI_ACL_DATA_PACKET, packet_buffer, len + 8); 186 packet_buffer_len = len + 8; 187 188 return 0; 189 } 190 191 void hci_disconnect_security_block(hci_con_handle_t con_handle){ 192 printf("hci_disconnect_security_block \n"); 193 } 194 195 void l2cap_run(void){ 196 } 197