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