Statistics
| Branch: | Tag: | Revision:

root / firmware / microblaze / lib / net_common.c @ bb86022d

History | View | Annotate | Download (10.9 KB)

1
/* -*- c -*- */
2
/*
3
 * Copyright 2009,2010 Ettus Research LLC
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 as published by
7
 * the Free Software Foundation, either version 3 of the License, or
8
 * (at your option) any later version.
9
 *
10
 * This program is distributed in the hope that it will be useful,
11
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13
 * GNU General Public License for more details.
14
 *
15
 * You should have received a copy of the GNU General Public License
16
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
17
 */
18

    
19
#ifdef HAVE_CONFIG_H
20
#include <config.h>
21
#endif
22
#include "net_common.h"
23
#include "banal.h"
24
#include <hal_io.h>
25
#include <buffer_pool.h>
26
#include <memory_map.h>
27
#include <memcpy_wa.h>
28
#include <ethernet.h>
29
#include <net/padded_eth_hdr.h>
30
#include <lwip/ip.h>
31
#include <lwip/udp.h>
32
#include <lwip/icmp.h>
33
#include <stdlib.h>
34
#include <nonstdio.h>
35
#include "arp_cache.h"
36
#include "if_arp.h"
37
#include <ethertype.h>
38
#include <string.h>
39

    
40

    
41
int cpu_tx_buf_dest_port = PORT_ETH;
42

    
43
// If this is non-zero, this dbsm could be writing to the ethernet
44
dbsm_t *ac_could_be_sending_to_eth;
45

    
46
static inline bool
47
ip_addr_eq(const struct ip_addr a, const struct ip_addr b)
48
{
49
  return a.addr == b.addr;
50
}
51

    
52
// ------------------------------------------------------------------------
53

    
54
get_eth_mac_addr_t _get_eth_mac_addr = NULL;
55

    
56
void register_get_eth_mac_addr(get_eth_mac_addr_t get_eth_mac_addr){
57
    _get_eth_mac_addr = get_eth_mac_addr;
58
}
59

    
60
get_ip_addr_t _get_ip_addr = NULL;
61

    
62
void register_get_ip_addr(get_ip_addr_t get_ip_addr){
63
    _get_ip_addr = get_ip_addr;
64
}
65

    
66
//-------------------------------------------------------------------------
67

    
68
#define        MAX_UDP_LISTENERS        6
69

    
70
struct listener_entry {
71
  unsigned short        port;
72
  udp_receiver_t        rcvr;
73
};
74

    
75
static struct listener_entry listeners[MAX_UDP_LISTENERS];
76

    
77
static struct listener_entry *
78
find_listener_by_port(unsigned short port)
79
{
80
  for (int i = 0; i < MAX_UDP_LISTENERS; i++){
81
    if (port == listeners[i].port)
82
      return &listeners[i];
83
  }
84
  return 0;
85
}
86

    
87
static struct listener_entry *
88
find_free_listener(void)
89
{
90
  for (int i = 0; i < MAX_UDP_LISTENERS; i++){
91
    if (listeners[i].rcvr == 0)
92
      return &listeners[i];
93
  }
94
  abort();
95
}
96

    
97
void
98
register_udp_listener(int port, udp_receiver_t rcvr)
99
{
100
  struct listener_entry *lx = find_listener_by_port(port);
101
  if (lx)
102
    lx->rcvr = rcvr;
103
  else {
104
    lx = find_free_listener();
105
    lx->port = port;
106
    lx->rcvr = rcvr;
107
  }
108
}
109

    
110
// ------------------------------------------------------------------------
111

    
112

    
113
/*!
114
 * low level routine to assembly an ethernet frame and send it.
115
 *
116
 * \param dst destination mac address
117
 * \param ethertype ethertype field
118
 * \param buf0 first part of data
119
 * \param len0 length of first part of data
120
 * \param buf1 second part of data
121
 * \param len1 length of second part of data
122
 * \param buf2 third part of data
123
 * \param len2 length of third part of data
124
 */
125
static void
126
send_pkt(eth_mac_addr_t dst, int ethertype,
127
         const void *buf0, size_t len0,
128
         const void *buf1, size_t len1,
129
         const void *buf2, size_t len2)
130
{
131
  // Wait for buffer to become idle
132
  // FIXME can this ever not be ready?
133

    
134
  //hal_set_leds(LED_BUF_BUSY, LED_BUF_BUSY);
135
  while((buffer_pool_status->status & BPS_IDLE(CPU_TX_BUF)) == 0)
136
    ;
137
  //hal_set_leds(0, LED_BUF_BUSY);
138

    
139
  // Assemble the header
140
  padded_eth_hdr_t        ehdr;
141
  ehdr.pad = 0;
142
  ehdr.dst = dst;
143
  ehdr.src = _get_eth_mac_addr();
144
  ehdr.ethertype = ethertype;
145

    
146
  uint32_t *p = buffer_ram(CPU_TX_BUF);
147

    
148
  // Copy the pieces into the buffer
149
  *p++ = 0x0;                                    // slow path
150
  memcpy_wa(p, &ehdr, sizeof(ehdr));      // 4 lines
151
  p += sizeof(ehdr)/sizeof(uint32_t);
152

    
153

    
154
  // FIXME modify memcpy_wa to do read/modify/write if reqd
155

    
156
  if (len0 && ((len0 & 0x3) || (intptr_t) buf0 & 0x3))
157
    printf("send_pkt: bad alignment of len0 and/or buf0\n");
158

    
159
  if (len1 && ((len1 & 0x3) || (intptr_t) buf1 & 0x3))
160
    printf("send_pkt: bad alignment of len1 and/or buf1\n");
161

    
162
  if (len2 && ((len2 & 0x3) || (intptr_t) buf2 & 0x3))
163
    printf("send_pkt: bad alignment of len2 and/or buf2\n");
164

    
165
  if (len0){
166
    memcpy_wa(p, buf0, len0);
167
    p += len0/sizeof(uint32_t);
168
  }
169
  if (len1){
170
    memcpy_wa(p, buf1, len1);
171
    p += len1/sizeof(uint32_t);
172
  }
173
  if (len2){
174
    memcpy_wa(p, buf2, len2);
175
    p += len2/sizeof(uint32_t);
176
  }
177

    
178
  size_t total_len = (p - buffer_ram(CPU_TX_BUF)) * sizeof(uint32_t);
179
  if (total_len < 60)                // ensure that we don't try to send a short packet
180
    total_len = 60;
181
  
182
  // wait until nobody else is sending to the ethernet
183
  if (ac_could_be_sending_to_eth){
184
    //hal_set_leds(LED_ETH_BUSY, LED_ETH_BUSY);
185
    dbsm_wait_for_opening(ac_could_be_sending_to_eth);
186
    //hal_set_leds(0x0, LED_ETH_BUSY);
187
  }
188

    
189
  if (0){
190
    printf("send_pkt to port %d, len = %d\n",
191
           cpu_tx_buf_dest_port, (int) total_len);
192
    print_buffer(buffer_ram(CPU_TX_BUF), total_len/4);
193
  }
194

    
195
  // fire it off
196
  bp_send_from_buf(CPU_TX_BUF, cpu_tx_buf_dest_port, 1, 0, total_len/4);
197

    
198
  // wait for it to complete (not long, it's a small pkt)
199
  while((buffer_pool_status->status & (BPS_DONE(CPU_TX_BUF) | BPS_ERROR(CPU_TX_BUF))) == 0)
200
    ;
201

    
202
  bp_clear_buf(CPU_TX_BUF);
203
}
204

    
205
unsigned int 
206
chksum_buffer(unsigned short *buf, int nshorts, unsigned int initial_chksum)
207
{
208
  unsigned int chksum = initial_chksum;
209
  for (int i = 0; i < nshorts; i++)
210
    CHKSUM(buf[i], &chksum);
211

    
212
  return chksum;
213
}
214

    
215

    
216
void
217
send_ip_pkt(struct ip_addr dst, int protocol,
218
            const void *buf0, size_t len0,
219
            const void *buf1, size_t len1)
220
{
221
  struct ip_addr src = _get_ip_addr();
222
  int ttl = 32;
223

    
224
  struct ip_hdr ip;
225
  IPH_VHLTOS_SET(&ip, 4, 5, 0);
226
  IPH_LEN_SET(&ip, IP_HLEN + len0 + len1);
227
  IPH_ID_SET(&ip, 0);
228
  IPH_OFFSET_SET(&ip, IP_DF);        /* don't fragment */
229
  ip._ttl_proto = (ttl << 8) | (protocol & 0xff);
230
  ip._chksum = 0;
231
  ip.src = src;
232
  ip.dest = dst;
233

    
234
  ip._chksum = ~chksum_buffer((unsigned short *) &ip,
235
                              sizeof(ip)/sizeof(short), 0);
236

    
237
  eth_mac_addr_t dst_mac;
238
  bool found = arp_cache_lookup_mac(&ip.dest, &dst_mac);
239
  if (!found){
240
    printf("net_common: failed to hit cache looking for ");
241
    print_ip(ip.dest);
242
    newline();
243
    return;
244
  }
245

    
246
  send_pkt(dst_mac, ETHERTYPE_IPV4,
247
           &ip, sizeof(ip), buf0, len0, buf1, len1);
248
}
249

    
250
void 
251
send_udp_pkt(int src_port, struct socket_address dst,
252
             const void *buf, size_t len)
253
{
254
  struct udp_hdr udp _AL4;
255
  udp.src = src_port;
256
  udp.dest = dst.port;
257
  udp.len = UDP_HLEN + len;
258
  udp.chksum = 0;
259

    
260
  send_ip_pkt(dst.addr, IP_PROTO_UDP,
261
              &udp, sizeof(udp), buf, len);
262
}
263

    
264
static void
265
handle_udp_packet(struct ip_addr src_ip, struct ip_addr dst_ip,
266
                  struct udp_hdr *udp, size_t len)
267
{
268
  if (len != udp->len){
269
    printf("UDP inconsistent lengths: %d %d\n", (int)len, udp->len);
270
    return;
271
  }
272

    
273
  unsigned char *payload = ((unsigned char *) udp) + UDP_HLEN;
274
  int payload_len = len - UDP_HLEN;
275

    
276
  if (0){
277
    printf("\nUDP: src = %d  dst = %d  len = %d\n",
278
           udp->src, udp->dest, udp->len);
279

    
280
    //print_bytes(0, payload, payload_len);
281
  }
282

    
283
  struct listener_entry *lx = find_listener_by_port(udp->dest);
284
  if (lx){
285
    struct socket_address src = make_socket_address(src_ip, udp->src);
286
    struct socket_address dst = make_socket_address(dst_ip, udp->dest);
287
    lx->rcvr(src, dst, payload, payload_len);
288
  }
289
}
290

    
291
static void
292
handle_icmp_packet(struct ip_addr src, struct ip_addr dst,
293
                   struct icmp_echo_hdr *icmp, size_t len)
294
{
295
  switch (icmp->type){
296
  case ICMP_DUR:        // Destinatino Unreachable
297
    //stop_streaming(); //FIXME
298
    if (icmp->code == ICMP_DUR_PORT){        // port unreachable
299
      //struct udp_hdr *udp = (struct udp_hdr *)((char *)icmp + 28);
300
      //printf("icmp port unr %d\n", udp->dest);
301
      putchar('i');
302
    }
303
    else {
304
      //printf("icmp dst unr (code: %d)", icmp->code);
305
      putchar('i');
306
    }
307
    break;
308

    
309
  case ICMP_ECHO:
310
  default:
311
    break;
312
  }
313
}
314

    
315
static void __attribute__((unused))
316
print_arp_ip(const unsigned char ip[4])
317
{
318
  printf("%d.%d.%d.%d", ip[0], ip[1], ip[2],ip[3]);
319
}
320

    
321
static void
322
send_arp_reply(struct arp_eth_ipv4 *req, eth_mac_addr_t our_mac)
323
{
324
  struct arp_eth_ipv4 reply _AL4;
325
  reply.ar_hrd = req->ar_hrd;
326
  reply.ar_pro = req->ar_pro;
327
  reply.ar_hln = req->ar_hln;
328
  reply.ar_pln = req->ar_pln;
329
  reply.ar_op = ARPOP_REPLY;
330
  memcpy(reply.ar_sha, &our_mac, 6);
331
  memcpy(reply.ar_sip, req->ar_tip, 4);
332
  memcpy(reply.ar_tha, req->ar_sha, 6);
333
  memcpy(reply.ar_tip, req->ar_sip, 4);
334

    
335
  eth_mac_addr_t t;
336
  memcpy(t.addr, reply.ar_tha, 6);
337
  send_pkt(t, ETHERTYPE_ARP, &reply, sizeof(reply), 0, 0, 0, 0);
338
}
339

    
340

    
341
static void
342
handle_arp_packet(struct arp_eth_ipv4 *p, size_t size)
343
{
344
  if (size < sizeof(struct arp_eth_ipv4)){
345
    printf("\nhandle_arp: weird size = %d\n", (int)size);
346
    return;
347
  }
348

    
349
  if (0){
350
    printf("ar_hrd = %d\n", p->ar_hrd);
351
    printf("ar_pro = %d\n", p->ar_pro);
352
    printf("ar_hln = %d\n", p->ar_hln);
353
    printf("ar_pln = %d\n", p->ar_pln);
354
    printf("ar_op  = %d\n", p->ar_op);
355
    printf("ar_sha = "); print_mac_addr(p->ar_sha); newline();
356
    printf("ar_sip = "); print_arp_ip(p->ar_sip);    newline();
357
    printf("ar_tha = "); print_mac_addr(p->ar_tha); newline();
358
    printf("ar_tip = "); print_arp_ip(p->ar_tip);    newline();
359
  }
360

    
361
  if (p->ar_hrd != ARPHRD_ETHER
362
      || p->ar_pro != ETHERTYPE_IPV4
363
      || p->ar_hln != 6
364
      || p->ar_pln != 4)
365
    return;
366
  
367
  if (p->ar_op != ARPOP_REQUEST)
368
    return;
369

    
370
  struct ip_addr sip;
371
  struct ip_addr tip;
372

    
373
  sip.addr = get_int32(p->ar_sip);
374
  tip.addr = get_int32(p->ar_tip);
375

    
376
  if (ip_addr_eq(tip, _get_ip_addr())){        // They're looking for us...
377
    send_arp_reply(p, _get_eth_mac_addr());
378
  }
379
}
380

    
381
void
382
handle_eth_packet(uint32_t *p, size_t nlines)
383
{
384
  //print_buffer(p, nlines);
385

    
386
  int ethertype = p[3] & 0xffff;
387

    
388
  if (ethertype == ETHERTYPE_ARP){
389
    struct arp_eth_ipv4 *arp = (struct arp_eth_ipv4 *)(p + 4);
390
    handle_arp_packet(arp, nlines*sizeof(uint32_t) - 14);
391
  }
392
  else if (ethertype == ETHERTYPE_IPV4){
393
    struct ip_hdr *ip = (struct ip_hdr *)(p + 4);
394
    if (IPH_V(ip) != 4 || IPH_HL(ip) != 5)        // ignore pkts w/ bad version or options
395
      return;
396

    
397
    if (IPH_OFFSET(ip) & (IP_MF | IP_OFFMASK))        // ignore fragmented packets
398
      return;
399

    
400
    // FIXME filter on dest ip addr (should be broadcast or for us)
401

    
402
    arp_cache_update(&ip->src, (eth_mac_addr_t *)(((char *)p)+8));
403

    
404
    int protocol = IPH_PROTO(ip);
405
    int len = IPH_LEN(ip) - IP_HLEN;
406

    
407
    switch (protocol){
408
    case IP_PROTO_UDP:
409
      handle_udp_packet(ip->src, ip->dest, (struct udp_hdr *)(((char *)ip) + IP_HLEN), len);
410
      break;
411

    
412
    case IP_PROTO_ICMP:
413
      handle_icmp_packet(ip->src, ip->dest, (struct icmp_echo_hdr *)(((char *)ip) + IP_HLEN), len);
414
      break;
415

    
416
    default:        // ignore
417
      break;
418
    }
419
  }
420
  else
421
    return;        // Not ARP or IPV4, ignore
422
}
423

    
424
// ------------------------------------------------------------------------
425

    
426
void
427
print_ip(struct ip_addr ip)
428
{
429
  unsigned int t = ntohl(ip.addr);
430
  printf("%d.%d.%d.%d",
431
         (t >> 24) & 0xff,
432
         (t >> 16) & 0xff,
433
         (t >>  8) & 0xff,
434
         t & 0xff);
435
}