From adfffcd8fb96c6c2eadb749b4ce921c624353191 Mon Sep 17 00:00:00 2001 From: jaseg Date: Wed, 24 Mar 2021 14:48:01 +0100 Subject: Add passthrough reports --- fw/src/demo.c | 108 +++++++++++++++++++++++++++++++++++++++++++--------------- 1 file changed, 80 insertions(+), 28 deletions(-) (limited to 'fw/src/demo.c') diff --git a/fw/src/demo.c b/fw/src/demo.c index 5ceecec..6f01dbd 100644 --- a/fw/src/demo.c +++ b/fw/src/demo.c @@ -77,13 +77,31 @@ static struct { } mgmt __attribute__((aligned(4))); } keystore __attribute__((section(".backup_sram"))) = {0}; +enum operation_mode_t { + OPMODE_DIRECT, + OPMODE_ENCRYPTED, + OPMODE_NUM, +} g_operation_mode = OPMODE_DIRECT; +bool send_plaintext_message(const uint8_t *msg, size_t len); void _fini(void); static inline void delay(uint32_t n) { for (volatile uint32_t i = 0; i < 1490*n; i++); } +void set_opmode(enum operation_mode_t new_opmode) { + g_operation_mode = new_opmode; + + if (g_operation_mode == OPMODE_DIRECT) { + gpio_clear(GPIOE, GPIO13); + gpio_set(GPIOE, GPIO14); + + } else { /* OPMODE_ENCRYPTED */ + gpio_set(GPIOE, GPIO13); + gpio_clear(GPIOE, GPIO14); + } +} /* Set STM32 to 168 MHz. */ static void clock_setup(void) { @@ -417,12 +435,19 @@ static void hid_in_message_handler(uint8_t device_id, const uint8_t *data, uint3 }; memcpy(pkt.report.report, data, length); - if (send_encrypted_message(&noise_state, (uint8_t *)&pkt, sizeof(pkt))) { - LOG_PRINTF("Error sending HID report packet\n"); - TRACING_CLEAR(TR_HID_MESSAGE_HANDLER); - return; + if (g_operation_mode == OPMODE_DIRECT) { + if (!send_plaintext_message((uint8_t *)&pkt, sizeof(pkt))) { + LOG_PRINTF("Error sending plaintext HID report packet\n"); + } + + } else { /* OPMODE_ENCRYPTED */ + if (send_encrypted_message(&noise_state, (uint8_t *)&pkt, sizeof(pkt))) { + LOG_PRINTF("Error sending encrypted HID report packet\n"); + } } + TRACING_CLEAR(TR_HID_MESSAGE_HANDLER); + return; } volatile struct { @@ -444,7 +469,7 @@ struct dma_usart_file *debug_out = &debug_out_s; /* FIXME start unsafe debug code */ bool debug_emulate_pairing_input(char c); bool debug_fill_report_from_ascii(char c, struct hid_report *out); -bool debug_send_encrypted_emulated_report(char c); +bool debug_send_emulated_report(char c); bool debug_enqueue_emulated_input(char c); /* Feed keyboard input from debug UART into keyboard subsystem */ @@ -550,7 +575,7 @@ bool debug_emulate_pairing_input(char c) { } /* Returns true on success */ -bool debug_send_encrypted_emulated_report(char c) { +bool debug_send_emulated_report(char c) { struct hid_report_packet pkt = { .type = REPORT_KEYBOARD, .report = { @@ -563,17 +588,33 @@ bool debug_send_encrypted_emulated_report(char c) { if (!debug_fill_report_from_ascii(c, &pkt.report.report)) return false; - if (send_encrypted_message(&noise_state, (uint8_t *)&pkt, sizeof(pkt))) { - LOG_PRINTF("Error sending emulated HID report packet\n"); - return false; - } + if (g_operation_mode == OPMODE_ENCRYPTED) { + if (send_encrypted_message(&noise_state, (uint8_t *)&pkt, sizeof(pkt))) { + LOG_PRINTF("Error sending emulated encrypted HID report packet\n"); + return false; + } - /* key release */ - memset(&pkt.report.report, 0, sizeof(struct hid_report)); + /* key release */ + memset(&pkt.report.report, 0, sizeof(struct hid_report)); - if (send_encrypted_message(&noise_state, (uint8_t *)&pkt, sizeof(pkt))) { - LOG_PRINTF("Error sending emulated HID report packet\n"); - return false; + if (send_encrypted_message(&noise_state, (uint8_t *)&pkt, sizeof(pkt))) { + LOG_PRINTF("Error sending emulated encrypted HID report packet\n"); + return false; + } + + } else { /* OPMODE_DIRECT */ + if (!send_plaintext_message((uint8_t *)&pkt, sizeof(pkt))) { + LOG_PRINTF("Error sending emulated plaintext HID report packet\n"); + return false; + } + + /* key release */ + memset(&pkt.report.report, 0, sizeof(struct hid_report)); + + if (!send_plaintext_message((uint8_t *)&pkt, sizeof(pkt))) { + LOG_PRINTF("Error sending emulated plaintext HID report packet\n"); + return false; + } } return true; @@ -618,6 +659,7 @@ void handle_host_packet(struct NoiseState *st, const struct control_packet *pkt, LOG_PRINTF("Extraneous data in INITIATE_HANDSHAKE message\n"); } else if (st->failed_handshakes < MAX_FAILED_HANDSHAKES) { LOG_PRINTF("Starting noise protocol handshake...\n"); + set_opmode(OPMODE_ENCRYPTED); if (reset_protocol_handshake(st)) LOG_PRINTF("Error starting protocol handshake.\n"); pairing_buf_pos = 0; /* Reset channel binding keyboard input buffer */ @@ -718,8 +760,7 @@ int main(void) int debounce_ctr_test = 0; int mode_debug = 0; #define DEBOUNCE_IVL 1000 - gpio_set(GPIOE, GPIO13); - gpio_clear(GPIOE, GPIO14); + set_opmode(OPMODE_DIRECT); int dbg_keyem_ctr = 0; #define DBG_KEYEM_IVL 100 @@ -731,7 +772,7 @@ int main(void) int rc = debug_dequeue_emulated_input(); if (rc >= 0) { dbg_keyem_ctr = DBG_KEYEM_IVL; - if (!debug_send_encrypted_emulated_report((char) rc)) { + if (!debug_send_emulated_report((char) rc)) { LOG_PRINTF("Error sending emulated keyboard input\n"); } else { LOG_PRINTF("Sent emulated keypress %02x\n", (unsigned char)rc); @@ -769,16 +810,7 @@ int main(void) if (last_btn_st_mode) { debounce_ctr_mode = DEBOUNCE_IVL; - mode_debug = !mode_debug; - - if (mode_debug) { - gpio_clear(GPIOE, GPIO13); - gpio_set(GPIOE, GPIO14); - - } else { - gpio_set(GPIOE, GPIO13); - gpio_clear(GPIOE, GPIO14); - } + set_opmode((g_operation_mode + 1) % OPMODE_NUM); } last_btn_st_mode = 0; @@ -865,3 +897,23 @@ void _fini() { while (1); } +/* Returns true on success */ +bool send_plaintext_message(const uint8_t *msg, size_t len) { + int err; + struct { + struct control_packet header; + uint8_t payload[MAX_HOST_PACKET_SIZE]; + } pkt; + + if (len > sizeof(pkt.payload)) { + LOG_PRINTF("Packet too long\n"); + return false; + } + + pkt.header.type = HOST_PASSTHROUGH_REPORT; + memcpy(pkt.payload, msg, len); + + send_packet(uart4_out, (uint8_t *)&pkt, len + sizeof(pkt.header)); + return true; +} + -- cgit