1 2 #include "mock.h" 3 4 #include <stdint.h> 5 #include <string.h> 6 7 #include "ble/att_db.h" 8 #include "hci.h" 9 #include "hci_dump.h" 10 #include "l2cap.h" 11 #include "rijndael.h" 12 13 14 static btstack_linked_list_t event_packet_handlers; 15 16 static uint8_t packet_buffer[256]; 17 static uint16_t packet_buffer_len = 0; 18 19 static uint8_t aes128_cyphertext[16]; 20 21 static int report_aes128; 22 static int report_random; 23 24 static uint32_t lfsr_random; 25 // #define ENABLE_PACKET_LOGGER 26 // #define ENABLE_AES128_LOGGER 27 28 /* taps: 32 31 29 1; characteristic polynomial: x^32 + x^31 + x^29 + x + 1 */ 29 #define LFSR(a) ((a >> 1) ^ (uint32_t)((0 - (a & 1u)) & 0xd0000001u)) 30 31 void mock_init(void){ 32 lfsr_random = 0x12345678; 33 } 34 35 uint8_t * mock_packet_buffer(void){ 36 return packet_buffer; 37 } 38 39 void mock_clear_packet_buffer(void){ 40 packet_buffer_len = 0; 41 memset(packet_buffer, 0, sizeof(packet_buffer)); 42 } 43 44 static void dump_packet(int packet_type, uint8_t * buffer, uint16_t size){ 45 #ifdef ENABLE_PACKET_LOGGER 46 static int packet_counter = 1; 47 char var_name[80]; 48 sprintf(var_name, "test_%s_packet_%02u", packet_type == HCI_COMMAND_DATA_PACKET ? "command" : "acl", packet_counter); 49 printf("uint8_t %s[] = { ", var_name); 50 for (int i = 0; i < size ; i++){ 51 if ((i % 16) == 0) printf("\n "); 52 printf ("0x%02x, ", buffer[i]); 53 } 54 printf("};\n"); 55 packet_counter++; 56 #else 57 UNUSED(packet_type); 58 UNUSED(buffer); 59 UNUSED(size); 60 #endif 61 } 62 63 static void aes128_calc_cyphertext(uint8_t key[16], uint8_t plaintext[16], uint8_t cyphertext[16]){ 64 uint32_t rk[RKLENGTH(KEYBITS)]; 65 int nrounds = rijndaelSetupEncrypt(rk, &key[0], KEYBITS); 66 rijndaelEncrypt(rk, nrounds, plaintext, cyphertext); 67 } 68 69 void mock_simulate_hci_event(uint8_t * packet, uint16_t size){ 70 dump_packet(HCI_EVENT_PACKET, packet, size); 71 hci_dump_packet(HCI_EVENT_PACKET, 1, packet, size); 72 // dispatch to all event handlers 73 btstack_linked_list_iterator_t it; 74 btstack_linked_list_iterator_init(&it, &event_packet_handlers); 75 while (btstack_linked_list_iterator_has_next(&it)){ 76 btstack_packet_callback_registration_t * entry = (btstack_packet_callback_registration_t*) btstack_linked_list_iterator_next(&it); 77 entry->callback(HCI_EVENT_PACKET, 0, packet, size); 78 } 79 } 80 81 static void aes128_report_result(void){ 82 uint8_t le_enc_result[22]; 83 uint8_t enc1_data[] = { 0x0e, 0x14, 0x01, 0x17, 0x20, 0x00 }; 84 memcpy (le_enc_result, enc1_data, 6); 85 reverse_128(aes128_cyphertext, &le_enc_result[6]); 86 mock_simulate_hci_event(le_enc_result, sizeof(le_enc_result)); 87 } 88 89 static void random_report_result(void){ 90 uint8_t le_rand_result[14]; 91 uint8_t rand_complete[] = { 0x0e, 0x0c, 0x01, 0x18, 0x20, 0x00 }; 92 memcpy (le_rand_result, rand_complete, 6); 93 int i; 94 for (i=0;i<8;i++){ 95 lfsr_random = LFSR(lfsr_random); 96 le_rand_result[6+i]= lfsr_random; 97 } 98 mock_simulate_hci_event(le_rand_result, sizeof(le_rand_result)); 99 } 100 101 int mock_process_hci_cmd(void){ 102 if (report_aes128){ 103 report_aes128 = 0; 104 aes128_report_result(); 105 return 1; 106 } 107 if (report_random){ 108 report_random = 0; 109 random_report_result(); 110 return 1; 111 } 112 return 0; 113 } 114 115 void mock_simulate_hci_state_working(void){ 116 uint8_t packet[] = {BTSTACK_EVENT_STATE, 0, HCI_STATE_WORKING}; 117 mock_simulate_hci_event((uint8_t *)&packet, sizeof(packet)); 118 } 119 120 bool hci_can_send_command_packet_now(void){ 121 return true; 122 } 123 124 uint8_t hci_send_cmd(const hci_cmd_t *cmd, ...){ 125 va_list argptr; 126 va_start(argptr, cmd); 127 uint16_t len = hci_cmd_create_from_template(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 reverse_128(key_flipped, key); 138 uint8_t * plaintext_flipped = &packet_buffer[19]; 139 uint8_t plaintext[16]; 140 reverse_128(plaintext_flipped, plaintext); 141 aes128_calc_cyphertext(key, plaintext, aes128_cyphertext); 142 report_aes128 = 1; 143 #ifdef ENABLE_AES128_LOGGER 144 printf("AES128 Operation\n"); 145 printf("Key: "); printf_hexdump(key, 16); 146 printf("Plain: "); printf_hexdump(plaintext, 16); 147 printf("Cipher: "); printf_hexdump(aes128_cyphertext, 16); 148 #endif 149 } 150 if (cmd->opcode == hci_le_rand.opcode){ 151 report_random = 1; 152 } 153 return 0; 154 } 155 156 void hci_add_event_handler(btstack_packet_callback_registration_t * callback_handler){ 157 btstack_linked_list_add_tail(&event_packet_handlers, (btstack_linked_item_t*) callback_handler); 158 } 159 160 HCI_STATE hci_get_state(void){ 161 return HCI_STATE_WORKING; 162 } 163 164 void btstack_run_loop_add_timer(btstack_timer_source_t * ts){ 165 UNUSED(ts); 166 } 167 int btstack_run_loop_remove_timer(btstack_timer_source_t * ts){ 168 UNUSED(ts); 169 return 0; 170 } 171 void btstack_run_loop_set_timer(btstack_timer_source_t * ts, uint32_t timeout){ 172 UNUSED(ts); 173 UNUSED(timeout); 174 } 175 void btstack_run_loop_set_timer_handler(btstack_timer_source_t * ts, void (*fn)(btstack_timer_source_t * ts)){ 176 UNUSED(ts); 177 UNUSED(fn); 178 } 179 static void * timer_context; 180 void btstack_run_loop_set_timer_context(btstack_timer_source_t * ts, void * context){ 181 UNUSED(ts); 182 timer_context = context; 183 } 184 void * btstack_run_loop_get_timer_context(btstack_timer_source_t * ts){ 185 UNUSED(ts); 186 return timer_context; 187 } 188 void hci_halting_defer(void){ 189 } 190 191