[image] Allow for zero embedded images
[people/asdlkf/gpxe.git] / src / drivers / net / mlx_ipoib / ipoib.c
1 /*
2   This software is available to you under a choice of one of two
3   licenses.  You may choose to be licensed under the terms of the GNU
4   General Public License (GPL) Version 2, available at
5   <http://www.fsf.org/copyleft/gpl.html>, or the OpenIB.org BSD
6   license, available in the LICENSE.TXT file accompanying this
7   software.  These details are also available at
8   <http://openib.org/license.html>.
9
10   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
11   EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
12   MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
13   NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
14   BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
15   ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
16   CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
17   SOFTWARE.
18
19   Copyright (c) 2004 Mellanox Technologies Ltd.  All rights reserved.
20 */
21
22 #include "ipoib.h"
23 #include "ib_driver.h"
24 #include "ib_mad.h"
25
26 static const __u8 arp_packet_template[] = {
27         0x00, 0x20,             /* hardware type */
28         0x08, 0x00,             /* protocol type */
29         20,                     /* hw size */
30         4,                      /* protocol size */
31         0x00, 0x00,             /* opcode */
32
33         0, 0, 0, 0, 0,
34         0, 0, 0, 0, 0,
35         0, 0, 0, 0, 0,
36         0, 0, 0, 0, 0,          /* sender's mac */
37         0, 0, 0, 0,             /* sender's IP address */
38
39         0, 0, 0, 0, 0,
40         0, 0, 0, 0, 0,
41         0, 0, 0, 0, 0,
42         0, 0, 0, 0, 0,          /* Target's mac */
43         0, 0, 0, 0              /* targets's IP address */
44 };
45
46 struct ipoib_data_st {
47         __u32 ipoib_qpn;
48         udqp_t ipoib_qph;
49         ud_av_t bcast_av;
50         cq_t snd_cqh;
51         cq_t rcv_cqh;
52         __u8 *port_gid_raw;
53 } ipoib_data;
54
55 #define NUM_MAC_ENTRIES (NUM_AVS+2)
56
57 static struct mac_xlation_st mac_tbl[NUM_MAC_ENTRIES];
58 static __u32 mac_counter = 1;
59 static __u32 youth_counter = 0;
60
61 #define EQUAL_GUIDS(g1, g2) (       \
62                         ((g1)[0]==(g2)[0]) &&   \
63                         ((g1)[1]==(g2)[1]) &&   \
64                         ((g1)[2]==(g2)[2]) &&   \
65                         ((g1)[3]==(g2)[3]) &&   \
66                         ((g1)[4]==(g2)[4]) &&   \
67                         ((g1)[5]==(g2)[5]) &&   \
68                         ((g1)[6]==(g2)[6]) &&   \
69                         ((g1)[7]==(g2)[7]) )
70
71 #define MAC_IDX(i) (((mac_tbl[i].eth_mac_lsb[0])<<16) | \
72                                         ((mac_tbl[i].eth_mac_lsb[0])<<8) |  \
73                                         (mac_tbl[i].eth_mac_lsb[0]))
74
75 static inline const void *qpn2buf(__u32 qpn, const void *buf)
76 {
77         ((__u8 *) buf)[0] = qpn >> 16;
78         ((__u8 *) buf)[1] = (qpn >> 8) & 0xff;
79         ((__u8 *) buf)[2] = qpn & 0xff;
80         return buf;
81 }
82
83 static inline __u32 buf2qpn(const void *buf)
84 {
85         __u32 qpn;
86
87         qpn = ((((__u8 *) buf)[0]) << 16) +
88             ((((__u8 *) buf)[1]) << 8) + (((__u8 *) buf)[2]);
89
90         return qpn;
91 }
92
93 static int is_bcast_mac(const char *dest)
94 {
95         int i;
96         __u8 mac = 0xff;
97
98         for (i = 0; i < 6; ++i)
99                 mac &= dest[i];
100
101         return mac == 0xff;
102 }
103
104 /* find a free entry. if not found kick
105  * another entry.
106  */
107 static int find_free_entry(void)
108 {
109         __u32 youth = 0xffffffff;
110         __u8 i, remove_idx = NUM_MAC_ENTRIES;
111
112         /* find a free entry */
113         for (i = 0; i < NUM_MAC_ENTRIES; ++i) {
114                 if (!mac_tbl[i].valid) {
115                         mac_tbl[i].valid = 1;
116                         mac_tbl[i].youth = youth_counter;
117                         youth_counter++;
118                         return i;
119                 }
120         }
121
122         for (i = 0; i < NUM_MAC_ENTRIES; ++i) {
123                 if ((mac_tbl[i].av == NULL) && (mac_tbl[i].youth < youth)) {
124                         youth = mac_tbl[i].youth;
125                         remove_idx = i;
126                 }
127         }
128
129         if (remove_idx < NUM_MAC_ENTRIES) {
130                 /* update the new youth value */
131                 mac_tbl[remove_idx].youth = youth_counter;
132                 youth_counter++;
133                 return remove_idx;
134         } else {
135                 tprintf("did not find an entry to kick");
136                 return -1;
137         }
138 }
139
140 static int find_qpn_gid(__u32 qpn, const __u8 * gid)
141 {
142         __u16 i;
143
144         for (i = 0; i < NUM_MAC_ENTRIES; ++i) {
145                 if (mac_tbl[i].valid &&
146                     (mac_tbl[i].qpn == qpn) &&
147                     !memcmp(mac_tbl[i].gid.raw, gid, 16)) {
148                         return i;
149                 }
150         }
151         return -1;
152 }
153
154 static void allocate_new_mac6(__u8 * mac_lsb)
155 {
156         __u32 eth_counter;
157
158         eth_counter = mac_counter;
159         mac_counter = (mac_counter + 1) & 0xffffff;
160
161         mac_lsb[0] = eth_counter >> 16;
162         mac_lsb[1] = eth_counter >> 8;
163         mac_lsb[2] = eth_counter & 0xff;
164         tprintf("add mac: %x:%x:%x", mac_lsb[0], mac_lsb[1], mac_lsb[2]);
165 }
166
167 static void modify_arp_reply(__u8 * eth_mac_lsb, void *data)
168 {
169         __u8 *packet;
170
171         /* skip 4 bytes */
172         packet = ((__u8 *) data) + 4;
173
174         /* modify hw type */
175         packet[0] = 0;
176         packet[1] = 1;
177
178         /* modify hw size */
179         packet[4] = 6;
180
181         /* modify sender's mac */
182         packet[8] = MLX_ETH_BYTE0;
183         packet[9] = MLX_ETH_BYTE1;
184         packet[10] = MLX_ETH_BYTE2;
185         packet[11] = eth_mac_lsb[0];
186         packet[12] = eth_mac_lsb[1];
187         packet[13] = eth_mac_lsb[2];
188
189         /* move sender's IP address */
190         memcpy(packet + 14, packet + 28, 4);
191
192         /* set target MAC - that's us */
193         packet[18] = MLX_ETH_BYTE0;
194         packet[19] = MLX_ETH_BYTE1;
195         packet[20] = MLX_ETH_BYTE2;
196         packet[21] = 0;
197         packet[22] = 0;
198         packet[23] = 0;
199
200         /* move target's IP address */
201         memcpy(packet + 24, packet + 52, 4);
202 }
203
204 static void modify_arp_request(__u8 * eth_mac_lsb, void *data)
205 {
206         __u8 *packet;
207
208         /* skip 4 bytes */
209         packet = ((__u8 *) data) + 4;
210
211         /* modify hw type */
212         packet[0] = 0;
213         packet[1] = 1;
214
215         /* modify hw size */
216         packet[4] = 6;
217
218         /* modify sender's mac */
219         packet[8] = MLX_ETH_BYTE0;
220         packet[9] = MLX_ETH_BYTE1;
221         packet[10] = MLX_ETH_BYTE2;
222         packet[11] = eth_mac_lsb[0];
223         packet[12] = eth_mac_lsb[1];
224         packet[13] = eth_mac_lsb[2];
225
226         /* move sender's IP address */
227         memcpy(packet + 14, packet + 28, 4);
228
229         /* set target MAC - that's us */
230         packet[18] = 0;
231         packet[19] = 0;
232         packet[20] = 0;
233         packet[21] = 0;
234         packet[22] = 0;
235         packet[23] = 0;
236
237         /* move target's IP address */
238         memcpy(packet + 24, packet + 52, 4);
239 }
240
241 static int handle_arp_packet(void *buf, void **out_buf_p,
242                              unsigned int *new_size_p)
243 {
244         __u16 opcode;
245         const void *p;
246         const __u8 *gid;
247         __u32 qpn;
248         int idx;
249
250         opcode = get_opcode(buf);
251         switch (opcode) {
252         case ARP_OP_REQUESET:
253         case ARP_OP_REPLY:
254                 break;
255
256         default:
257                 return -1;
258         }
259
260         p = arp_mac20_get_sender_qpn(buf);
261         qpn = buf2qpn(p);
262         gid = arp_mac20_get_sender_gid(buf);
263
264         if (!memcmp(gid, get_port_gid(), 16)) {
265                 /* my own gid */
266                 *out_buf_p = NULL;
267                 return 0;
268         }
269
270         idx = find_qpn_gid(qpn, gid);
271         if (idx == -1) {
272                 /* entry not in the table */
273                 idx = find_free_entry();
274                 if (idx == -1) {
275                         eprintf("we're in broch\n");
276                         return -1;
277                 }
278                 allocate_new_mac6(mac_tbl[idx].eth_mac_lsb);
279                 mac_tbl[idx].av = NULL; // free the av id it exists ?? !!
280                 mac_tbl[idx].qpn = qpn;
281                 memcpy(mac_tbl[idx].gid.raw, gid, 16);
282         }
283
284         if (opcode == ARP_OP_REQUESET) {
285                 modify_arp_request(mac_tbl[idx].eth_mac_lsb, buf);
286         } else {
287                 /* we want to filter possible broadcast arp
288                    replies not directed to us */
289                 p = arp_mac20_get_target_qpn(buf);
290                 qpn = buf2qpn(p);
291                 gid = arp_mac20_get_target_gid(buf);
292
293                 if ((qpn != ipoib_data.ipoib_qpn) ||
294                     (memcmp(gid, get_port_gid(), 16))) {
295                         *out_buf_p = NULL;
296                         return 0;
297                 }
298
299                 modify_arp_reply(mac_tbl[idx].eth_mac_lsb, buf);
300                 {
301                         __u8 i;
302                         tprintf("arp reply dump:\n");
303                         for (i = 4; i < 32; ++i) {
304                                 tprintf("%x: ", ((__u8 *) buf)[i]);
305                         }
306                         tprintf("\n");
307                 }
308         }
309         *out_buf_p = ((__u8 *) buf) + 4;
310         *new_size_p = 28;       /* size of eth arp packet */
311
312         tprintf("");
313
314         return 0;
315 }
316
317 static void modify_udp_csum(void *buf, __u16 size)
318 {
319         __u8 *ptr = (__u8 *) buf;
320         __u32 csum = 0;
321         __u16 chksum;
322         __u16 buf_size;
323         __u16 *tmp;
324         int i;
325
326         buf_size = (size & 1) ? size + 1 : size;
327         tmp = (__u16 *) (ptr + 12);     /* src and dst ip addresses */
328         for (i = 0; i < 4; ++i) {
329                 csum += tmp[i];
330         }
331
332         csum += 0x1100;         // udp protocol
333
334         tmp = (__u16 *) (ptr + 26);
335         tmp[0] = 0;             /* zero the checksum */
336
337         tmp = (__u16 *) (ptr + 24);
338         csum += tmp[0];
339
340         tmp = (__u16 *) (ptr + 20);
341
342         for (i = 0; i < ((buf_size - 20) >> 1); ++i) {
343                 csum += tmp[i];
344         }
345
346         chksum = ~((__u16) ((csum & 0xffff) + (csum >> 16)));
347
348         tmp = (__u16 *) (ptr + 26);
349         tmp[0] = chksum;        /* set the checksum */
350 }
351
352 static void modify_dhcp_resp(void *buf, __u16 size)
353 {
354         set_eth_hwtype(buf);
355         set_eth_hwlen(buf);
356         set_own_mac(buf);
357         modify_udp_csum(buf, size);
358 }
359
360 static void get_my_client_id(__u8 * my_client_id)
361 {
362
363         my_client_id[0] = 0;
364         qpn2buf(ipoib_data.ipoib_qpn, my_client_id + 1);
365         memcpy(my_client_id + 4, ipoib_data.port_gid_raw, 16);
366 }
367
368 static const __u8 *get_client_id(const void *buf, int len)
369 {
370         const __u8 *ptr;
371         int delta;
372
373         if (len < 268)
374                 return NULL;
375
376         /* pointer to just after magic cookie */
377         ptr = (const __u8 *)buf + 268;
378
379         /* find last client identifier option */
380         do {
381                 if (ptr[0] == 255) {
382                         /* found end of options list */
383                         return NULL;
384                 }
385
386                 if (ptr[0] == 0x3d) {
387                         /* client identifer option */
388                         return ptr + 3;
389                 }
390
391                 delta = ptr[1] + 2;
392                 ptr += delta;
393                 len -= delta;
394         } while (len > 0);
395
396         return NULL;
397 }
398
399 static int handle_ipv4_packet(void *buf, void **out_buf_p,
400                               unsigned int *new_size_p, int *is_bcast_p)
401 {
402         void *new_buf;
403         __u16 new_size;
404         __u8 msg_type;
405         __u8 my_client_id[20];
406
407         new_buf = (void *)(((__u8 *) buf) + 4);
408         new_size = (*new_size_p) - 4;
409         *out_buf_p = new_buf;
410         *new_size_p = new_size;
411
412         if (get_ip_protocl(new_buf) == IP_PROT_UDP) {
413                 __u16 udp_dst_port;
414                 const __u8 *client_id;
415
416                 udp_dst_port = get_udp_dst_port(new_buf);
417
418                 if (udp_dst_port == 67) {
419                         /* filter dhcp requests */
420                         *out_buf_p = 0;
421                         return 0;
422                 }
423
424                 if (udp_dst_port == 68) {
425                         get_my_client_id(my_client_id);
426
427                         /* packet client id */
428                         client_id = get_client_id(new_buf, new_size);
429                         if (!client_id) {
430                                 *out_buf_p = 0;
431                                 return 0;
432                         }
433
434                         if (memcmp(client_id, my_client_id, 20)) {
435                                 *out_buf_p = 0;
436                                 return 0;
437                         }
438                 }
439         }
440
441         msg_type = get_dhcp_msg_type(new_buf);
442         if ((get_ip_protocl(new_buf) == IP_PROT_UDP) &&
443             (get_udp_dst_port(new_buf) == 68) &&
444             ((msg_type == DHCP_TYPE_RESPONSE) || (msg_type == DHCP_TYPE_ACK))
445             ) {
446                 *is_bcast_p = 1;
447                 modify_dhcp_resp(new_buf, new_size);
448         }
449
450         return 0;
451 }
452
453 static int is_valid_arp(void *buf, unsigned int size)
454 {
455         __u8 *ptr = buf;
456         __u16 tmp;
457
458         if (size != 60) {
459                 eprintf("");
460                 return 0;
461         }
462         if (be16_to_cpu(*((__u16 *) ptr)) != ARP_PROT_TYPE)
463                 return 0;
464
465         if (be16_to_cpu(*((__u16 *) (ptr + 4))) != IPOIB_HW_TYPE)
466                 return 0;
467
468         if (be16_to_cpu(*((__u16 *) (ptr + 6))) != IPV4_PROT_TYPE)
469                 return 0;
470
471         if (ptr[8] != 20)       /* hw addr len */
472                 return 0;
473
474         if (ptr[9] != 4)        /* protocol len  = 4 for IP */
475                 return 0;
476
477         tmp = be16_to_cpu(*((__u16 *) (ptr + 10)));
478         if ((tmp != ARP_OP_REQUESET) && (tmp != ARP_OP_REPLY))
479                 return 0;
480
481         return 1;
482 }
483
484 static int ipoib_handle_rcv(void *buf, void **out_buf_p,
485                             unsigned int *new_size_p, int *is_bcast_p)
486 {
487         __u16 prot_type;
488         int rc;
489
490         prot_type = get_prot_type(buf);
491         switch (prot_type) {
492         case ARP_PROT_TYPE:
493                 tprintf("");
494                 if (is_valid_arp(buf, *new_size_p)) {
495                         tprintf("got valid arp");
496                         rc = handle_arp_packet(buf, out_buf_p, new_size_p);
497                         if (rc) {
498                                 eprintf("");
499                                 return rc;
500                         }
501                         if (!out_buf_p) {
502                                 tprintf("");
503                         }
504                         tprintf("arp for me");
505                         *is_bcast_p = 1;
506                         return rc;
507                 } else {
508                         tprintf("got invalid arp");
509                         *out_buf_p = NULL;
510                         return 0;
511                 }
512
513         case IPV4_PROT_TYPE:
514                 tprintf("");
515                 rc = handle_ipv4_packet(buf, out_buf_p, new_size_p, is_bcast_p);
516                 return rc;
517         }
518         eprintf("prot=0x%x", prot_type);
519         return -1;
520 }
521
522 static int is_null_mac(const __u8 * mac)
523 {
524         __u8 i, tmp = 0;
525         __u8 lmac[6];
526
527         memcpy(lmac, mac, 6);
528
529         for (i = 0; i < 6; ++i) {
530                 tmp |= lmac[i];
531         }
532
533         if (tmp == 0)
534                 return 1;
535         else
536                 return 0;
537 }
538
539 static int find_mac(const __u8 * mac)
540 {
541         int i;
542         const __u8 *tmp = mac + 3;
543
544         for (i = 0; i < NUM_MAC_ENTRIES; ++i) {
545                 tprintf("checking 0x%02x:0x%02x:0x%02x valid=%d",
546                         mac_tbl[i].eth_mac_lsb[0], mac_tbl[i].eth_mac_lsb[1],
547                         mac_tbl[i].eth_mac_lsb[2], mac_tbl[i].valid);
548                 if (mac_tbl[i].valid && !memcmp(mac_tbl[i].eth_mac_lsb, tmp, 3))
549                         return i;
550         }
551         tprintf("mac: %x:%x:%x - dumping", tmp[0], tmp[1], tmp[2]);
552         for (i = 0; i < NUM_MAC_ENTRIES; ++i) {
553                 //__u8 *gid= mac_tbl[i].gid.raw;
554                 //__u8 *m= mac_tbl[i].eth_mac_lsb;
555                 /*if (mac_tbl[i].valid) {
556                    tprintf("%d: qpn=0x%lx, "
557                    "gid=%x:%x:%x:%x:%x:%x:%x:%x:%x:%x:%x:%x:%x:%x:%x:%x, "
558                    "av=0x%lx, "
559                    "youth= %ld, "
560                    "mac=%x:%x:%x\n",
561                    i, mac_tbl[i].qpn,
562                    gid[0], gid[1], gid[2], gid[3], gid[4], gid[5], gid[6], gid[7], 
563                    gid[8], gid[9], gid[10], gid[11], gid[12], gid[13], gid[14], gid[15], 
564                    mac_tbl[i].av, mac_tbl[i].youth,
565                    m[0], m[1], m[2]); 
566                    } */
567         }
568         return -1;
569 }
570
571 static int send_bcast_packet(__u16 protocol, const void *data, __u16 size)
572 {
573         ud_send_wqe_t snd_wqe, tmp_wqe;
574         int rc;
575         int is_good;
576         void *send_buffer;
577
578         snd_wqe = alloc_send_wqe(ipoib_data.ipoib_qph);
579         if (!snd_wqe) {
580                 eprintf("");
581                 return -1;
582         }
583
584         send_buffer = get_send_wqe_buf(snd_wqe, 0);
585         *((__u32 *) send_buffer) = cpu_to_be32(protocol << 16);
586         prep_send_wqe_buf(ipoib_data.ipoib_qph, ipoib_data.bcast_av,
587                           snd_wqe, data, 4, size, 0);
588
589         rc = post_send_req(ipoib_data.ipoib_qph, snd_wqe, 1);
590         if (rc) {
591                 eprintf("");
592                 goto ex;
593         }
594
595         rc = poll_cqe_tout(ipoib_data.snd_cqh, SEND_CQE_POLL_TOUT, &tmp_wqe,
596                            &is_good);
597         if (rc) {
598                 eprintf("");
599                 goto ex;
600         }
601         if (!is_good) {
602                 eprintf("");
603                 rc = -1;
604                 goto ex;
605         }
606         if (tmp_wqe != snd_wqe) {
607                 eprintf("");
608                 rc = -1;
609                 goto ex;
610         }
611
612       ex:free_wqe(snd_wqe);
613         return rc;
614 }
615
616 static int send_ucast_packet(const __u8 * mac, __u16 protocol, const void *data,
617                              __u16 size)
618 {
619         ud_send_wqe_t snd_wqe, tmp_wqe;
620         ud_av_t av;
621         udqp_t qph;
622         __u16 dlid;
623         __u8 sl, rate;
624         int rc;
625         int i;
626         int is_good;
627
628         i = find_mac(mac);
629         if (i < 0) {
630                 tprintf("");
631                 return -1;
632         }
633
634         if (!mac_tbl[i].av) {
635                 rc = get_path_record(&mac_tbl[i].gid, &dlid, &sl, &rate);
636                 if (rc) {
637                         eprintf("");
638                         return -1;
639                 } else {
640                         tprintf("get_path_record() success dlid=0x%x", dlid);
641                 }
642
643                 /* no av - allocate one */
644                 av = alloc_ud_av();
645                 if (!av) {
646                         eprintf("");
647                         return -1;
648                 }
649                 modify_av_params(av, dlid, 1, sl, rate, &mac_tbl[i].gid,
650                                  mac_tbl[i].qpn);
651                 mac_tbl[i].av = av;
652         } else {
653                 av = mac_tbl[i].av;
654         }
655         qph = ipoib_data.ipoib_qph;
656         snd_wqe = alloc_send_wqe(qph);
657         if (!snd_wqe) {
658                 eprintf("");
659                 return -1;
660         }
661
662         *((__u32 *) get_send_wqe_buf(snd_wqe, 0)) = cpu_to_be32(protocol << 16);
663         prep_send_wqe_buf(qph, av, snd_wqe, data, 4, size, 0);
664
665         rc = post_send_req(qph, snd_wqe, 1);
666         if (rc) {
667                 eprintf("");
668                 return -1;
669         }
670
671         rc = poll_cqe_tout(ipoib_data.snd_cqh, SEND_CQE_POLL_TOUT, &tmp_wqe,
672                            &is_good);
673         if (rc) {
674                 eprintf("");
675                 goto ex;
676         }
677         if (!is_good) {
678                 eprintf("");
679                 rc = -1;
680                 goto ex;
681         }
682         if (tmp_wqe != snd_wqe) {
683                 eprintf("");
684                 rc = -1;
685                 goto ex;
686         }
687
688       ex:free_wqe(snd_wqe);
689         return rc;
690 }
691
692 static void *alloc_convert_arp6_msg(const void *data,
693                                     struct arp_packet_st *ipoib_arp)
694 {
695         void *buf;
696         const void *p1;
697         int idx;
698         __u8 qpn[3];
699
700         memcpy(ipoib_arp, arp_packet_template, sizeof arp_packet_template);
701         buf = ipoib_arp;
702
703         /* update opcode */
704         p1 = arp_mac6_get_opcode(data);
705         arp_mac20_set_opcode(p1, buf);
706
707         /* update sender ip */
708         p1 = arp_mac6_get_sender_ip(data);
709         arp_mac20_set_sender_ip(p1, buf);
710
711         /* update target ip */
712         p1 = arp_mac6_get_target_ip(data);
713         arp_mac20_set_target_ip(p1, buf);
714
715         /* update sender mac */
716         qpn2buf(ipoib_data.ipoib_qpn, qpn);
717         arp_mac20_set_sender_mac(qpn, ipoib_data.port_gid_raw, buf);
718
719         /* update target mac */
720         p1 = arp_mac6_get_target_mac(data);
721         if (!is_null_mac(p1)) {
722                 idx = find_mac(p1);
723                 if (idx == -1) {
724                         __u8 *_ptr = (__u8 *) p1;
725                         eprintf("could not find mac %x:%x:%x",
726                                 _ptr[3], _ptr[4], _ptr[5]);
727                         return NULL;
728                 }
729                 qpn2buf(mac_tbl[idx].qpn, qpn);
730                 arp_mac20_set_target_mac(qpn, mac_tbl[idx].gid.raw, buf);
731         }
732
733         return buf;
734 }
735
736 static __u16 set_client_id(__u8 * packet)
737 {
738         __u8 *ptr;
739         __u8 y[3];
740         __u16 new_size;
741
742         /* pointer to just after magic cookie */
743         ptr = packet + 268;
744
745         /* find last option */
746         do {
747                 if (ptr[0] == 255) {
748                         /* found end of options list */
749                         break;
750                 }
751                 ptr = ptr + ptr[1] + 2;
752         } while (1);
753
754         ptr[0] = 61;            /* client id option identifier */
755         ptr[1] = 21;            /* length of the option */
756         ptr[2] = IPOIB_HW_TYPE;
757         ptr[3] = 0;
758         qpn2buf(ipoib_data.ipoib_qpn, y);
759         memcpy(ptr + 4, y, 3);
760         memcpy(ptr + 7, ipoib_data.port_gid_raw, 16);
761         ptr[23] = 255;
762         new_size = (__u16) (ptr + 24 - packet);
763         if (new_size & 3) {
764                 new_size += (4 - (new_size & 3));
765         }
766         return new_size;
767 }
768
769 static __u16 calc_udp_csum(__u8 * packet)
770 {
771         __u16 *ptr;
772         int i;
773         __u32 sum = 0;
774         __u16 udp_length, udp_csum;
775
776         /* src ip, dst ip */
777         ptr = (__u16 *) (packet + 12);
778         for (i = 0; i < 4; ++i) {
779                 sum += be16_to_cpu(ptr[i]);
780         }
781
782         /* udp protocol */
783         sum += IP_PROT_UDP;
784
785         /* udp length */
786         ptr = (__u16 *) (packet + 24);
787         udp_length = be16_to_cpu(*ptr);
788         sum += udp_length;
789
790         /* udp part */
791         ptr = (__u16 *) (packet + 20);
792         do {
793                 sum += be16_to_cpu(*ptr);
794                 ptr++;
795                 udp_length -= 2;
796         } while (udp_length);
797
798         udp_csum = ~((__u16) ((sum & 0xffff) + (sum >> 16)));
799         return udp_csum;
800 }
801
802 static __u16 modify_dhcp_request(__u8 * packet, __u16 size)
803 {
804         __u16 csum, new_size;
805
806         set_hw_type(packet);
807         zero_hw_len(packet);
808         zero_chaddr(packet);
809         set_bcast_flag(packet);
810         new_size = set_client_id(packet);
811         if (new_size > size) {
812                 add_udp_len(packet, new_size - size);
813         } else
814                 new_size = size;
815         set_udp_csum(packet, 0);
816         csum = calc_udp_csum(packet);
817         set_udp_csum(packet, csum);
818         return new_size;
819 }
820
821 static __u16 copy_dhcp_message(__u8 * buf, const void *packet, __u16 size)
822 {
823         memcpy(buf, packet, size);
824         return size;
825 }
826
827 static void modify_ip_hdr(__u8 * buf, __u16 add_size)
828 {
829         __u16 *ptr, ip_csum;
830         __u16 tmp;
831         __u32 sum = 0;
832         __u8 i;
833
834         /* update ip length */
835         ptr = (__u16 *) buf;
836         tmp = be16_to_cpu(ptr[1]);
837         ptr[1] = cpu_to_be16(tmp + add_size);
838
839         ptr[5] = 0;             /* zero csum */
840         for (i = 0; i < 10; ++i) {
841                 sum += be16_to_cpu(ptr[i]);
842         }
843
844         ip_csum = ~((__u16) ((sum & 0xffff) + (sum >> 16)));
845         ptr[5] = cpu_to_be16(ip_csum);
846
847 }
848
849 static void *update_dhcp_request(const void *packet, unsigned int size,
850                                  __u16 * new_size_p)
851 {
852         __u8 ip_proto, dhcp_message_type;
853         __u16 dest_port, new_size, orig_size;
854         static __u8 dhcp_send_buffer[576];
855
856         ip_proto = get_ip_protocl_type(packet);
857         if (ip_proto != IP_PROT_UDP) {
858                 return NULL;
859         }
860
861         dest_port = get_udp_dest_port(packet);
862         if (dest_port != 0x4300 /*67 */ )
863                 return NULL;
864
865         dhcp_message_type = get_dhcp_message_type(packet);
866         if (dhcp_message_type != DHCP_TYPE_REQUEST)
867                 return NULL;
868
869         memset(dhcp_send_buffer, 0, sizeof dhcp_send_buffer);
870         orig_size = copy_dhcp_message(dhcp_send_buffer, packet, size);
871
872         new_size = modify_dhcp_request(dhcp_send_buffer, orig_size);
873         if (new_size != orig_size) {
874                 modify_ip_hdr(dhcp_send_buffer, new_size - orig_size);
875         }
876         *new_size_p = new_size;
877         return dhcp_send_buffer;
878 }
879
880 static int ipoib_send_packet(const __u8 * mac, __u16 protocol, const void *data,
881                              unsigned int size)
882 {
883         const void *packet;
884         __u16 new_size, dhcp_req_sz;
885         void *tmp;
886         int rc;
887         struct arp_packet_st ipoib_arp;
888
889         tprintf("");
890
891         if (protocol == ARP_PROT_TYPE) {
892                 /* special treatment for ARP */
893                 tmp = alloc_convert_arp6_msg(data, &ipoib_arp);
894                 if (!tmp) {
895                         eprintf("");
896                         return -1;
897                 }
898                 packet = tmp;
899                 new_size = sizeof(struct arp_packet_st);
900                 tprintf("sending arp");
901         } else {
902                 tmp = update_dhcp_request(data, size, &dhcp_req_sz);
903                 if (tmp) {
904                         /* it was a dhcp request so we use a special
905                            buffer because we may have to enlarge the size of the packet */
906                         tprintf("sending dhcp");
907                         packet = tmp;
908                         new_size = dhcp_req_sz;
909                 } else {
910                         packet = data;
911                         new_size = size;
912                         tprintf("sending packet");
913                 }
914         }
915
916         //eprintf("press key ..."); getchar();
917         if (is_bcast_mac(mac)) {
918                 tprintf("");
919                 rc = send_bcast_packet(protocol, packet, new_size);
920         } else {
921                 tprintf("");
922                 rc = send_ucast_packet(mac, protocol, packet, new_size);
923         }
924
925         return rc;
926 }
927
928 static int ipoib_read_packet(__u16 * prot_p, void *data, unsigned int *size_p,
929                              int *is_bcast_p)
930 {
931         int rc;
932         struct ib_cqe_st ib_cqe;
933         __u8 num_cqes;
934         unsigned int new_size;
935         void *buf, *out_buf;
936         __u16 prot_type;
937
938         rc = ib_poll_cq(ipoib_data.rcv_cqh, &ib_cqe, &num_cqes);
939         if (rc) {
940                 return rc;
941         }
942
943         if (num_cqes == 0) {
944                 *size_p = 0;
945                 return 0;
946         }
947
948         if (ib_cqe.is_error) {
949                 eprintf("");
950                 rc = -1;
951                 goto ex_func;
952         }
953
954         new_size = ib_cqe.count - GRH_SIZE;
955         buf = get_rcv_wqe_buf(ib_cqe.wqe, 1);
956         tprintf("buf=%lx", buf);
957         rc = ipoib_handle_rcv(buf, &out_buf, &new_size, is_bcast_p);
958         if (rc) {
959                 eprintf("");
960                 rc = -1;
961                 goto ex_func;
962         }
963         if (out_buf) {
964                 tprintf("");
965                 prot_type = get_prot_type(buf);
966                 *size_p = new_size;
967                 tprintf("new_size=%d", new_size);
968                 if (new_size > 1560) {
969                         eprintf("sizzzzzze = %d", new_size);
970                 } else {
971                         memcpy(data, out_buf, new_size);
972                 }
973                 *prot_p = prot_type;
974         } else {
975                 tprintf("skip message");
976                 *size_p = 0;
977         }
978
979       ex_func:
980         if (free_wqe(ib_cqe.wqe)) {
981                 eprintf("");
982         }
983
984         return rc;
985 }
986
987 static int ipoib_init(struct pci_device *pci)
988 {
989         int rc;
990         udqp_t qph;
991         int i;
992
993         tprintf("");
994         rc = ib_driver_init(pci, &qph);
995         if (rc)
996                 return rc;
997
998         tprintf("");
999         ipoib_data.ipoib_qph = qph;
1000         ipoib_data.ipoib_qpn = ib_get_qpn(qph);
1001
1002         if(print_info)
1003                 printf("local ipoib qpn=0x%x\n", ipoib_data.ipoib_qpn);
1004
1005         ipoib_data.bcast_av = ib_data.bcast_av;
1006         ipoib_data.port_gid_raw = ib_data.port_gid.raw;
1007         ipoib_data.snd_cqh = ib_data.ipoib_snd_cq;
1008         ipoib_data.rcv_cqh = ib_data.ipoib_rcv_cq;
1009
1010         mac_counter = 1;
1011         youth_counter = 0;
1012         for (i = 0; i < NUM_MAC_ENTRIES; ++i) {
1013                 mac_tbl[i].valid = 0;
1014                 mac_tbl[i].av = NULL;
1015         }
1016
1017         return 0;
1018 }
1019
1020 static int ipoib_close(int fw_fatal)
1021 {
1022         int rc;
1023
1024         rc = ib_driver_close(fw_fatal);
1025
1026         return rc;
1027 }