diff --git a/arch/arm/mach-feroceon-kw2/core.c b/arch/arm/mach-feroceon-kw2/core.c
index d972a30..613d635 100755
--- a/arch/arm/mach-feroceon-kw2/core.c
+++ b/arch/arm/mach-feroceon-kw2/core.c
@@ -533,7 +533,7 @@
 #ifndef CONFIG_MV_DRAM_DEFAULT_ACCESS_CFG
         /* Support DRAM access configuration for Avanta-MC only */
         if (gBoardId == DB_88F6601_BP_ID || gBoardId == RD_88F6601_MC_ID ||
-	    gBoardId == RD_88F6601_MC2L_ID) {
+	    gBoardId == RD_88F6601_MC2L_ID || gBoardId == RD_88F6601_SFP_ID) {
                 printk("DRAM access: ");
 #ifdef CONFIG_MV_DRAM_XBAR_ACCESS_CFG
                 printk("XBAR\n");
diff --git a/arch/arm/mach-feroceon-kw2/include/mach/system.h b/arch/arm/mach-feroceon-kw2/include/mach/system.h
index 5b5a6b2..782a34b 100644
--- 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.7.25_RC32"
+#define UPON_SDK_VERSION "uPON_2.7.25_RC32+thunderbolt_v1"
 
 #ifdef __BIG_ENDIAN
 #define MV_ARM_32BIT_LE(X) ((((X)&0xff)<<24) |                       \
diff --git a/arch/arm/mach-feroceon-kw2/kw2_family/boardEnv/mvBoardEnvLib.c b/arch/arm/mach-feroceon-kw2/kw2_family/boardEnv/mvBoardEnvLib.c
index f358f00..a21298b 100755
--- a/arch/arm/mach-feroceon-kw2/kw2_family/boardEnv/mvBoardEnvLib.c
+++ b/arch/arm/mach-feroceon-kw2/kw2_family/boardEnv/mvBoardEnvLib.c
@@ -160,7 +160,7 @@
 	MV_REG_WRITE(GPP_DATA_OUT_REG(0), BOARD_INFO(boardId)->gppOutValLow);
 	MV_REG_WRITE(GPP_DATA_OUT_REG(1), BOARD_INFO(boardId)->gppOutValMid);
 	if (boardId != DB_88F6601_BP_ID && boardId != RD_88F6601_MC_ID &&
-	   boardId != RD_88F6601_MC2L_ID &&
+	   boardId != RD_88F6601_MC2L_ID && boardId != RD_88F6601_SFP_ID &&
 	   !gfiber_is_any_jack())
 		MV_REG_WRITE(GPP_DATA_OUT_REG(2), BOARD_INFO(boardId)->gppOutValHigh);
 
@@ -168,7 +168,7 @@
 	mvGppPolaritySet(0, 0xFFFFFFFF, BOARD_INFO(boardId)->gppPolarityValLow);
 	mvGppPolaritySet(1, 0xFFFFFFFF, BOARD_INFO(boardId)->gppPolarityValMid);
 	if (boardId != DB_88F6601_BP_ID && boardId != RD_88F6601_MC_ID &&
-	   boardId != RD_88F6601_MC2L_ID &&
+	   boardId != RD_88F6601_MC2L_ID && boardId != RD_88F6601_SFP_ID &&
 	   !gfiber_is_any_jack())
 		mvGppPolaritySet(2, 0xFFFFFFFF, BOARD_INFO(boardId)->gppPolarityValHigh);
 
@@ -176,7 +176,7 @@
 	mvGppTypeSet(0, 0xFFFFFFFF, BOARD_INFO(boardId)->gppOutEnValLow);
 	mvGppTypeSet(1, 0xFFFFFFFF, BOARD_INFO(boardId)->gppOutEnValMid);
 	if (boardId != DB_88F6601_BP_ID && boardId != RD_88F6601_MC_ID &&
-	    boardId != RD_88F6601_MC2L_ID &&
+	    boardId != RD_88F6601_MC2L_ID && boardId != RD_88F6601_SFP_ID &&
 	    !gfiber_is_any_jack())
 		mvGppTypeSet(2, 0xFFFFFFFF, BOARD_INFO(boardId)->gppOutEnValHigh);
 }
@@ -355,6 +355,14 @@
 			return MV_TRUE;
 		return MV_FALSE;
 	}
+	else if (boardId == RD_88F6601_SFP_ID)
+	{
+		if (ethPortNum == 0)
+			return MV_TRUE;
+		else
+			return MV_FALSE;
+	}
+
 	if (boardId == RD_88F6601_MC_ID || boardId == RD_88F6601_MC2L_ID ||
 	    gfiber_is_any_jack())
 		return MV_FALSE;
@@ -655,7 +663,7 @@
 		return MV_ERROR;
 	}
 	if (boardId == RD_88F6601_MC_ID || boardId == RD_88F6601_MC2L_ID ||
-	    boardId == DB_88F6601_BP_ID ||
+	    boardId == DB_88F6601_BP_ID || boardId == RD_88F6601_SFP_ID ||
 	    gfiber_is_any_jack())
 		return 0;
 
@@ -672,7 +680,7 @@
 			numPorts += 1;
 		if (ethCompOpt & (ESC_OPT_RGMIIA_SW_P5 | ESC_OPT_RGMIIA_SW_P6))
 			numPorts += 1;
-		if (ethCompOpt & ESC_OPT_SGMII_2_SW_P1)
+		if (ethCompOpt & ESC_OPT_SGMII)
 			numPorts += 1;
 	}
 
@@ -854,8 +862,9 @@
 			i = 3;
 		}
 		else if (ethCompOpt & ESC_OPT_SGMII_2_SW_P1) {
-			BOARD_INFO(boardId)->pSwitchInfo[swIdx].switchPort[0] = 1;
-			i = 1;
+			BOARD_INFO(boardId)->pSwitchInfo[swIdx].switchPort[0] = -1;
+			BOARD_INFO(boardId)->pSwitchInfo[swIdx].switchPort[1] = 0;
+			i = 2;
 		}
 
 		if (ethCompOpt & ESC_OPT_GEPHY_SW_P0)
@@ -1255,7 +1264,7 @@
 
 	tmpTClkRate = MV_REG_READ(MPP_SAMPLE_AT_RESET(0));
 	if (boardId == RD_88F6601_MC_ID || boardId == RD_88F6601_MC2L_ID ||
-	    boardId == DB_88F6601_BP_ID ||
+	    boardId == DB_88F6601_BP_ID || boardId == RD_88F6601_SFP_ID ||
 	    gfiber_is_any_jack()) {
 		tmpTClkRate &= MSAR_TCLCK_6601_MASK;
 		if (tmpTClkRate)
@@ -1312,7 +1321,7 @@
 	clockSatr = MSAR_CPU_DDR_L2_CLCK_EXTRACT(sar0);
 	i = 0;
 	if (boardId == RD_88F6601_MC_ID || boardId == RD_88F6601_MC2L_ID ||
-	    boardId == DB_88F6601_BP_ID ||
+	    boardId == DB_88F6601_BP_ID || boardId == RD_88F6601_SFP_ID ||
 	    gfiber_is_any_jack()) {
 		while (cpuDdrTbl6601[i].satrValue != -1) {
 			if (cpuDdrTbl6601[i].satrValue == clockSatr) {
@@ -1722,7 +1731,7 @@
 	BOARD_INFO(boardId)->pBoardMppTypeValue->ethSataComplexOpt = ethConfig;
 
 	if (boardId != DB_88F6601_BP_ID && boardId != RD_88F6601_MC_ID &&
-	    boardId != RD_88F6601_MC2L_ID &&
+	    boardId != RD_88F6601_MC2L_ID && boardId != RD_88F6601_SFP_ID &&
 	    !gfiber_is_any_jack()) {
 		/* KW2 only */
 		/* Update link speed for MAC0 / 1 */
@@ -2275,7 +2284,7 @@
 		if (mvBoardSmiScanModeGet(0) == 1)
 			mvOsOutput("       Switch in Single-Chip Address Mode.\n");
 	}
-
+                                 
 	/* 3xFE PHY */
 	if (ethConfig & ESC_OPT_FE3PHY)
 		mvOsOutput("       3xFE PHY Module.\n");
@@ -2355,7 +2364,7 @@
 		return MV_FALSE;
 	}
 	if (boardId == RD_88F6601_MC_ID || boardId == RD_88F6601_MC2L_ID ||
-	    gfiber_is_any_jack()) {
+	    boardId == RD_88F6601_SFP_ID || gfiber_is_any_jack()) {
 		if (ethPortNum == 0)
 			return MV_TRUE;
 		return MV_FALSE;
@@ -2758,6 +2767,8 @@
 		tmpBoardId = RD_88F6601_MC_ID;
 #elif defined(RD_88F6601MC2L)
 		tmpBoardId = RD_88F6601_MC2L_ID;
+#elif defined(RD_88F6601SFP)
+		tmpBoardId = RD_88F6601_SFP_ID;
 
 #elif defined(DB_CUSTOMER)
 		tmpBoardId = DB_CUSTOMER_ID;
@@ -3181,7 +3192,7 @@
 		if (0 == MV_BOARD_6601_CFG_FXS(boardCfg))
 			*modules |= MV_BOARD_MODULE_TDM_1_ID;
 
-		/* Configure XCVR mux */
+		/*   Configure XCVR mux */
 		twsiSlave.slaveAddr.address = MV_BOARD_XCVR_MUX_ADDR;
 		twsiSlave.slaveAddr.type = MV_BOARD_XCVR_MUX_ADDR_TYPE;
 		twsiSlave.validOffset = MV_TRUE;
diff --git a/arch/arm/mach-feroceon-kw2/kw2_family/boardEnv/mvBoardEnvSpec.c b/arch/arm/mach-feroceon-kw2/kw2_family/boardEnv/mvBoardEnvSpec.c
index 7b41233..597be00 100755
--- a/arch/arm/mach-feroceon-kw2/kw2_family/boardEnv/mvBoardEnvSpec.c
+++ b/arch/arm/mach-feroceon-kw2/kw2_family/boardEnv/mvBoardEnvSpec.c
@@ -1097,6 +1097,125 @@
 };
 
 /***************************************************************************
+** RD-88F6601SFP Avanta
+****************************************************************************/
+/* NAND not supported  */
+
+MV_BOARD_TWSI_INFO rd88f6601SFPInfoBoardTwsiDev[] = {
+	/* {{MV_BOARD_DEV_CLASS devClass, MV_U8 twsiDevAddr, MV_U8 twsiDevAddrType}} */
+};
+
+MV_BOARD_MAC_INFO rd88f6601SFPInfoBoardMacInfo[] = {
+	/* {{MV_BOARD_MAC_SPEED boardMacSpeed, MV_U8 boardEthSmiAddr}} */
+	{BOARD_MAC_SPEED_AUTO, 0x0},
+	{BOARD_MAC_SPEED_AUTO, 0x1},
+	{N_A,N_A}
+};
+
+MV_BOARD_MPP_TYPE_INFO rd88f6601SFPInfoBoardMppTypeInfo[] = {
+	{
+		.boardMppTdm = MV_BOARD_AUTO,
+		.ethSataComplexOpt = ESC_OPT_SGMII,
+		.ethPortsMode = 0x0,
+	}
+};
+
+MV_BOARD_GPP_INFO rd88f6601SFPInfoBoardGppInfo[] = {
+	/* {{MV_BOARD_GPP_CLASS devClass, MV_U8 gppPinNum}} */
+//	{BOARD_GPP_PON_XVR_TX, 17},
+	{BOARD_GPP_PON_XVR_TX, 30},
+	{BOARD_GPP_PON_XVR_TX_POWER, 37}, /* should be MPP 9 at FIT design, but now MPP9 is for UART */
+	{BOARD_GPP_PON_XVR_TX_IND, 24},
+};
+
+MV_DEV_CS_INFO rd88f6601SFPInfoBoardDeCsInfo[] = {
+	/*{deviceCS, params, devType, devWidth} */
+#ifdef MV_SPI
+	{SPI_CS0, N_A, BOARD_DEV_SPI_FLASH, 8},		/* SPI DEV */
+#endif
+#if !defined(MV_SPI)
+	{N_A, N_A, N_A, N_A}			/* No device */
+#endif
+};
+
+MV_BOARD_MPP_INFO rd88f6601SFPInfoBoardMppConfigValue[] = {
+	{{
+	  RD_88F6601SFP_MPP0_7,
+	  RD_88F6601SFP_MPP8_15,
+	  RD_88F6601SFP_MPP16_23,
+	  RD_88F6601SFP_MPP24_31,
+	  RD_88F6601SFP_MPP32_37
+	  }
+	 }
+};
+
+MV_BOARD_INFO rd88f6601SFPInfo = {
+	.boardName = "RD-88F6601-BP-FIT-GPON_SFP",
+	.numBoardMppTypeValue = MV_ARRAY_SIZE(rd88f6601SFPInfoBoardMppTypeInfo),
+	.pBoardMppTypeValue = rd88f6601SFPInfoBoardMppTypeInfo,
+	.numBoardMppConfigValue = MV_ARRAY_SIZE(rd88f6601SFPInfoBoardMppConfigValue),
+	.pBoardMppConfigValue = rd88f6601SFPInfoBoardMppConfigValue,
+	.intsGppMaskLow = 0,
+	.intsGppMaskMid = 0,
+	.intsGppMaskHigh = 0,
+	.numBoardDeviceIf = MV_ARRAY_SIZE(rd88f6601SFPInfoBoardDeCsInfo),
+	.pDevCsInfo = rd88f6601SFPInfoBoardDeCsInfo,
+	.numBoardTwsiDev = MV_ARRAY_SIZE(rd88f6601SFPInfoBoardTwsiDev),
+	.pBoardTwsiDev = rd88f6601SFPInfoBoardTwsiDev,
+	.numBoardMacInfo = MV_ARRAY_SIZE(rd88f6601SFPInfoBoardMacInfo),
+	.pBoardMacInfo = rd88f6601SFPInfoBoardMacInfo,
+	.numBoardGppInfo = MV_ARRAY_SIZE(rd88f6601SFPInfoBoardGppInfo),
+	.pBoardGppInfo = rd88f6601SFPInfoBoardGppInfo,
+	.activeLedsNumber = 0,
+	.pLedGppPin = NULL,
+	.ledsPolarity = 0,
+
+	/* GPP values */
+	.gppOutEnValLow = RD_88F6601SFP_GPP_OUT_ENA_LOW,
+	.gppOutEnValMid = RD_88F6601SFP_GPP_OUT_ENA_MID,
+	.gppOutEnValHigh = 0, // Does not exist in 6601
+	.gppOutValLow = RD_88F6601SFP_GPP_OUT_VAL_LOW,
+	.gppOutValMid = RD_88F6601SFP_GPP_OUT_VAL_MID,
+	.gppOutValHigh = 0,
+	.gppPolarityValLow = RD_88F6601SFP_GPP_POL_LOW,
+	.gppPolarityValMid = RD_88F6601SFP_GPP_POL_MID,
+	.gppPolarityValHigh = 0,
+
+	/* External Switch Configuration */
+	.pSwitchInfo = NULL,
+	.switchInfoNum = 0,
+
+	/* PON configuration. */
+	.ponConfigValue = BOARD_GPON_CONFIG,
+
+	/* TDM configuration */
+	/* We hold a different configuration array for each possible slic that
+	 ** can be connected to board.
+	 ** When modules are scanned, then we select the index of the relevant
+	 ** slic's information array.
+	 ** For RD and Customers boards we only need to initialize a single
+	 ** entry of the arrays below, and set the boardTdmInfoIndex to 0.
+	 */
+	.numBoardTdmInfo = {0},
+	.pBoardTdmInt2CsInfo = {NULL},
+	.boardTdmInfoIndex = -1,
+
+	.pBoardSpecInit = NULL,
+
+	.deepIdlePwrUpDelay = 2400,	/* 12uS */
+
+	/* NAND init params */
+	.nandFlashParamsValid = MV_FALSE,
+	.nandFlashReadParams = 0,
+	.nandFlashWriteParams = 0,
+	.nandFlashControl = 0,
+	.pBoardTdmSpiInfo = NULL,
+
+	/* Enable modules auto-detection. */
+	.moduleAutoDetect = MV_FALSE
+};
+
+/***************************************************************************
 ** DB-88F6601-BP Avanta
 ****************************************************************************/
 /* NAND not supported  */
@@ -1877,6 +1996,7 @@
 	&gflt200Info,
 	&gflt110Info,
 	&rd88f6601MC2LInfo,
+	&rd88f6601SFPInfo,
 	&gflt300Info,
 	&gflt400Info,
 };
diff --git a/arch/arm/mach-feroceon-kw2/kw2_family/boardEnv/mvBoardEnvSpec.h b/arch/arm/mach-feroceon-kw2/kw2_family/boardEnv/mvBoardEnvSpec.h
index b739ac7..436f6ae 100755
--- a/arch/arm/mach-feroceon-kw2/kw2_family/boardEnv/mvBoardEnvSpec.h
+++ b/arch/arm/mach-feroceon-kw2/kw2_family/boardEnv/mvBoardEnvSpec.h
@@ -115,9 +115,10 @@
 #define GFLT200_ID			DB_CUSTOMER_ID
 #define GFLT110_ID			(BOARD_ID_BASE+0x8)
 #define RD_88F6601_MC2L_ID		(BOARD_ID_BASE+0x9)
-#define GFLT300_ID			(BOARD_ID_BASE+0xA)
-#define GFLT400_ID			(BOARD_ID_BASE+0xB)
-#define MV_MAX_BOARD_ID			(DB_CUSTOMER_ID + 5)
+#define RD_88F6601_SFP_ID		(BOARD_ID_BASE+0xA)
+#define GFLT300_ID			(BOARD_ID_BASE+0xB)
+#define GFLT400_ID			(BOARD_ID_BASE+0xC)
+#define MV_MAX_BOARD_ID			(DB_CUSTOMER_ID + 6)
 // These are Marvells defines, we should switch to match these in
 // uboot to make future merges easier.
 //#define DB_CUSTOMER_ID			(BOARD_ID_BASE+0x8)
@@ -441,6 +442,7 @@
 #define RD_88F6601_MPP24_31		0x00200650
 #define RD_88F6601_MPP32_37		0x00000000
 
+
 /* GPPs
  1 SPI0_MOSI (out)
  2 SPI0_SCK (out)
@@ -734,4 +736,44 @@
 #define RD_88F6601MC2L_GPP_POL_LOW		(BIT15)
 #define RD_88F6601MC2L_GPP_POL_MID		0x0
 
+/***************************************************************************
+** RD-88F6601-SFP
+****************************************************************************/
+
+#define RD_88F6601SFP_MPP0_7		0x22222220
+#define RD_88F6601SFP_MPP8_15		0x00000002
+#define RD_88F6601SFP_MPP16_23		0x00000000
+#define RD_88F6601SFP_MPP24_31		0x04000000
+#define RD_88F6601SFP_MPP32_37		0x00050000
+
+
+/* GPPs
+ 1 SPI0_MOSI (out)
+ 2 SPI0_SCK (out)
+ 3 SPI0_CSn[0] (out)
+ 4 SPI0_MISO (in)
+ 5 I2C0_SDA (inout)
+ 6 I2C0_SCK (inout)
+ 7 UA0_TXD (out)
+ 8 UA0_RXD (in)
+20 LED_PON
+21 PON_BEN (out)
+24 XVR_Tx_IND
+25 LED_G
+26 LED_Y
+28 NF&SPI_WP
+29 XVR_SD (in)
+33 TX_Fault/TX_indication
+37 TX_PD
+*/
+
+#define RD_88F6601SFP_GPP_OUT_ENA_LOW	~(0x0)
+#define RD_88F6601SFP_GPP_OUT_ENA_MID	~(BIT5)
+
+#define RD_88F6601SFP_GPP_OUT_VAL_LOW	0x0
+#define RD_88F6601SFP_GPP_OUT_VAL_MID	0x0
+
+#define RD_88F6601SFP_GPP_POL_LOW		0x0
+#define RD_88F6601SFP_GPP_POL_MID		0x0
+
 #endif /* __INCmvBoardEnvSpech */
diff --git a/arch/arm/mach-feroceon-kw2/kw2_family/cpu/mvCpu.c b/arch/arm/mach-feroceon-kw2/kw2_family/cpu/mvCpu.c
index 3155d5a..1b2fbb1 100755
--- a/arch/arm/mach-feroceon-kw2/kw2_family/cpu/mvCpu.c
+++ b/arch/arm/mach-feroceon-kw2/kw2_family/cpu/mvCpu.c
@@ -113,7 +113,7 @@
 	/* Search for a matching entry */
 	i = 0;
 	if (boardId == RD_88F6601_MC_ID || boardId == RD_88F6601_MC2L_ID ||
-	    boardId == DB_88F6601_BP_ID ||
+	    boardId == DB_88F6601_BP_ID || boardId == RD_88F6601_SFP_ID ||
 	    gfiber_is_any_jack()) {
 		while (cpuDdrTbl6601[i].satrValue != -1) {
 			if (cpuDdrTbl6601[i].satrValue == clockSatr) {
@@ -165,6 +165,7 @@
 		if (boardId == RD_88F6601_MC_ID ||
 		    boardId == RD_88F6601_MC2L_ID ||
 		    boardId == DB_88F6601_BP_ID ||
+		    boardId == RD_88F6601_SFP_ID ||
 		    gfiber_is_any_jack())
 			return cpuDdrTbl6601[idx].cpuClk;
 		else
@@ -201,6 +202,7 @@
 		if (boardId == RD_88F6601_MC_ID ||
 		    boardId == RD_88F6601_MC2L_ID ||
 		    boardId == RD_88F6601_MC_ID ||
+		    boardId == RD_88F6601_SFP_ID ||
 		    gfiber_is_any_jack())
 			return cpuDdrTbl6601[idx].l2Clk;
 		else
@@ -229,6 +231,7 @@
 
 	if (id == RD_88F6510_SFU_ID || id == DB_88F6601_BP_ID ||
 	    id == RD_88F6601_MC_ID || id == RD_88F6601_MC2L_ID ||
+	    id == RD_88F6601_SFP_ID ||
 	    gfiber_is_any_jack())
 		return MV_FALSE;
 
@@ -363,24 +366,24 @@
 	MV_8 Cpu_Stream_str[MV_PROC_STR_SIZE];
 
 	if (mvCtrlModelGet() != MV_6601_DEV_ID) {
-		mvCpuIfGetL2Mode(L2_En_str);
-		mvCpuIfGetL2EccMode(L2_ECC_str);
-		mvCpuIfGetL2PrefetchMode(L2_Prefetch_str);
+	mvCpuIfGetL2Mode(L2_En_str);
+	mvCpuIfGetL2EccMode(L2_ECC_str);
+	mvCpuIfGetL2PrefetchMode(L2_Prefetch_str);
 	}
 	mvCpuIfGetWriteAllocMode(Write_Alloc_str);
 	mvCpuIfGetCpuStreamMode(Cpu_Stream_str);
 	mvCpuIfPrintCpuRegs();
 
 	if (mvCtrlModelGet() != MV_6601_DEV_ID) {
-		count += mvOsSPrintf(buffer + count + index, "%s\n", L2_En_str);
-		count += mvOsSPrintf(buffer + count + index, "%s\n", L2_ECC_str);
-		count += mvOsSPrintf(buffer + count + index, "%s\n", L2_Prefetch_str);
+	count += mvOsSPrintf(buffer + count + index, "%s\n", L2_En_str);
+	count += mvOsSPrintf(buffer + count + index, "%s\n", L2_ECC_str);
+	count += mvOsSPrintf(buffer + count + index, "%s\n", L2_Prefetch_str);
 	}
 	count += mvOsSPrintf(buffer + count + index, "%s\n", Write_Alloc_str);
 	count += mvOsSPrintf(buffer + count + index, "%s\n", Cpu_Stream_str);
 	count += mvOsSPrintf(buffer + count + index, "CPU Config Reg = 0x%08x\n", MV_REG_READ(CPU_CONFIG_REG));
 	if (mvCtrlModelGet() != MV_6601_DEV_ID)
-		count += mvOsSPrintf(buffer + count + index, "L2 Config Reg = 0x%08x\n", MV_REG_READ(CPU_L2_CONFIG_REG));
+	count += mvOsSPrintf(buffer + count + index, "L2 Config Reg = 0x%08x\n", MV_REG_READ(CPU_L2_CONFIG_REG));
 
 	return count;
 }
diff --git a/arch/arm/mach-feroceon-kw2/kw2_family/ctrlEnv/mvCtrlEnvRegs.h b/arch/arm/mach-feroceon-kw2/kw2_family/ctrlEnv/mvCtrlEnvRegs.h
index c36c68e..69baa40 100755
--- a/arch/arm/mach-feroceon-kw2/kw2_family/ctrlEnv/mvCtrlEnvRegs.h
+++ b/arch/arm/mach-feroceon-kw2/kw2_family/ctrlEnv/mvCtrlEnvRegs.h
@@ -518,6 +518,26 @@
 #define THERMAL_TEMPERATURE_OFFSET 10
 #define THERMAL_TEMPERATURE_MASK (0x1FF << THERMAL_TEMPERATURE_OFFSET)
 
+// LP SERDES PHY AND NETWORKING REGISTERS
+#define ETHERNET_COMPLEX_CTRL_REG_0()		(0x18810)
+#define SOFTWARE_RESET_CTRL_REG()			(0x18220)
+#define GE_MAC_CTRL_REG(port)				(0x72C00 + (port * 0x4000))
+#define ETH_UNIT_CTRL_REG(port)				(0x720B0 + (port * 0x4000))
+#define PORT_MAC_CTRL_REG2(port)			(0x72C08 + (port * 0x4000))
+#define PORT_AUTO_NEG_CTRL_REG(port)		(0x72C0C + (port * 0x4000))
+#define SYNC_PATTERN_REG()					(0x72E90)
+#define SERDES_CONFIG_REG(port)				(0x724A0 + (port * 0x4000))
+#define SERDES_STATUS_REG(port)				(0x724A4 + (port * 0x4000))
+#define ONEMS_CLK_DIVIDER_CTRL_REG(port)	(0x724F4 + (port * 0x4000))
+#define POWER_PLL_CTRL_REG()				(0x72E04)
+#define KVCO_CALIBRATION_CTRL_REG()			(0x72E08)
+#define IMPEDANCE_CALIBRATION_CTRL_REG()	(0x72E0C)
+#define GENERATION_1_SETTING_0_REG()		(0x72E34)
+#define GENERATION_1_SETTING_1_REG()		(0x72E38)
+#define DIGITAL_LOOPBACK_ENABLE_REG()		(0x72E8C)
+#define PHY_ISOLATION_MODE_CTRL_REG()		(0x72E98)
+
+
 #endif /* MV_ASMLANGUAGE */
 
 #ifdef __cplusplus
diff --git a/arch/arm/mach-feroceon-kw2/kw2_family/ctrlEnv/mvCtrlEthCompLib.c b/arch/arm/mach-feroceon-kw2/kw2_family/ctrlEnv/mvCtrlEthCompLib.c
index dc7c5b6..d13f4b6 100755
--- a/arch/arm/mach-feroceon-kw2/kw2_family/ctrlEnv/mvCtrlEthCompLib.c
+++ b/arch/arm/mach-feroceon-kw2/kw2_family/ctrlEnv/mvCtrlEthCompLib.c
@@ -1553,13 +1553,16 @@
 	mvEthernetComplexPreInit(ethCompCfg);
 
 	if (mvCtrlModelGet() == MV_6601_DEV_ID) {
+	#if 1
+		printk("MTL: mvEthCompMac0ToSgmiiConfig bypassed, ethCompCfg=0x%x\n", ethCompCfg);
 		
+	#else
 		/*  MAC0 to GE PHY. */
 		mvEthCompMac0ToGePhyConfig(ethCompCfg, mvBoardPhyAddrGet(0));
 
 		/*  MAC0/1 to SGMII. */
 		mvEthCompMac0ToSgmiiConfig(ethCompCfg);
-
+	#endif
 	} else {
 
 		/*  First, initialize the switch. */
diff --git a/arch/arm/mach-feroceon-kw2/sysmap.c b/arch/arm/mach-feroceon-kw2/sysmap.c
index c6e8fda..36673d4 100755
--- a/arch/arm/mach-feroceon-kw2/sysmap.c
+++ b/arch/arm/mach-feroceon-kw2/sysmap.c
@@ -228,6 +228,7 @@
 		case DB_88F6601_BP_ID:
 		case RD_88F6601_MC_ID:
 		case RD_88F6601_MC2L_ID:
+		case RD_88F6601_SFP_ID:
 			return SYSMAP_88F6601;
 		default:
 			printk("ERROR: can't find system address map\n");
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
index 2fe434a..48a59e7 100755
--- 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
@@ -125,6 +125,8 @@
     { 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"},     
+    { TPM_PON_WAN_G0_SINGLE_PORT_SGMII, "TPM_PON_WAN_G0_SINGLE_PORT_SGMII"},
+    { TPM_PON_G1_SGMII_WAN_G0_SINGLE_PORT, "TPM_PON_G1_SGMII_WAN_G0_SINGLE_PORT"},
 };
 
 static MV_ENUM_ARRAY_T g_enum_array_profile_id =
@@ -218,9 +220,9 @@
     MV_STATUS rc = MV_OK;
 
     /* Check the range of profile_id */
-    if (profile_id > TPM_PON_WAN_G0_G1_DUAL_LAN)
+    if (profile_id >= TPM_COMPLEX_PROFILE_LAST)
     {
-        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);
+        MV_CPH_PRINT(CPH_ERR_LEVEL, "profile_id[%d] is out of range[1~%d] \n", profile_id, TPM_COMPLEX_PROFILE_LAST-1);
         return MV_OUT_OF_RANGE;
     }
 
@@ -776,6 +778,7 @@
             break;
         case TPM_PON_WAN_G0_INT_SWITCH:
         case TPM_PON_WAN_G0_SINGLE_PORT:            
+        case TPM_PON_WAN_G0_SINGLE_PORT_SGMII:
         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;
@@ -798,7 +801,8 @@
             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:            
+        case TPM_PON_G1_WAN_G0_SINGLE_PORT:
+        case TPM_PON_G1_SGMII_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;
@@ -1275,6 +1279,8 @@
     BOOL                  state     = FALSE;
     struct sk_buff       *skb_old   = NULL;
     struct sk_buff       *skb_new   = NULL;
+    tpm_eth_complex_profile_t  profile_id  = 0;
+    MV_APP_GMAC_PORT_E         active_port = 0;
     MV_STATUS             rc        = MV_OK;
     
     /* Check whether need to handle broadcast packet */
@@ -1316,6 +1322,16 @@
                 skb_new = skb_old;
                 goto out;
             }
+            /* If WAN interface is GMAC1, remove MH in upstream  */
+            cph_db_get_param(CPH_DB_PARAM_PROFILE_ID, &profile_id);
+            cph_db_get_param(CPH_DB_PARAM_ACTIVE_PORT, &active_port);
+            if ((profile_id == TPM_PON_G1_SGMII_WAN_G0_SINGLE_PORT) && (active_port == MV_APP_GMAC_PORT_1))
+            {
+              skb_old->data += MV_ETH_MH_SIZE;
+              skb_old->tail -= MV_ETH_MH_SIZE;
+              skb_old->len  -= MV_ETH_MH_SIZE;
+          }
+
             mv_net_devs[peer_port]->netdev_ops->ndo_start_xmit(skb_old, mv_net_devs[peer_port]);      
         }
     }
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
index 68a0358..4554a0f 100755
--- 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
@@ -191,7 +191,10 @@
     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_PON_WAN_G0_G1_DUAL_LAN,
+    TPM_PON_WAN_G0_SINGLE_PORT_SGMII,
+    TPM_PON_G1_SGMII_WAN_G0_SINGLE_PORT,
+    TPM_COMPLEX_PROFILE_LAST,
 } tpm_eth_complex_profile_t;
 
 /* PON type definition
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_mtd/sflash.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_mtd/sflash.c
index cd7c20a..08463e1 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_mtd/sflash.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_mtd/sflash.c
@@ -185,6 +185,7 @@
 	DB(printk("\n           Device ID       : 0x%04x", sflash->deviceId));
 	DB(printk("\n           Sector Size     : 0x%x", sflash->sectorSize));
 	DB(printk("\n           Sector Number   : %d", sflash->sectorNumber));
+	DB(printk("\n           Name   : %s", mtd->name));
 	
 	printk("SPI Serial flash detected @ 0x%08x, %dKB (%dsec x %dKB)\n",
 	         sflash->baseAddr, ((sflash->sectorNumber * sflash->sectorSize)/1024), 
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 ac63e63..a976a8a 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
@@ -543,6 +543,46 @@
 
 /*******************************************************************************
 **
+**  onuGponDbXvrSdPolaritySet
+**  ____________________________________________________________________________
+**
+**  DESCRIPTION: The function set onu XVR SD polarity
+**
+**  PARAMETERS:  MV_U32 state
+**
+**  OUTPUTS:	None
+**
+**  RETURNS:	MV_OK or error
+**
+*******************************************************************************/
+MV_STATUS onuGponDbXvrSdPolaritySet(MV_U32 polarity)
+{
+  onuGponDb_s.onuGponGenTbl_s.onuGponXvrSdPolarity = polarity;
+
+  return(MV_OK);
+}
+
+/*******************************************************************************
+**
+**  onuGponDbXvrSdPolarityGet
+**  ____________________________________________________________________________
+**
+**  DESCRIPTION: The function return XVR SD polarity
+**
+**  PARAMETERS:  None
+**
+**  OUTPUTS:	None
+**
+**  RETURNS:	XVR SD polarity 
+**
+*******************************************************************************/
+MV_U32 onuGponDbXvrSdPolarityGet(void)
+{
+  return(onuGponDb_s.onuGponGenTbl_s.onuGponXvrSdPolarity);
+}
+
+/*******************************************************************************
+**
 **  onuGponDbOnuIdOverrideValueGet
 **  ____________________________________________________________________________
 **
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 9144514..b32b60c 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
@@ -134,6 +134,7 @@
   MV_U32      onuGponOmccPortOverrideEn;
   MV_BOOL     onuGponDyingGaspEn;
   MV_BOOL     onuGponBwMapDumpEn;
+  MV_U32      onuGponXvrSdPolarity;
 
   ALARMNOTIFYFUNC   alarmCallback;
   STATUSNOTIFYFUNC  statusCallback;
@@ -268,6 +269,8 @@
 MV_U32    onuGponDbOnuSignalDetectGet(void);
 MV_STATUS onuGponDbOnuDsSyncOnSet(MV_U32 state);
 MV_U32    onuGponDbOnuDsSyncOnGet(void);
+MV_STATUS onuGponDbXvrSdPolaritySet(MV_U32 polarity);
+MV_U32    onuGponDbXvrSdPolarityGet(void);
 MV_STATUS onuGponDbOnuIdOverrideSet(MV_BOOL enable);
 MV_BOOL   onuGponDbOnuIdOverrideGet(void);
 MV_STATUS onuGponDbOnuIdOverrideValueSet(MV_U32 onuId);
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuInit.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuInit.c
index af11474..520a136 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuInit.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuInit.c
@@ -1258,6 +1258,7 @@
   }
 
     ponXvrFunc = funcGponXvrSDStatus(onuInit->ponXvrPolarity);
+    onuGponDbXvrSdPolaritySet(onuInit->ponXvrPolarity);
 
     onuP2PDbXvrBurstEnablePolaritySet(onuInit->p2pXvrBurstEnPolarity);
 
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuIsr.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuIsr.c
index a610756..ee221a7 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuIsr.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_pon/core/gpon/gponOnuIsr.c
@@ -302,8 +302,7 @@
     {
         state = ponXvrFunc(interruptStatus, ONU_GPON_XVR_SIGNAL_DETECT_STATUS_MASK);
 
-	  if (state == MV_TRUE)
-	  {
+	  if (state == MV_TRUE) {
 		onuGponPonMngIntrAlarmHandler(ONU_PON_MNGR_LOS_ALARM, MV_FALSE);
 		/* Set signal detect to ON */
 		onuGponDbOnuSignalDetectSet(1);
@@ -315,7 +314,6 @@
 #ifndef PON_FPGA
 		onuGponIsrXvrReset();
 		onuGponIsrXvrResetStateSet(MV_TRUE);
-		//onuGponPonMngIntrMessageHandler();
 #endif /* PON_FPGA */
 	  }
 	  else if (state == MV_FALSE)
@@ -342,6 +340,22 @@
     if (interruptEvent & ONU_GPON_LOF_ALARM_MASK)
     {
       state = (interruptStatus & ONU_GPON_LOF_ALARM_MASK) ? MV_TRUE : MV_FALSE;
+	  if ((ONU_GPON_XVR_SD_LOW_ACTIVE == onuGponDbXvrSdPolarityGet()) &&
+	  (state != MV_TRUE)) {
+            onuGponPonMngIntrAlarmHandler(ONU_PON_MNGR_LOS_ALARM, MV_FALSE);
+            /* Set signal detect to ON */
+            onuGponDbOnuSignalDetectSet(1);
+
+#ifdef MV_GPON_DEBUG_PRINT
+            mvPonPrint(PON_PRINT_DEBUG, PON_ISR_MODULE,
+                        "DEBUG: (%s:%d) Set signal detect to ON = 1 \n", __FILE_DESC__, __LINE__);
+#endif /* MV_GPON_DEBUG_PRINT */
+#ifndef PON_FPGA
+            onuGponIsrXvrReset();
+            onuGponIsrXvrResetStateSet(MV_TRUE);
+            //onuGponPonMngIntrMessageHandler();
+#endif /* PON_FPGA */
+      }
       onuGponPonMngIntrAlarmHandler(ONU_PON_MNGR_LOF_ALARM, state);
 
       onuGponSyncLog(ONU_GPON_LOG_INTERRUPT_LOF, state, 0, 0);
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 11ee96b..3d0f780 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
@@ -150,6 +150,12 @@
   ONU_GPON_ISR_TYPE_MAX
 }E_OnuGponIsrTypes;
 
+typedef enum
+{
+  ONU_GPON_XVR_SD_HIGH_ACTIVE = 0,
+  ONU_GPON_XVR_SD_LOW_ACTIVE
+}E_OnuGponXvrSdPolarity;
+
 /* Typedefs
 ------------------------------------------------------------------------------*/
 typedef struct
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_db.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_db.c
index 97d033e..0019b1b 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_db.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_db.c
@@ -478,6 +478,23 @@
 		gmac1 = TPM_GMAC_FUNC_NONE;
 		break;
 
+	case TPM_PON_WAN_G0_SINGLE_PORT_SGMII:
+		pon = TPM_GMAC_FUNC_WAN;
+		gmac0 = TPM_GMAC_FUNC_LAN;
+		gmac1 = TPM_GMAC_FUNC_NONE;
+		break;
+
+	case TPM_PON_G1_SGMII_WAN_G0_SINGLE_PORT:
+	  gmac0 = TPM_GMAC_FUNC_LAN;
+	  if (TPM_ENUM_GMAC_1 == tpm_db.init_misc.active_wan) {
+	    pon = TPM_GMAC_FUNC_NONE;
+	    gmac1 = TPM_GMAC_FUNC_WAN;
+	  }else{
+	    gmac1 = TPM_GMAC_FUNC_NONE;
+	    pon = TPM_GMAC_FUNC_WAN;
+	  }
+    break;
+
 	case TPM_G0_WAN_G1_INT_SWITCH:
 		pon = TPM_GMAC_FUNC_NONE;
 		gmac0 = TPM_GMAC_FUNC_WAN;
@@ -599,6 +616,7 @@
 {
 	switch (tpm_db.eth_cmplx_profile) {
 	case TPM_PON_WAN_G0_SINGLE_PORT:
+	case TPM_PON_WAN_G0_SINGLE_PORT_SGMII:
 	case TPM_PON_WAN_G1_SINGLE_PORT:
 	case TPM_PON_WAN_G0_G1_LPBK:
 		tpm_db.max_uni_port_nr = TPM_SRC_PORT_UNI_0;
@@ -607,6 +625,7 @@
 	case TPM_PON_G0_WAN_G1_SINGLE_PORT:
 	case TPM_PON_G1_WAN_G0_SINGLE_PORT:
 	case TPM_PON_WAN_G0_G1_DUAL_LAN:
+	case TPM_PON_G1_SGMII_WAN_G0_SINGLE_PORT:
 		tpm_db.max_uni_port_nr = TPM_SRC_PORT_UNI_1;
        	break;
 
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 b72eb6d..a6da59d 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
@@ -127,21 +127,74 @@
 	(((TPM_ENUM_PMAC == gmac) && (TPM_NONE != tpm_init.pon_type)) ||	\
 	 ((1 == tpm_init.gmac_port_conf[gmac].valid) && (TPM_GMAC_CON_DISC != tpm_init.gmac_port_conf[gmac].conn)))
 
-uint32_t opt_tbl[] = { 	ESC_OPT_RGMIIA_MAC0, ESC_OPT_RGMIIA_MAC1, ESC_OPT_RGMIIA_SW_P5, ESC_OPT_RGMIIA_SW_P6,
-			 	ESC_OPT_RGMIIB_MAC0, ESC_OPT_MAC0_2_SW_P4, ESC_OPT_MAC1_2_SW_P5, ESC_OPT_GEPHY_MAC1,
-			    	ESC_OPT_GEPHY_SW_P0, ESC_OPT_GEPHY_SW_P5, ESC_OPT_FE3PHY, ESC_OPT_SGMII_2_SW_P1, ESC_OPT_SGMII,
-				ESC_OPT_QSGMII,	ESC_OPT_SATA, ESC_OPT_SGMII_2_5 };
-char *opt_str_tlb[] = { "RGMIIA_MAC0", "RGMIIA_MAC1", "RGMIIA_SW_P5", "RGMIIA_SW_P6",
-		 	"RGMIIB_MAC0", "MAC0_2_SW_P4", "MAC1_2_SW_P5", "GEPHY_MAC1",
-		    	"GEPHY_SW_P0", "GEPHY_SW_P5", "FE3PHY", "PCS", "SGMII", "QSGMII", "SATA" , "SGMII_2_5"};
-char *prof_str_tlb[] = { "", "PON_WAN_DUAL_MAC_INT_SWITCH", 	"PON_WAN_G0_INT_SWITCH",
-			"PON_WAN_G1_LAN_G0_INT_SWITCH", 	"G0_WAN_G1_INT_SWITCH",
-		 	"G1_WAN_G0_INT_SWITCH", 		"PON_G1_WAN_G0_INT_SWITCH",
-			"PON_G0_WAN_G1_INT_SWITCH",		"PON_WAN_DUAL_MAC_EXT_SWITCH",
-		    	"PON_WAN_G1_MNG_EXT_SWITCH",		"PON_WAN_G0_SINGLE_PORT",
-			"PON_WAN_G1_SINGLE_PORT",		"PON_G1_WAN_G0_SINGLE_PORT",
-			"PON_G0_WAN_G1_SINGLE_PORT",		"PON_WAN_G0_G1_LPBK",
-			"TPM_PON_WAN_G0_G1_DUAL_LAN"};
+uint32_t opt_tbl[] = {
+    ESC_OPT_RGMIIA_MAC0,
+    ESC_OPT_RGMIIA_MAC1,
+    ESC_OPT_RGMIIA_SW_P5,
+    ESC_OPT_RGMIIA_SW_P6,
+    ESC_OPT_RGMIIB_MAC0,
+    ESC_OPT_MAC0_2_SW_P4,
+    ESC_OPT_MAC1_2_SW_P5,
+    ESC_OPT_GEPHY_MAC1,
+    ESC_OPT_GEPHY_SW_P0,
+    ESC_OPT_GEPHY_SW_P5,
+    ESC_OPT_FE3PHY,
+    ESC_OPT_SGMII_2_SW_P1,
+    ESC_OPT_SGMII,
+    ESC_OPT_QSGMII,
+    ESC_OPT_SATA,
+    ESC_OPT_SGMII_2_5,
+    ESC_OPT_GEPHY_MAC0,
+    ESC_OPT_LP_SERDES_FE_GE_PHY,
+    ESC_OPT_AUTO,
+    ESC_OPT_ILLEGAL,
+    ESC_OPT_ALL
+};
+
+char *opt_str_tlb[] = {
+    "RGMIIA_MAC0",
+    "RGMIIA_MAC1",
+    "RGMIIA_SW_P5",
+    "RGMIIA_SW_P6",
+    "RGMIIB_MAC0",
+    "MAC0_2_SW_P4",
+    "MAC1_2_SW_P5",
+    "GEPHY_MAC1",
+    "GEPHY_SW_P0",
+    "GEPHY_SW_P5",
+    "FE3PHY",
+    "PCS",
+    "SGMII",
+    "QSGMII",
+    "SATA" ,
+    "SGMII_2_5",
+    "GEPHY_MAC0",
+    "LP_SERDES_FE_GE_PHY",
+    "AUTO",
+    "ILLEGAL",
+    "ALL"
+};
+
+char *prof_str_tlb[] = {
+    "",                                     // 0
+    "PON_WAN_DUAL_MAC_INT_SWITCH",          // 1
+    "PON_WAN_G0_INT_SWITCH",                // 2
+    "PON_WAN_G1_LAN_G0_INT_SWITCH",         // 3
+    "G0_WAN_G1_INT_SWITCH",                 // 4
+    "G1_WAN_G0_INT_SWITCH",                 // 5
+    "PON_G1_WAN_G0_INT_SWITCH",             // 6
+    "PON_G0_WAN_G1_INT_SWITCH",             // 7
+    "PON_WAN_DUAL_MAC_EXT_SWITCH",          // 8
+    "PON_WAN_G1_MNG_EXT_SWITCH",            // 9
+    "PON_WAN_G0_SINGLE_PORT",               // 10
+    "PON_WAN_G1_SINGLE_PORT",               // 11
+    "PON_G1_WAN_G0_SINGLE_PORT",            // 12
+    "PON_G0_WAN_G1_SINGLE_PORT",            // 13
+    "PON_WAN_G0_G1_LPBK",                   // 14
+    "TPM_PON_WAN_G0_G1_DUAL_LAN",           // 15
+    "TPM_PON_WAN_G0_SINGLE_PORT_SGMII",     // 16
+    "TPM_PON_G1_SGMII_WAN_G0_SINGLE_PORT",  // 17
+};
 
 static tpm_pnc_range_lookup_map_t pnc_range_lookup_tbl[TPM_MAX_NUM_RANGES] = {
 /*  Range_num             lu_id  last_range  valid */
@@ -568,11 +621,13 @@
 {
 	if (tpm_init.switch_init == MV_TPM_UN_INITIALIZED_INIT_PARAM) {
 		if (   tpm_init.eth_cmplx_profile == TPM_PON_WAN_G0_SINGLE_PORT
+			|| tpm_init.eth_cmplx_profile == TPM_PON_WAN_G0_SINGLE_PORT_SGMII
 		    || tpm_init.eth_cmplx_profile == TPM_PON_WAN_G1_SINGLE_PORT
 		    || tpm_init.eth_cmplx_profile == TPM_PON_G1_WAN_G0_SINGLE_PORT
 		    || tpm_init.eth_cmplx_profile == TPM_PON_G0_WAN_G1_SINGLE_PORT
 		    || tpm_init.eth_cmplx_profile == TPM_PON_WAN_G0_G1_LPBK
-		    || tpm_init.eth_cmplx_profile == TPM_PON_WAN_G0_G1_DUAL_LAN)
+		    || tpm_init.eth_cmplx_profile == TPM_PON_WAN_G0_G1_DUAL_LAN
+		    || tpm_init.eth_cmplx_profile == TPM_PON_G1_SGMII_WAN_G0_SINGLE_PORT )
 			tpm_init.switch_init = 0;
 		else
 			tpm_init.switch_init = 1;
@@ -599,8 +654,8 @@
 	char buff[1024];
 	uint32_t profile[7]= {0};
 
-	off += sprintf(buff+off, "\nSelected Eth Complex Profile: %s", prof_str_tlb[tpm_init.eth_cmplx_profile]);
-	off += sprintf(buff+off, "\nHW enabled options:\n\t");
+	off += sprintf(buff+off, "\nSelected Eth Complex Profile: %s %d", prof_str_tlb[tpm_init.eth_cmplx_profile], tpm_init.eth_cmplx_profile);
+	off += sprintf(buff+off, "\nHW enabled options: 0x%x\n\t", hwEthCmplx);
 	for (i = 0;  i<sizeof(opt_tbl)/sizeof(uint32_t); i++) {
 		if (opt_tbl[i] & hwEthCmplx)
 			off += sprintf(buff+off, "%s ", opt_str_tlb[i]);
@@ -671,6 +726,15 @@
 		profile[0] = ESC_OPT_RGMIIB_MAC0;
 		break;
 
+	case TPM_PON_G1_SGMII_WAN_G0_SINGLE_PORT:
+	  profile[0] = ESC_OPT_RGMIIB_MAC0;
+	  profile[1] = ESC_OPT_SGMII | ESC_OPT_SGMII_2_5;
+	  break;
+
+	case TPM_PON_WAN_G0_SINGLE_PORT_SGMII:
+		profile[0] = ESC_OPT_SGMII;
+		break;
+
 	case TPM_PON_WAN_G1_SINGLE_PORT:
 		profile[0] = ESC_OPT_RGMIIB_MAC0 | ESC_OPT_RGMIIA_MAC1;
 		profile[1] = ESC_OPT_RGMIIB_MAC0 | ESC_OPT_GEPHY_MAC1;
@@ -1247,6 +1311,66 @@
 			tpm_init.eth_port_conf[i].valid = TPM_FALSE;
 		break;
 
+	case TPM_PON_WAN_G0_SINGLE_PORT_SGMII:
+		if (1 == tpm_init.virt_uni_info.enabled)
+			goto virt_uni_err;
+
+		if (!VALID_ONLY((ESC_OPT_SGMII), hwEthCmplx))
+			goto setup_err;
+
+		tpm_init.gmac_port_conf[0].valid = TPM_TRUE;
+		tpm_init.gmac_port_conf[0].port_src = TPM_SRC_PORT_UNI_0;
+		tpm_init.gmac_port_conf[0].conn = TPM_GMAC_CON_SGMII;
+
+		tpm_init.gmac_port_conf[1].valid = TPM_FALSE;
+		tpm_init.gmac_port_conf[1].conn = TPM_GMAC_CON_DISC;
+
+		tpm_init.gmac_port_conf[2].valid = TPM_TRUE;
+		tpm_init.gmac_port_conf[2].port_src = TPM_SRC_PORT_WAN;
+
+		tpm_init.eth_port_conf[0].valid = TPM_TRUE;
+		tpm_init.eth_port_conf[0].port_src = TPM_SRC_PORT_UNI_0;
+		tpm_init.eth_port_conf[0].chip_connect = TPM_CONN_DISC;
+		tpm_init.eth_port_conf[0].int_connect = TPM_INTCON_GMAC0;
+
+		for (i=1 ; i < TPM_MAX_NUM_ETH_PORTS; i++)
+			tpm_init.eth_port_conf[i].valid = TPM_FALSE;
+		break;
+
+  case TPM_PON_G1_SGMII_WAN_G0_SINGLE_PORT:
+    if (1 == tpm_init.virt_uni_info.enabled)
+      goto virt_uni_err;
+
+    if (!VALID_ONLY((ESC_OPT_GEPHY_MAC0 | ESC_OPT_SGMII_2_5), hwEthCmplx) &&
+        !VALID_ONLY((ESC_OPT_GEPHY_MAC0 | ESC_OPT_SGMII    ), hwEthCmplx)
+        )
+      goto setup_err;
+
+    tpm_init.gmac_port_conf[0].valid = TPM_TRUE;
+    tpm_init.gmac_port_conf[0].port_src = TPM_SRC_PORT_UNI_0;
+    tpm_init.gmac_port_conf[0].conn = TPM_GMAC_CON_RGMII2;
+
+    tpm_init.gmac_port_conf[1].valid = TPM_TRUE;
+    tpm_init.gmac_port_conf[1].conn = TPM_SRC_PORT_WAN;
+    tpm_init.gmac_port_conf[1].conn = TPM_GMAC_CON_SGMII;
+
+    tpm_init.gmac_port_conf[2].valid = TPM_TRUE;
+    tpm_init.gmac_port_conf[2].port_src = TPM_SRC_PORT_WAN;
+
+    tpm_init.eth_port_conf[0].valid = TPM_TRUE;
+    tpm_init.eth_port_conf[0].port_src = TPM_SRC_PORT_UNI_0;
+    tpm_init.eth_port_conf[0].chip_connect = TPM_CONN_RGMII2;
+    tpm_init.eth_port_conf[0].int_connect = TPM_INTCON_GMAC0;
+
+    tpm_init.eth_port_conf[1].valid = TPM_TRUE;
+    tpm_init.eth_port_conf[1].port_src = TPM_SRC_PORT_WAN;
+    tpm_init.eth_port_conf[1].chip_connect = TPM_CONN_DISC;
+    tpm_init.eth_port_conf[1].int_connect = TPM_INTCON_GMAC1;
+
+    for (i=2 ; i < TPM_MAX_NUM_ETH_PORTS; i++)
+      tpm_init.eth_port_conf[i].valid = TPM_FALSE;
+    break;
+
 	case TPM_PON_WAN_G1_SINGLE_PORT:
 		if (1 == tpm_init.virt_uni_info.enabled)
 			goto virt_uni_err;
@@ -1739,14 +1863,14 @@
 	}
 
 	/********************** GMAC_0 connectivity validation *************************************/
-	if ((tpm_init.gmac_port_conf[0].conn < TPM_GMAC_CON_DISC) || (tpm_init.gmac_port_conf[0].conn > TPM_GMAC_CON_GE_PHY)) {
+	if ((tpm_init.gmac_port_conf[0].conn < TPM_GMAC_CON_DISC) || (tpm_init.gmac_port_conf[0].conn > TPM_GMAC_CON_SGMII)) {
 		TPM_OS_FATAL(TPM_INIT_MOD,
 			     "\n GMAC_0 connectivity: wrong init value(%d) => legal values <0-7> \n",
 			     tpm_init.gmac_port_conf[0].conn);
 		return (TPM_FAIL);
 	}
 	/********************** GMAC_1 connectivity validation *************************************/
-	if ((tpm_init.gmac_port_conf[1].conn < TPM_GMAC_CON_DISC) || (tpm_init.gmac_port_conf[1].conn > TPM_GMAC_CON_GE_PHY)) {
+	if ((tpm_init.gmac_port_conf[1].conn < TPM_GMAC_CON_DISC) || (tpm_init.gmac_port_conf[1].conn > TPM_GMAC_CON_SGMII)) {
 		TPM_OS_FATAL(TPM_INIT_MOD,
 			     "\n GMAC_1 connectivity: wrong init value(%d) => legal values <0-7> \n",
 			     tpm_init.gmac_port_conf[1].conn);
@@ -2221,11 +2345,13 @@
 			return (TPM_FAIL);
 		}
 	} else if ((tpm_init.eth_cmplx_profile == TPM_PON_WAN_G0_SINGLE_PORT ||
+			tpm_init.eth_cmplx_profile == TPM_PON_WAN_G0_SINGLE_PORT_SGMII ||
 		    tpm_init.eth_cmplx_profile == TPM_PON_WAN_G1_SINGLE_PORT ||
 		    tpm_init.eth_cmplx_profile == TPM_PON_G1_WAN_G0_SINGLE_PORT ||
 		    tpm_init.eth_cmplx_profile == TPM_PON_G0_WAN_G1_SINGLE_PORT ||
 		    tpm_init.eth_cmplx_profile == TPM_PON_WAN_G0_G1_LPBK ||
-		    tpm_init.eth_cmplx_profile == TPM_PON_WAN_G0_G1_DUAL_LAN)
+		    tpm_init.eth_cmplx_profile == TPM_PON_WAN_G0_G1_DUAL_LAN ||
+		    tpm_init.eth_cmplx_profile == TPM_PON_G1_SGMII_WAN_G0_SINGLE_PORT)
 		 && tpm_init.switch_init == 1) {
 			TPM_OS_FATAL(TPM_INIT_MOD,
 					"\n Switch can not be Init at this profile: [%d]! \n", tpm_init.eth_cmplx_profile);
@@ -2573,13 +2699,13 @@
 
 	/* virt uni not enabled */
 	if (virt_uni_info.enabled == 0) {
-		/* Convert UNI_Vector to Amber Vector, applies to uni_0..7, uni_vector from 0x00 to 0xFF */
-		for (i = 0; i < TPM_TX_MAX_MH_REGS; i++) {
-			amber_port_vector = tpm_init_uni_amber_vec_map(i + 1);
-			tpm_db_port_vector_tbl_info_set(i, i + 1, amber_port_vector, 0);
-		}
-		return TPM_DB_OK;
-       }
+	  /* Convert UNI_Vector to Amber Vector, applies to uni_0..7, uni_vector from 0x00 to 0xFF */
+	  for (i = 0; i < TPM_TX_MAX_MH_REGS; i++) {
+	    amber_port_vector = tpm_init_uni_amber_vec_map(i + 1);
+	    tpm_db_port_vector_tbl_info_set(i, i + 1, amber_port_vector, 0);
+	  }
+	  return TPM_DB_OK;
+	}
 
 	/* virt uni enabled */
 
@@ -3161,11 +3287,13 @@
 	case TPM_PON_WAN_DUAL_MAC_EXT_SWITCH:
 	case TPM_PON_WAN_G1_MNG_EXT_SWITCH:
 	case TPM_PON_WAN_G0_SINGLE_PORT:
+	case TPM_PON_WAN_G0_SINGLE_PORT_SGMII:
 	case TPM_PON_WAN_G1_SINGLE_PORT:
 	case TPM_PON_G1_WAN_G0_SINGLE_PORT:
 	case TPM_PON_G0_WAN_G1_SINGLE_PORT:
 	case TPM_PON_WAN_G0_G1_LPBK:
 	case TPM_PON_WAN_G0_G1_DUAL_LAN:
+	case TPM_PON_G1_SGMII_WAN_G0_SINGLE_PORT:
 		return (TPM_OK);
        default:
        	break;
@@ -3829,6 +3957,8 @@
 {
 	uint32_t regVal;
 
+	printk("GMAC port(%d) is invalid\n", port);
+
 	if (port > TPM_MAX_GMAC) {
 		TPM_OS_ERROR(TPM_INIT_MOD, "GMAC port(%d) is invalid\n", port);
 		return TPM_FAIL;
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 21c8a80..7b2d153 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
@@ -899,7 +899,7 @@
 				if (((gmac_bm & TPM_BM_GMAC_0) && (tx_owner != TPM_Q_OWNER_GMAC0)) ||
 				    ((gmac_bm & TPM_BM_GMAC_1) && (tx_owner != TPM_Q_OWNER_GMAC1)) ||
 				    ((gmac_bm & TPM_BM_PMAC) && (tx_owner != TPM_Q_OWNER_PMAC))) {
-					TPM_OS_ERROR(TPM_TPM_LOG_MOD, "Target Queue Owner Invalid, gmac_bm: [%d], tx_owner: [%d]\n",
+					TPM_OS_ERROR(TPM_TPM_LOG_MOD, "Target Queue Owner Invalid, gmac_bm: [0x%x], tx_owner: [%d]\n",
 									gmac_bm, tx_owner);
 					return(ERR_FRWD_INVALID);
 				}
@@ -7094,7 +7094,7 @@
 
 		/* if port is valid */
 		if(!tpm_db_eth_port_valid(src_port)) {
-			TPM_OS_ERROR(TPM_TPM_LOG_MOD, "dest_port(0x%x) is not valid\n", dst_port);
+			TPM_OS_ERROR(TPM_TPM_LOG_MOD, "dest_port(0x%x) - src_port(%d) is not valid\n", dst_port, src_port);
 			return ERR_MC_DST_PORT_INVALID;
 		}
 	}
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_print.c b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_print.c
index 7fc477a..d9948af 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_print.c
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_print.c
@@ -144,6 +144,7 @@
 	BuildEnumString(TPM_GMAC_CON_RGMII1),
 	BuildEnumString(TPM_GMAC_CON_RGMII2),
 	BuildEnumString(TPM_GMAC_CON_GE_PHY),
+	BuildEnumString(TPM_GMAC_CON_SGMII),
 };
 
 db_enum_string_t tpm_db_gmac_func_str[] = {
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_types.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_types.h
index 89a4375..ca167a3 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_types.h
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/core/tpm_types.h
@@ -768,11 +768,13 @@
 *	TPM_PHY_SPEED_10_MBPS   - 10Mbps
 *	TPM_PHY_SPEED_100_MBPS	- 100Mbps
 *	TPM_PHY_SPEED_1000_MBPS - 1000Mbps
+*	TPM_PHY_SPEED_1000_MBPS - 2500Mbps
 */
 typedef enum {
 	TPM_PHY_SPEED_10_MBPS,
 	TPM_PHY_SPEED_100_MBPS,
-	TPM_PHY_SPEED_1000_MBPS
+	TPM_PHY_SPEED_1000_MBPS,
+	TPM_PHY_SPEED_2500_MBPS,
 } tpm_phy_speed_t;
 
 /* switch mirror type */
@@ -1027,7 +1029,10 @@
 	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_PON_WAN_G0_G1_DUAL_LAN,
+	TPM_PON_WAN_G0_SINGLE_PORT_SGMII,
+  TPM_PON_G1_SGMII_WAN_G0_SINGLE_PORT,
+  TPM_COMPLEX_PROFILE_LAST,
 } tpm_eth_complex_profile_t;
 
 typedef enum {
@@ -1083,7 +1088,8 @@
 	TPM_GMAC_CON_SWITCH_6,	/* Amber Switch Port #6 */
 	TPM_GMAC_CON_RGMII1,
 	TPM_GMAC_CON_RGMII2,
-	TPM_GMAC_CON_GE_PHY	/* Internal Gig PHY */
+	TPM_GMAC_CON_GE_PHY,	/* Internal Gig PHY */
+	TPM_GMAC_CON_SGMII,
 } tpm_init_gmac_conn_t;
 
 typedef enum {
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 46cf9ca..1d993d0 100644
--- 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
@@ -167,21 +167,37 @@
     }
 
     /* Check gmac port connection info */
-    for (i = 0; i < TPM_MAX_NUM_GMACS; i++) {
-        if (TPM_DB_OK != tpm_db_gmac_conn_conf_get(i, &gmac_conn_info)) {
+    for (i = 0; i < TPM_MAX_NUM_GMACS; i++)
+    {
+        if (TPM_DB_OK != tpm_db_gmac_conn_conf_get(i, &gmac_conn_info))
+        {
             printk(KERN_ERR "ERROR: (%s:%d) Gmac port(%d) connection info get failed\n", __FUNCTION__, __LINE__, i);
             return ERR_PHY_SRC_PORT_CONN_INVALID;
         }
-        if (TPM_TRUE == gmac_conn_info.valid && src_port == gmac_conn_info.port_src) {
-            /* PHY access directly */
-            *phy_access_way = PHY_SMI_MASTER_CPU;
-            /*get PHY addr on GMAC*/
-            *phy_direct_addr = mvBoardPhyAddrGet(i);
+        if (TPM_TRUE == gmac_conn_info.valid && src_port == gmac_conn_info.port_src)
+        {
+        	if(gmac_conn_info.conn == TPM_GMAC_CON_SGMII)
+        	{
+				/* No PHY available */
+				*phy_access_way = PHY_SGMII;
 
-            if (trace_sw_dbg_flag)
-                printk(KERN_INFO "Port%d PHY access directly, phyaddr %d\n", src_port, *phy_direct_addr);
+				if (trace_sw_dbg_flag)
+					printk(KERN_INFO "Port%d SGMII connection, no PHY\n", src_port, *phy_direct_addr);
 
-            return TPM_RC_OK;
+				return TPM_RC_OK;
+        	}
+        	else // We keep the original code for the rest of cases
+        	{
+				/* PHY access directly */
+				*phy_access_way = PHY_SMI_MASTER_CPU;
+				/*get PHY addr on GMAC*/
+				*phy_direct_addr = mvBoardPhyAddrGet(i);
+
+				if (trace_sw_dbg_flag)
+					printk(KERN_INFO "Port%d PHY access directly, phyaddr %d\n", src_port, *phy_direct_addr);
+
+				return TPM_RC_OK;
+        	}
         }
     }
 
@@ -334,7 +350,9 @@
                 return retVal;
             }
         }
-    } else {
+    }
+    else if (PHY_SGMII != phy_access_way)
+    {
         printk(KERN_ERR "PHY not connected to GMAC\n");
         retVal = ERR_GENERAL;
         return retVal;
@@ -5077,6 +5095,10 @@
                "Port%d PHY access way check failed\n", src_port);
         return retVal;
     }
+    // Possible connection ways:
+    //  -RGMII phy accessed directly
+    //  -RGMII phy accessed through a switch
+    //  -SGMII - No autoneg available
     /* PHY accessed directly, call lsp API */
     if (PHY_SMI_MASTER_CPU == phy_access_way) {
         if (mvEthPhyAutoNegoSet(phy_direct_addr, state)) {
@@ -5084,12 +5106,15 @@
                    "ERROR: (%s:%d) PHY LSP API call failed on port(%d)\n", __FUNCTION__, __LINE__, src_port);
             retVal = ERR_PHY_SRC_PORT_CONN_INVALID;
         }
-        if (state && (retVal == TPM_RC_OK)) {
-            if (mvEthPhyAdvertiseSet(phy_direct_addr, lsp_mode)) {
-                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;
-            }
+
+        if(retVal == TPM_RC_OK )
+        {
+        	// Enable always advertisement
+			if (mvEthPhyAdvertiseSet(phy_direct_addr, lsp_mode)) {
+				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;
+			}
         }
         /* Update GMAC speed, duplex and flow control */
         retVal = tpm_sw_set_gmac_speed_duplex_fc(owner_id, src_port, (state ? true : false));
@@ -5097,7 +5122,9 @@
             printk(KERN_ERR "Failed to update GMAC config with port%d\n", src_port);
             return retVal;
         }
-    } else {
+    }
+    else if (PHY_SMI_MASTER_SWT == phy_access_way)
+    {
         /* PHY accessed through switch, as original do */
         lPort = tpm_db_eth_port_switch_port_get(src_port);
         if (lPort == TPM_DB_ERR_PORT_NUM)
@@ -5119,6 +5146,9 @@
             }
         }
     }
+    else if (PHY_SGMII == phy_access_way)
+    {  /* nothing to do*/ }
+
     if (trace_sw_dbg_flag)
     {
         printk(KERN_INFO
@@ -5262,7 +5292,7 @@
         } else {
             mode = TPM_SPEED_AUTO_DUPLEX_AUTO;
         }
-    } else {
+    } else if (PHY_SMI_MASTER_SWT == phy_access_way){
         /* PHY accessed through switch, as original do */
         lPort = tpm_db_eth_port_switch_port_get(src_port);
         if (lPort == TPM_DB_ERR_PORT_NUM)
@@ -5278,6 +5308,8 @@
                    "%s:%d: function failed\r\n", __FUNCTION__,__LINE__);
         }
     }
+    else if (PHY_SGMII == phy_access_way)
+    {  /* nothing to do*/ }
 
     if(GT_TRUE == state)
         *autoneg_state = true;
@@ -5346,13 +5378,17 @@
         return retVal;
     }
     /* PHY accessed directly, call lsp API */
-    if (PHY_SMI_MASTER_CPU == phy_access_way) {
-        if (mvEthPhyRestartAN(phy_direct_addr, 0)) {
+    if (PHY_SMI_MASTER_CPU == phy_access_way)
+    {
+        if (mvEthPhyRestartAN(phy_direct_addr, 0))
+        {
             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 {
+    }
+    else if (PHY_SMI_MASTER_SWT == phy_access_way)
+    {
         /* PHY accessed through switch, as original do */
         lPort = tpm_db_eth_port_switch_port_get(src_port);
         if (lPort == TPM_DB_ERR_PORT_NUM)
@@ -5368,6 +5404,8 @@
                     "%s:%d: function failed\r\n", __FUNCTION__,__LINE__);
         }
     }
+    else if (PHY_SGMII == phy_access_way)
+    {  /* nothing to do*/ }
 
     if (trace_sw_dbg_flag)
     {
@@ -5434,13 +5472,16 @@
         return retVal;
     }
     /* PHY accessed directly, call lsp API */
-    if (PHY_SMI_MASTER_CPU == phy_access_way) {
+    if (PHY_SMI_MASTER_CPU == phy_access_way)
+    {
         if (mvEthPhySetAdminState(phy_direct_addr, (int)state)) {
             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 {
+    }
+    else if (PHY_SMI_MASTER_SWT == phy_access_way)
+    {
         /* PHY accessed through switch, as original do */
         lPort = tpm_db_eth_port_switch_port_get(src_port);
         if (lPort == TPM_DB_ERR_PORT_NUM)
@@ -5462,6 +5503,8 @@
             }
         }
     }
+    else if (PHY_SGMII == phy_access_way)
+    {  /* nothing to do*/ }
 
     if (trace_sw_dbg_flag)
     {
@@ -5521,13 +5564,16 @@
         return retVal;
     }
     /* PHY accessed directly, call lsp API */
-    if (PHY_SMI_MASTER_CPU == phy_access_way) {
+    if (PHY_SMI_MASTER_CPU == phy_access_way)
+    {
         if (mvEthPhyGetAdminState(phy_direct_addr, (int *)&state)) {
             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 {
+    }
+    else if (PHY_SMI_MASTER_SWT == phy_access_way)
+    {
         /* PHY accessed through switch, as original do */
         lPort = tpm_db_eth_port_switch_port_get(src_port);
         if (lPort == TPM_DB_ERR_PORT_NUM)
@@ -5543,6 +5589,8 @@
                    "%s:%d: function failed\r\n", __FUNCTION__,__LINE__);
         }
     }
+    else if (PHY_SGMII == phy_access_way)
+    {  /* nothing to do*/ }
 
     if(state == GT_TRUE)
         *phy_port_state = true;
@@ -5607,9 +5655,12 @@
         return retVal;
     }
     /* PHY accessed directly, call lsp API */
-    if (PHY_SMI_MASTER_CPU == phy_access_way) {
+    if (PHY_SMI_MASTER_CPU == phy_access_way)
+    {
         state = (GT_BOOL)mvEthPhyCheckLink(phy_direct_addr);
-    } else {
+    }
+    else if (PHY_SMI_MASTER_SWT == phy_access_way)
+    {
         /* PHY accessed through switch, as original do */
         lPort = tpm_db_eth_port_switch_port_get(src_port);
         if (lPort == TPM_DB_ERR_PORT_NUM)
@@ -5625,6 +5676,8 @@
                    "%s:%d: function failed\r\n", __FUNCTION__,__LINE__);
         }
     }
+    else if (PHY_SGMII == phy_access_way)
+    {  /* nothing to do*/ }
 
     if(state == GT_TRUE)
         *port_link_status = true;
@@ -5693,7 +5746,8 @@
         return retVal;
     }
     /* PHY accessed directly, call lsp API */
-    if (PHY_SMI_MASTER_CPU == phy_access_way) {
+    if (PHY_SMI_MASTER_CPU == phy_access_way)
+    {
         if (mvEthPhyDuplexOperGet(phy_direct_addr, (int *)&status_valid, (int *)&state)) {
             printk(KERN_ERR
                    "ERROR: (%s:%d) PHY LSP API call failed on port(%d)\n", __FUNCTION__, __LINE__, src_port);
@@ -5705,7 +5759,9 @@
                    "ERROR: (%s:%d) PHY duplex status is not valid on port(%d)\n", __FUNCTION__, __LINE__, src_port);
             retVal = ERR_PHY_STATUS_UNKNOWN;
         }
-    } else {
+    }
+    else if (PHY_SMI_MASTER_SWT == phy_access_way)
+    {
         /* PHY accessed through switch, as original do */
         lPort = tpm_db_eth_port_switch_port_get(src_port);
         if (lPort == TPM_DB_ERR_PORT_NUM)
@@ -5721,6 +5777,8 @@
                    "%s:%d: function failed\r\n", __FUNCTION__,__LINE__);
         }
     }
+    else if (PHY_SGMII == phy_access_way)
+    {  /* nothing to do*/ }
 
     if(state == GT_TRUE)
         *port_duplex_status = true;
@@ -5789,7 +5847,8 @@
         return retVal;
     }
     /* PHY accessed directly, call lsp API */
-    if (PHY_SMI_MASTER_CPU == phy_access_way) {
+    if (PHY_SMI_MASTER_CPU == phy_access_way)
+    {
         if (mvEthPhySpeedOperGet(phy_direct_addr, (uint32_t *)&tmpSpeed)) {
             printk(KERN_ERR
                    "ERROR: (%s:%d) PHY LSP API call failed on port(%d)\n", __FUNCTION__, __LINE__, src_port);
@@ -5801,7 +5860,9 @@
                    "ERROR: (%s:%d) PHY speed status is not valid on port(%d)\n", __FUNCTION__, __LINE__, src_port);
             retVal = ERR_PHY_STATUS_UNKNOWN;
         }
-    } else {
+    }
+    else if (PHY_SMI_MASTER_SWT == phy_access_way)
+    {
         /* PHY accessed through switch, as original do */
         lPort = tpm_db_eth_port_switch_port_get(src_port);
         if (lPort == TPM_DB_ERR_PORT_NUM)
@@ -5817,6 +5878,8 @@
                    "%s:%d: function failed\r\n", __FUNCTION__,__LINE__);
         }
     }
+    else if (PHY_SGMII == phy_access_way)
+    {  /* nothing to do*/ }
 
     *speed = (uint32_t)tmpSpeed;
 
@@ -5888,7 +5951,8 @@
         return retVal;
     }
     /* PHY accessed directly, call lsp API */
-    if (PHY_SMI_MASTER_CPU == phy_access_way) {
+    if (PHY_SMI_MASTER_CPU == phy_access_way)
+    {
         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;
@@ -5951,7 +6015,9 @@
                    "ERROR: (%s:%d) PHY LSP API call failed on port(%d)\n", __FUNCTION__, __LINE__, src_port);
             retVal = ERR_PHY_SRC_PORT_CONN_INVALID;
         }
-    } else {
+    }
+    else if (PHY_SMI_MASTER_SWT == phy_access_way)
+    {
         /* PHY accessed through switch, as original do */
         lPort = tpm_db_eth_port_switch_port_get(src_port);
         if (lPort == TPM_DB_ERR_PORT_NUM)
@@ -5973,6 +6039,8 @@
             }
         }
     }
+    else if (PHY_SGMII == phy_access_way)
+    {  /* nothing to do*/ }
 
     if (trace_sw_dbg_flag)
     {
@@ -6030,13 +6098,16 @@
         return retVal;
     }
     /* PHY accessed directly, call lsp API */
-    if (PHY_SMI_MASTER_CPU == phy_access_way) {
+    if (PHY_SMI_MASTER_CPU == phy_access_way)
+    {
         if (mvEthPhyPauseAdminGet(phy_direct_addr, (uint32_t *)&pause_state)) {
             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 {
+    }
+    else if (PHY_SMI_MASTER_SWT == phy_access_way)
+    {
         /* PHY accessed through switch, as original do */
         lPort = tpm_db_eth_port_switch_port_get(src_port);
         if (lPort == TPM_DB_ERR_PORT_NUM)
@@ -6052,6 +6123,8 @@
                    "%s:%d: function failed\r\n", __FUNCTION__,__LINE__);
         }
     }
+    else if (PHY_SGMII == phy_access_way)
+    {  /* nothing to do*/ }
 
     if(pause_state == GT_PHY_NO_PAUSE)
         *state = false;
@@ -6149,27 +6222,30 @@
                    "Port%d PHY access way check failed\n", src_port);
             return retVal;
         }
-        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(PHY_SMI_MASTER_CPU == phy_access_way)
+        {
+			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;
+			}
 
-        /* Get GMAC Flow Control state */
-        if (mvNetaFlowCtrlGet((int)gmac_port, &fc_mode) != MV_OK) {
-            printk(KERN_ERR "ERROR: (%s:%d) Can not find gmac port\n", __FUNCTION__, __LINE__);
-            retVal = ERR_GENERAL;
-            return retVal;
-        }
+			/* Get GMAC Flow Control state */
+			if (mvNetaFlowCtrlGet((int)gmac_port, &fc_mode) != MV_OK) {
+				printk(KERN_ERR "ERROR: (%s:%d) Can not find gmac port\n", __FUNCTION__, __LINE__);
+				retVal = ERR_GENERAL;
+				return retVal;
+			}
 
-        if (fc_mode == MV_ETH_FC_DISABLE)
-            *state = false;
-        else
-            *state = true;
+			if (fc_mode == MV_ETH_FC_DISABLE)
+				*state = false;
+			else
+				*state = true;
+        }
     }
 
     if (trace_sw_dbg_flag)
@@ -6318,7 +6394,9 @@
                    "==ENTER==%s: mode[%d] invalid\n\r", __FUNCTION__,mode);
             retVal = ERR_GENERAL;
         }
-    } else {
+    }
+    else if (PHY_SMI_MASTER_SWT == phy_access_way)
+    {
         /* PHY accessed through switch, as original do */
         lPort = tpm_db_eth_port_switch_port_get(src_port);
         if (lPort == TPM_DB_ERR_PORT_NUM)
@@ -6373,6 +6451,8 @@
                    "%s:%d: function failed\r\n", __FUNCTION__,__LINE__);
         }
     }
+    else if (PHY_SGMII == phy_access_way)
+    {  /* nothing to do*/ }
 
     if (trace_sw_dbg_flag)
     {
@@ -6449,7 +6529,9 @@
                    "==ENTER==%s: mode[%d] invalid\n\r", __FUNCTION__,mode);
             retVal = ERR_GENERAL;
         }
-    } else {
+    }
+    else if (PHY_SMI_MASTER_SWT == phy_access_way)
+    {
         /* PHY accessed through switch, as original do */
         lPort = tpm_db_eth_port_switch_port_get(src_port);
         if (lPort == TPM_DB_ERR_PORT_NUM)
@@ -6475,6 +6557,8 @@
                    "%s:%d: function failed\r\n", __FUNCTION__,__LINE__);
         }
     }
+    else if (PHY_SGMII == phy_access_way)
+    {  /* nothing to do*/ }
 
     if(state == GT_TRUE)
     {
@@ -6568,7 +6652,9 @@
                 return retVal;
             }
         }
-    } else {
+    }
+    else if (PHY_SMI_MASTER_SWT == phy_access_way)
+    {
         /* PHY accessed through switch, as original do */
         lPort = tpm_db_eth_port_switch_port_get(src_port);
         if (lPort == TPM_DB_ERR_PORT_NUM)
@@ -6584,6 +6670,8 @@
                    "%s:%d: function failed\r\n", __FUNCTION__,__LINE__);
         }
     }
+    else if (PHY_SGMII == phy_access_way)
+    {  /* nothing to do*/ }
 
     if (trace_sw_dbg_flag)
     {
@@ -6651,7 +6739,9 @@
                    "ERROR: (%s:%d) PHY LSP API call failed on port(%d)\n", __FUNCTION__, __LINE__, src_port);
             retVal = ERR_PHY_SRC_PORT_CONN_INVALID;
         }
-    } else {
+    }
+    else if (PHY_SMI_MASTER_SWT == phy_access_way)
+    {
         /* PHY accessed through switch, as original do */
         lPort = tpm_db_eth_port_switch_port_get(src_port);
         if (lPort == TPM_DB_ERR_PORT_NUM)
@@ -6673,6 +6763,8 @@
                     "==EXIT== %s:enable[%d]\n\r",__FUNCTION__,  *enable);
         }
     }
+    else if (PHY_SGMII == phy_access_way)
+    {  /* nothing to do*/ }
 
     if(state == GT_TRUE)
     {
@@ -6750,7 +6842,9 @@
                 return retVal;
             }
         }
-    } else {
+    }
+    else if (PHY_SMI_MASTER_SWT == phy_access_way)
+    {
         /* PHY accessed through switch, as original do */
         lPort = tpm_db_eth_port_switch_port_get(src_port);
         if (lPort == TPM_DB_ERR_PORT_NUM)
@@ -6766,6 +6860,8 @@
                    "%s:%d: function failed\r\n", __FUNCTION__,__LINE__);
         }
     }
+    else if (PHY_SGMII == phy_access_way)
+    {  /* nothing to do*/ }
 
     if (trace_sw_dbg_flag)
     {
@@ -6835,7 +6931,9 @@
                    "ERROR: (%s:%d) PHY LSP API call failed on port(%d)\n", __FUNCTION__, __LINE__, src_port);
             retVal = ERR_PHY_SRC_PORT_CONN_INVALID;
         }
-    } else {
+    }
+    else if (PHY_SMI_MASTER_SWT == phy_access_way)
+    {
         /* PHY accessed through switch, as original do */
         lPort = tpm_db_eth_port_switch_port_get(src_port);
         if (lPort == TPM_DB_ERR_PORT_NUM)
@@ -6853,6 +6951,8 @@
 
         *speed = (tpm_phy_speed_t)lSpeed;
     }
+    else if (PHY_SGMII == phy_access_way)
+    {  /* nothing to do*/ }
 
     if (trace_sw_dbg_flag)
     {
@@ -7563,20 +7663,26 @@
         tpm_swport_pm_1->alignmentErrorCounter           = 0;
         tpm_swport_pm_1->internalMacReceiveErrorCounter  = statsCounterSet.InMACRcvErr;
     } else {
-        /* check PHY access way */
         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;
         }
-        for (gmac_port = TPM_ENUM_GMAC_0; gmac_port < TPM_MAX_NUM_GMACS; gmac_port++) {
-            if (phy_direct_addr == mvBoardPhyAddrGet(gmac_port))
-                break;
+        if (PHY_SMI_MASTER_CPU == phy_access_way)/* check PHY access way */
+        {
+			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__);
+				return ERR_GENERAL;
+			}
         }
-        if (gmac_port == TPM_MAX_NUM_GMACS) {
-            printk(KERN_ERR "ERROR: (%s:%d) Can not find gmac port\n", __FUNCTION__, __LINE__);
-            return ERR_GENERAL;
+        else if (PHY_SGMII == phy_access_way)
+        {
+        	gmac_port = 0;
         }
 
         /* Read counter from GMAC */
@@ -7586,7 +7692,7 @@
             return ERR_GENERAL;
         }
 
-	g_mc_tpm_swport_pm_1.fcsErrors                       += tpm_swport_pm_1->fcsErrors;
+        g_mc_tpm_swport_pm_1.fcsErrors                       += tpm_swport_pm_1->fcsErrors;
         g_mc_tpm_swport_pm_1.excessiveCollisionCounter       += tpm_swport_pm_1->excessiveCollisionCounter;
         g_mc_tpm_swport_pm_1.lateCollisionCounter            += tpm_swport_pm_1->lateCollisionCounter;
         g_mc_tpm_swport_pm_1.frameTooLongs                   += tpm_swport_pm_1->frameTooLongs;
@@ -7760,13 +7866,20 @@
                    "Port%d PHY access way check failed\n", src_port);
             return retVal;
         }
-        for (gmac_port = TPM_ENUM_GMAC_0; gmac_port < TPM_MAX_NUM_GMACS; gmac_port++) {
-            if (phy_direct_addr == mvBoardPhyAddrGet(gmac_port))
-                break;
+        if (PHY_SMI_MASTER_CPU == phy_access_way)/* check PHY access way */
+        {
+			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__);
+				return ERR_GENERAL;
+			}
         }
-        if (gmac_port == TPM_MAX_NUM_GMACS) {
-            printk(KERN_ERR "ERROR: (%s:%d) Can not find gmac port\n", __FUNCTION__, __LINE__);
-            return ERR_GENERAL;
+        else if (PHY_SGMII == phy_access_way)
+        {
+        	gmac_port = 0;
         }
 
         /* Read counter from GMAC */
@@ -8123,7 +8236,9 @@
                 return retVal;
             }
         }
-    } else {
+    }
+    else if (PHY_SGMII == phy_access_way)
+    {
         /* PHY accessed through switch, as original do */
         lPort = tpm_db_eth_port_switch_port_get(src_port);
         if (lPort == TPM_DB_ERR_PORT_NUM)
diff --git a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/plat/tpm_switch_mgr.h b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/plat/tpm_switch_mgr.h
index 0f6141c..53aca50 100755
--- a/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/plat/tpm_switch_mgr.h
+++ b/arch/arm/plat-feroceon/mv_drivers_lsp/mv_tpm/plat/tpm_switch_mgr.h
@@ -116,6 +116,7 @@
 typedef enum {
         PHY_SMI_MASTER_CPU,/*Phy directly accessed through LSP function*/
         PHY_SMI_MASTER_SWT,/*Phy accessed through switch*/
+        PHY_SGMII,/*No Phy, direct SGMII connection*/
 } tpm_phy_ctrl_t;
 
 /*******************************************************************************
diff --git a/arch/arm/plat-feroceon/mv_hal/sflash/mvSFlash.c b/arch/arm/plat-feroceon/mv_hal/sflash/mvSFlash.c
index 71e0abf..d328ae4 100755
--- a/arch/arm/plat-feroceon/mv_hal/sflash/mvSFlash.c
+++ b/arch/arm/plat-feroceon/mv_hal/sflash/mvSFlash.c
@@ -281,6 +281,31 @@
      MV_MX25L6405_MAX_FAST_SPI_FREQ,
      MV_MX25L6405_FAST_READ_DUMMY_BYTES
     },
+    /* Macronix MXIC MX25L12805E SPI flash, 16MB, 256 sectors of 64K each */
+    {
+     MV_MX25L_WREN_CMND_OPCD,
+     MV_MX25L_WRDI_CMND_OPCD,
+     MV_MX25L_RDID_CMND_OPCD,
+     MV_MX25L_RDSR_CMND_OPCD,
+     MV_MX25L_WRSR_CMND_OPCD,
+     MV_MX25L_READ_CMND_OPCD,
+     MV_MX25L_FAST_RD_CMND_OPCD,
+     MV_MX25L_PP_CMND_OPCD,
+     MV_MX25L_SE_CMND_OPCD,
+     MV_MX25L_BE_CMND_OPCD,
+     MV_MX25L_RES_CMND_OPCD,
+     MV_MX25L_DP_CMND_OPCD,
+     MV_MX25L_EN4B_CMND_OPCD,
+     MV_MX25L12805E_SECTOR_SIZE,
+     MV_MX25L12805E_SECTOR_NUMBER,
+     MV_MXIC_PAGE_SIZE,
+     "MXIC MX25L12805E",
+     MV_MXIC_MANF_ID,
+     MV_MX25L12805E_DEVICE_ID,
+     MV_MX25L12805E_MAX_SPI_FREQ,
+     MV_MX25L12805E_MAX_FAST_SPI_FREQ,
+     MV_MX25L12805E_FAST_READ_DUMMY_BYTES
+    },
     /* Macronix MXIC MX25L25635E SPI flash, 32MB, 512 sectors of 64K each */
     {
      MV_MX25L_WREN_CMND_OPCD,
@@ -330,6 +355,56 @@
      MV_S25FL128_MAX_SPI_FREQ,
      MV_M25P128_MAX_FAST_SPI_FREQ,
      MV_M25P128_FAST_READ_DUMMY_BYTES
+    },
+   /* WINBOND M25Q128 SPI flash, 16MB, 256 sectors of 64K each */
+    {
+     MV_W25Q_WREN_CMND_OPCD,
+     MV_W25Q_WRDI_CMND_OPCD,
+     MV_W25Q_RDID_CMND_OPCD,
+     MV_W25Q_RDSR_CMND_OPCD,
+     MV_W25Q_WRSR_CMND_OPCD,
+     MV_W25Q_READ_CMND_OPCD,
+     MV_W25Q_FAST_RD_CMND_OPCD,
+     MV_W25Q_PP_CMND_OPCD,
+     MV_W25Q_SE_CMND_OPCD,
+     MV_W25Q_BE_CMND_OPCD,
+     MV_W25Q_RES_CMND_OPCD,
+     MV_SFLASH_NO_SPECIFIC_OPCD,    /* power save not supported */
+	 MV_SFLASH_NO_SPECIFIC_OPCD,
+     MV_W25Q128_SECTOR_SIZE,
+     MV_W25Q128_SECTOR_NUMBER,
+     MV_W25Q_PAGE_SIZE,
+     "WINBOND W25Q128",
+     MV_WINBOND_MANF_ID,
+     MV_W25Q128_DEVICE_ID,
+     MV_W25Q128_MAX_SPI_FREQ,
+     MV_W25Q128_MAX_FAST_SPI_FREQ,
+     MV_W25Q128_FAST_READ_DUMMY_BYTES
+    },
+    /* WINBOND M25Q256 SPI flash, 16MB, 512 sectors of 64K each */
+     {
+      MV_W25Q_WREN_CMND_OPCD,
+      MV_W25Q_WRDI_CMND_OPCD,
+      MV_W25Q_RDID_CMND_OPCD,
+      MV_W25Q_RDSR_CMND_OPCD,
+      MV_W25Q_WRSR_CMND_OPCD,
+      MV_W25Q_READ_4B_CMND_OPCD,
+      MV_W25Q_FAST_RD_4B_CMND_OPCD,
+      MV_W25Q_PP_CMND_OPCD,
+      MV_W25Q_SE_CMND_OPCD,
+      MV_W25Q_BE_CMND_OPCD,
+      MV_W25Q_RES_CMND_OPCD,
+      MV_SFLASH_NO_SPECIFIC_OPCD,    /* power save not supported */
+      MV_W25Q_EN4B_CMND_OPCD,
+      MV_W25Q256_SECTOR_SIZE,
+      MV_W25Q256_SECTOR_NUMBER,
+      MV_W25Q_PAGE_SIZE,
+      "WINBOND W25Q256",
+      MV_WINBOND_MANF_ID,
+      MV_W25Q256_DEVICE_ID,
+      MV_W25Q256_MAX_SPI_FREQ,
+      MV_W25Q256_MAX_FAST_SPI_FREQ,
+      MV_W25Q256_FAST_READ_DUMMY_BYTES
     }
 };
 
@@ -638,7 +713,7 @@
 	mvOsPrintf("%s ERROR: Failed to get the SFlash ID!\n", __func__);
 	return ret;
     }
-
+printk("MTL SPI device manf = 0x%X, dev = 0x%X\n", manf, dev);
     /* loop over the whole table and look for the appropriate SFLASH */
     for (indx = 0; indx < MV_ARRAY_SIZE(sflash); indx++) {
 	if ((manf == sflash[indx].manufacturerId) && (dev == sflash[indx].deviceId)) {
@@ -662,20 +737,24 @@
     /* Enable 4B address mode in case needed and supported */
     flSize = (pFlinfo->sectorSize  * pFlinfo->sectorNumber);
     if (flSize > _16M) {
-	cmndLength = 5;
-    	if (sflash[pFlinfo->index].opcdEn4B != MV_SFLASH_NO_SPECIFIC_OPCD) {
-		mvOsPrintf("%s: Enabling 4-Byte address mode\n", __func__);
+    	cmndLength = 5;
+    	if (sflash[pFlinfo->index].opcdEn4B != MV_SFLASH_NO_SPECIFIC_OPCD)
+    	{
+			mvOsPrintf("%s: Enabling 4-Byte address mode\n", __func__);
 
-	 	cmd = sflash[pFlinfo->index].opcdEn4B;
+			cmd = sflash[pFlinfo->index].opcdEn4B;
 
-		/*  mvSpiWriteThenWrite(MV_SFLASH_RES_CMND_LENGTH) */
-		ret = mvSysSflashCommandSet(NULL, &cmd, 1, SYS_SFLASH_TRANS_ATOMIC);
-		
-		if (ret != MV_OK)
-			return ret;
-	}
-    } else
-	cmndLength = 4;
+			/*  mvSpiWriteThenWrite(MV_SFLASH_RES_CMND_LENGTH) */
+			ret = mvSysSflashCommandSet(NULL, &cmd, 1, SYS_SFLASH_TRANS_ATOMIC);
+
+			if (ret != MV_OK)
+				return ret;
+    	}
+    }
+    else
+    {
+    	cmndLength = 4;
+    }
 
     /* Set the SPI frequency to the MAX allowed for the device for best performance */
     /*  mvSpiBaudRateSet(sflash[pFlinfo->index].spiMaxFreq) */
@@ -1297,7 +1376,52 @@
 	    DB(mvOsPrintf("%s WARNING: Invaild parameter WP region!\n", __func__);)
 	    return MV_BAD_PARAM;
 	}
-    } else {
+    }else if (pFlinfo->manufacturerId == MV_WINBOND_MANF_ID) {
+	/* check if the manufacturer is SPANSION then the WP is 4bits */
+	switch (wpRegion) {
+	case MV_WP_NONE:
+	    wpMask = MV_W25Q_STATUS_BP_NONE;
+	    break;
+
+	case MV_WP_UPR_1OF128:
+	    DB(mvOsPrintf("%s WARNING: Invaild option for this flash chip!\n", __func__);)
+	    return MV_NOT_SUPPORTED;
+
+	case MV_WP_UPR_1OF64:
+	    wpMask = MV_W25Q_STATUS_BP_1_OF_64;
+	    break;
+
+	case MV_WP_UPR_1OF32:
+	    wpMask = MV_W25Q_STATUS_BP_1_OF_32;
+	    break;
+
+	case MV_WP_UPR_1OF16:
+	    wpMask = MV_W25Q_STATUS_BP_1_OF_16;
+	    break;
+
+	case MV_WP_UPR_1OF8:
+	    wpMask = MV_W25Q_STATUS_BP_1_OF_8;
+	    break;
+
+	case MV_WP_UPR_1OF4:
+	    wpMask = MV_W25Q_STATUS_BP_1_OF_4;
+	    break;
+
+	case MV_WP_UPR_1OF2:
+	    wpMask = MV_W25Q_STATUS_BP_1_OF_2;
+	    break;
+
+	case MV_WP_ALL:
+	    wpMask = MV_W25Q_STATUS_BP_ALL;
+	    break;
+
+
+	default:
+	    DB(mvOsPrintf("%s WARNING: Invaild parameter WP region!\n", __func__);)
+	    return MV_BAD_PARAM;
+	}
+    }
+	else {
 	DB(mvOsPrintf("%s WARNING: Invaild parameter Manufacturer ID!\n", __func__);)
 	return MV_BAD_PARAM;
     }
@@ -1467,7 +1591,48 @@
 	    DB(mvOsPrintf("%s WARNING: Unidentified WP region in h/w!\n", __func__);)
 	    return MV_BAD_VALUE;
 	}
-    } else {
+    } 
+	else if (pFlinfo->manufacturerId == MV_WINBOND_MANF_ID) {
+		/* Check if the chip is an SPANSION flash; then WP supports only 3 bits */
+	switch ((reg & MV_W25Q_STATUS_REG_WP_MASK)) {
+	case MV_W25Q_STATUS_BP_NONE:
+	    *pWpRegion = MV_WP_NONE;
+	    break;
+
+	case MV_W25Q_STATUS_BP_1_OF_64:
+	    *pWpRegion = MV_WP_UPR_1OF64;
+	    break;
+
+	case MV_W25Q_STATUS_BP_1_OF_32:
+	    *pWpRegion = MV_WP_UPR_1OF32;
+	    break;
+
+	case MV_W25Q_STATUS_BP_1_OF_16:
+	    *pWpRegion = MV_WP_UPR_1OF16;
+	    break;
+
+	case MV_W25Q_STATUS_BP_1_OF_8:
+	    *pWpRegion = MV_WP_UPR_1OF8;
+	    break;
+
+	case MV_W25Q_STATUS_BP_1_OF_4:
+	    *pWpRegion = MV_WP_UPR_1OF4;
+	    break;
+
+	case MV_W25Q_STATUS_BP_1_OF_2:
+	    *pWpRegion = MV_WP_UPR_1OF2;
+	    break;
+
+	case MV_W25Q_STATUS_BP_ALL:
+	    *pWpRegion = MV_WP_ALL;
+	    break;
+
+	default:
+	    DB(mvOsPrintf("%s WARNING: Unidentified WP region in h/w!\n", __func__);)
+	    return MV_BAD_VALUE;
+	}
+    }
+	else {
 	DB(mvOsPrintf("%s WARNING: Invaild parameter Manufacturer ID!\n", __func__);)
 	return MV_BAD_PARAM;
     }
diff --git a/arch/arm/plat-feroceon/mv_hal/sflash/mvSFlashSpec.h b/arch/arm/plat-feroceon/mv_hal/sflash/mvSFlashSpec.h
index b999e5b..030a303 100755
--- a/arch/arm/plat-feroceon/mv_hal/sflash/mvSFlashSpec.h
+++ b/arch/arm/plat-feroceon/mv_hal/sflash/mvSFlashSpec.h
@@ -189,6 +189,10 @@
 #define     MV_MX25L6405_MAX_SPI_FREQ           20000000    /* 20MHz */
 #define     MV_MX25L6405_MAX_FAST_SPI_FREQ      50000000    /* 50MHz */
 #define     MV_MX25L6405_FAST_READ_DUMMY_BYTES   1
+#define     MV_MX25L12805E_DEVICE_ID             0x2018
+#define     MV_MX25L12805E_MAX_SPI_FREQ          20000000    /* 20MHz */
+#define     MV_MX25L12805E_MAX_FAST_SPI_FREQ     50000000    /* 50MHz */
+#define     MV_MX25L12805E_FAST_READ_DUMMY_BYTES 1
 #define     MV_MX25L25635E_DEVICE_ID             0x2019
 #define     MV_MX25L25635E_MAX_SPI_FREQ          20000000    /* 20MHz */
 #define     MV_MX25L25635E_MAX_FAST_SPI_FREQ     50000000    /* 50MHz */
@@ -199,10 +203,12 @@
 #define     MV_MX25L1605_SECTOR_SIZE            0x10000 /* 64K */
 #define     MV_MX25L3205_SECTOR_SIZE            0x10000 /* 64K */
 #define     MV_MX25L6405_SECTOR_SIZE            0x10000 /* 64K */
+#define     MV_MX25L12805E_SECTOR_SIZE          0x10000 /* 64K */
 #define     MV_MX25L25635E_SECTOR_SIZE          0x10000 /* 64K */
 #define     MV_MX25L1605_SECTOR_NUMBER          32
 #define     MV_MX25L3205_SECTOR_NUMBER          64
 #define     MV_MX25L6405_SECTOR_NUMBER          128
+#define     MV_MX25L12805E_SECTOR_NUMBER        256
 #define     MV_MX25L25635E_SECTOR_NUMBER        512
 
 #define		MV_MXIC_PAGE_SIZE			        0x100   /* 256 byte */
@@ -274,6 +280,76 @@
 #define     	MV_S25FL_STATUS_BP_1_OF_2           	(0x07 << MV_SFLASH_STATUS_REG_WP_OFFSET)
 #define     	MV_S25FL_STATUS_BP_ALL              	(0x0F << MV_SFLASH_STATUS_REG_WP_OFFSET)
 
+
+
+/************************************/
+/* WINBOND W25Qxxx Device Specific  */
+/************************************/
+
+/* Manufacturer IDs and Device IDs for SFLASHs supported by the driver */
+#define		MV_WINBOND_MANF_ID					0xEF
+#define     MV_W25Q128_DEVICE_ID                0x4018
+#define     MV_W25Q128_MAX_SPI_FREQ             20000000    /* 20MHz */
+#define     MV_W25Q128_MAX_FAST_SPI_FREQ        50000000    /* 50MHz */
+#define     MV_W25Q128_FAST_READ_DUMMY_BYTES    1
+
+#define     MV_W25Q256_DEVICE_ID                0x4019
+#define     MV_W25Q256_MAX_SPI_FREQ             20000000    /* 20MHz */
+#define     MV_W25Q256_MAX_FAST_SPI_FREQ        50000000    /* 50MHz */
+#define     MV_W25Q256_FAST_READ_DUMMY_BYTES    1
+
+/* Sector Sizes and population per device model*/
+#define     MV_W25Q128_SECTOR_SIZE              0x10000 /* 64K */
+#define     MV_W25Q128_SECTOR_NUMBER            256
+#define	    MV_W25Q_PAGE_SIZE					0x100   /* 256 byte */
+
+#define     MV_W25Q256_SECTOR_SIZE              0x10000 /* 64K */
+#define     MV_W25Q256_SECTOR_NUMBER            512		/* In the datasheet named block instead of sector */
+#define		MV_W25Q_FAST_RD_4B_CMND_OPCD		0x0C	/* Fast Read */
+#define		MV_W25Q_READ_4B_CMND_OPCD		    0x03	/* Sequential Read */
+#define	    MV_W25Q_PAGE_SIZE					0x100   /* 256 byte */
+
+#define     MV_W25Q256_SECTOR_SIZE              0x10000 /* 64K */
+#define     MV_W25Q256_SECTOR_NUMBER            512		/* In the datasheet named block instead of sector */
+#define		MV_W25Q_FAST_RD_4B_CMND_OPCD		0x0C	/* Fast Read */
+#define		MV_W25Q_READ_4B_CMND_OPCD		    0x03	/* Sequential Read */
+#define		MV_W25Q_WREN_CMND_OPCD			    0x06	/* Write Enable */
+#define		MV_W25Q_WRDI_CMND_OPCD			    0x04	/* Write Disable */
+#define		MV_W25Q_RDID_CMND_OPCD			    0x9F	/* Read ID */
+#define		MV_W25Q_RDSR_CMND_OPCD			    0x05	/* Read Status Register */
+#define		MV_W25Q_WRSR_CMND_OPCD			    0x01	/* Write Status Register */
+#define		MV_W25Q_READ_CMND_OPCD			    0x03	/* Sequential Read */
+#define		MV_W25Q_FAST_RD_CMND_OPCD		    0x0B	/* Fast Read */
+#define		MV_W25Q_PP_CMND_OPCD			    0x02	/* Page Program */
+#define		MV_W25Q_SE_CMND_OPCD			    0xD8	/* Sector Erase */
+#define		MV_W25Q_BE_CMND_OPCD			    0xC7	/* Bulk Erase */
+#define		MV_W25Q_RES_CMND_OPCD			    0xAB	/* Read Electronic Signature */
+#define		MV_W25Q_EN4B_CMND_OPCD			    0xB7	/* Enable 4-byte mode */
+
+/* Status Register Write Protect Bit Masks - 3bits */
+#define		MV_W25Q_STATUS_REG_WP_MASK	        (0x07 << MV_SFLASH_STATUS_REG_WP_OFFSET)
+#define     MV_W25Q_STATUS_BP_NONE              (0x00 << MV_SFLASH_STATUS_REG_WP_OFFSET)
+#define     MV_W25Q_STATUS_BP_1_OF_64           (0x01 << MV_SFLASH_STATUS_REG_WP_OFFSET)
+#define     MV_W25Q_STATUS_BP_1_OF_32           (0x02 << MV_SFLASH_STATUS_REG_WP_OFFSET)
+#define     MV_W25Q_STATUS_BP_1_OF_16           (0x03 << MV_SFLASH_STATUS_REG_WP_OFFSET)
+#define     MV_W25Q_STATUS_BP_1_OF_8            (0x04 << MV_SFLASH_STATUS_REG_WP_OFFSET)
+#define     MV_W25Q_STATUS_BP_1_OF_4            (0x05 << MV_SFLASH_STATUS_REG_WP_OFFSET)
+#define     MV_W25Q_STATUS_BP_1_OF_2            (0x06 << MV_SFLASH_STATUS_REG_WP_OFFSET)
+#define     MV_W25Q_STATUS_BP_ALL               (0x07 << MV_SFLASH_STATUS_REG_WP_OFFSET)
+
+/* Status Register Write Protect Bit Masks - 4bits */
+#define		MV_W25Q256_STATUS_REG_WP_MASK	        (0x0F << MV_SFLASH_STATUS_REG_WP_OFFSET)
+#define     MV_W25Q256_STATUS_BP_NONE              (0x00 << MV_SFLASH_STATUS_REG_WP_OFFSET)
+#define     MV_W25Q256_STATUS_BP_1_OF_128          (0x03 << MV_SFLASH_STATUS_REG_WP_OFFSET)
+#define     MV_W25Q256_STATUS_BP_1_OF_64           (0x04 << MV_SFLASH_STATUS_REG_WP_OFFSET)
+#define     MV_W25Q256_STATUS_BP_1_OF_32           (0x05 << MV_SFLASH_STATUS_REG_WP_OFFSET)
+#define     MV_W25Q256_STATUS_BP_1_OF_16           (0x06 << MV_SFLASH_STATUS_REG_WP_OFFSET)
+#define     MV_W25Q256_STATUS_BP_1_OF_8            (0x07 << MV_SFLASH_STATUS_REG_WP_OFFSET)
+#define     MV_W25Q256_STATUS_BP_1_OF_4            (0x08 << MV_SFLASH_STATUS_REG_WP_OFFSET)
+#define     MV_W25Q256_STATUS_BP_1_OF_2            (0x09 << MV_SFLASH_STATUS_REG_WP_OFFSET)
+#define     MV_W25Q256_STATUS_BP_ALL               (0x0C << MV_SFLASH_STATUS_REG_WP_OFFSET)
+
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/kernel/timeconst.pl b/kernel/timeconst.pl
index eb51d76..0461239 100644
--- a/kernel/timeconst.pl
+++ b/kernel/timeconst.pl
@@ -370,7 +370,7 @@
 	}
 
 	@val = @{$canned_values{$hz}};
-	if (!defined(@val)) {
+	if (!@val) {
 		@val = compute_values($hz);
 	}
 	output($hz, @val);