1
0
mirror of https://github.com/trezor/trezor-firmware.git synced 2024-12-17 11:58:13 +00:00
trezor-firmware/legacy/emulator/udp.c
2019-06-17 20:28:29 +02:00

128 lines
3.3 KiB
C

/*
* This file is part of the Trezor project, https://trezor.io/
*
* Copyright (C) 2017 Saleem Rashid <trezor@saleemrashid.com>
*
* This library is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this library. If not, see <http://www.gnu.org/licenses/>.
*/
#include <arpa/inet.h>
#include <errno.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#define TREZOR_UDP_PORT 21324
struct usb_socket {
int fd;
struct sockaddr_in from;
socklen_t fromlen;
};
static struct usb_socket usb_main;
static struct usb_socket usb_debug;
static int socket_setup(int port) {
int fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (fd < 0) {
perror("Failed to create socket");
exit(1);
}
struct sockaddr_in addr;
addr.sin_family = AF_INET;
addr.sin_port = htons(port);
addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
if (bind(fd, (struct sockaddr *)&addr, sizeof(addr)) != 0) {
perror("Failed to bind socket");
exit(1);
}
return fd;
}
static size_t socket_write(struct usb_socket *sock, const void *buffer,
size_t size) {
if (sock->fromlen > 0) {
ssize_t n = sendto(sock->fd, buffer, size, MSG_DONTWAIT,
(const struct sockaddr *)&sock->from, sock->fromlen);
if (n < 0 || ((size_t)n) != size) {
perror("Failed to write socket");
return 0;
}
}
return size;
}
static size_t socket_read(struct usb_socket *sock, void *buffer, size_t size) {
sock->fromlen = sizeof(sock->from);
ssize_t n = recvfrom(sock->fd, buffer, size, MSG_DONTWAIT,
(struct sockaddr *)&sock->from, &sock->fromlen);
if (n < 0) {
if (errno != EAGAIN && errno != EWOULDBLOCK) {
perror("Failed to read socket");
}
return 0;
}
static const char msg_ping[] = {'P', 'I', 'N', 'G', 'P', 'I', 'N', 'G'};
static const char msg_pong[] = {'P', 'O', 'N', 'G', 'P', 'O', 'N', 'G'};
if (n == sizeof(msg_ping) &&
memcmp(buffer, msg_ping, sizeof(msg_ping)) == 0) {
socket_write(sock, msg_pong, sizeof(msg_pong));
return 0;
}
return n;
}
void emulatorSocketInit(void) {
usb_main.fd = socket_setup(TREZOR_UDP_PORT);
usb_main.fromlen = 0;
usb_debug.fd = socket_setup(TREZOR_UDP_PORT + 1);
usb_debug.fromlen = 0;
}
size_t emulatorSocketRead(int *iface, void *buffer, size_t size) {
size_t n = socket_read(&usb_main, buffer, size);
if (n > 0) {
*iface = 0;
return n;
}
n = socket_read(&usb_debug, buffer, size);
if (n > 0) {
*iface = 1;
return n;
}
return 0;
}
size_t emulatorSocketWrite(int iface, const void *buffer, size_t size) {
if (iface == 0) {
return socket_write(&usb_main, buffer, size);
}
if (iface == 1) {
return socket_write(&usb_debug, buffer, size);
}
return 0;
}