/************************************************************************
* Copyright (C) 2010, Marvell Technology Group Ltd.
* All Rights Reserved.
*
* This is UNPUBLISHED PROPRIETARY SOURCE CODE of Marvell Technology Group;
* the contents of this file may not be disclosed to third parties, copied
* or duplicated in any form, in whole or in part, without the prior
* written permission of Marvell Technology Group.
*
*********************************************************************************
* Marvell GPL License Option
*
* If you received this File from Marvell, you may opt to use, redistribute and/or
* modify this File in accordance with the terms and conditions of the General
* Public License Version 2, June 1991 (the "GPL License"), a copy of which is
* available along with the File in the license.txt file or by writing to the Free
* Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 or
* on the worldwide web at http://www.gnu.org/licenses/gpl.txt.
*
* THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE IMPLIED
* WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE ARE EXPRESSLY
* DISCLAIMED.  The GPL License provides additional details about this warranty
* disclaimer.
*
*********************************************************************************
* mv_cust_netdev.c
*
* DESCRIPTION:
*
*
*******************************************************************************/
#include <mvCommon.h>

#include <linux/kernel.h>
#include <linux/version.h>
#include <linux/netdevice.h>
#include <linux/etherdevice.h>
#include <linux/skbuff.h>
#include <linux/if_vlan.h>
#include <net/ip.h>
#include <net/ipv6.h>
#ifdef CONFIG_MV_CUST_MLD_HANDLE
#include <linux/icmpv6.h>
#endif

#include <mvOs.h>
#include <ctrlEnv/mvCtrlEnvLib.h>

#include "mv_cust_dev.h"
#include "mv_cust_netdev.h"
#include "mv_cust_flow_map.h"
#include "mv_cust_mng_if.h"

/*----------------------------------------------------------------------------*/
/* External  declaration                                                      */
/*----------------------------------------------------------------------------*/

/*----------------------------------------------------------------------------*/
/* Global static definition                                                   */
/*----------------------------------------------------------------------------*/
/* YUVAL - update to pnc define */
#define     MH_GEM_PORT_MASK                (0x0FFF)
#define     MH_EPON_OAM_TYPE                (0x8809)
#define     ETY_IPV4                        (0x0800)
#define     IPV4_PROTO_OFFSET               (9)
#define     MV_CUST_SWF_TX_QUEUE            (6)

#define     CUST_TBL_NUM_ENTRIES(a)         (sizeof(a)/sizeof(a[0]))

/*Static Declarations */
static int    mv_eth_ports_num     = 0;
static int    mv_cust_debug_code   = 0;

static int    mv_cust_omci_gemport = 0;
static int    mv_cust_omci_rx_gh   = 0;
static int    mv_cust_oam_rx_gh    = 0;

/* Protocol definitions */
static struct mv_eth_tx_spec     omci_mgmt_tx_spec = {0, 0, 0, MV_CUST_SWF_TX_QUEUE};
static struct mv_eoam_llid_spec  epon_mgmt_tx_spec[MV_CUST_NUM_LLID];
#define EPON_MGMT_ENTRIES   CUST_TBL_NUM_ENTRIES(epon_mgmt_tx_spec)

#ifdef CONFIG_MV_CUST_UDP_SAMPLE_HANDLE
static struct mv_port_tx_spec    udp_port_spec_cfg[CONFIG_MV_ETH_PORTS_NUM];
#define PORT_ENTRIES        CUST_TBL_NUM_ENTRIES(udp_port_spec_cfg)
#endif

#ifdef CONFIG_MV_CUST_FLOW_MAP_HANDLE
static int    mv_cust_flow_map = 0;
#endif

/* Default Application Ethernet type used for socket and skb */
#define MV_CUST_ETH_TYPE_IGMP      (0xA000)
#define MV_CUST_ETH_TYPE_MLD       (0xAB00)
#define MV_CUST_ETH_TYPE_LPBK      (0xFFFA)
#define MV_CUST_ETH_TYPE_OAM       (0xBABA)
/* The Ethernet tpye of OAM/OMCI could be same since they will not use at the same time*/
#define MV_CUST_ETH_TYPE_OMCI      (0xBABA)

/* Global cust configuration*/
/* Pay attention that the order could be be changed and the entry could not be removed */
static mv_cust_app_config_t gCustConfig[] =
{
    /* Application Type */   /* Enable Flag */   /*Application Eth type*/   /* Application Description*/
    {MV_CUST_APP_TYPE_IGMP,  MV_CUST_APP_DISABLE, MV_CUST_ETH_TYPE_IGMP,     "IGMP application"},
    {MV_CUST_APP_TYPE_MLD,   MV_CUST_APP_DISABLE, MV_CUST_ETH_TYPE_MLD,      "MLD application"},
    {MV_CUST_APP_TYPE_LPBK,  MV_CUST_APP_DISABLE, MV_CUST_ETH_TYPE_LPBK,     "Loopback detection application"},
    {MV_CUST_APP_TYPE_OAM,   MV_CUST_APP_DISABLE, MV_CUST_ETH_TYPE_OAM,      "eOAM application"},
    {MV_CUST_APP_TYPE_OMCI,  MV_CUST_APP_DISABLE, MV_CUST_ETH_TYPE_OMCI,     "OMCI application"},    
};

/*----------------------------------------------------------------------------*/
/* Function implementation                                                    */
/*----------------------------------------------------------------------------*/

void mv_cust_debug_info_set(int val)
{
    mv_cust_debug_code = val;
    return;
}


void mv_cust_app_flag_set(mv_cust_app_type_e app_type, uint16_t enable)
{
    if (mv_cust_debug_code)
        printk("%s() In, app_type[%d], enable[%d] \n", __func__, app_type, enable);
    
    if (app_type > (MV_CUST_APP_TYPE_MAX-1))
    {
        printk("%s: illegal application type[%d], allowed max type[%d]  \n",
                __func__, app_type, MV_CUST_APP_TYPE_MAX-1);
        return;
    }

    if (app_type == MV_CUST_APP_TYPE_OMCI)
    {
        if (enable) 
        {
            if (gCustConfig[MV_CUST_APP_TYPE_OAM].enable) 
            {
                MVCUST_ERR_PRINT("EPON is already valid\n");
                return;
            }
            gCustConfig[MV_CUST_APP_TYPE_OMCI].enable = MV_CUST_APP_ENABLE;
#ifdef CONFIG_MV_CUST_FLOW_MAP_HANDLE
            mv_cust_flow_map   = 1;
#endif
        }
        else 
        {
            gCustConfig[MV_CUST_APP_TYPE_OMCI].enable = MV_CUST_APP_DISABLE;
#ifdef CONFIG_MV_CUST_FLOW_MAP_HANDLE
            mv_cust_flow_map   = 0;
#endif
        }
    }
    else if(app_type == MV_CUST_APP_TYPE_OAM)
    {
        if (enable) 
        {
            if (gCustConfig[MV_CUST_APP_TYPE_OMCI].enable) 
            {
                MVCUST_ERR_PRINT("GPON is already valid\n");
                return;
            }
            gCustConfig[MV_CUST_APP_TYPE_OAM].enable = MV_CUST_APP_ENABLE;
#ifdef CONFIG_MV_CUST_FLOW_MAP_HANDLE
            mv_cust_flow_map   = 0;
#endif
        }
        else 
        {
            gCustConfig[MV_CUST_APP_TYPE_OAM].enable = MV_CUST_APP_DISABLE;
#ifdef CONFIG_MV_CUST_FLOW_MAP_HANDLE
            mv_cust_flow_map   = 0;
#endif
        }

    }
    else
    {
        if (enable == MV_CUST_APP_ENABLE)
            gCustConfig[app_type].enable = MV_CUST_APP_ENABLE;
        else
            gCustConfig[app_type].enable = MV_CUST_APP_DISABLE;
    }

    return;
}
EXPORT_SYMBOL(mv_cust_app_flag_set);


void mv_cust_app_etype_set(mv_cust_app_type_e app_type, uint16_t eth_type)
{
    if (mv_cust_debug_code)
        printk("%s() In, app_type[%d], eth_type[%d] \n", __func__, app_type, eth_type);
    
    if (app_type > (MV_CUST_APP_TYPE_MAX-1))
    {
        printk("%s: illegal application type[%d], allowed max type[%d]  \n",
                __func__, app_type, MV_CUST_APP_TYPE_MAX-1);
        return;
    }

    gCustConfig[app_type].eth_type = eth_type;

    return;
}
EXPORT_SYMBOL(mv_cust_app_etype_set);


void mv_cust_rec_skb(int port, struct sk_buff *skb)
{
    uint32_t rx_status;
    struct eth_port *pp;

    rx_status = netif_receive_skb(skb);
    pp = mv_eth_port_by_id(port);
    STAT_DBG(if (rx_status) (pp->stats.rx_drop_sw++));
}

void mv_cust_omci_rx_gh_set(int val)
{
    mv_cust_omci_rx_gh = val;
    return;
}

int mv_cust_omci_tx_set(int tcont, int txq)
{
    if (mvNetaTxpCheck(MV_PON_PORT_ID, tcont)) {
        return -EINVAL;
    }

    if (txq<0 || txq>CONFIG_MV_ETH_TXQ) {
        return -EINVAL;
    }

    omci_mgmt_tx_spec.txp = tcont;
    omci_mgmt_tx_spec.txq = txq;

    return 0;
}

void mv_cust_omci_gemport_set(int gemport)
{
    mv_cust_omci_gemport = gemport;
    return;
}

void mv_cust_omci_hw_cmd_set(uint32_t hw_cmd)
{
    omci_mgmt_tx_spec.hw_cmd = hw_cmd;
}

int mv_cust_omci_set(int tcont, int txq, int gem_port, int keep_rx_mh)
{
    int ret, hw_cmd;

    omci_mgmt_tx_spec.tx_func = NULL;
    omci_mgmt_tx_spec.flags = MV_ETH_F_NO_PAD | MV_ETH_F_MH;

    ret = mv_cust_omci_tx_set(tcont, txq);
    if (ret) {
        MVCUST_ERR_PRINT("mv_cust_omci_tx_set - ret(%d)  \n", ret);
        return ret;
    }

    mv_cust_omci_gemport_set(gem_port);
    hw_cmd = ((gem_port << 8) | 0x0010);
    mv_cust_omci_hw_cmd_set(hw_cmd);
    mv_cust_omci_rx_gh_set(keep_rx_mh);

    mv_cust_app_flag_set(MV_CUST_APP_TYPE_OMCI, MV_CUST_APP_ENABLE);

    return 0;
}
EXPORT_SYMBOL(mv_cust_omci_set);

void mv_cust_omci_print(void)
{
    printk("************* OMCI Configuration *****************\n\n");
    printk("OMCI: valid = %d, gemport = %d, ethtype = 0x%04x, gh_keep = %d\n",
           gCustConfig[MV_CUST_APP_TYPE_OMCI].enable,
           mv_cust_omci_gemport, 
           ntohs(gCustConfig[MV_CUST_APP_TYPE_OMCI].eth_type), 
           mv_cust_omci_rx_gh);
    printk("OMCI: txp   = %d, txq = %d, hw_cmd = 0x%08x, flags = 0x%04x on TX \n",
           omci_mgmt_tx_spec.txp, omci_mgmt_tx_spec.txq, omci_mgmt_tx_spec.hw_cmd, omci_mgmt_tx_spec.flags);
    printk("\n");
}

static int mv_cust_omci_gem_parse(uint8_t *data)
{
    uint16_t gh;

    gh = ntohs(*(uint16_t *)data);

    if(mv_cust_debug_code)
        printk("%s:gh= 0x(%04x) - mv_cust_omci_gemport= 0x(%04x)  \n", __func__, gh, mv_cust_omci_gemport);

    /* Compare GH for omci_gemport */
    if ( (gh & MH_GEM_PORT_MASK) != mv_cust_omci_gemport ) {
        if(mv_cust_debug_code)
            printk("%s: compare GH for OMCI_gemport failed: gh= 0x(%04x) - mv_cust_omci_gemport= 0x(%04x)  \n", __func__, gh, mv_cust_omci_gemport);
        return(0);
    }

    return(1);
}

static int mv_cust_omci_rx(int port, struct net_device *dev, struct sk_buff *skb, struct neta_rx_desc *rx_desc)
{
    uint32_t rx_bytes;

    if (!mv_cust_omci_gem_parse(skb->data))
        return 0;
    if (mv_cust_omci_rx_gh) {
        rx_bytes = rx_desc->dataSize;
    }
    else {
        skb->data += MV_ETH_MH_SIZE;
        rx_bytes = rx_desc->dataSize - MV_ETH_MH_SIZE;
    }
    skb->tail += rx_bytes;
    skb->len = rx_bytes;
    skb->protocol = eth_type_trans(skb, dev);
    skb->protocol = htons(gCustConfig[MV_CUST_APP_TYPE_OMCI].eth_type);
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)
    skb->dev = dev;
#endif
    mv_cust_rec_skb(port, skb);

    return 1;
}

int mv_cust_omci_tx(int port, struct net_device *dev, struct sk_buff *skb,
                   struct mv_eth_tx_spec *tx_spec_out)
{
    if ((skb->protocol == htons(gCustConfig[MV_CUST_APP_TYPE_OMCI].eth_type))
        && (MV_CUST_APP_ENABLE == gCustConfig[MV_CUST_APP_TYPE_OMCI].enable)
        && (port == MV_PON_PORT_ID)) {
        memcpy (tx_spec_out, &omci_mgmt_tx_spec, sizeof(struct mv_eth_tx_spec));
        if(mv_cust_debug_code)
            printk("%s", __func__);
        return 1;
    }
    return 0;
}


void mv_cust_oam_rx_gh_set(int val)
{
    mv_cust_oam_rx_gh = val;
    return;
}

int mv_cust_eoam_llid_set(int llid, uint8_t *llid_mac, int txq)
{

    if (mvNetaTxpCheck(MV_PON_PORT_ID, llid))
        return -EINVAL;

    if (txq<0 || txq>CONFIG_MV_ETH_TXQ) {
            return -EINVAL;
    }
    if (!llid_mac) {
        return -EINVAL;
    }
    memcpy((void *) &(epon_mgmt_tx_spec[llid].llid_mac_address), (void *) llid_mac, ETH_ALEN);

    epon_mgmt_tx_spec[llid].tx_spec.txq = txq;

    return 0;
}
EXPORT_SYMBOL(mv_cust_eoam_llid_set);

void mv_cust_eoam_init(void)
{
    int i;

    for (i=0;i <(EPON_MGMT_ENTRIES);i++) {
        memset(&epon_mgmt_tx_spec[i],0, sizeof(struct mv_eoam_llid_spec));

        /* Each table entry (i) a-priori corresponds to LLID (i) */
        epon_mgmt_tx_spec[i].tx_spec.txp = i;

        /* The mv_neta driver will add the default MH. The MH value has no significance in EPON Upstream */
        epon_mgmt_tx_spec[i].tx_spec.flags = MV_ETH_F_MH;
    }

    /* In Rx, keep the MH for EOAM */
    mv_cust_oam_rx_gh_set(1);
    return ;
}

void mv_cust_eoam_print(void)
{
    int i;
    printk("************* eOAM Configuration *****************\n\n");
    printk("EOAM: valid = %d, ethtype = 0x%04x, gh_keep = %d\n",
           gCustConfig[MV_CUST_APP_TYPE_OAM].enable, 
           ntohs(gCustConfig[MV_CUST_APP_TYPE_OAM].eth_type), 
           mv_cust_oam_rx_gh);
    for (i=0;i <(EPON_MGMT_ENTRIES);i++) {
        printk("llid%d: mac=%02x:%02x:%02x:%02x:%02x:%02x, txp=%d, txq=%d, hw_cmd=0x%08x, flags = 0x%04x\n",
               i,
               epon_mgmt_tx_spec[i].llid_mac_address[0],epon_mgmt_tx_spec[i].llid_mac_address[1],
               epon_mgmt_tx_spec[i].llid_mac_address[2],epon_mgmt_tx_spec[i].llid_mac_address[3],
               epon_mgmt_tx_spec[i].llid_mac_address[4],epon_mgmt_tx_spec[i].llid_mac_address[5],
               epon_mgmt_tx_spec[i].tx_spec.txp, epon_mgmt_tx_spec[i].tx_spec.txq,
               epon_mgmt_tx_spec[i].tx_spec.hw_cmd, epon_mgmt_tx_spec[i].tx_spec.flags);
        printk("\n");
    }
}

static int mv_cust_eoam_type_parse(uint8_t *data)
{
    uint16_t ety;

    ety = ntohs(*(uint16_t *)(data + MV_ETH_MH_SIZE + ETH_ALEN + ETH_ALEN));

    if(mv_cust_debug_code)
        printk("%s: ety 0x(%04x)\n", __func__, ety);

    /* Compare EPON OAM ether_type */
    if (ety == MH_EPON_OAM_TYPE)
        return(1);

    return(0);
}

static int mv_cust_epon_oam_rx(int port, struct net_device *dev, struct sk_buff *skb, struct neta_rx_desc *rx_desc)
{
    uint32_t rx_bytes;

    if (!mv_cust_eoam_type_parse(skb->data))
        return 0;

    if (mv_cust_oam_rx_gh) {
        rx_bytes = rx_desc->dataSize;
    }
    else {
        skb->data += MV_ETH_MH_SIZE;
        rx_bytes = rx_desc->dataSize - MV_ETH_MH_SIZE;
    }

    skb->tail += rx_bytes;
    skb->len = rx_bytes;
    skb->protocol = eth_type_trans(skb, dev);
    skb->protocol = htons(gCustConfig[MV_CUST_APP_TYPE_OAM].eth_type);
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)
    skb->dev = dev;
#endif
    mv_cust_rec_skb(port, skb);

    return 1;
}

int mv_cust_eoam_tx(int port, struct net_device *dev, struct sk_buff *skb,
                    struct mv_eth_tx_spec *tx_spec_out)
{
    int mac_match, i;

    if ((skb->protocol == htons(gCustConfig[MV_CUST_APP_TYPE_OAM].eth_type))
        && (MV_CUST_APP_ENABLE == gCustConfig[MV_CUST_APP_TYPE_OAM].enable)
        && port == MV_PON_PORT_ID) {
        /* Lookup MAC Address */
        for (i=0; i<(EPON_MGMT_ENTRIES);i++) {
            mac_match = memcmp((void *) &(epon_mgmt_tx_spec[i].llid_mac_address[0]),
                               (void *)(skb->data + /*MV_ETH_MH_SIZE +*/ ETH_ALEN),
                               ETH_ALEN);
            if (!mac_match) {
                memcpy (tx_spec_out, &epon_mgmt_tx_spec[i].tx_spec, sizeof(struct mv_eth_tx_spec));
                if(mv_cust_debug_code)
                    printk("%s, llid = %d", __func__, i);
                return 1;
            }
        }
        /* Source MAC Address not found */
        if(mv_cust_debug_code) {
            printk("(%s)Input Packet first bytes:\n", __func__);
            for (i=0;i<24;i++) {
                if (i%8== 0)
                    printk("\n");
                printk ("%02x ", *(skb->data + i));
            }
        }
    }
    return 0;
}


#ifdef CONFIG_MV_CUST_IGMP_HANDLE
void mv_cust_igmp_print(void)
{
    printk("************* IGMP Configuration *****************\n\n");
    printk("IGMP valid = %d,  ethtype = 0x%04x \n",
           gCustConfig[MV_CUST_APP_TYPE_IGMP].enable, 
           gCustConfig[MV_CUST_APP_TYPE_IGMP].eth_type);
    printk("IGMP default txq = %d\n",MV_CUST_SWF_TX_QUEUE);
    printk("\n");
}

static int mv_cust_igmp_parse(uint8_t *data)
{
    uint16_t ety;
    uint8_t  proto;
    uint8_t *fieldp = data + MV_ETH_MH_SIZE + ETH_ALEN + ETH_ALEN;

    /* Loop through VLAN tags */
    ety = ntohs(*(uint16_t *)fieldp);
    while (ety == 0x8100 || ety == 0x9100 || ety == 0x88A8) {
        fieldp+= VLAN_HLEN;
        ety = ntohs(*(uint16_t *)fieldp);
    }

    if(mv_cust_debug_code)
        printk("%s:ety 0x(%04x)\n", __func__, ety);

    if (ety == ETY_IPV4) {
        fieldp+= 2;
        fieldp+= IPV4_PROTO_OFFSET;
        proto = *fieldp;
        if (mv_cust_debug_code)
            printk("%s:proto 0x(%02x)\n", __func__, proto);

        if (proto == IPPROTO_IGMP)
            return(1);
    }

    return(0);
}

static int mv_cust_igmp_rx(int port, struct net_device *dev, struct sk_buff *skb, struct neta_rx_desc *rx_desc)
{
    uint32_t rx_bytes;

    if (!mv_cust_igmp_parse(skb->data))
        return 0;

    /* To Indicate the source GMAC */
    skb->data[0] = port;

    rx_bytes = rx_desc->dataSize;

    skb->tail += rx_bytes;
    skb->len = rx_bytes;
    skb->protocol = eth_type_trans(skb, dev);
    skb->protocol = htons(gCustConfig[MV_CUST_APP_TYPE_IGMP].eth_type);
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)
    skb->dev = dev;
#endif
    mv_cust_rec_skb(port, skb);

    return 1;
}

int mv_cust_igmp_tx(int port, struct net_device *dev, struct sk_buff *skb,
                    struct mv_eth_tx_spec *tx_spec_out)
{
    if (gCustConfig[MV_CUST_APP_TYPE_IGMP].enable == MV_CUST_APP_ENABLE) {
        /* Check application Ethernet type */
        if (skb->protocol == htons(gCustConfig[MV_CUST_APP_TYPE_IGMP].eth_type)) {

            /* The Mapping and VLAN mod should be support in next phase */
            if (MV_PON_PORT_ID == port)
            {
                tx_spec_out->flags = MV_ETH_F_MH;
            }
            else
            {
                tx_spec_out->flags = 0;
            }
            
            tx_spec_out->txp     = 0;
            tx_spec_out->txq     = MV_CUST_SWF_TX_QUEUE;
            tx_spec_out->hw_cmd  = 0;
#ifdef CONFIG_MV_ETH_TX_SPECIAL
            tx_spec_out->tx_func = NULL;
#endif
            return 1;
        }
    }
    return 0;
}

#endif


#ifdef CONFIG_MV_CUST_MLD_HANDLE
void mv_cust_mld_print(void)
{
    printk("************* MLD Configuration *****************\n\n");
    printk("MLD valid = %d,  ethtype = 0x%04x \n",
           gCustConfig[MV_CUST_APP_TYPE_MLD].enable, 
           gCustConfig[MV_CUST_APP_TYPE_MLD].eth_type);
    printk("MLD default txq = %d\n",MV_CUST_SWF_TX_QUEUE);
    printk("\n");
}

static int mv_cust_mld_parse(uint8_t *data)
{
    uint16_t ety;
    uint8_t *fieldp = data + MV_ETH_MH_SIZE + ETH_ALEN + ETH_ALEN;


    /* Loop through VLAN tags */
    ety = ntohs(*(uint16_t *)fieldp);
    while (ety == 0x8100 || ety == 0x9100 ||ety == 0x88A8) {
        fieldp+= VLAN_HLEN;
        ety = ntohs(*(uint16_t *)fieldp);
    }

    if(mv_cust_debug_code)
        printk("%s:ety 0x(%04x)\n", __func__, ety);

    if (ety == ETH_P_IPV6) 
    {
        struct ipv6hdr *hdr = (struct ipv6hdr *)(fieldp+2);
        struct ipv6_hopopt_hdr *hopopthdr ;
        struct icmp6hdr *pic;

        if (hdr->nexthdr != NEXTHDR_HOP )
          return 0;

        hopopthdr = (struct ipv6_hopopt_hdr *)((uint8_t *)hdr+ sizeof(struct ipv6hdr));

        if ( hopopthdr->nexthdr != IPPROTO_ICMPV6)
            return 0;

        pic =  (struct icmp6hdr *)((uint8_t *)hopopthdr+ipv6_optlen(hopopthdr));

        switch (pic->icmp6_type) {
        case ICMPV6_MGM_QUERY:
        case ICMPV6_MGM_REPORT:
        case ICMPV6_MGM_REDUCTION:
        case ICMPV6_MLD2_REPORT:
            return 1;
        default:
            break;
        }
        
    }
    
    return(0);
}

static int mv_cust_mld_rx(int port, struct net_device *dev, struct sk_buff *skb, struct neta_rx_desc *rx_desc)
{
    uint32_t rx_bytes;

    if (!mv_cust_mld_parse(skb->data))
        return 0;

    /* To Indicate the source GMAC */
    skb->data[0] = port;

    rx_bytes = rx_desc->dataSize;

    skb->tail += rx_bytes;
    skb->len = rx_bytes;
    skb->protocol = eth_type_trans(skb, dev);
    skb->protocol = htons(gCustConfig[MV_CUST_APP_TYPE_MLD].eth_type);
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)
    skb->dev = dev;
#endif

    mv_cust_rec_skb(port, skb);

    return 1;
}

int mv_cust_mld_tx(int port, struct net_device *dev, struct sk_buff *skb,
                   struct mv_eth_tx_spec *tx_spec_out)
{
    if (gCustConfig[MV_CUST_APP_TYPE_MLD].enable == MV_CUST_APP_ENABLE) {
        /* Check application Ethernet type */
        if (skb->protocol == htons(gCustConfig[MV_CUST_APP_TYPE_MLD].eth_type)) {

            /* The Mapping and VLAN mod should be support in next phase */
            if (MV_PON_PORT_ID == port)
            {
                tx_spec_out->flags = MV_ETH_F_MH;
            }
            else
            {
                tx_spec_out->flags = 0;
            }
            
            tx_spec_out->txp     = 0;
            tx_spec_out->txq     = MV_CUST_SWF_TX_QUEUE;
            tx_spec_out->hw_cmd  = 0;
#ifdef CONFIG_MV_ETH_TX_SPECIAL
            tx_spec_out->tx_func = NULL;
#endif
            return 1;
        }
    }
    return 0;
}

#endif


#ifdef CONFIG_MV_CUST_LPBK_DETECT_HANDLE
void mv_cust_loopdet_print(void)
{
    printk("************* UNI loopback detection Configuration *****************\n\n");
    printk("Lpbk detect valid = %d,  ethtype = 0x%04x \n",
           gCustConfig[MV_CUST_APP_TYPE_LPBK].enable, 
           gCustConfig[MV_CUST_APP_TYPE_LPBK].eth_type);
    printk("Lpbk detect default txq = %d\n",MV_CUST_SWF_TX_QUEUE);
    printk("\n");
}

static int mv_cust_loopdet_parse(uint8_t *data)
{
    uint16_t ety;
    uint8_t *fieldp = data + MV_ETH_MH_SIZE + ETH_ALEN + ETH_ALEN;

    /* Loop through VLAN tags */
    ety = ntohs(*(uint16_t *)fieldp);
    while (ety == 0x8100 || ety == 0x9100 ||ety == 0x88A8) {
        fieldp+= VLAN_HLEN;
        ety = ntohs(*(uint16_t *)fieldp);
    }
    if(mv_cust_debug_code)
        printk("%s: ety 0x(%04x)\n", __func__, ety);

    /* Compare EPON OAM ether_type */
    if (ety == gCustConfig[MV_CUST_APP_TYPE_LPBK].eth_type)
        return(1);

    return(0);
}

static int mv_cust_loopdet_rx(int port, struct net_device *dev, struct sk_buff *skb, struct neta_rx_desc *rx_desc)
{
    uint32_t rx_bytes;

    if (!mv_cust_loopdet_parse(skb->data))
        return 0;
    
    /* To Indicate the source GMAC */
    skb->data[0] = port;

    rx_bytes = rx_desc->dataSize;

    skb->tail += rx_bytes;
    skb->len = rx_bytes;
    skb->protocol = eth_type_trans(skb, dev);
    skb->protocol = htons(gCustConfig[MV_CUST_APP_TYPE_LPBK].eth_type);
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)
    skb->dev = dev;
#endif
    mv_cust_rec_skb(port, skb);

    return 1;
}

int mv_cust_loopdet_tx(int port, struct net_device *dev, struct sk_buff *skb,
                       struct mv_eth_tx_spec *tx_spec_out)
{
    if (MV_CUST_APP_ENABLE == gCustConfig[MV_CUST_APP_TYPE_LPBK].enable) {
        /* Check application Ethernet type */
        if (skb->protocol == htons(gCustConfig[MV_CUST_APP_TYPE_LPBK].eth_type)) {
         
            if (MV_PON_PORT_ID == port)
            {
                tx_spec_out->flags = MV_ETH_F_MH;
            }
            else
            {
                tx_spec_out->flags = 0;
            }
            
            tx_spec_out->txp     = 0;
            tx_spec_out->txq     = MV_CUST_SWF_TX_QUEUE;
            tx_spec_out->hw_cmd  = 0;
#ifdef CONFIG_MV_ETH_TX_SPECIAL
            tx_spec_out->tx_func = NULL;
#endif          
            return 1;
        }
    }
    return 0;
}

#endif


#ifdef CONFIG_MV_CUST_UDP_SAMPLE_HANDLE
static inline void mv_cust_copy_tx_spec(struct mv_eth_tx_spec * tx_spec,
                                        uint8_t txp, uint8_t txq,
                                        uint16_t flags, uint32_t hw_cmd)
{
    tx_spec->txp = txp;
    tx_spec->txq = txq;
    tx_spec->hw_cmd = hw_cmd;
    tx_spec->flags = flags;
}

int mv_cust_udp_spec_print(int port)
{
    int i;
    struct eth_port *pp = mv_eth_port_by_id(port);
    struct mv_udp_port_tx_spec *udp_spec;

    if (!pp)
        return -ENODEV;

    udp_spec = &(udp_port_spec_cfg[port].udp_dst[0]);

    printk("\n**** port #%d - TX UDP Dest Port configuration *****\n", port);
    printk("----------------------------------------------------\n");
    printk("ID udp_dst   txp    txq    flags    hw_cmd     func_add\n");
    for (i = 0; i < sizeof(udp_port_spec_cfg[port].udp_dst)/sizeof(udp_port_spec_cfg[port].udp_dst[0]); i++) {
        if (udp_spec[i].tx_spec.txq != MV_ETH_TXQ_INVALID)
            printk("%2d   %04d      %d      %d     0x%04x   0x%08x   0x%p\n",
                   i, ntohs(udp_spec[i].udp_port),
                   udp_spec[i].tx_spec.txp, udp_spec[i].tx_spec.txq,
                   udp_spec[i].tx_spec.flags, udp_spec[i].tx_spec.hw_cmd,
                   udp_spec[i].tx_spec.tx_func);
    }
    printk("-----------------------------------------------------\n");

    udp_spec = &(udp_port_spec_cfg[port].udp_src[0]);

    printk("**** port #%d - TX UDP Source Port configuration *****\n", port);
    printk("-----------------------------------------------------\n");
    printk("ID udp_src   txp    txq     flags    hw_cmd     func_add\n");
    for (i = 0; i < sizeof(udp_port_spec_cfg[port].udp_src)/sizeof(udp_port_spec_cfg[port].udp_src[0]); i++) {
        if (udp_spec[i].tx_spec.txq != MV_ETH_TXQ_INVALID)
            printk("%2d   %04d      %d      %d     0x%04x   0x%08x   0x%p\n",
                   i, ntohs(udp_spec[i].udp_port),
                   udp_spec[i].tx_spec.txp, udp_spec[i].tx_spec.txq,
                   udp_spec[i].tx_spec.flags, udp_spec[i].tx_spec.hw_cmd,
                   udp_spec[i].tx_spec.tx_func);
    }
    printk("**************************************************************\n");

    return 0;
}


void mv_cust_udp_spec_print_all(void)
{
    int port;

    for (port=0;port < CONFIG_MV_ETH_PORTS_NUM ;port++) {
        mv_cust_udp_spec_print(port);
    }
}

MV_STATUS  mv_cust_udp_int_spec_set(struct mv_udp_port_tx_spec *udp_spec, uint16_t udp_port, int table_size,
                                    uint8_t txp, uint8_t txq, uint16_t flags, uint32_t hw_cmd)
{
    int i;

    /* Check if already exists */
    for (i=0; i < table_size;i++) {
        if (udp_spec[i].udp_port == htons(udp_port) &&
            udp_spec[i].tx_spec.txq != MV_ETH_TXQ_INVALID) {
            mv_cust_copy_tx_spec(&(udp_spec[i].tx_spec), txp, txq, flags, hw_cmd);
            return MV_OK;
        }
    }
    /* Check empty */
    for (i=0; i < table_size;i++) {
        if (udp_spec[i].tx_spec.txq == MV_ETH_TXQ_INVALID) {
            udp_spec[i].udp_port = htons(udp_port);
            mv_cust_copy_tx_spec(&(udp_spec[i].tx_spec), txp, txq, flags, hw_cmd);
            return MV_OK;
        }
    }

    return(MV_FULL);
}


MV_STATUS  mv_cust_udp_src_spec_set(int tx_port, uint16_t udp_src_port, uint8_t txp, uint8_t txq, uint16_t flags, uint32_t hw_cmd)
{
    struct eth_port *pp = mv_eth_port_by_id(tx_port);
    struct mv_udp_port_tx_spec *udp_src_spec = udp_port_spec_cfg[tx_port].udp_src;
    MV_STATUS mv_status;

    if (!pp)
        return -ENODEV;

    mv_status = mv_cust_udp_int_spec_set(udp_src_spec, udp_src_port,
                                         sizeof(udp_port_spec_cfg[tx_port].udp_src)/sizeof(udp_port_spec_cfg[tx_port].udp_src[0]),
                                         txp, txq, flags, hw_cmd);

    if (mv_status != MV_OK)
        printk("%s: UDP Special Source Port Table is full\n", __func__);

    return(mv_status);
}
EXPORT_SYMBOL(mv_cust_udp_src_spec_set);


MV_STATUS  mv_cust_udp_dest_spec_set(int tx_port, uint16_t udp_dest_port, uint8_t txp, uint8_t txq, uint16_t flags, uint32_t hw_cmd)
{
    struct eth_port *pp = mv_eth_port_by_id(tx_port);
    struct mv_udp_port_tx_spec *udp_dst_spec = udp_port_spec_cfg[tx_port].udp_dst;
    MV_STATUS mv_status;

    if (!pp)
        return -ENODEV;

    mv_status = mv_cust_udp_int_spec_set(udp_dst_spec, udp_dest_port,
                                         sizeof(udp_port_spec_cfg[tx_port].udp_dst)/sizeof(udp_port_spec_cfg[tx_port].udp_dst[0]),
                                         txp, txq, flags, hw_cmd);

    if (mv_status != MV_OK)
        printk("%s: UDP Special Dest. Port Table is full\n", __func__);

    return(mv_status);
}
EXPORT_SYMBOL(mv_cust_udp_dest_spec_set);


void  mv_cust_udp_table_del(void)
{
    int num_ports = PORT_ENTRIES;
    int tx_port, i;

    if (num_ports > mv_eth_ports_num)
        num_ports = mv_eth_ports_num;


    for (tx_port=0; tx_port<num_ports;tx_port++) {

        /* Invalidate UDP Dest ports, set txq=invalid  */
        for (i=0;i<(sizeof(udp_port_spec_cfg[tx_port].udp_dst)/sizeof(udp_port_spec_cfg[tx_port].udp_dst[0]));i++) {
            memset(&(udp_port_spec_cfg[tx_port].udp_dst[i]), 0, sizeof(struct mv_udp_port_tx_spec));
            udp_port_spec_cfg[tx_port].udp_dst[i].tx_spec.txq = MV_ETH_TXQ_INVALID;
        }

        /* Invalidate UDP Source ports, , set txq=invalid */
        for (i=0;i<(sizeof(udp_port_spec_cfg[tx_port].udp_src)/sizeof(udp_port_spec_cfg[tx_port].udp_src[0]));i++) {
            memset(&(udp_port_spec_cfg[tx_port].udp_src[i]), 0, sizeof(struct mv_udp_port_tx_spec));
            udp_port_spec_cfg[tx_port].udp_src[i].tx_spec.txq = MV_ETH_TXQ_INVALID;
        }

    }
    return;
}

int mv_cust_udp_port_tx(int port, struct net_device *dev, struct sk_buff *skb,
                        struct mv_eth_tx_spec *tx_spec_out)
{
    struct iphdr  * iphdrp   = NULL;
    struct udphdr * udphdrp  = NULL;
    int i;

    if (port > CONFIG_MV_ETH_PORTS_NUM) {
        printk("Port Error\n");
        return(0);
    }

    if (skb->protocol == ETY_IPV4) {
        /* Get UDP Port */
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,22)
    iphdrp = skb->nh.iph;
#else
    iphdrp = ip_hdr(skb);
#endif

        if ((iphdrp) && (iphdrp->protocol == IPPROTO_UDP)) {
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,22)
        udphdrp = skb->h.uh;
#else
        udphdrp = udp_hdr(skb);
#endif
            if (udphdrp) {
                if (udphdrp == (struct udphdr *)iphdrp) {
                    udphdrp = (struct udphdr *)((char *)udphdrp + (4*(iphdrp->ihl)));
                }
                /* Find configured UDP Source Port*/
                for (i=0; i < sizeof(udp_port_spec_cfg[port].udp_src)/sizeof(udp_port_spec_cfg[port].udp_src[0]);i++) {
                    if ((udphdrp->source == udp_port_spec_cfg[port].udp_src[i].udp_port) &&
                        (udp_port_spec_cfg[port].udp_src[i].tx_spec.txq != MV_ETH_TXQ_INVALID)) {
                        memcpy (tx_spec_out, &(udp_port_spec_cfg[port].udp_src[i].tx_spec), sizeof(struct mv_eth_tx_spec));
                        if (mv_cust_debug_code)
                            printk("%s: found udp_src 0x(%04x)\n", __func__, ntohs(udphdrp->source));
                        return 1;
                    }
                }
                /* Find configured UDP Dest. Port*/
                for (i=0; i < sizeof(udp_port_spec_cfg[port].udp_dst)/sizeof(udp_port_spec_cfg[port].udp_dst[0]);i++) {
                    if ((udphdrp->dest == udp_port_spec_cfg[port].udp_dst[i].udp_port) &&
                        (udp_port_spec_cfg[port].udp_src[i].tx_spec.txq != MV_ETH_TXQ_INVALID)) {
                        memcpy (tx_spec_out, &(udp_port_spec_cfg[port].udp_dst[i].tx_spec), sizeof(struct mv_eth_tx_spec));
                        if (mv_cust_debug_code)
                            printk("%s: found udp_dst 0x(%04x)\n", __func__, ntohs(udphdrp->dest));
                        return 1;
                    }
                }

        if ( (port == MV_PON_PORT_ID) && mv_cust_debug_code) {
            printk("%s:DEBUG : Packet UDP, udp source or dest port not found udp_src(%x)x udp_dst(%x)x\n",
                   __func__,ntohs(udphdrp->source), ntohs(udphdrp->dest));
        }

            }
        }
        else if ( (port == MV_PON_PORT_ID) && mv_cust_debug_code )
            printk("%s:DEBUG : NOT UDP, ip_proto(%d) \n", __func__, iphdrp->protocol);
    }
    else if ( (port == MV_PON_PORT_ID) && mv_cust_debug_code )
        printk("%s:DEBUG : NOT IP, proto(%d) \n", __func__, skb->protocol);

    return 0;

}
#endif


#ifdef CONFIG_MV_CUST_FLOW_MAP_HANDLE
void mv_cust_flow_map_print(void)
{
    printk("************* Flow Mapping Configuration *****************\n\n");
    printk("Flow mapping valid = %d\n", mv_cust_flow_map);
}

static int mv_cust_flow_map_parse(uint8_t *data, uint16_t *vlan, uint8_t *pbits, uint8_t dir)
{
    uint16_t ety;
    uint8_t *fieldp ;

    if (MV_CUST_FLOW_DIR_US == dir)
        fieldp = data + ETH_ALEN + ETH_ALEN;
    else
        fieldp = data + ETH_ALEN + ETH_ALEN + MV_ETH_MH_SIZE;

    /* Loop through VLAN tags */
    ety = ntohs(*(uint16_t *)fieldp);
    if (ety == 0x8100 || ety == 0x88A8 || ety == 0x9100) {
        fieldp += 2;
        *vlan  = ntohs(*(uint16_t *)fieldp);
        *pbits = (*vlan >> 13 ) & 0x7;
        *vlan  = (*vlan) & 0xfff;
        return(1);
    }
    else {
        return(0);
    }

    return(0);
}

static int mv_cust_flow_map_mod(uint8_t *data, uint16_t vid, uint8_t pbits, uint8_t dir)
{
    uint16_t ety  = 0;
    uint16_t vlan = 0;
    uint8_t *fieldp;

    if (MV_CUST_FLOW_DIR_US == dir)
        fieldp = data + ETH_ALEN + ETH_ALEN;
    else
        fieldp = data + ETH_ALEN + ETH_ALEN + MV_ETH_MH_SIZE;

    /* If not need to modify VID or P-bits */
    if((vid == MV_CUST_VID_NOT_CARE_VALUE) &&
       (pbits == MV_CUST_PBITS_NOT_CARE_VALUE))
        return (1);

    /* Loop through VLAN tags */
    ety = ntohs(*(uint16_t *)fieldp);
    if (ety == 0x8100 || ety == 0x88A8 || ety == 0x9100) {
        fieldp += 2;

        vlan = ntohs(*(uint16_t *)fieldp);

        if (vid < MV_CUST_VID_NOT_CARE_VALUE)
            vlan = (vlan & 0xf000) | (vid & 0xfff);
        if (pbits < MV_CUST_PBITS_NOT_CARE_VALUE)
            vlan = (vlan & 0x0fff) | ((pbits & 0x7) << 13);

        *(uint16_t *)fieldp = htons(vlan);
        return(1);
    }
    else {
        return(0);
    }

    return(0);
}

static int mv_cust_flow_map_rx(int port, struct net_device *dev, struct sk_buff *skb, struct neta_rx_desc *rx_desc)
{
    uint16_t vlan        = 0;
    uint8_t  pbits       = 0;
    int      btag        = 0;
    int      ret         = 0;
    mv_cust_ioctl_flow_map_t cust_flow;

    if (MV_PON_PORT_ID != port)
        return 0;

    if (mv_cust_flow_map) {

        /* Parse packets to check whether it is tagged or untagged, and get vlan and pbits in tagged mode */
        btag = mv_cust_flow_map_parse(skb->data, &vlan, &pbits, MV_CUST_FLOW_DIR_DS);
        //printk(KERN_ERR " %s TX packet 1 btag[%d] vlan[%d]  pbits[%d]\n", __func__, btag, vlan, pbits);

        /* The frame is tagged */
        if (btag == 1) {
            cust_flow.vid    = vlan;
            cust_flow.pbits  = pbits;
            cust_flow.dir    = MV_CUST_FLOW_DIR_DS;

            ret = mv_cust_tag_map_rule_get(&cust_flow);
            //printk(KERN_ERR " %s TX packet 2 trg_port[%d] trg_queue[%d]  gem_port[%d]\n", __func__, pkt_fwd.trg_port, pkt_fwd.trg_queue, pkt_fwd.gem_port);

            /* Modify VID and P-bits if needed */
            if (ret == MV_CUST_OK) {

                /* modify VID and P-bits if needed */
                ret = mv_cust_flow_map_mod(skb->data, cust_flow.mod_vid, cust_flow.mod_pbits, MV_CUST_FLOW_DIR_DS);
            }
        }
    }

    return 1;
}

int mv_cust_flow_map_tx(int port, struct net_device *dev, struct sk_buff *skb,
                        struct mv_eth_tx_spec *tx_spec_out)
{
    struct iphdr *iphdrp = NULL;
    uint16_t vlan        = 0;
    uint8_t  pbits       = 0;
    int      btag        = 0;
    int      ret         = 0;
    mv_cust_ioctl_flow_map_t cust_flow;

    if (MV_PON_PORT_ID != port)
        return 0;

    if (mv_cust_flow_map) {

        cust_flow.dir  = MV_CUST_FLOW_DIR_US;

        /* Parse packets to check whether it is tagged or untagged, and get vlan and pbits in tagged mode */
        btag = mv_cust_flow_map_parse(skb->data, &vlan, &pbits, MV_CUST_FLOW_DIR_US);

        /* The frame is tagged */
        if (btag == 1) {
            cust_flow.vid    = vlan;
            cust_flow.pbits  = pbits;

            ret = mv_cust_tag_map_rule_get(&cust_flow);
            //printk(KERN_ERR " %s TX packet 2 trg_port[%d] trg_queue[%d]  gem_port[%d]\n", __func__, pkt_fwd.trg_port, pkt_fwd.trg_queue, pkt_fwd.gem_port);

            /* Set GEM port, target port and queue */
            if (ret == MV_CUST_OK) {
                    tx_spec_out->txp     = (u8)cust_flow.pkt_frwd.trg_port;
                    tx_spec_out->txq     = (u8)cust_flow.pkt_frwd.trg_queue;
                    tx_spec_out->hw_cmd  = ((cust_flow.pkt_frwd.gem_port << 8)|0x0010);
                    tx_spec_out->tx_func = NULL;
                    tx_spec_out->flags   = MV_ETH_F_MH;

                    /* modify VID and P-bits if needed */
                    ret = mv_cust_flow_map_mod(skb->data, cust_flow.mod_vid, cust_flow.mod_pbits, MV_CUST_FLOW_DIR_US);
                    return 1;
            }
            else /* Look for rules that does not care VLAN */
            {
                cust_flow.vid   = MV_CUST_DEFAULT_SINGLE_TAG_RULE;
                ret = mv_cust_tag_map_rule_get(&cust_flow);

                /* Set GEM port, target port and queue */
                if (ret == MV_CUST_OK) {
                        tx_spec_out->txp     = (u8)cust_flow.pkt_frwd.trg_port;
                        tx_spec_out->txq     = (u8)cust_flow.pkt_frwd.trg_queue;
                        tx_spec_out->hw_cmd  = ((cust_flow.pkt_frwd.gem_port << 8)|0x0010);
                        tx_spec_out->tx_func = NULL;
                        tx_spec_out->flags   = MV_ETH_F_MH;

                        /* modify VID and P-bits if needed */
                        ret = mv_cust_flow_map_mod(skb->data, cust_flow.mod_vid, cust_flow.mod_pbits, MV_CUST_FLOW_DIR_US);
                        return 1;
                }
            }
        }
        /* The frame is untagged, try to get DSCP value */
        else {
            if (skb->protocol == ETY_IPV4) {
                /* Get UDP Port */
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,22)
                iphdrp = skb->nh.iph;
#else
                iphdrp = ip_hdr(skb);
#endif
                cust_flow.dscp  = (iphdrp->tos >> 2) & 0x3f;
                cust_flow.dir   = MV_CUST_FLOW_DIR_US;

                ret = mv_cust_untag_map_rule_get(&cust_flow);
                /* Set GEM port, target port and queue */
                if (ret == MV_CUST_OK) {
                        tx_spec_out->txp     = cust_flow.pkt_frwd.trg_port;
                        tx_spec_out->txq     = cust_flow.pkt_frwd.trg_queue;
                        tx_spec_out->hw_cmd  = ((cust_flow.pkt_frwd.gem_port << 8)|0x0010);
                        tx_spec_out->tx_func = NULL;
                        tx_spec_out->flags   = MV_ETH_F_MH;;

                        return 1;
                }
            }
            else
            {
                cust_flow.dscp  = 0;
                cust_flow.dir   = MV_CUST_FLOW_DIR_US;

                ret = mv_cust_untag_map_rule_get(&cust_flow);
                /* Set GEM port, target port and queue */
                if (ret == MV_CUST_OK) {
                        tx_spec_out->txp     = cust_flow.pkt_frwd.trg_port;
                        tx_spec_out->txq     = cust_flow.pkt_frwd.trg_queue;
                        tx_spec_out->hw_cmd  = ((cust_flow.pkt_frwd.gem_port << 8)|0x0010);
                        tx_spec_out->tx_func = NULL;
                        tx_spec_out->flags   = MV_ETH_F_MH;;

                        return 1;
                }
            }
        }
    }
    return 0;
}

#endif


void mv_cust_print(int type)
{
    switch (type)
    {
#ifdef CONFIG_MV_CUST_IGMP_HANDLE
        case MV_CUST_APP_TYPE_IGMP:
            mv_cust_igmp_print();
            break;
#endif
#ifdef CONFIG_MV_CUST_MLD_HANDLE
        case MV_CUST_APP_TYPE_MLD:
            mv_cust_mld_print();
            break;
#endif
#ifdef CONFIG_MV_CUST_LPBK_DETECT_HANDLE
        case MV_CUST_APP_TYPE_LPBK:
            mv_cust_loopdet_print();
            break;
#endif
        case MV_CUST_APP_TYPE_OAM:
            mv_cust_eoam_print();
            break;
        case MV_CUST_APP_TYPE_OMCI:
            mv_cust_omci_print();
            break;
        default:
            break;
    }

    return;
}
EXPORT_SYMBOL(mv_cust_print);


void mv_cust_rx_func(int port, int rxq, struct net_device *dev,
                     struct sk_buff *skb, struct neta_rx_desc *rx_desc)
{
    uint32_t i;

    if(mv_cust_debug_code)
        printk("%s\n", __func__);

    if (dev == NULL || skb == NULL || rx_desc == NULL) {
        printk("%s: NULL Pointer dev(%p) skb(%p) rx_desc(%p)\n",
               __func__, dev, skb, rx_desc);
        return;
    }

    if (rx_desc->pncInfo & NETA_PNC_RX_SPECIAL)
    {
        if (gCustConfig[MV_CUST_APP_TYPE_OMCI].enable == MV_CUST_APP_ENABLE) {
            if (mv_cust_omci_rx(port, dev, skb, rx_desc)) {
                if(mv_cust_debug_code)
                    printk("%s omci_packet\n", __func__);
                return;
            }
        }
        else{
            if (gCustConfig[MV_CUST_APP_TYPE_OAM].enable == MV_CUST_APP_ENABLE) {
                if (mv_cust_epon_oam_rx(port, dev, skb, rx_desc)) {
                    if (mv_cust_debug_code)
                        printk("%s eoam_packet\n", __func__);
                    return;
                }
            }
        }

#ifdef CONFIG_MV_CUST_IGMP_HANDLE
        if (gCustConfig[MV_CUST_APP_TYPE_IGMP].enable == MV_CUST_APP_ENABLE) {
            if (mv_cust_igmp_rx(port, dev, skb, rx_desc)) {
                if(mv_cust_debug_code)
                    printk("%s igmp_packet\n", __func__);
                return;
            }
        }
#endif

#ifdef CONFIG_MV_CUST_MLD_HANDLE
        if (gCustConfig[MV_CUST_APP_TYPE_MLD].enable == MV_CUST_APP_ENABLE) {
            if (mv_cust_mld_rx(port, dev, skb, rx_desc)) {
                if(mv_cust_debug_code)
                    printk("%s mld_packet\n", __func__);
                return;
            }
        }
#endif

#ifdef CONFIG_MV_CUST_LPBK_DETECT_HANDLE
        if (gCustConfig[MV_CUST_APP_TYPE_LPBK].enable == MV_CUST_APP_ENABLE) {
            if (mv_cust_loopdet_rx(port, dev, skb, rx_desc)) {
                if(mv_cust_debug_code)
                    printk("%s loop_det_packet\n", __func__);
                return;
            }
        }
#endif

        //MVCUST_ERR_PRINT("Special pkt arrived from port(%d), was not handled. \n", port);
        dev_kfree_skb_any(skb);
        if(mv_cust_debug_code) {
            printk("Input Packet first bytes:\n");
            for (i=0;i<24;i++) {
                if (i%8== 0)
                    printk("\n");
                printk ("%02x ", *(skb->data + i));
            }
        }
        
    }
    else
    {
#ifdef CONFIG_MV_CUST_FLOW_MAP_HANDLE
        if (mv_cust_flow_map) {
            if (mv_cust_flow_map_rx(port, dev, skb, rx_desc)) {
                if(mv_cust_debug_code)
                    printk("%s flow map\n", __func__);
            }
        }
        return;
#endif
    }

    return;
}


int mv_cust_tx_func(int port, struct net_device *dev, struct sk_buff *skb,
                    struct mv_eth_tx_spec *tx_spec_out)
{
    if (mv_cust_omci_tx(port, dev, skb, tx_spec_out))
        return 1;

    if (mv_cust_eoam_tx(port, dev, skb, tx_spec_out))
        return 1;
    
#ifdef CONFIG_MV_CUST_IGMP_HANDLE
    if (mv_cust_igmp_tx(port, dev, skb, tx_spec_out))
        return 1;
#endif

#ifdef CONFIG_MV_CUST_MLD_HANDLE
    if (mv_cust_mld_tx(port, dev, skb, tx_spec_out))
        return 1;
#endif

#ifdef CONFIG_MV_CUST_LPBK_DETECT_HANDLE
    if (mv_cust_loopdet_tx(port, dev, skb, tx_spec_out))
        return 1;
#endif

#ifdef CONFIG_MV_CUST_FLOW_MAP_HANDLE
    if (mv_cust_flow_map_tx(port, dev, skb, tx_spec_out))
        return 1;
#endif

#ifdef CONFIG_MV_CUST_UDP_SAMPLE_HANDLE
    /* Simple example for UDP Source Port or Dest Port Parsing */
    /* Per UDP source or dest port configuration */
    if (mv_cust_udp_port_tx(port, dev, skb, tx_spec_out))
        return 1;
#endif

    return 0;
}


int mvcust_netdev_init(void)
{
    uint32_t port_i;

    /* Retrieve num_ports, as in mv_eth_init_module*/
    mv_eth_ports_num = mvCtrlEthMaxPortGet();

    if (mv_eth_ports_num > CONFIG_MV_ETH_PORTS_NUM)
        mv_eth_ports_num = CONFIG_MV_ETH_PORTS_NUM;

    mv_cust_eoam_init();
    
#ifdef CONFIG_MV_CUST_UDP_SAMPLE_HANDLE
    mv_cust_udp_table_del();
#endif

/* Initialize flow mapping data structure */
#ifdef CONFIG_MV_CUST_FLOW_MAP_HANDLE
    mv_cust_flow_map_init();
#endif

    /* Register special receive check function */
#ifdef CONFIG_MV_ETH_RX_SPECIAL
    for (port_i=0;port_i< mv_eth_ports_num;port_i++) {
        mv_eth_rx_special_proc_func(port_i, mv_cust_rx_func);
    }
#endif /* CONFIG_MV_ETH_RX_SPECIAL */


    /* Register special transmit check function */
#ifdef CONFIG_MV_ETH_TX_SPECIAL
    for (port_i=0;port_i< mv_eth_ports_num;port_i++) {
        mv_eth_tx_special_check_func(port_i, mv_cust_tx_func);
    }
#endif /* CONFIG_MV_ETH_TX_SPECIAL */

    return 0;
}