net/netpacket.c

   1 #include <errno.h>
   2 #include <stdio.h>
   3 #include <string.h>
   4 #include <endianess.h>
   5 #include <bip_buf.h>
   6 #include <net/netpacket.h>
   7
   8 //#define NETPACKET_DEBUG
   9
  10 /* these are just wrappers and handlers for memory etc.
  11  * should contain no code that talks to hw
  12  */
  13
  14
  15 #ifdef NETPACKET_DEBUG
  16 #define dprintf printf
  17 #else
  18 #define dprintf(...)
  19 #endif
  20
  21 // will not work for odd lengths
  22 u16 ip_csum(u8 *src, int len)
  23 {
  24         unsigned csum = 0;
  25         int i;
  26
  27         for (i=0; i<len; i+=2)
  28                 csum += (src[i]<<8) + src[i+1];
  29
  30         csum = (csum&0xffff)+(csum>>16);
  31         csum = (csum&0xffff)+(csum>>16); /* in some cases it overflows again */
  32         return ~csum;
  33 }
  34
  35 const static u8 ip_template[] = { 0x45, 0, 0,0, 0,0, 0,0, 0x20 };
  36
  37 /*
  38  * netpacket_prepare_ip
  39  * Can fail if IP is not yet known. In this case it sends an arp request.
  40  * Then fails. User should resubmit the packet in a second or so.
  41  */
  42 int netpacket_prepare_ip(struct netpacket *packet, const ip_t dest, u16 payload_len)
  43 {
  44         struct ip_packet *ip = &packet->ip;
  45         struct ethernet_frame *ethernet = &packet->ethernet;
  46
  47         memcpy(packet->ip.raw, ip_template, sizeof(ip_template));
  48         ip->protocol = IP_PROT_UDP;
  49         put_be16(ip->len, payload_len + sizeof(struct ip_packet));
  50         ip->hcsum[0] = ip->hcsum[1] = 0;
  51         memcpy(ip->src, netconfig.ip, sizeof(ip_t));
  52         memcpy(ip->dest, dest, sizeof(ip_t));
  53
  54         u16 csum = ip_csum(packet->ip.raw, 20);
  55         ip->hcsum[0] = csum>>8;
  56         ip->hcsum[1] = csum;
  57
  58         memcpy(ethernet->src, netconfig.mac, sizeof(mac_t));
  59         if (arp_ip2mac(ethernet->dest, dest) < 0) {
  60                 netpacket_alloc_discard();
  61                 int ret = arp_request(dest);
  62                 /* now we must fail the current packet, since we can't send before getting arp reply */
  63                 printf("%s: mac not cached, arp request: %d\n", __func__, ret);
  64                 return -EAGAIN;
  65         }
  66
  67         put_be16(ethernet->type, ETHERTYPE_IP);
  68         return 0;
  69 }
  70
  71 int netpacket_prepare_udp(struct netpacket *packet, const ip_t dest, u16 sport, u16 dport, u16 payload_len)
  72 {
  73         int ret;
  74
  75         ret = netpacket_prepare_ip(packet, dest, payload_len+sizeof(struct udp_packet));
  76         if (ret)
  77                 return ret;
  78
  79         put_be16(packet->udp.sport, sport);
  80         put_be16(packet->udp.dport, dport);
  81         put_be16(packet->udp.len, payload_len+sizeof(struct udp_packet));
  82         put_be16(packet->udp.checksum, 0); /* it is optional */
  83
  84         return 0;
  85 }
  86
  87 static struct netpacket *netpacket_alloc_udp(int payloadlen)
  88 {
  89         return netpacket_alloc(offsetof(struct netpacket, udp) + sizeof(struct udp_packet) + payloadlen);
  90 }
  91
  92 int netpacket_alloc_and_prepare_udp(struct netpacket **_packet, const ip_t dest, u16 sport, u16 dport, u16 len)
  93 {
  94         struct netpacket *packet = netpacket_alloc_udp(len);
  95         if (!packet)
  96                 return -ENOMEM;
  97         if (netpacket_prepare_udp(packet, dest, sport, dport, len) < 0) {
  98                 /* no need to deallocate, since we haven't called commit yet */
  99                 return -EAGAIN;
 100         }
 101         *_packet = packet;
 102         return 0;
 103 }
 104
 105
 106 static u8 netpacket_buf_data[1024];
 107 static struct bip_buf netpacket_buf;
 108 static int netpacket_in_alloc; // XXX consider just having a spinlock - code shouldn't block anyway
 109 struct netpacket *netpacket_alloc(unsigned len)
 110 {
 111         if (netpacket_in_alloc) {
 112                 dprintf("%s(%d): in alloc!\n", __func__, len);
 113                 return NULL;
 114         }
 115
 116         if (!netpacket_buf.data) // XXX i don't like this
 117                 bip_buf_init(&netpacket_buf, netpacket_buf_data, sizeof(netpacket_buf_data));
 118
 119         struct netpacket *packet = bip_buf_alloc(&netpacket_buf, len);
 120         dprintf("%s(%d): %p\n", __func__, len, packet);
 121         if (packet) {
 122                 packet->len = len;
 123                 netpacket_in_alloc = 1;
 124         }
 125         return packet;
 126 }
 127
 128 void netpacket_alloc_discard(void)
 129 {
 130         dprintf("%s\n", __func__);
 131         netpacket_in_alloc = 0;
 132 }
 133
 134 int netpacket_send(struct netpacket *packet)
 135 {
 136         printf("TX ");
 137         netpacket_print(packet);
 138         bip_buf_alloc_commit(&netpacket_buf, packet->len);
 139         dprintf("%s(%d)\n", __func__, packet->len);
 140         netpacket_in_alloc = 0;
 141         return 0;
 142 }
 143
 144 struct netpacket* netpacket_getdata(void)
 145 {
 146         return bip_buf_getdata(&netpacket_buf);
 147 }
 148
 149 void netpacket_free(struct netpacket *packet)
 150 {
 151         bip_buf_free(&netpacket_buf, packet->len);
 152         dprintf("%s(%d)\n", __func__, packet->len);
 153 }
 154
 155 void netpacket_print(struct netpacket *packet)
 156 {
 157         u16 type = get_be16(packet->ethernet.type);
 158
 159         printf("ethernet: type:%x ", type);
 160         if (type == ETHERTYPE_ARP) {
 161                 printf("ARP %d", packet->arp.oper[1]);
 162         } else if (type == ETHERTYPE_IP) {
 163                 u8 *ip = &packet->ip.src[0];
 164                 printf("TCP ");
 165                 printf("%d.%d.%d.%d ", ip[0], ip[1], ip[2], ip[3]);
 166                 ip = &packet->ip.dest[0];
 167                 printf("%d.%d.%d.%d ", ip[0], ip[1], ip[2], ip[3]);
 168
 169                 int prot = packet->ip.protocol;
 170                 printf("protocol: %d ", prot);
 171                 if (prot == IP_PROT_UDP) {
 172                         printf("UDP %d %d", get_be16(packet->udp.sport), get_be16(packet->udp.dport));
 173                 }
 174         }
 175         printf("\n");
 176 #ifdef DEBUG_NETPACKET
 177         int i;
 178         for (i=0; i<len; i++) {
 179                 if (i == 14 || i == 14+20)
 180                         printf(" |");
 181                 printf(" %02x", packet->ethernet.raw[i]);
 182         }
 183         printf("\n");
 184 #endif
 185 }