TFTP boot working
[mw/milkymist.git] / software / libnet / microudp.c
1 /*
2  * Milkymist VJ SoC (Software)
3  * Copyright (C) 2007, 2008, 2009, 2010 Sebastien Bourdeauducq
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, version 3 of the License.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
16  */
17
18 #include <stdio.h>
19 #include <crc.h>
20 #include <hw/minimac.h>
21 #include <hw/sysctl.h>
22
23 #include <net/microudp.h>
24
25 #define ETHERTYPE_ARP 0x0806
26 #define ETHERTYPE_IP  0x0800
27
28 struct ethernet_header {
29         unsigned char preamble[8];
30         unsigned char destmac[6];
31         unsigned char srcmac[6];
32         unsigned short ethertype;
33 } __attribute__((packed));
34
35 static void fill_eth_header(struct ethernet_header *h, const unsigned char *destmac, const unsigned char *srcmac, unsigned short ethertype)
36 {
37         int i;
38
39         for(i=0;i<7;i++)
40                 h->preamble[i] = 0x55;
41         h->preamble[7] = 0xd5;
42         for(i=0;i<6;i++)
43                 h->destmac[i] = destmac[i];
44         for(i=0;i<6;i++)
45                 h->srcmac[i] = srcmac[i];
46         h->ethertype = ethertype;
47 }
48
49 #define ARP_HWTYPE_ETHERNET 0x0001
50 #define ARP_PROTO_IP        0x0800
51
52 #define ARP_OPCODE_REQUEST  0x0001
53 #define ARP_OPCODE_REPLY    0x0002
54
55 struct arp_frame {
56         unsigned short hwtype;
57         unsigned short proto;
58         unsigned char hwsize;
59         unsigned char protosize;
60         unsigned short opcode;
61         unsigned char sender_mac[6];
62         unsigned int sender_ip;
63         unsigned char target_mac[6];
64         unsigned int target_ip;
65         unsigned char padding[18];
66 } __attribute__((packed));
67
68 #define IP_IPV4                 0x45
69 #define IP_DONT_FRAGMENT        0x4000
70 #define IP_TTL                  64
71 #define IP_PROTO_UDP            0x11
72
73 struct ip_header {
74         unsigned char version;
75         unsigned char diff_services;
76         unsigned short total_length;
77         unsigned short identification;
78         unsigned short fragment_offset;
79         unsigned char ttl;
80         unsigned char proto;
81         unsigned short checksum;
82         unsigned int src_ip;
83         unsigned int dst_ip;
84 } __attribute__((packed));
85
86 struct udp_header {
87         unsigned short src_port;
88         unsigned short dst_port;
89         unsigned short length;
90         unsigned short checksum;
91 } __attribute__((packed));
92
93 struct udp_frame {
94         struct ip_header ip;
95         struct udp_header udp;
96         char payload[];
97 } __attribute__((packed));
98
99 struct ethernet_frame {
100         struct ethernet_header eth_header;
101         union {
102                 struct arp_frame arp;
103                 struct udp_frame udp;
104         } contents;
105 } __attribute__((packed));
106
107 typedef union {
108         struct ethernet_frame frame;
109         unsigned char raw[1532];
110 } ethernet_buffer;
111
112
113 static int rxlen;
114 static ethernet_buffer *rxbuffer;
115 static int txlen;
116 static ethernet_buffer *txbuffer;
117
118 static void send_packet()
119 {
120         unsigned int crc;
121         
122         crc = crc32(&txbuffer->raw[8], txlen-8);
123         txbuffer->raw[txlen  ] = (crc & 0xff);
124         txbuffer->raw[txlen+1] = (crc & 0xff00) >> 8;
125         txbuffer->raw[txlen+2] = (crc & 0xff0000) >> 16;
126         txbuffer->raw[txlen+3] = (crc & 0xff000000) >> 24;
127         txlen += 4;
128         CSR_MINIMAC_TXADR = (unsigned int)txbuffer;
129         CSR_MINIMAC_TXREMAINING = txlen;
130         while(CSR_MINIMAC_TXREMAINING != 0);
131 }
132
133 static unsigned char my_mac[6];
134 static unsigned int my_ip;
135
136 /* ARP cache - one entry only */
137 static unsigned char cached_mac[6];
138 static unsigned int cached_ip;
139
140 static void process_arp()
141 {
142         if(rxlen < 68) return;
143         if(rxbuffer->frame.contents.arp.hwtype != ARP_HWTYPE_ETHERNET) return;
144         if(rxbuffer->frame.contents.arp.proto != ARP_PROTO_IP) return;
145         if(rxbuffer->frame.contents.arp.hwsize != 6) return;
146         if(rxbuffer->frame.contents.arp.protosize != 4) return;
147         if(rxbuffer->frame.contents.arp.opcode == ARP_OPCODE_REPLY) {
148                 if(rxbuffer->frame.contents.arp.sender_ip == cached_ip) {
149                         int i;
150                         for(i=0;i<6;i++)
151                                 cached_mac[i] = rxbuffer->frame.contents.arp.sender_mac[i];
152                 }
153                 return;
154         }
155         if(rxbuffer->frame.contents.arp.opcode == ARP_OPCODE_REQUEST) {
156                 if(rxbuffer->frame.contents.arp.target_ip == my_ip) {
157                         int i;
158                         
159                         fill_eth_header(&txbuffer->frame.eth_header,
160                                 rxbuffer->frame.contents.arp.sender_mac,
161                                 my_mac,
162                                 ETHERTYPE_ARP);
163                         txlen = 68;
164                         txbuffer->frame.contents.arp.hwtype = ARP_HWTYPE_ETHERNET;
165                         txbuffer->frame.contents.arp.proto = ARP_PROTO_IP;
166                         txbuffer->frame.contents.arp.hwsize = 6;
167                         txbuffer->frame.contents.arp.protosize = 4;
168                         txbuffer->frame.contents.arp.opcode = ARP_OPCODE_REPLY;
169                         txbuffer->frame.contents.arp.sender_ip = my_ip;
170                         for(i=0;i<6;i++)
171                                 txbuffer->frame.contents.arp.sender_mac[i] = my_mac[i];
172                         txbuffer->frame.contents.arp.target_ip = rxbuffer->frame.contents.arp.sender_ip;
173                         for(i=0;i<6;i++)
174                                 txbuffer->frame.contents.arp.target_mac[i] = rxbuffer->frame.contents.arp.sender_mac[i];
175                         send_packet();
176                 }
177                 return;
178         }
179 }
180
181 static const unsigned char broadcast[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
182
183 int microudp_arp_resolve(unsigned int ip)
184 {
185         int i;
186         int tries;
187         int timeout;
188
189         if(cached_ip == ip) {
190                 for(i=0;i<6;i++)
191                         if(cached_mac[i]) return 1;
192         }
193         cached_ip = ip;
194         for(i=0;i<6;i++)
195                 cached_mac[i] = 0;
196
197         for(tries=0;tries<5;tries++) {
198                 /* Send an ARP request */
199                 fill_eth_header(&txbuffer->frame.eth_header,
200                                 broadcast,
201                                 my_mac,
202                                 ETHERTYPE_ARP);
203                 txlen = 68;
204                 txbuffer->frame.contents.arp.hwtype = ARP_HWTYPE_ETHERNET;
205                 txbuffer->frame.contents.arp.proto = ARP_PROTO_IP;
206                 txbuffer->frame.contents.arp.hwsize = 6;
207                 txbuffer->frame.contents.arp.protosize = 4;
208                 txbuffer->frame.contents.arp.opcode = ARP_OPCODE_REQUEST;
209                 txbuffer->frame.contents.arp.sender_ip = my_ip;
210                 for(i=0;i<6;i++)
211                         txbuffer->frame.contents.arp.sender_mac[i] = my_mac[i];
212                 txbuffer->frame.contents.arp.target_ip = ip;
213                 for(i=0;i<6;i++)
214                         txbuffer->frame.contents.arp.target_mac[i] = 0;
215                 send_packet();
216
217                 /* Do we get a reply ? */
218                 for(timeout=0;timeout<2000000;timeout++) {
219                         microudp_service();
220                         for(i=0;i<6;i++)
221                                 if(cached_mac[i]) return 1;
222                 }
223         }
224
225         return 0;
226 }
227
228 static unsigned short ip_checksum(unsigned int r, void *buffer, unsigned int length, int complete)
229 {
230         unsigned char *ptr;
231         int i;
232
233         ptr = (unsigned char *)buffer;
234         length >>= 1;
235
236         for(i=0;i<length;i++)
237                 r += ((unsigned int)(ptr[2*i]) << 8)|(unsigned int)(ptr[2*i+1]) ;
238
239         /* Add overflows */
240         while(r >> 16)
241                 r = (r & 0xffff) + (r >> 16);
242
243         if(complete) {
244                 r = ~r;
245                 r &= 0xffff;
246                 if(r == 0) r = 0xffff;
247         }
248         return r;
249 }
250
251 void *microudp_get_tx_buffer()
252 {
253         return txbuffer->frame.contents.udp.payload;
254 }
255
256 struct pseudo_header {
257         unsigned int src_ip;
258         unsigned int dst_ip;
259         unsigned char zero;
260         unsigned char proto;
261         unsigned short length;
262 } __attribute__((packed));
263
264 int microudp_send(unsigned short src_port, unsigned short dst_port, unsigned int length)
265 {
266         struct pseudo_header h;
267         unsigned int r;
268         
269         if((cached_mac[0] == 0) && (cached_mac[1] == 0) && (cached_mac[2] == 0)
270                 && (cached_mac[3] == 0) && (cached_mac[4] == 0) && (cached_mac[5] == 0))
271                 return 0;
272
273         txlen = length + sizeof(struct ethernet_header) + sizeof(struct udp_frame) + 8;
274         if(txlen < 72) txlen = 72;
275         
276         fill_eth_header(&txbuffer->frame.eth_header,
277                 cached_mac,
278                 my_mac,
279                 ETHERTYPE_IP);
280         
281         txbuffer->frame.contents.udp.ip.version = IP_IPV4;
282         txbuffer->frame.contents.udp.ip.diff_services = 0;
283         txbuffer->frame.contents.udp.ip.total_length = length + sizeof(struct udp_frame);
284         txbuffer->frame.contents.udp.ip.identification = 0;
285         txbuffer->frame.contents.udp.ip.fragment_offset = IP_DONT_FRAGMENT;
286         txbuffer->frame.contents.udp.ip.ttl = IP_TTL;
287         h.proto = txbuffer->frame.contents.udp.ip.proto = IP_PROTO_UDP;
288         txbuffer->frame.contents.udp.ip.checksum = 0;
289         h.src_ip = txbuffer->frame.contents.udp.ip.src_ip = my_ip;
290         h.dst_ip = txbuffer->frame.contents.udp.ip.dst_ip = cached_ip;
291         txbuffer->frame.contents.udp.ip.checksum = ip_checksum(0, &txbuffer->frame.contents.udp.ip,
292                 sizeof(struct ip_header), 1);
293
294         txbuffer->frame.contents.udp.udp.src_port = src_port;
295         txbuffer->frame.contents.udp.udp.dst_port = dst_port;
296         h.length = txbuffer->frame.contents.udp.udp.length = length + sizeof(struct udp_header);
297         txbuffer->frame.contents.udp.udp.checksum = 0;
298
299         h.zero = 0;
300         r = ip_checksum(0, &h, sizeof(struct pseudo_header), 0);
301         if(length & 1) {
302                 txbuffer->frame.contents.udp.payload[length] = 0;
303                 length++;
304         }
305         r = ip_checksum(r, &txbuffer->frame.contents.udp.udp,
306                 sizeof(struct udp_header)+length, 1);
307         txbuffer->frame.contents.udp.udp.checksum = r;
308         
309         send_packet();
310
311         return 1;
312 }
313
314 static udp_callback rx_callback;
315
316 static void process_ip()
317 {
318         if(rxlen < (sizeof(struct ethernet_header)+sizeof(struct udp_frame))) return;
319         /* We don't verify UDP and IP checksums and rely on the Ethernet checksum solely */
320         if(rxbuffer->frame.contents.udp.ip.version != IP_IPV4) return;
321         if(rxbuffer->frame.contents.udp.ip.diff_services != 0) return;
322         if(rxbuffer->frame.contents.udp.ip.total_length < sizeof(struct udp_frame)) return;
323         if(rxbuffer->frame.contents.udp.ip.fragment_offset != IP_DONT_FRAGMENT) return;
324         if(rxbuffer->frame.contents.udp.ip.proto != IP_PROTO_UDP) return;
325         if(rxbuffer->frame.contents.udp.ip.dst_ip != my_ip) return;
326         if(rxbuffer->frame.contents.udp.udp.length < sizeof(struct udp_header)) return;
327
328         if(rx_callback)
329                 rx_callback(rxbuffer->frame.contents.udp.ip.src_ip, rxbuffer->frame.contents.udp.udp.src_port, rxbuffer->frame.contents.udp.udp.dst_port, rxbuffer->frame.contents.udp.payload, rxbuffer->frame.contents.udp.udp.length-sizeof(struct udp_header));
330 }
331
332 void microudp_set_callback(udp_callback callback)
333 {
334         rx_callback = callback;
335 }
336
337 static void process_frame()
338 {
339         int i;
340         unsigned int received_crc;
341         unsigned int computed_crc;
342
343         for(i=0;i<7;i++)
344                 if(rxbuffer->frame.eth_header.preamble[i] != 0x55) return;
345         if(rxbuffer->frame.eth_header.preamble[7] != 0xd5) return;
346         received_crc = ((unsigned int)rxbuffer->raw[rxlen-1] << 24)
347                 |((unsigned int)rxbuffer->raw[rxlen-2] << 16)
348                 |((unsigned int)rxbuffer->raw[rxlen-3] <<  8)
349                 |((unsigned int)rxbuffer->raw[rxlen-4]);
350         computed_crc = crc32(&rxbuffer->raw[8], rxlen-12);
351         if(received_crc != computed_crc) return;
352
353         rxlen -= 4; /* strip CRC here to be consistent with TX */
354         if(rxbuffer->frame.eth_header.ethertype == ETHERTYPE_ARP) process_arp();
355         else if(rxbuffer->frame.eth_header.ethertype == ETHERTYPE_IP) process_ip();
356 }
357
358 void microudp_start(unsigned char *macaddr, unsigned int ip, void *buffers)
359 {
360         int i;
361         char *_buffers = (char *)buffers;
362
363         rxbuffer = (ethernet_buffer *)_buffers;
364         txbuffer = (ethernet_buffer *)(_buffers + sizeof(ethernet_buffer));
365
366         for(i=0;i<6;i++)
367                 my_mac[i] = macaddr[i];
368         my_ip = ip;
369
370         cached_ip = 0;
371         for(i=0;i<6;i++)
372                 cached_mac[i] = 0;
373
374         rx_callback = (udp_callback)0;
375         
376         CSR_MINIMAC_ADDR0 = (unsigned int)rxbuffer;
377         CSR_MINIMAC_STATE0 = MINIMAC_STATE_LOADED;
378         CSR_MINIMAC_SETUP = 0;
379 }
380
381 void microudp_service()
382 {
383         if(CSR_MINIMAC_STATE0 == MINIMAC_STATE_PENDING) {
384                 asm volatile( /* Invalidate Level-1 data cache */
385                         "wcsr DCC, r0\n"
386                         "nop\n"
387                 );
388                 rxlen = CSR_MINIMAC_COUNT0;
389                 process_frame();
390                 CSR_MINIMAC_STATE0 = MINIMAC_STATE_LOADED;
391         }
392         CSR_MINIMAC_SETUP = 0;
393 }
394
395 void microudp_shutdown()
396 {
397         CSR_MINIMAC_STATE0 = MINIMAC_STATE_EMPTY;
398         CSR_MINIMAC_TXREMAINING = 0;
399         /* This transfer should be last. It will make the CPU request the Wishbone bus,
400          * and therefore, when it completes,
401          * it makes sure that any ongoing DMA bus transaction is finished.
402          */
403         CSR_MINIMAC_SETUP = MINIMAC_SETUP_RXRST|MINIMAC_SETUP_TXRST;
404 }