devif: added dst MAC lookup to UDP queue init
[barrelfish] / lib / devif / backends / net / udp / devif_backend_udp.c
1 /*
2  * Copyright (c) 2017, ETH Zurich.
3  * All rights reserved.
4  *
5  * This file is distributed under the terms in the attached LICENSE file.
6  * If you do not find this file, copies can be found by writing to:
7  * ETH Zurich D-INFK, Universit√§tstrasse 4, CH-8092 Zurich. Attn: Systems Group.
8  */
9
10 #include <stdlib.h>
11 #include <stdio.h>
12 #include <stdbool.h>
13 #include <barrelfish/barrelfish.h>
14 #include <barrelfish/waitset.h>
15 #include <devif/queue_interface.h>
16 #include <devif/backends/net/udp.h>
17 #include <lwip/inet_chksum.h>
18 #include <lwip/lwip/inet.h>
19 #include <net_interfaces/flags.h>
20 #include <net/net.h>
21 #include <net/net_filter.h>
22 #include <net/dhcp.h>
23 #include "../../../queue_interface_internal.h"
24 #include "../headers.h"
25
26 #define MAX_NUM_REGIONS 64
27
28 //#define DEBUG_ENABLED
29
30 #if defined(DEBUG_ENABLED) 
31 #define DEBUG(x...) do { printf("UDP_QUEUE: %s.%d:%s:%d: ", \
32             disp_name(), disp_get_core_id(), __func__, __LINE__); \
33                 printf(x);\
34         } while (0)
35
36 #else
37 #define DEBUG(x...) ((void)0)
38 #endif 
39
40 struct region_vaddr {
41     void* va;
42     regionid_t rid;
43 };
44
45 struct udp_q {
46     struct devq my_q;
47     struct devq* q;
48     struct udp_hdr header; // can fill in this header and reuse it by copying
49     struct region_vaddr regions[MAX_NUM_REGIONS];
50     struct net_filter_state* filter;
51 };
52
53
54 #ifdef DEBUG_ENABLED
55 static void print_buffer(struct udp_q* q, void* start, uint64_t len)
56 {
57     uint8_t* buf = (uint8_t*) start;
58     printf("Packet in region at address %p len %zu \n",
59            buf, len);
60     for (int i = 0; i < len; i+=2) {
61         if (((i % 16) == 0) && i > 0) {
62             printf("\n");
63         }
64         printf("%2X", buf[i]);
65         printf("%2X ", buf[i+1]);
66     }
67     printf("\n");
68 }
69 #endif
70
71 static errval_t udp_register(struct devq* q, struct capref cap,
72                             regionid_t rid) 
73 {
74        
75     errval_t err;
76     struct frame_identity frameid = { .base = 0, .bytes = 0 };
77
78     struct udp_q* que = (struct udp_q*) q;
79
80     // Map device registers
81     invoke_frame_identify(cap, &frameid);
82
83     err = vspace_map_one_frame_attr(&que->regions[rid % MAX_NUM_REGIONS].va, 
84                                     frameid.bytes, cap, VREGION_FLAGS_READ_WRITE, 
85                                     NULL, NULL);
86     if (err_is_fail(err)) {
87         DEBUG_ERR(err, "vspace_map_one_frame failed");
88         return err;
89     }
90     que->regions[rid % MAX_NUM_REGIONS].rid = rid;
91     DEBUG("id-%d va-%p \n", que->regions[rid % MAX_NUM_REGIONS].rid, 
92           que->regions[rid % MAX_NUM_REGIONS].va);
93
94     return que->q->f.reg(que->q, cap, rid);
95 }
96
97 static errval_t udp_deregister(struct devq* q, regionid_t rid) 
98 {
99     
100     struct udp_q* que = (struct udp_q*) q;
101     que->regions[rid % MAX_NUM_REGIONS].va = NULL;
102     que->regions[rid % MAX_NUM_REGIONS].rid = 0;
103     return que->q->f.dereg(que->q, rid);
104 }
105
106
107 static errval_t udp_control(struct devq* q, uint64_t cmd, uint64_t value,
108                            uint64_t* result)
109 {
110     struct udp_q* que = (struct udp_q*) q;
111     return que->q->f.ctrl(que->q, cmd, value, result);
112 }
113
114
115 static errval_t udp_notify(struct devq* q)
116 {
117     struct udp_q* que = (struct udp_q*) q;
118     return que->q->f.notify(que->q);
119 }
120
121 static errval_t udp_enqueue(struct devq* q, regionid_t rid, 
122                            genoffset_t offset, genoffset_t length,
123                            genoffset_t valid_data, genoffset_t valid_length,
124                            uint64_t flags)
125 {
126
127     // for now limit length
128     //  TODO fragmentation
129
130     struct udp_q* que = (struct udp_q*) q;
131     if (flags & NETIF_TXFLAG) {
132         
133         DEBUG("TX rid: %d offset %ld length %ld valid_length %ld \n", rid, offset, 
134               length, valid_length);
135         assert(valid_length <= 1500);    
136         que->header.len = htons(valid_length + UDP_HLEN);
137
138         assert(que->regions[rid % MAX_NUM_REGIONS].va != NULL);
139
140         uint8_t* start = (uint8_t*) que->regions[rid % MAX_NUM_REGIONS].va + 
141                          offset + valid_data + ETH_HLEN + IP_HLEN;   
142
143         memcpy(start, &que->header, sizeof(que->header));   
144
145         return que->q->f.enq(que->q, rid, offset, length, valid_data, 
146                              valid_length + UDP_HLEN, flags);
147     } 
148
149     if (flags & NETIF_RXFLAG) {
150         assert(valid_length <= 2048);    
151         DEBUG("RX rid: %d offset %ld length %ld valid_length %ld \n", rid, offset, 
152               length, valid_length);
153         return que->q->f.enq(que->q, rid, offset, length, valid_data, 
154                              valid_length, flags);
155     } 
156
157     return NET_QUEUE_ERR_UNKNOWN_BUF_TYPE;
158 }
159
160 static errval_t udp_dequeue(struct devq* q, regionid_t* rid, genoffset_t* offset,
161                            genoffset_t* length, genoffset_t* valid_data,
162                            genoffset_t* valid_length, uint64_t* flags)
163 {
164     errval_t err;
165     struct udp_q* que = (struct udp_q*) q;
166
167     err = que->q->f.deq(que->q, rid, offset, length, valid_data, valid_length, flags);
168     if (err_is_fail(err)) {    
169         return err;
170     }
171
172     if (*flags & NETIF_RXFLAG) {
173         DEBUG("RX rid: %d offset %ld valid_data %ld length %ld va %p \n", *rid, 
174               *offset, *valid_data, 
175               *valid_length, que->regions[*rid % MAX_NUM_REGIONS].va + *offset + *valid_data);
176
177         struct udp_hdr* header = (struct udp_hdr*) 
178                                  (que->regions[*rid % MAX_NUM_REGIONS].va +
179                                  *offset + *valid_data);
180  
181         // Correct port for this queue?
182         if (header->dest != que->header.dest) {
183             printf("UDP queue: dropping packet, wrong port %d %d \n",
184                    header->dest, que->header.dest);
185             err = que->q->f.enq(que->q, *rid, *offset, *length, 0, 0, NETIF_RXFLAG);
186             return err_push(err, NET_QUEUE_ERR_WRONG_PORT);
187         }
188         
189 #ifdef DEBUG_ENABLED
190         print_buffer(que, que->regions[*rid % MAX_NUM_REGIONS].va + *offset, *valid_length);
191 #endif
192
193         *valid_length = ntohs(header->len) - UDP_HLEN;
194         *valid_data += UDP_HLEN;
195         //print_buffer(que, que->regions[*rid % MAX_NUM_REGIONS].va + *offset+ *valid_data, *valid_length);
196         return SYS_ERR_OK;
197     }
198
199 #ifdef DEBUG_ENABLED
200     DEBUG("TX rid: %d offset %ld length %ld \n", *rid, *offset, 
201           *valid_length);
202 #endif
203
204     return SYS_ERR_OK;
205 }
206
207 /*
208  * Public functions
209  *
210  */
211 errval_t udp_create(struct udp_q** q, const char* card_name, 
212                     uint16_t src_port, uint16_t dst_port,
213                     uint32_t dst_ip, void(*interrupt)(void*), bool poll)
214 {
215     errval_t err;
216     struct udp_q* que;
217     que = calloc(1, sizeof(struct udp_q));
218     assert(que);
219
220     uint32_t src_ip;
221     err = net_config_current_ip_query(NET_FLAGS_BLOCKING_INIT, &src_ip);
222     if (err_is_fail(err)) {
223         return err;
224     }
225
226     // init other queue
227     uint64_t qid;
228     err = ip_create((struct ip_q**) &que->q, card_name, &qid, UDP_PROT, dst_ip, 
229                     interrupt, poll);
230     if (err_is_fail(err)) {
231         return err;
232     }
233
234     err = net_filter_init(&que->filter, card_name);
235     if (err_is_fail(err)) {
236         return err;
237     }  
238     
239     src_ip = htonl(src_ip);
240     struct net_filter_ip ip = {
241         .qid = qid,
242         .ip_src = dst_ip,
243         .ip_dst = src_ip,
244         .port_dst = dst_port,
245         .type = NET_FILTER_UDP,    
246     };
247
248     err = net_filter_ip_install(que->filter, &ip);
249     if (err_is_fail(err)) {
250         return err;
251     }
252
253     err = devq_init(&que->my_q, false);
254     if (err_is_fail(err)) {
255         errval_t err2;
256         err2 = net_filter_ip_remove(que->filter, &ip);
257         if (err_is_fail(err)) {
258             return err_push(err2, err);
259         }
260         return err;
261     }   
262
263     // UDP fields
264     que->header.src = htons(src_port);
265     que->header.dest = htons(dst_port);
266     que->header.chksum = 0x0;
267
268     que->my_q.f.reg = udp_register;
269     que->my_q.f.dereg = udp_deregister;
270     que->my_q.f.ctrl = udp_control;
271     que->my_q.f.notify = udp_notify;
272     que->my_q.f.enq = udp_enqueue;
273     que->my_q.f.deq = udp_dequeue;
274     *q = que;
275
276     return SYS_ERR_OK;
277 }
278
279 errval_t udp_destroy(struct udp_q* q)
280 {
281     // TODO destroy q->q;
282     free(q);    
283
284     return SYS_ERR_OK;
285 }
286
287 errval_t udp_write_buffer(struct udp_q* q, regionid_t rid, genoffset_t offset,
288                           void* data, uint16_t len) 
289 {
290     assert(len <= 1500);
291     if (q->regions[rid % MAX_NUM_REGIONS].va != NULL) {
292         uint8_t* start = q->regions[rid % MAX_NUM_REGIONS].va + offset 
293                          + sizeof (struct udp_hdr) 
294                          + sizeof (struct ip_hdr)
295                          + sizeof (struct eth_hdr);
296         memcpy(start, data, len);
297         return SYS_ERR_OK;
298     } else {
299         return DEVQ_ERR_INVALID_REGION_ARGS;
300     }
301 }
302
303
304