drivers/staging/rtl8188eu/core/rtw_br_ext.c

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
   4  *
   5  * This program is free software; you can redistribute it and/or modify it
   6  * under the terms of version 2 of the GNU General Public License as
   7  * published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful, but WITHOUT
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  11  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
  12  * more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along with
  15  * this program; if not, write to the Free Software Foundation, Inc.,
  16  * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
  17  *
  18  *
  19  ******************************************************************************/
  20 #define _RTW_BR_EXT_C_
  21
  22 #include <linux/if_arp.h>
  23 #include <net/ip.h>
  24 #include <net/ipx.h>
  25 #include <linux/atalk.h>
  26 #include <linux/udp.h>
  27 #include <linux/if_pppox.h>
  28
  29 #include <drv_types.h>
  30 #include "rtw_br_ext.h"
  31 #include <usb_osintf.h>
  32 #include <recv_osdep.h>
  33
  34 #ifndef csum_ipv6_magic
  35 #include <net/ip6_checksum.h>
  36 #endif
  37
  38 #include <linux/ipv6.h>
  39 #include <linux/icmpv6.h>
  40 #include <net/ndisc.h>
  41 #include <net/checksum.h>
  42
  43 #define NAT25_IPV4              01
  44 #define NAT25_IPV6              02
  45 #define NAT25_IPX               03
  46 #define NAT25_APPLE             04
  47 #define NAT25_PPPOE             05
  48
  49 #define RTL_RELAY_TAG_LEN (ETH_ALEN)
  50 #define TAG_HDR_LEN             4
  51
  52 #define MAGIC_CODE              0x8186
  53 #define MAGIC_CODE_LEN  2
  54 #define WAIT_TIME_PPPOE 5       /*  waiting time for pppoe server in sec */
  55
  56 /*-----------------------------------------------------------------
  57   How database records network address:
  58            0    1    2    3    4    5    6    7    8    9   10
  59         |----|----|----|----|----|----|----|----|----|----|----|
  60   IPv4  |type|                             |      IP addr      |
  61   IPX   |type|      Net addr     |          Node addr          |
  62   IPX   |type|      Net addr     |Sckt addr|
  63   Apple |type| Network |node|
  64   PPPoE |type|   SID   |           AC MAC            |
  65 -----------------------------------------------------------------*/
  66
  67
  68 /* Find a tag in pppoe frame and return the pointer */
  69 static inline unsigned char *__nat25_find_pppoe_tag(struct pppoe_hdr *ph, unsigned short type)
  70 {
  71         unsigned char *cur_ptr, *start_ptr;
  72         unsigned short tagLen, tagType;
  73
  74         start_ptr = cur_ptr = (unsigned char *)ph->tag;
  75         while ((cur_ptr - start_ptr) < ntohs(ph->length)) {
  76                 /*  prevent un-alignment access */
  77                 tagType = (unsigned short)((cur_ptr[0] << 8) + cur_ptr[1]);
  78                 tagLen  = (unsigned short)((cur_ptr[2] << 8) + cur_ptr[3]);
  79                 if (tagType == type)
  80                         return cur_ptr;
  81                 cur_ptr = cur_ptr + TAG_HDR_LEN + tagLen;
  82         }
  83         return NULL;
  84 }
  85
  86
  87 static inline int __nat25_add_pppoe_tag(struct sk_buff *skb, struct pppoe_tag *tag)
  88 {
  89         struct pppoe_hdr *ph = (struct pppoe_hdr *)(skb->data + ETH_HLEN);
  90         int data_len;
  91
  92         data_len = tag->tag_len + TAG_HDR_LEN;
  93         if (skb_tailroom(skb) < data_len) {
  94                 _DEBUG_ERR("skb_tailroom() failed in add SID tag!\n");
  95                 return -1;
  96         }
  97
  98         skb_put(skb, data_len);
  99         /*  have a room for new tag */
 100         memmove(((unsigned char *)ph->tag + data_len), (unsigned char *)ph->tag, ntohs(ph->length));
 101         ph->length = htons(ntohs(ph->length) + data_len);
 102         memcpy((unsigned char *)ph->tag, tag, data_len);
 103         return data_len;
 104 }
 105
 106 static int skb_pull_and_merge(struct sk_buff *skb, unsigned char *src, int len)
 107 {
 108         int tail_len;
 109         unsigned long end, tail;
 110
 111         if ((src+len) > skb_tail_pointer(skb) || skb->len < len)
 112                 return -1;
 113
 114         tail = (unsigned long)skb_tail_pointer(skb);
 115         end = (unsigned long)src+len;
 116         if (tail < end)
 117                 return -1;
 118
 119         tail_len = (int)(tail-end);
 120         if (tail_len > 0)
 121                 memmove(src, src+len, tail_len);
 122
 123         skb_trim(skb, skb->len-len);
 124         return 0;
 125 }
 126
 127 static inline unsigned long __nat25_timeout(struct adapter *priv)
 128 {
 129         unsigned long timeout;
 130
 131         timeout = jiffies - NAT25_AGEING_TIME*HZ;
 132
 133         return timeout;
 134 }
 135
 136
 137 static inline int  __nat25_has_expired(struct adapter *priv,
 138                                 struct nat25_network_db_entry *fdb)
 139 {
 140         if (time_before_eq(fdb->ageing_timer, __nat25_timeout(priv)))
 141                 return 1;
 142
 143         return 0;
 144 }
 145
 146
 147 static inline void __nat25_generate_ipv4_network_addr(unsigned char *networkAddr,
 148                                 unsigned int *ipAddr)
 149 {
 150         memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
 151
 152         networkAddr[0] = NAT25_IPV4;
 153         memcpy(networkAddr+7, (unsigned char *)ipAddr, 4);
 154 }
 155
 156
 157 static inline void __nat25_generate_ipx_network_addr_with_node(unsigned char *networkAddr,
 158                                 unsigned int *ipxNetAddr, unsigned char *ipxNodeAddr)
 159 {
 160         memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
 161
 162         networkAddr[0] = NAT25_IPX;
 163         memcpy(networkAddr+1, (unsigned char *)ipxNetAddr, 4);
 164         memcpy(networkAddr+5, ipxNodeAddr, 6);
 165 }
 166
 167
 168 static inline void __nat25_generate_ipx_network_addr_with_socket(unsigned char *networkAddr,
 169                                 unsigned int *ipxNetAddr, unsigned short *ipxSocketAddr)
 170 {
 171         memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
 172
 173         networkAddr[0] = NAT25_IPX;
 174         memcpy(networkAddr+1, (unsigned char *)ipxNetAddr, 4);
 175         memcpy(networkAddr+5, (unsigned char *)ipxSocketAddr, 2);
 176 }
 177
 178
 179 static inline void __nat25_generate_apple_network_addr(unsigned char *networkAddr,
 180                                 unsigned short *network, unsigned char *node)
 181 {
 182         memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
 183
 184         networkAddr[0] = NAT25_APPLE;
 185         memcpy(networkAddr+1, (unsigned char *)network, 2);
 186         networkAddr[3] = *node;
 187 }
 188
 189 static inline void __nat25_generate_pppoe_network_addr(unsigned char *networkAddr,
 190                                 unsigned char *ac_mac, unsigned short *sid)
 191 {
 192         memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
 193
 194         networkAddr[0] = NAT25_PPPOE;
 195         memcpy(networkAddr+1, (unsigned char *)sid, 2);
 196         memcpy(networkAddr+3, (unsigned char *)ac_mac, 6);
 197 }
 198
 199 static  void __nat25_generate_ipv6_network_addr(unsigned char *networkAddr,
 200                                 unsigned int *ipAddr)
 201 {
 202         memset(networkAddr, 0, MAX_NETWORK_ADDR_LEN);
 203
 204         networkAddr[0] = NAT25_IPV6;
 205         memcpy(networkAddr+1, (unsigned char *)ipAddr, 16);
 206 }
 207
 208 static unsigned char *scan_tlv(unsigned char *data, int len, unsigned char tag, unsigned char len8b)
 209 {
 210         while (len > 0) {
 211                 if (*data == tag && *(data+1) == len8b && len >= len8b*8)
 212                         return data+2;
 213
 214                 len -= (*(data+1))*8;
 215                 data += (*(data+1))*8;
 216         }
 217         return NULL;
 218 }
 219
 220 static int update_nd_link_layer_addr(unsigned char *data, int len, unsigned char *replace_mac)
 221 {
 222         struct icmp6hdr *icmphdr = (struct icmp6hdr *)data;
 223         unsigned char *mac;
 224
 225         if (icmphdr->icmp6_type == NDISC_ROUTER_SOLICITATION) {
 226                 if (len >= 8) {
 227                         mac = scan_tlv(&data[8], len-8, 1, 1);
 228                         if (mac) {
 229                                 _DEBUG_INFO("Router Solicitation, replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n",
 230                                         mac[0], mac[1], mac[2], mac[3], mac[4], mac[5],
 231                                         replace_mac[0], replace_mac[1], replace_mac[2], replace_mac[3], replace_mac[4], replace_mac[5]);
 232                                 memcpy(mac, replace_mac, 6);
 233                                 return 1;
 234                         }
 235                 }
 236         } else if (icmphdr->icmp6_type == NDISC_ROUTER_ADVERTISEMENT) {
 237                 if (len >= 16) {
 238                         mac = scan_tlv(&data[16], len-16, 1, 1);
 239                         if (mac) {
 240                                 _DEBUG_INFO("Router Advertisement, replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n",
 241                                         mac[0], mac[1], mac[2], mac[3], mac[4], mac[5],
 242                                         replace_mac[0], replace_mac[1], replace_mac[2], replace_mac[3], replace_mac[4], replace_mac[5]);
 243                                 memcpy(mac, replace_mac, 6);
 244                                 return 1;
 245                         }
 246                 }
 247         } else if (icmphdr->icmp6_type == NDISC_NEIGHBOUR_SOLICITATION) {
 248                 if (len >= 24) {
 249                         mac = scan_tlv(&data[24], len-24, 1, 1);
 250                         if (mac) {
 251                                 _DEBUG_INFO("Neighbor Solicitation, replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n",
 252                                         mac[0], mac[1], mac[2], mac[3], mac[4], mac[5],
 253                                         replace_mac[0], replace_mac[1], replace_mac[2], replace_mac[3], replace_mac[4], replace_mac[5]);
 254                                 memcpy(mac, replace_mac, 6);
 255                                 return 1;
 256                         }
 257                 }
 258         } else if (icmphdr->icmp6_type == NDISC_NEIGHBOUR_ADVERTISEMENT) {
 259                 if (len >= 24) {
 260                         mac = scan_tlv(&data[24], len-24, 2, 1);
 261                         if (mac) {
 262                                 _DEBUG_INFO("Neighbor Advertisement, replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n",
 263                                         mac[0], mac[1], mac[2], mac[3], mac[4], mac[5],
 264                                         replace_mac[0], replace_mac[1], replace_mac[2], replace_mac[3], replace_mac[4], replace_mac[5]);
 265                                 memcpy(mac, replace_mac, 6);
 266                                 return 1;
 267                         }
 268                 }
 269         } else if (icmphdr->icmp6_type == NDISC_REDIRECT) {
 270                 if (len >= 40) {
 271                         mac = scan_tlv(&data[40], len-40, 2, 1);
 272                         if (mac) {
 273                                 _DEBUG_INFO("Redirect,  replace MAC From: %02x:%02x:%02x:%02x:%02x:%02x, To: %02x:%02x:%02x:%02x:%02x:%02x\n",
 274                                         mac[0], mac[1], mac[2], mac[3], mac[4], mac[5],
 275                                         replace_mac[0], replace_mac[1], replace_mac[2], replace_mac[3], replace_mac[4], replace_mac[5]);
 276                                 memcpy(mac, replace_mac, 6);
 277                                 return 1;
 278                         }
 279                 }
 280         }
 281         return 0;
 282 }
 283
 284 static inline int __nat25_network_hash(unsigned char *networkAddr)
 285 {
 286         if (networkAddr[0] == NAT25_IPV4) {
 287                 unsigned long x;
 288
 289                 x = networkAddr[7] ^ networkAddr[8] ^ networkAddr[9] ^ networkAddr[10];
 290
 291                 return x & (NAT25_HASH_SIZE - 1);
 292         } else if (networkAddr[0] == NAT25_IPX) {
 293                 unsigned long x;
 294
 295                 x = networkAddr[1] ^ networkAddr[2] ^ networkAddr[3] ^ networkAddr[4] ^ networkAddr[5] ^
 296                         networkAddr[6] ^ networkAddr[7] ^ networkAddr[8] ^ networkAddr[9] ^ networkAddr[10];
 297
 298                 return x & (NAT25_HASH_SIZE - 1);
 299         } else if (networkAddr[0] == NAT25_APPLE) {
 300                 unsigned long x;
 301
 302                 x = networkAddr[1] ^ networkAddr[2] ^ networkAddr[3];
 303
 304                 return x & (NAT25_HASH_SIZE - 1);
 305         } else if (networkAddr[0] == NAT25_PPPOE) {
 306                 unsigned long x;
 307
 308                 x = networkAddr[0] ^ networkAddr[1] ^ networkAddr[2] ^ networkAddr[3] ^ networkAddr[4] ^ networkAddr[5] ^ networkAddr[6] ^ networkAddr[7] ^ networkAddr[8];
 309
 310                 return x & (NAT25_HASH_SIZE - 1);
 311         } else if (networkAddr[0] == NAT25_IPV6) {
 312                 unsigned long x;
 313
 314                 x = networkAddr[1] ^ networkAddr[2] ^ networkAddr[3] ^ networkAddr[4] ^ networkAddr[5] ^
 315                         networkAddr[6] ^ networkAddr[7] ^ networkAddr[8] ^ networkAddr[9] ^ networkAddr[10] ^
 316                         networkAddr[11] ^ networkAddr[12] ^ networkAddr[13] ^ networkAddr[14] ^ networkAddr[15] ^
 317                         networkAddr[16];
 318
 319                 return x & (NAT25_HASH_SIZE - 1);
 320         } else {
 321                 unsigned long x = 0;
 322                 int i;
 323
 324                 for (i = 0; i < MAX_NETWORK_ADDR_LEN; i++)
 325                         x ^= networkAddr[i];
 326
 327                 return x & (NAT25_HASH_SIZE - 1);
 328         }
 329 }
 330
 331 static inline void __network_hash_link(struct adapter *priv,
 332                                 struct nat25_network_db_entry *ent, int hash)
 333 {
 334         /*  Caller must _enter_critical_bh already! */
 335         ent->next_hash = priv->nethash[hash];
 336         if (ent->next_hash != NULL)
 337                 ent->next_hash->pprev_hash = &ent->next_hash;
 338         priv->nethash[hash] = ent;
 339         ent->pprev_hash = &priv->nethash[hash];
 340 }
 341
 342 static inline void __network_hash_unlink(struct nat25_network_db_entry *ent)
 343 {
 344         /*  Caller must _enter_critical_bh already! */
 345         *(ent->pprev_hash) = ent->next_hash;
 346         if (ent->next_hash != NULL)
 347                 ent->next_hash->pprev_hash = ent->pprev_hash;
 348         ent->next_hash = NULL;
 349         ent->pprev_hash = NULL;
 350 }
 351
 352 static int __nat25_db_network_lookup_and_replace(struct adapter *priv,
 353                                 struct sk_buff *skb, unsigned char *networkAddr)
 354 {
 355         struct nat25_network_db_entry *db;
 356         unsigned long irqL;
 357         _enter_critical_bh(&priv->br_ext_lock, &irqL);
 358
 359         db = priv->nethash[__nat25_network_hash(networkAddr)];
 360         while (db != NULL) {
 361                 if (!memcmp(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN)) {
 362                         if (!__nat25_has_expired(priv, db)) {
 363                                 /*  replace the destination mac address */
 364                                 memcpy(skb->data, db->macAddr, ETH_ALEN);
 365                                 atomic_inc(&db->use_count);
 366
 367                                 DEBUG_INFO("NAT25: Lookup M:%02x%02x%02x%02x%02x%02x N:%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x"
 368                                                         "%02x%02x%02x%02x%02x%02x\n",
 369                                         db->macAddr[0],
 370                                         db->macAddr[1],
 371                                         db->macAddr[2],
 372                                         db->macAddr[3],
 373                                         db->macAddr[4],
 374                                         db->macAddr[5],
 375                                         db->networkAddr[0],
 376                                         db->networkAddr[1],
 377                                         db->networkAddr[2],
 378                                         db->networkAddr[3],
 379                                         db->networkAddr[4],
 380                                         db->networkAddr[5],
 381                                         db->networkAddr[6],
 382                                         db->networkAddr[7],
 383                                         db->networkAddr[8],
 384                                         db->networkAddr[9],
 385                                         db->networkAddr[10],
 386                                         db->networkAddr[11],
 387                                         db->networkAddr[12],
 388                                         db->networkAddr[13],
 389                                         db->networkAddr[14],
 390                                         db->networkAddr[15],
 391                                         db->networkAddr[16]);
 392                         }
 393                         _exit_critical_bh(&priv->br_ext_lock, &irqL);
 394                         return 1;
 395                 }
 396                 db = db->next_hash;
 397         }
 398         _exit_critical_bh(&priv->br_ext_lock, &irqL);
 399         return 0;
 400 }
 401
 402 static void __nat25_db_network_insert(struct adapter *priv,
 403                                 unsigned char *macAddr, unsigned char *networkAddr)
 404 {
 405         struct nat25_network_db_entry *db;
 406         int hash;
 407         unsigned long irqL;
 408
 409         _enter_critical_bh(&priv->br_ext_lock, &irqL);
 410         hash = __nat25_network_hash(networkAddr);
 411         db = priv->nethash[hash];
 412         while (db != NULL) {
 413                 if (!memcmp(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN)) {
 414                         memcpy(db->macAddr, macAddr, ETH_ALEN);
 415                         db->ageing_timer = jiffies;
 416                         _exit_critical_bh(&priv->br_ext_lock, &irqL);
 417                         return;
 418                 }
 419                 db = db->next_hash;
 420         }
 421         db = (struct nat25_network_db_entry *) rtw_malloc(sizeof(*db));
 422         if (db == NULL) {
 423                 _exit_critical_bh(&priv->br_ext_lock, &irqL);
 424                 return;
 425         }
 426         memcpy(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN);
 427         memcpy(db->macAddr, macAddr, ETH_ALEN);
 428         atomic_set(&db->use_count, 1);
 429         db->ageing_timer = jiffies;
 430
 431         __network_hash_link(priv, db, hash);
 432
 433         _exit_critical_bh(&priv->br_ext_lock, &irqL);
 434 }
 435
 436 static void __nat25_db_print(struct adapter *priv)
 437 {
 438 }
 439
 440 /*
 441  *      NAT2.5 interface
 442  */
 443
 444 void nat25_db_cleanup(struct adapter *priv)
 445 {
 446         int i;
 447         unsigned long irqL;
 448         _enter_critical_bh(&priv->br_ext_lock, &irqL);
 449
 450         for (i = 0; i < NAT25_HASH_SIZE; i++) {
 451                 struct nat25_network_db_entry *f;
 452                 f = priv->nethash[i];
 453                 while (f != NULL) {
 454                         struct nat25_network_db_entry *g;
 455
 456                         g = f->next_hash;
 457                         if (priv->scdb_entry == f) {
 458                                 memset(priv->scdb_mac, 0, ETH_ALEN);
 459                                 memset(priv->scdb_ip, 0, 4);
 460                                 priv->scdb_entry = NULL;
 461                         }
 462                         __network_hash_unlink(f);
 463                         kfree(f);
 464                         f = g;
 465                 }
 466         }
 467         _exit_critical_bh(&priv->br_ext_lock, &irqL);
 468 }
 469
 470 void nat25_db_expire(struct adapter *priv)
 471 {
 472         int i;
 473         unsigned long irqL;
 474         _enter_critical_bh(&priv->br_ext_lock, &irqL);
 475
 476         for (i = 0; i < NAT25_HASH_SIZE; i++) {
 477                 struct nat25_network_db_entry *f;
 478                 f = priv->nethash[i];
 479
 480                 while (f != NULL) {
 481                         struct nat25_network_db_entry *g;
 482                         g = f->next_hash;
 483
 484                         if (__nat25_has_expired(priv, f)) {
 485                                 if (atomic_dec_and_test(&f->use_count)) {
 486                                         if (priv->scdb_entry == f) {
 487                                                 memset(priv->scdb_mac, 0, ETH_ALEN);
 488                                                 memset(priv->scdb_ip, 0, 4);
 489                                                 priv->scdb_entry = NULL;
 490                                         }
 491                                         __network_hash_unlink(f);
 492                                         kfree(f);
 493                                 }
 494                         }
 495                         f = g;
 496                 }
 497         }
 498         _exit_critical_bh(&priv->br_ext_lock, &irqL);
 499 }
 500
 501 int nat25_db_handle(struct adapter *priv, struct sk_buff *skb, int method)
 502 {
 503         unsigned short protocol;
 504         unsigned char networkAddr[MAX_NETWORK_ADDR_LEN];
 505         unsigned int tmp;
 506
 507         if (skb == NULL)
 508                 return -1;
 509
 510         if ((method <= NAT25_MIN) || (method >= NAT25_MAX))
 511                 return -1;
 512
 513         protocol = be16_to_cpu(*((__be16 *)(skb->data + 2 * ETH_ALEN)));
 514
 515         /*---------------------------------------------------*/
 516         /*                 Handle IP frame                   */
 517         /*---------------------------------------------------*/
 518         if (protocol == ETH_P_IP) {
 519                 struct iphdr *iph = (struct iphdr *)(skb->data + ETH_HLEN);
 520
 521                 if (((unsigned char *)(iph) + (iph->ihl<<2)) >= (skb->data + ETH_HLEN + skb->len)) {
 522                         DEBUG_WARN("NAT25: malformed IP packet !\n");
 523                         return -1;
 524                 }
 525
 526                 switch (method) {
 527                 case NAT25_CHECK:
 528                         return -1;
 529                 case NAT25_INSERT:
 530                         /* some multicast with source IP is all zero, maybe other case is illegal */
 531                         /* in class A, B, C, host address is all zero or all one is illegal */
 532                         if (iph->saddr == 0)
 533                                 return 0;
 534                         tmp = be32_to_cpu(iph->saddr);
 535                         DEBUG_INFO("NAT25: Insert IP, SA =%08x, DA =%08x\n", tmp, iph->daddr);
 536                         __nat25_generate_ipv4_network_addr(networkAddr, &tmp);
 537                         /* record source IP address and , source mac address into db */
 538                         __nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
 539
 540                         __nat25_db_print(priv);
 541                         return 0;
 542                 case NAT25_LOOKUP:
 543                         DEBUG_INFO("NAT25: Lookup IP, SA =%08x, DA =%08x\n", iph->saddr, iph->daddr);
 544                         tmp = be32_to_cpu(iph->daddr);
 545                         __nat25_generate_ipv4_network_addr(networkAddr, &tmp);
 546
 547                         if (!__nat25_db_network_lookup_and_replace(priv, skb, networkAddr)) {
 548                                 if (*((unsigned char *)&iph->daddr + 3) == 0xff) {
 549                                         /*  L2 is unicast but L3 is broadcast, make L2 bacome broadcast */
 550                                         DEBUG_INFO("NAT25: Set DA as boardcast\n");
 551                                         memset(skb->data, 0xff, ETH_ALEN);
 552                                 } else {
 553                                         /*  forward unknow IP packet to upper TCP/IP */
 554                                         DEBUG_INFO("NAT25: Replace DA with BR's MAC\n");
 555                                         if ((*(u32 *)priv->br_mac) == 0 && (*(u16 *)(priv->br_mac+4)) == 0) {
 556                                                 printk("Re-init netdev_br_init() due to br_mac == 0!\n");
 557                                                 netdev_br_init(priv->pnetdev);
 558                                         }
 559                                         memcpy(skb->data, priv->br_mac, ETH_ALEN);
 560                                 }
 561                         }
 562                         return 0;
 563                 default:
 564                         return -1;
 565                 }
 566         } else if (protocol == ETH_P_ARP) {
 567                 /*---------------------------------------------------*/
 568                 /*                 Handle ARP frame                  */
 569                 /*---------------------------------------------------*/
 570                 struct arphdr *arp = (struct arphdr *)(skb->data + ETH_HLEN);
 571                 unsigned char *arp_ptr = (unsigned char *)(arp + 1);
 572                 unsigned int *sender, *target;
 573
 574                 if (arp->ar_pro != __constant_htons(ETH_P_IP)) {
 575                         DEBUG_WARN("NAT25: arp protocol unknown (%4x)!\n", be16_to_cpu(arp->ar_pro));
 576                         return -1;
 577                 }
 578
 579                 switch (method) {
 580                 case NAT25_CHECK:
 581                         return 0;       /*  skb_copy for all ARP frame */
 582                 case NAT25_INSERT:
 583                         DEBUG_INFO("NAT25: Insert ARP, MAC =%02x%02x%02x%02x%02x%02x\n", arp_ptr[0],
 584                                 arp_ptr[1], arp_ptr[2], arp_ptr[3], arp_ptr[4], arp_ptr[5]);
 585
 586                         /*  change to ARP sender mac address to wlan STA address */
 587                         memcpy(arp_ptr, GET_MY_HWADDR(priv), ETH_ALEN);
 588                         arp_ptr += arp->ar_hln;
 589                         sender = (unsigned int *)arp_ptr;
 590                         __nat25_generate_ipv4_network_addr(networkAddr, sender);
 591                         __nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
 592                         __nat25_db_print(priv);
 593                         return 0;
 594                 case NAT25_LOOKUP:
 595                         DEBUG_INFO("NAT25: Lookup ARP\n");
 596
 597                         arp_ptr += arp->ar_hln;
 598                         sender = (unsigned int *)arp_ptr;
 599                         arp_ptr += (arp->ar_hln + arp->ar_pln);
 600                         target = (unsigned int *)arp_ptr;
 601                         __nat25_generate_ipv4_network_addr(networkAddr, target);
 602                         __nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
 603                         /*  change to ARP target mac address to Lookup result */
 604                         arp_ptr = (unsigned char *)(arp + 1);
 605                         arp_ptr += (arp->ar_hln + arp->ar_pln);
 606                         memcpy(arp_ptr, skb->data, ETH_ALEN);
 607                         return 0;
 608                 default:
 609                         return -1;
 610                 }
 611         } else if ((protocol == ETH_P_IPX) ||
 612                    (protocol <= ETH_FRAME_LEN)) {
 613                 /*---------------------------------------------------*/
 614                 /*         Handle IPX and Apple Talk frame           */
 615                 /*---------------------------------------------------*/
 616                 unsigned char ipx_header[2] = {0xFF, 0xFF};
 617                 struct ipxhdr   *ipx = NULL;
 618                 struct elapaarp *ea = NULL;
 619                 struct ddpehdr  *ddp = NULL;
 620                 unsigned char *framePtr = skb->data + ETH_HLEN;
 621
 622                 if (protocol == ETH_P_IPX) {
 623                         DEBUG_INFO("NAT25: Protocol = IPX (Ethernet II)\n");
 624                         ipx = (struct ipxhdr *)framePtr;
 625                 } else if (protocol <= ETH_FRAME_LEN) {
 626                         if (!memcmp(ipx_header, framePtr, 2)) {
 627                                 DEBUG_INFO("NAT25: Protocol = IPX (Ethernet 802.3)\n");
 628                                 ipx = (struct ipxhdr *)framePtr;
 629                         } else {
 630                                 unsigned char ipx_8022_type =  0xE0;
 631                                 unsigned char snap_8022_type = 0xAA;
 632
 633                                 if (*framePtr == snap_8022_type) {
 634                                         unsigned char ipx_snap_id[5] = {0x0, 0x0, 0x0, 0x81, 0x37};             /*  IPX SNAP ID */
 635                                         unsigned char aarp_snap_id[5] = {0x00, 0x00, 0x00, 0x80, 0xF3}; /*  Apple Talk AARP SNAP ID */
 636                                         unsigned char ddp_snap_id[5] = {0x08, 0x00, 0x07, 0x80, 0x9B};  /*  Apple Talk DDP SNAP ID */
 637
 638                                         framePtr += 3;  /*  eliminate the 802.2 header */
 639
 640                                         if (!memcmp(ipx_snap_id, framePtr, 5)) {
 641                                                 framePtr += 5;  /*  eliminate the SNAP header */
 642
 643                                                 DEBUG_INFO("NAT25: Protocol = IPX (Ethernet SNAP)\n");
 644                                                 ipx = (struct ipxhdr *)framePtr;
 645                                         } else if (!memcmp(aarp_snap_id, framePtr, 5)) {
 646                                                 framePtr += 5;  /*  eliminate the SNAP header */
 647
 648                                                 ea = (struct elapaarp *)framePtr;
 649                                         } else if (!memcmp(ddp_snap_id, framePtr, 5)) {
 650                                                 framePtr += 5;  /*  eliminate the SNAP header */
 651
 652                                                 ddp = (struct ddpehdr *)framePtr;
 653                                         } else {
 654                                                 DEBUG_WARN("NAT25: Protocol = Ethernet SNAP %02x%02x%02x%02x%02x\n", framePtr[0],
 655                                                         framePtr[1], framePtr[2], framePtr[3], framePtr[4]);
 656                                                 return -1;
 657                                         }
 658                                 } else if (*framePtr == ipx_8022_type) {
 659                                         framePtr += 3;  /*  eliminate the 802.2 header */
 660
 661                                         if (!memcmp(ipx_header, framePtr, 2)) {
 662                                                 DEBUG_INFO("NAT25: Protocol = IPX (Ethernet 802.2)\n");
 663                                                 ipx = (struct ipxhdr *)framePtr;
 664                                         } else {
 665                                                 return -1;
 666                                         }
 667                                 } else {
 668                                         return -1;
 669                                 }
 670                         }
 671                 } else {
 672                         return -1;
 673                 }
 674
 675                 /*   IPX   */
 676                 if (ipx != NULL) {
 677                         switch (method) {
 678                         case NAT25_CHECK:
 679                                 if (!memcmp(skb->data+ETH_ALEN, ipx->ipx_source.node, ETH_ALEN))
 680                                         DEBUG_INFO("NAT25: Check IPX skb_copy\n");
 681                                 return 0;
 682                         case NAT25_INSERT:
 683                                 DEBUG_INFO("NAT25: Insert IPX, Dest =%08x,%02x%02x%02x%02x%02x%02x,%04x Source =%08x,%02x%02x%02x%02x%02x%02x,%04x\n",
 684                                         ipx->ipx_dest.net,
 685                                         ipx->ipx_dest.node[0],
 686                                         ipx->ipx_dest.node[1],
 687                                         ipx->ipx_dest.node[2],
 688                                         ipx->ipx_dest.node[3],
 689                                         ipx->ipx_dest.node[4],
 690                                         ipx->ipx_dest.node[5],
 691                                         ipx->ipx_dest.sock,
 692                                         ipx->ipx_source.net,
 693                                         ipx->ipx_source.node[0],
 694                                         ipx->ipx_source.node[1],
 695                                         ipx->ipx_source.node[2],
 696                                         ipx->ipx_source.node[3],
 697                                         ipx->ipx_source.node[4],
 698                                         ipx->ipx_source.node[5],
 699                                         ipx->ipx_source.sock);
 700
 701                                 if (!memcmp(skb->data+ETH_ALEN, ipx->ipx_source.node, ETH_ALEN)) {
 702                                         DEBUG_INFO("NAT25: Use IPX Net, and Socket as network addr\n");
 703
 704                                         __nat25_generate_ipx_network_addr_with_socket(networkAddr, &ipx->ipx_source.net, &ipx->ipx_source.sock);
 705
 706                                         /*  change IPX source node addr to wlan STA address */
 707                                         memcpy(ipx->ipx_source.node, GET_MY_HWADDR(priv), ETH_ALEN);
 708                                 } else {
 709                                         __nat25_generate_ipx_network_addr_with_node(networkAddr, &ipx->ipx_source.net, ipx->ipx_source.node);
 710                                 }
 711                                 __nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
 712                                 __nat25_db_print(priv);
 713                                 return 0;
 714                         case NAT25_LOOKUP:
 715                                 if (!memcmp(GET_MY_HWADDR(priv), ipx->ipx_dest.node, ETH_ALEN)) {
 716                                         DEBUG_INFO("NAT25: Lookup IPX, Modify Destination IPX Node addr\n");
 717
 718                                         __nat25_generate_ipx_network_addr_with_socket(networkAddr, &ipx->ipx_dest.net, &ipx->ipx_dest.sock);
 719
 720                                         __nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
 721
 722                                         /*  replace IPX destination node addr with Lookup destination MAC addr */
 723                                         memcpy(ipx->ipx_dest.node, skb->data, ETH_ALEN);
 724                                 } else {
 725                                         __nat25_generate_ipx_network_addr_with_node(networkAddr, &ipx->ipx_dest.net, ipx->ipx_dest.node);
 726
 727                                         __nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
 728                                 }
 729                                 return 0;
 730                         default:
 731                                 return -1;
 732                         }
 733                 } else if (ea != NULL) {
 734                         /* Sanity check fields. */
 735                         if (ea->hw_len != ETH_ALEN || ea->pa_len != AARP_PA_ALEN) {
 736                                 DEBUG_WARN("NAT25: Appletalk AARP Sanity check fail!\n");
 737                                 return -1;
 738                         }
 739
 740                         switch (method) {
 741                         case NAT25_CHECK:
 742                                 return 0;
 743                         case NAT25_INSERT:
 744                                 /*  change to AARP source mac address to wlan STA address */
 745                                 memcpy(ea->hw_src, GET_MY_HWADDR(priv), ETH_ALEN);
 746
 747                                 DEBUG_INFO("NAT25: Insert AARP, Source =%d,%d Destination =%d,%d\n",
 748                                         ea->pa_src_net,
 749                                         ea->pa_src_node,
 750                                         ea->pa_dst_net,
 751                                         ea->pa_dst_node);
 752
 753                                 __nat25_generate_apple_network_addr(networkAddr, &ea->pa_src_net, &ea->pa_src_node);
 754
 755                                 __nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
 756
 757                                 __nat25_db_print(priv);
 758                                 return 0;
 759                         case NAT25_LOOKUP:
 760                                 DEBUG_INFO("NAT25: Lookup AARP, Source =%d,%d Destination =%d,%d\n",
 761                                         ea->pa_src_net,
 762                                         ea->pa_src_node,
 763                                         ea->pa_dst_net,
 764                                         ea->pa_dst_node);
 765
 766                                 __nat25_generate_apple_network_addr(networkAddr, &ea->pa_dst_net, &ea->pa_dst_node);
 767
 768                                 __nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
 769
 770                                 /*  change to AARP destination mac address to Lookup result */
 771                                 memcpy(ea->hw_dst, skb->data, ETH_ALEN);
 772                                 return 0;
 773                         default:
 774                                 return -1;
 775                         }
 776                 } else if (ddp != NULL) {
 777                         switch (method) {
 778                         case NAT25_CHECK:
 779                                 return -1;
 780                         case NAT25_INSERT:
 781                                 DEBUG_INFO("NAT25: Insert DDP, Source =%d,%d Destination =%d,%d\n",
 782                                         ddp->deh_snet,
 783                                         ddp->deh_snode,
 784                                         ddp->deh_dnet,
 785                                         ddp->deh_dnode);
 786
 787                                 __nat25_generate_apple_network_addr(networkAddr, &ddp->deh_snet, &ddp->deh_snode);
 788
 789                                 __nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
 790
 791                                 __nat25_db_print(priv);
 792                                 return 0;
 793                         case NAT25_LOOKUP:
 794                                 DEBUG_INFO("NAT25: Lookup DDP, Source =%d,%d Destination =%d,%d\n",
 795                                         ddp->deh_snet,
 796                                         ddp->deh_snode,
 797                                         ddp->deh_dnet,
 798                                         ddp->deh_dnode);
 799                                 __nat25_generate_apple_network_addr(networkAddr, &ddp->deh_dnet, &ddp->deh_dnode);
 800                                 __nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
 801                                 return 0;
 802                         default:
 803                                 return -1;
 804                         }
 805                 }
 806
 807                 return -1;
 808         } else if ((protocol == ETH_P_PPP_DISC) ||
 809                    (protocol == ETH_P_PPP_SES)) {
 810                 /*---------------------------------------------------*/
 811                 /*                Handle PPPoE frame                 */
 812                 /*---------------------------------------------------*/
 813                 struct pppoe_hdr *ph = (struct pppoe_hdr *)(skb->data + ETH_HLEN);
 814                 unsigned short *pMagic;
 815
 816                 switch (method) {
 817                 case NAT25_CHECK:
 818                         if (ph->sid == 0)
 819                                 return 0;
 820                         return 1;
 821                 case NAT25_INSERT:
 822                         if (ph->sid == 0) {     /*  Discovery phase according to tag */
 823                                 if (ph->code == PADI_CODE || ph->code == PADR_CODE) {
 824                                         if (priv->ethBrExtInfo.addPPPoETag) {
 825                                                 struct pppoe_tag *tag, *pOldTag;
 826                                                 unsigned char tag_buf[40];
 827                                                 int old_tag_len = 0;
 828
 829                                                 tag = (struct pppoe_tag *)tag_buf;
 830                                                 pOldTag = (struct pppoe_tag *)__nat25_find_pppoe_tag(ph, ntohs(PTT_RELAY_SID));
 831                                                 if (pOldTag) { /*  if SID existed, copy old value and delete it */
 832                                                         old_tag_len = ntohs(pOldTag->tag_len);
 833                                                         if (old_tag_len+TAG_HDR_LEN+MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN > sizeof(tag_buf)) {
 834                                                                 DEBUG_ERR("SID tag length too long!\n");
 835                                                                 return -1;
 836                                                         }
 837
 838                                                         memcpy(tag->tag_data+MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN,
 839                                                                 pOldTag->tag_data, old_tag_len);
 840
 841                                                         if (skb_pull_and_merge(skb, (unsigned char *)pOldTag, TAG_HDR_LEN+old_tag_len) < 0) {
 842                                                                 DEBUG_ERR("call skb_pull_and_merge() failed in PADI/R packet!\n");
 843                                                                 return -1;
 844                                                         }
 845                                                         ph->length = htons(ntohs(ph->length)-TAG_HDR_LEN-old_tag_len);
 846                                                 }
 847
 848                                                 tag->tag_type = PTT_RELAY_SID;
 849                                                 tag->tag_len = htons(MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN+old_tag_len);
 850
 851                                                 /*  insert the magic_code+client mac in relay tag */
 852                                                 pMagic = (unsigned short *)tag->tag_data;
 853                                                 *pMagic = htons(MAGIC_CODE);
 854                                                 memcpy(tag->tag_data+MAGIC_CODE_LEN, skb->data+ETH_ALEN, ETH_ALEN);
 855
 856                                                 /* Add relay tag */
 857                                                 if (__nat25_add_pppoe_tag(skb, tag) < 0)
 858                                                         return -1;
 859
 860                                                 DEBUG_INFO("NAT25: Insert PPPoE, forward %s packet\n",
 861                                                                                 (ph->code == PADI_CODE ? "PADI" : "PADR"));
 862                                         } else { /*  not add relay tag */
 863                                                 if (priv->pppoe_connection_in_progress &&
 864                                                                 memcmp(skb->data+ETH_ALEN, priv->pppoe_addr, ETH_ALEN))  {
 865                                                         DEBUG_ERR("Discard PPPoE packet due to another PPPoE connection is in progress!\n");
 866                                                         return -2;
 867                                                 }
 868
 869                                                 if (priv->pppoe_connection_in_progress == 0)
 870                                                         memcpy(priv->pppoe_addr, skb->data+ETH_ALEN, ETH_ALEN);
 871
 872                                                 priv->pppoe_connection_in_progress = WAIT_TIME_PPPOE;
 873                                         }
 874                                 } else {
 875                                         return -1;
 876                                 }
 877                         } else {        /*  session phase */
 878                                 DEBUG_INFO("NAT25: Insert PPPoE, insert session packet to %s\n", skb->dev->name);
 879
 880                                 __nat25_generate_pppoe_network_addr(networkAddr, skb->data, &(ph->sid));
 881
 882                                 __nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
 883
 884                                 __nat25_db_print(priv);
 885
 886                                 if (!priv->ethBrExtInfo.addPPPoETag &&
 887                                     priv->pppoe_connection_in_progress &&
 888                                     !memcmp(skb->data+ETH_ALEN, priv->pppoe_addr, ETH_ALEN))
 889                                         priv->pppoe_connection_in_progress = 0;
 890                         }
 891                         return 0;
 892                 case NAT25_LOOKUP:
 893                         if (ph->code == PADO_CODE || ph->code == PADS_CODE) {
 894                                 if (priv->ethBrExtInfo.addPPPoETag) {
 895                                         struct pppoe_tag *tag;
 896                                         unsigned char *ptr;
 897                                         unsigned short tagType, tagLen;
 898                                         int offset = 0;
 899
 900                                         ptr = __nat25_find_pppoe_tag(ph, ntohs(PTT_RELAY_SID));
 901                                         if (ptr == NULL) {
 902                                                 DEBUG_ERR("Fail to find PTT_RELAY_SID in FADO!\n");
 903                                                 return -1;
 904                                         }
 905
 906                                         tag = (struct pppoe_tag *)ptr;
 907                                         tagType = (unsigned short)((ptr[0] << 8) + ptr[1]);
 908                                         tagLen = (unsigned short)((ptr[2] << 8) + ptr[3]);
 909
 910                                         if ((tagType != ntohs(PTT_RELAY_SID)) || (tagLen < (MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN))) {
 911                                                 DEBUG_ERR("Invalid PTT_RELAY_SID tag length [%d]!\n", tagLen);
 912                                                 return -1;
 913                                         }
 914
 915                                         pMagic = (unsigned short *)tag->tag_data;
 916                                         if (ntohs(*pMagic) != MAGIC_CODE) {
 917                                                 DEBUG_ERR("Can't find MAGIC_CODE in %s packet!\n",
 918                                                         (ph->code == PADO_CODE ? "PADO" : "PADS"));
 919                                                 return -1;
 920                                         }
 921
 922                                         memcpy(skb->data, tag->tag_data+MAGIC_CODE_LEN, ETH_ALEN);
 923
 924                                         if (tagLen > MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN)
 925                                                 offset = TAG_HDR_LEN;
 926
 927                                         if (skb_pull_and_merge(skb, ptr+offset, TAG_HDR_LEN+MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN-offset) < 0) {
 928                                                 DEBUG_ERR("call skb_pull_and_merge() failed in PADO packet!\n");
 929                                                 return -1;
 930                                         }
 931                                         ph->length = htons(ntohs(ph->length)-(TAG_HDR_LEN+MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN-offset));
 932                                         if (offset > 0)
 933                                                 tag->tag_len = htons(tagLen-MAGIC_CODE_LEN-RTL_RELAY_TAG_LEN);
 934
 935                                         DEBUG_INFO("NAT25: Lookup PPPoE, forward %s Packet from %s\n",
 936                                                 (ph->code == PADO_CODE ? "PADO" : "PADS"),      skb->dev->name);
 937                                 } else { /*  not add relay tag */
 938                                         if (!priv->pppoe_connection_in_progress) {
 939                                                 DEBUG_ERR("Discard PPPoE packet due to no connection in progresss!\n");
 940                                                 return -1;
 941                                         }
 942                                         memcpy(skb->data, priv->pppoe_addr, ETH_ALEN);
 943                                         priv->pppoe_connection_in_progress = WAIT_TIME_PPPOE;
 944                                 }
 945                         } else {
 946                                 if (ph->sid != 0) {
 947                                         DEBUG_INFO("NAT25: Lookup PPPoE, lookup session packet from %s\n", skb->dev->name);
 948                                         __nat25_generate_pppoe_network_addr(networkAddr, skb->data+ETH_ALEN, &(ph->sid));
 949                                         __nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
 950                                         __nat25_db_print(priv);
 951                                 } else {
 952                                         return -1;
 953                                 }
 954                         }
 955                         return 0;
 956                 default:
 957                         return -1;
 958                 }
 959         } else if (protocol == 0x888e) {
 960                 /*---------------------------------------------------*/
 961                 /*                 Handle EAP frame                  */
 962                 /*---------------------------------------------------*/
 963                 switch (method) {
 964                 case NAT25_CHECK:
 965                         return -1;
 966                 case NAT25_INSERT:
 967                         return 0;
 968                 case NAT25_LOOKUP:
 969                         return 0;
 970                 default:
 971                         return -1;
 972                 }
 973         } else if ((protocol == 0xe2ae) || (protocol == 0xe2af)) {
 974                 /*---------------------------------------------------*/
 975                 /*         Handle C-Media proprietary frame          */
 976                 /*---------------------------------------------------*/
 977                 switch (method) {
 978                 case NAT25_CHECK:
 979                         return -1;
 980                 case NAT25_INSERT:
 981                         return 0;
 982                 case NAT25_LOOKUP:
 983                         return 0;
 984                 default:
 985                         return -1;
 986                 }
 987         } else if (protocol == ETH_P_IPV6) {
 988                 /*------------------------------------------------*/
 989                 /*         Handle IPV6 frame                      */
 990                 /*------------------------------------------------*/
 991                 struct ipv6hdr *iph = (struct ipv6hdr *)(skb->data + ETH_HLEN);
 992
 993                 if (sizeof(*iph) >= (skb->len - ETH_HLEN)) {
 994                         DEBUG_WARN("NAT25: malformed IPv6 packet !\n");
 995                         return -1;
 996                 }
 997
 998                 switch (method) {
 999                 case NAT25_CHECK:
1000                         if (skb->data[0] & 1)
1001                                 return 0;
1002                         return -1;
1003                 case NAT25_INSERT:
1004                         DEBUG_INFO("NAT25: Insert IP, SA =%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x,"
1005                                                         " DA =%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x\n",
1006                                 iph->saddr.s6_addr16[0], iph->saddr.s6_addr16[1], iph->saddr.s6_addr16[2], iph->saddr.s6_addr16[3],
1007                                 iph->saddr.s6_addr16[4], iph->saddr.s6_addr16[5], iph->saddr.s6_addr16[6], iph->saddr.s6_addr16[7],
1008                                 iph->daddr.s6_addr16[0], iph->daddr.s6_addr16[1], iph->daddr.s6_addr16[2], iph->daddr.s6_addr16[3],
1009                                 iph->daddr.s6_addr16[4], iph->daddr.s6_addr16[5], iph->daddr.s6_addr16[6], iph->daddr.s6_addr16[7]);
1010
1011                         if (memcmp(&iph->saddr, "\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0", 16)) {
1012                                 __nat25_generate_ipv6_network_addr(networkAddr, (unsigned int *)&iph->saddr);
1013                                 __nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
1014                                 __nat25_db_print(priv);
1015
1016                                 if (iph->nexthdr == IPPROTO_ICMPV6 &&
1017                                                 skb->len > (ETH_HLEN +  sizeof(*iph) + 4)) {
1018                                         if (update_nd_link_layer_addr(skb->data + ETH_HLEN + sizeof(*iph),
1019                                                                       skb->len - ETH_HLEN - sizeof(*iph), GET_MY_HWADDR(priv))) {
1020                                                 struct icmp6hdr  *hdr = (struct icmp6hdr *)(skb->data + ETH_HLEN + sizeof(*iph));
1021                                                 hdr->icmp6_cksum = 0;
1022                                                 hdr->icmp6_cksum = csum_ipv6_magic(&iph->saddr, &iph->daddr,
1023                                                                                 iph->payload_len,
1024                                                                                 IPPROTO_ICMPV6,
1025                                                                                 csum_partial((__u8 *)hdr, iph->payload_len, 0));
1026                                         }
1027                                 }
1028                         }
1029                         return 0;
1030                 case NAT25_LOOKUP:
1031                         DEBUG_INFO("NAT25: Lookup IP, SA =%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x, DA =%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x\n",
1032                                    iph->saddr.s6_addr16[0], iph->saddr.s6_addr16[1], iph->saddr.s6_addr16[2], iph->saddr.s6_addr16[3],
1033                                    iph->saddr.s6_addr16[4], iph->saddr.s6_addr16[5], iph->saddr.s6_addr16[6], iph->saddr.s6_addr16[7],
1034                                    iph->daddr.s6_addr16[0], iph->daddr.s6_addr16[1], iph->daddr.s6_addr16[2], iph->daddr.s6_addr16[3],
1035                                    iph->daddr.s6_addr16[4], iph->daddr.s6_addr16[5], iph->daddr.s6_addr16[6], iph->daddr.s6_addr16[7]);
1036                         __nat25_generate_ipv6_network_addr(networkAddr, (unsigned int *)&iph->daddr);
1037                         __nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
1038                         return 0;
1039                 default:
1040                         return -1;
1041                 }
1042         }
1043         return -1;
1044 }
1045
1046 int nat25_handle_frame(struct adapter *priv, struct sk_buff *skb)
1047 {
1048         if (!(skb->data[0] & 1)) {
1049                 int is_vlan_tag = 0, i, retval = 0;
1050                 unsigned short vlan_hdr = 0;
1051                 unsigned short protocol;
1052
1053                 protocol = be16_to_cpu(*((__be16 *)(skb->data + 2 * ETH_ALEN)));
1054                 if (protocol == ETH_P_8021Q) {
1055                         is_vlan_tag = 1;
1056                         vlan_hdr = *((unsigned short *)(skb->data+ETH_ALEN*2+2));
1057                         for (i = 0; i < 6; i++)
1058                                 *((unsigned short *)(skb->data+ETH_ALEN*2+2-i*2)) = *((unsigned short *)(skb->data+ETH_ALEN*2-2-i*2));
1059                         skb_pull(skb, 4);
1060                 }
1061
1062                 if (!priv->ethBrExtInfo.nat25_disable) {
1063                         unsigned long irqL;
1064                         _enter_critical_bh(&priv->br_ext_lock, &irqL);
1065                         /*
1066                          *      This function look up the destination network address from
1067                          *      the NAT2.5 database. Return value = -1 means that the
1068                          *      corresponding network protocol is NOT support.
1069                          */
1070                         if (!priv->ethBrExtInfo.nat25sc_disable &&
1071                             (be16_to_cpu(*((__be16 *)(skb->data+ETH_ALEN*2))) == ETH_P_IP) &&
1072                             !memcmp(priv->scdb_ip, skb->data+ETH_HLEN+16, 4)) {
1073                                 memcpy(skb->data, priv->scdb_mac, ETH_ALEN);
1074
1075                                 _exit_critical_bh(&priv->br_ext_lock, &irqL);
1076                         } else {
1077                                 _exit_critical_bh(&priv->br_ext_lock, &irqL);
1078
1079                                 retval = nat25_db_handle(priv, skb, NAT25_LOOKUP);
1080                         }
1081                 } else {
1082                         if (((be16_to_cpu(*((__be16 *)(skb->data+ETH_ALEN*2))) == ETH_P_IP) &&
1083                             !memcmp(priv->br_ip, skb->data+ETH_HLEN+16, 4)) ||
1084                             ((be16_to_cpu(*((__be16 *)(skb->data+ETH_ALEN*2))) == ETH_P_ARP) &&
1085                             !memcmp(priv->br_ip, skb->data+ETH_HLEN+24, 4))) {
1086                                 /*  for traffic to upper TCP/IP */
1087                                 retval = nat25_db_handle(priv, skb, NAT25_LOOKUP);
1088                         }
1089                 }
1090
1091                 if (is_vlan_tag) {
1092                         skb_push(skb, 4);
1093                         for (i = 0; i < 6; i++)
1094                                 *((unsigned short *)(skb->data+i*2)) = *((unsigned short *)(skb->data+4+i*2));
1095                         *((__be16 *)(skb->data+ETH_ALEN*2)) = __constant_htons(ETH_P_8021Q);
1096                         *((unsigned short *)(skb->data+ETH_ALEN*2+2)) = vlan_hdr;
1097                 }
1098
1099                 if (retval == -1) {
1100                         /* DEBUG_ERR("NAT25: Lookup fail!\n"); */
1101                         return -1;
1102                 }
1103         }
1104
1105         return 0;
1106 }
1107
1108 #define SERVER_PORT                     67
1109 #define CLIENT_PORT                     68
1110 #define DHCP_MAGIC                      0x63825363
1111 #define BROADCAST_FLAG          0x8000
1112
1113 struct dhcpMessage {
1114         u_int8_t op;
1115         u_int8_t htype;
1116         u_int8_t hlen;
1117         u_int8_t hops;
1118         u_int32_t xid;
1119         u_int16_t secs;
1120         u_int16_t flags;
1121         u_int32_t ciaddr;
1122         u_int32_t yiaddr;
1123         u_int32_t siaddr;
1124         u_int32_t giaddr;
1125         u_int8_t chaddr[16];
1126         u_int8_t sname[64];
1127         u_int8_t file[128];
1128         u_int32_t cookie;
1129         u_int8_t options[308]; /* 312 - cookie */
1130 };
1131
1132 void dhcp_flag_bcast(struct adapter *priv, struct sk_buff *skb)
1133 {
1134         if (skb == NULL)
1135                 return;
1136
1137         if (!priv->ethBrExtInfo.dhcp_bcst_disable) {
1138                 __be16 protocol = *((__be16 *)(skb->data + 2 * ETH_ALEN));
1139
1140                 if (protocol == __constant_htons(ETH_P_IP)) { /*  IP */
1141                         struct iphdr *iph = (struct iphdr *)(skb->data + ETH_HLEN);
1142
1143                         if (iph->protocol == IPPROTO_UDP) { /*  UDP */
1144                                 struct udphdr *udph = (struct udphdr *)((size_t)iph + (iph->ihl << 2));
1145
1146                                 if ((udph->source == __constant_htons(CLIENT_PORT)) &&
1147                                     (udph->dest == __constant_htons(SERVER_PORT))) { /*  DHCP request */
1148                                         struct dhcpMessage *dhcph =
1149                                                 (struct dhcpMessage *)((size_t)udph + sizeof(struct udphdr));
1150                                         u32 cookie = be32_to_cpu((__be32)dhcph->cookie);
1151
1152                                         if (cookie == DHCP_MAGIC) { /*  match magic word */
1153                                                 if (!(dhcph->flags & htons(BROADCAST_FLAG))) {
1154                                                         /*  if not broadcast */
1155                                                         register int sum = 0;
1156
1157                                                         DEBUG_INFO("DHCP: change flag of DHCP request to broadcast.\n");
1158                                                         /*  or BROADCAST flag */
1159                                                         dhcph->flags |= htons(BROADCAST_FLAG);
1160                                                         /*  recalculate checksum */
1161                                                         sum = ~(udph->check) & 0xffff;
1162                                                         sum += be16_to_cpu(dhcph->flags);
1163                                                         while (sum >> 16)
1164                                                                 sum = (sum & 0xffff) + (sum >> 16);
1165                                                         udph->check = ~sum;
1166                                                 }
1167                                         }
1168                                 }
1169                         }
1170                 }
1171         }
1172 }
1173
1174
1175 void *scdb_findEntry(struct adapter *priv, unsigned char *macAddr,
1176                                 unsigned char *ipAddr)
1177 {
1178         unsigned char networkAddr[MAX_NETWORK_ADDR_LEN];
1179         struct nat25_network_db_entry *db;
1180         int hash;
1181         /* unsigned long irqL; */
1182         /* _enter_critical_bh(&priv->br_ext_lock, &irqL); */
1183
1184         __nat25_generate_ipv4_network_addr(networkAddr, (unsigned int *)ipAddr);
1185         hash = __nat25_network_hash(networkAddr);
1186         db = priv->nethash[hash];
1187         while (db != NULL) {
1188                 if (!memcmp(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN)) {
1189                         /* _exit_critical_bh(&priv->br_ext_lock, &irqL); */
1190                         return (void *)db;
1191                 }
1192
1193                 db = db->next_hash;
1194         }
1195
1196         /* _exit_critical_bh(&priv->br_ext_lock, &irqL); */
1197         return NULL;
1198 }