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