1// Copyright 2017 The Fuchsia Authors. All rights reserved. 2// Use of this source code is governed by a BSD-style license that can be 3// found in the LICENSE file. 4 5#include <inet6/inet6.h> 6#include <zircon/device/ethernet.h> 7#include <zircon/process.h> 8#include <zircon/syscalls.h> 9#include <zircon/syscalls/port.h> 10 11#include <fcntl.h> 12#include <limits.h> 13#include <stdio.h> 14#include <stdlib.h> 15#include <string.h> 16#include <threads.h> 17 18#define SRC_PORT 5004 19#define DST_PORT 5005 20 21#define BUFSIZE 2048 22#define BUFS 256 23 24#define RX_FIFO 0 25#define TX_FIFO 1 26 27typedef struct eth_buf eth_buf_t; 28 29struct eth_buf { 30 eth_buf_t* next; 31 eth_fifo_entry_t* e; 32}; 33 34typedef struct { 35 mac_addr_t dst; 36 mac_addr_t src; 37 uint16_t type; 38} __PACKED eth_hdr_t; 39 40eth_buf_t* avail_tx_buffers = NULL; 41eth_buf_t* pending_tx = NULL; 42zx_handle_t port = ZX_HANDLE_INVALID; 43 44void flip_src_dst(void* packet) { 45 eth_hdr_t* eth = packet; 46 mac_addr_t src_mac = eth->src; 47 eth->src = eth->dst; 48 eth->dst = src_mac; 49 50 ip6_hdr_t* ip = packet + ETH_HDR_LEN; 51 ip6_addr_t src_ip = ip->src; 52 ip->src = ip->dst; 53 ip->dst = src_ip; 54 ip->next_header = HDR_UDP; 55 56 udp_hdr_t* udp = packet + ETH_HDR_LEN + IP6_HDR_LEN; 57 uint16_t src_port = udp->src_port; 58 udp->src_port = udp->dst_port; 59 udp->dst_port = src_port; 60 udp->checksum = 0; 61 udp->checksum = ip6_checksum(ip, HDR_UDP, ntohs(ip->length)); 62} 63 64void send_pending_tx(zx_handle_t tx_fifo) { 65 zx_status_t status; 66 while (pending_tx != NULL) { 67 eth_fifo_entry_t* e = pending_tx->e; 68 e->cookie = pending_tx; 69 if ((status = zx_fifo_write(tx_fifo, sizeof(*e), e, 1, NULL)) != ZX_OK) { 70 fprintf(stderr, "netreflector: error reflecting packet %d\n", status); 71 return; 72 } 73 pending_tx = pending_tx->next; 74 } 75} 76 77void tx_complete(eth_fifo_entry_t* e) { 78 if (e->flags & ETH_FIFO_TX_OK) { 79 eth_buf_t* buf = e->cookie; 80 buf->next = avail_tx_buffers; 81 avail_tx_buffers = buf; 82 } 83} 84 85zx_status_t acquire_tx_buffer(eth_buf_t** out) { 86 if (avail_tx_buffers == NULL) { 87 fprintf(stderr, "netreflector: no tx buffers available.\n"); 88 return ZX_ERR_SHOULD_WAIT; 89 } 90 *out = avail_tx_buffers; 91 avail_tx_buffers = avail_tx_buffers->next; 92 return ZX_OK; 93} 94 95void queue_tx_buffer(eth_buf_t* tx) { 96 tx->next = pending_tx; 97 pending_tx = tx; 98} 99 100zx_status_t reflect_packet(char* iobuf, eth_fifo_entry_t* e) { 101 eth_buf_t* tx; 102 zx_status_t status; 103 if ((status = acquire_tx_buffer(&tx)) != ZX_OK) { 104 return status; 105 } 106 tx->e->length = e->length; 107 108 void* in_pkt = iobuf + e->offset; 109 void* out_pkt = iobuf + tx->e->offset; 110 memcpy(out_pkt, in_pkt, tx->e->length); 111 flip_src_dst(out_pkt); 112 113 queue_tx_buffer(tx); 114 return ZX_OK; 115} 116 117void rx_complete(char* iobuf, zx_handle_t rx_fifo, eth_fifo_entry_t* e) { 118 if (!(e->flags & ETH_FIFO_RX_OK)) { 119 return; 120 } 121 if (e->length < ETH_HDR_LEN + IP6_HDR_LEN + UDP_HDR_LEN) { 122 goto queue; 123 } 124 125 // Only reflect packets arriving from DST_PORT on SRC_PORT. 126 void* in_pkt = iobuf + e->offset; 127 udp_hdr_t* udp = in_pkt + ETH_HDR_LEN + IP6_HDR_LEN; 128 if (ntohs(udp->dst_port) != DST_PORT || ntohs(udp->src_port) != SRC_PORT) { 129 goto queue; 130 } 131 reflect_packet(iobuf, e); 132 133queue: 134 e->length = BUFSIZE; 135 e->flags = 0; 136 zx_status_t status; 137 if ((status = zx_fifo_write(rx_fifo, sizeof(*e), e, 1, NULL)) != ZX_OK) { 138 fprintf(stderr, "netreflector: failed to queue rx packet: %d\n", status); 139 } 140} 141 142void handle(char* iobuf, eth_fifos_t* fifos) { 143 zx_port_packet_t packet; 144 zx_status_t status; 145 size_t n; 146 eth_fifo_entry_t entries[BUFS]; 147 for (;;) { 148 status = zx_port_wait(port, ZX_TIME_INFINITE, &packet); 149 if (status != ZX_OK) { 150 fprintf(stderr, "netreflector: error while waiting on port %d\n", status); 151 return; 152 } 153 154 if (packet.signal.observed & ZX_FIFO_PEER_CLOSED) { 155 fprintf(stderr, "netreflector: fifo closed\n"); 156 return; 157 } 158 159 if (packet.signal.observed & ZX_FIFO_READABLE) { 160 uint8_t fifo_id = (uint8_t)packet.key; 161 zx_handle_t fifo = (fifo_id == RX_FIFO ? fifos->rx_fifo : fifos->tx_fifo); 162 if ((status = zx_fifo_read(fifo, sizeof(entries[0]), entries, countof(entries), &n)) != ZX_OK) { 163 fprintf(stderr, "netreflector: error reading fifo %d\n", status); 164 continue; 165 } 166 167 eth_fifo_entry_t* e = entries; 168 switch (fifo_id) { 169 case TX_FIFO: 170 for (size_t i = 0; i < n; i++, e++) { 171 tx_complete(e); 172 } 173 break; 174 case RX_FIFO: 175 for (size_t i = 0; i < n; i++, e++) { 176 rx_complete(iobuf, fifos->rx_fifo, e); 177 } 178 break; 179 default: 180 fprintf(stderr, "netreflector: unknown key %lu\n", packet.key); 181 break; 182 } 183 } 184 send_pending_tx(fifos->tx_fifo); 185 } 186} 187 188int main(int argc, char** argv) { 189 const char* ethdev = (argc > 1 ? argv[1] : "/dev/class/ethernet/000"); 190 int fd; 191 if ((fd = open(ethdev, O_RDWR)) < 0) { 192 fprintf(stderr, "netreflector: cannot open '%s'\n", argv[1]); 193 return -1; 194 } 195 196 eth_fifos_t fifos; 197 zx_status_t status; 198 199 ssize_t r; 200 if ((r = ioctl_ethernet_set_client_name(fd, "netreflector", 13)) < 0) { 201 fprintf(stderr, "netreflector: failed to set client name %zd\n", r); 202 } 203 204 if ((r = ioctl_ethernet_get_fifos(fd, &fifos)) < 0) { 205 fprintf(stderr, "netreflector: failed to get fifos: %zd\n", r); 206 return r; 207 } 208 209 // Allocate shareable ethernet buffer data heap. The first BUFS entries represent rx buffers, 210 // followed by BUFS entries representing tx buffers. 211 unsigned count = BUFS * 2; 212 zx_handle_t iovmo; 213 if ((status = zx_vmo_create(count * BUFSIZE, ZX_VMO_NON_RESIZABLE, &iovmo)) < 0) { 214 return -1; 215 } 216 char* iobuf; 217 if ((status = zx_vmar_map(zx_vmar_root_self(), 218 ZX_VM_PERM_READ | ZX_VM_PERM_WRITE, 219 0, iovmo, 0, count * BUFSIZE, (uintptr_t*)&iobuf)) < 0) { 220 return -1; 221 } 222 223 if ((r = ioctl_ethernet_set_iobuf(fd, &iovmo)) < 0) { 224 fprintf(stderr, "netreflector: failed to set iobuf: %zd\n", r); 225 return -1; 226 } 227 228 // Write first BUFS entries to rx fifo... 229 unsigned n = 0; 230 for (; n < BUFS; n++) { 231 eth_fifo_entry_t entry = { 232 .offset = n * BUFSIZE, .length = BUFSIZE, .flags = 0, .cookie = NULL, 233 }; 234 if ((status = zx_fifo_write(fifos.rx_fifo, sizeof(entry), &entry, 1, NULL)) < 0) { 235 fprintf(stderr, "netreflector: failed to queue rx packet: %d\n", status); 236 return -1; 237 } 238 } 239 240 // ... continue writing next BUFS entries to tx fifo. 241 eth_buf_t* buf = malloc(sizeof(eth_buf_t) * BUFS); 242 for (; n < count; n++, buf++) { 243 eth_fifo_entry_t entry = { 244 .offset = n * BUFSIZE, .length = BUFSIZE, .flags = 0, .cookie = buf, 245 }; 246 buf->e = &entry; 247 buf->next = avail_tx_buffers; 248 avail_tx_buffers = buf; 249 } 250 251 if (ioctl_ethernet_start(fd) < 0) { 252 fprintf(stderr, "netreflector: failed to start network interface\n"); 253 return -1; 254 } 255 256 if (zx_port_create(0, &port) != ZX_OK) { 257 fprintf(stderr, "netreflector: failed to create port\n"); 258 return -1; 259 } 260 261 u_int32_t signals = ZX_FIFO_READABLE | ZX_FIFO_PEER_CLOSED; 262 if ((status = zx_object_wait_async(fifos.rx_fifo, port, RX_FIFO, signals, 263 ZX_WAIT_ASYNC_REPEATING)) != ZX_OK) { 264 fprintf(stderr, "netreflector: failed binding port to rx fifo %d\n", status); 265 return -1; 266 } 267 268 if ((status = zx_object_wait_async(fifos.tx_fifo, port, TX_FIFO, signals, 269 ZX_WAIT_ASYNC_REPEATING)) != ZX_OK) { 270 fprintf(stderr, "netreflector: failed binding port to tx fifo %d\n", status); 271 return -1; 272 } 273 274 handle(iobuf, &fifos); 275 276 return 0; 277} 278