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 muticast 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                                 return -1;
 683                         case NAT25_INSERT:
 684                                 DEBUG_INFO("NAT25: Insert IPX, Dest =%08x,%02x%02x%02x%02x%02x%02x,%04x Source =%08x,%02x%02x%02x%02x%02x%02x,%04x\n",
 685                                         ipx->ipx_dest.net,
 686                                         ipx->ipx_dest.node[0],
 687                                         ipx->ipx_dest.node[1],
 688                                         ipx->ipx_dest.node[2],
 689                                         ipx->ipx_dest.node[3],
 690                                         ipx->ipx_dest.node[4],
 691                                         ipx->ipx_dest.node[5],
 692                                         ipx->ipx_dest.sock,
 693                                         ipx->ipx_source.net,
 694                                         ipx->ipx_source.node[0],
 695                                         ipx->ipx_source.node[1],
 696                                         ipx->ipx_source.node[2],
 697                                         ipx->ipx_source.node[3],
 698                                         ipx->ipx_source.node[4],
 699                                         ipx->ipx_source.node[5],
 700                                         ipx->ipx_source.sock);
 701
 702                                 if (!memcmp(skb->data+ETH_ALEN, ipx->ipx_source.node, ETH_ALEN)) {
 703                                         DEBUG_INFO("NAT25: Use IPX Net, and Socket as network addr\n");
 704
 705                                         __nat25_generate_ipx_network_addr_with_socket(networkAddr, &ipx->ipx_source.net, &ipx->ipx_source.sock);
 706
 707                                         /*  change IPX source node addr to wlan STA address */
 708                                         memcpy(ipx->ipx_source.node, GET_MY_HWADDR(priv), ETH_ALEN);
 709                                 } else {
 710                                         __nat25_generate_ipx_network_addr_with_node(networkAddr, &ipx->ipx_source.net, ipx->ipx_source.node);
 711                                 }
 712                                 __nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
 713                                 __nat25_db_print(priv);
 714                                 return 0;
 715                         case NAT25_LOOKUP:
 716                                 if (!memcmp(GET_MY_HWADDR(priv), ipx->ipx_dest.node, ETH_ALEN)) {
 717                                         DEBUG_INFO("NAT25: Lookup IPX, Modify Destination IPX Node addr\n");
 718
 719                                         __nat25_generate_ipx_network_addr_with_socket(networkAddr, &ipx->ipx_dest.net, &ipx->ipx_dest.sock);
 720
 721                                         __nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
 722
 723                                         /*  replace IPX destination node addr with Lookup destination MAC addr */
 724                                         memcpy(ipx->ipx_dest.node, skb->data, ETH_ALEN);
 725                                 } else {
 726                                         __nat25_generate_ipx_network_addr_with_node(networkAddr, &ipx->ipx_dest.net, ipx->ipx_dest.node);
 727
 728                                         __nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
 729                                 }
 730                                 return 0;
 731                         default:
 732                                 return -1;
 733                         }
 734                 } else if (ea != NULL) {
 735                         /* Sanity check fields. */
 736                         if (ea->hw_len != ETH_ALEN || ea->pa_len != AARP_PA_ALEN) {
 737                                 DEBUG_WARN("NAT25: Appletalk AARP Sanity check fail!\n");
 738                                 return -1;
 739                         }
 740
 741                         switch (method) {
 742                         case NAT25_CHECK:
 743                                 return 0;
 744                         case NAT25_INSERT:
 745                                 /*  change to AARP source mac address to wlan STA address */
 746                                 memcpy(ea->hw_src, GET_MY_HWADDR(priv), ETH_ALEN);
 747
 748                                 DEBUG_INFO("NAT25: Insert AARP, Source =%d,%d Destination =%d,%d\n",
 749                                         ea->pa_src_net,
 750                                         ea->pa_src_node,
 751                                         ea->pa_dst_net,
 752                                         ea->pa_dst_node);
 753
 754                                 __nat25_generate_apple_network_addr(networkAddr, &ea->pa_src_net, &ea->pa_src_node);
 755
 756                                 __nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
 757
 758                                 __nat25_db_print(priv);
 759                                 return 0;
 760                         case NAT25_LOOKUP:
 761                                 DEBUG_INFO("NAT25: Lookup AARP, Source =%d,%d Destination =%d,%d\n",
 762                                         ea->pa_src_net,
 763                                         ea->pa_src_node,
 764                                         ea->pa_dst_net,
 765                                         ea->pa_dst_node);
 766
 767                                 __nat25_generate_apple_network_addr(networkAddr, &ea->pa_dst_net, &ea->pa_dst_node);
 768
 769                                 __nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
 770
 771                                 /*  change to AARP destination mac address to Lookup result */
 772                                 memcpy(ea->hw_dst, skb->data, ETH_ALEN);
 773                                 return 0;
 774                         default:
 775                                 return -1;
 776                         }
 777                 } else if (ddp != NULL) {
 778                         switch (method) {
 779                         case NAT25_CHECK:
 780                                 return -1;
 781                         case NAT25_INSERT:
 782                                 DEBUG_INFO("NAT25: Insert DDP, Source =%d,%d Destination =%d,%d\n",
 783                                         ddp->deh_snet,
 784                                         ddp->deh_snode,
 785                                         ddp->deh_dnet,
 786                                         ddp->deh_dnode);
 787
 788                                 __nat25_generate_apple_network_addr(networkAddr, &ddp->deh_snet, &ddp->deh_snode);
 789
 790                                 __nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
 791
 792                                 __nat25_db_print(priv);
 793                                 return 0;
 794                         case NAT25_LOOKUP:
 795                                 DEBUG_INFO("NAT25: Lookup DDP, Source =%d,%d Destination =%d,%d\n",
 796                                         ddp->deh_snet,
 797                                         ddp->deh_snode,
 798                                         ddp->deh_dnet,
 799                                         ddp->deh_dnode);
 800                                 __nat25_generate_apple_network_addr(networkAddr, &ddp->deh_dnet, &ddp->deh_dnode);
 801                                 __nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
 802                                 return 0;
 803                         default:
 804                                 return -1;
 805                         }
 806                 }
 807
 808                 return -1;
 809         } else if ((protocol == ETH_P_PPP_DISC) ||
 810                    (protocol == ETH_P_PPP_SES)) {
 811                 /*---------------------------------------------------*/
 812                 /*                Handle PPPoE frame                 */
 813                 /*---------------------------------------------------*/
 814                 struct pppoe_hdr *ph = (struct pppoe_hdr *)(skb->data + ETH_HLEN);
 815                 unsigned short *pMagic;
 816
 817                 switch (method) {
 818                 case NAT25_CHECK:
 819                         if (ph->sid == 0)
 820                                 return 0;
 821                         return 1;
 822                 case NAT25_INSERT:
 823                         if (ph->sid == 0) {     /*  Discovery phase according to tag */
 824                                 if (ph->code == PADI_CODE || ph->code == PADR_CODE) {
 825                                         if (priv->ethBrExtInfo.addPPPoETag) {
 826                                                 struct pppoe_tag *tag, *pOldTag;
 827                                                 unsigned char tag_buf[40];
 828                                                 int old_tag_len = 0;
 829
 830                                                 tag = (struct pppoe_tag *)tag_buf;
 831                                                 pOldTag = (struct pppoe_tag *)__nat25_find_pppoe_tag(ph, ntohs(PTT_RELAY_SID));
 832                                                 if (pOldTag) { /*  if SID existed, copy old value and delete it */
 833                                                         old_tag_len = ntohs(pOldTag->tag_len);
 834                                                         if (old_tag_len+TAG_HDR_LEN+MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN > sizeof(tag_buf)) {
 835                                                                 DEBUG_ERR("SID tag length too long!\n");
 836                                                                 return -1;
 837                                                         }
 838
 839                                                         memcpy(tag->tag_data+MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN,
 840                                                                 pOldTag->tag_data, old_tag_len);
 841
 842                                                         if (skb_pull_and_merge(skb, (unsigned char *)pOldTag, TAG_HDR_LEN+old_tag_len) < 0) {
 843                                                                 DEBUG_ERR("call skb_pull_and_merge() failed in PADI/R packet!\n");
 844                                                                 return -1;
 845                                                         }
 846                                                         ph->length = htons(ntohs(ph->length)-TAG_HDR_LEN-old_tag_len);
 847                                                 }
 848
 849                                                 tag->tag_type = PTT_RELAY_SID;
 850                                                 tag->tag_len = htons(MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN+old_tag_len);
 851
 852                                                 /*  insert the magic_code+client mac in relay tag */
 853                                                 pMagic = (unsigned short *)tag->tag_data;
 854                                                 *pMagic = htons(MAGIC_CODE);
 855                                                 memcpy(tag->tag_data+MAGIC_CODE_LEN, skb->data+ETH_ALEN, ETH_ALEN);
 856
 857                                                 /* Add relay tag */
 858                                                 if (__nat25_add_pppoe_tag(skb, tag) < 0)
 859                                                         return -1;
 860
 861                                                 DEBUG_INFO("NAT25: Insert PPPoE, forward %s packet\n",
 862                                                                                 (ph->code == PADI_CODE ? "PADI" : "PADR"));
 863                                         } else { /*  not add relay tag */
 864                                                 if (priv->pppoe_connection_in_progress &&
 865                                                                 memcmp(skb->data+ETH_ALEN, priv->pppoe_addr, ETH_ALEN))  {
 866                                                         DEBUG_ERR("Discard PPPoE packet due to another PPPoE connection is in progress!\n");
 867                                                         return -2;
 868                                                 }
 869
 870                                                 if (priv->pppoe_connection_in_progress == 0)
 871                                                         memcpy(priv->pppoe_addr, skb->data+ETH_ALEN, ETH_ALEN);
 872
 873                                                 priv->pppoe_connection_in_progress = WAIT_TIME_PPPOE;
 874                                         }
 875                                 } else {
 876                                         return -1;
 877                                 }
 878                         } else {        /*  session phase */
 879                                 DEBUG_INFO("NAT25: Insert PPPoE, insert session packet to %s\n", skb->dev->name);
 880
 881                                 __nat25_generate_pppoe_network_addr(networkAddr, skb->data, &(ph->sid));
 882
 883                                 __nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
 884
 885                                 __nat25_db_print(priv);
 886
 887                                 if (!priv->ethBrExtInfo.addPPPoETag &&
 888                                     priv->pppoe_connection_in_progress &&
 889                                     !memcmp(skb->data+ETH_ALEN, priv->pppoe_addr, ETH_ALEN))
 890                                         priv->pppoe_connection_in_progress = 0;
 891                         }
 892                         return 0;
 893                 case NAT25_LOOKUP:
 894                         if (ph->code == PADO_CODE || ph->code == PADS_CODE) {
 895                                 if (priv->ethBrExtInfo.addPPPoETag) {
 896                                         struct pppoe_tag *tag;
 897                                         unsigned char *ptr;
 898                                         unsigned short tagType, tagLen;
 899                                         int offset = 0;
 900
 901                                         ptr = __nat25_find_pppoe_tag(ph, ntohs(PTT_RELAY_SID));
 902                                         if (ptr == NULL) {
 903                                                 DEBUG_ERR("Fail to find PTT_RELAY_SID in FADO!\n");
 904                                                 return -1;
 905                                         }
 906
 907                                         tag = (struct pppoe_tag *)ptr;
 908                                         tagType = (unsigned short)((ptr[0] << 8) + ptr[1]);
 909                                         tagLen = (unsigned short)((ptr[2] << 8) + ptr[3]);
 910
 911                                         if ((tagType != ntohs(PTT_RELAY_SID)) || (tagLen < (MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN))) {
 912                                                 DEBUG_ERR("Invalid PTT_RELAY_SID tag length [%d]!\n", tagLen);
 913                                                 return -1;
 914                                         }
 915
 916                                         pMagic = (unsigned short *)tag->tag_data;
 917                                         if (ntohs(*pMagic) != MAGIC_CODE) {
 918                                                 DEBUG_ERR("Can't find MAGIC_CODE in %s packet!\n",
 919                                                         (ph->code == PADO_CODE ? "PADO" : "PADS"));
 920                                                 return -1;
 921                                         }
 922
 923                                         memcpy(skb->data, tag->tag_data+MAGIC_CODE_LEN, ETH_ALEN);
 924
 925                                         if (tagLen > MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN)
 926                                                 offset = TAG_HDR_LEN;
 927
 928                                         if (skb_pull_and_merge(skb, ptr+offset, TAG_HDR_LEN+MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN-offset) < 0) {
 929                                                 DEBUG_ERR("call skb_pull_and_merge() failed in PADO packet!\n");
 930                                                 return -1;
 931                                         }
 932                                         ph->length = htons(ntohs(ph->length)-(TAG_HDR_LEN+MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN-offset));
 933                                         if (offset > 0)
 934                                                 tag->tag_len = htons(tagLen-MAGIC_CODE_LEN-RTL_RELAY_TAG_LEN);
 935
 936                                         DEBUG_INFO("NAT25: Lookup PPPoE, forward %s Packet from %s\n",
 937                                                 (ph->code == PADO_CODE ? "PADO" : "PADS"),      skb->dev->name);
 938                                 } else { /*  not add relay tag */
 939                                         if (!priv->pppoe_connection_in_progress) {
 940                                                 DEBUG_ERR("Discard PPPoE packet due to no connection in progresss!\n");
 941                                                 return -1;
 942                                         }
 943                                         memcpy(skb->data, priv->pppoe_addr, ETH_ALEN);
 944                                         priv->pppoe_connection_in_progress = WAIT_TIME_PPPOE;
 945                                 }
 946                         } else {
 947                                 if (ph->sid != 0) {
 948                                         DEBUG_INFO("NAT25: Lookup PPPoE, lookup session packet from %s\n", skb->dev->name);
 949                                         __nat25_generate_pppoe_network_addr(networkAddr, skb->data+ETH_ALEN, &(ph->sid));
 950                                         __nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
 951                                         __nat25_db_print(priv);
 952                                 } else {
 953                                         return -1;
 954                                 }
 955                         }
 956                         return 0;
 957                 default:
 958                         return -1;
 959                 }
 960         } else if (protocol == 0x888e) {
 961                 /*---------------------------------------------------*/
 962                 /*                 Handle EAP frame                  */
 963                 /*---------------------------------------------------*/
 964                 switch (method) {
 965                 case NAT25_CHECK:
 966                         return -1;
 967                 case NAT25_INSERT:
 968                         return 0;
 969                 case NAT25_LOOKUP:
 970                         return 0;
 971                 default:
 972                         return -1;
 973                 }
 974         } else if ((protocol == 0xe2ae) || (protocol == 0xe2af)) {
 975                 /*---------------------------------------------------*/
 976                 /*         Handle C-Media proprietary frame          */
 977                 /*---------------------------------------------------*/
 978                 switch (method) {
 979                 case NAT25_CHECK:
 980                         return -1;
 981                 case NAT25_INSERT:
 982                         return 0;
 983                 case NAT25_LOOKUP:
 984                         return 0;
 985                 default:
 986                         return -1;
 987                 }
 988         } else if (protocol == ETH_P_IPV6) {
 989                 /*------------------------------------------------*/
 990                 /*         Handle IPV6 frame                      */
 991                 /*------------------------------------------------*/
 992                 struct ipv6hdr *iph = (struct ipv6hdr *)(skb->data + ETH_HLEN);
 993
 994                 if (sizeof(*iph) >= (skb->len - ETH_HLEN)) {
 995                         DEBUG_WARN("NAT25: malformed IPv6 packet !\n");
 996                         return -1;
 997                 }
 998
 999                 switch (method) {
1000                 case NAT25_CHECK:
1001                         if (skb->data[0] & 1)
1002                                 return 0;
1003                         return -1;
1004                 case NAT25_INSERT:
1005                         DEBUG_INFO("NAT25: Insert IP, SA =%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x,"
1006                                                         " DA =%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x\n",
1007                                 iph->saddr.s6_addr16[0], iph->saddr.s6_addr16[1], iph->saddr.s6_addr16[2], iph->saddr.s6_addr16[3],
1008                                 iph->saddr.s6_addr16[4], iph->saddr.s6_addr16[5], iph->saddr.s6_addr16[6], iph->saddr.s6_addr16[7],
1009                                 iph->daddr.s6_addr16[0], iph->daddr.s6_addr16[1], iph->daddr.s6_addr16[2], iph->daddr.s6_addr16[3],
1010                                 iph->daddr.s6_addr16[4], iph->daddr.s6_addr16[5], iph->daddr.s6_addr16[6], iph->daddr.s6_addr16[7]);
1011
1012                         if (memcmp(&iph->saddr, "\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0\x0", 16)) {
1013                                 __nat25_generate_ipv6_network_addr(networkAddr, (unsigned int *)&iph->saddr);
1014                                 __nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
1015                                 __nat25_db_print(priv);
1016
1017                                 if (iph->nexthdr == IPPROTO_ICMPV6 &&
1018                                                 skb->len > (ETH_HLEN +  sizeof(*iph) + 4)) {
1019                                         if (update_nd_link_layer_addr(skb->data + ETH_HLEN + sizeof(*iph),
1020                                                                       skb->len - ETH_HLEN - sizeof(*iph), GET_MY_HWADDR(priv))) {
1021                                                 struct icmp6hdr  *hdr = (struct icmp6hdr *)(skb->data + ETH_HLEN + sizeof(*iph));
1022                                                 hdr->icmp6_cksum = 0;
1023                                                 hdr->icmp6_cksum = csum_ipv6_magic(&iph->saddr, &iph->daddr,
1024                                                                                 iph->payload_len,
1025                                                                                 IPPROTO_ICMPV6,
1026                                                                                 csum_partial((__u8 *)hdr, iph->payload_len, 0));
1027                                         }
1028                                 }
1029                         }
1030                         return 0;
1031                 case NAT25_LOOKUP:
1032                         DEBUG_INFO("NAT25: Lookup IP, SA =%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x, DA =%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x\n",
1033                                    iph->saddr.s6_addr16[0], iph->saddr.s6_addr16[1], iph->saddr.s6_addr16[2], iph->saddr.s6_addr16[3],
1034                                    iph->saddr.s6_addr16[4], iph->saddr.s6_addr16[5], iph->saddr.s6_addr16[6], iph->saddr.s6_addr16[7],
1035                                    iph->daddr.s6_addr16[0], iph->daddr.s6_addr16[1], iph->daddr.s6_addr16[2], iph->daddr.s6_addr16[3],
1036                                    iph->daddr.s6_addr16[4], iph->daddr.s6_addr16[5], iph->daddr.s6_addr16[6], iph->daddr.s6_addr16[7]);
1037                         __nat25_generate_ipv6_network_addr(networkAddr, (unsigned int *)&iph->daddr);
1038                         __nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
1039                         return 0;
1040                 default:
1041                         return -1;
1042                 }
1043         }
1044         return -1;
1045 }
1046
1047 int nat25_handle_frame(struct adapter *priv, struct sk_buff *skb)
1048 {
1049         if (!(skb->data[0] & 1)) {
1050                 int is_vlan_tag = 0, i, retval = 0;
1051                 unsigned short vlan_hdr = 0;
1052                 unsigned short protocol;
1053
1054                 protocol = be16_to_cpu(*((__be16 *)(skb->data + 2 * ETH_ALEN)));
1055                 if (protocol == ETH_P_8021Q) {
1056                         is_vlan_tag = 1;
1057                         vlan_hdr = *((unsigned short *)(skb->data+ETH_ALEN*2+2));
1058                         for (i = 0; i < 6; i++)
1059                                 *((unsigned short *)(skb->data+ETH_ALEN*2+2-i*2)) = *((unsigned short *)(skb->data+ETH_ALEN*2-2-i*2));
1060                         skb_pull(skb, 4);
1061                 }
1062
1063                 if (!priv->ethBrExtInfo.nat25_disable) {
1064                         unsigned long irqL;
1065                         _enter_critical_bh(&priv->br_ext_lock, &irqL);
1066                         /*
1067                          *      This function look up the destination network address from
1068                          *      the NAT2.5 database. Return value = -1 means that the
1069                          *      corresponding network protocol is NOT support.
1070                          */
1071                         if (!priv->ethBrExtInfo.nat25sc_disable &&
1072                             (be16_to_cpu(*((__be16 *)(skb->data+ETH_ALEN*2))) == ETH_P_IP) &&
1073                             !memcmp(priv->scdb_ip, skb->data+ETH_HLEN+16, 4)) {
1074                                 memcpy(skb->data, priv->scdb_mac, ETH_ALEN);
1075
1076                                 _exit_critical_bh(&priv->br_ext_lock, &irqL);
1077                         } else {
1078                                 _exit_critical_bh(&priv->br_ext_lock, &irqL);
1079
1080                                 retval = nat25_db_handle(priv, skb, NAT25_LOOKUP);
1081                         }
1082                 } else {
1083                         if (((be16_to_cpu(*((__be16 *)(skb->data+ETH_ALEN*2))) == ETH_P_IP) &&
1084                             !memcmp(priv->br_ip, skb->data+ETH_HLEN+16, 4)) ||
1085                             ((be16_to_cpu(*((__be16 *)(skb->data+ETH_ALEN*2))) == ETH_P_ARP) &&
1086                             !memcmp(priv->br_ip, skb->data+ETH_HLEN+24, 4))) {
1087                                 /*  for traffic to upper TCP/IP */
1088                                 retval = nat25_db_handle(priv, skb, NAT25_LOOKUP);
1089                         }
1090                 }
1091
1092                 if (is_vlan_tag) {
1093                         skb_push(skb, 4);
1094                         for (i = 0; i < 6; i++)
1095                                 *((unsigned short *)(skb->data+i*2)) = *((unsigned short *)(skb->data+4+i*2));
1096                         *((__be16 *)(skb->data+ETH_ALEN*2)) = __constant_htons(ETH_P_8021Q);
1097                         *((unsigned short *)(skb->data+ETH_ALEN*2+2)) = vlan_hdr;
1098                 }
1099
1100                 if (retval == -1) {
1101                         /* DEBUG_ERR("NAT25: Lookup fail!\n"); */
1102                         return -1;
1103                 }
1104         }
1105
1106         return 0;
1107 }
1108
1109 #define SERVER_PORT                     67
1110 #define CLIENT_PORT                     68
1111 #define DHCP_MAGIC                      0x63825363
1112 #define BROADCAST_FLAG          0x8000
1113
1114 struct dhcpMessage {
1115         u_int8_t op;
1116         u_int8_t htype;
1117         u_int8_t hlen;
1118         u_int8_t hops;
1119         u_int32_t xid;
1120         u_int16_t secs;
1121         u_int16_t flags;
1122         u_int32_t ciaddr;
1123         u_int32_t yiaddr;
1124         u_int32_t siaddr;
1125         u_int32_t giaddr;
1126         u_int8_t chaddr[16];
1127         u_int8_t sname[64];
1128         u_int8_t file[128];
1129         u_int32_t cookie;
1130         u_int8_t options[308]; /* 312 - cookie */
1131 };
1132
1133 void dhcp_flag_bcast(struct adapter *priv, struct sk_buff *skb)
1134 {
1135         if (skb == NULL)
1136                 return;
1137
1138         if (!priv->ethBrExtInfo.dhcp_bcst_disable) {
1139                 __be16 protocol = *((__be16 *)(skb->data + 2 * ETH_ALEN));
1140
1141                 if (protocol == __constant_htons(ETH_P_IP)) { /*  IP */
1142                         struct iphdr *iph = (struct iphdr *)(skb->data + ETH_HLEN);
1143
1144                         if (iph->protocol == IPPROTO_UDP) { /*  UDP */
1145                                 struct udphdr *udph = (struct udphdr *)((size_t)iph + (iph->ihl << 2));
1146
1147                                 if ((udph->source == __constant_htons(CLIENT_PORT)) &&
1148                                     (udph->dest == __constant_htons(SERVER_PORT))) { /*  DHCP request */
1149                                         struct dhcpMessage *dhcph =
1150                                                 (struct dhcpMessage *)((size_t)udph + sizeof(struct udphdr));
1151                                         u32 cookie = be32_to_cpu((__be32)dhcph->cookie);
1152
1153                                         if (cookie == DHCP_MAGIC) { /*  match magic word */
1154                                                 if (!(dhcph->flags & htons(BROADCAST_FLAG))) {
1155                                                         /*  if not broadcast */
1156                                                         register int sum = 0;
1157
1158                                                         DEBUG_INFO("DHCP: change flag of DHCP request to broadcast.\n");
1159                                                         /*  or BROADCAST flag */
1160                                                         dhcph->flags |= htons(BROADCAST_FLAG);
1161                                                         /*  recalculate checksum */
1162                                                         sum = ~(udph->check) & 0xffff;
1163                                                         sum += be16_to_cpu(dhcph->flags);
1164                                                         while (sum >> 16)
1165                                                                 sum = (sum & 0xffff) + (sum >> 16);
1166                                                         udph->check = ~sum;
1167                                                 }
1168                                         }
1169                                 }
1170                         }
1171                 }
1172         }
1173 }
1174
1175
1176 void *scdb_findEntry(struct adapter *priv, unsigned char *macAddr,
1177                                 unsigned char *ipAddr)
1178 {
1179         unsigned char networkAddr[MAX_NETWORK_ADDR_LEN];
1180         struct nat25_network_db_entry *db;
1181         int hash;
1182         /* unsigned long irqL; */
1183         /* _enter_critical_bh(&priv->br_ext_lock, &irqL); */
1184
1185         __nat25_generate_ipv4_network_addr(networkAddr, (unsigned int *)ipAddr);
1186         hash = __nat25_network_hash(networkAddr);
1187         db = priv->nethash[hash];
1188         while (db != NULL) {
1189                 if (!memcmp(db->networkAddr, networkAddr, MAX_NETWORK_ADDR_LEN)) {
1190                         /* _exit_critical_bh(&priv->br_ext_lock, &irqL); */
1191                         return (void *)db;
1192                 }
1193
1194                 db = db->next_hash;
1195         }
1196
1197         /* _exit_critical_bh(&priv->br_ext_lock, &irqL); */
1198         return NULL;
1199 }