diff options
Diffstat (limited to 'firmware/microblaze/lib/net_common.c')
| -rw-r--r-- | firmware/microblaze/lib/net_common.c | 449 |
1 files changed, 449 insertions, 0 deletions
diff --git a/firmware/microblaze/lib/net_common.c b/firmware/microblaze/lib/net_common.c new file mode 100644 index 000000000..6c9509c92 --- /dev/null +++ b/firmware/microblaze/lib/net_common.c @@ -0,0 +1,449 @@ +/* -*- c -*- */ +/* + * Copyright 2009,2010 Ettus Research LLC + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +#ifdef HAVE_CONFIG_H +#include <config.h> +#endif +#include "net_common.h" +#include "banal.h" +#include <hal_io.h> +#include <buffer_pool.h> +#include <memory_map.h> +#include <memcpy_wa.h> +#include <ethernet.h> +#include <net/padded_eth_hdr.h> +#include <lwip/ip.h> +#include <lwip/udp.h> +#include <lwip/icmp.h> +#include <stdlib.h> +#include <nonstdio.h> +#include "arp_cache.h" +#include "if_arp.h" +#include <ethertype.h> +#include <string.h> + + +int cpu_tx_buf_dest_port = PORT_ETH; + +// If this is non-zero, this dbsm could be writing to the ethernet +dbsm_t *ac_could_be_sending_to_eth; + +static inline bool +ip_addr_eq(const struct ip_addr a, const struct ip_addr b) +{ + return a.addr == b.addr; +} + +// ------------------------------------------------------------------------ + +static eth_mac_addr_t _local_mac_addr; +void register_mac_addr(const eth_mac_addr_t *mac_addr){ + _local_mac_addr = *mac_addr; +} + +static struct ip_addr _local_ip_addr; +void register_ip_addr(const struct ip_addr *ip_addr){ + _local_ip_addr = *ip_addr; +} + +//------------------------------------------------------------------------- + +#define MAX_UDP_LISTENERS 6 + +struct listener_entry { + unsigned short port; + udp_receiver_t rcvr; +}; + +static struct listener_entry listeners[MAX_UDP_LISTENERS]; + +static struct listener_entry * +find_listener_by_port(unsigned short port) +{ + for (int i = 0; i < MAX_UDP_LISTENERS; i++){ + if (port == listeners[i].port) + return &listeners[i]; + } + return 0; +} + +static struct listener_entry * +find_free_listener(void) +{ + for (int i = 0; i < MAX_UDP_LISTENERS; i++){ + if (listeners[i].rcvr == 0) + return &listeners[i]; + } + abort(); +} + +void +register_udp_listener(int port, udp_receiver_t rcvr) +{ + struct listener_entry *lx = find_listener_by_port(port); + if (lx) + lx->rcvr = rcvr; + else { + lx = find_free_listener(); + lx->port = port; + lx->rcvr = rcvr; + } +} + +// ------------------------------------------------------------------------ + + +/*! + * low level routine to assembly an ethernet frame and send it. + * + * \param dst destination mac address + * \param ethertype ethertype field + * \param buf0 first part of data + * \param len0 length of first part of data + * \param buf1 second part of data + * \param len1 length of second part of data + * \param buf2 third part of data + * \param len2 length of third part of data + */ +static void +send_pkt(eth_mac_addr_t dst, int ethertype, + const void *buf0, size_t len0, + const void *buf1, size_t len1, + const void *buf2, size_t len2) +{ + // Wait for buffer to become idle + // FIXME can this ever not be ready? + + //hal_set_leds(LED_BUF_BUSY, LED_BUF_BUSY); + while((buffer_pool_status->status & BPS_IDLE(CPU_TX_BUF)) == 0) + ; + //hal_set_leds(0, LED_BUF_BUSY); + + // Assemble the header + padded_eth_hdr_t ehdr; + ehdr.pad = 0; + ehdr.dst = dst; + ehdr.src = _local_mac_addr; + ehdr.ethertype = ethertype; + + uint32_t *p = buffer_ram(CPU_TX_BUF); + + // Copy the pieces into the buffer + *p++ = 0x0; // slow path + memcpy_wa(p, &ehdr, sizeof(ehdr)); // 4 lines + p += sizeof(ehdr)/sizeof(uint32_t); + + + // FIXME modify memcpy_wa to do read/modify/write if reqd + + if (len0 && ((len0 & 0x3) || (intptr_t) buf0 & 0x3)) + printf("send_pkt: bad alignment of len0 and/or buf0\n"); + + if (len1 && ((len1 & 0x3) || (intptr_t) buf1 & 0x3)) + printf("send_pkt: bad alignment of len1 and/or buf1\n"); + + if (len2 && ((len2 & 0x3) || (intptr_t) buf2 & 0x3)) + printf("send_pkt: bad alignment of len2 and/or buf2\n"); + + if (len0){ + memcpy_wa(p, buf0, len0); + p += len0/sizeof(uint32_t); + } + if (len1){ + memcpy_wa(p, buf1, len1); + p += len1/sizeof(uint32_t); + } + if (len2){ + memcpy_wa(p, buf2, len2); + p += len2/sizeof(uint32_t); + } + + size_t total_len = (p - buffer_ram(CPU_TX_BUF)) * sizeof(uint32_t); + if (total_len < 60) // ensure that we don't try to send a short packet + total_len = 60; + + // wait until nobody else is sending to the ethernet + if (ac_could_be_sending_to_eth){ + //hal_set_leds(LED_ETH_BUSY, LED_ETH_BUSY); + dbsm_wait_for_opening(ac_could_be_sending_to_eth); + //hal_set_leds(0x0, LED_ETH_BUSY); + } + + if (0){ + printf("send_pkt to port %d, len = %d\n", + cpu_tx_buf_dest_port, (int) total_len); + print_buffer(buffer_ram(CPU_TX_BUF), total_len/4); + } + + // fire it off + bp_send_from_buf(CPU_TX_BUF, cpu_tx_buf_dest_port, 1, 0, total_len/4); + + // wait for it to complete (not long, it's a small pkt) + while((buffer_pool_status->status & (BPS_DONE(CPU_TX_BUF) | BPS_ERROR(CPU_TX_BUF))) == 0) + ; + + bp_clear_buf(CPU_TX_BUF); +} + +unsigned int +chksum_buffer(unsigned short *buf, int nshorts, unsigned int initial_chksum) +{ + unsigned int chksum = initial_chksum; + for (int i = 0; i < nshorts; i++) + CHKSUM(buf[i], &chksum); + + return chksum; +} + + +void +send_ip_pkt(struct ip_addr dst, int protocol, + const void *buf0, size_t len0, + const void *buf1, size_t len1) +{ + int ttl = 32; + + struct ip_hdr ip; + IPH_VHLTOS_SET(&ip, 4, 5, 0); + IPH_LEN_SET(&ip, IP_HLEN + len0 + len1); + IPH_ID_SET(&ip, 0); + IPH_OFFSET_SET(&ip, IP_DF); /* don't fragment */ + ip._ttl_proto = (ttl << 8) | (protocol & 0xff); + ip._chksum = 0; + ip.src = _local_ip_addr; + ip.dest = dst; + + ip._chksum = ~chksum_buffer((unsigned short *) &ip, + sizeof(ip)/sizeof(short), 0); + + eth_mac_addr_t dst_mac; + bool found = arp_cache_lookup_mac(&ip.dest, &dst_mac); + if (!found){ + printf("net_common: failed to hit cache looking for "); + print_ip(ip.dest); + newline(); + return; + } + + send_pkt(dst_mac, ETHERTYPE_IPV4, + &ip, sizeof(ip), buf0, len0, buf1, len1); +} + +void +send_udp_pkt(int src_port, struct socket_address dst, + const void *buf, size_t len) +{ + struct udp_hdr udp _AL4; + udp.src = src_port; + udp.dest = dst.port; + udp.len = UDP_HLEN + len; + udp.chksum = 0; + + send_ip_pkt(dst.addr, IP_PROTO_UDP, + &udp, sizeof(udp), buf, len); +} + +static void +handle_udp_packet(struct ip_addr src_ip, struct ip_addr dst_ip, + struct udp_hdr *udp, size_t len) +{ + if (len != udp->len){ + printf("UDP inconsistent lengths: %d %d\n", (int)len, udp->len); + return; + } + + unsigned char *payload = ((unsigned char *) udp) + UDP_HLEN; + int payload_len = len - UDP_HLEN; + + if (0){ + printf("\nUDP: src = %d dst = %d len = %d\n", + udp->src, udp->dest, udp->len); + + //print_bytes(0, payload, payload_len); + } + + struct listener_entry *lx = find_listener_by_port(udp->dest); + if (lx){ + struct socket_address src = make_socket_address(src_ip, udp->src); + struct socket_address dst = make_socket_address(dst_ip, udp->dest); + lx->rcvr(src, dst, payload, payload_len); + } +} + +static void +handle_icmp_packet(struct ip_addr src, struct ip_addr dst, + struct icmp_echo_hdr *icmp, size_t len) +{ + switch (icmp->type){ + case ICMP_DUR: // Destinatino Unreachable + //stop_streaming(); //FIXME + if (icmp->code == ICMP_DUR_PORT){ // port unreachable + //struct udp_hdr *udp = (struct udp_hdr *)((char *)icmp + 28); + //printf("icmp port unr %d\n", udp->dest); + putchar('i'); + } + else { + //printf("icmp dst unr (code: %d)", icmp->code); + putchar('i'); + } + break; + + case ICMP_ECHO:{ + struct icmp_echo_hdr echo_reply; + echo_reply.type = 0; + echo_reply.code = 0; + echo_reply.chksum = 0; + echo_reply.id = icmp->id; + echo_reply.seqno = icmp->seqno; + echo_reply.chksum = ~chksum_buffer( + (unsigned short *)&echo_reply, + sizeof(echo_reply)/sizeof(short), + 0); + send_ip_pkt( + src, IP_PROTO_ICMP, &echo_reply, sizeof(echo_reply), + ((uint8_t*)icmp) + sizeof(struct icmp_echo_hdr), + len - sizeof(struct icmp_echo_hdr) + ); + break; + } + default: + break; + } +} + +static void __attribute__((unused)) +print_arp_ip(const unsigned char ip[4]) +{ + printf("%d.%d.%d.%d", ip[0], ip[1], ip[2],ip[3]); +} + +static void +send_arp_reply(struct arp_eth_ipv4 *req, eth_mac_addr_t our_mac) +{ + struct arp_eth_ipv4 reply _AL4; + reply.ar_hrd = req->ar_hrd; + reply.ar_pro = req->ar_pro; + reply.ar_hln = req->ar_hln; + reply.ar_pln = req->ar_pln; + reply.ar_op = ARPOP_REPLY; + memcpy(reply.ar_sha, &our_mac, 6); + memcpy(reply.ar_sip, req->ar_tip, 4); + memcpy(reply.ar_tha, req->ar_sha, 6); + memcpy(reply.ar_tip, req->ar_sip, 4); + + eth_mac_addr_t t; + memcpy(t.addr, reply.ar_tha, 6); + send_pkt(t, ETHERTYPE_ARP, &reply, sizeof(reply), 0, 0, 0, 0); +} + + +static void +handle_arp_packet(struct arp_eth_ipv4 *p, size_t size) +{ + if (size < sizeof(struct arp_eth_ipv4)){ + printf("\nhandle_arp: weird size = %d\n", (int)size); + return; + } + + if (0){ + printf("ar_hrd = %d\n", p->ar_hrd); + printf("ar_pro = %d\n", p->ar_pro); + printf("ar_hln = %d\n", p->ar_hln); + printf("ar_pln = %d\n", p->ar_pln); + printf("ar_op = %d\n", p->ar_op); + printf("ar_sha = "); print_mac_addr(p->ar_sha); newline(); + printf("ar_sip = "); print_arp_ip(p->ar_sip); newline(); + printf("ar_tha = "); print_mac_addr(p->ar_tha); newline(); + printf("ar_tip = "); print_arp_ip(p->ar_tip); newline(); + } + + if (p->ar_hrd != ARPHRD_ETHER + || p->ar_pro != ETHERTYPE_IPV4 + || p->ar_hln != 6 + || p->ar_pln != 4) + return; + + if (p->ar_op != ARPOP_REQUEST) + return; + + struct ip_addr sip; + struct ip_addr tip; + + sip.addr = get_int32(p->ar_sip); + tip.addr = get_int32(p->ar_tip); + + if (ip_addr_eq(tip, _local_ip_addr)){ // They're looking for us... + send_arp_reply(p, _local_mac_addr); + } +} + +void +handle_eth_packet(uint32_t *p, size_t nlines) +{ + //print_buffer(p, nlines); + + int ethertype = p[3] & 0xffff; + + if (ethertype == ETHERTYPE_ARP){ + struct arp_eth_ipv4 *arp = (struct arp_eth_ipv4 *)(p + 4); + handle_arp_packet(arp, nlines*sizeof(uint32_t) - 14); + } + else if (ethertype == ETHERTYPE_IPV4){ + struct ip_hdr *ip = (struct ip_hdr *)(p + 4); + if (IPH_V(ip) != 4 || IPH_HL(ip) != 5) // ignore pkts w/ bad version or options + return; + + if (IPH_OFFSET(ip) & (IP_MF | IP_OFFMASK)) // ignore fragmented packets + return; + + // FIXME filter on dest ip addr (should be broadcast or for us) + + arp_cache_update(&ip->src, (eth_mac_addr_t *)(((char *)p)+8)); + + int protocol = IPH_PROTO(ip); + int len = IPH_LEN(ip) - IP_HLEN; + + switch (protocol){ + case IP_PROTO_UDP: + handle_udp_packet(ip->src, ip->dest, (struct udp_hdr *)(((char *)ip) + IP_HLEN), len); + break; + + case IP_PROTO_ICMP: + handle_icmp_packet(ip->src, ip->dest, (struct icmp_echo_hdr *)(((char *)ip) + IP_HLEN), len); + break; + + default: // ignore + break; + } + } + else + return; // Not ARP or IPV4, ignore +} + +// ------------------------------------------------------------------------ + +void +print_ip(struct ip_addr ip) +{ + unsigned int t = ntohl(ip.addr); + printf("%d.%d.%d.%d", + (t >> 24) & 0xff, + (t >> 16) & 0xff, + (t >> 8) & 0xff, + t & 0xff); +} |