From 16b4fa6ca3be3ab264deeab260c72aaf51225675 Mon Sep 17 00:00:00 2001 From: Pol Henarejos Date: Thu, 23 Feb 2023 07:57:35 +0100 Subject: [PATCH] Emulation is now a higher define which also accepts interfaces. Signed-off-by: Pol Henarejos --- pico_hsm_sdk_import.cmake | 22 ++--- src/apdu.c | 14 ++- src/usb/emulation/emulation.c | 168 +++++++++++++++++++++++++--------- src/usb/usb.c | 46 +++++----- src/usb/usb.h | 24 ++--- src/usb/usb_descriptors.c | 3 +- 6 files changed, 176 insertions(+), 101 deletions(-) diff --git a/pico_hsm_sdk_import.cmake b/pico_hsm_sdk_import.cmake index e4b515f..677b172 100644 --- a/pico_hsm_sdk_import.cmake +++ b/pico_hsm_sdk_import.cmake @@ -29,9 +29,6 @@ endif() if (NOT DEFINED ENABLE_EMULATION) set(ENABLE_EMULATION 0) endif() -if (NOT DEFINED ENABLE_EMULATION) - set(ENABLE_EMULATION 0) -endif() option(ENABLE_DELAYED_BOOT "Enable/disable delayed boot" OFF) if(ENABLE_DELAYED_BOOT) @@ -40,17 +37,14 @@ if(ENABLE_DELAYED_BOOT) else() message(STATUS "Delayed boot:\t\t disabled") endif(ENABLE_DELAYED_BOOT) -if(ENABLE_EMULATION) -else() - if(USB_ITF_HID) - add_definitions(-DUSB_ITF_HID=1) - message(STATUS "USB HID Interface:\t\t enabled") - endif(USB_ITF_HID) - if(USB_ITF_CCID) - add_definitions(-DUSB_ITF_CCID=1) - message(STATUS "USB CCID Interface:\t\t enabled") - endif(USB_ITF_CCID) -endif() +if(USB_ITF_HID) + add_definitions(-DUSB_ITF_HID=1) + message(STATUS "USB HID Interface:\t\t enabled") +endif(USB_ITF_HID) +if(USB_ITF_CCID) + add_definitions(-DUSB_ITF_CCID=1) + message(STATUS "USB CCID Interface:\t\t enabled") +endif(USB_ITF_CCID) add_definitions(-DDEBUG_APDU=${DEBUG_APDU}) add_definitions(-DMBEDTLS_CONFIG_FILE="${CMAKE_CURRENT_LIST_DIR}/config/mbedtls_config.h") diff --git a/src/apdu.c b/src/apdu.c index 0c4ec5e..e42adbf 100644 --- a/src/apdu.c +++ b/src/apdu.c @@ -95,6 +95,7 @@ size_t apdu_process(uint8_t itf, const uint8_t *buffer, size_t buffer_size) { timeout_stop(); *(uint16_t *) rdata_gr = rdata_bk; if (apdu.rlen <= apdu.ne) { +#ifndef ENABLE_EMULATION #ifdef USB_ITF_HID if (itf == ITF_HID) { driver_exec_finished_cont_hid(apdu.rlen + 2, rdata_gr - usb_get_tx(itf)); @@ -105,10 +106,8 @@ size_t apdu_process(uint8_t itf, const uint8_t *buffer, size_t buffer_size) { driver_exec_finished_cont_ccid(apdu.rlen + 2, rdata_gr - usb_get_tx(itf)); } #endif -#ifdef ENABLE_EMULATION - if (itf == ITF_EMUL) { - driver_exec_finished_cont_emul(apdu.rlen + 2, rdata_gr - usb_get_tx(itf)); - } +#else + driver_exec_finished_cont_emul(itf, apdu.rlen + 2, rdata_gr - usb_get_tx(itf)); #endif //Prepare next RAPDU apdu.sw = 0; @@ -125,6 +124,7 @@ size_t apdu_process(uint8_t itf, const uint8_t *buffer, size_t buffer_size) { else { rdata_gr[1] = apdu.rlen - apdu.ne; } +#ifndef ENABLE_EMULATION #ifdef USB_ITF_HID if (itf == ITF_HID) { driver_exec_finished_cont_hid(apdu.ne + 2, rdata_gr - apdu.ne - usb_get_tx(itf)); @@ -135,10 +135,8 @@ size_t apdu_process(uint8_t itf, const uint8_t *buffer, size_t buffer_size) { driver_exec_finished_cont_ccid(apdu.ne + 2, rdata_gr - apdu.ne - usb_get_tx(itf)); } #endif -#ifdef ENABLE_EMULATION - if (itf == ITF_EMUL) { - driver_exec_finished_cont_emul(apdu.ne + 2, rdata_gr - apdu.ne - usb_get_tx(itf)); - } +#else + driver_exec_finished_cont_emul(itf, apdu.ne + 2, rdata_gr - apdu.ne - usb_get_tx(itf)); #endif apdu.rlen -= apdu.ne; } diff --git a/src/usb/emulation/emulation.c b/src/usb/emulation/emulation.c index 2f7f431..1baadee 100644 --- a/src/usb/emulation/emulation.c +++ b/src/usb/emulation/emulation.c @@ -25,13 +25,16 @@ #include #include #include +#include #include "hsm.h" #include "apdu.h" #include "usb.h" #include "ccid/ccid.h" -int sock = 0; +int ccid_sock = 0; +int hid_server_sock = 0; +int hid_client_sock = -1; int msleep(long msec) { struct timespec ts; @@ -55,8 +58,8 @@ int msleep(long msec) { int emul_init(char *host, uint16_t port) { struct sockaddr_in serv_addr; fprintf(stderr, "\n Starting emulation envionrment\n"); - if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) { - fprintf(stderr, "\n Socket creation error \n"); + if ((ccid_sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) { + perror("socket"); return -1; } @@ -66,27 +69,81 @@ int emul_init(char *host, uint16_t port) { // Convert IPv4 and IPv6 addresses from text to binary // form if (inet_pton(AF_INET, host, &serv_addr.sin_addr) <= 0) { - fprintf(stderr, "\nInvalid address/ Address not supported \n"); + perror("inet_pton"); + close(ccid_sock); return -1; } - if (connect(sock, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) { - fprintf(stderr, "\nConnection Failed \n"); + if (connect(ccid_sock, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) { + perror("connect"); + close(ccid_sock); return -1; } - int x = fcntl(sock, F_GETFL, 0); - fcntl(sock, F_SETFL, x | O_NONBLOCK); + int x = fcntl(ccid_sock, F_GETFL, 0); + fcntl(ccid_sock, F_SETFL, x | O_NONBLOCK); + + // HID server + + socklen_t yes = 1; + uint16_t hid_port = port - 1; + struct sockaddr_in server_sockaddr; + + if ((hid_server_sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) { + perror("socket"); + return -1; + } + + if (setsockopt(hid_server_sock, SOL_SOCKET, SO_REUSEADDR, (void *) &yes, sizeof yes) != 0) { + perror("setsockopt"); + close(hid_server_sock); + return 1; + } + +#if HAVE_DECL_SO_NOSIGPIPE + if (setsockopt(hid_server_sock, SOL_SOCKET, SO_NOSIGPIPE, (void *) &yes, sizeof yes) != 0) { + perror("setsockopt"); + close(hid_server_sock); + return 1; + } +#endif + + memset(&server_sockaddr, 0, sizeof server_sockaddr); + server_sockaddr.sin_family = PF_INET; + server_sockaddr.sin_port = htons(hid_port); + server_sockaddr.sin_addr.s_addr = htonl(INADDR_ANY); + + if (bind(hid_server_sock, (struct sockaddr *) &server_sockaddr, + sizeof server_sockaddr) != 0) { + perror("bind"); + close(hid_server_sock); + return 1; + } + + if (listen(hid_server_sock, 0) != 0) { + perror("listen"); + close(hid_server_sock); + return 1; + } return 0; } -uint8_t *driver_prepare_response_emul() { - apdu.rdata = usb_get_tx(ITF_EMUL); +uint8_t *driver_prepare_response_emul(uint8_t itf) { + apdu.rdata = usb_get_tx(itf); return apdu.rdata; } -int driver_write_emul(const uint8_t *buffer, size_t buffer_size) { +int get_sock_itf(uint8_t itf) { + if (itf == ITF_CCID) + return ccid_sock; + else if (itf == ITF_HID) + return hid_client_sock; + return -1; +} + +int driver_write_emul(uint8_t itf, const uint8_t *buffer, size_t buffer_size) { uint16_t size = htons(buffer_size); - //DEBUG_PAYLOAD(buffer,buffer_size); + int sock = get_sock_itf(itf); + // DEBUG_PAYLOAD(buffer,buffer_size); int ret = 0; do { ret = send(sock, &size, sizeof(size), 0); @@ -103,71 +160,98 @@ int driver_write_emul(const uint8_t *buffer, size_t buffer_size) { return buffer_size; } -uint32_t emul_write_offset(uint16_t size, uint16_t offset) { +uint32_t emul_write_offset(uint8_t itf, uint16_t size, uint16_t offset) { if (size > 0) { - //DEBUG_PAYLOAD(usb_get_tx(ITF_EMUL)+offset, size); - return usb_write_offset(ITF_EMUL, size, offset); + //DEBUG_PAYLOAD(usb_get_tx(itf)+offset, size); + return usb_write_offset(itf, size, offset); } return 0; } -uint32_t emul_write(uint16_t size) { - return emul_write_offset(size, 0); +uint32_t emul_write(uint8_t itf, uint16_t size) { + return emul_write_offset(itf, size, 0); } -void driver_exec_finished_cont_emul(size_t size_next, size_t offset) { - emul_write_offset(size_next, offset); +void driver_exec_finished_cont_emul(uint8_t itf, size_t size_next, size_t offset) { + emul_write_offset(itf, size_next, offset); } -int driver_process_usb_packet_emul(uint16_t len) { +int driver_process_usb_packet_emul(uint8_t itf, uint16_t len) { if (len > 0) { - uint8_t *data = usb_get_rx(ITF_EMUL), *rdata = usb_get_tx(ITF_EMUL); - if (len == 1) { - uint8_t c = data[0]; - if (c == 4) { - if (ccid_atr) { - memcpy(rdata, ccid_atr + 1, ccid_atr[0]); + uint8_t *data = usb_get_rx(itf), *rdata = usb_get_tx(itf); + if (itf == ITF_CCID) { + if (len == 1) { + uint8_t c = data[0]; + if (c == 4) { + if (ccid_atr) { + memcpy(rdata, ccid_atr + 1, ccid_atr[0]); + } + emul_write(itf, ccid_atr ? ccid_atr[0] : 0); } - emul_write(ccid_atr ? ccid_atr[0] : 0); + } + else { + DEBUG_PAYLOAD(data, len); + if (apdu_process(itf, data, len) > 0) { + process_apdu(); + } + apdu_finish(); + size_t ret = apdu_next(); + DEBUG_PAYLOAD(rdata, ret); + emul_write(itf, ret); } } - else { - DEBUG_PAYLOAD(data, len); - if (apdu_process(ITF_EMUL, data, len) > 0) { - process_apdu(); - } - apdu_finish(); - size_t ret = apdu_next(); - DEBUG_PAYLOAD(rdata, ret); - emul_write(ret); + else if (itf == ITF_HID) { + } } - usb_clear_rx(ITF_EMUL); + usb_clear_rx(itf); return 0; } -uint16_t emul_read() { +uint16_t emul_read(uint8_t itf) { + /* First we look for a client */ + if (itf == ITF_HID && hid_client_sock == -1) { + struct sockaddr_in client_sockaddr; + socklen_t client_socklen = sizeof client_sockaddr; + + int timeout; + struct pollfd pfd; + + pfd.fd = hid_server_sock; + pfd.events = POLLIN; + pfd.revents = 0; + + timeout = (0 * 1000 + 1000 / 1000); + + if (poll(&pfd, 1, timeout) == -1) + return 0; + + if(pfd.revents & POLLIN) + hid_client_sock = accept(hid_server_sock, (struct sockaddr *) &client_sockaddr, + &client_socklen); + } + int sock = get_sock_itf(itf); uint16_t len = 0; fd_set input; FD_ZERO(&input); FD_SET(sock, &input); struct timeval timeout; - timeout.tv_sec = 2; + timeout.tv_sec = 0; timeout.tv_usec = 0 * 1000; int n = select(sock + 1, &input, NULL, NULL, &timeout); if (n == -1) { - printf("read wrong\n"); + //printf("read wrong [itf:%d]\n", itf); //something wrong } else if (n == 0) { - printf("read timeout\n"); + //printf("read timeout [itf:%d]\n", itf); } if (FD_ISSET(sock, &input)) { int valread = recv(sock, &len, sizeof(len), 0); len = ntohs(len); if (len > 0) { while (true) { - valread = recv(sock, usb_get_rx(ITF_EMUL), len, 0); + valread = recv(sock, usb_get_rx(itf), len, 0); if (valread > 0) { return valread; } diff --git a/src/usb/usb.c b/src/usb/usb.c index 7879414..7e4d6f1 100644 --- a/src/usb/usb.c +++ b/src/usb/usb.c @@ -53,6 +53,7 @@ uint32_t usb_write_offset(uint8_t itf, uint16_t len, uint16_t offset) { } w_len[itf] = len; tx_r_offset[itf] = offset; +#ifndef ENABLE_EMULATION #ifdef USB_ITF_HID if (itf == ITF_HID) { w = driver_write_hid(tx_buffer[itf] + offset, MIN(len, pkt_max)); @@ -63,10 +64,8 @@ uint32_t usb_write_offset(uint8_t itf, uint16_t len, uint16_t offset) { w = driver_write_ccid(tx_buffer[itf] + offset, MIN(len, pkt_max)); } #endif -#ifdef ENABLE_EMULATION - if (itf == ITF_EMUL) { - w = driver_write_emul(tx_buffer[itf] + offset, len); - } +#else + w = driver_write_emul(itf, tx_buffer[itf] + offset, len); #endif w_len[itf] -= w; tx_r_offset[itf] += w; @@ -99,6 +98,7 @@ size_t usb_rx(uint8_t itf, const uint8_t *buffer, size_t len) { uint32_t usb_write_flush(uint8_t itf) { int w = 0; if (w_len[itf] > 0) { +#ifndef ENABLE_EMULATION #ifdef USB_ITF_HID if (itf == ITF_HID) { w = driver_write_hid(tx_buffer[itf] + tx_r_offset[itf], MIN(w_len[itf], 64)); @@ -109,10 +109,8 @@ uint32_t usb_write_flush(uint8_t itf) { w = driver_write_ccid(tx_buffer[itf] + tx_r_offset[itf], MIN(w_len[itf], 64)); } #endif -#ifdef ENABLE_EMULATION - if (itf == ITF_EMUL) { - w = driver_write_emul(tx_buffer[itf] + tx_r_offset[itf], w_len[itf]); - } +#else + w = driver_write_emul(itf, tx_buffer[itf] + tx_r_offset[itf], w_len[itf]); #endif tx_r_offset[itf] += w; w_len[itf] -= w; @@ -172,9 +170,10 @@ static int usb_event_handle(uint8_t itf) { #ifndef ENABLE_EMULATION uint16_t rx_read = usb_read_available(itf); #else - uint16_t rx_read = emul_read(); + uint16_t rx_read = emul_read(itf); #endif int proc_packet = 0; +#ifndef ENABLE_EMULATION #ifdef USB_ITF_HID if (itf == ITF_HID) { proc_packet = driver_process_usb_packet_hid(rx_read); @@ -185,10 +184,8 @@ static int usb_event_handle(uint8_t itf) { proc_packet = driver_process_usb_packet_ccid(rx_read); } #endif -#ifdef ENABLE_EMULATION - if (itf == ITF_EMUL) { - proc_packet = driver_process_usb_packet_emul(rx_read); - } +#else + proc_packet = driver_process_usb_packet_emul(itf, rx_read); #endif if (proc_packet > 0) { card_locked_itf = itf; @@ -199,6 +196,7 @@ static int usb_event_handle(uint8_t itf) { timeout_start(); } else { +#ifndef ENABLE_EMULATION #ifdef USB_ITF_HID if (itf == ITF_HID) { driver_process_usb_nopacket_hid(); @@ -208,6 +206,7 @@ static int usb_event_handle(uint8_t itf) { if (itf == ITF_CCID) { driver_process_usb_nopacket_ccid(); } +#endif #endif } return 0; @@ -257,6 +256,7 @@ void usb_task() { bool mounted = true; #endif for (uint8_t itf = 0; itf < ITF_TOTAL; itf++) { +#ifndef ENABLE_EMULATION #ifdef USB_ITF_HID if (itf == ITF_HID) { mounted = driver_mounted_hid(); @@ -266,6 +266,7 @@ void usb_task() { if (itf == ITF_CCID) { mounted = driver_mounted_ccid(); } +#endif #endif if (mounted == true) { @@ -303,16 +304,16 @@ void usb_task() { else { if (timeout > 0) { if (timeout + timeout_counter[itf] < board_millis()) { - #ifdef USB_ITF_HID +#ifdef USB_ITF_HID if (itf == ITF_HID) { driver_exec_timeout_hid(); } - #endif - #ifdef USB_ITF_CCID +#endif +#ifdef USB_ITF_CCID if (itf == ITF_CCID) { driver_exec_timeout_ccid(); } - #endif +#endif timeout = board_millis(); } } @@ -321,13 +322,16 @@ void usb_task() { #endif } } +#ifndef ENABLE_EMULATION #ifdef USB_ITF_HID hid_task(); #endif +#endif } uint8_t *usb_prepare_response(uint8_t itf) { +#ifndef ENABLE_EMULATION #ifdef USB_ITF_HID if (itf == ITF_HID) { return driver_prepare_response_hid(); @@ -337,11 +341,9 @@ uint8_t *usb_prepare_response(uint8_t itf) { if (itf == ITF_CCID) { return driver_prepare_response_ccid(); } -#endif -#ifdef ENABLE_EMULATION - if (itf == ITF_EMUL) { - return driver_prepare_response_emul(); - } #endif return NULL; +#else + return driver_prepare_response_emul(itf); +#endif } diff --git a/src/usb/usb.h b/src/usb/usb.h index 0a755e6..fe19dea 100644 --- a/src/usb/usb.h +++ b/src/usb/usb.h @@ -42,16 +42,12 @@ enum { -#ifndef ENABLE_EMULATION #ifdef USB_ITF_HID ITF_HID = 0, ITF_KEYBOARD, #endif #ifdef USB_ITF_CCID ITF_CCID, -#endif -#else - ITF_EMUL = 0, #endif ITF_TOTAL }; @@ -93,16 +89,16 @@ extern int driver_process_usb_nopacket_ccid(); #endif #ifdef ENABLE_EMULATION -extern int driver_process_usb_packet_emul(uint16_t rx_read); -extern void driver_exec_finished_emul(size_t size_next); -extern void driver_exec_finished_cont_emul(size_t size_next, size_t offset); -extern void driver_exec_timeout_emul(); -extern bool driver_mounted_emul(); -extern uint8_t *driver_prepare_response_emul(); -extern int driver_write_emul(const uint8_t *, size_t); -extern size_t driver_read_emul(uint8_t *, size_t); -extern int driver_process_usb_nopacket_emul(); -extern uint16_t emul_read(); +extern int driver_process_usb_packet_emul(uint8_t, uint16_t rx_read); +extern void driver_exec_finished_emul(uint8_t, size_t size_next); +extern void driver_exec_finished_cont_emul(uint8_t, size_t size_next, size_t offset); +extern void driver_exec_timeout_emul(uint8_t); +extern bool driver_mounted_emul(uint8_t); +extern uint8_t *driver_prepare_response_emul(uint8_t); +extern int driver_write_emul(uint8_t, const uint8_t *, size_t); +extern size_t driver_read_emul(uint8_t, uint8_t *, size_t); +extern int driver_process_usb_nopacket_emul(uint8_t); +extern uint16_t emul_read(uint8_t); #endif extern size_t usb_rx(uint8_t itf, const uint8_t *buffer, size_t len); diff --git a/src/usb/usb_descriptors.c b/src/usb/usb_descriptors.c index c673222..c5b23b0 100644 --- a/src/usb/usb_descriptors.c +++ b/src/usb/usb_descriptors.c @@ -190,9 +190,10 @@ static uint8_t desc_hid_kb[] = { TUD_HID_DESCRIPTOR(ITF_KEYBOARD, ITF_KEYBOARD + 5, HID_ITF_PROTOCOL_NONE, sizeof(desc_hid_report_kb), 0x80 | (EPNUM_HID + 1), 16, 5) }; - +#include "apdu.h" uint8_t const *tud_hid_descriptor_report_cb(uint8_t itf) { printf("report_cb %d\n", itf); +DEBUG_DATA(desc_hid_report, sizeof(desc_hid_report)); if (itf == ITF_HID) { return desc_hid_report; }