Update to SDK_2.7.25-RC5

cp -prf ~/ljn/marvel/kernel.rc5/* .
git add -Af .
git commit

Change-Id: I11888b46c3d8af9ebba5a77c64898b67cad06f57
diff --git a/arch/arm/mach-feroceon-kw2/Makefile b/arch/arm/mach-feroceon-kw2/Makefile
index 2368c22..56bc302 100755
--- a/arch/arm/mach-feroceon-kw2/Makefile
+++ b/arch/arm/mach-feroceon-kw2/Makefile
@@ -221,8 +221,8 @@
 #TPM
 obj-$(CONFIG_MV_TPM) += ../plat-feroceon/mv_drivers_lsp/mv_tpm/
 
-#CUST
-obj-$(CONFIG_MV_CUST) += ../plat-feroceon/mv_drivers_lsp/mv_cust/
+#CPH
+obj-$(CONFIG_MV_CPH) += ../plat-feroceon/mv_drivers_lsp/mv_cph/
 
 #MAC_LEARN
 obj-$(CONFIG_MV_MAC_LEARN) += ../plat-feroceon/mv_drivers_lsp/mv_mac_learn/
diff --git a/arch/arm/mach-feroceon-kw2/config/mvRules.mk b/arch/arm/mach-feroceon-kw2/config/mvRules.mk
index 7c44a5d..1efb9b9 100755
--- a/arch/arm/mach-feroceon-kw2/config/mvRules.mk
+++ b/arch/arm/mach-feroceon-kw2/config/mvRules.mk
@@ -63,8 +63,8 @@
 LSP_TPM_PLAT_DIR  = $(PLAT_DRIVERS)/mv_tpm/plat
 endif
 
-ifeq ($(CONFIG_MV_INCLUDE_CUST),y)
-LSP_CUST_CORE_DIR  = $(PLAT_DRIVERS)/mv_cust
+ifeq ($(CONFIG_MV_INCLUDE_CPH),y)
+LSP_CPH_CORE_DIR  = $(PLAT_DRIVERS)/mv_cph
 endif
 
 LSP_EZXML_DIR     = $(PLAT_DRIVERS)/mv_ezxml
diff --git a/arch/arm/mach-feroceon-kw2/include/mach/system.h b/arch/arm/mach-feroceon-kw2/include/mach/system.h
index 6cab463..b426fee 100755
--- a/arch/arm/mach-feroceon-kw2/include/mach/system.h
+++ b/arch/arm/mach-feroceon-kw2/include/mach/system.h
@@ -31,7 +31,7 @@
 	cpu_do_idle();
 }
 
-#define UPON_SDK_VERSION "uPON_2.6.25_RC77"
+#define UPON_SDK_VERSION "uPON_2.7.25_RC5"
 
 #ifdef __BIG_ENDIAN
 #define MV_ARM_32BIT_LE(X) ((((X)&0xff)<<24) |                       \
diff --git a/arch/arm/plat-feroceon/Kconfig b/arch/arm/plat-feroceon/Kconfig
index a99448e..13cd540 100755
--- a/arch/arm/plat-feroceon/Kconfig
+++ b/arch/arm/plat-feroceon/Kconfig
@@ -121,8 +121,8 @@
         ---help---
         Please don't change this configs unless you know what you are doing.
 
-config MV_INCLUDE_CUST
-	bool "Customer driver Support"
+config MV_INCLUDE_CPH
+	bool "CPH Support"
 	depends on MV88F6500
 	default y
         ---help---
@@ -403,7 +403,7 @@
 
 source arch/arm/plat-feroceon/mv_drivers_lsp/mv_tsu/Kconfig
 
-source arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/Kconfig
+source arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/Kconfig
 
 source arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/Kconfig
 
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/Kconfig b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/Kconfig
new file mode 100755
index 0000000..985e839
--- /dev/null
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/Kconfig
@@ -0,0 +1,37 @@
+menu "SoC CPH support"
+depends on MV_INCLUDE_CPH
+
+config  MV_CPH
+        bool "Support for Marvell CPU Packet Handler Driver"
+        default y
+        ---help---
+
+comment "CPH Driver Options"
+depends on MV_CPH
+
+config  MV_CPH_IGMP_HANDLE
+        bool "Enable MV_CPH IGMP handling"
+        default y
+        ---help---
+
+config  MV_CPH_MLD_HANDLE
+        bool "Enable MV_CPH MLD handling"
+        default y
+        ---help---
+
+config  MV_CPH_BC_HANDLE
+        bool "Enable MV_CPH broadcast handling"
+        default y
+        ---help---
+        
+config  MV_CPH_UDP_SAMPLE_HANDLE
+        bool "Enable MV_CPH sample UDP handling"
+        default n
+        ---help---
+
+config  MV_CPH_FLOW_MAP_HANDLE
+        bool "Enable MV_CPH flow mapping handling"
+        default y
+        ---help---        
+
+endmenu
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/Makefile b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/Makefile
new file mode 100755
index 0000000..2d0dcda
--- /dev/null
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/Makefile
@@ -0,0 +1,25 @@
+#
+# Makefile for the Marvell CPU Packet Handler
+#
+
+ifeq ($(CONFIG_ARCH_FEROCEON),y)
+	include $(srctree)/$(MACHINE)/config/mvRules.mk
+endif
+
+ifdef CONFIG_MV_HAL_RULES_PATH
+include $(srctree)/include/config/auto.conf
+include $(srctree)/$(subst ",,$(CONFIG_MV_HAL_RULES_PATH))
+endif
+
+CPH_OBJS += mv_cph_api.o \
+		mv_cph_app.o \
+		mv_cph_db.o \
+		mv_cph_dev.o \
+		mv_cph_flow.o \
+		mv_cph_infra.o \
+		mv_cph_mod.o \
+		mv_cph_netdev.o \
+		mv_cph_sysfs.o
+                                
+mv_cph-objs := $(CPH_OBJS)
+obj-$(CONFIG_MV_CPH) += mv_cph.o
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_api.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_api.c
new file mode 100755
index 0000000..23ff7e1
--- /dev/null
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_api.c
@@ -0,0 +1,542 @@
+/*******************************************************************************
+Copyright (C) Marvell International Ltd. and its affiliates
+
+This software file (the "File") is owned and distributed by Marvell 
+International Ltd. and/or its affiliates ("Marvell") under the following
+alternative licensing terms.  Once you have made an election to distribute the
+File under one of the following license alternatives, please (i) delete this
+introductory statement regarding license alternatives, (ii) delete the two
+license alternatives that you have not elected to use and (iii) preserve the
+Marvell copyright notice above.
+
+********************************************************************************
+Marvell Commercial License Option
+
+If you received this File from Marvell and you have entered into a commercial
+license agreement (a "Commercial License") with Marvell, the File is licensed
+to you under the terms of the applicable Commercial License.
+
+********************************************************************************
+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.
+********************************************************************************
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or 
+modify this File under the following licensing terms. 
+Redistribution and use in source and binary forms, with or without modification, 
+are permitted provided that the following conditions are met:
+
+    *   Redistributions of source code must retain the above copyright notice,
+        this list of conditions and the following disclaimer. 
+
+    *   Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution. 
+
+    *   Neither the name of Marvell nor the names of its contributors may be 
+        used to endorse or promote products derived from this software without 
+        specific prior written permission. 
+    
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+********************************************************************************
+* mv_cph_api.c
+*
+* DESCRIPTION: Marvell CPH(CPH Packet Handler) API definition
+*
+* DEPENDENCIES:
+*               None
+*
+* CREATED BY:   VictorGu
+*
+* DATE CREATED: 22Jan2013
+*
+* FILE REVISION NUMBER:
+*               Revision: 1.0
+*
+*
+*******************************************************************************/
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/delay.h>
+#include <linux/wait.h>
+#include <linux/mutex.h>
+#include <linux/spinlock.h>
+#include <linux/poll.h>
+#include <linux/clk.h>
+#include <linux/fs.h>
+#include <linux/vmalloc.h>
+#include <linux/cdev.h>
+#include <linux/device.h>
+#include <linux/miscdevice.h>
+
+#include "mv_cph_header.h"
+
+
+/******************************************************************************
+ * Variable Definition
+ ******************************************************************************/
+
+
+/******************************************************************************
+ * Function Definition
+ ******************************************************************************/
+/******************************************************************************
+* cph_set_complex_profile()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Set TPM complex profile ID 
+*
+* INPUTS:
+*       profile_id   - TPM complex profile ID
+*       active_port  - Active WAN port
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_set_complex_profile (tpm_eth_complex_profile_t profile_id, MV_APP_GMAC_PORT_E active_port)
+{
+    MV_STATUS rc = MV_OK;
+
+    rc = cph_app_set_complex_profile(profile_id, active_port);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "Fail to call cph_app_set_complex_profile");
+        
+    return rc;
+}
+
+/******************************************************************************
+* cph_set_feature_flag()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Enable or disable feature support in CPH 
+*
+* INPUTS:
+*       feature - CPH supported features
+*       state   - Enable or disable this feature in CPH
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_set_feature_flag (CPH_APP_FEATURE_E feature, BOOL state)
+{
+    MV_STATUS rc = MV_OK;
+
+    rc = cph_app_set_feature_flag(feature, state);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "fail to call cph_app_set_feature_flag");
+       
+    return rc;
+}
+
+/******************************************************************************
+* cph_add_app_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Add CPH rule 
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_add_app_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key,  
+    CPH_APP_MOD_FIELD_E   mod_bm, 
+    CPH_APP_MOD_T        *mod_value, 
+    CPH_APP_FRWD_FIELD_E  frwd_bm, 
+    CPH_APP_FRWD_T       *frwd_value)
+{
+    MV_STATUS rc = MV_OK;
+
+    rc = cph_app_add_rule(parse_bm, parse_key, mod_bm, mod_value, frwd_bm, frwd_value);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "Fail to call cph_app_add_rule");
+        
+    return rc;
+}
+
+/******************************************************************************
+* cph_del_app_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Delete CPH rule 
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_del_app_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key)
+{
+    MV_STATUS rc = MV_OK;
+
+    rc = cph_app_del_rule(parse_bm, parse_key);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "Fail to call cph_app_del_rule");
+        
+    return rc;
+}
+
+/******************************************************************************
+* cph_update_app_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Update CPH rule 
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_update_app_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key,  
+    CPH_APP_MOD_FIELD_E   mod_bm, 
+    CPH_APP_MOD_T        *mod_value, 
+    CPH_APP_FRWD_FIELD_E  frwd_bm, 
+    CPH_APP_FRWD_T       *frwd_value)
+{
+    MV_STATUS rc = MV_OK;
+
+    rc = cph_app_update_rule(parse_bm, parse_key, mod_bm, mod_value, frwd_bm, frwd_value);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "Fail to call cph_app_update_rule");
+        
+    return rc;
+}
+
+/******************************************************************************
+* cph_get_app_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Get CPH rule 
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*
+* OUTPUTS:
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_get_app_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key,  
+    CPH_APP_MOD_FIELD_E  *mod_bm, 
+    CPH_APP_MOD_T        *mod_value, 
+    CPH_APP_FRWD_FIELD_E *frwd_bm, 
+    CPH_APP_FRWD_T       *frwd_value)
+{
+    MV_STATUS rc = MV_OK;
+
+    rc = cph_app_get_rule(parse_bm, parse_key, mod_bm, mod_value, frwd_bm, frwd_value);
+    if (rc != MV_OK)
+        MV_CPH_PRINT(CPH_DEBUG_LEVEL, "fail to call cph_app_get_rule\n");
+        
+    return rc;
+}
+
+/******************************************************************************
+* cph_add_flow_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Sets flow mapping rule
+*
+* INPUTS:
+*       cph_flow - VLAN ID, 802.1p value, pkt_fwd information.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_add_flow_rule(CPH_FLOW_ENTRY_T *cph_flow)
+{
+    MV_STATUS rc = MV_OK;
+
+    rc = cph_flow_add_rule(cph_flow);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "Fail to call cph_flow_add_rule");
+        
+    return rc;
+}
+
+/******************************************************************************
+* cph_del_flow_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Deletes flow mapping rule
+*
+* INPUTS:
+*       cph_flow - VLAN ID, 802.1p value, pkt_fwd information.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_del_flow_rule(CPH_FLOW_ENTRY_T *cph_flow)
+{
+    MV_STATUS rc = MV_OK;
+
+    rc = cph_flow_del_rule(cph_flow);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "Fail to call cph_flow_del_rule");
+        
+    return rc;
+}
+
+/******************************************************************************
+* cph_get_flow_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Gets flow mapping rule for tagged frames.
+*
+* INPUTS:
+*       cph_flow - Input vid, pbits, dir
+*
+* OUTPUTS:
+*       cph_flow - output packet forwarding information, including GEM port, 
+*                   T-CONT, queue and packet modification for VID, P-bits.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_get_flow_rule(CPH_FLOW_ENTRY_T *cph_flow)
+{
+    MV_STATUS rc = MV_OK;
+
+    rc = cph_flow_get_rule(cph_flow);
+    if (rc != MV_OK)
+        MV_CPH_PRINT(CPH_DEBUG_LEVEL, "fail to call cph_flow_get_rule\n");
+          
+    return rc;
+}
+
+/******************************************************************************
+* cph_clear_flow_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Clears all flow mapping rules
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_clear_flow_rule(VOID)
+{
+    MV_STATUS rc = MV_OK;
+
+    rc = cph_flow_clear_rule();
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "Fail to call cph_flow_clear_rule");
+        
+    return rc;
+}
+
+/******************************************************************************
+* cph_clear_flow_rule_by_mh()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Clears flow mapping rules by MH
+*
+* INPUTS:
+*       mh   -  Marvell header.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_clear_flow_rule_by_mh(UINT16 mh)
+{
+    MV_STATUS rc = MV_OK;
+
+    rc = cph_flow_clear_rule_by_mh(mh);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "Fail to call cph_flow_clear_rule_by_mh");
+        
+    return rc;
+}
+
+/******************************************************************************
+* cph_set_flow_dscp_map()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Sets DSCP to P-bits mapping rules
+*
+* INPUTS:
+*       dscp_map  - DSCP to P-bits mapping rules.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_set_flow_dscp_map(CPH_DSCP_PBITS_T *dscp_map)
+{
+    MV_STATUS rc = MV_OK;
+
+    rc = cph_flow_set_dscp_map(dscp_map);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "Fail to call cph_flow_set_dscp_map");
+        
+    return rc;
+}
+
+/******************************************************************************
+* cph_del_flow_dscp_map()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Deletes DSCP to P-bits mapping rules
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_del_flow_dscp_map(VOID)
+{
+    MV_STATUS rc = MV_OK;
+
+    rc = cph_flow_del_dscp_map();
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "Fail to call cph_flow_del_dscp_map");
+        
+    return rc;
+}
+
+/*******************************************************************************
+**
+** cph_get_tcont_state
+** ___________________________________________________________________________
+**
+** DESCRIPTION: The function get T-CONT state
+**
+** INPUTS:
+**   tcont - T-CONT
+**
+** OUTPUTS:
+**   None. 
+**
+** RETURNS:     
+**   state - State of T-CONT, enabled or disabled.  
+**
+*******************************************************************************/
+bool cph_get_tcont_state(unsigned int tcont)
+{
+    return cph_db_get_tcont_state(tcont);
+}
+
+/*******************************************************************************
+**
+** cph_set_tcont_state
+** ___________________________________________________________________________
+**
+** DESCRIPTION: The function sets T-CONT state in mv_cust
+**
+** INPUTS:
+**   tcont - T-CONT
+**   state - State of T-CONT, enabled or disabled.
+**
+** OUTPUTS:
+**   None. 
+**
+** RETURNS:     
+**  On success, the function returns (MV_OK). On error different types are 
+**  returned according to the case.  
+**
+*******************************************************************************/
+int cph_set_tcont_state(unsigned int tcont, bool state)
+{
+    return cph_db_set_tcont_state(tcont, state);
+}
+
+EXPORT_SYMBOL(cph_set_complex_profile);
+EXPORT_SYMBOL(cph_set_feature_flag);
+EXPORT_SYMBOL(cph_add_app_rule);
+EXPORT_SYMBOL(cph_del_app_rule);
+EXPORT_SYMBOL(cph_update_app_rule);
+EXPORT_SYMBOL(cph_get_app_rule);
+EXPORT_SYMBOL(cph_add_flow_rule);
+EXPORT_SYMBOL(cph_del_flow_rule);
+EXPORT_SYMBOL(cph_get_flow_rule);
+EXPORT_SYMBOL(cph_clear_flow_rule);
+EXPORT_SYMBOL(cph_set_flow_dscp_map);
+EXPORT_SYMBOL(cph_del_flow_dscp_map);
+EXPORT_SYMBOL(cph_get_tcont_state);
+EXPORT_SYMBOL(cph_set_tcont_state);
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_api.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_api.h
new file mode 100755
index 0000000..dfdb8a2
--- /dev/null
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_api.h
@@ -0,0 +1,411 @@
+/*******************************************************************************
+Copyright (C) Marvell International Ltd. and its affiliates
+
+This software file (the "File") is owned and distributed by Marvell 
+International Ltd. and/or its affiliates ("Marvell") under the following
+alternative licensing terms.  Once you have made an election to distribute the
+File under one of the following license alternatives, please (i) delete this
+introductory statement regarding license alternatives, (ii) delete the two
+license alternatives that you have not elected to use and (iii) preserve the
+Marvell copyright notice above.
+
+********************************************************************************
+Marvell Commercial License Option
+
+If you received this File from Marvell and you have entered into a commercial
+license agreement (a "Commercial License") with Marvell, the File is licensed
+to you under the terms of the applicable Commercial License.
+
+********************************************************************************
+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.
+********************************************************************************
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or 
+modify this File under the following licensing terms. 
+Redistribution and use in source and binary forms, with or without modification, 
+are permitted provided that the following conditions are met:
+
+    *   Redistributions of source code must retain the above copyright notice,
+        this list of conditions and the following disclaimer. 
+
+    *   Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution. 
+
+    *   Neither the name of Marvell nor the names of its contributors may be 
+        used to endorse or promote products derived from this software without 
+        specific prior written permission. 
+    
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+********************************************************************************
+* mv_cph_api.h
+*
+* DESCRIPTION: Marvell CPH(CPH Packet Handler) API definition
+*
+* DEPENDENCIES:
+*               None
+*
+* CREATED BY:   VictorGu
+*
+* DATE CREATED: 22Jan2013
+*
+* FILE REVISION NUMBER:
+*               Revision: 1.0
+*
+*
+*******************************************************************************/
+#ifndef _MV_CPH_API_H_
+#define _MV_CPH_API_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/******************************************************************************
+ * Type Definition
+ ******************************************************************************/
+
+
+
+/******************************************************************************
+ * Function Declaration
+ ******************************************************************************/
+/******************************************************************************
+* cph_set_complex_profile()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Set TPM complex profile ID 
+*
+* INPUTS:
+*       profile_id   - TPM complex profile ID
+*       active_port  - Active WAN port
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_set_complex_profile (tpm_eth_complex_profile_t profile_id, MV_APP_GMAC_PORT_E active_port);
+
+/******************************************************************************
+* cph_set_feature_flag()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Enable or disable feature support in CPH 
+*
+* INPUTS:
+*       feature - CPH supported features
+*       state   - Enable or disable this feature in CPH
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_set_feature_flag (CPH_APP_FEATURE_E feature, BOOL state);
+
+/******************************************************************************
+* cph_add_app_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Add CPH rule 
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_add_app_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key,  
+    CPH_APP_MOD_FIELD_E   mod_bm, 
+    CPH_APP_MOD_T        *mod_value, 
+    CPH_APP_FRWD_FIELD_E  frwd_bm, 
+    CPH_APP_FRWD_T       *frwd_value);
+
+/******************************************************************************
+* cph_del_app_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Delete CPH rule 
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_del_app_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key);
+
+/******************************************************************************
+* cph_update_app_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Update CPH rule 
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_update_app_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key,  
+    CPH_APP_MOD_FIELD_E   mod_bm, 
+    CPH_APP_MOD_T        *mod_value, 
+    CPH_APP_FRWD_FIELD_E  frwd_bm, 
+    CPH_APP_FRWD_T       *frwd_value);
+
+/******************************************************************************
+* cph_get_app_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Get CPH rule 
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*
+* OUTPUTS:
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_get_app_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key,  
+    CPH_APP_MOD_FIELD_E  *mod_bm, 
+    CPH_APP_MOD_T        *mod_value, 
+    CPH_APP_FRWD_FIELD_E *frwd_bm, 
+    CPH_APP_FRWD_T       *frwd_value);
+
+/******************************************************************************
+* cph_add_flow_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Sets flow mapping rule
+*
+* INPUTS:
+*       cph_flow - VLAN ID, 802.1p value, pkt_fwd information.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_add_flow_rule(CPH_FLOW_ENTRY_T *cph_flow);
+
+/******************************************************************************
+* cph_del_flow_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Deletes flow mapping rule
+*
+* INPUTS:
+*       cph_flow - VLAN ID, 802.1p value, pkt_fwd information.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_del_flow_rule(CPH_FLOW_ENTRY_T *cph_flow);
+
+/******************************************************************************
+* cph_get_flow_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Gets flow mapping rule for tagged frames.
+*
+* INPUTS:
+*       cph_flow - Input vid, pbits, dir
+*
+* OUTPUTS:
+*       cph_flow - output packet forwarding information, including GEM port, 
+*                   T-CONT, queue and packet modification for VID, P-bits.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_get_flow_rule(CPH_FLOW_ENTRY_T *cph_flow);
+
+/******************************************************************************
+* cph_clear_flow_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Clears all flow mapping rules
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_clear_flow_rule(VOID);
+
+/******************************************************************************
+* cph_clear_flow_rule_by_mh()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Clears flow mapping rules by MH
+*
+* INPUTS:
+*       mh   -  Marvell header.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_clear_flow_rule_by_mh(UINT16 mh);
+
+/******************************************************************************
+* cph_set_flow_dscp_map()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Sets DSCP to P-bits mapping rules
+*
+* INPUTS:
+*       dscp_map  - DSCP to P-bits mapping rules.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_set_flow_dscp_map(CPH_DSCP_PBITS_T *dscp_map);
+
+/******************************************************************************
+* cph_del_flow_dscp_map()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Deletes DSCP to P-bits mapping rules
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_del_flow_dscp_map(VOID);
+
+/*******************************************************************************
+**
+** cph_get_tcont_state
+** ___________________________________________________________________________
+**
+** DESCRIPTION: The function get T-CONT state
+**
+** INPUTS:
+**   tcont - T-CONT
+**
+** OUTPUTS:
+**   None. 
+**
+** RETURNS:     
+**   state - State of T-CONT, enabled or disabled.  
+**
+*******************************************************************************/
+bool cph_get_tcont_state(unsigned int tcont);
+
+/*******************************************************************************
+**
+** cph_set_tcont_state
+** ___________________________________________________________________________
+**
+** DESCRIPTION: The function sets T-CONT state in mv_cust
+**
+** INPUTS:
+**   tcont - T-CONT
+**   state - State of T-CONT, enabled or disabled.
+**
+** OUTPUTS:
+**   None. 
+**
+** RETURNS:     
+**  On success, the function returns (MV_OK). On error different types are 
+**  returned according to the case.  
+**
+*******************************************************************************/
+int cph_set_tcont_state(unsigned int tcont, bool state);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _MV_CPH_API_H_ */
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_app.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_app.c
new file mode 100755
index 0000000..8c77fd6
--- /dev/null
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_app.c
@@ -0,0 +1,1484 @@
+/*******************************************************************************
+Copyright (C) Marvell International Ltd. and its affiliates
+
+This software file (the "File") is owned and distributed by Marvell 
+International Ltd. and/or its affiliates ("Marvell") under the following
+alternative licensing terms.  Once you have made an election to distribute the
+File under one of the following license alternatives, please (i) delete this
+introductory statement regarding license alternatives, (ii) delete the two
+license alternatives that you have not elected to use and (iii) preserve the
+Marvell copyright notice above.
+
+********************************************************************************
+Marvell Commercial License Option
+
+If you received this File from Marvell and you have entered into a commercial
+license agreement (a "Commercial License") with Marvell, the File is licensed
+to you under the terms of the applicable Commercial License.
+
+********************************************************************************
+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.
+********************************************************************************
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or 
+modify this File under the following licensing terms. 
+Redistribution and use in source and binary forms, with or without modification, 
+are permitted provided that the following conditions are met:
+
+    *   Redistributions of source code must retain the above copyright notice,
+        this list of conditions and the following disclaimer. 
+
+    *   Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution. 
+
+    *   Neither the name of Marvell nor the names of its contributors may be 
+        used to endorse or promote products derived from this software without 
+        specific prior written permission. 
+    
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+********************************************************************************
+* mv_cph_app.c
+*
+* DESCRIPTION: Marvell CPH(CPH Packet Handler) application module to implement
+*              CPH main logic and handle application packets such as OMCI, eOAM, 
+*              IGMP packets.
+*
+* DEPENDENCIES:
+*               None
+*
+* CREATED BY:   VictorGu
+*
+* DATE CREATED: 22Jan2013
+*
+* FILE REVISION NUMBER:
+*               Revision: 1.0
+*
+*
+*******************************************************************************/
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/delay.h>
+#include <linux/wait.h>
+#include <linux/mutex.h>
+#include <linux/spinlock.h>
+#include <linux/poll.h>
+#include <linux/clk.h>
+#include <linux/fs.h>
+#include <linux/vmalloc.h>
+#include <linux/cdev.h>
+#include <linux/device.h>
+#include <linux/miscdevice.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>
+
+#include "mv_cph_header.h"
+
+
+/******************************************************************************
+ * Variable Definition
+ ******************************************************************************/
+/* CPH global trace flag */
+UINT32 g_cph_global_trace = CPH_ERR_LEVEL|CPH_WARN_LEVEL|CPH_INFO_LEVEL;
+
+MV_ENUM_ENTRY_T g_enum_map_profile_id[] =
+{
+    { TPM_PON_WAN_DUAL_MAC_INT_SWITCH,  "TPM_PON_WAN_DUAL_MAC_INT_SWITCH"},
+    { TPM_PON_WAN_G0_INT_SWITCH,        "TPM_PON_WAN_G0_INT_SWITCH"},
+    { TPM_PON_WAN_G1_LAN_G0_INT_SWITCH, "TPM_PON_WAN_G1_LAN_G0_INT_SWITCH"},
+    { TPM_G0_WAN_G1_INT_SWITCH,         "TPM_G0_WAN_G1_INT_SWITCH"},
+    { TPM_G1_WAN_G0_INT_SWITCH,         "TPM_G1_WAN_G0_INT_SWITCH"},
+    { TPM_PON_G1_WAN_G0_INT_SWITCH,     "TPM_PON_G1_WAN_G0_INT_SWITCH"},
+    { TPM_PON_G0_WAN_G1_INT_SWITCH,     "TPM_PON_G0_WAN_G1_INT_SWITCH"},
+    { TPM_PON_WAN_DUAL_MAC_EXT_SWITCH,  "TPM_PON_WAN_DUAL_MAC_EXT_SWITCH"},
+    { TPM_PON_WAN_G1_MNG_EXT_SWITCH,    "TPM_PON_WAN_G1_MNG_EXT_SWITCH"},
+    { TPM_PON_WAN_G0_SINGLE_PORT,       "TPM_PON_WAN_G0_SINGLE_PORT"},
+    { TPM_PON_WAN_G1_SINGLE_PORT,       "TPM_PON_WAN_G1_SINGLE_PORT"},
+    { TPM_PON_G1_WAN_G0_SINGLE_PORT,    "TPM_PON_G1_WAN_G0_SINGLE_PORT"},
+    { TPM_PON_G0_WAN_G1_SINGLE_PORT,    "TPM_PON_G0_WAN_G1_SINGLE_PORTg"},
+    { TPM_PON_WAN_G0_G1_LPBK,           "TPM_PON_WAN_G0_G1_LPBK"},
+    { TPM_PON_WAN_G0_G1_DUAL_LAN,       "TPM_PON_WAN_G0_G1_DUAL_LAN"},     
+};
+
+static MV_ENUM_ARRAY_T g_enum_array_profile_id =
+{
+    sizeof(g_enum_map_profile_id)/sizeof(g_enum_map_profile_id[0]),
+    g_enum_map_profile_id
+};
+
+static MV_ENUM_ENTRY_T g_enum_map_pon_type[] =
+{
+    { CPH_PON_TYPE_EPON, "EPON"},
+    { CPH_PON_TYPE_GPON, "GPON"},
+    { CPH_PON_TYPE_GBE,  "GBE"},
+    { CPH_PON_TYPE_P2P,  "P2P"},
+};
+
+static MV_ENUM_ARRAY_T g_enum_array_pon_type =
+{
+    sizeof(g_enum_map_pon_type)/sizeof(g_enum_map_pon_type[0]),
+    g_enum_map_pon_type
+};
+
+static MV_ENUM_ENTRY_T g_enum_map_dir[] =
+{
+    { CPH_DIR_US,       "US"},
+    { CPH_DIR_DS,       "DS"},
+    { CPH_DIR_NOT_CARE, "Not Care"},
+};
+
+static MV_ENUM_ARRAY_T g_enum_array_dir =
+{
+    sizeof(g_enum_map_dir)/sizeof(g_enum_map_dir[0]),
+    g_enum_map_dir
+};
+
+static MV_ENUM_ENTRY_T g_enum_map_rx_tx[] =
+{
+    { CPH_DIR_RX,         "RX"},
+    { CPH_DIR_TX,         "TX"},
+    { CPH_RX_TX_NOT_CARE, "Not Care"},
+};
+
+static MV_ENUM_ARRAY_T g_enum_array_rx_tx =
+{
+    sizeof(g_enum_map_rx_tx)/sizeof(g_enum_map_rx_tx[0]),
+    g_enum_map_rx_tx
+};
+
+static MV_ENUM_ENTRY_T g_enum_map_gmac[] =
+{
+    { MV_APP_GMAC_PORT_0,  "GMAC0"},
+    { MV_APP_GMAC_PORT_1,  "GMAC1"},
+    { MV_APP_PON_MAC_PORT, "PON MAC"},
+};
+
+static MV_ENUM_ARRAY_T g_enum_array_gmac =
+{
+    sizeof(g_enum_map_gmac)/sizeof(g_enum_map_gmac[0]),
+    g_enum_map_gmac
+};
+
+
+/******************************************************************************
+ * External Declaration
+ ******************************************************************************/
+extern CHAR *cph_lookup_enum_str(MV_ENUM_ARRAY_T *p_enum_array, INT32 enum_value);
+
+
+/******************************************************************************
+ * Function Definition
+ ******************************************************************************/
+/******************************************************************************
+* cph_app_set_complex_profile()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Set TPM complex profile ID 
+*
+* INPUTS:
+*       profile_id   - TPM complex profile ID
+*       active_port  - Active WAN port
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_set_complex_profile (tpm_eth_complex_profile_t profile_id, MV_APP_GMAC_PORT_E active_port)
+{
+    MV_STATUS rc = MV_OK;
+
+    /* Check the range of profile_id */
+    if (profile_id > TPM_PON_WAN_G0_G1_DUAL_LAN)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "profile_id[%d] is out of range[1~%d] \n", profile_id, TPM_PON_WAN_G0_G1_DUAL_LAN);
+        return MV_OUT_OF_RANGE;
+    }
+
+    /* Check the range of active_port */
+    if (active_port > MV_APP_PON_MAC_PORT)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "active_port[%d] is out of range[0~%d] \n", active_port, MV_APP_PON_MAC_PORT);
+        return MV_OUT_OF_RANGE;
+    }
+
+    rc = cph_db_set_param(CPH_DB_PARAM_PROFILE_ID, &profile_id);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "fail to call cph_db_set_param");
+
+    rc = cph_db_set_param(CPH_DB_PARAM_ACTIVE_PORT, &active_port);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "fail to call cph_db_set_param");
+   
+    return rc;    
+}
+
+/******************************************************************************
+* cph_app_set_feature_flag()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Enable or disable feature support in CPH 
+*
+* INPUTS:
+*       feature - CPH supported features
+*       state   - Enable or disable this feature in CPH
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_set_feature_flag (CPH_APP_FEATURE_E feature, BOOL state)
+{
+    MV_STATUS rc = MV_OK;
+
+    switch (feature)
+    {
+        case CPH_APP_FEATURE_APP:
+            cph_db_set_param(CPH_DB_PARAM_APP_SUPPORT, &state);
+            break;           
+        case CPH_APP_FEATURE_IGMP:
+            cph_db_set_param(CPH_DB_PARAM_IGMP_SUPPORT,&state);
+            break;
+        case CPH_APP_FEATURE_BC:
+            cph_db_set_param(CPH_DB_PARAM_BC_SUPPORT,  &state);
+            break;
+        case CPH_APP_FEATURE_FLOW:
+            cph_db_set_param(CPH_DB_PARAM_FLOW_SUPPORT,&state);
+            break;
+        case CPH_APP_FEATURE_UDP:
+            cph_db_set_param(CPH_DB_PARAM_UDP_SUPPORT, &state);
+            break;
+        default:
+            break;
+    }
+    return rc;    
+}
+
+/******************************************************************************
+* cph_app_validate_parse_field()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Validate the parsing filed of CPH rule
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_validate_parse_field (
+    CPH_APP_PARSE_FIELD_E parse_bm,
+    CPH_APP_PARSE_T      *parse_key)
+{
+    MV_STATUS rc = MV_OK;
+
+    /* Check the range of parse_bm */
+    if (parse_bm >= CPH_APP_PARSE_FIELD_END)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "parse_bm[0x%x] is out of range[0x01~0x%x] \n", parse_bm, CPH_APP_PARSE_FIELD_END);
+        return MV_OUT_OF_RANGE;
+    }
+
+    /* Validate direction */
+    if (parse_bm & CPH_APP_PARSE_FIELD_DIR)
+    {
+        if (parse_key->dir > CPH_DIR_NOT_CARE)
+        {
+            MV_CPH_PRINT(CPH_ERR_LEVEL, "dir[%d] is out of range[0~%d] \n", parse_key->dir, CPH_DIR_NOT_CARE);
+            return MV_OUT_OF_RANGE;
+        }
+    }
+
+    /* Validate RX/TX direction */
+    if (parse_bm & CPH_APP_PARSE_FIELD_RX_TX)
+    {
+        if (parse_key->rx_tx > CPH_RX_TX_NOT_CARE)
+        {
+            MV_CPH_PRINT(CPH_ERR_LEVEL, "rx_tx[%d] is out of range[0~%d] \n", parse_key->rx_tx, CPH_RX_TX_NOT_CARE);
+            return MV_OUT_OF_RANGE;
+        }
+    }    
+
+    /* Could not parse None IPv4 Eth type and IPv4 protocol type at the same ime */
+    if ((parse_bm & CPH_APP_PARSE_FIELD_ETH_TYPE) &&
+        (parse_bm & CPH_APP_PARSE_FIELD_IPV4_TYPE) &&
+        (parse_key->eth_type != MV_CPH_ETH_TYPE_IPV4))
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "parse_bm[0x%x], eth_type[0x%x], does not support parsing None IPv4 Eth type and IPv4 protocol type at the same time\n",
+                     parse_bm, parse_key->eth_type);
+        return MV_BAD_VALUE;
+    }
+
+    /* Could not parse None IPv6 Eth type and IPv4 protocol type at the same ime */
+    if ((parse_bm & CPH_APP_PARSE_FIELD_ETH_TYPE) &&
+        ((parse_bm & CPH_APP_PARSE_FIELD_IPV6_NH1) ||
+         (parse_bm & CPH_APP_PARSE_FIELD_IPV6_NH2) || 
+         (parse_bm & CPH_APP_PARSE_FIELD_ICMPV6_TYPE)) &&
+        (parse_key->eth_type != MV_CPH_ETH_TYPE_IPV6))
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "parse_bm[0x%x], eth_type[0x%x], does not support parsing None IPv6 Eth type and IPv6 NH or ICMP type at the same time\n",
+                     parse_bm, parse_key->eth_type);
+        return MV_BAD_VALUE;
+    }    
+
+    /* Could not parse Eth subtype and IPv4 the same time */
+    if ((parse_bm & CPH_APP_PARSE_FIELD_ETH_SUBTYPE) &&
+        (parse_bm & CPH_APP_PARSE_FIELD_IPV4_TYPE))
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "parse_bm[0x%x], does not support parsing Eth subtype and IPv4 type at the same time \n", parse_bm);
+        return MV_BAD_VALUE;
+    }
+
+    /* Could not parse Eth subtype and IPv6 the same time */
+    if ((parse_bm & CPH_APP_PARSE_FIELD_ETH_SUBTYPE) &&
+        ((parse_bm & CPH_APP_PARSE_FIELD_IPV6_NH1) ||
+         (parse_bm & CPH_APP_PARSE_FIELD_IPV6_NH2) || 
+         (parse_bm & CPH_APP_PARSE_FIELD_ICMPV6_TYPE)))
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "parse_bm[0x%x], does not support parsing Eth subtype and IPv6 type at the same time \n", parse_bm);
+        return MV_BAD_VALUE;
+    }    
+
+    /* Could not parse IPv4 and IPv6 at the same time */
+    if ((parse_bm & CPH_APP_PARSE_FIELD_IPV4_TYPE) &&
+        ((parse_bm & CPH_APP_PARSE_FIELD_IPV6_NH1) ||
+         (parse_bm & CPH_APP_PARSE_FIELD_IPV6_NH2) || 
+         (parse_bm & CPH_APP_PARSE_FIELD_ICMPV6_TYPE)))
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "parse_bm[0x%x], does not support parsing IPv4 and IPv6 type at the same time\n", parse_bm);
+        return MV_BAD_VALUE;
+    }
+
+    /* Validate IGMPv6 type */
+    if ((parse_bm & CPH_APP_PARSE_FIELD_ICMPV6_TYPE) &&
+        (parse_key->icmpv6_type != MV_ICMPV6_TYPE_MLD))
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "parse_bm[0x%x], icmpv6_type[%d], currently only support ICMPv6 MLD type[%d]\n",
+                     parse_bm, parse_key->icmpv6_type, MV_ICMPV6_TYPE_MLD);
+        return MV_BAD_VALUE;
+    }    
+    return rc;
+}
+
+/******************************************************************************
+* cph_app_validate_mod_field()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Validate the modification filed of CPH rule
+*
+* INPUTS:
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_validate_mod_field (
+    CPH_APP_MOD_FIELD_E   mod_bm,
+    CPH_APP_MOD_T        *mod_value)
+{
+    MV_STATUS rc = MV_OK;
+
+    /* Check the range of mod_bm */
+    if (mod_bm >= CPH_APP_MOD_FIELD_END)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "mod_bm[0x%x] is out of range[0x01~0x%x] \n", mod_bm, CPH_APP_MOD_FIELD_END);
+        return MV_OUT_OF_RANGE;
+    }
+
+    /* Does not support adding GMAC information and strip MH at the same time */
+    if ((mod_bm & CPH_APP_RX_MOD_ADD_GMAC) &&
+        (mod_bm & CPH_APP_RX_MOD_STRIP_MH))
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "mod_bm[0x%x], does not support adding GMAC information and stripping MH at the same time \n", mod_bm);
+        return MV_BAD_VALUE;
+    }
+
+    return rc;
+}
+
+/******************************************************************************
+* cph_app_validate_frwd_field()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Validate the forwarding filed of CPH rule
+*
+* INPUTS:
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_validate_frwd_field (
+    CPH_APP_FRWD_FIELD_E  frwd_bm, 
+    CPH_APP_FRWD_T       *frwd_value)
+{
+    MV_STATUS rc = MV_OK;
+
+    /* Check the range of frwd_bm */
+    if (frwd_bm >= CPH_APP_FRWD_FIELD_END)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "frwd_bm[0x%x] is out of range[0x01~0x%x] \n", frwd_bm, CPH_APP_FRWD_FIELD_END);
+        return MV_OUT_OF_RANGE;
+    }
+
+    return rc;
+}
+
+/******************************************************************************
+* cph_app_add_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Add CPH rule 
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_add_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key,  
+    CPH_APP_MOD_FIELD_E   mod_bm, 
+    CPH_APP_MOD_T        *mod_value, 
+    CPH_APP_FRWD_FIELD_E  frwd_bm, 
+    CPH_APP_FRWD_T       *frwd_value)
+{
+    MV_STATUS rc = MV_OK;
+
+    rc = cph_app_validate_parse_field(parse_bm, parse_key);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "fail to valid parsing field");
+    
+    rc = cph_app_validate_mod_field(mod_bm, mod_value);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "fail to valid modification field");
+
+    rc = cph_app_validate_frwd_field(frwd_bm, frwd_value);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "fail to valid forwarding field");
+
+    rc = cph_db_add_app_rule(parse_bm, parse_key, mod_bm, mod_value, frwd_bm, frwd_value);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "fail to call cph_db_add_app_rule");
+
+    return rc;
+}
+
+/******************************************************************************
+* cph_app_del_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Delete CPH rule 
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_del_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key)
+{
+    MV_STATUS rc = MV_OK;
+
+    rc = cph_app_validate_parse_field(parse_bm, parse_key);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "fail to valid parsing field");
+    
+    rc = cph_db_del_app_rule(parse_bm, parse_key);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "fail to call cph_db_del_app_rule");
+
+    return rc;
+}
+
+/******************************************************************************
+* cph_app_update_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Update CPH rule 
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_update_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key,  
+    CPH_APP_MOD_FIELD_E   mod_bm, 
+    CPH_APP_MOD_T        *mod_value, 
+    CPH_APP_FRWD_FIELD_E  frwd_bm, 
+    CPH_APP_FRWD_T       *frwd_value)
+{
+    MV_STATUS rc = MV_OK;
+
+    rc = cph_app_validate_parse_field(parse_bm, parse_key);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "fail to valid parsing field");
+    
+    rc = cph_app_validate_mod_field(mod_bm, mod_value);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "fail to valid modification field");
+
+    rc = cph_app_validate_frwd_field(frwd_bm, frwd_value);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "fail to valid forwarding field");
+
+    rc = cph_db_update_app_rule(parse_bm, parse_key, mod_bm, mod_value, frwd_bm, frwd_value);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "fail to call cph_db_update_app_rule");
+
+    return rc;
+}
+
+/******************************************************************************
+* cph_app_get_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Get CPH rule 
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*
+* OUTPUTS:
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_get_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key,  
+    CPH_APP_MOD_FIELD_E  *mod_bm, 
+    CPH_APP_MOD_T        *mod_value, 
+    CPH_APP_FRWD_FIELD_E *frwd_bm, 
+    CPH_APP_FRWD_T       *frwd_value)
+{
+    MV_STATUS rc = MV_OK;
+
+    rc = cph_app_validate_parse_field(parse_bm, parse_key);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "fail to valid parsing field");
+    
+    rc = cph_db_get_app_rule(parse_bm, parse_key, mod_bm, mod_value, frwd_bm, frwd_value);
+    if (rc != MV_OK)
+        MV_CPH_PRINT(CPH_DEBUG_LEVEL, "fail to call cph_app_get_rule\n");
+
+    return rc;
+}
+
+/******************************************************************************
+* cph_app_get_rule_by_dir_proto()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Get CPH rule according to protocol type
+*
+* INPUTS:
+*       dir        - Direction
+*       proto_type - SKB protocol type
+*
+* OUTPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_get_rule_by_dir_proto (
+    CPH_DIR_E              dir,
+    UINT16                 proto_type,
+    CPH_APP_PARSE_FIELD_E *parse_bm,
+    CPH_APP_PARSE_T       *parse_key,
+    CPH_APP_MOD_FIELD_E   *mod_bm,
+    CPH_APP_MOD_T         *mod_value,
+    CPH_APP_FRWD_FIELD_E  *frwd_bm,
+    CPH_APP_FRWD_T        *frwd_value)
+{
+    MV_STATUS rc = MV_OK;
+
+    rc = cph_db_get_app_rule_by_dir_proto(dir, proto_type, parse_bm, parse_key, mod_bm, mod_value, frwd_bm, frwd_value);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "fail to call cph_db_get_app_rule_by_dir_proto");
+
+    return rc;
+}
+
+/******************************************************************************
+* cph_app_increase_counter()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Increase RX counter
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_increase_counter (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key)
+{
+    MV_STATUS rc = MV_OK;
+
+    rc = cph_app_validate_parse_field(parse_bm, parse_key);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "fail to valid parsing field");
+    
+    rc = cph_db_increase_counter(parse_bm, parse_key);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "fail to call cph_db_increase_counter");
+
+    return rc;
+}
+
+/******************************************************************************
+* cph_app_increase_counter_by_dir_proto()
+* _____________________________________________________________________________
+*
+* DESCRIPTION:  Increase RX counter according to protocol type
+*
+* INPUTS:
+*       dir        - Direction
+*       proto_type - SKB protocol type
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_increase_counter_by_dir_proto (
+    CPH_DIR_E dir,
+    UINT16    proto_type)
+{
+    MV_STATUS rc = MV_OK;
+
+    rc = cph_db_increase_counter_by_dir_proto(dir, proto_type);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "fail to call cph_db_increase_counter_by_dir_proto");
+
+    return rc;
+}
+
+/******************************************************************************
+* cph_app_parse_ge_port_type()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Get GEMAC port type by profile ID
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       port_type   - Modification bitmap
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_parse_ge_port_type (
+    CPH_PORT_STATE_T          *port_type)
+{
+    tpm_eth_complex_profile_t  profile_id  = 0;
+    MV_APP_GMAC_PORT_E         active_port = 0;
+    MV_STATUS                  rc          = MV_OK;
+
+    /* Get Profile ID and active WAN port */
+    cph_db_get_param(CPH_DB_PARAM_PROFILE_ID,  &profile_id);
+    cph_db_get_param(CPH_DB_PARAM_ACTIVE_PORT, &active_port);
+
+    switch (profile_id)
+    {
+        case TPM_PON_WAN_DUAL_MAC_INT_SWITCH:
+        case TPM_PON_WAN_G1_LAN_G0_INT_SWITCH:            
+        case TPM_PON_WAN_DUAL_MAC_EXT_SWITCH:            
+        case TPM_PON_WAN_G0_G1_DUAL_LAN:            
+            port_type[MV_APP_GMAC_PORT_0].port_type   = MV_APP_PORT_LAN;
+            port_type[MV_APP_GMAC_PORT_0].port_state  = MV_GE_PORT_ACTIVE;
+            port_type[MV_APP_GMAC_PORT_1].port_type   = MV_APP_PORT_LAN;
+            port_type[MV_APP_GMAC_PORT_1].port_state  = MV_GE_PORT_ACTIVE;
+            port_type[MV_APP_PON_MAC_PORT].port_type  = MV_APP_PORT_WAN;
+            port_type[MV_APP_PON_MAC_PORT].port_state = MV_GE_PORT_ACTIVE;
+            break;
+        case TPM_PON_WAN_G0_INT_SWITCH:
+        case TPM_PON_WAN_G0_SINGLE_PORT:            
+        case TPM_PON_WAN_G0_G1_LPBK:            
+            port_type[MV_APP_GMAC_PORT_0].port_type   = MV_APP_PORT_LAN;
+            port_type[MV_APP_GMAC_PORT_0].port_state  = MV_GE_PORT_ACTIVE;
+            port_type[MV_APP_GMAC_PORT_1].port_state  = MV_GE_PORT_INVALID;            
+            port_type[MV_APP_PON_MAC_PORT].port_type  = MV_APP_PORT_WAN;
+            port_type[MV_APP_PON_MAC_PORT].port_state = MV_GE_PORT_ACTIVE;
+            break;
+        case TPM_G0_WAN_G1_INT_SWITCH:
+            port_type[MV_APP_GMAC_PORT_0].port_type   = MV_APP_PORT_WAN;
+            port_type[MV_APP_GMAC_PORT_0].port_state  = MV_GE_PORT_ACTIVE;
+            port_type[MV_APP_GMAC_PORT_1].port_type   = MV_APP_PORT_LAN;
+            port_type[MV_APP_GMAC_PORT_1].port_state  = MV_GE_PORT_ACTIVE;
+            port_type[MV_APP_PON_MAC_PORT].port_state = MV_GE_PORT_INVALID;
+            break;
+        case TPM_G1_WAN_G0_INT_SWITCH:
+            port_type[MV_APP_GMAC_PORT_0].port_type   = MV_APP_PORT_LAN;
+            port_type[MV_APP_GMAC_PORT_0].port_state  = MV_GE_PORT_ACTIVE;
+            port_type[MV_APP_GMAC_PORT_1].port_type   = MV_APP_PORT_WAN;
+            port_type[MV_APP_GMAC_PORT_1].port_state  = MV_GE_PORT_ACTIVE;
+            port_type[MV_APP_PON_MAC_PORT].port_state = MV_GE_PORT_INVALID;
+            break;
+        case TPM_PON_G1_WAN_G0_INT_SWITCH:
+        case TPM_PON_G1_WAN_G0_SINGLE_PORT:            
+            port_type[MV_APP_GMAC_PORT_0].port_type   = MV_APP_PORT_LAN;
+            port_type[MV_APP_GMAC_PORT_0].port_state  = MV_GE_PORT_ACTIVE;
+            port_type[MV_APP_GMAC_PORT_1].port_type   = MV_APP_PORT_WAN;
+            port_type[MV_APP_PON_MAC_PORT].port_type  = MV_APP_PORT_WAN;
+
+            if (active_port == MV_APP_GMAC_PORT_1) {
+                port_type[MV_APP_GMAC_PORT_1].port_state  = MV_GE_PORT_ACTIVE;
+                port_type[MV_APP_PON_MAC_PORT].port_state = MV_GE_PORT_INACTIVE;
+            }
+            else {
+                port_type[MV_APP_GMAC_PORT_1].port_state  = MV_GE_PORT_INACTIVE;
+                port_type[MV_APP_PON_MAC_PORT].port_state = MV_GE_PORT_ACTIVE;
+            }
+            break;
+        case TPM_PON_G0_WAN_G1_INT_SWITCH:
+        case TPM_PON_G0_WAN_G1_SINGLE_PORT:            
+            port_type[MV_APP_GMAC_PORT_0].port_type   = MV_APP_PORT_WAN;
+            port_type[MV_APP_GMAC_PORT_1].port_type   = MV_APP_PORT_LAN;
+            port_type[MV_APP_GMAC_PORT_1].port_state  = MV_GE_PORT_ACTIVE;            
+            port_type[MV_APP_PON_MAC_PORT].port_type  = MV_APP_PORT_WAN;
+
+            if (active_port == MV_APP_GMAC_PORT_0) {
+                port_type[MV_APP_GMAC_PORT_0].port_state  = MV_GE_PORT_ACTIVE;
+                port_type[MV_APP_PON_MAC_PORT].port_state = MV_GE_PORT_INACTIVE;
+            }
+            else {
+                port_type[MV_APP_GMAC_PORT_0].port_state  = MV_GE_PORT_INACTIVE;
+                port_type[MV_APP_PON_MAC_PORT].port_state = MV_GE_PORT_ACTIVE;
+            }            
+            break;
+        case TPM_PON_WAN_G1_MNG_EXT_SWITCH:
+        case TPM_PON_WAN_G1_SINGLE_PORT:            
+            port_type[MV_APP_GMAC_PORT_0].port_state  = MV_GE_PORT_INVALID;
+            port_type[MV_APP_GMAC_PORT_1].port_type   = MV_APP_PORT_LAN;
+            port_type[MV_APP_GMAC_PORT_1].port_state  = MV_GE_PORT_ACTIVE;
+            port_type[MV_APP_PON_MAC_PORT].port_type  = MV_APP_PORT_WAN;
+            port_type[MV_APP_PON_MAC_PORT].port_state = MV_GE_PORT_ACTIVE;
+            break;
+        default:
+            port_type[MV_APP_GMAC_PORT_0].port_state  = MV_GE_PORT_INVALID;
+            port_type[MV_APP_GMAC_PORT_1].port_state  = MV_GE_PORT_INVALID;
+            port_type[MV_APP_PON_MAC_PORT].port_state = MV_GE_PORT_INVALID;            
+            break;
+    }
+
+    return rc;
+}
+
+/******************************************************************************
+* cph_app_parse_peer_port()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Get peer GEMAC port
+*
+* INPUTS:
+*       port        - Original port
+*
+* OUTPUTS:
+*       peer_port   - Peer port
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_parse_peer_port (
+    INT32    port,
+    INT32   *peer_port)
+{
+    UINT32            idx = 0;
+    CPH_PORT_STATE_T  port_type[MV_APP_GMAC_PORT_NUM];
+    MV_STATUS         rc  = MV_FAIL;
+
+    /* Verify port */
+    if (port > MV_APP_PON_MAC_PORT ) {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "port[%d] is out of range[0~%d] \n", port, MV_APP_PON_MAC_PORT);
+        return MV_OUT_OF_RANGE;
+    }
+    
+    /* Get port type */
+    rc = cph_app_parse_ge_port_type(&port_type[0]);
+
+    /* Search for peer port */
+    if (port_type[port].port_type == MV_APP_PORT_LAN)
+    {
+        for (idx = 0; idx < MV_APP_GMAC_PORT_NUM; idx++)
+        {
+            if (idx == port)
+                continue;
+            if ((port_type[idx].port_type  == MV_APP_PORT_WAN) &&
+                (port_type[idx].port_state == MV_GE_PORT_ACTIVE))
+            {
+                *peer_port = idx;
+                rc  = MV_OK;
+                break;
+            }
+        }
+    }
+    else if (port_type[port].port_type == MV_APP_PORT_WAN)
+    {
+         for (idx = 0; idx < MV_APP_GMAC_PORT_NUM; idx++)
+        {
+            if (idx == port)
+                continue;
+            if (port_type[idx].port_type  == MV_APP_PORT_LAN)
+            {
+                *peer_port = idx;
+                rc  = MV_OK;
+                break;
+            }
+        }   
+    }
+
+    return rc;
+}
+
+/******************************************************************************
+* cph_app_parse_dir()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Parse application packet to output parse bitmap and value
+*
+* INPUTS:
+*       port  - GE MAC port
+*       rx    - Whether RX path
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       Return direction.
+*******************************************************************************/
+CPH_DIR_E cph_app_parse_dir (
+    INT32    port,
+    BOOL     rx)
+{
+    CPH_PORT_STATE_T port_type[MV_APP_GMAC_PORT_NUM];
+    MV_STATUS        rc  = MV_OK;
+    CPH_DIR_E        dir = CPH_DIR_INVALID;
+
+    /* Parse port */
+    if (port > MV_APP_PON_MAC_PORT ) {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "port[%d] is out of range[0~%d] \n", port, MV_APP_PON_MAC_PORT);
+        return dir;
+    }
+
+    /* Get GMAC WAN/LAN type */
+    rc = cph_app_parse_ge_port_type(&port_type[0]);
+    if (rc != MV_OK)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "fail to call cph_app_parse_ge_port_type() \n");
+        return dir;
+    }
+
+    /* Rx dir */
+    if (rx == TRUE)
+    {
+        if ((port_type[port].port_type  == MV_APP_PORT_WAN) &&
+            (port_type[port].port_state == MV_GE_PORT_ACTIVE))
+            dir = CPH_DIR_DS;
+        else if ((port_type[port].port_type  == MV_APP_PORT_LAN) &&
+                 (port_type[port].port_state == MV_GE_PORT_ACTIVE))
+            dir = CPH_DIR_US;
+        else
+        {
+            dir = CPH_DIR_INVALID;
+            MV_CPH_PRINT(CPH_ERR_LEVEL, "dir[%d] is invalid \n", dir);
+        }
+    }
+    /* Tx dir */
+    else
+    {
+        if ((port_type[port].port_type  == MV_APP_PORT_WAN) &&
+            (port_type[port].port_state == MV_GE_PORT_ACTIVE))
+            dir = CPH_DIR_US;
+        else if ((port_type[port].port_type  == MV_APP_PORT_LAN) &&
+                 (port_type[port].port_state == MV_GE_PORT_ACTIVE))
+            dir = CPH_DIR_DS;
+        else
+        {
+            dir = CPH_DIR_INVALID;
+            MV_CPH_PRINT(CPH_ERR_LEVEL, "dir[%d] is invalid \n", dir);
+        }    
+    }
+
+    return dir;
+}
+
+/******************************************************************************
+* cph_app_parse_packet()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Parse application packet to output parse bitmap and value
+*
+* INPUTS:
+*       port       - GE MAC port
+*       skb_data   - Pointer to SKB data holding application packet
+*
+* OUTPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_parse_packet (
+    INT32                  port,
+    UINT8                 *skb_data,
+    CPH_APP_PARSE_FIELD_E *parse_bm, 
+    CPH_APP_PARSE_T       *parse_key)
+{
+    UINT16                  eth_type     = 0;
+    struct ipv6hdr         *p_ipv6_hdr   = NULL;
+    struct ipv6_hopopt_hdr *p_hopopt_hdr = NULL;
+    struct icmp6hdr        *p_icmp_hdr   = NULL;    
+    UINT8                  *p_field      = NULL;
+    MV_STATUS               rc           = MV_OK;
+
+    *parse_bm = 0;
+    memset(parse_key, 0, sizeof(CPH_APP_PARSE_T));
+
+    /* Parse dir */
+    parse_key->dir = cph_app_parse_dir(port, TRUE);
+    if(parse_key->dir == CPH_DIR_INVALID)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "dir[%d] is invalid \n", parse_key->dir);
+        return MV_BAD_VALUE;
+    }
+    *parse_bm |= CPH_APP_PARSE_FIELD_DIR;
+
+    /* Parse RX/TX */
+    parse_key->rx_tx = CPH_DIR_RX;
+    *parse_bm |= CPH_APP_PARSE_FIELD_RX_TX;
+    
+    /* Parse Marvell header */
+    if (parse_key->dir == CPH_DIR_US)
+        parse_key->mh = (ntohs(*(UINT16 *)skb_data) & MV_VALID_MH_MASK);
+    else
+        parse_key->mh = (ntohs(*(UINT16 *)skb_data) & MV_VALID_GH_MASK);
+    *parse_bm    |= CPH_APP_PARSE_FIELD_MH;
+    
+    /* Parse Eth type */
+    p_field  = skb_data + MV_ETH_MH_SIZE + ETH_ALEN + ETH_ALEN;
+    eth_type = ntohs(*(UINT16 *)p_field);
+    while (eth_type == MV_TPID_8100 || eth_type == MV_TPID_88A8 || eth_type == MV_TPID_9100) {
+        p_field += VLAN_HLEN;
+        eth_type = ntohs(*(UINT16 *)p_field);
+    }
+    parse_key->eth_type = eth_type;
+    *parse_bm |= CPH_APP_PARSE_FIELD_ETH_TYPE;
+
+    /* Parse IPv4 type */
+    if (eth_type == ETH_P_IP) {
+        p_field += MV_CPH_ETH_TYPE_LEN;
+        p_field += MV_IPV4_PROTO_OFFSET;
+        parse_key->ipv4_type = *(UINT8 *)p_field;
+        *parse_bm |= CPH_APP_PARSE_FIELD_IPV4_TYPE;
+    }
+    /* Parse IPv6 type */
+    else if (eth_type == ETH_P_IPV6) {
+        
+        p_ipv6_hdr = (struct ipv6hdr *)(p_field + MV_CPH_ETH_TYPE_LEN);
+        parse_key->ipv6_nh1 = p_ipv6_hdr->nexthdr;
+        *parse_bm |= CPH_APP_PARSE_FIELD_IPV6_NH1;
+
+        if (p_ipv6_hdr->nexthdr != NEXTHDR_HOP)
+          return rc;
+
+        p_hopopt_hdr = (struct ipv6_hopopt_hdr *)((UINT8 *)p_ipv6_hdr + sizeof(struct ipv6hdr));
+
+        parse_key->ipv6_nh2 = p_hopopt_hdr->nexthdr;
+        *parse_bm |= CPH_APP_PARSE_FIELD_IPV6_NH2;
+
+        if (p_hopopt_hdr->nexthdr != IPPROTO_ICMPV6)
+            return rc;
+
+        p_icmp_hdr =  (struct icmp6hdr *)((UINT8 *)p_hopopt_hdr + ipv6_optlen(p_hopopt_hdr));
+        
+        switch (p_icmp_hdr->icmp6_type) {
+            case ICMPV6_MGM_QUERY:
+            case ICMPV6_MGM_REPORT:
+            case ICMPV6_MGM_REDUCTION:
+            case ICMPV6_MLD2_REPORT:
+                parse_key->icmpv6_type = MV_ICMPV6_TYPE_MLD;
+                *parse_bm |= CPH_APP_PARSE_FIELD_ICMPV6_TYPE;
+                break;
+            default:
+                break;
+        }
+
+    }
+    /* Parse Ethenet subtype */
+    else {
+        parse_key->eth_subtype = (*(UINT8 *)(p_field + MV_CPH_ETH_TYPE_LEN));
+        *parse_bm |= CPH_APP_PARSE_FIELD_ETH_SUBTYPE;
+    }
+    
+    return rc;
+}
+
+/******************************************************************************
+* cph_app_mod_rx_packet()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Modify RX application packet
+*
+* INPUTS:
+*       port      - Gmac port the packet from
+*       dev       - Net device
+*       skb       - SKB buffer to receive packet
+*       rx_desc   - RX descriptor
+*       mod_bm    - Modification bitmap
+*       mod_value - Modification value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_mod_rx_packet (  
+    INT32                port,
+    struct net_device   *dev,
+    struct sk_buff      *skb,
+    struct neta_rx_desc *rx_desc,
+    CPH_APP_MOD_FIELD_E  mod_bm, 
+    CPH_APP_MOD_T       *mod_value)
+{
+    UINT8     *p_data = NULL;
+    MV_STATUS  rc     = MV_OK;
+
+    /* Save GMAC Information */    
+    if (mod_bm & CPH_APP_RX_MOD_ADD_GMAC)
+    {
+        p_data     = (UINT8 *)skb->data;
+        p_data[0] &= 0x0F;
+        p_data[0] |= ((port & 0x0F) << 4);
+    }
+
+    /* Replace protocol type */
+    skb->tail    += rx_desc->dataSize;
+    skb->len      = rx_desc->dataSize;
+
+    if (mod_bm & CPH_APP_RX_MOD_STRIP_MH)
+    {
+
+        skb->data += MV_ETH_MH_SIZE;
+        skb->tail -= MV_ETH_MH_SIZE;
+        skb->len  -= MV_ETH_MH_SIZE;
+    } 
+    
+    skb->protocol = eth_type_trans(skb, dev);
+    if (mod_bm & CPH_APP_RX_MOD_REPLACE_PROTO_TYPE)
+    {
+        skb->protocol = mod_value->proto_type;
+    }    
+
+    return rc;
+}
+
+/******************************************************************************
+* cph_app_mod_tx_packet()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Modify TX application packet
+*
+* INPUTS:
+*       skb         - Pointer to SKB data hoding application packet
+*       tx_spec_out - TX descriptor
+*       mod_bm      - Modification bitmap
+*       mod_value   - Modification value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_mod_tx_packet (
+    struct sk_buff        *skb,
+    struct mv_eth_tx_spec *tx_spec_out,
+    CPH_APP_MOD_FIELD_E    mod_bm, 
+    CPH_APP_MOD_T         *mod_value)
+{
+    MV_STATUS rc = MV_OK;
+
+    if (mod_bm & CPH_APP_TX_MOD_ADD_MH_BY_DRIVER)
+    {
+        tx_spec_out->flags |= MV_ETH_F_MH;
+    } 
+
+    if (mod_bm & CPH_APP_TX_MOD_NO_PAD)
+    {
+        tx_spec_out->flags |= MV_ETH_F_NO_PAD;
+    } 
+
+    return rc;
+}
+
+/******************************************************************************
+* cph_app_set_frwd()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Set packet forwarding information
+*
+* INPUTS:
+*       skb         - Pointer to SKB data hoding application packet
+*       tx_spec_out - TX descriptor
+*       frwd_bm     - Forwarding bitmap
+*       frwd_value  - Forwarding value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_set_frwd (
+    struct sk_buff        *skb,
+    struct mv_eth_tx_spec *tx_spec_out,
+    CPH_APP_FRWD_FIELD_E   frwd_bm, 
+    CPH_APP_FRWD_T        *frwd_value) 
+{
+    MV_STATUS rc = MV_OK;
+
+    if (frwd_bm & CPH_APP_FRWD_SET_TRG_PORT)
+    {
+       tx_spec_out->txp = frwd_value->trg_port;
+    } 
+
+    if (frwd_bm & CPH_APP_FRWD_SET_TRG_QUEUE)
+    {
+       tx_spec_out->txq = frwd_value->trg_queue;
+    } 
+
+    if (frwd_bm & CPH_APP_FRWD_SET_GEM_PORT)
+    {
+       tx_spec_out->hw_cmd  = ((frwd_value->gem_port << 8)|0x0010);
+    }
+
+    tx_spec_out->tx_func = NULL;
+
+    return rc;
+}
+
+/******************************************************************************
+* cph_app_rx_bc()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: CPH function to handle the received broadcast packets 
+*
+* INPUTS:
+*       port    - Gmac port the packet from
+*       dev     - Net device
+*       pkt     - Marvell packet information
+*       rx_desc - RX descriptor
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns 1. 
+*       On error returns 0.
+*******************************************************************************/
+INT32 cph_app_rx_bc(INT32 port, struct net_device *dev, struct eth_pbuf *pkt, struct neta_rx_desc *rx_desc)
+{
+    CPH_FLOW_ENTRY_T      flow_rule;
+    INT32                 peer_port = 0;
+    INT32                 rx_size   = 0;
+    INT32                 offset    = 0;
+    BOOL                  state     = FALSE;
+    struct sk_buff       *skb_old   = NULL;
+    struct sk_buff       *skb_new   = NULL;
+    MV_STATUS             rc        = MV_OK;
+    
+    /* Check whether need to handle broadcast packet */
+    cph_db_get_param(CPH_DB_PARAM_BC_SUPPORT, &state);
+    if (state == FALSE)
+        return 0;
+
+    /* Parse packets */
+    skb_old = (struct sk_buff *)(pkt->osInfo);
+    skb_new = (struct sk_buff *)(pkt->osInfo);
+    rc = cph_flow_parse_packet(port, skb_old->data, TRUE, TRUE, &flow_rule);
+    if (rc != MV_OK)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "fail to call cph_flow_parse_packet, rc<%d> \n", rc);
+        return 0;
+    }
+
+    /* U/S */
+    if(flow_rule.dir == CPH_DIR_US)
+    {  
+        /* Copy a new SKB */
+        skb_old->tail += rx_desc->dataSize;
+        skb_old->len   = rx_desc->dataSize;
+        skb_new = skb_copy(skb_old, GFP_ATOMIC);
+        if(skb_new == NULL)
+        {   
+            skb_new = skb_old;
+            goto out;
+        }
+        
+        /* Forward packet to peer port */
+        rc = cph_app_parse_peer_port(port, &peer_port);
+        if (rc != MV_OK)
+        {
+            MV_CPH_PRINT(CPH_ERR_LEVEL, "fail to call cph_app_parse_peer_port, rc<%d> \n", rc);
+            return 0;
+        }
+
+        /* Forward packet */
+        mv_net_devs[peer_port]->netdev_ops->ndo_start_xmit(skb_old, mv_net_devs[peer_port]);      
+    }
+out:
+    /* Stripe VLAN tag, then send to Linux network stack */
+    offset         = cph_flow_strip_vlan(TRUE, skb_new->data);
+    skb_new->data += offset;
+    rx_size       -= offset;
+
+    /* Strip MH */
+    skb_new->data += MV_ETH_MH_SIZE;
+    offset        += MV_ETH_MH_SIZE;
+
+    skb_new->tail    -= offset;
+    skb_new->len     -= offset;
+    skb_new->protocol = eth_type_trans(skb_new, dev);
+
+    cph_rec_skb(port, skb_new);
+
+    return 1;    
+}
+
+/******************************************************************************
+* cph_app_lookup_profile_id()
+* _____________________________________________________________________________
+*
+* DESCRIPTION:lookup profile ID string according to value
+*
+* INPUTS:
+*       enum_value - The enum value to be matched
+*
+* OUTPUTS:
+*       None
+*
+* RETURNS:
+*       Enum string
+*******************************************************************************/
+CHAR *cph_app_lookup_profile_id(INT32 enum_value)
+{
+    return mtype_lookup_enum_str(&g_enum_array_profile_id, enum_value);
+}
+
+/******************************************************************************
+* cph_app_lookup_pon_type()
+* _____________________________________________________________________________
+*
+* DESCRIPTION:lookup PON type string according to value
+*
+* INPUTS:
+*       enum_value - The enum value to be matched
+*
+* OUTPUTS:
+*       None
+*
+* RETURNS:
+*       Enum string
+*******************************************************************************/
+CHAR *cph_app_lookup_pon_type(INT32 enum_value)
+{
+    return mtype_lookup_enum_str(&g_enum_array_pon_type, enum_value);
+}
+
+/******************************************************************************
+* cph_app_lookup_dir()
+* _____________________________________________________________________________
+*
+* DESCRIPTION:lookup direction string according to value
+*
+* INPUTS:
+*       enum_value - The enum value to be matched
+*
+* OUTPUTS:
+*       None
+*
+* RETURNS:
+*       Enum string
+*******************************************************************************/
+CHAR *cph_app_lookup_dir(INT32 enum_value)
+{
+    return mtype_lookup_enum_str(&g_enum_array_dir, enum_value);
+}
+
+/******************************************************************************
+* cph_app_lookup_rx_tx()
+* _____________________________________________________________________________
+*
+* DESCRIPTION:lookup RX/TX direction string according to value
+*
+* INPUTS:
+*       enum_value - The enum value to be matched
+*
+* OUTPUTS:
+*       None
+*
+* RETURNS:
+*       Enum string
+*******************************************************************************/
+CHAR *cph_app_lookup_rx_tx(INT32 enum_value)
+{
+    return mtype_lookup_enum_str(&g_enum_array_rx_tx, enum_value);
+}
+
+/******************************************************************************
+* cph_app_lookup_gmac()
+* _____________________________________________________________________________
+*
+* DESCRIPTION:lookup GMAC string according to value
+*
+* INPUTS:
+*       enum_value - The enum value to be matched
+*
+* OUTPUTS:
+*       None
+*
+* RETURNS:
+*       Enum string
+*******************************************************************************/
+CHAR *cph_app_lookup_gmac(INT32 enum_value)
+{
+    return mtype_lookup_enum_str(&g_enum_array_gmac, enum_value);
+}
+
+
+/******************************************************************************
+* cph_app_init()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Initializes CPH application module.
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32  cph_app_init(VOID)
+{
+    
+    cph_db_init();
+    
+    return MV_OK;
+}
+
+/******************************************************************************
+* cph_set_trace_flag()
+* _____________________________________________________________________________
+*
+* DESCRIPTION:sets cph trace flag.
+*
+* INPUTS:
+*       enum_value - The enum value to be matched
+*
+* OUTPUTS:
+*       None
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_set_trace_flag(UINT32 flag)
+{
+    g_cph_global_trace = flag;
+    
+    return MV_OK;
+}
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_app.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_app.h
new file mode 100755
index 0000000..68a0358
--- /dev/null
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_app.h
@@ -0,0 +1,779 @@
+/*******************************************************************************
+Copyright (C) Marvell International Ltd. and its affiliates
+
+This software file (the "File") is owned and distributed by Marvell 
+International Ltd. and/or its affiliates ("Marvell") under the following
+alternative licensing terms.  Once you have made an election to distribute the
+File under one of the following license alternatives, please (i) delete this
+introductory statement regarding license alternatives, (ii) delete the two
+license alternatives that you have not elected to use and (iii) preserve the
+Marvell copyright notice above.
+
+********************************************************************************
+Marvell Commercial License Option
+
+If you received this File from Marvell and you have entered into a commercial
+license agreement (a "Commercial License") with Marvell, the File is licensed
+to you under the terms of the applicable Commercial License.
+
+********************************************************************************
+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.
+********************************************************************************
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or 
+modify this File under the following licensing terms. 
+Redistribution and use in source and binary forms, with or without modification, 
+are permitted provided that the following conditions are met:
+
+    *   Redistributions of source code must retain the above copyright notice,
+        this list of conditions and the following disclaimer. 
+
+    *   Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution. 
+
+    *   Neither the name of Marvell nor the names of its contributors may be 
+        used to endorse or promote products derived from this software without 
+        specific prior written permission. 
+    
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+********************************************************************************
+* mv_cph_app.h
+*
+* DESCRIPTION: Marvell CPH(CPH Packet Handler) application module to implement
+*              CPH main logic and handle application packets such as OMCI, eOAM, 
+*              IGMP packets.
+*
+* DEPENDENCIES:
+*               None
+*
+* CREATED BY:   VictorGu
+*
+* DATE CREATED: 22Jan2013
+*
+* FILE REVISION NUMBER:
+*               Revision: 1.0
+*
+*
+*******************************************************************************/
+#ifndef _MV_CPH_APP_H_
+#define _MV_CPH_APP_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/******************************************************************************
+ * Type Definition
+ ******************************************************************************/
+#define MV_VALID_MH_MASK  0x007F  /* Valid MH filed in CPH */
+#define MV_VALID_GH_MASK  0x0FFF  /* Valid MH filed in CPH */
+
+
+/* CPH application packets parsing field definition
+------------------------------------------------------------------------------*/
+typedef enum
+{
+    CPH_APP_PARSE_FIELD_DIR          = 0x1,
+    CPH_APP_PARSE_FIELD_RX_TX        = 0x2,
+    CPH_APP_PARSE_FIELD_MH           = 0x4,
+    CPH_APP_PARSE_FIELD_ETH_TYPE     = 0x8,
+    CPH_APP_PARSE_FIELD_ETH_SUBTYPE  = 0x10,
+    CPH_APP_PARSE_FIELD_IPV4_TYPE    = 0x20,
+    CPH_APP_PARSE_FIELD_IPV6_NH1     = 0x40,
+    CPH_APP_PARSE_FIELD_IPV6_NH2     = 0x80,
+    CPH_APP_PARSE_FIELD_ICMPV6_TYPE  = 0x100,
+    CPH_APP_PARSE_FIELD_END          = 0x200
+} CPH_APP_PARSE_FIELD_E;
+
+typedef enum
+{
+    CPH_DIR_US       = 0,
+    CPH_DIR_DS       = 1,
+    CPH_DIR_NOT_CARE = 2,
+    CPH_DIR_NUM      = 2,
+    CPH_DIR_INVALID  = 3
+} CPH_DIR_E;
+
+typedef enum
+{
+    CPH_DIR_RX         = 0,
+    CPH_DIR_TX         = 1,
+    CPH_RX_TX_NOT_CARE = 2,
+} CPH_RX_TX_E;
+
+typedef struct
+{
+    CPH_DIR_E   dir;
+    CPH_RX_TX_E rx_tx;
+    UINT16      mh;
+    UINT16      eth_type;
+    UINT8       eth_subtype;
+    UINT8       ipv4_type;
+    UINT8       ipv6_nh1;
+    UINT8       ipv6_nh2;
+    UINT32      icmpv6_type;
+} CPH_APP_PARSE_T;
+
+/* CPH application packets modification field definition
+------------------------------------------------------------------------------*/
+typedef enum
+{
+    CPH_APP_RX_MOD_ADD_GMAC           = 0x1,
+    CPH_APP_RX_MOD_REPLACE_PROTO_TYPE = 0x2,
+    CPH_APP_RX_MOD_STRIP_MH           = 0x4,
+    CPH_APP_TX_MOD_ADD_MH_BY_DRIVER   = 0x8,
+    CPH_APP_TX_MOD_NO_PAD             = 0x10,
+    CPH_APP_MOD_CHANGE_STATE          = 0x20,
+    CPH_APP_MOD_FIELD_END             = 0x40
+} CPH_APP_MOD_FIELD_E;
+
+typedef struct
+{
+    UINT16   proto_type;
+    BOOL     state;
+} CPH_APP_MOD_T;
+
+/* CPH application packets forwarding field definition
+------------------------------------------------------------------------------*/
+typedef enum
+{
+    CPH_APP_FRWD_SET_TRG_PORT     = 0x1,
+    CPH_APP_FRWD_SET_TRG_QUEUE    = 0x2,
+    CPH_APP_FRWD_SET_GEM_PORT     = 0x4,
+    CPH_APP_FRWD_FIELD_END        = 0x8
+} CPH_APP_FRWD_FIELD_E;
+
+typedef struct
+{
+    UINT8   trg_port;
+    UINT8   trg_queue;
+    UINT16  gem_port;
+} CPH_APP_FRWD_T;
+
+/* TPM connection profile definition from "tpm_types.h"
+------------------------------------------------------------------------------*/
+typedef enum {
+    TPM_PON_WAN_DUAL_MAC_INT_SWITCH = 1,
+    TPM_PON_WAN_G0_INT_SWITCH,
+    TPM_PON_WAN_G1_LAN_G0_INT_SWITCH,
+    TPM_G0_WAN_G1_INT_SWITCH,
+    TPM_G1_WAN_G0_INT_SWITCH,
+    TPM_PON_G1_WAN_G0_INT_SWITCH,
+    TPM_PON_G0_WAN_G1_INT_SWITCH,
+    TPM_PON_WAN_DUAL_MAC_EXT_SWITCH,
+    TPM_PON_WAN_G1_MNG_EXT_SWITCH,
+    TPM_PON_WAN_G0_SINGLE_PORT,
+    TPM_PON_WAN_G1_SINGLE_PORT,
+    TPM_PON_G1_WAN_G0_SINGLE_PORT,
+    TPM_PON_G0_WAN_G1_SINGLE_PORT,
+    TPM_PON_WAN_G0_G1_LPBK,
+    TPM_PON_WAN_G0_G1_DUAL_LAN
+} tpm_eth_complex_profile_t;
+
+/* PON type definition
+------------------------------------------------------------------------------*/
+typedef enum
+{
+    CPH_PON_TYPE_EPON = 0,
+    CPH_PON_TYPE_GPON,
+    CPH_PON_TYPE_GBE,
+    CPH_PON_TYPE_P2P,
+    CPH_PON_TYPE_MAX
+} CPH_PON_TYPE_E;
+
+
+/* CPH feature flag
+------------------------------------------------------------------------------*/
+typedef enum
+{
+    CPH_APP_FEATURE_APP = 0,
+    CPH_APP_FEATURE_IGMP,
+    CPH_APP_FEATURE_BC,
+    CPH_APP_FEATURE_FLOW,
+    CPH_APP_FEATURE_UDP,
+} CPH_APP_FEATURE_E;
+
+
+/* CPH parse/modification field definition for bc/igmp 
+------------------------------------------------------------------------------*/
+typedef struct
+{
+    CPH_DIR_E      dir;   
+    UINT32         src_port;
+    UINT16         vid;
+    UINT8          pbits;
+    UINT8          dscp;
+} CPH_PACKET_PARSE_T;
+
+
+typedef struct
+{
+    CPH_DIR_E      dir;   
+    UINT32         src_port;
+    UINT16         vid;
+    UINT8          pbits;
+    UINT8          dscp;
+} CPH_PACKET_MOD_T;
+
+/* GMAC port state definition
+------------------------------------------------------------------------------*/
+typedef struct
+{
+    MV_APP_PORT_TYPE_E   port_type;
+    MV_GE_PORT_STATE_E   port_state;
+} CPH_PORT_STATE_T;
+
+/* Debug related definition
+------------------------------------------------------------------------------*/
+extern UINT32 g_cph_global_trace;
+
+#define CPH_GLOBAL_TRACE g_cph_global_trace
+#define CPH_DEBUG_LEVEL  0x00000001
+#define CPH_INFO_LEVEL   0x00000002
+#define CPH_WARN_LEVEL   0x00000004
+#define CPH_ERR_LEVEL    0x00000008
+
+#define MV_CPH_PRINT(level, format, ...) \
+    { \
+        if (level & CPH_GLOBAL_TRACE) \
+            printk(KERN_INFO "%s(line:%d) "format, __FUNCTION__, __LINE__, ##__VA_ARGS__); \
+    }
+
+#define MV_CPH_CLEAN_PRINT(level, format, ...) \
+    { \
+        if (level & CPH_GLOBAL_TRACE) \
+            printk(KERN_INFO format, ##__VA_ARGS__); \
+    }
+
+/******************************************************************************
+ * Function Declaration
+ ******************************************************************************/
+/******************************************************************************
+* cph_app_set_complex_profile()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Set TPM complex profile ID 
+*
+* INPUTS:
+*       profile_id   - TPM complex profile ID
+*       active_port  - Active WAN port
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_set_complex_profile (tpm_eth_complex_profile_t profile_id, MV_APP_GMAC_PORT_E active_port);
+
+/******************************************************************************
+* cph_app_set_feature_flag()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Enable or disable feature support in CPH 
+*
+* INPUTS:
+*       feature - CPH supported features
+*       state   - Enable or disable this feature in CPH
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_set_feature_flag (CPH_APP_FEATURE_E feature, BOOL state);
+
+/******************************************************************************
+* cph_app_add_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Add CPH rule 
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_add_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key,  
+    CPH_APP_MOD_FIELD_E   mod_bm, 
+    CPH_APP_MOD_T        *mod_value, 
+    CPH_APP_FRWD_FIELD_E  frwd_bm, 
+    CPH_APP_FRWD_T       *frwd_value);
+
+/******************************************************************************
+* cph_app_del_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Del CPH rule 
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_del_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key);
+
+/******************************************************************************
+* cph_app_update_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Update CPH rule 
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_update_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key,  
+    CPH_APP_MOD_FIELD_E   mod_bm, 
+    CPH_APP_MOD_T        *mod_value, 
+    CPH_APP_FRWD_FIELD_E  frwd_bm, 
+    CPH_APP_FRWD_T       *frwd_value);
+
+/******************************************************************************
+* cph_app_get_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Get CPH rule 
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*
+* OUTPUTS:
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_get_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key,  
+    CPH_APP_MOD_FIELD_E  *mod_bm, 
+    CPH_APP_MOD_T        *mod_value, 
+    CPH_APP_FRWD_FIELD_E *frwd_bm, 
+    CPH_APP_FRWD_T       *frwd_value);
+
+/******************************************************************************
+* cph_app_get_rule_by_dir_proto()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Get CPH rule according to protocol type
+*
+* INPUTS:
+*       dir        - Direction
+*       proto_type - SKB protocol type
+*
+* OUTPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_get_rule_by_dir_proto (
+    CPH_DIR_E              dir,
+    UINT16                 proto_type,
+    CPH_APP_PARSE_FIELD_E *parse_bm,
+    CPH_APP_PARSE_T       *parse_key,
+    CPH_APP_MOD_FIELD_E   *mod_bm,
+    CPH_APP_MOD_T         *mod_value,
+    CPH_APP_FRWD_FIELD_E  *frwd_bm,
+    CPH_APP_FRWD_T        *frwd_value);
+
+/******************************************************************************
+* cph_app_increase_counter()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Increase RX counter
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_increase_counter (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key);
+
+/******************************************************************************
+* cph_app_increase_counter_by_dir_proto()
+* _____________________________________________________________________________
+*
+* DESCRIPTION:  Increase RX counter according to protocol type
+*
+* INPUTS:
+*       dir        - Direction
+*       proto_type - SKB protocol type
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_increase_counter_by_dir_proto (
+    CPH_DIR_E dir,
+    UINT16    proto_type);
+
+/******************************************************************************
+* cph_app_parse_ge_port_type()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Get GEMAC port type by profile ID
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       port_type   - Modification bitmap
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_parse_ge_port_type (
+    CPH_PORT_STATE_T          *port_type);
+
+/******************************************************************************
+* cph_app_parse_dir()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Parse application packet to output parse bitmap and value
+*
+* INPUTS:
+*       port  - GE MAC port
+*       rx    - Whether RX path
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       Return direction.
+*******************************************************************************/
+CPH_DIR_E cph_app_parse_dir (
+    INT32    port,
+    BOOL     rx);
+
+/******************************************************************************
+* cph_app_parse_packet()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Parse application packet to output parse bitmap and value
+*
+* INPUTS:
+*       port       - GE MAC port
+*       skb_data   - Pointer to SKB data holding application packet
+*
+* OUTPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_parse_packet (
+    INT32                  port,
+    UINT8                 *skb_data,
+    CPH_APP_PARSE_FIELD_E *parse_bm, 
+    CPH_APP_PARSE_T       *parse_key);
+
+/******************************************************************************
+* cph_app_mod_rx_packet()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Modify RX application packet
+*
+* INPUTS:
+*       port      - Gmac port the packet from
+*       dev       - Net device
+*       skb       - SKB buffer to receive packet
+*       rx_desc   - RX descriptor
+*       mod_bm    - Modification bitmap
+*       mod_value - Modification value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_mod_rx_packet (  
+    INT32                port,
+    struct net_device   *dev,
+    struct sk_buff      *skb,
+    struct neta_rx_desc *rx_desc,
+    CPH_APP_MOD_FIELD_E  mod_bm, 
+    CPH_APP_MOD_T       *mod_value);
+
+/******************************************************************************
+* cph_app_mod_tx_packet()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Modify TX application packet
+*
+* INPUTS:
+*       skb         - Pointer to SKB data hoding application packet
+*       tx_spec_out - TX descriptor
+*       mod_bm      - Modification bitmap
+*       mod_value   - Modification value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_mod_tx_packet (
+    struct sk_buff        *skb,
+    struct mv_eth_tx_spec *tx_spec_out,
+    CPH_APP_MOD_FIELD_E    mod_bm, 
+    CPH_APP_MOD_T         *mod_value);
+
+/******************************************************************************
+* cph_app_set_frwd()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Set packet forwarding information
+*
+* INPUTS:
+*       skb         - Pointer to SKB data hoding application packet
+*       tx_spec_out - TX descriptor
+*       frwd_bm     - Forwarding bitmap
+*       frwd_value  - Forwarding value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_app_set_frwd (
+    struct sk_buff        *skb,
+    struct mv_eth_tx_spec *tx_spec_out,
+    CPH_APP_FRWD_FIELD_E   frwd_bm, 
+    CPH_APP_FRWD_T        *frwd_value);
+
+/******************************************************************************
+* cph_app_rx_bc()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: CPH function to handle the received broadcast packets 
+*
+* INPUTS:
+*       port    - Gmac port the packet from
+*       dev     - Net device
+*       pkt     - Marvell packet information
+*       rx_desc - RX descriptor
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns 1. 
+*       On error returns 0.
+*******************************************************************************/
+INT32 cph_app_rx_bc(INT32 port, struct net_device *dev, struct eth_pbuf *pkt, struct neta_rx_desc *rx_desc);
+
+/******************************************************************************
+* cph_app_lookup_profile_id()
+*
+* DESCRIPTION:lookup profile ID string according to value
+*
+* INPUTS:
+*       enum_value - The enum value to be matched
+*
+* OUTPUTS:
+*       None
+*
+* RETURNS:
+*       Enum string
+*******************************************************************************/
+CHAR *cph_app_lookup_profile_id(INT32 enum_value);
+
+/******************************************************************************
+* cph_app_lookup_pon_type()
+*
+* DESCRIPTION:lookup PON type string according to value
+*
+* INPUTS:
+*       enum_value - The enum value to be matched
+*
+* OUTPUTS:
+*       None
+*
+* RETURNS:
+*       Enum string
+*******************************************************************************/
+CHAR *cph_app_lookup_pon_type(INT32 enum_value);
+
+/******************************************************************************
+* cph_app_lookup_dir()
+*
+* DESCRIPTION:lookup direction string according to value
+*
+* INPUTS:
+*       enum_value - The enum value to be matched
+*
+* OUTPUTS:
+*       None
+*
+* RETURNS:
+*       Enum string
+*******************************************************************************/
+CHAR *cph_app_lookup_dir(INT32 enum_value);
+
+/******************************************************************************
+* cph_app_lookup_rx_tx()
+* _____________________________________________________________________________
+*
+* DESCRIPTION:lookup RX/TX direction string according to value
+*
+* INPUTS:
+*       enum_value - The enum value to be matched
+*
+* OUTPUTS:
+*       None
+*
+* RETURNS:
+*       Enum string
+*******************************************************************************/
+CHAR *cph_app_lookup_rx_tx(INT32 enum_value);
+
+/******************************************************************************
+* cph_app_lookup_gmac()
+* _____________________________________________________________________________
+*
+* DESCRIPTION:lookup GMAC string according to value
+*
+* INPUTS:
+*       enum_value - The enum value to be matched
+*
+* OUTPUTS:
+*       None
+*
+* RETURNS:
+*       Enum string
+*******************************************************************************/
+CHAR *cph_app_lookup_gmac(INT32 enum_value);
+
+/******************************************************************************
+* cph_app_init()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Initializes CPH application module.
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32  cph_app_init(VOID);
+
+/******************************************************************************
+* cph_set_trace_flag()
+* _____________________________________________________________________________
+*
+* DESCRIPTION:sets cph trace flag.
+*
+* INPUTS:
+*       enum_value - The enum value to be matched
+*
+* OUTPUTS:
+*       None
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_set_trace_flag(UINT32 flag);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _MV_CPH_APP_H_ */
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_db.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_db.c
new file mode 100755
index 0000000..6e4d062
--- /dev/null
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_db.c
@@ -0,0 +1,1542 @@
+/*******************************************************************************
+Copyright (C) Marvell International Ltd. and its affiliates
+
+This software file (the "File") is owned and distributed by Marvell 
+International Ltd. and/or its affiliates ("Marvell") under the following
+alternative licensing terms.  Once you have made an election to distribute the
+File under one of the following license alternatives, please (i) delete this
+introductory statement regarding license alternatives, (ii) delete the two
+license alternatives that you have not elected to use and (iii) preserve the
+Marvell copyright notice above.
+
+********************************************************************************
+Marvell Commercial License Option
+
+If you received this File from Marvell and you have entered into a commercial
+license agreement (a "Commercial License") with Marvell, the File is licensed
+to you under the terms of the applicable Commercial License.
+
+********************************************************************************
+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.
+********************************************************************************
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or 
+modify this File under the following licensing terms. 
+Redistribution and use in source and binary forms, with or without modification, 
+are permitted provided that the following conditions are met:
+
+    *   Redistributions of source code must retain the above copyright notice,
+        this list of conditions and the following disclaimer. 
+
+    *   Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution. 
+
+    *   Neither the name of Marvell nor the names of its contributors may be 
+        used to endorse or promote products derived from this software without 
+        specific prior written permission. 
+    
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+********************************************************************************
+* mv_cph_db.c
+*
+* DESCRIPTION: Marvell CPH(CPH Packet Handler) data base implementation
+*
+* DEPENDENCIES:
+*               None
+*
+* CREATED BY:   VictorGu
+*
+* DATE CREATED: 22Jan2013
+*
+* FILE REVISION NUMBER:
+*               Revision: 1.0
+*
+*
+*******************************************************************************/
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/delay.h>
+#include <linux/wait.h>
+#include <linux/mutex.h>
+#include <linux/spinlock.h>
+#include <linux/poll.h>
+#include <linux/clk.h>
+#include <linux/fs.h>
+#include <linux/vmalloc.h>
+#include <linux/cdev.h>
+#include <linux/device.h>
+#include <linux/miscdevice.h>
+
+#include "mv_cph_header.h"
+#include "ezxml.h"
+
+
+/******************************************************************************
+ * Variable Definition
+ ******************************************************************************/
+static CPH_APP_DB_T g_cph_app_db;
+CHAR *g_cph_xml_cfg_file         = US_CPH_XML_CFG_FILE;
+CHAR *g_onu_profile_xml_cfg_file = US_ONU_PROFILE_XML_CFG_FILE;
+
+
+/******************************************************************************
+ * Function Definition
+ ******************************************************************************/
+/******************************************************************************
+* cph_db_set_param()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Set CPH DB parameter
+*
+* INPUTS:
+*       param   - The parameter type
+*       value   - Parameter value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_set_param (CPH_DB_PARAM_E param, VOID *value)
+{
+    MV_STATUS rc = MV_OK;
+
+    switch (param)
+    {
+        case CPH_DB_PARAM_PROFILE_ID:
+            g_cph_app_db.profile_id   = *(tpm_eth_complex_profile_t *)value;
+            break;
+        case CPH_DB_PARAM_ACTIVE_PORT:
+            g_cph_app_db.active_port  = *(MV_APP_GMAC_PORT_E *)value;
+            break;
+        case CPH_DB_PARAM_APP_SUPPORT:
+            g_cph_app_db.app_support  = *(BOOL *)value;
+            break;            
+        case CPH_DB_PARAM_IGMP_SUPPORT:
+            g_cph_app_db.igmp_support = *(BOOL *)value;
+            break;
+        case CPH_DB_PARAM_BC_SUPPORT:
+            g_cph_app_db.bc_support   = *(BOOL *)value;
+            break;
+        case CPH_DB_PARAM_FLOW_SUPPORT:
+            g_cph_app_db.flow_support = *(BOOL *)value;
+            break;
+        case CPH_DB_PARAM_UDP_SUPPORT:
+            g_cph_app_db.udp_support  = *(BOOL *)value;            
+            break;
+        case CPH_DB_PARAM_BC_COUNTER:
+            g_cph_app_db.bc_count     = *(UINT32 *)value;            
+            break;
+        default:
+            break;
+    }
+
+    return rc;
+}
+
+/******************************************************************************
+* cph_db_get_param()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Get CPH DB parameter
+*
+* INPUTS:
+*       param   - The parameter type
+*
+* OUTPUTS:
+*       value   - Parameter value
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_get_param (CPH_DB_PARAM_E param, VOID *value)
+{
+    MV_STATUS rc = MV_OK;
+
+    switch (param)
+    {
+        case CPH_DB_PARAM_PROFILE_ID:
+            *(tpm_eth_complex_profile_t *)value = g_cph_app_db.profile_id;
+            break;
+        case CPH_DB_PARAM_ACTIVE_PORT:
+            *(MV_APP_GMAC_PORT_E *)value = g_cph_app_db.active_port;
+            break;
+        case CPH_DB_PARAM_APP_SUPPORT:
+            *(BOOL *)value = g_cph_app_db.app_support;
+            break;            
+        case CPH_DB_PARAM_IGMP_SUPPORT:
+            *(BOOL *)value = g_cph_app_db.igmp_support;
+            break;
+        case CPH_DB_PARAM_BC_SUPPORT:
+            *(BOOL *)value = g_cph_app_db.bc_support;
+            break;
+        case CPH_DB_PARAM_FLOW_SUPPORT:
+            *(BOOL *)value = g_cph_app_db.flow_support;
+            break;
+        case CPH_DB_PARAM_UDP_SUPPORT:
+            *(BOOL *)value = g_cph_app_db.udp_support;
+            break;
+        case CPH_DB_PARAM_BC_COUNTER:
+            *(UINT32 *)value = g_cph_app_db.bc_count;
+            break;            
+        default:
+            break;
+    }
+    return rc;
+}
+
+/******************************************************************************
+* cph_db_compare_rules()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Compare the parse_bm and parse_key of two rules
+*
+* INPUTS:
+*       parse_bm_1   - Parsing bitmap of first CPH rule
+*       parse_key_1  - Parsing key of first CPH rule
+*       parse_bm_2   - Parsing bitmap of second CPH rule
+*       parse_key_2  - Parsing key of second CPH rule
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       In case same, return TRUE, 
+*       In case different, return FALSE.
+*******************************************************************************/
+BOOL cph_db_compare_rules (
+    CPH_APP_PARSE_FIELD_E parse_bm_1, 
+    CPH_APP_PARSE_T      *parse_key_1, 
+    CPH_APP_PARSE_FIELD_E parse_bm_2,
+    CPH_APP_PARSE_T      *parse_key_2)
+{
+    if (parse_bm_1 == parse_bm_2)
+    {
+        /* Compare direction */
+        if (parse_bm_1 & CPH_APP_PARSE_FIELD_DIR)
+        {
+            if (parse_key_1->dir != parse_key_2->dir)
+            {
+                MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Old dir[%d], new dir[%d] \n", parse_key_1->dir, parse_key_2->dir);
+                return FALSE;
+            }
+        }
+
+        /* Compare RX direction */
+        if (parse_bm_1 & CPH_APP_PARSE_FIELD_RX_TX)
+        {
+            if (parse_key_1->rx_tx != parse_key_2->rx_tx)
+            {
+                MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Old rx_tx[%d], new rx_tx[%d] \n", parse_key_1->rx_tx, parse_key_2->rx_tx);
+                return FALSE;
+            }
+        }
+        
+        /* Compare Marvell header */
+        if (parse_bm_1 & CPH_APP_PARSE_FIELD_MH)
+        {
+            if (parse_key_1->mh != parse_key_2->mh)
+            {
+                MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Old mh[%d], new mh[%d] \n", parse_key_1->mh, parse_key_2->mh);
+                return FALSE;
+            }
+        }
+
+        /* Compare Eth type */
+        if (parse_bm_1 & CPH_APP_PARSE_FIELD_ETH_TYPE)
+        {
+            if (parse_key_1->eth_type!= parse_key_2->eth_type)
+            {
+                MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Old eth_type[%d], new eth_type[%d] \n", parse_key_1->eth_type, parse_key_2->eth_type);
+                return FALSE;
+            }
+        }
+        
+        /* Compare Eth subtype */
+        if (parse_bm_1 & CPH_APP_PARSE_FIELD_ETH_SUBTYPE)
+        {
+            if (parse_key_1->eth_subtype != parse_key_2->eth_subtype)
+            {
+                MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Old eth_subtype[%d], new eth_subtype[%d] \n", parse_key_1->eth_subtype, parse_key_2->eth_subtype);
+                return FALSE;
+            }
+        }
+
+        /* Compare IPV4 type */
+        if (parse_bm_1 & CPH_APP_PARSE_FIELD_IPV4_TYPE)
+        {
+            if (parse_key_1->ipv4_type != parse_key_2->ipv4_type)
+            {
+                MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Old ipv4_type[%d], new ipv4_type[%d] \n", parse_key_1->ipv4_type, parse_key_2->ipv4_type);
+                return FALSE;
+            }
+        }
+
+        /* Compare IPV6 type */
+        if (parse_bm_1 & CPH_APP_PARSE_FIELD_IPV6_NH1)
+        {
+            if (parse_key_1->ipv6_nh1 != parse_key_2->ipv6_nh1)
+            {
+                MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Old ipv6_nh1[%d], new ipv6_nh1[%d] \n", parse_key_1->ipv6_nh1, parse_key_2->ipv6_nh1);
+                return FALSE;
+            }
+        }
+
+        /* Compare IPv6 NH */
+        if (parse_bm_1 & CPH_APP_PARSE_FIELD_IPV6_NH2)
+        {
+            if (parse_key_1->ipv6_nh2!= parse_key_2->ipv6_nh2)
+            {
+                MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Old ipv6_nh2[%d], new ipv6_nh2[%d] \n", parse_key_1->ipv6_nh2, parse_key_2->ipv6_nh2);
+                return FALSE;
+            }
+        }
+
+        /* Compare ICMPv6 type */
+        if (parse_bm_1 & CPH_APP_PARSE_FIELD_ICMPV6_TYPE)
+        {
+            if (parse_key_1->icmpv6_type != parse_key_2->icmpv6_type)
+            {
+                MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Old icmpv6_type[%d], new icmpv6_type[%d] \n", parse_key_1->icmpv6_type, parse_key_2->icmpv6_type);
+                return FALSE;
+            }
+        }
+
+        return TRUE;
+    }
+    else
+    {
+        return FALSE;
+    }
+}
+
+/******************************************************************************
+* cph_db_compare_rule_and_packet()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Compare the parse_bm and parse_key of CPH and the packet
+*
+* INPUTS:
+*       parse_bm_rule     - Parsing bitmap of CPH rule
+*       parse_key_rule    - Parsing key of CPH rule
+*       parse_bm_packet   - Parsing bitmap of packet
+*       parse_key_packet  - Parsing key of packet
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       In case same, return TRUE, 
+*       In case different, return FALSE.
+*******************************************************************************/
+BOOL cph_db_compare_rule_and_packet (
+    CPH_APP_PARSE_FIELD_E parse_bm_rule,
+    CPH_APP_PARSE_T      *parse_key_rule,
+    CPH_APP_PARSE_FIELD_E parse_bm_packet,
+    CPH_APP_PARSE_T      *parse_key_packet)
+{  
+    /* Check direction */
+    if (parse_bm_rule & CPH_APP_PARSE_FIELD_DIR)
+    {
+        if (parse_key_rule->dir != CPH_DIR_NOT_CARE)
+        {
+            if (! (parse_bm_packet & CPH_APP_PARSE_FIELD_DIR))
+            {
+                MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Packet has no dir field\n");
+                return FALSE;
+            }
+            
+            if (parse_key_rule->dir != parse_key_packet->dir)
+            {
+                MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Packet dir[%d] is different with rule dir[%d], mis-mathced!\n", parse_key_packet->dir, parse_key_rule->dir);
+                return FALSE;
+            }
+        }
+    }
+
+    /* Check RX/TX direction */
+    if (parse_bm_rule & CPH_APP_PARSE_FIELD_RX_TX)
+    {
+        if (parse_key_rule->rx_tx != CPH_RX_TX_NOT_CARE)
+        {
+            if (! (parse_bm_packet & CPH_APP_PARSE_FIELD_RX_TX))
+            {
+                MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Packet has no dir RX/TX field\n");
+                return FALSE;
+            }
+            
+            if (parse_key_rule->rx_tx != parse_key_packet->rx_tx)
+            {
+                MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Packet rx_tx[%d] is different with rule rx_tx[%d], mis-mathced!\n", parse_key_packet->rx_tx, parse_key_rule->rx_tx);
+                return FALSE;
+            }
+        }
+    }
+
+    /* Check Marvell header */
+    if (parse_bm_rule & CPH_APP_PARSE_FIELD_MH)
+    {
+        if (! (parse_bm_packet & CPH_APP_PARSE_FIELD_MH))
+        {
+            MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Packet has no MH field\n");
+            return FALSE;
+        }
+        
+        if (parse_key_rule->mh != parse_key_packet->mh)
+        {
+            MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Packet mh[0x%x] is different with rule mh[0x%x], mis-mathced!\n", parse_key_packet->mh, parse_key_rule->mh);
+            return FALSE;
+        }
+    }
+
+    /* Check Eth type */
+    if (parse_bm_rule & CPH_APP_PARSE_FIELD_ETH_TYPE)
+    {
+        if (! (parse_bm_packet & CPH_APP_PARSE_FIELD_ETH_TYPE))
+        {
+            MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Packet has no eth_type field\n");
+            return FALSE;
+        }
+        
+        if (parse_key_rule->eth_type != parse_key_packet->eth_type)
+        {
+            MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Packet eth_type[0x%x] is different with rule eth_type[0x%x], mis-mathced!\n", parse_key_packet->eth_type, parse_key_rule->eth_type);
+            return FALSE;
+        }
+    }
+
+    /* Check Eth subtype */
+    if (parse_bm_rule & CPH_APP_PARSE_FIELD_ETH_SUBTYPE)
+    {
+        if (! (parse_bm_packet & CPH_APP_PARSE_FIELD_ETH_SUBTYPE))
+        {
+            MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Packet has no eth_subtype field\n");
+            return FALSE;
+        }
+        
+        if (parse_key_rule->eth_subtype != parse_key_packet->eth_subtype)
+        {
+            MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Packet eth_subtype[%d] is different with rule eth_subtype[%d], mis-mathced!\n", parse_key_packet->eth_subtype, parse_key_rule->eth_subtype);
+            return FALSE;
+        }
+    }
+
+    /* Check IPV4 type */
+    if (parse_bm_rule & CPH_APP_PARSE_FIELD_IPV4_TYPE)
+    {
+        if (! (parse_bm_packet & CPH_APP_PARSE_FIELD_IPV4_TYPE))
+        {
+            MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Packet has no ipv4_type field\n");
+            return FALSE;
+        }
+        
+        if (parse_key_rule->ipv4_type != parse_key_packet->ipv4_type)
+        {
+            MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Packet ipv4_type[%d] is different with rule ipv4_type[%d], mis-mathced!\n", parse_key_packet->ipv4_type, parse_key_rule->ipv4_type);
+            return FALSE;
+        }
+    }
+
+    /* Check IPV6 NH1 */
+    if (parse_bm_rule & CPH_APP_PARSE_FIELD_IPV6_NH1)
+    {
+        if (! (parse_bm_packet & CPH_APP_PARSE_FIELD_IPV6_NH1))
+        {
+            MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Packet has no ipv6_nh1 field\n");
+            return FALSE;
+        }
+        
+        if (parse_key_rule->ipv6_nh1 != parse_key_packet->ipv6_nh1)
+        {
+            MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Packet ipv6_nh1[%d] is different with rule ipv6_nh1[%d], mis-mathced!\n", parse_key_packet->ipv6_nh1, parse_key_rule->ipv6_nh1);
+            return FALSE;
+        }
+    }
+
+    /* Check IPv6 NH2 */
+    if (parse_bm_rule & CPH_APP_PARSE_FIELD_IPV6_NH2)
+    {
+        if (! (parse_bm_packet & CPH_APP_PARSE_FIELD_IPV6_NH2))
+        {
+            MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Packet has no ipv6_nh2 field\n");
+            return FALSE;
+        }
+        
+        if (parse_key_rule->ipv6_nh2 != parse_key_packet->ipv6_nh2)
+        {
+            MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Packet ipv6_nh2[%d] is different with rule ipv6_nh2[%d], mis-mathced!\n", parse_key_packet->ipv6_nh2, parse_key_rule->ipv6_nh2);
+            return FALSE;
+        }
+    }
+
+    /* Check ICMPv6 type */
+    if (parse_bm_rule & CPH_APP_PARSE_FIELD_ICMPV6_TYPE)
+    {
+        if (! (parse_bm_packet & CPH_APP_PARSE_FIELD_ICMPV6_TYPE))
+        {
+            MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Packet has no icmpv6_type field\n");
+            return FALSE;
+        }
+        
+        if (parse_key_rule->icmpv6_type != parse_key_packet->icmpv6_type)
+        {
+            MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Packet icmpv6_type[%d] is different with rule icmpv6_type[%d], mis-mathced!\n", parse_key_packet->icmpv6_type, parse_key_rule->icmpv6_type);
+            return FALSE;
+        }
+    }
+
+    return TRUE;
+}
+
+/******************************************************************************
+* cph_db_check_duplicate_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Check whether there is duplicate CPH rule w/ same parse bitmap
+*              value
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       In case has duplicated rule, return TRUE, 
+*       In case has not duplicated rule, return FALSE.
+*******************************************************************************/
+BOOL cph_db_check_duplicate_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key)
+{
+    UINT32           idx         = 0;
+    UINT32           rule_idx    = 0;
+    CPH_APP_RULE_T  *p_cph_rule  = NULL;
+    BOOL             rc          = FALSE;
+   
+    for (idx = 0, rule_idx = 0; (idx < CPH_APP_MAX_RULE_NUM) && (rule_idx < g_cph_app_db.rule_num); idx++)
+    {
+        p_cph_rule = &g_cph_app_db.cph_rule[idx];
+        
+        /* Compare parse_bm and parse_key */
+        if (p_cph_rule->valid == TRUE)
+        {
+            rule_idx++;
+
+            rc = cph_db_compare_rules(p_cph_rule->parse_bm, &p_cph_rule->parse_key, parse_bm, parse_key);
+            if (rc == TRUE)
+            {
+                return rc;
+            }
+        }
+    }
+
+    return rc;
+}
+
+/******************************************************************************
+* cph_db_add_app_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Add application type CPH rule to data base
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_add_app_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key,
+    CPH_APP_MOD_FIELD_E   mod_bm,
+    CPH_APP_MOD_T        *mod_value,
+    CPH_APP_FRWD_FIELD_E  frwd_bm, 
+    CPH_APP_FRWD_T       *frwd_value)
+{
+    UINT32           idx        = 0;
+    CPH_APP_RULE_T  *p_cph_rule = NULL;
+    BOOL             rc         = TRUE;
+    unsigned long    flags;
+    
+    spin_lock_irqsave(&g_cph_app_db.app_lock, flags);
+    /* Seach for an free entry */
+    for (idx = 0; idx < CPH_APP_MAX_RULE_NUM; idx++)
+    {
+        if (g_cph_app_db.cph_rule[idx].valid == FALSE)
+            break;
+    }
+
+    /* No free entry */
+    if (idx == CPH_APP_MAX_RULE_NUM)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "No free CPH entry \n");
+        spin_unlock_irqrestore(&g_cph_app_db.app_lock, flags);
+        return MV_FULL;
+    }
+
+    /* Do not add new rule if there is already duplicated rule */
+    rc = cph_db_check_duplicate_rule(parse_bm, parse_key);
+    if (rc == TRUE)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "Already has duplicated rule, could not add new CPH rule \n");
+        spin_unlock_irqrestore(&g_cph_app_db.app_lock, flags);
+        return MV_ERROR;
+    }        
+    
+    /* Save CPH rule for application packet */
+    p_cph_rule = &g_cph_app_db.cph_rule[idx];
+    p_cph_rule->parse_bm = parse_bm;
+    memcpy(&p_cph_rule->parse_key,  parse_key,  sizeof(CPH_APP_PARSE_T));
+    p_cph_rule->mod_bm   = mod_bm;
+    memcpy(&p_cph_rule->mod_value,  mod_value,  sizeof(CPH_APP_MOD_T));
+    p_cph_rule->frwd_bm  = frwd_bm;
+    memcpy(&p_cph_rule->frwd_value, frwd_value, sizeof(CPH_APP_FRWD_T));
+    p_cph_rule->valid    = TRUE;
+    g_cph_app_db.rule_num ++;
+
+    spin_unlock_irqrestore(&g_cph_app_db.app_lock, flags);
+    return MV_OK;
+}
+
+/******************************************************************************
+* cph_db_del_app_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Delete application type CPH rule from data base
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_del_app_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key)
+{
+    UINT32           idx         = 0;
+    UINT32           rule_idx    = 0;
+    CPH_APP_RULE_T  *p_cph_rule  = NULL;
+    BOOL             rc          = FALSE;
+    unsigned long    flags;
+
+    spin_lock_irqsave(&g_cph_app_db.app_lock, flags);
+    for (idx = 0, rule_idx = 0; (idx < CPH_APP_MAX_RULE_NUM) && (rule_idx < g_cph_app_db.rule_num); idx++)
+    {
+        p_cph_rule = &g_cph_app_db.cph_rule[idx];
+        
+        /* Compare parse_bm and parse_key */
+        if (p_cph_rule->valid == TRUE)
+        {
+            rule_idx++;
+
+            rc = cph_db_compare_rules(p_cph_rule->parse_bm, &p_cph_rule->parse_key, parse_bm, parse_key);
+            if (rc == TRUE)
+            {
+                memset(p_cph_rule, 0, sizeof(CPH_APP_RULE_T));
+                p_cph_rule->valid = FALSE;
+                g_cph_app_db.rule_num --;
+                
+                spin_unlock_irqrestore(&g_cph_app_db.app_lock, flags);
+                return MV_OK;
+            }
+        }
+    }
+    spin_unlock_irqrestore(&g_cph_app_db.app_lock, flags);
+
+    return MV_OK;
+}
+
+/******************************************************************************
+* cph_db_update_app_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Update application type CPH rule from data base 
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_update_app_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key,  
+    CPH_APP_MOD_FIELD_E   mod_bm, 
+    CPH_APP_MOD_T        *mod_value, 
+    CPH_APP_FRWD_FIELD_E  frwd_bm, 
+    CPH_APP_FRWD_T       *frwd_value)
+{
+    UINT32           idx         = 0;
+    UINT32           rule_idx    = 0;
+    CPH_APP_RULE_T  *p_cph_rule  = NULL;
+    BOOL             rc          = FALSE;
+    unsigned long    flags;
+
+    spin_lock_irqsave(&g_cph_app_db.app_lock, flags);   
+    for (idx = 0, rule_idx = 0; (idx < CPH_APP_MAX_RULE_NUM) && (rule_idx < g_cph_app_db.rule_num); idx++)
+    {
+        p_cph_rule = &g_cph_app_db.cph_rule[idx];
+        
+        /* Compare parse_bm and parse_key */
+        if (p_cph_rule->valid == TRUE)
+        {
+            rule_idx++;
+
+            rc = cph_db_compare_rules(p_cph_rule->parse_bm, &p_cph_rule->parse_key, parse_bm, parse_key);
+            if (rc == TRUE)
+            {
+                p_cph_rule->mod_bm   = mod_bm;
+                memcpy(&p_cph_rule->mod_value,  mod_value,  sizeof(CPH_APP_MOD_T));
+                p_cph_rule->frwd_bm  = frwd_bm;
+                memcpy(&p_cph_rule->frwd_value, frwd_value, sizeof(CPH_APP_FRWD_T));
+
+                spin_unlock_irqrestore(&g_cph_app_db.app_lock, flags);
+                return MV_OK;
+            }
+        }
+    }
+    spin_unlock_irqrestore(&g_cph_app_db.app_lock, flags);
+
+    return MV_OK;
+}
+
+/******************************************************************************
+* cph_db_get_app_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Get application type CPH rule from data base 
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*
+* OUTPUTS:
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_get_app_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key,  
+    CPH_APP_MOD_FIELD_E  *mod_bm, 
+    CPH_APP_MOD_T        *mod_value, 
+    CPH_APP_FRWD_FIELD_E *frwd_bm, 
+    CPH_APP_FRWD_T       *frwd_value)
+{
+    UINT32           idx         = 0;
+    UINT32           rule_idx    = 0;
+    CPH_APP_RULE_T  *p_cph_rule  = NULL;
+    BOOL             rc          = FALSE;
+    unsigned long    flags;
+
+    spin_lock_irqsave(&g_cph_app_db.app_lock, flags);   
+    for (idx = 0, rule_idx = 0; (idx < CPH_APP_MAX_RULE_NUM) && (rule_idx < g_cph_app_db.rule_num); idx++)
+    {
+        p_cph_rule = &g_cph_app_db.cph_rule[idx];
+        
+        /* Compare parse_bm and parse_key */
+        if (p_cph_rule->valid == TRUE)
+        {
+            rule_idx++;
+
+            rc = cph_db_compare_rule_and_packet(p_cph_rule->parse_bm, &p_cph_rule->parse_key, parse_bm, parse_key);
+            if (rc == TRUE)
+            {   
+                if (p_cph_rule->mod_value.state == TRUE) 
+                {
+                    *mod_bm  = p_cph_rule->mod_bm;
+                    memcpy(mod_value, &p_cph_rule->mod_value, sizeof(CPH_APP_MOD_T));
+                    *frwd_bm = p_cph_rule->frwd_bm;
+                    memcpy(frwd_value, &p_cph_rule->frwd_value, sizeof(CPH_APP_FRWD_T));
+
+                    spin_unlock_irqrestore(&g_cph_app_db.app_lock, flags);
+                    return MV_OK;
+               }
+            }
+        }
+    }
+    spin_unlock_irqrestore(&g_cph_app_db.app_lock, flags);
+
+    return MV_FAIL;
+}
+
+/******************************************************************************
+* cph_db_get_app_rule_by_dir_proto()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Get application type CPH rule from data base by protocol type
+*
+* INPUTS:
+*       dir        - Direction
+*       proto_type - SKB protocol type
+*
+* OUTPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_get_app_rule_by_dir_proto (
+    CPH_DIR_E              dir,
+    UINT16                 proto_type,
+    CPH_APP_PARSE_FIELD_E *parse_bm,
+    CPH_APP_PARSE_T       *parse_key,    
+    CPH_APP_MOD_FIELD_E   *mod_bm, 
+    CPH_APP_MOD_T         *mod_value, 
+    CPH_APP_FRWD_FIELD_E  *frwd_bm, 
+    CPH_APP_FRWD_T        *frwd_value)
+{
+    UINT32           idx         = 0;
+    UINT32           rule_idx    = 0;
+    CPH_APP_RULE_T  *p_cph_rule  = NULL;
+    unsigned long    flags;
+
+    spin_lock_irqsave(&g_cph_app_db.app_lock, flags);   
+    for (idx = 0, rule_idx = 0; (idx < CPH_APP_MAX_RULE_NUM) && (rule_idx < g_cph_app_db.rule_num); idx++)
+    {
+        p_cph_rule = &g_cph_app_db.cph_rule[idx];
+        
+        /* Compare parse_bm and parse_key */
+        if (p_cph_rule->valid == TRUE)
+        {
+            rule_idx++;
+
+            if ((p_cph_rule->mod_bm & CPH_APP_RX_MOD_REPLACE_PROTO_TYPE) &&
+                (p_cph_rule->mod_value.proto_type == proto_type) &&
+                (p_cph_rule->mod_value.state      == TRUE))
+            {
+                if ((p_cph_rule->parse_bm & CPH_APP_PARSE_FIELD_DIR) &&
+                    ((p_cph_rule->parse_key.dir == CPH_DIR_NOT_CARE)||(p_cph_rule->parse_key.dir == dir)) &&
+                     (p_cph_rule->parse_bm & CPH_APP_PARSE_FIELD_RX_TX) &&
+                     (p_cph_rule->parse_key.rx_tx == CPH_DIR_TX))
+                {
+                    *parse_bm = p_cph_rule->parse_bm;
+                    memcpy(parse_key, &p_cph_rule->parse_key, sizeof(CPH_APP_PARSE_T));
+                    *mod_bm   = p_cph_rule->mod_bm;
+                    memcpy(mod_value, &p_cph_rule->mod_value, sizeof(CPH_APP_MOD_T));
+                    *frwd_bm  = p_cph_rule->frwd_bm;
+                    memcpy(frwd_value, &p_cph_rule->frwd_value, sizeof(CPH_APP_FRWD_T));
+
+                    spin_unlock_irqrestore(&g_cph_app_db.app_lock, flags);
+                    return MV_OK;
+                }
+            }
+        }
+    }
+    spin_unlock_irqrestore(&g_cph_app_db.app_lock, flags);
+
+    return MV_FAIL;
+}
+
+/******************************************************************************
+* cph_db_increase_counter()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Increase RX counter
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_increase_counter (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key)
+{
+    UINT32           idx         = 0;
+    UINT32           rule_idx    = 0;
+    CPH_APP_RULE_T  *p_cph_rule  = NULL;
+    BOOL             rc          = MV_FAIL;
+    unsigned long    flags;
+
+    spin_lock_irqsave(&g_cph_app_db.app_lock, flags);   
+    for (idx = 0, rule_idx = 0; (idx < CPH_APP_MAX_RULE_NUM) && (rule_idx < g_cph_app_db.rule_num); idx++)
+    {
+        p_cph_rule = &g_cph_app_db.cph_rule[idx];
+        
+        /* Compare parse_bm and parse_key */
+        if (p_cph_rule->valid == TRUE)
+        {
+            rule_idx++;
+
+            rc = cph_db_compare_rule_and_packet(p_cph_rule->parse_bm, &p_cph_rule->parse_key, parse_bm, parse_key);
+            if (rc == TRUE)
+            {   
+                if (p_cph_rule->mod_value.state == TRUE) 
+                {
+                    p_cph_rule->count++;
+
+                    spin_unlock_irqrestore(&g_cph_app_db.app_lock, flags);
+                    return MV_OK;
+               }
+            }
+        }
+    }
+    spin_unlock_irqrestore(&g_cph_app_db.app_lock, flags);
+
+    return rc;
+}
+
+/******************************************************************************
+* cph_db_increase_counter_by_dir_proto()
+* _____________________________________________________________________________
+*
+* DESCRIPTION:  Increase RX counter according to protocol type
+*
+* INPUTS:
+*       dir        - Direction
+*       proto_type - SKB protocol type
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_increase_counter_by_dir_proto (CPH_DIR_E dir,
+    UINT16    proto_type)
+{
+    UINT32           idx         = 0;
+    UINT32           rule_idx    = 0;
+    CPH_APP_RULE_T  *p_cph_rule  = NULL;
+    unsigned long    flags;
+
+    spin_lock_irqsave(&g_cph_app_db.app_lock, flags);   
+    for (idx = 0, rule_idx = 0; (idx < CPH_APP_MAX_RULE_NUM) && (rule_idx < g_cph_app_db.rule_num); idx++)
+    {
+        p_cph_rule = &g_cph_app_db.cph_rule[idx];
+        
+        /* Compare dir and protocol type */
+        if (p_cph_rule->valid == TRUE)
+        {
+            rule_idx++;
+
+            if ((p_cph_rule->mod_bm & CPH_APP_RX_MOD_REPLACE_PROTO_TYPE) &&
+                (p_cph_rule->mod_value.proto_type == proto_type) &&
+                (p_cph_rule->mod_value.state      == TRUE))
+            {
+               if ((p_cph_rule->parse_bm & CPH_APP_PARSE_FIELD_DIR) &&
+                    ((p_cph_rule->parse_key.dir == CPH_DIR_NOT_CARE) || 
+                     (p_cph_rule->parse_key.dir == dir)) &&
+                     (p_cph_rule->parse_bm & CPH_APP_PARSE_FIELD_RX_TX) &&
+                     (p_cph_rule->parse_key.rx_tx == CPH_DIR_TX))
+                {
+                    p_cph_rule->count++;
+
+                    spin_unlock_irqrestore(&g_cph_app_db.app_lock, flags);
+                    return MV_OK;
+                }            
+            }
+        }
+    }
+    spin_unlock_irqrestore(&g_cph_app_db.app_lock, flags);
+
+    return MV_OK;
+
+}
+
+/******************************************************************************
+* cph_db_get_xml_param()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Get the XML parameter
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_get_xml_param(VOID)
+{
+    ezxml_t     xml_head;
+    ezxml_t     xml_element;
+    ezxml_t     xml_attr;
+    UINT32      attr_value = 0;
+    BOOL        bool_value = TRUE;
+    const CHAR *name       = NULL;
+    const CHAR *value      = NULL;  
+    INT32       rc         = MV_OK;
+
+    /* Read ONU profile XML file */
+    if ((xml_head = ezxml_parse_file(g_onu_profile_xml_cfg_file)) == 0)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "%s: ezxml_parse_file() of %s failed", __FUNCTION__, g_onu_profile_xml_cfg_file);
+        rc = MV_NOT_FOUND;        
+        goto free_exit;
+    }
+
+    if ((xml_element = ezxml_get(xml_head, XML_PROFILE_ELM_CAPABILITY, -1)) == 0)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "%s: ezxml_get() of %s failed\n", __FUNCTION__, XML_PROFILE_ELM_CAPABILITY);
+        rc = MV_FAIL;        
+        goto free_exit;
+
+    }
+    
+    for (xml_attr = ezxml_child(xml_element, XML_PROFILE_ELM_ATTRIB); xml_attr; xml_attr = xml_attr->next)
+    { 
+        /* Parse attribute type and value */
+        name = ezxml_attr(xml_attr, XML_PROFILE_ATTR_NAME);
+        if (name == NULL)
+        {
+            MV_CPH_PRINT(CPH_ERR_LEVEL, "%s: ezxml_attr() of %s failed\n", __FUNCTION__, XML_PROFILE_ATTR_NAME);
+            rc = MV_NOT_FOUND;        
+            goto free_exit;
+        }
+
+        if (strcmp(name, XML_PROFILE_NAME_PROFILE) == 0)
+        {
+            value = ezxml_attr(xml_attr, XML_PROFILE_ATTR_VALUE);
+            if (value == NULL)
+            {
+                MV_CPH_PRINT(CPH_ERR_LEVEL, "%s: ezxml_attr() of %s failed\n", __FUNCTION__, XML_PROFILE_ATTR_VALUE);
+                rc = MV_NOT_FOUND;        
+                goto free_exit;
+            }
+            attr_value = mtype_get_digit_num(value);
+
+            cph_db_set_param(CPH_DB_PARAM_PROFILE_ID, &attr_value);
+        } 
+        else if (strcmp(name, XML_PROFILE_NAME_ACTIVE_PORT) == 0)
+        {
+            value = ezxml_attr(xml_attr, XML_PROFILE_ATTR_VALUE);
+            if (value == NULL)
+            {
+                MV_CPH_PRINT(CPH_ERR_LEVEL, "%s: ezxml_attr() of %s failed\n", __FUNCTION__, XML_PROFILE_ATTR_VALUE);
+                rc = MV_NOT_FOUND;        
+                goto free_exit;
+            }
+            attr_value = mtype_get_digit_num(value);
+
+            cph_db_set_param(CPH_DB_PARAM_ACTIVE_PORT, &attr_value);
+            break;
+        }
+    }
+    ezxml_free(xml_head);
+
+    /* Read CPH XML file */
+    if ((xml_head = ezxml_parse_file(g_cph_xml_cfg_file)) == 0)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "%s: ezxml_parse_file() of %s failed", __FUNCTION__, g_cph_xml_cfg_file);
+        rc = MV_NOT_FOUND;        
+        goto free_exit;
+    }
+
+    /* Read application support flag*/
+    if ((xml_element = ezxml_get(xml_head, XML_CPH_ELM_APP_SUPPORT, -1)) == 0)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "%s: ezxml_get() of %s failed\n", __FUNCTION__, XML_CPH_ELM_APP_SUPPORT);
+        rc = MV_FAIL;        
+        goto free_exit;
+    }
+    if (xml_element->txt == NULL)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "%s: xml_element->txt == NULL \n", __FUNCTION__);
+        rc = MV_FAIL;        
+        goto free_exit;
+    }
+    attr_value = mtype_get_digit_num(xml_element->txt);
+    bool_value = (attr_value != 0)? TRUE:FALSE;
+    cph_db_set_param(CPH_DB_PARAM_APP_SUPPORT, &bool_value);
+
+    /* Read IGMP support flag*/
+    if ((xml_element = ezxml_get(xml_head, XML_CPH_ELM_IGMP_SUPPORT, -1)) == 0)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "%s: ezxml_get() of %s failed\n", __FUNCTION__, XML_CPH_ELM_IGMP_SUPPORT);
+        rc = MV_FAIL;        
+        goto free_exit;
+    }
+    if (xml_element->txt == NULL)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "%s: xml_element->txt == NULL \n", __FUNCTION__);
+        rc = MV_FAIL;        
+        goto free_exit;
+    }
+    attr_value = mtype_get_digit_num(xml_element->txt);
+    bool_value = (attr_value != 0)? TRUE:FALSE;
+    cph_db_set_param(CPH_DB_PARAM_IGMP_SUPPORT, &bool_value);
+
+    /* Read BC support flag*/
+    if ((xml_element = ezxml_get(xml_head, XML_CPH_ELM_BC_SUPPORT, -1)) == 0)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "%s: ezxml_get() of %s failed\n", __FUNCTION__, XML_CPH_ELM_BC_SUPPORT);
+        rc = MV_FAIL;        
+        goto free_exit;
+    }
+    if (xml_element->txt == NULL)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "%s: xml_element->txt == NULL \n", __FUNCTION__);
+        rc = MV_FAIL;        
+        goto free_exit;
+    }
+    attr_value = mtype_get_digit_num(xml_element->txt);
+    bool_value = (attr_value != 0)? TRUE:FALSE;
+    cph_db_set_param(CPH_DB_PARAM_BC_SUPPORT, &bool_value);
+
+    /* Read Data flow mapping support flag*/
+    if ((xml_element = ezxml_get(xml_head, XML_CPH_ELM_FLOW_SUPPORT, -1)) == 0)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "%s: ezxml_get() of %s failed\n", __FUNCTION__, XML_CPH_ELM_FLOW_SUPPORT);
+        rc = MV_FAIL;        
+        goto free_exit;
+    }
+    if (xml_element->txt == NULL)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "%s: xml_element->txt == NULL \n", __FUNCTION__);
+        rc = MV_FAIL;        
+        goto free_exit;
+    }
+    attr_value = mtype_get_digit_num(xml_element->txt);
+    bool_value = (attr_value != 0)? TRUE:FALSE;
+    cph_db_set_param(CPH_DB_PARAM_FLOW_SUPPORT, &bool_value);
+
+    /* Read UDP port mapping support flag*/
+    if ((xml_element = ezxml_get(xml_head, XML_CPH_ELM_UDP_SUPPORT, -1)) == 0)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "%s: ezxml_get() of %s failed\n", __FUNCTION__, XML_CPH_ELM_UDP_SUPPORT);
+        rc = MV_FAIL;        
+        goto free_exit;
+    }
+    if (xml_element->txt == NULL)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "%s: xml_element->txt == NULL \n", __FUNCTION__);
+        rc = MV_FAIL;        
+        goto free_exit;
+    }
+    attr_value = mtype_get_digit_num(xml_element->txt);
+    bool_value = (attr_value != 0)? TRUE:FALSE;
+    cph_db_set_param(CPH_DB_PARAM_UDP_SUPPORT, &bool_value);
+
+free_exit:
+    ezxml_free(xml_head);
+    return rc;        
+}
+
+/*******************************************************************************
+**
+** cph_db_get_tcont_state
+** ___________________________________________________________________________
+**
+** DESCRIPTION: The function get T-CONT state
+**
+** INPUTS:
+**   tcont - T-CONT
+**
+** OUTPUTS:
+**   None. 
+**
+** RETURNS:     
+**   state - State of T-CONT, enabled or disabled.  
+**
+*******************************************************************************/
+BOOL cph_db_get_tcont_state(UINT32 tcont)
+{
+    /* Check tcont */
+    if (tcont >= MV_TCONT_LLID_NUM)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "tcont[%d] is illegal, should be less than [%d]\n", tcont, MV_TCONT_LLID_NUM); 
+        return FALSE;
+    }
+
+    return g_cph_app_db.tcont_state[tcont];
+}
+
+/*******************************************************************************
+**
+** cph_db_set_tcont_state
+** ___________________________________________________________________________
+**
+** DESCRIPTION: The function sets T-CONT state in mv_cph
+**
+** INPUTS:
+**   tcont - T-CONT
+**   state - State of T-CONT, enabled or disabled.
+**
+** OUTPUTS:
+**   None. 
+**
+** RETURNS:     
+**  On success, the function returns (MV_OK). On error different types are 
+**  returned according to the case.  
+**
+*******************************************************************************/
+MV_STATUS cph_db_set_tcont_state(UINT32 tcont, BOOL state)
+{
+    /* Check tcont */
+    if (tcont >= MV_TCONT_LLID_NUM)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "tcont[%d] is illegal, should be less than [%d]\n", tcont, MV_TCONT_LLID_NUM); 
+        return MV_FAIL;
+    }
+
+    /* Apply t-cont state to mv_cph */
+    g_cph_app_db.tcont_state[tcont] = state;
+
+    return MV_OK;
+}
+
+/******************************************************************************
+* cph_db_display_parse_field()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Display CPH rule parsing field
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_display_parse_field(
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key)
+{
+    printk(KERN_INFO "Parse field(0x%x): ", parse_bm);    
+
+    /* Print direction */
+    if (parse_bm & CPH_APP_PARSE_FIELD_DIR)
+    {
+        printk(KERN_INFO "Dir(%s), ", cph_app_lookup_dir(parse_key->dir));
+    }
+
+    /* Print TX/RX direction */
+    if (parse_bm & CPH_APP_PARSE_FIELD_RX_TX)
+    {
+        printk(KERN_INFO "RX/TX(%s), ", cph_app_lookup_rx_tx(parse_key->dir));
+    }
+
+    /* Print Marvell header */
+    if (parse_bm & CPH_APP_PARSE_FIELD_MH)
+    {
+        printk(KERN_INFO "MH(0x%x), ", parse_key->mh);
+    }
+
+    /* Print Eth type */
+    if (parse_bm & CPH_APP_PARSE_FIELD_ETH_TYPE)
+    {
+        printk(KERN_INFO "Eth type(0x%04x), ", parse_key->eth_type);
+    }
+
+    /* Print Eth subtype */
+    if (parse_bm & CPH_APP_PARSE_FIELD_ETH_SUBTYPE)
+    {
+        printk(KERN_INFO "Eth subtype(%d), ", parse_key->eth_subtype);
+    }
+
+    /* Print IPV4 type */
+    if (parse_bm & CPH_APP_PARSE_FIELD_IPV4_TYPE)
+    {
+        printk(KERN_INFO "IPv4 type(%d), ", parse_key->ipv4_type);
+    }
+
+    /* Print IPv6 NH1 */
+    if (parse_bm & CPH_APP_PARSE_FIELD_IPV6_NH1)
+    {
+        printk(KERN_INFO "IPv6 NH1(%d), ", parse_key->ipv6_nh1);
+    }
+
+    /* Print IPv6 NH2 */
+    if (parse_bm & CPH_APP_PARSE_FIELD_IPV6_NH2)
+    {
+        printk(KERN_INFO "IPv6 NH2(%d), ", parse_key->ipv6_nh2);
+    }
+
+    /* Print ICMPv6 type */
+    if (parse_bm & CPH_APP_PARSE_FIELD_ICMPV6_TYPE)
+    {
+        printk(KERN_INFO "ICMPv6 type(%d)", parse_key->icmpv6_type);
+    }
+
+    printk(KERN_INFO "\n");
+
+    return MV_OK;
+}
+
+/******************************************************************************
+* cph_db_display_mod_field()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Display CPH rule modification field
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_display_mod_field(
+    CPH_APP_MOD_FIELD_E   mod_bm,
+    CPH_APP_MOD_T        *mod_value)
+{
+    printk(KERN_INFO "Mod field(0x%x): ", mod_bm);    
+
+    /* Print protocol type */
+    if (mod_bm & CPH_APP_RX_MOD_REPLACE_PROTO_TYPE)
+    {
+        printk(KERN_INFO "Proto type(0x%x), ", mod_value->proto_type);
+    }
+
+    printk(KERN_INFO "state(%s)", (mod_value->state==TRUE)?"Enabled":"Disabled");
+
+    printk(KERN_INFO "\n");  
+
+    return MV_OK;
+}
+
+/******************************************************************************
+* cph_db_display_frwd_field()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Display CPH rule forwarding field
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_display_frwd_field(
+    CPH_APP_FRWD_FIELD_E  frwd_bm, 
+    CPH_APP_FRWD_T       *frwd_value)
+{
+    printk(KERN_INFO "Forward field(0x%x): ", frwd_bm);    
+
+    /* Print target port */
+    if (frwd_bm & CPH_APP_FRWD_SET_TRG_PORT)
+    {
+        printk(KERN_INFO "Target port(%d), ", frwd_value->trg_port);
+    }
+
+    /* Print target queue */
+    if (frwd_bm & CPH_APP_FRWD_SET_TRG_QUEUE)
+    {
+        printk(KERN_INFO "Target queue(%d), ", frwd_value->trg_queue);
+    }
+
+    /* Print GEM port */
+    if (frwd_bm & CPH_APP_FRWD_SET_GEM_PORT)
+    {
+        printk(KERN_INFO "Gem port(%d)", frwd_value->gem_port);
+    }    
+
+
+    printk(KERN_INFO "\n");  
+
+    return MV_OK;
+}
+
+
+/******************************************************************************
+* cph_db_display_all()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Display all CPH rules in data base
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_display_all(VOID)
+{
+    CPH_APP_RULE_T *p_cph_rule = NULL;
+    UINT32          idx        = 0;
+    UINT32          rule_idx   = 0;
+
+    printk(KERN_INFO "CPH Application Data Base, total rule number[%u]\n", g_cph_app_db.rule_num);
+    printk(KERN_INFO "-------------------------------------------------------\n");
+
+    printk(KERN_INFO "TPM complex profile: %s, active WAN: %s\n", 
+           cph_app_lookup_profile_id(g_cph_app_db.profile_id), 
+           cph_app_lookup_gmac(g_cph_app_db.active_port));
+
+    printk(KERN_INFO "Generic application handling suppport: %s\n", 
+       (g_cph_app_db.app_support == TRUE)?"Enabled":"Disabled");
+    
+#ifdef CONFIG_MV_CPH_IGMP_HANDLE
+    printk(KERN_INFO "IGMP/MLD handling suppport: %s\n", 
+           (g_cph_app_db.igmp_support == TRUE)?"Enabled":"Disabled");
+#endif
+
+#ifdef CONFIG_MV_CPH_BC_HANDLE  
+    printk(KERN_INFO "Broadcast handling suppport: %s\n", 
+           (g_cph_app_db.bc_support == TRUE)?"Enabled":"Disabled");
+#endif
+
+#ifdef CONFIG_MV_CPH_FLOW_MAP_HANDLE   
+    printk(KERN_INFO "Data flow mapping/modification suppport: %s\n",     
+           (g_cph_app_db.flow_support == TRUE)?"Enabled":"Disabled");
+#endif
+
+#ifdef CONFIG_MV_CPH_UDP_SAMPLE_HANDLE
+    printk(KERN_INFO "UDP port mapping suppport: %s\n",
+           (g_cph_app_db.udp_support == TRUE)?"Enabled":"Disabled");
+#endif
+
+    printk(KERN_INFO "-------------------------------------------------------\n");
+    
+    printk(KERN_INFO "CPH total rule number: %d\n", g_cph_app_db.rule_num);    
+
+    for (idx = 0, rule_idx = 0; (idx < CPH_APP_MAX_RULE_NUM) && (rule_idx < g_cph_app_db.rule_num); idx++)
+    {
+        p_cph_rule = &g_cph_app_db.cph_rule[idx];
+        if (p_cph_rule->valid == TRUE)
+        {
+            rule_idx++;
+            
+            printk(KERN_INFO "CPH rule: #%d\n", rule_idx);
+            printk(KERN_INFO "-----------------------\n");
+            cph_db_display_parse_field(p_cph_rule->parse_bm, &p_cph_rule->parse_key);
+            cph_db_display_mod_field(p_cph_rule->mod_bm,     &p_cph_rule->mod_value);
+            cph_db_display_frwd_field(p_cph_rule->frwd_bm,   &p_cph_rule->frwd_value);
+            printk(KERN_INFO "Counter: %d\n\n", p_cph_rule->count);
+        }
+    }
+
+    printk(KERN_INFO "Mis-matched or broadcast counter: %d\n", g_cph_app_db.bc_count);    
+
+    printk(KERN_INFO "T-CONT State\n");
+    for (idx = 0; idx < MV_TCONT_LLID_NUM; idx++)
+    {
+        printk(KERN_INFO "T-CONT[%d]: %s\n", idx, (g_cph_app_db.tcont_state[idx] == TRUE)?"TRUE":"FALSE");
+    }
+
+    return MV_OK;
+}
+
+
+/******************************************************************************
+* cph_db_init()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Initialize CPH data base 
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_init(VOID)
+{
+    UINT32    idx = 0;
+    MV_STATUS rc  = MV_OK;
+
+    memset(&g_cph_app_db, 0, sizeof(g_cph_app_db));
+    for (idx = 0; idx < CPH_APP_MAX_RULE_NUM; idx++)
+        g_cph_app_db.cph_rule[idx].valid = FALSE;
+
+    /* Set the default value */
+    g_cph_app_db.profile_id   = TPM_PON_WAN_DUAL_MAC_INT_SWITCH;
+    g_cph_app_db.active_port  = MV_APP_PON_MAC_PORT;
+    g_cph_app_db.app_support  = TRUE;
+    g_cph_app_db.igmp_support = FALSE;
+    g_cph_app_db.bc_support   = FALSE;
+    g_cph_app_db.flow_support = FALSE;
+    g_cph_app_db.udp_support  = FALSE;
+
+    for (idx = 0; idx < MV_TCONT_LLID_NUM; idx++)
+        g_cph_app_db.tcont_state[idx] = FALSE;
+
+    /* Init spin lock */
+    spin_lock_init(&g_cph_app_db.app_lock);
+
+    return rc;
+}
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_db.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_db.h
new file mode 100755
index 0000000..41903de
--- /dev/null
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_db.h
@@ -0,0 +1,532 @@
+/*******************************************************************************
+Copyright (C) Marvell International Ltd. and its affiliates
+
+This software file (the "File") is owned and distributed by Marvell 
+International Ltd. and/or its affiliates ("Marvell") under the following
+alternative licensing terms.  Once you have made an election to distribute the
+File under one of the following license alternatives, please (i) delete this
+introductory statement regarding license alternatives, (ii) delete the two
+license alternatives that you have not elected to use and (iii) preserve the
+Marvell copyright notice above.
+
+********************************************************************************
+Marvell Commercial License Option
+
+If you received this File from Marvell and you have entered into a commercial
+license agreement (a "Commercial License") with Marvell, the File is licensed
+to you under the terms of the applicable Commercial License.
+
+********************************************************************************
+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.
+********************************************************************************
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or 
+modify this File under the following licensing terms. 
+Redistribution and use in source and binary forms, with or without modification, 
+are permitted provided that the following conditions are met:
+
+    *   Redistributions of source code must retain the above copyright notice,
+        this list of conditions and the following disclaimer. 
+
+    *   Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution. 
+
+    *   Neither the name of Marvell nor the names of its contributors may be 
+        used to endorse or promote products derived from this software without 
+        specific prior written permission. 
+    
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+********************************************************************************
+* mv_cph_db.h
+*
+* DESCRIPTION: Marvell CPH(CPH Packet Handler) data base implementation
+*
+* DEPENDENCIES:
+*               None
+*
+* CREATED BY:   VictorGu
+*
+* DATE CREATED: 22Jan2013
+*
+* FILE REVISION NUMBER:
+*               Revision: 1.0
+*
+*
+*******************************************************************************/
+#ifndef _MV_CPH_DB_H_
+#define _MV_CPH_DB_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/******************************************************************************
+ * Data type Definition
+ ******************************************************************************/
+#define US_CPH_XML_CFG_FILE          "/etc/xml_params/cph_xml_cfg_file.xml"
+#define US_ONU_PROFILE_XML_CFG_FILE  "/etc/xml_params/onu_profile_xml_cfg_file.xml"
+
+#define XML_PROFILE_ELM_CAPABILITY   "Capability"
+#define XML_PROFILE_ELM_ATTRIB       "attrib"
+#define XML_PROFILE_ATTR_NAME        "name"
+#define XML_PROFILE_ATTR_VALUE       "value"
+#define XML_PROFILE_NAME_PROFILE     "Complex profile"
+#define XML_PROFILE_NAME_ACTIVE_PORT "Active wan"
+
+#define XML_CPH_ELM_APP_SUPPORT      "app_support"
+#define XML_CPH_ELM_IGMP_SUPPORT     "igmp_support"
+#define XML_CPH_ELM_BC_SUPPORT       "bc_support"
+#define XML_CPH_ELM_FLOW_SUPPORT     "flow_support"
+#define XML_CPH_ELM_UDP_SUPPORT      "udp_support"
+
+
+/* CPH rule definition for application packet handling
+------------------------------------------------------------------------------*/
+typedef struct
+{
+    BOOL                  valid;
+    CPH_APP_PARSE_FIELD_E parse_bm;
+    CPH_APP_PARSE_T       parse_key;
+    CPH_APP_MOD_FIELD_E   mod_bm;
+    CPH_APP_MOD_T         mod_value;
+    CPH_APP_FRWD_FIELD_E  frwd_bm;
+    CPH_APP_FRWD_T        frwd_value;
+    UINT32                count; 
+} CPH_APP_RULE_T;
+
+/* CPH data base for application packet handling
+------------------------------------------------------------------------------*/
+#define CPH_APP_MAX_RULE_NUM  (64)
+typedef struct
+{
+    tpm_eth_complex_profile_t  profile_id;       /* Complex profile ID, see tpm_eth_complex_profile_t  */
+    MV_APP_GMAC_PORT_E         active_port;      /* Current active WAN GE port, see MV_APP_GMAC_PORT_E */
+    BOOL                       app_support;      /* Whether support generic application handling       */
+    BOOL                       igmp_support;     /* Whether support IGMP/MLD packet handling           */
+    BOOL                       bc_support;       /* Whether support U/S broadcast packet handling      */
+    BOOL                       flow_support;     /* Whether support flow mapping handling in CPH       */
+    BOOL                       udp_support;      /* Whether support UDP port mapping in CPH            */
+    UINT32                     rule_num;         /* Current application rule number                    */
+    CPH_APP_RULE_T             cph_rule[CPH_APP_MAX_RULE_NUM]; /* CPH application rules                */
+    spinlock_t                 app_lock;         /* Spin lock for application rule operation           */
+    UINT32                     bc_count;         /* Counter for mis-matched packets, usually is bc     */
+    BOOL                       tcont_state[MV_TCONT_LLID_NUM];/* T-CONT state used to control SWF      */
+} CPH_APP_DB_T;
+
+/* CPH database parameter enum
+------------------------------------------------------------------------------*/
+typedef enum
+{
+    CPH_DB_PARAM_PROFILE_ID = 0,
+    CPH_DB_PARAM_ACTIVE_PORT,
+    CPH_DB_PARAM_APP_SUPPORT,    
+    CPH_DB_PARAM_IGMP_SUPPORT,
+    CPH_DB_PARAM_BC_SUPPORT,
+    CPH_DB_PARAM_FLOW_SUPPORT,
+    CPH_DB_PARAM_UDP_SUPPORT,    
+    CPH_DB_PARAM_BC_COUNTER
+} CPH_DB_PARAM_E;
+
+/******************************************************************************
+ * Function Declaration
+ ******************************************************************************/
+/******************************************************************************
+* cph_db_set_param()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Set CPH DB parameter
+*
+* INPUTS:
+*       param   - The parameter type
+*       value   - Parameter value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_set_param (CPH_DB_PARAM_E param, VOID *value);
+
+/******************************************************************************
+* cph_db_get_param()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Get CPH DB parameter
+*
+* INPUTS:
+*       param   - The parameter type
+*
+* OUTPUTS:
+*       value   - Parameter value
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_get_param (CPH_DB_PARAM_E param, VOID *value);
+
+/******************************************************************************
+* cph_db_add_app_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Add CPH rule
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK.
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_add_app_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm,
+    CPH_APP_PARSE_T      *parse_key,
+    CPH_APP_MOD_FIELD_E   mod_bm,
+    CPH_APP_MOD_T        *mod_value,
+    CPH_APP_FRWD_FIELD_E  frwd_bm,
+    CPH_APP_FRWD_T       *frwd_value);
+
+/******************************************************************************
+* cph_db_del_app_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Deletes CPH rule
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK.
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_del_app_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm,
+    CPH_APP_PARSE_T      *parse_key);
+
+/******************************************************************************
+* cph_db_update_app_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Update CPH rule
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK.
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_update_app_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm,
+    CPH_APP_PARSE_T      *parse_key,
+    CPH_APP_MOD_FIELD_E   mod_bm,
+    CPH_APP_MOD_T        *mod_value,
+    CPH_APP_FRWD_FIELD_E  frwd_bm,
+    CPH_APP_FRWD_T       *frwd_value);
+
+/******************************************************************************
+* cph_db_get_app_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Get CPH rule 
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*
+* OUTPUTS:
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* RETURNS:
+*       On success, the function returns MV_OK.
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_get_app_rule (
+    CPH_APP_PARSE_FIELD_E parse_bm,
+    CPH_APP_PARSE_T      *parse_key,
+    CPH_APP_MOD_FIELD_E  *mod_bm,
+    CPH_APP_MOD_T        *mod_value,
+    CPH_APP_FRWD_FIELD_E *frwd_bm,
+    CPH_APP_FRWD_T       *frwd_value);
+
+/******************************************************************************
+* cph_db_get_app_rule_by_dir_proto()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Get application type CPH rule from data base by protocol type
+*
+* INPUTS:
+*       dir        - Direction
+*       proto_type - SKB protocol type
+*
+* OUTPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*       mod_bm     - Modification bitmap
+*       mod_value  - Modification value
+*       frwd_bm    - Forwarding bitmap
+*       frwd_value - Forwarding value
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_get_app_rule_by_dir_proto (
+    CPH_DIR_E              dir,
+    UINT16                 proto_type,
+    CPH_APP_PARSE_FIELD_E *parse_bm,
+    CPH_APP_PARSE_T       *parse_key,    
+    CPH_APP_MOD_FIELD_E   *mod_bm, 
+    CPH_APP_MOD_T         *mod_value, 
+    CPH_APP_FRWD_FIELD_E  *frwd_bm, 
+    CPH_APP_FRWD_T        *frwd_value);
+
+/******************************************************************************
+* cph_db_increase_counter()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Increase RX counter
+*
+* INPUTS:
+*       parse_bm   - Parsing bitmap
+*       parse_key  - Parsing key
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_increase_counter (
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key);
+
+/******************************************************************************
+* cph_db_increase_counter_by_dir_proto()
+* _____________________________________________________________________________
+*
+* DESCRIPTION:  Increase RX counter according to protocol type
+*
+* INPUTS:
+*       dir        - Direction
+*       proto_type - SKB protocol type
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_increase_counter_by_dir_proto (CPH_DIR_E dir,
+    UINT16    proto_type);
+
+/******************************************************************************
+* cph_db_get_xml_param()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Get the XML parameter
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_get_xml_param(VOID);
+
+/*******************************************************************************
+**
+** cph_db_get_tcont_state
+** ___________________________________________________________________________
+**
+** DESCRIPTION: The function get T-CONT state
+**
+** INPUTS:
+**   tcont - T-CONT
+**
+** OUTPUTS:
+**   None. 
+**
+** RETURNS:     
+**   state - State of T-CONT, enabled or disabled.  
+**
+*******************************************************************************/
+BOOL cph_db_get_tcont_state(UINT32 tcont);
+
+/*******************************************************************************
+**
+** cph_db_set_tcont_state
+** ___________________________________________________________________________
+**
+** DESCRIPTION: The function sets T-CONT state in mv_cph
+**
+** INPUTS:
+**   tcont - T-CONT
+**   state - State of T-CONT, enabled or disabled.
+**
+** OUTPUTS:
+**   None. 
+**
+** RETURNS:     
+**  On success, the function returns (MV_OK). On error different types are 
+**  returned according to the case.  
+**
+*******************************************************************************/
+MV_STATUS cph_db_set_tcont_state(UINT32 tcont, BOOL state);
+
+/******************************************************************************
+* cph_db_display_parse_field()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Display CPH rule parsing field
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_display_parse_field(
+    CPH_APP_PARSE_FIELD_E parse_bm, 
+    CPH_APP_PARSE_T      *parse_key);
+
+/******************************************************************************
+* cph_db_display_mod_field()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Display CPH rule modification field
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_display_mod_field(
+    CPH_APP_MOD_FIELD_E   mod_bm,
+    CPH_APP_MOD_T        *mod_value);
+
+/******************************************************************************
+* cph_db_display_frwd_field()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Display CPH rule forwarding field
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_display_frwd_field(
+    CPH_APP_FRWD_FIELD_E  frwd_bm, 
+    CPH_APP_FRWD_T       *frwd_value);
+
+
+/******************************************************************************
+* cph_db_display_all()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Display CPH data base
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK.
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_display_all(VOID);
+
+/******************************************************************************
+* cph_db_init()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Initialize CPH data base
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK.
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_db_init(VOID);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _MV_CPH_DB_H_ */
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_dev.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_dev.c
new file mode 100755
index 0000000..bc6851e
--- /dev/null
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_dev.c
@@ -0,0 +1,452 @@
+/*******************************************************************************
+Copyright (C) Marvell International Ltd. and its affiliates
+
+This software file (the "File") is owned and distributed by Marvell 
+International Ltd. and/or its affiliates ("Marvell") under the following
+alternative licensing terms.  Once you have made an election to distribute the
+File under one of the following license alternatives, please (i) delete this
+introductory statement regarding license alternatives, (ii) delete the two
+license alternatives that you have not elected to use and (iii) preserve the
+Marvell copyright notice above.
+
+********************************************************************************
+Marvell Commercial License Option
+
+If you received this File from Marvell and you have entered into a commercial
+license agreement (a "Commercial License") with Marvell, the File is licensed
+to you under the terms of the applicable Commercial License.
+
+********************************************************************************
+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.
+********************************************************************************
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or 
+modify this File under the following licensing terms. 
+Redistribution and use in source and binary forms, with or without modification, 
+are permitted provided that the following conditions are met:
+
+    *   Redistributions of source code must retain the above copyright notice,
+        this list of conditions and the following disclaimer. 
+
+    *   Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution. 
+
+    *   Neither the name of Marvell nor the names of its contributors may be 
+        used to endorse or promote products derived from this software without 
+        specific prior written permission. 
+    
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+********************************************************************************
+* mv_cph_dev.c
+*
+* DESCRIPTION: Marvell CPH(CPH Packet Handler) char device definition
+*
+* DEPENDENCIES:
+*               None
+*
+* CREATED BY:   VictorGu
+*
+* DATE CREATED: 22Jan2013
+*
+* FILE REVISION NUMBER:
+*               Revision: 1.0
+*
+*
+*******************************************************************************/
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/delay.h>
+#include <linux/wait.h>
+#include <linux/mutex.h>
+#include <linux/spinlock.h>
+#include <linux/poll.h>
+#include <linux/clk.h>
+#include <linux/fs.h>
+#include <linux/vmalloc.h>
+#include <linux/cdev.h>
+#include <linux/device.h>
+#include <linux/miscdevice.h>
+
+#include "mv_cph_header.h"
+
+
+/* Used to prevent multiple access to device */
+static int               g_cph_device_open = 0;
+static struct miscdevice g_cph_misc_dev;
+
+/******************************************************************************
+* cph_dev_open()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: The function executes device open actions
+*
+* INPUTS:
+*       inode - Device inode pointer.
+*       file  - File handler.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+static INT32 cph_dev_open(struct inode *inode, struct file *file)
+{
+    MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Enter\n");
+
+#if 0
+    if (g_cph_device_open > 0)
+        return -EBUSY;
+#endif
+
+    g_cph_device_open++;
+
+    return MV_OK;
+}
+
+/******************************************************************************
+* cph_dev_release()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: The function executes device release actions
+*
+* INPUTS:
+*       inode - Device inode pointer.
+*       file  - File handler.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+static INT32 cph_dev_release(struct inode *inode, struct file *file)
+{
+    MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Enter\n");
+
+#if 0
+    if (cph_device_open > 0)
+        cph_device_open--;
+#endif
+    return MV_OK;
+}
+
+/******************************************************************************
+* cph_dev_ioctl()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: The function executes ioctl commands
+*
+* INPUTS:
+*       inode - Device inode pointer.
+*       file  - File handler.
+*       cmd   - Command.
+*       arg   - Ponter to arg.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+static INT32 cph_dev_ioctl(struct inode *inode, struct file *file, UINT32 cmd, ULONG arg)
+{
+    CPH_IOCTL_APP_RULE_T    cph_app_rule;
+    CPH_IOCTL_FLOW_MAP_T    cph_flow_map;
+    CPH_IOCTL_DSCP_MAP_T    cph_dscp_map;
+    CPH_IOCTL_MISC_T        cph_misc;
+    CPH_IOCTL_TCONT_STATE_T cph_tcont;
+    INT32                   rc  = -EINVAL;
+
+    MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Enter\n");
+
+    switch (cmd)
+    {
+        /* CPH application packet handling IOCTL
+        -------------------------------------------*/
+        case MV_CPH_IOCTL_SET_COMPLEX_PROFILE:        
+            if(copy_from_user(&cph_misc, (CPH_IOCTL_MISC_T *)arg, sizeof(CPH_IOCTL_MISC_T)))
+            {
+                MV_CPH_PRINT(CPH_ERR_LEVEL, "copy_from_user failed\n");
+                goto ioctl_err;
+            }
+
+            rc = cph_set_complex_profile(cph_misc.profile_id, cph_misc.active_port);
+            break;
+
+        case MV_CPH_IOCTL_SET_FEATURE_FLAG:        
+            if(copy_from_user(&cph_misc, (CPH_IOCTL_MISC_T *)arg, sizeof(CPH_IOCTL_MISC_T)))
+            {
+                MV_CPH_PRINT(CPH_ERR_LEVEL, "copy_from_user failed\n");
+                goto ioctl_err;
+            }
+
+            rc = cph_set_feature_flag(cph_misc.feature_type, cph_misc.feature_flag);
+            break;
+                
+        case MV_CPH_IOCTL_APP_ADD_RULE:        
+            if(copy_from_user(&cph_app_rule, (CPH_IOCTL_APP_RULE_T *)arg, sizeof(CPH_IOCTL_APP_RULE_T)))
+            {
+                MV_CPH_PRINT(CPH_ERR_LEVEL, "copy_from_user failed\n");
+                goto ioctl_err;
+            }
+
+            rc = cph_add_app_rule(cph_app_rule.parse_bm, &cph_app_rule.parse_key, cph_app_rule.mod_bm, &cph_app_rule.mod_value, 
+                                  cph_app_rule.frwd_bm, &cph_app_rule.frwd_value);
+            break;
+
+        case MV_CPH_IOCTL_APP_DEL_RULE:        
+            if(copy_from_user(&cph_app_rule, (CPH_IOCTL_APP_RULE_T *)arg, sizeof(CPH_IOCTL_APP_RULE_T)))
+            {
+                MV_CPH_PRINT(CPH_ERR_LEVEL, "copy_from_user failed\n");
+                goto ioctl_err;
+            }
+
+            rc = cph_del_app_rule(cph_app_rule.parse_bm, &cph_app_rule.parse_key);
+            break;
+
+        case MV_CPH_IOCTL_APP_UPDATE_RULE:        
+            if(copy_from_user(&cph_app_rule, (CPH_IOCTL_APP_RULE_T *)arg, sizeof(CPH_IOCTL_APP_RULE_T)))
+            {
+                MV_CPH_PRINT(CPH_ERR_LEVEL, "copy_from_user failed\n");
+                goto ioctl_err;
+            }
+
+            rc = cph_update_app_rule(cph_app_rule.parse_bm, &cph_app_rule.parse_key, cph_app_rule.mod_bm, &cph_app_rule.mod_value, 
+                                     cph_app_rule.frwd_bm, &cph_app_rule.frwd_value);
+            break;
+
+        case MV_CPH_IOCTL_APP_GET_RULE:        
+            if(copy_from_user(&cph_app_rule, (CPH_IOCTL_APP_RULE_T *)arg, sizeof(CPH_IOCTL_APP_RULE_T)))
+            {
+                MV_CPH_PRINT(CPH_ERR_LEVEL, "copy_from_user failed\n");
+                goto ioctl_err;
+            }
+
+            rc = cph_get_app_rule(cph_app_rule.parse_bm, &cph_app_rule.parse_key, &cph_app_rule.mod_bm, &cph_app_rule.mod_value, 
+                                  &cph_app_rule.frwd_bm, &cph_app_rule.frwd_value);
+
+            if(rc != MV_OK)
+                goto ioctl_err;
+
+            if(copy_to_user((CPH_IOCTL_APP_RULE_T*)arg, &cph_app_rule, sizeof(CPH_IOCTL_APP_RULE_T)))
+            {
+                MV_CPH_PRINT(CPH_ERR_LEVEL, "copy_to_user failed\n");                
+                goto ioctl_err;
+            }               
+            break;
+            
+        /* CPH flow mapping IOCTL
+        -------------------------------------------*/
+        case MV_CPH_IOCTL_FLOW_ADD_RULE:
+            if(copy_from_user(&cph_flow_map, (CPH_IOCTL_FLOW_MAP_T *)arg, sizeof(CPH_IOCTL_FLOW_MAP_T)))
+            {
+                MV_CPH_PRINT(CPH_ERR_LEVEL, "copy_from_user failed\n");
+                goto ioctl_err;
+            }
+
+            rc = cph_add_flow_rule(&cph_flow_map.flow_map);
+            break;
+
+        case MV_CPH_IOCTL_FLOW_DEL_RULE:
+            if(copy_from_user(&cph_flow_map, (CPH_IOCTL_FLOW_MAP_T *)arg, sizeof(CPH_IOCTL_FLOW_MAP_T)))
+            {
+                MV_CPH_PRINT(CPH_ERR_LEVEL, "copy_from_user failed\n");
+                goto ioctl_err;
+            }
+
+            rc = cph_del_flow_rule(&cph_flow_map.flow_map);
+            break;
+            
+        case MV_CPH_IOCTL_FLOW_GET_RULE:
+            if(copy_from_user(&cph_flow_map, (CPH_IOCTL_FLOW_MAP_T *)arg, sizeof(CPH_IOCTL_FLOW_MAP_T)))
+            {
+                MV_CPH_PRINT(CPH_ERR_LEVEL, "copy_from_user failed\n");
+                goto ioctl_err;
+            }
+        
+            rc = cph_get_flow_rule(&cph_flow_map.flow_map);
+        
+            if(rc != MV_OK)
+                goto ioctl_err;
+        
+            if(copy_to_user((CPH_IOCTL_FLOW_MAP_T*)arg, &cph_flow_map, sizeof(CPH_IOCTL_FLOW_MAP_T)))
+            {
+                MV_CPH_PRINT(CPH_ERR_LEVEL, "copy_to_user failed\n");
+                goto ioctl_err;
+            }          
+            break;
+
+        case MV_CPH_IOCTL_FLOW_CLEAR_RULE:
+            rc = cph_clear_flow_rule();
+            break; 
+
+        case MV_CPH_IOCTL_FLOW_CLEAR_RULE_BY_MH:
+            if(copy_from_user(&cph_flow_map, (CPH_IOCTL_FLOW_MAP_T *)arg, sizeof(CPH_IOCTL_FLOW_MAP_T)))
+            {
+                MV_CPH_PRINT(CPH_ERR_LEVEL, "copy_from_user failed\n");
+                goto ioctl_err;
+            }
+
+            rc = cph_clear_flow_rule_by_mh(cph_flow_map.flow_map.mh);
+            break;
+
+        case MV_CPH_IOCTL_FLOW_SET_DSCP_MAP:
+            if(copy_from_user(&cph_dscp_map, (CPH_IOCTL_DSCP_MAP_T *)arg, sizeof(CPH_IOCTL_DSCP_MAP_T)))
+            {
+                MV_CPH_PRINT(CPH_ERR_LEVEL, "copy_from_user failed\n");
+                goto ioctl_err;
+            }
+
+            rc = cph_set_flow_dscp_map(&cph_dscp_map.dscp_map);
+            break;  
+
+        case MV_CPH_IOCTL_FLOW_DEL_DSCP_MAP:
+            rc = cph_del_flow_dscp_map();
+            break;
+
+        case MV_CPH_IOCTL_SET_TCONT_LLID_STATE:
+            if(copy_from_user(&cph_tcont, (UINT32 *)arg, sizeof(CPH_IOCTL_TCONT_STATE_T)))
+            {
+                MV_CPH_PRINT(CPH_ERR_LEVEL, "copy_from_user failed\n");
+                goto ioctl_err;
+            }
+
+            rc = cph_set_tcont_state(cph_tcont.tcont, cph_tcont.state);
+            break;  
+            
+        case MV_CPH_IOCTL_SETUP:
+            rc = cph_dev_setup();
+            break;
+            
+        default:
+          rc = -EINVAL;
+    }
+
+ioctl_err:
+    return(rc);
+}
+
+
+static const struct file_operations g_cph_dev_fops = 
+{
+    .open    = cph_dev_open,
+    .release = cph_dev_release,
+    .ioctl   = cph_dev_ioctl,
+};
+
+/******************************************************************************
+* cph_dev_setup()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Setup device
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_dev_setup(VOID)
+{
+    MV_STATUS rc  = MV_OK;
+
+    /* Get parameter from XML file */
+    rc = cph_db_get_xml_param();
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "fail to call cph_db_get_xml_param");
+
+    return rc;
+}
+
+/******************************************************************************
+* cph_dev_init()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Initialize CPH device
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_dev_init(VOID)
+{
+    MV_STATUS rc = MV_OK;
+    MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Enter\n");
+
+    g_cph_misc_dev.minor = MISC_DYNAMIC_MINOR;
+    g_cph_misc_dev.name  = MV_CPH_DEVICE_NAME;
+    g_cph_misc_dev.fops  = &g_cph_dev_fops;
+
+    rc = misc_register(&g_cph_misc_dev);
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "fail to call misc_register");
+
+    rc = cph_netdev_init();
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "fail to call cph_netdev_init");
+
+    rc = cph_sysfs_init();
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "fail to call cph_sysfs_init");
+
+    printk(KERN_INFO "CPH: misc device %s registered with minor: %d\n", MV_CPH_DEVICE_NAME, g_cph_misc_dev.minor);
+    return rc;
+}
+
+/******************************************************************************
+* cph_dev_shutdown()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Shutdown CPH device
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       None.
+*******************************************************************************/
+VOID cph_dev_shutdown(VOID)
+{
+    MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Enter\n");
+
+    cph_sysfs_exit();
+
+    misc_deregister(&g_cph_misc_dev);
+}
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_dev.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_dev.h
new file mode 100755
index 0000000..7844c88
--- /dev/null
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_dev.h
@@ -0,0 +1,151 @@
+/*******************************************************************************
+Copyright (C) Marvell International Ltd. and its affiliates
+
+This software file (the "File") is owned and distributed by Marvell 
+International Ltd. and/or its affiliates ("Marvell") under the following
+alternative licensing terms.  Once you have made an election to distribute the
+File under one of the following license alternatives, please (i) delete this
+introductory statement regarding license alternatives, (ii) delete the two
+license alternatives that you have not elected to use and (iii) preserve the
+Marvell copyright notice above.
+
+********************************************************************************
+Marvell Commercial License Option
+
+If you received this File from Marvell and you have entered into a commercial
+license agreement (a "Commercial License") with Marvell, the File is licensed
+to you under the terms of the applicable Commercial License.
+
+********************************************************************************
+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.
+********************************************************************************
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or 
+modify this File under the following licensing terms. 
+Redistribution and use in source and binary forms, with or without modification, 
+are permitted provided that the following conditions are met:
+
+    *   Redistributions of source code must retain the above copyright notice,
+        this list of conditions and the following disclaimer. 
+
+    *   Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution. 
+
+    *   Neither the name of Marvell nor the names of its contributors may be 
+        used to endorse or promote products derived from this software without 
+        specific prior written permission. 
+    
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+********************************************************************************
+* mv_cph_dev.h
+*
+* DESCRIPTION: Marvell CPH(CPH Packet Handler) char device definition
+*
+* DEPENDENCIES:
+*               None
+*
+* CREATED BY:   VictorGu
+*
+* DATE CREATED: 22Jan2013
+*
+* FILE REVISION NUMBER:
+*               Revision: 1.0
+*
+*
+*******************************************************************************/
+#ifndef _MV_CPH_DEV_H_
+#define _MV_CPH_DEV_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+#define MV_CPH_DEVICE_NAME  "cph"
+#define MV_CPH_IOCTL_MAGIC  ('C')
+
+/******************************************************************************
+ * Function Declaration
+ ******************************************************************************/
+/******************************************************************************
+* cph_dev_setup()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Setup device
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_dev_setup(VOID);
+
+/******************************************************************************
+* cph_dev_shutdown()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Initialize CPH device
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32  cph_dev_init (VOID);
+
+/******************************************************************************
+* cph_dev_shutdown()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Shutdown CPH device
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       None.
+*******************************************************************************/
+VOID   cph_dev_shutdown (VOID);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _MV_CPH_DEV_H_ */
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_flow.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_flow.c
new file mode 100755
index 0000000..aae2125
--- /dev/null
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_flow.c
@@ -0,0 +1,1861 @@
+/*******************************************************************************
+Copyright (C) Marvell International Ltd. and its affiliates
+
+This software file (the "File") is owned and distributed by Marvell 
+International Ltd. and/or its affiliates ("Marvell") under the following
+alternative licensing terms.  Once you have made an election to distribute the
+File under one of the following license alternatives, please (i) delete this
+introductory statement regarding license alternatives, (ii) delete the two
+license alternatives that you have not elected to use and (iii) preserve the
+Marvell copyright notice above.
+
+********************************************************************************
+Marvell Commercial License Option
+
+If you received this File from Marvell and you have entered into a commercial
+license agreement (a "Commercial License") with Marvell, the File is licensed
+to you under the terms of the applicable Commercial License.
+
+********************************************************************************
+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.
+********************************************************************************
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or 
+modify this File under the following licensing terms. 
+Redistribution and use in source and binary forms, with or without modification, 
+are permitted provided that the following conditions are met:
+
+    *   Redistributions of source code must retain the above copyright notice,
+        this list of conditions and the following disclaimer. 
+
+    *   Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution. 
+
+    *   Neither the name of Marvell nor the names of its contributors may be 
+        used to endorse or promote products derived from this software without 
+        specific prior written permission. 
+    
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+********************************************************************************
+* mv_cph_flow.c
+*
+* DESCRIPTION: Marvell CPH(CPH Packet Handler) flow module to handle the 
+*              flow mapping, VLAN modification of data traffic
+*
+* DEPENDENCIES:
+*               None
+*
+* CREATED BY:   VictorGu
+*
+* DATE CREATED: 12Dec2011
+*
+* FILE REVISION NUMBER:
+*               Revision: 1.1
+*
+*
+*******************************************************************************/
+#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>
+
+#include "mv_cph_header.h"
+
+/****************************************************************************** 
+*                           Global Data Definitions                                                                                                                                  
+******************************************************************************/
+static CPH_FLOW_TABLE_T gs_flow_table;
+static CPH_DSCP_PBITS_T gs_dscp_map_table;
+
+static MV_ENUM_ENTRY_T g_enum_map_op_type[] =
+{
+    { CPH_VLAN_OP_ASIS,                              "ASIS"},
+    { CPH_VLAN_OP_DISCARD,                           "DISCARD"},
+    { CPH_VLAN_OP_ADD,                               "ADD"},
+    { CPH_VLAN_OP_ADD_COPY_DSCP,                     "ADD_COPY_DSCP"},
+    { CPH_VLAN_OP_ADD_COPY_OUTER_PBIT,               "ADD_COPY_OUTER_PBIT"},
+    { CPH_VLAN_OP_ADD_COPY_INNER_PBIT,               "ADD_COPY_INNER_PBIT"},
+    { CPH_VLAN_OP_ADD_2_TAGS,                        "ADD_2_TAGS"},
+    { CPH_VLAN_OP_ADD_2_TAGS_COPY_DSCP,              "ADD_2_TAGS_COPY_DSCP"},
+    { CPH_VLAN_OP_ADD_2_TAGS_COPY_PBIT,              "ADD_2_TAGS_COPY_PBIT"},
+    { CPH_VLAN_OP_REM,                               "REM"},
+    { CPH_VLAN_OP_REM_2_TAGS,                        "REM_2_TAGS"},
+    { CPH_VLAN_OP_REPLACE,                           "REPLACE"},    
+    { CPH_VLAN_OP_REPLACE_VID,                       "REPLACE_VID"},
+    { CPH_VLAN_OP_REPLACE_PBIT,                      "REPLACE_PBIT"},
+    { CPH_VLAN_OP_REPLACE_INNER_ADD_OUTER,           "REPLACE_INNER_ADD_OUTER"},
+    { CPH_VLAN_OP_REPLACE_INNER_ADD_OUTER_COPY_PBIT, "REPLACE_INNER_ADD_OUTER_COPY_PBIT"},
+    { CPH_VLAN_OP_REPLACE_INNER_REM_OUTER,           "REPLACE_INNER_REM_OUTER"},
+    { CPH_VLAN_OP_REPLACE_2TAGS,                     "REPLACE_2TAGS"},    
+    { CPH_VLAN_OP_REPLACE_2TAGS_VID,                 "REPLACE_2TAGS_VID"},
+    { CPH_VLAN_OP_SWAP,                              "SWAP"}    
+};
+
+static MV_ENUM_ARRAY_T g_enum_array_op_type =
+{
+    sizeof(g_enum_map_op_type)/sizeof(g_enum_map_op_type[0]),
+    g_enum_map_op_type
+};
+
+/****************************************************************************** 
+*                           External Declarations                                                                                                                                 
+******************************************************************************/
+
+
+
+/****************************************************************************** 
+*                           Function Definitions                                                                                                                                  
+******************************************************************************/
+/******************************************************************************
+* cph_flow_db_get_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Get CPH flow mapping rule.
+*
+* INPUTS:
+*       flow       - Flow parsing field values
+*       for_packet - Whether get rule for packet or for new CPH rule
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_flow_db_get_rule (CPH_FLOW_ENTRY_T *flow, BOOL for_packet)
+{
+    UINT32            idx         = 0;
+    UINT32            rule_idx    = 0;
+    CPH_FLOW_ENTRY_T *p_flow_rule = NULL;
+    BOOL              rc          = FALSE;
+    unsigned long     flags;
+
+    spin_lock_irqsave(&gs_flow_table.flow_lock, flags);
+    /* Traverse CPH flow rule table */
+    for (idx = 0, rule_idx = 0; (idx < CPH_FLOW_ENTRY_NUM) && (rule_idx < gs_flow_table.rule_num); idx++)
+    {
+        p_flow_rule = &gs_flow_table.flow_rule[idx];
+        
+        /* Compare packet or new rule rule data base rule */
+        if (p_flow_rule->valid == TRUE)
+        {
+            rule_idx++;
+
+            if (for_packet == TRUE)                
+                rc = cph_flow_compare_packet_and_rule(flow, p_flow_rule);
+            else
+                rc = cph_flow_compare_rules(flow, p_flow_rule);
+
+            if (rc == TRUE)
+            {   
+                flow->op_type = p_flow_rule->op_type;
+                memcpy(&flow->mod_outer_tci, &p_flow_rule->mod_outer_tci, sizeof(CPH_FLOW_TCI_T));
+                memcpy(&flow->mod_inner_tci, &p_flow_rule->mod_inner_tci, sizeof(CPH_FLOW_TCI_T));
+                memcpy(&flow->pkt_frwd,      &p_flow_rule->pkt_frwd,      sizeof(CPH_FLOW_FRWD_T));
+
+                /* Increase count */
+                if (for_packet == TRUE)
+                {
+                    if (p_flow_rule->count == 0xFFFFFFFF)
+                        p_flow_rule->count = 0;
+                    else
+                        p_flow_rule->count++;
+                }
+
+                spin_unlock_irqrestore(&gs_flow_table.flow_lock, flags);
+                return MV_OK;
+            }
+        }
+    }
+    spin_unlock_irqrestore(&gs_flow_table.flow_lock, flags);
+
+    return MV_FAIL;
+}
+
+/******************************************************************************
+* cph_flow_db_add_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Add flow rule to database 
+*
+* INPUTS:
+*       cph_flow - VLAN ID, 802.1p value, pkt_fwd information.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32  cph_flow_db_add_rule(CPH_FLOW_ENTRY_T *cph_flow)
+{
+    UINT32            idx         = 0;
+    CPH_FLOW_ENTRY_T *p_flow_rule = NULL;
+    BOOL              rc          = MV_OK;
+    unsigned long     flags;
+
+    /* If the flow table is full */
+    if (gs_flow_table.rule_num >= CPH_FLOW_ENTRY_NUM)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "%s(), flow rule table is full<%d> \n", __FUNCTION__, gs_flow_table.rule_num);
+        return MV_FULL;
+    }
+
+    /* Check if there is already conflicted rules */
+    if (cph_flow_db_get_rule(cph_flow, FALSE) == MV_OK)
+    {
+        MV_CPH_PRINT(CPH_DEBUG_LEVEL, "%s(), already has conflict flow rule \n", __FUNCTION__);
+        return MV_OK;
+    }
+
+    spin_lock_irqsave(&gs_flow_table.flow_lock, flags);    
+    /* Traverse CPH flow rule tale */
+    for (idx = 0; idx < CPH_FLOW_ENTRY_NUM; idx++)
+    {
+        p_flow_rule = &gs_flow_table.flow_rule[idx];
+        
+        /* Compare parse_bm and parse_key */
+        if (p_flow_rule->valid == FALSE)
+           break;
+    }
+
+    if (idx == CPH_FLOW_ENTRY_NUM)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "%s(), flow rule table is full<%d> \n", __FUNCTION__, gs_flow_table.rule_num);
+        spin_unlock_irqrestore(&gs_flow_table.flow_lock, flags);        
+        return MV_FULL;    
+    }
+
+    /* Save to db */
+    memcpy(p_flow_rule, cph_flow, sizeof(CPH_FLOW_ENTRY_T));
+    p_flow_rule->valid = TRUE;
+    p_flow_rule->count = 0;
+    gs_flow_table.rule_num ++;
+    spin_unlock_irqrestore(&gs_flow_table.flow_lock, flags);
+
+    return rc;   
+}
+
+/******************************************************************************
+* cph_flow_db_del_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Delete CPH flow mapping rule.
+*
+* INPUTS:
+*       flow - Flow parsing field values
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_flow_db_del_rule (CPH_FLOW_ENTRY_T *flow)
+{
+    UINT32            idx         = 0;
+    UINT32            rule_idx    = 0;
+    CPH_FLOW_ENTRY_T *p_flow_rule = NULL;
+    BOOL              rc          = MV_OK;
+    unsigned long     flags;
+ 
+    spin_lock_irqsave(&gs_flow_table.flow_lock, flags);    
+    /* Traverse CPH flow rule tale */
+    for (idx = 0, rule_idx = 0; (idx < CPH_FLOW_ENTRY_NUM) && (rule_idx < gs_flow_table.rule_num); idx++)
+    {
+        p_flow_rule = &gs_flow_table.flow_rule[idx];
+        
+        /* Compare parse_bm and parse_key */
+        if (p_flow_rule->valid == TRUE)
+        {
+            rule_idx++;
+
+            rc = cph_flow_compare_rules(flow, p_flow_rule);
+            if (rc == TRUE)
+            {   
+                memset(p_flow_rule, 0, sizeof(CPH_FLOW_ENTRY_T));
+                p_flow_rule->valid = FALSE;
+                gs_flow_table.rule_num--;
+
+                spin_unlock_irqrestore(&gs_flow_table.flow_lock, flags);                
+                return MV_OK;
+            }
+        }
+    }
+    spin_unlock_irqrestore(&gs_flow_table.flow_lock, flags);
+
+    return rc;
+}
+
+/******************************************************************************
+* cph_flow_db_clear_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Clear CPH flow mapping rules.
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_flow_db_clear_rule (VOID)
+{
+    UINT32 idx = 0;
+    unsigned long flags;
+
+    spin_lock_irqsave(&gs_flow_table.flow_lock, flags);    
+    /* Initializes flow rule */
+    memset((UINT8 *)&gs_flow_table, 0, sizeof(CPH_FLOW_TABLE_T));
+    for (idx = 0; idx < CPH_FLOW_ENTRY_NUM; idx++)
+        gs_flow_table.flow_rule[idx].valid = FALSE;
+    
+    spin_unlock_irqrestore(&gs_flow_table.flow_lock, flags);
+
+    return MV_OK;
+}
+
+/******************************************************************************
+* cph_flow_db_clear_rule_by_mh()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Clear CPH flow mapping rules by MH.
+*
+* INPUTS:
+*       mh  -  Marvell header.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_flow_db_clear_rule_by_mh (UINT16 mh)
+{
+    CPH_FLOW_ENTRY_T   *p_flow_rule = NULL;
+    UINT32              idx = 0;
+    unsigned long       flags;
+    
+    spin_lock_irqsave(&gs_flow_table.flow_lock, flags);    
+    /* Initializes flow rule */
+    for (idx = 0; idx < CPH_FLOW_ENTRY_NUM; idx++)
+    {
+        p_flow_rule = &gs_flow_table.flow_rule[idx];
+
+        if ((p_flow_rule->valid == TRUE) &&
+            (p_flow_rule->mh == mh))
+        {
+            memset(p_flow_rule, 0, sizeof(CPH_FLOW_ENTRY_T));
+            p_flow_rule->valid = FALSE;
+            gs_flow_table.rule_num--;
+        }
+    }
+    spin_unlock_irqrestore(&gs_flow_table.flow_lock, flags);
+
+    return MV_OK;
+}
+
+/******************************************************************************
+* cph_flow_db_init()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Initialize CPH flow mapping database.
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_flow_db_init (VOID)
+{
+    UINT32 idx = 0;
+    
+    /* Initializes flow rule */
+    memset((UINT8 *)&gs_flow_table, 0, sizeof(CPH_FLOW_TABLE_T));
+    for (idx = 0; idx < CPH_FLOW_ENTRY_NUM; idx++)
+        gs_flow_table.flow_rule[idx].valid = FALSE;
+
+    spin_lock_init(&gs_flow_table.flow_lock);
+
+    /* Initializes DSCP to P-bits mapping table */
+    memset((UINT8 *)&gs_dscp_map_table, 0, sizeof(CPH_DSCP_PBITS_T));
+    gs_dscp_map_table.in_use = 0;
+
+    return MV_OK;
+}
+
+/******************************************************************************
+* cph_flow_verify_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Verify flow mapping rule
+*
+* INPUTS:
+*       tci         - TPID, VLAN ID, P-bits information.
+*       parse_field - Whether the TCI is from parsing field.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns TRUE. 
+*       On error returns FALSE.
+*******************************************************************************/
+BOOL  cph_flow_verify_tci(CPH_FLOW_TCI_T *tci, BOOL parse_field)
+{
+    UINT16 max_vid   = 0;
+    UINT16 max_pbits = 0;
+    
+    if (parse_field == TRUE)
+    {
+        max_vid   = MV_CPH_VID_NOT_CARE_VALUE;
+        max_pbits = MV_CPH_PBITS_NOT_CARE_VALUE;
+    }
+    else
+    {
+        max_vid   = MV_VLAN_ID_MAX;
+        max_pbits = MV_PBITS_MAX;        
+    }
+    
+    /* Check TPID */
+    if ((tci->tpid != MV_TPID_8100) &&
+        (tci->tpid != MV_TPID_9100) &&
+        (tci->tpid != MV_TPID_88A8) &&
+        ((tci->tpid != MV_CPH_TPID_NOT_CARE_VALUE) && (parse_field == TRUE))) {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "tpid[0x%x] is invalid \n", tci->tpid);
+        return FALSE;
+    }
+
+    /* Check VID */
+    if (tci->vid > max_vid) {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "vid[%d] exceeds maximum value[%d] \n", tci->vid, max_vid);
+        return FALSE;
+    }
+    
+    /* Check P-bits */
+    if (tci->pbits > max_pbits) {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "pbits[%d] exceeds maximum value[%d] \n", tci->pbits, max_pbits);
+        return FALSE;
+    }
+    
+    return TRUE;
+}
+
+/******************************************************************************
+* cph_flow_verify_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Verify flow mapping rule
+*
+* INPUTS:
+*       cph_flow - VLAN ID, 802.1p value, pkt_fwd information.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns TRUE. 
+*       On error returns FALSE.
+*******************************************************************************/
+BOOL  cph_flow_verify_rule(CPH_FLOW_ENTRY_T *cph_flow, BOOL full)
+{
+    CPH_DIR_E          dir         = CPH_DIR_US;
+    CPH_FLOW_PARSE_E   parse_bm    = 0;
+    CPH_VLAN_OP_TYPE_E op_type     = 0;    
+    CPH_FLOW_FRWD_T   *p_pkt_fwd   = NULL;
+    BOOL               rc          = TRUE;
+
+    /* Get input information: VID, P-bits... */
+    if (cph_flow == NULL) {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "cph_flow is NULL \n");
+        return FALSE;
+    } 
+
+    dir       =  cph_flow->dir;    
+    parse_bm  =  cph_flow->parse_bm;
+    op_type   =  cph_flow->op_type;    
+    p_pkt_fwd = &cph_flow->pkt_frwd;
+
+    /* Check dir */
+    if (dir >= CPH_DIR_NOT_CARE) {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "dir[%d] exceeds maximum value[%d] \n", dir, CPH_DIR_NOT_CARE);
+        return FALSE;
+    }
+
+    /* Check parse_bm */
+    if ((parse_bm & CPH_FLOW_PARSE_EXT_VLAN) && 
+        (parse_bm & CPH_FLOW_PARSE_TWO_VLAN)) {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "Cann't set parse_bm CPH_FLOW_PARSE_EXT_VLAN and CPH_FLOW_PARSE_EXT_VLAN at the same time \n");
+        return FALSE;
+    }      
+
+    /* Check op_type */
+    if (op_type > CPH_VLAN_OP_SWAP) {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "op_type[%d] exceeds maximum value[%d] \n", op_type, CPH_VLAN_OP_SWAP);
+        return FALSE;
+    }
+    
+    /* Check TCI */
+    rc = cph_flow_verify_tci(&cph_flow->parse_outer_tci, TRUE);
+    if (rc == FALSE) {
+        return FALSE;
+    }
+    rc = cph_flow_verify_tci(&cph_flow->parse_inner_tci, TRUE);
+    if (rc == FALSE) {
+        return FALSE;
+    }
+    rc = cph_flow_verify_tci(&cph_flow->mod_outer_tci, FALSE);
+    if (rc == FALSE) {
+        return FALSE;
+    }
+    rc = cph_flow_verify_tci(&cph_flow->mod_inner_tci, FALSE);
+    if (rc == FALSE) {
+        return FALSE;
+    }
+    
+    /* Check target port/queue/GEM port */ 
+    if (p_pkt_fwd->trg_port > MV_TCONT_LLID_MAX) {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "trg_port[%d] exceeds maximum value[%d] \n", p_pkt_fwd->trg_port, MV_TCONT_LLID_MAX);
+        return FALSE;
+    }
+
+    if (p_pkt_fwd->trg_queue > MV_QUEUE_MAX) {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "SWF trg_queue[%d] exceeds maximum value[%d] \n", p_pkt_fwd->trg_queue, MV_QUEUE_MAX);
+        return FALSE;
+    }   
+
+    if (p_pkt_fwd->trg_hwf_queue > MV_QUEUE_MAX) {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "HWF trg_queue[%d] exceeds maximum value[%d] \n", p_pkt_fwd->trg_hwf_queue, MV_QUEUE_MAX);
+        return FALSE;
+    }       
+
+    if (p_pkt_fwd->gem_port > MV_GEM_PORT_MAX) {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "gem_port[%d] exceeds maximum value[%d] \n", p_pkt_fwd->gem_port, MV_GEM_PORT_MAX);
+        return FALSE;
+    }
+
+    return TRUE;
+}
+
+/******************************************************************************
+* cph_flow_display_tci()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Display TCI value
+*
+* INPUTS:
+*       tci         - TCI field
+*       trace_level - Trace and debug level
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+VOID  cph_flow_display_tci(CPH_FLOW_TCI_T *tci, UINT32 trace_level)
+{
+    MV_CPH_PRINT(trace_level, 
+                 "TPID[0x%x], vid[%d], p-bits[%d]\n",
+                 ((tci != NULL)? tci->tpid:0),
+                 ((tci != NULL)? tci->vid:0), 
+                 ((tci != NULL)? tci->pbits:0));
+    return;
+}
+
+/******************************************************************************
+* cph_flow_add_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Sets flow mapping rule
+*
+* INPUTS:
+*       cph_flow - VLAN ID, 802.1p value, pkt_fwd information.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32  cph_flow_add_rule(CPH_FLOW_ENTRY_T *cph_flow)
+{
+    BOOL rc = TRUE;
+
+    /* Display input CPH flow */
+    MV_CPH_PRINT(CPH_DEBUG_LEVEL, 
+                 "---->\n default rule[%s], dir[%d], parse bm[%x], mh[%d], eth type[0x%x], op type[%d], trg port[%d], SWF queue[%d], HWF queue[%d], GEM port[%d]\n",
+                 (cph_flow->is_default == TRUE)? "Yes":"No",
+                 cph_flow->dir, cph_flow->parse_bm, cph_flow->mh, cph_flow->eth_type, cph_flow->op_type,
+                 cph_flow->pkt_frwd.trg_port, cph_flow->pkt_frwd.trg_queue, cph_flow->pkt_frwd.trg_hwf_queue, cph_flow->pkt_frwd.gem_port);
+    cph_flow_display_tci(&cph_flow->parse_outer_tci, CPH_DEBUG_LEVEL);
+    cph_flow_display_tci(&cph_flow->parse_inner_tci, CPH_DEBUG_LEVEL);
+    cph_flow_display_tci(&cph_flow->mod_outer_tci, CPH_DEBUG_LEVEL);
+    cph_flow_display_tci(&cph_flow->mod_inner_tci, CPH_DEBUG_LEVEL);
+            
+    /* Verify CPH flow rule */
+    rc = cph_flow_verify_rule(cph_flow, TRUE);
+    if (rc == FALSE) {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "%s(), verify rule failed \n", __FUNCTION__);
+        return MV_FAIL;
+    }
+
+    /* Add flow rule to data base */
+    if (cph_flow_db_add_rule(cph_flow) != MV_OK) {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "%s(), fail to call cph_flow_db_add_rule\n", __FUNCTION__);
+        return MV_FAIL;
+    }
+
+    MV_CPH_PRINT(CPH_DEBUG_LEVEL, "<----\n");
+
+    return MV_OK;
+}
+
+/******************************************************************************
+* cph_flow_del_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Deletes flow mapping rule
+*
+* INPUTS:
+*       cph_flow - VLAN ID, 802.1p value, pkt_fwd information.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32  cph_flow_del_rule(CPH_FLOW_ENTRY_T *cph_flow)
+{
+    /* Display input CPH flow */
+    MV_CPH_PRINT(CPH_DEBUG_LEVEL, 
+                 "---->\n default rule[%s], dir[%d], parse bm[%x], mh[%d], eth type[0x%x], op type[%d]\n",
+                 (cph_flow->is_default == TRUE)? "Yes":"No",
+                 cph_flow->dir, cph_flow->parse_bm, cph_flow->mh, cph_flow->eth_type, cph_flow->op_type);
+    cph_flow_display_tci(&cph_flow->parse_outer_tci, CPH_DEBUG_LEVEL);
+    cph_flow_display_tci(&cph_flow->parse_inner_tci, CPH_DEBUG_LEVEL);
+
+    /* Delete flow rule from data base */
+    if (cph_flow_db_del_rule(cph_flow) != MV_OK) {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "%s(), fail to call cph_flow_db_del_rule\n", __FUNCTION__);
+        return MV_FAIL;
+    }
+
+    MV_CPH_PRINT(CPH_DEBUG_LEVEL, "<----\n");
+
+    return MV_OK; 
+}
+
+/******************************************************************************
+* cph_flow_get_tag_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Gets flow mapping rule for tagged frames.
+*
+* INPUTS:
+*       cph_flow - Input vid, pbits, dir
+*
+* OUTPUTS:
+*       cph_flow - output packet forwarding information, including GEM port, 
+*                  T-CONT, queue and packet modification for VID, P-bits.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32  cph_flow_get_rule(CPH_FLOW_ENTRY_T *cph_flow)
+{
+    /* Display input CPH flow */
+    MV_CPH_PRINT(CPH_DEBUG_LEVEL,
+                 "---->\nPacket flow rule: default rule[%s], dir[%d], parse bm[%x], mh[%d], eth type[0x%x]\n",
+                 (cph_flow->is_default == TRUE)? "Yes":"No",
+                 cph_flow->dir, cph_flow->parse_bm, cph_flow->mh, cph_flow->eth_type);
+    cph_flow_display_tci(&cph_flow->parse_outer_tci, CPH_DEBUG_LEVEL);
+    cph_flow_display_tci(&cph_flow->parse_inner_tci, CPH_DEBUG_LEVEL);
+
+    /* Get flow rule from data base */
+    if (cph_flow_db_get_rule(cph_flow, TRUE) != MV_OK) {
+        MV_CPH_PRINT(CPH_DEBUG_LEVEL, "%s(), fail to call cph_flow_db_get_rule\n", __FUNCTION__);
+        return MV_FAIL;
+    }
+
+    MV_CPH_PRINT(CPH_DEBUG_LEVEL, "<----\n");
+
+    return MV_OK;
+}
+
+/******************************************************************************
+* cph_flow_clear_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Clears all flow mapping rules
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_flow_clear_rule(VOID)
+{
+    /* Display input CPH flow */
+    MV_CPH_PRINT(CPH_DEBUG_LEVEL, 
+                 "---->\n");
+
+    /* Clear flow rule from data base */
+    if (cph_flow_db_clear_rule() != MV_OK) {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "%s(), fail to call cph_flow_db_clear_rule\n", __FUNCTION__);
+        return MV_FAIL;
+    }
+
+    MV_CPH_PRINT(CPH_DEBUG_LEVEL, "<----\n"); 
+    return MV_OK;
+}
+
+/******************************************************************************
+* cph_flow_clear_rule_by_mh()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Clears flow mapping rules by MH
+*
+* INPUTS:
+*       mh   -  Marvell header.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_flow_clear_rule_by_mh(UINT16 mh)
+{
+    /* Display input CPH flow */
+    MV_CPH_PRINT(CPH_DEBUG_LEVEL, 
+                 "---->  mh(%d)\n", mh);
+
+    /* Clear flow rule from data base */
+    if (cph_flow_db_clear_rule_by_mh(mh) != MV_OK) {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "%s(), fail to call cph_flow_db_clear_rule_by_mh\n", __FUNCTION__);
+        return MV_FAIL;
+    }
+
+    MV_CPH_PRINT(CPH_DEBUG_LEVEL, "<----\n"); 
+    return MV_OK;
+}
+
+/******************************************************************************
+* cph_flow_set_dscp_map()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Sets DSCP to P-bits mapping rules
+*
+* INPUTS:
+*       dscp_map  - DSCP to P-bits mapping rules.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32  cph_flow_set_dscp_map(CPH_DSCP_PBITS_T *dscp_map)
+{
+    MV_CPH_PRINT(CPH_DEBUG_LEVEL, 
+                 "----> in_use[%d]\n",
+                 ((dscp_map!= NULL)? dscp_map->in_use:0));
+
+    if (dscp_map == NULL) {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "Input dscp_map is NULL\n");
+        return MV_FAIL;
+    }
+
+    /* Case 1: to enable DSCP to P-bits mapping */
+    if (dscp_map->in_use != 0) {
+        memcpy(&gs_dscp_map_table.pbits[0], &dscp_map->pbits[0], sizeof(gs_dscp_map_table.pbits));
+        gs_dscp_map_table.in_use = 1;
+    
+    }
+    /* Case 2: to disable DSCP to P-bits mapping */
+    else {
+        memset((UINT8 *)&gs_dscp_map_table, 0, sizeof(gs_dscp_map_table));
+        gs_dscp_map_table.in_use = 0;
+    }
+    
+    MV_CPH_PRINT(CPH_DEBUG_LEVEL, "<----\n");
+
+    return MV_OK;   
+}
+
+/******************************************************************************
+* cph_flow_del_dscp_map()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Deletes DSCP to P-bits mapping rules
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32  cph_flow_del_dscp_map(VOID)
+{
+    MV_CPH_PRINT(CPH_DEBUG_LEVEL, "---->\n");
+
+    /* Clear DSCP to P-bits mapping */
+    memset((UINT8 *)&gs_dscp_map_table, 0, sizeof(gs_dscp_map_table));
+    gs_dscp_map_table.in_use = 0;
+    
+    MV_CPH_PRINT(CPH_DEBUG_LEVEL, "<----\n");
+
+    return MV_OK;
+}
+
+/******************************************************************************
+* cph_flow_add_vlan()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Add one VLAN tag behind of source MAC address.
+*
+* INPUTS:
+*       mh     - Whether has MH or not
+*       p_data - Pointer to packet
+*       tpid   - Type of VLAN ID
+*       vid    - VLAN to be added
+*       pbits  - P-bits value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       The shift of SKB data.
+*******************************************************************************/
+INLINE INT32 cph_flow_add_vlan(BOOL mh, UINT8 *p_data, UINT16 tpid, UINT16 vid, UINT8 pbits)
+{
+    UINT8  *p_new  = NULL;
+    UINT16 *p_vlan = NULL;
+    UINT32  len    = 0;
+
+    p_new = p_data - MV_VLAN_HLEN;
+
+    if (TRUE == mh)
+        len = MV_ETH_MH_SIZE + MV_MAC_ADDR_SIZE + MV_MAC_ADDR_SIZE;
+    else
+        len = MV_MAC_ADDR_SIZE + MV_MAC_ADDR_SIZE;
+
+    memmove(p_new, p_data, len);
+
+    p_vlan = (UINT16 *)(p_new  + len);
+    
+    /* Set VLAN Type */
+    *p_vlan = htons(tpid);
+    p_vlan++;
+
+    /* Set VID + priority */    
+    *p_vlan = htons((vid & MV_VLAN_ID_MASK) | ((pbits & MV_PBITS_MASK) << MV_PBITS_SHIFT));
+
+    return -MV_VLAN_HLEN;
+}
+
+/******************************************************************************
+* cph_flow_del_vlan()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Delete one VLAN tag behind of source MAC address.
+*
+* INPUTS:
+*       mh     - Whether has MH or not
+*       p_data - Pointer to packet.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       The shift of SKB data.
+*******************************************************************************/
+INLINE INT32 cph_flow_del_vlan(BOOL mh, UINT8 *p_data)
+{
+    UINT8  *p_new  = NULL;
+    UINT32  len    = 0;
+
+    p_new = p_data + MV_VLAN_HLEN;
+
+    if (TRUE == mh)
+        len = MV_ETH_MH_SIZE + MV_MAC_ADDR_SIZE + MV_MAC_ADDR_SIZE;
+    else
+        len = MV_MAC_ADDR_SIZE + MV_MAC_ADDR_SIZE;
+
+    memmove(p_new, p_data, len);
+
+    return MV_VLAN_HLEN;
+}
+
+/******************************************************************************
+* cph_flow_strip_vlan()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Delete all VLAN tags behind of source MAC address.
+*
+* INPUTS:
+*       mh     - Whether has MH or not
+*       p_data - Pointer to packet.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       The shift of SKB data.
+*******************************************************************************/
+INLINE INT32 cph_flow_strip_vlan(BOOL mh, UINT8 *p_data)
+{
+    INT32  offset       = 0;
+    INT32  total_offset = 0;    
+    UINT16 eth_type     = 0;
+    UINT8 *p_field      = NULL;
+    UINT32 len          = 0;
+
+    if (TRUE == mh)
+        len = MV_ETH_MH_SIZE + MV_MAC_ADDR_SIZE + MV_MAC_ADDR_SIZE;
+    else
+        len = MV_MAC_ADDR_SIZE + MV_MAC_ADDR_SIZE;
+
+    p_field  = p_data + len;
+    eth_type = ntohs(*(UINT16 *)p_field);
+
+    while (eth_type == MV_TPID_8100 || eth_type == MV_TPID_88A8 || eth_type == MV_TPID_9100) 
+    {
+        offset   = cph_flow_del_vlan(mh, p_data+offset);
+        
+        p_field += offset;
+        eth_type = ntohs(*(UINT16 *)p_field);
+        total_offset += offset;
+    }    
+
+    return total_offset;
+}
+
+/******************************************************************************
+* cph_flow_replace_vlan()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Replace one VLAN tag behind of source MAC address.
+*
+* INPUTS:
+*       mh     - Whether has MH or not
+*       p_data - Pointer to packet
+*       tpid   - Type of VLAN ID
+*       vid    - VLAN to be added
+*       pbits  - P-bits value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       The shift of SKB data.
+*******************************************************************************/
+INLINE INT32 cph_flow_replace_vlan(BOOL mh, UINT8 *p_data, UINT16 tpid, UINT16 vid, UINT8 pbits)
+{
+    UINT16 *p_vlan = NULL;
+    UINT32  len    = 0;
+
+    if (TRUE == mh)
+        len = MV_ETH_MH_SIZE + MV_MAC_ADDR_SIZE + MV_MAC_ADDR_SIZE;
+    else
+        len = MV_MAC_ADDR_SIZE + MV_MAC_ADDR_SIZE;
+
+    p_vlan = (UINT16 *)(p_data + len);
+    
+    /* Set VLAN Type */
+    *p_vlan = htons(tpid);
+    p_vlan++;
+    
+    /* Set VID + priority */
+    *p_vlan = htons((vid & MV_VLAN_ID_MASK) | ((pbits & MV_PBITS_MASK) << MV_PBITS_SHIFT));
+
+    return 0;
+}
+
+/******************************************************************************
+* cph_flow_swap_vlan()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Swap between two VLAN tag.
+*
+* INPUTS:
+*       mh     - Whether has MH or not
+*       p_data - Pointer to packet
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       The shift of SKB data.
+*******************************************************************************/
+INLINE INT32 cph_flow_swap_vlan(BOOL mh, UINT8 *p_data)
+{
+    UINT32 *p_tci = NULL;
+    UINT32  tci1  = 0;
+    UINT32  tci2  = 0;
+    UINT32  len    = 0;
+
+    if (TRUE == mh)
+        len = MV_ETH_MH_SIZE + MV_MAC_ADDR_SIZE + MV_MAC_ADDR_SIZE;
+    else
+        len = MV_MAC_ADDR_SIZE + MV_MAC_ADDR_SIZE;
+
+    p_tci = (UINT32 *)(p_data + len);
+    
+    /* Save first TCI */
+    tci1 = ntohl(*p_tci);
+    p_tci++;
+
+    /* Save second TCI and replace it w/ first TCI */
+    tci2   = ntohl(*p_tci);
+    *p_tci = htonl(tci1);
+
+    /* Go back to replce first TCI */
+    p_tci--;    
+    *p_tci = htonl(tci2);
+    
+    return 0;
+}
+
+/******************************************************************************
+* cph_flow_parse_packet()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Parse packet and output flow information.
+*
+* INPUTS:
+*       port - Source GMAC port
+*       data - Pointer to packet
+*       rx   - Whether in RX dir
+*       mh   - Whether has Marvell header
+*
+* OUTPUTS:
+*       flow - Flow parsing field values
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_flow_parse_packet (INT32 port, UINT8 *data, BOOL rx, BOOL mh, CPH_FLOW_ENTRY_T *flow)
+{
+    UINT16      eth_type = 0; 
+    UINT8      *p_field  = NULL;
+    UINT8       proto    = 0;
+    MV_STATUS   rc       = MV_OK;
+    struct ipv6hdr         *p_ipv6_hdr   = NULL;
+    struct ipv6_hopopt_hdr *p_hopopt_hdr = NULL;
+    struct icmp6hdr        *p_icmp_hdr   = NULL;     
+
+    memset(flow, 0, sizeof(CPH_FLOW_ENTRY_T));
+    
+    /* Parse Direction */
+    flow->dir = cph_app_parse_dir(port, rx);
+    if(flow->dir == CPH_DIR_INVALID)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "dir[%d] is invalid \n", flow->dir);
+        return MV_BAD_VALUE;
+    }
+
+    if (TRUE == mh)
+    {
+        /* Parse Marvell header */
+        if (flow->dir == CPH_DIR_US)
+            flow->mh = (ntohs(*(UINT16 *)data) & MV_VALID_MH_MASK);
+        else
+            flow->mh = (ntohs(*(UINT16 *)data) & MV_VALID_GH_MASK);
+        
+        flow->parse_bm |= CPH_FLOW_PARSE_MH;
+        p_field  = data + MV_ETH_MH_SIZE + ETH_ALEN + ETH_ALEN;            
+    }
+    else
+    {
+        p_field  = data + ETH_ALEN + ETH_ALEN; 
+    }
+
+    /* Parse VLAN tag */
+    eth_type = ntohs(*(UINT16 *)p_field);
+    if (eth_type == MV_TPID_8100 || eth_type == MV_TPID_88A8 || eth_type == MV_TPID_9100) 
+    {
+        flow->parse_bm |= CPH_FLOW_PARSE_EXT_VLAN;
+        
+        flow->parse_outer_tci.tpid = ntohs(*(UINT16 *)p_field);
+        p_field += MV_CPH_TPID_LEN;
+
+        flow->parse_outer_tci.vid   = (ntohs(*(UINT16 *)p_field) & MV_VLAN_ID_MASK);
+        flow->parse_outer_tci.pbits = ((ntohs(*(UINT16 *)p_field) >> MV_PBITS_SHIFT)& MV_PBITS_MASK);
+        
+        p_field += MV_CPH_VLAN_TAG_LEN;
+
+        eth_type = ntohs(*(UINT16 *)p_field);
+        if (eth_type == MV_TPID_8100 || eth_type == MV_TPID_88A8 || eth_type == MV_TPID_9100) 
+        {
+            flow->parse_bm &= ~CPH_FLOW_PARSE_EXT_VLAN;
+            flow->parse_bm |= CPH_FLOW_PARSE_TWO_VLAN;
+            
+            flow->parse_inner_tci.tpid = ntohs(*(UINT16 *)p_field);
+            p_field += MV_CPH_TPID_LEN;
+
+            flow->parse_inner_tci.vid   = (ntohs(*(UINT16 *)p_field) & MV_VLAN_ID_MASK);
+            flow->parse_inner_tci.pbits = ((ntohs(*(UINT16 *)p_field) >> MV_PBITS_SHIFT)& MV_PBITS_MASK);
+            
+            p_field += MV_CPH_VLAN_TAG_LEN;
+
+            eth_type = ntohs(*(UINT16 *)p_field);
+        }      
+    }    
+    while (eth_type == MV_TPID_8100 || eth_type == MV_TPID_88A8 || eth_type == MV_TPID_9100) {
+        p_field += VLAN_HLEN;
+        eth_type = ntohs(*(UINT16 *)p_field);
+    }
+    /* Parse Eth type */    
+    flow->eth_type = eth_type;
+    flow->parse_bm |= CPH_FLOW_PARSE_ETH_TYPE;
+
+    /* Parse Multicast protocol */
+    if (MV_CPH_ETH_TYPE_IPV4 == flow->eth_type) {
+        p_field += MV_CPH_ETH_TYPE_LEN;
+        p_field += MV_IPV4_PROTO_OFFSET;
+        proto = *(UINT8 *)p_field;
+
+        if (IPPROTO_IGMP == proto)
+            flow->parse_bm |= CPH_FLOW_PARSE_MC_PROTO;
+    }
+    else if (MV_CPH_ETH_TYPE_IPV6 == flow->eth_type) {
+        p_ipv6_hdr = (struct ipv6hdr *)(p_field + MV_CPH_ETH_TYPE_LEN);
+
+        if (NEXTHDR_HOP == p_ipv6_hdr->nexthdr) {
+            p_hopopt_hdr = (struct ipv6_hopopt_hdr *)((UINT8 *)p_ipv6_hdr + sizeof(struct ipv6hdr));
+
+            if (IPPROTO_ICMPV6 == p_hopopt_hdr->nexthdr) {
+                p_icmp_hdr =  (struct icmp6hdr *)((UINT8 *)p_hopopt_hdr + ipv6_optlen(p_hopopt_hdr));
+                
+                switch (p_icmp_hdr->icmp6_type) {
+                    case ICMPV6_MGM_QUERY:
+                    case ICMPV6_MGM_REPORT:
+                    case ICMPV6_MGM_REDUCTION:
+                    case ICMPV6_MLD2_REPORT:
+                        flow->parse_bm |= CPH_FLOW_PARSE_MC_PROTO;
+                        break;
+                    default:
+                        break;
+                }
+            }
+        }
+    }
+
+    return rc;
+}
+
+/******************************************************************************
+* cph_flow_compare_rules()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Compare two flow rules.
+*
+* INPUTS:
+*       parse_rule  - The parsing field values come from the packets
+*       db_rule     - The flow rule stored in flow database
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       In case same, return TRUE, 
+*       In case different, return FALSE.
+*******************************************************************************/
+BOOL cph_flow_compare_rules (CPH_FLOW_ENTRY_T *parse_rule, CPH_FLOW_ENTRY_T *db_rule)
+{
+    /* Check direction */
+    if (parse_rule->dir != db_rule->dir)
+        return FALSE;
+
+    /* Check parse_bm */
+    if (parse_rule->parse_bm != db_rule->parse_bm)
+    {
+        return FALSE;
+    }  
+
+    /* Check MH if needed */
+    if (db_rule->parse_bm & CPH_FLOW_PARSE_MH)
+    {
+        if (parse_rule->mh != db_rule->mh)
+            return FALSE;
+    }
+
+    /* Check if it is default rule */
+    if (parse_rule->is_default != db_rule->is_default)
+        return FALSE;
+    
+    /* Check VLAN ID */
+    if (parse_rule->is_default == FALSE)
+    {
+        if ((db_rule->parse_bm & CPH_FLOW_PARSE_EXT_VLAN) ||  
+            (db_rule->parse_bm & CPH_FLOW_PARSE_TWO_VLAN))
+        {
+            if (parse_rule->parse_outer_tci.tpid != db_rule->parse_outer_tci.tpid)
+                return FALSE;
+
+            if (parse_rule->parse_outer_tci.vid != db_rule->parse_outer_tci.vid)
+                return FALSE;
+
+            if (parse_rule->parse_outer_tci.pbits != db_rule->parse_outer_tci.pbits)
+                return FALSE;
+        }
+        if (db_rule->parse_bm & CPH_FLOW_PARSE_TWO_VLAN)
+        {
+            if (parse_rule->parse_inner_tci.tpid != db_rule->parse_inner_tci.tpid)
+                return FALSE;
+
+            if (parse_rule->parse_inner_tci.vid != db_rule->parse_inner_tci.vid)
+                return FALSE;
+
+            if (parse_rule->parse_inner_tci.pbits != db_rule->parse_inner_tci.pbits)
+                return FALSE;
+        }
+    }
+    /* Check Ethernet type if needed */
+    if (db_rule->parse_bm & CPH_FLOW_PARSE_ETH_TYPE)
+    {
+        if (parse_rule->eth_type != db_rule->eth_type)
+            return FALSE;
+    }
+
+    return TRUE;
+}
+
+/******************************************************************************
+* cph_flow_compare_packet_and_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Compare flow packet and rule.
+*
+* INPUTS:
+*       packet_rule - The parsing field values come from the packets
+*       db_rule     - The flow rule stored in flow database
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       In case same, return TRUE, 
+*       In case different, return FALSE.
+*******************************************************************************/
+BOOL cph_flow_compare_packet_and_rule (CPH_FLOW_ENTRY_T *packet_rule, CPH_FLOW_ENTRY_T *db_rule)
+{
+    /* Check direction */
+    if ((packet_rule->dir != db_rule->dir) &&
+        (db_rule->dir != CPH_DIR_NOT_CARE))
+        return FALSE;
+
+    /* Check Multicast protocol */
+     if ((db_rule->parse_bm & CPH_FLOW_PARSE_MC_PROTO) != (packet_rule->parse_bm & CPH_FLOW_PARSE_MC_PROTO))
+    {
+            return FALSE;
+    }
+
+    /* Check MH if needed */
+    if ((db_rule->parse_bm & CPH_FLOW_PARSE_MH) &&
+        (packet_rule->parse_bm & CPH_FLOW_PARSE_MH) )
+    {
+        if (packet_rule->mh != db_rule->mh)
+            return FALSE;
+    }
+
+    /* Check if it is default rule */
+    if (packet_rule->is_default != db_rule->is_default)
+        return FALSE;
+    
+    /* Check VLAN ID */
+    if ((packet_rule->parse_bm & (CPH_FLOW_PARSE_EXT_VLAN | CPH_FLOW_PARSE_TWO_VLAN))
+         != (db_rule->parse_bm & (CPH_FLOW_PARSE_EXT_VLAN | CPH_FLOW_PARSE_TWO_VLAN)))
+        return FALSE;
+    if (packet_rule->is_default == FALSE)
+    {
+        if ((db_rule->parse_bm & CPH_FLOW_PARSE_EXT_VLAN) ||  
+            (db_rule->parse_bm & CPH_FLOW_PARSE_TWO_VLAN))
+        {
+            if ((packet_rule->parse_outer_tci.tpid != db_rule->parse_outer_tci.tpid) &&
+                (db_rule->parse_outer_tci.tpid != MV_CPH_TPID_NOT_CARE_VALUE))
+                return FALSE;
+
+            if ((packet_rule->parse_outer_tci.vid != db_rule->parse_outer_tci.vid) &&
+                (db_rule->parse_outer_tci.vid != MV_CPH_VID_NOT_CARE_VALUE))
+                return FALSE;
+
+            if ((packet_rule->parse_outer_tci.pbits != db_rule->parse_outer_tci.pbits) &&
+                (db_rule->parse_outer_tci.pbits != MV_CPH_PBITS_NOT_CARE_VALUE))
+                return FALSE;
+        }
+        if (db_rule->parse_bm & CPH_FLOW_PARSE_TWO_VLAN)
+        {
+            if ((packet_rule->parse_inner_tci.tpid != db_rule->parse_inner_tci.tpid) &&
+                (db_rule->parse_inner_tci.tpid != MV_CPH_TPID_NOT_CARE_VALUE))
+                return FALSE;
+
+            if ((packet_rule->parse_inner_tci.vid != db_rule->parse_inner_tci.vid) &&
+                (db_rule->parse_inner_tci.vid != MV_CPH_VID_NOT_CARE_VALUE))
+                return FALSE;
+
+            if ((packet_rule->parse_inner_tci.pbits != db_rule->parse_inner_tci.pbits) &&
+                (db_rule->parse_inner_tci.pbits != MV_CPH_PBITS_NOT_CARE_VALUE))
+                return FALSE;
+        }
+    }
+    /* Check Ethernet type if needed */
+    if (db_rule->parse_bm & CPH_FLOW_PARSE_ETH_TYPE)
+    {
+        if (packet_rule->eth_type != db_rule->eth_type)
+            return FALSE;
+    }
+
+    return TRUE;
+}
+
+/******************************************************************************
+* cph_flow_mod_packet()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Modify packet according to flow rule
+*
+* INPUTS:
+*       skb        - Pointer to packet
+*       mh         - Whether has MH or not
+*       flow       - Flow parsing field values
+*       out_offset - Offset of packet
+*       rx         - Whether RX or TX
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_flow_mod_packet (struct sk_buff *skb,  BOOL mh, CPH_FLOW_ENTRY_T *flow, INT32 *out_offset)
+{
+    INT32  offset = 0;
+    UINT16 tpid   = 0;    
+    UINT16 vid    = 0;
+    UINT8  pbits  = 0;
+    BOOL   rc     = MV_OK;
+
+    switch (flow->op_type)
+    {
+        case CPH_VLAN_OP_ASIS:
+            break;            
+        case CPH_VLAN_OP_DISCARD:
+            break;            
+        case CPH_VLAN_OP_ADD:
+            tpid   = flow->mod_outer_tci.tpid ? flow->mod_outer_tci.tpid : MV_TPID_8100;
+            offset = cph_flow_add_vlan(mh, skb->data, tpid, 
+                                       flow->mod_outer_tci.vid, flow->mod_outer_tci.pbits);
+            break;
+        case CPH_VLAN_OP_ADD_COPY_DSCP:
+            tpid   = flow->mod_outer_tci.tpid ? flow->mod_outer_tci.tpid : MV_TPID_8100;
+            offset = cph_flow_add_vlan(mh, skb->data, tpid,
+                                       flow->mod_outer_tci.vid, flow->mod_outer_tci.pbits);
+            break;           
+        case CPH_VLAN_OP_ADD_COPY_OUTER_PBIT:
+            tpid   = flow->mod_outer_tci.tpid ? flow->mod_outer_tci.tpid : MV_TPID_8100;
+            pbits  = (flow->parse_outer_tci.pbits == MV_CPH_PBITS_NOT_CARE_VALUE)? 0:flow->parse_outer_tci.pbits;
+            offset = cph_flow_add_vlan(mh, skb->data, tpid,
+                                       flow->mod_outer_tci.vid, pbits);
+            break;
+        case CPH_VLAN_OP_ADD_COPY_INNER_PBIT:
+            tpid   = flow->mod_outer_tci.tpid ? flow->mod_outer_tci.tpid : MV_TPID_8100;
+            pbits  = (flow->parse_inner_tci.pbits == MV_CPH_PBITS_NOT_CARE_VALUE)? 0:flow->parse_inner_tci.pbits;
+            offset = cph_flow_add_vlan(mh, skb->data, tpid,
+                                       flow->mod_outer_tci.vid, pbits);
+            break;            
+        case CPH_VLAN_OP_ADD_2_TAGS:
+            tpid    = flow->mod_outer_tci.tpid ? flow->mod_outer_tci.tpid : MV_TPID_8100;
+            offset  = cph_flow_add_vlan(mh, skb->data, tpid,
+                                       flow->mod_outer_tci.vid, flow->mod_outer_tci.pbits);
+            tpid    = flow->mod_inner_tci.tpid ? flow->mod_inner_tci.tpid : MV_TPID_8100;   
+            offset += cph_flow_add_vlan(mh, skb->data, tpid,
+                                       flow->mod_inner_tci.vid, flow->mod_inner_tci.pbits);
+            break;            
+        case CPH_VLAN_OP_ADD_2_TAGS_COPY_DSCP:
+            tpid    = flow->mod_outer_tci.tpid ? flow->mod_outer_tci.tpid : MV_TPID_8100;
+            offset  = cph_flow_add_vlan(mh, skb->data, tpid,
+                                        flow->mod_outer_tci.vid, flow->mod_outer_tci.pbits);
+            tpid    = flow->mod_inner_tci.tpid ? flow->mod_inner_tci.tpid : MV_TPID_8100;   
+            offset += cph_flow_add_vlan(mh, skb->data, tpid,
+                                        flow->mod_inner_tci.vid, flow->mod_inner_tci.pbits);            
+            break;            
+        case CPH_VLAN_OP_ADD_2_TAGS_COPY_PBIT:
+            tpid   = flow->mod_outer_tci.tpid ? flow->mod_outer_tci.tpid : MV_TPID_8100;
+            pbits  = (flow->parse_outer_tci.pbits == MV_CPH_PBITS_NOT_CARE_VALUE)? 0:flow->parse_outer_tci.pbits;
+            offset = cph_flow_add_vlan(mh, skb->data, tpid,
+                                       flow->mod_inner_tci.vid, pbits);
+            tpid   = flow->mod_inner_tci.tpid ? flow->mod_inner_tci.tpid : MV_TPID_8100; 
+            pbits  = (flow->parse_inner_tci.pbits == MV_CPH_PBITS_NOT_CARE_VALUE)? 0:flow->parse_inner_tci.pbits;
+            offset += cph_flow_add_vlan(mh, skb->data, tpid,
+                                        flow->mod_inner_tci.vid, pbits);            
+            break;            
+        case CPH_VLAN_OP_REM:
+            offset = cph_flow_del_vlan(mh, skb->data);            
+            break;            
+        case CPH_VLAN_OP_REM_2_TAGS:
+            offset  = cph_flow_del_vlan(mh, skb->data);
+            offset += cph_flow_del_vlan(mh, skb->data);
+            break;            
+        case CPH_VLAN_OP_REPLACE:
+            tpid   = flow->mod_outer_tci.tpid ? flow->mod_outer_tci.tpid : MV_TPID_8100;
+            offset = cph_flow_replace_vlan(mh, skb->data, tpid, 
+                                           flow->mod_outer_tci.vid, flow->mod_outer_tci.pbits);
+            break;            
+        case CPH_VLAN_OP_REPLACE_VID:
+            tpid = flow->mod_outer_tci.tpid ? flow->mod_outer_tci.tpid : MV_TPID_8100;
+
+            pbits  = (flow->parse_outer_tci.pbits == MV_CPH_PBITS_NOT_CARE_VALUE)? 0:flow->parse_outer_tci.pbits;
+            offset = cph_flow_replace_vlan(mh, skb->data, tpid, 
+                                           flow->mod_outer_tci.vid, pbits);
+            break;            
+        case CPH_VLAN_OP_REPLACE_PBIT:
+            tpid = flow->mod_outer_tci.tpid ? flow->mod_outer_tci.tpid : MV_TPID_8100;
+
+            vid    = (flow->parse_outer_tci.vid == MV_CPH_VID_NOT_CARE_VALUE)? 0:flow->parse_outer_tci.vid;
+            offset = cph_flow_replace_vlan(mh, skb->data, tpid, 
+                                           vid, flow->mod_outer_tci.pbits);
+            break;            
+        case CPH_VLAN_OP_REPLACE_INNER_ADD_OUTER:
+            tpid = flow->mod_inner_tci.tpid ? flow->mod_inner_tci.tpid : MV_TPID_8100;
+            offset = cph_flow_replace_vlan(mh, skb->data, tpid, 
+                                           flow->mod_inner_tci.vid, flow->mod_inner_tci.pbits);
+            tpid = flow->mod_outer_tci.tpid ? flow->mod_outer_tci.tpid : MV_TPID_8100;
+            offset += cph_flow_add_vlan(mh, skb->data, tpid, 
+                                       flow->mod_outer_tci.vid, flow->mod_outer_tci.pbits);
+            break;            
+        case CPH_VLAN_OP_REPLACE_INNER_ADD_OUTER_COPY_PBIT:
+            tpid = flow->mod_inner_tci.tpid ? flow->mod_inner_tci.tpid : MV_TPID_8100;
+            pbits  = (flow->parse_inner_tci.pbits == MV_CPH_PBITS_NOT_CARE_VALUE)? 0:flow->parse_inner_tci.pbits;
+            offset = cph_flow_replace_vlan(mh, skb->data, tpid, 
+                                           flow->mod_inner_tci.vid, pbits);
+            tpid = flow->mod_outer_tci.tpid ? flow->mod_outer_tci.tpid : MV_TPID_8100;
+            pbits   = (flow->parse_outer_tci.pbits == MV_CPH_PBITS_NOT_CARE_VALUE)? 0:flow->parse_outer_tci.pbits;            
+            offset += cph_flow_add_vlan(mh, skb->data, tpid, 
+                                        flow->mod_outer_tci.vid, pbits);
+            break;            
+        case CPH_VLAN_OP_REPLACE_INNER_REM_OUTER:
+            offset  = cph_flow_del_vlan(mh, skb->data);
+            tpid    = flow->mod_outer_tci.tpid ? flow->mod_outer_tci.tpid : MV_TPID_8100;
+            offset += cph_flow_replace_vlan(mh, skb->data, tpid, 
+                                            flow->mod_outer_tci.vid, flow->mod_outer_tci.pbits);
+            break;            
+        case CPH_VLAN_OP_REPLACE_2TAGS:
+            tpid    = flow->mod_inner_tci.tpid ? flow->mod_inner_tci.tpid : MV_TPID_8100;
+            offset  = cph_flow_replace_vlan(mh, skb->data, tpid,
+                                           flow->mod_inner_tci.vid, flow->mod_inner_tci.pbits);
+            offset += cph_flow_swap_vlan(mh, skb->data);
+            tpid    = flow->mod_outer_tci.tpid ? flow->mod_outer_tci.tpid : MV_TPID_8100;
+            offset += cph_flow_replace_vlan(mh, skb->data, tpid,
+                                            flow->mod_outer_tci.vid, flow->mod_outer_tci.pbits);
+            break;            
+        case CPH_VLAN_OP_REPLACE_2TAGS_VID:
+            tpid    = flow->mod_inner_tci.tpid ? flow->mod_inner_tci.tpid : MV_TPID_8100;
+            pbits   = (flow->parse_inner_tci.pbits == MV_CPH_PBITS_NOT_CARE_VALUE)? 0:flow->parse_inner_tci.pbits;
+            offset  = cph_flow_replace_vlan(mh, skb->data, tpid,
+                                           flow->mod_inner_tci.vid, pbits);
+            offset += cph_flow_swap_vlan(mh, skb->data);
+            tpid    = flow->mod_outer_tci.tpid ? flow->mod_outer_tci.tpid : MV_TPID_8100;
+            pbits   = (flow->parse_outer_tci.pbits == MV_CPH_PBITS_NOT_CARE_VALUE)? 0:flow->parse_outer_tci.pbits;
+            offset += cph_flow_replace_vlan(mh, skb->data, tpid,
+                                            flow->mod_outer_tci.vid, pbits);
+            break;            
+        case CPH_VLAN_OP_SWAP:
+            offset  = cph_flow_swap_vlan(mh, skb->data);
+            break;
+        default:
+            break;
+    }
+
+    /* Save SKB data offset */
+    skb->data  += offset;
+    skb->len   -= offset;
+    *out_offset = offset;
+    
+    return rc;
+}
+
+/******************************************************************************
+* cph_flow_mod_frwd()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Modify forwarding parameter of transmiting packet according to flow rule
+*
+* INPUTS:
+*       flow        - Flow parsing field values
+*       tx_spec_out - TX descriptor
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_flow_mod_frwd (CPH_FLOW_ENTRY_T *flow, struct mv_eth_tx_spec *tx_spec_out)
+{
+    MV_STATUS rc = MV_OK;
+    
+    tx_spec_out->txp     = flow->pkt_frwd.trg_port;
+    tx_spec_out->txq     = flow->pkt_frwd.trg_queue;
+    if ((FALSE == cph_db_get_tcont_state(tx_spec_out->txp)) ||
+        (flow->op_type == CPH_VLAN_OP_DISCARD))
+        tx_spec_out->txq = MV_INVALID_QUEUE_NUM;
+    tx_spec_out->hw_cmd  = ((flow->pkt_frwd.gem_port << 8)|0x0010);
+    tx_spec_out->tx_func = NULL;
+    tx_spec_out->flags   = MV_ETH_F_MH;
+
+    return rc;
+}
+
+/******************************************************************************
+* cph_flow_send_packet()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: CPH function to handle the received application packets 
+*
+* INPUTS:
+*       dev_out     - Net device
+*       pkt         - Marvell packet information
+*       tx_spec_out - TX descriptor
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns 1. 
+*       On error returns 0.
+*******************************************************************************/
+MV_STATUS cph_flow_send_packet(struct net_device *dev_out, struct eth_pbuf *pkt, 
+    struct mv_eth_tx_spec *tx_spec_out)
+{
+
+
+#if 0
+    struct eth_port *pp = MV_ETH_PRIV(dev_out);
+    int frags = 0;
+    bool tx_spec_ready = false;
+    struct mv_eth_tx_spec tx_spec;
+    u32 tx_cmd;
+    struct tx_queue *txq_ctrl = NULL;
+    struct neta_tx_desc *tx_desc;
+    struct sk_buff  *skb;
+
+
+    skb = (struct sk_buff *)(pkt->osInfo);
+
+    read_lock(&pp->rwlock);
+
+    if (!(netif_running(dev_out))) {
+        printk(KERN_ERR "!netif_running() in %s\n", __func__);
+        goto out;
+    }
+
+    /* Get TXQ (without BM) to send packet generated by Linux */
+    if (tx_spec_ready == false) {
+        tx_spec.txp    = tx_spec_out->txp;
+        tx_spec.txq    = tx_spec_out->txq;
+        tx_spec.hw_cmd = tx_spec_out->hw_cmd;
+        tx_spec.flags  = tx_spec_out->flags;
+    }
+
+    txq_ctrl = &pp->txq_ctrl[tx_spec.txp * CONFIG_MV_ETH_TXQ + tx_spec.txq];
+    if (txq_ctrl == NULL) {
+        printk(KERN_ERR "%s: invalidate txp/txq (%d/%d)\n", __func__, tx_spec.txp, tx_spec.txq);
+        goto out;
+    }
+    spin_lock_irqsave(&txq_ctrl->queue_lock);
+
+#if 0
+#ifdef CONFIG_MV_ETH_TSO
+    /* GSO/TSO */
+    if (skb_is_gso(skb)) {
+        frags = mv_eth_tx_tso(skb, dev_out, &tx_spec, txq_ctrl);
+        goto out;
+    }
+#endif /* CONFIG_MV_ETH_TSO */
+#endif
+
+    frags = 1;
+
+#if 0
+    if (tx_spec.flags & MV_ETH_F_MH) {
+        if (tx_spec.flags & MV_ETH_F_SWITCH)
+            mh = dev_priv->tx_vlan_mh;
+        else
+            mh = pp->tx_mh;
+
+        if (mv_eth_skb_mh_add(skb, mh)) {
+            frags = 0;
+            goto out;
+        }
+    }
+#endif
+    tx_desc = mv_eth_tx_desc_get(txq_ctrl, frags);
+    if (tx_desc == NULL) {
+        frags = 0;
+        goto out;
+    }
+
+    tx_cmd = NETA_TX_L4_CSUM_NOT;
+
+#ifdef CONFIG_MV_PON
+    tx_desc->hw_cmd = tx_spec.hw_cmd;
+#endif
+
+    /* FIXME: beware of nonlinear --BK */
+    tx_desc->dataSize = skb_headlen(skb);
+
+    tx_desc->bufPhysAddr = mvOsCacheFlush(NULL, skb->data, tx_desc->dataSize);
+
+    if (frags == 1) {
+        /*
+         * First and Last descriptor
+         */
+        if (tx_spec.flags & MV_ETH_F_NO_PAD)
+            tx_cmd |= NETA_TX_F_DESC_MASK | NETA_TX_L_DESC_MASK;
+        else
+            tx_cmd |= NETA_TX_FLZ_DESC_MASK;
+
+        tx_desc->command = tx_cmd;
+        mv_eth_tx_desc_flush(tx_desc);
+
+        txq_ctrl->shadow_txq[txq_ctrl->shadow_txq_put_i] = ((MV_ULONG) skb | MV_ETH_SHADOW_SKB);
+        mv_eth_shadow_inc_put(txq_ctrl);
+    } 
+
+    txq_ctrl->txq_count += frags;
+
+#ifdef CONFIG_MV_ETH_DEBUG_CODE
+    if (pp->flags & MV_ETH_F_DBG_TX) {
+        printk(KERN_ERR "\n");
+        printk(KERN_ERR "%s - eth_tx_%lu: port=%d, txp=%d, txq=%d, skb=%p, head=%p, data=%p, size=%d\n",
+               dev_out->name, dev_out->stats.tx_packets, pp->port, tx_spec.txp, tx_spec.txq, skb,
+               skb->head, skb->data, skb->len);
+        mv_eth_tx_desc_print(tx_desc);
+    }
+#endif /* CONFIG_MV_ETH_DEBUG_CODE */
+
+#ifdef CONFIG_MV_PON
+    if (MV_PON_PORT(pp->port))
+        mvNetaPonTxqBytesAdd(pp->port, tx_spec.txp, tx_spec.txq, skb->len);
+#endif /* CONFIG_MV_PON */
+
+    /* Enable transmit */
+    mvNetaTxqPendDescAdd(pp->port, tx_spec.txp, tx_spec.txq, frags);
+
+    STAT_DBG(txq_ctrl->stats.txq_tx += frags);
+
+out:
+    if (frags > 0) {
+        dev_out->stats.tx_packets++;
+        dev_out->stats.tx_bytes += skb->len;
+    } else {
+        dev_out->stats.tx_dropped++;
+        dev_kfree_skb_any(skb);
+    }
+
+#ifndef CONFIG_MV_ETH_TXDONE_ISR
+    if (txq_ctrl) {
+        if (txq_ctrl->txq_count >= mv_ctrl_txdone) {
+            u32 tx_done = mv_eth_txq_done(pp, txq_ctrl);
+
+            STAT_DIST((tx_done < pp->dist_stats.tx_done_dist_size) ? pp->dist_stats.tx_done_dist[tx_done]++ : 0);
+
+        }
+        /* If after calling mv_eth_txq_done, txq_ctrl->txq_count equals frags, we need to set the timer */
+        if ((txq_ctrl->txq_count == frags) && (frags > 0))
+            mv_eth_add_tx_done_timer(pp);
+    }
+#endif /* CONFIG_MV_ETH_TXDONE_ISR */
+
+    if (txq_ctrl)
+        spin_unlock_irqrestore(&txq_ctrl->queue_lock);
+
+    read_unlock(&pp->rwlock);
+
+#endif    
+    return MV_OK;
+}
+
+/******************************************************************************
+* cph_flow_lookup_op_type()
+* _____________________________________________________________________________
+*
+* DESCRIPTION:lookup operation type string according to value
+*
+* INPUTS:
+*       enum_value - The enum value to be matched
+*
+* OUTPUTS:
+*       None
+*
+* RETURNS:
+*       Enum string
+*******************************************************************************/
+CHAR *cph_flow_lookup_op_type(INT32 enum_value)
+{
+    return mtype_lookup_enum_str(&g_enum_array_op_type, enum_value);
+}
+
+/******************************************************************************
+* cph_flow_display_all()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: The function displays valid flow mapping tables and DSCP 
+*              to P-bits mapping tablefor untagged frames.
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32  cph_flow_display_all (VOID)
+{
+    UINT32            idx         = 0;
+    UINT32            rule_idx    = 0;
+    CPH_FLOW_ENTRY_T *p_flow_rule = NULL;
+    INT32             offset      = 0;
+    UINT8             buff[512];
+
+    /* Print flow rule entries */
+    printk(KERN_INFO "MV_CPH Flow Rule Table\n----------------------------------\n");
+    printk(KERN_INFO "Total rule number:%d, Max rule number:%d \n", gs_flow_table.rule_num, CPH_FLOW_ENTRY_NUM);
+
+    printk(KERN_INFO "-------------------------------------------------------------------------------------------------------------------------------------------------------\n");
+    printk(KERN_INFO "                                |Parse outer       |Parse inner       |Mod outer         |Mod Inner         |Forward\n");
+    printk(KERN_INFO "dir default parse_bm mh   ety    tpid   vid  pbits  tpid   vid  pbits  tpid   vid  pbits  tpid   vid  pbits  port queue hwf_queue gem  count    op_type\n");
+    /* Traverse CPH flow rule tale */
+    for (idx = 0, rule_idx = 0; (idx < CPH_FLOW_ENTRY_NUM) && (rule_idx < gs_flow_table.rule_num); idx++)
+    {
+        p_flow_rule = &gs_flow_table.flow_rule[idx];
+        
+        /* Compare parse_bm and parse_key */
+        if (p_flow_rule->valid == TRUE)
+        {
+            rule_idx++;
+
+            printk(KERN_INFO  
+                   "%2.2s  %3.3s     0x%04x   %-4d 0x%04x 0x%04x %-4d %1d      0x%04x %-4d %1d      0x%04x %-4d %1d      0x%04x %-4d %1d      %1d    %1d     %1d         %-4d %-8d %s\n",
+                   cph_app_lookup_dir(p_flow_rule->dir), (p_flow_rule->is_default == TRUE)? "Yes":"No ", 
+                   p_flow_rule->parse_bm,             p_flow_rule->mh,                  p_flow_rule->eth_type,
+                   p_flow_rule->parse_outer_tci.tpid, p_flow_rule->parse_outer_tci.vid, p_flow_rule->parse_outer_tci.pbits, 
+                   p_flow_rule->parse_inner_tci.tpid, p_flow_rule->parse_inner_tci.vid, p_flow_rule->parse_inner_tci.pbits,
+                   p_flow_rule->mod_outer_tci.tpid,   p_flow_rule->mod_outer_tci.vid,   p_flow_rule->mod_outer_tci.pbits,
+                   p_flow_rule->mod_inner_tci.tpid,   p_flow_rule->mod_inner_tci.vid,   p_flow_rule->mod_inner_tci.pbits,
+                   p_flow_rule->pkt_frwd.trg_port,    p_flow_rule->pkt_frwd.trg_queue,  p_flow_rule->pkt_frwd.trg_hwf_queue, p_flow_rule->pkt_frwd.gem_port, 
+                   p_flow_rule->count,                cph_flow_lookup_op_type(p_flow_rule->op_type));
+        }
+    }   
+
+    /* Print  DSCP to P-bits mapping table */
+    printk(KERN_INFO "\nMV_CPH DSCP to P-bits Mapping Table\n----------------------------------\n");  
+    if (gs_dscp_map_table.in_use == 0) {
+        printk(KERN_INFO "No DSCP to P-bits mapping\n");
+    }
+    else {
+        printk(KERN_INFO "DSCP[Pbits]\n");
+
+        memset(buff, 0, sizeof(buff));
+        for (idx = 0; idx < MV_CPH_DSCP_PBITS_TABLE_MAX_SIZE; idx++) {
+            offset += sprintf(buff+offset, "%2.2d[%2.2d] ", idx, gs_dscp_map_table.pbits[idx]);
+            if (((idx+1) % 16) == 0)
+                offset += sprintf(buff+offset, "\n");
+        }
+        printk(KERN_INFO "%s\n", buff);
+    }
+    printk(KERN_INFO "\n");
+ 
+    return MV_OK; 
+}
+
+/******************************************************************************
+* cph_flow_init()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Initializes CPH flow mapping data structure.
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32  cph_flow_init(VOID)
+{
+    MV_STATUS rc = MV_OK;
+
+    rc = cph_flow_db_init();
+    CHECK_API_RETURN_AND_LOG_ERROR(rc, "Fail to call cph_flow_db_init");
+        
+    return rc;
+}
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_flow.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_flow.h
new file mode 100755
index 0000000..b6656f1
--- /dev/null
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_flow.h
@@ -0,0 +1,591 @@
+/*******************************************************************************
+Copyright (C) Marvell International Ltd. and its affiliates
+
+This software file (the "File") is owned and distributed by Marvell 
+International Ltd. and/or its affiliates ("Marvell") under the following
+alternative licensing terms.  Once you have made an election to distribute the
+File under one of the following license alternatives, please (i) delete this
+introductory statement regarding license alternatives, (ii) delete the two
+license alternatives that you have not elected to use and (iii) preserve the
+Marvell copyright notice above.
+
+********************************************************************************
+Marvell Commercial License Option
+
+If you received this File from Marvell and you have entered into a commercial
+license agreement (a "Commercial License") with Marvell, the File is licensed
+to you under the terms of the applicable Commercial License.
+
+********************************************************************************
+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.
+********************************************************************************
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or 
+modify this File under the following licensing terms. 
+Redistribution and use in source and binary forms, with or without modification, 
+are permitted provided that the following conditions are met:
+
+    *   Redistributions of source code must retain the above copyright notice,
+        this list of conditions and the following disclaimer. 
+
+    *   Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution. 
+
+    *   Neither the name of Marvell nor the names of its contributors may be 
+        used to endorse or promote products derived from this software without 
+        specific prior written permission. 
+    
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+********************************************************************************
+* mv_cph_flow.h
+*
+* DESCRIPTION: Marvell CPH(CPH Packet Handler) flow module to handle the 
+*              flow mapping, VLAN modification of data traffic
+*
+* DEPENDENCIES:
+*               None
+*
+* CREATED BY:   VictorGu
+*
+* DATE CREATED: 12Dec2011
+*
+* FILE REVISION NUMBER:
+*               Revision: 1.1
+*
+*
+*******************************************************************************/
+#ifndef _MV_CPH_FLOW_H_
+#define _MV_CPH_FLOW_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/****************************************************************************** 
+ *                        Data Enum and Structure                                                                                                                                  
+ ******************************************************************************/
+#define MV_CPH_TPID_NOT_CARE_VALUE       (0)     /* Does not care for TPID     */
+#define MV_CPH_VID_NOT_CARE_VALUE        (4096)  /* Does not care for VID      */
+#define MV_CPH_PBITS_NOT_CARE_VALUE      (8)     /* Does not care for P-bits   */
+#define MV_CPH_DSCP_NOT_CARE_VALUE       (64)    /* Does not care for DSCP     */
+
+#define MV_CPH_TPID_INVALID_VALUE        (0xFFFF)/* No valid TPID              */
+#define MV_CPH_VID_INVALID_VALUE         (0xFFFF)/* No valid VID               */
+#define MV_CPH_PBITS_INVALID_VALUE       (0xFF)  /* No valid P-bits            */
+#define MV_CPH_DSCP_INVALID_VALUE        (0xFF)  /* No valid DSCP              */
+
+#define MV_CPH_DEFAULT_UNTAG_RULE        (4096+1)/* Default untagged  rule     */
+#define MV_CPH_DEFAULT_SINGLE_TAG_RULE   (4096+2)/* Default sinlge tagged  rule*/
+#define MV_CPH_DEFAULT_DOUBLE_UNTAG_RULE (4096+3)/* Default double tagged  rule*/
+
+/* CPH flow mapping rule definition
+------------------------------------------------------------------------------*/
+typedef enum
+{
+    CPH_VLAN_OP_ASIS                               = 0,
+    CPH_VLAN_OP_DISCARD                            = 1,      
+    CPH_VLAN_OP_ADD                                = 2,  
+    CPH_VLAN_OP_ADD_COPY_DSCP                      = 3,
+    CPH_VLAN_OP_ADD_COPY_OUTER_PBIT                = 4,
+    CPH_VLAN_OP_ADD_COPY_INNER_PBIT                = 5,
+    CPH_VLAN_OP_ADD_2_TAGS                         = 6,
+    CPH_VLAN_OP_ADD_2_TAGS_COPY_DSCP               = 7,
+    CPH_VLAN_OP_ADD_2_TAGS_COPY_PBIT               = 8,
+    CPH_VLAN_OP_REM                                = 9,
+    CPH_VLAN_OP_REM_2_TAGS                         = 10,
+    CPH_VLAN_OP_REPLACE                            = 11,
+    CPH_VLAN_OP_REPLACE_VID                        = 12,
+    CPH_VLAN_OP_REPLACE_PBIT                       = 13,
+    CPH_VLAN_OP_REPLACE_INNER_ADD_OUTER            = 14,
+    CPH_VLAN_OP_REPLACE_INNER_ADD_OUTER_COPY_PBIT  = 15,
+    CPH_VLAN_OP_REPLACE_INNER_REM_OUTER            = 16,
+    CPH_VLAN_OP_REPLACE_2TAGS                      = 17,
+    CPH_VLAN_OP_REPLACE_2TAGS_VID                  = 18,
+    CPH_VLAN_OP_SWAP                               = 19
+} CPH_VLAN_OP_TYPE_E;
+
+typedef struct cph_flow_frwd {
+    UINT8  trg_port;
+    UINT8  trg_queue;
+    UINT8  trg_hwf_queue;
+    UINT16 gem_port;
+} CPH_FLOW_FRWD_T;
+
+typedef struct cph_flow_tci {
+    UINT16  tpid;
+    UINT16  vid;
+    UINT8   pbits;
+} CPH_FLOW_TCI_T;
+
+typedef enum
+{
+    CPH_FLOW_PARSE_MH        = 0x01,  /* parsing Marvell header                          */
+    CPH_FLOW_PARSE_EXT_VLAN  = 0x02,  /* parsing external VLAN tag                       */
+    CPH_FLOW_PARSE_TWO_VLAN  = 0x04,  /* parsing both of external and internal VLAN tags */
+    CPH_FLOW_PARSE_ETH_TYPE  = 0x08,  /* parsing Ethernet type                           */
+    CPH_FLOW_PARSE_MC_PROTO  = 0x10,  /* parsing multicast protocol                      */
+} CPH_FLOW_PARSE_E;
+
+typedef struct
+{
+    BOOL               valid;
+    CPH_DIR_E          dir;
+    CPH_FLOW_PARSE_E   parse_bm;
+    BOOL               is_default;
+    UINT16             mh;
+    CPH_FLOW_TCI_T     parse_outer_tci;
+    CPH_FLOW_TCI_T     parse_inner_tci;
+    UINT16             eth_type;
+    CPH_VLAN_OP_TYPE_E op_type;
+    CPH_FLOW_TCI_T     mod_outer_tci;
+    CPH_FLOW_TCI_T     mod_inner_tci;
+    CPH_FLOW_FRWD_T    pkt_frwd;
+    UINT32             count;
+} CPH_FLOW_ENTRY_T;
+
+#define CPH_FLOW_ENTRY_NUM   (512)
+
+typedef struct
+{
+    UINT32             rule_num;
+    spinlock_t         flow_lock;
+    CPH_FLOW_ENTRY_T   flow_rule[CPH_FLOW_ENTRY_NUM];
+} CPH_FLOW_TABLE_T;
+
+/* DSCP to P-bits mapping table definition
+------------------------------------------------------------------------------*/
+#define MV_CPH_DSCP_PBITS_TABLE_MAX_SIZE  (64)
+typedef struct {
+    UINT32 in_use;
+    UINT8  pbits[MV_CPH_DSCP_PBITS_TABLE_MAX_SIZE];
+} CPH_DSCP_PBITS_T;
+
+
+/****************************************************************************** 
+ *                        Function Declaration                                                                                                                                  
+ ******************************************************************************/
+/******************************************************************************
+* cph_flow_add_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Sets flow mapping rule
+*
+* INPUTS:
+*       cph_flow - VLAN ID, 802.1p value, pkt_fwd information.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_flow_add_rule(CPH_FLOW_ENTRY_T *cph_flow);
+
+/******************************************************************************
+* cph_flow_del_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Deletes flow mapping rule
+*
+* INPUTS:
+*       cph_flow - VLAN ID, 802.1p value, pkt_fwd information.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32  cph_flow_del_rule(CPH_FLOW_ENTRY_T *cph_flow);
+
+/******************************************************************************
+* cph_flow_clear_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Clears all flow mapping rules
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_flow_clear_rule(VOID);
+
+/******************************************************************************
+* cph_flow_clear_rule_by_mh()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Clears flow mapping rules by MH
+*
+* INPUTS:
+*       mh   -  Marvell header.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_flow_clear_rule_by_mh(UINT16 mh);
+
+/******************************************************************************
+* cph_flow_get_tag_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Gets flow mapping rule for tagged frames.
+*
+* INPUTS:
+*       cph_flow - Input vid, pbits, dir
+*
+* OUTPUTS:
+*       cph_flow - output packet forwarding information, including GEM port, 
+*                  T-CONT, queue and packet modification for VID, P-bits.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32  cph_flow_get_rule(CPH_FLOW_ENTRY_T *cph_flow);
+
+/******************************************************************************
+* cph_flow_set_dscp_map()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Sets DSCP to P-bits mapping rules
+*
+* INPUTS:
+*       dscp_map  - DSCP to P-bits mapping rules.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_flow_set_dscp_map(CPH_DSCP_PBITS_T *dscp_map);
+
+/******************************************************************************
+* cph_flow_del_dscp_map()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Deletes DSCP to P-bits mapping rules
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_flow_del_dscp_map(VOID);
+
+/******************************************************************************
+* cph_flow_add_vlan()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Add one VLAN tag behind of source MAC address.
+*
+* INPUTS:
+*       mh     - Whether has MH or not
+*       p_data - Pointer to packet
+*       tpid   - Type of VLAN ID
+*       vid    - VLAN to be added
+*       pbits  - P-bits value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       The shift of SKB data.
+*******************************************************************************/
+INLINE INT32 cph_flow_add_vlan(BOOL mh, UINT8 *p_data, UINT16 tpid, UINT16 vid, UINT8 pbits);
+
+/******************************************************************************
+* cph_flow_del_vlan()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Delete one VLAN tag behind of source MAC address.
+*
+* INPUTS:
+*       mh     - Whether has MH or not
+*       p_data - Pointer to packet.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       The shift of SKB data.
+*******************************************************************************/
+INLINE INT32 cph_flow_del_vlan(BOOL mh, UINT8 *p_data);
+
+/******************************************************************************
+* cph_flow_replace_vlan()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Replace one VLAN tag behind of source MAC address.
+*
+* INPUTS:
+*       mh     - Whether has MH or not
+*       p_data - Pointer to packet
+*       tpid   - Type of VLAN ID
+*       vid    - VLAN to be added
+*       pbits  - P-bits value
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       The shift of SKB data.
+*******************************************************************************/
+INLINE INT32 cph_flow_replace_vlan(BOOL mh, UINT8 *p_data, UINT16 tpid, UINT16 vid, UINT8 pbits);
+
+/******************************************************************************
+* cph_flow_swap_vlan()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Swap between two VLAN tag.
+*
+* INPUTS:
+*       mh     - Whether has MH or not
+*       p_data - Pointer to packet
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       The shift of SKB data.
+*******************************************************************************/
+INLINE INT32 cph_flow_swap_vlan(BOOL mh, UINT8 *p_data);
+
+/******************************************************************************
+* cph_flow_strip_vlan()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Delete all VLAN tags behind of source MAC address.
+*
+* INPUTS:
+*       mh     - Whether has MH or not
+*       p_data - Pointer to packet.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       The shift of SKB data.
+*******************************************************************************/
+INLINE INT32 cph_flow_strip_vlan(BOOL mh, UINT8 *p_data);
+
+/******************************************************************************
+* cph_flow_compare_rules()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Comparse two flow rules.
+*
+* INPUTS:
+*       parse_rule  - The parsing field values come from the packets
+*       db_rule     - The flow rule stored in flow database
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       In case same, return TRUE, 
+*       In case different, return FALSE.
+*******************************************************************************/
+BOOL cph_flow_compare_rules (CPH_FLOW_ENTRY_T *parse_rule, CPH_FLOW_ENTRY_T *db_rule);
+
+/******************************************************************************
+* cph_flow_compare_packet_and_rule()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Compare flow packet and rule.
+*
+* INPUTS:
+*       packet_rule - The parsing field values come from the packets
+*       db_rule     - The flow rule stored in flow database
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       In case same, return TRUE, 
+*       In case different, return FALSE.
+*******************************************************************************/
+BOOL cph_flow_compare_packet_and_rule (CPH_FLOW_ENTRY_T *packet_rule, CPH_FLOW_ENTRY_T *db_rule);
+
+/******************************************************************************
+* cph_flow_parse_packet()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Parse packet and output flow information.
+*
+* INPUTS:
+*       port - Source GMAC port
+*       data - Pointer to packet
+*       rx   - Whether in RX dir
+*
+* OUTPUTS:
+*       flow - Flow parsing field values
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_flow_parse_packet (INT32 port, UINT8 *data, BOOL rx, BOOL mh, CPH_FLOW_ENTRY_T *flow);
+
+/******************************************************************************
+* cph_flow_mod_packet()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Modify packet according to flow rule
+*
+* INPUTS:
+*       skb        - Pointer to packet
+*       mh         - Whether has MH or not
+*       flow       - Flow parsing field values
+*       out_offset - Offset of packet
+*       rx         - Whether RX or TX
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_flow_mod_packet (struct sk_buff *skb,  BOOL mh, CPH_FLOW_ENTRY_T *flow, INT32 *out_offset);
+
+/******************************************************************************
+* cph_flow_mod_frwd()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Modify forwarding parameter of transmiting packet according to flow rule
+*
+* INPUTS:
+*       flow        - Flow parsing field values
+*       tx_spec_out - TX descriptor
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+MV_STATUS cph_flow_mod_frwd (CPH_FLOW_ENTRY_T *flow, struct mv_eth_tx_spec *tx_spec_out);
+
+/******************************************************************************
+* cph_flow_send_packet()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: CPH function to handle the received application packets 
+*
+* INPUTS:
+*       dev_out     - Net device
+*       pkt         - Marvell packet information
+*       tx_spec_out - TX descriptor
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns 1. 
+*       On error returns 0.
+*******************************************************************************/
+MV_STATUS cph_flow_send_packet(struct net_device *dev_out,  struct eth_pbuf *pkt,
+    struct mv_eth_tx_spec *tx_spec_out);
+
+/******************************************************************************
+* cph_flow_lookup_op_type()
+* _____________________________________________________________________________
+*
+* DESCRIPTION:lookup operation type string according to value
+*
+* INPUTS:
+*       enum_value - The enum value to be matched
+*
+* OUTPUTS:
+*       None
+*
+* RETURNS:
+*       Enum string
+*******************************************************************************/
+CHAR *cph_flow_lookup_op_type(INT32 enum_value);
+
+/******************************************************************************
+* cph_flow_display_all()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: The function displays valid flow mapping tables and DSCP 
+*              to P-bits mapping tablefor untagged frames.
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_flow_display_all(VOID);
+
+/******************************************************************************
+* cph_flow_init()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Initializes CPH flow mapping data structure.
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_flow_init(VOID);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _MV_CPH_FLOW_MAP_H_ */
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_header.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_header.h
new file mode 100755
index 0000000..dcfde53
--- /dev/null
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_header.h
@@ -0,0 +1,97 @@
+/*******************************************************************************
+Copyright (C) Marvell International Ltd. and its affiliates
+
+This software file (the "File") is owned and distributed by Marvell 
+International Ltd. and/or its affiliates ("Marvell") under the following
+alternative licensing terms.  Once you have made an election to distribute the
+File under one of the following license alternatives, please (i) delete this
+introductory statement regarding license alternatives, (ii) delete the two
+license alternatives that you have not elected to use and (iii) preserve the
+Marvell copyright notice above.
+
+********************************************************************************
+Marvell Commercial License Option
+
+If you received this File from Marvell and you have entered into a commercial
+license agreement (a "Commercial License") with Marvell, the File is licensed
+to you under the terms of the applicable Commercial License.
+
+********************************************************************************
+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.
+********************************************************************************
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or 
+modify this File under the following licensing terms. 
+Redistribution and use in source and binary forms, with or without modification, 
+are permitted provided that the following conditions are met:
+
+    *   Redistributions of source code must retain the above copyright notice,
+        this list of conditions and the following disclaimer. 
+
+    *   Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution. 
+
+    *   Neither the name of Marvell nor the names of its contributors may be 
+        used to endorse or promote products derived from this software without 
+        specific prior written permission. 
+    
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+********************************************************************************
+* mv_cph_header.h
+*
+* DESCRIPTION: Marvell CPH(CPH Packet Handler) header file
+*
+* DEPENDENCIES:
+*               None
+*
+* CREATED BY:   VictorGu
+*
+* DATE CREATED: 22Jan2013
+*
+* FILE REVISION NUMBER:
+*               Revision: 1.0
+*
+*
+*******************************************************************************/
+#ifndef _MV_CPH_HEADER_H_
+#define _MV_CPH_HEADER_H_
+
+/* Include Files
+------------------------------------------------------------------------------*/
+#include <mvCommon.h>
+#include <mv_neta/net_dev/mv_netdev.h>
+
+#include "mv_cph_infra.h"
+#include "mv_cph_app.h"
+#include "mv_cph_flow.h"
+#include "mv_cph_db.h"
+#include "mv_cph_api.h"
+#include "mv_cph_mng_if.h"
+#include "mv_cph_dev.h"
+#include "mv_cph_netdev.h"
+#include "mv_cph_sysfs.h"
+
+#endif /* _MV_CPH_HEADER_H_ */
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_infra.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_infra.c
new file mode 100755
index 0000000..54b295b
--- /dev/null
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_infra.c
@@ -0,0 +1,246 @@
+/*******************************************************************************
+Copyright (C) Marvell International Ltd. and its affiliates
+
+This software file (the "File") is owned and distributed by Marvell 
+International Ltd. and/or its affiliates ("Marvell") under the following
+alternative licensing terms.  Once you have made an election to distribute the
+File under one of the following license alternatives, please (i) delete this
+introductory statement regarding license alternatives, (ii) delete the two
+license alternatives that you have not elected to use and (iii) preserve the
+Marvell copyright notice above.
+
+********************************************************************************
+Marvell Commercial License Option
+
+If you received this File from Marvell and you have entered into a commercial
+license agreement (a "Commercial License") with Marvell, the File is licensed
+to you under the terms of the applicable Commercial License.
+
+********************************************************************************
+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.
+********************************************************************************
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or 
+modify this File under the following licensing terms. 
+Redistribution and use in source and binary forms, with or without modification, 
+are permitted provided that the following conditions are met:
+
+    *   Redistributions of source code must retain the above copyright notice,
+        this list of conditions and the following disclaimer. 
+
+    *   Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution. 
+
+    *   Neither the name of Marvell nor the names of its contributors may be 
+        used to endorse or promote products derived from this software without 
+        specific prior written permission. 
+    
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+********************************************************************************
+* mv_cph_infra.c
+*
+* DESCRIPTION: Include user space infrastructure modules definitions
+*
+* DEPENDENCIES:
+*               None
+*
+* CREATED BY:   VictorGu
+*
+* DATE CREATED: 22Jan2013
+*
+* FILE REVISION NUMBER:
+*               Revision: 1.0
+*
+*
+*******************************************************************************/
+#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>
+
+#include "mv_cph_header.h"
+
+/******************************************************************************
+ * Variable Definition
+ ******************************************************************************/
+static CHAR g_unknown_str[] = "<unknown>";
+
+/******************************************************************************
+ * Function Definition
+ ******************************************************************************/
+/******************************************************************************
+* mindex_tpm_src_to_app_port()
+*
+* DESCRIPTION:Convert TPM source port to application UNI port 
+*
+* INPUTS:
+*       src_port    - TPM source port
+*
+* OUTPUTS:
+*       Application UNI port index
+*
+* RETURNS:
+*       On success, the function returns application UNI port index. 
+*       On error return invalid application UNI port index.
+*******************************************************************************/
+MV_APP_ETH_PORT_UNI_E mindex_tpm_src_to_app_port(tpm_src_port_type_t src_port)
+{
+    MV_APP_ETH_PORT_UNI_E app_port = MV_APP_ETH_PORT_INVALID;
+
+    /* Should modify below code in case support more than four UNI ports */
+    if (src_port <= TPM_SRC_PORT_UNI_3)
+    {
+        app_port = MV_APP_ETH_PORT_INDEX_MIN + (src_port - TPM_SRC_PORT_UNI_0);
+    }
+    
+    return app_port;
+}
+
+/******************************************************************************
+* mindex_mh_to_app_llid()
+*
+* DESCRIPTION:Convert Marvell header to application LLID 
+*
+* INPUTS:
+*       mh  - Marvell header
+*
+* OUTPUTS:
+*       Application LLID
+*
+* RETURNS:
+*       On success, the function returns application LLID. 
+*       On error return invalid application LLID.
+*******************************************************************************/
+MV_TCONT_LLID_E mindex_mh_to_app_llid(UINT16 mh)
+{
+    MV_TCONT_LLID_E llid       = MV_TCONT_LLID_INVALID;
+    UINT8           llid_index = 0;
+
+    llid_index = (mh >> 8) & 0x0f;
+    
+    if (llid_index > 0)
+    {
+        if (0x0f == llid_index)
+        {
+            llid = MV_TCONT_LLID_BROADCAST;
+        }
+        else
+        {
+            llid = llid_index -1;
+            if(llid > MV_TCONT_LLID_7)
+                llid = MV_TCONT_LLID_INVALID;
+        }
+    }
+    
+    return llid;
+}
+
+/******************************************************************************
+* mtype_get_digit_num()
+*
+* DESCRIPTION:Convert character string to digital number 
+*
+* INPUTS:
+*       str   - Character string
+*
+* OUTPUTS:
+*       None
+*
+* RETURNS:
+*       Digital numbe
+*******************************************************************************/
+UINT32 mtype_get_digit_num(const CHAR  *str)
+{
+    UINT32  val = 0;
+
+    if ((str[1] == 'x') || (str[1] == 'X'))
+        sscanf(&str[2], "%x", &val);
+    else
+        val = simple_strtoul(str, NULL, 10);
+
+    return val;
+}
+
+/******************************************************************************
+* mtype_lookup_enum_str()
+* _____________________________________________________________________________
+*
+* DESCRIPTION:lookup enum string according to enum value
+*
+* INPUTS:
+*       p_enum_array   - Pointer to enum array
+*       enum_value     - The enum value to be matched
+*
+* OUTPUTS:
+*       None
+*
+* RETURNS:
+*       Enum string
+*******************************************************************************/
+CHAR *mtype_lookup_enum_str(MV_ENUM_ARRAY_T *p_enum_array, INT32 enum_value)
+{
+    INT32 idx;
+    
+    for (idx = 0; idx < p_enum_array->enum_num; idx++)
+    {
+        if (enum_value == p_enum_array->enum_array[idx].enum_value)
+            return p_enum_array->enum_array[idx].enum_str;
+    }
+    return g_unknown_str;
+}
+
+/******************************************************************************
+* mutils_is_frwd_broadcast_packet()
+* _____________________________________________________________________________
+*
+* DESCRIPTION:Check whether packet is directly forwarded broadcast one
+*
+* INPUTS:
+*       data   - packet data
+*
+* OUTPUTS:
+*       None
+*
+* RETURNS:
+*       TRUE: broadcast packet, FALSE:none broadcast packet
+*******************************************************************************/
+BOOL mutils_is_frwd_broadcast_packet(char *data)
+{
+    char bc_mac[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+    char *p_data;
+
+    p_data = data + MV_ETH_MH_SIZE;
+
+    if (!memcmp(p_data, &bc_mac[0], sizeof(bc_mac))) {
+        return TRUE;
+    }
+    else
+        return FALSE;
+}
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_infra.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_infra.h
new file mode 100755
index 0000000..a38d1af
--- /dev/null
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_infra.h
@@ -0,0 +1,420 @@
+/*******************************************************************************
+Copyright (C) Marvell International Ltd. and its affiliates
+
+This software file (the "File") is owned and distributed by Marvell 
+International Ltd. and/or its affiliates ("Marvell") under the following
+alternative licensing terms.  Once you have made an election to distribute the
+File under one of the following license alternatives, please (i) delete this
+introductory statement regarding license alternatives, (ii) delete the two
+license alternatives that you have not elected to use and (iii) preserve the
+Marvell copyright notice above.
+
+********************************************************************************
+Marvell Commercial License Option
+
+If you received this File from Marvell and you have entered into a commercial
+license agreement (a "Commercial License") with Marvell, the File is licensed
+to you under the terms of the applicable Commercial License.
+
+********************************************************************************
+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.
+********************************************************************************
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or 
+modify this File under the following licensing terms. 
+Redistribution and use in source and binary forms, with or without modification, 
+are permitted provided that the following conditions are met:
+
+    *   Redistributions of source code must retain the above copyright notice,
+        this list of conditions and the following disclaimer. 
+
+    *   Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution. 
+
+    *   Neither the name of Marvell nor the names of its contributors may be 
+        used to endorse or promote products derived from this software without 
+        specific prior written permission. 
+    
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+********************************************************************************
+* mv_cph_infra.h
+*
+* DESCRIPTION: Include user space infrastructure modules definitions
+*
+* DEPENDENCIES:
+*               None
+*
+* CREATED BY:   VictorGu
+*
+* DATE CREATED: 22Jan2013
+*
+* FILE REVISION NUMBER:
+*               Revision: 1.0
+*
+*
+*******************************************************************************/
+#ifndef _MV_CPH_INFRA_H_
+#define _MV_CPH_INFRA_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/******************************************************************************
+ * Data Type Definition
+ ******************************************************************************/
+typedef          char      INT8;
+typedef unsigned char      UINT8;
+typedef          short     INT16;
+typedef unsigned short     UINT16;
+typedef          int       INT32;
+typedef unsigned int       UINT32;
+typedef          long long INT64;
+typedef unsigned long long UINT64;
+typedef          char      CHAR;
+typedef unsigned char      UCHAR;
+typedef          long      LONG;
+typedef unsigned long      ULONG;
+
+typedef void               VOID;
+typedef int                STATUS;
+
+typedef bool               BOOL;
+#define TRUE               true
+#define FALSE              false
+
+
+/******************************************************************************
+ * Port Index Definition
+ ******************************************************************************/
+/* Ethernet port index */
+#define MV_APP_ETH_PORT_INDEX_MIN 1
+#define MV_APP_ETH_PORT_INDEX_MAX 4
+#define MV_APP_ETH_PORT_NUM       (MV_APP_ETH_PORT_INDEX_MAX-MV_APP_ETH_PORT_INDEX_MIN+1)
+
+typedef enum {
+    MV_APP_ETH_PORT_UNI_0   = 1,
+    MV_APP_ETH_PORT_UNI_1,
+    MV_APP_ETH_PORT_UNI_2,
+    MV_APP_ETH_PORT_UNI_3,
+    MV_APP_ETH_PORT_UNI_4,
+    MV_APP_ETH_PORT_UNI_5,
+    MV_APP_ETH_PORT_UNI_6,
+    MV_APP_ETH_PORT_UNI_7,
+    MV_APP_ETH_PORT_INVALID   
+}  MV_APP_ETH_PORT_UNI_E;
+
+/* VoIP port index */
+#define MV_APP_VOIP_PORT_INDEX_MIN 1
+#define MV_APP_VOIP_PORT_INDEX_MAX 2
+#define MV_APP_VOIP_PORT_NUM       (MV_APP_VOIP_PORT_INDEX_MAX-MV_APP_VOIP_PORT_INDEX_MIN+1)
+
+typedef enum {
+    MV_APP_VOIP_PORT_0 = 1,
+    MV_APP_VOIP_PORT_1
+}  MV_APP_VOIP_PORT_E;
+
+/* WIFI SSID port index */
+#define MV_APP_SSID_INDEX_MIN 1
+#define MV_APP_SSID_INDEX_MAX 4
+typedef enum {
+    MV_APP_SSID_0 = 1,
+    MV_APP_SSID_1,
+    MV_APP_SSID_2,
+    MV_APP_SSID_3,
+    MV_APP_SSID_4,
+    MV_APP_SSID_5,
+    MV_APP_SSID_6,
+    MV_APP_SSID_7
+}  MV_APP_SSID_E;
+
+/* USB port index */
+#define MV_APP_USB_PORT_INDEX_MIN 1
+#define MV_APP_USB_PORT_INDEX_MAX 2
+#define MV_APP_USB_PORT_NUM       (MV_APP_USB_PORT_INDEX_MAX-MV_APP_USB_PORT_INDEX_MIN+1)
+
+typedef enum {
+    MV_APP_USB_PORT_0 = 1,
+    MV_APP_USB_PORT_1
+}  MV_APP_USB_PORT_E;
+
+/******************************************************************************
+ * GE MAC Port Index Definition
+ ******************************************************************************/
+typedef enum {
+    MV_APP_GMAC_PORT_0       = 0,
+    MV_APP_GMAC_PORT_1,
+    MV_APP_PON_MAC_PORT,
+    MV_APP_GMAC_PORT_NUM     = 3,
+    MV_APP_GMAC_PORT_INVALID = 3,
+}  MV_APP_GMAC_PORT_E;
+
+/******************************************************************************
+ * T-CONT/LLID Index Definition
+ ******************************************************************************/
+#define MV_TCONT_LLID_MIN 0
+#define MV_TCONT_LLID_MAX 7
+#define MV_TCONT_LLID_NUM (MV_TCONT_LLID_MAX-MV_TCONT_LLID_MIN+1)
+
+typedef enum {
+    MV_TCONT_LLID_0 = 0,
+    MV_TCONT_LLID_1,
+    MV_TCONT_LLID_2,
+    MV_TCONT_LLID_3,
+    MV_TCONT_LLID_4,
+    MV_TCONT_LLID_5,
+    MV_TCONT_LLID_6 ,
+    MV_TCONT_LLID_7, 
+    MV_TCONT_LLID_8,
+    MV_TCONT_LLID_9,
+    MV_TCONT_LLID_10,
+    MV_TCONT_LLID_11,
+    MV_TCONT_LLID_12,
+    MV_TCONT_LLID_13,
+    MV_TCONT_LLID_14 ,
+    MV_TCONT_LLID_15, 
+    MV_TCONT_LLID_BROADCAST,
+    MV_TCONT_LLID_INVALID
+} MV_TCONT_LLID_E;
+
+/******************************************************************************
+ * WAN and LAN Index Definition
+ ******************************************************************************/
+typedef enum {
+    MV_APP_PORT_WAN = 0,
+    MV_APP_PORT_LAN,
+    MV_APP_PORT_INVALID,
+}  MV_APP_PORT_TYPE_E;
+
+/******************************************************************************
+ * GEM Port Index Definition
+ ******************************************************************************/
+#define MV_GEM_PORT_MIN  0
+#define MV_GEM_PORT_MAX  4095
+#define MV_GEM_PORT_NUM  (MV_GEM_PORT_MAX-MV_GEM_PORT_MIN+1)
+#define MV_GEM_PORT_MASK 0x0FFF
+
+/******************************************************************************
+ * Queue Index Definition
+ ******************************************************************************/
+#define MV_QUEUE_MIN  0
+#define MV_QUEUE_MAX  7
+#define MV_QUEUE_NUM  (MV_QUEUE_MAX-MV_QUEUE_MIN+1)
+#define MV_INVALID_QUEUE_NUM  (0xFF) /* Invalid queue number  */
+
+typedef enum {
+    MV_QUEUE_0 = 0,
+    MV_QUEUE_1,
+    MV_QUEUE_2,
+    MV_QUEUE_3,
+    MV_QUEUE_4,
+    MV_QUEUE_5,
+    MV_QUEUE_6,
+    MV_QUEUE_7
+}  MV_QUEUE_E;
+
+/******************************************************************************
+ * VLAN ID/P-bits Index Definition
+ ******************************************************************************/
+#define MV_VLAN_ID_MIN  0
+#define MV_VLAN_ID_MAX 4095
+#define MV_VLAN_ID_NUM (MV_VLAN_ID_MAX-MV_VLAN_ID_MIN+1)
+#define MV_VLAN_ID_MASK 0x0FFF
+#define MV_VLAN_ID_INVALID_VALUE 0xFFFF
+
+#define MV_PBITS_MIN   0
+#define MV_PBITS_MAX   7
+#define MV_PBITS_NUM  (MV_PBITS_MAX-MV_PBITS_MIN+1)
+#define MV_PBITS_SHIFT 13
+#define MV_PBITS_MASK 0x07
+#define MV_PBITS_INVALID_VALUE 0xFF
+
+
+/******************************************************************************
+ * WAN Port State Definition
+ ******************************************************************************/
+typedef enum {
+    MV_GE_PORT_INACTIVE = 0,
+    MV_GE_PORT_ACTIVE,
+    MV_GE_PORT_INVALID,
+}  MV_GE_PORT_STATE_E;
+
+/* Enum for well known TPID
+------------------------------------------------------------------------------*/
+#define MV_TPID_8100 0x8100
+#define MV_TPID_88A8 0x88A8
+#define MV_TPID_9100 0x9100
+#define MV_CPH_TPID_LEN      (2)
+#define MV_CPH_VLAN_TAG_LEN  (2)
+#define MV_CPH_ETH_TYPE_LEN  (2)
+#define MV_IPV4_PROTO_OFFSET (9)
+#define MV_ICMPV6_TYPE_MLD   (1)
+#define MV_CPH_ETH_TYPE_IPV4 (0x0800)
+#define MV_CPH_ETH_TYPE_IPV6 (0x86DD)
+
+
+/******************************************************************************
+ * TPM Source port - used to specify through which port the packet entered the processor
+ ******************************************************************************/
+typedef enum {
+    TPM_SRC_PORT_UNI_0,        /* upstream */
+    TPM_SRC_PORT_UNI_1,        /* upstream */
+    TPM_SRC_PORT_UNI_2,        /* upstream */
+    TPM_SRC_PORT_UNI_3,        /* upstream */
+    TPM_SRC_PORT_UNI_4,        /* upstream */
+    TPM_SRC_PORT_UNI_5,        /* upstream */
+    TPM_SRC_PORT_UNI_6,        /* upstream */
+    TPM_SRC_PORT_UNI_7,        /* upstream */
+    TPM_SRC_PORT_UNI_VIRT,     /* upstream */
+    TPM_SRC_PORT_WAN,          /* downstram */
+    TPM_SRC_PORT_UNI_ANY,      /* upstream - all UNI ports */
+    TPM_SRC_PORT_WAN_OR_LAN,   /* Any Port, currently not supported */
+    TPM_SRC_PORT_ILLEGAL = 0xFF
+} tpm_src_port_type_t;
+
+/******************************************************************************
+ * Check API return value
+ ******************************************************************************/
+#define CHECK_API_RETURN_AND_LOG_ERROR(ret, log) \
+{\
+    INT32 rc = 0 ;\
+    if((rc = (ret)) != 0)\
+    {\
+        printk(KERN_ERR "%s(%d) error:%s , ret(%d)\n", __FUNCTION__, __LINE__, (log), rc);\
+        return (rc) ;\
+    }\
+}
+
+#define CPH_TBL_ENTRY_NUM(a) (sizeof(a)/sizeof(a[0]))
+
+/******************************************************************************
+ * Enum for display
+ ******************************************************************************/
+typedef struct
+{
+    INT32  enum_value;
+    CHAR  *enum_str;
+} MV_ENUM_ENTRY_T;
+
+typedef struct
+{
+    INT32            enum_num;
+    MV_ENUM_ENTRY_T *enum_array;
+} MV_ENUM_ARRAY_T;
+
+/******************************************************************************
+ * Function Declaration
+ ******************************************************************************/
+/******************************************************************************
+* mindex_tpm_src_to_app_port()
+*
+* DESCRIPTION:Convert TPM source port to application UNI port 
+*
+* INPUTS:
+*       src_port    - TPM source port
+*
+* OUTPUTS:
+*       Application UNI port index
+*
+* RETURNS:
+*       On success, the function returns application UNI port index. 
+*       On error return invalid application UNI port index.
+*******************************************************************************/
+MV_APP_ETH_PORT_UNI_E mindex_tpm_src_to_app_port(tpm_src_port_type_t src_port);
+
+/******************************************************************************
+* mindex_mh_to_app_llid()
+*
+* DESCRIPTION:Convert Marvell header to application LLID 
+*
+* INPUTS:
+*       mh  - Marvell header
+*
+* OUTPUTS:
+*       Application LLID
+*
+* RETURNS:
+*       On success, the function returns application LLID. 
+*       On error return invalid application LLID.
+*******************************************************************************/
+MV_TCONT_LLID_E mindex_mh_to_app_llid(UINT16 mh);
+
+/******************************************************************************
+* mtype_get_digit_num()
+*
+* DESCRIPTION:Convert character string to digital number 
+*
+* INPUTS:
+*       str   - Character string
+*
+* OUTPUTS:
+*       None
+*
+* RETURNS:
+*       Digital numbe
+*******************************************************************************/
+UINT32 mtype_get_digit_num(const CHAR  *str);
+
+/******************************************************************************
+* mtype_lookup_enum_str()
+* _____________________________________________________________________________
+*
+* DESCRIPTION:lookup enum string according to enum value
+*
+* INPUTS:
+*       p_enum_array   - Pointer to enum array
+*       enum_value     - The enum value to be matched
+*
+* OUTPUTS:
+*       None
+*
+* RETURNS:
+*       Enum string
+*******************************************************************************/
+CHAR *mtype_lookup_enum_str(MV_ENUM_ARRAY_T *p_enum_array, INT32 enum_value);
+
+/******************************************************************************
+* mutils_is_frwd_broadcast_packet()
+* _____________________________________________________________________________
+*
+* DESCRIPTION:Check whether packet is directly forwarded broadcast one
+*
+* INPUTS:
+*       data   - packet data
+*
+* OUTPUTS:
+*       None
+*
+* RETURNS:
+*       TRUE: broadcast packet, FALSE:none broadcast packet
+*******************************************************************************/
+BOOL mutils_is_frwd_broadcast_packet(char *data);
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _MV_CPH_INFRA_H_ */
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_mng_if.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_mng_if.h
new file mode 100755
index 0000000..2cfbd3e
--- /dev/null
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_mng_if.h
@@ -0,0 +1,169 @@
+/*******************************************************************************
+Copyright (C) Marvell International Ltd. and its affiliates
+
+This software file (the "File") is owned and distributed by Marvell 
+International Ltd. and/or its affiliates ("Marvell") under the following
+alternative licensing terms.  Once you have made an election to distribute the
+File under one of the following license alternatives, please (i) delete this
+introductory statement regarding license alternatives, (ii) delete the two
+license alternatives that you have not elected to use and (iii) preserve the
+Marvell copyright notice above.
+
+********************************************************************************
+Marvell Commercial License Option
+
+If you received this File from Marvell and you have entered into a commercial
+license agreement (a "Commercial License") with Marvell, the File is licensed
+to you under the terms of the applicable Commercial License.
+
+********************************************************************************
+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.
+********************************************************************************
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or 
+modify this File under the following licensing terms. 
+Redistribution and use in source and binary forms, with or without modification, 
+are permitted provided that the following conditions are met:
+
+    *   Redistributions of source code must retain the above copyright notice,
+        this list of conditions and the following disclaimer. 
+
+    *   Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution. 
+
+    *   Neither the name of Marvell nor the names of its contributors may be 
+        used to endorse or promote products derived from this software without 
+        specific prior written permission. 
+    
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+********************************************************************************
+* mv_cph_mng_if.h
+*
+* DESCRIPTION: Marvell CPH(CPH Packet Handler) management interface definition
+*              for ioctl
+*
+* DEPENDENCIES:
+*               None
+*
+* CREATED BY:   VictorGu
+*
+* DATE CREATED: 11Dec2011
+*
+* FILE REVISION NUMBER:
+*               Revision: 1.1
+*
+*
+*******************************************************************************/
+#ifndef _MV_CPH_MNG_IF_H_
+#define _MV_CPH_MNG_IF_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/* Include Files
+------------------------------------------------------------------------------*/
+#include <linux/cdev.h>
+
+/* Definitions
+------------------------------------------------------------------------------*/
+#define MV_CPH_IOCTL_SET_COMPLEX_PROFILE      _IOW(MV_CPH_IOCTL_MAGIC,  1,  UINT32)
+#define MV_CPH_IOCTL_SET_FEATURE_FLAG         _IOW(MV_CPH_IOCTL_MAGIC,  2,  UINT32)
+#define MV_CPH_IOCTL_APP_ADD_RULE             _IOW(MV_CPH_IOCTL_MAGIC,  3,  UINT32)
+#define MV_CPH_IOCTL_APP_DEL_RULE             _IOW(MV_CPH_IOCTL_MAGIC,  4,  UINT32)
+#define MV_CPH_IOCTL_APP_UPDATE_RULE          _IOW(MV_CPH_IOCTL_MAGIC,  5,  UINT32)
+#define MV_CPH_IOCTL_APP_GET_RULE             _IOR(MV_CPH_IOCTL_MAGIC,  6,  UINT32)
+#define MV_CPH_IOCTL_FLOW_ADD_RULE            _IOW(MV_CPH_IOCTL_MAGIC,  7,  UINT32)
+#define MV_CPH_IOCTL_FLOW_DEL_RULE            _IOW(MV_CPH_IOCTL_MAGIC,  8,  UINT32)
+#define MV_CPH_IOCTL_FLOW_GET_RULE            _IOR(MV_CPH_IOCTL_MAGIC,  9,  UINT32)
+#define MV_CPH_IOCTL_FLOW_CLEAR_RULE          _IOW(MV_CPH_IOCTL_MAGIC,  10, UINT32)
+#define MV_CPH_IOCTL_FLOW_CLEAR_RULE_BY_MH    _IOW(MV_CPH_IOCTL_MAGIC,  11, UINT32)
+#define MV_CPH_IOCTL_FLOW_SET_DSCP_MAP        _IOW(MV_CPH_IOCTL_MAGIC,  12, UINT32)
+#define MV_CPH_IOCTL_FLOW_DEL_DSCP_MAP        _IOW(MV_CPH_IOCTL_MAGIC,  13, UINT32)
+#define MV_CPH_IOCTL_SET_TCONT_LLID_STATE     _IOW(MV_CPH_IOCTL_MAGIC,  14, UINT32)
+#define MV_CPH_IOCTL_SETUP                    _IOW(MV_CPH_IOCTL_MAGIC,  15, UINT32)
+
+/* Typedefs
+------------------------------------------------------------------------------*/
+typedef struct
+{
+    CPH_APP_PARSE_FIELD_E parse_bm; 
+    CPH_APP_PARSE_T       parse_key;  
+    CPH_APP_MOD_FIELD_E   mod_bm; 
+    CPH_APP_MOD_T         mod_value; 
+    CPH_APP_FRWD_FIELD_E  frwd_bm; 
+    CPH_APP_FRWD_T        frwd_value;
+
+} CPH_IOCTL_APP_RULE_T;
+
+typedef struct
+{
+    CPH_FLOW_ENTRY_T flow_map;
+} CPH_IOCTL_FLOW_MAP_T;
+
+typedef struct
+{
+    CPH_DSCP_PBITS_T dscp_map;
+} CPH_IOCTL_DSCP_MAP_T;
+
+typedef struct
+{
+    tpm_eth_complex_profile_t profile_id;
+    MV_APP_GMAC_PORT_E        active_port;
+    CPH_APP_FEATURE_E         feature_type;
+    BOOL                      feature_flag; 
+} CPH_IOCTL_MISC_T;
+
+typedef struct
+{
+    UINT32  tcont;
+    BOOL    state;
+} CPH_IOCTL_TCONT_STATE_T;
+
+/* MV CPH Char Device Structure */
+typedef struct
+{
+  CPH_IOCTL_APP_RULE_T    cph_ioctl_app_rule;
+  CPH_IOCTL_FLOW_MAP_T    cph_ioctl_flow_map;
+  CPH_IOCTL_DSCP_MAP_T    cph_ioctl_dscp_map;
+  CPH_IOCTL_MISC_T        cph_ioctl_misc;
+  CPH_IOCTL_TCONT_STATE_T cph_ioctl_tcont;
+
+  struct cdev             cdev;  
+} CPH_CDEV_T;
+
+/* Global variables
+------------------------------------------------------------------------------*/
+
+/* Global functions
+------------------------------------------------------------------------------*/
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _MV_CPH_MNG_IF_H_ */
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_mod.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_mod.c
new file mode 100755
index 0000000..7c631c3
--- /dev/null
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_mod.c
@@ -0,0 +1,143 @@
+/*******************************************************************************
+Copyright (C) Marvell International Ltd. and its affiliates
+
+This software file (the "File") is owned and distributed by Marvell 
+International Ltd. and/or its affiliates ("Marvell") under the following
+alternative licensing terms.  Once you have made an election to distribute the
+File under one of the following license alternatives, please (i) delete this
+introductory statement regarding license alternatives, (ii) delete the two
+license alternatives that you have not elected to use and (iii) preserve the
+Marvell copyright notice above.
+
+********************************************************************************
+Marvell Commercial License Option
+
+If you received this File from Marvell and you have entered into a commercial
+license agreement (a "Commercial License") with Marvell, the File is licensed
+to you under the terms of the applicable Commercial License.
+
+********************************************************************************
+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.
+********************************************************************************
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or 
+modify this File under the following licensing terms. 
+Redistribution and use in source and binary forms, with or without modification, 
+are permitted provided that the following conditions are met:
+
+    *   Redistributions of source code must retain the above copyright notice,
+        this list of conditions and the following disclaimer. 
+
+    *   Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution. 
+
+    *   Neither the name of Marvell nor the names of its contributors may be 
+        used to endorse or promote products derived from this software without 
+        specific prior written permission. 
+    
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+********************************************************************************
+* mv_cph_mod.c
+*
+* DESCRIPTION: Marvell CPH(CPH Packet Handler) module definition 
+*
+* DEPENDENCIES:
+*               None
+*
+* CREATED BY:   VictorGu
+*
+* DATE CREATED: 22Jan2013
+*
+* FILE REVISION NUMBER:
+*               Revision: 1.0
+*
+*
+*******************************************************************************/
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+
+#include "mv_cph_header.h"
+
+#define CPH_MODULE_VERSION  "22-Jan-2013"
+#define CPH_MODULE_DESC     "Marvell CPU Packet Handler Module"
+
+/******************************************************************************
+* cph_mod_exit()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Exit from CPH module 
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       None.
+*******************************************************************************/
+static VOID __exit cph_mod_exit(VOID)
+{
+    cph_dev_shutdown();
+}
+
+/******************************************************************************
+* cph_mod_init()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Initialize CPH module 
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+static INT32 __init cph_mod_init(VOID)
+{
+    if (cph_dev_init() != 0)
+    {
+        printk(KERN_ERR "\nCPH module initialization failed \n\n");
+        return MV_ERROR;
+    }
+
+    printk(KERN_INFO "\nCPH module inserted - %s\n\n", CPH_MODULE_VERSION);
+
+    return MV_OK;
+}
+
+device_initcall_sync(cph_mod_init);
+
+module_exit(cph_mod_exit);
+
+MODULE_AUTHOR ("Victor Gu");
+MODULE_VERSION (CPH_MODULE_VERSION);
+MODULE_DESCRIPTION (CPH_MODULE_DESC);
+MODULE_LICENSE ("GPL");
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_netdev.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_netdev.c
new file mode 100755
index 0000000..9d1e6df
--- /dev/null
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_netdev.c
@@ -0,0 +1,882 @@
+/*******************************************************************************
+Copyright (C) Marvell International Ltd. and its affiliates
+
+This software file (the "File") is owned and distributed by Marvell 
+International Ltd. and/or its affiliates ("Marvell") under the following
+alternative licensing terms.  Once you have made an election to distribute the
+File under one of the following license alternatives, please (i) delete this
+introductory statement regarding license alternatives, (ii) delete the two
+license alternatives that you have not elected to use and (iii) preserve the
+Marvell copyright notice above.
+
+********************************************************************************
+Marvell Commercial License Option
+
+If you received this File from Marvell and you have entered into a commercial
+license agreement (a "Commercial License") with Marvell, the File is licensed
+to you under the terms of the applicable Commercial License.
+
+********************************************************************************
+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.
+********************************************************************************
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or 
+modify this File under the following licensing terms. 
+Redistribution and use in source and binary forms, with or without modification, 
+are permitted provided that the following conditions are met:
+
+    *   Redistributions of source code must retain the above copyright notice,
+        this list of conditions and the following disclaimer. 
+
+    *   Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution. 
+
+    *   Neither the name of Marvell nor the names of its contributors may be 
+        used to endorse or promote products derived from this software without 
+        specific prior written permission. 
+    
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+********************************************************************************
+* mv_cph_netdev.c
+*
+* DESCRIPTION: Marvell CPH(CPH Packet Handler) network device part definition 
+*
+* DEPENDENCIES:
+*               None
+*
+* CREATED BY:   VictorGu
+*
+* DATE CREATED: 22Jan2013
+*
+* FILE REVISION NUMBER:
+*               Revision: 1.1
+*
+*
+*******************************************************************************/
+#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>
+#include <linux/icmpv6.h>
+
+#include <ctrlEnv/mvCtrlEnvLib.h>
+
+#include "mvDebug.h"
+#include "mv_cph_header.h"
+
+
+/****************************************************************************** 
+*                           Global Definition                                                                                                                                
+******************************************************************************/
+
+/* Total Eth port number */
+static INT32 gs_mv_eth_port_num = 0;
+
+
+#ifdef CONFIG_MV_CPH_UDP_SAMPLE_HANDLE
+static struct mv_port_tx_spec    udp_port_spec_cfg[CONFIG_MV_ETH_PORTS_NUM];
+#define PORT_ENTRIES        CPH_TBL_ENTRY_NUM(udp_port_spec_cfg)
+#endif
+
+/****************************************************************************** 
+*                           External Declarations                                                                                                                                 
+******************************************************************************/
+extern INT32 cph_data_flow_tx(INT32 port, struct net_device *dev, struct sk_buff *skb,
+                              BOOL mh, struct mv_eth_tx_spec *tx_spec_out);
+
+
+
+/****************************************************************************** 
+*                           Function Definitions                                                                                                                                  
+******************************************************************************/
+/******************************************************************************
+* cph_rec_skb()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Send SKB packet to linux network and increse counter  
+*
+* INPUTS:
+*       port    - Gmac port the packet from
+*       skb     - SKB buffer to receive packet
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       None.
+*******************************************************************************/
+VOID cph_rec_skb(INT32 port, struct sk_buff *skb)
+{
+    UINT32 rx_status    = 0;
+    struct eth_port *pp = NULL;
+
+    rx_status = netif_receive_skb(skb);
+    pp        = mv_eth_port_by_id(port);
+    STAT_DBG(if (rx_status) (pp->stats.rx_drop_sw++));
+}
+
+/******************************************************************************
+* cph_app_packet_rx()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: CPH function to handle the received application packets 
+*
+* INPUTS:
+*       port    - Gmac port the packet from
+*       dev     - Net device
+*       skb     - SKB buffer to receive packet
+*       rx_desc - RX descriptor
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns 1. 
+*       On error returns 0.
+*******************************************************************************/
+static INT32 cph_app_packet_rx(INT32 port, struct net_device *dev, struct sk_buff *skb, struct neta_rx_desc *rx_desc)
+{
+    CPH_APP_PARSE_FIELD_E parse_bm = 0;
+    CPH_APP_PARSE_T       parse_key;
+    INT32                 rc       = MV_OK;
+    CPH_APP_MOD_FIELD_E   mod_bm   = 0;
+    CPH_APP_MOD_T         mod_value; 
+    CPH_APP_FRWD_FIELD_E  frwd_bm  = 0; 
+    CPH_APP_FRWD_T        frwd_value;
+    BOOL                  state    = FALSE;    
+
+    /* Check whether need to handle generic application packet */
+    cph_db_get_param(CPH_DB_PARAM_APP_SUPPORT, &state);
+    if (state == FALSE)
+        return 0;
+
+    memset(&parse_key,  0, sizeof(parse_key));
+    memset(&mod_value,  0, sizeof(mod_value));
+    memset(&frwd_value, 0, sizeof(frwd_value));    
+
+    /* Parse application packet */
+    rc = cph_app_parse_packet(port, skb->data, &parse_bm, &parse_key);
+    if (rc != MV_OK) {
+        printk(KERN_ERR "Fail to call cph_app_parse_packet, rc(%d)\n", rc);
+        return 0;
+    }
+
+    /* Get CPH application rule */
+    rc = cph_app_get_rule(parse_bm, &parse_key, &mod_bm, &mod_value, &frwd_bm, &frwd_value);
+    if (rc != MV_OK) {
+        printk(KERN_DEBUG "Fail to call cph_app_get_rule, rc(%d)\n", rc);
+        return 0;
+    }
+
+    /* Increase counter */
+    rc = cph_app_increase_counter(parse_bm, &parse_key);
+    if (rc != MV_OK) {
+        printk(KERN_ERR "Fail to call cph_app_increase_counter, rc(%d)\n", rc);
+        return 0;
+    }
+
+    /* Apply modification */
+    rc = cph_app_mod_rx_packet(port, dev, skb, rx_desc, mod_bm, &mod_value);
+    if (rc != MV_OK) {
+        printk(KERN_ERR "Fail to call cph_app_mod_rx_packet, rc(%d)\n", rc);
+        return 0;
+    }
+    
+    /* Send to Linux Network Stack */
+    cph_rec_skb(port, skb);
+
+    return 1;
+}
+
+/******************************************************************************
+* cph_app_packet_tx()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: CPH function to handle the trasmiting application packets 
+*
+* INPUTS:
+*       port        - Gmac port the packet from
+*       dev         - Net device
+*       skb         - SKB buffer to receive packet
+*       tx_spec_out - TX descriptor
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns 1. 
+*       On error returns 0.
+*******************************************************************************/
+INT32 cph_app_packet_tx(INT32 port, struct net_device *dev, struct sk_buff *skb,
+    struct mv_eth_tx_spec *tx_spec_out)
+{
+    CPH_DIR_E             dir;
+    UINT16                proto_type = 0;
+    CPH_APP_PARSE_FIELD_E parse_bm   = 0;
+    CPH_APP_PARSE_T       parse_key;     
+    CPH_APP_MOD_FIELD_E   mod_bm     = 0;
+    CPH_APP_MOD_T         mod_value; 
+    CPH_APP_FRWD_FIELD_E  frwd_bm    = 0; 
+    CPH_APP_FRWD_T        frwd_value;
+    INT32                 rc         = MV_OK;
+    BOOL                  state      = FALSE;    
+
+    /* Check whether need to handle generic application packet */
+    cph_db_get_param(CPH_DB_PARAM_APP_SUPPORT, &state);
+    if (state == FALSE)
+        return 0;
+
+    memset(&parse_key,  0, sizeof(parse_key));
+    memset(&mod_value,  0, sizeof(mod_value));
+    memset(&frwd_value, 0, sizeof(frwd_value));    
+    tx_spec_out->tx_func = NULL;
+    tx_spec_out->flags   = 0;
+
+    /* Get direction of packet */
+    dir = cph_app_parse_dir(port, FALSE);
+    if(dir == CPH_DIR_INVALID)
+    {
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "dir[%d] is invalid \n", dir);
+        return 0;
+    }   
+    
+    /* Get the protocol type application packet */
+    proto_type = skb->protocol;
+
+    /* Get CPH application rule by protocol type */
+    rc = cph_app_get_rule_by_dir_proto(dir, proto_type, &parse_bm, &parse_key, &mod_bm, &mod_value, &frwd_bm, &frwd_value);
+    if (rc != MV_OK) {
+        //printk(KERN_ERR "Fail to call cph_app_get_rule_by_dir_proto\n");
+        return 0;
+    }
+
+    /* Increase counter */
+    rc = cph_app_increase_counter_by_dir_proto(dir, proto_type);
+    if (rc != MV_OK) {
+        printk(KERN_ERR "Fail to call cph_app_increase_counter_by_dir_proto, rc(%d)\n", rc);
+        return 0;
+    }
+
+    /* Sepcial handling for IGMP and MLD */
+    if (((parse_bm & CPH_APP_PARSE_FIELD_IPV4_TYPE) && 
+        (parse_key.ipv4_type == MV_IP_PROTO_IGMP))  ||
+        ((parse_bm & CPH_APP_PARSE_FIELD_ICMPV6_TYPE) && 
+        (parse_key.icmpv6_type == MV_ICMPV6_TYPE_MLD)))
+    {
+        if (cph_data_flow_tx(port, dev, skb, TRUE, tx_spec_out))
+            return 1;
+    }
+
+    /* Apply modification and set forwarding information */
+    rc = cph_app_mod_tx_packet(skb, tx_spec_out, mod_bm, &mod_value);
+    if (rc != MV_OK) {
+        printk(KERN_ERR "Fail to call cph_app_mod_tx_packet, rc(%d)\n", rc);
+        return 0;
+    }
+
+    /* Set forwarding information */
+    rc = cph_app_set_frwd(skb, tx_spec_out, frwd_bm, &frwd_value);
+    if (rc != MV_OK) {
+        printk(KERN_ERR "Fail to call cph_app_set_frwd, rc(%d)\n", rc);
+        return 0;
+    }
+
+    return 1;
+}
+
+#ifdef CONFIG_MV_CPH_UDP_SAMPLE_HANDLE
+static inline void cph_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 cph_udp_spec_print(int port)
+{
+    INT32 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 cph_udp_spec_print_all(void)
+{
+    INT32 port;
+
+    for (port=0;port < CONFIG_MV_ETH_PORTS_NUM ;port++) {
+        cph_udp_spec_print(port);
+    }
+}
+
+MV_STATUS  cph_udp_int_spec_set(struct mv_udp_port_tx_spec *udp_spec, uint16_t udp_port, INT32 table_size,
+                                    uint8_t txp, uint8_t txq, uint16_t flags, uint32_t hw_cmd)
+{
+    INT32 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) {
+            cph_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);
+            cph_copy_tx_spec(&(udp_spec[i].tx_spec), txp, txq, flags, hw_cmd);
+            return MV_OK;
+        }
+    }
+
+    return(MV_FULL);
+}
+
+
+MV_STATUS  cph_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 = cph_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(cph_udp_src_spec_set);
+
+
+MV_STATUS  cph_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 = cph_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(cph_udp_dest_spec_set);
+
+
+void  cph_udp_table_init(void)
+{
+    INT32 num_ports = PORT_ENTRIES;
+    INT32 tx_port, i;
+
+    if (num_ports > gs_mv_eth_port_num)
+        num_ports = gs_mv_eth_port_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 cph_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;
+    INT32 i;
+
+    if (port > CONFIG_MV_ETH_PORTS_NUM) {
+        printk("Port Error\n");
+        return(0);
+    }
+
+    if (skb->protocol == MV_CPH_ETH_TYPE_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));
+                        MV_CPH_PRINT(CPH_DEBUG_LEVEL, "found udp_src 0x(%04x)\n", 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));
+                        MV_CPH_PRINT(CPH_DEBUG_LEVEL, "found udp_dst 0x(%04x)\n", ntohs(udphdrp->dest));
+                        return 1;
+                    }
+                }
+
+                if (port == MV_PON_PORT_ID) {
+                    MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Packet UDP, udp source or dest port not found udp_src(%x)x udp_dst(%x)x\n",
+                                 ntohs(udphdrp->source), ntohs(udphdrp->dest));
+                }
+            }
+        }
+        else if (port == MV_PON_PORT_ID )
+            MV_CPH_PRINT(CPH_DEBUG_LEVEL, "NOT UDP, ip_proto(%d) \n", iphdrp->protocol);
+    }
+    else if (port == MV_PON_PORT_ID)
+        MV_CPH_PRINT(CPH_DEBUG_LEVEL, "NOT IP, proto(%d) \n", skb->protocol);
+
+    return 0;
+}
+#endif
+
+
+#ifdef CONFIG_MV_CPH_FLOW_MAP_HANDLE
+/******************************************************************************
+* cph_data_flow_rx()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: CPH function to handle the received application packets 
+*
+* INPUTS:
+*       port    - Gmac port the packet from
+*       dev     - Net device
+*       skb     - SKB buffer to receive packet
+*       rx_desc - RX descriptor
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns 1. 
+*       On error returns 0.
+*******************************************************************************/
+static INT32 cph_data_flow_rx(INT32 port, struct net_device *dev, struct sk_buff *skb, struct neta_rx_desc *rx_desc)
+{
+    CPH_FLOW_ENTRY_T flow_rule;
+    INT32            offset = 0;
+    BOOL             state  = FALSE;
+    MV_STATUS        rc     = MV_OK;
+
+    if (MV_PON_PORT_ID != port)
+        return 0;
+
+    cph_db_get_param(CPH_DB_PARAM_FLOW_SUPPORT, &state);
+    
+    if (state == TRUE) 
+    {
+        /* Parse packets */
+        rc = cph_flow_parse_packet(port, skb->data, TRUE, TRUE, &flow_rule);
+        if (rc != MV_OK)
+        {
+            MV_CPH_PRINT(CPH_ERR_LEVEL, "%s():fail to call cph_flow_parse_packet, rc(%d) \n", __FUNCTION__, rc);
+            return 0;
+        }
+        
+        /* Get None default CPH data flow rule at first */
+        flow_rule.is_default = FALSE;
+        rc = cph_flow_get_rule(&flow_rule);
+        if (rc != MV_OK)
+        {
+            /* Get default CPH data flow rule secondly */
+            flow_rule.is_default = TRUE;
+            rc = cph_flow_get_rule(&flow_rule);
+            
+            /* Do nothing */
+            if (rc != MV_OK)
+                return 0;
+        }
+
+        /* modify packet */
+        rc = cph_flow_mod_packet(skb, TRUE, &flow_rule, &offset);
+        if (rc != MV_OK)
+        {
+            MV_CPH_PRINT(CPH_ERR_LEVEL, "%s():fail to call cph_flow_mod_rx_packet, rc(%d) \n", __FUNCTION__, rc);
+            return 0;
+        }
+    }
+
+    return 1;
+}
+
+/******************************************************************************
+* cph_data_flow_tx()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: CPH function to handle the trasmiting application packets 
+*
+* INPUTS:
+*       port        - Gmac port the packet from
+*       dev         - Net device
+*       skb         - SKB buffer to receive packet
+*       mh          - Whether has marvell header
+*       tx_spec_out - TX descriptor
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns 1. 
+*       On error returns 0.
+*******************************************************************************/
+INT32 cph_data_flow_tx(INT32 port, struct net_device *dev, struct sk_buff *skb,
+                       BOOL mh, struct mv_eth_tx_spec *tx_spec_out)
+{
+    CPH_FLOW_ENTRY_T flow_rule;
+    INT32            offset = 0;
+    BOOL             l_mh;
+    BOOL             state  = FALSE;
+    MV_STATUS        rc     = MV_OK;
+
+    if (MV_PON_PORT_ID != port)
+        return 0;
+
+    cph_db_get_param(CPH_DB_PARAM_FLOW_SUPPORT, &state);
+    
+    if (state == TRUE) 
+    {
+        /* Decide whether need to handle Marvell header */
+        l_mh = mh;
+        if (TRUE == mutils_is_frwd_broadcast_packet(skb->data))
+              l_mh = TRUE;
+
+        /* Parse packets */
+        rc = cph_flow_parse_packet(port, skb->data, FALSE, l_mh, &flow_rule);
+        if (rc != MV_OK)
+        {
+            MV_CPH_PRINT(CPH_ERR_LEVEL, "%s():fail to call cph_flow_parse_packet, rc<%d> \n", __FUNCTION__, rc);
+            return 0;
+        }
+
+        /* Get CPH data flow rule */
+        flow_rule.is_default = FALSE;
+        rc = cph_flow_get_rule(&flow_rule);
+        if (rc != MV_OK)
+        {
+            /* Handle multicat packets as unicast one */
+            if (flow_rule.parse_bm & CPH_FLOW_PARSE_MC_PROTO) {
+                
+                flow_rule.parse_bm &= ~CPH_FLOW_PARSE_MC_PROTO;
+                rc = cph_flow_get_rule(&flow_rule);
+
+                if (rc != MV_OK)
+                {
+                    flow_rule.is_default = TRUE;
+                    rc = cph_flow_get_rule(&flow_rule);
+                    if (rc != MV_OK)
+                    {
+                        MV_CPH_PRINT(CPH_DEBUG_LEVEL, "%s():fail to call cph_flow_get_rule, rc<%d> \n", __FUNCTION__, rc);
+                        return 0;
+                    }
+                }                
+            }
+            else {
+
+                flow_rule.is_default = TRUE;
+                rc = cph_flow_get_rule(&flow_rule);
+                if (rc != MV_OK)
+                {
+                    MV_CPH_PRINT(CPH_DEBUG_LEVEL, "%s():fail to call cph_flow_get_rule, rc<%d> \n", __FUNCTION__, rc);
+                    return 0;
+                }
+            }
+        }
+
+        /* modify packet */
+        rc = cph_flow_mod_packet(skb, l_mh, &flow_rule, &offset);
+        if (rc != MV_OK)
+        {
+            MV_CPH_PRINT(CPH_ERR_LEVEL, "%s():fail to call cph_flow_mod_rx_packet, rc<%d> \n", __FUNCTION__, rc);
+            return 0;
+        }
+
+        /* Strip TX MH */
+        if (TRUE == l_mh) 
+        {
+            skb->data  += MV_ETH_MH_SIZE;
+            skb->len   -= MV_ETH_MH_SIZE;
+        }
+        
+        /* modify packet */
+        rc = cph_flow_mod_frwd(&flow_rule, tx_spec_out);
+        if (rc != MV_OK)
+        {
+            MV_CPH_PRINT(CPH_ERR_LEVEL, "%s():fail to call cph_flow_mod_frwd, rc<%d> \n", __FUNCTION__, rc);
+            return 0;
+        } 
+        return 1;
+    }
+
+    return 0;
+}
+#endif
+
+/******************************************************************************
+* cph_rx_func()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: CPH function to handle the received special packets 
+*              from network driver
+*
+* INPUTS:
+*       port    - Gmac port the packet from
+*       rxq     - CPU received queue
+*       dev     - Net device
+*       pkt     - Marvell packet information
+*       rx_desc - RX descriptor
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       None.
+*******************************************************************************/
+void cph_rx_func(int port, int rxq, struct net_device *dev,
+                 struct eth_pbuf *pkt, struct neta_rx_desc *rx_desc)
+{
+    UINT32 idx    = 0;
+    INT32  offset = 0;
+    UINT8  buff[512];
+
+    MV_CPH_CLEAN_PRINT(CPH_DEBUG_LEVEL, "\n");
+    MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Enter\n");
+
+    if (dev == NULL || pkt == NULL || rx_desc == NULL) {
+        printk("%s: NULL Pointer dev(%p) pkt(%p) rx_desc(%p)\n",
+               __func__, dev, pkt, rx_desc);
+        return;
+    }
+
+    if (rx_desc->pncInfo & NETA_PNC_RX_SPECIAL)
+    {
+        /* Receive application packets */
+        if (cph_app_packet_rx(port, dev, (struct sk_buff *)(pkt->osInfo), rx_desc))
+            return;
+        
+        /* Handle the broadcast packet in case it is enabled */
+#ifdef CONFIG_MV_CPH_BC_HANDLE
+        if (cph_app_rx_bc(port, dev, pkt, rx_desc))
+            return;
+#endif 
+
+        /* Does not match with CPH application rules */
+        MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Special pkt arrived from port(%d), was not handled. \n", port);
+        dev_kfree_skb_any((struct sk_buff *)(pkt->osInfo));  
+        MV_CPH_CLEAN_PRINT(CPH_DEBUG_LEVEL, "Input Packet first 24 bytes:\n");
+        memset(buff, 0, sizeof(buff));
+        for (idx = 0; idx < 48; idx++) {
+            offset += sprintf(buff+offset, "%02x ", *(((struct sk_buff *)(pkt->osInfo))->data + idx));
+        }
+        MV_CPH_CLEAN_PRINT(CPH_DEBUG_LEVEL,  "%s\n", buff);
+    }
+    else
+    {   /* Handle received data flow packets */
+#ifdef CONFIG_MV_CPH_FLOW_MAP_HANDLE
+        if (cph_data_flow_rx(port, dev, (struct sk_buff *)(pkt->osInfo), rx_desc)) {
+            MV_CPH_PRINT(CPH_DEBUG_LEVEL, "Flow mapping\n");
+        }
+        return;
+#endif
+    }
+
+    return;
+}
+
+/******************************************************************************
+* cph_tx_func()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: CPH function to handle tranmitting special packets 
+*              to network driver
+*
+* INPUTS:
+*       port        - Gmac port the packet from
+*       dev         - Net device
+*       skb         - SKB buffer to receive packet
+*       tx_spec_out - TX descriptor
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       None.
+*******************************************************************************/
+INT32 cph_tx_func(INT32 port, struct net_device *dev, struct sk_buff *skb,
+                  struct mv_eth_tx_spec *tx_spec_out)
+{
+    /* Transmit application packets */
+    if (cph_app_packet_tx(port, dev, skb, tx_spec_out))
+        return 1;
+
+    /* Transmit data flow packets */
+#ifdef CONFIG_MV_CPH_FLOW_MAP_HANDLE
+    if (cph_data_flow_tx(port, dev, skb, FALSE, tx_spec_out))
+        return 1;
+#endif
+
+    /* Transmit data flow packets by UDP Source Port or Dest Port */
+#ifdef CONFIG_MV_CPH_UDP_SAMPLE_HANDLE
+    if (cph_udp_port_tx(port, dev, skb, tx_spec_out))
+        return 1;
+#endif
+
+    return 0;
+}
+
+/******************************************************************************
+* cph_netdev_init()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Initialize CPH network device
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_netdev_init(VOID)
+{
+    UINT32 port_i;
+
+    /* Retrieve Eth port number, as in mv_eth_init_module */
+    gs_mv_eth_port_num = mvCtrlEthMaxPortGet();
+
+    if (gs_mv_eth_port_num > CONFIG_MV_ETH_PORTS_NUM)
+        gs_mv_eth_port_num = CONFIG_MV_ETH_PORTS_NUM;
+
+    /* Initialize application packet handling */
+    cph_app_init();
+    
+    /* Initialize UDP port mapping feature */    
+#ifdef CONFIG_MV_CPH_UDP_SAMPLE_HANDLE
+    cph_udp_table_init();
+#endif
+
+    /* Initialize data flow mapping feature */
+#ifdef CONFIG_MV_CPH_FLOW_MAP_HANDLE
+    cph_flow_init();
+#endif
+
+    /* Register special receive check function */
+#ifdef CONFIG_MV_ETH_RX_SPECIAL
+    for (port_i = 0;port_i < gs_mv_eth_port_num; port_i++) {
+        mv_eth_rx_special_proc_func(port_i, cph_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 < gs_mv_eth_port_num; port_i++) {
+        mv_eth_tx_special_check_func(port_i, cph_tx_func);
+    }
+#endif /* CONFIG_MV_ETH_TX_SPECIAL */
+    return MV_OK;
+}
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_netdev.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_netdev.h
new file mode 100755
index 0000000..7e59bba
--- /dev/null
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_netdev.h
@@ -0,0 +1,157 @@
+/*******************************************************************************
+Copyright (C) Marvell International Ltd. and its affiliates
+
+This software file (the "File") is owned and distributed by Marvell 
+International Ltd. and/or its affiliates ("Marvell") under the following
+alternative licensing terms.  Once you have made an election to distribute the
+File under one of the following license alternatives, please (i) delete this
+introductory statement regarding license alternatives, (ii) delete the two
+license alternatives that you have not elected to use and (iii) preserve the
+Marvell copyright notice above.
+
+********************************************************************************
+Marvell Commercial License Option
+
+If you received this File from Marvell and you have entered into a commercial
+license agreement (a "Commercial License") with Marvell, the File is licensed
+to you under the terms of the applicable Commercial License.
+
+********************************************************************************
+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.
+********************************************************************************
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or 
+modify this File under the following licensing terms. 
+Redistribution and use in source and binary forms, with or without modification, 
+are permitted provided that the following conditions are met:
+
+    *   Redistributions of source code must retain the above copyright notice,
+        this list of conditions and the following disclaimer. 
+
+    *   Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution. 
+
+    *   Neither the name of Marvell nor the names of its contributors may be 
+        used to endorse or promote products derived from this software without 
+        specific prior written permission. 
+    
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+********************************************************************************
+* mv_cph_netdev.h
+*
+* DESCRIPTION: Marvell CPH(CPH Packet Handler) network device part definition
+*
+* DEPENDENCIES:
+*               None
+*
+* CREATED BY:   VictorGu
+*
+* DATE CREATED: 11Dec2011
+*
+* FILE REVISION NUMBER:
+*               Revision: 1.0
+*
+*
+*******************************************************************************/
+#ifndef _MV_CPH_NETDEV_H_
+#define _MV_CPH_NETDEV_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/skbuff.h>
+#include <net/ip.h>
+
+#include <mvCommon.h>
+#include <mvOs.h>
+#include <mv_neta/net_dev/mv_netdev.h>
+
+
+#define MV_CPH_MAS_UDP_SRC_PORT          8
+#define MV_CPH_MAS_UDP_DST_PORT          8
+#define MV_CPH_NUM_LLID                  8
+
+#ifdef CONFIG_MV_CPH_UDP_SAMPLE_HANDLE
+struct mv_udp_port_tx_spec {
+    __be16    udp_port;
+    struct mv_eth_tx_spec tx_spec;
+};
+
+struct mv_port_tx_spec {
+    struct mv_udp_port_tx_spec udp_src[MV_CPH_MAS_UDP_SRC_PORT];
+    struct mv_udp_port_tx_spec udp_dst[MV_CPH_MAS_UDP_DST_PORT];
+};
+
+void cph_udp_spec_print_all(void);
+MV_STATUS  cph_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);
+MV_STATUS  cph_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);
+#endif
+
+/******************************************************************************
+* cph_rec_skb()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Send SKB packet to linux network and increse counter  
+*
+* INPUTS:
+*       port    - Gmac port the packet from
+*       skb     - SKB buffer to receive packet
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       None.
+*******************************************************************************/
+VOID cph_rec_skb(INT32 port, struct sk_buff *skb);
+
+/******************************************************************************
+* cph_netdev_init()
+* _____________________________________________________________________________
+*
+* DESCRIPTION: Initialize CPH network device
+*
+* INPUTS:
+*       None.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success, the function returns MV_OK. 
+*       On error returns error code accordingly.
+*******************************************************************************/
+INT32 cph_netdev_init(VOID);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _MV_CPH_NETDEV_H_ */
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_sysfs.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_sysfs.c
new file mode 100755
index 0000000..0e540a0
--- /dev/null
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_sysfs.c
@@ -0,0 +1,1222 @@
+/*******************************************************************************
+Copyright (C) Marvell International Ltd. and its affiliates
+
+This software file (the "File") is owned and distributed by Marvell 
+International Ltd. and/or its affiliates ("Marvell") under the following
+alternative licensing terms.  Once you have made an election to distribute the
+File under one of the following license alternatives, please (i) delete this
+introductory statement regarding license alternatives, (ii) delete the two
+license alternatives that you have not elected to use and (iii) preserve the
+Marvell copyright notice above.
+
+********************************************************************************
+Marvell Commercial License Option
+
+If you received this File from Marvell and you have entered into a commercial
+license agreement (a "Commercial License") with Marvell, the File is licensed
+to you under the terms of the applicable Commercial License.
+
+********************************************************************************
+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.
+********************************************************************************
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or 
+modify this File under the following licensing terms. 
+Redistribution and use in source and binary forms, with or without modification, 
+are permitted provided that the following conditions are met:
+
+    *   Redistributions of source code must retain the above copyright notice,
+        this list of conditions and the following disclaimer. 
+
+    *   Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution. 
+
+    *   Neither the name of Marvell nor the names of its contributors may be 
+        used to endorse or promote products derived from this software without 
+        specific prior written permission. 
+    
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+********************************************************************************
+* mv_cph_sysfs.c
+*
+* DESCRIPTION: Marvell CPH(CPH Packet Handler) sysfs command definition 
+*
+* DEPENDENCIES:
+*               None
+*
+* CREATED BY:   VictorGu
+*
+* DATE CREATED: 22Jan2013
+*
+* FILE REVISION NUMBER:
+*               Revision: 1.1
+*
+*
+*******************************************************************************/
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/capability.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/netdevice.h>
+
+#include <gbe/mvNeta.h>
+#include <pnc/mvPnc.h>
+
+#include "mv_cph_header.h"
+
+static ssize_t cph_spec_proc_help(CHAR *buf)
+{
+    INT32 off = 0;
+    off += sprintf(buf+off, "cat  help                                 - show this help\n");
+    off += sprintf(buf+off, "cat  help_add                             - show additional help for parameters\n");    
+    off += sprintf(buf+off, "cat  show_app_db                          - show all information in application rule data base\n");
+    off += sprintf(buf+off, "cat  show_parse_name                      - show sysfs parsing rule data base\n");
+    off += sprintf(buf+off, "cat  show_mod_name                        - show sysfs modification rule data base\n");
+    off += sprintf(buf+off, "cat  show_frwd_name                       - show sysfs modification rule data base\n");
+#ifdef CONFIG_MV_CPH_UDP_SAMPLE_HANDLE    
+    off += sprintf(buf+off, "cat  udp_ports                            - show special udp source and destination port configuration\n");   
+#endif
+#ifdef CONFIG_MV_CPH_FLOW_MAP_HANDLE
+    off += sprintf(buf+off, "cat  show_flow_rule                       - show flow mapping rules \n");    
+    off += sprintf(buf+off, "cat  clear_flow_rule                      - clear all flow mapping rules\n");
+    off += sprintf(buf+off, "cat  del_dscp_map                         - delete DSCP to P-bits mapping rules\n");
+#endif
+    off += sprintf(buf+off, "echo profile_id active_port               > set_complex     - Set TPM complex profile ID and active GMAC port, 0:GMAC0, 1:GMAC1, 2:PON MAC\n");
+    off += sprintf(buf+off, "echo feature state                        > set_flag        - Set the support of CPH feature: refer to below additional info, state 0:disable, 1:enable\n");
+    off += sprintf(buf+off, "echo tcont state                          > set_tcont       - Set T-CONT state in CPH, T-CONT 0~7, state, 1:enable, 0:disable\n");    
+    off += sprintf(buf+off, "echo hex                                  > trace_level     - Set cph trace level bitmap. 0x01:debug, 0x02:info, 0x04:warn, 0x08:error\n");
+    off += sprintf(buf+off, "echo name bm(hex) dir rx(hex) mh(hex) ety(hex) esty ipv4ty nh1 nh2 icmpty > add_parse   - add parsing field, dir 0:U/S, 1:D/S, 2:Not care\n");
+    off += sprintf(buf+off, "echo name                                 > del_parse       - delete parsing field\n");    
+    off += sprintf(buf+off, "echo name bm(hex) proto_type(hex) state   > add_mod         - add modification field, state 0:diable, 1:enable\n");
+    off += sprintf(buf+off, "echo name                                 > del_mod         - delete modification field\n");    
+    off += sprintf(buf+off, "echo name bm(hex) trg_port trg_queue gem  > add_frwd        - add forwarding field\n");
+    off += sprintf(buf+off, "echo name                                 > del_frwd        - delete forwarding field\n");    
+    off += sprintf(buf+off, "echo parse_name mod_name frwd_name        > add_app_rule    - add application rule\n");    
+    off += sprintf(buf+off, "echo parse_name                           > del_app_rule    - delete application rule\n");     
+    off += sprintf(buf+off, "echo parse_name mod_name frwd_name        > update_app_rule - update application rule\n");    
+    off += sprintf(buf+off, "echo parse_name                           > get_app_rule    - get application rule\n");    
+#ifdef CONFIG_MV_CPH_UDP_SAMPLE_HANDLE
+    off += sprintf(buf+off, "echo p udp_src(dec) txp txq flags hw_cmd  > udp_src         - set udp source port special Tx behavior\n");
+    off += sprintf(buf+off, "echo p udp_dst(dec) txp txq flags hw_cmd  > udp_dst         - set udp destination port special Tx behavior\n");
+#endif
+#ifdef CONFIG_MV_CPH_FLOW_MAP_HANDLE
+    off += sprintf(buf+off, "---------------------------------------------------------------------------------------------------------------------------------------\n");
+    off += sprintf(buf+off, "                         |Parse outer    |Parse inner             |Mod outer      |Mod Inner      |Forward\n");
+    off += sprintf(buf+off, "echo dir parse_bm mh ety  tpid vid pbits  tpid vid pbits  op_type  tpid vid pbits  tpid vid pbits  port queue hwf_queue gem > add_flow_rule - Add flow rule\n");
+    off += sprintf(buf+off, "echo dir parse_bm mh ety  tpid vid pbits  tpid vid pbits  > del_flow_rule   - delete flow mapping rule\n");
+    off += sprintf(buf+off, "echo dir parse_bm mh ety  tpid vid pbits  tpid vid pbits  > get_flow_rule   - get flow mapping rule\n");
+    off += sprintf(buf+off, "echo pbits0 pbits1 ... pbits62 pbits63                    > set_dscp_map    - set DSCP to P-bits mapping rules\n");
+#endif
+    return off;
+}
+
+static ssize_t cph_spec_proc_help_add(CHAR *buf)
+{
+    INT32 off = 0;
+    off += sprintf(buf+off, "CPH additional help for parameters\n");
+    off += sprintf(buf+off, "---------------------------------------------------------------------------------------------------------------------------------------\n");
+    off += sprintf(buf+off, "[Generic Parameters]\n");
+    off += sprintf(buf+off, "feature:\n");
+    off += sprintf(buf+off, "   0:Generic application, 1:IGMP/MLD support, 2:Broadcast support, 3:Data flow mapping support, 4: UDP port mapping support\n");
+    off += sprintf(buf+off, "[App Parameters]\n");
+    off += sprintf(buf+off, "parse bm:\n");
+    off += sprintf(buf+off, "   0x01:PARSE_FIELD_DIR              0x02:PARSE_FIELD_MH               0x04:PARSE_FIELD_ETH_TYPE         0x08:PARSE_FIELD_ETH_SUBTYPE\n");
+    off += sprintf(buf+off, "   0x10:PARSE_FIELD_IPV4_TYPE        0x20:PARSE_FIELD_IPV6_NH1         0x40:PARSE_FIELD_IPV6_NH2         0x80:PARSE_FIELD_ICMPV6_TYPE\n");
+    off += sprintf(buf+off, "dir: 0: U/S, 1:D/S, 2: Not care\n");
+    off += sprintf(buf+off, "rx: 0: RX, 1:TX\n");
+    off += sprintf(buf+off, "mod bm:\n");
+    off += sprintf(buf+off, "   0x01:RX_MOD_ADD_GMAC              0x02:RX_MOD_REPLACE_PROTO_TYPE    0x04:RX_MOD_STRIP_MH              0x08:TX_MOD_ADD_MH_BY_DRIVER\n");
+    off += sprintf(buf+off, "   0x10:CPH_APP_TX_MOD_NO_PAD        0x20:MOD_SET_STATE\n");
+    off += sprintf(buf+off, "frwd bm:\n");
+    off += sprintf(buf+off, "   0x01:FRWD_SET_TRG_PORT            0x02:FRWD_SET_TRG_QUEUE           0x04:FRWD_SET_GEM_PORT\n");
+    off += sprintf(buf+off, "[Flow Parameters]\n");
+    off += sprintf(buf+off, "dir: 0: U/S, 1:D/S, 2: Not care\n");    
+    off += sprintf(buf+off, "bm:\n");
+    off += sprintf(buf+off, "   0x01:PARSE_MH                     0x02:PARSE_EXT_VLAN               0x04:PARSE_TWO_VLAN               0x08:PARSE_ETH_TYPE\n");
+    off += sprintf(buf+off, "mh(hex), ety(hex), tpid(hex), vid(dec), pbits(dec)\n");    
+    off += sprintf(buf+off, "op_type:\n");
+    off += sprintf(buf+off, "   00:ASIS                           01:DISCARD                        02:ADD                            03:ADD_COPY_DSCP\n");
+    off += sprintf(buf+off, "   04:ADD_COPY_OUTER_PBIT            05:ADD_COPY_INNER_PBIT            06:ADD_2_TAGS                     07:ADD_2_TAGS_COPY_DSCP\n");
+    off += sprintf(buf+off, "   08:ADD_2_TAGS_COPY_PBIT           09:REM                            10:REM_2_TAGS                     11:REPLACE\n");
+    off += sprintf(buf+off, "   12:REPLACE_VID                    13:REPLACE_PBIT                   14:REPLACE_INNER_ADD_OUTER        15:REPLACE_INNER_ADD_OUTER_COPY_PBIT\n");
+    off += sprintf(buf+off, "   16:REPLACE_INNER_REM_OUTER        17:REPLACE_2TAGS                  18:REPLACE_2TAGS_VID              19:SWAP\n");
+  
+    return off;
+}
+
+ 
+/********************************************************************************/
+/*                          Parsing field table                                 */
+/********************************************************************************/
+static CPH_SYSFS_PARSE_T cph_sysfs_parse_table[CPH_SYSFS_FIELD_MAX_ENTRY];
+
+static CPH_SYSFS_RULE_T cph_parse_rule_db =
+{
+    .max_entry_num    = CPH_SYSFS_FIELD_MAX_ENTRY,
+    .entry_num        = 0,
+    .entry_size       = sizeof(CPH_SYSFS_PARSE_T),
+    .entry_ara        = cph_sysfs_parse_table
+};
+
+static VOID cph_sysfs_init_parse_db(VOID)
+{
+    CPH_SYSFS_PARSE_T  *p_entry = (CPH_SYSFS_PARSE_T *)cph_parse_rule_db.entry_ara;
+    INT32               idx     = 0;
+
+    //printk(KERN_INFO "%s: Init DB\n", __FUNCTION__);
+    for (idx = 0; idx < cph_parse_rule_db.max_entry_num; idx++, p_entry++)
+    {
+        p_entry->name[0] = 0;
+    }
+}
+
+CPH_SYSFS_PARSE_T *cph_sysfs_find_parse_entry_by_name(CHAR *name)
+{
+    CPH_SYSFS_PARSE_T *p_entry = (CPH_SYSFS_PARSE_T *)cph_parse_rule_db.entry_ara;
+    INT32              idx     = 0;
+
+
+    for (idx = 0; idx < cph_parse_rule_db.max_entry_num; idx++, p_entry++)
+    {
+        if (strcmp(p_entry->name, name) == 0) 
+            return p_entry;
+    }
+    return 0;
+}
+
+CPH_SYSFS_PARSE_T *cph_sysfs_find_free_parse_entry(VOID)
+{
+    CPH_SYSFS_PARSE_T *p_entry = (CPH_SYSFS_PARSE_T *)cph_parse_rule_db.entry_ara;
+    INT32              idx     = 0;
+
+    for (idx = 0; idx < cph_parse_rule_db.max_entry_num; idx++, p_entry++)
+    {
+        if (p_entry->name[0] == 0) 
+            return p_entry;
+    }
+    return 0;
+}
+
+BOOL cph_sysfs_del_parse_entry_by_name(CHAR *name)
+{
+    CPH_SYSFS_PARSE_T *p_entry = (CPH_SYSFS_PARSE_T *)cph_parse_rule_db.entry_ara;
+    INT32              idx     = 0;
+
+    for (idx = 0; idx < cph_parse_rule_db.max_entry_num; idx++, p_entry++)
+    {
+        if (strcmp(p_entry->name, name) == 0)
+        {
+            p_entry->name[0]  = 0;
+            p_entry->parse_bm = 0;
+            memset(&p_entry->parse_key, 0, sizeof(p_entry->parse_key));
+            return TRUE;
+        }
+    }
+    return FALSE;
+}
+
+VOID cph_sysfs_show_parse_db(VOID)
+{
+    CPH_SYSFS_PARSE_T *p_entry = (CPH_SYSFS_PARSE_T *)cph_parse_rule_db.entry_ara;
+    INT32              idx     = 0;
+
+    for (idx = 0; idx < cph_parse_rule_db.max_entry_num; idx++, p_entry++)
+    {
+        if (p_entry->name[0] != 0)
+        {
+            printk(KERN_INFO "Parse entry(%d) name(%s)\n", idx, p_entry->name);
+            cph_db_display_parse_field(p_entry->parse_bm, &p_entry->parse_key);
+        }
+    }
+}
+
+/********************************************************************************/
+/*                          Modification field table                            */
+/********************************************************************************/
+static CPH_SYSFS_MOD_T cph_sysfs_mod_table[CPH_SYSFS_FIELD_MAX_ENTRY];
+
+static CPH_SYSFS_RULE_T cph_mod_rule_db =
+{
+    .max_entry_num    = CPH_SYSFS_FIELD_MAX_ENTRY,
+    .entry_num        = 0,
+    .entry_size       = sizeof(CPH_SYSFS_MOD_T),
+    .entry_ara        = cph_sysfs_mod_table
+};
+
+static VOID cph_sysfs_init_mod_db(VOID)
+{
+    CPH_SYSFS_MOD_T  *p_entry = (CPH_SYSFS_MOD_T *)cph_mod_rule_db.entry_ara;
+    INT32             idx     = 0;
+
+    //printk(KERN_INFO "%s: Init DB\n", __FUNCTION__);
+    for (idx = 0; idx < cph_mod_rule_db.max_entry_num; idx++, p_entry++)
+    {
+        p_entry->name[0] = 0;
+    }
+}
+
+CPH_SYSFS_MOD_T *cph_sysfs_find_mod_entry_by_name(CHAR *name)
+{
+    CPH_SYSFS_MOD_T  *p_entry = (CPH_SYSFS_MOD_T *)cph_mod_rule_db.entry_ara;
+    INT32             idx     = 0;
+
+    for (idx = 0; idx < cph_mod_rule_db.max_entry_num; idx++, p_entry++)
+    {
+        if (strcmp(p_entry->name, name) == 0) 
+            return p_entry;
+    }
+    return 0;
+}
+
+CPH_SYSFS_MOD_T *cph_sysfs_find_free_mod_entry(VOID)
+{
+    CPH_SYSFS_MOD_T  *p_entry = (CPH_SYSFS_MOD_T *)cph_mod_rule_db.entry_ara;
+    INT32             idx     = 0;
+
+    for (idx = 0; idx < cph_mod_rule_db.max_entry_num; idx++, p_entry++)
+    {
+        if (p_entry->name[0] == 0) 
+            return p_entry;
+    }
+    return 0;
+}
+
+BOOL cph_sysfs_del_mod_entry_by_name(CHAR *name)
+{
+    CPH_SYSFS_MOD_T  *p_entry = (CPH_SYSFS_MOD_T *)cph_mod_rule_db.entry_ara;
+    INT32             idx     = 0;
+
+    for (idx = 0; idx < cph_mod_rule_db.max_entry_num; idx++, p_entry++)
+    {
+        if (strcmp(p_entry->name, name) == 0)
+        {
+            p_entry->name[0] = 0;
+            p_entry->mod_bm  = 0;
+            memset(&p_entry->mod_value, 0, sizeof(p_entry->mod_value));
+            return TRUE;
+        }
+    }
+    return FALSE;
+}
+
+VOID cph_sysfs_show_mod_db(VOID)
+{
+    CPH_SYSFS_MOD_T  *p_entry = (CPH_SYSFS_MOD_T *)cph_mod_rule_db.entry_ara;
+    INT32             idx     = 0;
+
+    for (idx = 0; idx < cph_mod_rule_db.max_entry_num; idx++, p_entry++)
+    {
+        if (p_entry->name[0] != 0)
+        {
+            printk(KERN_INFO "Mod entry(%d) name(%s)\n", idx, p_entry->name);
+            cph_db_display_mod_field(p_entry->mod_bm, &p_entry->mod_value);
+        }
+    }
+}
+
+/********************************************************************************/
+/*                          Forwarding field table                              */
+/********************************************************************************/
+static CPH_SYSFS_FRWD_T cph_sysfs_frwd_table[CPH_SYSFS_FIELD_MAX_ENTRY];
+
+static CPH_SYSFS_RULE_T cph_frwd_rule_db =
+{
+    .max_entry_num    = CPH_SYSFS_FIELD_MAX_ENTRY,
+    .entry_num        = 0,
+    .entry_size       = sizeof(CPH_SYSFS_FRWD_T),
+    .entry_ara        = cph_sysfs_frwd_table
+};
+
+static VOID cph_sysfs_init_frwd_db(VOID)
+{
+    CPH_SYSFS_FRWD_T *p_entry = (CPH_SYSFS_FRWD_T *)cph_frwd_rule_db.entry_ara;
+    INT32             idx     = 0;
+
+    //printk(KERN_INFO "%s: Init DB\n", __FUNCTION__);
+    for (idx = 0; idx < cph_frwd_rule_db.max_entry_num; idx++, p_entry++)
+    {
+        p_entry->name[0] = 0;
+    }
+}
+
+CPH_SYSFS_FRWD_T *cph_sysfs_find_frwd_entry_by_name(CHAR *name)
+{
+    CPH_SYSFS_FRWD_T *p_entry = (CPH_SYSFS_FRWD_T *)cph_frwd_rule_db.entry_ara;
+    INT32             idx     = 0;
+
+    for (idx = 0; idx < cph_frwd_rule_db.max_entry_num; idx++, p_entry++)
+    {
+        if (strcmp(p_entry->name, name) == 0) 
+            return p_entry;
+    }
+    return 0;
+}
+
+CPH_SYSFS_FRWD_T *cph_sysfs_find_free_frwd_entry(VOID)
+{
+    CPH_SYSFS_FRWD_T *p_entry = (CPH_SYSFS_FRWD_T *)cph_frwd_rule_db.entry_ara;
+    INT32             idx     = 0;
+
+    for (idx = 0; idx < cph_frwd_rule_db.max_entry_num; idx++, p_entry++)
+    {
+        if (p_entry->name[0] == 0) 
+            return p_entry;
+    }
+    return 0;
+}
+
+BOOL cph_sysfs_del_frwd_entry_by_name(CHAR *name)
+{
+    CPH_SYSFS_FRWD_T *p_entry = (CPH_SYSFS_FRWD_T *)cph_frwd_rule_db.entry_ara;
+    INT32             idx     = 0;
+
+    for (idx = 0; idx < cph_frwd_rule_db.max_entry_num; idx++, p_entry++)
+    {
+        if (strcmp(p_entry->name, name) == 0)
+        {
+            p_entry->name[0] = 0;
+            p_entry->frwd_bm = 0;
+            memset(&p_entry->frwd_value, 0, sizeof(p_entry->frwd_value));
+            return TRUE;
+        }
+    }
+    return FALSE;
+}
+
+VOID cph_sysfs_show_frwd_db(VOID)
+{
+    CPH_SYSFS_FRWD_T *p_entry = (CPH_SYSFS_FRWD_T *)cph_frwd_rule_db.entry_ara;
+    INT32             idx     = 0;
+
+    for (idx = 0; idx < cph_frwd_rule_db.max_entry_num; idx++, p_entry++)
+    {
+        if (p_entry->name[0] != 0)
+        {
+            printk(KERN_INFO "Frwd entry(%d) name(%s)\n", idx, p_entry->name);
+            cph_db_display_frwd_field(p_entry->frwd_bm, &p_entry->frwd_value);
+        }
+    }
+}
+
+/********************************************************************************/
+/*                          SYS FS Parsing Functions                            */
+/********************************************************************************/
+static ssize_t cph_spec_proc_show(struct device *dev, struct device_attribute *attr, CHAR *buf)
+{
+    INT32       off  = 0;
+    const CHAR *name = attr->attr.name;
+
+    if (!capable(CAP_NET_ADMIN))
+        return -EPERM;
+
+    if (!strcmp(name, "help") ) {
+        off = cph_spec_proc_help(buf);
+    }
+    if (!strcmp(name, "help_add") ) {
+        off = cph_spec_proc_help_add(buf);
+    }    
+    else if (!strcmp(name, "show_app_db") ) {
+        cph_db_display_all();
+    }
+    else if (!strcmp(name, "show_parse_name") ) {
+        cph_sysfs_show_parse_db();
+    }
+    else if (!strcmp(name, "show_mod_name") ) {
+        cph_sysfs_show_mod_db();
+    }
+    else if (!strcmp(name, "show_frwd_name") ) {
+        cph_sysfs_show_frwd_db();
+    }
+#ifdef CONFIG_MV_CPH_UDP_SAMPLE_HANDLE       
+    else if (!strcmp(name, "udp_ports")) {
+        cph_udp_spec_print_all();
+    }
+#endif    
+#ifdef CONFIG_MV_CPH_FLOW_MAP_HANDLE          
+    else if (!strcmp(name, "show_flow_rule")) {
+        cph_flow_display_all();
+    } 
+    else if (!strcmp(name, "clear_flow_rule")) {
+        cph_flow_clear_rule();
+    }  
+    else if (!strcmp(name, "del_dscp_map")) {
+        cph_flow_del_dscp_map();
+    }     
+#endif    
+    else
+        off = cph_spec_proc_help(buf);
+
+    return off;
+}
+
+static ssize_t cph_spec_proc_1_store(struct device *dev,
+                                     struct device_attribute *attr,
+                                     const CHAR *buf, size_t len)
+{
+    const CHAR *name  = attr->attr.name;
+    UINT32      v1    = 0;
+    ULONG       flags = 0;    
+    MV_STATUS   rc    =  MV_OK;
+
+    if (!capable(CAP_NET_ADMIN))
+        return -EPERM;
+
+    /* Read input */
+    sscanf(buf, "%x", &v1);
+
+    raw_local_irq_save(flags);
+    
+    if (!strcmp(name, "trace_level")) {
+        rc = cph_set_trace_flag(v1);
+        if (rc == MV_OK) {
+            printk("Succeed to set trace level<0x%x>\n", v1);
+        } 
+        else
+        {
+            printk("Fail to set trace level<0x%x>\n", v1);
+        }
+    }
+    else
+        printk("%s: illegal operation <%s>\n", __FUNCTION__, attr->attr.name);
+
+    raw_local_irq_restore(flags);
+
+    return len;
+}
+
+static ssize_t cph_spec_proc_2_store(struct device *dev,
+                                     struct device_attribute *attr,
+                                     const CHAR *buf, size_t len)
+{
+    const CHAR *name  = attr->attr.name;
+    UINT32      v1    = 0;
+    UINT32      v2    = 0;
+    ULONG       flags = 0;    
+    MV_STATUS   rc    =  MV_OK;
+
+    if (!capable(CAP_NET_ADMIN))
+        return -EPERM;
+
+    /* Read input */
+    sscanf(buf, "%d %d", &v1, &v2);
+
+    raw_local_irq_save(flags);
+    
+    if (!strcmp(name, "set_complex")) {
+        rc = cph_set_complex_profile(v1, v2);
+        if (rc == MV_OK) {
+            printk("Succeed to set complex profile<%d> active port<%d>\n", v1, v2);
+        } 
+        else
+        {
+            printk("Fail to set complex profile<%d> active port<%d>\n", v1, v2);
+        }
+    }
+    else if (!strcmp(name, "set_flag")) {
+        rc = cph_set_feature_flag(v1, v2);
+        if (rc == MV_OK) {
+            printk("Succeed to set feature<%d> to <%d>\n", v1, v2);
+        } 
+        else
+        {
+            printk("Fail to set feature<%d> to<%d>\n", v1, v2);
+        }
+    }
+    else if (!strcmp(name, "set_tcont")) {
+        rc = cph_set_tcont_state(v1, v2);
+        if (rc == MV_OK) {
+            printk("Succeed to set tcont<%d> to <%d>\n", v1, v2);
+        } 
+        else
+        {
+            printk("Fail to set tcont<%d> to<%d>\n", v1, v2);
+        }
+    }    
+    else
+        printk("%s: illegal operation <%s>\n", __FUNCTION__, attr->attr.name);
+
+    raw_local_irq_restore(flags);
+
+    return len;
+}
+
+static ssize_t cph_spec_proc_name_store(struct device *dev,
+                                        struct device_attribute *attr,
+                                        const CHAR *buf, size_t len)
+{
+    const CHAR *name  = attr->attr.name;
+    CHAR        name1[CPH_SYSFS_FIELD_MAX_LEN+1];
+    UINT32      v1    = 0;
+    UINT32      v2    = 0;
+    UINT32      v3    = 0;
+    UINT32      v4    = 0;
+    UINT32      v5    = 0;
+    UINT32      v6    = 0;
+    UINT32      v7    = 0;
+    UINT32      v8    = 0;
+    UINT32      v9    = 0;
+    UINT32      v10   = 0;
+    ULONG       flags = 0;
+    CPH_SYSFS_PARSE_T *p_parse_entry = NULL;
+    CPH_SYSFS_MOD_T   *p_mod_entry   = NULL;
+    CPH_SYSFS_FRWD_T  *p_frwd_entry  = NULL;
+
+    if (!capable(CAP_NET_ADMIN))
+        return -EPERM;
+
+
+    if (!strcmp(name, "add_parse")) {
+        /* Read input */
+        sscanf(buf, "%s %x %d %d %d %x %d %d %d %d %d", name1, &v1, &v2, &v3, &v4, &v5, &v6, &v7, &v8, &v9, &v10);
+
+        raw_local_irq_save(flags);
+    
+        p_parse_entry = cph_sysfs_find_parse_entry_by_name(name1);
+        if(p_parse_entry) {
+            printk("Already has the parse field by name <%s>\n", name1);    
+            return -EPERM;
+        }
+
+        p_parse_entry = cph_sysfs_find_free_parse_entry();
+        if(!p_parse_entry) {
+            printk("No free parse entry\n");    
+            return -EPERM;
+        }
+        
+        strcpy(p_parse_entry->name, name1);
+        p_parse_entry->parse_bm              = v1;
+        p_parse_entry->parse_key.dir         = v2;
+        p_parse_entry->parse_key.rx_tx       = v3;
+        p_parse_entry->parse_key.mh          = v4;
+        p_parse_entry->parse_key.eth_type    = v5;
+        p_parse_entry->parse_key.eth_subtype = v6;
+        p_parse_entry->parse_key.ipv4_type   = v7;
+        p_parse_entry->parse_key.ipv6_nh1    = v8;
+        p_parse_entry->parse_key.ipv6_nh2    = v9;
+        p_parse_entry->parse_key.icmpv6_type = v10;
+        
+        printk("Succeed to add parse field by name <%s>\n", name1);
+
+        raw_local_irq_restore(flags);
+    }
+    else if (!strcmp(name, "add_mod")) {
+        /* Read input */
+        sscanf(buf, "%s %x %x %d", name1, &v1, &v2, &v3);
+
+        raw_local_irq_save(flags);
+        
+        p_mod_entry = cph_sysfs_find_mod_entry_by_name(name1);
+        if(p_mod_entry) {
+            printk("Already has the mod field by name <%s>\n", name1);    
+            return -EPERM;
+        }
+
+        p_mod_entry = cph_sysfs_find_free_mod_entry();
+        if(!p_mod_entry) {
+            printk("No free mod entry\n");    
+            return -EPERM;
+        }
+
+        strcpy(p_mod_entry->name, name1);
+        p_mod_entry->mod_bm                = v1;
+        p_mod_entry->mod_value.proto_type  = v2;
+        if (v3)
+            p_mod_entry->mod_value.state   = TRUE;
+        else
+            p_mod_entry->mod_value.state   = FALSE;
+        
+        printk("Succeed to add mod field by name <%s>\n", name1);
+        
+        raw_local_irq_restore(flags);
+    }
+    else if (!strcmp(name, "add_frwd")) {
+        /* Read input */
+        sscanf(buf, "%s %x %d %d %d", name1, &v1, &v2, &v3, &v4);
+
+        raw_local_irq_save(flags);
+        
+        p_frwd_entry = cph_sysfs_find_frwd_entry_by_name(name1);
+        if(p_frwd_entry) {
+            printk("Already has the frwd field by name <%s>\n", name1);    
+            return -EPERM;
+        }
+
+        p_frwd_entry = cph_sysfs_find_free_frwd_entry();
+        if(!p_frwd_entry) {
+            printk("No free frwd entry\n");    
+            return -EPERM;
+        }
+        
+        strcpy(p_frwd_entry->name, name1);
+        p_frwd_entry->frwd_bm              = v1;
+        p_frwd_entry->frwd_value.trg_port  = v2;
+        p_frwd_entry->frwd_value.trg_queue = v3;
+        p_frwd_entry->frwd_value.gem_port  = v4;
+        
+        printk("Succeed to add frwd field by name <%s>\n", name1);
+
+        raw_local_irq_restore(flags);
+    }   
+    else
+        printk("%s: illegal operation <%s>\n", __FUNCTION__, attr->attr.name);
+
+
+
+    return len;
+}
+
+static ssize_t cph_spec_proc_app_store(struct device *dev,
+                                       struct device_attribute *attr,
+                                       const CHAR *buf, size_t len)
+{
+    const CHAR *name  = attr->attr.name;
+    CHAR        name1[CPH_SYSFS_FIELD_MAX_LEN+1];
+    CHAR        name2[CPH_SYSFS_FIELD_MAX_LEN+1];
+    CHAR        name3[CPH_SYSFS_FIELD_MAX_LEN+1];    
+    ULONG       flags = 0;
+    MV_STATUS   rc    =  MV_OK;    
+    CPH_SYSFS_PARSE_T *p_parse_entry = NULL;
+    CPH_SYSFS_MOD_T   *p_mod_entry   = NULL;    
+    CPH_SYSFS_FRWD_T  *p_frwd_entry  = NULL;
+    CPH_SYSFS_MOD_T    mod_entry;    
+    CPH_SYSFS_FRWD_T   frwd_entry;
+
+    if (!capable(CAP_NET_ADMIN))
+        return -EPERM;
+
+    /* Read input */
+    sscanf(buf, "%s %s %s", name1, name2, name3);
+
+    raw_local_irq_save(flags);
+    
+    if (!strcmp(name, "add_app_rule")) {
+        p_parse_entry = cph_sysfs_find_parse_entry_by_name(name1);
+        if(!p_parse_entry) {
+            printk("add_app_rule: invalid parse name <%s>\n", name1);    
+            return -EPERM;
+        }
+        p_mod_entry = cph_sysfs_find_mod_entry_by_name(name2);
+        if(!p_mod_entry) {
+            printk("add_app_rule: invalid mod name <%s>\n", name2);    
+            return -EPERM;
+        }
+        p_frwd_entry = cph_sysfs_find_frwd_entry_by_name(name3);
+        if(!p_frwd_entry) {
+            printk("add_app_rule: invalid frwd name <%s>\n", name3);    
+            return -EPERM;
+        }         
+            
+        rc = cph_add_app_rule(p_parse_entry->parse_bm, &p_parse_entry->parse_key,
+                              p_mod_entry->mod_bm, &p_mod_entry->mod_value,
+                              p_frwd_entry->frwd_bm, &p_frwd_entry->frwd_value);
+        if (rc == MV_OK) {
+            printk("Succeed to add app rule\n");
+        } 
+        else
+        {
+            printk("Fail to add app rule\n");
+        }
+    }
+    else if (!strcmp(name, "del_app_rule")) {
+        p_parse_entry = cph_sysfs_find_parse_entry_by_name(name1);
+        if(!p_parse_entry) {
+            printk("add_app_rule: invalid parse name <%s>\n", name1);    
+            return -EPERM;
+        }
+        
+        rc = cph_del_app_rule(p_parse_entry->parse_bm, &p_parse_entry->parse_key);
+        if (rc == MV_OK) {
+            printk("Succeed to delete app rule\n");
+        } 
+        else
+        {
+            printk("Fail to delete app rule\n");
+        }        
+    }
+    else if (!strcmp(name, "update_app_rule")) {
+        p_parse_entry = cph_sysfs_find_parse_entry_by_name(name1);
+        if(!p_parse_entry) {
+            printk("add_app_rule: invalid parse name <%s>\n", name1);    
+            return -EPERM;
+        }
+        p_mod_entry = cph_sysfs_find_mod_entry_by_name(name2);
+        if(!p_mod_entry) {
+            printk("add_app_rule: invalid mod name <%s>\n", name2);    
+            return -EPERM;
+        }
+        p_frwd_entry = cph_sysfs_find_frwd_entry_by_name(name3);
+        if(!p_frwd_entry) {
+            printk("add_app_rule: invalid frwd name <%s>\n", name3);    
+            return -EPERM;
+        }         
+            
+        rc = cph_update_app_rule(p_parse_entry->parse_bm, &p_parse_entry->parse_key,
+                                 p_mod_entry->mod_bm, &p_mod_entry->mod_value,
+                                 p_frwd_entry->frwd_bm, &p_frwd_entry->frwd_value);
+        if (rc == MV_OK) {
+            printk("Succeed to update app rule\n");
+        } 
+        else
+        {
+            printk("Fail to update app rule\n");
+        }          
+    }
+    else if (!strcmp(name, "get_app_rule")) {
+        p_parse_entry = cph_sysfs_find_parse_entry_by_name(name1);
+        if(!p_parse_entry) {
+            printk("add_app_rule: invalid parse name <%s>\n", name1);    
+            return -EPERM;
+        }
+       
+        rc = cph_get_app_rule(p_parse_entry->parse_bm, &p_parse_entry->parse_key,
+                              &mod_entry.mod_bm, &mod_entry.mod_value,
+                              &frwd_entry.frwd_bm, &frwd_entry.frwd_value);
+        if (rc == MV_OK) {
+            cph_db_display_parse_field(p_parse_entry->parse_bm, &p_parse_entry->parse_key);
+            cph_db_display_mod_field(mod_entry.mod_bm, &mod_entry.mod_value);
+            cph_db_display_frwd_field(frwd_entry.frwd_bm, &frwd_entry.frwd_value);
+        } 
+        else
+        {
+            printk("No valid CPH app rule\n");
+        }
+        
+    }
+    else if (!strcmp(name, "del_parse")) {
+        rc = cph_sysfs_del_parse_entry_by_name(name1);
+        if (rc == TRUE) {
+            printk("Succeed to delete parse field by name <%s>\n", name1);
+        } 
+        else
+        {
+            printk("Fail to delete parse field by name <%s>\n", name1);
+        }
+    }
+    else if (!strcmp(name, "del_mod")) {
+        rc = cph_sysfs_del_mod_entry_by_name(name1);
+        if (rc == TRUE) {
+            printk("Succeed to delete mod field by name <%s>\n", name1);
+        } 
+        else
+        {
+            printk("Fail to delete mod field by name <%s>\n", name1);
+        }
+    }
+    else if (!strcmp(name, "del_frwd")) {
+        rc = cph_sysfs_del_frwd_entry_by_name(name1);
+        if (rc == TRUE) {
+            printk("Succeed to delete frwd field by name <%s>\n", name1);
+        } 
+        else
+        {
+            printk("Fail to delete frwd field by name <%s>\n", name1);
+        }  
+    }    
+    else
+        printk("%s: illegal operation <%s>\n", __FUNCTION__, attr->attr.name);
+
+    raw_local_irq_restore(flags);
+
+    return len;
+}
+
+#ifdef CONFIG_MV_CPH_UDP_SAMPLE_HANDLE
+static ssize_t cph_spec_proc_udp_store(struct device *dev,
+                                       struct device_attribute *attr,
+                                       const CHAR *buf, size_t len)
+{
+    const CHAR *name  = attr->attr.name;
+    UINT32      v1    = 0;
+    UINT32      v2    = 0;
+    UINT32      v3    = 0;
+    UINT32      v4    = 0;
+    UINT32      v5    = 0;
+    UINT32      v6    = 0;
+    ULONG       flags = 0;
+    MV_STATUS   rc    =  MV_OK;
+
+    if (!capable(CAP_NET_ADMIN))
+        return -EPERM;
+
+    /* Read input */
+    sscanf(buf, "%d %d %x %x %x %x", &v1, &v2, &v3, &v4, &v5, &v6);
+
+    raw_local_irq_save(flags);
+    
+    if (!strcmp(name, "udp_src")) {
+        rc = cph_udp_src_spec_set(v1, v2, v3, v4, v5, v6);
+        if (rc == MV_OK) {
+            printk("Succeed to add UDP src rule\n");
+        } 
+        else
+        {
+            printk("Fail to add UDP src rule\n");
+        }        
+    }
+    else if (!strcmp(name, "udp_dst")) {
+        rc = cph_udp_dest_spec_set(v1, v2, v3, v4, v5, v6);
+        if (rc == MV_OK) {
+            printk("Succeed to add UDP dest rule\n");
+        } 
+        else
+        {
+            printk("Fail to add UDP dest rule\n");
+        }         
+    }
+    else
+        printk("%s: illegal operation <%s>\n", __FUNCTION__, attr->attr.name);
+
+    raw_local_irq_restore(flags);
+
+    return len;
+}
+#endif
+
+#ifdef CONFIG_MV_CPH_FLOW_MAP_HANDLE
+static ssize_t cph_spec_proc_flow_store(struct device *dev,
+                                        struct device_attribute *attr,
+                                        const CHAR *buf, size_t len)
+{
+    const CHAR    *name  = attr->attr.name;
+    UINT32         v1    = 0;
+    UINT32         v2    = 0;
+    UINT32         v3    = 0;
+    UINT32         v4    = 0;
+    UINT32         v5    = 0;
+    UINT32         v6    = 0;
+    UINT32         v7    = 0;
+    UINT32         v8    = 0;
+    UINT32         v9    = 0;
+    UINT32         v10   = 0;    
+    UINT32         v11   = 0;
+    UINT32         v12   = 0;
+    UINT32         v13   = 0;
+    UINT32         v14   = 0;
+    UINT32         v15   = 0;
+    UINT32         v16   = 0;
+    UINT32         v17   = 0;
+    UINT32         v18   = 0; 
+    UINT32         v19   = 0;
+    UINT32         v20   = 0;
+    UINT32         v21   = 0;   
+    ULONG          flags = 0;
+    MV_STATUS      rc    =  MV_OK;
+    CPH_FLOW_ENTRY_T cph_flow;
+
+    if (!capable(CAP_NET_ADMIN))
+        return -EPERM;
+
+    /* Read input */
+    sscanf(buf, "%d %x %x %x %x %d %d %x %d %d %d %x %d %d %x %d %d %d %d %d %d", &v1, &v2, &v3, &v4, &v5, &v6, &v7, &v8, &v9, &v10, &v11, &v12, &v13, &v14, &v15, &v16, &v17, &v18, &v19, &v20, &v21);
+
+    raw_local_irq_save(flags);
+
+#ifdef CONFIG_MV_CPH_FLOW_MAP_HANDLE   
+    if (!strcmp(name, "add_flow_rule")) {
+        memset(&cph_flow, 0, sizeof(cph_flow));
+        cph_flow.dir       = (CPH_DIR_E)v1;
+        cph_flow.parse_bm  = (CPH_FLOW_PARSE_E)v2;
+        cph_flow.mh        = (UINT16)v3;
+        cph_flow.eth_type  = (UINT16)v4;
+        cph_flow.parse_outer_tci.tpid   = (UINT16)v5;
+        cph_flow.parse_outer_tci.vid    = (UINT16)v6;
+        cph_flow.parse_outer_tci.pbits  = (UINT8)v7;
+        cph_flow.parse_inner_tci.tpid   = (UINT16)v8;
+        cph_flow.parse_inner_tci.vid    = (UINT16)v9;
+        cph_flow.parse_inner_tci.pbits  = (UINT8)v10;
+        cph_flow.op_type                = (CPH_VLAN_OP_TYPE_E)v11;        
+        cph_flow.mod_outer_tci.tpid     = (UINT16)v12;
+        cph_flow.mod_outer_tci.vid      = (UINT16)v13;
+        cph_flow.mod_outer_tci.pbits    = (UINT8)v14;
+        cph_flow.mod_inner_tci.tpid     = (UINT16)v15;
+        cph_flow.mod_inner_tci.vid      = (UINT16)v16;
+        cph_flow.mod_inner_tci.pbits    = (UINT8)v17;
+        cph_flow.pkt_frwd.trg_port      = (UINT8)v18;
+        cph_flow.pkt_frwd.trg_queue     = (UINT8)v19;
+        cph_flow.pkt_frwd.trg_hwf_queue = (UINT8)v20;        
+        cph_flow.pkt_frwd.gem_port      = (UINT16)v21;
+        
+        rc = cph_flow_add_rule(&cph_flow);
+        if (rc == MV_OK) {
+            printk("Succeed to add flow mapping rule\n");
+        } 
+        else
+        {
+            printk("Fail to add flow mapping rule\n");
+        }        
+    }   
+    else if (!strcmp(name, "del_flow_rule")) {
+        memset(&cph_flow, 0, sizeof(cph_flow));
+        cph_flow.dir       = (CPH_DIR_E)v1;
+        cph_flow.parse_bm  = (CPH_FLOW_PARSE_E)v2;
+        cph_flow.mh        = (UINT16)v3;
+        cph_flow.eth_type  = (UINT16)v4;
+        cph_flow.parse_outer_tci.tpid   = (UINT16)v5;
+        cph_flow.parse_outer_tci.vid    = (UINT16)v6;
+        cph_flow.parse_outer_tci.pbits  = (UINT8)v7;
+        cph_flow.parse_inner_tci.tpid   = (UINT16)v8;
+        cph_flow.parse_inner_tci.vid    = (UINT16)v9;
+        cph_flow.parse_inner_tci.pbits  = (UINT8)v10;
+        
+        rc = cph_flow_del_rule(&cph_flow);
+        if (rc == MV_OK) {
+            printk("Succeed to delete flow mapping rule\n");
+        } 
+        else
+        {
+            printk("Fail to delete flow mapping rule\n");
+        }          
+    }
+    else if (!strcmp(name, "get_flow_rule")) {
+        memset(&cph_flow, 0, sizeof(cph_flow));
+        cph_flow.dir       = (CPH_DIR_E)v1;
+        cph_flow.parse_bm  = (CPH_FLOW_PARSE_E)v2;
+        cph_flow.mh        = (UINT16)v3;
+        cph_flow.eth_type  = (UINT16)v4;
+        cph_flow.parse_outer_tci.tpid   = (UINT16)v5;
+        cph_flow.parse_outer_tci.vid    = (UINT16)v6;
+        cph_flow.parse_outer_tci.pbits  = (UINT8)v7;
+        cph_flow.parse_inner_tci.tpid   = (UINT16)v8;
+        cph_flow.parse_inner_tci.vid    = (UINT16)v9;
+        cph_flow.parse_inner_tci.pbits  = (UINT8)v10;
+            
+        rc = cph_flow_get_rule(&cph_flow);
+        if (rc == MV_OK) {
+            printk("Succeed to get flow rule\n");
+            printk(KERN_INFO "                        |Parse outer       |Parse inner       |Mod outer         |Mod Inner         |Forward\n");
+            printk(KERN_INFO "dir parse_bm mh   ety    tpid   vid  pbits  tpid   vid  pbits  tpid   vid  pbits  tpid   vid  pbits  port queue hwf_queue gem  op_type\n");
+            printk(KERN_INFO  
+                   "%2.2s  0x%04x   %-4d 0x%04x 0x%04x %4d %1d      0x%04x %4d %1d      0x%04x %4d %1d      0x%04x %4d %1d      %1d    %1d     %1d         %4d %s \n",
+                   cph_app_lookup_dir(cph_flow.dir), cph_flow.parse_bm, cph_flow.mh, cph_flow.eth_type,
+                   cph_flow.parse_outer_tci.tpid, cph_flow.parse_outer_tci.vid, cph_flow.parse_outer_tci.pbits, 
+                   cph_flow.parse_inner_tci.tpid, cph_flow.parse_inner_tci.vid, cph_flow.parse_inner_tci.pbits,
+                   cph_flow.mod_outer_tci.tpid,   cph_flow.mod_outer_tci.vid,   cph_flow.mod_outer_tci.pbits,
+                   cph_flow.mod_inner_tci.tpid,   cph_flow.mod_inner_tci.vid,   cph_flow.mod_inner_tci.pbits,
+                   cph_flow.pkt_frwd.trg_port,    cph_flow.pkt_frwd.trg_queue,  cph_flow.pkt_frwd.trg_hwf_queue, cph_flow.pkt_frwd.gem_port, 
+                   cph_flow_lookup_op_type(cph_flow.op_type));
+             
+        } 
+        else
+        {
+            printk("Fail to get flow\n");
+        }
+    }   
+    else
+        printk("%s: illegal operation <%s>\n", __FUNCTION__, attr->attr.name);
+#endif
+
+    raw_local_irq_restore(flags);
+
+    return len;
+}
+
+static ssize_t cph_spec_proc_dscp_store(struct device *dev,
+                                      struct device_attribute *attr,
+                                      const CHAR *buf, size_t len)
+{
+    const CHAR      *name = attr->attr.name;
+    UINT32           v[64];
+    UINT32           index = 0;
+    ULONG            flags = 0;
+    MV_STATUS        rc    =  MV_OK;     
+    CPH_DSCP_PBITS_T dscp_map;
+
+    if (!capable(CAP_NET_ADMIN))
+        return -EPERM;
+
+    /* Read input */
+    sscanf(buf, "%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d ",
+           &v[0],  &v[1],  &v[2],  &v[3],  &v[4],  &v[5],  &v[6],  &v[7],
+           &v[8],  &v[9],  &v[10], &v[11], &v[12], &v[13], &v[14], &v[15],
+           &v[16], &v[17], &v[18], &v[19], &v[20], &v[21], &v[22], &v[23],
+           &v[24], &v[25], &v[26], &v[27], &v[28], &v[29], &v[30], &v[31],
+           &v[32], &v[33], &v[34], &v[35], &v[36], &v[37], &v[38], &v[39],
+           &v[40], &v[41], &v[42], &v[43], &v[44], &v[45], &v[46], &v[47],
+           &v[48], &v[49], &v[50], &v[51], &v[52], &v[53], &v[54], &v[55],
+           &v[56], &v[57], &v[58], &v[59], &v[60], &v[61], &v[62], &v[63]);
+    for (index = 0; index < 64; index++) {
+        dscp_map.pbits[index] = (UINT8)v[index];
+    }
+    dscp_map.in_use = 1;
+
+    raw_local_irq_save(flags);
+
+    if (!strcmp(name, "set_dscp_map")) {    
+        rc = cph_flow_set_dscp_map(&dscp_map);
+        if (rc == MV_OK) {
+            printk("Succeed to set DSCP to P-bits mapping\n");
+        } 
+        else
+        {
+            printk("Fail to set DSCP to P-bits mapping\n");
+        }        
+    }
+    else
+        printk("%s: illegal operation <%s>\n", __FUNCTION__, attr->attr.name);
+
+    raw_local_irq_restore(flags);
+
+    return len;
+}
+#endif
+
+static DEVICE_ATTR(help,            S_IRUSR, cph_spec_proc_show, NULL);
+static DEVICE_ATTR(help_add,        S_IRUSR, cph_spec_proc_show, NULL);
+static DEVICE_ATTR(show_app_db,     S_IRUSR, cph_spec_proc_show, NULL);
+static DEVICE_ATTR(show_parse_name, S_IRUSR, cph_spec_proc_show, NULL);
+static DEVICE_ATTR(show_mod_name,   S_IRUSR, cph_spec_proc_show, NULL);
+static DEVICE_ATTR(show_frwd_name,  S_IRUSR, cph_spec_proc_show, NULL);
+#ifdef CONFIG_MV_CPH_UDP_SAMPLE_HANDLE 
+static DEVICE_ATTR(udp_ports,       S_IRUSR, cph_spec_proc_show, NULL);
+#endif
+#ifdef CONFIG_MV_CPH_FLOW_MAP_HANDLE
+static DEVICE_ATTR(show_flow_rule,  S_IRUSR, cph_spec_proc_show, NULL);
+static DEVICE_ATTR(clear_flow_rule, S_IRUSR, cph_spec_proc_show, NULL);
+static DEVICE_ATTR(del_dscp_map,    S_IRUSR, cph_spec_proc_show, NULL);
+#endif
+static DEVICE_ATTR(set_complex,     S_IWUSR, cph_spec_proc_show, cph_spec_proc_2_store);
+static DEVICE_ATTR(set_flag,        S_IWUSR, cph_spec_proc_show, cph_spec_proc_2_store);
+static DEVICE_ATTR(add_parse,       S_IWUSR, cph_spec_proc_show, cph_spec_proc_name_store);
+static DEVICE_ATTR(del_parse,       S_IWUSR, cph_spec_proc_show, cph_spec_proc_app_store);
+static DEVICE_ATTR(add_mod,         S_IWUSR, cph_spec_proc_show, cph_spec_proc_name_store);
+static DEVICE_ATTR(del_mod,         S_IWUSR, cph_spec_proc_show, cph_spec_proc_app_store);
+static DEVICE_ATTR(add_frwd,        S_IWUSR, cph_spec_proc_show, cph_spec_proc_name_store);
+static DEVICE_ATTR(del_frwd,        S_IWUSR, cph_spec_proc_show, cph_spec_proc_app_store);
+static DEVICE_ATTR(add_app_rule,    S_IWUSR, cph_spec_proc_show, cph_spec_proc_app_store);
+static DEVICE_ATTR(del_app_rule,    S_IWUSR, cph_spec_proc_show, cph_spec_proc_app_store);
+static DEVICE_ATTR(update_app_rule, S_IWUSR, cph_spec_proc_show, cph_spec_proc_app_store);
+static DEVICE_ATTR(get_app_rule,    S_IWUSR, cph_spec_proc_show, cph_spec_proc_app_store);
+#ifdef CONFIG_MV_CPH_UDP_SAMPLE_HANDLE 
+static DEVICE_ATTR(udp_src,         S_IWUSR, cph_spec_proc_show, cph_spec_proc_udp_store);
+static DEVICE_ATTR(udp_dst,         S_IWUSR, cph_spec_proc_show, cph_spec_proc_udp_store);
+#endif
+#ifdef CONFIG_MV_CPH_FLOW_MAP_HANDLE
+static DEVICE_ATTR(add_flow_rule,   S_IWUSR, cph_spec_proc_show, cph_spec_proc_flow_store);
+static DEVICE_ATTR(del_flow_rule,   S_IWUSR, cph_spec_proc_show, cph_spec_proc_flow_store);
+static DEVICE_ATTR(get_flow_rule,   S_IWUSR, cph_spec_proc_show, cph_spec_proc_flow_store);
+static DEVICE_ATTR(set_dscp_map,    S_IWUSR, cph_spec_proc_show, cph_spec_proc_dscp_store);
+#endif
+static DEVICE_ATTR(set_tcont,       S_IWUSR, cph_spec_proc_show, cph_spec_proc_2_store);
+static DEVICE_ATTR(trace_level,     S_IWUSR, cph_spec_proc_show, cph_spec_proc_1_store);
+
+
+static struct attribute *cph_spec_proc_attrs[] = {
+    &dev_attr_help.attr,
+    &dev_attr_help_add.attr,
+    &dev_attr_show_app_db.attr,
+    &dev_attr_show_parse_name.attr,
+    &dev_attr_show_mod_name.attr,
+    &dev_attr_show_frwd_name.attr,
+#ifdef CONFIG_MV_CPH_UDP_SAMPLE_HANDLE     
+    &dev_attr_udp_ports.attr,
+#endif
+#ifdef CONFIG_MV_CPH_FLOW_MAP_HANDLE
+    &dev_attr_show_flow_rule.attr,
+    &dev_attr_clear_flow_rule.attr,
+    &dev_attr_del_dscp_map.attr,
+#endif    
+    &dev_attr_set_complex.attr,
+    &dev_attr_set_flag.attr,
+    &dev_attr_add_parse.attr,
+    &dev_attr_del_parse.attr,
+    &dev_attr_add_mod.attr,
+    &dev_attr_del_mod.attr,
+    &dev_attr_add_frwd.attr,
+    &dev_attr_del_frwd.attr,
+    &dev_attr_add_app_rule.attr,    
+    &dev_attr_del_app_rule.attr,
+    &dev_attr_update_app_rule.attr,
+    &dev_attr_get_app_rule.attr,
+#ifdef CONFIG_MV_CPH_UDP_SAMPLE_HANDLE       
+    &dev_attr_udp_src.attr,
+    &dev_attr_udp_dst.attr,
+#endif
+#ifdef CONFIG_MV_CPH_FLOW_MAP_HANDLE
+    &dev_attr_add_flow_rule.attr,
+    &dev_attr_del_flow_rule.attr,
+    &dev_attr_get_flow_rule.attr,
+    &dev_attr_set_dscp_map.attr,
+#endif
+    &dev_attr_set_tcont.attr,
+    &dev_attr_trace_level.attr,
+
+    NULL
+};
+
+static struct attribute_group cph_spec_proc_group = {
+    .name = "proto",
+    .attrs = cph_spec_proc_attrs,
+};
+
+INT32 cph_sysfs_init(VOID)
+{
+    INT32          err = 0;
+    struct device *pd  = NULL;
+
+    pd = bus_find_device_by_name(&platform_bus_type, NULL, "cph");
+    if (!pd) {
+        platform_device_register_simple("cph", -1, NULL, 0);
+        pd = bus_find_device_by_name(&platform_bus_type, NULL, "cph");
+    }
+
+    if (!pd) {
+        printk(KERN_ERR"%s: cannot find cph device\n", __FUNCTION__);
+        pd = &platform_bus;
+    }
+
+    err = sysfs_create_group(&pd->kobj, &cph_spec_proc_group);
+    if (err) {
+        printk(KERN_INFO "sysfs group failed %d\n", err);
+        goto out;
+    }
+
+    /* Init CPH SYS FS data base to hold parse/mod/frwd values */
+    cph_sysfs_init_parse_db();
+    cph_sysfs_init_mod_db();    
+    cph_sysfs_init_frwd_db();
+
+    printk(KERN_INFO "= CUST Module SYS FS init successfully =\n");
+out:
+    return err;
+}
+
+VOID cph_sysfs_exit(VOID)
+{
+    struct device *pd = NULL;
+
+    pd = bus_find_device_by_name(&platform_bus_type, NULL, "cph");
+    if (!pd)
+    {
+        printk(KERN_ERR"%s: cannot find CPH device\n", __FUNCTION__);
+        return;
+    }
+
+    sysfs_remove_group(&pd->kobj, &cph_spec_proc_group);
+
+    printk(KERN_INFO "= CPH Module SYS FS exit successfully =\n");
+}
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_sysfs.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_sysfs.h
new file mode 100755
index 0000000..183a15d
--- /dev/null
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cph/mv_cph_sysfs.h
@@ -0,0 +1,135 @@
+/*******************************************************************************
+Copyright (C) Marvell International Ltd. and its affiliates
+
+This software file (the "File") is owned and distributed by Marvell 
+International Ltd. and/or its affiliates ("Marvell") under the following
+alternative licensing terms.  Once you have made an election to distribute the
+File under one of the following license alternatives, please (i) delete this
+introductory statement regarding license alternatives, (ii) delete the two
+license alternatives that you have not elected to use and (iii) preserve the
+Marvell copyright notice above.
+
+********************************************************************************
+Marvell Commercial License Option
+
+If you received this File from Marvell and you have entered into a commercial
+license agreement (a "Commercial License") with Marvell, the File is licensed
+to you under the terms of the applicable Commercial License.
+
+********************************************************************************
+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.
+********************************************************************************
+Marvell BSD License Option
+
+If you received this File from Marvell, you may opt to use, redistribute and/or 
+modify this File under the following licensing terms. 
+Redistribution and use in source and binary forms, with or without modification, 
+are permitted provided that the following conditions are met:
+
+    *   Redistributions of source code must retain the above copyright notice,
+        this list of conditions and the following disclaimer. 
+
+    *   Redistributions in binary form must reproduce the above copyright
+        notice, this list of conditions and the following disclaimer in the
+        documentation and/or other materials provided with the distribution. 
+
+    *   Neither the name of Marvell nor the names of its contributors may be 
+        used to endorse or promote products derived from this software without 
+        specific prior written permission. 
+    
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+********************************************************************************
+* mv_cph_sysfs.h
+*
+* DESCRIPTION: Marvell CPH(CPH Packet Handler) sysfs command definition
+*
+* DEPENDENCIES:
+*               None
+*
+* CREATED BY:   VictorGu
+*
+* DATE CREATED: 11Dec2011
+*
+* FILE REVISION NUMBER:
+*               Revision: 1.1
+*
+*
+*******************************************************************************/
+#ifndef _MV_CPH_SYSFS_H_
+#define _MV_CPH_SYSFS_H_
+    
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define CPH_SYSFS_FIELD_MAX_LEN   (32)
+#define CPH_SYSFS_FIELD_MAX_ENTRY (64)
+
+
+/* Common DB structure for entries
+------------------------------------------------------------------------------*/
+typedef struct
+{
+    INT32  max_entry_num;
+    INT32  entry_num;
+    INT32  entry_size;
+    VOID  *entry_ara;
+} CPH_SYSFS_RULE_T;
+
+/* Parsing filed entry
+------------------------------------------------------------------------------*/
+typedef struct
+{
+    CHAR                  name[CPH_SYSFS_FIELD_MAX_LEN+1];
+    CPH_APP_PARSE_FIELD_E parse_bm;
+    CPH_APP_PARSE_T       parse_key;
+} CPH_SYSFS_PARSE_T;
+
+/* Modification filed entry
+------------------------------------------------------------------------------*/
+typedef struct
+{
+    CHAR                  name[CPH_SYSFS_FIELD_MAX_LEN+1];
+    CPH_APP_MOD_FIELD_E   mod_bm; 
+    CPH_APP_MOD_T         mod_value;
+} CPH_SYSFS_MOD_T;
+
+/* Forwarding filed entry
+------------------------------------------------------------------------------*/
+typedef struct
+{
+    CHAR                  name[CPH_SYSFS_FIELD_MAX_LEN+1];
+    CPH_APP_FRWD_FIELD_E  frwd_bm;
+    CPH_APP_FRWD_T        frwd_value;
+} CPH_SYSFS_FRWD_T;
+
+
+INT32 cph_sysfs_init(VOID);
+VOID cph_sysfs_exit(VOID);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _MV_CPH_SYSFS_H_ */
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/Kconfig b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/Kconfig
deleted file mode 100755
index 12c6a42..0000000
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/Kconfig
+++ /dev/null
@@ -1,34 +0,0 @@
-menu "SoC CUST support"
-depends on MV_INCLUDE_CUST
-
-config  MV_CUST
-        bool "Support for Marvell Customer Driver"
-        default y
-        ---help---
-
-config  MV_CUST_IGMP_HANDLE
-        bool "Enable MV_CUST IGMP handling"
-        default y
-        ---help---
-
-config  MV_CUST_MLD_HANDLE
-        bool "Enable MV_CUST MLD handling"
-        default y
-        ---help---
-
-config  MV_CUST_LPBK_DETECT_HANDLE
-        bool "Enable MV_CUST UNI loopback detect handling"
-        default y
-        ---help---
-        
-config  MV_CUST_UDP_SAMPLE_HANDLE
-        bool "Enable MV_CUST sample UDP handling"
-        default n
-        ---help---
-
-config  MV_CUST_FLOW_MAP_HANDLE
-        bool "Enable MV_CUST flow mapping handling"
-        default y
-        ---help---        
-
-endmenu
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/Makefile b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/Makefile
deleted file mode 100755
index 0fd28b0..0000000
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/Makefile
+++ /dev/null
@@ -1,22 +0,0 @@
-#
-# Makefile for the Marvell Customer driver
-#
-
-ifeq ($(CONFIG_ARCH_FEROCEON),y)
-	include $(srctree)/$(MACHINE)/config/mvRules.mk
-endif
-
-ifdef CONFIG_MV_HAL_RULES_PATH
-include $(srctree)/include/config/auto.conf
-include $(srctree)/$(subst ",,$(CONFIG_MV_HAL_RULES_PATH))
-endif
-
-CUST_OBJS += 	mv_cust_dev.o \
-		mv_cust_mod.o \
-		mv_cust_flow_map.o \
-		mv_cust_netdev.o \
-		mv_cust_sysfs.o
-                                
-mv_cust-objs := $(CUST_OBJS)
-obj-$(CONFIG_MV_CUST) += mv_cust.o
-
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_dev.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_dev.c
deleted file mode 100755
index 39f93ac..0000000
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_dev.c
+++ /dev/null
@@ -1,382 +0,0 @@
-/************************************************************************
-* 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_dev.c
-*
-* DESCRIPTION:
-*       
-* 
-*******************************************************************************/
-
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/version.h>
-#include <linux/delay.h>
-#include <linux/wait.h>
-#include <linux/mutex.h>
-#include <linux/spinlock.h>
-#include <linux/poll.h>
-#include <linux/clk.h>
-#include <linux/fs.h>
-#include <linux/vmalloc.h>
-#include <linux/cdev.h>
-#include <linux/device.h>
-#include <linux/miscdevice.h>
-
-#include "mv_cust_dev.h"
-#include "mv_cust_netdev.h"
-#include "mv_cust_flow_map.h"
-#include "mv_cust_mng_if.h"
-
-
-/* Used to prevent multiple access to device */
-static int               mvcust_device_open = 0;
-static struct miscdevice mvcust_misc_dev;
-
-/*******************************************************************************
-**
-**  mvcust_dev_open
-**  ___________________________________________________________________________
-**
-**  DESCRIPTION: The function execute device open actions
-**
-**  PARAMETERS:  
-**               
-**               
-**               
-**
-**  OUTPUTS:     None
-**
-**  RETURNS:     (0)
-**
-*******************************************************************************/
-static int mvcust_dev_open(struct inode *inode, struct file *file)
-{
-	MVCUST_DEBUG_PRINT("Enter");
-
-   // if (mvcust_device_open > 0)
-   //     return -EBUSY;
-
-	mvcust_device_open++;
-
-	return 0;
-}
-
-/*******************************************************************************
-**
-**  mvcust_dev_release
-**  ___________________________________________________________________________
-**
-**  DESCRIPTION: The function execute device release actions
-**
-**  PARAMETERS:  
-**               
-**               
-**               
-**
-**  OUTPUTS:     None
-**
-**  RETURNS:     (0)
-**
-*******************************************************************************/
-static int mvcust_dev_release(struct inode *inode, struct file *file)
-{
-    MVCUST_DEBUG_PRINT("Enter");
-
-   // if (mvcust_device_open > 0)
-   //     mvcust_device_open--;
-
-	return 0;
-}
-
-/*******************************************************************************
-**
-**  mv_cust_dev_ioctl
-**  ___________________________________________________________________________
-**
-**  DESCRIPTION: The function execute IO commands
-**
-**  PARAMETERS:  struct inode *inode
-**               struct file *filp
-**               unsigned int cmd
-**               unsigned long arg
-**
-**  OUTPUTS:     None
-**
-**  RETURNS:     (0)
-**
-*******************************************************************************/
-static int mvcust_dev_ioctl(struct inode *inode, struct file *filp, unsigned int cmd, unsigned long arg)
-{
-    mv_cust_ioctl_omci_set_t      cust_omci_set;
-    mv_cust_ioctl_llid_set_t      cust_llid_set;
-    mv_cust_ioctl_flow_map_t      cust_flow_map;
-    mv_cust_ioctl_dscp_map_t      cust_dscp_map;
-    mv_cust_ioctl_app_etype_t     cust_app_etype;
-    int                           enable;
-    int                           rc;
-
-  int  ret = -EINVAL;
-
-    MVCUST_DEBUG_PRINT("Enter");
-
-  switch(cmd)
-  {
-      case MV_CUST_IOCTL_OMCI_SET:
-
-          if(copy_from_user(&cust_omci_set, (mv_cust_ioctl_omci_set_t*)arg, sizeof(mv_cust_ioctl_omci_set_t)))
-          {
-            MVCUST_ERR_PRINT("copy_from_user failed\n");
-            goto ioctlErr;
-          }
-
-          rc = mv_cust_omci_set(cust_omci_set.tcont, cust_omci_set.txq, cust_omci_set.gemport, cust_omci_set.keep_rx_mh);
-          if (rc != 0) {
-              MVCUST_ERR_PRINT("mv_cust_omci_set failed\n");
-          }
-          ret = 0;
-          break;
-
-      case MV_CUST_IOCTL_OMCI_ENABLE:
-
-          if(copy_from_user(&enable, (int *)arg, sizeof(int)))
-          {
-            MVCUST_ERR_PRINT("copy_from_user failed\n");
-            goto ioctlErr;
-          }
-          mv_cust_app_flag_set(MV_CUST_APP_TYPE_OMCI, enable);
-          ret = 0;
-          break;
-
-      case MV_CUST_IOCTL_EOAM_ENABLE:
-
-          if(copy_from_user(&enable, (int *)arg, sizeof(int)))
-          {
-            MVCUST_ERR_PRINT("copy_from_user failed\n");
-            goto ioctlErr;
-          }
-          mv_cust_app_flag_set(MV_CUST_APP_TYPE_OAM, enable);
-          ret = 0;
-          break;
-
-      case MV_CUST_IOCTL_EOAM_LLID_SET:
-          if(copy_from_user(&cust_llid_set, (int *)arg, sizeof(mv_cust_ioctl_llid_set_t)))
-          {
-            MVCUST_ERR_PRINT("copy_from_user failed\n");
-            goto ioctlErr;
-          }
-
-          mv_cust_eoam_llid_set(cust_llid_set.llid, &(cust_llid_set.llid_mac[0]), cust_llid_set.txq);
-          ret = 0;
-          break;
-
-      case MV_CUST_IOCTL_MAP_RULE_SET:
-          if(copy_from_user(&cust_flow_map, (mv_cust_ioctl_flow_map_t *)arg, sizeof(mv_cust_ioctl_flow_map_t)))
-          {
-            MVCUST_ERR_PRINT("copy_from_user failed\n");
-            goto ioctlErr;
-          }
-
-          ret = mv_cust_map_rule_set(&cust_flow_map);
-          break;
-
-      case MV_CUST_IOCTL_DSCP_MAP_SET:
-          if(copy_from_user(&cust_dscp_map, (mv_cust_ioctl_dscp_map_t *)arg, sizeof(mv_cust_ioctl_dscp_map_t)))
-          {
-            MVCUST_ERR_PRINT("copy_from_user failed\n");
-            goto ioctlErr;
-          }
-
-          ret = mv_cust_dscp_map_set(&cust_dscp_map.dscp_map);
-          break;          
-
-      case MV_CUST_IOCTL_MAP_RULE_DEL:
-          if(copy_from_user(&cust_flow_map, (mv_cust_ioctl_flow_map_t *)arg, sizeof(mv_cust_ioctl_flow_map_t)))
-          {
-            MVCUST_ERR_PRINT("copy_from_user failed\n");
-            goto ioctlErr;
-          }
-
-          ret = mv_cust_map_rule_del((uint16_t)cust_flow_map.vid, (uint8_t)cust_flow_map.pbits, cust_flow_map.dir);
-          break; 
-
-      case MV_CUST_IOCTL_DSCP_MAP_DEL:
-
-          ret = mv_cust_dscp_map_del();
-          break; 
-          
-      case MV_CUST_IOCTL_MAP_RULE_CLEAR:
-      
-          ret = mv_cust_map_rule_clear();
-          break; 
-          
-      case MV_CUST_IOCTL_TAG_MAP_RULE_GET:
-          if(copy_from_user(&cust_flow_map, (mv_cust_ioctl_flow_map_t *)arg, sizeof(mv_cust_ioctl_flow_map_t)))
-          {
-            MVCUST_ERR_PRINT("copy_from_user failed\n");
-            goto ioctlErr;
-          }
-      
-          ret = mv_cust_tag_map_rule_get(&cust_flow_map);
-
-          if(ret != MV_CUST_OK)
-            goto ioctlErr;
-
-          if(copy_to_user((mv_cust_ioctl_flow_map_t*)arg, &cust_flow_map, sizeof(mv_cust_ioctl_flow_map_t)))
-          {
-            printk(KERN_ERR "ERROR: (%s:%d) copy_to_user failed\n", __FUNCTION__, __LINE__);
-            goto ioctlErr;
-          }          
-          break;
-
-      case MV_CUST_IOCTL_UNTAG_MAP_RULE_GET:
-          if(copy_from_user(&cust_flow_map, (mv_cust_ioctl_flow_map_t *)arg, sizeof(mv_cust_ioctl_flow_map_t)))
-          {
-            MVCUST_ERR_PRINT("copy_from_user failed\n");
-            goto ioctlErr;
-          }
-      
-          ret = mv_cust_untag_map_rule_get(&cust_flow_map);
-
-          if(ret != MV_CUST_OK)
-            goto ioctlErr;
-
-          if(copy_to_user((mv_cust_ioctl_flow_map_t*)arg, &cust_flow_map, sizeof(mv_cust_ioctl_flow_map_t)))
-          {
-            printk(KERN_ERR "ERROR: (%s:%d) copy_to_user failed\n", __FUNCTION__, __LINE__);
-            goto ioctlErr;
-          }          
-          break;
-
-      case MV_CUST_IOCTL_APP_ETH_TYPE_SET:
-          if(copy_from_user(&cust_app_etype, (mv_cust_ioctl_app_etype_t *)arg, sizeof(mv_cust_ioctl_app_etype_t)))
-          {
-            MVCUST_ERR_PRINT("copy_from_user failed\n");
-            goto ioctlErr;
-          }
-
-          mv_cust_app_etype_set(cust_app_etype.app_type, cust_app_etype.eth_type);\
-          ret = 0;
-          break; 
-
-      default:
-          ret = -EINVAL;
-  }
-
-ioctlErr:
-    return(ret);
-}
-
-
-static const struct file_operations mvcust_dev_fops = 
-{
-	.open    = mvcust_dev_open,
-	.release = mvcust_dev_release,
-	.ioctl   = mvcust_dev_ioctl,
-};
-
-/*******************************************************************************
-**
-**  mvcust_dev_init
-**  ___________________________________________________________________________
-**
-**  DESCRIPTION: 
-**
-**  PARAMETERS:  
-**               
-**               
-**               
-**
-**  OUTPUTS:     None
-**
-**  RETURNS:     (0)
-**
-*******************************************************************************/
-int32_t mvcust_dev_init(void)
-{
-    int rc;
-
-    MVCUST_DEBUG_PRINT("Enter");
-    
-    mvcust_misc_dev.minor = MISC_DYNAMIC_MINOR;
-    mvcust_misc_dev.name  = MVCUST_DEVICE_NAME;
-    mvcust_misc_dev.fops  = &mvcust_dev_fops;
-
-    rc = misc_register(&mvcust_misc_dev);
-    if (rc != 0) 
-    {
-        MVCUST_ERR_PRINT("rc=%d",rc);
-        return rc;
-    }
-
-    rc = mvcust_netdev_init();
-    if (rc != 0) 
-    {
-        MVCUST_ERR_PRINT("rc=%d",rc);
-        return rc;
-    }
-
-    rc = mvcust_sysfs_init();
-    if (rc != 0) 
-    {
-        MVCUST_ERR_PRINT("rc=%d",rc);
-        return rc;
-    }
-
-    printk("MVCUST: misc device %s registered with minor: %d\n", MVCUST_DEVICE_NAME, mvcust_misc_dev.minor);
-    return 0;
-}
-
-
-/*******************************************************************************
-**
-**  mvcust_dev_shutdown
-**  ___________________________________________________________________________
-**
-**  DESCRIPTION: 
-**
-**  PARAMETERS:  
-**               
-**               
-**               
-**
-**  OUTPUTS:     None
-**
-**  RETURNS:     (0)
-**
-*******************************************************************************/
-void mvcust_dev_shutdown(void)
-{
-	MVCUST_DEBUG_PRINT("Enter");
-
-    mvcust_sysfs_delete();
-
-    misc_deregister(&mvcust_misc_dev);
-}
-
-
-
-
-
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_dev.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_dev.h
deleted file mode 100755
index e42503c..0000000
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_dev.h
+++ /dev/null
@@ -1,81 +0,0 @@
-/************************************************************************
-* 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_dev.h
-*
-* DESCRIPTION:
-*
-*
-*******************************************************************************/
-
-#ifndef __MV_CUST_DEV_H__
-#define __MV_CUST_DEV_H__
-
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-
-#define MV_CUST_VERSION     "V2.6.25"
-#define MVCUST_DEVICE_NAME  "cust"
-#define MV_CUST_IOCTL_MAGIC ('C')
-
-#ifdef MV_CUST_DEBUG
-#define MVCUST_DEBUG_PRINT(format, ...)   printk("%s(%d):  "format,__FUNCTION__,__LINE__, ##__VA_ARGS__)
-#else
-#define MVCUST_DEBUG_PRINT(format, ...)
-#endif
-
-#define MVCUST_ERR_PRINT(format, ...)     printk("%s(%d) ERROR:  "format,__FUNCTION__,__LINE__, ##__VA_ARGS__)
-
-extern struct bus_type platform_bus_type;
-
-/******************************************************
- * Function prototypes --                             *
-*******************************************************/
-int32_t mvcust_dev_init        (void);
-void    mvcust_dev_shutdown    (void);
-
-int     mvcust_netdev_init     (void);
-int     mvcust_sysfs_init      (void);
-void    mvcust_sysfs_delete    (void);
-
-#ifdef CONFIG_MV_CUST_UDP_SAMPLE_HANDLE
-void    mv_cust_udp_spec_print_all (void);
-#endif
-void    mv_cust_debug_info_set     (int val);
-
-#ifdef CONFIG_MV_CUST_FLOW_MAP_HANDLE
-void mv_cust_flow_map_print(void);
-
-#endif
-
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /*__MV_CUST_DEV_H__*/
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_flow_map.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_flow_map.c
deleted file mode 100755
index 97733e9..0000000
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_flow_map.c
+++ /dev/null
@@ -1,1076 +0,0 @@
-/************************************************************************
-* 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_flow_map.c
-*
-* DESCRIPTION:
-*   Victor  - initial version created.   12/Dec/2011 
-*
-*******************************************************************************/
-#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>
-
-#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"
-
-/****************************************************************************** 
-*                           Global Data Definitions                                                                                                                                  
-******************************************************************************/
-static mv_cust_vid_index_t  gs_vid_index_table[MV_CUST_FLOW_DIR_NUM];
-static mv_cust_pbits_map_t  gs_pbits_map_table[MV_CUST_FLOW_DIR_NUM][MV_CUST_MAX_PBITS_MAP_TABLE_SIZE];
-static mv_cust_dscp_pbits_t gs_dscp_map_table;
-
-static uint32_t gs_mv_cust_trace_flag = 0;
-
-/* Defined to support T-CONT state */
-static bool gs_tcont_state[CPH_MAX_TCONT_NUM];
-
-
-/****************************************************************************** 
-*                           External Declarations                                                                                                                                 
-******************************************************************************/
-
-
-
-/****************************************************************************** 
-*                           Function Definitions                                                                                                                                  
-******************************************************************************/
-/*******************************************************************************
-**
-**    mv_cust_set_trace_flag
-**    ___________________________________________________________________________
-**
-**    DESCRIPTION: The function sets mv_cust trace flag.
-**
-**    INPUTS:
-**      enTrace     - Enable or disable mv_cust trace.
-**
-**    OUTPUTS:
-**      None. 
-**
-**    RETURNS:     
-**      On success, the function returns (MV_CUST_OK). On error different types are 
-**    returned according to the case.  
-**
-*******************************************************************************/    
-int mv_cust_set_trace_flag(uint32_t enTrace)
-{
-    gs_mv_cust_trace_flag = enTrace;
-    
-    return MV_CUST_OK;
-}
-
-/*******************************************************************************
-**
-**    mv_cust_valid_pbits_table_get
-**    ___________________________________________________________________________
-**
-**    DESCRIPTION: The function gets available P-bits mapping table.
-**
-**    INPUTS:
-**      None.
-**
-**    OUTPUTS:
-**      None. 
-**
-**    RETURNS:     
-**      Available P-bits mapping table index.  
-**
-*******************************************************************************/    
-uint32_t mv_cust_valid_pbits_table_get(mv_cust_flow_dir_e dir)
-{
-    uint32_t table_idx = 0;
-
-    /* Table index MV_CUST_MAX_PBITS_MAP_TABLE_SIZE is reserved for tagged default packets */
-    for (table_idx=0; table_idx<MV_CUST_MAX_PBITS_MAP_TABLE_SIZE-1; table_idx++) {
-    
-       if (gs_pbits_map_table[dir][table_idx].in_use == 0)
-           return table_idx;
-    }
-    
-    return MV_CUST_INVALID_PBITS_TABLE_INDEX;
-}
-
-/*******************************************************************************
-**
-**    mv_cust_pbits_table_status_get
-**    ___________________________________________________________________________
-**
-**    DESCRIPTION: The function gets P-bits mapping table status.
-**
-**    INPUTS:
-**      pbits_map   - P-bits mapping table.
-**
-**    OUTPUTS:
-**      None. 
-**
-**    RETURNS:     
-**      0:No P-bits mapping rule exist in current table, 1: still exists P-bits mapping rule.  
-**
-*******************************************************************************/    
-uint32_t mv_cust_pbits_table_status_get(mv_cust_pbits_map_t *pbits_map)
-{
-    uint32_t pbits_idx = 0;
-
-    for (pbits_idx=0; pbits_idx<MV_CUST_PBITS_MAP_MAX_ENTRY_NUM; pbits_idx++) {
-    
-       if (pbits_map->pkt_fwd[pbits_idx].in_use != 0)
-           return MV_CUST_OK;
-    }
-    
-    return MV_CUST_FAIL;
-}
-
-/*******************************************************************************
-**
-**    mv_cust_map_rule_set
-**    ___________________________________________________________________________
-**
-**    DESCRIPTION: The function sets GPON flow mapping rules
-**
-**    INPUTS:
-**      cust_flow  - VLAN ID, 802.1p value, pkt_fwd information.
-**
-**    OUTPUTS:
-**      None. 
-**
-**    RETURNS:     
-**      On success, the function returns (MV_CUST_OK). On error different types are 
-**    returned according to the case.  
-**
-*******************************************************************************/
-int mv_cust_map_rule_set(mv_cust_ioctl_flow_map_t *cust_flow)
-{
-    uint8_t              *pVidEntry  = NULL;
-    mv_cust_pbits_map_t  *pPbitsMap  = NULL;
-    uint32_t              pbitsIndex = 0;
-    uint32_t              index      = 0;
-    uint32_t              vid        = 0;
-    uint32_t              pbits      = 0;
-    uint32_t              mod_vid    = 0;
-    uint32_t              mod_pbits  = 0;   
-    mv_cust_flow_dir_e    dir        = MV_CUST_FLOW_DIR_US;    
-    mv_cust_pkt_frwd_t   *pkt_fwd    = NULL;
-
-    /* Get input information: VID, P-bits... */
-    if (cust_flow == NULL) {
-        MVCUST_ERR_PRINT(KERN_ERR "cust_flow is NULL \n\r");
-        return MV_CUST_FAIL;
-    } 
-    vid       =  cust_flow->vid;
-    pbits     =  cust_flow->pbits;
-    mod_vid   =  cust_flow->mod_vid;
-    mod_pbits =  cust_flow->mod_pbits;    
-    pkt_fwd   = &cust_flow->pkt_frwd;
-    dir       =  cust_flow->dir;
-
-    if (gs_mv_cust_trace_flag) {
-        MVCUST_TRACE_PRINT(KERN_INFO,
-                           "==ENTER==%s: vid[%d], pbits[%d], mod_vid[%d], mod_pbits[%d], T-CONT[%d], SWF queue[%d], HWF queue[%d], GEM port[%d], dir[%d]\n\r",
-                            __FUNCTION__, vid, pbits, mod_vid, mod_pbits,
-                           ((pkt_fwd!= NULL)? pkt_fwd->trg_port:0), 
-                           ((pkt_fwd!= NULL)? pkt_fwd->trg_queue:0), 
-                           ((pkt_fwd!= NULL)? pkt_fwd->trg_hwf_queue:0),                            
-                           ((pkt_fwd!= NULL)? pkt_fwd->gem_port:0),
-                           cust_flow->dir);
-    }
-
-    /* Check VID */
-    if (vid > (MV_CUST_VID_INDEX_TABLE_MAX_SIZE - 1)) {
-        MVCUST_ERR_PRINT(KERN_ERR "vid[%d] exceeds maximum value[%d] \n\r", vid, (MV_CUST_VID_INDEX_TABLE_MAX_SIZE - 1));
-        return MV_CUST_FAIL;
-    }
-    
-    /* Check P-bits */
-    if (pbits > MV_CUST_PBITS_NOT_CARE_VALUE) {
-        MVCUST_ERR_PRINT(KERN_ERR "P-bits[%d] exceeds maximum value[%d] \n\r", pbits, MV_CUST_PBITS_NOT_CARE_VALUE);
-        return MV_CUST_FAIL;
-    }
-
-    /* Check mod VID */
-    if (mod_vid > (MV_CUST_VID_INDEX_TABLE_MAX_SIZE - 1)) {
-        MVCUST_ERR_PRINT(KERN_ERR "mod_vid[%d] exceeds maximum value[%d] \n\r", mod_vid, (MV_CUST_VID_INDEX_TABLE_MAX_SIZE - 1));
-        return MV_CUST_FAIL;
-    }
-    
-    /* Check mod P-bits */
-    if (mod_pbits > MV_CUST_PBITS_NOT_CARE_VALUE) {
-        MVCUST_ERR_PRINT(KERN_ERR "mod_pbits[%d] exceeds maximum value[%d] \n\r", mod_pbits, MV_CUST_PBITS_NOT_CARE_VALUE);
-        return MV_CUST_FAIL;
-    }
-
-    /* Check dir */
-    if (dir >= MV_CUST_FLOW_DIR_NUM) {
-        MVCUST_ERR_PRINT(KERN_ERR "dir[%d] exceeds maximum value[%d] \n\r", dir, MV_CUST_FLOW_DIR_NUM-1);
-        return MV_CUST_FAIL;
-    }    
-
-    /* Check target port/queue/GEM port */ 
-    if (pkt_fwd->trg_port > MV_CUST_MAX_TRG_PORT_VALUE) {
-        MVCUST_ERR_PRINT(KERN_ERR "trg_port[%d] exceeds maximum value[%d] \n\r", pkt_fwd->trg_port, MV_CUST_MAX_TRG_PORT_VALUE);
-        return MV_CUST_FAIL;
-    }
-
-    if (pkt_fwd->trg_queue > MV_CUST_MAX_TRG_QUEUE_VALUE) {
-        MVCUST_ERR_PRINT(KERN_ERR "SWF trg_queue[%d] exceeds maximum value[%d] \n\r", pkt_fwd->trg_queue, MV_CUST_MAX_TRG_QUEUE_VALUE);
-        return MV_CUST_FAIL;
-    }   
-
-    if (pkt_fwd->trg_hwf_queue > MV_CUST_MAX_TRG_QUEUE_VALUE) {
-        MVCUST_ERR_PRINT(KERN_ERR "HWF trg_queue[%d] exceeds maximum value[%d] \n\r", pkt_fwd->trg_queue, MV_CUST_MAX_TRG_QUEUE_VALUE);
-        return MV_CUST_FAIL;
-    }       
-
-    if (pkt_fwd->gem_port > MV_CUST_MAX_GEM_PORT_VALUE) {
-        MVCUST_ERR_PRINT(KERN_ERR "trg_queue[%d] exceeds maximum value[%d] \n\r", pkt_fwd->gem_port, MV_CUST_MAX_GEM_PORT_VALUE);
-        return MV_CUST_FAIL;
-    }          
- 
-    
-    /* Find VID index entry by VID */    
-    pVidEntry = &gs_vid_index_table[dir].pbits_map_index[vid];
-
-    /* Get P-bits mapping table */
-    /* If this VID index entry does not point to any P-bits mapping table,
-       need to search for an available P-bits mapping table             */
-    if (*pVidEntry >= MV_CUST_MAX_PBITS_MAP_TABLE_SIZE) {
-        /* Reserved for default tagged rule */
-        if (vid == MV_CUST_DEFAULT_SINGLE_TAG_RULE)
-            pbitsIndex = MV_CUST_MAX_PBITS_MAP_TABLE_SIZE - 1;
-        else
-            pbitsIndex = mv_cust_valid_pbits_table_get(dir);
-        
-        if (pbitsIndex >= MV_CUST_MAX_PBITS_MAP_TABLE_SIZE) {
-            MVCUST_ERR_PRINT(KERN_ERR " %d P-bits mapping table has used out\n\r", MV_CUST_INVALID_PBITS_TABLE_INDEX);
-            return MV_CUST_FAIL;     
-        }            
-    }
-    /* In case that the VID index already points to a P-bits mapping table,
-       Need to replace the forwarding information of this P-bit mapping table */
-    else {
-        pbitsIndex = *pVidEntry;
-    }
-    pPbitsMap = &gs_pbits_map_table[dir][pbitsIndex];
-
-    /* If legal P-bits is configured */
-    if (pbits < MV_CUST_PBITS_NOT_CARE_VALUE) {
-        /* In case to enable packet forwarding */
-        if (pkt_fwd->in_use != 0) {
-        
-            /* Save forwarding information */        
-            pPbitsMap->pkt_fwd[pbits].trg_port      = pkt_fwd->trg_port;
-            pPbitsMap->pkt_fwd[pbits].trg_queue     = pkt_fwd->trg_queue;
-            pPbitsMap->pkt_fwd[pbits].trg_hwf_queue = pkt_fwd->trg_hwf_queue;
-            pPbitsMap->pkt_fwd[pbits].gem_port      = pkt_fwd->gem_port;
-
-            /* Save mod_vid mod_pbits */
-            pPbitsMap->mod_vid[pbits]           = mod_vid;
-            pPbitsMap->mod_pbits[pbits]         = mod_pbits;
-
-            /* Enable in_use flag */
-            pPbitsMap->pkt_fwd[pbits].in_use    = 1;
-            pPbitsMap->in_use                   = 1;
-
-            /* Save P-bit mapping table index in VID index table */
-            *pVidEntry = pbitsIndex;                
-        }
-        /* In case to disable packet forwarding */
-        else {
-            /* Clear forwarding information */        
-            pPbitsMap->pkt_fwd[pbits].trg_port      = 0;
-            pPbitsMap->pkt_fwd[pbits].trg_queue     = 0;
-            pPbitsMap->pkt_fwd[pbits].trg_hwf_queue = 0;
-            pPbitsMap->pkt_fwd[pbits].gem_port      = 0;
-
-            /* Clear mod_vid mod_pbits */
-            pPbitsMap->mod_vid[pbits]           = 0;
-            pPbitsMap->mod_pbits[pbits]         = 0;          
-
-            /* Disable in_use flag */
-            pPbitsMap->pkt_fwd[pbits].in_use    = 0;
-            if (mv_cust_pbits_table_status_get(pPbitsMap) != MV_CUST_OK) {
-                pPbitsMap->in_use               = 0;
-                *pVidEntry                      = MV_CUST_INVALID_PBITS_TABLE_INDEX;
-            }
-            else {
-                pPbitsMap->in_use               = 1;
-                *pVidEntry                      = pbitsIndex;
-            }
-        }
-    }
-    /* If does not care for P-bits, each P-bits mapping entry should be set */
-    else if (pbits == MV_CUST_PBITS_NOT_CARE_VALUE) {
-
-        index = MV_CUST_PBITS_NOT_CARE_VALUE;
-
-        /* In case to enable packet forwarding */
-        if (pkt_fwd->in_use != 0) {
-            
-            /* Save forwarding information */        
-            pPbitsMap->pkt_fwd[index].trg_port      = pkt_fwd->trg_port;
-            pPbitsMap->pkt_fwd[index].trg_queue     = pkt_fwd->trg_queue;
-            pPbitsMap->pkt_fwd[pbits].trg_hwf_queue = pkt_fwd->trg_hwf_queue;
-            pPbitsMap->pkt_fwd[index].gem_port      = pkt_fwd->gem_port;
-            pPbitsMap->pkt_fwd[index].in_use        = 1;
-            
-            /* Save mod_vid mod_pbits */
-            pPbitsMap->mod_vid[index]           = mod_vid;
-            pPbitsMap->mod_pbits[index]         = mod_pbits;           
-
-            /* Enable in_use flag */
-            pPbitsMap->in_use = 1;
-
-            /* Save P-bit mapping table index in VID index table */
-            *pVidEntry = pbitsIndex;                
-        }
-        /* In case to disable packet forwarding */
-        else { 
-            
-            /* Clear forwarding information */        
-            pPbitsMap->pkt_fwd[index].trg_port      = 0;
-            pPbitsMap->pkt_fwd[index].trg_queue     = 0;
-            pPbitsMap->pkt_fwd[pbits].trg_hwf_queue = 0;
-            pPbitsMap->pkt_fwd[index].gem_port      = 0;
-            pPbitsMap->pkt_fwd[index].in_use        = 0;
-            /* clear mod_vid mod_pbits */
-            pPbitsMap->mod_vid[index]           = 0;
-            pPbitsMap->mod_pbits[index]         = 0;
-
-            /* Disable in_use flag */
-            if (mv_cust_pbits_table_status_get(pPbitsMap) != MV_CUST_OK) {
-                pPbitsMap->in_use               = 0;
-                *pVidEntry                      = MV_CUST_INVALID_PBITS_TABLE_INDEX;
-            }
-            else {
-                pPbitsMap->in_use               = 1;
-                *pVidEntry                      = pbitsIndex;
-            }
-        }        
-    }
-
-    if (gs_mv_cust_trace_flag) {
-        MVCUST_TRACE_PRINT(KERN_INFO,
-                           "==EXIT== %s:\n\r",__FUNCTION__);
-    }
-
-    return MV_CUST_OK;   
-}
-
-/*******************************************************************************
-**
-**    mv_cust_dscp_map_set
-**    ___________________________________________________________________________
-**
-**    DESCRIPTION: The function sets GPON DSCP to P-bits mapping rules
-**
-**    INPUTS:
-**      dscp_map     - DSCP to P-bits mapping rules.
-**
-**    OUTPUTS:
-**      None. 
-**
-**    RETURNS:     
-**      On success, the function returns (MV_CUST_OK). On error different types are 
-**    returned according to the case.  
-**
-*******************************************************************************/
-int mv_cust_dscp_map_set(mv_cust_dscp_pbits_t *dscp_map)
-{
-    if (gs_mv_cust_trace_flag) {
-        MVCUST_TRACE_PRINT(KERN_INFO,
-                           "==ENTER==%s: in_use[%d]\n\r", __FUNCTION__,
-                           ((dscp_map!= NULL)? dscp_map->in_use:0));
-    }
-
-    if (dscp_map == NULL) {
-        MVCUST_ERR_PRINT(KERN_ERR "Input dscp_map is NULL\n\r");
-        return MV_CUST_FAIL;
-    }
-
-    /* Case 1: to enable DSCP to P-bits mapping */
-    if (dscp_map->in_use != 0) {
-        memcpy(&gs_dscp_map_table.pbits[0], &dscp_map->pbits[0], sizeof(gs_dscp_map_table.pbits));
-        gs_dscp_map_table.in_use = 1;
-    
-    }
-    /* Case 2: to disable DSCP to P-bits mapping */
-    else {
-        memset((uint8_t *)&gs_dscp_map_table, 0, sizeof(gs_dscp_map_table));
-        gs_dscp_map_table.in_use = 0;
-    }
-    
-    if (gs_mv_cust_trace_flag) {
-        MVCUST_TRACE_PRINT(KERN_INFO,
-                           "==EXIT== %s:\n\r",__FUNCTION__);
-    }
-    
-    return MV_CUST_OK;   
-}
-
-/*******************************************************************************
-**
-**    mv_cust_map_rule_del
-**    ___________________________________________________________________________
-**
-**    DESCRIPTION: The function deletes GPON flow mapping rules
-**
-**    INPUTS:
-**      vid         - VLAN ID.
-**      pbits      - 802.1p value.
-**
-**    OUTPUTS:
-**      None 
-**
-**    RETURNS:     
-**      On success, the function returns (MV_CUST_OK). On error different types are 
-**    returned according to the case.  
-**
-*******************************************************************************/
-int mv_cust_map_rule_del(uint16_t vid, uint8_t pbits, mv_cust_flow_dir_e dir)
-{
-    uint8_t              *pVidEntry  = NULL;
-    mv_cust_pbits_map_t  *pPbitsMap  = NULL;
-
-    if (gs_mv_cust_trace_flag) {
-        MVCUST_TRACE_PRINT(KERN_INFO,
-                           "==ENTER==: vid[%d],pbits[%d]\n\r", vid, pbits);
-    }
-
-    /* Check VID */
-    if (vid > (MV_CUST_VID_INDEX_TABLE_MAX_SIZE - 1)) {
-        MVCUST_ERR_PRINT(KERN_ERR "vid[%d] exceeds maximum value[%d] \n\r", vid, (MV_CUST_VID_INDEX_TABLE_MAX_SIZE - 1));
-        return MV_CUST_FAIL;
-    }
-    
-    /* Check P-bits */
-    if (pbits > MV_CUST_PBITS_NOT_CARE_VALUE) {
-        MVCUST_ERR_PRINT(KERN_ERR "P-bits[%d] exceeds maximum value[%d] \n\r", pbits, MV_CUST_PBITS_NOT_CARE_VALUE);
-        return MV_CUST_FAIL;
-    }
-
-    /* Check dir */
-    if (dir >= MV_CUST_FLOW_DIR_NUM) {
-        MVCUST_ERR_PRINT(KERN_ERR "dir[%d] exceeds maximum value[%d] \n\r", dir, MV_CUST_FLOW_DIR_NUM-1);
-        return MV_CUST_FAIL;
-    }   
-
-    /* Find VID index entry by VID */    
-    pVidEntry = &gs_vid_index_table[dir].pbits_map_index[vid];
-
-    if (*pVidEntry >= MV_CUST_MAX_PBITS_MAP_TABLE_SIZE) {
-         MVCUST_TRACE_PRINT(KERN_INFO,"%s, pVidEntry[%d], does not need to delete \n\r",__FUNCTION__, *pVidEntry);    
-        return MV_CUST_OK;                      
-    }
-
-    /* Find P-bits mapping table */
-    pPbitsMap = &gs_pbits_map_table[dir][*pVidEntry];
-
-    /* Delete P-bits mapping rule */
-    pPbitsMap->pkt_fwd[pbits].trg_port   = 0;
-    pPbitsMap->pkt_fwd[pbits].trg_queue  = 0;
-    pPbitsMap->pkt_fwd[pbits].gem_port   = 0;
-    pPbitsMap->pkt_fwd[pbits].in_use     = 0;
-    pPbitsMap->mod_vid[pbits]            = 0;
-    pPbitsMap->mod_pbits[pbits]          = 0;          
-    
-    /* Disable in_use flag */
-    if (mv_cust_pbits_table_status_get(pPbitsMap)!= MV_CUST_OK) {
-        pPbitsMap->in_use                = 0;
-        *pVidEntry                       = MV_CUST_INVALID_PBITS_TABLE_INDEX;
-    }
-
-    if (gs_mv_cust_trace_flag) {
-        MVCUST_TRACE_PRINT(KERN_INFO,
-                           "==EXIT==:\n\r");
-    }
-
-    return MV_CUST_OK;  
-}
-
-/*******************************************************************************
-**
-**    mv_cust_dscp_map_del
-**    ___________________________________________________________________________
-**
-**    DESCRIPTION: The function deletes DSCP to P-bits mapping rules
-**
-**    INPUTS:
-**      None.
-**
-**    OUTPUTS:
-**      None. 
-**
-**    RETURNS:     
-**      On success, the function returns (MV_CUST_OK). On error different types are 
-**    returned according to the case.  
-**
-*******************************************************************************/
-int mv_cust_dscp_map_del(void)
-{
-    if (gs_mv_cust_trace_flag) {
-        MVCUST_TRACE_PRINT(KERN_INFO,
-                           "==ENTER==\n\r");
-    }
-
-    /* Clear DSCP to P-bits mapping */
-    else {
-        memset((uint8_t *)&gs_dscp_map_table, 0, sizeof(gs_dscp_map_table));
-        gs_dscp_map_table.in_use = 0;
-    }
-    
-    if (gs_mv_cust_trace_flag) {
-        MVCUST_TRACE_PRINT(KERN_INFO,
-                           "==EXIT==\n\r");
-    }
-    
-    return MV_CUST_OK;
-}
-
-/*******************************************************************************
-**
-**    mv_cust_map_rule_clear
-**    ___________________________________________________________________________
-**
-**    DESCRIPTION: The function clears all GPON flow mapping rules
-**
-**    INPUTS:
-**      None.
-**
-**    OUTPUTS:
-**      None. 
-**
-**    RETURNS:     
-**      On success, the function returns (MV_CUST_OK). On error different types are 
-**    returned according to the case.  
-**
-*******************************************************************************/
-int mv_cust_map_rule_clear(void)
-{
-    uint32_t pbits_index = 0;
-
-    if (gs_mv_cust_trace_flag) {
-        MVCUST_TRACE_PRINT(KERN_INFO,
-                           "==ENTER==\n\r");
-    }
-
-    /* Clear VID index table              */
-    memset((uint8_t *)&gs_vid_index_table, MV_CUST_INVALID_PBITS_TABLE_INDEX, sizeof(gs_vid_index_table));
-
-    /* Clear P-bits mapping tables        */
-    for (pbits_index=0; pbits_index<MV_CUST_MAX_PBITS_MAP_TABLE_SIZE; pbits_index++) {
-        memset((uint8_t *)&gs_pbits_map_table[MV_CUST_FLOW_DIR_US][pbits_index], 0, sizeof(mv_cust_pbits_map_t));
-        gs_pbits_map_table[MV_CUST_FLOW_DIR_US][pbits_index].in_use = 0;
-        memset((uint8_t *)&gs_pbits_map_table[MV_CUST_FLOW_DIR_DS][pbits_index], 0, sizeof(mv_cust_pbits_map_t));
-        gs_pbits_map_table[MV_CUST_FLOW_DIR_DS][pbits_index].in_use = 0;        
-    }
-
-    if (gs_mv_cust_trace_flag) {
-        MVCUST_TRACE_PRINT(KERN_INFO,
-                           "==EXIT==\n\r");
-    }
-
-    return MV_CUST_OK;  
-}
-
-/*******************************************************************************
-**
-**    mv_cust_tag_map_rule_get
-**    ___________________________________________________________________________
-**
-**    DESCRIPTION: The function gets GPON flow mapping rule for tagged frames.
-**
-**  INPUTS:
-**    cust_flow  - parsing vid, pbits, dir
-**
-**  OUTPUTS:
-**    cust_flow  - out packet forwarding information, including GEM port, T-CONT, queue.
-**                 and packet modification for VID, P-bits
-**
-**    RETURNS:     
-**      On success, the function returns (MV_CUST_OK). On error different types are 
-**    returned according to the case.  
-**
-*******************************************************************************/
-int mv_cust_tag_map_rule_get(mv_cust_ioctl_flow_map_t *cust_flow)
-{
-    uint8_t              *pVidEntry  = NULL;
-    mv_cust_pbits_map_t  *pPbitsMap  = NULL;
-    mv_cust_pkt_frwd_t   *pPktFrwd   = NULL;
-    uint32_t              vid        = 0;
-    uint32_t              pbits      = 0;  
-    mv_cust_flow_dir_e    dir        = MV_CUST_FLOW_DIR_US;     
-    mv_cust_pkt_frwd_t   *pkt_fwd    = NULL;
-    uint32_t              index      = 0;    
-
-    /* Get input parameters */
-    vid     = cust_flow->vid;
-    pbits   = cust_flow->pbits;
-    dir     = cust_flow->dir;
-    pkt_fwd = &cust_flow->pkt_frwd;
-    
-    if (gs_mv_cust_trace_flag) {
-        MVCUST_TRACE_PRINT(KERN_INFO,
-                           "==ENTER==: vid[%d], pbits[%d], dir[%d]\n\r", vid, pbits, dir);
-    }
-
-    /* Check VID */
-    if (vid > (MV_CUST_VID_INDEX_TABLE_MAX_SIZE - 1)) {
-        MVCUST_ERR_PRINT(KERN_ERR "vid[%d] exceeds maximum value[%d] \n\r", vid, (MV_CUST_VID_INDEX_TABLE_MAX_SIZE - 1));
-        return MV_CUST_FAIL;
-    }
-    
-    /* Check P-bits */
-    if (pbits > MV_CUST_PBITS_NOT_CARE_VALUE) {
-        MVCUST_ERR_PRINT(KERN_ERR "P-bits[%d] exceeds maximum value[%d] \n\r", pbits, MV_CUST_PBITS_NOT_CARE_VALUE);
-        return MV_CUST_FAIL;
-    }
-
-    /* Check dir */
-    if (dir >= MV_CUST_FLOW_DIR_NUM) {
-        MVCUST_ERR_PRINT(KERN_ERR "dir[%d] exceeds maximum value[%d] \n\r", dir, MV_CUST_FLOW_DIR_NUM-1);
-        return MV_CUST_FAIL;
-    }    
-
-    /* Set default values */
-    cust_flow->mod_vid   = MV_CUST_VID_NOT_CARE_VALUE;
-    cust_flow->mod_pbits = MV_CUST_PBITS_NOT_CARE_VALUE; 
-    
-    /* Find VID index entry by VID */    
-    pVidEntry = &gs_vid_index_table[dir].pbits_map_index[vid];
-
-    if (*pVidEntry >= MV_CUST_MAX_PBITS_MAP_TABLE_SIZE) {
-         //MVCUST_ERR_PRINT(KERN_ERR "%s, pVidEntry[%d], No matched P-bits mapping table \n\r",__FUNCTION__, *pVidEntry);    
-
-        pkt_fwd->in_use = 0;
-        if (gs_mv_cust_trace_flag) {
-            MVCUST_TRACE_PRINT(KERN_INFO,
-                               "==EXIT==\n\r");
-        }
-
-        return MV_CUST_FAIL;                      
-    }
-
-    /* Find P-bits mapping table */
-    pPbitsMap = &gs_pbits_map_table[dir][*pVidEntry];    
-
-    /* Get packet forwarding information */
-    index = pbits;
-
-    pPktFrwd = &pPbitsMap->pkt_fwd[index];
-
-    /* If specific flow mapping rule exists */
-    if (pPktFrwd->in_use != 0) {
-        pkt_fwd->trg_port      = pPktFrwd->trg_port;
-        if (false == mv_cust_get_tcont_state(pPktFrwd->trg_port))
-            pkt_fwd->trg_queue = CPH_INVALID_TRGT_QUEUE;
-         else
-            pkt_fwd->trg_queue = pPktFrwd->trg_queue;
-        pkt_fwd->trg_hwf_queue = pPktFrwd->trg_hwf_queue;
-        pkt_fwd->gem_port      = pPktFrwd->gem_port;
-        cust_flow->mod_vid     = pPbitsMap->mod_vid[index];
-        cust_flow->mod_pbits   = pPbitsMap->mod_pbits[index];
-        pkt_fwd->in_use        = 1;
-        
-        if (gs_mv_cust_trace_flag) {
-            MVCUST_TRACE_PRINT(KERN_INFO,
-                               "trg_port(%d), trg_queue(%d) trg_hwf_queue(%d) gem_port(%d), mod_vid(%d), mod_pbits(%d)\n\r",
-                                cust_flow->pkt_frwd.trg_port, cust_flow->pkt_frwd.trg_queue, cust_flow->pkt_frwd.trg_hwf_queue, 
-                                cust_flow->pkt_frwd.gem_port, cust_flow->mod_vid, cust_flow->mod_pbits);
-        }
-        
-    }
-    /* If specific flow mapping rule does not exist, look for default rule */
-    else {
-        index = MV_CUST_PBITS_NOT_CARE_VALUE;
-        pPktFrwd = &pPbitsMap->pkt_fwd[index];
-
-        /* If default flow mapping rule exists */
-        if (pPktFrwd->in_use != 0) {
-            pkt_fwd->trg_port      = pPktFrwd->trg_port;
-            if (false == mv_cust_get_tcont_state(pPktFrwd->trg_port))
-                pkt_fwd->trg_queue = CPH_INVALID_TRGT_QUEUE;
-             else
-                pkt_fwd->trg_queue = pPktFrwd->trg_queue;
-            pkt_fwd->trg_hwf_queue = pPktFrwd->trg_hwf_queue;
-            pkt_fwd->gem_port      = pPktFrwd->gem_port;
-            cust_flow->mod_vid     = pPbitsMap->mod_vid[index];
-            cust_flow->mod_pbits   = pPbitsMap->mod_pbits[index];
-            pkt_fwd->in_use        = 1;
-            
-            if (gs_mv_cust_trace_flag) {
-                MVCUST_TRACE_PRINT(KERN_INFO,
-                                   "trg_port(%d), trg_queue(%d) trg_hwf_queue(%d) gem_port(%d), mod_vid(%d), mod_pbits(%d)\n\r",
-                                    cust_flow->pkt_frwd.trg_port, cust_flow->pkt_frwd.trg_queue, cust_flow->pkt_frwd.trg_hwf_queue, 
-                                    cust_flow->pkt_frwd.gem_port, cust_flow->mod_vid, cust_flow->mod_pbits);
-            }
-            
-        }
-        else {
-        
-            pkt_fwd->in_use      = 0;
-            if (gs_mv_cust_trace_flag) {
-                MVCUST_TRACE_PRINT(KERN_INFO,
-                                   "==EXIT==\n\r");
-            }        
-            return MV_CUST_FAIL;
-        }
-    }
-
-    if (gs_mv_cust_trace_flag) {
-        MVCUST_TRACE_PRINT(KERN_INFO,
-                           "==EXIT==\n\r");
-    }
-
-    return MV_CUST_OK;
-}
-
-/*******************************************************************************
-**
-**    mv_cust_untag_map_rule_get
-**    ___________________________________________________________________________
-**
-**    DESCRIPTION: The function gets GPON flow mapping rule for untagged frames.
-**
-**    INPUTS:
-**      dscp         - DSCP value.
-**
-**    OUTPUTS:
-**      cust_flow    - packet forwarding information, including GEM port, T-CONT, queue.
-**
-**    RETURNS:     
-**      On success, the function returns (MV_CUST_OK). On error different types are 
-**    returned according to the case.  
-**
-*******************************************************************************/
-int mv_cust_untag_map_rule_get(mv_cust_ioctl_flow_map_t *cust_flow)
-{
-    uint8_t              *pVidEntry  = NULL;
-    mv_cust_pbits_map_t  *pPbitsMap  = NULL;
-    mv_cust_pkt_frwd_t   *pPktFrwd   = NULL;
-    uint32_t              pbitsIndex = MV_CUST_PBITS_NOT_CARE_VALUE;
-    mv_cust_flow_dir_e    dir        = MV_CUST_FLOW_DIR_US;
-    uint32_t              dscp       = 0;
-
-    if (gs_mv_cust_trace_flag) {
-        MVCUST_TRACE_PRINT(KERN_INFO,
-                           "==ENTER==: dscp[%d] \n\r", dscp);
-    }
-
-    /* Check target port/queue/GEM port */
-    if (cust_flow == NULL) {
-        MVCUST_ERR_PRINT(KERN_ERR "cust_flow is NULL \n\r");
-        return MV_CUST_FAIL;
-    }
-
-    /* Set forwarding flag to false at first */
-    cust_flow->pkt_frwd.in_use = 0;
-    cust_flow->mod_vid         = MV_CUST_VID_NOT_CARE_VALUE;
-    cust_flow->mod_pbits       = MV_CUST_PBITS_NOT_CARE_VALUE; 
-
-    dir  = cust_flow->dir;  
-    dscp = cust_flow->dscp;  
-
-    /* Check DSCP */
-    if (dscp < MV_CUST_DSCP_PBITS_TABLE_MAX_SIZE) {
-        if (gs_dscp_map_table.in_use != 0) {
-            pbitsIndex = gs_dscp_map_table.pbits[dscp];
-        }
-    }
-
-    if(pbitsIndex > MV_CUST_PBITS_NOT_CARE_VALUE){
-        //MVCUST_ERR_PRINT(KERN_ERR "pbitsIndex[%d] is illegal \n\r", pbitsIndex);
-        return MV_CUST_FAIL;
-    }
-
-    /* Find P-bits mapping table */
-    pVidEntry = &gs_vid_index_table[dir].pbits_map_index[MV_CUST_DEFAULT_UNTAG_RULE];
-
-    if (*pVidEntry >= MV_CUST_MAX_PBITS_MAP_TABLE_SIZE) {
-        //MVCUST_TRACE_PRINT(KERN_INFO,"%s, pVidEntry[%d], does not exist \n\r",__FUNCTION__, *pVidEntry);                 
-    }
-    else {
-        
-        pPbitsMap = &gs_pbits_map_table[dir][*pVidEntry];
-
-        if (pPbitsMap->in_use != 0) {
-
-            pPktFrwd  = &pPbitsMap->pkt_fwd[pbitsIndex];
-            if (pPktFrwd->in_use != 0) {
-                cust_flow->pkt_frwd.trg_port      = pPktFrwd->trg_port;               
-                if (false == mv_cust_get_tcont_state(pPktFrwd->trg_port))
-                    cust_flow->pkt_frwd.trg_queue = CPH_INVALID_TRGT_QUEUE;
-                 else
-                    cust_flow->pkt_frwd.trg_queue = pPktFrwd->trg_queue;
-                cust_flow->pkt_frwd.trg_hwf_queue = pPktFrwd->trg_hwf_queue;
-                cust_flow->pkt_frwd.gem_port      = pPktFrwd->gem_port;
-                cust_flow->mod_vid                = pPbitsMap->mod_vid[pbitsIndex];
-                cust_flow->mod_pbits              = pPbitsMap->mod_pbits[pbitsIndex];
-                cust_flow->pkt_frwd.in_use        = 1;
-                
-                if (gs_mv_cust_trace_flag) {
-                    MVCUST_TRACE_PRINT(KERN_INFO,
-                                       "trg_port(%d), trg_queue(%d), trg_hwf_queue(%d), gem_port(%d), mod_vid(%d), mod_pbits(%d)\n\r",
-                                        cust_flow->pkt_frwd.trg_port, cust_flow->pkt_frwd.trg_queue, cust_flow->pkt_frwd.trg_hwf_queue,
-                                        cust_flow->pkt_frwd.gem_port, cust_flow->mod_vid, cust_flow->mod_pbits);
-                    MVCUST_TRACE_PRINT(KERN_INFO,
-                                       "==EXIT==:\n\r");                                            
-                }
-                
-                return MV_CUST_OK;
-            }             
-        }          
-    }
-
-    if (gs_mv_cust_trace_flag) {
-        MVCUST_TRACE_PRINT(KERN_INFO,
-                           "==EXIT==:\n\r");
-    }
-
-    return MV_CUST_FAIL;
-}
-
-/*******************************************************************************
-**
-**    mv_cust_map_table_print
-**    ___________________________________________________________________________
-**
-**    DESCRIPTION: The function displays valid GPON flow mapping tables and DSCP 
-**                 to P-bits mapping tablefor untagged frames.
-**
-**    INPUTS:
-**      None.
-**
-**    OUTPUTS:
-**      None.
-**
-**    RETURNS:     
-**      On success, the function returns (MV_CUST_OK). On error different types are 
-**    returned according to the case.  
-**
-*******************************************************************************/
-int mv_cust_map_table_print(void)
-{
-    uint32_t  index = 0;
-    uint32_t  table_index = 0;
-
-    //MVCUST_TRACE_PRINT(KERN_INFO,"==ENTER==\n\r");
-
-    /* Print Valid VID index entries */
-    printk(KERN_INFO "In Upstream Direction      \n----------------------------------\n"); 
-    printk(KERN_INFO "MV_CUST VLAN ID Index Table\n----------------------------------\n");         
-    printk(KERN_INFO "VID   P-bits_table_index\n");
-    for (index=0; index<MV_CUST_VID_INDEX_TABLE_MAX_SIZE; index++) {
-        if(gs_vid_index_table[MV_CUST_FLOW_DIR_US].pbits_map_index[index] < MV_CUST_MAX_PBITS_MAP_TABLE_SIZE)
-            printk(KERN_INFO "%4.4d   %d\n", index, gs_vid_index_table[MV_CUST_FLOW_DIR_US].pbits_map_index[index]);    
-    }
-    printk(KERN_INFO "\n\n");
-
-    /* Print P-bits mapping tables */
-    printk(KERN_INFO "MV_CUST P-bits Flow Mapping Tables\n----------------------------------\n\n");  
-    for (table_index=0; table_index<MV_CUST_MAX_PBITS_MAP_TABLE_SIZE; table_index++) {
-        if(gs_pbits_map_table[MV_CUST_FLOW_DIR_US][table_index].in_use != 0) {
-                printk(KERN_INFO "P-bits Flow Mapping Table %d\n----------------------------\n", table_index); 
-                printk(KERN_INFO "P-bits  in_use  mod_vid mod_pbits trg_port  trg_queue  trg_hwf_queue  gem_port\n"); 
-                for (index=0; index<MV_CUST_PBITS_MAP_MAX_ENTRY_NUM; index++) 
-                    printk(KERN_INFO "%1.1d       %3.3s     %4.4d    %1.1d          %2.2d        %2.2d         %2.2d            %4.4d\n", 
-                                       index,
-                                       (gs_pbits_map_table[MV_CUST_FLOW_DIR_US][table_index].pkt_fwd[index].in_use!=0)? "YES":"",
-                                       gs_pbits_map_table[MV_CUST_FLOW_DIR_US][table_index].mod_vid[index],
-                                       gs_pbits_map_table[MV_CUST_FLOW_DIR_US][table_index].mod_pbits[index],
-                                       gs_pbits_map_table[MV_CUST_FLOW_DIR_US][table_index].pkt_fwd[index].trg_port, 
-                                       gs_pbits_map_table[MV_CUST_FLOW_DIR_US][table_index].pkt_fwd[index].trg_queue,
-                                       gs_pbits_map_table[MV_CUST_FLOW_DIR_US][table_index].pkt_fwd[index].trg_hwf_queue,
-                                       gs_pbits_map_table[MV_CUST_FLOW_DIR_US][table_index].pkt_fwd[index].gem_port);    
-        
-        }
-    }
-    printk(KERN_INFO "\n\n");
-
-    printk(KERN_INFO "In Downstream Direction    \n----------------------------------\n"); 
-    printk(KERN_INFO "MV_CUST VLAN ID Index Table\n----------------------------------\n");         
-    printk(KERN_INFO "VID   P-bits_table_index\n");
-    for (index=0; index<MV_CUST_VID_INDEX_TABLE_MAX_SIZE; index++) {
-        if(gs_vid_index_table[MV_CUST_FLOW_DIR_DS].pbits_map_index[index] < MV_CUST_MAX_PBITS_MAP_TABLE_SIZE)
-            printk(KERN_INFO "%4.4d   %d\n", index, gs_vid_index_table[MV_CUST_FLOW_DIR_DS].pbits_map_index[index]);    
-    }
-    printk(KERN_INFO "\n\n");
-
-    /* Print P-bits mapping tables */
-    printk(KERN_INFO "MV_CUST P-bits Flow Mapping Tables\n----------------------------------\n\n");  
-    for (table_index=0; table_index<MV_CUST_MAX_PBITS_MAP_TABLE_SIZE; table_index++) {
-        if(gs_pbits_map_table[MV_CUST_FLOW_DIR_DS][table_index].in_use != 0) {
-                printk(KERN_INFO "P-bits Flow Mapping Table %d\n----------------------------\n", table_index); 
-                printk(KERN_INFO "P-bits  in_use  mod_vid mod_pbits trg_queue  trg_hwf_queue  \n"); 
-                for (index=0; index<MV_CUST_PBITS_MAP_MAX_ENTRY_NUM; index++) 
-                    printk(KERN_INFO "%1.1d       %3.3s     %4.4d    %1.1d        %2.2d         %2.2d              \n", 
-                                       index,
-                                       (gs_pbits_map_table[MV_CUST_FLOW_DIR_DS][table_index].pkt_fwd[index].in_use!=0)? "YES":"",
-                                       gs_pbits_map_table[MV_CUST_FLOW_DIR_DS][table_index].mod_vid[index],
-                                       gs_pbits_map_table[MV_CUST_FLOW_DIR_DS][table_index].mod_pbits[index],
-                                       gs_pbits_map_table[MV_CUST_FLOW_DIR_DS][table_index].pkt_fwd[index].trg_queue,
-                                       gs_pbits_map_table[MV_CUST_FLOW_DIR_DS][table_index].pkt_fwd[index].trg_hwf_queue);    
-        
-        }
-    }
-    printk(KERN_INFO "\n\n");    
-
-    /* Print DSCP to P-bits mapping table */
-    printk(KERN_INFO "MV_CUST DSCP to P-bits Mapping Table\n----------------------------------\n");  
-    if (gs_dscp_map_table.in_use == 0) {
-        printk(KERN_INFO "No DSCP to P-bits mapping\n");
-    }
-    else {
-        
-        printk(KERN_INFO "DSCP  Pbits\n"); 
-        for (index=0; index<MV_CUST_PBITS_MAP_MAX_ENTRY_NUM; index++)
-            printk(KERN_INFO "%2.2d     %1.1d\n", 
-                   index, gs_dscp_map_table.pbits[index]);    
-    }
-    printk(KERN_INFO "\n\n");
-
-    //MVCUST_TRACE_PRINT(KERN_INFO,"==EXIT==\n\r");    
-    return MV_CUST_OK; 
-}
-
-/*******************************************************************************
-**
-**    mv_cust_flow_map_init
-**    ___________________________________________________________________________
-**
-**    DESCRIPTION: The function initializes mv_cust flow mapping data structure.
-**
-**    INPUTS:
-**      None.
-**
-**    OUTPUTS:
-**      None. 
-**
-**    RETURNS:     
-**      On success, the function returns (MV_CUST_OK). On error different types are 
-**    returned according to the case.  
-**
-*******************************************************************************/    
-int mv_cust_flow_map_init(void)
-{
-    uint32_t index = 0;
-    
-    /* Initializes VID index table              */
-    memset((uint8_t *)&gs_vid_index_table, MV_CUST_INVALID_PBITS_TABLE_INDEX, sizeof(gs_vid_index_table));
-
-    /* Initializes P-bits mapping tables        */
-    for (index = 0; index < MV_CUST_MAX_PBITS_MAP_TABLE_SIZE; index++) {
-        memset((uint8_t *)&gs_pbits_map_table[MV_CUST_FLOW_DIR_US][index], 0, sizeof(mv_cust_pbits_map_t));
-        gs_pbits_map_table[MV_CUST_FLOW_DIR_US][index].in_use = 0;
-        memset((uint8_t *)&gs_pbits_map_table[MV_CUST_FLOW_DIR_DS][index], 0, sizeof(mv_cust_pbits_map_t));
-        gs_pbits_map_table[MV_CUST_FLOW_DIR_DS][index].in_use = 0;        
-    }
-
-    /* Initializes DSCP to P-bits mapping table */
-    memset((uint8_t *)&gs_dscp_map_table, 0, sizeof(mv_cust_dscp_pbits_t));
-    gs_dscp_map_table.in_use = 0;
-
-    /* Initializes T-CONT state, default value is false */
-    for (index = 0; index < CPH_MAX_TCONT_NUM; index++)
-        gs_tcont_state[index] = false;
-        
-    return MV_CUST_OK;
-}
-
-/*******************************************************************************
-**
-** mv_cust_get_tcont_state
-** ___________________________________________________________________________
-**
-** DESCRIPTION: The function get T-CONT state
-**
-** INPUTS:
-**   tcont - T-CONT
-**
-** OUTPUTS:
-**   None. 
-**
-** RETURNS:     
-**   state - State of T-CONT, enabled or disabled.  
-**
-*******************************************************************************/
-bool mv_cust_get_tcont_state(uint32_t tcont)
-{
-    /* Check tcont */
-    if (tcont >= CPH_MAX_TCONT_NUM)
-    {
-        MVCUST_TRACE_PRINT(KERN_ERR,"tcont[%d] is illegal, should be less than [%d]\n", tcont, CPH_MAX_TCONT_NUM); 
-        return false;
-    }
-
-    return gs_tcont_state[tcont];
-}
-
-/*******************************************************************************
-**
-** mv_cust_set_tcont_state
-** ___________________________________________________________________________
-**
-** DESCRIPTION: The function sets T-CONT state in mv_cust
-**
-** INPUTS:
-**   tcont - T-CONT
-**   state - State of T-CONT, enabled or disabled.
-**
-** OUTPUTS:
-**   None. 
-**
-** RETURNS:     
-**  On success, the function returns (MV_OK). On error different types are 
-**  returned according to the case.  
-**
-*******************************************************************************/
-MV_STATUS mv_cust_set_tcont_state(uint32_t tcont, bool state)
-{
-    /* Check tcont */
-    if (tcont >= CPH_MAX_TCONT_NUM)
-    {
-        MVCUST_TRACE_PRINT(KERN_ERR,"tcont[%d] is illegal, should be less than [%d]\n", tcont, CPH_MAX_TCONT_NUM); 
-        return MV_FAIL;
-    }
-
-    /* Apply t-cont state to mv_cust */
-    gs_tcont_state[tcont] = state;
-
-    return MV_OK;
-}
-EXPORT_SYMBOL(mv_cust_set_tcont_state);
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_flow_map.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_flow_map.h
deleted file mode 100755
index 787542a..0000000
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_flow_map.h
+++ /dev/null
@@ -1,359 +0,0 @@
-/************************************************************************
-* 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_flow_map.h
-*
-* DESCRIPTION:
-*   Victor  - initial version created.   12/Dec/2011 
-*
-*******************************************************************************/
-#ifndef __mv_cust_flow_map_h__
-#define __mv_cust_flow_map_h__
-
-
-/****************************************************************************** 
- *                        Data Enum and Structure                                                                                                                                  
- ******************************************************************************/
-#define MVCUST_TRACE_PRINT(level, format, ...)   printk(level "%s(%d):"format, __FUNCTION__, __LINE__, ##__VA_ARGS__)
-
-#define MV_CUST_OK                        (0)
-#define MV_CUST_FAIL                      (1)
-
-#define CPH_MAX_TCONT_NUM                 (8)    /* Maximum T-CONT number    */
-#define CPH_INVALID_TRGT_QUEUE            (0xFF) /* Invalid target queue number  */
-
-#define MV_CUST_VID_NOT_CARE_VALUE        (4096) /* Does not care for VID    */
-#define MV_CUST_PBITS_NOT_CARE_VALUE      (8)    /* Does not care for P-bits */
-#define MV_CUST_DSCP_NOT_CARE_VALUE       (64)   /* Does not care for DSCP   */
-
-#define MV_CUST_MAX_TRG_PORT_VALUE        (8-1)  /* Maximum target port value, T-CONT7 */
-#define MV_CUST_MAX_TRG_QUEUE_VALUE       (7)    /* Maximum target queue value         */
-#define MV_CUST_MAX_GEM_PORT_VALUE        (4095) /* Maximum GEM port value             */
-
-#define MV_CUST_INVALID_PBITS_TABLE_INDEX (0xff) /* Invalid Pbits table index value in VID index table*/
-
-#define MV_CUST_DEFAULT_UNTAG_RULE        (4096+1)/* Default untagged  rule        */
-#define MV_CUST_DEFAULT_SINGLE_TAG_RULE   (4096+2)/* Default sinlge tagged  rule   */
-#define MV_CUST_DEFAULT_DOUBLE_UNTAG_RULE (4096+3)/* Default double tagged  rule   */
-
-/* VLAN ID index table definition for flow mapping */
-#define MV_CUST_VID_INDEX_TABLE_MAX_SIZE  (4096+4)
-typedef struct {
-    uint8_t pbits_map_index[MV_CUST_VID_INDEX_TABLE_MAX_SIZE];
-} mv_cust_vid_index_t;
-
-/* P-bits flow mapping table definition */
-typedef uint32_t mv_cust_trg_port_type_t;
-typedef uint32_t mv_cust_gem_port_key_t;
-typedef struct mv_cust_pkt_frwd {
-    uint32_t                in_use;
-    mv_cust_trg_port_type_t trg_port;                                         
-    uint32_t                trg_queue;                      
-    uint32_t                trg_hwf_queue;    
-    mv_cust_gem_port_key_t  gem_port;
-} mv_cust_pkt_frwd_t;
-
-#define MV_CUST_PBITS_MAP_MAX_ENTRY_NUM   (8+1)
-#define MV_CUST_MAX_PBITS_MAP_TABLE_SIZE  (64)
-
-typedef struct {
-    uint32_t           in_use;
-    int                mod_vid[MV_CUST_PBITS_MAP_MAX_ENTRY_NUM];
-    int                mod_pbits[MV_CUST_PBITS_MAP_MAX_ENTRY_NUM];    
-    mv_cust_pkt_frwd_t pkt_fwd[MV_CUST_PBITS_MAP_MAX_ENTRY_NUM];
-} mv_cust_pbits_map_t;
-
-/* DSCP to P-bits mapping table definition */
-#define MV_CUST_DSCP_PBITS_TABLE_MAX_SIZE  (64)
-typedef struct {
-    uint32_t in_use;
-    uint8_t  pbits[MV_CUST_DSCP_PBITS_TABLE_MAX_SIZE];
-} mv_cust_dscp_pbits_t;
- 
-typedef enum
-{
-    MV_CUST_FLOW_DIR_US   = 0,
-    MV_CUST_FLOW_DIR_DS   = 1,
-    MV_CUST_FLOW_DIR_NUM  = 2
-} mv_cust_flow_dir_e;
-
-typedef struct
-{
-    mv_cust_flow_dir_e dir;
-    int                vid;
-    int                pbits;
-    int                dscp;  
-    int                mod_vid;
-    int                mod_pbits;
-    mv_cust_pkt_frwd_t pkt_frwd;
-} mv_cust_ioctl_flow_map_t;
-
-/****************************************************************************** 
- *                        Function Declaration                                                                                                                                  
- ******************************************************************************/
-/*******************************************************************************
-**
-**    mv_cust_set_trace_flag
-**    ___________________________________________________________________________
-**
-**    DESCRIPTION: The function sets mv_cust trace flag.
-**
-**    INPUTS:
-**      enTrace     - Enable or disable mv_cust trace.
-**
-**    OUTPUTS:
-**      None. 
-**
-**    RETURNS:     
-**      On success, the function returns (MV_CUST_OK). On error different types are 
-**    returned according to the case.  
-**
-*******************************************************************************/    
-int mv_cust_set_trace_flag(uint32_t enTrace);
-
-/*******************************************************************************
-**
-**    mv_cust_map_rule_set
-**    ___________________________________________________________________________
-**
-**    DESCRIPTION: The function sets GPON flow mapping rules
-**
-**    INPUTS:
-**      cust_flow  - VLAN ID, 802.1p value, pkt_fwd information.
-**
-**    OUTPUTS:
-**      None. 
-**
-**    RETURNS:     
-**      On success, the function returns (MV_CUST_OK). On error different types are 
-**    returned according to the case.  
-**
-*******************************************************************************/
-int mv_cust_map_rule_set(mv_cust_ioctl_flow_map_t *cust_flow);
-
-/*******************************************************************************
-**
-**    mv_cust_dscp_map_set
-**    ___________________________________________________________________________
-**
-**    DESCRIPTION: The function sets GPON DSCP to P-bits mapping rules
-**
-**  INPUTS:
-**    dscp_map   - DSCP to P-bits mapping rules.
-**
-**  OUTPUTS:
-**    None. 
-**
-**    RETURNS:     
-**    On success, the function returns (0). On error different types are 
-**  returned according to the case.  
-**
-*******************************************************************************/
-int mv_cust_dscp_map_set(mv_cust_dscp_pbits_t *dscp_map);
-
-/*******************************************************************************
-**
-**    mv_cust_map_rule_del
-**    ___________________________________________________________________________
-**
-**    DESCRIPTION: The function deletes GPON flow mapping rules
-**
-**    INPUTS:
-**      vid         - VLAN ID.
-**      pbits      - 802.1p value.
-**
-**    OUTPUTS:
-**      None 
-**
-**    RETURNS:     
-**      On success, the function returns (MV_CUST_OK). On error different types are 
-**    returned according to the case.  
-**
-*******************************************************************************/
-int mv_cust_map_rule_del(uint16_t vid, uint8_t pbits, mv_cust_flow_dir_e dir);
-
-/*******************************************************************************
-**
-**    mv_cust_dscp_map_del
-**    ___________________________________________________________________________
-**
-**    DESCRIPTION: The function deletes DSCP to P-bits mapping rules
-**
-**  INPUTS:
-**    None.
-**
-**  OUTPUTS:
-**    None. 
-**
-**    RETURNS:     
-**    On success, the function returns (0). On error different types are 
-**  returned according to the case.  
-**
-*******************************************************************************/
-int mv_cust_dscp_map_del(void);
-
-/*******************************************************************************
-**
-**    mv_cust_map_rule_clear
-**    ___________________________________________________________________________
-**
-**    DESCRIPTION: The function clears all GPON flow mapping rules
-**
-**  INPUTS:
-**    None.
-**
-**  OUTPUTS:
-**    None. 
-**
-**    RETURNS:     
-**    On success, the function returns (0). On error different types are 
-**  returned according to the case.  
-**
-*******************************************************************************/
-int mv_cust_map_rule_clear(void);
-
-/*******************************************************************************
-**
-**    mv_cust_tag_map_rule_get
-**    ___________________________________________________________________________
-**
-**    DESCRIPTION: The function gets GPON flow mapping rule for tagged frames.
-**
-**  INPUTS:
-**    cust_flow  - parsing vid, pbits, dir
-**
-**  OUTPUTS:
-**    cust_flow  - out packet forwarding information, including GEM port, T-CONT, queue.
-**                 and packet modification for VID, P-bits
-**
-**    RETURNS:     
-**    On success, the function returns (0). On error different types are 
-**  returned according to the case.  
-**
-*******************************************************************************/
-int mv_cust_tag_map_rule_get(mv_cust_ioctl_flow_map_t *cust_flow);
-
-/*******************************************************************************
-**
-**    mv_cust_untag_map_rule_get
-**    ___________________________________________________________________________
-**
-**    DESCRIPTION: The function gets GPON flow mapping rule for untagged frames.
-**
-**    INPUTS:
-**      dscp         - DSCP value.
-**
-**    OUTPUTS:
-**      cust_flow    - packet forwarding information, including GEM port, T-CONT, queue.
-**
-**    RETURNS:     
-**      On success, the function returns (MV_CUST_OK). On error different types are 
-**    returned according to the case.  
-**
-*******************************************************************************/
-int mv_cust_untag_map_rule_get(mv_cust_ioctl_flow_map_t *cust_flow);
-
-/*******************************************************************************
-**
-**    mv_cust_map_table_print
-**    ___________________________________________________________________________
-**
-**    DESCRIPTION: The function displays valid GPON flow mapping tables and DSCP 
-**               to P-bits mapping tablefor untagged frames.
-**
-**  INPUTS:
-**    None.
-**
-**  OUTPUTS:
-**    None.
-**
-**    RETURNS:     
-**    On success, the function returns (0). On error different types are 
-**  returned according to the case.  
-**
-*******************************************************************************/
-int mv_cust_map_table_print(void);
-
-/*******************************************************************************
-**
-**    mv_cust_flow_map_init
-**    ___________________________________________________________________________
-**
-**    DESCRIPTION: The function initializes mv_cust flow mapping data structure.
-**
-**    INPUTS:
-**      None.
-**
-**    OUTPUTS:
-**      None. 
-**
-**    RETURNS:     
-**      On success, the function returns (0). On error different types are 
-**    returned according to the case.  
-**
-*******************************************************************************/    
-int mv_cust_flow_map_init(void);
-
-/*******************************************************************************
-**
-** mv_cust_get_tcont_state
-** ___________________________________________________________________________
-**
-** DESCRIPTION: The function get T-CONT state
-**
-** INPUTS:
-**   tcont - T-CONT
-**
-** OUTPUTS:
-**   None. 
-**
-** RETURNS:     
-**   state - State of T-CONT, enabled or disabled.  
-**
-*******************************************************************************/
-bool mv_cust_get_tcont_state(uint32_t tcont);
-
-/*******************************************************************************
-**
-** mv_cust_set_tcont_state
-** ___________________________________________________________________________
-**
-** DESCRIPTION: The function sets T-CONT state in mv_cust
-**
-** INPUTS:
-**   tcont - T-CONT
-**   state - State of T-CONT, enabled or disabled.
-**
-** OUTPUTS:
-**   None. 
-**
-** RETURNS:     
-**  On success, the function returns (MV_OK). On error different types are 
-**  returned according to the case.  
-**
-*******************************************************************************/
-MV_STATUS mv_cust_set_tcont_state(uint32_t tcont, bool state);
-
-#endif /* __mv_cust_flow_map_h__ */
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_mng_if.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_mng_if.h
deleted file mode 100755
index 470a81c..0000000
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_mng_if.h
+++ /dev/null
@@ -1,111 +0,0 @@
-/************************************************************************
-* 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_mng_if.h                                           
-*                                                                           
-* DESCRIPTION : This file contains ONU MV CUST Management Interface        
-********************************************************************************/
-#ifndef _MV_CUST_MNG_IF_H_
-#define _MV_CUST_MNG_IF_H_
-
-/* Include Files
-------------------------------------------------------------------------------*/
-#include <linux/cdev.h>
-
-/* Definitions
-------------------------------------------------------------------------------*/
-#define MV_CUST_IOCTL_OMCI_SET                _IOW(MV_CUST_IOCTL_MAGIC,  1,  unsigned int)
-#define MV_CUST_IOCTL_EOAM_LLID_SET           _IOW(MV_CUST_IOCTL_MAGIC,  2,  unsigned int)
-#define MV_CUST_IOCTL_EOAM_ENABLE             _IOW(MV_CUST_IOCTL_MAGIC,  3,  unsigned int)
-#define MV_CUST_IOCTL_OMCI_ENABLE             _IOW(MV_CUST_IOCTL_MAGIC,  4,  unsigned int)
-#define MV_CUST_IOCTL_MAP_RULE_SET            _IOW(MV_CUST_IOCTL_MAGIC,  5,  unsigned int)
-#define MV_CUST_IOCTL_DSCP_MAP_SET            _IOW(MV_CUST_IOCTL_MAGIC,  6,  unsigned int)
-#define MV_CUST_IOCTL_MAP_RULE_DEL            _IOW(MV_CUST_IOCTL_MAGIC,  7,  unsigned int)
-#define MV_CUST_IOCTL_DSCP_MAP_DEL            _IOW(MV_CUST_IOCTL_MAGIC,  8,  unsigned int)
-#define MV_CUST_IOCTL_MAP_RULE_CLEAR          _IOW(MV_CUST_IOCTL_MAGIC,  9,  unsigned int)
-#define MV_CUST_IOCTL_TAG_MAP_RULE_GET        _IOR(MV_CUST_IOCTL_MAGIC,  10, unsigned int)
-#define MV_CUST_IOCTL_UNTAG_MAP_RULE_GET      _IOR(MV_CUST_IOCTL_MAGIC,  11, unsigned int)
-#define MV_CUST_IOCTL_APP_ETH_TYPE_SET        _IOW(MV_CUST_IOCTL_MAGIC,  12, unsigned int)
-
-/* Enums
-------------------------------------------------------------------------------*/
-
-/* Typedefs
-------------------------------------------------------------------------------*/
-typedef struct
-{
-    int                tcont;
-    int                txq;
-    int                gemport;
-    int                keep_rx_mh;
-} mv_cust_ioctl_omci_set_t;
-
-typedef struct
-{
-    int                llid;
-    int                txq;
-    uint8_t            llid_mac[6];
-} mv_cust_ioctl_llid_set_t;
-
-typedef struct
-{
-    mv_cust_dscp_pbits_t dscp_map;
-} mv_cust_ioctl_dscp_map_t;
-
-typedef struct
-{
-    mv_cust_app_type_e app_type;
-    uint16_t           eth_type;
-} mv_cust_ioctl_app_etype_t;
-
-/* MV_CUST Char Device Structure */
-/* ========================= */
-typedef struct
-{
-  mv_cust_ioctl_omci_set_t      mv_cust_ioctl_omci_set;
-  mv_cust_ioctl_llid_set_t      mv_cust_ioctl_llid_set;
-  mv_cust_ioctl_flow_map_t      mv_cust_ioctl_flow_map;
-  mv_cust_ioctl_dscp_map_t      mv_cust_ioctl_dscp_map;
-  mv_cust_ioctl_app_etype_t     mv_cust_ioctl_app_etype;
-
-  struct cdev  cdev;
-} mv_cust_cdev_t;
-
-/* Global variables
-------------------------------------------------------------------------------*/
-
-/* Global functions
-------------------------------------------------------------------------------*/
-
-/* Global variables
-------------------------------------------------------------------------------*/
-
-/* Global functions
-------------------------------------------------------------------------------*/
-
-/* Macros
-------------------------------------------------------------------------------*/
-
-#endif /* _MV_CUST_MNG_IF_H_ */
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_mod.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_mod.c
deleted file mode 100755
index 4ce9218..0000000
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_mod.c
+++ /dev/null
@@ -1,63 +0,0 @@
-/************************************************************************
-* 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_mod.c
-*
-* DESCRIPTION:
-*       
-* 
-*******************************************************************************/
-#include <linux/module.h>
-#include <linux/moduleparam.h>
-#include "mv_cust_dev.h"
-
-
-#ifdef MODULE_LICENSE
-MODULE_LICENSE("GPL");
-#endif
-
-static void __exit mv_cust_mod_exit(void)
-{
-    mvcust_dev_shutdown();
-}
-module_exit(mv_cust_mod_exit);
-
-
-static int __init mv_cust_mod_init(void)
-{
-    if (mvcust_dev_init() != 0)
-    {
-        //mv_cust_mod_exit();
-        printk(KERN_ERR "\nMV_CUST module initialization failed \n\n");
-        return -1;
-    }
-
-    printk(KERN_INFO "\nMV_CUST module inserted - %s\n\n",MV_CUST_VERSION);
-
-    return 0;
-}
-
-//module_init(mv_cust_mod_init);
-device_initcall_sync(mv_cust_mod_init);
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_netdev.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_netdev.c
deleted file mode 100755
index c57b076..0000000
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_netdev.c
+++ /dev/null
@@ -1,1441 +0,0 @@
-/************************************************************************
-* 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;
-}
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_netdev.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_netdev.h
deleted file mode 100755
index 4079631..0000000
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_netdev.h
+++ /dev/null
@@ -1,112 +0,0 @@
-/************************************************************************
-* 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.h
-*
-* DESCRIPTION:
-*
-*
-*******************************************************************************/
-#ifndef __mv_cust_netdev_h__
-#define __mv_cust_netdev_h__
-
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-#include <linux/skbuff.h>
-#include <net/ip.h>
-
-#include <mvCommon.h>
-#include <mvOs.h>
-#include <mv_neta/net_dev/mv_netdev.h>
-
-
-#define MV_CUST_MAS_UDP_SRC_PORT          8
-#define MV_CUST_MAS_UDP_DST_PORT          8
-#define MV_CUST_NUM_LLID                  8
-
-
-struct mv_udp_port_tx_spec {
-    __be16    udp_port;
-    struct mv_eth_tx_spec tx_spec;
-};
-
-struct mv_port_tx_spec {
-    struct mv_udp_port_tx_spec udp_src[MV_CUST_MAS_UDP_SRC_PORT];
-    struct mv_udp_port_tx_spec udp_dst[MV_CUST_MAS_UDP_DST_PORT];
-};
-
-
-struct mv_eoam_llid_spec {
-    uint8_t llid_mac_address[6];
-    struct  mv_eth_tx_spec tx_spec;
-};
-
-typedef enum
-{
-    MV_CUST_APP_TYPE_IGMP = 0, /* For IGMP application               */
-    MV_CUST_APP_TYPE_MLD,      /* For MLD application                */
-    MV_CUST_APP_TYPE_LPBK,     /* For loopback detection application */
-    MV_CUST_APP_TYPE_OAM,      /* For eOAM application               */
-    MV_CUST_APP_TYPE_OMCI,     /* For OMCI application               */
-    MV_CUST_APP_TYPE_MAX       /* Max number of application          */ 
-} mv_cust_app_type_e;
-
-/*  Enum: enable or disable application*/
-typedef enum
-{
-    MV_CUST_APP_DISABLE  = 0,
-    MV_CUST_APP_ENABLE   = 1,
-} mv_cust_app_flag_e;
-
-typedef struct
-{
-    mv_cust_app_type_e app_type; /* Application type, such as IGMP, MLD                */
-    uint16_t           enable;   /* Flag indicates whether to enable application Rx/Tx */         
-    uint16_t           eth_type; /* Application Ethernet type used for socket/skb      */
-    char              *name;     /* The readable name of the application               */
-} mv_cust_app_config_t;
-
-
-/******************************************************
- * Function prototypes --                             *
-*******************************************************/
-void        mv_cust_omci_hw_cmd_set(unsigned int hw_cmd);
-int         mv_cust_omci_tx_set(int tcont, int txq);
-
-#ifdef  CONFIG_MV_CUST_UDP_SAMPLE_HANDLE
-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);
-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);
-#endif
-
-void        mv_cust_oam_rx_gh_set(int val);
-int         mv_cust_omci_set(int tcont, int txq, int gemport, int keep_rx_mh);
-int         mv_cust_eoam_llid_set(int llid, uint8_t *llid_mac, int txq);
-
-void        mv_cust_app_flag_set(mv_cust_app_type_e app_type, uint16_t enable);
-void        mv_cust_app_etype_set(mv_cust_app_type_e app_type, uint16_t eth_type);
-void        mv_cust_print(int type);
-
-
-#endif /* __mv_cust_netdev_h__ */
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_sysfs.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_sysfs.c
deleted file mode 100755
index 3c50c88..0000000
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_cust/mv_cust_sysfs.c
+++ /dev/null
@@ -1,525 +0,0 @@
-/************************************************************************
-* 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_sysfs.c
-*
-* DESCRIPTION:
-*
-*
-*******************************************************************************/
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/types.h>
-#include <linux/capability.h>
-#include <linux/kernel.h>
-#include <linux/platform_device.h>
-#include <linux/netdevice.h>
-
-#include <gbe/mvNeta.h>
-#include <pnc/mvPnc.h>
-
-#include "mv_cust_dev.h"
-#include "mv_cust_netdev.h"
-#include "mv_cust_flow_map.h"
-#include "mv_cust_mng_if.h"
-
-static int      eoam_txq = 0;
-static uint8_t  eoam_mac[6] = {0};
-
-
-static ssize_t mv_cust_spec_proc_help(char *buf)
-{
-    int off = 0;
-    off += sprintf(buf+off, "cat  help                              - show this help\n");
-#ifdef CONFIG_MV_CUST_UDP_SAMPLE_HANDLE    
-    off += sprintf(buf+off, "cat  udp_ports                         - show special udp source and dest. port configuration\n");
-#endif
-#ifdef CONFIG_MV_CUST_FLOW_MAP_HANDLE
-    off += sprintf(buf+off, "cat  flow_map                          - show flow mapping configuration\n");        
-    off += sprintf(buf+off, "cat  flow_map_show                     - show flow mapping rules \n");    
-    off += sprintf(buf+off, "cat  flow_map_clear                    - clear all flow mapping rules\n");
-    off += sprintf(buf+off, "cat  dscp_map_del                      - delete DSCP to P-bits mapping rules\n");    
-#endif
-    off += sprintf(buf+off, "cat  tcont_show                        - Show all T-CONT state\n");
-    off += sprintf(buf+off, "echo tcont state        > tcont_set    - Set T-CONT tatus, tcont(0~7), state(1:enable, 0:disable) \n");
-    off += sprintf(buf+off, "echo hex                > debug        - Set Customized module debug information \n");
-    off += sprintf(buf+off, "echo app_type           > app_show     - Show application configuration. 0:igmp, 1:mld, 2:lpbk, 3:eoam, 4:omci\n");
-    off += sprintf(buf+off, "echo app_type enable    > app_flag_set - Enable/disable application. 0:igmp, 1:mld, 2:lpbk, 3:eoam, 4:omci, 1:enable, 0:disable\n");
-    off += sprintf(buf+off, "echo app_type hex       > app_etype_set- Set application Eth type. 0:igmp, 1:mld, 2:lpbk, 3:eoam, 4:omci\n");
-    off += sprintf(buf+off, "echo tc txq gem keep_mh > omci_set     - set udp source port special Tx behavior\n");
-    off += sprintf(buf+off, "echo keep_mh            > eoam_gh_keep - Keep or not keep GH for eOAM packets, 1:keep, 0:do not keep\n");    
-    off += sprintf(buf+off, "echo txq                > eoam_txq     - set TX queue into sw_buffer for EOAM llid command\n");
-    off += sprintf(buf+off, "echo mac[0]-mac[5]      > eoam_mac     - set LLID mac into sw_buffer for EOAM llid command\n");
-    off += sprintf(buf+off, "echo llid               > eoam_write   - Write EOAM txq and mac address from sw_buffer into LLID\n");
-#ifdef CONFIG_MV_CUST_UDP_SAMPLE_HANDLE    
-    off += sprintf(buf+off, "echo p udp_src(dec) txp txq flags hw_cmd  > udp_src - set udp source port special Tx behavior\n");
-    off += sprintf(buf+off, "echo p udp_dst(dec) txp txq flags hw_cmd  > udp_dst - set udp dest.  port special Tx behavior\n");
-#endif    
-#ifdef CONFIG_MV_CUST_FLOW_MAP_HANDLE
-    off += sprintf(buf+off, "echo hex                                  > flow_map_debug  - Set flow mapping debug flag, 1:enable, 0:disable \n");    
-    off += sprintf(buf+off, "echo vid pbits mod_vid mod_pbits trg_port trg_queue trg_hwf_queue gem_port  > flow_map_us_set - set U/S flow mapping rule\n");
-    off += sprintf(buf+off, "echo vid pbits mod_vid mod_pbits          > flow_map_ds_set - set D/S flow mapping rule\n");    
-    off += sprintf(buf+off, "echo pbits0 pbits1 ... pbits62 pbits63    > dscp_map_set    - set DSCP to P-bits mapping rules\n");    
-    off += sprintf(buf+off, "echo vlan pbits dir(0:U/S, 1:D/S)         > flow_map_del    - delete flow mapping rule\n");
-    off += sprintf(buf+off, "echo vlan pbits dir(0:U/S, 1:D/S)         > tag_flow_get    - get tagged flow mapping rule\n");
-    off += sprintf(buf+off, "echo dscp dir(0:U/S, 1:D/S)               > untag_flow_get  - get untagged flow mapping rule\n");
-#endif        
-
-    return off;
-}
-
-
-static ssize_t mv_cust_spec_proc_show(struct device *dev,
-                                      struct device_attribute *attr, char *buf)
-{
-    int off   = 0;
-    int index = 0;
-    bool state;
-    const char* name = attr->attr.name;
-
-    if (!capable(CAP_NET_ADMIN))
-        return -EPERM;
-
-    if (!strcmp(name, "help") ) {
-        off = mv_cust_spec_proc_help(buf);
-    }
-#ifdef CONFIG_MV_CUST_UDP_SAMPLE_HANDLE       
-    else if (!strcmp(name, "udp_ports")) {
-        mv_cust_udp_spec_print_all();
-    }
-#endif    
-#ifdef CONFIG_MV_CUST_FLOW_MAP_HANDLE       
-    else if (!strcmp(name, "flow_map")) { 
-        mv_cust_flow_map_print();
-    }     
-    else if (!strcmp(name, "flow_map_show")) {
-        mv_cust_map_table_print();
-    } 
-    else if (!strcmp(name, "flow_map_clear")) {
-        mv_cust_map_rule_clear();
-    }  
-    else if (!strcmp(name, "dscp_map_del")) {
-        mv_cust_dscp_map_del();
-    }     
-#endif
-    else if (!strcmp(name, "tcont_show")) {
-        printk(KERN_INFO "MV_CUST T-CONT state table\n");
-        printk(KERN_INFO "Default invalid Txq:%d \n", CPH_INVALID_TRGT_QUEUE);
-        printk(KERN_INFO "--------------------------\n");
-        for (index = 0; index < CPH_MAX_TCONT_NUM; index++)
-        {
-            state = mv_cust_get_tcont_state(index);
-            printk(KERN_INFO "T-CONT%1.1d: %s\n", index, (state==true)?"enabled":"disabled");
-        }
-    } 
-    else
-        off = mv_cust_spec_proc_help(buf);
-
-    return off;
-}
-
-
-static ssize_t mv_cust_spec_proc_1_store(struct device *dev,
-                                         struct device_attribute *attr,
-                                         const char *buf, size_t len)
-{
-    const char*     name = attr->attr.name;
-    unsigned int    v;
-    unsigned long   flags;
-
-    if (!capable(CAP_NET_ADMIN))
-        return -EPERM;
-
-    /* Read input */
-    v = 0;
-
-    sscanf(buf, "%x", &v);
-    
-    raw_local_irq_save(flags);
-
-    if (!strcmp(name, "debug")) {
-        mv_cust_debug_info_set(v);
-    }
-    else if (!strcmp(name, "app_show")) {
-        mv_cust_print(v);
-    }
-    else if (!strcmp(name, "eoam_gh_keep")) {
-        mv_cust_oam_rx_gh_set(v);
-    }   
-    else if (!strcmp(name, "eoam_txq")) {
-        eoam_txq = v;
-    }
-    else if (!strcmp(name, "eoam_write")) {
-        mv_cust_eoam_llid_set((int)v, &eoam_mac[0], eoam_txq);
-    }
-    else
-        printk("%s: illegal operation <%s>\n", __FUNCTION__, attr->attr.name);
-
-    raw_local_irq_restore(flags);
-
-    return len;
-}
-
-
-static ssize_t mv_cust_spec_proc_2_store(struct device *dev,
-                                         struct device_attribute *attr,
-                                         const char *buf, size_t len)
-{
-    const char*     name = attr->attr.name;
-    unsigned int    p, v;
-    unsigned long   flags;
-
-    if (!capable(CAP_NET_ADMIN))
-        return -EPERM;
-
-    /* Read input */
-    p = 0;
-    v = 0;
-    sscanf(buf, "%d %x", &p, &v);
-
-    raw_local_irq_save(flags);
-
-    if (!strcmp(name, "app_flag_set")) {
-        mv_cust_app_flag_set(p, v);
-    }
-    else if (!strcmp(name, "app_etype_set")) {
-        mv_cust_app_etype_set(p, v);
-    }
-    else if (!strcmp(name, "tcont_set")) {
-        if (v)
-            mv_cust_set_tcont_state(p, true);
-        else
-            mv_cust_set_tcont_state(p, false);
-    }    
-    else
-        printk("%s: illegal operation <%s>\n", __FUNCTION__, attr->attr.name);
-
-    raw_local_irq_restore(flags);
-
-    return len;
-}
-
-
-static ssize_t mv_cust_spec_proc_6_store(struct device *dev,
-                                         struct device_attribute *attr,
-                                         const char *buf, size_t len)
-{
-    const char*     name = attr->attr.name;
-    unsigned int    p, v1=0, v2=0, v3=0, v4=0, v5=0;
-    unsigned long   flags;
-
-    if (!capable(CAP_NET_ADMIN))
-        return -EPERM;
-
-    /* Read input */
-    sscanf(buf, "%d %d %x %x %x %x", &p, &v1, &v2, &v3, &v4, &v5);
-
-    raw_local_irq_save(flags);
-
-    if (!strcmp(name, "udp_src")) {
-#ifdef CONFIG_MV_CUST_UDP_SAMPLE_HANDLE
-        mv_cust_udp_src_spec_set(p, v1, v2, v3, v4, v5);
-#else
-        printk("mv_cust module was not compiled with UDP SAMPLE Config Support\n");
-#endif
-    }
-    else if (!strcmp(name, "udp_dst")) {
-#ifdef CONFIG_MV_CUST_UDP_SAMPLE_HANDLE
-        mv_cust_udp_dest_spec_set(p, v1, v2, v3, v4, v5);
-#else
-        printk("mv_cust module was not compiled with UDP SAMPLE Config Support\n");
-#endif
-    }
-    else if (!strcmp(name, "omci_set")) {
-        mv_cust_omci_set(p, v1, v2, v3);
-    }
-    else
-        printk("%s: illegal operation <%s>\n", __FUNCTION__, attr->attr.name);
-
-    raw_local_irq_restore(flags);
-
-    return len;
-}
-
-static ssize_t mv_cust_spec_proc_flow_store(struct device *dev,
-                                            struct device_attribute *attr,
-                                            const char *buf, size_t len)
-{
-    const char*        name = attr->attr.name;
-    unsigned int       v1=0, v2=0, v3=0, v4=0, v5=0, v6=0, v7=0, v8=0;
-    unsigned long      flags;
-    mv_cust_ioctl_flow_map_t cust_flow;
-
-    if (!capable(CAP_NET_ADMIN))
-        return -EPERM;
-
-    /* Read input */
-    sscanf(buf, "%d %d %d %d %d %d %d %d", &v1, &v2, &v3, &v4, &v5, &v6, &v7, &v8);
-
-    raw_local_irq_save(flags);
-
-#ifdef CONFIG_MV_CUST_FLOW_MAP_HANDLE   
-
-    if (!strcmp(name, "flow_map_debug")) {
-        mv_cust_set_trace_flag((uint32_t)v1);
-    }
-    else if (!strcmp(name, "flow_map_us_set")) {
-        memset(&cust_flow, 0, sizeof(cust_flow));
-        cust_flow.dir       = MV_CUST_FLOW_DIR_US;
-        cust_flow.vid       = v1;
-        cust_flow.pbits     = v2;
-        cust_flow.mod_vid   = v3;
-        cust_flow.mod_pbits = v4;
-        
-        cust_flow.pkt_frwd.in_use        = 1;
-        cust_flow.pkt_frwd.trg_port      = (mv_cust_trg_port_type_t)v5;
-        cust_flow.pkt_frwd.trg_queue     = (uint32_t)v6;
-        cust_flow.pkt_frwd.trg_hwf_queue = (uint32_t)v7;        
-        cust_flow.pkt_frwd.gem_port      = (mv_cust_gem_port_key_t)v8;
-        
-        mv_cust_map_rule_set(&cust_flow);
-    }
-    else if (!strcmp(name, "flow_map_ds_set")) {
-        memset(&cust_flow, 0, sizeof(cust_flow));
-        cust_flow.dir       = MV_CUST_FLOW_DIR_DS;
-        cust_flow.vid       = v1;
-        cust_flow.pbits     = v2;
-        cust_flow.mod_vid   = v3;
-        cust_flow.mod_pbits = v4;
-        cust_flow.pkt_frwd.in_use    = 1;
-        
-        mv_cust_map_rule_set(&cust_flow);
-    }    
-    else if (!strcmp(name, "flow_map_del")) {
-        mv_cust_map_rule_del((uint16_t)v1, (uint8_t)v2, (mv_cust_flow_dir_e)v3);
-    }
-    else if (!strcmp(name, "tag_flow_get")) {
-        memset(&cust_flow, 0, sizeof(cust_flow));
-        cust_flow.vid   = v1;
-        cust_flow.pbits = v2;   
-        cust_flow.dir   = (mv_cust_flow_dir_e)v3;        
-        mv_cust_tag_map_rule_get(&cust_flow);
-        printk("in_use[%d], mod_vid[%d] mod_pbits[%d] trg_port[%d], trg_queue[%d], gem_port[%d]\r\n",
-                cust_flow.pkt_frwd.in_use, cust_flow.mod_vid, 
-                cust_flow.mod_pbits, cust_flow.pkt_frwd.trg_port, 
-                cust_flow.pkt_frwd.trg_queue, cust_flow.pkt_frwd.gem_port);
-    }    
-    else if (!strcmp(name, "untag_flow_get")) {
-        memset(&cust_flow, 0, sizeof(cust_flow));
-        cust_flow.dscp  =  v1;
-        cust_flow.dir   = (mv_cust_flow_dir_e)v2;        
-        mv_cust_untag_map_rule_get(&cust_flow);    
-        printk("in_use[%d], mod_vid[%d] mod_pbits[%d] trg_port[%d], trg_queue[%d], gem_port[%d]\r\n", 
-                cust_flow.pkt_frwd.in_use, cust_flow.mod_vid, 
-                cust_flow.mod_pbits, cust_flow.pkt_frwd.trg_port, 
-                cust_flow.pkt_frwd.trg_queue, cust_flow.pkt_frwd.gem_port);        
-    }      
-    else
-        printk("%s: illegal operation <%s>\n", __FUNCTION__, attr->attr.name);
-#endif
-
-    raw_local_irq_restore(flags);
-
-    return len;
-}
-
-static ssize_t mv_cust_spec_proc_6h_store(struct device *dev,
-                                          struct device_attribute *attr,
-                                          const char *buf, size_t len)
-{
-    const char*     name = attr->attr.name;
-    unsigned int    v1=0, v2=0, v3=0, v4=0, v5=0, v6=0;
-    unsigned long   flags;
-
-    if (!capable(CAP_NET_ADMIN))
-        return -EPERM;
-
-    /* Read input */
-    sscanf(buf, "%x %x %x %x %x %x", &v1, &v2, &v3, &v4, &v5, &v6);
-
-    raw_local_irq_save(flags);
-
-    if (!strcmp(name, "eoam_mac")) {
-        eoam_mac[0] = (uint8_t)v1;
-        eoam_mac[1] = (uint8_t)v2;
-        eoam_mac[2] = (uint8_t)v3;
-        eoam_mac[3] = (uint8_t)v4;
-        eoam_mac[4] = (uint8_t)v5;
-        eoam_mac[5] = (uint8_t)v6;
-    }
-    else
-        printk("%s: illegal operation <%s>\n", __FUNCTION__, attr->attr.name);
-
-    raw_local_irq_restore(flags);
-
-    return len;
-}
-
-static ssize_t mv_cust_spec_proc_64_store(struct device *dev,
-                                          struct device_attribute *attr,
-                                          const char *buf, size_t len)
-{
-    const char*          name = attr->attr.name;
-    unsigned int         v[64];
-    unsigned int         index = 0;
-    unsigned long        flags;
-    mv_cust_dscp_pbits_t dscp_map;
-
-    if (!capable(CAP_NET_ADMIN))
-        return -EPERM;
-
-    /* Read input */
-    for (index=0; index<64; index++) {
-        sscanf(buf, "%d", &v[index]);
-        dscp_map.pbits[index] = (uint8_t)v[index];
-    }
-    dscp_map.in_use = 1;
-
-    raw_local_irq_save(flags);
-
-    if (!strcmp(name, "dscp_map_set")) {
-#ifdef CONFIG_MV_CUST_FLOW_MAP_HANDLE       
-        mv_cust_dscp_map_set(&dscp_map);
-#endif        
-    }
-    else
-        printk("%s: illegal operation <%s>\n", __FUNCTION__, attr->attr.name);
-
-    raw_local_irq_restore(flags);
-
-    return len;
-}
-
-static DEVICE_ATTR(help,           S_IRUSR, mv_cust_spec_proc_show, NULL);
-static DEVICE_ATTR(debug,          S_IWUSR, mv_cust_spec_proc_show, mv_cust_spec_proc_1_store);
-
-static DEVICE_ATTR(omci_set,       S_IWUSR, mv_cust_spec_proc_show, mv_cust_spec_proc_6_store);
-static DEVICE_ATTR(eoam_gh_keep,   S_IWUSR, mv_cust_spec_proc_show, mv_cust_spec_proc_1_store);
-static DEVICE_ATTR(eoam_txq,       S_IWUSR, mv_cust_spec_proc_show, mv_cust_spec_proc_1_store);
-static DEVICE_ATTR(eoam_mac,       S_IWUSR, mv_cust_spec_proc_show, mv_cust_spec_proc_6h_store);
-static DEVICE_ATTR(eoam_write,     S_IWUSR, mv_cust_spec_proc_show, mv_cust_spec_proc_1_store);
-
-static DEVICE_ATTR(tcont_show,     S_IRUSR, mv_cust_spec_proc_show, NULL);
-static DEVICE_ATTR(tcont_set,      S_IWUSR, mv_cust_spec_proc_show, mv_cust_spec_proc_2_store);
-
-static DEVICE_ATTR(app_show,       S_IWUSR, mv_cust_spec_proc_show, mv_cust_spec_proc_1_store);
-static DEVICE_ATTR(app_flag_set,   S_IWUSR, mv_cust_spec_proc_show, mv_cust_spec_proc_2_store);
-static DEVICE_ATTR(app_etype_set,  S_IWUSR, mv_cust_spec_proc_show, mv_cust_spec_proc_2_store);
-
-static DEVICE_ATTR(udp_ports,      S_IRUSR, mv_cust_spec_proc_show, NULL);
-static DEVICE_ATTR(udp_src,        S_IWUSR, mv_cust_spec_proc_show, mv_cust_spec_proc_6_store);
-static DEVICE_ATTR(udp_dst,        S_IWUSR, mv_cust_spec_proc_show, mv_cust_spec_proc_6_store);
-
-static DEVICE_ATTR(flow_map,       S_IRUSR, mv_cust_spec_proc_show, NULL);
-static DEVICE_ATTR(flow_map_show,  S_IRUSR, mv_cust_spec_proc_show, NULL);
-static DEVICE_ATTR(flow_map_clear, S_IRUSR, mv_cust_spec_proc_show, NULL);
-static DEVICE_ATTR(dscp_map_del,   S_IRUSR, mv_cust_spec_proc_show, NULL);
-static DEVICE_ATTR(flow_map_debug, S_IWUSR, mv_cust_spec_proc_show, mv_cust_spec_proc_flow_store);
-static DEVICE_ATTR(flow_map_us_set,S_IWUSR, mv_cust_spec_proc_show, mv_cust_spec_proc_flow_store);
-static DEVICE_ATTR(flow_map_ds_set,S_IWUSR, mv_cust_spec_proc_show, mv_cust_spec_proc_flow_store);
-static DEVICE_ATTR(flow_map_del,   S_IWUSR, mv_cust_spec_proc_show, mv_cust_spec_proc_flow_store);
-static DEVICE_ATTR(tag_flow_get,   S_IWUSR, mv_cust_spec_proc_show, mv_cust_spec_proc_flow_store);
-static DEVICE_ATTR(untag_flow_get, S_IWUSR, mv_cust_spec_proc_show, mv_cust_spec_proc_flow_store);
-static DEVICE_ATTR(dscp_map_set,   S_IWUSR, mv_cust_spec_proc_show, mv_cust_spec_proc_64_store);
-
-
-static struct attribute *mv_cust_spec_proc_attrs[] = {
-    &dev_attr_help.attr,    
-    &dev_attr_debug.attr,
-    &dev_attr_omci_set.attr,
-    &dev_attr_eoam_gh_keep.attr,
-    &dev_attr_eoam_txq.attr,
-    &dev_attr_eoam_mac.attr,
-    &dev_attr_eoam_write.attr,
-    &dev_attr_tcont_show.attr,
-    &dev_attr_tcont_set.attr,    
-    &dev_attr_app_show.attr,
-    &dev_attr_app_flag_set.attr,
-    &dev_attr_app_etype_set.attr,   
-    &dev_attr_udp_ports.attr,    
-    &dev_attr_udp_src.attr,
-    &dev_attr_udp_dst.attr,
-    &dev_attr_flow_map_debug.attr,
-    &dev_attr_flow_map_us_set.attr,
-    &dev_attr_flow_map_ds_set.attr,    
-    &dev_attr_dscp_map_set.attr,
-    &dev_attr_flow_map_del.attr,
-    &dev_attr_tag_flow_get.attr,
-    &dev_attr_untag_flow_get.attr,
-    &dev_attr_flow_map.attr,    
-    &dev_attr_flow_map_show.attr,     
-    &dev_attr_flow_map_clear.attr,   
-    &dev_attr_dscp_map_del.attr,       
-
-    NULL
-};
-
-static struct attribute_group mv_cust_spec_proc_group = {
-    .name = "proto",
-    .attrs = mv_cust_spec_proc_attrs,
-};
-
-int mvcust_sysfs_init(void)
-{
-    int err;
-    struct device *pd;
-
-    pd = bus_find_device_by_name(&platform_bus_type, NULL, "cust");
-    if (!pd) {
-        platform_device_register_simple("cust", -1, NULL, 0);
-        pd = bus_find_device_by_name(&platform_bus_type, NULL, "cust");
-    }
-
-    if (!pd) {
-        printk(KERN_ERR"%s: cannot find cust device\n", __FUNCTION__);
-        pd = &platform_bus;
-    }
-
-    err = sysfs_create_group(&pd->kobj, &mv_cust_spec_proc_group);
-    if (err) {
-        printk(KERN_INFO "sysfs group failed %d\n", err);
-        goto out;
-    }
-
-    printk(KERN_INFO "= CUST Module SYS FS Init ended successfully =\n");
-    out:
-    return err;
-}
-
-void mvcust_sysfs_delete(void)
-{
-    struct device *pd;
-
-    pd = bus_find_device_by_name(&platform_bus_type, NULL, "cust");
-    if (!pd)
-    {
-        printk(KERN_ERR"%s: cannot find CUST device\n", __FUNCTION__);
-        return;
-    }
-
-    sysfs_remove_group(&pd->kobj, &mv_cust_spec_proc_group);
-
-    printk(KERN_INFO "= CUST Module SYS FS Remove ended successfully =\n");
-}
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_ezxml/ezxml_readme.txt b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_ezxml/ezxml_readme.txt
new file mode 100755
index 0000000..6972afb
--- /dev/null
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_ezxml/ezxml_readme.txt
@@ -0,0 +1,19 @@
+ezxml.c

+1.	Original file line 25, removed “#include <stdlib.h>”, it is for user space

+2.	Original file line 32, removed “#ifndef EZXML_NOMMAP” block, and add linux kernel header files

+3.	Original file line 38, added function ezxml_realloc/malloc/realloc/free/stat definitions

+4.	Original file line 59, added function strdup

+5.	Original file line 604, remove function ezxml_parse_fp

+6.	Original file line 626, changed ezxml_parse_fd implementation to kernel style

+7.	Original file line 656, changed ezxml_parse_file implementation to kernel style

+8.	Original file line 814, removed “#ifndef EZXML_NOMMAP” block, not used code 

+9.	Original file line 999, removed “#ifdef EZXML_TEST” block, not used code 

+10.	Original file line 999, added function ezxml_realloc implementation

+ 

+ezxml.h

+1.	Original file line 28, removed user space header files

+2.	Original file line 42, added FILE definition

+3.	Original file line 62, added function ezxml_t ezxml_parse_file( char *file) declaration

+4.	Original file line 62, added “#ifdef EZXML_TEST” 

+5.	Original file line 68, removed function ezxml_t ezxml_parse_file( char *file) declaration

+6.	Original file line 122, 130, 137, 145, changed strdup to MMP_OS_STRDUP

diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_neta/net_dev/mv_netdev.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_neta/net_dev/mv_netdev.c
index 86d8494..4d54029 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_neta/net_dev/mv_netdev.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_neta/net_dev/mv_netdev.c
@@ -663,9 +663,6 @@
 	if (pp == NULL)
 		return -ENODEV;
 
-	if (pp == NULL)
-		return -ENODEV;
-
 	txq_ctrl = &pp->txq_ctrl[txp * CONFIG_MV_ETH_TXQ + txq];
 	if (txq_ctrl->cpu_owner) {
 		mode = MV_ETH_TXQ_CPU;
@@ -826,7 +823,7 @@
 #ifdef CONFIG_MV_ETH_RX_SPECIAL
 /* Register special transmit check function */
 void mv_eth_rx_special_proc_func(int port, void (*func)(int port, int rxq, struct net_device *dev,
-							struct sk_buff *skb, struct neta_rx_desc *rx_desc))
+							struct eth_pbuf *pkt, struct neta_rx_desc *rx_desc))
 {
 	struct eth_port *pp = mv_eth_port_by_id(port);
 
@@ -1620,13 +1617,13 @@
 
 #if defined(CONFIG_MV_ETH_PNC) && defined(CONFIG_MV_ETH_RX_SPECIAL)
 		/* Special RX processing */
-#ifdef CONFIG_MV_CUST_FLOW_MAP_HANDLE
+#ifdef CONFIG_MV_CPH_FLOW_MAP_HANDLE
 		if ((MV_PON_PORT_ID == pp->port) || (rx_desc->pncInfo & NETA_PNC_RX_SPECIAL)) {
 #else
 		if (rx_desc->pncInfo & NETA_PNC_RX_SPECIAL) {
 #endif
 			if (pp->rx_special_proc) {
-				pp->rx_special_proc(pp->port, rxq, dev, (struct sk_buff *)(pkt->osInfo), rx_desc);
+				pp->rx_special_proc(pp->port, rxq, dev, pkt, rx_desc);
 				STAT_INFO(pp->stats.rx_special++);
 
                 if (rx_desc->pncInfo & NETA_PNC_RX_SPECIAL) {
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_neta/net_dev/mv_netdev.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_neta/net_dev/mv_netdev.h
index 607a4bc..4b4d2bd 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_neta/net_dev/mv_netdev.h
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_neta/net_dev/mv_netdev.h
@@ -333,7 +333,7 @@
 #endif /* CONFIG_MV_ETH_RX_CSUM_OFFLOAD */
 #ifdef CONFIG_MV_ETH_RX_SPECIAL
 	void    (*rx_special_proc)(int port, int rxq, struct net_device *dev,
-					struct sk_buff *skb, struct neta_rx_desc *rx_desc);
+					struct eth_pbuf *pkt, struct neta_rx_desc *rx_desc);
 #endif /* CONFIG_MV_ETH_RX_SPECIAL */
 #ifdef CONFIG_MV_ETH_TX_SPECIAL
 	int     (*tx_special_check)(int port, struct net_device *dev, struct sk_buff *skb,
@@ -723,7 +723,7 @@
 
 #ifdef CONFIG_MV_ETH_RX_SPECIAL
 void        mv_eth_rx_special_proc_func(int port, void (*func)(int port, int rxq, struct net_device *dev,
-							struct sk_buff *skb, struct neta_rx_desc *rx_desc));
+							struct eth_pbuf *pkt, struct neta_rx_desc *rx_desc));
 #endif /* CONFIG_MV_ETH_RX_SPECIAL */
 
 #ifdef CONFIG_MV_MAC_LEARN
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/epon/eponOnuApi.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/epon/eponOnuApi.c
index 9ea7d59..eb96979 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/epon/eponOnuApi.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/epon/eponOnuApi.c
@@ -806,9 +806,23 @@
 	}

 }

 

+/*******************************************************************************

+**

+**  mvEponApi2kSupportedSet

+**  ____________________________________________________________________________

+**

+**  DESCRIPTION: The function sets 2K byte packets supported on MC board

+**

+**  PARAMETERS:  pkt2kSupported - 2K byte enable or disable

+**

+**  OUTPUTS:     None

+**

+**  RETURNS:     MV_TRUE or MV_FLASE

+**

+*******************************************************************************/

 MV_STATUS mvEponApi2kSupportedSet(MV_U32 pkt2kSupported)

 {

-    MV_STATUS status = 0;

+    MV_STATUS status;

     MV_U32    devId;

 

     devId = mvCtrlModelGet();

@@ -835,4 +849,292 @@
     return (status);

 }

 

+/*******************************************************************************

+**

+**  mvEponApiLosTimeConfig

+**  ____________________________________________________________________________

+**

+**  DESCRIPTION: The function sets time duration to expire for entering holdover

+**

+**  PARAMETERS:  opticalLosTime - time duration when no optical signal detected

+**               macLosTime     - time duration when no GATE MPCPDU received

+**

+**  OUTPUTS:     None

+**

+**  RETURNS:     MV_TRUE or MV_FLASE

+**

+*******************************************************************************/

+MV_STATUS mvEponApiLosTimeConfig(MV_U16 opticalLosTime, MV_U16 macLosTime)

+{

+    MV_STATUS status = MV_OK;

+    

+    onuEponDbOnuOpticalLosTimeSet(opticalLosTime);

+    onuEponDbOnuMacLosTimeSet(macLosTime);

+

+    /* If the holdover timer is not active, we add the OpticalLosTimer to the holdover time and save */

+    if (onuPonResourceTbl_s.onuPonHoldoverTimerId.onuPonTimerActive == ONU_PON_TIMER_NOT_ACTIVE)

+    {

+        /* Add the configured OptLosTime to holdover time and save for next holdover */

+        onuPonTimerUpdate(&(onuPonResourceTbl_s.onuPonHoldoverTimerId), 0, 

+                          onuEponDbOnuHoldoverTimeGet() + onuEponDbOnuOpticalLosTimeGet(), 0);

+    }

+

+    return status;

+}

+

+/*******************************************************************************

+**

+**  mvEponApiTxPowerCtrlConfig

+**  ____________________________________________________________________________

+**

+**  DESCRIPTION: The function controls the TX power on and off

+**

+**  PARAMETERS:  action - turn on and turn off the TX power, or turn off after 

+**                        certain time duration.

+**               time   - time duration to be expired before turning off TX

+**

+**  OUTPUTS:     None

+**

+**  RETURNS:     MV_TRUE or MV_FLASE

+**

+*******************************************************************************/

+MV_STATUS mvEponApiTxPowerCtrlConfig(MV_U16 action, MV_U16 time)

+{

+    MV_STATUS status = MV_OK;

+    

+    if (action == E_EPON_IOCTL_ENABLE_TX)

+    {

+        onuPonTxPowerOn(MV_TRUE);

+    }

+    else if (action == E_EPON_IOCTL_DISABLE_TX)

+    {

+        onuPonTxPowerOn(MV_FALSE);

+    }

+    else if (action == E_EPON_IOCTL_DISABLE_TX_DELAY)

+    {        

+        onuPonTimerUpdate(&(onuPonResourceTbl_s.onuEponTxControlTimerId), 0, time, 1);

+    }

+

+    return status;

+}

+

+/*******************************************************************************

+**

+**  mvEponApiPowerSavingConfig

+**  ____________________________________________________________________________

+**

+**  DESCRIPTION: The function sets the parameters for power saving mode

+**

+**  PARAMETERS:  earlyWakeup - set the ability of supporting early wake up

+**               maxSleepDuration - time period for keeping power saving mode

+**

+**  OUTPUTS:     None

+**

+**  RETURNS:     MV_TRUE or MV_FLASE

+**

+*******************************************************************************/

+MV_STATUS mvEponApiPowerSavingConfig(MV_U8 earlyWakeup, MV_U64 maxSleepDuration)

+{

+    MV_STATUS status = MV_OK;

+    

+    if ((onuEponDbOnuSleepDurationGet() + onuEponDbOnuWakeupDurationGet())

+        >= maxSleepDuration)

+    {

+        mvPonPrint(PON_PRINT_INFO, PON_API_MODULE,

+			       "ERROR: Wrong parameters, sleep(%d) + wakeup(%d) >= maxSleepDuration(%d)\r\n", 

+			       onuEponDbOnuSleepDurationGet(), onuEponDbOnuWakeupDurationGet(), 

+			       maxSleepDuration);

+        

+        return MV_BAD_PARAM;

+    }

+

+    onuEponDbOnuPowerSavingWakeupSet(earlyWakeup);

+    onuEponDbOnuPowerSavingMaxSleepDurationSet(maxSleepDuration);

+

+    if (onuPonResourceTbl_s.onuEponMaxSleepTimerId.onuPonTimerActive == ONU_PON_TIMER_ACTIVE)

+    {

+        onuPonTimerUpdate(&(onuPonResourceTbl_s.onuEponMaxSleepTimerId), 0, 

+                          maxSleepDuration, 1);

+    }

+

+    return status;

+}

+

+/*******************************************************************************

+**

+**  mvEponApiSleepModeCtrl

+**  ____________________________________________________________________________

+**

+**  DESCRIPTION: The function controls sleep mode

+**

+**  PARAMETERS:  enable - turn off and turn on the TX or RX

+**

+**  OUTPUTS:     None

+**

+**  RETURNS:     MV_TRUE or MV_FLASE

+**

+**  Ref:         Called by mvEponApiSleepCtrlEnable, mvEponApiSleepCtrlDisable, 

+**               mvEponApiSleepCtrlCfg, onuEponMaxSleepTimerHndl, 

+**               onuEponSleepWakeDurationTimerHndl

+*******************************************************************************/

+MV_STATUS mvEponApiSleepModeCtrl(MV_U32 enable)

+{

+    MV_STATUS status = MV_OK;

+    

+    if (onuEponDbOnuSleepModeGet() == E_EPON_SLEEP_MODE_CTRL_TX)

+    {

+        if (enable == MV_TRUE)

+        {

+            status |= onuPonTxPowerOn(MV_FALSE);

+        }

+        else

+        {

+            status |= onuPonTxPowerOn(MV_TRUE);

+        }

+    }

+    else if (onuEponDbOnuSleepModeGet() == E_EPON_SLEEP_MODE_CTRL_TX_RX)

+    {

+        if (enable == MV_TRUE)

+        {

+            status |= onuPonTxPowerOn(MV_FALSE);

+            status |= mvOnuEponMacSerdesPuRxWrite(MV_FALSE);

+        }

+        else

+        {

+            status |= onuPonTxPowerOn(MV_TRUE);

+            status |= mvOnuEponMacSerdesPuRxWrite(MV_TRUE);

+        }

+    }

+

+    return status;

+}

+

+/*******************************************************************************

+**

+**  mvEponApiSleepCtrlDisable

+**  ____________________________________________________________________________

+**

+**  DESCRIPTION: The function sets ONU to leave the power saving mode

+**

+**  PARAMETERS:  

+**

+**  OUTPUTS:     None

+**

+**  RETURNS:     MV_TRUE or MV_FLASE

+**

+*******************************************************************************/

+MV_STATUS mvEponApiSleepCtrlDisable()

+{   

+    /* Stop max sleep timer */

+    if (onuPonResourceTbl_s.onuEponMaxSleepTimerId.onuPonTimerActive == ONU_PON_TIMER_ACTIVE)

+    {

+        onuPonTimerDisable(&(onuPonResourceTbl_s.onuEponMaxSleepTimerId));

+    }

+

+    /* Stop sleep/wakeup duration timer */

+    if (onuPonResourceTbl_s.onuEponSleepDurationTimerId.onuPonTimerActive == ONU_PON_TIMER_ACTIVE)

+    {

+        onuPonTimerDisable(&(onuPonResourceTbl_s.onuEponSleepDurationTimerId));

+    }

+

+    /* Enable TX/RX power */

+    mvEponApiSleepModeCtrl(MV_FALSE);

+

+    /* Leave the power saving status */

+    onuEponDbOnuSleepWakeupStatusSet(E_EPON_NOT_POWER_SAVING_STATUS);

+

+    return(MV_OK);

+}

+

+/*******************************************************************************

+**

+**  mvEponApiSleepCtrlEnable

+**  ____________________________________________________________________________

+**

+**  DESCRIPTION: The function sets ONU to enter the power saving mode

+**

+**  PARAMETERS:  

+**

+**  OUTPUTS:     None

+**

+**  RETURNS:     MV_TRUE or MV_FLASE

+**

+*******************************************************************************/

+MV_STATUS mvEponApiSleepCtrlEnable()

+{

+    /* refresh max sleep timer, no matter it's active or not */

+    onuPonTimerUpdate(&(onuPonResourceTbl_s.onuEponMaxSleepTimerId), 0, 

+                      onuEponDbOnuPowerSavingMaxSleepDurationGet(), 1);

+

+    /* refresh sleep/wakeup duration timer, no matter it's active or not */

+    onuPonTimerUpdate(&(onuPonResourceTbl_s.onuEponSleepDurationTimerId), 0, 

+                      onuEponDbOnuSleepDurationGet(), 1);

+

+    /* if the current status is not sleep, then go to sleep status */

+    if (onuEponDbOnuSleepWakeupStatusGet() != E_EPON_POWER_SAVING_SLEEP_STATUS)

+    {

+        /* Disable TX/RX power */

+        mvEponApiSleepModeCtrl(MV_TRUE);

+

+        /* update status to sleep status */

+        onuEponDbOnuSleepWakeupStatusSet(E_EPON_POWER_SAVING_SLEEP_STATUS);

+    }

+

+    return(MV_OK);

+}

+

+/*******************************************************************************

+**

+**  mvEponApiSleepCtrlCfg

+**  ____________________________________________________________________________

+**

+**  DESCRIPTION: The function sets the parameters used in the power saving mode

+**

+**  PARAMETERS:  mode - 0: do nothing, 1: turn off Tx, 2: turn off Tx and Rx

+**               sleepDuration - time duration for keeping sleep

+**               wakeupDuration - time duration for being wakeup

+**

+**  OUTPUTS:     None

+**

+**  RETURNS:     MV_TRUE or MV_FLASE

+**

+*******************************************************************************/

+MV_STATUS mvEponApiSleepCtrlCfg(MV_U8 mode, MV_U32 sleepDuration, MV_U32 wakeupDuration)

+{

+    if ((sleepDuration + wakeupDuration) >= onuEponDbOnuPowerSavingMaxSleepDurationGet())

+    {

+        mvPonPrint(PON_PRINT_ERROR, PON_API_MODULE,

+			       "ERROR: Wrong parameters, sleep(%d) + wakeup(%d) >= maxSleepDuration(%d)\r\n", 

+			       sleepDuration, sleepDuration, 

+			       onuEponDbOnuPowerSavingMaxSleepDurationGet());

+        

+        return MV_BAD_PARAM;

+    }

+

+    /* save paraters into DB */

+    onuEponDbOnuSleepModeSet(mode);

+    onuEponDbOnuSleepDurationSet(sleepDuration);

+    onuEponDbOnuWakeupDurationSet(wakeupDuration);

+

+    /* refresh max sleep timer, no matter it's active or not */

+    onuPonTimerUpdate(&(onuPonResourceTbl_s.onuEponMaxSleepTimerId), 0, 

+                      onuEponDbOnuPowerSavingMaxSleepDurationGet(), 1);

+

+    /* refresh sleep/wakeup duration timer, no matter it's active or not */

+    onuPonTimerUpdate(&(onuPonResourceTbl_s.onuEponSleepDurationTimerId), 0, 

+                      onuEponDbOnuSleepDurationGet(), 1);

+

+    /* if the current status is not sleep, then go to sleep status */

+    if (onuEponDbOnuSleepWakeupStatusGet() != E_EPON_POWER_SAVING_SLEEP_STATUS)

+    {

+        /* Disable TX/RX power */

+        mvEponApiSleepModeCtrl(MV_TRUE);

+

+        /* update status to sleep status */

+        onuEponDbOnuSleepWakeupStatusSet(E_EPON_POWER_SAVING_SLEEP_STATUS);

+    }

+

+    return(MV_OK);

+}

+

 

diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/epon/eponOnuApi.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/epon/eponOnuApi.h
index 9e23984..7590daf 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/epon/eponOnuApi.h
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/epon/eponOnuApi.h
@@ -83,6 +83,7 @@
 

 /* Include Files

 ------------------------------------------------------------------------------*/

+#include "eponOnuDb.h"

  

 /* Definitions

 ------------------------------------------------------------------------------*/ 

@@ -145,6 +146,14 @@
 /* Config API */

 MV_STATUS mvEponApi2kSupportedSet(MV_U32 pkt2kSupported);

 

+MV_STATUS mvEponApiLosTimeConfig(MV_U16 opticalLosTime, MV_U16 macLosTime);

+MV_STATUS mvEponApiTxPowerCtrlConfig(MV_U16 action, MV_U16 time);

+MV_STATUS mvEponApiPowerSavingConfig(MV_U8 earlyWakeup, MV_U64 maxSleepDuration);

+MV_STATUS mvEponApiSleepModeCtrl(MV_U32 enable);

+MV_STATUS mvEponApiSleepCtrlDisable(void);

+MV_STATUS mvEponApiSleepCtrlEnable(void);

+MV_STATUS mvEponApiSleepCtrlCfg(MV_U8 mode, MV_U32 sleepDuration, MV_U32 wakeupDuration);

+

 /* Macros

 ------------------------------------------------------------------------------*/    

 

diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/epon/eponOnuDb.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/epon/eponOnuDb.c
index c5bcf3b..3bf6b57 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/epon/eponOnuDb.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/epon/eponOnuDb.c
@@ -282,11 +282,19 @@
 

 	/* set holdover */

 	onuEponDbOnuHoldoverStateSet(ONU_HOLDOVER_NOT_ACTIVE);

+    onuEponDbOnuHoldoverTimeSet(ONU_PON_TIMER_HOLDOVER_INTERVAL);

+    onuEponDbOnuOpticalLosTimeSet(0);

+

+    onuEponDbOnuPowerSavingMaxSleepDurationSet(ONU_PON_TIMER_MAX_SLEEP_INTERVAL);

+    onuEponDbOnuPowerSavingWakeupSet(0);

+    onuEponDbOnuSleepModeSet(E_EPON_SLEEP_MODE_CTRL_TX);

+    onuEponDbOnuSleepDurationSet(ONU_PON_TIMER_SLEEP_DURATION);

+    onuEponDbOnuWakeupDurationSet(ONU_PON_TIMER_WAKEUP_DURATION);

+    onuEponDbOnuSleepWakeupStatusSet(E_EPON_NOT_POWER_SAVING_STATUS);

 

 	/* set silence */

 	onuEponDbOnuSilenceStateSet(ONU_SILENCE_NOT_ACTIVE);

 

-

 	/* SW DBA params */

 	onuEponDbOnuSwRprtTimerTypeSet(ONU_EPON_SW_DBA_RPRT_TIMER);

 	onuEponDbOnuSwRprtMacTimerIntervalSet(62500); /* 1 msec in 16nano units */

@@ -1453,6 +1461,332 @@
 }

 

 /*******************************************************************************

+ **

+ **  onuEponDbOnuOpticalLosTimeSet

+ **  ____________________________________________________________________________

+ ** 

+ **  DESCRIPTION: The function sets time duration value in database 

+ **               when no optical signal is detected

+ **               

+ **  PARAMETERS:  losTime - time duration

+ **

+ **  OUTPUTS:     None

+ **

+ **  RETURNS:     

+ **                   

+ *******************************************************************************/

+MV_STATUS onuEponDbOnuOpticalLosTimeSet(MV_U16 losTime)

+{

+    onuEponDb_s.onuEponGenTbl_s.onuEponOpticalLosTime = losTime;

+    

+    return MV_OK;

+}

+

+/*******************************************************************************

+**

+**  onuEponDbOnuOpticalLosTimeGet

+**  ____________________________________________________________________________

+** 

+**  DESCRIPTION: The function returns time duration to be used 

+**               when no optical signal is detected

+**               

+**  PARAMETERS:  None

+**

+**  OUTPUTS:     None

+**

+**  RETURNS:     time duration value

+**                   

+*******************************************************************************/

+MV_U32 onuEponDbOnuOpticalLosTimeGet()

+{

+    return onuEponDb_s.onuEponGenTbl_s.onuEponOpticalLosTime;

+}

+

+/*******************************************************************************

+ **

+ **  onuEponDbOnuMacLosTimeSet

+ **  ____________________________________________________________________________

+ ** 

+ **  DESCRIPTION: The function sets time duration value in database 

+ **               when no GATE MPCPDU is received

+ **

+ **  PARAMETERS:  losTime - time duration

+ **

+ **  OUTPUTS:     None

+ **

+ **  RETURNS:     

+ **                   

+ *******************************************************************************/

+MV_STATUS onuEponDbOnuMacLosTimeSet(MV_U16 losTime)

+{

+    onuEponDb_s.onuEponGenTbl_s.onuEponMacLosTime = losTime;

+    

+    return MV_OK;

+}

+

+/*******************************************************************************

+**

+**  onuEponDbOnuMacLosTimeGet

+**  ____________________________________________________________________________

+** 

+**  DESCRIPTION: The function returns time duration to be used 

+**               when no GATE MPCPDU is received

+**               

+**  PARAMETERS:  None

+**

+**  OUTPUTS:     None

+**

+**  RETURNS:     time duration value

+**                   

+*******************************************************************************/

+MV_U32 onuEponDbOnuMacLosTimeGet()

+{

+    return onuEponDb_s.onuEponGenTbl_s.onuEponMacLosTime;

+}

+

+/*******************************************************************************

+ **

+ **  onuEponDbOnuPowerSavingWakeupSet

+ **  ____________________________________________________________________________

+ ** 

+ **  DESCRIPTION: The function sets the support for early wakeup 

+ **               in power saving mode

+ **

+ **  PARAMETERS:  wakeUpEnable - Enable or disable the early wakeup support

+ **

+ **  OUTPUTS:     None

+ **

+ **  RETURNS:     

+ **                   

+ *******************************************************************************/

+MV_STATUS onuEponDbOnuPowerSavingWakeupSet(MV_U8 wakeUpEnable)

+{

+    onuEponDb_s.onuEponGenTbl_s.onuEponPowerSavingWakeup = wakeUpEnable;

+

+    return (MV_OK);

+}

+

+/*******************************************************************************

+**

+**  onuEponDbOnuPowerSavingWakeupGet

+**  ____________________________________________________________________________

+** 

+**  DESCRIPTION: The function returns the support for early wakeup

+**               

+**  PARAMETERS:  None

+**

+**  OUTPUTS:     None

+**

+**  RETURNS:     enable or disable

+**                   

+*******************************************************************************/

+MV_U32 onuEponDbOnuPowerSavingWakeupGet()

+{

+    return onuEponDb_s.onuEponGenTbl_s.onuEponPowerSavingWakeup;

+}

+

+/*******************************************************************************

+ **

+ **  onuEponDbOnuPowerSavingMaxSleepDurationSet

+ **  ____________________________________________________________________________

+ ** 

+ **  DESCRIPTION: The function sets the max sleep duration 

+ **               in power saving mode

+ **

+ **  PARAMETERS:  maxSleepDuration - max sleep duration

+ **

+ **  OUTPUTS:     None

+ **

+ **  RETURNS:     

+ **                   

+ *******************************************************************************/

+MV_STATUS onuEponDbOnuPowerSavingMaxSleepDurationSet(MV_U64 maxSleepDuration)

+{

+    onuEponDb_s.onuEponGenTbl_s.onuEponPowerSavingMaxSleepDuration= maxSleepDuration;

+

+    return (MV_OK);

+}

+

+/*******************************************************************************

+**

+**  onuEponDbOnuPowerSavingMaxSleepDurationGet

+**  ____________________________________________________________________________

+** 

+**  DESCRIPTION: The function returns the max sleep duration in power saving mode

+**               

+**  PARAMETERS:  None

+**

+**  OUTPUTS:     None

+**

+**  RETURNS:     max sleep duration

+**                   

+*******************************************************************************/

+MV_U32 onuEponDbOnuPowerSavingMaxSleepDurationGet()

+{

+    return onuEponDb_s.onuEponGenTbl_s.onuEponPowerSavingMaxSleepDuration;

+}

+

+/*******************************************************************************

+ **

+ **  onuEponDbOnuSleepModeSet

+ **  ____________________________________________________________________________

+ ** 

+ **  DESCRIPTION: The function sets control mode for power saving

+ **

+ **  PARAMETERS:  mode - turn off tx or tx/rx

+ **

+ **  OUTPUTS:     None

+ **

+ **  RETURNS:     

+ **                   

+ *******************************************************************************/

+MV_STATUS onuEponDbOnuSleepModeSet(MV_U8 mode)

+{

+    onuEponDb_s.onuEponGenTbl_s.onuEponSleepMode = mode;

+

+    return MV_OK;

+}

+

+/*******************************************************************************

+**

+**  onuEponDbOnuSleepModeGet

+**  ____________________________________________________________________________

+** 

+**  DESCRIPTION: The function returns the sleep mode for power saving mode

+**               

+**  PARAMETERS:  None

+**

+**  OUTPUTS:     None

+**

+**  RETURNS:     sleep mode

+**                   

+*******************************************************************************/

+E_EponSleepCtrlMode onuEponDbOnuSleepModeGet()

+{

+    return onuEponDb_s.onuEponGenTbl_s.onuEponSleepMode;

+}

+

+/*******************************************************************************

+ **

+ **  onuEponDbOnuSleepDurationSet

+ **  ____________________________________________________________________________

+ ** 

+ **  DESCRIPTION: The function sets the periodical sleep duration 

+ **               for power saving

+ **

+ **  PARAMETERS:  sleepDuration - periodical sleep duration

+ **

+ **  OUTPUTS:     None

+ **

+ **  RETURNS:     

+ **                   

+ *******************************************************************************/

+MV_STATUS onuEponDbOnuSleepDurationSet(MV_U32 sleepDuration)

+{

+    onuEponDb_s.onuEponGenTbl_s.onuEponSleepDuration = sleepDuration;

+

+    return MV_OK;

+}

+

+/*******************************************************************************

+**

+**  onuEponDbOnuSleepDurationGet

+**  ____________________________________________________________________________

+** 

+**  DESCRIPTION: The function returns the periodical sleep duration for power saving

+**               

+**  PARAMETERS:  None

+**

+**  OUTPUTS:     None

+**

+**  RETURNS:     max sleep duration

+**                   

+*******************************************************************************/

+MV_U32 onuEponDbOnuSleepDurationGet()

+{

+    return onuEponDb_s.onuEponGenTbl_s.onuEponSleepDuration;

+}

+

+/*******************************************************************************

+ **

+ **  onuEponDbOnuWakeupDurationSet

+ **  ____________________________________________________________________________

+ ** 

+ **  DESCRIPTION: The function sets the periodical wakeup duration 

+ **               for power saving

+ **

+ **  PARAMETERS:  wakeupDuration - periodical wakeup duration

+ **

+ **  OUTPUTS:     None

+ **

+ **  RETURNS:     

+ **                   

+ *******************************************************************************/

+MV_STATUS onuEponDbOnuWakeupDurationSet(MV_U32 wakeupDuration)

+{

+    onuEponDb_s.onuEponGenTbl_s.onuEponWakeupDuration = wakeupDuration;

+

+    return MV_OK;

+}

+

+/*******************************************************************************

+**

+**  onuEponDbOnuWakeupDurationGet

+**  ____________________________________________________________________________

+** 

+**  DESCRIPTION: The function returns the periodical wakeup duration for power saving

+**               

+**  PARAMETERS:  None

+**

+**  OUTPUTS:     None

+**

+**  RETURNS:     max sleep duration

+**                   

+*******************************************************************************/

+MV_U32 onuEponDbOnuWakeupDurationGet()

+{

+    return onuEponDb_s.onuEponGenTbl_s.onuEponWakeupDuration;

+}

+

+/*******************************************************************************

+ **

+ **  onuEponDbOnuSleepWakeupStatusSet

+ **  ____________________________________________________________________________

+ ** 

+ **  DESCRIPTION: The function sets power saving status

+ **

+ **  PARAMETERS:  status - no power saving/sleep/wakeup

+ **

+ **  OUTPUTS:     None

+ **

+ **  RETURNS:     

+ **                   

+ *******************************************************************************/

+MV_STATUS onuEponDbOnuSleepWakeupStatusSet(MV_U32 status)

+{

+    onuEponDb_s.onuEponGenTbl_s.onuEponPowerSavingStatus = status;

+}

+

+/*******************************************************************************

+**

+**  onuEponDbOnuSleepWakeupStatusGet

+**  ____________________________________________________________________________

+** 

+**  DESCRIPTION: The function returns the power saving status

+**               

+**  PARAMETERS:  None

+**

+**  OUTPUTS:     None

+**

+**  RETURNS:     power saving status

+**                   

+*******************************************************************************/

+MV_U32 onuEponDbOnuSleepWakeupStatusGet()

+{

+    return onuEponDb_s.onuEponGenTbl_s.onuEponPowerSavingStatus;

+}

+

+/*******************************************************************************

 **

 **  onuEponDbOnuSilenceStateSet

 **  ____________________________________________________________________________

diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/epon/eponOnuDb.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/epon/eponOnuDb.h
index df2003e..b2f285d 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/epon/eponOnuDb.h
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/epon/eponOnuDb.h
@@ -83,6 +83,7 @@
 

 /* Include Files

 ------------------------------------------------------------------------------*/

+#include "eponOnuLnxKsMI.h"

  

 /* Definitions

 ------------------------------------------------------------------------------*/ 

@@ -330,6 +331,20 @@
   MV_U32 shadow;                              

 }S_OnuEponDbaShadowRegMap;

 

+typedef enum

+{

+    E_EPON_SLEEP_MODE_CTRL_NOTHING = 0,

+    E_EPON_SLEEP_MODE_CTRL_TX      = 1,

+    E_EPON_SLEEP_MODE_CTRL_TX_RX   = 2, 

+} E_EponSleepCtrlMode;

+

+typedef enum

+{

+    E_EPON_NOT_POWER_SAVING_STATUS    = 0, 

+    E_EPON_POWER_SAVING_SLEEP_STATUS  = 1, 

+    E_EPON_POWER_SAVING_WAKEUP_STATUS = 2, 

+} E_EponPowerSavingStatus;

+

 typedef struct                                         

 {                                                      

   MV_U32           onuEponValid[EPON_MAX_MAC_NUM];      /* ONU Valid for sync */  

@@ -358,7 +373,16 @@
   MV_U32           onuEponOverHead;             

   MV_U32           onuEponPhyOutput;             

   MV_U32           onuEponXvrBurstEnPolarity;

-}S_OnuEponGenTbl;                                     

+  MV_U32           onuEponOpticalLosTime;

+  MV_U32           onuEponMacLosTime;

+  MV_U32           onuEponPowerSavingWakeup;

+  MV_U32           onuEponPowerSavingMaxSleepDuration;

+  MV_U32           onuEponSleepAction;

+  MV_U32           onuEponSleepMode;

+  MV_U32           onuEponSleepDuration;

+  MV_U32           onuEponWakeupDuration;

+  MV_U32           onuEponPowerSavingStatus;

+}S_OnuEponGenTbl;

 

 /* ONU EPON Data Path tables */

 typedef struct

@@ -464,6 +488,24 @@
 MV_U32    onuEponDbOnuHoldoverTimeGet(void);

 MV_STATUS onuEponDbOnuHoldoverExecSet(MV_U32 state);

 MV_U32    onuEponDbOnuHoldoverExecGet(void);

+MV_STATUS onuEponDbOnuOpticalLosTimeSet(MV_U16 losTime);

+MV_U32    onuEponDbOnuOpticalLosTimeGet(void);

+MV_STATUS onuEponDbOnuMacLosTimeSet(MV_U16 losTime);

+MV_U32    onuEponDbOnuMacLosTimeGet(void);

+MV_STATUS onuEponDbOnuPowerSavingWakeupSet(MV_U8 wakeUpEnable);

+MV_U32    onuEponDbOnuPowerSavingWakeupGet(void);

+MV_STATUS onuEponDbOnuPowerSavingMaxSleepDurationSet(MV_U64 maxSleepDuration);

+MV_U32    onuEponDbOnuPowerSavingMaxSleepDurationGet(void);

+MV_STATUS onuEponDbOnuSleepActionSet(MV_U8 action);

+MV_U32    onuEponDbOnuSleepActionGet(void);

+MV_STATUS onuEponDbOnuSleepModeSet(MV_U8 mode);

+MV_U32    onuEponDbOnuSleepModeGet(void);

+MV_STATUS onuEponDbOnuSleepDurationSet(MV_U32 sleepDuration);

+MV_U32    onuEponDbOnuSleepDurationGet(void);

+MV_STATUS onuEponDbOnuWakeupDurationSet(MV_U32 wakeupDuration);

+MV_U32    onuEponDbOnuWakeupDurationGet(void);

+MV_STATUS onuEponDbOnuSleepWakeupStatusSet(MV_U32 status);

+MV_U32    onuEponDbOnuSleepWakeupStatusGet(void);

 MV_STATUS onuEponDbOnuSilenceStateSet(MV_U32 state);

 MV_U32    onuEponDbOnuSilenceStateGet(void);

 MV_STATUS onuEponDbOnuSwRprtTimerTypeSet(MV_U32 timerType);

diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/epon/eponOnuIsr.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/epon/eponOnuIsr.c
index 654e56f..fafce2a 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/epon/eponOnuIsr.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/epon/eponOnuIsr.c
@@ -1095,84 +1095,87 @@
 		return(MV_ERROR);
 	  }
 	}
-	/* KW2 ASIC Rev Z2 */
-	/* =============== */
-	else if (mvCtrlRevGet() == ONU_ASIC_REV_Z2)
-	{
-	  /* PHY control register - output status*/
-	  PON_GPIO_GET(BOARD_GPP_PON_XVR_TX, gpioGroup, gpioMask);
-	  if (gpioMask != PON_GPIO_NOT_USED)
-	  {
-	  	status = mvGppTypeSet(gpioGroup, gpioMask, ~gpioMask/*output*/);
-	  	if (status != MV_OK)
-	  	{
-	  	  mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
-	  			   "ERROR: (%s:%d) mvGppTypeSet\n\r", __FILE_DESC__, __LINE__);
-	  	  return(MV_ERROR);
-	  	}
+   else
+   {
+	   /* KW2 ASIC Rev Z2 */
+	   /* =============== */
+	   if (mvCtrlRevGet() == ONU_ASIC_REV_Z2)
+	   {
+		/* PHY control register - output status*/
+		PON_GPIO_GET(BOARD_GPP_PON_XVR_TX, gpioGroup, gpioMask);
+		if (gpioMask != PON_GPIO_NOT_USED)
+		{
+		     status = mvGppTypeSet(gpioGroup, gpioMask, ~gpioMask/*output*/);
+		     if (status != MV_OK)
+		     {
+		       mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
+					"ERROR: (%s:%d) mvGppTypeSet\n\r", __FILE_DESC__, __LINE__);
+		       return(MV_ERROR);
+		     }
 
-	  	status = mvGppValueSet(gpioGroup, gpioMask, gpioMask /*enable*/);
-	  	if (status != MV_OK)
-	  	{
-	  	  mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
-	  			   "ERROR: (%s:%d) mvGppTypeSet\n\r", __FILE_DESC__, __LINE__);
-	  	  return(MV_ERROR);
-	  	}
-	  }
+		     status = mvGppValueSet(gpioGroup, gpioMask, gpioMask /*enable*/);
+		     if (status != MV_OK)
+		     {
+		       mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
+					"ERROR: (%s:%d) mvGppTypeSet\n\r", __FILE_DESC__, __LINE__);
+		       return(MV_ERROR);
+		     }
+		}
 
-	  status  = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_1_BEN_IO_EN, ONU_PHY_INPUT, 0);
-	  if (status != MV_OK)
-	  {
-	    mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
-	  		   "ERROR: (%s:%d) asicOntMiscRegWrite\n\r", __FILE_DESC__, __LINE__);
-	    return(MV_ERROR);
-	  }
-	}
+		status  = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_1_BEN_IO_EN, ONU_PHY_INPUT, 0);
+		if (status != MV_OK)
+		{
+		 mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
+				"ERROR: (%s:%d) asicOntMiscRegWrite\n\r", __FILE_DESC__, __LINE__);
+		 return(MV_ERROR);
+		}
+	   }
 
-	/* KW2 ASIC Rev A0 */
-	/* =============== */
-	else if (mvCtrlRevGet() == ONU_ASIC_REV_A0)
-	{
-      status  = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_1_BEN_IO_EN, ONU_PHY_OUTPUT, 0);
-	  if (status != MV_OK)
-	  {
-	    mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
-	  		 "ERROR: (%s:%d) asicOntMiscRegWrite\n\r", __FILE_DESC__, __LINE__);
-	    return(MV_ERROR);
-	  }
-
-	  /* PHY control register - force enable */
-	  status  = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_1_FORCE_BEN_IO_EN, 1, 0);
-	  if (status != MV_OK)
-	  {
-		mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
+	   /* KW2 ASIC Rev A0 */
+	   /* =============== */
+	   else if (mvCtrlRevGet() == ONU_ASIC_REV_A0)
+	   {
+		status  = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_1_BEN_IO_EN, ONU_PHY_OUTPUT, 0);
+		if (status != MV_OK)
+		{
+		  mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
 			       "ERROR: (%s:%d) asicOntMiscRegWrite\n\r", __FILE_DESC__, __LINE__);
-		return(MV_ERROR);
-	  }
+		  return(MV_ERROR);
+		}
 
-      polarity = onuP2PDbXvrBurstEnablePolarityGet();
+		/* PHY control register - force enable */
+		status  = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_1_FORCE_BEN_IO_EN, 1, 0);
+		if (status != MV_OK)
+		{
+		      mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
+				     "ERROR: (%s:%d) asicOntMiscRegWrite\n\r", __FILE_DESC__, __LINE__);
+		      return(MV_ERROR);
+		}
 
-	  /* XVR polarity */
-	  /* XVR polarity == 0, Active High, transmit 1 to the line  */
-	  /* XVR polarity == 1, Active Low, transmit 0 to the line  */
+		polarity = onuP2PDbXvrBurstEnablePolarityGet();
 
-	  /* P2P mode */
-	  /* Force Value == 0, transmit 0 to the line  */
-	  /* Force Value == 1, transmit 1 to the line  */
+		/* XVR polarity */
+		/* XVR polarity == 0, Active High, transmit 1 to the line  */
+		/* XVR polarity == 1, Active Low, transmit 0 to the line  */
 
-	  /* Setting P2P should be reversed from XVR polarity */
-	  /* XVR polarity == 0, Active High, write 1 for Force Value */
-	  /* XVR polarity == 1, Active Low, write 0 for Force Value */
+		/* P2P mode */
+		/* Force Value == 0, transmit 0 to the line  */
+		/* Force Value == 1, transmit 1 to the line  */
 
-	  /* PHY control register - force enable value - according to polarity */
-	  status  = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_1_FORCE_BEN_IO_VAL, polarity, 0);
-	  if (status != MV_OK)
-	  {
-		mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
-			       "ERROR: (%s:%d) asicOntMiscRegWrite\n\r", __FILE_DESC__, __LINE__);
-		return(MV_ERROR);
-	  }
-	}
+		/* Setting P2P should be reversed from XVR polarity */
+		/* XVR polarity == 0, Active High, write 1 for Force Value */
+		/* XVR polarity == 1, Active Low, write 0 for Force Value */
+
+		/* PHY control register - force enable value - according to polarity */
+		status  = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_1_FORCE_BEN_IO_VAL, polarity, 0);
+		if (status != MV_OK)
+		{
+		      mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
+				     "ERROR: (%s:%d) asicOntMiscRegWrite\n\r", __FILE_DESC__, __LINE__);
+		      return(MV_ERROR);
+		}
+	   }
+   }
 
 	/* Read current FEC configuration */
     status = mvOnuEponMacRxpEncConfigGet(&isrP2pPreviousFecState);
@@ -1318,69 +1321,73 @@
 		 return(MV_ERROR);
 	   }
    }
-	/* KW2 ASIC Rev Z2 */
-	/* =============== */
-	else if (mvCtrlRevGet() == ONU_ASIC_REV_Z2)
-	{
-	  PON_GPIO_GET(BOARD_GPP_PON_XVR_TX, gpioGroup, gpioMask);
-	  if (gpioMask != PON_GPIO_NOT_USED)
-	  {
-	  	status = mvGppTypeSet(gpioGroup, gpioMask, ~gpioMask/*output*/);
-	  	if (status != MV_OK)
-	  	{
-	  	  mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
-	  			     "ERROR: (%s:%d) mvGppTypeSet\n\r", __FILE_DESC__, __LINE__);
-	  	  return(MV_ERROR);
-	  	}
+   else
+   {
+	   /* KW2 ASIC Rev Z2 */
+	   /* =============== */
+	   if (mvCtrlRevGet() == ONU_ASIC_REV_Z2)
+	   {
+		PON_GPIO_GET(BOARD_GPP_PON_XVR_TX, gpioGroup, gpioMask);
+		if (gpioMask != PON_GPIO_NOT_USED)
+		{
+		     status = mvGppTypeSet(gpioGroup, gpioMask, ~gpioMask/*output*/);
+		     if (status != MV_OK)
+		     {
+		       mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
+					  "ERROR: (%s:%d) mvGppTypeSet\n\r", __FILE_DESC__, __LINE__);
+		       return(MV_ERROR);
+		     }
 
-	  	status = mvGppValueSet(gpioGroup, gpioMask, ~gpioMask/*disable*/);
-	  	if (status != MV_OK)
-	  	{
-	  	  mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
-	  			     "ERROR: (%s:%d) mvGppValueSet\n\r", __FILE_DESC__, __LINE__);
-	  	  return(MV_ERROR);
-	  	}
+		     status = mvGppValueSet(gpioGroup, gpioMask, ~gpioMask/*disable*/);
+		     if (status != MV_OK)
+		     {
+		       mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
+					  "ERROR: (%s:%d) mvGppValueSet\n\r", __FILE_DESC__, __LINE__);
+		       return(MV_ERROR);
+		     }
 
-	  	status = mvGppTypeSet(gpioGroup, gpioMask, gpioMask/*input*/);
-	  	if (status != MV_OK)
-	  	{
-	  	  mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
-	  			     "ERROR: (%s:%d) mvGppTypeSet\n\r", __FILE_DESC__, __LINE__);
-	  	  return(MV_ERROR);
-	  	}
-	  }
+		     status = mvGppTypeSet(gpioGroup, gpioMask, gpioMask/*input*/);
+		     if (status != MV_OK)
+		     {
+		       mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
+					  "ERROR: (%s:%d) mvGppTypeSet\n\r", __FILE_DESC__, __LINE__);
+		       return(MV_ERROR);
+		     }
+		}
 
-	  /* PHY control register - output status set */
-	  status  = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_1_BEN_IO_EN, ONU_PHY_OUTPUT, 0);
-	  if (status != MV_OK)
-      {
-	    mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
-	  		       "ERROR: (%s:%d) asicOntMiscRegWrite\n\r", __FILE_DESC__, __LINE__);
-	    return(MV_ERROR);
-	  }
-	}
-	/* KW2 ASIC Rev A0 */
-	/* =============== */
-	else if (mvCtrlRevGet() == ONU_ASIC_REV_A0)
-	{
-	  /* PHY control register - output status set */
-	  status  = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_1_BEN_IO_EN, ONU_PHY_OUTPUT, 0);
-	  if (status != MV_OK)
-	  {
-	    mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
-	  		        "ERROR: (%s:%d) asicOntMiscRegWrite\n\r", __FILE_DESC__, __LINE__);
-	    return(MV_ERROR);
-	  }
+		/* PHY control register - output status set */
+		status  = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_1_BEN_IO_EN, ONU_PHY_OUTPUT, 0);
+		if (status != MV_OK)
+		{
+			 mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
+					    "ERROR: (%s:%d) asicOntMiscRegWrite\n\r", __FILE_DESC__, __LINE__);
+			 return(MV_ERROR);
+		}
+	   }
 
-	  /* PHY control register - force disable */
-	  status  = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_1_FORCE_BEN_IO_EN, 0, 0);
-	  if (status != MV_OK)
-	  {
-		mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
-			       "ERROR: (%s:%d) asicOntMiscRegWrite\n\r", __FILE_DESC__, __LINE__);
-		return(MV_ERROR);
-	  }
-	}
+	   /* KW2 ASIC Rev A0 */
+	   /* =============== */
+	   else if (mvCtrlRevGet() == ONU_ASIC_REV_A0)
+	   {
+		/* PHY control register - output status set */
+		status  = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_1_BEN_IO_EN, ONU_PHY_OUTPUT, 0);
+		if (status != MV_OK)
+		{
+		  mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
+				      "ERROR: (%s:%d) asicOntMiscRegWrite\n\r", __FILE_DESC__, __LINE__);
+		  return(MV_ERROR);
+		}
+
+		/* PHY control register - force disable */
+		status  = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_1_FORCE_BEN_IO_EN, 0, 0);
+		if (status != MV_OK)
+		{
+		      mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
+				     "ERROR: (%s:%d) asicOntMiscRegWrite\n\r", __FILE_DESC__, __LINE__);
+		      return(MV_ERROR);
+		}
+	   }
+   }
 
 		if (onuEponDbP2PForceModeGet())
 		{
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuAlloc.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuAlloc.c
index 22c34d8..5442fac 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuAlloc.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuAlloc.c
@@ -162,10 +162,10 @@
 **
 **  OUTPUTS:     MV_U32 *entry
 **
-**  RETURNS:     MV_OK or MV_ERROR
+**  RETURNS:     MV_TRUE or MV_FALSE
 **
 *******************************************************************************/
-MV_STATUS onuGponAllocIdMacAllocExistCheck(MV_U32 allocId, MV_U32 *entry, MV_U32 *tcont)
+MV_BOOL onuGponAllocIdMacAllocExistCheck(MV_U32 allocId, MV_U32 *entry, MV_U32 *tcont)
 {
 	MV_U32  iEntry;
 	MV_U32  macAllocId;
@@ -178,50 +178,15 @@
 		if ((status == MV_OK) && (macAllocId == allocId) && (valid == MV_TRUE)) {
 			*entry = iEntry;
 			*tcont = tcontNum;
-			return (MV_OK);
+			return (MV_TRUE);
 		}
 	}
 
-	return (MV_ERROR);
+	return (MV_FALSE);
 }
 
 /*******************************************************************************
 **
-**  onuGponAllocIdMacAllocCheck
-**  ____________________________________________________________________________
-**
-**  DESCRIPTION: The function checks if BW Map entry is valid and return T-Cont
-**               and Alloc Number
-**
-**  PARAMETERS:  none
-**
-**  OUTPUTS:     MV_U32 *allocId
-**      	 MV_U32 *tcont
-**
-**  RETURNS:     MV_OK or MV_ERROR
-**
-*******************************************************************************/
-MV_STATUS onuGponAllocIdMacAllocCheck(MV_U32 index, MV_U32 *allocId, MV_U32 *tcont)
-{
-	MV_U32  macAllocId;
-	MV_U32  tcontNum;
-	MV_BOOL valid;
-	MV_STATUS status;
-
-       status = mvOnuGponMacRxBwMapGet(index, &macAllocId, &valid, &tcontNum);
-       if ((status == MV_OK) && (valid == MV_TRUE))
-       {
-	       *allocId = macAllocId;
-	       *tcont   = tcontNum;
-	       return (MV_OK);
-       }
-
-       return (MV_ERROR);
-}
-
-
-/*******************************************************************************
-**
 **  onuGponAllocIdMacAllocFreeEntryGet
 **  ____________________________________________________________________________
 **
@@ -231,10 +196,10 @@
 **
 **  OUTPUTS:     MV_U32 *entry
 **
-**  RETURNS:     MV_OK or MV_ERROR
+**  RETURNS:     MV_TRUE or MV_FALSE
 **
 *******************************************************************************/
-MV_STATUS onuGponAllocIdMacAllocFreeEntryGet(MV_U32 *entry)
+MV_BOOL onuGponAllocIdMacAllocFreeEntryGet(MV_U32 *entry)
 {
 	MV_U32  iEntry;
 	MV_U32  macAllocId;
@@ -246,11 +211,11 @@
 		status = mvOnuGponMacRxBwMapGet(iEntry, &macAllocId, &valid, &tcontNum);
 		if ((status == MV_OK) && (valid == MV_FALSE)) {
 			*entry = iEntry;
-			return (MV_OK);
+			return (MV_TRUE);
 		}
 	}
 
-	return (MV_ERROR);
+	return (MV_FALSE);
 }
 
 /*******************************************************************************
@@ -272,9 +237,10 @@
 {
 	MV_STATUS status = MV_OK;
 	MV_U32    entry;
+	MV_BOOL   exist;
 
-	status = onuGponAllocIdMacAllocFreeEntryGet(&entry);
-	if (status != MV_OK)
+	exist = onuGponAllocIdMacAllocFreeEntryGet(&entry);
+	if (exist == MV_FALSE)
 		return (MV_ERROR);
 
 	status = mvOnuGponMacRxBwMapSet(entry, allocId, tcontId, MV_TRUE);
@@ -307,6 +273,7 @@
 MV_STATUS onuGponAllocIdMacConnect(MV_U32 allocId, MV_U32 tcontNum)
 {
 	MV_STATUS status;
+	MV_BOOL   exist;
 	MV_U32    entry;
 	MV_U32    tcont;
 	MV_U32    newIdleTcontEntry;
@@ -321,8 +288,8 @@
 #endif /* MV_GPON_DEBUG_PRINT */
 
 	/* validate that requested alloc-Id is configured in HW and is valid */
-	status = onuGponAllocIdMacAllocExistCheck(allocId, &entry, &tcont);
-	if (status != MV_OK) {
+	exist = onuGponAllocIdMacAllocExistCheck(allocId, &entry, &tcont);
+	if (exist != MV_TRUE) {
 		mvPonPrint(PON_PRINT_ERROR, PON_ALLOC_MODULE,
 					"ERROR: (%s:%d) onuGponAllocIdMacConnect, alloc Id(%d) entry(%d)\n",
 					__FILE_DESC__, __LINE__, allocId, entry);
@@ -334,8 +301,8 @@
 		** should be changed to another Idle T-Cont */
 
 		/* Check for a free (idle) entry in the Tcont DB table */
-		status = onuGponDbBwTcontFreeGet(allocId, &freeTcontEntry);
-		if (status != MV_OK) {
+		exist = onuGponDbBwTcontFreeGet(allocId, &freeTcontEntry);
+		if (exist != MV_TRUE) {
 #ifdef MV_GPON_DEBUG_PRINT
 			mvPonPrint(PON_PRINT_DEBUG, PON_ALLOC_MODULE,
 						"DEBUG: (%s:%d) onuGponAllocIdMacConnect, alloc Id(%d) freeTcontEntry(%d)\n",
@@ -351,8 +318,8 @@
 				onuGponDbBwIdleAllocGet(newIdleTcontEntry, &entryAllocId);
 				if (entryAllocId != PON_ONU_ALLOC_NOT_EXIST) {
 					/* Check if this idle Alloc ID exists in the HW table */
-					status = onuGponAllocIdMacAllocExistCheck(entryAllocId, &bwMapEntry, &tcont);
-					if (status == MV_OK) {
+					exist = onuGponAllocIdMacAllocExistCheck(entryAllocId, &bwMapEntry, &tcont);
+					if (exist == MV_TRUE) {
 						/* re-assign this Alloc ID to the new idle Tcont in HW */
 						status = mvOnuGponMacRxBwMapSet(bwMapEntry, entryAllocId, onuIdleAllocTcont, MV_TRUE);
 						if (status != MV_OK) {
@@ -378,11 +345,19 @@
 
 #ifdef MV_GPON_DEBUG_PRINT
 	mvPonPrint(PON_PRINT_DEBUG, PON_ALLOC_MODULE,
-		   "DEBUG: (%s:%d) TCONT (%d) active, Connect\n",
-		   __FILE_DESC__, __LINE__, tcontNum);
+              "DEBUG: (%s:%d) AllocIdMacConnect, tpm_active_tcont, Tcont(%d)\n",
+              __FILE_DESC__, __LINE__, tcontNum);
 #endif /* MV_GPON_DEBUG_PRINT */
 
-	onuGponWqTcontActivate(tcontNum);
+	printk("TCONT (%d) active, Connect\n", tcontNum);
+	status = onuGponWqTcontActivate(tcontNum);
+	if (status != MV_OK)
+	{
+		mvPonPrint(PON_PRINT_ERROR, PON_ALLOC_MODULE,
+			   "ERROR: (%s:%d) onuGponAllocIdMacAdd, failed to schedule T-Cont(%d) activate in WQ\n",
+			   __FILE_DESC__, __LINE__, tcontNum);
+		return (status);
+	}
 
 	return (status);
 }
@@ -404,6 +379,7 @@
 *******************************************************************************/
 MV_STATUS onuGponAllocIdMacReconnect(MV_U32 allocId, MV_U32 tcontNum)
 {
+	MV_BOOL   exist;
 	MV_STATUS status = MV_OK;
 	MV_U32    entry;
 	MV_U32    onuId;
@@ -417,12 +393,12 @@
 #endif /* MV_GPON_DEBUG_PRINT */
 
 	/* Get free entry */
-	status = onuGponAllocIdMacAllocExistCheck(allocId, &entry, &tcont);
-	if (status == MV_OK)
-		return(status);
+	exist = onuGponAllocIdMacAllocExistCheck(allocId, &entry, &tcont);
+	if (exist != MV_TRUE)
+		return (MV_ERROR);
 
-	status = onuGponDbBwTcontFreeGet(allocId, &tcontFreeNum);
-	if (status != MV_OK)
+	exist = onuGponDbBwTcontFreeGet(allocId, &tcontFreeNum);
+	if (exist != MV_TRUE)
 		onuIdleAllocTcont = tcontFreeNum;
 
 	/* Check if default Alloc Id */
@@ -435,6 +411,15 @@
                       __FILE_DESC__, __LINE__, tcontNum);
 #endif /* MV_GPON_DEBUG_PRINT */
 
+	       // printk("TCONT (%d) flush, Reconnect\n", tcontNum);
+	       // status = onuGponWqTcontFlush(tcontNum);
+	       // if (status != MV_OK)
+	       //{
+	       // 	mvPonPrint(PON_PRINT_ERROR, PON_ALLOC_MODULE,
+	       // 		   "ERROR: (%s:%d) onuGponAllocIdMacReconnect, failed to schedule T-Cont(%d) flush in WQ\n",
+	       // 		   __FILE_DESC__, __LINE__, tcontNum);
+	       //}
+
 		/* Set Alloc Id to T-Cont in the GMAC hardware */
 		status = mvOnuGponMacRxBwMapSet(entry, allocId, onuIdleAllocTcont, MV_TRUE);
 	}
@@ -442,51 +427,6 @@
 	return (status);
 }
 
-
-/*******************************************************************************
-**
-**  onuGponAllocIdMacReActivate
-**  ____________________________________________________________________________
-**
-**  DESCRIPTION: The function reactivate onu tcont incase the tcont is not
-**               clear and set by mng
-**
-**  PARAMETERS:  None
-**
-**  OUTPUTS:     None
-**
-**  RETURNS:     MV_OK or error
-**
-*******************************************************************************/
-MV_STATUS onuGponAllocIdMacReActivate(void)
-{
-	MV_STATUS status;
-	MV_U32    entry;
-	MV_U32	  allocId;
-	MV_U32    tcont;
-
-#ifdef MV_GPON_DEBUG_PRINT
-	mvPonPrint(PON_PRINT_DEBUG, PON_ALLOC_MODULE,
-              "DEBUG: (%s:%d) onuGponAllocIdMacReActivate\n", __FILE_DESC__, __LINE__);
-#endif /* MV_GPON_DEBUG_PRINT */
-
-	for (entry = 0; entry < ONU_GPON_MAX_NUM_OF_T_CONTS; entry++)
-	{
-		status = onuGponAllocIdMacAllocCheck(entry, &allocId, &tcont);
-		if (status == MV_OK)
-		{
-			mvPonPrint(PON_PRINT_DEBUG, PON_ALLOC_MODULE,
-				   "DEBUG: (%s:%d) TCONT (%d) active, Connect\n",
-				   __FILE_DESC__, __LINE__, tcont);
-
-			onuGponWqTcontActivate(tcont);
-		}
-	}
-
-
-	return (MV_OK);
-}
-
 /*******************************************************************************
 **
 **  onuGponAllocIdMacDisconnect
@@ -506,6 +446,7 @@
 	MV_U32  onuId;
 	MV_U32  entry;
 	MV_U32  tcont;
+	MV_BOOL exist;
 	MV_STATUS status = MV_OK;
 
 #ifdef MV_GPON_DEBUG_PRINT
@@ -515,21 +456,28 @@
 #endif /* MV_GPON_DEBUG_PRINT */
 
 	/* Check if already exist */
-	status = onuGponAllocIdMacAllocExistCheck(allocId, &entry, &tcont);
-	if (status != MV_OK)
-		return(MV_ERROR);
+	exist = onuGponAllocIdMacAllocExistCheck(allocId, &entry, &tcont);
+	if (exist == MV_FALSE)
+		return (MV_ERROR);
 
 	/* Check if default Alloc Id */
 	onuId = onuGponDbOnuIdGet();
 	if (allocId != onuId)
 	{
 #ifdef MV_GPON_DEBUG_PRINT
-		mvPonPrint(PON_PRINT_DEBUG, PON_ALLOC_MODULE,
-			   "DEBUG: (%s:%d) TCONT (%d) flush, Disconnect\n",
-			   __FILE_DESC__, __LINE__, tcont);
+	        mvPonPrint(PON_PRINT_DEBUG, PON_ALLOC_MODULE,
+                      "DEBUG: (%s:%d) tpm_deactive_tcont, Tcont(%d)\n",
+                      __FILE_DESC__, __LINE__, tcont);
 #endif /* MV_GPON_DEBUG_PRINT */
 
-		onuGponWqTcontFlush(tcont);
+		printk("TCONT (%d) flush, Disconnect\n", tcont);
+		status = onuGponWqTcontFlush(tcont);
+		if (status != MV_OK)
+		{
+			mvPonPrint(PON_PRINT_ERROR, PON_ALLOC_MODULE,
+				   "ERROR: (%s:%d) onuGponAllocIdMacDisconnect, failed to schedule T-Cont(%d) flush in WQ\n",
+				   __FILE_DESC__, __LINE__, tcont);
+		}
 
 		status = mvOnuGponMacRxBwMapSet(entry, 0, 0, MV_FALSE);
 		if (status != MV_OK)
@@ -612,7 +560,7 @@
 	/* check if Alloc-Id exist in the alloc Id DB table */
 	allocIdExist = onuGponDbBwAllocExist(allocId);
 	if (allocIdExist == MV_TRUE)
-		return (MV_NO_CHANGE/*MV_OK*/);
+		return (MV_OK);
 
 	/* set the Alloc-Id exist to the database */
 	status = onuGponDbBwAllocInsert(allocId, &entry);
@@ -766,15 +714,21 @@
 *******************************************************************************/
 MV_STATUS onuGponAllocIdFreeAllBuffers(void)
 {
-    MV_STATUS status = MV_OK;
+    MV_STATUS status;
     MV_U32    tcontNum = 0xFF;
-
 #ifdef MV_GPON_DEBUG_PRINT
     mvPonPrint(PON_PRINT_DEBUG, PON_ALLOC_MODULE,
-	       "DEBUG: (%s:%d) FreeAllBuffers\n", __FILE_DESC__, __LINE__);
+               "DEBUG: (%s:%d) release All T-Conts buffers, tpm_deactive_tcont\n", __FILE_DESC__, __LINE__);
 #endif /* MV_GPON_DEBUG_PRINT */
 
-    onuGponWqTcontFlush(tcontNum);
+    printk("TCONT flush, FreeAllBuffers\n");
+    status = onuGponWqTcontFlush(tcontNum);
+    if (status != MV_OK)
+    {
+            mvPonPrint(PON_PRINT_ERROR, PON_ALLOC_MODULE,
+        	       "ERROR: (%s:%d) onuGponAllocIdFreeAllBuffers, failed to schedule All T-Cont flush in WQ\n",
+        	       __FILE_DESC__, __LINE__, tcontNum);
+    }
 
     return(status);
 }
@@ -1007,14 +961,14 @@
 *******************************************************************************/
 MV_STATUS onuGponAllocIdTcontClearAll(void)
 {
-	MV_U32    iEntry;
-	MV_BOOL   exist;
-	MV_STATUS status = MV_OK;
+	MV_U32  iEntry;
+	MV_BOOL exist;
+	MV_STATUS status;
 	MV_U32    tcontNum = 0xFF;
 
 #ifdef MV_GPON_DEBUG_PRINT
 	mvPonPrint(PON_PRINT_DEBUG, PON_ALLOC_MODULE,
-		   "DEBUG: (%s:%d) TCONT flush, Clear All\n", __FILE_DESC__, __LINE__);
+				"DEBUG: (%s:%d) Clear All Alloc-Ids from T-TCONTs\n", __FILE_DESC__, __LINE__);
 #endif /* MV_GPON_DEBUG_PRINT */
 
 	for (iEntry = 0 ; iEntry < ONU_GPON_MAX_NUM_OF_T_CONTS ; iEntry++) {
@@ -1024,7 +978,15 @@
 			onuGponAllocIdTcontClear(iEntry);
 	}
 
-	onuGponWqTcontFlush(tcontNum);
+	printk("TCONT flush, Clear All\n");
+	status = onuGponWqTcontFlush(tcontNum);
+	if (status != MV_OK)
+	{
+	 	mvPonPrint(PON_PRINT_ERROR, PON_ALLOC_MODULE,
+	 		   "ERROR: (%s:%d) onuGponAllocIdTcontClearAll, failed to schedule All T-Cont flush in WQ\n",
+	 		   __FILE_DESC__, __LINE__);
+	}
+
 
 	return (MV_OK);
 }
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuAlloc.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuAlloc.h
index 5ca2db5..14d2f5c 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuAlloc.h
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuAlloc.h
@@ -105,7 +105,6 @@
 MV_STATUS onuGponAllocIdDeAssignAll(void);

 MV_STATUS onuGponAllocIdFreeAllBuffers(void);

 MV_STATUS onuGponAllocIdTcontClearAll(void);

-MV_STATUS onuGponAllocIdMacReActivate(void);

 MV_U32    onuGponIdleAllocIdTcontGet(void);

 

 /* Macros

diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuDb.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuDb.c
index 52c6ddc..391bc35 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuDb.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuDb.c
@@ -3254,10 +3254,10 @@
 **
 **  OUTPUTS:     None
 **
-**  RETURNS:     MV_OK if there is a free entry, MV_ERROR otherwise
+**  RETURNS:     MV_TRUE if there is a free entry, MV_FALSE otherwise
 **
 *******************************************************************************/
-MV_STATUS onuGponDbBwTcontFreeGet(MV_U32 allocId, MV_U32 *tcontNum)
+MV_BOOL onuGponDbBwTcontFreeGet(MV_U32 allocId, MV_U32 *tcontNum)
 {
 	MV_BOOL exist;
 	MV_U32  dbAllocId;
@@ -3270,7 +3270,7 @@
 		onuGponDbBwTcontGet(iTcont, &exist, &dbAllocId, &dbValid);
 		if ((exist == MV_TRUE) && (dbValid == MV_FALSE)) {
 			*tcontNum = iTcont;
-			return(MV_OK);
+			return(MV_TRUE);
 		}
 
 		if (dbAllocId == allocId)
@@ -3279,7 +3279,7 @@
 
 	*tcontNum = tempTcont;
 
-	return(MV_ERROR);
+	return(MV_FALSE);
 }
 
 /*******************************************************************************
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuDb.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuDb.h
index 59deb72..535b6ea 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuDb.h
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuDb.h
@@ -392,7 +392,7 @@
 MV_STATUS onuGponDbBwTcontClear(MV_U32 tcontNum);
 MV_STATUS onuGponDbBwTcontAlloc(MV_U32 tcontNum, MV_U32 allocId);
 MV_STATUS onuGponDbBwTcontConnectCheck(MV_U32 allocId, MV_U32 *tcontNum, MV_BOOL *valid);
-MV_STATUS onuGponDbBwTcontFreeGet(MV_U32 allocId, MV_U32 *tcontNum);
+MV_BOOL   onuGponDbBwTcontFreeGet(MV_U32 allocId, MV_U32 *tcontNum);
 
 MV_STATUS onuGponDbBwMapCntDumpSet(MV_U32 enable);
 MV_BOOL   onuGponDbBwMapCntDumpGet(void);
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuIsr.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuIsr.h
index 49f5c92..11ee96b 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuIsr.h
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuIsr.h
@@ -113,7 +113,8 @@
                                                

 #define ON_GPON_MAX_ALARMS                      (3)

 

-#define ONU_GPON_INT_ALARMS                     (ONU_GPON_LOF_ALARM_MASK)

+#define ONU_GPON_INT_ALARMS                     (ONU_GPON_LOF_ALARM_MASK |\

+                                                 ONU_GPON_LCDG_ALARM_MASK)

 

 #define ONU_GPON_PARITY_ERROR                   (ONU_GPON_RAM_TEST_GRX_PARITY_ERR_MASK  |\

                                                  ONU_GPON_RAM_TEST_GEM_PARITY_ERR_MASK  |\

diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuMngr.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuMngr.c
index ec1957c..139e964 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuMngr.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuMngr.c
@@ -364,7 +364,7 @@
   MV_U32          onuState;

   GPONFUNCPTR     ptrFunc;

   S_OnuGponGenTbl *onuGponGenTbl_p = &(onuGponDb_s.onuGponGenTbl_s);

-  MV_U32          *msgData_p = (MV_U32*)msgData;

+  MV_U32          *msgData_p = msgData;

 

   /* get onu Id */

   appOnuId = onuGponDbOnuIdGet();

@@ -388,11 +388,11 @@
       }

       else

       {

-	onuGponSyncLog(ONU_GPON_LOG_MSG, onuId, msgId, 0);

-	onuGponSyncLog(ONU_GPON_LOG_MSG_CONTENT, *(msgData_p), *(msgData_p + 1), *(msgData_p + 2));

         (*ptrFunc)(onuId, msgId, msgData);

       }           

 

+      onuGponSyncLog(ONU_GPON_LOG_MSG, onuId, msgId, 0);

+      onuGponSyncLog(ONU_GPON_LOG_MSG_CONTENT, *(msgData_p), *(msgData_p + 1), *(msgData_p + 2));

     }

     /* handle invalid messages */

     else

diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuMngrStateMachine.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuMngrStateMachine.c
index 2686826..f645b34 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuMngrStateMachine.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuMngrStateMachine.c
@@ -84,12 +84,6 @@
 ------------------------------------------------------------------------------*/
 #define __FILE_DESC__ "mv_pon/core/gpon/gponOnuStateMachine.c"
 
-#define INDICATION_BIT_MAP_NONE                 (0x00000000)
-#define INDICATION_BIT_MAP_FIBER_EVENT          (0x00000001)
-#define INDICATION_BIT_MAP_FORCE_TCONT_CLEAR    (0x00000002)
-#define INDICATION_BIT_MAP_FORCE_TCONT_RECONFIG (0x00000004)
-#define INDICATION_BIT_MAP_RESERVED             (0x00000008)
-
 /* Global Variables
 ------------------------------------------------------------------------------*/
 extern spinlock_t onuPonIrqLock;
@@ -107,9 +101,9 @@
 /* Local Functions
 ------------------------------------------------------------------------------*/
 MV_STATUS onuGponPonMngrUpdateState(MV_U32 newState);
-MV_STATUS onuGponPonMngClearOnuInfo(MV_U32 indicationBitMap);
-MV_STATUS onuGponPonMngClearOnuTconts(MV_U32 indicationBitMap);
-MV_STATUS onuGponPonMngClearOnuBuffers(MV_U32 indicationBitMap);
+MV_STATUS onuGponPonMngClearOnuInfo(void);
+MV_STATUS onuGponPonMngClearOnuBuffers(void);
+MV_STATUS onuGponPonMngClearOnuTconts(void);
 MV_STATUS onuGponPonMngClearOnuPorts(void);
 MV_STATUS onuGponPonMngClearOnuId(void);
 MV_STATUS onuGponPonMngClearOnuDelay(void);
@@ -253,9 +247,9 @@
 **  RETURNS:     MV_OK or error
 **
 *******************************************************************************/
-MV_STATUS onuGponPonMngClearOnuBuffers(MV_U32 indicationBitMap)
+MV_STATUS onuGponPonMngClearOnuBuffers(void)
 {
-   MV_STATUS rcode = MV_OK;
+   MV_STATUS rcode;
 
    rcode = onuGponAllocIdFreeAllBuffers();
    if (rcode != MV_OK)
@@ -264,19 +258,6 @@
    	     "ERROR: (%s:%d) onuGponPonMngClearOnuBuffers\n", __FILE_DESC__, __LINE__);
    }
 
-
-   if ((onuGponDbTcontResetGet() != MV_TRUE) &&
-       (indicationBitMap & INDICATION_BIT_MAP_FIBER_EVENT))
-   {
-	 rcode = onuGponAllocIdMacReActivate();
-	 if (rcode != MV_OK)
-	 {
-	   mvPonPrint(PON_PRINT_ERROR, PON_SM_MODULE,
-		      "ERROR: (%s:%d) onuGponPonMngClearOnuTconts\n", __FILE_DESC__, __LINE__);
-	   return(rcode);
-	 }
-   }
-
    return(MV_OK);
 }
 
@@ -295,7 +276,7 @@
 **  RETURNS:     MV_OK or error
 **
 *******************************************************************************/
-MV_STATUS onuGponPonMngClearOnuInfo(MV_U32 indicationBitMap)
+MV_STATUS onuGponPonMngClearOnuInfo(void)
 {
   MV_STATUS rcode;
 
@@ -303,32 +284,29 @@
   if (rcode != MV_OK)
   {
     mvPonPrint(PON_PRINT_ERROR, PON_SM_MODULE,
-               "ERROR: (%s:%d) onuGponPonMngClearOnuPorts\n", __FILE_DESC__, __LINE__);
-    return(rcode);
+               "ERROR: (%s:%d) onuGponPonMngClearOnuPorts\n", __FILE_DESC__, __LINE__);    return(rcode);
   }
 
-  rcode = onuGponPonMngClearOnuTconts(indicationBitMap);
+  rcode = onuGponPonMngClearOnuTconts();
   if (rcode != MV_OK)
   {
     mvPonPrint(PON_PRINT_ERROR, PON_SM_MODULE,
-               "ERROR: (%s:%d) onuGponPonMngClearOnuTconts\n", __FILE_DESC__, __LINE__);
-    return(rcode);
+               "ERROR: (%s:%d) onuGponPonMngClearOnuTconts\n", __FILE_DESC__, __LINE__);    return(rcode);
   }
 
-  rcode = onuGponPonMngClearOnuBuffers(indicationBitMap);
+  rcode = onuGponPonMngClearOnuBuffers();
   if (rcode != MV_OK)
   {
     mvPonPrint(PON_PRINT_ERROR, PON_SM_MODULE,
                "ERROR: (%s:%d) onuGponPonMngClearOnuBuffers\n", __FILE_DESC__, __LINE__);
-    return(rcode);
+    return;
   }
 
   rcode = onuGponPonMngClearOnuId();
   if (rcode != MV_OK)
   {
     mvPonPrint(PON_PRINT_ERROR, PON_SM_MODULE,
-               "ERROR: (%s:%d) onuGponPonMngClearOnuId\n", __FILE_DESC__, __LINE__);
-    return(rcode);
+               "ERROR: (%s:%d) onuGponPonMngClearOnuId\n", __FILE_DESC__, __LINE__);    return(rcode);
   }
 
   rcode = onuGponPonMngClearOnuDelay();
@@ -409,20 +387,18 @@
 **  RETURNS:     MV_OK or error
 **
 *******************************************************************************/
-MV_STATUS onuGponPonMngClearOnuTconts(MV_U32 indicationBitMap)
+MV_STATUS onuGponPonMngClearOnuTconts(void)
 {
   MV_STATUS rcode = MV_OK;
 
-  if ((onuGponDbTcontResetGet() == MV_TRUE) ||
-      (indicationBitMap & INDICATION_BIT_MAP_FORCE_TCONT_CLEAR))
-  {
-	rcode = onuGponAllocIdDeAssignAll();
-	if (rcode != MV_OK)
-	{
-	  mvPonPrint(PON_PRINT_ERROR, PON_SM_MODULE,
-		     "ERROR: (%s:%d) onuGponPonMngClearOnuTconts\n", __FILE_DESC__, __LINE__);
-	  return(rcode);
-	}
+  if (onuGponDbTcontResetGet()==MV_TRUE) {
+      rcode = onuGponAllocIdDeAssignAll();
+      if (rcode != MV_OK)
+      {
+        mvPonPrint(PON_PRINT_ERROR, PON_SM_MODULE,
+                   "ERROR: (%s:%d) onuGponAllocIdDeAssignAll\n", __FILE_DESC__, __LINE__);
+        return(rcode);
+      }
   }
 
   return(MV_OK);
@@ -1012,21 +988,14 @@
 
     /* set Default allocId according to the onuId */
     rcode = onuGponAllocIdAssign(msgOnuId, 0);
-    if ((rcode != MV_OK) && (rcode != MV_NO_CHANGE))
+    if (rcode != MV_OK)
     {
-	    mvPonPrint(PON_PRINT_ERROR, PON_SM_MODULE,
-		       "ERROR: (%s:%d) onuGponAllocIdAssign\n", __FILE_DESC__, __LINE__);
-		       return;
+      mvPonPrint(PON_PRINT_ERROR, PON_SM_MODULE,
+                 "ERROR: (%s:%d) onuGponAllocIdAssign\n", __FILE_DESC__, __LINE__);
+      return;
     }
-    else if ((rcode != MV_OK) && (rcode == MV_NO_CHANGE))
-    {
-	    mvPonPrint(PON_PRINT_ERROR, PON_SM_MODULE,
-		       "ERROR: (%s:%d) onuGponAllocIdAssign, Alloc Id exist\n", __FILE_DESC__, __LINE__);
-    }
-    else /* MV_OK */
-    {
-	    onuGponAllocIdMacAdd(msgOnuId, PON_ONU_DEFAULT_ALLOC_TCONT);
-    }
+
+    onuGponAllocIdMacAdd(msgOnuId, PON_ONU_DEFAULT_ALLOC_TCONT);
 
     /* Set delay received from OLT */
     equalizationDelayTemp = onuGponDbEqualizationDelayGet();
@@ -1294,12 +1263,12 @@
 		 __FILE_DESC__, __LINE__, currFinalDelay);
 
       /* Check if can change the TX Final Delay only */
-      if (changeDelay + currFinalDelay <= GPON_TX_FINAL_DELAY_MAX)
+      if (changeDelay + currFinalDelay > GPON_TX_FINAL_DELAY_MAX)
       {
         finalDelay = currFinalDelay + changeDelay;
 
 	mvPonPrint(PON_PRINT_DEBUG, PON_SM_STATE_MODULE,
-		   "Range Debug Info: (%s:%d), State O5, Change delay + current Final delay <= MAX Final delay(0x3F), updated final delay, finalDelay = currFinalDelay + changeDelay, (0x%x)\n",
+		   "Range Debug Info: (%s:%d), State O5, Change delay + current Final delay > MAX Final delay(0x3F), updated final delay, finalDelay = currFinalDelay + changeDelay, (0x%x)\n",
 		   __FILE_DESC__, __LINE__, finalDelay);
       }
       else
@@ -1310,7 +1279,7 @@
 #endif /* MV_GPON_DEBUG_PRINT */
 
 	mvPonPrint(PON_PRINT_DEBUG, PON_SM_STATE_MODULE,
-		   "Range Debug Info: (%s:%d), State O5, Change delay + Final delay > MAX Final delay = 0x3F, updated equalization delay\n",
+		   "Range Debug Info: (%s:%d), State O5, Change delay + Final delay < MAX Final delay = 0x3F, updated equalization delay\n",
 		   __FILE_DESC__, __LINE__);
 
         /* calc delay */
@@ -1403,7 +1372,7 @@
 
   /* clear onu information */
   /* ===================== */
-  rcode = onuGponPonMngClearOnuInfo(INDICATION_BIT_MAP_FORCE_TCONT_CLEAR);
+  rcode = onuGponPonMngClearOnuInfo();
   if (rcode != MV_OK)
   {
     mvPonPrint(PON_PRINT_ERROR, PON_SM_MODULE,
@@ -1499,7 +1468,7 @@
 
 		/* clear onu information */
 		/* ===================== */
-		rcode = onuGponPonMngClearOnuInfo(INDICATION_BIT_MAP_FORCE_TCONT_CLEAR);
+		rcode = onuGponPonMngClearOnuInfo();
 		if (rcode != MV_OK) {
 			mvPonPrint(PON_PRINT_ERROR, PON_SM_MODULE,
 					   "ERROR: (%s:%d) DISABLE: onuGponPonMngClearOnuInfo\n", __FILE_DESC__, __LINE__);
@@ -1871,7 +1840,7 @@
     if (newAllocStatus == MV_TRUE)
     {
       status = onuGponAllocIdAssign(allocId, 1);
-      if ((status != MV_OK) && (status != MV_NO_CHANGE))
+      if (status != MV_OK)
       {
         mvPonPrint(PON_PRINT_ERROR, PON_SM_MODULE,
                    "ERROR: (%s:%d) onuGponAllocIdAssign onuId(%d), allocId(%d)\n", __FILE_DESC__, __LINE__, onuId, allocId);
@@ -2662,7 +2631,7 @@
 
   /* clear onu information */
   /* ===================== */
-  rcode = onuGponPonMngClearOnuInfo(INDICATION_BIT_MAP_FIBER_EVENT);
+  rcode = onuGponPonMngClearOnuInfo();
   if (rcode != MV_OK)
   {
     mvPonPrint(PON_PRINT_ERROR, PON_SM_MODULE,
@@ -2735,7 +2704,7 @@
 
   /* clear onu information */
   /* ===================== */
-  rcode = onuGponPonMngClearOnuInfo(INDICATION_BIT_MAP_FIBER_EVENT);
+  rcode = onuGponPonMngClearOnuInfo();
   if (rcode != MV_OK)
   {
     mvPonPrint(PON_PRINT_ERROR, PON_SM_MODULE,
@@ -2919,7 +2888,7 @@
 
     /* clear onu information */
     /* ===================== */
-    rcode = onuGponPonMngClearOnuInfo(INDICATION_BIT_MAP_FIBER_EVENT);
+    rcode = onuGponPonMngClearOnuInfo();
     if (rcode != MV_OK)
     {
       mvPonPrint(PON_PRINT_ERROR, PON_SM_MODULE,
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuSrvc.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuSrvc.c
index 5b3c454..044f589 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuSrvc.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuSrvc.c
@@ -1221,7 +1221,7 @@
             break;

 

         case ONU_GPON_LOG_MSG_CONTENT:

-            printk("[%03d] PLOAM message, data=0x%08x%08x%08x [030201000706050411100908]\n", printEntry,

+            printk("[%03d] PLOAM message, data=0x%08x%08x%08x\n", printEntry,

                     onuGponLogDb[printEntry].dataVal1,

                     onuGponLogDb[printEntry].dataVal2,

                     onuGponLogDb[printEntry].dataVal3);

@@ -1232,7 +1232,6 @@
                     onuGponLogDb[printEntry].state,

                     onuGponLogDb[printEntry].time,

                     onuGponLogDb[printEntry].dataVal1);

-            printk("==========================================================\n");

             break;

 

         case ONU_GPON_LOG_INTERRUPT_XVR_SD:

@@ -1256,7 +1255,7 @@
             break;

 

         case ONU_GPON_LOG_INTERRUPT_SERDES_STOP:

-            printk("[%03d] SERDES Stop, state=%d, time=%08x\n", printEntry,

+            printk("[%03d] Interrupt, state=%d, time=%08x\n", printEntry,

                     onuGponLogDb[printEntry].state,

                     onuGponLogDb[printEntry].time);

             break;

@@ -1267,14 +1266,6 @@
                     onuGponLogDb[printEntry].time);

             break;

 

-        case ONU_GPON_LOG_FREE_TEXT:

-            printk("[%03d] Free, time=%08x %d %d %d\n", printEntry,

-                   onuGponLogDb[printEntry].time,

-		   onuGponLogDb[printEntry].dataVal1,

-		   onuGponLogDb[printEntry].dataVal2,

-		   onuGponLogDb[printEntry].dataVal3);

-            break;

-

         default:

             printk("[%03d] Other\r\n", printEntry);

             break;

@@ -1310,10 +1301,15 @@
 *******************************************************************************/

 MV_STATUS onuGponWqTcontFlush(MV_U32 tcont)

 {

+    MV_STATUS rcode;

+

     if (tcont != 0xFF)

-	queue_work(gponTcontFlushWq.ponWq, (struct work_struct *)&gponTcontCleanWork[tcont]);

+	rcode = queue_work(gponTcontFlushWq.ponWq, (struct work_struct *)&gponTcontCleanWork[tcont]);

     else

-        queue_work(gponTcontFlushWq.ponWq, (struct work_struct *)&gponTcontCleanAllWork);

+        rcode = queue_work(gponTcontFlushWq.ponWq, (struct work_struct *)&gponTcontCleanAllWork);

+

+    if(rcode == 0)

+	return(MV_ERROR);

 

     return(MV_OK);

 }

@@ -1335,7 +1331,11 @@
 *******************************************************************************/

 MV_STATUS onuGponWqTcontActivate(MV_U32 tcont)

 {

-    queue_work(gponTcontFlushWq.ponWq, (struct work_struct *)&gponTcontActiveWork[tcont]);

+    MV_STATUS rcode;

+

+    rcode = queue_work(gponTcontFlushWq.ponWq, (struct work_struct *)&gponTcontActiveWork[tcont]);

+    if(rcode == 0)

+        return(MV_ERROR);

 

     return(MV_OK);

 }

@@ -1371,17 +1371,14 @@
     {

 	if (onuGponTcontFlushState[tcont] == TCONT_FLUSH_RUNNING_STATE)

 	{

-		mvPonPrint(PON_PRINT_DEBUG, PON_ALLOC_MODULE,

-			   "DEBUG: (%s:%d) TCONT (%d) CLEAN_EVENT\n", __FILE_DESC__, __LINE__, tcont);

-		onuGponTcontFlushState[tcont] = TCONT_FLUSH_BLOCKING_STATE;

-		tpm_deactive_tcont(tcont);

-		onuGponTcontFlushState[tcont] = TCONT_FLUSH_READY_STATE;

+	   onuGponTcontFlushState[tcont] = TCONT_FLUSH_BLOCKING_STATE;

+	   tpm_deactive_tcont(tcont);

+	   printk("TCONT (%d) CLEAN_EVENT\n", tcont);

+	   onuGponTcontFlushState[tcont] = TCONT_FLUSH_READY_STATE;

 	}

 	else

 	{

-		mvPonPrint(PON_PRINT_DEBUG, PON_ALLOC_MODULE,

-			   "DEBUG: (%s:%d) Received TCONT_CLEAN_EVENT while in T-CONT(%d) state(%s)\n",

-			   __FILE_DESC__, __LINE__, tcont, stateText[onuGponTcontFlushState[tcont]]);

+	   printk("Received TCONT_CLEAN_EVENT while in T-CONT(%d) state(%s)\n", tcont, stateText[onuGponTcontFlushState[tcont]]);

 	}

     }

 

@@ -1393,17 +1390,14 @@
 	{

 	    if (onuGponTcontFlushState[tcont] == TCONT_FLUSH_RUNNING_STATE)

 	    {

-		    mvPonPrint(PON_PRINT_DEBUG, PON_ALLOC_MODULE,

-			       "DEBUG: (%s:%d) TCONT (%d) CLEAN_EVENT\n", __FILE_DESC__, __LINE__, tcont);

-		    onuGponTcontFlushState[tcont] = TCONT_FLUSH_BLOCKING_STATE;

-		    tpm_deactive_tcont(tcont);

-		    onuGponTcontFlushState[tcont] = TCONT_FLUSH_READY_STATE;

+	       onuGponTcontFlushState[tcont] = TCONT_FLUSH_BLOCKING_STATE;

+	       tpm_deactive_tcont(tcont);

+	       printk("TCONT (%d) CLEAN_EVENT\n", tcont);

+	       onuGponTcontFlushState[tcont] = TCONT_FLUSH_READY_STATE;

 	    }

 	    else

 	    {

-		    mvPonPrint(PON_PRINT_DEBUG, PON_ALLOC_MODULE,

-			       "DEBUG: (%s:%d) Received TCONT_CLEAN_EVENT while in T-CONT(%d) state(%s)\n",

-			       __FILE_DESC__, __LINE__, tcont, stateText[onuGponTcontFlushState[tcont]]);

+	       printk("Received TCONT_CLEAN_EVENT while in T-CONT(%d) state(%s)\n", tcont, stateText[onuGponTcontFlushState[tcont]]);

 	    }

 	}

     }

@@ -1414,16 +1408,13 @@
     {

        if (onuGponTcontFlushState[tcont] == TCONT_FLUSH_READY_STATE)

        {

-	       mvPonPrint(PON_PRINT_DEBUG, PON_ALLOC_MODULE,

-	            "DEBUG: (%s:%d) TCONT (%d) ACTIVE_EVENT\n", __FILE_DESC__, __LINE__, tcont);

-	       onuGponTcontFlushState[tcont] = TCONT_FLUSH_RUNNING_STATE;

-	       tpm_active_tcont(tcont);

+          onuGponTcontFlushState[tcont] = TCONT_FLUSH_RUNNING_STATE;

+          tpm_active_tcont(tcont);

+          printk("TCONT (%d) ACTIVE_EVENT\n", tcont);

        }

        else

        {

-	       mvPonPrint(PON_PRINT_DEBUG, PON_ALLOC_MODULE,

-			  "DEBUG: (%s:%d) Received TCONT_ACTIVE_EVENT while in T-CONT state(%s)\n",

-			  __FILE_DESC__, __LINE__, stateText[onuGponTcontFlushState[tcont]]);

+           printk("Received TCONT_ACTIVE_EVENT while in T-CONT state(%s)\n", stateText[onuGponTcontFlushState[tcont]]);

        }

     }

 

diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuSrvc.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuSrvc.h
index 5d84b13..ed08b2d 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuSrvc.h
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuSrvc.h
@@ -89,7 +89,6 @@
 #define ONU_GPON_LOG_STATE			(6)

 #define ONU_GPON_LOG_MSG			(7)

 #define ONU_GPON_LOG_MSG_CONTENT		(8)

-#define ONU_GPON_LOG_FREE_TEXT			(9)

 

 #define ONU_GPON_LOG_SIZE                       (512)

 

diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/perf/epon/eponOnuLnxKsMI.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/perf/epon/eponOnuLnxKsMI.c
index cb527e0..f185fd4 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/perf/epon/eponOnuLnxKsMI.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/perf/epon/eponOnuLnxKsMI.c
@@ -89,8 +89,6 @@
 
 /* Local Variables
 ------------------------------------------------------------------------------*/
-static   S_EponIoctlTdmQueue ioctlTdmQueue;
-static   S_EponIoctlDba      ioctlDba;
 
 /* Export Functions
 ------------------------------------------------------------------------------*/
@@ -477,7 +475,9 @@
   onuEponDbOnuHoldoverStateSet(ioctlHoldover->holdoverState);
   onuEponDbOnuHoldoverTimeSet(ioctlHoldover->holdoverTime);
 
-  onuEponIsrTimerHoldoverIntervalSet(ioctlHoldover->holdoverTime, 0);
+  /* Add the newly configured holdover time with OpticalLosTime */
+  onuEponIsrTimerHoldoverIntervalSet(onuEponDbOnuHoldoverTimeGet() + 
+                                     onuEponDbOnuOpticalLosTimeGet(), 0);
 
   mvPonPrint(PON_PRINT_INFO, PON_API_MODULE,
 			 "DEBUG: Holdover config, state(%d), time(%d))\n",
@@ -512,6 +512,132 @@
 
 /*******************************************************************************
 **
+**  onuEponMiLosTimeConfig
+**  ____________________________________________________________________________
+**
+**  DESCRIPTION: The function sets the time delay for entering holdover status
+**
+**  PARAMETERS:  *ioctlLosTime - time delay value
+**
+**  OUTPUTS:     None
+**
+**  RETURNS:     None
+**
+*******************************************************************************/
+MV_STATUS onuEponMiLosTimeConfig(S_EponIoctlLosTime *ioctlLosTime)
+{
+    MV_STATUS status = MV_OK;
+
+    MV_U16 opticalLosTime;
+    MV_U16 macLosTime;
+
+    opticalLosTime = ioctlLosTime->optical_los_time;
+    macLosTime     = ioctlLosTime->mac_los_time;
+
+    status = mvEponApiLosTimeConfig(opticalLosTime, macLosTime);
+
+    return status;
+}
+
+/*******************************************************************************
+**
+**  onuEponMiTxPowerControlConfig
+**  ____________________________________________________________________________
+**
+**  DESCRIPTION: The function controls the TX power on and off
+**
+**  PARAMETERS:  ioctlTxControl - on, off and time delay before off
+**
+**  OUTPUTS:     None
+**
+**  RETURNS:     None
+**
+*******************************************************************************/
+MV_STATUS onuEponMiTxPowerControlConfig(S_EponIoctlTxControl *ioctlTxControl)
+{
+    MV_STATUS status = MV_OK;
+    MV_U16 action;
+    MV_U16 time;
+        
+    action = ioctlTxControl->action;
+    time   = ioctlTxControl->time * 1000;   /* The time from user space is expressed in second */
+
+    status = mvEponApiTxPowerCtrlConfig(action, time);
+
+    return status;
+}
+
+/*******************************************************************************
+**
+**  onuEponMiPowerSavingConfig
+**  ____________________________________________________________________________
+**
+**  DESCRIPTION: The function sets maxSleepDuration for power saving mode
+**
+**  PARAMETERS:  *ioctlPowerSaving - power saving mode duration
+**
+**  OUTPUTS:     None
+**
+**  RETURNS:     None
+**
+*******************************************************************************/
+MV_STATUS onuEponMiPowerSavingConfig(S_EponIoctlPowerSaving *ioctlPowerSaving)
+{
+    MV_STATUS status = MV_OK;
+
+    MV_U8  earlyWakeup;
+    MV_U64 maxSleepDuration;
+
+    earlyWakeup      = ioctlPowerSaving->earlyWakeUp;
+    maxSleepDuration = ioctlPowerSaving->maxSleepDuration;
+
+    status = mvEponApiPowerSavingConfig(earlyWakeup, maxSleepDuration);
+
+    return status;
+}
+
+/*******************************************************************************
+**
+**  onuEponMiControlSleepConfig
+**  ____________________________________________________________________________
+**
+**  DESCRIPTION: The function triggers the action of power mode transformation
+**
+**  PARAMETERS:  *ioctlControlSleep - power control and sleep/wakeup duration
+**
+**  OUTPUTS:     None
+**
+**  RETURNS:     None
+**
+*******************************************************************************/
+MV_STATUS onuEponMiControlSleepConfig(S_EponIoctlControlSleep *ioctlControlSleep)
+{
+    MV_STATUS status = MV_OK;
+
+    uint8_t action;
+
+    action = ioctlControlSleep->action;
+
+    if (action == E_GPON_SLEEP_ACTION_DISABLE)
+    {
+        mvEponApiSleepCtrlDisable();
+    }
+    else if (action == E_GPON_SLEEP_ACTION_ENABLE)
+    {
+        mvEponApiSleepCtrlEnable();
+    }
+    else if (action == E_GPON_SLEEP_ACTION_CONFIG)
+    {
+        mvEponApiSleepCtrlCfg(ioctlControlSleep->sleepMode, 
+                              ioctlControlSleep->sleepDuration, 
+                              ioctlControlSleep->waitDuration);
+    }
+
+    return status;
+}
+
+/*******************************************************************************
+**
 **  onuEponMiSilenceConfig
 **  ____________________________________________________________________________
 **
@@ -650,11 +776,17 @@
   S_EponIoctlFec      ioctlFec;
   S_EponIoctlEnc      ioctlEnc;
   S_EponIoctlOamTx    ioctlTxOam;
+  S_EponIoctlDba      ioctlDba;
   S_EponIoctlHoldOver ioctlHoldover;
   S_EponIoctlSilence  ioctlSilence;
+  S_EponIoctlTdmQueue ioctlTdmQueue;
   MV_U32              ioctlState;
   MV_U32              ioctlAlarm;
   S_EponIoctlRogueOnu ioctlRogueOnu;
+  S_EponIoctlLosTime      ioctlLosTime;
+  S_EponIoctlTxControl    ioctlTxControl;
+  S_EponIoctlPowerSaving  ioctlPowerSaving;
+  S_EponIoctlControlSleep ioctlControlSleep;
   unsigned long       flags;
   int                 ret = -EINVAL;
 
@@ -870,6 +1002,90 @@
       ret = 0;
       break;
 
+    /* ====== MVEPON_IOCTL_LOS_TIME ==================== */
+    case MVEPON_IOCTL_LOS_TIME:
+        if (copy_from_user(&ioctlLosTime, (S_EponIoctlLosTime*)arg, sizeof(S_EponIoctlLosTime)))
+        {
+            mvPonPrint(PON_PRINT_ERROR, PON_API_MODULE,
+      		           "ERROR: (%s:%d) copy_from_user failed\n", __FILE_DESC__, __LINE__);
+            goto ioctlErr;
+        }
+
+        spin_lock_irqsave(&onuPonIrqLock, flags);
+        status = onuEponMiLosTimeConfig(&(ioctlLosTime));
+        spin_unlock_irqrestore(&onuPonIrqLock, flags);
+        if (status != MV_OK)
+        {
+            goto ioctlErr;
+        }
+
+        ret = 0;
+        
+        break;
+
+    /* ====== MVEPON_IOCTL_TX_CONTROL ==================== */
+    case MVEPON_IOCTL_TX_CONTROL:
+        if (copy_from_user(&ioctlTxControl, (S_EponIoctlTxControl*)arg, sizeof(S_EponIoctlTxControl)))
+        {
+            mvPonPrint(PON_PRINT_ERROR, PON_API_MODULE,
+      		           "ERROR: (%s:%d) copy_from_user failed\n", __FILE_DESC__, __LINE__);
+            goto ioctlErr;
+        }
+
+        spin_lock_irqsave(&onuPonIrqLock, flags);
+        status = onuEponMiTxPowerControlConfig(&(ioctlTxControl));
+        spin_unlock_irqrestore(&onuPonIrqLock, flags);
+        if (status != MV_OK)
+        {
+            goto ioctlErr;
+        }
+
+        ret = 0;
+        
+        break;
+
+    /* ====== MVEPON_IOCTL_POWER_SAVING ==================== */
+    case MVEPON_IOCTL_POWER_SAVING:
+        if (copy_from_user(&ioctlPowerSaving, (S_EponIoctlPowerSaving*)arg, sizeof(S_EponIoctlPowerSaving)))
+        {
+            mvPonPrint(PON_PRINT_ERROR, PON_API_MODULE,
+      		           "ERROR: (%s:%d) copy_from_user failed\n", __FILE_DESC__, __LINE__);
+            goto ioctlErr;
+        }
+
+        spin_lock_irqsave(&onuPonIrqLock, flags);
+        status = onuEponMiPowerSavingConfig(&(ioctlPowerSaving));
+        spin_unlock_irqrestore(&onuPonIrqLock, flags);
+        if (status != MV_OK)
+        {
+            goto ioctlErr;
+        }
+
+        ret = 0;
+
+        break;
+
+    /* ====== MVEPON_IOCTL_CONTROL_SLEEP ==================== */
+    case MVEPON_IOCTL_CONTROL_SLEEP:
+        if (copy_from_user(&ioctlControlSleep, (S_EponIoctlControlSleep*)arg, sizeof(S_EponIoctlControlSleep)))
+        {
+            mvPonPrint(PON_PRINT_ERROR, PON_API_MODULE,
+      		           "ERROR: (%s:%d) copy_from_user failed\n", __FILE_DESC__, __LINE__);
+            goto ioctlErr;
+        }
+
+        spin_lock_irqsave(&onuPonIrqLock, flags);
+        status = onuEponMiControlSleepConfig(&(ioctlControlSleep));
+        spin_unlock_irqrestore(&onuPonIrqLock, flags);
+        if (status != MV_OK)
+        {
+            goto ioctlErr;
+        }
+
+        ret = 0;
+        
+        break;
+
     /* ====== MVEPON_IOCTL_SILENCE ==================== */
     case MVEPON_IOCTL_SILENCE:
       if(copy_from_user(&ioctlSilence, (S_EponIoctlSilence*)arg, sizeof(S_EponIoctlSilence)))
@@ -925,7 +1141,7 @@
 
         spin_lock_irqsave(&onuPonIrqLock, flags);
         
-        status = onuEponDbP2PForceModeSet(ioctlState);
+        onuEponDbP2PForceModeSet(ioctlState);
         
         spin_unlock_irqrestore(&onuPonIrqLock, flags);
 
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/perf/epon/eponOnuLnxKsMI.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/perf/epon/eponOnuLnxKsMI.h
index d73b4e3..fe4aa39 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/perf/epon/eponOnuLnxKsMI.h
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/perf/epon/eponOnuLnxKsMI.h
@@ -98,6 +98,10 @@
 #define MVEPON_IOCTL_TDM_QUE_CFG   _IOW(MVPON_IOCTL_MAGIC, 14, unsigned int)

 #define MVEPON_IOCTL_ALARM_GET     _IOR(MVPON_IOCTL_MAGIC, 15, unsigned int)

 #define MVEPON_IOCTL_ROGUE_ONU_SET _IOW(MVPON_IOCTL_MAGIC, 16, unsigned int)
+#define MVEPON_IOCTL_LOS_TIME      _IOW(MVPON_IOCTL_MAGIC, 17, unsigned int)

+#define MVEPON_IOCTL_TX_CONTROL    _IOW(MVPON_IOCTL_MAGIC, 18, unsigned int)

+#define MVEPON_IOCTL_POWER_SAVING  _IOW(MVPON_IOCTL_MAGIC, 19, unsigned int)

+#define MVEPON_IOCTL_CONTROL_SLEEP _IOW(MVPON_IOCTL_MAGIC, 20, unsigned int)

 

 #define EPON_MAX_NUM_OF_MAC          (8)

 #define EPON_MAX_NUM_OF_QUEUE        (8)

@@ -142,9 +146,30 @@
 {

   E_EPON_IOCTL_STD_MODE,

   E_EPON_IOCTL_P2P_MODE,

-  E_EPON_IOCTL_MAX_MODE_NUM

+  E_EPON_IOCTL_MAX_MODE_NUM,

 }E_EponIoctlMode;

 

+typedef enum

+{

+    E_EPON_IOCTL_ENABLE_TX        = 0, 

+    E_EPON_IOCTL_DISABLE_TX       = 1, 

+    E_EPON_IOCTL_DISABLE_TX_DELAY = 2, 

+} E_EponIoctlTxControl;

+

+typedef enum

+{

+    E_GPON_SLEEP_ACTION_DISABLE = 0, 

+    E_GPON_SLEEP_ACTION_ENABLE  = 1, 

+    E_GPON_SLEEP_ACTION_CONFIG  = 2, 

+} E_GponSleepAction;

+

+typedef enum

+{

+    E_GPON_SLEEP_MODE_DO_NOTHING = 0, 

+    E_GPON_SLEEP_MODE_TX         = 1, 

+    E_GPON_SLEEP_MODE_TX_RX      = 2, 

+} E_GponSleepMode;

+

 /* Typedefs

 ------------------------------------------------------------------------------*/

 /* TX Counters */

@@ -292,6 +317,32 @@
   MV_U32 holdoverTime;

 }S_EponIoctlHoldOver;

 

+typedef struct

+{

+    uint16_t optical_los_time;

+    uint16_t mac_los_time;

+} S_EponIoctlLosTime;

+

+typedef struct

+{

+    uint16_t action;

+    uint16_t time;

+} S_EponIoctlTxControl;

+

+typedef struct

+{

+    uint8_t  earlyWakeUp;

+    uint64_t maxSleepDuration;

+} S_EponIoctlPowerSaving;

+

+typedef struct

+{

+    uint8_t  action;

+    uint8_t  sleepMode;

+    uint32_t sleepDuration;

+    uint32_t waitDuration;

+} S_EponIoctlControlSleep;

+

 /* Silence */

 typedef struct

 {

@@ -335,6 +386,11 @@
 MV_STATUS onuEponMngInterfaceRelease(void);

 void      onuEponMiNotifyCallback(MV_U32 onuState);

 

+MV_STATUS onuEponMiLosTimeConfig(S_EponIoctlLosTime *ioctlLosTime);

+MV_STATUS onuEponMiTxPowerControlConfig(S_EponIoctlTxControl *ioctlTxControl);

+MV_STATUS onuEponMiPowerSavingConfig(S_EponIoctlPowerSaving *ioctlPowerSaving);

+MV_STATUS onuEponMiControlSleepConfig(S_EponIoctlControlSleep *ioctlControlSleep);

+

 MV_STATUS mvP2PStart(void);

 MV_STATUS mvP2PStop(void);

 /* Macros

diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/perf/epon/eponOnuLnxKsUI.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/perf/epon/eponOnuLnxKsUI.c
index a49d831..66dbbb0 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/perf/epon/eponOnuLnxKsUI.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/perf/epon/eponOnuLnxKsUI.c
@@ -101,7 +101,6 @@
 /* Local Variables
 ------------------------------------------------------------------------------*/
 MV_BOOL infoEponCntReadClearInd = MV_TRUE;
-static S_EponIoctlDba ioctlDba;
 
 /* Export Functions
 ------------------------------------------------------------------------------*/
@@ -882,14 +881,15 @@
 										"Interrupt\n");
 	off += mvOsSPrintf(buf+off, " echo [#Queue][Thresh]> dgCfg            - configure Dying Gasp DBA report\n");
 	off += mvOsSPrintf(buf+off, " echo [module][Level][Options] > printMask - change printing options\n");
-	off += mvOsSPrintf(buf+off, " echo [enable][pattern][burst][duration]\n");
-	off += mvOsSPrintf(buf+off, "      [period]        > pattern          - start enable[1] or stop[0] transmission"
-										" of pattern: 0x1-1T, 0x2-2T, 0x80-RPBS"
-										"-9, 0x82-RPBS-15, 0x83-RPBS-23 in "
-										"burst: 0-static, 1- periodic for "
-										"duration: peac time interval"
-										"[micro seconds] in period - full cicle"
-										" time interval[micro seconds]\n");
+    off += mvOsSPrintf(buf+off, " echo [action][time]  > txPowerControl   - action(0:enable,1:disable,2:shut down then enable)\n");
+    off += mvOsSPrintf(buf+off, " echo [enable][time]  > powerSaving      - configure early wakeup and max sleep duration\n");
+    off += mvOsSPrintf(buf+off, " echo [action][mode][time][time] > sleepControl - configure sleep/wakeup parameters\n");
+    off += mvOsSPrintf(buf+off, " echo [patternMSB][pattern][patternLSB]\n");
+    off += mvOsSPrintf(buf+off, "                      > prbsUserDefinedPattern - 80bit user data: 16bit patternMLSB + 32bit pattern + 32bit patternLSB\n");
+    off += mvOsSPrintf(buf+off, " echo [enable][pattern][burst][duration][period] \n");
+    off += mvOsSPrintf(buf+off, "                      > prbsPreDefinedPattern - start enable[1] or stop [0] transmission of\n");
+    off += mvOsSPrintf(buf+off, "                                           pattern: 0x1-1T, 0x2-2T, 0x5-User, 0x80-PRBS-7, 0x81-PRBS-9, 0x82-PRBS-15, 0x83-PRBS-23\n");
+    off += mvOsSPrintf(buf+off, "                                           burst: 0-static, 1-periodic\n");
 	if (devId == MV_6601_DEV_ID) {
 		off += mvOsSPrintf(buf+off, " echo [latency][size] > pcsFrameLimits   - Configure PCS Frame Size "
 											"limits\n");
@@ -904,6 +904,7 @@
 	off += mvOsSPrintf(buf+off, "============================================================================\n");
 	off += mvOsSPrintf(buf+off, " cat printMask                           - dump printing options\n");
 	off += mvOsSPrintf(buf+off, " cat dbaSwCfgPrint                       - dump DBA cfg\n");
+   off += mvOsSPrintf(buf+off, " cat prbsUserPattern                     - dump PRBS user defined pattern\n");
 	if (devId == MV_6601_DEV_ID) {
 		off += mvOsSPrintf(buf+off, " cat pcsFrameLimits                     - dump PCS Frame Size Limits\n");
 		off += mvOsSPrintf(buf+off, " cat rxDataFifoThresh                   - dump RX Parser Data FIFO "
@@ -1089,6 +1090,7 @@
   MV_U32         idx;
   MV_U32         qsetidx;
   MV_U32         numOfQueues = 0;
+  S_EponIoctlDba ioctlDba;
 
   memset (&ioctlDba, 0, sizeof(S_EponIoctlDba));
 
@@ -1283,7 +1285,32 @@
   return(off);
 }
 
+/*******************************************************************************
+**
+**  onuEponUiCfgPrbsUserPattern
+**  ____________________________________________________________________________
+**
+**  DESCRIPTION: The function print PRBS user defined pattern
+**
+**  PARAMETERS:  char* buf
+**
+**  OUTPUTS:     char* buf
+**
+**  RETURNS:     void
+**
+*******************************************************************************/
+int onuEponUiCfgPrbsUserPattern(char* buf)
+{
+  MV_U32 userPattern[3];
+  int off = 0;
 
+  mvOnuPonMacPrbsUserDefinedPatternGet(userPattern);
+
+  off += mvOsSPrintf(buf+off, "PRBS User Pattern 0x%4x%8x%8x\n",
+		     userPattern[2], userPattern[1], userPattern[0]);
+
+  return(off);
+}
 
 /*******************************************************************************
 **
@@ -1390,10 +1417,11 @@
 
 /*******************************************************************************
 **
-**  onuEponUiCfgSetPatternBurst
+**  onuEponUiCfgSetUserPatternBurst
 **  ____________________________________________________________________________
 **
-**  DESCRIPTION: The function config start/stop onu transmission of pattern burst
+**  DESCRIPTION: The function config start/stop onu transmission of user defined
+**               pattern burst
 **
 **  PARAMETERS:  char* buf
 **
@@ -1402,7 +1430,33 @@
 **  RETURNS:     void
 **
 *******************************************************************************/
-void onuEponUiCfgSetPatternBurst(MV_U32 enable, MV_U32 pattern, MV_U32 burst, MV_U32 duration, MV_U32 period)
+void onuEponUiCfgSetUserPatternBurst(MV_U32 patternMSB, MV_U32 pattern, MV_U32 patternLSB)
+{
+  MV_U32 userPattern[3];
+
+  userPattern[0] = patternLSB;
+  userPattern[1] = pattern;
+  userPattern[2] = patternMSB;
+
+  mvOnuPonMacPrbsUserDefinedPatternSet(userPattern);
+}
+
+/*******************************************************************************
+**
+**  onuEponUiCfgSetDefinedPatternBurst
+**  ____________________________________________________________________________
+**
+**  DESCRIPTION: The function config start/stop onu transmission of pre-defined
+**               pattern burst
+**
+**  PARAMETERS:  char* buf
+**
+**  OUTPUTS:     char* buf
+**
+**  RETURNS:     void
+**
+*******************************************************************************/
+void onuEponUiCfgSetDefinedPatternBurst(MV_U32 enable, MV_U32 pattern, MV_U32 burst, MV_U32 duration, MV_U32 period)
 {
   if (enable == 0)
   {
@@ -1417,6 +1471,139 @@
 
 /*******************************************************************************
 **
+**  onuEponUiCfgLosTime
+**  ____________________________________________________________________________
+**
+**  DESCRIPTION: The function sets the time duration to be used 
+**               when no optical detected and no GATE MPCPDU received
+**
+**  PARAMETERS:  opticalLosTime - time duration to be used when no optical detected
+**               macLosTime     - time duration to be used when no GATE MPCPDU received
+**  OUTPUTS:     void
+**
+**  RETURNS:     void
+**
+*******************************************************************************/
+void onuEponUiCfgLosTime(MV_U32 opticalLosTime, MV_U32 macLosTime)
+{
+    onuEponDbOnuOpticalLosTimeSet(opticalLosTime);
+    onuEponDbOnuMacLosTimeSet(macLosTime);
+
+    /* If the holdover timer is not active, we add the OpticalLosTimer to the holdover time and save */
+    if (onuPonResourceTbl_s.onuPonHoldoverTimerId.onuPonTimerActive == ONU_PON_TIMER_NOT_ACTIVE)
+    {
+        /* Add the configured OptLosTime to holdover time and save for next holdover */
+        onuPonTimerUpdate(&(onuPonResourceTbl_s.onuPonHoldoverTimerId), 0, 
+                          onuEponDbOnuHoldoverTimeGet() + onuEponDbOnuOpticalLosTimeGet(), 0);
+    }
+}
+
+/*******************************************************************************
+**
+**  onuEponUiCfgTxPowerControl
+**  ____________________________________________________________________________
+**
+**  DESCRIPTION: The function controls the TX power on and off
+**
+**  PARAMETERS:  action - turn on and turn off the TX power, or turn off after 
+**                        certain time duration.
+**               time   - time duration to be expired before turning off TX
+**
+**  OUTPUTS:     None
+**
+**  RETURNS:     MV_TRUE or MV_FLASE
+**
+*******************************************************************************/
+void onuEponUiCfgTxPowerControl(MV_U32 action, MV_U32 time)
+{
+    if (action == E_EPON_IOCTL_ENABLE_TX)
+    {
+        onuPonTimerDisable(&(onuPonResourceTbl_s.onuEponTxControlTimerId));
+        onuPonTxPowerOn(MV_TRUE);
+    }
+    else if (action == E_EPON_IOCTL_DISABLE_TX)
+    {
+        onuPonTimerDisable(&(onuPonResourceTbl_s.onuEponTxControlTimerId));
+        onuPonTxPowerOn(MV_FALSE);
+    }
+    else if (action == E_EPON_IOCTL_DISABLE_TX_DELAY)
+    {        
+        onuPonTimerUpdate(&(onuPonResourceTbl_s.onuEponTxControlTimerId), 0, time, 1);
+    }
+}
+
+/*******************************************************************************
+**
+**  onuEponUiCfgPowerSaving
+**  ____________________________________________________________________________
+**
+**  DESCRIPTION: The function sets the parameters for power saving mode
+**
+**  PARAMETERS:  earlyWakeup - set the ability of supporting early wake up
+**               maxSleepDuration - time period for keeping power saving mode
+**
+**  OUTPUTS:     None
+**
+**  RETURNS:     MV_TRUE or MV_FLASE
+**
+*******************************************************************************/
+void onuEponUiCfgPowerSaving(MV_U32 earlyWakeup, MV_U32 maxSleepDuration)
+{    
+    if ((onuEponDbOnuSleepDurationGet() + onuEponDbOnuWakeupDurationGet()) >= maxSleepDuration)
+    {
+        mvPonPrint(PON_PRINT_ERROR, PON_API_MODULE,
+        	       "ERROR: Wrong parameters, sleep(%d) + wakeup(%d) >= maxSleepDuration(%d)\r\n", 
+        	       onuEponDbOnuSleepDurationGet(), onuEponDbOnuWakeupDurationGet(), 
+        	       maxSleepDuration);
+
+        return MV_BAD_PARAM;
+    }
+
+    onuEponDbOnuPowerSavingWakeupSet(earlyWakeup);
+    onuEponDbOnuPowerSavingMaxSleepDurationSet(maxSleepDuration);
+
+    if (onuPonResourceTbl_s.onuEponMaxSleepTimerId.onuPonTimerActive == ONU_PON_TIMER_ACTIVE)
+    {
+        onuPonTimerUpdate(&(onuPonResourceTbl_s.onuEponMaxSleepTimerId), 0, 
+                          maxSleepDuration, 1);
+    }
+}
+
+/*******************************************************************************
+**
+**  onuEponUiCfgControlSleep
+**  ____________________________________________________________________________
+**
+**  DESCRIPTION: The function triggers the action of power mode transformation
+**
+**  PARAMETERS:  action         - enable, disable or configure
+**               sleepMode      - turn off tx or tx/rx
+**               sleepDuration  - periodical time duration for sleep
+**               wakeupDuration - periodical time duration for wakeup
+**
+**  OUTPUTS:     None
+**
+**  RETURNS:     None
+**
+*******************************************************************************/
+void onuEponUiCfgControlSleep(MV_U32 action, MV_U32 sleepMode, MV_U32 sleepDuration, MV_U32 wakeupDuration)
+{
+    if (action == E_GPON_SLEEP_ACTION_DISABLE)
+    {
+        mvEponApiSleepCtrlDisable();
+    }
+    else if (action == E_GPON_SLEEP_ACTION_ENABLE)
+    {
+        mvEponApiSleepCtrlEnable();
+    }
+    else if (action == E_GPON_SLEEP_ACTION_CONFIG)
+    {
+        mvEponApiSleepCtrlCfg(sleepMode, sleepDuration, wakeupDuration);
+    }
+}
+
+/*******************************************************************************
+**
 **  onuEponUiCfgSetPcsFrameLimits
 **  ____________________________________________________________________________
 **
@@ -1573,9 +1760,9 @@
 		off += mvOsSPrintf(buf+off, "Failed to obtain 2K packet support related register value!\r\n");
 	else {
 		off += mvOsSPrintf(buf+off, "\r\n");
-		off += mvOsSPrintf(buf+off, "PcsFrameSizeLimitSize    = %#d\r\n", frameSizeLimitSize);
-        off += mvOsSPrintf(buf+off, "PcsFrameSizeLimitLatency = %#d\r\n", frameSizeLimitLatency);
-        off += mvOsSPrintf(buf+off, "DataFifoThreshold        = %#d\r\n", dataFifoThreshold);
+		off += mvOsSPrintf(buf+off, "PcsFrameSizeLimitSize    = %d\r\n", frameSizeLimitSize);
+        off += mvOsSPrintf(buf+off, "PcsFrameSizeLimitLatency = %d\r\n", frameSizeLimitLatency);
+        off += mvOsSPrintf(buf+off, "DataFifoThreshold        = %d\r\n", dataFifoThreshold);
 	}
 
 	return(off);
@@ -1848,6 +2035,8 @@
 		return(onuEponUiCfgDbaCfgPrint(buf));
 	else if (!strcmp(name, "dbaStaticCfgPrint"))
 		return(onuEponUiCfgDbaStaticCfgPrint(buf));
+    else if (!strcmp(name, "prbsUserPattern"))
+        return (onuEponUiCfgPrbsUserPattern(buf));
 	else if (!strcmp(name, "helpCfg"))
 		return(onuEponUiCfgHelpShow(buf));
 	else if (devId == MV_6601_DEV_ID) {
@@ -1920,8 +2109,22 @@
 		onuEponUiCfgDgCfg((MV_U32)param1, (MV_U32)param2);
 	else if (!strcmp(name, "printMask"))					/* module, print level, options */
 		ponOnuChangePrintStatus((MV_U32)param1, (MV_U32)param2, (MV_U32)param3);
-	else if (!strcmp(name, "pattern"))
-		onuEponUiCfgSetPatternBurst((MV_U32)param1,(MV_U32)param2,(MV_U32)param3,(MV_U32)param4,(MV_U32)param5);
+    else if (!strcmp(name, "txPowerControl"))
+    {
+        onuEponUiCfgTxPowerControl((MV_U32)param1, (MV_U32)param2);
+    }
+    else if (!strcmp(name, "powerSaving"))
+    {
+        onuEponUiCfgPowerSaving((MV_U32)param1, (MV_U32)param2);
+    }
+    else if (!strcmp(name, "sleepControl"))
+    {
+        onuEponUiCfgControlSleep((MV_U32)param1, (MV_U32)param2, (MV_U32)param3, (MV_U32)param4);
+    }
+    else if (!strcmp(name, "prbsUserDefinedPattern"))
+        onuEponUiCfgSetUserPatternBurst((MV_U32)param1, (MV_U32)param2, (MV_U32)param3); /* 80bit user data: 16bit patternMLSB + 32bit pattern + 32bit patternLSB\n" */
+    else if (!strcmp(name, "prbsPreDefinedPattern"))
+        onuEponUiCfgSetDefinedPatternBurst((MV_U32)param1, (MV_U32)param2, (MV_U32)param3 , (MV_U32)param4 , (MV_U32)param5);  /* pattern type, burst type, duration, period */
 	else if (devId == MV_6601_DEV_ID) {
 		if (!strcmp(name, "pcsFrameLimits"))
 			onuEponUiCfgSetPcsFrameLimits((MV_U32)param1, (MV_U32)param2);
@@ -1964,12 +2167,17 @@
 static DEVICE_ATTR(silenceSim,              S_IWUSR, cfg_show, cfg_store);
 static DEVICE_ATTR(dgDebug,                 S_IWUSR, cfg_show, cfg_store);
 static DEVICE_ATTR(dgCfg,                   S_IWUSR, cfg_show, cfg_store);
-static DEVICE_ATTR(pattern,                 S_IWUSR, cfg_show, cfg_store);
+static DEVICE_ATTR(prbsUserPattern,         S_IRUSR, cfg_show, cfg_store);
+static DEVICE_ATTR(prbsUserDefinedPattern,  S_IWUSR, cfg_show, cfg_store);
+static DEVICE_ATTR(prbsPreDefinedPattern,   S_IWUSR, cfg_show, cfg_store);
 static DEVICE_ATTR(printMask,               S_IRUSR | S_IWUSR, cfg_show, cfg_store);
 static DEVICE_ATTR(pcsFrameLimits,          S_IWUSR | S_IRUSR, cfg_show, cfg_store);
 static DEVICE_ATTR(rxDataFifoThresh,        S_IWUSR | S_IRUSR, cfg_show, cfg_store);
 static DEVICE_ATTR(ddmTxPolarity,           S_IWUSR, cfg_show, cfg_store);
 static DEVICE_ATTR(pkt2kEn,                 S_IWUSR | S_IRUSR, cfg_show, cfg_store);
+static DEVICE_ATTR(txPowerControl,          S_IWUSR, cfg_show, cfg_store);
+static DEVICE_ATTR(powerSaving,             S_IWUSR, cfg_show, cfg_store);
+static DEVICE_ATTR(sleepControl,            S_IWUSR, cfg_show, cfg_store);
 static DEVICE_ATTR(helpCfg,                 S_IRUSR, cfg_show, cfg_store);
  
 static struct attribute *cfg_attrs[] = {
@@ -1990,12 +2198,17 @@
 	&dev_attr_silenceSim.attr,
 	&dev_attr_dgDebug.attr,
 	&dev_attr_dgCfg.attr,
-	&dev_attr_pattern.attr,
+	&dev_attr_prbsUserPattern.attr,
+	&dev_attr_prbsUserDefinedPattern.attr,
+	&dev_attr_prbsPreDefinedPattern.attr,
 	&dev_attr_printMask.attr,
 	&dev_attr_pcsFrameLimits.attr,
 	&dev_attr_rxDataFifoThresh.attr, 
 	&dev_attr_ddmTxPolarity.attr,
 	&dev_attr_pkt2kEn.attr, 
+	&dev_attr_txPowerControl.attr, 
+	&dev_attr_powerSaving.attr, 
+	&dev_attr_sleepControl.attr, 
 	&dev_attr_helpCfg.attr,
 	NULL
 };
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/perf/epon/eponOnuLnxKsUI.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/perf/epon/eponOnuLnxKsUI.h
index f8c9cde..a6e92f1 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/perf/epon/eponOnuLnxKsUI.h
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/perf/epon/eponOnuLnxKsUI.h
@@ -96,6 +96,12 @@
 MV_STATUS onuEponUsrInterfaceCreate(void);

 MV_STATUS onuEponUsrInterfaceRelease(void);

 

+void onuEponUiCfgLosTime(MV_U32 opticalLosTime, MV_U32 macLosTime);

+void onuEponUiCfgTxPowerControl(MV_U32 action, MV_U32 time);

+void onuEponUiCfgPowerSaving(MV_U32 earlyWakeup, MV_U32 maxSleepDuration);

+void onuEponUiCfgControlSleep(MV_U32 action, MV_U32 sleepMode, 

+                              MV_U32 sleepDuration, MV_U32 wakeupDuration);

+

 /* Macros

 ------------------------------------------------------------------------------*/    

 

diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/perf/gpon/gponOnuLnxKsUI.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/perf/gpon/gponOnuLnxKsUI.c
index d3bf5ef..cf45075 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/perf/gpon/gponOnuLnxKsUI.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/perf/gpon/gponOnuLnxKsUI.c
@@ -2946,10 +2946,11 @@
 }
 
 /*******************************************************************************
-****  onuGponUiCfgSetPatternBurst
+****  onuGponUiCfgSetUserPatternBurst
 **  ____________________________________________________________________________
 **
-**  DESCRIPTION: The function config start onu transmission of pattern burst
+**  DESCRIPTION: The function config start onu transmission of user-defined
+**               pattern burst
 **
 **  PARAMETERS:  char* buf
 **
@@ -2958,11 +2959,34 @@
 **  RETURNS:     void
 **
 *******************************************************************************/
-void onuGponUiCfgSetPatternBurst(MV_U32 enable,MV_U32 pattern, MV_U32 burst, MV_U32 duration, MV_U32 period)
+void onuGponUiCfgSetUserPatternBurst(MV_U32 patternMSB, MV_U32 pattern, MV_U32 patternLSB)
+{
+  MV_U32 userPattern[3];
+
+  userPattern[0] = patternLSB;
+  userPattern[1] = pattern;
+  userPattern[2] = patternMSB;
+
+  mvOnuPonMacPrbsUserDefinedPatternSet(userPattern);
+}
+/*******************************************************************************
+****  onuGponUiCfgSetDefinedPatternBurst
+**  ____________________________________________________________________________
+**
+**  DESCRIPTION: The function config start onu transmission of pre-defined
+**               pattern burst
+**
+**  PARAMETERS:  char* buf
+**
+**  OUTPUTS:     char* buf
+**
+**  RETURNS:     void
+**
+*******************************************************************************/
+void onuGponUiCfgSetDefinedPatternBurst(MV_U32 enable, MV_U32 pattern, MV_U32 burst, MV_U32 duration, MV_U32 period)
 {
 
-    if (enable == 0)
-    {
+    if (enable == 0) {
       onuPonPatternBurstOff();
     }
     else
@@ -3190,6 +3214,33 @@
 
 /*******************************************************************************
 **
+**  onuGponUiCfgPrbsUserPattern
+**  ____________________________________________________________________________
+**
+**  DESCRIPTION: The function print PRBS user defined pattern
+**
+**  PARAMETERS:  char* buf
+**
+**  OUTPUTS:     char* buf
+**
+**  RETURNS:     void
+**
+*******************************************************************************/
+int onuGponUiCfgPrbsUserPattern(char* buf)
+{
+  MV_U32 userPattern[3];
+  int off = 0;
+
+  mvOnuPonMacPrbsUserDefinedPatternGet(userPattern);
+
+  off += mvOsSPrintf(buf+off, "PRBS User Pattern 0x%4x%8x%8x\n",
+		     userPattern[2], userPattern[1], userPattern[0]);
+
+  return(off);
+}
+
+/*******************************************************************************
+**
 **  onuGponUiSyncLogEnable
 **  ____________________________________________________________________________
 **
@@ -3267,17 +3318,15 @@
 	off += mvOsSPrintf(buf+off, " echo [module] [Level] [Options] > printMask       - change printing options\n");
 	off += mvOsSPrintf(buf+off, " echo [state]                    > adminCfg        - "
 						"change PON BE (TX EN) state 0=Enable, 1=Disable\n");
-	off += mvOsSPrintf(buf+off, " echo [enable][pattern][burst][duration][period] > pattern\n");
-	off += mvOsSPrintf(buf+off, "                                                   - "
-						"start enable[1] or stop [0] transmission of\n");
-	off += mvOsSPrintf(buf+off, "                                                     "
-						"pattern: 0x1-1T, 0x2-2T, 0x80-RPBS-9, 0x82-RPBS-15, 0x83-RPBS-23\n");
-	off += mvOsSPrintf(buf+off, "                                                     "
-						"burst: 0-static, 1-periodic\n");
-	off += mvOsSPrintf(buf+off, "                                                     "
-						"duration: peak time interval[micro seconds]\n");
-	off += mvOsSPrintf(buf+off, "                                                     "
-						"period - full cicle time interval[micro seconds]\n");
+  off += mvOsSPrintf(buf+off, " echo [patternMSB][pattern][patternLSB]\n");
+  off += mvOsSPrintf(buf+off, "                                 > prbsUserDefinedPattern - /* 80bit user data: 16bit patternMLSB + 32bit pattern + 32bit patternLSB\n");
+  off += mvOsSPrintf(buf+off, " echo [enable][pattern][burst][duration][period]\n");
+  off += mvOsSPrintf(buf+off, "                                 > prbsPreDefinedPattern\n");
+  off += mvOsSPrintf(buf+off, "                                                   - start enable[1] or stop [0] transmission of\n");
+  off += mvOsSPrintf(buf+off, "                                                     pattern: 0x1-1T, 0x2-2T, 0x5-User, 0x80-PRBS-7, 0x81-PRBS-9, 0x82-PRBS-15\n");
+  off += mvOsSPrintf(buf+off, "                                                     burst: 0-static, 1-periodic\n");
+  off += mvOsSPrintf(buf+off, "                                                     duration: peak time interval[micro seconds]\n");
+  off += mvOsSPrintf(buf+off, "                                                     period - full cycle time interval[micro seconds]\n");
 	off += mvOsSPrintf(buf+off, " echo [T01 Interval]             > t01IntervalCfg  - "
 						"config T01 timer interval in mS\n");
 	off += mvOsSPrintf(buf+off, " echo [T02 Interval]             > t02IntervalCfg  - "
@@ -3296,7 +3345,7 @@
 int onuGponUiSyncLogEnableShow()
 {
     printk("The Sync Log is ");
-    
+
     if (onuGponLogEnable == MV_TRUE)
     {
         printk("enabled\r\n");
@@ -3305,7 +3354,7 @@
     {
         printk("disabled\r\n");
     }
-    
+
     return 0;
 }
 
@@ -3342,6 +3391,8 @@
 
 	if (!strcmp(name, "printMask"))
 		return ponOnuPrintStatus(buf);
+  else if (!strcmp(name, "prbsUserPattern"))
+  	return onuGponUiCfgPrbsUserPattern(buf);
 	else if (!strcmp(name, "helpMisc"))
 		return onuGponUiMiscHelpShow(buf);
 	else if (!strcmp(name, "syncLogEnable"))	/* sync log enable or disable */
@@ -3399,9 +3450,10 @@
 #endif /* MV_GPON_PERFORMANCE_CHECK */
 	else if (!strcmp(name, "printMask"))		/* module, print level, options */
 		ponOnuChangePrintStatus((MV_U32)param1, (MV_U32)param2, (MV_U32)param3);
-	else if (!strcmp(name, "pattern"))		/* pattern type, burst type, duration, period */
-		onuGponUiCfgSetPatternBurst((MV_U32)param1, (MV_U32)param2, (MV_U32)param3 ,
-					    (MV_U32)param4 , (MV_U32)param5);
+  else if (!strcmp(name, "prbsUserDefinedPattern"))
+    onuGponUiCfgSetUserPatternBurst((MV_U32)param1, (MV_U32)param2, (MV_U32)param3); /* 80bit user data: 16bit patternMLSB + 32bit pattern + 32bit patternLSB\n" */
+  else if (!strcmp(name, "prbsPreDefinedPattern"))
+    onuGponUiCfgSetDefinedPatternBurst((MV_U32)param1, (MV_U32)param2, (MV_U32)param3 , (MV_U32)param4 , (MV_U32)param5);  /* pattern type, burst type, duration, period */
 	else if (!strcmp(name, "adminCfg"))		/* admin 0=enable 1 = disable */
 		onuGponUiDebugAdminMode((MV_U32)param1);
 	else if (!strcmp(name, "t01IntervalCfg"))	/* T01 interval in mS */
@@ -3435,10 +3487,12 @@
 static DEVICE_ATTR(pmCheck,               S_IWUSR, misc_show, misc_store);
 static DEVICE_ATTR(pmClear,               S_IWUSR, misc_show, misc_store);
 #endif /* MV_GPON_PERFORMANCE_CHECK */
-static DEVICE_ATTR(printMask,             S_IRUSR | S_IWUSR, misc_show, misc_store);
-static DEVICE_ATTR(pattern,               S_IWUSR, misc_show, misc_store);
+static DEVICE_ATTR(prbsUserDefinedPattern,S_IWUSR, misc_show, misc_store);
+static DEVICE_ATTR(prbsPreDefinedPattern, S_IWUSR, misc_show, misc_store);
 static DEVICE_ATTR(adminCfg,              S_IWUSR, misc_show, misc_store);
+static DEVICE_ATTR(printMask,             S_IRUSR | S_IWUSR, misc_show, misc_store);
 static DEVICE_ATTR(helpMisc,              S_IRUSR, misc_show, misc_store);
+static DEVICE_ATTR(prbsUserPattern,       S_IRUSR | S_IWUSR, misc_show, misc_store);
 static DEVICE_ATTR(t01IntervalCfg,        S_IWUSR, misc_show, misc_store);
 static DEVICE_ATTR(t02IntervalCfg,        S_IWUSR, misc_show, misc_store);
 static DEVICE_ATTR(syncLogEnable,         S_IRUSR | S_IWUSR, misc_show, misc_store);
@@ -3459,10 +3513,12 @@
 	&dev_attr_pmCheck.attr,
 	&dev_attr_pmClear.attr,
 #endif /* MV_GPON_PERFORMANCE_CHECK */
+	&dev_attr_prbsUserDefinedPattern.attr,
+	&dev_attr_prbsPreDefinedPattern.attr,
+	&dev_attr_adminCfg.attr,
 	&dev_attr_printMask.attr,
 	&dev_attr_helpMisc.attr,
-	&dev_attr_pattern.attr,
-	&dev_attr_adminCfg.attr,
+	&dev_attr_prbsUserPattern.attr,
 	&dev_attr_t01IntervalCfg.attr,
 	&dev_attr_t02IntervalCfg.attr,
 	&dev_attr_syncLogEnable.attr,
@@ -3505,8 +3561,8 @@
 	else if (devId == MV_6601_DEV_ID) {
 		if (!strcmp(name, "acCoupling"))
 			return onuGponUiDebugManagerAcCouplingGet(buf);
-		else if (!strcmp(name, "activeTxBm"))
-			return onuGponUiDebugManagerActiveTxBitmapGet(buf);
+	        else if (!strcmp(name, "activeTxBm"))
+                        return onuGponUiDebugManagerActiveTxBitmapGet(buf);
 	}
 	return 0;
 }
@@ -3564,7 +3620,7 @@
 	else if (!strcmp(name, "fifoSupport"))		/* enable/disable */
 		onuGponUiDebugManagerFifoSupportSet((MV_U32)param1);
 	else if (devId == MV_6601_DEV_ID) {
-		if (!strcmp(name, "acCoupling"))	
+		if (!strcmp(name, "acCoupling"))
             /* mode, time, pattern1, pattern2 */
 			onuGponUiDebugManagerAcCouplingSet((MV_U32)param1, (MV_U32)param2, (MV_U32)param3, (MV_U32)param4);
 		else if (!strcmp(name, "activeTxBm"))				/* bitmap, valid */
@@ -3619,7 +3675,7 @@
 	&dev_attr_ploamBurstCfg.attr,
 	&dev_attr_clearFifoCnts.attr,
 	&dev_attr_fifoSupport.attr,
-	&dev_attr_acCoupling.attr, 
+	&dev_attr_acCoupling.attr,
 	&dev_attr_activeTxBm.attr,
 	&dev_attr_helpProto.attr,
 	NULL
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/epon/eponOnuBoard.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/epon/eponOnuBoard.c
index 7d5cd97..cfb6b63 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/epon/eponOnuBoard.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/epon/eponOnuBoard.c
@@ -77,6 +77,7 @@
 /* Include Files
 ------------------------------------------------------------------------------*/
 #include "ponOnuHeader.h"
+#include "eponOnuDb.h"
 
 /* Local Constant
 ------------------------------------------------------------------------------*/                                               
@@ -92,6 +93,7 @@
 
 /* Export Functions
 ------------------------------------------------------------------------------*/
+extern MV_STATUS mvEponApiSleepModeCtrl(MV_U32 enable);
 
 /* Local Functions
 ------------------------------------------------------------------------------*/
@@ -538,4 +540,126 @@
   return(MV_OK);
 }
 #endif
+
+/*******************************************************************************
+**
+**  onuEponTxControlTimerHndl
+**  ____________________________________________________________________________
+**
+**  DESCRIPTION: The function is the timer callback when timer for 
+**               shutting the Tx is expired
+**
+**  PARAMETERS:  void
+**
+**  OUTPUTS:     void
+**
+**  RETURNS:     void
+**
+*******************************************************************************/
+void onuEponTxControlTimerHndl(void)
+{
+    unsigned long flags;
+    
+    spin_lock_irqsave(&onuPonIrqLock, flags);
+    
+    onuPonTimerDisable(&(onuPonResourceTbl_s.onuEponTxControlTimerId));
+
+    /* Turn off the tx power */
+    onuPonTxPowerOn(MV_FALSE);
+
+    spin_unlock_irqrestore(&onuPonIrqLock, flags);
+}
+
+/*******************************************************************************
+**
+**  onuEponMaxSleepTimerHndl
+**  ____________________________________________________________________________
+**
+**  DESCRIPTION: The function is the timer callback when timer for 
+**               keeping power saving mode is expired
+**
+**  PARAMETERS:  void
+**
+**  OUTPUTS:     void
+**
+**  RETURNS:     void
+**
+*******************************************************************************/
+void onuEponMaxSleepTimerHndl(void)
+{
+    unsigned long flags;
+    
+    spin_lock_irqsave(&onuPonIrqLock, flags);
+    
+    onuPonTimerDisable(&(onuPonResourceTbl_s.onuEponMaxSleepTimerId));
+
+    if (onuPonResourceTbl_s.onuEponSleepDurationTimerId.onuPonTimerActive == ONU_PON_TIMER_ACTIVE)
+    {
+        onuPonTimerDisable(&(onuPonResourceTbl_s.onuEponSleepDurationTimerId));
+    }
+
+    /* Leave power saving mode */
+    onuEponDbOnuSleepWakeupStatusSet(E_EPON_NOT_POWER_SAVING_STATUS);
+
+    /* Enable TX/RX */
+    mvEponApiSleepModeCtrl(MV_FALSE);
+
+    /* Send SleepStatusUpdate alarm to OLT */
+    // todo
+    
+
+    spin_unlock_irqrestore(&onuPonIrqLock, flags);
+}
+
+/*******************************************************************************
+**
+**  onuEponSleepWakeDurationTimerHndl
+**  ____________________________________________________________________________
+**
+**  DESCRIPTION: The function is the timer callback when periodical timer for
+**               switching between sleep status and wakeup status is expired
+**
+**  PARAMETERS:  void
+**
+**  OUTPUTS:     void
+**
+**  RETURNS:     void
+**
+*******************************************************************************/
+void onuEponSleepWakeDurationTimerHndl(void)
+{
+    unsigned long flags;
+    
+    spin_lock_irqsave(&onuPonIrqLock, flags);
+    
+    if (onuEponDbOnuSleepWakeupStatusGet() == E_EPON_POWER_SAVING_SLEEP_STATUS)
+    {
+        /* Change the status to wakeup */
+        onuEponDbOnuSleepWakeupStatusSet(E_EPON_POWER_SAVING_WAKEUP_STATUS);
+        
+        onuPonTimerUpdate(&(onuPonResourceTbl_s.onuEponSleepDurationTimerId), 
+                          0, 
+                          onuEponDbOnuWakeupDurationGet(), 
+                          1);
+
+        /* Enable TX/RX */
+        mvEponApiSleepModeCtrl(MV_FALSE);
+    }
+    else if(onuEponDbOnuSleepWakeupStatusGet() == E_EPON_POWER_SAVING_WAKEUP_STATUS)
+    {
+        /* Change the status to sleep */
+        onuEponDbOnuSleepWakeupStatusSet(E_EPON_POWER_SAVING_SLEEP_STATUS);
+        
+        onuPonTimerUpdate(&(onuPonResourceTbl_s.onuEponSleepDurationTimerId), 
+                          0, 
+                          onuEponDbOnuSleepDurationGet(), 
+                          1);
+
+        /* Disable TX/RX */
+        mvEponApiSleepModeCtrl(MV_TRUE);
+    }
+
+    spin_unlock_irqrestore(&onuPonIrqLock, flags);
+}
+
 #endif /* PON_FPGA */
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/epon/eponOnuBoard.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/epon/eponOnuBoard.h
index 46d169c..f0aaa99 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/epon/eponOnuBoard.h
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/epon/eponOnuBoard.h
@@ -93,6 +93,9 @@
 

 /* Global functions

 ------------------------------------------------------------------------------*/

+void onuEponTxControlTimerHndl(void);

+void onuEponMaxSleepTimerHndl(void);

+void onuEponSleepWakeDurationTimerHndl(void);

 

 #ifndef PON_FPGA

 MV_STATUS onuEponSerdesInit(void);

diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/epon/eponOnuLnxKsOs.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/epon/eponOnuLnxKsOs.c
index af3d3e5..9056455 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/epon/eponOnuLnxKsOs.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/epon/eponOnuLnxKsOs.c
@@ -252,6 +252,52 @@
     return(MV_ERROR);

   }

 

+    /* ONU EPON tx_power supply control timer */

+     retcode = onuPonTimerCreate(&(onuPonResourceTbl_s.onuEponTxControlTimerId),    /* timer Id */

+                                 "epon_TxControlTimer",                             /* timer description */

+                                 (PTIMER_FUNCPTR)onuEponTxControlTimerHndl,         /* timer function */

+                                 0,                                                 /* timer function param */

+                                 ONU_PON_TIMER_NOT_ACTIVE ,                         /* init value */

+                                 0);                                                /* periodic value */

+    if (retcode != MV_OK)

+    {

+        mvPonPrint(PON_PRINT_ERROR, PON_API_MODULE,

+                   "ERROR: (%s:%d) EPON Tx power control timer create\n\r", 

+                   __FILE_DESC__, __LINE__);

+        return (MV_ERROR);

+    }

+

+     /* ONU EPON sleep mode: max sleep timer */

+     retcode = onuPonTimerCreate(&(onuPonResourceTbl_s.onuEponMaxSleepTimerId),     /* timer Id */

+                                 "epon_MaxSleepTimer",                              /* timer description */

+                                 (PTIMER_FUNCPTR)onuEponMaxSleepTimerHndl,          /* timer function */

+                                 0,                                                 /* timer function param */

+                                 ONU_PON_TIMER_NOT_ACTIVE,                          /* init value */

+                                 0);                                                /* periodic value */

+    if (retcode != MV_OK)

+    {

+        mvPonPrint(PON_PRINT_ERROR, PON_API_MODULE,

+                   "ERROR: (%s:%d) EPON power saving mode: max sleep timer create\n\r", 

+                   __FILE_DESC__, __LINE__);

+        return (MV_ERROR);

+    }

+

+    /* ONU EPON sleep mode: sleep duration and wait duration timer */

+    retcode = onuPonTimerCreate(&(onuPonResourceTbl_s.onuEponSleepDurationTimerId), /* timer Id */

+                                "epon_SleepWakeDurationTimer",                      /* timer description */

+                                (PTIMER_FUNCPTR)onuEponSleepWakeDurationTimerHndl,  /* timer function */

+                                0,                                                  /* timer function param */

+                                ONU_PON_TIMER_NOT_ACTIVE ,                          /* init value */ 

+                                0);                                                 /* periodic value */ 

+    if (retcode != MV_OK)

+    {

+        mvPonPrint(PON_PRINT_ERROR, PON_API_MODULE,

+                   "ERROR: (%s:%d) EPON power saving mode: sleep and wait duration timer create\n\r", 

+                   __FILE_DESC__, __LINE__);

+        return(MV_ERROR);

+    }

+  

+

   /* onu epon silence timer */

   for (idx = 0; idx < EPON_MAX_MAC_NUM; idx++) 

   {

diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/gpon/gponOnuLnxKsOs.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/gpon/gponOnuLnxKsOs.c
index acd659e..86b921c 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/gpon/gponOnuLnxKsOs.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/gpon/gponOnuLnxKsOs.c
@@ -154,6 +154,8 @@
     INIT_WORK((struct work_struct *) &gponTcontCleanAllWork, onuGponWqTcontFunc);

     gponTcontCleanAllWork.action = TCONT_CLEAN_ALL_EVENT;

 

+    printk("STARTING PON WORK QUEUE!!!!!\n");

+

     return(MV_OK);

   }

 

diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/ponOnuBoard.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/ponOnuBoard.c
index 0a3d36e..79d2e17 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/ponOnuBoard.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/ponOnuBoard.c
@@ -88,6 +88,9 @@
 
 /* Global functions
 ------------------------------------------------------------------------------*/
+MV_U32 prbsUserDefinedPattern[3] = {0xA5A5A5A5, /* [31:00] */
+				    0xA5A5A5A5, /* [63:32] */
+				    0xA5A5};    /* [79:64] */
 
 /* Local Variables
 ------------------------------------------------------------------------------*/
@@ -300,6 +303,48 @@
 
 /*******************************************************************************
 **
+**  mvOnuPonMacPrbsUserDefinedPatternSet
+**  ____________________________________________________________________________
+**
+**  DESCRIPTION: The function init User defined PRBS pattern
+**
+**  PARAMETERS:  None
+**
+**  OUTPUTS:     None
+**
+**  RETURNS:     MV_OK or error
+**
+*******************************************************************************/
+void mvOnuPonMacPrbsUserDefinedPatternSet(MV_U32 *prbsUserPattern)
+{
+  prbsUserDefinedPattern[0] = prbsUserPattern[0];
+  prbsUserDefinedPattern[1] = prbsUserPattern[1];
+  prbsUserDefinedPattern[2] = prbsUserPattern[2];
+}
+
+/*******************************************************************************
+**
+**  mvOnuPonMacPrbsUserDefinedPatternGet
+**  ____________________________________________________________________________
+**
+**  DESCRIPTION: The function return User defined PRBS pattern
+**
+**  PARAMETERS:  None
+**
+**  OUTPUTS:     None
+**
+**  RETURNS:     MV_OK or error
+**
+*******************************************************************************/
+void mvOnuPonMacPrbsUserDefinedPatternGet(MV_U32 *prbsUserPattern)
+{
+  prbsUserPattern[0] = prbsUserDefinedPattern[0];
+  prbsUserPattern[1] = prbsUserDefinedPattern[1];
+  prbsUserPattern[2] = prbsUserDefinedPattern[2];
+}
+
+/*******************************************************************************
+**
 **  mvOnuPonMacBurstEnableInit
 **  ____________________________________________________________________________
 **
@@ -342,7 +387,10 @@
 			     "ERROR: asicOntMiscRegWrite failed for PON phy SW ctrl not force\n\r");
 		       return(MV_ERROR);
 	       }
-	} else if (mvCtrlRevGet() == ONU_ASIC_REV_Z2) {
+	}
+	else
+	{
+		if (mvCtrlRevGet() == ONU_ASIC_REV_Z2) {
 
 		/* KW2 ASIC Rev Z2 */
 		/* =============== */
@@ -360,24 +408,26 @@
 		if (status == MV_OK)
 			status = mvGppTypeSet(gpioGroup, gpioMask, gpioMask /* 0-input NOT allow transsmit*/);
 
-	} else if (mvCtrlRevGet() == ONU_ASIC_REV_A0) {
+		}
+		else if (mvCtrlRevGet() == ONU_ASIC_REV_A0) {
 
 		/* KW2 ASIC Rev A0 */
 		/* =============== */
 		/* PHY control register - output status set */
-		status  = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_1_BEN_IO_EN, ONU_PHY_OUTPUT, 0);
-		if (status != MV_OK) {
-			mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
-				   "ERROR: asicOntMiscRegWrite failed for PON phy ctrl enable - output\n\r");
-			return(MV_ERROR);
-		}
+			status  = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_1_BEN_IO_EN, ONU_PHY_OUTPUT, 0);
+			if (status != MV_OK) {
+				mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
+					   "ERROR: asicOntMiscRegWrite failed for PON phy ctrl enable - output\n\r");
+				return(MV_ERROR);
+			}
 
-		/* PHY control register - force disable */
-		status  = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_1_FORCE_BEN_IO_EN, 0, 0);
-		if (status != MV_OK) {
-			mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
-				   "ERROR: asicOntMiscRegWrite failed for PON phy ctrl not force\n\r");
-			return(MV_ERROR);
+			/* PHY control register - force disable */
+			status  = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_1_FORCE_BEN_IO_EN, 0, 0);
+			if (status != MV_OK) {
+				mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
+					   "ERROR: asicOntMiscRegWrite failed for PON phy ctrl not force\n\r");
+				return(MV_ERROR);
+			}
 		}
 	}
 
@@ -519,53 +569,57 @@
 		       return(MV_ERROR);
 	       }
 
-	} else if (mvCtrlRevGet() == ONU_ASIC_REV_Z2) {
+	}
+	else
+	{
+		if (mvCtrlRevGet() == ONU_ASIC_REV_Z2) {
 
-	    	/* ASIC Rev Z2 */
-	    	/* =========== */
-		PON_GPIO_GET(BOARD_GPP_PON_XVR_TX, gpioGroup, gpioMask);
-		if (gpioMask == PON_GPIO_NOT_USED)
-			return MV_ERROR;
+			/* ASIC Rev Z2 */
+			/* =========== */
+			PON_GPIO_GET(BOARD_GPP_PON_XVR_TX, gpioGroup, gpioMask);
+			if (gpioMask == PON_GPIO_NOT_USED)
+				return MV_ERROR;
 
-		trans_value = ((on == MV_TRUE) ? (gpioMask/*1*/) : (~gpioMask/*0*/));
+			trans_value = ((on == MV_TRUE) ? (gpioMask/*1*/) : (~gpioMask/*0*/));
 
-		status = mvGppValueSet(gpioGroup, gpioMask, trans_value);
-		if (status != MV_OK)
-			return(MV_ERROR);
-
-	} else if (mvCtrlRevGet() == ONU_ASIC_REV_A0) {
-
-		/* ASIC Rev A0 */
-		/* =========== */
-		/* PHY control register - force enable */
-		status  = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_1_FORCE_BEN_IO_EN, 1, 0);
-		if (status != MV_OK) {
-			mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
-				   "ERROR: asicOntMiscRegWrite failed for PON phy ctrl force\n\r");
-			return(MV_ERROR);
+			status = mvGppValueSet(gpioGroup, gpioMask, trans_value);
+			if (status != MV_OK)
+				return(MV_ERROR);
 		}
+		else if	(mvCtrlRevGet() == ONU_ASIC_REV_A0) {
 
-        polarity = onuP2PDbXvrBurstEnablePolarityGet();
+			/* ASIC Rev A0 */
+			/* =========== */
+			/* PHY control register - force enable */
+			status  = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_1_FORCE_BEN_IO_EN, 1, 0);
+			if (status != MV_OK) {
+				mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
+					   "ERROR: asicOntMiscRegWrite failed for PON phy ctrl force\n\r");
+				return(MV_ERROR);
+			}
 
-		/* XVR polarity */
-		/* XVR polarity == 0, Active High, transmit 1 to the line  */
-		/* XVR polarity == 1, Active Low, transmit 0 to the line  */
+			polarity = onuP2PDbXvrBurstEnablePolarityGet();
 
-		/* P2P mode */
-		/* Force Value == 0, transmit 0 to the line  */
-		/* Force Value == 1, transmit 1 to the line  */
+			/* XVR polarity */
+			/* XVR polarity == 0, Active High, transmit 1 to the line  */
+			/* XVR polarity == 1, Active Low, transmit 0 to the line  */
 
-		/* Setting P2P should be reversed from XVR polarity */
-		/* XVR polarity == 0, Active High, write 1 for Force Value */
-		/* XVR polarity == 1, Active Low, write 0 for Force Value */
+			/* P2P mode */
+			/* Force Value == 0, transmit 0 to the line  */
+			/* Force Value == 1, transmit 1 to the line  */
 
-		/* PHY control register - force enable value - according to polarity */
-		status  = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_1_FORCE_BEN_IO_VAL, polarity, 0);
-		if (status != MV_OK) {
-			mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
-				   "ERROR: asicOntMiscRegWrite failed for PON phy ctrl force value %d\n\r",
-				   trans_value);
-			return(MV_ERROR);
+			/* Setting P2P should be reversed from XVR polarity */
+			/* XVR polarity == 0, Active High, write 1 for Force Value */
+			/* XVR polarity == 1, Active Low, write 0 for Force Value */
+
+			/* PHY control register - force enable value - according to polarity */
+			status  = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_1_FORCE_BEN_IO_VAL, polarity, 0);
+			if (status != MV_OK) {
+				mvPonPrint(PON_PRINT_ERROR, PON_ISR_MODULE,
+					   "ERROR: asicOntMiscRegWrite failed for PON phy ctrl force value %d\n\r",
+					   trans_value);
+				return(MV_ERROR);
+				}
 		}
 	}
 
@@ -605,6 +659,8 @@
 	/*pattern validation*/
 	if (!((pattern == ONU_PON_TX_PATTERN_TYPE_T1) ||
 	      (pattern == ONU_PON_TX_PATTERN_TYPE_T2) ||
+	      (pattern == ONU_PON_TX_PATTERN_TYPE_USER) ||
+          (pattern == ONU_PON_TX_PATTERN_TYPE_PRBS_7) ||
 	      (pattern == ONU_PON_TX_PATTERN_TYPE_PRBS_9) ||
 	      (pattern == ONU_PON_TX_PATTERN_TYPE_PRBS_15) ||
 	      (pattern == ONU_PON_TX_PATTERN_TYPE_PRBS_23))) {
@@ -663,10 +719,32 @@
 	if (status != MV_OK)
 		return(status);
 
-	/*set pattern type*/
-	status  = asicOntMiscRegWrite(mvAsicReg_PT_PATTERN_DATA, pattern, 0);
-	if (status != MV_OK)
-		return(status);
+    /*set pattern type*/
+    if (pattern != ONU_PON_TX_PATTERN_TYPE_USER)
+    {
+       status  = asicOntMiscRegWrite(mvAsicReg_PT_PATTERN_DATA, pattern, 0);
+       if (status != MV_OK)
+         return(status);
+    }
+    else
+    {
+       /*
+       **  0xF10A2E6C <- PHY Test Data[15:0];  [UserData[8:0], Type = 5}
+       **  0xF10A2E68 <- PHY Test Data[31:16]; [UserData[23:9]}
+       **  0xF10A2E64 <- PHY Test Data[47:32]; [UserData[39:24]}
+       **  0xF10A2E60 <- PHY Test Data[63:48]; [UserData[55:40]}
+       **  0xF10A2E5C <- PHY Test Data[79:64]; [UserData[71:56]}
+       **  0xF10A2E58 <- PHY Test Data[96:80]; [UserData[79:72]}
+       */
+       status  = asicOntMiscRegWrite(mvAsicReg_PT_PATTERN_DATA,         (((prbsUserDefinedPattern[0] &  0xFF) << 8)   | (pattern & 0xFF)), 0);
+       status |= asicOntMiscRegWrite(mvAsicReg_PT_PATTERN_USER_DATA_01,  ((prbsUserDefinedPattern[0] >> 8)  & 0xFFFF), 0);
+       status |= asicOntMiscRegWrite(mvAsicReg_PT_PATTERN_USER_DATA_02, (((prbsUserDefinedPattern[0] >> 24) & 0xFF)   | ((prbsUserDefinedPattern[1] & 0xFF) << 8)), 0);
+       status |= asicOntMiscRegWrite(mvAsicReg_PT_PATTERN_USER_DATA_03,  ((prbsUserDefinedPattern[1] >> 8)  & 0xFFFF), 0);
+       status |= asicOntMiscRegWrite(mvAsicReg_PT_PATTERN_USER_DATA_04, (((prbsUserDefinedPattern[1] >> 24) & 0xFF)   | ((prbsUserDefinedPattern[2] & 0xFF) << 8)), 0);
+       status |= asicOntMiscRegWrite(mvAsicReg_PT_PATTERN_USER_DATA_05,  ((prbsUserDefinedPattern[2] >> 8)  & 0xFF), 0);
+       if (status != MV_OK)
+         return(status);
+    }
 
 	/*turn on selected pattern*/
 	status  = asicOntMiscRegWrite(mvAsicReg_PT_PATTERN_ENABLED, 0x1, 0);
@@ -904,15 +982,15 @@
 **
 **  onuP2PDbXvrBurstEnablePolaritySet
 **  ____________________________________________________________________________
-** 
+**
 **  DESCRIPTION: The function sets EPON XVR polarity register value in the database
-**               
+**
 **  PARAMETERS:  MV_U32 val
 **
 **  OUTPUTS:     None
 **
 **  RETURNS:     MV_OK or error
-**                   
+**
 *******************************************************************************/
 MV_STATUS onuP2PDbXvrBurstEnablePolaritySet(MV_U32 val)
 {
@@ -925,15 +1003,15 @@
 **
 **  onuP2PDbXvrBurstEnablePolarityGet
 **  ____________________________________________________________________________
-** 
+**
 **  DESCRIPTION: The function returns EPON XVR polarity register value
-**               
+**
 **  PARAMETERS:  None
 **
 **  OUTPUTS:     None
 **
 **  RETURNS:     MV_U32 mode
-**                   
+**
 *******************************************************************************/
 MV_U32 onuP2PDbXvrBurstEnablePolarityGet(void)
 {
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/ponOnuBoard.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/ponOnuBoard.h
index 1077443..4819e79 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/ponOnuBoard.h
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/ponOnuBoard.h
@@ -93,7 +93,9 @@
 
 #define ONU_PON_TX_PATTERN_TYPE_T1        (0x1)
 #define ONU_PON_TX_PATTERN_TYPE_T2        (0x2)
-#define ONU_PON_TX_PATTERN_TYPE_PRBS_9    (0x80)
+#define ONU_PON_TX_PATTERN_TYPE_USER      (0x5)
+#define ONU_PON_TX_PATTERN_TYPE_PRBS_7    (0x80)
+#define ONU_PON_TX_PATTERN_TYPE_PRBS_9    (0x81)
 #define ONU_PON_TX_PATTERN_TYPE_PRBS_15   (0x82)
 #define ONU_PON_TX_PATTERN_TYPE_PRBS_23   (0x83)
 
@@ -155,6 +157,8 @@
 MV_STATUS onuPonLedHandler(MV_U32 led, MV_U32 action);
 MV_STATUS onuPonDyingGaspProcess(void);
 MV_STATUS onuPonDyingGaspExit(void);
+void      mvOnuPonMacPrbsUserDefinedPatternSet(MV_U32 *prbsUserPattern);
+void      mvOnuPonMacPrbsUserDefinedPatternGet(MV_U32 *prbsUserPattern);
 MV_STATUS mvOnuPonMacBurstEnableInit(void);
 MV_STATUS mvOnuPonMacBurstEnablePolarityInit(MV_U32 polarity);
 MV_STATUS onuPonPatternBurstOn(MV_U32 pattern, MV_BOOL isPeriodic, MV_U32 period, MV_U32 duration);
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/ponOnuLnxKsOs.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/ponOnuLnxKsOs.h
index 5205e1c..c3b1b64 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/ponOnuLnxKsOs.h
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/plat/ponOnuLnxKsOs.h
@@ -184,10 +184,13 @@
 #define ONU_PON_TIMER_HW_RPRT_T0_INTERVAL    (2)     /* 2 msec */

 #define ONU_PON_TIMER_HW_RPRT_T1_INTERVAL    (150)   /* 150 msec */

 #define ONU_PON_TIMER_HOLDOVER_INTERVAL      (200)   /* 200 msec */

+#define ONU_PON_TIMER_MAX_SLEEP_INTERVAL     (60000) /* 60 sec */

+#define ONU_PON_TIMER_SLEEP_DURATION         (500)    /* 0.5 sec */

+#define ONU_PON_TIMER_WAKEUP_DURATION        (500)    /* 0.5 sec */

 #define ONU_PON_TIMER_SILENCE_INTERVAL       (60000) /* 60 sec */

 #define ONU_PON_TIMER_TX_MOD_INTERVAL        (1)   /* 1 msec */

 #define ONU_PON_TIMER_EVENT_MISS_INTERVAL    (10)   /* 10 msec */

-#define ONU_PON_TIMER_TX_PWR_INTERVAL        (6000)  /* 1   sec  */

+#define ONU_PON_TIMER_TX_PWR_INTERVAL        (1000)  /* 1 sec */

 

 #define ONU_PON_TIMER_ACTIVE                 (1)

 #define ONU_PON_TIMER_NOT_ACTIVE             (0)

@@ -256,6 +259,11 @@
   S_OnuPonTimer onuPonTxPwrTimerId;         /* ONU PON TX Power timer */

   S_OnuPonTimer onuPonPmTimerId;            /* ONU PON PM timer */

   S_OnuPonTimer onuPonPatternBurstTimerId;  /* ONU PON Pattern Burst timer */

+

+    S_OnuPonTimer onuEponTxControlTimerId;      /* EPON tx_control delay timer */

+    S_OnuPonTimer onuEponMaxSleepTimerId;       /* EPON power_saving mode: max sleep duration timer */

+    S_OnuPonTimer onuEponSleepDurationTimerId;  /* EPON power_saving mode: sleep duration and wait duration timer */

+  

 #ifndef PON_FPGA                            

   S_OnuPonTimer onuPonIsrXvrRstTimerId;     /* ONU PON XVR Reset timer */

 #endif /* PON_FPGA */                       

@@ -275,6 +283,7 @@
 /* Global variables

 ------------------------------------------------------------------------------*/

 extern S_OnuPonResourceTbl onuPonResourceTbl_s;

+extern spinlock_t onuPonIrqLock;

 

 /* Global functions

 ------------------------------------------------------------------------------*/

@@ -315,6 +324,8 @@
 extern MV_STATUS ponOnuGetPrintStatus(MV_U32 module, MV_U32 *printLevel, MV_U32 *moduleOptions);

 extern int       ponOnuPrintStatus(char* buf);

 

+extern int 	 onuGponUiCfgPrbsUserPattern(char* buf);

+

 /* Macros

 ------------------------------------------------------------------------------*/    

 

diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_api.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_api.h
index 11fdd2d..62f8bf1 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_api.h
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_api.h
@@ -4181,6 +4181,37 @@
 *******************************************************************************/
 tpm_error_code_t tpm_deactive_tcont(uint32_t tcont_num);
 
+/*******************************************************************************
+* tpm_sw_set_static_multicast_mac
+*
+* DESCRIPTION:
+*       This function creates or destory a static MAC entry in the MAC address table for taget
+*       UNI port bitmap in the integrated switch
+*
+* INPUTS:
+*       owner_id    	- APP owner id, should be used for all API calls.
+*       tpm_trg_port    - target port bm in UNI port index, TPM_TRG_UNI_0 | TPM_TRG_UNI_1 | ...
+*       static_mac  	- 6 byte network order MAC source address.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success - TPM_RC_OK.
+*       On error different types are returned according to the case - see tpm_error_code_t.
+*
+* COMMENTS:
+*       None.
+*
+*******************************************************************************/
+tpm_error_code_t tpm_sw_set_static_multicast_mac
+(
+    uint32_t            owner_id,
+    tpm_trg_port_type_t tpm_trg_port,
+    uint8_t             static_mac[6]
+);
+
+
 /* OLD API functions */
 #define tpm_add_l2_prim_acl_rule                            tpm_add_l2_rule
 #define tpm_add_l3_type_acl_rule                            tpm_add_l3_type_rule
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_init.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_init.c
index 4cf3632..f3f38f2 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_init.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_init.c
@@ -3151,6 +3151,7 @@
 	uint32_t i;
 	uint32_t sw_port_bmp = 0;
 	unsigned char gq_da[6] = { 0x01, 0x00, 0x5e, 0x00, 0x00, 0x01 };
+	unsigned char gq_mld_da[6] = { 0x33, 0x33, 0x00, 0x00, 0x00, 0x01 };
 	tpm_gmacs_enum_t gmac_i;
 	int32_t switch_port;
 
@@ -3321,7 +3322,9 @@
 	}
 	if (mv_switch_mac_addr_set(gq_da, 0, sw_port_bmp, 1))
 		TPM_OS_WARN(TPM_INIT_MOD, "mv_switch_mac_addr_set err. sw_port_bmp 0X%x\n", sw_port_bmp);
-
+	if (mv_switch_mac_addr_set(gq_mld_da, 0, sw_port_bmp, 1))
+		TPM_OS_WARN(TPM_INIT_MOD, "mv_switch_mac_addr_set err.for mld general query sw_port_bmp 0X%x\n", sw_port_bmp);
+    
 	/* If Switch port MAC no connected to PHY, disable GMAC PHY polling */
 	for (gmac_i = TPM_ENUM_GMAC_0; gmac_i < TPM_MAX_NUM_GMACS; gmac_i++) {
 		if (TPM_FALSE == tpm_init.gmac_port_conf[gmac_i].valid)
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_pkt_proc_logic.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_pkt_proc_logic.c
index fd0a827..05af79d 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_pkt_proc_logic.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_pkt_proc_logic.c
@@ -84,9 +84,7 @@
 #include "tpm_common.h"
 #include "tpm_header.h"
 
-/* Local definitions */
-extern MV_STATUS mv_cust_set_tcont_state(uint32_t tcont, bool state);
-
+extern int cph_set_tcont_state(unsigned int tcont, bool state);
 typedef tpm_error_code_t (*tpm_proc_common_int_del_func_t) (uint32_t, uint32_t);
 
 int32_t tpm_proc_virt_uni_trg_port_validation(tpm_trg_port_type_t trg_port);
@@ -14250,7 +14248,7 @@
 		ret_code = tpm_db_api_entry_invalidate(api_section, stream_num);
 		IF_ERROR(ret_code);
 
-		
+
 		if (TPM_IP_VER_6 == ip_version) {
 			/* remove SIP PNC entry */
 			ret_code = tpm_db_get_ipv6_mc_stream_entry(stream_num, &mc_stream);
@@ -17825,7 +17823,7 @@
 
 	//TPM_OS_INFO(TPM_PNCL_MOD, "port = %d txp = %d enable = %d\n", port, txp, enable);
 
-    printk(KERN_DEBUG "TPM T-Cont API, port(%d), txp(%d) enable(%d)\n", port, txp, enable);
+	printk("TPM T-Cont API, port(%d), txp(%d) enable(%d)\n", port, txp, enable);
 
 	tx_mod = port + txp;
 
@@ -17855,7 +17853,7 @@
 
 			/* enable/disable hwf */
 			if (enable == true)
-                printk(KERN_DEBUG "HWF T-Cont active, port(%d), txp(%d), que(%d)\n", port, txp, q_num);
+				printk("HWF T-Cont active, port(%d), txp(%d), que(%d)\n", port, txp, q_num);
 
 			mvNetaHwfTxqEnable((queue_owner - 1), port, txp, q_num, enable);
 
@@ -17864,7 +17862,7 @@
 			if ((!enable) && (curr_state))
 			{
 				//TPM_OS_INFO(TPM_PNCL_MOD, "Call mv_eth_txq_clean - current state: %d, enable %d\n", curr_state, enable);
-                printk(KERN_DEBUG "HWF T-Cont stop/clean, port(%d), txp(%d), que(%d)\n", port, txp, q_num);
+				printk("HWF T-Cont stop/clean, port(%d), txp(%d), que(%d)\n", port, txp, q_num);
 				mv_eth_txq_clean(port, txp, q_num);
 			}
 		}
@@ -17874,14 +17872,14 @@
 		{
 			if (!enable)
 			{
-                printk(KERN_DEBUG "SWF T-Cont stop/clean, port(%d), txp(%d), que(%d)\n", port, txp, q_num);
-				mv_cust_set_tcont_state(txp, false);
+				printk("SWF T-Cont stop/clean, port(%d), txp(%d), que(%d)\n", port, txp, q_num);
+				cph_set_tcont_state(txp, false);
 				mv_eth_txq_clean(port, txp, q_num);
 			}
 			else
 			{
-                printk(KERN_DEBUG "SWF T-Cont active, port(%d), txp(%d), que(%d)\n", port, txp, q_num);
-				mv_cust_set_tcont_state(txp, true);
+				printk("SWF T-Cont active, port(%d), txp(%d), que(%d)\n", port, txp, q_num);
+				cph_set_tcont_state(txp, true);
 			}
 		}
 	}
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/perf/tpm_mng_if.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/perf/tpm_mng_if.c
index 6a467be..0b9e728 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/perf/tpm_mng_if.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/perf/tpm_mng_if.c
@@ -1695,6 +1695,16 @@
                   ret = 0;
                   break;
 
+              /* ====== MV_TPM_IOCTL_SW_SET_STATIC_MULTICAST_MAC ========= */
+              case MV_TPM_IOCTL_SW_SET_STATIC_MULTICAST_MAC:
+                  rcode = tpm_sw_set_static_multicast_mac(  tpm_sw_mac_security->owner_id,
+                                                  tpm_sw_mac_security->port_vector,
+                                                &(tpm_sw_mac_security->static_mac[0]));
+                  if(rcode != TPM_OK)
+                    goto ioctlErr;
+                  ret = 0;
+                  break;
+
               default:
                 ret = -EINVAL;
           }
@@ -3800,6 +3810,7 @@
 EXPORT_SYMBOL(tpm_sw_get_port_mirror);
 EXPORT_SYMBOL(tpm_sw_set_isolate_eth_port_vector);
 EXPORT_SYMBOL(tpm_sw_get_isolate_eth_port_vector);
+EXPORT_SYMBOL(tpm_sw_set_static_multicast_mac);
 EXPORT_SYMBOL(tpm_sw_set_port_tagged);
 EXPORT_SYMBOL(tpm_sw_set_port_untagged);
 EXPORT_SYMBOL(tpm_sw_set_port_def_vlan);
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/perf/tpm_mng_if.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/perf/tpm_mng_if.h
index 9ceffc6..61b0df8 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/perf/tpm_mng_if.h
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/perf/tpm_mng_if.h
@@ -319,6 +319,7 @@
     MV_TPM_IOCTL_ADD_IPv4_MC_STREAM_SET_QUEUE,
     MV_TPM_IOCTL_ADD_IPv6_MC_STREAM_SET_QUEUE,
     MV_TPM_IOCTL_SW_PORT_ADD_VID_SET_EGRESS_MODE,
+    MV_TPM_IOCTL_SW_SET_STATIC_MULTICAST_MAC,
 
 } tpm_ioctl_cmd_type_t;
 
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/plat/tpm_switch_mgr.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/plat/tpm_switch_mgr.c
index e62bce4..0ba9472 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/plat/tpm_switch_mgr.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/plat/tpm_switch_mgr.c
@@ -234,6 +234,113 @@
 }
 
 /*******************************************************************************
+* tpm_sw_set_gmac_speed_duplex_fc
+*
+* DESCRIPTION:
+*       This function will update GMAC Port Auto-Negotiation Configuration Register if PHY is
+*        connected to GMAC directly. Maily for Media Convert.
+*
+* INPUTS:
+*       src_port         - Source port in UNI port index, UNI0, UNI1...UNI4.
+*       auto_nego_en - Auto-negotiation enable status
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success - TPM_RC_OK.
+*       On error different types are returned according to the case - see tpm_error_code_t.
+*
+* COMMENTS:
+*        NONE.
+*******************************************************************************/
+static tpm_error_code_t tpm_sw_set_gmac_speed_duplex_fc(uint32_t owner_id,
+                                                        tpm_src_port_type_t src_port,
+                                                        bool auto_nego_en)
+{
+    tpm_error_code_t retVal = TPM_RC_OK;
+    tpm_phy_ctrl_t   phy_access_way;
+    uint32_t switch_init;
+    tpm_gmacs_enum_t gmac_port;
+    uint32_t phy_direct_addr;
+    tpm_phy_speed_t cfg_speed;
+    bool  cfg_duplex, cfg_fc;
+
+    retVal = tpm_phy_access_check(src_port, &phy_access_way, &phy_direct_addr);
+    if (retVal != TPM_RC_OK) {
+        printk(KERN_ERR
+               "Port%d PHY access way check failed\n", src_port);
+        return retVal;
+    }
+    /* check swicth init or not */
+    if (tpm_db_switch_init_get(&switch_init) != TPM_DB_OK) {
+        printk(KERN_ERR "ERROR: (%s:%d) tpm_db_switch_init_get Failed\n", __FUNCTION__, __LINE__);
+        retVal = ERR_GENERAL;
+        return retVal;
+    }
+    /* The routine only apply PHY connected to GMAC */
+    if ((PHY_SMI_MASTER_CPU == phy_access_way) &&
+         (!switch_init)) {
+        for (gmac_port = TPM_ENUM_GMAC_0; gmac_port < TPM_MAX_NUM_GMACS; gmac_port++) {
+            if (phy_direct_addr == mvBoardPhyAddrGet(gmac_port))
+            break;
+        }
+        if (gmac_port == TPM_MAX_NUM_GMACS) {
+            printk(KERN_ERR "ERROR: (%s:%d) Can not find gmac port\n", __FUNCTION__, __LINE__);
+            retVal = ERR_GENERAL;
+            return retVal;
+        }
+
+        if (auto_nego_en) {/* auto-nego enable */
+            if (mvNetaSpeedDuplexSet((int)gmac_port, MV_ETH_SPEED_AN, MV_ETH_DUPLEX_AN) != MV_OK) {
+                printk(KERN_ERR "ERROR: (%s:%d) mvNetaSpeedDuplexSet Failed\n", __FUNCTION__, __LINE__);
+                retVal = ERR_GENERAL;
+                return retVal;
+            }
+            if (mvNetaFlowCtrlSet((int)gmac_port, MV_ETH_FC_AN_NO)  != MV_OK){
+                printk(KERN_ERR "ERROR: (%s:%d) mvNetaFlowCtrlSet Failed\n", __FUNCTION__, __LINE__);
+                retVal = ERR_GENERAL;
+                return retVal;
+            }
+        } else {/* auto-nego disable */
+            retVal = tpm_phy_get_port_speed(owner_id, src_port, &cfg_speed);
+            if (retVal) {
+                printk(KERN_ERR "ERROR: (%s:%d) PHY speed config get failed\n", __FUNCTION__, __LINE__);
+                return retVal;
+            }
+            retVal = tpm_phy_get_port_duplex_mode(owner_id, src_port, &cfg_duplex);
+            if (retVal) {
+                printk(KERN_ERR "ERROR: (%s:%d) PHY duplex config get failed\n", __FUNCTION__, __LINE__);
+                return retVal;
+            }
+            retVal = tpm_phy_get_port_flow_control_support(owner_id, src_port, &cfg_fc);
+            if (retVal) {
+                printk(KERN_ERR "ERROR: (%s:%d) PHY flow control config get failed\n", __FUNCTION__, __LINE__);
+                return retVal;
+            }
+            if (mvNetaSpeedDuplexSet((int)gmac_port,
+                                     (cfg_speed + MV_ETH_SPEED_10),
+                                     (cfg_duplex ? MV_ETH_DUPLEX_FULL : MV_ETH_DUPLEX_HALF)) != MV_OK) {
+                printk(KERN_ERR "ERROR: (%s:%d) mvNetaSpeedDuplexSet Failed\n", __FUNCTION__, __LINE__);
+                retVal = ERR_GENERAL;
+                return retVal;
+            }
+            if (mvNetaFlowCtrlSet((int)gmac_port, (cfg_fc ? MV_ETH_FC_ENABLE : MV_ETH_FC_DISABLE)) != MV_OK) {
+                printk(KERN_ERR "ERROR: (%s:%d) mvNetaFlowCtrlSet Failed\n", __FUNCTION__, __LINE__);
+                retVal = ERR_GENERAL;
+                return retVal;
+            }
+        }
+    } else {
+        printk(KERN_ERR "PHY not connected to GMAC\n");
+        retVal = ERR_GENERAL;
+        return retVal;
+    }
+
+    return retVal;
+}
+
+/*******************************************************************************
 * tpm_sw_set_debug_trace_flag
 *
 * DESCRIPTION:
@@ -4981,6 +5088,12 @@
                 retVal = ERR_PHY_SRC_PORT_CONN_INVALID;
             }
         }
+        /* Update GMAC speed, duplex and flow control */
+        retVal = tpm_sw_set_gmac_speed_duplex_fc(owner_id, src_port, (state ? true : false));
+        if (retVal != TPM_RC_OK) {
+            printk(KERN_ERR "Failed to update GMAC config with port%d\n", src_port);
+            return retVal;
+        }
     } else {
         /* PHY accessed through switch, as original do */
         lPort = tpm_db_eth_port_switch_port_get(src_port);
@@ -6444,6 +6557,13 @@
             printk(KERN_ERR
                    "ERROR: (%s:%d) PHY LSP API call failed on port(%d)\n", __FUNCTION__, __LINE__, src_port);
             retVal = ERR_PHY_SRC_PORT_CONN_INVALID;
+        } else {
+            /* Update GMAC speed, duplex and flow control */
+            retVal = tpm_sw_set_gmac_speed_duplex_fc(owner_id, src_port, false);
+            if (retVal != TPM_RC_OK) {
+                printk(KERN_ERR "Failed to update GMAC config with port%d\n", src_port);
+                return retVal;
+            }
         }
     } else {
         /* PHY accessed through switch, as original do */
@@ -6619,6 +6739,13 @@
             printk(KERN_ERR
                    "ERROR: (%s:%d) PHY LSP API call failed on port(%d)\n", __FUNCTION__, __LINE__, src_port);
             retVal = ERR_PHY_SRC_PORT_CONN_INVALID;
+        } else {
+            /* Update GMAC speed, duplex and flow control */
+            retVal = tpm_sw_set_gmac_speed_duplex_fc(owner_id, src_port, false);
+            if (retVal != TPM_RC_OK) {
+                printk(KERN_ERR "Failed to update GMAC config with port%d\n", src_port);
+                return retVal;
+            }
         }
     } else {
         /* PHY accessed through switch, as original do */
@@ -7883,6 +8010,13 @@
             printk(KERN_ERR
                    "ERROR: (%s:%d) PHY LSP API call failed on port(%d)\n", __FUNCTION__, __LINE__, src_port);
             retVal = ERR_PHY_SRC_PORT_CONN_INVALID;
+        } else {
+            /* Update GMAC speed, duplex and flow control */
+            retVal = tpm_sw_set_gmac_speed_duplex_fc(owner_id, src_port, false);
+            if (retVal != TPM_RC_OK) {
+                printk(KERN_ERR "Failed to update GMAC config with port%d\n", src_port);
+                return retVal;
+            }
         }
     } else {
         /* PHY accessed through switch, as original do */
@@ -8036,3 +8170,70 @@
                 "==EXIT== %s\n\r",__FUNCTION__);
     }
 }
+
+/*******************************************************************************
+* tpm_sw_set_static_multicast_mac
+*
+* DESCRIPTION:
+*       This function creates or destory a static MAC entry in the MAC address table for taget
+*       UNI port bitmap in the integrated switch
+*
+* INPUTS:
+*       owner_id    	- APP owner id, should be used for all API calls.
+*       tpm_trg_port    - target port bm in UNI port index, TPM_TRG_UNI_0 | TPM_TRG_UNI_1 | ...
+*       static_mac  	- 6 byte network order MAC source address.
+*
+* OUTPUTS:
+*       None.
+*
+* RETURNS:
+*       On success - TPM_RC_OK.
+*       On error different types are returned according to the case - see tpm_error_code_t.
+*
+* COMMENTS:
+*       None.
+*
+*******************************************************************************/
+tpm_error_code_t tpm_sw_set_static_multicast_mac
+(
+    uint32_t            owner_id,
+    tpm_trg_port_type_t tpm_trg_port,
+    uint8_t             static_mac[6]
+)
+{
+    int32_t ret_code;
+    uint32_t trg_port_bm = 0;
+
+    SWITCH_INIT_CHECK();
+
+    if (trace_sw_dbg_flag)
+    {
+        printk(KERN_INFO
+               "==ENTER==%s: owner_id[%d],tpm_trg_port[0x%x],static_mac [0x%02x%02x%02x%02x%02x%02x]\n",
+               __FUNCTION__,owner_id,tpm_trg_port,static_mac[0],static_mac[1], static_mac[2],
+               static_mac[3],static_mac[4], static_mac[5]);
+    }
+
+	if((static_mac[0] != 0x01) && (static_mac[0] != 0x33 || static_mac[1] != 0x33))
+    {
+    	printk(KERN_ERR
+           "%s:%d: function failed, 0x%x:%x:%x:%x:%x:%x is not multicast address \r\n", 
+           __FUNCTION__,__LINE__, static_mac[0], static_mac[1], static_mac[2], static_mac[3], 
+           static_mac[4], static_mac[5]);
+    	IF_ERROR(ERR_SW_MAC_INVALID);
+    }
+    
+    trg_port_bm = tpm_db_trg_port_switch_port_get(tpm_trg_port);
+
+    if (tpm_trg_port == 0) {
+        ret_code = tpm_sw_del_static_mac(TPM_MOD_OWNER_TPM, static_mac);
+        IF_ERROR(ret_code);
+    } else {
+        ret_code = tpm_sw_set_static_mac_w_ports_mask(TPM_MOD_OWNER_TPM, trg_port_bm, static_mac);
+        IF_ERROR(ret_code);
+    }
+
+    return(TPM_RC_OK);
+}
+
+
diff --git a/arch/arm/plat-feroceon/mv_hal/pon/mvPonOnuMac.c b/arch/arm/plat-feroceon/mv_hal/pon/mvPonOnuMac.c
index 3d4d2d0..fbb575f 100755
--- a/arch/arm/plat-feroceon/mv_hal/pon/mvPonOnuMac.c
+++ b/arch/arm/plat-feroceon/mv_hal/pon/mvPonOnuMac.c
@@ -8197,6 +8197,30 @@
   return(status);
 }
 
+/*******************************************************************************
+**
+**  mvOnuEponMacSerdesPuRxWrite
+**  ____________________________________________________________________________
+**
+**  DESCRIPTION: The function write to serdes power up receiver 
+**
+**  PARAMETERS:  enable - 0: power down, 1:  power up
+**
+**  OUTPUTS:     None
+**
+**  RETURNS:     MV_OK or MV_ERROR
+**
+*******************************************************************************/
+MV_STATUS mvOnuEponMacSerdesPuRxWrite(MV_U32 enable)
+{
+    MV_STATUS status;
+
+    status = asicOntMiscRegWrite(mvAsicReg_PON_SERDES_PHY_CTRL_0_PU_RX, enable, 0);
+
+    return(status);
+}
+
+
 /******************************************************************************/
 /******************************************************************************/
 /* ========================================================================== */
diff --git a/arch/arm/plat-feroceon/mv_hal/pon/mvPonOnuMac.h b/arch/arm/plat-feroceon/mv_hal/pon/mvPonOnuMac.h
index 41c62b8..b115c1a 100755
--- a/arch/arm/plat-feroceon/mv_hal/pon/mvPonOnuMac.h
+++ b/arch/arm/plat-feroceon/mv_hal/pon/mvPonOnuMac.h
@@ -736,6 +736,11 @@
 MV_STATUS mvOnuEponMacCpqTxCtrlQueueFree(MV_U32 *freeCount, MV_U32 macId);
 MV_STATUS mvOnuEponMacCpqTxCtrlQueueWrite(MV_U32 data, MV_U32 macId);
 
+/* ========================================================================== */
+/*                        SERDES Functions Section                            */
+/* ========================================================================== */
+MV_STATUS mvOnuEponMacSerdesPuRxWrite(MV_U32 enable);
+
 /******************************************************************************/
 /******************************************************************************/
 /* ========================================================================== */
diff --git a/arch/arm/plat-feroceon/mv_hal/pon/mvPonOnuRegs.c b/arch/arm/plat-feroceon/mv_hal/pon/mvPonOnuRegs.c
index 8ce83f0..af39f42 100755
--- a/arch/arm/plat-feroceon/mv_hal/pon/mvPonOnuRegs.c
+++ b/arch/arm/plat-feroceon/mv_hal/pon/mvPonOnuRegs.c
@@ -711,7 +711,12 @@
   [mvAsicReg_PON_DG_THRESHOLD]                      = {mvAsicReg_PON_DG_THRESHOLD,                      0x18274,                         0x18274,  asicRW,     0x00000000,     0,      0,     0,      0,     "ONU Dying Gasp threshold"},
   [mvAsicReg_PT_PATTERN_SELECT]                     = {mvAsicReg_PT_PATTERN_SELECT,                     0xA2E54,                         0xA2E54,  asicRW,     0x00000007,     5,      0,     0,      0,     "PHY test pattern select"},
   [mvAsicReg_PT_PATTERN_ENABLED]                    = {mvAsicReg_PT_PATTERN_ENABLED,                    0xA2E54,                         0xA2E54,  asicRW,     0x00000001,     15,     0,     0,      0,     "PHY test pattern enabled"},
-  [mvAsicReg_PT_PATTERN_DATA]                       = {mvAsicReg_PT_PATTERN_DATA,                       0xA2E6C,                         0xA2E6C,  asicRW,     0x000000FF,     0,      0,     0,      0,     "PHY test pattern data"},
+  [mvAsicReg_PT_PATTERN_DATA]                       = {mvAsicReg_PT_PATTERN_DATA,                       0xA2E6C,                         0xA2E6C,  asicRW,     0x000000FF,     0,      0,     0,      0,     "PHY test pattern data / user pattern [15:0]"},
+  [mvAsicReg_PT_PATTERN_USER_DATA_01]   	        = {mvAsicReg_PT_PATTERN_USER_DATA_01,               0xA2E68,                         0xA2E68,  asicRW,     0x0000FFFF,     0,      0,     0,      0,     "PHY test user pattern [31:16]"},
+  [mvAsicReg_PT_PATTERN_USER_DATA_02]   	        = {mvAsicReg_PT_PATTERN_USER_DATA_02,               0xA2E64,                         0xA2E64,  asicRW,     0x0000FFFF,     0,      0,     0,      0,     "PHY test user pattern [47:32]"},
+  [mvAsicReg_PT_PATTERN_USER_DATA_03]   	        = {mvAsicReg_PT_PATTERN_USER_DATA_03,               0xA2E60,                         0xA2E60,  asicRW,     0x0000FFFF,     0,      0,     0,      0,     "PHY test user pattern [63:48]"},
+  [mvAsicReg_PT_PATTERN_USER_DATA_04]   	        = {mvAsicReg_PT_PATTERN_USER_DATA_04,               0xA2E5C,                         0xA2E5C,  asicRW,     0x0000FFFF,     0,      0,     0,      0,     "PHY test user pattern [79:64]"},
+  [mvAsicReg_PT_PATTERN_USER_DATA_05]   	        = {mvAsicReg_PT_PATTERN_USER_DATA_05,               0xA2E58,                         0xA2E58,  asicRW,     0x0000FFFF,     0,      0,     0,      0,     "PHY test user pattern [96:80]"},
   [mvAsicReg_GUNIT_TX_0_QUEUES]                     = {mvAsicReg_GUNIT_TX_0_QUEUES,                     0xA5900,                         0xA5900,  asicRW,     0xFFFFFFFF,     0,      8,     1,      0,     "GPON GUNIT Queues for TX 0"},
   [mvAsicReg_GUNIT_TX_1_QUEUES]                     = {mvAsicReg_GUNIT_TX_1_QUEUES,                     0xA5D00,                         0xA5D00,  asicRW,     0xFFFFFFFF,     0,      8,     1,      0,     "GPON GUNIT Queues for TX 1"},
   [mvAsicReg_GUNIT_TX_2_QUEUES]                     = {mvAsicReg_GUNIT_TX_2_QUEUES,                     0xA7900,                         0xA7900,  asicRW,     0xFFFFFFFF,     0,      8,     1,      0,     "GPON GUNIT Queues for TX 2"},
diff --git a/arch/arm/plat-feroceon/mv_hal/pon/mvPonOnuRegs.h b/arch/arm/plat-feroceon/mv_hal/pon/mvPonOnuRegs.h
index 96a6ba3..cde1418 100755
--- a/arch/arm/plat-feroceon/mv_hal/pon/mvPonOnuRegs.h
+++ b/arch/arm/plat-feroceon/mv_hal/pon/mvPonOnuRegs.h
@@ -607,33 +607,38 @@
   mvAsicReg_PT_PATTERN_SELECT                        = 457,
   mvAsicReg_PT_PATTERN_ENABLED                       = 458,
   mvAsicReg_PT_PATTERN_DATA                          = 459,
+  mvAsicReg_PT_PATTERN_USER_DATA_01                  = 460,
+  mvAsicReg_PT_PATTERN_USER_DATA_02                  = 461,
+  mvAsicReg_PT_PATTERN_USER_DATA_03                  = 462,
+  mvAsicReg_PT_PATTERN_USER_DATA_04                  = 463,
+  mvAsicReg_PT_PATTERN_USER_DATA_05                  = 464,
 
-  mvAsicReg_GUNIT_TX_0_QUEUES                        = 460,
-  mvAsicReg_GUNIT_TX_1_QUEUES                        = 461,
-  mvAsicReg_GUNIT_TX_2_QUEUES                        = 462,
-  mvAsicReg_GUNIT_TX_3_QUEUES                        = 463,
-  mvAsicReg_GUNIT_TX_4_QUEUES                        = 464,
-  mvAsicReg_GUNIT_TX_5_QUEUES                        = 465,
-  mvAsicReg_GUNIT_TX_6_QUEUES                        = 466,
-  mvAsicReg_GUNIT_TX_7_QUEUES                        = 467,
+  mvAsicReg_GUNIT_TX_0_QUEUES                        = 470,
+  mvAsicReg_GUNIT_TX_1_QUEUES                        = 471,
+  mvAsicReg_GUNIT_TX_2_QUEUES                        = 472,
+  mvAsicReg_GUNIT_TX_3_QUEUES                        = 473,
+  mvAsicReg_GUNIT_TX_4_QUEUES                        = 474,
+  mvAsicReg_GUNIT_TX_5_QUEUES                        = 475,
+  mvAsicReg_GUNIT_TX_6_QUEUES                        = 476,
+  mvAsicReg_GUNIT_TX_7_QUEUES                        = 477,
 
-  mvAsicReg_GUNIT_TX_0_PKT_MOD_MAX_HEAD_SIZE_CFG     = 470,
-  mvAsicReg_GUNIT_TX_1_PKT_MOD_MAX_HEAD_SIZE_CFG     = 471,
-  mvAsicReg_GUNIT_TX_2_PKT_MOD_MAX_HEAD_SIZE_CFG     = 472,
-  mvAsicReg_GUNIT_TX_3_PKT_MOD_MAX_HEAD_SIZE_CFG     = 473,
-  mvAsicReg_GUNIT_TX_4_PKT_MOD_MAX_HEAD_SIZE_CFG     = 474,
-  mvAsicReg_GUNIT_TX_5_PKT_MOD_MAX_HEAD_SIZE_CFG     = 475,
-  mvAsicReg_GUNIT_TX_6_PKT_MOD_MAX_HEAD_SIZE_CFG     = 476,
-  mvAsicReg_GUNIT_TX_7_PKT_MOD_MAX_HEAD_SIZE_CFG     = 477,
+  mvAsicReg_GUNIT_TX_0_PKT_MOD_MAX_HEAD_SIZE_CFG     = 480,
+  mvAsicReg_GUNIT_TX_1_PKT_MOD_MAX_HEAD_SIZE_CFG     = 481,
+  mvAsicReg_GUNIT_TX_2_PKT_MOD_MAX_HEAD_SIZE_CFG     = 482,
+  mvAsicReg_GUNIT_TX_3_PKT_MOD_MAX_HEAD_SIZE_CFG     = 483,
+  mvAsicReg_GUNIT_TX_4_PKT_MOD_MAX_HEAD_SIZE_CFG     = 484,
+  mvAsicReg_GUNIT_TX_5_PKT_MOD_MAX_HEAD_SIZE_CFG     = 485,
+  mvAsicReg_GUNIT_TX_6_PKT_MOD_MAX_HEAD_SIZE_CFG     = 486,
+  mvAsicReg_GUNIT_TX_7_PKT_MOD_MAX_HEAD_SIZE_CFG     = 487,
 
-  mvAsicReg_GUNIT_TX_0_PKT_MOD_STATS_PKT_COUNT       = 480,
-  mvAsicReg_GUNIT_TX_1_PKT_MOD_STATS_PKT_COUNT       = 481,
-  mvAsicReg_GUNIT_TX_2_PKT_MOD_STATS_PKT_COUNT       = 482,
-  mvAsicReg_GUNIT_TX_3_PKT_MOD_STATS_PKT_COUNT       = 483,
-  mvAsicReg_GUNIT_TX_4_PKT_MOD_STATS_PKT_COUNT       = 484,
-  mvAsicReg_GUNIT_TX_5_PKT_MOD_STATS_PKT_COUNT       = 485,
-  mvAsicReg_GUNIT_TX_6_PKT_MOD_STATS_PKT_COUNT       = 486,
-  mvAsicReg_GUNIT_TX_7_PKT_MOD_STATS_PKT_COUNT       = 487,
+  mvAsicReg_GUNIT_TX_0_PKT_MOD_STATS_PKT_COUNT       = 490,
+  mvAsicReg_GUNIT_TX_1_PKT_MOD_STATS_PKT_COUNT       = 491,
+  mvAsicReg_GUNIT_TX_2_PKT_MOD_STATS_PKT_COUNT       = 492,
+  mvAsicReg_GUNIT_TX_3_PKT_MOD_STATS_PKT_COUNT       = 493,
+  mvAsicReg_GUNIT_TX_4_PKT_MOD_STATS_PKT_COUNT       = 494,
+  mvAsicReg_GUNIT_TX_5_PKT_MOD_STATS_PKT_COUNT       = 495,
+  mvAsicReg_GUNIT_TX_6_PKT_MOD_STATS_PKT_COUNT       = 496,
+  mvAsicReg_GUNIT_TX_7_PKT_MOD_STATS_PKT_COUNT       = 497,
 
 #endif /* PON_FPGA */
   mvAsicReg_MAX_NUM_OF_REGS