Harald Welte | 095ac6c | 2016-03-19 13:39:33 +0100 | [diff] [blame] | 1 | /* simtrace - main program for the host PC |
| 2 | * |
| 3 | * (C) 2010-2016 by Harald Welte <hwelte@hmw-consulting.de> |
| 4 | * |
| 5 | * This program is free software; you can redistribute it and/or modify |
| 6 | * it under the terms of the GNU General Public License version 2 |
| 7 | * as published by the Free Software Foundation |
| 8 | * |
| 9 | * This program is distributed in the hope that it will be useful, |
| 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 12 | * GNU General Public License for more details. |
| 13 | * |
| 14 | * You should have received a copy of the GNU General Public License |
| 15 | * along with this program; if not, write to the Free Software |
| 16 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
| 17 | */ |
| 18 | |
| 19 | #include <errno.h> |
| 20 | #include <unistd.h> |
| 21 | #include <stdio.h> |
| 22 | #include <stdlib.h> |
| 23 | #include <string.h> |
| 24 | #include <stdint.h> |
| 25 | #include <time.h> |
| 26 | #define _GNU_SOURCE |
| 27 | #include <getopt.h> |
| 28 | #include <poll.h> |
| 29 | |
| 30 | #include <sys/time.h> |
| 31 | #include <sys/types.h> |
| 32 | #include <sys/socket.h> |
| 33 | #include <netinet/in.h> |
| 34 | #include <arpa/inet.h> |
| 35 | |
| 36 | #include <libusb.h> |
| 37 | |
| 38 | #include "simtrace.h" |
| 39 | #include "cardemu_prot.h" |
| 40 | #include "apdu_dispatch.h" |
Harald Welte | 236caf6 | 2016-03-19 21:28:09 +0100 | [diff] [blame] | 41 | #include "simtrace2-discovery.h" |
Harald Welte | 095ac6c | 2016-03-19 13:39:33 +0100 | [diff] [blame] | 42 | |
| 43 | #include <osmocom/core/utils.h> |
| 44 | #include <osmocom/core/socket.h> |
| 45 | #include <osmocom/core/select.h> |
| 46 | |
| 47 | struct libusb_device_handle *g_devh; |
| 48 | static struct sockaddr_in g_sa_remote; |
| 49 | static struct osmo_fd g_udp_ofd; |
| 50 | |
| 51 | static void print_welcome(void) |
| 52 | { |
| 53 | printf("usb2udp - UDP/IP forwarding of SIMtrace card emulation\n" |
| 54 | "(C) 2016 by Harald Welte <laforge@gnumonks.org>\n\n"); |
| 55 | } |
| 56 | |
| 57 | static void print_help(void) |
| 58 | { |
| 59 | printf( "\t-h\t--help\n" |
Harald Welte | 236caf6 | 2016-03-19 21:28:09 +0100 | [diff] [blame] | 60 | "\t-i\t--interface <0-255>\n" |
Harald Welte | 095ac6c | 2016-03-19 13:39:33 +0100 | [diff] [blame] | 61 | "\n" |
| 62 | ); |
| 63 | } |
| 64 | |
| 65 | struct ep_buf { |
| 66 | uint8_t ep; |
| 67 | uint8_t buf[1024]; |
| 68 | struct libusb_transfer *xfer; |
| 69 | }; |
| 70 | static struct ep_buf g_buf_in; |
| 71 | static struct ep_buf g_buf_out; |
| 72 | |
| 73 | static void usb_in_xfer_cb(struct libusb_transfer *xfer) |
| 74 | { |
| 75 | int rc; |
| 76 | |
| 77 | printf("xfer_cb(ep=%02x): status=%d, flags=0x%x, type=%u, len=%u, act_len=%u\n", |
| 78 | xfer->endpoint, xfer->status, xfer->flags, xfer->type, xfer->length, xfer->actual_length); |
| 79 | switch (xfer->status) { |
| 80 | case LIBUSB_TRANSFER_COMPLETED: |
Harald Welte | 236caf6 | 2016-03-19 21:28:09 +0100 | [diff] [blame] | 81 | if (xfer->endpoint == g_buf_in.ep) { |
Harald Welte | 095ac6c | 2016-03-19 13:39:33 +0100 | [diff] [blame] | 82 | /* process the data */ |
| 83 | printf("read %d bytes from SIMTRACE, forwarding to UDP\n", xfer->actual_length); |
| 84 | rc = sendto(g_udp_ofd.fd, xfer->buffer, xfer->actual_length, 0, (struct sockaddr *)&g_sa_remote, sizeof(g_sa_remote)); |
| 85 | if (rc <= 0) { |
| 86 | fprintf(stderr, "error writing to UDP\n"); |
| 87 | } |
| 88 | /* and re-submit the URB */ |
| 89 | libusb_submit_transfer(xfer); |
Harald Welte | 236caf6 | 2016-03-19 21:28:09 +0100 | [diff] [blame] | 90 | } else if (xfer->endpoint == g_buf_out.ep) { |
Harald Welte | 095ac6c | 2016-03-19 13:39:33 +0100 | [diff] [blame] | 91 | /* re-enable reading from the UDP side */ |
| 92 | g_udp_ofd.when |= BSC_FD_READ; |
Harald Welte | 095ac6c | 2016-03-19 13:39:33 +0100 | [diff] [blame] | 93 | } |
| 94 | break; |
| 95 | default: |
| 96 | fprintf(stderr, "xfer_cb(ERROR '%s')\n", osmo_hexdump_nospc(xfer->buffer, xfer->actual_length)); |
| 97 | break; |
| 98 | } |
| 99 | } |
| 100 | |
Harald Welte | 236caf6 | 2016-03-19 21:28:09 +0100 | [diff] [blame] | 101 | static void init_ep_buf(struct ep_buf *epb) |
Harald Welte | 095ac6c | 2016-03-19 13:39:33 +0100 | [diff] [blame] | 102 | { |
Harald Welte | 095ac6c | 2016-03-19 13:39:33 +0100 | [diff] [blame] | 103 | if (!epb->xfer) |
| 104 | epb->xfer = libusb_alloc_transfer(0); |
| 105 | |
| 106 | epb->xfer->flags = 0; |
| 107 | |
| 108 | libusb_fill_bulk_transfer(epb->xfer, g_devh, epb->ep, epb->buf, sizeof(epb->buf), usb_in_xfer_cb, NULL, 0); |
| 109 | } |
| 110 | |
| 111 | /*********************************************************************** |
| 112 | * libosmocore main loop integration of libusb async I/O |
| 113 | ***********************************************************************/ |
| 114 | |
| 115 | static int g_libusb_pending = 0; |
| 116 | |
| 117 | static int ofd_libusb_cb(struct osmo_fd *ofd, unsigned int what) |
| 118 | { |
| 119 | /* FIXME */ |
| 120 | g_libusb_pending = 1; |
| 121 | |
| 122 | return 0; |
| 123 | } |
| 124 | |
| 125 | /* call-back when libusb adds a FD */ |
| 126 | static void libusb_fd_added_cb(int fd, short events, void *user_data) |
| 127 | { |
| 128 | struct osmo_fd *ofd = talloc_zero(NULL, struct osmo_fd); |
| 129 | |
| 130 | printf("%s(%u, %x)\n", __func__, fd, events); |
| 131 | |
| 132 | ofd->fd = fd; |
| 133 | ofd->cb = &ofd_libusb_cb; |
| 134 | if (events & POLLIN) |
| 135 | ofd->when |= BSC_FD_READ; |
| 136 | if (events & POLLOUT) |
| 137 | ofd->when |= BSC_FD_WRITE; |
| 138 | |
| 139 | osmo_fd_register(ofd); |
| 140 | } |
| 141 | |
| 142 | /* call-back when libusb removes a FD */ |
| 143 | static void libusb_fd_removed_cb(int fd, void *user_data) |
| 144 | { |
| 145 | struct osmo_fd *ofd; |
| 146 | |
| 147 | printf("%s(%u)\n", __func__, fd); |
| 148 | #if 0 |
| 149 | /* FIXME: This needs new export in libosmocore! */ |
| 150 | ofd = osmo_fd_get_by_fd(fd); |
| 151 | |
| 152 | if (ofd) { |
| 153 | osmo_fd_unregister(ofd); |
| 154 | talloc_free(ofd); |
| 155 | } |
| 156 | #endif |
| 157 | } |
| 158 | |
| 159 | /* call-back when the UDP socket is readable */ |
| 160 | static int ofd_udp_cb(struct osmo_fd *ofd, unsigned int what) |
| 161 | { |
| 162 | int rc; |
| 163 | int addrlen = sizeof(g_sa_remote); |
| 164 | |
| 165 | rc = recvfrom(ofd->fd, g_buf_out.buf, sizeof(g_buf_out.buf), 0, |
| 166 | (struct sockaddr *)&g_sa_remote, &addrlen); |
| 167 | if (rc <= 0) { |
| 168 | fprintf(stderr, "error reading from UDP\n"); |
| 169 | return 0; |
| 170 | } |
| 171 | printf("read %d bytes from UDP, forwarding to SIMTRACE\n", rc); |
| 172 | g_buf_out.xfer->length = rc; |
| 173 | |
| 174 | /* disable further READ interest for the UDP socket */ |
| 175 | ofd->when &= ~BSC_FD_READ; |
| 176 | |
| 177 | /* submit the URB on the OUT end point */ |
| 178 | libusb_submit_transfer(g_buf_out.xfer); |
| 179 | |
| 180 | return 0; |
| 181 | } |
| 182 | |
| 183 | static void run_mainloop(void) |
| 184 | { |
| 185 | int rc; |
| 186 | |
| 187 | printf("Entering main loop\n"); |
| 188 | |
| 189 | while (1) { |
| 190 | osmo_select_main(0); |
| 191 | if (g_libusb_pending) { |
| 192 | struct timeval tv; |
| 193 | memset(&tv, 0, sizeof(tv)); |
| 194 | rc = libusb_handle_events_timeout_completed(NULL, &tv, NULL); |
| 195 | if (rc != 0) { |
| 196 | fprintf(stderr, "handle_events_timeout_completed == %d\n", rc); |
| 197 | break; |
| 198 | } |
| 199 | } |
| 200 | } |
| 201 | } |
| 202 | |
| 203 | int main(int argc, char **argv) |
| 204 | { |
| 205 | int rc; |
| 206 | int c, ret = 1; |
| 207 | char *remote_host = NULL; |
| 208 | int local_udp_port = 52342; |
Harald Welte | 236caf6 | 2016-03-19 21:28:09 +0100 | [diff] [blame] | 209 | unsigned int if_num = 0; |
Harald Welte | 095ac6c | 2016-03-19 13:39:33 +0100 | [diff] [blame] | 210 | |
| 211 | print_welcome(); |
| 212 | |
| 213 | while (1) { |
| 214 | int option_index = 0; |
| 215 | static const struct option opts[] = { |
| 216 | { "udp-port", 1, 0, 'u' }, |
Harald Welte | 236caf6 | 2016-03-19 21:28:09 +0100 | [diff] [blame] | 217 | { "interface", 1, 0, 'I' }, |
Harald Welte | 095ac6c | 2016-03-19 13:39:33 +0100 | [diff] [blame] | 218 | { "help", 0, 0, 'h' }, |
| 219 | { NULL, 0, 0, 0 } |
| 220 | }; |
| 221 | |
Harald Welte | 236caf6 | 2016-03-19 21:28:09 +0100 | [diff] [blame] | 222 | c = getopt_long(argc, argv, "u:I:h", opts, &option_index); |
Harald Welte | 095ac6c | 2016-03-19 13:39:33 +0100 | [diff] [blame] | 223 | if (c == -1) |
| 224 | break; |
| 225 | switch (c) { |
| 226 | case 'u': |
| 227 | local_udp_port = atoi(optarg); |
| 228 | break; |
Harald Welte | 236caf6 | 2016-03-19 21:28:09 +0100 | [diff] [blame] | 229 | case 'I': |
| 230 | if_num = atoi(optarg); |
| 231 | break; |
Harald Welte | 095ac6c | 2016-03-19 13:39:33 +0100 | [diff] [blame] | 232 | case 'h': |
| 233 | print_help(); |
| 234 | exit(0); |
| 235 | break; |
| 236 | } |
| 237 | } |
| 238 | |
| 239 | rc = libusb_init(NULL); |
| 240 | if (rc < 0) { |
| 241 | fprintf(stderr, "libusb initialization failed\n"); |
| 242 | goto close_exit; |
| 243 | } |
| 244 | |
| 245 | libusb_set_pollfd_notifiers(NULL, &libusb_fd_added_cb, &libusb_fd_removed_cb, NULL); |
| 246 | |
| 247 | g_devh = libusb_open_device_with_vid_pid(NULL, SIMTRACE_USB_VENDOR, SIMTRACE_USB_PRODUCT); |
| 248 | if (!g_devh) { |
| 249 | fprintf(stderr, "can't open USB device\n"); |
| 250 | goto close_exit; |
| 251 | } |
| 252 | |
Harald Welte | 236caf6 | 2016-03-19 21:28:09 +0100 | [diff] [blame] | 253 | rc = libusb_claim_interface(g_devh, if_num); |
Harald Welte | 095ac6c | 2016-03-19 13:39:33 +0100 | [diff] [blame] | 254 | if (rc < 0) { |
Harald Welte | 236caf6 | 2016-03-19 21:28:09 +0100 | [diff] [blame] | 255 | fprintf(stderr, "can't claim interface %u; rc=%d\n", if_num, rc); |
Harald Welte | 095ac6c | 2016-03-19 13:39:33 +0100 | [diff] [blame] | 256 | goto close_exit; |
| 257 | } |
| 258 | |
| 259 | /* open UDP socket, register with select handling and mark it |
| 260 | * readable */ |
| 261 | g_udp_ofd.cb = ofd_udp_cb; |
Harald Welte | 236caf6 | 2016-03-19 21:28:09 +0100 | [diff] [blame] | 262 | osmo_sock_init_ofd(&g_udp_ofd, AF_INET, SOCK_DGRAM, IPPROTO_UDP, NULL, local_udp_port + if_num, OSMO_SOCK_F_BIND); |
Harald Welte | 095ac6c | 2016-03-19 13:39:33 +0100 | [diff] [blame] | 263 | |
Harald Welte | 236caf6 | 2016-03-19 21:28:09 +0100 | [diff] [blame] | 264 | rc = get_usb_ep_addrs(g_devh, if_num, &g_buf_out.ep, &g_buf_in.ep, NULL); |
| 265 | if (rc < 0) { |
| 266 | fprintf(stderr, "couldn't find enpdoint addresses; rc=%d\n", rc); |
| 267 | goto close_exit; |
| 268 | } |
Harald Welte | 095ac6c | 2016-03-19 13:39:33 +0100 | [diff] [blame] | 269 | /* initialize USB buffers / transfers */ |
Harald Welte | 236caf6 | 2016-03-19 21:28:09 +0100 | [diff] [blame] | 270 | init_ep_buf(&g_buf_out); |
| 271 | init_ep_buf(&g_buf_in); |
Harald Welte | 095ac6c | 2016-03-19 13:39:33 +0100 | [diff] [blame] | 272 | |
| 273 | /* submit the first transfer for the IN endpoint */ |
| 274 | libusb_submit_transfer(g_buf_in.xfer); |
| 275 | |
| 276 | run_mainloop(); |
| 277 | |
| 278 | ret = 0; |
| 279 | |
| 280 | libusb_release_interface(g_devh, 0); |
| 281 | close_exit: |
| 282 | if (g_devh) |
| 283 | libusb_close(g_devh); |
| 284 | |
| 285 | release_exit: |
| 286 | libusb_exit(NULL); |
| 287 | return ret; |
| 288 | } |