webadmin/pico-keys-sdk
1
0
Fork 0
mirror of https://github.com/polhenarejos/pico-keys-sdk synced 2026-05-08 22:16:10 +02:00
Code Issues Packages Projects Releases Wiki Activity

Emulation is now a higher define which also accepts interfaces.

Browse Source 
Signed-off-by: Pol Henarejos <pol.henarejos@cttc.es>
This commit is contained in:
Pol Henarejos
2023-02-23 07:57:35 +01:00
parent e5e5af634c
commit 16b4fa6ca3
6 changed files with 176 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files

168
src/usb/emulation/emulation.c
View File

@@ -25,13 +25,16 @@
#include <errno.h>
#include <fcntl.h>
#include <time.h>
#include <poll.h>
#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;
}