drivers/staging/rt3090/common/igmp_snoop.c

   1 /*
   2  *************************************************************************
   3  * Ralink Tech Inc.
   4  * 5F., No.36, Taiyuan St., Jhubei City,
   5  * Hsinchu County 302,
   6  * Taiwan, R.O.C.
   7  *
   8  * (c) Copyright 2002-2007, Ralink Technology, Inc.
   9  *
  10  * This program is free software; you can redistribute it and/or modify  *
  11  * it under the terms of the GNU General Public License as published by  *
  12  * the Free Software Foundation; either version 2 of the License, or     *
  13  * (at your option) any later version.                                   *
  14  *                                                                       *
  15  * This program is distributed in the hope that it will be useful,       *
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  18  * GNU General Public License for more details.                          *
  19  *                                                                       *
  20  * You should have received a copy of the GNU General Public License     *
  21  * along with this program; if not, write to the                         *
  22  * Free Software Foundation, Inc.,                                       *
  23  * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  24  *                                                                       *
  25  *************************************************************************
  26  */
  27
  28
  29 #ifdef IGMP_SNOOP_SUPPORT
  30
  31 #include "../rt_config.h"
  32 #include "../ipv6.h"
  33 #include "../igmp_snoop.h"
  34
  35
  36 static inline void initFreeEntryList(
  37         IN PMULTICAST_FILTER_TABLE pMulticastFilterTable,
  38         IN PLIST_HEADER pList)
  39 {
  40         int i;
  41
  42         for (i = 0; i < FREE_MEMBER_POOL_SIZE; i++)
  43                 insertTailList(pList, (PLIST_ENTRY)&(pMulticastFilterTable->freeMemberPool[i]));
  44
  45         return;
  46 }
  47
  48 static inline PMEMBER_ENTRY AllocaGrpMemberEntry(
  49         IN PMULTICAST_FILTER_TABLE pMulticastFilterTable)
  50 {
  51         PMEMBER_ENTRY pMemberEntry;
  52
  53         RTMP_SEM_LOCK(&pMulticastFilterTable->FreeMemberPoolTabLock);
  54
  55         pMemberEntry = (PMEMBER_ENTRY)removeHeadList(&pMulticastFilterTable->freeEntryList);
  56
  57         RTMP_SEM_UNLOCK(&pMulticastFilterTable->FreeMemberPoolTabLock);
  58
  59         return (PMEMBER_ENTRY)pMemberEntry;
  60 }
  61
  62 static inline VOID FreeGrpMemberEntry(
  63         IN PMULTICAST_FILTER_TABLE pMulticastFilterTable,
  64         IN PMEMBER_ENTRY pEntry)
  65 {
  66         RTMP_SEM_LOCK(&pMulticastFilterTable->FreeMemberPoolTabLock);
  67
  68         insertTailList(&pMulticastFilterTable->freeEntryList, (PLIST_ENTRY)pEntry);
  69
  70         RTMP_SEM_UNLOCK(&pMulticastFilterTable->FreeMemberPoolTabLock);
  71 }
  72
  73 static VOID IGMPTableDisplay(
  74         IN PRTMP_ADAPTER pAd);
  75
  76 static BOOLEAN isIgmpMacAddr(
  77         IN PUCHAR pMacAddr);
  78
  79 static VOID InsertIgmpMember(
  80         IN PMULTICAST_FILTER_TABLE pMulticastFilterTable,
  81         IN PLIST_HEADER pList,
  82         IN PUCHAR pMemberAddr);
  83
  84 static VOID DeleteIgmpMember(
  85         IN PMULTICAST_FILTER_TABLE pMulticastFilterTable,
  86         IN PLIST_HEADER pList,
  87         IN PUCHAR pMemberAddr);
  88
  89 static VOID DeleteIgmpMemberList(
  90         IN PMULTICAST_FILTER_TABLE pMulticastFilterTable,
  91         IN PLIST_HEADER pList);
  92
  93
  94 /*
  95     ==========================================================================
  96     Description:
  97         This routine init the entire IGMP table.
  98     ==========================================================================
  99  */
 100 VOID MulticastFilterTableInit(
 101         IN PMULTICAST_FILTER_TABLE *ppMulticastFilterTable)
 102 {
 103         // Initialize MAC table and allocate spin lock
 104         *ppMulticastFilterTable = kmalloc(sizeof(MULTICAST_FILTER_TABLE), MEM_ALLOC_FLAG);
 105         if (*ppMulticastFilterTable == NULL)
 106         {
 107                 DBGPRINT(RT_DEBUG_ERROR, ("%s unable to alloc memory for Multicase filter table, size=%d\n",
 108                         __FUNCTION__, sizeof(MULTICAST_FILTER_TABLE)));
 109                 return;
 110         }
 111
 112         NdisZeroMemory(*ppMulticastFilterTable, sizeof(MULTICAST_FILTER_TABLE));
 113         NdisAllocateSpinLock(&((*ppMulticastFilterTable)->MulticastFilterTabLock));
 114
 115         NdisAllocateSpinLock(&((*ppMulticastFilterTable)->FreeMemberPoolTabLock));
 116         initList(&((*ppMulticastFilterTable)->freeEntryList));
 117         initFreeEntryList(*ppMulticastFilterTable, &((*ppMulticastFilterTable)->freeEntryList));
 118         return;
 119 }
 120
 121 /*
 122     ==========================================================================
 123     Description:
 124         This routine reset the entire IGMP table.
 125     ==========================================================================
 126  */
 127 VOID MultiCastFilterTableReset(
 128         IN PMULTICAST_FILTER_TABLE *ppMulticastFilterTable)
 129 {
 130         if(*ppMulticastFilterTable == NULL)
 131         {
 132                 DBGPRINT(RT_DEBUG_ERROR, ("%s Multicase filter table is not ready.\n", __FUNCTION__));
 133                 return;
 134         }
 135
 136         NdisFreeSpinLock(&((*ppMulticastFilterTable)->FreeMemberPoolTabLock));
 137         NdisFreeSpinLock(&((*ppMulticastFilterTable)->MulticastFilterTabLock));
 138         kfree(*ppMulticastFilterTable);
 139         *ppMulticastFilterTable = NULL;
 140 }
 141
 142 /*
 143     ==========================================================================
 144     Description:
 145         Display all entrys in IGMP table
 146     ==========================================================================
 147  */
 148 static VOID IGMPTableDisplay(
 149         IN PRTMP_ADAPTER pAd)
 150 {
 151         int i;
 152         MULTICAST_FILTER_TABLE_ENTRY *pEntry = NULL;
 153         PMULTICAST_FILTER_TABLE pMulticastFilterTable = pAd->pMulticastFilterTable;
 154
 155         if (pMulticastFilterTable == NULL)
 156         {
 157                 DBGPRINT(RT_DEBUG_OFF, ("%s Multicase filter table is not ready.\n", __FUNCTION__));
 158                 return;
 159         }
 160
 161         // if FULL, return
 162         if (pMulticastFilterTable->Size == 0)
 163         {
 164                 DBGPRINT(RT_DEBUG_ERROR, ("Table empty.\n"));
 165                 return;
 166         }
 167
 168         // allocate one MAC entry
 169         RTMP_SEM_LOCK(&pMulticastFilterTable->MulticastFilterTabLock);
 170
 171         for (i = 0; i< MAX_LEN_OF_MULTICAST_FILTER_TABLE; i++)
 172         {
 173                 // pick up the first available vacancy
 174                 if (pMulticastFilterTable->Content[i].Valid == TRUE)
 175                 {
 176                         PMEMBER_ENTRY pMemberEntry = NULL;
 177                         pEntry = &pMulticastFilterTable->Content[i];
 178
 179                         DBGPRINT(RT_DEBUG_OFF, ("IF(%s) entry #%d, type=%s, GrpId=(%02x:%02x:%02x:%02x:%02x:%02x) memberCnt=%d\n",
 180                                 RTMP_OS_NETDEV_GET_DEVNAME(pEntry->net_dev), i, (pEntry->type==0 ? "static":"dynamic"),
 181                                 PRINT_MAC(pEntry->Addr), IgmpMemberCnt(&pEntry->MemberList)));
 182
 183                         pMemberEntry = (PMEMBER_ENTRY)pEntry->MemberList.pHead;
 184                         while (pMemberEntry)
 185                         {
 186                                 DBGPRINT(RT_DEBUG_OFF, ("member mac=(%02x:%02x:%02x:%02x:%02x:%02x)\n",
 187                                                                                 PRINT_MAC(pMemberEntry->Addr)));
 188
 189                                 pMemberEntry = pMemberEntry->pNext;
 190                         }
 191                 }
 192         }
 193
 194         RTMP_SEM_UNLOCK(&pMulticastFilterTable->MulticastFilterTabLock);
 195
 196         return;
 197 }
 198
 199 /*
 200     ==========================================================================
 201     Description:
 202         Add and new entry into MAC table
 203     ==========================================================================
 204  */
 205 BOOLEAN MulticastFilterTableInsertEntry(
 206         IN PRTMP_ADAPTER pAd,
 207         IN PUCHAR pGrpId,
 208         IN PUCHAR pMemberAddr,
 209         IN PNET_DEV dev,
 210         IN MulticastFilterEntryType type)
 211 {
 212         UCHAR HashIdx;
 213         int i;
 214         MULTICAST_FILTER_TABLE_ENTRY *pEntry = NULL, *pCurrEntry, *pPrevEntry;
 215         PMEMBER_ENTRY pMemberEntry;
 216         PMULTICAST_FILTER_TABLE pMulticastFilterTable = pAd->pMulticastFilterTable;
 217
 218         if (pMulticastFilterTable == NULL)
 219         {
 220                 DBGPRINT(RT_DEBUG_ERROR, ("%s Multicase filter table is not ready.\n", __FUNCTION__));
 221                 return FALSE;
 222         }
 223
 224         // if FULL, return
 225         if (pMulticastFilterTable->Size >= MAX_LEN_OF_MULTICAST_FILTER_TABLE)
 226         {
 227                 DBGPRINT(RT_DEBUG_ERROR, ("%s Multicase filter table full. max-entries = %d\n",
 228                         __FUNCTION__, MAX_LEN_OF_MULTICAST_FILTER_TABLE));
 229                 return FALSE;
 230         }
 231
 232         // check the rule is in table already or not.
 233         if ((pEntry = MulticastFilterTableLookup(pMulticastFilterTable, pGrpId, dev)))
 234         {
 235                 // doesn't indicate member mac address.
 236                 if(pMemberAddr == NULL)
 237                 {
 238                         return FALSE;
 239                 }
 240
 241                 pMemberEntry = (PMEMBER_ENTRY)pEntry->MemberList.pHead;
 242
 243                 while (pMemberEntry)
 244                 {
 245                         if (MAC_ADDR_EQUAL(pMemberAddr, pMemberEntry->Addr))
 246                         {
 247                                 DBGPRINT(RT_DEBUG_ERROR, ("%s: already in Members list.\n", __FUNCTION__));
 248                                 return FALSE;
 249                         }
 250
 251                         pMemberEntry = pMemberEntry->pNext;
 252                 }
 253         }
 254
 255         RTMP_SEM_LOCK(&pMulticastFilterTable->MulticastFilterTabLock);
 256         do
 257         {
 258                 ULONG Now;
 259                 // the multicast entry already exist but doesn't include the member yet.
 260                 if (pEntry != NULL && pMemberAddr != NULL)
 261                 {
 262                         InsertIgmpMember(pMulticastFilterTable, &pEntry->MemberList, pMemberAddr);
 263                         break;
 264                 }
 265
 266                 // allocate one MAC entry
 267                 for (i = 0; i < MAX_LEN_OF_MULTICAST_FILTER_TABLE; i++)
 268                 {
 269                         // pick up the first available vacancy
 270                         pEntry = &pMulticastFilterTable->Content[i];
 271                         NdisGetSystemUpTime(&Now);
 272                         if ((pEntry->Valid == TRUE) && (pEntry->type == MCAT_FILTER_DYNAMIC)
 273                                 && ((Now - pEntry->lastTime) > IGMPMAC_TB_ENTRY_AGEOUT_TIME))
 274                         {
 275                                 PMULTICAST_FILTER_TABLE_ENTRY pHashEntry;
 276
 277                                 HashIdx = MULTICAST_ADDR_HASH_INDEX(pEntry->Addr);
 278                                 pHashEntry = pMulticastFilterTable->Hash[HashIdx];
 279
 280                                 if ((pEntry->net_dev == pHashEntry->net_dev)
 281                                         && MAC_ADDR_EQUAL(pEntry->Addr, pHashEntry->Addr))
 282                                 {
 283                                         pMulticastFilterTable->Hash[HashIdx] = pHashEntry->pNext;
 284                                         pMulticastFilterTable->Size --;
 285                                         DBGPRINT(RT_DEBUG_TRACE, ("MCastFilterTableDeleteEntry 1 - Total= %d\n", pMulticastFilterTable->Size));
 286                                 } else
 287                                 {
 288                                         while (pHashEntry->pNext)
 289                                         {
 290                                                 pPrevEntry = pHashEntry;
 291                                                 pHashEntry = pHashEntry->pNext;
 292                                                 if ((pEntry->net_dev == pHashEntry->net_dev)
 293                                                         && MAC_ADDR_EQUAL(pEntry->Addr, pHashEntry->Addr))
 294                                                 {
 295                                                         pPrevEntry->pNext = pHashEntry->pNext;
 296                                                         pMulticastFilterTable->Size --;
 297                                                         DBGPRINT(RT_DEBUG_TRACE, ("MCastFilterTableDeleteEntry 2 - Total= %d\n", pMulticastFilterTable->Size));
 298                                                         break;
 299                                                 }
 300                                         }
 301                                 }
 302                                 pEntry->Valid = FALSE;
 303                                 DeleteIgmpMemberList(pMulticastFilterTable, &pEntry->MemberList);
 304                         }
 305
 306                         if (pEntry->Valid == FALSE)
 307                         {
 308                                 NdisZeroMemory(pEntry, sizeof(MULTICAST_FILTER_TABLE_ENTRY));
 309                                 pEntry->Valid = TRUE;
 310
 311                                 COPY_MAC_ADDR(pEntry->Addr, pGrpId);
 312                                 pEntry->net_dev = dev;
 313                                 NdisGetSystemUpTime(&Now);
 314                                 pEntry->lastTime = Now;
 315                                 pEntry->type = type;
 316                                 initList(&pEntry->MemberList);
 317                                 if (pMemberAddr != NULL)
 318                                         InsertIgmpMember(pMulticastFilterTable, &pEntry->MemberList, pMemberAddr);
 319
 320                                 pMulticastFilterTable->Size ++;
 321
 322                                 DBGPRINT(RT_DEBUG_TRACE, ("MulticastFilterTableInsertEntry -IF(%s) allocate entry #%d, Total= %d\n", RTMP_OS_NETDEV_GET_DEVNAME(dev), i, pMulticastFilterTable->Size));
 323                                 break;
 324                         }
 325                 }
 326
 327                 // add this MAC entry into HASH table
 328                 if (pEntry)
 329                 {
 330                         HashIdx = MULTICAST_ADDR_HASH_INDEX(pGrpId);
 331                         if (pMulticastFilterTable->Hash[HashIdx] == NULL)
 332                         {
 333                                 pMulticastFilterTable->Hash[HashIdx] = pEntry;
 334                         } else
 335                         {
 336                                 pCurrEntry = pMulticastFilterTable->Hash[HashIdx];
 337                                 while (pCurrEntry->pNext != NULL)
 338                                         pCurrEntry = pCurrEntry->pNext;
 339                                 pCurrEntry->pNext = pEntry;
 340                         }
 341                 }
 342         }while(FALSE);
 343
 344         RTMP_SEM_UNLOCK(&pMulticastFilterTable->MulticastFilterTabLock);
 345
 346         return TRUE;
 347 }
 348
 349 /*
 350     ==========================================================================
 351     Description:
 352         Delete a specified client from MAC table
 353     ==========================================================================
 354  */
 355 BOOLEAN MulticastFilterTableDeleteEntry(
 356         IN PRTMP_ADAPTER pAd,
 357         IN PUCHAR pGrpId,
 358         IN PUCHAR pMemberAddr,
 359         IN PNET_DEV dev)
 360 {
 361         USHORT HashIdx;
 362         MULTICAST_FILTER_TABLE_ENTRY *pEntry, *pPrevEntry;
 363         PMULTICAST_FILTER_TABLE pMulticastFilterTable = pAd->pMulticastFilterTable;
 364         USHORT Aid = MCAST_WCID;
 365         SST     Sst = SST_ASSOC;
 366         UCHAR PsMode = PWR_ACTIVE, Rate;
 367
 368         if (pMulticastFilterTable == NULL)
 369         {
 370                 DBGPRINT(RT_DEBUG_ERROR, ("%s Multicase filter table is not ready.\n", __FUNCTION__));
 371                 return FALSE;
 372         }
 373
 374         RTMP_SEM_LOCK(&pMulticastFilterTable->MulticastFilterTabLock);
 375
 376         do
 377         {
 378                 HashIdx = MULTICAST_ADDR_HASH_INDEX(pGrpId);
 379                 pPrevEntry = pEntry = pMulticastFilterTable->Hash[HashIdx];
 380
 381                 while (pEntry && pEntry->Valid)
 382                 {
 383                         if ((pEntry->net_dev ==  dev)
 384                                 && MAC_ADDR_EQUAL(pEntry->Addr, pGrpId))
 385                                 break;
 386                         else
 387                         {
 388                                 pPrevEntry = pEntry;
 389                                 pEntry = pEntry->pNext;
 390                         }
 391                 }
 392
 393                 // check the rule is in table already or not.
 394                 if (pEntry && (pMemberAddr != NULL))
 395                 {
 396                         if(APSsPsInquiry(pAd, pMemberAddr, &Sst, &Aid, &PsMode, &Rate))
 397                                 DeleteIgmpMember(pMulticastFilterTable, &pEntry->MemberList, pMemberAddr);
 398                         if (IgmpMemberCnt(&pEntry->MemberList) > 0)
 399                                 break;
 400                 }
 401
 402                 if (pEntry)
 403                 {
 404                         if (pEntry == pMulticastFilterTable->Hash[HashIdx])
 405                         {
 406                                 pMulticastFilterTable->Hash[HashIdx] = pEntry->pNext;
 407                                 DeleteIgmpMemberList(pMulticastFilterTable, &pEntry->MemberList);
 408                                 NdisZeroMemory(pEntry, sizeof(MULTICAST_FILTER_TABLE_ENTRY));
 409                                 pMulticastFilterTable->Size --;
 410                                 DBGPRINT(RT_DEBUG_TRACE, ("MCastFilterTableDeleteEntry 1 - Total= %d\n", pMulticastFilterTable->Size));
 411                         }
 412                         else
 413                         {
 414                                 pPrevEntry->pNext = pEntry->pNext;
 415                                 DeleteIgmpMemberList(pMulticastFilterTable, &pEntry->MemberList);
 416                                 NdisZeroMemory(pEntry, sizeof(MULTICAST_FILTER_TABLE_ENTRY));
 417                                 pMulticastFilterTable->Size --;
 418                                 DBGPRINT(RT_DEBUG_TRACE, ("MCastFilterTableDeleteEntry 2 - Total= %d\n", pMulticastFilterTable->Size));
 419                         }
 420                 }
 421                 else
 422                 {
 423                         DBGPRINT(RT_DEBUG_ERROR, ("%s: the Group doesn't exist.\n", __FUNCTION__));
 424                 }
 425         } while(FALSE);
 426
 427         RTMP_SEM_UNLOCK(&pMulticastFilterTable->MulticastFilterTabLock);
 428
 429         return TRUE;
 430 }
 431
 432 /*
 433     ==========================================================================
 434     Description:
 435         Look up the MAC address in the IGMP table. Return NULL if not found.
 436     Return:
 437         pEntry - pointer to the MAC entry; NULL is not found
 438     ==========================================================================
 439 */
 440 PMULTICAST_FILTER_TABLE_ENTRY MulticastFilterTableLookup(
 441         IN PMULTICAST_FILTER_TABLE pMulticastFilterTable,
 442         IN PUCHAR pAddr,
 443         IN PNET_DEV dev)
 444 {
 445         ULONG HashIdx, Now;
 446         PMULTICAST_FILTER_TABLE_ENTRY pEntry = NULL, pPrev = NULL;
 447
 448         if (pMulticastFilterTable == NULL)
 449         {
 450                 DBGPRINT(RT_DEBUG_ERROR, ("%s Multicase filter table is not ready.\n", __FUNCTION__));
 451                 return NULL;
 452         }
 453
 454         RTMP_SEM_LOCK(&pMulticastFilterTable->MulticastFilterTabLock);
 455
 456         HashIdx = MULTICAST_ADDR_HASH_INDEX(pAddr);
 457         pEntry = pPrev = pMulticastFilterTable->Hash[HashIdx];
 458
 459         while (pEntry && pEntry->Valid)
 460         {
 461                 if ((pEntry->net_dev ==  dev)
 462                         && MAC_ADDR_EQUAL(pEntry->Addr, pAddr))
 463                 {
 464                         NdisGetSystemUpTime(&Now);
 465                         pEntry->lastTime = Now;
 466                         break;
 467                 }
 468                 else
 469                 {
 470                         NdisGetSystemUpTime(&Now);
 471                         if ((pEntry->Valid == TRUE) && (pEntry->type == MCAT_FILTER_DYNAMIC)
 472                                 && RTMP_TIME_AFTER(Now, pEntry->lastTime+IGMPMAC_TB_ENTRY_AGEOUT_TIME))
 473                         {
 474                                 // Remove the aged entry
 475                                 if (pEntry == pMulticastFilterTable->Hash[HashIdx])
 476                                 {
 477                                         pMulticastFilterTable->Hash[HashIdx] = pEntry->pNext;
 478                                         pPrev = pMulticastFilterTable->Hash[HashIdx];
 479                                         DeleteIgmpMemberList(pMulticastFilterTable, &pEntry->MemberList);
 480                                         NdisZeroMemory(pEntry, sizeof(MULTICAST_FILTER_TABLE_ENTRY));
 481                                         pMulticastFilterTable->Size --;
 482                                         pEntry = pPrev;
 483                                         DBGPRINT(RT_DEBUG_TRACE, ("MCastFilterTableDeleteEntry 2 - Total= %d\n", pMulticastFilterTable->Size));
 484                                 }
 485                                 else
 486                                 {
 487                                         pPrev->pNext = pEntry->pNext;
 488                                         DeleteIgmpMemberList(pMulticastFilterTable, &pEntry->MemberList);
 489                                         NdisZeroMemory(pEntry, sizeof(MULTICAST_FILTER_TABLE_ENTRY));
 490                                         pMulticastFilterTable->Size --;
 491                                         pEntry = (pPrev == NULL ? NULL: pPrev->pNext);
 492                                         DBGPRINT(RT_DEBUG_TRACE, ("MCastFilterTableDeleteEntry 2 - Total= %d\n", pMulticastFilterTable->Size));
 493                                 }
 494                         }
 495                         else
 496                         {
 497                                 pPrev = pEntry;
 498                                 pEntry = pEntry->pNext;
 499                         }
 500                 }
 501         }
 502
 503         RTMP_SEM_UNLOCK(&pMulticastFilterTable->MulticastFilterTabLock);
 504
 505         return pEntry;
 506 }
 507
 508 VOID IGMPSnooping(
 509         IN PRTMP_ADAPTER pAd,
 510         IN PUCHAR pDstMacAddr,
 511         IN PUCHAR pSrcMacAddr,
 512         IN PUCHAR pIpHeader,
 513         IN PNET_DEV pDev)
 514 {
 515         INT i;
 516         INT IpHeaderLen;
 517         UCHAR GroupType;
 518         UINT16 numOfGroup;
 519         UCHAR IgmpVerType;
 520         PUCHAR pIgmpHeader;
 521         PUCHAR pGroup;
 522         UCHAR AuxDataLen;
 523         UINT16 numOfSources;
 524         PUCHAR pGroupIpAddr;
 525         UCHAR GroupMacAddr[6];
 526         PUCHAR pGroupMacAddr = (PUCHAR)&GroupMacAddr;
 527
 528         if(isIgmpPkt(pDstMacAddr, pIpHeader))
 529         {
 530                 IpHeaderLen = (*(pIpHeader + 2) & 0x0f) * 4;
 531                 pIgmpHeader = pIpHeader + 2 + IpHeaderLen;
 532                 IgmpVerType = (UCHAR)(*(pIgmpHeader));
 533
 534                 DBGPRINT(RT_DEBUG_TRACE, ("IGMP type=%0x\n", IgmpVerType));
 535
 536                 switch(IgmpVerType)
 537                 {
 538                 case IGMP_V1_MEMBERSHIP_REPORT: // IGMP version 1 membership report.
 539                 case IGMP_V2_MEMBERSHIP_REPORT: // IGMP version 2 membership report.
 540                         pGroupIpAddr = (PUCHAR)(pIgmpHeader + 4);
 541                                 ConvertMulticastIP2MAC(pGroupIpAddr, (PUCHAR *)&pGroupMacAddr, ETH_P_IP);
 542                         DBGPRINT(RT_DEBUG_TRACE, ("IGMP Group=%02x:%02x:%02x:%02x:%02x:%02x\n",
 543                                 GroupMacAddr[0], GroupMacAddr[1], GroupMacAddr[2], GroupMacAddr[3], GroupMacAddr[4], GroupMacAddr[5]));
 544                         MulticastFilterTableInsertEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev, MCAT_FILTER_DYNAMIC);
 545                         break;
 546
 547                 case IGMP_LEAVE_GROUP: // IGMP version 1 and version 2 leave group.
 548                         pGroupIpAddr = (PUCHAR)(pIgmpHeader + 4);
 549                                 ConvertMulticastIP2MAC(pGroupIpAddr, (PUCHAR *)&pGroupMacAddr, ETH_P_IP);
 550                         DBGPRINT(RT_DEBUG_TRACE, ("IGMP Group=%02x:%02x:%02x:%02x:%02x:%02x\n",
 551                                 GroupMacAddr[0], GroupMacAddr[1], GroupMacAddr[2], GroupMacAddr[3], GroupMacAddr[4], GroupMacAddr[5]));
 552                         MulticastFilterTableDeleteEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev);
 553                         break;
 554
 555                 case IGMP_V3_MEMBERSHIP_REPORT: // IGMP version 3 membership report.
 556                         numOfGroup = ntohs(*((UINT16 *)(pIgmpHeader + 6)));
 557                         pGroup = (PUCHAR)(pIgmpHeader + 8);
 558                         for (i=0; i < numOfGroup; i++)
 559                         {
 560                                 GroupType = (UCHAR)(*pGroup);
 561                                 AuxDataLen = (UCHAR)(*(pGroup + 1));
 562                                 numOfSources = ntohs(*((UINT16 *)(pGroup + 2)));
 563                                 pGroupIpAddr = (PUCHAR)(pGroup + 4);
 564                                 DBGPRINT(RT_DEBUG_TRACE, ("IGMPv3 Type=%d, ADL=%d, numOfSource=%d\n", GroupType, AuxDataLen, numOfSources));
 565                                         ConvertMulticastIP2MAC(pGroupIpAddr, (PUCHAR *)&pGroupMacAddr, ETH_P_IP);
 566                                 DBGPRINT(RT_DEBUG_TRACE, ("IGMP Group=%02x:%02x:%02x:%02x:%02x:%02x\n",
 567                                         GroupMacAddr[0], GroupMacAddr[1], GroupMacAddr[2], GroupMacAddr[3], GroupMacAddr[4], GroupMacAddr[5]));
 568
 569                                 do
 570                                 {
 571                                         if((GroupType == MODE_IS_EXCLUDE) || (GroupType == CHANGE_TO_EXCLUDE_MODE) || (GroupType == ALLOW_NEW_SOURCES))
 572                                         {
 573                                                 MulticastFilterTableInsertEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev, MCAT_FILTER_DYNAMIC);
 574                                                 break;
 575                                         }
 576
 577                                         if((GroupType == MODE_IS_INCLUDE) || (GroupType == BLOCK_OLD_SOURCES))
 578                                         {
 579                                                 MulticastFilterTableDeleteEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev);
 580                                                 break;
 581                                         }
 582
 583                                         if((GroupType == CHANGE_TO_INCLUDE_MODE))
 584                                         {
 585                                                 if(numOfSources == 0)
 586                                                         MulticastFilterTableDeleteEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev);
 587                                                 else
 588                                                         MulticastFilterTableInsertEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev, MCAT_FILTER_DYNAMIC);
 589                                                 break;
 590                                         }
 591                                 } while(FALSE);
 592                                 pGroup += (8 + (numOfSources * 4) + AuxDataLen);
 593                         }
 594                         break;
 595
 596                 default:
 597                         DBGPRINT(RT_DEBUG_TRACE, ("unknow IGMP Type=%d\n", IgmpVerType));
 598                         break;
 599                 }
 600         }
 601
 602         return;
 603 }
 604
 605
 606 static BOOLEAN isIgmpMacAddr(
 607         IN PUCHAR pMacAddr)
 608 {
 609         if((pMacAddr[0] == 0x01)
 610                 && (pMacAddr[1] == 0x00)
 611                 && (pMacAddr[2] == 0x5e))
 612                 return TRUE;
 613         return FALSE;
 614 }
 615
 616 BOOLEAN isIgmpPkt(
 617         IN PUCHAR pDstMacAddr,
 618         IN PUCHAR pIpHeader)
 619 {
 620         UINT16 IpProtocol = ntohs(*((UINT16 *)(pIpHeader)));
 621         UCHAR IgmpProtocol;
 622
 623         if(!isIgmpMacAddr(pDstMacAddr))
 624                 return FALSE;
 625
 626         if(IpProtocol == ETH_P_IP)
 627         {
 628                 IgmpProtocol = (UCHAR)*(pIpHeader + 11);
 629                 if(IgmpProtocol == IGMP_PROTOCOL_DESCRIPTOR)
 630                                 return TRUE;
 631         }
 632
 633         return FALSE;
 634 }
 635
 636 static VOID InsertIgmpMember(
 637         IN PMULTICAST_FILTER_TABLE pMulticastFilterTable,
 638         IN PLIST_HEADER pList,
 639         IN PUCHAR pMemberAddr)
 640 {
 641         PMEMBER_ENTRY pMemberEntry;
 642
 643         if(pList == NULL)
 644         {
 645                 DBGPRINT(RT_DEBUG_ERROR, ("%s: membert list doesn't exist.\n", __FUNCTION__));
 646                 return;
 647         }
 648
 649         if (pMemberAddr == NULL)
 650         {
 651                 DBGPRINT(RT_DEBUG_ERROR, ("%s: invalid member.\n", __FUNCTION__));
 652                 return;
 653         }
 654
 655         if((pMemberEntry = (PMEMBER_ENTRY)AllocaGrpMemberEntry(pMulticastFilterTable)) != NULL)
 656         {
 657                 NdisZeroMemory(pMemberEntry, sizeof(MEMBER_ENTRY));
 658                 COPY_MAC_ADDR(pMemberEntry->Addr, pMemberAddr);
 659                 insertTailList(pList, (PLIST_ENTRY)pMemberEntry);
 660
 661                 DBGPRINT(RT_DEBUG_TRACE, ("%s Member Mac=%02x:%02x:%02x:%02x:%02x:%02x\n", __FUNCTION__,
 662                         pMemberEntry->Addr[0], pMemberEntry->Addr[1], pMemberEntry->Addr[2],
 663                         pMemberEntry->Addr[3], pMemberEntry->Addr[4], pMemberEntry->Addr[5]));
 664         }
 665         return;
 666 }
 667
 668 static VOID DeleteIgmpMember(
 669         IN PMULTICAST_FILTER_TABLE pMulticastFilterTable,
 670         IN PLIST_HEADER pList,
 671         IN PUCHAR pMemberAddr)
 672 {
 673         PMEMBER_ENTRY pCurEntry;
 674
 675         if((pList == NULL) || (pList->pHead == NULL))
 676         {
 677                 DBGPRINT(RT_DEBUG_ERROR, ("%s: membert list doesn't exist.\n", __FUNCTION__));
 678                 return;
 679         }
 680
 681         if (pMemberAddr == NULL)
 682         {
 683                 DBGPRINT(RT_DEBUG_ERROR, ("%s: invalid member.\n", __FUNCTION__));
 684                 return;
 685         }
 686
 687         pCurEntry = (PMEMBER_ENTRY)pList->pHead;
 688         while (pCurEntry)
 689         {
 690                 if(MAC_ADDR_EQUAL(pMemberAddr, pCurEntry->Addr))
 691                 {
 692                         delEntryList(pList, (PLIST_ENTRY)pCurEntry);
 693                         FreeGrpMemberEntry(pMulticastFilterTable, pCurEntry);
 694                         break;
 695                 }
 696                 pCurEntry = pCurEntry->pNext;
 697         }
 698
 699         return;
 700 }
 701
 702 static VOID DeleteIgmpMemberList(
 703         IN PMULTICAST_FILTER_TABLE pMulticastFilterTable,
 704         IN PLIST_HEADER pList)
 705 {
 706         PMEMBER_ENTRY pCurEntry, pPrvEntry;
 707
 708         if((pList == NULL) || (pList->pHead == NULL))
 709         {
 710                 DBGPRINT(RT_DEBUG_ERROR, ("%s: membert list doesn't exist.\n", __FUNCTION__));
 711                 return;
 712         }
 713
 714         pPrvEntry = pCurEntry = (PMEMBER_ENTRY)pList->pHead;
 715         while (pCurEntry)
 716         {
 717                 delEntryList(pList, (PLIST_ENTRY)pCurEntry);
 718                 pPrvEntry = pCurEntry;
 719                 pCurEntry = pCurEntry->pNext;
 720                 FreeGrpMemberEntry(pMulticastFilterTable, pPrvEntry);
 721         }
 722
 723         initList(pList);
 724         return;
 725 }
 726
 727
 728 UCHAR IgmpMemberCnt(
 729         IN PLIST_HEADER pList)
 730 {
 731         if(pList == NULL)
 732         {
 733                 DBGPRINT(RT_DEBUG_ERROR, ("%s: membert list doesn't exist.\n", __FUNCTION__));
 734                 return 0;
 735         }
 736
 737         return getListSize(pList);
 738 }
 739
 740 VOID IgmpGroupDelMembers(
 741         IN PRTMP_ADAPTER pAd,
 742         IN PUCHAR pMemberAddr,
 743         IN PNET_DEV pDev)
 744 {
 745         INT i;
 746         MULTICAST_FILTER_TABLE_ENTRY *pEntry = NULL;
 747         PMULTICAST_FILTER_TABLE pMulticastFilterTable = pAd->pMulticastFilterTable;
 748
 749         for (i = 0; i < MAX_LEN_OF_MULTICAST_FILTER_TABLE; i++)
 750         {
 751                 // pick up the first available vacancy
 752                 pEntry = &pMulticastFilterTable->Content[i];
 753                 if (pEntry->Valid == TRUE)
 754                 {
 755                         if(pMemberAddr != NULL)
 756                         {
 757                                 RTMP_SEM_LOCK(&pMulticastFilterTable->MulticastFilterTabLock);
 758                                 DeleteIgmpMember(pMulticastFilterTable, &pEntry->MemberList, pMemberAddr);
 759                                 RTMP_SEM_UNLOCK(&pMulticastFilterTable->MulticastFilterTabLock);
 760                         }
 761
 762                         if((pEntry->type == MCAT_FILTER_DYNAMIC)
 763                                 && (IgmpMemberCnt(&pEntry->MemberList) == 0))
 764                                 MulticastFilterTableDeleteEntry(pAd, pEntry->Addr, pMemberAddr, pDev);
 765                 }
 766         }
 767 }
 768
 769 INT Set_IgmpSn_Enable_Proc(
 770         IN PRTMP_ADAPTER pAd,
 771         IN PSTRING arg)
 772 {
 773         UINT Enable;
 774         POS_COOKIE pObj;
 775         UCHAR ifIndex;
 776         PNET_DEV pDev;
 777
 778         pObj = (POS_COOKIE) pAd->OS_Cookie;
 779         ifIndex = pObj->ioctl_if;
 780
 781         pDev = (ifIndex == MAIN_MBSSID) ? (pAd->net_dev) : (pAd->ApCfg.MBSSID[ifIndex].MSSIDDev);
 782         Enable = (UINT) simple_strtol(arg, 0, 10);
 783
 784         pAd->ApCfg.MBSSID[ifIndex].IgmpSnoopEnable = (BOOLEAN)(Enable == 0 ? 0 : 1);
 785         DBGPRINT(RT_DEBUG_TRACE, ("%s::(%s) %s\n", __FUNCTION__, RTMP_OS_NETDEV_GET_DEVNAME(pDev), Enable == TRUE ? "Enable IGMP Snooping":"Disable IGMP Snooping"));
 786
 787         return TRUE;
 788 }
 789
 790 INT Set_IgmpSn_AddEntry_Proc(
 791         IN PRTMP_ADAPTER pAd,
 792         IN PSTRING arg)
 793 {
 794         INT i;
 795         BOOLEAN bGroupId = 1;
 796         PSTRING value;
 797         PSTRING thisChar;
 798         UCHAR IpAddr[4];
 799         UCHAR Addr[ETH_LENGTH_OF_ADDRESS];
 800         UCHAR GroupId[ETH_LENGTH_OF_ADDRESS];
 801         PUCHAR *pAddr = (PUCHAR *)&Addr;
 802         PNET_DEV pDev;
 803         POS_COOKIE pObj;
 804         UCHAR ifIndex;
 805
 806         pObj = (POS_COOKIE) pAd->OS_Cookie;
 807         ifIndex = pObj->ioctl_if;
 808
 809         pDev = (ifIndex == MAIN_MBSSID) ? (pAd->net_dev) : (pAd->ApCfg.MBSSID[ifIndex].MSSIDDev);
 810
 811         while ((thisChar = strsep((char **)&arg, "-")) != NULL)
 812         {
 813                 // refuse the Member if it's not a MAC address.
 814                 if((bGroupId == 0) && (strlen(thisChar) != 17))
 815                         continue;
 816
 817                 if(strlen(thisChar) == 17)  //Mac address acceptable format 01:02:03:04:05:06 length 17
 818                 {
 819                         for (i=0, value = rstrtok(thisChar,":"); value; value = rstrtok(NULL,":"))
 820                         {
 821                                 if((strlen(value) != 2) || (!isxdigit(*value)) || (!isxdigit(*(value+1))) )
 822                                         return FALSE;  //Invalid
 823
 824                                 AtoH(value, &Addr[i++], 1);
 825                         }
 826
 827                         if(i != 6)
 828                                 return FALSE;  //Invalid
 829                 }
 830                 else
 831                 {
 832                         for (i=0, value = rstrtok(thisChar,"."); value; value = rstrtok(NULL,"."))
 833                         {
 834                                 if((strlen(value) > 0) && (strlen(value) <= 3))
 835                                 {
 836                                         int ii;
 837                                         for(ii=0; ii<strlen(value); ii++)
 838                                                 if (!isxdigit(*(value + ii)))
 839                                                         return FALSE;
 840                                 }
 841                                 else
 842                                         return FALSE;  //Invalid
 843
 844                                 IpAddr[i] = (UCHAR)simple_strtol(value, NULL, 10);
 845                                 i++;
 846                         }
 847
 848                         if(i != 4)
 849                                 return FALSE;  //Invalid
 850
 851                         ConvertMulticastIP2MAC(IpAddr, (PUCHAR *)&pAddr, ETH_P_IP);
 852                 }
 853
 854                 if(bGroupId == 1)
 855                         COPY_MAC_ADDR(GroupId, Addr);
 856
 857                 // Group-Id must be a MCAST address.
 858                 if((bGroupId == 1) && IS_MULTICAST_MAC_ADDR(Addr))
 859                         MulticastFilterTableInsertEntry(pAd, GroupId, NULL, pDev, MCAT_FILTER_STATIC);
 860                 // Group-Member must be a UCAST address.
 861                 else if ((bGroupId == 0) && !IS_MULTICAST_MAC_ADDR(Addr))
 862                         MulticastFilterTableInsertEntry(pAd, GroupId, Addr, pDev, MCAT_FILTER_STATIC);
 863                 else
 864                 {
 865                         DBGPRINT(RT_DEBUG_TRACE, ("%s (%2X:%2X:%2X:%2X:%2X:%2X) is not a acceptable address.\n",
 866                                 __FUNCTION__, Addr[0], Addr[1], Addr[2], Addr[3], Addr[4], Addr[5]));
 867                         return FALSE;
 868                 }
 869
 870                 bGroupId = 0;
 871                 DBGPRINT(RT_DEBUG_TRACE, ("%s (%2X:%2X:%2X:%2X:%2X:%2X)\n",
 872                         __FUNCTION__, Addr[0], Addr[1], Addr[2], Addr[3], Addr[4], Addr[5]));
 873
 874         }
 875
 876         return TRUE;
 877 }
 878
 879 INT Set_IgmpSn_DelEntry_Proc(
 880         IN PRTMP_ADAPTER pAd,
 881         IN PSTRING arg)
 882 {
 883         INT i, memberCnt = 0;
 884         BOOLEAN bGroupId = 1;
 885         PSTRING value;
 886         PSTRING thisChar;
 887         UCHAR IpAddr[4];
 888         UCHAR Addr[ETH_LENGTH_OF_ADDRESS];
 889         UCHAR GroupId[ETH_LENGTH_OF_ADDRESS];
 890         PUCHAR *pAddr = (PUCHAR *)&Addr;
 891         PNET_DEV pDev;
 892         POS_COOKIE pObj;
 893         UCHAR ifIndex;
 894
 895         pObj = (POS_COOKIE) pAd->OS_Cookie;
 896         ifIndex = pObj->ioctl_if;
 897
 898         pDev = (ifIndex == MAIN_MBSSID) ? (pAd->net_dev) : (pAd->ApCfg.MBSSID[ifIndex].MSSIDDev);
 899
 900         while ((thisChar = strsep((char **)&arg, "-")) != NULL)
 901         {
 902                 // refuse the Member if it's not a MAC address.
 903                 if((bGroupId == 0) && (strlen(thisChar) != 17))
 904                         continue;
 905
 906                 if(strlen(thisChar) == 17)  //Mac address acceptable format 01:02:03:04:05:06 length 17
 907                 {
 908                         for (i=0, value = rstrtok(thisChar,":"); value; value = rstrtok(NULL,":"))
 909                         {
 910                                 if((strlen(value) != 2) || (!isxdigit(*value)) || (!isxdigit(*(value+1))) )
 911                                         return FALSE;  //Invalid
 912
 913                                 AtoH(value, &Addr[i++], 1);
 914                         }
 915
 916                         if(i != 6)
 917                                 return FALSE;  //Invalid
 918                 }
 919                 else
 920                 {
 921                         for (i=0, value = rstrtok(thisChar,"."); value; value = rstrtok(NULL,"."))
 922                         {
 923                                 if((strlen(value) > 0) && (strlen(value) <= 3))
 924                                 {
 925                                         int ii;
 926                                         for(ii=0; ii<strlen(value); ii++)
 927                                                 if (!isxdigit(*(value + ii)))
 928                                                         return FALSE;
 929                                 }
 930                                 else
 931                                         return FALSE;  //Invalid
 932
 933                                 IpAddr[i] = (UCHAR)simple_strtol(value, NULL, 10);
 934                                 i++;
 935                         }
 936
 937                         if(i != 4)
 938                                 return FALSE;  //Invalid
 939
 940                         ConvertMulticastIP2MAC(IpAddr, (PUCHAR *)&pAddr, ETH_P_IP);
 941                 }
 942
 943                 if(bGroupId == 1)
 944                         COPY_MAC_ADDR(GroupId, Addr);
 945                 else
 946                         memberCnt++;
 947
 948                 if (memberCnt > 0 )
 949                         MulticastFilterTableDeleteEntry(pAd, (PUCHAR)GroupId, Addr, pDev);
 950
 951                 bGroupId = 0;
 952         }
 953
 954         if(memberCnt == 0)
 955                 MulticastFilterTableDeleteEntry(pAd, (PUCHAR)GroupId, NULL, pDev);
 956
 957         DBGPRINT(RT_DEBUG_TRACE, ("%s (%2X:%2X:%2X:%2X:%2X:%2X)\n",
 958                 __FUNCTION__, Addr[0], Addr[1], Addr[2], Addr[3], Addr[4], Addr[5]));
 959
 960         return TRUE;
 961 }
 962
 963 INT Set_IgmpSn_TabDisplay_Proc(
 964         IN PRTMP_ADAPTER pAd,
 965         IN PSTRING arg)
 966 {
 967         IGMPTableDisplay(pAd);
 968         return TRUE;
 969 }
 970
 971 void rtmp_read_igmp_snoop_from_file(
 972         IN  PRTMP_ADAPTER pAd,
 973         PSTRING tmpbuf,
 974         PSTRING buffer)
 975 {
 976         PSTRING         macptr;
 977         INT                     i=0;
 978
 979         //IgmpSnEnable
 980         if(RTMPGetKeyParameter("IgmpSnEnable", tmpbuf, 128, buffer, TRUE))
 981         {
 982                 for (i = 0, macptr = rstrtok(tmpbuf,";"); (macptr && i < pAd->ApCfg.BssidNum); macptr = rstrtok(NULL,";"), i++)
 983                 {
 984                         if ((strncmp(macptr, "0", 1) == 0))
 985                                 pAd->ApCfg.MBSSID[i].IgmpSnoopEnable = FALSE;
 986                         else if ((strncmp(macptr, "1", 1) == 0))
 987                                 pAd->ApCfg.MBSSID[i].IgmpSnoopEnable = TRUE;
 988                 else
 989                                 pAd->ApCfg.MBSSID[i].IgmpSnoopEnable = FALSE;
 990
 991                         DBGPRINT(RT_DEBUG_TRACE, ("MBSSID[%d].Enable=%d\n", i, pAd->ApCfg.MBSSID[i].IgmpSnoopEnable));
 992             }
 993         }
 994 }
 995
 996 NDIS_STATUS IgmpPktInfoQuery(
 997         IN PRTMP_ADAPTER pAd,
 998         IN PUCHAR pSrcBufVA,
 999         IN PNDIS_PACKET pPacket,
1000         IN UCHAR apidx,
1001         OUT BOOLEAN *pInIgmpGroup,
1002         OUT PMULTICAST_FILTER_TABLE_ENTRY *ppGroupEntry)
1003 {
1004         if(IS_MULTICAST_MAC_ADDR(pSrcBufVA))
1005         {
1006                 BOOLEAN IgmpMldPkt = FALSE;
1007                 PUCHAR pIpHeader = pSrcBufVA + 12;
1008
1009                 if(ntohs(*((UINT16 *)(pIpHeader))) == ETH_P_IPV6)
1010                         IgmpMldPkt = isMldPkt(pSrcBufVA, pIpHeader, NULL, NULL);
1011                 else
1012                         IgmpMldPkt = isIgmpPkt(pSrcBufVA, pIpHeader);
1013
1014                 if (IgmpMldPkt)
1015                 {
1016                         *ppGroupEntry = NULL;
1017                 }
1018                 else if ((*ppGroupEntry = MulticastFilterTableLookup(pAd->pMulticastFilterTable, pSrcBufVA,
1019                                                                         pAd->ApCfg.MBSSID[apidx].MSSIDDev)) == NULL)
1020                 {
1021                         RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
1022                         return NDIS_STATUS_FAILURE;
1023                 }
1024                 *pInIgmpGroup = TRUE;
1025         }
1026         else if (IS_BROADCAST_MAC_ADDR(pSrcBufVA))
1027         {
1028                 PUCHAR pDstIpAddr = pSrcBufVA + 30; // point to Destination of Ip address of IP header.
1029                 UCHAR GroupMacAddr[6];
1030                 PUCHAR pGroupMacAddr = (PUCHAR)&GroupMacAddr;
1031
1032                 ConvertMulticastIP2MAC(pDstIpAddr, (PUCHAR *)&pGroupMacAddr, ETH_P_IP);
1033                 if ((*ppGroupEntry = MulticastFilterTableLookup(pAd->pMulticastFilterTable, pGroupMacAddr,
1034                                                                 pAd->ApCfg.MBSSID[apidx].MSSIDDev)) != NULL)
1035                 {
1036                         *pInIgmpGroup = TRUE;
1037                 }
1038         }
1039         return NDIS_STATUS_SUCCESS;
1040 }
1041
1042 NDIS_STATUS IgmpPktClone(
1043         IN PRTMP_ADAPTER pAd,
1044         IN PNDIS_PACKET pPacket,
1045         IN UCHAR QueIdx,
1046         IN PMULTICAST_FILTER_TABLE_ENTRY pGroupEntry)
1047 {
1048         PNDIS_PACKET pSkbClone = NULL;
1049         PMEMBER_ENTRY pMemberEntry = (PMEMBER_ENTRY)pGroupEntry->MemberList.pHead;
1050         MAC_TABLE_ENTRY *pMacEntry = NULL;
1051         USHORT Aid;
1052         SST     Sst = SST_ASSOC;
1053         UCHAR PsMode = PWR_ACTIVE;
1054         UCHAR Rate;
1055         unsigned long IrqFlags;
1056
1057         // check all members of the IGMP group.
1058         while(pMemberEntry != NULL)
1059         {
1060                 pMacEntry = APSsPsInquiry(pAd, pMemberEntry->Addr, &Sst, &Aid, &PsMode, &Rate);
1061
1062                 if (pMacEntry && (Sst == SST_ASSOC) && (PsMode != PWR_SAVE))
1063                 {
1064                 pSkbClone = skb_clone(RTPKT_TO_OSPKT(pPacket), MEM_ALLOC_FLAG);
1065                 if(pSkbClone)
1066                 {
1067                                 RTMP_SET_PACKET_WCID(pSkbClone, (UCHAR)Aid);
1068                                 // Pkt type must set to PKTSRC_NDIS.
1069                                 // It cause of the deason that APHardTransmit()
1070                                 // doesn't handle PKTSRC_DRIVER pkt type in version 1.3.0.0.
1071                                 RTMP_SET_PACKET_SOURCE(pSkbClone, PKTSRC_NDIS);
1072                         }
1073                         else
1074                         {
1075                                 pMemberEntry = pMemberEntry->pNext;
1076                                 continue;
1077                         }
1078
1079                         // insert the pkt to TxSwQueue.
1080                         if (pAd->TxSwQueue[QueIdx].Number >= MAX_PACKETS_IN_QUEUE)
1081                         {
1082 #ifdef BLOCK_NET_IF
1083                                 StopNetIfQueue(pAd, QueIdx, pSkbClone);
1084 #endif // BLOCK_NET_IF //
1085                                 RELEASE_NDIS_PACKET(pAd, pSkbClone, NDIS_STATUS_FAILURE);
1086                                 return NDIS_STATUS_FAILURE;
1087                         }
1088                         else
1089                         {
1090                                 RTMP_IRQ_LOCK(&pAd->irq_lock, IrqFlags);
1091                                 InsertTailQueueAc(pAd, pMacEntry, &pAd->TxSwQueue[QueIdx], PACKET_TO_QUEUE_ENTRY(pSkbClone));
1092                                 RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
1093                         }
1094                 }
1095                 pMemberEntry = pMemberEntry->pNext;
1096         }
1097         return NDIS_STATUS_SUCCESS;
1098 }
1099
1100 static inline BOOLEAN isMldMacAddr(
1101         IN PUCHAR pMacAddr)
1102 {
1103         return ((pMacAddr[0] == 0x33) && (pMacAddr[1] == 0x33)) ? TRUE : FALSE;
1104 }
1105
1106 static inline BOOLEAN IsSupportedMldMsg(
1107         IN UINT8 MsgType)
1108 {
1109         BOOLEAN result = FALSE;
1110         switch(MsgType)
1111         {
1112                 case MLD_V1_LISTENER_REPORT:
1113                 case MLD_V1_LISTENER_DONE:
1114                 case MLD_V2_LISTERNER_REPORT:
1115                         result = TRUE;
1116                         break;
1117                 default:
1118                         result = FALSE;
1119                         break;
1120         }
1121
1122         return result;
1123 }
1124
1125 BOOLEAN isMldPkt(
1126         IN PUCHAR pDstMacAddr,
1127         IN PUCHAR pIpHeader,
1128         OUT UINT8 *pProtoType,
1129         OUT PUCHAR *pMldHeader)
1130 {
1131         BOOLEAN result = FALSE;
1132         UINT16 IpProtocol = ntohs(*((UINT16 *)(pIpHeader)));
1133
1134         if(!isMldMacAddr(pDstMacAddr))
1135                 return FALSE;
1136
1137         if(IpProtocol != ETH_P_IPV6)
1138                 return FALSE;
1139
1140         // skip protocol (2 Bytes).
1141         pIpHeader += 2;
1142         do
1143         {
1144                 PRT_IPV6_HDR pIpv6Hdr = (PRT_IPV6_HDR)(pIpHeader);
1145                 UINT8 nextProtocol = pIpv6Hdr->nextHdr;
1146                 UINT32 offset = IPV6_HDR_LEN;
1147
1148                 while(nextProtocol != IPV6_NEXT_HEADER_ICMPV6)
1149                 {
1150                         if(IPv6ExtHdrHandle((RT_IPV6_EXT_HDR *)(pIpHeader + offset), &nextProtocol, &offset) == FALSE)
1151                                 break;
1152                 }
1153
1154                 if(nextProtocol == IPV6_NEXT_HEADER_ICMPV6)
1155                 {
1156                         PRT_ICMPV6_HDR pICMPv6Hdr = (PRT_ICMPV6_HDR)(pIpHeader + offset);
1157                         if (IsSupportedMldMsg(pICMPv6Hdr->type) == TRUE)
1158                         {
1159                                 if (pProtoType != NULL)
1160                                         *pProtoType = pICMPv6Hdr->type;
1161                                 if (pMldHeader != NULL)
1162                                         *pMldHeader = (PUCHAR)pICMPv6Hdr;
1163                                 result = TRUE;
1164                         }
1165                 }
1166         }while(FALSE);
1167
1168         return result;
1169 }
1170
1171 /*  MLD v1 messages have the following format:
1172         0                   1                   2                   3
1173         0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
1174         +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
1175         |     Type      |     Code      |          Checksum             |
1176         +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
1177         |     Maximum Response Delay    |          Reserved             |
1178         +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
1179         |                                                               |
1180         +                                                               +
1181         |                                                               |
1182         +                       Multicast Address                       +
1183         |                                                               |
1184         +                                                               +
1185         |                                                               |
1186         +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
1187 */
1188
1189 /*      Version 3 Membership Report Message
1190         0                   1                   2                   3
1191         0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
1192         +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
1193         |  Type = 143   |    Reserved   |           Checksum            |
1194         +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
1195         |           Reserved            |  Number of Group Records (M)  |
1196         +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
1197         |                                                               |
1198         .                                                               .
1199         .               Multicast Address Record [1]                    .
1200         .                                                               .
1201         |                                                               |
1202         +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
1203         |                                                               |
1204         .                                                               .
1205         .               Multicast Address Record [2]                    .
1206         .                                                               .
1207         |                                                               |
1208         +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
1209         |                               .                               |
1210         .                               .                               .
1211         |                               .                               |
1212         +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
1213         |                                                               |
1214         .                                                               .
1215         .               Multicast Address Record [M]                    .
1216         .                                                               .
1217         |                                                               |
1218         +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
1219
1220
1221         where each Group Record has the following internal format:
1222         +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
1223         |  Record Type  |  Aux Data Len |     Number of Sources (N)     |
1224         +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
1225     |                                                               |
1226     *                                                               *
1227     |                                                               |
1228     *                       Multicast Address                       *
1229     |                                                               |
1230     *                                                               *
1231     |                                                               |
1232         +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
1233     |                                                               |
1234     *                                                               *
1235     |                                                               |
1236     *                       Source Address [1]                      *
1237     |                                                               |
1238     *                                                               *
1239     |                                                               |
1240     +-                                                             -+
1241     |                                                               |
1242     *                                                               *
1243     |                                                               |
1244     *                       Source Address [2]                      *
1245     |                                                               |
1246     *                                                               *
1247     |                                                               |
1248     +-                                                             -+
1249     .                               .                               .
1250     .                               .                               .
1251     .                               .                               .
1252     +-                                                             -+
1253     |                                                               |
1254     *                                                               *
1255     |                                                               |
1256     *                       Source Address [N]                      *
1257     |                                                               |
1258     *                                                               *
1259     |                                                               |
1260     +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
1261     |                                                               |
1262     .                                                               .
1263     .                         Auxiliary Data                        .
1264     .                                                               .
1265     |                                                               |
1266     +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
1267 */
1268
1269 VOID MLDSnooping(
1270         IN PRTMP_ADAPTER pAd,
1271         IN PUCHAR pDstMacAddr,
1272         IN PUCHAR pSrcMacAddr,
1273         IN PUCHAR pIpHeader,
1274         IN PNET_DEV pDev)
1275 {
1276         INT i;
1277         UCHAR GroupType;
1278         UINT16 numOfGroup;
1279         PUCHAR pGroup;
1280         UCHAR AuxDataLen;
1281         UINT16 numOfSources;
1282         PUCHAR pGroupIpAddr;
1283         UCHAR GroupMacAddr[6];
1284         PUCHAR pGroupMacAddr = (PUCHAR)&GroupMacAddr;
1285
1286         UINT8 MldType;
1287         PUCHAR pMldHeader;
1288
1289         if(isMldPkt(pDstMacAddr, pIpHeader, &MldType, &pMldHeader) == TRUE)
1290         {
1291                 DBGPRINT(RT_DEBUG_TRACE, ("MLD type=%0x\n", MldType));
1292
1293                 switch(MldType)
1294                 {
1295                         case MLD_V1_LISTENER_REPORT:
1296                                 // skip Type(1 Byte), code(1 Byte), checksum(2 Bytes), Maximum Rsp Delay(2 Bytes), Reserve(2 Bytes).
1297                                 pGroupIpAddr = (PUCHAR)(pMldHeader + 8);
1298                                 ConvertMulticastIP2MAC(pGroupIpAddr, (PUCHAR *)&pGroupMacAddr, ETH_P_IPV6);
1299                                 DBGPRINT(RT_DEBUG_TRACE, ("Group Id=%02x:%02x:%02x:%02x:%02x:%02x\n",
1300                                                 GroupMacAddr[0], GroupMacAddr[1], GroupMacAddr[2], GroupMacAddr[3], GroupMacAddr[4], GroupMacAddr[5]));
1301                                 MulticastFilterTableInsertEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev, MCAT_FILTER_DYNAMIC);
1302                                 break;
1303
1304                         case MLD_V1_LISTENER_DONE:
1305                                 // skip Type(1 Byte), code(1 Byte), checksum(2 Bytes), Maximum Rsp Delay(2 Bytes), Reserve(2 Bytes).
1306                                 pGroupIpAddr = (PUCHAR)(pMldHeader + 8);
1307                                 ConvertMulticastIP2MAC(pGroupIpAddr, (PUCHAR *)&pGroupMacAddr, ETH_P_IPV6);
1308                                 DBGPRINT(RT_DEBUG_TRACE, ("Group Id=%02x:%02x:%02x:%02x:%02x:%02x\n",
1309                                                 GroupMacAddr[0], GroupMacAddr[1], GroupMacAddr[2], GroupMacAddr[3], GroupMacAddr[4], GroupMacAddr[5]));
1310                                 MulticastFilterTableDeleteEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev);
1311                                 break;
1312
1313                         case MLD_V2_LISTERNER_REPORT: // IGMP version 3 membership report.
1314                                 numOfGroup = ntohs(*((UINT16 *)(pMldHeader + 6)));
1315                                 pGroup = (PUCHAR)(pMldHeader + 8);
1316                                 for (i=0; i < numOfGroup; i++)
1317                                 {
1318                                         GroupType = (UCHAR)(*pGroup);
1319                                         AuxDataLen = (UCHAR)(*(pGroup + 1));
1320                                         numOfSources = ntohs(*((UINT16 *)(pGroup + 2)));
1321                                         pGroupIpAddr = (PUCHAR)(pGroup + 4);
1322                                         DBGPRINT(RT_DEBUG_TRACE, ("MLDv2 Type=%d, ADL=%d, numOfSource=%d\n", GroupType, AuxDataLen, numOfSources));
1323                                         ConvertMulticastIP2MAC(pGroupIpAddr, (PUCHAR *)&pGroupMacAddr, ETH_P_IPV6);
1324                                         DBGPRINT(RT_DEBUG_TRACE, ("MLD Group=%02x:%02x:%02x:%02x:%02x:%02x\n",
1325                                                         GroupMacAddr[0], GroupMacAddr[1], GroupMacAddr[2], GroupMacAddr[3], GroupMacAddr[4], GroupMacAddr[5]));
1326
1327                                         do
1328                                         {
1329                                                 if((GroupType == MODE_IS_EXCLUDE) || (GroupType == CHANGE_TO_EXCLUDE_MODE) || (GroupType == ALLOW_NEW_SOURCES))
1330                                                 {
1331                                                         MulticastFilterTableInsertEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev, MCAT_FILTER_DYNAMIC);
1332                                                         break;
1333                                                 }
1334
1335                                                 if((GroupType == MODE_IS_INCLUDE) || (GroupType == BLOCK_OLD_SOURCES))
1336                                                 {
1337                                                         MulticastFilterTableDeleteEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev);
1338                                                         break;
1339                                                 }
1340
1341                                                 if((GroupType == CHANGE_TO_INCLUDE_MODE))
1342                                                 {
1343                                                         if(numOfSources == 0)
1344                                                                 MulticastFilterTableDeleteEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev);
1345                                                         else
1346                                                                 MulticastFilterTableInsertEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev, MCAT_FILTER_DYNAMIC);
1347                                                         break;
1348                                                 }
1349                                         } while(FALSE);
1350                                         // skip 4 Bytes (Record Type, Aux Data Len, Number of Sources) + a IPv6 address.
1351                                         pGroup += (4 + IPV6_ADDR_LEN + (numOfSources * 16) + AuxDataLen);
1352                                 }
1353                                 break;
1354
1355                         default:
1356                                 DBGPRINT(RT_DEBUG_TRACE, ("unknow MLD Type=%d\n", MldType));
1357                                 break;
1358                 }
1359         }
1360
1361         return;
1362 }
1363
1364
1365 #endif // IGMP_SNOOP_SUPPORT //