drivers/staging/r8188eu/core/rtw_br_ext.c

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