/*
 *************************************************************************
 * Ralink Tech Inc.
 * 5F., No.36, Taiyuan St., Jhubei City,
 * Hsinchu County 302,
 * Taiwan, R.O.C.
 *
 * (c) Copyright 2002-2007, Ralink Technology, Inc.
 *
 * This program is free software; you can redistribute it and/or modify  *
 * it under the terms of the GNU General Public License as published by  *
 * the Free Software Foundation; either version 2 of the License, or     *
 * (at your option) any later version.                                   *
 *                                                                       *
 * This program is distributed in the hope that it will be useful,       *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 * GNU General Public License for more details.                          *
 *                                                                       *
 * You should have received a copy of the GNU General Public License     *
 * along with this program; if not, write to the                         *
 * Free Software Foundation, Inc.,                                       *
 * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 *                                                                       *
 *************************************************************************
 */


#ifdef IGMP_SNOOP_SUPPORT

#include "../rt_config.h"
#include "../ipv6.h"
#include "../igmp_snoop.h"


static inline void initFreeEntryList(
        IN PMULTICAST_FILTER_TABLE pMulticastFilterTable,
        IN PLIST_HEADER pList)
{
        int i;

        for (i = 0; i < FREE_MEMBER_POOL_SIZE; i++)
                insertTailList(pList, (PLIST_ENTRY)&(pMulticastFilterTable->freeMemberPool[i]));

        return;
}

static inline PMEMBER_ENTRY AllocaGrpMemberEntry(
        IN PMULTICAST_FILTER_TABLE pMulticastFilterTable)
{
        PMEMBER_ENTRY pMemberEntry;

        RTMP_SEM_LOCK(&pMulticastFilterTable->FreeMemberPoolTabLock);

        pMemberEntry = (PMEMBER_ENTRY)removeHeadList(&pMulticastFilterTable->freeEntryList);

        RTMP_SEM_UNLOCK(&pMulticastFilterTable->FreeMemberPoolTabLock);

        return (PMEMBER_ENTRY)pMemberEntry;
}

static inline VOID FreeGrpMemberEntry(
        IN PMULTICAST_FILTER_TABLE pMulticastFilterTable,
        IN PMEMBER_ENTRY pEntry)
{
        RTMP_SEM_LOCK(&pMulticastFilterTable->FreeMemberPoolTabLock);

        insertTailList(&pMulticastFilterTable->freeEntryList, (PLIST_ENTRY)pEntry);

        RTMP_SEM_UNLOCK(&pMulticastFilterTable->FreeMemberPoolTabLock);
}

static VOID IGMPTableDisplay(
        IN PRTMP_ADAPTER pAd);

static BOOLEAN isIgmpMacAddr(
        IN PUCHAR pMacAddr);

static VOID InsertIgmpMember(
        IN PMULTICAST_FILTER_TABLE pMulticastFilterTable,
        IN PLIST_HEADER pList,
        IN PUCHAR pMemberAddr);

static VOID DeleteIgmpMember(
        IN PMULTICAST_FILTER_TABLE pMulticastFilterTable,
        IN PLIST_HEADER pList,
        IN PUCHAR pMemberAddr);

static VOID DeleteIgmpMemberList(
        IN PMULTICAST_FILTER_TABLE pMulticastFilterTable,
        IN PLIST_HEADER pList);


/*
    ==========================================================================
    Description:
        This routine init the entire IGMP table.
    ==========================================================================
 */
VOID MulticastFilterTableInit(
        IN PMULTICAST_FILTER_TABLE *ppMulticastFilterTable)
{
        // Initialize MAC table and allocate spin lock
        *ppMulticastFilterTable = kmalloc(sizeof(MULTICAST_FILTER_TABLE), MEM_ALLOC_FLAG);
        if (*ppMulticastFilterTable == NULL)
        {
                DBGPRINT(RT_DEBUG_ERROR, ("%s unable to alloc memory for Multicase filter table, size=%d\n",
                        __FUNCTION__, sizeof(MULTICAST_FILTER_TABLE)));
                return;
        }

        NdisZeroMemory(*ppMulticastFilterTable, sizeof(MULTICAST_FILTER_TABLE));
        NdisAllocateSpinLock(&((*ppMulticastFilterTable)->MulticastFilterTabLock));

        NdisAllocateSpinLock(&((*ppMulticastFilterTable)->FreeMemberPoolTabLock));
        initList(&((*ppMulticastFilterTable)->freeEntryList));
        initFreeEntryList(*ppMulticastFilterTable, &((*ppMulticastFilterTable)->freeEntryList));
        return;
}

/*
    ==========================================================================
    Description:
        This routine reset the entire IGMP table.
    ==========================================================================
 */
VOID MultiCastFilterTableReset(
        IN PMULTICAST_FILTER_TABLE *ppMulticastFilterTable)
{
        if(*ppMulticastFilterTable == NULL)
        {
                DBGPRINT(RT_DEBUG_ERROR, ("%s Multicase filter table is not ready.\n", __FUNCTION__));
                return;
        }

        NdisFreeSpinLock(&((*ppMulticastFilterTable)->FreeMemberPoolTabLock));
        NdisFreeSpinLock(&((*ppMulticastFilterTable)->MulticastFilterTabLock));
        kfree(*ppMulticastFilterTable);
        *ppMulticastFilterTable = NULL;
}

/*
    ==========================================================================
    Description:
        Display all entrys in IGMP table
    ==========================================================================
 */
static VOID IGMPTableDisplay(
        IN PRTMP_ADAPTER pAd)
{
        int i;
        MULTICAST_FILTER_TABLE_ENTRY *pEntry = NULL;
        PMULTICAST_FILTER_TABLE pMulticastFilterTable = pAd->pMulticastFilterTable;

        if (pMulticastFilterTable == NULL)
        {
                DBGPRINT(RT_DEBUG_OFF, ("%s Multicase filter table is not ready.\n", __FUNCTION__));
                return;
        }

        // if FULL, return
        if (pMulticastFilterTable->Size == 0)
        {
                DBGPRINT(RT_DEBUG_ERROR, ("Table empty.\n"));
                return;
        }

        // allocate one MAC entry
        RTMP_SEM_LOCK(&pMulticastFilterTable->MulticastFilterTabLock);

        for (i = 0; i< MAX_LEN_OF_MULTICAST_FILTER_TABLE; i++)
        {
                // pick up the first available vacancy
                if (pMulticastFilterTable->Content[i].Valid == TRUE)
                {
                        PMEMBER_ENTRY pMemberEntry = NULL;
                        pEntry = &pMulticastFilterTable->Content[i];

                        DBGPRINT(RT_DEBUG_OFF, ("IF(%s) entry #%d, type=%s, GrpId=(%02x:%02x:%02x:%02x:%02x:%02x) memberCnt=%d\n",
                                RTMP_OS_NETDEV_GET_DEVNAME(pEntry->net_dev), i, (pEntry->type==0 ? "static":"dynamic"),
                                PRINT_MAC(pEntry->Addr), IgmpMemberCnt(&pEntry->MemberList)));

                        pMemberEntry = (PMEMBER_ENTRY)pEntry->MemberList.pHead;
                        while (pMemberEntry)
                        {
                                DBGPRINT(RT_DEBUG_OFF, ("member mac=(%02x:%02x:%02x:%02x:%02x:%02x)\n",
                                                                                PRINT_MAC(pMemberEntry->Addr)));

                                pMemberEntry = pMemberEntry->pNext;
                        }
                }
        }

        RTMP_SEM_UNLOCK(&pMulticastFilterTable->MulticastFilterTabLock);

        return;
}

/*
    ==========================================================================
    Description:
        Add and new entry into MAC table
    ==========================================================================
 */
BOOLEAN MulticastFilterTableInsertEntry(
        IN PRTMP_ADAPTER pAd,
        IN PUCHAR pGrpId,
        IN PUCHAR pMemberAddr,
        IN PNET_DEV dev,
        IN MulticastFilterEntryType type)
{
        UCHAR HashIdx;
        int i;
        MULTICAST_FILTER_TABLE_ENTRY *pEntry = NULL, *pCurrEntry, *pPrevEntry;
        PMEMBER_ENTRY pMemberEntry;
        PMULTICAST_FILTER_TABLE pMulticastFilterTable = pAd->pMulticastFilterTable;

        if (pMulticastFilterTable == NULL)
        {
                DBGPRINT(RT_DEBUG_ERROR, ("%s Multicase filter table is not ready.\n", __FUNCTION__));
                return FALSE;
        }

        // if FULL, return
        if (pMulticastFilterTable->Size >= MAX_LEN_OF_MULTICAST_FILTER_TABLE)
        {
                DBGPRINT(RT_DEBUG_ERROR, ("%s Multicase filter table full. max-entries = %d\n",
                        __FUNCTION__, MAX_LEN_OF_MULTICAST_FILTER_TABLE));
                return FALSE;
        }

        // check the rule is in table already or not.
        if ((pEntry = MulticastFilterTableLookup(pMulticastFilterTable, pGrpId, dev)))
        {
                // doesn't indicate member mac address.
                if(pMemberAddr == NULL)
                {
                        return FALSE;
                }

                pMemberEntry = (PMEMBER_ENTRY)pEntry->MemberList.pHead;

                while (pMemberEntry)
                {
                        if (MAC_ADDR_EQUAL(pMemberAddr, pMemberEntry->Addr))
                        {
                                DBGPRINT(RT_DEBUG_ERROR, ("%s: already in Members list.\n", __FUNCTION__));
                                return FALSE;
                        }

                        pMemberEntry = pMemberEntry->pNext;
                }
        }

        RTMP_SEM_LOCK(&pMulticastFilterTable->MulticastFilterTabLock);
        do
        {
                ULONG Now;
                // the multicast entry already exist but doesn't include the member yet.
                if (pEntry != NULL && pMemberAddr != NULL)
                {
                        InsertIgmpMember(pMulticastFilterTable, &pEntry->MemberList, pMemberAddr);
                        break;
                }

                // allocate one MAC entry
                for (i = 0; i < MAX_LEN_OF_MULTICAST_FILTER_TABLE; i++)
                {
                        // pick up the first available vacancy
                        pEntry = &pMulticastFilterTable->Content[i];
                        NdisGetSystemUpTime(&Now);
                        if ((pEntry->Valid == TRUE) && (pEntry->type == MCAT_FILTER_DYNAMIC)
                                && ((Now - pEntry->lastTime) > IGMPMAC_TB_ENTRY_AGEOUT_TIME))
                        {
                                PMULTICAST_FILTER_TABLE_ENTRY pHashEntry;

                                HashIdx = MULTICAST_ADDR_HASH_INDEX(pEntry->Addr);
                                pHashEntry = pMulticastFilterTable->Hash[HashIdx];

                                if ((pEntry->net_dev == pHashEntry->net_dev)
                                        && MAC_ADDR_EQUAL(pEntry->Addr, pHashEntry->Addr))
                                {
                                        pMulticastFilterTable->Hash[HashIdx] = pHashEntry->pNext;
                                        pMulticastFilterTable->Size --;
                                        DBGPRINT(RT_DEBUG_TRACE, ("MCastFilterTableDeleteEntry 1 - Total= %d\n", pMulticastFilterTable->Size));
                                } else
                                {
                                        while (pHashEntry->pNext)
                                        {
                                                pPrevEntry = pHashEntry;
                                                pHashEntry = pHashEntry->pNext;
                                                if ((pEntry->net_dev == pHashEntry->net_dev)
                                                        && MAC_ADDR_EQUAL(pEntry->Addr, pHashEntry->Addr))
                                                {
                                                        pPrevEntry->pNext = pHashEntry->pNext;
                                                        pMulticastFilterTable->Size --;
                                                        DBGPRINT(RT_DEBUG_TRACE, ("MCastFilterTableDeleteEntry 2 - Total= %d\n", pMulticastFilterTable->Size));
                                                        break;
                                                }
                                        }
                                }
                                pEntry->Valid = FALSE;
                                DeleteIgmpMemberList(pMulticastFilterTable, &pEntry->MemberList);
                        }

                        if (pEntry->Valid == FALSE)
                        {
                                NdisZeroMemory(pEntry, sizeof(MULTICAST_FILTER_TABLE_ENTRY));
                                pEntry->Valid = TRUE;

                                COPY_MAC_ADDR(pEntry->Addr, pGrpId);
                                pEntry->net_dev = dev;
                                NdisGetSystemUpTime(&Now);
                                pEntry->lastTime = Now;
                                pEntry->type = type;
                                initList(&pEntry->MemberList);
                                if (pMemberAddr != NULL)
                                        InsertIgmpMember(pMulticastFilterTable, &pEntry->MemberList, pMemberAddr);

                                pMulticastFilterTable->Size ++;

                                DBGPRINT(RT_DEBUG_TRACE, ("MulticastFilterTableInsertEntry -IF(%s) allocate entry #%d, Total= %d\n", RTMP_OS_NETDEV_GET_DEVNAME(dev), i, pMulticastFilterTable->Size));
                                break;
                        }
                }

                // add this MAC entry into HASH table
                if (pEntry)
                {
                        HashIdx = MULTICAST_ADDR_HASH_INDEX(pGrpId);
                        if (pMulticastFilterTable->Hash[HashIdx] == NULL)
                        {
                                pMulticastFilterTable->Hash[HashIdx] = pEntry;
                        } else
                        {
                                pCurrEntry = pMulticastFilterTable->Hash[HashIdx];
                                while (pCurrEntry->pNext != NULL)
                                        pCurrEntry = pCurrEntry->pNext;
                                pCurrEntry->pNext = pEntry;
                        }
                }
        }while(FALSE);

        RTMP_SEM_UNLOCK(&pMulticastFilterTable->MulticastFilterTabLock);

        return TRUE;
}

/*
    ==========================================================================
    Description:
        Delete a specified client from MAC table
    ==========================================================================
 */
BOOLEAN MulticastFilterTableDeleteEntry(
        IN PRTMP_ADAPTER pAd,
        IN PUCHAR pGrpId,
        IN PUCHAR pMemberAddr,
        IN PNET_DEV dev)
{
        USHORT HashIdx;
        MULTICAST_FILTER_TABLE_ENTRY *pEntry, *pPrevEntry;
        PMULTICAST_FILTER_TABLE pMulticastFilterTable = pAd->pMulticastFilterTable;
        USHORT Aid = MCAST_WCID;
        SST     Sst = SST_ASSOC;
        UCHAR PsMode = PWR_ACTIVE, Rate;

        if (pMulticastFilterTable == NULL)
        {
                DBGPRINT(RT_DEBUG_ERROR, ("%s Multicase filter table is not ready.\n", __FUNCTION__));
                return FALSE;
        }

        RTMP_SEM_LOCK(&pMulticastFilterTable->MulticastFilterTabLock);

        do
        {
                HashIdx = MULTICAST_ADDR_HASH_INDEX(pGrpId);
                pPrevEntry = pEntry = pMulticastFilterTable->Hash[HashIdx];

                while (pEntry && pEntry->Valid)
                {
                        if ((pEntry->net_dev ==  dev)
                                && MAC_ADDR_EQUAL(pEntry->Addr, pGrpId))
                                break;
                        else
                        {
                                pPrevEntry = pEntry;
                                pEntry = pEntry->pNext;
                        }
                }

                // check the rule is in table already or not.
                if (pEntry && (pMemberAddr != NULL))
                {
                        if(APSsPsInquiry(pAd, pMemberAddr, &Sst, &Aid, &PsMode, &Rate))
                                DeleteIgmpMember(pMulticastFilterTable, &pEntry->MemberList, pMemberAddr);
                        if (IgmpMemberCnt(&pEntry->MemberList) > 0)
                                break;
                }

                if (pEntry)
                {
                        if (pEntry == pMulticastFilterTable->Hash[HashIdx])
                        {
                                pMulticastFilterTable->Hash[HashIdx] = pEntry->pNext;
                                DeleteIgmpMemberList(pMulticastFilterTable, &pEntry->MemberList);
                                NdisZeroMemory(pEntry, sizeof(MULTICAST_FILTER_TABLE_ENTRY));
                                pMulticastFilterTable->Size --;
                                DBGPRINT(RT_DEBUG_TRACE, ("MCastFilterTableDeleteEntry 1 - Total= %d\n", pMulticastFilterTable->Size));
                        }
                        else
                        {
                                pPrevEntry->pNext = pEntry->pNext;
                                DeleteIgmpMemberList(pMulticastFilterTable, &pEntry->MemberList);
                                NdisZeroMemory(pEntry, sizeof(MULTICAST_FILTER_TABLE_ENTRY));
                                pMulticastFilterTable->Size --;
                                DBGPRINT(RT_DEBUG_TRACE, ("MCastFilterTableDeleteEntry 2 - Total= %d\n", pMulticastFilterTable->Size));
                        }
                }
                else
                {
                        DBGPRINT(RT_DEBUG_ERROR, ("%s: the Group doesn't exist.\n", __FUNCTION__));
                }
        } while(FALSE);

        RTMP_SEM_UNLOCK(&pMulticastFilterTable->MulticastFilterTabLock);

        return TRUE;
}

/*
    ==========================================================================
    Description:
        Look up the MAC address in the IGMP table. Return NULL if not found.
    Return:
        pEntry - pointer to the MAC entry; NULL is not found
    ==========================================================================
*/
PMULTICAST_FILTER_TABLE_ENTRY MulticastFilterTableLookup(
        IN PMULTICAST_FILTER_TABLE pMulticastFilterTable,
        IN PUCHAR pAddr,
        IN PNET_DEV dev)
{
        ULONG HashIdx, Now;
        PMULTICAST_FILTER_TABLE_ENTRY pEntry = NULL, pPrev = NULL;

        if (pMulticastFilterTable == NULL)
        {
                DBGPRINT(RT_DEBUG_ERROR, ("%s Multicase filter table is not ready.\n", __FUNCTION__));
                return NULL;
        }

        RTMP_SEM_LOCK(&pMulticastFilterTable->MulticastFilterTabLock);

        HashIdx = MULTICAST_ADDR_HASH_INDEX(pAddr);
        pEntry = pPrev = pMulticastFilterTable->Hash[HashIdx];

        while (pEntry && pEntry->Valid)
        {
                if ((pEntry->net_dev ==  dev)
                        && MAC_ADDR_EQUAL(pEntry->Addr, pAddr))
                {
                        NdisGetSystemUpTime(&Now);
                        pEntry->lastTime = Now;
                        break;
                }
                else
                {
                        NdisGetSystemUpTime(&Now);
                        if ((pEntry->Valid == TRUE) && (pEntry->type == MCAT_FILTER_DYNAMIC)
                                && RTMP_TIME_AFTER(Now, pEntry->lastTime+IGMPMAC_TB_ENTRY_AGEOUT_TIME))
                        {
                                // Remove the aged entry
                                if (pEntry == pMulticastFilterTable->Hash[HashIdx])
                                {
                                        pMulticastFilterTable->Hash[HashIdx] = pEntry->pNext;
                                        pPrev = pMulticastFilterTable->Hash[HashIdx];
                                        DeleteIgmpMemberList(pMulticastFilterTable, &pEntry->MemberList);
                                        NdisZeroMemory(pEntry, sizeof(MULTICAST_FILTER_TABLE_ENTRY));
                                        pMulticastFilterTable->Size --;
                                        pEntry = pPrev;
                                        DBGPRINT(RT_DEBUG_TRACE, ("MCastFilterTableDeleteEntry 2 - Total= %d\n", pMulticastFilterTable->Size));
                                }
                                else
                                {
                                        pPrev->pNext = pEntry->pNext;
                                        DeleteIgmpMemberList(pMulticastFilterTable, &pEntry->MemberList);
                                        NdisZeroMemory(pEntry, sizeof(MULTICAST_FILTER_TABLE_ENTRY));
                                        pMulticastFilterTable->Size --;
                                        pEntry = (pPrev == NULL ? NULL: pPrev->pNext);
                                        DBGPRINT(RT_DEBUG_TRACE, ("MCastFilterTableDeleteEntry 2 - Total= %d\n", pMulticastFilterTable->Size));
                                }
                        }
                        else
                        {
                                pPrev = pEntry;
                                pEntry = pEntry->pNext;
                        }
                }
        }

        RTMP_SEM_UNLOCK(&pMulticastFilterTable->MulticastFilterTabLock);

        return pEntry;
}

VOID IGMPSnooping(
        IN PRTMP_ADAPTER pAd,
        IN PUCHAR pDstMacAddr,
        IN PUCHAR pSrcMacAddr,
        IN PUCHAR pIpHeader,
        IN PNET_DEV pDev)
{
        INT i;
        INT IpHeaderLen;
        UCHAR GroupType;
        UINT16 numOfGroup;
        UCHAR IgmpVerType;
        PUCHAR pIgmpHeader;
        PUCHAR pGroup;
        UCHAR AuxDataLen;
        UINT16 numOfSources;
        PUCHAR pGroupIpAddr;
        UCHAR GroupMacAddr[6];
        PUCHAR pGroupMacAddr = (PUCHAR)&GroupMacAddr;

        if(isIgmpPkt(pDstMacAddr, pIpHeader))
        {
                IpHeaderLen = (*(pIpHeader + 2) & 0x0f) * 4;
                pIgmpHeader = pIpHeader + 2 + IpHeaderLen;
                IgmpVerType = (UCHAR)(*(pIgmpHeader));

                DBGPRINT(RT_DEBUG_TRACE, ("IGMP type=%0x\n", IgmpVerType));

                switch(IgmpVerType)
                {
                case IGMP_V1_MEMBERSHIP_REPORT: // IGMP version 1 membership report.
                case IGMP_V2_MEMBERSHIP_REPORT: // IGMP version 2 membership report.
                        pGroupIpAddr = (PUCHAR)(pIgmpHeader + 4);
                                ConvertMulticastIP2MAC(pGroupIpAddr, (PUCHAR *)&pGroupMacAddr, ETH_P_IP);
                        DBGPRINT(RT_DEBUG_TRACE, ("IGMP Group=%02x:%02x:%02x:%02x:%02x:%02x\n",
                                GroupMacAddr[0], GroupMacAddr[1], GroupMacAddr[2], GroupMacAddr[3], GroupMacAddr[4], GroupMacAddr[5]));
                        MulticastFilterTableInsertEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev, MCAT_FILTER_DYNAMIC);
                        break;

                case IGMP_LEAVE_GROUP: // IGMP version 1 and version 2 leave group.
                        pGroupIpAddr = (PUCHAR)(pIgmpHeader + 4);
                                ConvertMulticastIP2MAC(pGroupIpAddr, (PUCHAR *)&pGroupMacAddr, ETH_P_IP);
                        DBGPRINT(RT_DEBUG_TRACE, ("IGMP Group=%02x:%02x:%02x:%02x:%02x:%02x\n",
                                GroupMacAddr[0], GroupMacAddr[1], GroupMacAddr[2], GroupMacAddr[3], GroupMacAddr[4], GroupMacAddr[5]));
                        MulticastFilterTableDeleteEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev);
                        break;

                case IGMP_V3_MEMBERSHIP_REPORT: // IGMP version 3 membership report.
                        numOfGroup = ntohs(*((UINT16 *)(pIgmpHeader + 6)));
                        pGroup = (PUCHAR)(pIgmpHeader + 8);
                        for (i=0; i < numOfGroup; i++)
                        {
                                GroupType = (UCHAR)(*pGroup);
                                AuxDataLen = (UCHAR)(*(pGroup + 1));
                                numOfSources = ntohs(*((UINT16 *)(pGroup + 2)));
                                pGroupIpAddr = (PUCHAR)(pGroup + 4);
                                DBGPRINT(RT_DEBUG_TRACE, ("IGMPv3 Type=%d, ADL=%d, numOfSource=%d\n", GroupType, AuxDataLen, numOfSources));
                                        ConvertMulticastIP2MAC(pGroupIpAddr, (PUCHAR *)&pGroupMacAddr, ETH_P_IP);
                                DBGPRINT(RT_DEBUG_TRACE, ("IGMP Group=%02x:%02x:%02x:%02x:%02x:%02x\n",
                                        GroupMacAddr[0], GroupMacAddr[1], GroupMacAddr[2], GroupMacAddr[3], GroupMacAddr[4], GroupMacAddr[5]));

                                do
                                {
                                        if((GroupType == MODE_IS_EXCLUDE) || (GroupType == CHANGE_TO_EXCLUDE_MODE) || (GroupType == ALLOW_NEW_SOURCES))
                                        {
                                                MulticastFilterTableInsertEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev, MCAT_FILTER_DYNAMIC);
                                                break;
                                        }

                                        if((GroupType == MODE_IS_INCLUDE) || (GroupType == BLOCK_OLD_SOURCES))
                                        {
                                                MulticastFilterTableDeleteEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev);
                                                break;
                                        }

                                        if((GroupType == CHANGE_TO_INCLUDE_MODE))
                                        {
                                                if(numOfSources == 0)
                                                        MulticastFilterTableDeleteEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev);
                                                else
                                                        MulticastFilterTableInsertEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev, MCAT_FILTER_DYNAMIC);
                                                break;
                                        }
                                } while(FALSE);
                                pGroup += (8 + (numOfSources * 4) + AuxDataLen);
                        }
                        break;

                default:
                        DBGPRINT(RT_DEBUG_TRACE, ("unknow IGMP Type=%d\n", IgmpVerType));
                        break;
                }
        }

        return;
}


static BOOLEAN isIgmpMacAddr(
        IN PUCHAR pMacAddr)
{
        if((pMacAddr[0] == 0x01)
                && (pMacAddr[1] == 0x00)
                && (pMacAddr[2] == 0x5e))
                return TRUE;
        return FALSE;
}

BOOLEAN isIgmpPkt(
        IN PUCHAR pDstMacAddr,
        IN PUCHAR pIpHeader)
{
        UINT16 IpProtocol = ntohs(*((UINT16 *)(pIpHeader)));
        UCHAR IgmpProtocol;

        if(!isIgmpMacAddr(pDstMacAddr))
                return FALSE;

        if(IpProtocol == ETH_P_IP)
        {
                IgmpProtocol = (UCHAR)*(pIpHeader + 11);
                if(IgmpProtocol == IGMP_PROTOCOL_DESCRIPTOR)
                                return TRUE;
        }

        return FALSE;
}

static VOID InsertIgmpMember(
        IN PMULTICAST_FILTER_TABLE pMulticastFilterTable,
        IN PLIST_HEADER pList,
        IN PUCHAR pMemberAddr)
{
        PMEMBER_ENTRY pMemberEntry;

        if(pList == NULL)
        {
                DBGPRINT(RT_DEBUG_ERROR, ("%s: membert list doesn't exist.\n", __FUNCTION__));
                return;
        }

        if (pMemberAddr == NULL)
        {
                DBGPRINT(RT_DEBUG_ERROR, ("%s: invalid member.\n", __FUNCTION__));
                return;
        }

        if((pMemberEntry = (PMEMBER_ENTRY)AllocaGrpMemberEntry(pMulticastFilterTable)) != NULL)
        {
                NdisZeroMemory(pMemberEntry, sizeof(MEMBER_ENTRY));
                COPY_MAC_ADDR(pMemberEntry->Addr, pMemberAddr);
                insertTailList(pList, (PLIST_ENTRY)pMemberEntry);

                DBGPRINT(RT_DEBUG_TRACE, ("%s Member Mac=%02x:%02x:%02x:%02x:%02x:%02x\n", __FUNCTION__,
                        pMemberEntry->Addr[0], pMemberEntry->Addr[1], pMemberEntry->Addr[2],
                        pMemberEntry->Addr[3], pMemberEntry->Addr[4], pMemberEntry->Addr[5]));
        }
        return;
}

static VOID DeleteIgmpMember(
        IN PMULTICAST_FILTER_TABLE pMulticastFilterTable,
        IN PLIST_HEADER pList,
        IN PUCHAR pMemberAddr)
{
        PMEMBER_ENTRY pCurEntry;

        if((pList == NULL) || (pList->pHead == NULL))
        {
                DBGPRINT(RT_DEBUG_ERROR, ("%s: membert list doesn't exist.\n", __FUNCTION__));
                return;
        }

        if (pMemberAddr == NULL)
        {
                DBGPRINT(RT_DEBUG_ERROR, ("%s: invalid member.\n", __FUNCTION__));
                return;
        }

        pCurEntry = (PMEMBER_ENTRY)pList->pHead;
        while (pCurEntry)
        {
                if(MAC_ADDR_EQUAL(pMemberAddr, pCurEntry->Addr))
                {
                        delEntryList(pList, (PLIST_ENTRY)pCurEntry);
                        FreeGrpMemberEntry(pMulticastFilterTable, pCurEntry);
                        break;
                }
                pCurEntry = pCurEntry->pNext;
        }

        return;
}

static VOID DeleteIgmpMemberList(
        IN PMULTICAST_FILTER_TABLE pMulticastFilterTable,
        IN PLIST_HEADER pList)
{
        PMEMBER_ENTRY pCurEntry, pPrvEntry;

        if((pList == NULL) || (pList->pHead == NULL))
        {
                DBGPRINT(RT_DEBUG_ERROR, ("%s: membert list doesn't exist.\n", __FUNCTION__));
                return;
        }

        pPrvEntry = pCurEntry = (PMEMBER_ENTRY)pList->pHead;
        while (pCurEntry)
        {
                delEntryList(pList, (PLIST_ENTRY)pCurEntry);
                pPrvEntry = pCurEntry;
                pCurEntry = pCurEntry->pNext;
                FreeGrpMemberEntry(pMulticastFilterTable, pPrvEntry);
        }

        initList(pList);
        return;
}


UCHAR IgmpMemberCnt(
        IN PLIST_HEADER pList)
{
        if(pList == NULL)
        {
                DBGPRINT(RT_DEBUG_ERROR, ("%s: membert list doesn't exist.\n", __FUNCTION__));
                return 0;
        }

        return getListSize(pList);
}

VOID IgmpGroupDelMembers(
        IN PRTMP_ADAPTER pAd,
        IN PUCHAR pMemberAddr,
        IN PNET_DEV pDev)
{
        INT i;
        MULTICAST_FILTER_TABLE_ENTRY *pEntry = NULL;
        PMULTICAST_FILTER_TABLE pMulticastFilterTable = pAd->pMulticastFilterTable;

        for (i = 0; i < MAX_LEN_OF_MULTICAST_FILTER_TABLE; i++)
        {
                // pick up the first available vacancy
                pEntry = &pMulticastFilterTable->Content[i];
                if (pEntry->Valid == TRUE)
                {
                        if(pMemberAddr != NULL)
                        {
                                RTMP_SEM_LOCK(&pMulticastFilterTable->MulticastFilterTabLock);
                                DeleteIgmpMember(pMulticastFilterTable, &pEntry->MemberList, pMemberAddr);
                                RTMP_SEM_UNLOCK(&pMulticastFilterTable->MulticastFilterTabLock);
                        }

                        if((pEntry->type == MCAT_FILTER_DYNAMIC)
                                && (IgmpMemberCnt(&pEntry->MemberList) == 0))
                                MulticastFilterTableDeleteEntry(pAd, pEntry->Addr, pMemberAddr, pDev);
                }
        }
}

INT Set_IgmpSn_Enable_Proc(
        IN PRTMP_ADAPTER pAd,
        IN PSTRING arg)
{
        UINT Enable;
        POS_COOKIE pObj;
        UCHAR ifIndex;
        PNET_DEV pDev;

        pObj = (POS_COOKIE) pAd->OS_Cookie;
        ifIndex = pObj->ioctl_if;

        pDev = (ifIndex == MAIN_MBSSID) ? (pAd->net_dev) : (pAd->ApCfg.MBSSID[ifIndex].MSSIDDev);
        Enable = (UINT) simple_strtol(arg, 0, 10);

        pAd->ApCfg.MBSSID[ifIndex].IgmpSnoopEnable = (BOOLEAN)(Enable == 0 ? 0 : 1);
        DBGPRINT(RT_DEBUG_TRACE, ("%s::(%s) %s\n", __FUNCTION__, RTMP_OS_NETDEV_GET_DEVNAME(pDev), Enable == TRUE ? "Enable IGMP Snooping":"Disable IGMP Snooping"));

        return TRUE;
}

INT Set_IgmpSn_AddEntry_Proc(
        IN PRTMP_ADAPTER pAd,
        IN PSTRING arg)
{
        INT i;
        BOOLEAN bGroupId = 1;
        PSTRING value;
        PSTRING thisChar;
        UCHAR IpAddr[4];
        UCHAR Addr[ETH_LENGTH_OF_ADDRESS];
        UCHAR GroupId[ETH_LENGTH_OF_ADDRESS];
        PUCHAR *pAddr = (PUCHAR *)&Addr;
        PNET_DEV pDev;
        POS_COOKIE pObj;
        UCHAR ifIndex;

        pObj = (POS_COOKIE) pAd->OS_Cookie;
        ifIndex = pObj->ioctl_if;

        pDev = (ifIndex == MAIN_MBSSID) ? (pAd->net_dev) : (pAd->ApCfg.MBSSID[ifIndex].MSSIDDev);

        while ((thisChar = strsep((char **)&arg, "-")) != NULL)
        {
                // refuse the Member if it's not a MAC address.
                if((bGroupId == 0) && (strlen(thisChar) != 17))
                        continue;

                if(strlen(thisChar) == 17)  //Mac address acceptable format 01:02:03:04:05:06 length 17
                {
                        for (i=0, value = rstrtok(thisChar,":"); value; value = rstrtok(NULL,":"))
                        {
                                if((strlen(value) != 2) || (!isxdigit(*value)) || (!isxdigit(*(value+1))) )
                                        return FALSE;  //Invalid

                                AtoH(value, &Addr[i++], 1);
                        }

                        if(i != 6)
                                return FALSE;  //Invalid
                }
                else
                {
                        for (i=0, value = rstrtok(thisChar,"."); value; value = rstrtok(NULL,"."))
                        {
                                if((strlen(value) > 0) && (strlen(value) <= 3))
                                {
                                        int ii;
                                        for(ii=0; ii<strlen(value); ii++)
                                                if (!isxdigit(*(value + ii)))
                                                        return FALSE;
                                }
                                else
                                        return FALSE;  //Invalid

                                IpAddr[i] = (UCHAR)simple_strtol(value, NULL, 10);
                                i++;
                        }

                        if(i != 4)
                                return FALSE;  //Invalid

                        ConvertMulticastIP2MAC(IpAddr, (PUCHAR *)&pAddr, ETH_P_IP);
                }

                if(bGroupId == 1)
                        COPY_MAC_ADDR(GroupId, Addr);

                // Group-Id must be a MCAST address.
                if((bGroupId == 1) && IS_MULTICAST_MAC_ADDR(Addr))
                        MulticastFilterTableInsertEntry(pAd, GroupId, NULL, pDev, MCAT_FILTER_STATIC);
                // Group-Member must be a UCAST address.
                else if ((bGroupId == 0) && !IS_MULTICAST_MAC_ADDR(Addr))
                        MulticastFilterTableInsertEntry(pAd, GroupId, Addr, pDev, MCAT_FILTER_STATIC);
                else
                {
                        DBGPRINT(RT_DEBUG_TRACE, ("%s (%2X:%2X:%2X:%2X:%2X:%2X) is not a acceptable address.\n",
                                __FUNCTION__, Addr[0], Addr[1], Addr[2], Addr[3], Addr[4], Addr[5]));
                        return FALSE;
                }

                bGroupId = 0;
                DBGPRINT(RT_DEBUG_TRACE, ("%s (%2X:%2X:%2X:%2X:%2X:%2X)\n",
                        __FUNCTION__, Addr[0], Addr[1], Addr[2], Addr[3], Addr[4], Addr[5]));

        }

        return TRUE;
}

INT Set_IgmpSn_DelEntry_Proc(
        IN PRTMP_ADAPTER pAd,
        IN PSTRING arg)
{
        INT i, memberCnt = 0;
        BOOLEAN bGroupId = 1;
        PSTRING value;
        PSTRING thisChar;
        UCHAR IpAddr[4];
        UCHAR Addr[ETH_LENGTH_OF_ADDRESS];
        UCHAR GroupId[ETH_LENGTH_OF_ADDRESS];
        PUCHAR *pAddr = (PUCHAR *)&Addr;
        PNET_DEV pDev;
        POS_COOKIE pObj;
        UCHAR ifIndex;

        pObj = (POS_COOKIE) pAd->OS_Cookie;
        ifIndex = pObj->ioctl_if;

        pDev = (ifIndex == MAIN_MBSSID) ? (pAd->net_dev) : (pAd->ApCfg.MBSSID[ifIndex].MSSIDDev);

        while ((thisChar = strsep((char **)&arg, "-")) != NULL)
        {
                // refuse the Member if it's not a MAC address.
                if((bGroupId == 0) && (strlen(thisChar) != 17))
                        continue;

                if(strlen(thisChar) == 17)  //Mac address acceptable format 01:02:03:04:05:06 length 17
                {
                        for (i=0, value = rstrtok(thisChar,":"); value; value = rstrtok(NULL,":"))
                        {
                                if((strlen(value) != 2) || (!isxdigit(*value)) || (!isxdigit(*(value+1))) )
                                        return FALSE;  //Invalid

                                AtoH(value, &Addr[i++], 1);
                        }

                        if(i != 6)
                                return FALSE;  //Invalid
                }
                else
                {
                        for (i=0, value = rstrtok(thisChar,"."); value; value = rstrtok(NULL,"."))
                        {
                                if((strlen(value) > 0) && (strlen(value) <= 3))
                                {
                                        int ii;
                                        for(ii=0; ii<strlen(value); ii++)
                                                if (!isxdigit(*(value + ii)))
                                                        return FALSE;
                                }
                                else
                                        return FALSE;  //Invalid

                                IpAddr[i] = (UCHAR)simple_strtol(value, NULL, 10);
                                i++;
                        }

                        if(i != 4)
                                return FALSE;  //Invalid

                        ConvertMulticastIP2MAC(IpAddr, (PUCHAR *)&pAddr, ETH_P_IP);
                }

                if(bGroupId == 1)
                        COPY_MAC_ADDR(GroupId, Addr);
                else
                        memberCnt++;

                if (memberCnt > 0 )
                        MulticastFilterTableDeleteEntry(pAd, (PUCHAR)GroupId, Addr, pDev);

                bGroupId = 0;
        }

        if(memberCnt == 0)
                MulticastFilterTableDeleteEntry(pAd, (PUCHAR)GroupId, NULL, pDev);

        DBGPRINT(RT_DEBUG_TRACE, ("%s (%2X:%2X:%2X:%2X:%2X:%2X)\n",
                __FUNCTION__, Addr[0], Addr[1], Addr[2], Addr[3], Addr[4], Addr[5]));

        return TRUE;
}

INT Set_IgmpSn_TabDisplay_Proc(
        IN PRTMP_ADAPTER pAd,
        IN PSTRING arg)
{
        IGMPTableDisplay(pAd);
        return TRUE;
}

void rtmp_read_igmp_snoop_from_file(
        IN  PRTMP_ADAPTER pAd,
        PSTRING tmpbuf,
        PSTRING buffer)
{
        PSTRING         macptr;
        INT                     i=0;

        //IgmpSnEnable
        if(RTMPGetKeyParameter("IgmpSnEnable", tmpbuf, 128, buffer, TRUE))
        {
                for (i = 0, macptr = rstrtok(tmpbuf,";"); (macptr && i < pAd->ApCfg.BssidNum); macptr = rstrtok(NULL,";"), i++)
                {
                        if ((strncmp(macptr, "0", 1) == 0))
                                pAd->ApCfg.MBSSID[i].IgmpSnoopEnable = FALSE;
                        else if ((strncmp(macptr, "1", 1) == 0))
                                pAd->ApCfg.MBSSID[i].IgmpSnoopEnable = TRUE;
                else
                                pAd->ApCfg.MBSSID[i].IgmpSnoopEnable = FALSE;

                        DBGPRINT(RT_DEBUG_TRACE, ("MBSSID[%d].Enable=%d\n", i, pAd->ApCfg.MBSSID[i].IgmpSnoopEnable));
            }
        }
}

NDIS_STATUS IgmpPktInfoQuery(
        IN PRTMP_ADAPTER pAd,
        IN PUCHAR pSrcBufVA,
        IN PNDIS_PACKET pPacket,
        IN UCHAR apidx,

        OUT BOOLEAN *pInIgmpGroup,
        OUT PMULTICAST_FILTER_TABLE_ENTRY *ppGroupEntry)
{
        if(IS_MULTICAST_MAC_ADDR(pSrcBufVA))
        {
                BOOLEAN IgmpMldPkt = FALSE;
                PUCHAR pIpHeader = pSrcBufVA + 12;

                if(ntohs(*((UINT16 *)(pIpHeader))) == ETH_P_IPV6)
                        IgmpMldPkt = isMldPkt(pSrcBufVA, pIpHeader, NULL, NULL);
                else
                        IgmpMldPkt = isIgmpPkt(pSrcBufVA, pIpHeader);

                if (IgmpMldPkt)
                {
                        *ppGroupEntry = NULL;
                }
                else if ((*ppGroupEntry = MulticastFilterTableLookup(pAd->pMulticastFilterTable, pSrcBufVA,
                                                                        pAd->ApCfg.MBSSID[apidx].MSSIDDev)) == NULL)
                {
                        RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
                        return NDIS_STATUS_FAILURE;
                }
                *pInIgmpGroup = TRUE;
        }
        else if (IS_BROADCAST_MAC_ADDR(pSrcBufVA))
        {
                PUCHAR pDstIpAddr = pSrcBufVA + 30; // point to Destination of Ip address of IP header.
                UCHAR GroupMacAddr[6];
                PUCHAR pGroupMacAddr = (PUCHAR)&GroupMacAddr;

                ConvertMulticastIP2MAC(pDstIpAddr, (PUCHAR *)&pGroupMacAddr, ETH_P_IP);
                if ((*ppGroupEntry = MulticastFilterTableLookup(pAd->pMulticastFilterTable, pGroupMacAddr,
                                                                pAd->ApCfg.MBSSID[apidx].MSSIDDev)) != NULL)
                {
                        *pInIgmpGroup = TRUE;
                }
        }
        return NDIS_STATUS_SUCCESS;
}

NDIS_STATUS IgmpPktClone(
        IN PRTMP_ADAPTER pAd,
        IN PNDIS_PACKET pPacket,
        IN UCHAR QueIdx,
        IN PMULTICAST_FILTER_TABLE_ENTRY pGroupEntry)
{
        PNDIS_PACKET pSkbClone = NULL;
        PMEMBER_ENTRY pMemberEntry = (PMEMBER_ENTRY)pGroupEntry->MemberList.pHead;
        MAC_TABLE_ENTRY *pMacEntry = NULL;
        USHORT Aid;
        SST     Sst = SST_ASSOC;
        UCHAR PsMode = PWR_ACTIVE;
        UCHAR Rate;
        unsigned long IrqFlags;

        // check all members of the IGMP group.
        while(pMemberEntry != NULL)
        {
                pMacEntry = APSsPsInquiry(pAd, pMemberEntry->Addr, &Sst, &Aid, &PsMode, &Rate);

                if (pMacEntry && (Sst == SST_ASSOC) && (PsMode != PWR_SAVE))
                {
                pSkbClone = skb_clone(RTPKT_TO_OSPKT(pPacket), MEM_ALLOC_FLAG);
                if(pSkbClone)
                {
                                RTMP_SET_PACKET_WCID(pSkbClone, (UCHAR)Aid);
                                // Pkt type must set to PKTSRC_NDIS.
                                // It cause of the deason that APHardTransmit()
                                // doesn't handle PKTSRC_DRIVER pkt type in version 1.3.0.0.
                                RTMP_SET_PACKET_SOURCE(pSkbClone, PKTSRC_NDIS);
                        }
                        else
                        {
                                pMemberEntry = pMemberEntry->pNext;
                                continue;
                        }

                        // insert the pkt to TxSwQueue.
                        if (pAd->TxSwQueue[QueIdx].Number >= MAX_PACKETS_IN_QUEUE)
                        {
#ifdef BLOCK_NET_IF
                                StopNetIfQueue(pAd, QueIdx, pSkbClone);
#endif // BLOCK_NET_IF //
                                RELEASE_NDIS_PACKET(pAd, pSkbClone, NDIS_STATUS_FAILURE);
                                return NDIS_STATUS_FAILURE;
                        }
                        else
                        {
                                RTMP_IRQ_LOCK(&pAd->irq_lock, IrqFlags);
                                InsertTailQueueAc(pAd, pMacEntry, &pAd->TxSwQueue[QueIdx], PACKET_TO_QUEUE_ENTRY(pSkbClone));
                                RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
                        }
                }
                pMemberEntry = pMemberEntry->pNext;
        }
        return NDIS_STATUS_SUCCESS;
}

static inline BOOLEAN isMldMacAddr(
        IN PUCHAR pMacAddr)
{
        return ((pMacAddr[0] == 0x33) && (pMacAddr[1] == 0x33)) ? TRUE : FALSE;
}

static inline BOOLEAN IsSupportedMldMsg(
        IN UINT8 MsgType)
{
        BOOLEAN result = FALSE;
        switch(MsgType)
        {
                case MLD_V1_LISTENER_REPORT:
                case MLD_V1_LISTENER_DONE:
                case MLD_V2_LISTERNER_REPORT:
                        result = TRUE;
                        break;
                default:
                        result = FALSE;
                        break;
        }

        return result;
}

BOOLEAN isMldPkt(
        IN PUCHAR pDstMacAddr,
        IN PUCHAR pIpHeader,
        OUT UINT8 *pProtoType,
        OUT PUCHAR *pMldHeader)
{
        BOOLEAN result = FALSE;
        UINT16 IpProtocol = ntohs(*((UINT16 *)(pIpHeader)));

        if(!isMldMacAddr(pDstMacAddr))
                return FALSE;

        if(IpProtocol != ETH_P_IPV6)
                return FALSE;

        // skip protocol (2 Bytes).
        pIpHeader += 2;
        do
        {
                PRT_IPV6_HDR pIpv6Hdr = (PRT_IPV6_HDR)(pIpHeader);
                UINT8 nextProtocol = pIpv6Hdr->nextHdr;
                UINT32 offset = IPV6_HDR_LEN;

                while(nextProtocol != IPV6_NEXT_HEADER_ICMPV6)
                {
                        if(IPv6ExtHdrHandle((RT_IPV6_EXT_HDR *)(pIpHeader + offset), &nextProtocol, &offset) == FALSE)
                                break;
                }

                if(nextProtocol == IPV6_NEXT_HEADER_ICMPV6)
                {
                        PRT_ICMPV6_HDR pICMPv6Hdr = (PRT_ICMPV6_HDR)(pIpHeader + offset);
                        if (IsSupportedMldMsg(pICMPv6Hdr->type) == TRUE)
                        {
                                if (pProtoType != NULL)
                                        *pProtoType = pICMPv6Hdr->type;
                                if (pMldHeader != NULL)
                                        *pMldHeader = (PUCHAR)pICMPv6Hdr;
                                result = TRUE;
                        }
                }
        }while(FALSE);

        return result;
}

/*  MLD v1 messages have the following format:
        0                   1                   2                   3
        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
        +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
        |     Type      |     Code      |          Checksum             |
        +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
        |     Maximum Response Delay    |          Reserved             |
        +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
        |                                                               |
        +                                                               +
        |                                                               |
        +                       Multicast Address                       +
        |                                                               |
        +                                                               +
        |                                                               |
        +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
*/

/*      Version 3 Membership Report Message
        0                   1                   2                   3
        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
        +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
        |  Type = 143   |    Reserved   |           Checksum            |
        +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
        |           Reserved            |  Number of Group Records (M)  |
        +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
        |                                                               |
        .                                                               .
        .               Multicast Address Record [1]                    .
        .                                                               .
        |                                                               |
        +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
        |                                                               |
        .                                                               .
        .               Multicast Address Record [2]                    .
        .                                                               .
        |                                                               |
        +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
        |                               .                               |
        .                               .                               .
        |                               .                               |
        +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
        |                                                               |
        .                                                               .
        .               Multicast Address Record [M]                    .
        .                                                               .
        |                                                               |
        +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+


        where each Group Record has the following internal format:
        +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
        |  Record Type  |  Aux Data Len |     Number of Sources (N)     |
        +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
    |                                                               |
    *                                                               *
    |                                                               |
    *                       Multicast Address                       *
    |                                                               |
    *                                                               *
    |                                                               |
        +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
    |                                                               |
    *                                                               *
    |                                                               |
    *                       Source Address [1]                      *
    |                                                               |
    *                                                               *
    |                                                               |
    +-                                                             -+
    |                                                               |
    *                                                               *
    |                                                               |
    *                       Source Address [2]                      *
    |                                                               |
    *                                                               *
    |                                                               |
    +-                                                             -+
    .                               .                               .
    .                               .                               .
    .                               .                               .
    +-                                                             -+
    |                                                               |
    *                                                               *
    |                                                               |
    *                       Source Address [N]                      *
    |                                                               |
    *                                                               *
    |                                                               |
    +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
    |                                                               |
    .                                                               .
    .                         Auxiliary Data                        .
    .                                                               .
    |                                                               |
    +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
*/

VOID MLDSnooping(
        IN PRTMP_ADAPTER pAd,
        IN PUCHAR pDstMacAddr,
        IN PUCHAR pSrcMacAddr,
        IN PUCHAR pIpHeader,
        IN PNET_DEV pDev)
{
        INT i;
        UCHAR GroupType;
        UINT16 numOfGroup;
        PUCHAR pGroup;
        UCHAR AuxDataLen;
        UINT16 numOfSources;
        PUCHAR pGroupIpAddr;
        UCHAR GroupMacAddr[6];
        PUCHAR pGroupMacAddr = (PUCHAR)&GroupMacAddr;

        UINT8 MldType;
        PUCHAR pMldHeader;

        if(isMldPkt(pDstMacAddr, pIpHeader, &MldType, &pMldHeader) == TRUE)
        {
                DBGPRINT(RT_DEBUG_TRACE, ("MLD type=%0x\n", MldType));

                switch(MldType)
                {
                        case MLD_V1_LISTENER_REPORT:
                                // skip Type(1 Byte), code(1 Byte), checksum(2 Bytes), Maximum Rsp Delay(2 Bytes), Reserve(2 Bytes).
                                pGroupIpAddr = (PUCHAR)(pMldHeader + 8);
                                ConvertMulticastIP2MAC(pGroupIpAddr, (PUCHAR *)&pGroupMacAddr, ETH_P_IPV6);
                                DBGPRINT(RT_DEBUG_TRACE, ("Group Id=%02x:%02x:%02x:%02x:%02x:%02x\n",
                                                GroupMacAddr[0], GroupMacAddr[1], GroupMacAddr[2], GroupMacAddr[3], GroupMacAddr[4], GroupMacAddr[5]));
                                MulticastFilterTableInsertEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev, MCAT_FILTER_DYNAMIC);
                                break;

                        case MLD_V1_LISTENER_DONE:
                                // skip Type(1 Byte), code(1 Byte), checksum(2 Bytes), Maximum Rsp Delay(2 Bytes), Reserve(2 Bytes).
                                pGroupIpAddr = (PUCHAR)(pMldHeader + 8);
                                ConvertMulticastIP2MAC(pGroupIpAddr, (PUCHAR *)&pGroupMacAddr, ETH_P_IPV6);
                                DBGPRINT(RT_DEBUG_TRACE, ("Group Id=%02x:%02x:%02x:%02x:%02x:%02x\n",
                                                GroupMacAddr[0], GroupMacAddr[1], GroupMacAddr[2], GroupMacAddr[3], GroupMacAddr[4], GroupMacAddr[5]));
                                MulticastFilterTableDeleteEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev);
                                break;

                        case MLD_V2_LISTERNER_REPORT: // IGMP version 3 membership report.
                                numOfGroup = ntohs(*((UINT16 *)(pMldHeader + 6)));
                                pGroup = (PUCHAR)(pMldHeader + 8);
                                for (i=0; i < numOfGroup; i++)
                                {
                                        GroupType = (UCHAR)(*pGroup);
                                        AuxDataLen = (UCHAR)(*(pGroup + 1));
                                        numOfSources = ntohs(*((UINT16 *)(pGroup + 2)));
                                        pGroupIpAddr = (PUCHAR)(pGroup + 4);
                                        DBGPRINT(RT_DEBUG_TRACE, ("MLDv2 Type=%d, ADL=%d, numOfSource=%d\n", GroupType, AuxDataLen, numOfSources));
                                        ConvertMulticastIP2MAC(pGroupIpAddr, (PUCHAR *)&pGroupMacAddr, ETH_P_IPV6);
                                        DBGPRINT(RT_DEBUG_TRACE, ("MLD Group=%02x:%02x:%02x:%02x:%02x:%02x\n",
                                                        GroupMacAddr[0], GroupMacAddr[1], GroupMacAddr[2], GroupMacAddr[3], GroupMacAddr[4], GroupMacAddr[5]));

                                        do
                                        {
                                                if((GroupType == MODE_IS_EXCLUDE) || (GroupType == CHANGE_TO_EXCLUDE_MODE) || (GroupType == ALLOW_NEW_SOURCES))
                                                {
                                                        MulticastFilterTableInsertEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev, MCAT_FILTER_DYNAMIC);
                                                        break;
                                                }

                                                if((GroupType == MODE_IS_INCLUDE) || (GroupType == BLOCK_OLD_SOURCES))
                                                {
                                                        MulticastFilterTableDeleteEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev);
                                                        break;
                                                }

                                                if((GroupType == CHANGE_TO_INCLUDE_MODE))
                                                {
                                                        if(numOfSources == 0)
                                                                MulticastFilterTableDeleteEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev);
                                                        else
                                                                MulticastFilterTableInsertEntry(pAd, GroupMacAddr, pSrcMacAddr, pDev, MCAT_FILTER_DYNAMIC);
                                                        break;
                                                }
                                        } while(FALSE);
                                        // skip 4 Bytes (Record Type, Aux Data Len, Number of Sources) + a IPv6 address.
                                        pGroup += (4 + IPV6_ADDR_LEN + (numOfSources * 16) + AuxDataLen);
                                }
                                break;

                        default:
                                DBGPRINT(RT_DEBUG_TRACE, ("unknow MLD Type=%d\n", MldType));
                                break;
                }
        }

        return;
}


#endif // IGMP_SNOOP_SUPPORT //