Move to rsdk 3.2.4. Compiles cleanly.

Signed-off-by: Roman Yeryomin <roman@advem.lv>

12 files changed, 3410 insertions, 1811 deletions

diff --git a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/96C/rtl865x_asicBasic.S b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/96C/rtl865x_asicBasic.S
index 9f3d9d621..7ec0e330f 100644
--- a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/96C/rtl865x_asicBasic.S
+++ b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/96C/rtl865x_asicBasic.S
@@ -382,31 +382,29 @@ $LC0:
 	.type	bsp_swcore_init, @function
 bsp_swcore_init:
 	.set	nomips16
-	.frame	$sp,48,$31		# vars= 0, regs= 8/0, args= 16, gp= 0
-	.mask	0x807f0000,-4
+	.frame	$sp,40,$31		# vars= 0, regs= 6/0, args= 16, gp= 0
+	.mask	0x801f0000,-4
 	.fmask	0x00000000,0
 	.set	noreorder
 	.set	nomacro
 	
-	addiu	$sp,$sp,-48
-	sw	$31,44($sp)
-	sw	$22,40($sp)
-	sw	$21,36($sp)
+	addiu	$sp,$sp,-40
+	sw	$31,36($sp)
 	sw	$20,32($sp)
 	sw	$19,28($sp)
 	sw	$18,24($sp)
 	sw	$17,20($sp)
 	sw	$16,16($sp)
-	move	$22,$4
-	move	$20,$0
-	li	$16,-1207959552			# 0xffffffffb8000000
-	lw	$2,0($16)
-	li	$17,-65536			# 0xffffffffffff0000
-	and	$18,$2,$17
-	ori	$21,$16,0xc
-	lw	$2,0($21)
-	nop
-	andi	$19,$2,0xf
+	move	$20,$4
+	move	$19,$0
+	li	$2,-1207959552			# 0xffffffffb8000000
+	lw	$4,0($2)
+	li	$3,-65536			# 0xffffffffffff0000
+	and	$16,$4,$3
+	ori	$2,$2,0xc
+	lw	$17,0($2)
+	nop
+	andi	$18,$17,0xf
 	lui	$4,%hi(fun_enable)
 	jal	rtl865x_initAsicFun
 	addiu	$4,$4,%lo(fun_enable)
@@ -415,117 +413,103 @@ bsp_swcore_init:
 	beq	$2,$3,$L36
 	li	$4,-1			# 0xffffffffffffffff
 
-	lw	$2,0($16)
-	nop
-	and	$2,$2,$17
-	li	$3,-1073741824			# 0xffffffffc0000000
-	bne	$2,$3,$L52
-	li	$2,-1073741824			# 0xffffffffc0000000
-
-	lw	$2,0($21)
-	nop
-	andi	$2,$2,0x8
-	bne	$2,$0,$L52
-	li	$2,-1073741824			# 0xffffffffc0000000
-
-	addiu	$2,$22,-8
-	sltu	$2,$2,2
-	bne	$2,$0,$L36
-	move	$4,$0
-
 	li	$2,-1073741824			# 0xffffffffc0000000
-$L52:
-	bne	$18,$2,$L41
+	bne	$16,$2,$L39
 	li	$2,-2147483648			# 0xffffffff80000000
 
 	li	$2,9			# 0x9
-	bne	$22,$2,$L44
-	move	$20,$0
+	bne	$20,$2,$L40
+	nop
+
+	beq	$18,$20,$L40
+	andi	$2,$17,0x8
 
-	beq	$19,$22,$L44
+	beq	$2,$0,$L40
 	lui	$4,%hi($LC0)
 
 	addiu	$4,$4,%lo($LC0)
 	jal	early_console_write
 	li	$5,27			# 0x1b
 
-	j	$L44
-	li	$20,-1			# 0xffffffffffffffff
+	j	$L42
+	li	$19,-1			# 0xffffffffffffffff
+
+$L40:
+	j	$L42
+	move	$19,$0
 
-$L41:
-	bne	$18,$2,$L36
-	move	$4,$20
+$L39:
+	bne	$16,$2,$L36
+	move	$4,$19
 
 	li	$2,7			# 0x7
-	bne	$22,$2,$L53
+	bne	$20,$2,$L50
 	li	$2,15			# 0xf
 
-	beq	$19,$22,$L53
+	beq	$18,$20,$L50
 	lui	$4,%hi($LC0)
 
 	addiu	$4,$4,%lo($LC0)
 	jal	early_console_write
 	li	$5,27			# 0x1b
 
-	j	$L44
-	li	$20,-1			# 0xffffffffffffffff
+	j	$L42
+	li	$19,-1			# 0xffffffffffffffff
 
-$L53:
-	bne	$22,$2,$L54
+$L50:
+	bne	$20,$2,$L51
 	li	$2,3			# 0x3
 
-	beq	$19,$22,$L48
+	beq	$18,$20,$L46
 	li	$2,7			# 0x7
 
-	beq	$19,$2,$L48
+	beq	$18,$2,$L46
 	li	$2,3			# 0x3
 
-	beq	$19,$2,$L48
+	beq	$18,$2,$L46
 	li	$2,11			# 0xb
 
-	beq	$19,$2,$L48
+	beq	$18,$2,$L46
 	lui	$4,%hi($LC0)
 
 	addiu	$4,$4,%lo($LC0)
 	jal	early_console_write
 	li	$5,27			# 0x1b
 
-	j	$L44
-	li	$20,-1			# 0xffffffffffffffff
+	j	$L42
+	li	$19,-1			# 0xffffffffffffffff
 
-$L48:
+$L46:
 	li	$2,3			# 0x3
-$L54:
-	bne	$22,$2,$L44
-	move	$20,$0
+$L51:
+	bne	$20,$2,$L42
+	move	$19,$0
 
-	beq	$19,$22,$L44
+	beq	$18,$20,$L42
 	li	$2,7			# 0x7
 
-	beq	$19,$2,$L55
+	beq	$18,$2,$L52
 	lui	$4,%hi($LC0)
 
 	addiu	$4,$4,%lo($LC0)
 	jal	early_console_write
 	li	$5,27			# 0x1b
 
-	li	$20,-1			# 0xffffffffffffffff
-$L44:
-$L55:
-	move	$4,$20
+	li	$19,-1			# 0xffffffffffffffff
+$L42:
+$L52:
+	move	$4,$19
 $L38:
 $L36:
 	move	$2,$4
-	lw	$31,44($sp)
-	lw	$22,40($sp)
-	lw	$21,36($sp)
+	lw	$31,36($sp)
 	lw	$20,32($sp)
 	lw	$19,28($sp)
 	lw	$18,24($sp)
 	lw	$17,20($sp)
 	lw	$16,16($sp)
 	j	$31
-	addiu	$sp,$sp,48
+	addiu	$sp,$sp,40
 
 	.set	macro
 	.set	reorder
@@ -553,60 +537,60 @@ rtl865x_accessAsicTable:
 	addiu	$4,$sp,16
 
 	sltu	$2,$16,15
-	beq	$2,$0,$L72
+	beq	$2,$0,$L69
 	li	$2,1			# 0x1
 
 	li	$3,1			# 0x1
 	sll	$4,$3,$16
 	andi	$2,$4,0xe22
-	bne	$2,$0,$L65
+	bne	$2,$0,$L62
 	andi	$2,$4,0x8
 
-	bne	$2,$0,$L68
+	bne	$2,$0,$L65
 	andi	$2,$4,0x4000
 
-	beq	$2,$0,$L71
+	beq	$2,$0,$L68
 	li	$2,1			# 0x1
 
 	lw	$2,16($sp)
 	nop
 	andi	$2,$2,0x20
 	movz	$3,$0,$2		#RLX4181/RLX4281:conditional move
-	j	$L57
+	j	$L54
 	sw	$3,0($17)
 
-$L65:
+$L62:
 	lw	$2,16($sp)
 	nop
 	andi	$2,$2,0x4
-	beq	$2,$0,$L66
+	beq	$2,$0,$L63
 	li	$2,1			# 0x1
 
-	j	$L57
+	j	$L54
 	sw	$2,0($17)
 
-$L66:
-	j	$L57
+$L63:
+	j	$L54
 	sw	$0,0($17)
 
-$L68:
+$L65:
 	lw	$2,16($sp)
 	nop
 	andi	$2,$2,0x2
-	beq	$2,$0,$L69
+	beq	$2,$0,$L66
 	li	$2,1			# 0x1
 
-	j	$L57
+	j	$L54
 	sw	$2,0($17)
 
-$L69:
-	j	$L57
+$L66:
+	j	$L54
 	sw	$0,0($17)
 
-$L71:
-$L72:
+$L68:
+$L69:
 	sw	$2,0($17)
-$L57:
+$L54:
 	move	$2,$0
 	lw	$31,32($sp)
 	lw	$17,28($sp)
@@ -617,32 +601,50 @@ $L57:
 	.set	macro
 	.set	reorder
 	.end	rtl865x_accessAsicTable
+	.rdata
+	.align	2
+$LC1:
+	.ascii	"\n"
+	.ascii	"Assert Fail: %s %d\000"
+	.align	2
+$LC2:
+	.ascii	"drivers/net/rtl819x/rtl865x/../AsicDriver/rtl865x_asicBa"
+	.ascii	"sic.c\000"
 	.section	.text._rtl8651_asicTableAccessForward,"ax",@progbits
 	.align	2
 	.ent	_rtl8651_asicTableAccessForward
 	.type	_rtl8651_asicTableAccessForward, @function
 _rtl8651_asicTableAccessForward:
 	.set	nomips16
-	.frame	$sp,0,$31		# vars= 0, regs= 0/0, args= 0, gp= 0
-	.mask	0x00000000,0
+	.frame	$sp,24,$31		# vars= 0, regs= 1/0, args= 16, gp= 0
+	.mask	0x80000000,-8
 	.fmask	0x00000000,0
 	.set	noreorder
 	.set	nomacro
 	
-	bne	$6,$0,$L74
-	li	$2,-1149239296			# 0xffffffffbb800000
+	addiu	$sp,$sp,-24
+	bne	$6,$0,$L71
+	sw	$31,16($sp)
 
-$L76:
-	j	$L76
+	lui	$4,%hi($LC1)
+	addiu	$4,$4,%lo($LC1)
+	lui	$5,%hi($LC2)
+	addiu	$5,$5,%lo($LC2)
+	jal	panic_printk
+	li	$6,324			# 0x144
+
+$L72:
+	j	$L72
 	nop
 
-$L74:
+$L71:
+	li	$2,-1149239296			# 0xffffffffbb800000
 	ori	$3,$2,0x4d00
-$L78:
+$L74:
 	lw	$2,0($3)
 	nop
 	andi	$2,$2,0x1
-	bne	$2,$0,$L78
+	bne	$2,$0,$L74
 	sll	$2,$4,2
 
 	move	$8,$2
@@ -651,13 +653,13 @@ $L78:
 	addu	$2,$2,$3
 	lw	$2,0($2)
 	nop
-	beq	$2,$0,$L85
+	beq	$2,$0,$L81
 	move	$7,$0
 
 	li	$9,-1149239296			# 0xffffffffbb800000
 	addu	$8,$8,$3
 	sll	$2,$7,2
-$L86:
+$L82:
 	addu	$3,$2,$9
 	addu	$2,$2,$6
 	lw	$2,0($2)
@@ -667,10 +669,10 @@ $L86:
 	lw	$2,0($8)
 	nop
 	sltu	$2,$7,$2
-	bne	$2,$0,$L86
+	bne	$2,$0,$L82
 	sll	$2,$7,2
 
-$L85:
+$L81:
 	sll	$3,$4,16
 	sll	$2,$5,5
 	addu	$3,$3,$2
@@ -679,8 +681,10 @@ $L85:
 	li	$2,-1149239296			# 0xffffffffbb800000
 	ori	$2,$2,0x4d08
 	sw	$3,0($2)
-	j	$31
+	lw	$31,16($sp)
 	nop
+	j	$31
+	addiu	$sp,$sp,24
 
 	.set	macro
 	.set	reorder
@@ -711,7 +715,7 @@ _rtl8651_addAsicEntry:
 
 	lw	$2,16($sp)
 	nop
-	beq	$2,$0,$L87
+	beq	$2,$0,$L83
 	li	$3,-1			# 0xffffffffffffffff
 
 	move	$4,$16
@@ -722,28 +726,28 @@ _rtl8651_addAsicEntry:
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,2			# 0x2
-	bne	$3,$2,$L103
+	bne	$3,$2,$L99
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
 	nop
-	bgtz	$2,$L104
+	bgtz	$2,$L100
 	li	$3,-1149239296			# 0xffffffffbb800000
 
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
-$L103:
+$L99:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,3			# 0x3
-	beq	$3,$2,$L90
+	beq	$3,$2,$L86
 	li	$2,4			# 0x4
 
-	bne	$3,$2,$L105
+	bne	$3,$2,$L101
 	li	$2,3			# 0x3
 
-$L90:
+$L86:
 	li	$3,-1149239296			# 0xffffffffbb800000
-$L104:
+$L100:
 	ori	$3,$3,0x4418
 	lw	$2,0($3)
 	li	$4,262144			# 0x40000
@@ -751,52 +755,52 @@ $L104:
 	sw	$2,0($3)
 	move	$4,$3
 	li	$3,524288			# 0x80000
-$L92:
+$L88:
 	lw	$2,0($4)
 	nop
 	and	$2,$2,$3
-	beq	$2,$0,$L92
+	beq	$2,$0,$L88
 	li	$2,3			# 0x3
 
-$L105:
+$L101:
 	li	$3,-1149239296			# 0xffffffffbb800000
 	ori	$3,$3,0x4d00
 	sw	$2,0($3)
-$L94:
+$L90:
 	lw	$2,0($3)
 	nop
 	andi	$2,$2,0x1
-	bne	$2,$0,$L94
+	bne	$2,$0,$L90
 	li	$2,-1149239296			# 0xffffffffbb800000
 
 	ori	$2,$2,0x4d04
 	lw	$2,0($2)
 	nop
 	andi	$2,$2,0x1
-	beq	$2,$0,$L96
+	beq	$2,$0,$L92
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,2			# 0x2
-	bne	$3,$2,$L106
+	bne	$3,$2,$L102
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
 	nop
-	bgtz	$2,$L98
+	bgtz	$2,$L94
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
-$L106:
+$L102:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,3			# 0x3
-	beq	$3,$2,$L98
+	beq	$3,$2,$L94
 	li	$2,4			# 0x4
 
-	bne	$3,$2,$L97
+	bne	$3,$2,$L93
 	nop
 
-$L98:
+$L94:
 	li	$4,-1149239296			# 0xffffffffbb800000
 	ori	$4,$4,0x4418
 	lw	$3,0($4)
@@ -804,35 +808,35 @@ $L98:
 	ori	$2,$2,0xffff
 	and	$3,$3,$2
 	sw	$3,0($4)
-$L97:
-	j	$L87
+$L93:
+	j	$L83
 	li	$3,-1			# 0xffffffffffffffff
 
-$L96:
+$L92:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,2			# 0x2
-	bne	$3,$2,$L107
+	bne	$3,$2,$L103
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
 	nop
-	bgtz	$2,$L108
+	bgtz	$2,$L104
 	li	$4,-1149239296			# 0xffffffffbb800000
 
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
-$L107:
+$L103:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,3			# 0x3
-	beq	$3,$2,$L101
+	beq	$3,$2,$L97
 	li	$2,4			# 0x4
 
-	bne	$3,$2,$L87
+	bne	$3,$2,$L83
 	move	$3,$0
 
-$L101:
+$L97:
 	li	$4,-1149239296			# 0xffffffffbb800000
-$L108:
+$L104:
 	ori	$4,$4,0x4418
 	lw	$3,0($4)
 	li	$2,-327680			# 0xfffffffffffb0000
@@ -840,7 +844,7 @@ $L108:
 	and	$3,$3,$2
 	sw	$3,0($4)
 	move	$3,$0
-$L87:
+$L83:
 	move	$2,$3
 	lw	$31,36($sp)
 	lw	$18,32($sp)
@@ -905,11 +909,11 @@ _rtl8651_forceAddAsicEntry:
 
 	lw	$2,16($sp)
 	nop
-	beq	$2,$0,$L110
+	beq	$2,$0,$L106
 	li	$3,-1			# 0xffffffffffffffff
 
 	li	$2,3			# 0x3
-	bne	$16,$2,$L123
+	bne	$16,$2,$L119
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$3,%hi(mcastForceAddOpCnt)
@@ -918,31 +922,31 @@ _rtl8651_forceAddAsicEntry:
 	addiu	$2,$2,1
 	sw	$2,%lo(mcastForceAddOpCnt)($3)
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
-$L123:
+$L119:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,2			# 0x2
-	bne	$3,$2,$L124
+	bne	$3,$2,$L120
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
 	nop
-	bgtz	$2,$L125
+	bgtz	$2,$L121
 	li	$3,-1149239296			# 0xffffffffbb800000
 
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
-$L124:
+$L120:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,3			# 0x3
-	beq	$3,$2,$L114
+	beq	$3,$2,$L110
 	li	$2,4			# 0x4
 
-	bne	$3,$2,$L126
+	bne	$3,$2,$L122
 	move	$4,$16
 
-$L114:
+$L110:
 	li	$3,-1149239296			# 0xffffffffbb800000
-$L125:
+$L121:
 	ori	$3,$3,0x4418
 	lw	$2,0($3)
 	li	$4,262144			# 0x40000
@@ -950,15 +954,15 @@ $L125:
 	sw	$2,0($3)
 	move	$4,$3
 	li	$3,524288			# 0x80000
-$L116:
+$L112:
 	lw	$2,0($4)
 	nop
 	and	$2,$2,$3
-	beq	$2,$0,$L116
+	beq	$2,$0,$L112
 	nop
 
 	move	$4,$16
-$L126:
+$L122:
 	move	$5,$17
 	jal	_rtl8651_asicTableAccessForward
 	move	$6,$18
@@ -967,37 +971,37 @@ $L126:
 	li	$3,-1149239296			# 0xffffffffbb800000
 	ori	$3,$3,0x4d00
 	sw	$2,0($3)
-$L118:
+$L114:
 	lw	$2,0($3)
 	nop
 	andi	$2,$2,0x1
-	bne	$2,$0,$L118
+	bne	$2,$0,$L114
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,2			# 0x2
-	bne	$3,$2,$L127
+	bne	$3,$2,$L123
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
 	nop
-	bgtz	$2,$L128
+	bgtz	$2,$L124
 	li	$4,-1149239296			# 0xffffffffbb800000
 
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
-$L127:
+$L123:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,3			# 0x3
-	beq	$3,$2,$L121
+	beq	$3,$2,$L117
 	li	$2,4			# 0x4
 
-	bne	$3,$2,$L110
+	bne	$3,$2,$L106
 	move	$3,$0
 
-$L121:
+$L117:
 	li	$4,-1149239296			# 0xffffffffbb800000
-$L128:
+$L124:
 	ori	$4,$4,0x4418
 	lw	$3,0($4)
 	li	$2,-327680			# 0xfffffffffffb0000
@@ -1005,7 +1009,7 @@ $L128:
 	and	$3,$3,$2
 	sw	$3,0($4)
 	move	$3,$0
-$L110:
+$L106:
 	move	$2,$3
 	lw	$31,36($sp)
 	lw	$18,32($sp)
@@ -1024,15 +1028,14 @@ $L110:
 	.type	_rtl8651_readAsicEntry, @function
 _rtl8651_readAsicEntry:
 	.set	nomips16
-	.frame	$sp,112,$31		# vars= 72, regs= 6/0, args= 16, gp= 0
-	.mask	0x801f0000,-4
+	.frame	$sp,112,$31		# vars= 72, regs= 5/0, args= 16, gp= 0
+	.mask	0x800f0000,-8
 	.fmask	0x00000000,0
 	.set	noreorder
 	.set	nomacro
 	
 	addiu	$sp,$sp,-112
-	sw	$31,108($sp)
-	sw	$20,104($sp)
+	sw	$31,104($sp)
 	sw	$19,100($sp)
 	sw	$18,96($sp)
 	sw	$17,92($sp)
@@ -1040,145 +1043,137 @@ _rtl8651_readAsicEntry:
 	move	$17,$4
 	move	$18,$5
 	move	$19,$6
-	li	$16,2			# 0x2
-	li	$20,10			# 0xa
+	li	$16,10			# 0xa
 	jal	rtl865x_accessAsicTable
 	addiu	$5,$sp,80
 
 	lw	$2,80($sp)
 	nop
-	beq	$2,$0,$L129
+	beq	$2,$0,$L125
 	li	$3,-1			# 0xffffffffffffffff
 
-	bne	$19,$0,$L131
-	sll	$10,$17,16
+	bne	$19,$0,$L127
+	sll	$6,$17,16
 
-$L133:
-	j	$L133
+	lui	$4,%hi($LC1)
+	addiu	$4,$4,%lo($LC1)
+	lui	$5,%hi($LC2)
+	addiu	$5,$5,%lo($LC2)
+	jal	panic_printk
+	li	$6,462			# 0x1ce
+
+$L128:
+	j	$L128
 	nop
 
-$L131:
+$L127:
 	sll	$2,$18,5
-	addu	$10,$10,$2
+	addu	$6,$6,$2
 	li	$2,-1157627904			# 0xffffffffbb000000
-	addu	$10,$10,$2
+	addu	$6,$6,$2
 	li	$2,-1149239296			# 0xffffffffbb800000
 	ori	$3,$2,0x4d00
-$L135:
+$L130:
 	lw	$2,0($3)
 	nop
 	andi	$2,$2,0x1
-	bne	$2,$0,$L135
-	addiu	$12,$sp,16
-
-$L137:
-	move	$9,$0
-$L144:
-	beq	$16,$9,$L142
-	sll	$3,$9,5
+	bne	$2,$0,$L130
+	addiu	$10,$sp,16
 
-	addu	$3,$3,$12
-	lw	$2,0($10)
+	move	$12,$10
+	li	$11,1			# 0x1
+$L132:
+	move	$8,$0
+	sll	$3,$8,5
+$L160:
+	addu	$3,$3,$10
+	lw	$2,0($6)
 	nop
 	sw	$2,0($3)
-	lw	$2,4($10)
+	lw	$2,4($6)
 	nop
 	sw	$2,4($3)
-	lw	$2,8($10)
+	lw	$2,8($6)
 	nop
 	sw	$2,8($3)
-	lw	$2,12($10)
+	lw	$2,12($6)
 	nop
 	sw	$2,12($3)
-	lw	$2,16($10)
+	lw	$2,16($6)
 	nop
 	sw	$2,16($3)
-	lw	$2,20($10)
+	lw	$2,20($6)
 	nop
 	sw	$2,20($3)
-	lw	$2,24($10)
+	lw	$2,24($6)
 	nop
 	sw	$2,24($3)
-	lw	$2,28($10)
+	lw	$2,28($6)
 	nop
 	sw	$2,28($3)
-	move	$16,$9
-$L142:
-	addiu	$9,$9,1
-	sltu	$2,$9,2
-	bne	$2,$0,$L144
-	move	$11,$0
+	addiu	$8,$8,1
+	sltu	$2,$8,2
+	bne	$2,$0,$L160
+	sll	$3,$8,5
 
 	move	$9,$0
-	addiu	$13,$sp,16
-	sltu	$2,$9,2
-$L173:
-	beq	$2,$0,$L167
-	move	$8,$9
-
-	sll	$7,$9,3
-	move	$5,$13
-$L157:
+	li	$8,1			# 0x1
+	move	$5,$12
+$L148:
 	move	$4,$0
-	sll	$6,$8,3
+	sll	$7,$8,3
 	addu	$3,$7,$4
-$L172:
+$L161:
 	sll	$3,$3,2
 	addu	$3,$3,$5
-	addu	$2,$6,$4
-	sll	$2,$2,2
+	sll	$2,$4,2
 	addu	$2,$2,$5
 	lw	$3,0($3)
 	lw	$2,0($2)
 	nop
-	beq	$3,$2,$L153
+	beq	$3,$2,$L144
 	addiu	$4,$4,1
 
 	addiu	$4,$4,-1
-	j	$L155
-	li	$11,1			# 0x1
+	j	$L146
+	li	$9,1			# 0x1
 
-$L153:
+$L144:
 	slt	$2,$4,8
-	bne	$2,$0,$L172
+	bne	$2,$0,$L161
 	addu	$3,$7,$4
 
 	addiu	$8,$8,1
 	sltu	$2,$8,2
-	bne	$2,$0,$L157
-	nop
-
-$L167:
-	addiu	$9,$9,1
-	sltu	$2,$9,2
-	bne	$2,$0,$L173
+	bne	$2,$0,$L148
 	nop
 
-$L155:
-	bne	$11,$0,$L138
-	addiu	$20,$20,-1
+$L146:
+	bne	$9,$11,$L133
+	addiu	$16,$16,-1
 
-	bne	$20,$0,$L137
+	bne	$16,$0,$L132
 	nop
 
-$L138:
-	sll	$3,$16,5
-	addiu	$2,$sp,16
-	addu	$10,$2,$3
+$L133:
+	li	$2,1			# 0x1
+	sll	$2,$2,5
+	addiu	$3,$sp,16
+	addu	$6,$3,$2
 	sll	$3,$17,2
 	lui	$2,%hi(_rtl8651_asicTableSize)
 	addiu	$2,$2,%lo(_rtl8651_asicTableSize)
 	addu	$3,$3,$2
 	lw	$2,0($3)
 	nop
-	beq	$2,$0,$L171
+	beq	$2,$0,$L159
 	move	$5,$0
 
 	move	$4,$3
 	sll	$2,$5,2
-$L174:
+$L162:
 	addu	$3,$2,$19
-	addu	$2,$2,$10
+	addu	$2,$2,$6
 	lw	$2,0($2)
 	nop
 	sw	$2,0($3)
@@ -1186,15 +1181,14 @@ $L174:
 	lw	$2,0($4)
 	nop
 	sltu	$2,$5,$2
-	bne	$2,$0,$L174
+	bne	$2,$0,$L162
 	sll	$2,$5,2
 
-$L171:
+$L159:
 	move	$3,$0
-$L129:
+$L125:
 	move	$2,$3
-	lw	$31,108($sp)
-	lw	$20,104($sp)
+	lw	$31,104($sp)
 	lw	$19,100($sp)
 	lw	$18,96($sp)
 	lw	$17,92($sp)
@@ -1231,17 +1225,24 @@ _rtl8651_readAsicEntryStopTLU:
 
 	lw	$2,16($sp)
 	nop
-	beq	$2,$0,$L175
+	beq	$2,$0,$L163
 	li	$3,-1			# 0xffffffffffffffff
 
-	bne	$18,$0,$L177
+	bne	$18,$0,$L165
 	sll	$6,$16,16
 
-$L179:
-	j	$L179
+	lui	$4,%hi($LC1)
+	addiu	$4,$4,%lo($LC1)
+	lui	$5,%hi($LC2)
+	addiu	$5,$5,%lo($LC2)
+	jal	panic_printk
+	li	$6,557			# 0x22d
+
+$L166:
+	j	$L166
 	nop
 
-$L177:
+$L165:
 	sll	$2,$17,5
 	addu	$6,$6,$2
 	li	$2,-1157627904			# 0xffffffffbb000000
@@ -1249,47 +1250,47 @@ $L177:
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,2			# 0x2
-	bne	$3,$2,$L193
+	bne	$3,$2,$L180
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
 	nop
-	bgtz	$2,$L194
+	bgtz	$2,$L181
 	li	$2,-1149239296			# 0xffffffffbb800000
 
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
-$L193:
+$L180:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,3			# 0x3
-	beq	$3,$2,$L182
+	beq	$3,$2,$L169
 	li	$2,4			# 0x4
 
-	bne	$3,$2,$L195
+	bne	$3,$2,$L182
 	sll	$2,$16,2
 
-$L182:
+$L169:
 	li	$2,-1149239296			# 0xffffffffbb800000
-$L194:
+$L181:
 	ori	$2,$2,0x4418
 	lw	$3,0($2)
 	li	$4,262144			# 0x40000
 	or	$3,$3,$4
 	sw	$3,0($2)
 	sll	$2,$16,2
-$L195:
+$L182:
 	move	$4,$2
 	lui	$3,%hi(_rtl8651_asicTableSize)
 	addiu	$3,$3,%lo(_rtl8651_asicTableSize)
 	addu	$2,$2,$3
 	lw	$2,0($2)
 	nop
-	beq	$2,$0,$L192
+	beq	$2,$0,$L179
 	move	$5,$0
 
 	addu	$4,$4,$3
 	sll	$2,$5,2
-$L196:
+$L183:
 	addu	$3,$2,$18
 	addu	$2,$2,$6
 	lw	$2,0($2)
@@ -1299,32 +1300,32 @@ $L196:
 	lw	$2,0($4)
 	nop
 	sltu	$2,$5,$2
-	bne	$2,$0,$L196
+	bne	$2,$0,$L183
 	sll	$2,$5,2
 
-$L192:
+$L179:
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,2			# 0x2
-	bne	$3,$2,$L197
+	bne	$3,$2,$L184
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
 	nop
-	bgtz	$2,$L189
+	bgtz	$2,$L176
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
-$L197:
+$L184:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,3			# 0x3
-	beq	$3,$2,$L189
+	beq	$3,$2,$L176
 	li	$2,4			# 0x4
 
-	bne	$3,$2,$L175
+	bne	$3,$2,$L163
 	move	$3,$0
 
-$L189:
+$L176:
 	li	$4,-1149239296			# 0xffffffffbb800000
 	ori	$4,$4,0x4418
 	lw	$3,0($4)
@@ -1333,7 +1334,7 @@ $L189:
 	and	$3,$3,$2
 	sw	$3,0($4)
 	move	$3,$0
-$L175:
+$L163:
 	move	$2,$3
 	lw	$31,36($sp)
 	lw	$18,32($sp)
@@ -1360,11 +1361,11 @@ _rtl8651_delAsicEntry:
 	
 	li	$2,-1149239296			# 0xffffffffbb800000
 	ori	$3,$2,0x4d00
-$L199:
+$L186:
 	lw	$2,0($3)
 	nop
 	andi	$2,$2,0x1
-	bne	$2,$0,$L199
+	bne	$2,$0,$L186
 	sll	$2,$4,2
 
 	move	$7,$2
@@ -1373,7 +1374,7 @@ $L199:
 	addu	$2,$2,$3
 	lw	$2,0($2)
 	nop
-	beq	$2,$0,$L212
+	beq	$2,$0,$L199
 	move	$3,$0
 
 	li	$8,-1149239296			# 0xffffffffbb800000
@@ -1381,19 +1382,19 @@ $L199:
 	addiu	$2,$2,%lo(_rtl8651_asicTableSize)
 	addu	$7,$7,$2
 	sll	$2,$3,2
-$L215:
+$L202:
 	addu	$2,$2,$8
 	sw	$0,19744($2)
 	addiu	$3,$3,1
 	lw	$2,0($7)
 	nop
 	sltu	$2,$3,$2
-	bne	$2,$0,$L215
+	bne	$2,$0,$L202
 	sll	$2,$3,2
 
-$L212:
+$L199:
 	sltu	$2,$6,$5
-	bne	$2,$0,$L214
+	bne	$2,$0,$L201
 	nop
 
 	li	$2,-1149239296			# 0xffffffffbb800000
@@ -1403,41 +1404,187 @@ $L212:
 	ori	$3,$2,0x4d00
 	li	$8,9			# 0x9
 	ori	$7,$2,0x4d04
-$L210:
+$L197:
 	sll	$2,$5,5
 	addu	$2,$4,$2
 	addu	$2,$2,$9
 	sw	$2,0($10)
 	sw	$8,0($3)
-$L207:
+$L194:
 	lw	$2,0($3)
 	nop
 	andi	$2,$2,0x1
-	bne	$2,$0,$L207
+	bne	$2,$0,$L194
 	nop
 
 	lw	$2,0($7)
 	nop
 	andi	$2,$2,0x1
-	beq	$2,$0,$L209
+	beq	$2,$0,$L196
 	addiu	$5,$5,1
 
 	addiu	$5,$5,-1
 	j	$31
 	li	$2,-1			# 0xffffffffffffffff
 
-$L209:
+$L196:
 	sltu	$2,$6,$5
-	beq	$2,$0,$L210
+	beq	$2,$0,$L197
 	nop
 
-$L214:
+$L201:
 	j	$31
 	move	$2,$0
 
 	.set	macro
 	.set	reorder
 	.end	_rtl8651_delAsicEntry
+	.data
+	.align	2
+	.type	rtl819x_lastTxDesc, @object
+	.size	rtl819x_lastTxDesc, 4
+rtl819x_lastTxDesc:
+	.word	0
+	.align	2
+	.type	rtl819x_lastRxDesc, @object
+	.size	rtl819x_lastRxDesc, 4
+rtl819x_lastRxDesc:
+	.word	0
+	.align	2
+	.type	rtl819x_swHangCnt, @object
+	.size	rtl819x_swHangCnt, 4
+rtl819x_swHangCnt:
+	.word	0
+	.align	2
+	.type	rtl819x_checkSwCoreTimer, @object
+	.size	rtl819x_checkSwCoreTimer, 4
+rtl819x_checkSwCoreTimer:
+	.word	0
+	.rdata
+	.align	2
+$LC3:
+	.ascii	".........................................\n\000"
+	.section	.text.rtl819x_poll_sw,"ax",@progbits
+	.align	2
+	.globl	rtl819x_poll_sw
+	.ent	rtl819x_poll_sw
+	.type	rtl819x_poll_sw, @function
+rtl819x_poll_sw:
+	.set	nomips16
+	.frame	$sp,24,$31		# vars= 0, regs= 1/0, args= 16, gp= 0
+	.mask	0x80000000,-8
+	.fmask	0x00000000,0
+	.set	noreorder
+	.set	nomacro
+	
+	addiu	$sp,$sp,-24
+	sw	$31,16($sp)
+	li	$2,-1149239296			# 0xffffffffbb800000
+	ori	$2,$2,0x610c
+	lw	$3,0($2)
+	li	$2,65536			# 0x10000
+	and	$8,$3,$2
+	li	$3,-1207894016			# 0xffffffffb8010000
+	ori	$2,$3,0x4
+	lw	$2,0($2)
+	li	$4,-4			# 0xfffffffffffffffc
+	and	$6,$2,$4
+	ori	$3,$3,0x20
+	lw	$2,0($3)
+	nop
+	and	$7,$2,$4
+	lui	$5,%hi(rtl819x_checkSwCoreTimer)
+	lw	$4,%lo(rtl819x_checkSwCoreTimer)($5)
+	li	$2,-859045888			# 0xffffffffcccc0000
+	ori	$2,$2,0xcccd
+	multu	$4,$2
+	mfhi	$2
+	srl	$2,$2,4
+	sll	$3,$2,2
+	addu	$3,$3,$2
+	sll	$3,$3,2
+	subu	$3,$4,$3
+	addiu	$4,$4,1
+	bne	$3,$0,$L203
+	sw	$4,%lo(rtl819x_checkSwCoreTimer)($5)
+
+	bne	$8,$0,$L205
+	lui	$2,%hi(rtl819x_lastTxDesc)
+
+	lui	$2,%hi(rtl819x_swHangCnt)
+	j	$L206
+	sw	$0,%lo(rtl819x_swHangCnt)($2)
+
+$L205:
+	lw	$2,%lo(rtl819x_lastTxDesc)($2)
+	nop
+	beq	$2,$0,$L213
+	lui	$2,%hi(rtl819x_lastRxDesc)
+
+	lw	$2,%lo(rtl819x_lastRxDesc)($2)
+	nop
+	bne	$2,$0,$L207
+	lui	$2,%hi(rtl819x_lastRxDesc)
+
+$L213:
+	sw	$6,%lo(rtl819x_lastRxDesc)($2)
+	lui	$2,%hi(rtl819x_lastTxDesc)
+	sw	$7,%lo(rtl819x_lastTxDesc)($2)
+	lui	$2,%hi(rtl819x_swHangCnt)
+	j	$L206
+	sw	$0,%lo(rtl819x_swHangCnt)($2)
+
+$L207:
+	lw	$2,%lo(rtl819x_lastRxDesc)($2)
+	nop
+	bne	$2,$6,$L214
+	lui	$2,%hi(rtl819x_swHangCnt)
+
+	lui	$2,%hi(rtl819x_lastTxDesc)
+	lw	$2,%lo(rtl819x_lastTxDesc)($2)
+	nop
+	bne	$2,$7,$L210
+	lui	$2,%hi(rtl819x_swHangCnt)
+
+	lui	$3,%hi(rtl819x_swHangCnt)
+	lw	$2,%lo(rtl819x_swHangCnt)($3)
+	nop
+	addiu	$2,$2,1
+	j	$L206
+	sw	$2,%lo(rtl819x_swHangCnt)($3)
+
+$L210:
+$L214:
+	sw	$0,%lo(rtl819x_swHangCnt)($2)
+$L206:
+	lui	$2,%hi(rtl819x_lastRxDesc)
+	sw	$6,%lo(rtl819x_lastRxDesc)($2)
+	lui	$2,%hi(rtl819x_lastTxDesc)
+	sw	$7,%lo(rtl819x_lastTxDesc)($2)
+	lui	$3,%hi(rtl819x_swHangCnt)
+	lw	$2,%lo(rtl819x_swHangCnt)($3)
+	nop
+	sltu	$2,$2,3
+	bne	$2,$0,$L203
+	nop
+
+	sw	$0,%lo(rtl819x_swHangCnt)($3)
+	lui	$4,%hi($LC3)
+	jal	panic_printk
+	addiu	$4,$4,%lo($LC3)
+
+	jal	machine_restart
+	move	$4,$0
+
+$L203:
+	lw	$31,16($sp)
+	nop
+	j	$31
+	addiu	$sp,$sp,24
+
+	.set	macro
+	.set	reorder
+	.end	rtl819x_poll_sw
 	.globl	RtkHomeGatewayChipName
 	.section	.bss
 	.align	2
diff --git a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/96E/rtl865x_asicBasic.S b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/96E/rtl865x_asicBasic.S
index 4ec854b51..c6e965574 100644
--- a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/96E/rtl865x_asicBasic.S
+++ b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/96E/rtl865x_asicBasic.S
@@ -392,31 +392,29 @@ $LC0:
 	.type	bsp_swcore_init, @function
 bsp_swcore_init:
 	.set	nomips16
-	.frame	$sp,48,$31		# vars= 0, regs= 8/0, args= 16, gp= 0
-	.mask	0x807f0000,-4
+	.frame	$sp,40,$31		# vars= 0, regs= 6/0, args= 16, gp= 0
+	.mask	0x801f0000,-4
 	.fmask	0x00000000,0
 	.set	noreorder
 	.set	nomacro
 	
-	addiu	$sp,$sp,-48
-	sw	$31,44($sp)
-	sw	$22,40($sp)
-	sw	$21,36($sp)
+	addiu	$sp,$sp,-40
+	sw	$31,36($sp)
 	sw	$20,32($sp)
 	sw	$19,28($sp)
 	sw	$18,24($sp)
 	sw	$17,20($sp)
 	sw	$16,16($sp)
-	move	$22,$4
-	move	$19,$0
-	li	$16,-1207959552			# 0xffffffffb8000000
-	lw	$2,0($16)
-	li	$18,-65536			# 0xffffffffffff0000
-	and	$17,$2,$18
-	ori	$21,$16,0xc
-	lw	$2,0($21)
-	nop
-	andi	$20,$2,0xf
+	move	$20,$4
+	move	$17,$0
+	li	$2,-1207959552			# 0xffffffffb8000000
+	lw	$4,0($2)
+	li	$3,-65536			# 0xffffffffffff0000
+	and	$16,$4,$3
+	ori	$2,$2,0xc
+	lw	$18,0($2)
+	nop
+	andi	$19,$18,0xf
 	lui	$4,%hi(fun_enable)
 	jal	rtl865x_initAsicFun
 	addiu	$4,$4,%lo(fun_enable)
@@ -425,121 +423,107 @@ bsp_swcore_init:
 	beq	$2,$3,$L38
 	li	$4,-1			# 0xffffffffffffffff
 
-	lw	$2,0($16)
-	nop
-	and	$2,$2,$18
-	li	$3,-1073741824			# 0xffffffffc0000000
-	bne	$2,$3,$L56
-	li	$2,-1073741824			# 0xffffffffc0000000
-
-	lw	$2,0($21)
-	nop
-	andi	$2,$2,0x8
-	bne	$2,$0,$L56
-	li	$2,-1073741824			# 0xffffffffc0000000
-
-	addiu	$2,$22,-8
-	sltu	$2,$2,2
-	bne	$2,$0,$L38
-	move	$4,$0
-
 	li	$2,-1073741824			# 0xffffffffc0000000
-$L56:
-	bne	$17,$2,$L43
+	bne	$16,$2,$L41
 	li	$2,-2147483648			# 0xffffffff80000000
 
 	li	$2,9			# 0x9
-	bne	$22,$2,$L46
-	move	$19,$0
+	bne	$20,$2,$L42
+	nop
 
-	beq	$20,$22,$L46
+	beq	$19,$20,$L42
+	andi	$2,$18,0x8
+
+	beq	$2,$0,$L42
 	lui	$4,%hi($LC0)
 
 	addiu	$4,$4,%lo($LC0)
 	jal	early_console_write
 	li	$5,27			# 0x1b
 
-	j	$L46
-	li	$19,-1			# 0xffffffffffffffff
+	j	$L44
+	li	$17,-1			# 0xffffffffffffffff
+
+$L42:
+	j	$L44
+	move	$17,$0
 
-$L43:
-	bne	$17,$2,$L47
+$L41:
+	bne	$16,$2,$L45
 	li	$2,-2120876032			# 0xffffffff81960000
 
 	li	$2,7			# 0x7
-	bne	$22,$2,$L57
+	bne	$20,$2,$L54
 	li	$2,15			# 0xf
 
-	beq	$20,$22,$L57
+	beq	$19,$20,$L54
 	lui	$4,%hi($LC0)
 
 	addiu	$4,$4,%lo($LC0)
 	jal	early_console_write
 	li	$5,27			# 0x1b
 
-	j	$L46
-	li	$19,-1			# 0xffffffffffffffff
+	j	$L44
+	li	$17,-1			# 0xffffffffffffffff
 
-$L57:
-	bne	$22,$2,$L58
+$L54:
+	bne	$20,$2,$L55
 	li	$2,3			# 0x3
 
-	beq	$20,$22,$L50
+	beq	$19,$20,$L48
 	li	$2,7			# 0x7
 
-	beq	$20,$2,$L50
+	beq	$19,$2,$L48
 	li	$2,3			# 0x3
 
-	beq	$20,$2,$L50
+	beq	$19,$2,$L48
 	li	$2,11			# 0xb
 
-	beq	$20,$2,$L50
+	beq	$19,$2,$L48
 	lui	$4,%hi($LC0)
 
 	addiu	$4,$4,%lo($LC0)
 	jal	early_console_write
 	li	$5,27			# 0x1b
 
-	j	$L46
-	li	$19,-1			# 0xffffffffffffffff
+	j	$L44
+	li	$17,-1			# 0xffffffffffffffff
 
-$L50:
+$L48:
 	li	$2,3			# 0x3
-$L58:
-	bne	$22,$2,$L46
-	move	$19,$0
+$L55:
+	bne	$20,$2,$L44
+	move	$17,$0
 
-	beq	$20,$22,$L46
+	beq	$19,$20,$L44
 	li	$2,7			# 0x7
 
-	beq	$20,$2,$L46
+	beq	$19,$2,$L44
 	lui	$4,%hi($LC0)
 
 	addiu	$4,$4,%lo($LC0)
 	jal	early_console_write
 	li	$5,27			# 0x1b
 
-	j	$L46
-	li	$19,-1			# 0xffffffffffffffff
+	j	$L44
+	li	$17,-1			# 0xffffffffffffffff
 
-$L47:
-	xor	$2,$17,$2
-	movz	$19,$0,$2		#RLX4181/RLX4281:conditional move
-$L46:
-	move	$4,$19
+$L45:
+	xor	$2,$16,$2
+	movz	$17,$0,$2		#RLX4181/RLX4281:conditional move
+$L44:
+	move	$4,$17
 $L40:
 $L38:
 	move	$2,$4
-	lw	$31,44($sp)
-	lw	$22,40($sp)
-	lw	$21,36($sp)
+	lw	$31,36($sp)
 	lw	$20,32($sp)
 	lw	$19,28($sp)
 	lw	$18,24($sp)
 	lw	$17,20($sp)
 	lw	$16,16($sp)
 	j	$31
-	addiu	$sp,$sp,48
+	addiu	$sp,$sp,40
 
 	.set	macro
 	.set	reorder
@@ -567,60 +551,60 @@ rtl865x_accessAsicTable:
 	addiu	$4,$sp,16
 
 	sltu	$2,$16,15
-	beq	$2,$0,$L75
+	beq	$2,$0,$L72
 	li	$2,1			# 0x1
 
 	li	$3,1			# 0x1
 	sll	$4,$3,$16
 	andi	$2,$4,0xe22
-	bne	$2,$0,$L68
+	bne	$2,$0,$L65
 	andi	$2,$4,0x8
 
-	bne	$2,$0,$L71
+	bne	$2,$0,$L68
 	andi	$2,$4,0x4000
 
-	beq	$2,$0,$L74
+	beq	$2,$0,$L71
 	li	$2,1			# 0x1
 
 	lw	$2,16($sp)
 	nop
 	andi	$2,$2,0x20
 	movz	$3,$0,$2		#RLX4181/RLX4281:conditional move
-	j	$L60
+	j	$L57
 	sw	$3,0($17)
 
-$L68:
+$L65:
 	lw	$2,16($sp)
 	nop
 	andi	$2,$2,0x4
-	beq	$2,$0,$L69
+	beq	$2,$0,$L66
 	li	$2,1			# 0x1
 
-	j	$L60
+	j	$L57
 	sw	$2,0($17)
 
-$L69:
-	j	$L60
+$L66:
+	j	$L57
 	sw	$0,0($17)
 
-$L71:
+$L68:
 	lw	$2,16($sp)
 	nop
 	andi	$2,$2,0x2
-	beq	$2,$0,$L72
+	beq	$2,$0,$L69
 	li	$2,1			# 0x1
 
-	j	$L60
+	j	$L57
 	sw	$2,0($17)
 
-$L72:
-	j	$L60
+$L69:
+	j	$L57
 	sw	$0,0($17)
 
-$L74:
-$L75:
+$L71:
+$L72:
 	sw	$2,0($17)
-$L60:
+$L57:
 	move	$2,$0
 	lw	$31,32($sp)
 	lw	$17,28($sp)
@@ -653,7 +637,7 @@ _rtl8651_asicTableAccessForward:
 	.set	nomacro
 	
 	addiu	$sp,$sp,-24
-	bne	$6,$0,$L77
+	bne	$6,$0,$L74
 	sw	$31,16($sp)
 
 	lui	$4,%hi($LC1)
@@ -661,20 +645,20 @@ _rtl8651_asicTableAccessForward:
 	lui	$5,%hi($LC2)
 	addiu	$5,$5,%lo($LC2)
 	jal	panic_printk
-	li	$6,306			# 0x132
+	li	$6,324			# 0x144
 
-$L78:
-	j	$L78
+$L75:
+	j	$L75
 	nop
 
-$L77:
+$L74:
 	li	$2,-1149239296			# 0xffffffffbb800000
 	ori	$3,$2,0x4d00
-$L80:
+$L77:
 	lw	$2,0($3)
 	nop
 	andi	$2,$2,0x1
-	bne	$2,$0,$L80
+	bne	$2,$0,$L77
 	sll	$2,$4,2
 
 	move	$8,$2
@@ -683,13 +667,13 @@ $L80:
 	addu	$2,$2,$3
 	lw	$2,0($2)
 	nop
-	beq	$2,$0,$L87
+	beq	$2,$0,$L84
 	move	$7,$0
 
 	li	$9,-1149239296			# 0xffffffffbb800000
 	addu	$8,$8,$3
 	sll	$2,$7,2
-$L88:
+$L85:
 	addu	$3,$2,$9
 	addu	$2,$2,$6
 	lw	$2,0($2)
@@ -699,10 +683,10 @@ $L88:
 	lw	$2,0($8)
 	nop
 	sltu	$2,$7,$2
-	bne	$2,$0,$L88
+	bne	$2,$0,$L85
 	sll	$2,$7,2
 
-$L87:
+$L84:
 	sll	$3,$4,16
 	sll	$2,$5,5
 	addu	$3,$3,$2
@@ -745,7 +729,7 @@ _rtl8651_addAsicEntry:
 
 	lw	$2,16($sp)
 	nop
-	beq	$2,$0,$L89
+	beq	$2,$0,$L86
 	li	$3,-1			# 0xffffffffffffffff
 
 	move	$4,$16
@@ -756,28 +740,28 @@ _rtl8651_addAsicEntry:
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,2			# 0x2
-	bne	$3,$2,$L105
+	bne	$3,$2,$L102
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
 	nop
-	bgtz	$2,$L106
+	bgtz	$2,$L103
 	li	$3,-1149239296			# 0xffffffffbb800000
 
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
-$L105:
+$L102:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,3			# 0x3
-	beq	$3,$2,$L92
+	beq	$3,$2,$L89
 	li	$2,4			# 0x4
 
-	bne	$3,$2,$L107
+	bne	$3,$2,$L104
 	li	$2,3			# 0x3
 
-$L92:
+$L89:
 	li	$3,-1149239296			# 0xffffffffbb800000
-$L106:
+$L103:
 	ori	$3,$3,0x4418
 	lw	$2,0($3)
 	li	$4,262144			# 0x40000
@@ -785,52 +769,52 @@ $L106:
 	sw	$2,0($3)
 	move	$4,$3
 	li	$3,524288			# 0x80000
-$L94:
+$L91:
 	lw	$2,0($4)
 	nop
 	and	$2,$2,$3
-	beq	$2,$0,$L94
+	beq	$2,$0,$L91
 	li	$2,3			# 0x3
 
-$L107:
+$L104:
 	li	$3,-1149239296			# 0xffffffffbb800000
 	ori	$3,$3,0x4d00
 	sw	$2,0($3)
-$L96:
+$L93:
 	lw	$2,0($3)
 	nop
 	andi	$2,$2,0x1
-	bne	$2,$0,$L96
+	bne	$2,$0,$L93
 	li	$2,-1149239296			# 0xffffffffbb800000
 
 	ori	$2,$2,0x4d04
 	lw	$2,0($2)
 	nop
 	andi	$2,$2,0x1
-	beq	$2,$0,$L98
+	beq	$2,$0,$L95
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,2			# 0x2
-	bne	$3,$2,$L108
+	bne	$3,$2,$L105
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
 	nop
-	bgtz	$2,$L100
+	bgtz	$2,$L97
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
-$L108:
+$L105:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,3			# 0x3
-	beq	$3,$2,$L100
+	beq	$3,$2,$L97
 	li	$2,4			# 0x4
 
-	bne	$3,$2,$L99
+	bne	$3,$2,$L96
 	nop
 
-$L100:
+$L97:
 	li	$4,-1149239296			# 0xffffffffbb800000
 	ori	$4,$4,0x4418
 	lw	$3,0($4)
@@ -838,35 +822,35 @@ $L100:
 	ori	$2,$2,0xffff
 	and	$3,$3,$2
 	sw	$3,0($4)
-$L99:
-	j	$L89
+$L96:
+	j	$L86
 	li	$3,-1			# 0xffffffffffffffff
 
-$L98:
+$L95:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,2			# 0x2
-	bne	$3,$2,$L109
+	bne	$3,$2,$L106
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
 	nop
-	bgtz	$2,$L110
+	bgtz	$2,$L107
 	li	$4,-1149239296			# 0xffffffffbb800000
 
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
-$L109:
+$L106:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,3			# 0x3
-	beq	$3,$2,$L103
+	beq	$3,$2,$L100
 	li	$2,4			# 0x4
 
-	bne	$3,$2,$L89
+	bne	$3,$2,$L86
 	move	$3,$0
 
-$L103:
+$L100:
 	li	$4,-1149239296			# 0xffffffffbb800000
-$L110:
+$L107:
 	ori	$4,$4,0x4418
 	lw	$3,0($4)
 	li	$2,-327680			# 0xfffffffffffb0000
@@ -874,7 +858,7 @@ $L110:
 	and	$3,$3,$2
 	sw	$3,0($4)
 	move	$3,$0
-$L89:
+$L86:
 	move	$2,$3
 	lw	$31,36($sp)
 	lw	$18,32($sp)
@@ -939,11 +923,11 @@ _rtl8651_forceAddAsicEntry:
 
 	lw	$2,16($sp)
 	nop
-	beq	$2,$0,$L112
+	beq	$2,$0,$L109
 	li	$3,-1			# 0xffffffffffffffff
 
 	li	$2,3			# 0x3
-	bne	$16,$2,$L125
+	bne	$16,$2,$L122
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$3,%hi(mcastForceAddOpCnt)
@@ -952,31 +936,31 @@ _rtl8651_forceAddAsicEntry:
 	addiu	$2,$2,1
 	sw	$2,%lo(mcastForceAddOpCnt)($3)
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
-$L125:
+$L122:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,2			# 0x2
-	bne	$3,$2,$L126
+	bne	$3,$2,$L123
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
 	nop
-	bgtz	$2,$L127
+	bgtz	$2,$L124
 	li	$3,-1149239296			# 0xffffffffbb800000
 
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
-$L126:
+$L123:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,3			# 0x3
-	beq	$3,$2,$L116
+	beq	$3,$2,$L113
 	li	$2,4			# 0x4
 
-	bne	$3,$2,$L128
+	bne	$3,$2,$L125
 	move	$4,$16
 
-$L116:
+$L113:
 	li	$3,-1149239296			# 0xffffffffbb800000
-$L127:
+$L124:
 	ori	$3,$3,0x4418
 	lw	$2,0($3)
 	li	$4,262144			# 0x40000
@@ -984,15 +968,15 @@ $L127:
 	sw	$2,0($3)
 	move	$4,$3
 	li	$3,524288			# 0x80000
-$L118:
+$L115:
 	lw	$2,0($4)
 	nop
 	and	$2,$2,$3
-	beq	$2,$0,$L118
+	beq	$2,$0,$L115
 	nop
 
 	move	$4,$16
-$L128:
+$L125:
 	move	$5,$17
 	jal	_rtl8651_asicTableAccessForward
 	move	$6,$18
@@ -1001,37 +985,37 @@ $L128:
 	li	$3,-1149239296			# 0xffffffffbb800000
 	ori	$3,$3,0x4d00
 	sw	$2,0($3)
-$L120:
+$L117:
 	lw	$2,0($3)
 	nop
 	andi	$2,$2,0x1
-	bne	$2,$0,$L120
+	bne	$2,$0,$L117
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,2			# 0x2
-	bne	$3,$2,$L129
+	bne	$3,$2,$L126
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
 	nop
-	bgtz	$2,$L130
+	bgtz	$2,$L127
 	li	$4,-1149239296			# 0xffffffffbb800000
 
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
-$L129:
+$L126:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,3			# 0x3
-	beq	$3,$2,$L123
+	beq	$3,$2,$L120
 	li	$2,4			# 0x4
 
-	bne	$3,$2,$L112
+	bne	$3,$2,$L109
 	move	$3,$0
 
-$L123:
+$L120:
 	li	$4,-1149239296			# 0xffffffffbb800000
-$L130:
+$L127:
 	ori	$4,$4,0x4418
 	lw	$3,0($4)
 	li	$2,-327680			# 0xfffffffffffb0000
@@ -1039,7 +1023,7 @@ $L130:
 	and	$3,$3,$2
 	sw	$3,0($4)
 	move	$3,$0
-$L112:
+$L109:
 	move	$2,$3
 	lw	$31,36($sp)
 	lw	$18,32($sp)
@@ -1058,168 +1042,152 @@ $L112:
 	.type	_rtl8651_readAsicEntry, @function
 _rtl8651_readAsicEntry:
 	.set	nomips16
-	.frame	$sp,112,$31		# vars= 72, regs= 6/0, args= 16, gp= 0
-	.mask	0x801f0000,-4
+	.frame	$sp,112,$31		# vars= 72, regs= 5/0, args= 16, gp= 0
+	.mask	0x800f0000,-8
 	.fmask	0x00000000,0
 	.set	noreorder
 	.set	nomacro
 	
 	addiu	$sp,$sp,-112
-	sw	$31,108($sp)
-	sw	$20,104($sp)
+	sw	$31,104($sp)
 	sw	$19,100($sp)
 	sw	$18,96($sp)
 	sw	$17,92($sp)
 	sw	$16,88($sp)
-	move	$18,$4
-	move	$19,$5
-	move	$20,$6
-	li	$16,2			# 0x2
-	li	$17,10			# 0xa
+	move	$17,$4
+	move	$18,$5
+	move	$19,$6
+	li	$16,10			# 0xa
 	jal	rtl865x_accessAsicTable
 	addiu	$5,$sp,80
 
 	lw	$2,80($sp)
 	nop
-	beq	$2,$0,$L131
+	beq	$2,$0,$L128
 	li	$3,-1			# 0xffffffffffffffff
 
-	bne	$20,$0,$L133
-	sll	$10,$18,16
+	bne	$19,$0,$L130
+	sll	$6,$17,16
 
 	lui	$4,%hi($LC1)
 	addiu	$4,$4,%lo($LC1)
 	lui	$5,%hi($LC2)
 	addiu	$5,$5,%lo($LC2)
 	jal	panic_printk
-	li	$6,444			# 0x1bc
+	li	$6,462			# 0x1ce
 
-$L134:
-	j	$L134
+$L131:
+	j	$L131
 	nop
 
-$L133:
-	sll	$2,$19,5
-	addu	$10,$10,$2
+$L130:
+	sll	$2,$18,5
+	addu	$6,$6,$2
 	li	$2,-1157627904			# 0xffffffffbb000000
-	addu	$10,$10,$2
+	addu	$6,$6,$2
 	li	$2,-1149239296			# 0xffffffffbb800000
 	ori	$3,$2,0x4d00
-$L136:
+$L133:
 	lw	$2,0($3)
 	nop
 	andi	$2,$2,0x1
-	bne	$2,$0,$L136
-	addiu	$13,$sp,16
+	bne	$2,$0,$L133
+	addiu	$10,$sp,16
 
-$L138:
-	move	$9,$0
-$L145:
-	beq	$16,$9,$L143
-	sll	$3,$9,5
-
-	addu	$3,$3,$13
-	lw	$2,0($10)
+	move	$12,$10
+	li	$11,1			# 0x1
+$L135:
+	move	$8,$0
+	sll	$3,$8,5
+$L163:
+	addu	$3,$3,$10
+	lw	$2,0($6)
 	nop
 	sw	$2,0($3)
-	lw	$2,4($10)
+	lw	$2,4($6)
 	nop
 	sw	$2,4($3)
-	lw	$2,8($10)
+	lw	$2,8($6)
 	nop
 	sw	$2,8($3)
-	lw	$2,12($10)
+	lw	$2,12($6)
 	nop
 	sw	$2,12($3)
-	lw	$2,16($10)
+	lw	$2,16($6)
 	nop
 	sw	$2,16($3)
-	lw	$2,20($10)
+	lw	$2,20($6)
 	nop
 	sw	$2,20($3)
-	lw	$2,24($10)
+	lw	$2,24($6)
 	nop
 	sw	$2,24($3)
-	lw	$2,28($10)
+	lw	$2,28($6)
 	nop
 	sw	$2,28($3)
-	move	$16,$9
-$L143:
-	addiu	$9,$9,1
-	sltu	$2,$9,2
-	bne	$2,$0,$L145
-	move	$11,$0
+	addiu	$8,$8,1
+	sltu	$2,$8,2
+	bne	$2,$0,$L163
+	sll	$3,$8,5
 
 	move	$9,$0
-	addiu	$12,$sp,16
-	sltu	$2,$9,2
-$L174:
-	beq	$2,$0,$L168
-	move	$8,$9
-
-	sll	$7,$9,3
+	li	$8,1			# 0x1
 	move	$5,$12
-$L158:
+$L151:
 	move	$4,$0
-	sll	$6,$8,3
+	sll	$7,$8,3
 	addu	$3,$7,$4
-$L173:
+$L164:
 	sll	$3,$3,2
 	addu	$3,$3,$5
-	addu	$2,$6,$4
-	sll	$2,$2,2
+	sll	$2,$4,2
 	addu	$2,$2,$5
 	lw	$3,0($3)
 	lw	$2,0($2)
 	nop
-	beq	$3,$2,$L154
+	beq	$3,$2,$L147
 	addiu	$4,$4,1
 
 	addiu	$4,$4,-1
-	j	$L156
-	li	$11,1			# 0x1
+	j	$L149
+	li	$9,1			# 0x1
 
-$L154:
+$L147:
 	slt	$2,$4,8
-	bne	$2,$0,$L173
+	bne	$2,$0,$L164
 	addu	$3,$7,$4
 
 	addiu	$8,$8,1
 	sltu	$2,$8,2
-	bne	$2,$0,$L158
-	nop
-
-$L168:
-	addiu	$9,$9,1
-	sltu	$2,$9,2
-	bne	$2,$0,$L174
+	bne	$2,$0,$L151
 	nop
 
-$L156:
-	bne	$11,$0,$L139
-	addiu	$17,$17,-1
+$L149:
+	bne	$9,$11,$L136
+	addiu	$16,$16,-1
 
-	bne	$17,$0,$L138
+	bne	$16,$0,$L135
 	nop
 
-$L139:
-	sll	$3,$16,5
-	addiu	$2,$sp,16
-	addu	$10,$2,$3
-	sll	$3,$18,2
+$L136:
+	li	$2,1			# 0x1
+	sll	$2,$2,5
+	addiu	$3,$sp,16
+	addu	$6,$3,$2
+	sll	$3,$17,2
 	lui	$2,%hi(_rtl8651_asicTableSize)
 	addiu	$2,$2,%lo(_rtl8651_asicTableSize)
 	addu	$3,$3,$2
 	lw	$2,0($3)
 	nop
-	beq	$2,$0,$L172
+	beq	$2,$0,$L162
 	move	$5,$0
 
 	move	$4,$3
 	sll	$2,$5,2
-$L175:
-	addu	$3,$2,$20
-	addu	$2,$2,$10
+$L165:
+	addu	$3,$2,$19
+	addu	$2,$2,$6
 	lw	$2,0($2)
 	nop
 	sw	$2,0($3)
@@ -1227,15 +1195,14 @@ $L175:
 	lw	$2,0($4)
 	nop
 	sltu	$2,$5,$2
-	bne	$2,$0,$L175
+	bne	$2,$0,$L165
 	sll	$2,$5,2
 
-$L172:
+$L162:
 	move	$3,$0
-$L131:
+$L128:
 	move	$2,$3
-	lw	$31,108($sp)
-	lw	$20,104($sp)
+	lw	$31,104($sp)
 	lw	$19,100($sp)
 	lw	$18,96($sp)
 	lw	$17,92($sp)
@@ -1272,10 +1239,10 @@ _rtl8651_readAsicEntryStopTLU:
 
 	lw	$2,16($sp)
 	nop
-	beq	$2,$0,$L176
+	beq	$2,$0,$L166
 	li	$3,-1			# 0xffffffffffffffff
 
-	bne	$18,$0,$L178
+	bne	$18,$0,$L168
 	sll	$6,$16,16
 
 	lui	$4,%hi($LC1)
@@ -1283,13 +1250,13 @@ _rtl8651_readAsicEntryStopTLU:
 	lui	$5,%hi($LC2)
 	addiu	$5,$5,%lo($LC2)
 	jal	panic_printk
-	li	$6,550			# 0x226
+	li	$6,557			# 0x22d
 
-$L179:
-	j	$L179
+$L169:
+	j	$L169
 	nop
 
-$L178:
+$L168:
 	sll	$2,$17,5
 	addu	$6,$6,$2
 	li	$2,-1157627904			# 0xffffffffbb000000
@@ -1297,47 +1264,47 @@ $L178:
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,2			# 0x2
-	bne	$3,$2,$L193
+	bne	$3,$2,$L183
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
 	nop
-	bgtz	$2,$L194
+	bgtz	$2,$L184
 	li	$2,-1149239296			# 0xffffffffbb800000
 
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
-$L193:
+$L183:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,3			# 0x3
-	beq	$3,$2,$L182
+	beq	$3,$2,$L172
 	li	$2,4			# 0x4
 
-	bne	$3,$2,$L195
+	bne	$3,$2,$L185
 	sll	$2,$16,2
 
-$L182:
+$L172:
 	li	$2,-1149239296			# 0xffffffffbb800000
-$L194:
+$L184:
 	ori	$2,$2,0x4418
 	lw	$3,0($2)
 	li	$4,262144			# 0x40000
 	or	$3,$3,$4
 	sw	$3,0($2)
 	sll	$2,$16,2
-$L195:
+$L185:
 	move	$4,$2
 	lui	$3,%hi(_rtl8651_asicTableSize)
 	addiu	$3,$3,%lo(_rtl8651_asicTableSize)
 	addu	$2,$2,$3
 	lw	$2,0($2)
 	nop
-	beq	$2,$0,$L192
+	beq	$2,$0,$L182
 	move	$5,$0
 
 	addu	$4,$4,$3
 	sll	$2,$5,2
-$L196:
+$L186:
 	addu	$3,$2,$18
 	addu	$2,$2,$6
 	lw	$2,0($2)
@@ -1347,32 +1314,32 @@ $L196:
 	lw	$2,0($4)
 	nop
 	sltu	$2,$5,$2
-	bne	$2,$0,$L196
+	bne	$2,$0,$L186
 	sll	$2,$5,2
 
-$L192:
+$L182:
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,2			# 0x2
-	bne	$3,$2,$L197
+	bne	$3,$2,$L187
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
 	nop
-	bgtz	$2,$L189
+	bgtz	$2,$L179
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
-$L197:
+$L187:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,3			# 0x3
-	beq	$3,$2,$L189
+	beq	$3,$2,$L179
 	li	$2,4			# 0x4
 
-	bne	$3,$2,$L176
+	bne	$3,$2,$L166
 	move	$3,$0
 
-$L189:
+$L179:
 	li	$4,-1149239296			# 0xffffffffbb800000
 	ori	$4,$4,0x4418
 	lw	$3,0($4)
@@ -1381,7 +1348,7 @@ $L189:
 	and	$3,$3,$2
 	sw	$3,0($4)
 	move	$3,$0
-$L176:
+$L166:
 	move	$2,$3
 	lw	$31,36($sp)
 	lw	$18,32($sp)
@@ -1408,11 +1375,11 @@ _rtl8651_delAsicEntry:
 	
 	li	$2,-1149239296			# 0xffffffffbb800000
 	ori	$3,$2,0x4d00
-$L199:
+$L189:
 	lw	$2,0($3)
 	nop
 	andi	$2,$2,0x1
-	bne	$2,$0,$L199
+	bne	$2,$0,$L189
 	sll	$2,$4,2
 
 	move	$7,$2
@@ -1421,7 +1388,7 @@ $L199:
 	addu	$2,$2,$3
 	lw	$2,0($2)
 	nop
-	beq	$2,$0,$L212
+	beq	$2,$0,$L202
 	move	$3,$0
 
 	li	$8,-1149239296			# 0xffffffffbb800000
@@ -1429,19 +1396,19 @@ $L199:
 	addiu	$2,$2,%lo(_rtl8651_asicTableSize)
 	addu	$7,$7,$2
 	sll	$2,$3,2
-$L215:
+$L205:
 	addu	$2,$2,$8
 	sw	$0,19744($2)
 	addiu	$3,$3,1
 	lw	$2,0($7)
 	nop
 	sltu	$2,$3,$2
-	bne	$2,$0,$L215
+	bne	$2,$0,$L205
 	sll	$2,$3,2
 
-$L212:
+$L202:
 	sltu	$2,$6,$5
-	bne	$2,$0,$L214
+	bne	$2,$0,$L204
 	nop
 
 	li	$2,-1149239296			# 0xffffffffbb800000
@@ -1451,41 +1418,187 @@ $L212:
 	ori	$3,$2,0x4d00
 	li	$8,9			# 0x9
 	ori	$7,$2,0x4d04
-$L210:
+$L200:
 	sll	$2,$5,5
 	addu	$2,$4,$2
 	addu	$2,$2,$9
 	sw	$2,0($10)
 	sw	$8,0($3)
-$L207:
+$L197:
 	lw	$2,0($3)
 	nop
 	andi	$2,$2,0x1
-	bne	$2,$0,$L207
+	bne	$2,$0,$L197
 	nop
 
 	lw	$2,0($7)
 	nop
 	andi	$2,$2,0x1
-	beq	$2,$0,$L209
+	beq	$2,$0,$L199
 	addiu	$5,$5,1
 
 	addiu	$5,$5,-1
 	j	$31
 	li	$2,-1			# 0xffffffffffffffff
 
-$L209:
+$L199:
 	sltu	$2,$6,$5
-	beq	$2,$0,$L210
+	beq	$2,$0,$L200
 	nop
 
-$L214:
+$L204:
 	j	$31
 	move	$2,$0
 
 	.set	macro
 	.set	reorder
 	.end	_rtl8651_delAsicEntry
+	.data
+	.align	2
+	.type	rtl819x_lastTxDesc, @object
+	.size	rtl819x_lastTxDesc, 4
+rtl819x_lastTxDesc:
+	.word	0
+	.align	2
+	.type	rtl819x_lastRxDesc, @object
+	.size	rtl819x_lastRxDesc, 4
+rtl819x_lastRxDesc:
+	.word	0
+	.align	2
+	.type	rtl819x_swHangCnt, @object
+	.size	rtl819x_swHangCnt, 4
+rtl819x_swHangCnt:
+	.word	0
+	.align	2
+	.type	rtl819x_checkSwCoreTimer, @object
+	.size	rtl819x_checkSwCoreTimer, 4
+rtl819x_checkSwCoreTimer:
+	.word	0
+	.rdata
+	.align	2
+$LC3:
+	.ascii	".........................................\n\000"
+	.section	.text.rtl819x_poll_sw,"ax",@progbits
+	.align	2
+	.globl	rtl819x_poll_sw
+	.ent	rtl819x_poll_sw
+	.type	rtl819x_poll_sw, @function
+rtl819x_poll_sw:
+	.set	nomips16
+	.frame	$sp,24,$31		# vars= 0, regs= 1/0, args= 16, gp= 0
+	.mask	0x80000000,-8
+	.fmask	0x00000000,0
+	.set	noreorder
+	.set	nomacro
+	
+	addiu	$sp,$sp,-24
+	sw	$31,16($sp)
+	li	$2,-1149239296			# 0xffffffffbb800000
+	ori	$2,$2,0x610c
+	lw	$3,0($2)
+	li	$2,65536			# 0x10000
+	and	$8,$3,$2
+	li	$3,-1207894016			# 0xffffffffb8010000
+	ori	$2,$3,0x4
+	lw	$2,0($2)
+	li	$4,-4			# 0xfffffffffffffffc
+	and	$6,$2,$4
+	ori	$3,$3,0x20
+	lw	$2,0($3)
+	nop
+	and	$7,$2,$4
+	lui	$5,%hi(rtl819x_checkSwCoreTimer)
+	lw	$4,%lo(rtl819x_checkSwCoreTimer)($5)
+	li	$2,-859045888			# 0xffffffffcccc0000
+	ori	$2,$2,0xcccd
+	multu	$4,$2
+	mfhi	$2
+	srl	$2,$2,4
+	sll	$3,$2,2
+	addu	$3,$3,$2
+	sll	$3,$3,2
+	subu	$3,$4,$3
+	addiu	$4,$4,1
+	bne	$3,$0,$L206
+	sw	$4,%lo(rtl819x_checkSwCoreTimer)($5)
+
+	bne	$8,$0,$L208
+	lui	$2,%hi(rtl819x_lastTxDesc)
+
+	lui	$2,%hi(rtl819x_swHangCnt)
+	j	$L209
+	sw	$0,%lo(rtl819x_swHangCnt)($2)
+
+$L208:
+	lw	$2,%lo(rtl819x_lastTxDesc)($2)
+	nop
+	beq	$2,$0,$L216
+	lui	$2,%hi(rtl819x_lastRxDesc)
+
+	lw	$2,%lo(rtl819x_lastRxDesc)($2)
+	nop
+	bne	$2,$0,$L210
+	lui	$2,%hi(rtl819x_lastRxDesc)
+
+$L216:
+	sw	$6,%lo(rtl819x_lastRxDesc)($2)
+	lui	$2,%hi(rtl819x_lastTxDesc)
+	sw	$7,%lo(rtl819x_lastTxDesc)($2)
+	lui	$2,%hi(rtl819x_swHangCnt)
+	j	$L209
+	sw	$0,%lo(rtl819x_swHangCnt)($2)
+
+$L210:
+	lw	$2,%lo(rtl819x_lastRxDesc)($2)
+	nop
+	bne	$2,$6,$L217
+	lui	$2,%hi(rtl819x_swHangCnt)
+
+	lui	$2,%hi(rtl819x_lastTxDesc)
+	lw	$2,%lo(rtl819x_lastTxDesc)($2)
+	nop
+	bne	$2,$7,$L213
+	lui	$2,%hi(rtl819x_swHangCnt)
+
+	lui	$3,%hi(rtl819x_swHangCnt)
+	lw	$2,%lo(rtl819x_swHangCnt)($3)
+	nop
+	addiu	$2,$2,1
+	j	$L209
+	sw	$2,%lo(rtl819x_swHangCnt)($3)
+
+$L213:
+$L217:
+	sw	$0,%lo(rtl819x_swHangCnt)($2)
+$L209:
+	lui	$2,%hi(rtl819x_lastRxDesc)
+	sw	$6,%lo(rtl819x_lastRxDesc)($2)
+	lui	$2,%hi(rtl819x_lastTxDesc)
+	sw	$7,%lo(rtl819x_lastTxDesc)($2)
+	lui	$3,%hi(rtl819x_swHangCnt)
+	lw	$2,%lo(rtl819x_swHangCnt)($3)
+	nop
+	sltu	$2,$2,3
+	bne	$2,$0,$L206
+	nop
+
+	sw	$0,%lo(rtl819x_swHangCnt)($3)
+	lui	$4,%hi($LC3)
+	jal	panic_printk
+	addiu	$4,$4,%lo($LC3)
+
+	jal	machine_restart
+	move	$4,$0
+
+$L206:
+	lw	$31,16($sp)
+	nop
+	j	$31
+	addiu	$sp,$sp,24
+
+	.set	macro
+	.set	reorder
+	.end	rtl819x_poll_sw
 	.globl	RtkHomeGatewayChipName
 	.section	.bss
 	.align	2
diff --git a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/98/rtl865x_asicBasic.S b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/98/rtl865x_asicBasic.S
index f846701f8..d2305e681 100644
--- a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/98/rtl865x_asicBasic.S
+++ b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/98/rtl865x_asicBasic.S
@@ -367,30 +367,28 @@ $LC0:
 	.type	bsp_swcore_init, @function
 bsp_swcore_init:
 	.set	nomips16
-	.frame	$sp,48,$31		# vars= 0, regs= 8/0, args= 16, gp= 0
-	.mask	0x807f0000,-4
+	.frame	$sp,40,$31		# vars= 0, regs= 6/0, args= 16, gp= 0
+	.mask	0x801f0000,-4
 	.fmask	0x00000000,0
 	.set	noreorder
 	.set	nomacro
 	
-	addiu	$sp,$sp,-48
-	sw	$31,44($sp)
-	sw	$22,40($sp)
-	sw	$21,36($sp)
+	addiu	$sp,$sp,-40
+	sw	$31,36($sp)
 	sw	$20,32($sp)
 	sw	$19,28($sp)
 	sw	$18,24($sp)
 	sw	$17,20($sp)
 	sw	$16,16($sp)
-	move	$22,$4
-	move	$20,$0
-	li	$16,-1207959552			# 0xb8000000
-	lw	$2,0($16)
-	li	$17,-65536			# 0xffff0000
-	and	$18,$2,$17
-	ori	$21,$16,0xc
-	lw	$2,0($21)
-	andi	$19,$2,0xf
+	move	$20,$4
+	move	$19,$0
+	li	$2,-1207959552			# 0xb8000000
+	lw	$4,0($2)
+	li	$3,-65536			# 0xffff0000
+	and	$16,$4,$3
+	ori	$2,$2,0xc
+	lw	$17,0($2)
+	andi	$18,$17,0xf
 	lui	$4,%hi(fun_enable)
 	jal	rtl865x_initAsicFun
 	addiu	$4,$4,%lo(fun_enable)
@@ -399,115 +397,103 @@ bsp_swcore_init:
 	beq	$2,$3,$L36
 	li	$4,-1			# 0xffffffff
 
-	lw	$2,0($16)
-	and	$2,$2,$17
-	li	$3,-1073741824			# 0xc0000000
-	bne	$2,$3,$L52
-	li	$2,-1073741824			# 0xc0000000
-
-	lw	$2,0($21)
-	andi	$2,$2,0x8
-	bne	$2,$0,$L52
 	li	$2,-1073741824			# 0xc0000000
-
-	addiu	$2,$22,-8
-	sltu	$2,$2,2
-	bne	$2,$0,$L36
-	move	$4,$0
-
-	li	$2,-1073741824			# 0xc0000000
-$L52:
-	bne	$18,$2,$L41
+	bne	$16,$2,$L39
 	li	$2,-2147483648			# 0x80000000
 
 	li	$2,9			# 0x9
-	bne	$22,$2,$L44
-	move	$20,$0
+	bne	$20,$2,$L40
+	nop
+
+	beq	$18,$20,$L40
+	andi	$2,$17,0x8
 
-	beq	$19,$22,$L44
+	beq	$2,$0,$L40
 	lui	$4,%hi($LC0)
 
 	addiu	$4,$4,%lo($LC0)
 	jal	early_console_write
 	li	$5,27			# 0x1b
 
-	j	$L44
-	li	$20,-1			# 0xffffffff
+	j	$L42
+	li	$19,-1			# 0xffffffff
+
+$L40:
+	j	$L42
+	move	$19,$0
 
-$L41:
-	bne	$18,$2,$L36
-	move	$4,$20
+$L39:
+	bne	$16,$2,$L36
+	move	$4,$19
 
 	li	$2,7			# 0x7
-	bne	$22,$2,$L53
+	bne	$20,$2,$L50
 	li	$2,15			# 0xf
 
-	beq	$19,$22,$L53
+	beq	$18,$20,$L50
 	lui	$4,%hi($LC0)
 
 	addiu	$4,$4,%lo($LC0)
 	jal	early_console_write
 	li	$5,27			# 0x1b
 
-	j	$L44
-	li	$20,-1			# 0xffffffff
+	j	$L42
+	li	$19,-1			# 0xffffffff
 
-$L53:
-	bne	$22,$2,$L54
+$L50:
+	bne	$20,$2,$L51
 	li	$2,3			# 0x3
 
-	beq	$19,$22,$L48
+	beq	$18,$20,$L46
 	li	$2,7			# 0x7
 
-	beq	$19,$2,$L48
+	beq	$18,$2,$L46
 	li	$2,3			# 0x3
 
-	beq	$19,$2,$L48
+	beq	$18,$2,$L46
 	li	$2,11			# 0xb
 
-	beq	$19,$2,$L48
+	beq	$18,$2,$L46
 	lui	$4,%hi($LC0)
 
 	addiu	$4,$4,%lo($LC0)
 	jal	early_console_write
 	li	$5,27			# 0x1b
 
-	j	$L44
-	li	$20,-1			# 0xffffffff
+	j	$L42
+	li	$19,-1			# 0xffffffff
 
-$L48:
+$L46:
 	li	$2,3			# 0x3
-$L54:
-	bne	$22,$2,$L44
-	move	$20,$0
+$L51:
+	bne	$20,$2,$L42
+	move	$19,$0
 
-	beq	$19,$22,$L44
+	beq	$18,$20,$L42
 	li	$2,7			# 0x7
 
-	beq	$19,$2,$L55
+	beq	$18,$2,$L52
 	lui	$4,%hi($LC0)
 
 	addiu	$4,$4,%lo($LC0)
 	jal	early_console_write
 	li	$5,27			# 0x1b
 
-	li	$20,-1			# 0xffffffff
-$L44:
-$L55:
-	move	$4,$20
+	li	$19,-1			# 0xffffffff
+$L42:
+$L52:
+	move	$4,$19
 $L38:
 $L36:
 	move	$2,$4
-	lw	$31,44($sp)
-	lw	$22,40($sp)
-	lw	$21,36($sp)
+	lw	$31,36($sp)
 	lw	$20,32($sp)
 	lw	$19,28($sp)
 	lw	$18,24($sp)
 	lw	$17,20($sp)
 	lw	$16,16($sp)
 	j	$31
-	addiu	$sp,$sp,48
+	addiu	$sp,$sp,40
 
 	.set	macro
 	.set	reorder
@@ -535,57 +521,57 @@ rtl865x_accessAsicTable:
 	addiu	$4,$sp,16
 
 	sltu	$2,$16,15
-	beq	$2,$0,$L72
+	beq	$2,$0,$L69
 	li	$2,1			# 0x1
 
 	li	$3,1			# 0x1
 	sll	$4,$3,$16
 	andi	$2,$4,0xe22
-	bne	$2,$0,$L65
+	bne	$2,$0,$L62
 	andi	$2,$4,0x8
 
-	bne	$2,$0,$L68
+	bne	$2,$0,$L65
 	andi	$2,$4,0x4000
 
-	beq	$2,$0,$L71
+	beq	$2,$0,$L68
 	li	$2,1			# 0x1
 
 	lw	$2,16($sp)
 	andi	$2,$2,0x20
 	movz	$3,$0,$2		#conditional move
-	j	$L57
+	j	$L54
 	sw	$3,0($17)
 
-$L65:
+$L62:
 	lw	$2,16($sp)
 	andi	$2,$2,0x4
-	beq	$2,$0,$L66
+	beq	$2,$0,$L63
 	li	$2,1			# 0x1
 
-	j	$L57
+	j	$L54
 	sw	$2,0($17)
 
-$L66:
-	j	$L57
+$L63:
+	j	$L54
 	sw	$0,0($17)
 
-$L68:
+$L65:
 	lw	$2,16($sp)
 	andi	$2,$2,0x2
-	beq	$2,$0,$L69
+	beq	$2,$0,$L66
 	li	$2,1			# 0x1
 
-	j	$L57
+	j	$L54
 	sw	$2,0($17)
 
-$L69:
-	j	$L57
+$L66:
+	j	$L54
 	sw	$0,0($17)
 
-$L71:
-$L72:
+$L68:
+$L69:
 	sw	$2,0($17)
-$L57:
+$L54:
 	move	$2,$0
 	lw	$31,32($sp)
 	lw	$17,28($sp)
@@ -596,31 +582,49 @@ $L57:
 	.set	macro
 	.set	reorder
 	.end	rtl865x_accessAsicTable
+	.rdata
+	.align	2
+$LC1:
+	.ascii	"\n"
+	.ascii	"Assert Fail: %s %d\000"
+	.align	2
+$LC2:
+	.ascii	"drivers/net/rtl819x/rtl865x/../AsicDriver/rtl865x_asicBa"
+	.ascii	"sic.c\000"
 	.section	.text._rtl8651_asicTableAccessForward,"ax",@progbits
 	.align	2
 	.ent	_rtl8651_asicTableAccessForward
 	.type	_rtl8651_asicTableAccessForward, @function
 _rtl8651_asicTableAccessForward:
 	.set	nomips16
-	.frame	$sp,0,$31		# vars= 0, regs= 0/0, args= 0, gp= 0
-	.mask	0x00000000,0
+	.frame	$sp,24,$31		# vars= 0, regs= 1/0, args= 16, gp= 0
+	.mask	0x80000000,-8
 	.fmask	0x00000000,0
 	.set	noreorder
 	.set	nomacro
 	
-	bne	$6,$0,$L74
-	li	$2,-1149239296			# 0xbb800000
+	addiu	$sp,$sp,-24
+	bne	$6,$0,$L71
+	sw	$31,16($sp)
 
-$L76:
-	j	$L76
+	lui	$4,%hi($LC1)
+	addiu	$4,$4,%lo($LC1)
+	lui	$5,%hi($LC2)
+	addiu	$5,$5,%lo($LC2)
+	jal	panic_printk
+	li	$6,324			# 0x144
+
+$L72:
+	j	$L72
 	nop
 
-$L74:
+$L71:
+	li	$2,-1149239296			# 0xbb800000
 	ori	$3,$2,0x4d00
-$L78:
+$L74:
 	lw	$2,0($3)
 	andi	$2,$2,0x1
-	bne	$2,$0,$L78
+	bne	$2,$0,$L74
 	sll	$2,$4,2
 
 	move	$8,$2
@@ -628,13 +632,13 @@ $L78:
 	addiu	$3,$3,%lo(_rtl8651_asicTableSize)
 	addu	$2,$2,$3
 	lw	$2,0($2)
-	beq	$2,$0,$L85
+	beq	$2,$0,$L81
 	move	$7,$0
 
 	li	$9,-1149239296			# 0xbb800000
 	addu	$8,$8,$3
 	sll	$2,$7,2
-$L86:
+$L82:
 	addu	$3,$2,$9
 	addu	$2,$2,$6
 	lw	$2,0($2)
@@ -642,10 +646,10 @@ $L86:
 	addiu	$7,$7,1
 	lw	$2,0($8)
 	sltu	$2,$7,$2
-	bne	$2,$0,$L86
+	bne	$2,$0,$L82
 	sll	$2,$7,2
 
-$L85:
+$L81:
 	sll	$3,$4,16
 	sll	$2,$5,5
 	addu	$3,$3,$2
@@ -654,8 +658,9 @@ $L85:
 	li	$2,-1149239296			# 0xbb800000
 	ori	$2,$2,0x4d08
 	sw	$3,0($2)
+	lw	$31,16($sp)
 	j	$31
-	nop
+	addiu	$sp,$sp,24
 
 	.set	macro
 	.set	reorder
@@ -685,7 +690,7 @@ _rtl8651_addAsicEntry:
 	addiu	$5,$sp,16
 
 	lw	$2,16($sp)
-	beq	$2,$0,$L87
+	beq	$2,$0,$L83
 	li	$3,-1			# 0xffffffff
 
 	move	$4,$16
@@ -696,27 +701,27 @@ _rtl8651_addAsicEntry:
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,2			# 0x2
-	bne	$3,$2,$L103
+	bne	$3,$2,$L99
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
-	bgtz	$2,$L104
+	bgtz	$2,$L100
 	li	$3,-1149239296			# 0xbb800000
 
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
-$L103:
+$L99:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,3			# 0x3
-	beq	$3,$2,$L90
+	beq	$3,$2,$L86
 	li	$2,4			# 0x4
 
-	bne	$3,$2,$L105
+	bne	$3,$2,$L101
 	li	$2,3			# 0x3
 
-$L90:
+$L86:
 	li	$3,-1149239296			# 0xbb800000
-$L104:
+$L100:
 	ori	$3,$3,0x4418
 	lw	$2,0($3)
 	li	$4,262144			# 0x40000
@@ -724,48 +729,48 @@ $L104:
 	sw	$2,0($3)
 	move	$4,$3
 	li	$3,524288			# 0x80000
-$L92:
+$L88:
 	lw	$2,0($4)
 	and	$2,$2,$3
-	beq	$2,$0,$L92
+	beq	$2,$0,$L88
 	li	$2,3			# 0x3
 
-$L105:
+$L101:
 	li	$3,-1149239296			# 0xbb800000
 	ori	$3,$3,0x4d00
 	sw	$2,0($3)
-$L94:
+$L90:
 	lw	$2,0($3)
 	andi	$2,$2,0x1
-	bne	$2,$0,$L94
+	bne	$2,$0,$L90
 	li	$2,-1149239296			# 0xbb800000
 
 	ori	$2,$2,0x4d04
 	lw	$2,0($2)
 	andi	$2,$2,0x1
-	beq	$2,$0,$L96
+	beq	$2,$0,$L92
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,2			# 0x2
-	bne	$3,$2,$L106
+	bne	$3,$2,$L102
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
-	bgtz	$2,$L98
+	bgtz	$2,$L94
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
-$L106:
+$L102:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,3			# 0x3
-	beq	$3,$2,$L98
+	beq	$3,$2,$L94
 	li	$2,4			# 0x4
 
-	bne	$3,$2,$L97
+	bne	$3,$2,$L93
 	nop
 
-$L98:
+$L94:
 	li	$4,-1149239296			# 0xbb800000
 	ori	$4,$4,0x4418
 	lw	$3,0($4)
@@ -773,34 +778,34 @@ $L98:
 	ori	$2,$2,0xffff
 	and	$3,$3,$2
 	sw	$3,0($4)
-$L97:
-	j	$L87
+$L93:
+	j	$L83
 	li	$3,-1			# 0xffffffff
 
-$L96:
+$L92:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,2			# 0x2
-	bne	$3,$2,$L107
+	bne	$3,$2,$L103
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
-	bgtz	$2,$L108
+	bgtz	$2,$L104
 	li	$4,-1149239296			# 0xbb800000
 
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
-$L107:
+$L103:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,3			# 0x3
-	beq	$3,$2,$L101
+	beq	$3,$2,$L97
 	li	$2,4			# 0x4
 
-	bne	$3,$2,$L87
+	bne	$3,$2,$L83
 	move	$3,$0
 
-$L101:
+$L97:
 	li	$4,-1149239296			# 0xbb800000
-$L108:
+$L104:
 	ori	$4,$4,0x4418
 	lw	$3,0($4)
 	li	$2,-327680			# 0xfffb0000
@@ -808,7 +813,7 @@ $L108:
 	and	$3,$3,$2
 	sw	$3,0($4)
 	move	$3,$0
-$L87:
+$L83:
 	move	$2,$3
 	lw	$31,36($sp)
 	lw	$18,32($sp)
@@ -872,11 +877,11 @@ _rtl8651_forceAddAsicEntry:
 	addiu	$5,$sp,16
 
 	lw	$2,16($sp)
-	beq	$2,$0,$L110
+	beq	$2,$0,$L106
 	li	$3,-1			# 0xffffffff
 
 	li	$2,3			# 0x3
-	bne	$16,$2,$L123
+	bne	$16,$2,$L119
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$3,%hi(mcastForceAddOpCnt)
@@ -884,30 +889,30 @@ _rtl8651_forceAddAsicEntry:
 	addiu	$2,$2,1
 	sw	$2,%lo(mcastForceAddOpCnt)($3)
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
-$L123:
+$L119:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,2			# 0x2
-	bne	$3,$2,$L124
+	bne	$3,$2,$L120
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
-	bgtz	$2,$L125
+	bgtz	$2,$L121
 	li	$3,-1149239296			# 0xbb800000
 
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
-$L124:
+$L120:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,3			# 0x3
-	beq	$3,$2,$L114
+	beq	$3,$2,$L110
 	li	$2,4			# 0x4
 
-	bne	$3,$2,$L126
+	bne	$3,$2,$L122
 	move	$4,$16
 
-$L114:
+$L110:
 	li	$3,-1149239296			# 0xbb800000
-$L125:
+$L121:
 	ori	$3,$3,0x4418
 	lw	$2,0($3)
 	li	$4,262144			# 0x40000
@@ -915,14 +920,14 @@ $L125:
 	sw	$2,0($3)
 	move	$4,$3
 	li	$3,524288			# 0x80000
-$L116:
+$L112:
 	lw	$2,0($4)
 	and	$2,$2,$3
-	beq	$2,$0,$L116
+	beq	$2,$0,$L112
 	nop
 
 	move	$4,$16
-$L126:
+$L122:
 	move	$5,$17
 	jal	_rtl8651_asicTableAccessForward
 	move	$6,$18
@@ -931,35 +936,35 @@ $L126:
 	li	$3,-1149239296			# 0xbb800000
 	ori	$3,$3,0x4d00
 	sw	$2,0($3)
-$L118:
+$L114:
 	lw	$2,0($3)
 	andi	$2,$2,0x1
-	bne	$2,$0,$L118
+	bne	$2,$0,$L114
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,2			# 0x2
-	bne	$3,$2,$L127
+	bne	$3,$2,$L123
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
-	bgtz	$2,$L128
+	bgtz	$2,$L124
 	li	$4,-1149239296			# 0xbb800000
 
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
-$L127:
+$L123:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,3			# 0x3
-	beq	$3,$2,$L121
+	beq	$3,$2,$L117
 	li	$2,4			# 0x4
 
-	bne	$3,$2,$L110
+	bne	$3,$2,$L106
 	move	$3,$0
 
-$L121:
+$L117:
 	li	$4,-1149239296			# 0xbb800000
-$L128:
+$L124:
 	ori	$4,$4,0x4418
 	lw	$3,0($4)
 	li	$2,-327680			# 0xfffb0000
@@ -967,7 +972,7 @@ $L128:
 	and	$3,$3,$2
 	sw	$3,0($4)
 	move	$3,$0
-$L110:
+$L106:
 	move	$2,$3
 	lw	$31,36($sp)
 	lw	$18,32($sp)
@@ -986,15 +991,14 @@ $L110:
 	.type	_rtl8651_readAsicEntry, @function
 _rtl8651_readAsicEntry:
 	.set	nomips16
-	.frame	$sp,112,$31		# vars= 72, regs= 6/0, args= 16, gp= 0
-	.mask	0x801f0000,-4
+	.frame	$sp,112,$31		# vars= 72, regs= 5/0, args= 16, gp= 0
+	.mask	0x800f0000,-8
 	.fmask	0x00000000,0
 	.set	noreorder
 	.set	nomacro
 	
 	addiu	$sp,$sp,-112
-	sw	$31,108($sp)
-	sw	$20,104($sp)
+	sw	$31,104($sp)
 	sw	$19,100($sp)
 	sw	$18,96($sp)
 	sw	$17,92($sp)
@@ -1002,147 +1006,138 @@ _rtl8651_readAsicEntry:
 	move	$17,$4
 	move	$18,$5
 	move	$19,$6
-	li	$16,2			# 0x2
-	li	$20,10			# 0xa
+	li	$16,10			# 0xa
 	jal	rtl865x_accessAsicTable
 	addiu	$5,$sp,80
 
 	lw	$2,80($sp)
-	beq	$2,$0,$L129
+	beq	$2,$0,$L125
 	li	$3,-1			# 0xffffffff
 
-	bne	$19,$0,$L131
-	sll	$10,$17,16
+	bne	$19,$0,$L127
+	sll	$6,$17,16
 
-$L133:
-	j	$L133
+	lui	$4,%hi($LC1)
+	addiu	$4,$4,%lo($LC1)
+	lui	$5,%hi($LC2)
+	addiu	$5,$5,%lo($LC2)
+	jal	panic_printk
+	li	$6,462			# 0x1ce
+
+$L128:
+	j	$L128
 	nop
 
-$L131:
+$L127:
 	sll	$2,$18,5
-	addu	$10,$10,$2
+	addu	$6,$6,$2
 	li	$2,-1157627904			# 0xbb000000
-	addu	$10,$10,$2
+	addu	$6,$6,$2
 	li	$2,-1149239296			# 0xbb800000
 	ori	$3,$2,0x4d00
-$L135:
+$L130:
 	lw	$2,0($3)
 	andi	$2,$2,0x1
-	bne	$2,$0,$L135
-	addiu	$12,$sp,16
-
-$L137:
-	move	$9,$0
-$L144:
-	beq	$16,$9,$L142
-	sll	$3,$9,5
+	bne	$2,$0,$L130
+	addiu	$10,$sp,16
 
-	addu	$3,$3,$12
-	lw	$2,0($10)
+	move	$12,$10
+	li	$11,1			# 0x1
+$L132:
+	move	$8,$0
+	sll	$3,$8,5
+$L160:
+	addu	$3,$3,$10
+	lw	$2,0($6)
 	sw	$2,0($3)
-	lw	$2,4($10)
+	lw	$2,4($6)
 	sw	$2,4($3)
-	lw	$2,8($10)
+	lw	$2,8($6)
 	sw	$2,8($3)
-	lw	$2,12($10)
+	lw	$2,12($6)
 	sw	$2,12($3)
-	lw	$2,16($10)
+	lw	$2,16($6)
 	sw	$2,16($3)
-	lw	$2,20($10)
+	lw	$2,20($6)
 	sw	$2,20($3)
-	lw	$2,24($10)
+	lw	$2,24($6)
 	sw	$2,24($3)
-	lw	$2,28($10)
+	lw	$2,28($6)
 	sw	$2,28($3)
-	move	$16,$9
-$L142:
-	addiu	$9,$9,1
-	sltu	$2,$9,2
-	bne	$2,$0,$L144
-	move	$11,$0
+	addiu	$8,$8,1
+	sltu	$2,$8,2
+	bne	$2,$0,$L160
+	sll	$3,$8,5
 
 	move	$9,$0
-	addiu	$13,$sp,16
-	sltu	$2,$9,2
-$L173:
-	beq	$2,$0,$L167
-	move	$8,$9
-
-	sll	$7,$9,3
-	move	$5,$13
-$L157:
+	li	$8,1			# 0x1
+	move	$5,$12
+$L148:
 	move	$4,$0
-	sll	$6,$8,3
+	sll	$7,$8,3
 	addu	$3,$7,$4
-$L172:
+$L161:
 	sll	$3,$3,2
 	addu	$3,$3,$5
-	addu	$2,$6,$4
-	sll	$2,$2,2
+	sll	$2,$4,2
 	addu	$2,$2,$5
 	lw	$3,0($3)
 	lw	$2,0($2)
-	beq	$3,$2,$L153
+	beq	$3,$2,$L144
 	addiu	$4,$4,1
 
 	addiu	$4,$4,-1
-	j	$L155
-	li	$11,1			# 0x1
+	j	$L146
+	li	$9,1			# 0x1
 
-$L153:
+$L144:
 	slt	$2,$4,8
-	bne	$2,$0,$L172
+	bne	$2,$0,$L161
 	addu	$3,$7,$4
 
 	addiu	$8,$8,1
 	sltu	$2,$8,2
-	bne	$2,$0,$L157
-	nop
-
-$L167:
-	addiu	$9,$9,1
-	sltu	$2,$9,2
-	bne	$2,$0,$L173
+	bne	$2,$0,$L148
 	nop
 
-$L155:
-	bne	$11,$0,$L138
-	addiu	$20,$20,-1
+$L146:
+	bne	$9,$11,$L133
+	addiu	$16,$16,-1
 
-	bne	$20,$0,$L137
+	bne	$16,$0,$L132
 	nop
 
-$L138:
-	sll	$3,$16,5
-	addiu	$2,$sp,16
-	addu	$10,$2,$3
+$L133:
+	li	$2,1			# 0x1
+	sll	$2,$2,5
+	addiu	$3,$sp,16
+	addu	$6,$3,$2
 	sll	$3,$17,2
 	lui	$2,%hi(_rtl8651_asicTableSize)
 	addiu	$2,$2,%lo(_rtl8651_asicTableSize)
 	addu	$3,$3,$2
 	lw	$2,0($3)
-	beq	$2,$0,$L171
+	beq	$2,$0,$L159
 	move	$5,$0
 
 	move	$4,$3
 	sll	$2,$5,2
-$L174:
+$L162:
 	addu	$3,$2,$19
-	addu	$2,$2,$10
+	addu	$2,$2,$6
 	lw	$2,0($2)
 	sw	$2,0($3)
 	addiu	$5,$5,1
 	lw	$2,0($4)
 	sltu	$2,$5,$2
-	bne	$2,$0,$L174
+	bne	$2,$0,$L162
 	sll	$2,$5,2
 
-$L171:
+$L159:
 	move	$3,$0
-$L129:
+$L125:
 	move	$2,$3
-	lw	$31,108($sp)
-	lw	$20,104($sp)
+	lw	$31,104($sp)
 	lw	$19,100($sp)
 	lw	$18,96($sp)
 	lw	$17,92($sp)
@@ -1178,17 +1173,24 @@ _rtl8651_readAsicEntryStopTLU:
 	addiu	$5,$sp,16
 
 	lw	$2,16($sp)
-	beq	$2,$0,$L175
+	beq	$2,$0,$L163
 	li	$3,-1			# 0xffffffff
 
-	bne	$18,$0,$L177
+	bne	$18,$0,$L165
 	sll	$6,$16,16
 
-$L179:
-	j	$L179
+	lui	$4,%hi($LC1)
+	addiu	$4,$4,%lo($LC1)
+	lui	$5,%hi($LC2)
+	addiu	$5,$5,%lo($LC2)
+	jal	panic_printk
+	li	$6,557			# 0x22d
+
+$L166:
+	j	$L166
 	nop
 
-$L177:
+$L165:
 	sll	$2,$17,5
 	addu	$6,$6,$2
 	li	$2,-1157627904			# 0xbb000000
@@ -1196,45 +1198,45 @@ $L177:
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,2			# 0x2
-	bne	$3,$2,$L193
+	bne	$3,$2,$L180
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
-	bgtz	$2,$L194
+	bgtz	$2,$L181
 	li	$2,-1149239296			# 0xbb800000
 
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
-$L193:
+$L180:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,3			# 0x3
-	beq	$3,$2,$L182
+	beq	$3,$2,$L169
 	li	$2,4			# 0x4
 
-	bne	$3,$2,$L195
+	bne	$3,$2,$L182
 	sll	$2,$16,2
 
-$L182:
+$L169:
 	li	$2,-1149239296			# 0xbb800000
-$L194:
+$L181:
 	ori	$2,$2,0x4418
 	lw	$3,0($2)
 	li	$4,262144			# 0x40000
 	or	$3,$3,$4
 	sw	$3,0($2)
 	sll	$2,$16,2
-$L195:
+$L182:
 	move	$4,$2
 	lui	$3,%hi(_rtl8651_asicTableSize)
 	addiu	$3,$3,%lo(_rtl8651_asicTableSize)
 	addu	$2,$2,$3
 	lw	$2,0($2)
-	beq	$2,$0,$L192
+	beq	$2,$0,$L179
 	move	$5,$0
 
 	addu	$4,$4,$3
 	sll	$2,$5,2
-$L196:
+$L183:
 	addu	$3,$2,$18
 	addu	$2,$2,$6
 	lw	$2,0($2)
@@ -1242,31 +1244,31 @@ $L196:
 	addiu	$5,$5,1
 	lw	$2,0($4)
 	sltu	$2,$5,$2
-	bne	$2,$0,$L196
+	bne	$2,$0,$L183
 	sll	$2,$5,2
 
-$L192:
+$L179:
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,2			# 0x2
-	bne	$3,$2,$L197
+	bne	$3,$2,$L184
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
-	bgtz	$2,$L189
+	bgtz	$2,$L176
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
-$L197:
+$L184:
 	lw	$3,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,3			# 0x3
-	beq	$3,$2,$L189
+	beq	$3,$2,$L176
 	li	$2,4			# 0x4
 
-	bne	$3,$2,$L175
+	bne	$3,$2,$L163
 	move	$3,$0
 
-$L189:
+$L176:
 	li	$4,-1149239296			# 0xbb800000
 	ori	$4,$4,0x4418
 	lw	$3,0($4)
@@ -1275,7 +1277,7 @@ $L189:
 	and	$3,$3,$2
 	sw	$3,0($4)
 	move	$3,$0
-$L175:
+$L163:
 	move	$2,$3
 	lw	$31,36($sp)
 	lw	$18,32($sp)
@@ -1302,10 +1304,10 @@ _rtl8651_delAsicEntry:
 	
 	li	$2,-1149239296			# 0xbb800000
 	ori	$3,$2,0x4d00
-$L199:
+$L186:
 	lw	$2,0($3)
 	andi	$2,$2,0x1
-	bne	$2,$0,$L199
+	bne	$2,$0,$L186
 	sll	$2,$4,2
 
 	move	$7,$2
@@ -1313,7 +1315,7 @@ $L199:
 	addiu	$3,$3,%lo(_rtl8651_asicTableSize)
 	addu	$2,$2,$3
 	lw	$2,0($2)
-	beq	$2,$0,$L212
+	beq	$2,$0,$L199
 	move	$3,$0
 
 	li	$8,-1149239296			# 0xbb800000
@@ -1321,18 +1323,18 @@ $L199:
 	addiu	$2,$2,%lo(_rtl8651_asicTableSize)
 	addu	$7,$7,$2
 	sll	$2,$3,2
-$L215:
+$L202:
 	addu	$2,$2,$8
 	sw	$0,19744($2)
 	addiu	$3,$3,1
 	lw	$2,0($7)
 	sltu	$2,$3,$2
-	bne	$2,$0,$L215
+	bne	$2,$0,$L202
 	sll	$2,$3,2
 
-$L212:
+$L199:
 	sltu	$2,$6,$5
-	bne	$2,$0,$L214
+	bne	$2,$0,$L201
 	nop
 
 	li	$2,-1149239296			# 0xbb800000
@@ -1342,39 +1344,177 @@ $L212:
 	ori	$3,$2,0x4d00
 	li	$8,9			# 0x9
 	ori	$7,$2,0x4d04
-$L210:
+$L197:
 	sll	$2,$5,5
 	addu	$2,$4,$2
 	addu	$2,$2,$9
 	sw	$2,0($10)
 	sw	$8,0($3)
-$L207:
+$L194:
 	lw	$2,0($3)
 	andi	$2,$2,0x1
-	bne	$2,$0,$L207
+	bne	$2,$0,$L194
 	nop
 
 	lw	$2,0($7)
 	andi	$2,$2,0x1
-	beq	$2,$0,$L209
+	beq	$2,$0,$L196
 	addiu	$5,$5,1
 
 	addiu	$5,$5,-1
 	j	$31
 	li	$2,-1			# 0xffffffff
 
-$L209:
+$L196:
 	sltu	$2,$6,$5
-	beq	$2,$0,$L210
+	beq	$2,$0,$L197
 	nop
 
-$L214:
+$L201:
 	j	$31
 	move	$2,$0
 
 	.set	macro
 	.set	reorder
 	.end	_rtl8651_delAsicEntry
+	.data
+	.align	2
+	.type	rtl819x_lastTxDesc, @object
+	.size	rtl819x_lastTxDesc, 4
+rtl819x_lastTxDesc:
+	.word	0
+	.align	2
+	.type	rtl819x_lastRxDesc, @object
+	.size	rtl819x_lastRxDesc, 4
+rtl819x_lastRxDesc:
+	.word	0
+	.align	2
+	.type	rtl819x_swHangCnt, @object
+	.size	rtl819x_swHangCnt, 4
+rtl819x_swHangCnt:
+	.word	0
+	.align	2
+	.type	rtl819x_checkSwCoreTimer, @object
+	.size	rtl819x_checkSwCoreTimer, 4
+rtl819x_checkSwCoreTimer:
+	.word	0
+	.rdata
+	.align	2
+$LC3:
+	.ascii	".........................................\n\000"
+	.section	.text.rtl819x_poll_sw,"ax",@progbits
+	.align	2
+	.globl	rtl819x_poll_sw
+	.ent	rtl819x_poll_sw
+	.type	rtl819x_poll_sw, @function
+rtl819x_poll_sw:
+	.set	nomips16
+	.frame	$sp,24,$31		# vars= 0, regs= 1/0, args= 16, gp= 0
+	.mask	0x80000000,-8
+	.fmask	0x00000000,0
+	.set	noreorder
+	.set	nomacro
+	
+	addiu	$sp,$sp,-24
+	sw	$31,16($sp)
+	li	$2,-1149239296			# 0xbb800000
+	ori	$2,$2,0x610c
+	lw	$3,0($2)
+	li	$2,65536			# 0x10000
+	and	$8,$3,$2
+	li	$3,-1207894016			# 0xb8010000
+	ori	$2,$3,0x4
+	lw	$2,0($2)
+	li	$4,-4			# 0xfffffffc
+	and	$6,$2,$4
+	ori	$3,$3,0x20
+	lw	$2,0($3)
+	and	$7,$2,$4
+	lui	$5,%hi(rtl819x_checkSwCoreTimer)
+	lw	$4,%lo(rtl819x_checkSwCoreTimer)($5)
+	li	$2,-859045888			# 0xcccc0000
+	ori	$2,$2,0xcccd
+	multu	$4,$2
+	mfhi	$2
+	srl	$2,$2,4
+	sll	$3,$2,2
+	addu	$3,$3,$2
+	sll	$3,$3,2
+	subu	$3,$4,$3
+	addiu	$4,$4,1
+	bne	$3,$0,$L203
+	sw	$4,%lo(rtl819x_checkSwCoreTimer)($5)
+
+	bne	$8,$0,$L205
+	lui	$2,%hi(rtl819x_lastTxDesc)
+
+	lui	$2,%hi(rtl819x_swHangCnt)
+	j	$L206
+	sw	$0,%lo(rtl819x_swHangCnt)($2)
+
+$L205:
+	lw	$2,%lo(rtl819x_lastTxDesc)($2)
+	beq	$2,$0,$L213
+	lui	$2,%hi(rtl819x_lastRxDesc)
+
+	lw	$2,%lo(rtl819x_lastRxDesc)($2)
+	bne	$2,$0,$L207
+	lui	$2,%hi(rtl819x_lastRxDesc)
+
+$L213:
+	sw	$6,%lo(rtl819x_lastRxDesc)($2)
+	lui	$2,%hi(rtl819x_lastTxDesc)
+	sw	$7,%lo(rtl819x_lastTxDesc)($2)
+	lui	$2,%hi(rtl819x_swHangCnt)
+	j	$L206
+	sw	$0,%lo(rtl819x_swHangCnt)($2)
+
+$L207:
+	lw	$2,%lo(rtl819x_lastRxDesc)($2)
+	bne	$2,$6,$L214
+	lui	$2,%hi(rtl819x_swHangCnt)
+
+	lui	$2,%hi(rtl819x_lastTxDesc)
+	lw	$2,%lo(rtl819x_lastTxDesc)($2)
+	bne	$2,$7,$L210
+	lui	$2,%hi(rtl819x_swHangCnt)
+
+	lui	$3,%hi(rtl819x_swHangCnt)
+	lw	$2,%lo(rtl819x_swHangCnt)($3)
+	addiu	$2,$2,1
+	j	$L206
+	sw	$2,%lo(rtl819x_swHangCnt)($3)
+
+$L210:
+$L214:
+	sw	$0,%lo(rtl819x_swHangCnt)($2)
+$L206:
+	lui	$2,%hi(rtl819x_lastRxDesc)
+	sw	$6,%lo(rtl819x_lastRxDesc)($2)
+	lui	$2,%hi(rtl819x_lastTxDesc)
+	sw	$7,%lo(rtl819x_lastTxDesc)($2)
+	lui	$3,%hi(rtl819x_swHangCnt)
+	lw	$2,%lo(rtl819x_swHangCnt)($3)
+	sltu	$2,$2,3
+	bne	$2,$0,$L203
+	nop
+
+	sw	$0,%lo(rtl819x_swHangCnt)($3)
+	lui	$4,%hi($LC3)
+	jal	panic_printk
+	addiu	$4,$4,%lo($LC3)
+
+	jal	machine_restart
+	move	$4,$0
+
+$L203:
+	lw	$31,16($sp)
+	j	$31
+	addiu	$sp,$sp,24
+
+	.set	macro
+	.set	reorder
+	.end	rtl819x_poll_sw
 	.globl	RtkHomeGatewayChipName
 	.section	.bss
 	.align	2
diff --git a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/9xD/rtl865x_asicBasic.S b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/9xD/rtl865x_asicBasic.S
index c5b42ae50..c692e703a 100644
--- a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/9xD/rtl865x_asicBasic.S
+++ b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/9xD/rtl865x_asicBasic.S
@@ -197,14 +197,14 @@ rtl865x_initAsicFun:
 	andi	$3,$3,0xf
 
 	li	$2,9			# 0x9
-	beq	$3,$2,$L25
+	beq	$3,$2,$L29
 	li	$2,46			# 0x2e
 
 	andi	$2,$3,0x8
 	bne	$2,$0,$L15
 	li	$2,46			# 0x2e
 
-$L25:
+$L29:
 	sw	$2,0($4)
 	j	$31
 	move	$2,$0
@@ -271,15 +271,48 @@ $L22:
 	move	$2,$0
 
 $L19:
-	li	$3,-2120810496			# 0x81970000
-	beq	$2,$3,$L23
-	nop
+	li	$5,-2120810496			# 0x81970000
+	beq	$2,$5,$L23
+	li	$2,-1			# 0xffffffff
 
 	j	$31
-	li	$2,-1			# 0xffffffff
+	nop
 
 $L23:
+	li	$2,15			# 0xf
+	beq	$3,$2,$L30
+	li	$2,3			# 0x3
+
+	li	$2,8			# 0x8
+	bne	$3,$2,$L25
+	li	$2,3			# 0x3
+
+$L30:
+	sw	$2,0($4)
+	j	$31
+	move	$2,$0
+
+$L25:
 	li	$2,14			# 0xe
+	beq	$3,$2,$L31
+	li	$2,46			# 0x2e
+
+	li	$2,9			# 0x9
+	beq	$3,$2,$L26
+	andi	$3,$3,0x8
+
+	bne	$3,$0,$L27
+	nop
+
+$L26:
+	li	$2,46			# 0x2e
+$L31:
+	sw	$2,0($4)
+	j	$31
+	move	$2,$0
+
+$L27:
+	li	$2,3			# 0x3
 	sw	$2,0($4)
 	j	$31
 	move	$2,$0
@@ -309,7 +342,7 @@ rtl865x_getAsicFun:
 	lw	$3,%lo(fun_enable)($2)
 	sw	$3,0($4)
 	lw	$2,%lo(fun_enable)($2)
-	bne	$2,$0,$L27
+	bne	$2,$0,$L35
 	move	$17,$4
 
 	lui	$16,%hi(fun_enable)
@@ -318,7 +351,7 @@ rtl865x_getAsicFun:
 
 	lw	$2,%lo(fun_enable)($16)
 	sw	$2,0($17)
-$L27:
+$L35:
 	move	$2,$0
 	lw	$31,28($sp)
 	lw	$17,24($sp)
@@ -362,115 +395,151 @@ bsp_swcore_init:
 	addiu	$4,$4,%lo(fun_enable)
 
 	li	$3,-1			# 0xffffffff
-	beq	$2,$3,$L31
+	beq	$2,$3,$L39
 	li	$2,-1			# 0xffffffff
 
-	li	$2,-1207959552			# 0xb8000000
-	lw	$3,0($2)
-	li	$2,-65536			# 0xffff0000
-	and	$3,$3,$2
-	li	$2,-1073741824			# 0xc0000000
-	bne	$3,$2,$L38
-	li	$2,-65536			# 0xffff0000
-
-	li	$2,-1207959552			# 0xb8000000
-	ori	$2,$2,0xc
-	lw	$2,0($2)
-	andi	$2,$2,0x8
-	bne	$2,$0,$L38
 	li	$2,-65536			# 0xffff0000
-
-	addiu	$2,$16,-8
-	sltu	$2,$2,2
-	bne	$2,$0,$L31
-	move	$2,$0
-
-	li	$2,-65536			# 0xffff0000
-$L38:
 	and	$17,$17,$2
 	li	$2,-1073741824			# 0xc0000000
-	bne	$17,$2,$L33
+	bne	$17,$2,$L40
 	andi	$18,$18,0xf
 
 	li	$2,9			# 0x9
-	bne	$16,$2,$L34
+	bne	$16,$2,$L41
 	nop
 
-	beq	$18,$2,$L34
+	beq	$18,$2,$L41
+	andi	$18,$18,0x8
+
+	beq	$18,$0,$L41
 	lui	$4,%hi($LC0)
 
 	addiu	$4,$4,%lo($LC0)
 	jal	early_console_write
 	li	$5,27			# 0x1b
 
-	j	$L31
+	j	$L39
 	li	$2,-1			# 0xffffffff
 
-$L33:
+$L40:
 	li	$2,-2147483648			# 0x80000000
-	bne	$17,$2,$L39
-	move	$2,$0
+	bne	$17,$2,$L42
+	li	$2,-2120810496			# 0x81970000
 
 	li	$2,7			# 0x7
-	bne	$16,$2,$L35
+	bne	$16,$2,$L43
 	nop
 
-	beq	$18,$2,$L34
+	beq	$18,$2,$L41
 	lui	$4,%hi($LC0)
 
 	addiu	$4,$4,%lo($LC0)
 	jal	early_console_write
 	li	$5,27			# 0x1b
 
-	j	$L31
+	j	$L39
 	li	$2,-1			# 0xffffffff
 
-$L35:
+$L43:
 	li	$2,15			# 0xf
-	bne	$16,$2,$L36
+	bne	$16,$2,$L44
 	nop
 
-	beq	$18,$2,$L36
+	beq	$18,$2,$L44
 	li	$2,7			# 0x7
 
-	beq	$18,$2,$L34
+	beq	$18,$2,$L41
 	li	$2,3			# 0x3
 
-	beq	$18,$2,$L34
+	beq	$18,$2,$L41
 	li	$2,11			# 0xb
 
-	beq	$18,$2,$L34
+	beq	$18,$2,$L41
 	lui	$4,%hi($LC0)
 
 	addiu	$4,$4,%lo($LC0)
 	jal	early_console_write
 	li	$5,27			# 0x1b
 
-	j	$L31
+	j	$L39
 	li	$2,-1			# 0xffffffff
 
-$L36:
+$L44:
 	li	$2,3			# 0x3
-	bne	$16,$2,$L34
+	bne	$16,$2,$L41
 	nop
 
-	beq	$18,$2,$L34
+	beq	$18,$2,$L41
 	li	$2,7			# 0x7
 
-	beq	$18,$2,$L34
+	beq	$18,$2,$L41
 	lui	$4,%hi($LC0)
 
 	addiu	$4,$4,%lo($LC0)
 	jal	early_console_write
 	li	$5,27			# 0x1b
 
-	j	$L31
+	j	$L39
 	li	$2,-1			# 0xffffffff
 
-$L34:
+$L42:
+	bne	$17,$2,$L47
+	move	$2,$0
+
+	li	$2,8			# 0x8
+	bne	$16,$2,$L48
+	li	$2,11			# 0xb
+
+	li	$2,15			# 0xf
+	beq	$18,$2,$L45
+	li	$2,8			# 0x8
+
+	beq	$18,$2,$L41
+	li	$2,14			# 0xe
+
+	beq	$18,$2,$L41
+	li	$2,9			# 0x9
+
+	beq	$18,$2,$L41
+	andi	$18,$18,0x8
+
+	beq	$18,$0,$L41
+	lui	$4,%hi($LC0)
+
+	addiu	$4,$4,%lo($LC0)
+	jal	early_console_write
+	li	$5,27			# 0x1b
+
+	j	$L39
+	li	$2,-1			# 0xffffffff
+
+$L45:
+	li	$2,11			# 0xb
+$L48:
+	bne	$16,$2,$L47
+	move	$2,$0
+
+	li	$2,14			# 0xe
+	beq	$18,$2,$L41
+	li	$2,9			# 0x9
+
+	beq	$18,$2,$L41
+	andi	$18,$18,0x8
+
+	beq	$18,$0,$L41
+	lui	$4,%hi($LC0)
+
+	addiu	$4,$4,%lo($LC0)
+	jal	early_console_write
+	li	$5,27			# 0x1b
+
+	j	$L39
+	li	$2,-1			# 0xffffffff
+
+$L41:
 	move	$2,$0
-$L31:
 $L39:
+$L47:
 	lw	$31,28($sp)
 	lw	$18,24($sp)
 	lw	$17,20($sp)
@@ -505,65 +574,65 @@ rtl865x_accessAsicTable:
 	addiu	$4,$sp,16
 
 	sltu	$2,$16,15
-	beq	$2,$0,$L41
+	beq	$2,$0,$L50
 	li	$2,1			# 0x1
 
 	sll	$16,$2,$16
 	andi	$2,$16,0xe22
-	bne	$2,$0,$L42
+	bne	$2,$0,$L51
 	andi	$2,$16,0x4000
 
-	bne	$2,$0,$L44
+	bne	$2,$0,$L53
 	andi	$16,$16,0x8
 
-	bne	$16,$0,$L43
+	bne	$16,$0,$L52
 	li	$2,1			# 0x1
 
-	j	$L46
+	j	$L55
 	sw	$2,0($17)
 
-$L44:
+$L53:
 	lw	$2,16($sp)
 	andi	$2,$2,0x20
-	beq	$2,$0,$L45
+	beq	$2,$0,$L54
 	li	$2,1			# 0x1
 
-	j	$L46
+	j	$L55
 	sw	$2,0($17)
 
-$L45:
-	j	$L46
+$L54:
+	j	$L55
 	sw	$0,0($17)
 
-$L42:
+$L51:
 	lw	$2,16($sp)
 	andi	$2,$2,0x4
-	beq	$2,$0,$L47
+	beq	$2,$0,$L56
 	li	$2,1			# 0x1
 
-	j	$L46
+	j	$L55
 	sw	$2,0($17)
 
-$L47:
-	j	$L46
+$L56:
+	j	$L55
 	sw	$0,0($17)
 
-$L43:
+$L52:
 	lw	$2,16($sp)
 	andi	$2,$2,0x2
-	beq	$2,$0,$L48
+	beq	$2,$0,$L57
 	li	$2,1			# 0x1
 
-	j	$L46
+	j	$L55
 	sw	$2,0($17)
 
-$L48:
-	j	$L46
+$L57:
+	j	$L55
 	sw	$0,0($17)
 
-$L41:
+$L50:
 	sw	$2,0($17)
-$L46:
+$L55:
 	move	$2,$0
 	lw	$31,36($sp)
 	lw	$17,32($sp)
@@ -612,33 +681,33 @@ _rtl8651_delAsicEntry:
 	
 	li	$3,-1149239296			# 0xbb800000
 	ori	$3,$3,0x4d00
-$L54:
+$L63:
 	lw	$2,0($3)
 	andi	$2,$2,0x1
-	bne	$2,$0,$L54
+	bne	$2,$0,$L63
 	lui	$2,%hi(_rtl8651_asicTableSize)
 
 	sll	$3,$4,2
 	addiu	$2,$2,%lo(_rtl8651_asicTableSize)
 	addu	$2,$3,$2
 	lw	$7,0($2)
-	beq	$7,$0,$L55
+	beq	$7,$0,$L64
 	move	$2,$0
 
 	li	$8,-1149239296			# 0xbb800000
 	ori	$8,$8,0x4d20
 	sll	$3,$2,2
-$L67:
+$L76:
 	addu	$3,$3,$8
 	sw	$0,0($3)
 	addiu	$2,$2,1
 	sltu	$3,$2,$7
-	bne	$3,$0,$L67
+	bne	$3,$0,$L76
 	sll	$3,$2,2
 
-$L55:
+$L64:
 	sltu	$2,$6,$5
-	bne	$2,$0,$L57
+	bne	$2,$0,$L66
 	nop
 
 	sll	$4,$4,16
@@ -651,30 +720,30 @@ $L55:
 	ori	$3,$7,0x4d00
 	li	$8,9			# 0x9
 	ori	$7,$7,0x4d04
-$L61:
+$L70:
 	sw	$4,0($9)
 	sw	$8,0($3)
-$L58:
+$L67:
 	lw	$2,0($3)
 	andi	$2,$2,0x1
-	bne	$2,$0,$L58
+	bne	$2,$0,$L67
 	nop
 
 	lw	$2,0($7)
 	andi	$2,$2,0x1
-	beq	$2,$0,$L59
+	beq	$2,$0,$L68
 	addiu	$5,$5,1
 
 	addiu	$5,$5,-1
 	j	$31
 	li	$2,-1			# 0xffffffff
 
-$L59:
+$L68:
 	sltu	$2,$6,$5
-	beq	$2,$0,$L61
+	beq	$2,$0,$L70
 	addiu	$4,$4,32
 
-$L57:
+$L66:
 	j	$31
 	move	$2,$0
 
@@ -715,10 +784,10 @@ _rtl8651_readAsicEntryStopTLU:
 	addiu	$5,$sp,16
 
 	lw	$2,16($sp)
-	beq	$2,$0,$L70
+	beq	$2,$0,$L79
 	li	$2,-1			# 0xffffffff
 
-	bne	$16,$0,$L71
+	bne	$16,$0,$L80
 	sll	$3,$17,16
 
 	lui	$4,%hi($LC1)
@@ -726,13 +795,13 @@ _rtl8651_readAsicEntryStopTLU:
 	lui	$5,%hi($LC2)
 	addiu	$5,$5,%lo($LC2)
 	jal	panic_printk
-	li	$6,550			# 0x226
+	li	$6,557			# 0x22d
 
-$L72:
-	j	$L72
+$L81:
+	j	$L81
 	nop
 
-$L71:
+$L80:
 	li	$2,-1157627904			# 0xbb000000
 	addu	$2,$3,$2
 	sll	$18,$18,5
@@ -740,39 +809,39 @@ $L71:
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 	lw	$2,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$3,2			# 0x2
-	bne	$2,$3,$L73
+	bne	$2,$3,$L82
 	li	$3,3			# 0x3
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
-	bgtz	$2,$L85
+	bgtz	$2,$L94
 	li	$2,-1149239296			# 0xbb800000
 
-	j	$L83
+	j	$L92
 	sll	$3,$17,2
 
-$L73:
-	beq	$2,$3,$L74
+$L82:
+	beq	$2,$3,$L83
 	li	$3,4			# 0x4
 
-	bne	$2,$3,$L83
+	bne	$2,$3,$L92
 	sll	$3,$17,2
 
-$L74:
+$L83:
 	li	$2,-1149239296			# 0xbb800000
-$L85:
+$L94:
 	ori	$2,$2,0x4418
 	lw	$4,0($2)
 	li	$3,262144			# 0x40000
 	or	$3,$4,$3
 	sw	$3,0($2)
 	sll	$3,$17,2
-$L83:
+$L92:
 	lui	$2,%hi(_rtl8651_asicTableSize)
 	addiu	$2,$2,%lo(_rtl8651_asicTableSize)
 	addu	$2,$3,$2
 	lw	$2,0($2)
-	beq	$2,$0,$L76
+	beq	$2,$0,$L85
 	move	$2,$0
 
 	move	$17,$3
@@ -780,39 +849,39 @@ $L83:
 	addiu	$3,$3,%lo(_rtl8651_asicTableSize)
 	addu	$17,$17,$3
 	lw	$4,0($17)
-$L77:
+$L86:
 	lw	$3,0($18)
 	sw	$3,0($16)
 	addiu	$2,$2,1
 	addiu	$18,$18,4
 	sltu	$3,$2,$4
-	bne	$3,$0,$L77
+	bne	$3,$0,$L86
 	addiu	$16,$16,4
 
-$L76:
+$L85:
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 	lw	$2,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$3,2			# 0x2
-	bne	$2,$3,$L78
+	bne	$2,$3,$L87
 	nop
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
-	bgtz	$2,$L79
+	bgtz	$2,$L88
 	nop
 
-	j	$L84
+	j	$L93
 	move	$2,$0
 
-$L78:
+$L87:
 	li	$3,3			# 0x3
-	beq	$2,$3,$L79
+	beq	$2,$3,$L88
 	li	$3,4			# 0x4
 
-	bne	$2,$3,$L80
+	bne	$2,$3,$L89
 	nop
 
-$L79:
+$L88:
 	li	$2,-1149239296			# 0xbb800000
 	ori	$2,$2,0x4418
 	lw	$4,0($2)
@@ -820,13 +889,13 @@ $L79:
 	ori	$3,$3,0xffff
 	and	$3,$4,$3
 	sw	$3,0($2)
-	j	$L70
+	j	$L79
 	move	$2,$0
 
-$L80:
+$L89:
 	move	$2,$0
-$L70:
-$L84:
+$L79:
+$L93:
 	lw	$31,36($sp)
 	lw	$18,32($sp)
 	lw	$17,28($sp)
@@ -845,20 +914,17 @@ $L84:
 	.ent	_rtl8651_readAsicEntry
 	.type	_rtl8651_readAsicEntry, @function
 _rtl8651_readAsicEntry:
-	.frame	$sp,120,$31		# vars= 72, regs= 7/0, args= 16, gp= 0
-	.mask	0x803f0000,-4
+	.frame	$sp,104,$31		# vars= 72, regs= 4/0, args= 16, gp= 0
+	.mask	0x80070000,-4
 	.fmask	0x00000000,0
 	.set	noreorder
 	.set	nomacro
 	
-	addiu	$sp,$sp,-120
-	sw	$31,116($sp)
-	sw	$21,112($sp)
-	sw	$20,108($sp)
-	sw	$19,104($sp)
-	sw	$18,100($sp)
-	sw	$17,96($sp)
-	sw	$16,92($sp)
+	addiu	$sp,$sp,-104
+	sw	$31,100($sp)
+	sw	$18,96($sp)
+	sw	$17,92($sp)
+	sw	$16,88($sp)
 	move	$17,$4
 	move	$18,$5
 	move	$16,$6
@@ -866,10 +932,10 @@ _rtl8651_readAsicEntry:
 	addiu	$5,$sp,16
 
 	lw	$2,16($sp)
-	beq	$2,$0,$L88
+	beq	$2,$0,$L97
 	li	$2,-1			# 0xffffffff
 
-	bne	$16,$0,$L89
+	bne	$16,$0,$L98
 	sll	$3,$17,16
 
 	lui	$4,%hi($LC1)
@@ -877,157 +943,119 @@ _rtl8651_readAsicEntry:
 	lui	$5,%hi($LC2)
 	addiu	$5,$5,%lo($LC2)
 	jal	panic_printk
-	li	$6,444			# 0x1bc
+	li	$6,462			# 0x1ce
 
-$L90:
-	j	$L90
+$L99:
+	j	$L99
 	nop
 
-$L89:
+$L98:
 	li	$2,-1157627904			# 0xbb000000
 	addu	$2,$3,$2
 	sll	$18,$18,5
 	addu	$18,$2,$18
 	li	$3,-1149239296			# 0xbb800000
 	ori	$3,$3,0x4d00
-$L91:
+$L100:
 	lw	$2,0($3)
 	andi	$2,$2,0x1
-	bne	$2,$0,$L91
-	li	$25,10			# 0xa
-
-	li	$11,2			# 0x2
-	addiu	$10,$sp,20
-	move	$19,$10
-	move	$12,$0
-	li	$15,2			# 0x2
-	li	$7,8			# 0x8
-	li	$14,1			# 0x1
-	move	$24,$10
-$L110:
-	move	$2,$19
-	move	$3,$12
-$L93:
-	beq	$11,$3,$L92
-	nop
-
-	lw	$4,0($18)
-	sw	$4,0($2)
-	lw	$4,4($18)
-	sw	$4,4($2)
-	lw	$4,8($18)
-	sw	$4,8($2)
-	lw	$4,12($18)
-	sw	$4,12($2)
-	lw	$4,16($18)
-	sw	$4,16($2)
-	lw	$4,20($18)
-	sw	$4,20($2)
-	lw	$4,24($18)
-	sw	$4,24($2)
-	lw	$4,28($18)
-	sw	$4,28($2)
-	move	$11,$3
-$L92:
-	addiu	$3,$3,1
-	bne	$3,$15,$L93
-	addiu	$2,$2,32
-
-	move	$21,$12
-	j	$L94
-	move	$8,$12
-
-$L96:
-	lw	$6,0($3)
-	lw	$5,0($2)
-	bne	$6,$5,$L95
-	addiu	$4,$4,1
-
-	addiu	$3,$3,4
-	bne	$4,$7,$L96
-	addiu	$2,$2,4
-
-	addiu	$8,$8,1
-	sltu	$2,$8,2
-	beq	$2,$0,$L97
-	addiu	$9,$9,32
-
-$L99:
-	lw	$2,0($9)
-	bne	$13,$2,$L95
-	move	$3,$20
-
-	sll	$2,$8,5
+	bne	$2,$0,$L100
+	li	$14,10			# 0xa
+
+	addiu	$12,$18,4
+	addiu	$11,$18,8
+	addiu	$10,$18,12
+	addiu	$9,$18,16
+	addiu	$8,$18,20
+	addiu	$7,$18,24
+	addiu	$6,$18,28
+	addiu	$13,$sp,56
+	addiu	$5,$sp,84
+$L104:
+	lw	$2,0($18)
+	sw	$2,20($sp)
+	lw	$3,0($12)
+	sw	$3,24($sp)
+	lw	$3,0($11)
+	sw	$3,28($sp)
+	lw	$3,0($10)
+	sw	$3,32($sp)
+	lw	$3,0($9)
+	sw	$3,36($sp)
+	lw	$3,0($8)
+	sw	$3,40($sp)
+	lw	$3,0($7)
+	sw	$3,44($sp)
+	lw	$3,0($6)
+	sw	$3,48($sp)
+	lw	$3,0($18)
+	sw	$3,52($sp)
+	lw	$4,0($12)
+	sw	$4,56($sp)
+	lw	$4,0($11)
+	sw	$4,60($sp)
+	lw	$4,0($10)
+	sw	$4,64($sp)
+	lw	$4,0($9)
+	sw	$4,68($sp)
+	lw	$4,0($8)
+	sw	$4,72($sp)
+	lw	$4,0($7)
+	sw	$4,76($sp)
+	lw	$4,0($6)
+	bne	$3,$2,$L101
+	sw	$4,80($sp)
+
+	move	$2,$13
+$L102:
+	lw	$4,0($2)
+	lw	$3,-32($2)
+	bne	$4,$3,$L101
 	addiu	$2,$2,4
-	addu	$2,$10,$2
-	j	$L96
-	move	$4,$14
 
-$L97:
-	addiu	$21,$21,1
-	beq	$21,$15,$L98
-	addiu	$24,$24,32
+	bne	$2,$5,$L102
+	sll	$3,$17,2
 
-	move	$8,$21
-$L94:
-	sltu	$2,$8,2
-	beq	$2,$0,$L97
-	sll	$9,$8,5
+	j	$L113
+	lui	$2,%hi(_rtl8651_asicTableSize)
 
-	lw	$13,0($24)
-	addu	$9,$10,$9
-	sll	$20,$21,5
-	addiu	$20,$20,4
-	j	$L99
-	addu	$20,$10,$20
+$L101:
+	addiu	$14,$14,-1
+	bne	$14,$0,$L104
+	sll	$3,$17,2
 
-$L95:
-	sll	$11,$11,5
-$L109:
-	addiu	$3,$sp,20
-	sll	$4,$17,2
 	lui	$2,%hi(_rtl8651_asicTableSize)
+$L113:
 	addiu	$2,$2,%lo(_rtl8651_asicTableSize)
-	addu	$2,$4,$2
+	addu	$2,$3,$2
 	lw	$2,0($2)
-	beq	$2,$0,$L100
-	addu	$3,$3,$11
-
+	beq	$2,$0,$L105
 	move	$2,$0
-	move	$17,$4
+
+	addiu	$3,$sp,52
+	sll	$17,$17,2
 	lui	$4,%hi(_rtl8651_asicTableSize)
 	addiu	$4,$4,%lo(_rtl8651_asicTableSize)
 	addu	$17,$17,$4
 	lw	$5,0($17)
-$L101:
+$L106:
 	lw	$4,0($3)
 	sw	$4,0($16)
 	addiu	$2,$2,1
 	addiu	$3,$3,4
 	sltu	$4,$2,$5
-	bne	$4,$0,$L101
+	bne	$4,$0,$L106
 	addiu	$16,$16,4
 
-$L100:
+$L105:
 	move	$2,$0
-$L88:
-	lw	$31,116($sp)
-	lw	$21,112($sp)
-	lw	$20,108($sp)
-	lw	$19,104($sp)
-	lw	$18,100($sp)
-	lw	$17,96($sp)
-	lw	$16,92($sp)
+$L97:
+	lw	$31,100($sp)
+	lw	$18,96($sp)
+	lw	$17,92($sp)
+	lw	$16,88($sp)
 	j	$31
-	addiu	$sp,$sp,120
-
-$L98:
-	addiu	$25,$25,-1
-	bne	$25,$0,$L110
-	move	$24,$10
-
-	j	$L109
-	sll	$11,$11,5
+	addiu	$sp,$sp,104
 
 	.set	macro
 	.set	reorder
@@ -1047,7 +1075,7 @@ _rtl8651_asicTableAccessForward:
 	
 	addiu	$sp,$sp,-24
 	sw	$31,20($sp)
-	bne	$6,$0,$L112
+	bne	$6,$0,$L115
 	li	$3,-1149239296			# 0xbb800000
 
 	lui	$4,%hi($LC1)
@@ -1055,25 +1083,25 @@ _rtl8651_asicTableAccessForward:
 	lui	$5,%hi($LC2)
 	addiu	$5,$5,%lo($LC2)
 	jal	panic_printk
-	li	$6,306			# 0x132
+	li	$6,324			# 0x144
 
-$L113:
-	j	$L113
+$L116:
+	j	$L116
 	nop
 
-$L112:
+$L115:
 	ori	$3,$3,0x4d00
-$L117:
+$L120:
 	lw	$2,0($3)
 	andi	$2,$2,0x1
-	bne	$2,$0,$L117
+	bne	$2,$0,$L120
 	lui	$2,%hi(_rtl8651_asicTableSize)
 
 	sll	$3,$4,2
 	addiu	$2,$2,%lo(_rtl8651_asicTableSize)
 	addu	$2,$3,$2
 	lw	$2,0($2)
-	beq	$2,$0,$L114
+	beq	$2,$0,$L117
 	move	$2,$0
 
 	li	$9,-1149239296			# 0xbb800000
@@ -1083,17 +1111,17 @@ $L117:
 	addiu	$3,$3,%lo(_rtl8651_asicTableSize)
 	addu	$3,$7,$3
 	lw	$8,0($3)
-$L115:
+$L118:
 	sll	$3,$2,2
 	addu	$3,$3,$9
 	lw	$7,0($6)
 	sw	$7,0($3)
 	addiu	$2,$2,1
 	sltu	$3,$2,$8
-	bne	$3,$0,$L115
+	bne	$3,$0,$L118
 	addiu	$6,$6,4
 
-$L114:
+$L117:
 	sll	$4,$4,16
 	li	$2,-1157627904			# 0xbb000000
 	addu	$4,$4,$2
@@ -1135,11 +1163,11 @@ _rtl8651_forceAddAsicEntry:
 	addiu	$5,$sp,16
 
 	lw	$2,16($sp)
-	beq	$2,$0,$L122
+	beq	$2,$0,$L125
 	li	$2,-1			# 0xffffffff
 
 	li	$2,3			# 0x3
-	bne	$18,$2,$L137
+	bne	$18,$2,$L140
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lui	$2,%hi(mcastForceAddOpCnt)
@@ -1147,30 +1175,30 @@ _rtl8651_forceAddAsicEntry:
 	addiu	$3,$3,1
 	sw	$3,%lo(mcastForceAddOpCnt)($2)
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
-$L137:
+$L140:
 	lw	$2,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$3,2			# 0x2
-	bne	$2,$3,$L124
+	bne	$2,$3,$L127
 	li	$3,3			# 0x3
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
-	bgtz	$2,$L138
+	bgtz	$2,$L141
 	li	$2,-1149239296			# 0xbb800000
 
-	j	$L135
+	j	$L138
 	move	$4,$18
 
-$L124:
-	beq	$2,$3,$L125
+$L127:
+	beq	$2,$3,$L128
 	li	$3,4			# 0x4
 
-	bne	$2,$3,$L135
+	bne	$2,$3,$L138
 	move	$4,$18
 
-$L125:
+$L128:
 	li	$2,-1149239296			# 0xbb800000
-$L138:
+$L141:
 	ori	$2,$2,0x4418
 	lw	$4,0($2)
 	li	$3,262144			# 0x40000
@@ -1178,14 +1206,14 @@ $L138:
 	sw	$3,0($2)
 	move	$4,$2
 	li	$3,524288			# 0x80000
-$L127:
+$L130:
 	lw	$2,0($4)
 	and	$2,$2,$3
-	beq	$2,$0,$L127
+	beq	$2,$0,$L130
 	nop
 
 	move	$4,$18
-$L135:
+$L138:
 	move	$5,$16
 	jal	_rtl8651_asicTableAccessForward
 	move	$6,$17
@@ -1195,34 +1223,34 @@ $L135:
 	ori	$2,$2,0x4d00
 	sw	$3,0($2)
 	move	$3,$2
-$L128:
+$L131:
 	lw	$2,0($3)
 	andi	$2,$2,0x1
-	bne	$2,$0,$L128
+	bne	$2,$0,$L131
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 
 	lw	$2,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$3,2			# 0x2
-	bne	$2,$3,$L129
+	bne	$2,$3,$L132
 	nop
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
-	bgtz	$2,$L130
+	bgtz	$2,$L133
 	nop
 
-	j	$L136
+	j	$L139
 	move	$2,$0
 
-$L129:
+$L132:
 	li	$3,3			# 0x3
-	beq	$2,$3,$L130
+	beq	$2,$3,$L133
 	li	$3,4			# 0x4
 
-	bne	$2,$3,$L131
+	bne	$2,$3,$L134
 	nop
 
-$L130:
+$L133:
 	li	$2,-1149239296			# 0xbb800000
 	ori	$2,$2,0x4418
 	lw	$4,0($2)
@@ -1230,13 +1258,13 @@ $L130:
 	ori	$3,$3,0xffff
 	and	$3,$4,$3
 	sw	$3,0($2)
-	j	$L122
+	j	$L125
 	move	$2,$0
 
-$L131:
+$L134:
 	move	$2,$0
-$L122:
-$L136:
+$L125:
+$L139:
 	lw	$31,36($sp)
 	lw	$18,32($sp)
 	lw	$17,28($sp)
@@ -1273,7 +1301,7 @@ _rtl8651_addAsicEntry:
 	addiu	$5,$sp,16
 
 	lw	$2,16($sp)
-	beq	$2,$0,$L158
+	beq	$2,$0,$L161
 	li	$2,-1			# 0xffffffff
 
 	move	$4,$18
@@ -1284,27 +1312,27 @@ _rtl8651_addAsicEntry:
 	lui	$2,%hi(RtkHomeGatewayChipNameID)
 	lw	$4,%lo(RtkHomeGatewayChipNameID)($2)
 	li	$2,2			# 0x2
-	bne	$4,$2,$L141
+	bne	$4,$2,$L144
 	li	$2,3			# 0x3
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
-	bgtz	$2,$L159
+	bgtz	$2,$L162
 	li	$2,-1149239296			# 0xbb800000
 
-	j	$L161
+	j	$L164
 	li	$3,3			# 0x3
 
-$L141:
-	beq	$4,$2,$L142
+$L144:
+	beq	$4,$2,$L145
 	li	$2,4			# 0x4
 
-	bne	$4,$2,$L156
+	bne	$4,$2,$L159
 	li	$3,3			# 0x3
 
-$L142:
+$L145:
 	li	$2,-1149239296			# 0xbb800000
-$L159:
+$L162:
 	ori	$2,$2,0x4418
 	lw	$5,0($2)
 	li	$3,262144			# 0x40000
@@ -1312,101 +1340,101 @@ $L159:
 	sw	$3,0($2)
 	move	$5,$2
 	li	$3,524288			# 0x80000
-$L144:
+$L147:
 	lw	$2,0($5)
 	and	$2,$2,$3
-	beq	$2,$0,$L144
+	beq	$2,$0,$L147
 	nop
 
 	li	$3,3			# 0x3
-$L156:
+$L159:
 	li	$2,-1149239296			# 0xbb800000
-$L161:
+$L164:
 	ori	$2,$2,0x4d00
 	sw	$3,0($2)
 	move	$3,$2
-$L145:
+$L148:
 	lw	$2,0($3)
 	andi	$2,$2,0x1
-	bne	$2,$0,$L145
+	bne	$2,$0,$L148
 	li	$2,-1149239296			# 0xbb800000
 
 	ori	$2,$2,0x4d04
 	lw	$2,0($2)
 	andi	$2,$2,0x1
-	beq	$2,$0,$L146
+	beq	$2,$0,$L149
 	li	$2,2			# 0x2
 
-	bne	$4,$2,$L147
+	bne	$4,$2,$L150
 	li	$2,3			# 0x3
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
-	blez	$2,$L140
+	blez	$2,$L143
 	nop
 
-	j	$L157
+	j	$L160
 	li	$2,-1149239296			# 0xbb800000
 
-$L147:
-	beq	$4,$2,$L157
+$L150:
+	beq	$4,$2,$L160
 	li	$2,-1149239296			# 0xbb800000
 
 	li	$2,4			# 0x4
-	bne	$4,$2,$L140
+	bne	$4,$2,$L143
 	nop
 
 	li	$2,-1149239296			# 0xbb800000
-$L157:
+$L160:
 	ori	$2,$2,0x4418
 	lw	$4,0($2)
 	li	$3,-327680			# 0xfffb0000
 	ori	$3,$3,0xffff
 	and	$3,$4,$3
 	sw	$3,0($2)
-	j	$L149
+	j	$L152
 	li	$2,-1			# 0xffffffff
 
-$L146:
-	bne	$4,$2,$L150
+$L149:
+	bne	$4,$2,$L153
 	li	$2,3			# 0x3
 
 	lui	$2,%hi(RtkHomeGatewayChipRevisionID)
 	lw	$2,%lo(RtkHomeGatewayChipRevisionID)($2)
-	bgtz	$2,$L151
+	bgtz	$2,$L154
 	nop
 
-	j	$L158
+	j	$L161
 	move	$2,$0
 
-$L150:
-	beq	$4,$2,$L160
+$L153:
+	beq	$4,$2,$L163
 	li	$2,-1149239296			# 0xbb800000
 
 	li	$2,4			# 0x4
-	bne	$4,$2,$L152
+	bne	$4,$2,$L155
 	nop
 
-$L151:
+$L154:
 	li	$2,-1149239296			# 0xbb800000
-$L160:
+$L163:
 	ori	$2,$2,0x4418
 	lw	$4,0($2)
 	li	$3,-327680			# 0xfffb0000
 	ori	$3,$3,0xffff
 	and	$3,$4,$3
 	sw	$3,0($2)
-	j	$L149
+	j	$L152
 	move	$2,$0
 
-$L140:
-	j	$L149
+$L143:
+	j	$L152
 	li	$2,-1			# 0xffffffff
 
-$L152:
+$L155:
 	move	$2,$0
-$L149:
-$L158:
+$L152:
+$L161:
 	lw	$31,36($sp)
 	lw	$18,32($sp)
 	lw	$17,28($sp)
@@ -1418,6 +1446,135 @@ $L158:
 	.set	reorder
 	.end	_rtl8651_addAsicEntry
 	.size	_rtl8651_addAsicEntry, .-_rtl8651_addAsicEntry
+	.section	.rodata.str1.4
+	.align	2
+$LC3:
+	.ascii	".........................................\012\000"
+	.section	.text.rtl819x_poll_sw,"ax",@progbits
+	.align	2
+	.globl	rtl819x_poll_sw
+	.set	nomips16
+	.ent	rtl819x_poll_sw
+	.type	rtl819x_poll_sw, @function
+rtl819x_poll_sw:
+	.frame	$sp,24,$31		# vars= 0, regs= 1/0, args= 16, gp= 0
+	.mask	0x80000000,-4
+	.fmask	0x00000000,0
+	.set	noreorder
+	.set	nomacro
+	
+	addiu	$sp,$sp,-24
+	sw	$31,20($sp)
+	li	$2,-1149239296			# 0xbb800000
+	ori	$2,$2,0x610c
+	lw	$4,0($2)
+	li	$2,-1207894016			# 0xb8010000
+	ori	$3,$2,0x4
+	lw	$6,0($3)
+	ori	$2,$2,0x20
+	lw	$5,0($2)
+	lui	$3,%hi(rtl819x_checkSwCoreTimer)
+	lw	$2,%lo(rtl819x_checkSwCoreTimer)($3)
+	addiu	$7,$2,1
+	sw	$7,%lo(rtl819x_checkSwCoreTimer)($3)
+	li	$3,-859045888			# 0xcccc0000
+	ori	$3,$3,0xcccd
+	multu	$2,$3
+	mfhi	$3
+	srl	$3,$3,4
+	sll	$7,$3,2
+	sll	$3,$3,4
+	addu	$3,$7,$3
+	bne	$2,$3,$L173
+	li	$2,-4			# 0xfffffffc
+
+	and	$6,$6,$2
+	and	$5,$5,$2
+	li	$2,65536			# 0x10000
+	and	$4,$4,$2
+	bne	$4,$0,$L167
+	lui	$2,%hi(rtl819x_lastTxDesc)
+
+	lui	$2,%hi(rtl819x_swHangCnt)
+	j	$L168
+	sw	$0,%lo(rtl819x_swHangCnt)($2)
+
+$L167:
+	lw	$2,%lo(rtl819x_lastTxDesc)($2)
+	beq	$2,$0,$L169
+	nop
+
+	lui	$3,%hi(rtl819x_lastRxDesc)
+	lw	$3,%lo(rtl819x_lastRxDesc)($3)
+	bne	$3,$0,$L170
+	nop
+
+$L169:
+	lui	$2,%hi(rtl819x_swHangCnt)
+	j	$L168
+	sw	$0,%lo(rtl819x_swHangCnt)($2)
+
+$L170:
+	bne	$6,$3,$L171
+	nop
+
+	bne	$5,$2,$L175
+	lui	$2,%hi(rtl819x_swHangCnt)
+
+	lui	$3,%hi(rtl819x_swHangCnt)
+	lw	$2,%lo(rtl819x_swHangCnt)($3)
+	addiu	$2,$2,1
+	sw	$2,%lo(rtl819x_swHangCnt)($3)
+	lui	$3,%hi(rtl819x_lastRxDesc)
+	sw	$6,%lo(rtl819x_lastRxDesc)($3)
+	lui	$3,%hi(rtl819x_lastTxDesc)
+	sltu	$2,$2,3
+	bne	$2,$0,$L173
+	sw	$5,%lo(rtl819x_lastTxDesc)($3)
+
+	j	$L174
+	lui	$2,%hi(rtl819x_swHangCnt)
+
+$L171:
+	lui	$2,%hi(rtl819x_swHangCnt)
+$L175:
+	j	$L168
+	sw	$0,%lo(rtl819x_swHangCnt)($2)
+
+$L174:
+	sw	$0,%lo(rtl819x_swHangCnt)($2)
+	lui	$4,%hi($LC3)
+	jal	panic_printk
+	addiu	$4,$4,%lo($LC3)
+
+	jal	machine_restart
+	move	$4,$0
+
+	j	$L173
+	nop
+
+$L168:
+	lui	$2,%hi(rtl819x_lastRxDesc)
+	sw	$6,%lo(rtl819x_lastRxDesc)($2)
+	lui	$2,%hi(rtl819x_lastTxDesc)
+	sw	$5,%lo(rtl819x_lastTxDesc)($2)
+$L173:
+	lw	$31,20($sp)
+	j	$31
+	addiu	$sp,$sp,24
+
+	.set	macro
+	.set	reorder
+	.end	rtl819x_poll_sw
+	.size	rtl819x_poll_sw, .-rtl819x_poll_sw
+	.local	rtl819x_checkSwCoreTimer
+	.comm	rtl819x_checkSwCoreTimer,4,4
+	.local	rtl819x_swHangCnt
+	.comm	rtl819x_swHangCnt,4,4
+	.local	rtl819x_lastTxDesc
+	.comm	rtl819x_lastTxDesc,4,4
+	.local	rtl819x_lastRxDesc
+	.comm	rtl819x_lastRxDesc,4,4
 	.rdata
 	.align	2
 	.type	_rtl8651_asicTableSize, @object
diff --git a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/Makefile b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/Makefile
index 42ee7237b..c54c8b892 100644
--- a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/Makefile
+++ b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/Makefile
@@ -23,13 +23,23 @@ ifdef CONFIG_RTL_8198T
 L4_SUPPORT=1
 endif
 
-ifeq ($(CONFIG_RTL_8198),y)
-TARGET = 98
-else
-TARGET = 96C
+ifdef CONFIG_RTL_819XDT
+L4_SUPPORT=1
 endif
 
 
+ifeq ($(CONFIG_RTL_819XD),y)
+TARGET = 9xD
+endif
+ifeq ($(CONFIG_RTL_8196E),y)
+TARGET = 96E
+endif
+ifeq ($(CONFIG_RTL_8196C),y)
+TARGET = 96C
+endif
+ifeq ($(CONFIG_RTL_8198),y)
+TARGET = 98
+endif
 
 ifeq ($(CONFIG_RTK_IPTABLES_FAST_PATH),m)
 EXTRA_CFLAGS += -DCONFIG_FAST_PATH_MODULE
diff --git a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/asicTabs.h b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/asicTabs.h
index 4a811ba9f..12260a0d8 100644
--- a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/asicTabs.h
+++ b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/asicTabs.h
@@ -9,65 +9,6 @@
 *
 * $Author: davidhsu $
 *
-* $Log: not supported by cvs2svn $
-* Revision 1.2  2006/07/03 03:05:03  chenyl
-* *: convert files from DOS format to UNIX format
-*
-* Revision 1.1  2004/02/25 14:26:33  chhuang
-* *** empty log message ***
-*
-* Revision 1.1  2004/02/16 01:50:49  chhuang
-* *** empty log message ***
-*
-* Revision 1.2  2004/01/08 07:47:10  cfliu
-* update wiith new data structure definition.
-*
-* Revision 1.1  2003/12/30 14:16:23  cfliu
-* initial version
-*
-* Revision 1.1  2003/12/26 05:26:40  chhuang
-* init
-*
-* Revision 1.13  2003/12/19 01:20:13  danwu
-* little endian definitions for acl, next hop, rate limit tables
-*
-* Revision 1.12  2003/12/18 02:30:56  danwu
-* define _LITTLE_ENDIAN for Veri plateform
-*
-* Revision 1.11  2003/10/27 06:54:38  danwu
-* no message
-*
-* Revision 1.10  2003/10/21 02:53:09  danwu
-* update pktOpApp
-*
-* Revision 1.9  2003/09/23 09:55:17  danwu
-* update ACL table structure for 51B
-*
-* Revision 1.8  2003/09/03 09:22:07  danwu
-* modify for 51b
-*
-* Revision 1.7  2003/08/26 09:56:31  cfliu
-* no message
-*
-* Revision 1.6  2003/08/12 09:57:10  danwu
-* no message
-*
-* Revision 1.5  2003/08/12 06:47:18  danwu
-* to ensure backward compatibility, every table structure is extended to eight
-* words, and unused bits must be cleared before setting
-*
-* Revision 1.4  2003/08/11 14:53:20  cfliu
-* add col2 bit for Hash2 collision
-*
-* Revision 1.3  2003/07/16 06:47:43  cfliu
-* no message
-*
-* Revision 1.2  2003/06/17 12:09:53  cfliu
-* no message
-*
-* Revision 1.1  2003/06/16 03:41:01  danwu
-* init
-*
 * ---------------------------------------------------------------
 */
 
diff --git a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865x_asicCom.c b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865x_asicCom.c
index d8fcf085d..341c03e66 100644
--- a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865x_asicCom.c
+++ b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865x_asicCom.c
@@ -1,10 +1,13 @@
 /*
-* Copyright c                  Realtek Semiconductor Corporation, 2009  
-* All rights reserved.
-* 
 * Program : Switch table basic operation driver
 * Abstract :
 * Author : hyking (hyking_liu@realsil.com.cn)  
+*
+*  Copyright (c) 2011 Realtek Semiconductor Corp.
+*
+*  This program is free software; you can redistribute it and/or modify
+*  it under the terms of the GNU General Public License version 2 as
+*  published by the Free Software Foundation.
 */
 
 #include <linux/delay.h>
@@ -17,6 +20,8 @@
 #include "rtl865x_asicBasic.h"
 #include "rtl865x_asicCom.h"
 
+
+
 #define tick_Delay10ms(x) { int i=x; while(i--) __delay(5000); }
 
 static int32 _rtl865xC_lockTLUCounter	=	0;
@@ -31,12 +36,68 @@ int32		rtl8651_totalExtPortNum=0; //this replaces all RTL8651_EXTPORT_NUMBER def
 int32		rtl8651_allExtPortMask=0; //this replaces all RTL8651_EXTPORTMASK defines
 rtl8651_tblAsic_InitPara_t rtl8651_tblAsicDrvPara;
 
+#if defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
+int rtl8651_findAsicVlanIndexByVid(uint16 *vid)
+{
+	int i;
+	rtl865x_tblAsicDrv_vlanParam_t vlan;
+
+	for ( i = 0; i < RTL865XC_VLANTBL_SIZE; i++ )
+	{
+		if (rtl8651_getAsicVlan(i, &vlan) == SUCCESS){
+			if(*vid == vlan.vid){
+				*vid = i;
+				return SUCCESS;
+			}
+		}
+	}
+
+	return FAILED;
+}
+
+static int rtl8651_getAsicVlanIndex(rtl865xc_tblAsic_vlanTable_t *entry, uint16 *vid)
+{
+	int i;
+	int ret = FAILED;
+	rtl865x_tblAsicDrv_vlanParam_t vlan;
+
+	for ( i = 0; i < RTL865XC_VLANTBL_SIZE; i++ )
+	{
+		if ((rtl8651_getAsicVlan(i, &vlan) == SUCCESS) && (entry->vid == vlan.vid)){
+			if((entry->memberPort != vlan.memberPortMask) || (entry->egressUntag != vlan.untagPortMask) ||(entry->fid != vlan.fid)){
+				*vid = i;
+				return SUCCESS;
+			}else{
+				return FAILED;
+			}
+		}
+	}
+
+	for ( i = 0; i < RTL865XC_VLANTBL_SIZE; i++ )
+	{
+		if ( rtl8651_getAsicVlan( i, &vlan ) == FAILED )
+			break;
+	}
+
+	if(i == RTL865XC_VLANTBL_SIZE){
+		ret = FAILED;	//vlan table is full
+	}else{
+		*vid = i;
+		ret = SUCCESS;
+	}
+
+	return ret;
+}
+#endif
 /*=========================================
   * ASIC DRIVER API: VLAN TABLE
   *=========================================*/
 int32 rtl8651_setAsicVlan(uint16 vid, rtl865x_tblAsicDrv_vlanParam_t *vlanp)
 {
 	rtl865xc_tblAsic_vlanTable_t entry;
+#if defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
+	int flag = FAILED;
+#endif
 
 	memset(&entry,0,sizeof(entry));
 	if(vlanp == NULL)
@@ -50,6 +111,13 @@ int32 rtl8651_setAsicVlan(uint16 vid, rtl865x_tblAsicDrv_vlanParam_t *vlanp)
 	entry.memberPort = vlanp->memberPortMask & RTL8651_PHYSICALPORTMASK;
 	entry.egressUntag = vlanp->untagPortMask & RTL8651_PHYSICALPORTMASK;
 	entry.fid=vlanp->fid;
+
+#if defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
+	entry.vid=vid;
+	flag = rtl8651_getAsicVlanIndex(&entry, &vid);
+	if(flag == FAILED)
+		return FAILED;
+#endif
 	_rtl8651_forceAddAsicEntry(TYPE_VLAN_TABLE, vid, &entry);
 	
 	return SUCCESS;
@@ -58,7 +126,12 @@ int32 rtl8651_setAsicVlan(uint16 vid, rtl865x_tblAsicDrv_vlanParam_t *vlanp)
 
 int32 rtl8651_delAsicVlan(uint16 vid) {
 	rtl8651_tblAsic_vlanTable_t entry;
-
+#if defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
+	int flag = FAILED;
+	flag = rtl8651_findAsicVlanIndexByVid(&vid);
+	if(flag == FAILED)
+		return FAILED;
+#endif
 	memset(&entry,0,sizeof(entry));
 	entry.valid = 0;
 	return _rtl8651_forceAddAsicEntry(TYPE_VLAN_TABLE, vid, &entry);
@@ -78,6 +151,10 @@ int32 rtl8651_getAsicVlan(uint16 vid, rtl865x_tblAsicDrv_vlanParam_t *vlanp) {
 	vlanp->memberPortMask = (entry.extMemberPort<<RTL8651_PORT_NUMBER) | entry.memberPort;
 	vlanp->untagPortMask = (entry.extEgressUntag<<RTL8651_PORT_NUMBER) |entry.egressUntag;
 	vlanp->fid=entry.fid;
+
+#if defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
+	vlanp->vid=entry.vid;
+#endif	
 	return SUCCESS;
 }
 
@@ -185,7 +262,7 @@ int32 rtl8651_setProtocolBasedVLAN( uint32 ruleNo, uint32 port, uint8 valid, uin
 		vlanId = 0; // clear it for looking pretty.
 	}
 
-	if ( port < RTL865XC_PORT_NUMBER )
+	if ( port < RTL8651_MAC_NUMBER )
 	{
 		// Port0 ~ Port9
 		addr=PBVR0_0 +(ruleNo*5*4) + ((port/2)*4) ;
@@ -661,15 +738,6 @@ int32 rtl865xC_lockSWCore(void)
 		WRITE_MEM32(PCRP4, ((READ_MEM32(PCRP4))&(~PauseFlowControlNway)) ); /* Jumbo Frame */
 		TOGGLE_BIT_IN_REG_TWICE(PCRP4,EnForceMode);
 
-#if 0 //def CONFIG_RTL_8196C_ESD
-		{
-			extern int _96c_esd_counter;
-			if (_96c_esd_counter) {
-				_96c_esd_counter = 1;
-			}
-		}
-#endif
-
 		if (rtl8651_tblAsicDrvPara.externalPHYProperty & RTL8651_TBLASIC_EXTPHYPROPERTY_PORT5_RTL8211B)
 		{
 			WRITE_MEM32(PCRP5, ((READ_MEM32(PCRP5))&(~PauseFlowControlNway)) ); /* Jumbo Frame */
@@ -715,15 +783,6 @@ int32 rtl865xC_lockSWCore(void)
 		WRITE_MEM32(PCRP4, ((READ_MEM32(PCRP4))&(~EnablePHYIf)) ); /* Jumbo Frame */
 		TOGGLE_BIT_IN_REG_TWICE(PCRP4,EnForceMode);
 
-#ifdef CONFIG_RTL_8196C_ESD
-		{
-			extern int _96c_esd_counter;
-			if (_96c_esd_counter) {
-				_96c_esd_counter = 1;
-			}
-		}
-#endif
-
 		if (rtl8651_tblAsicDrvPara.externalPHYProperty & RTL8651_TBLASIC_EXTPHYPROPERTY_PORT5_RTL8211B)
 		{
 			WRITE_MEM32(PCRP5, ((READ_MEM32(PCRP5))&(~EnablePHYIf)) ); /* Jumbo Frame */
@@ -860,7 +919,7 @@ int32 rtl8651_getChipVersion(int8 *name,uint32 size, int32 *rev)
 int32 rtl8651_getChipNameID(int32 *id)
 {
 
-#if defined(CONFIG_RTL_8196C) || defined (CONFIG_RTL_8198)
+#if defined(CONFIG_RTL_8196C) || defined (CONFIG_RTL_8198) || defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
 	*id = RTL865X_CHIP_VER_RTL8196C;
 #elif defined(CONFIG_RTL8196B)
 	*id = RTL865X_CHIP_VER_RTL8196B;
@@ -952,7 +1011,7 @@ void rtl8651_clearRegister(void)
 	WRITE_MEM32(PBVR5_4,  0x00000000);	/* User-defined 2 */
 	WRITE_MEM32(MSCR,0);   
 	
-#if defined(CONFIG_RTL_8196C) || defined(CONFIG_RTL_8198)
+#if defined(CONFIG_RTL_8196C) || defined(CONFIG_RTL_8198) || defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
 	WRITE_MEM32(PCRP0, (1 | MacSwReset));
 	TOGGLE_BIT_IN_REG_TWICE(PCRP0,EnForceMode);
 	WRITE_MEM32(PCRP1, (1 | MacSwReset));  
@@ -1086,7 +1145,7 @@ int32 rtl8651_clearAsicCommTable(void)
 	//rtl8651_clearSpecifiedAsicTable(TYPE_L2_SWITCH_TABLE, RTL8651_L2TBL_ROW*RTL8651_L2TBL_COLUMN);
 	//rtl8651_clearSpecifiedAsicTable(TYPE_RATE_LIMIT_TABLE, RTL8651_RATELIMITTBL_SIZE);
 	rtl8651_clearSpecifiedAsicTable(TYPE_NETINTERFACE_TABLE, RTL865XC_NETINTERFACE_NUMBER);
-	rtl8651_clearSpecifiedAsicTable(TYPE_VLAN_TABLE, RTL865XC_VLAN_NUMBER);
+	rtl8651_clearSpecifiedAsicTable(TYPE_VLAN_TABLE, RTL865XC_VLANTBL_SIZE);
 	rtl8651_clearSpecifiedAsicTable(TYPE_ACL_RULE_TABLE, RTL8651_ACLTBL_SIZE);
 
 	/*
@@ -1293,12 +1352,12 @@ void rtl865x_start(void)
 	REG32(CPUICR) = TXCMD | RXCMD | BUSBURST_32WORDS | MBUF_2048BYTES;
 	REG32(CPUIISR) = REG32(CPUIISR);
 #if 1
-	REG32(CPUIIMR) = RX_DONE_IE_ALL | TX_ALL_DONE_IE_ALL | LINK_CHANGE_IE;
+	REG32(CPUIIMR) = RX_DONE_IE_ALL | TX_ALL_DONE_IE_ALL | LINK_CHANGE_IE | PKTHDR_DESC_RUNOUT_IE_ALL;
 #else
 	//REG32(CPUIIMR) = RX_DONE_IE_ALL | LINK_CHANGE_IE | PKTHDR_DESC_RUNOUT_IE_ALL | MBUF_DESC_RUNOUT_IE_ALL | TX_ALL_DONE_IE_ALL;
 	REG32(CPUIIMR) = RX_DONE_IE_ALL | LINK_CHANGE_IE | PKTHDR_DESC_RUNOUT_IE_ALL | MBUF_DESC_RUNOUT_IE_ALL;
 #endif
-	REG32(SIRR) = 1;
+	REG32(SIRR) = TRXRDY;
 	REG32(GIMR) |= (BSP_SW_IE);
 //	REG32(0xbb804508) = 0xd400f8; // adjust internal buffer threshold
 //	REG32(0xbb804908) = 0x00000400;/*mark_patch for correcting the Leaky bucket value*/	
@@ -1431,7 +1490,7 @@ static void _rtl8651_initialRead(void) {//RTL8651 read counter for the first tim
 	}
 }
 
-int32 rtl8651_returnAsicCounter(uint32 offset) 
+uint32 rtl8651_returnAsicCounter(uint32 offset) 
 {
 	if(offset & 0x3)
 		return 0;
@@ -1442,7 +1501,12 @@ uint64 rtl865xC_returnAsicCounter64(uint32 offset)
 {
 	if ( offset & 0x3 )
 		return 0;
+
+#if defined(CONFIG_RTL_8198) || defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E) || defined(CONFIG_RTL_8196C)
+	return ( READ_MEM32( MIB_COUNTER_BASE + offset ) + ( ( uint64 ) READ_MEM32( MIB_COUNTER_BASE + offset + 4 ) << 22 ) );
+#else
 	return ( READ_MEM32( MIB_COUNTER_BASE + offset ) + ( ( uint64 ) READ_MEM32( MIB_COUNTER_BASE + offset + 4 ) << 32 ) );
+#endif
 }
 
 int32 rtl8651_clearAsicCounter(void) 
@@ -1723,10 +1787,13 @@ int32 rtl865xC_dumpAsicDiagCounter(void)
 			rtlglue_printf("<Port: %d>\n", i);
 
 		rtlglue_printf("Rx counters\n");
-		rtlglue_printf("   Rcv %llu bytes, Drop %u pkts, CRCAlignErr %u, FragErr %u, JabberErr %u\n", 
+		rtlglue_printf("   Rcv %llu bytes, Drop %u pkts,etherStatsDropEvents %u\n", 
 			rtl865xC_returnAsicCounter64( OFFSET_IFINOCTETS_P0 + addrOffset_fromP0 ), 
 			rtl8651_returnAsicCounter( OFFSET_DOT1DTPPORTINDISCARDS_P0 + addrOffset_fromP0 ), 
+			rtl8651_returnAsicCounter(OFFSET_ETHERSTATSDROPEVENTS_P0 + addrOffset_fromP0 ));
+		rtlglue_printf("   CRCAlignErr %u, SymbolErr %u, FragErr %u, JabberErr %u\n",
 			rtl8651_returnAsicCounter( OFFSET_DOT3STATSFCSERRORS_P0 + addrOffset_fromP0 ), 
+			rtl8651_returnAsicCounter( OFFSET_DOT3STATSSYMBOLERRORS_P0 + addrOffset_fromP0 ), 
 			rtl8651_returnAsicCounter( OFFSET_ETHERSTATSFRAGMEMTS_P0 + addrOffset_fromP0 ), 
 			rtl8651_returnAsicCounter( OFFSET_ETHERSTATSJABBERS_P0 + addrOffset_fromP0 ));
 		rtlglue_printf("   Unicast %u pkts, Multicast %u pkts, Broadcast %u pkts\n", 
@@ -1935,23 +2002,38 @@ int32 rtl8651_getAsicCounter(uint32 counterIdx, rtl865x_tblAsicDrv_basicCounterP
 //sync from rtl865x kernel 2.4
 void FullAndSemiReset( void )
 {
-#if 1//#ifdef CONFIG_RTL865XC
+#if defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
+	REG32(SIRR) |= FULL_RST;
+	mdelay(300);
 
-	/* FIXME: Currently workable for FPGA, may need further modification for real chip */
+	REG32(SYS_CLK_MAG) |= CM_PROTECT;
+	REG32(SYS_CLK_MAG) &= ~CM_ACTIVE_SWCORE;
+	mdelay(300);
+	
+	REG32(SYS_CLK_MAG) |= CM_ACTIVE_SWCORE;
+	REG32(SYS_CLK_MAG) &= ~CM_PROTECT;
+	mdelay(50);
 
-	/* Perform full-reset for sw-core. */ 
-	#ifdef CONFIG_RTL8196B_TLD
-	if ((REG32(CPUSSR) & 0x0000ffff) != _MAGIC_FORM_BOOT) 
-	#endif
+#elif defined(CONFIG_RTL8198_REVISION_B) 
+ 	if (REG32(BSP_REVR) >= BSP_RTL8198_REVISION_B)
 	{
-
+	  	REG32(SYS_CLK_MAG)&=(~(SYS_SW_RESET));
+		mdelay(300);
+		REG32(SYS_CLK_MAG)|=(SYS_SW_RESET);
+		mdelay(50);
+	}
+	else {
 		REG32(SIRR) |= FULL_RST;
 		tick_Delay10ms(50);
 	}
+#else
+	/* Perform full-reset for sw-core. */ 
+	REG32(SIRR) |= FULL_RST;
+	tick_Delay10ms(50);
+#endif
 
+	// 08-15-2012, set TRXRDY bit in rtl865x_start() in rtl865x_asicCom.c
 	/* Enable TRXRDY */
-	REG32(SIRR) |= TRXRDY;
-
-#endif /* CONFIG_RTL865XC */
+	//REG32(SIRR) |= TRXRDY;
 }
 
diff --git a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865x_asicCom.h b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865x_asicCom.h
index 083ae249e..545f2311a 100644
--- a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865x_asicCom.h
+++ b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865x_asicCom.h
@@ -10,6 +10,12 @@
 #ifndef RTL865X_ASICCOM_H
 #define RTL865X_ASICCOM_H
 
+#if defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
+#define RTL865XC_VLANTBL_SIZE				16
+#else
+#define RTL865XC_VLANTBL_SIZE				4096
+#endif
+
 #define RTL865XC_NETIFTBL_SIZE			8
 #define RTL8651_ACLTBL_SIZE			125
 #define RTL8651_ACLHWTBL_SIZE			128
@@ -224,7 +230,11 @@ typedef struct {
 typedef struct {
 #ifndef _LITTLE_ENDIAN
 	 /* word 0 */
+#if defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
+	uint32	vid:12;
+#else
 	uint32	reserved1:12;
+#endif
 	uint32	fid:2;
 	uint32     extEgressUntag  : 3;
 	uint32     egressUntag : 6;
@@ -608,6 +618,9 @@ typedef struct rtl865x_tblAsicDrv_vlanParam_s {
 	uint32 	memberPortMask; /*extension ports [rtl8651_totalExtPortNum-1:0] are located at bits [RTL8651_PORT_NUMBER+rtl8651_totalExtPortNum-1:RTL8651_PORT_NUMBER]*/
 	uint32 	untagPortMask; /*extension ports [rtl8651_totalExtPortNum-1:0] are located at bits [RTL8651_PORT_NUMBER+rtl8651_totalExtPortNum-1:RTL8651_PORT_NUMBER]*/
 	uint32  fid:2;
+#if defined(CONFIG_RTL_819XD)  || defined(CONFIG_RTL_8196E)
+	uint32  vid:12;
+#endif
 } rtl865x_tblAsicDrv_vlanParam_t;
 
 typedef struct rtl865x_tblAsicDrv_intfParam_s {
@@ -700,6 +713,9 @@ extern rtl8651_tblAsic_InitPara_t rtl8651_tblAsicDrvPara;
 
 int32 rtl8651_clearAsicCommTable(void);
 /*vlan*/
+#if defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
+int rtl8651_findAsicVlanIndexByVid(uint16 *vid);
+#endif
 int32 rtl8651_setAsicVlan(uint16 vid, rtl865x_tblAsicDrv_vlanParam_t *vlanp);
 int32 rtl8651_getAsicVlan(uint16 vid, rtl865x_tblAsicDrv_vlanParam_t *vlanp);
 int32 rtl8651_delAsicVlan(uint16 vid);
@@ -744,7 +760,7 @@ void rtl865x_start(void);
 void rtl865x_down(void);
 
 /*counter*/
-int32 rtl8651_returnAsicCounter(uint32 offset);
+uint32 rtl8651_returnAsicCounter(uint32 offset);
 int32 rtl8651_clearAsicCounter(void);
 int32 rtl865xC_dumpAsicDiagCounter(void);
 int32 rtl865xC_dumpAsicCounter(void);
diff --git a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865x_asicL2.c b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865x_asicL2.c
index 2b6b9e9d3..ebfb74abd 100644
--- a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865x_asicL2.c
+++ b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865x_asicL2.c
@@ -1,11 +1,11 @@
 /*
-* Copyright c                  Realtek Semiconductor Corporation, 2009  
+* Copyright c                  Realtek Semiconductor Corporation, 2009
 * All rights reserved.
-* 
+*
 * Program : Switch table Layer2 switch driver,following features are included:
 *	PHY/MII/Port/STP/QOS
 * Abstract :
-* Author : hyking (hyking_liu@realsil.com.cn)  
+* Author : hyking (hyking_liu@realsil.com.cn)
 */
 #include <net/rtl/rtl_types.h>
 #include <net/rtl/rtl_glue.h>
@@ -19,10 +19,6 @@
 
 #include <linux/delay.h>
 
-#if defined(CONFIG_RTL_8198)
-#define RTL8198_EEE_MAC 	1
-#endif
-
 static uint8 fidHashTable[]={0x00,0x0f,0xf0,0xff};
 
 __DRAM_FWD int32		rtl865x_wanPortMask;
@@ -75,7 +71,7 @@ void eee_phy_disable_98(void);
 int eee_enabled = 0;
 #endif
 
-#if defined(CONFIG_RTL_8198_NFBI_BOARD) 
+#if defined(CONFIG_RTL_8198_NFBI_BOARD)
 #define RTL8198_NFBI_PORT5_GMII 1 //mark_nfbi , default port5 set to GMII , you can undef here to set to MII mode!!!
 //#undef RTL8198_NFBI_PORT5_GMII  //mark_nfbi , default port5 set to GMII , you can undef here to set to MII mode!!!
 #endif
@@ -92,6 +88,7 @@ int eee_enabled = 0;
 #define	QNUM_IDX_45		1
 #define	QNUM_IDX_6		2
 
+#if !defined(CONFIG_RTL_819XD	) && !defined(CONFIG_RTL_8196E)
 static int32 _rtl865x_setQosThresholdByQueueIdx(uint32 qidx);
 
 #if 0
@@ -192,6 +189,7 @@ static rtl865xC_outputQueuePara_t	outputQueuePara[3] = {
 										}
 										};
 #endif
+#endif
 
 static void _rtl8651_syncToAsicEthernetBandwidthControl(void);
 #if defined(CONFIG_RTL_HW_QOS_SUPPORT)
@@ -282,7 +280,7 @@ static const unsigned short phy_data[]={
 };
 
 static int ram_code_done=0;
- 
+
 void set_ram_code(void)
 {
 	uint32 reg;
@@ -290,9 +288,9 @@ void set_ram_code(void)
 
 	if (ram_code_done)
 		return;
-	
+
 	rtl8651_getAsicEthernetPHYReg( 4, 0x19, &reg );
-	
+
 	// turn on rg_eeeprg_rst
 	rtl8651_setAsicEthernetPHYReg(4, 0x19, ((reg & ~(P4R25_rg_eeeprg_en)) | P4R25_rg_eeeprg_rst));
 
@@ -303,11 +301,11 @@ void set_ram_code(void)
 	for(i=0;i<len;i++) {
 		rtl8651_setAsicEthernetPHYReg(4, 0x1d, phy_data[i]);
 	}
-	
+
 	for(i=0;i<63;i++) {
 		rtl8651_setAsicEthernetPHYReg(4, 0x1d, 0);
-	}	
-	
+	}
+
 	// finish reading all RAM
 	// turn off mem_mdio_mode
 	rtl8651_setAsicEthernetPHYReg(4, 0x1c, 0x0080);
@@ -399,13 +397,13 @@ void set_ram_code_b(void)
 		return;
 
 	rtl8651_getAsicEthernetPHYReg(4, 0x19, &reg );
-	rtl8651_setAsicEthernetPHYReg(4, 0x19, ((reg & ~(P4R25_rg_eeeprg_en)) | P4R25_rg_eeeprg_rst));	
+	rtl8651_setAsicEthernetPHYReg(4, 0x19, ((reg & ~(P4R25_rg_eeeprg_en)) | P4R25_rg_eeeprg_rst));
 	rtl8651_setAsicEthernetPHYReg(4, 0x1c, 0x0180);
 
 	for(i=0;i<len;i++) {
 		rtl8651_setAsicEthernetPHYReg(4, 0x1d, phy_data_b[i]);
 	}
-	
+
 	rtl8651_setAsicEthernetPHYReg(4, 0x1c, 0x0080);
 	rtl8651_setAsicEthernetPHYReg(4, 0x19, ((reg & ~(P4R25_rg_eeeprg_rst)) | P4R25_rg_eeeprg_en));
 
@@ -423,7 +421,7 @@ void eee_phy_enable_by_port(int port)
 	rtl8651_getAsicEthernetPHYReg( port, 16, &reg );
 	reg |= (P4R16_eee_nway_en | P4R16_tx_quiet_en | P4R16_rx_quiet_en);
 
-#ifdef CONFIG_RTL8196C_ETH_IOT	
+#ifdef CONFIG_RTL8196C_ETH_IOT
 	reg |= P4R16_eee_10_cap;	// enable 10M_EEE also.
 #endif
 	rtl8651_setAsicEthernetPHYReg( port, 16, reg );
@@ -432,7 +430,7 @@ void eee_phy_enable_by_port(int port)
 	rtl8651_getAsicEthernetPHYReg( port, 25, &reg );
 //	reg = reg & 0xF9FF | P4R25_rg_dacquiet_en | P4R25_rg_ldvquiet_en;
 	reg |= (P4R25_rg_dacquiet_en | P4R25_rg_ldvquiet_en | P4R25_rg_eeeprg_en);
-	
+
 	rtl8651_setAsicEthernetPHYReg( port, 25, reg );
 
 	rtl8651_setAsicEthernetPHYReg( port, 17, 0xa2a2 );
@@ -445,7 +443,7 @@ void eee_phy_enable_by_port(int port)
 	else if ((REG32(REVR) == RTL8196C_REVISION_B) && (port == 4)) {
 		set_ram_code_b();
 	}
-	
+
 	// switch to page 0
 	rtl8651_setAsicEthernetPHYReg(port, 31, 0 );
 }
@@ -509,11 +507,11 @@ void eee_phy_disable(void)
 		// switch to page 0
 		rtl8651_setAsicEthernetPHYReg(i, 31, 0 );
 
-		rtl8651_restartAsicEthernetPHYNway(i+1);				
+		rtl8651_restartAsicEthernetPHYNway(i+1);
 	}
 
 	// EEE MAC disable
-	
+
 }
 #endif
 
@@ -525,9 +523,9 @@ void eee_phy_enable_98(void)
 {
 	int i;
 
-	for(i=0; i<5; i++)        
+	for(i=0; i<5; i++)
 		REG32(PCRP0+i*4) |= (EnForceMode);
-	
+
 	// EEE PHY enable
 	for (i=0; i<5; i++)
 	{
@@ -536,19 +534,23 @@ void eee_phy_enable_98(void)
 		Set_GPHYWB(i,32,21,0xffff - 0xffff,0x0100);
 		Set_GPHYWB(i,5,5,0xffff - 0xffff,0x8b84);
 		Set_GPHYWB(i,5,6,0xffff - 0xffff,0x0062);
-		
+
+		/* enable "EEE auto off" for JMicron's bug */
+		Set_GPHYWB(i,5,5,0,0x857a);
+		Set_GPHYWB(i,5,6,0,0x0770);
+
 		rtl8651_restartAsicEthernetPHYNway(i+1);
 	}
-	
-	for(i=0; i<5; i++)            
-		REG32(PCRP0+i*4) &= ~(EnForceMode);
+
+//	for(i=0; i<5; i++)
+//		REG32(PCRP0+i*4) &= ~(EnForceMode);
 }
 
 void eee_phy_disable_98(void)
 {
 	int i;
-	
-	for(i=0; i<5; i++)        
+
+	for(i=0; i<5; i++)
 		REG32(PCRP0+i*4) |= (EnForceMode);
 
 	for (i=0; i<5; i++)
@@ -558,22 +560,114 @@ void eee_phy_disable_98(void)
 		Set_GPHYWB(i,32,21,0xffff - 0xffff,0x0);
 		Set_GPHYWB(i,5,5,0xffff - 0xffff,0x8b84);
 		Set_GPHYWB(i,5,6,0xffff - 0xffff,0x0042);
-		
+
 		rtl8651_restartAsicEthernetPHYNway(i+1);
 	}
 	// EEE PHY disable
 
-	for(i=0; i<5; i++)            
-		REG32(PCRP0+i*4) &= ~(EnForceMode);	
+//	for(i=0; i<5; i++)
+//		REG32(PCRP0+i*4) &= ~(EnForceMode);
+}
+#endif
+
+#if defined(CONFIG_RTL_8198)
+int rtl8198_power_saving_config(uint32 mode)
+{
+	unsigned long flags;
+	int i, _8198_ALDPS, _8198_green_eth;
+
+	if(mode == 0)
+	{
+		// 8198 green ethernet / EEE / ALDPS off
+		eee_enabled = 0;
+		_8198_green_eth = 0;
+		_8198_ALDPS = 0;
+	}
+	else if(mode == 1)
+	{
+		// 8198 green ethernet / EEE / ALDPS on
+		eee_enabled = 1;
+		_8198_green_eth = 1;
+		_8198_ALDPS = 1;
+	}
+	else if(mode == 2)
+	{
+		// 8198 green ethernet on / EEE off
+		eee_enabled = 0;
+		_8198_green_eth = 1;
+		_8198_ALDPS = 1;
+	}
+	else if(mode == 3)
+	{
+		// 8198 green ethernet off / EEE on
+		eee_enabled = 1;
+		_8198_green_eth = 0;
+		_8198_ALDPS = 1;
+	}
+	else {
+		return (-1);
+	}
+
+	local_irq_save(flags);
+
+	for(i=0; i<5; i++)
+		REG32(PCRP0+i*4) |= (EnForceMode);
+
+	/*
+		8198 ALDPS feature is on by default.
+		It is in PHY page 44, reg 21, bit 0: 1 enable, 0 disable
+	*/
+	if (_8198_ALDPS) {
+		Set_GPHYWB(999, 44, 21, 0xfffe, 1);
+	}
+	else {
+		Set_GPHYWB(999, 44, 21, 0xfffe, 0);
+	}
+
+	/*
+		write Page 5 Reg 5 = 0x85E4
+		read Page 5 Reg 6; #bit0 = 1, for enable green Rx
+		write Page 5 Reg 5 = 0x85E7
+		read Page 5 Reg 6; #bit0 = 1, for enable green Tx
+
+	*/
+	if (_8198_green_eth) {
+		Set_GPHYWB(999, 5, 5, 0, 0x85e4);
+		Set_GPHYWB(999, 5, 6, 0xfffe, 1);
+
+		Set_GPHYWB(999, 5, 5, 0, 0x85e7);
+		Set_GPHYWB(999, 5, 6, 0xfffe, 1);
+	}
+	else {
+		Set_GPHYWB(999, 5, 5, 0, 0x85e4);
+		Set_GPHYWB(999, 5, 6, 0xfffe, 0);
+
+		Set_GPHYWB(999, 5, 5, 0, 0x85e7);
+		Set_GPHYWB(999, 5, 6, 0xfffe, 0);
+	}
+
+	if (eee_enabled) {
+		eee_phy_enable_98();
+	}
+	else {
+		eee_phy_disable_98();
+	}
+
+	for(i=0; i<5; i++)
+		REG32(PCRP0+i*4) &= ~(EnForceMode);
+
+	local_irq_restore(flags);
+
+	return 0;
 }
 #endif
 
 #ifdef CONFIG_RTL8196C_GREEN_ETHERNET
 void set_phy_pwr_save(int id, int val)
 {
-	uint32 reg_val;	
+	uint32 reg_val;
 	int i, start, end;
-	
+
 	if (id == 99)
 		{ start=0; end=4; }
 	else if (id <= 4)
@@ -586,20 +680,37 @@ void set_phy_pwr_save(int id, int val)
 
 		if (val == 1)
 			rtl8651_setAsicEthernetPHYReg( i, 24, (reg_val | BIT(15)) );
-		else 
+		else
 			rtl8651_setAsicEthernetPHYReg( i, 24, (reg_val & (~BIT(15))) );
 	}
 }
 #endif
 
-uint32 rtl8651_filterDbIndex(ether_addr_t * macAddr,uint16 fid) 
+int32 mmd_read(uint32 phyId, uint32 devId, uint32 regId, uint32 *rData)
+{
+	rtl8651_setAsicEthernetPHYReg( phyId, 13, devId);
+	rtl8651_setAsicEthernetPHYReg( phyId, 14, regId);
+	rtl8651_setAsicEthernetPHYReg( phyId, 13, (devId | 0x4000));
+	return (rtl8651_getAsicEthernetPHYReg(phyId, 14, rData));
+}
+
+int32 mmd_write(uint32 phyId, uint32 devId, uint32 regId, uint32 wData)
+{
+	rtl8651_setAsicEthernetPHYReg( phyId, 13, devId);
+	rtl8651_setAsicEthernetPHYReg( phyId, 14, regId);
+	rtl8651_setAsicEthernetPHYReg( phyId, 13, (devId | 0x4000));
+	rtl8651_setAsicEthernetPHYReg( phyId, 14, wData);
+	return SUCCESS;
+}
+
+uint32 rtl8651_filterDbIndex(ether_addr_t * macAddr,uint16 fid)
 {
     return ( macAddr->octet[0] ^ macAddr->octet[1] ^
                     macAddr->octet[2] ^ macAddr->octet[3] ^
                     macAddr->octet[4] ^ macAddr->octet[5] ^fidHashTable[fid]) & 0xFF;
 }
 
-int32 rtl8651_setAsicL2Table(uint32 row, uint32 column, rtl865x_tblAsicDrv_l2Param_t *l2p) 
+int32 rtl8651_setAsicL2Table(uint32 row, uint32 column, rtl865x_tblAsicDrv_l2Param_t *l2p)
 {
 	rtl865xc_tblAsic_l2Table_t entry;
 
@@ -616,7 +727,7 @@ int32 rtl8651_setAsicL2Table(uint32 row, uint32 column, rtl865x_tblAsicDrv_l2Par
 
 #if 1 //chhuang: #ifdef CONFIG_RTL8650B
 	if( l2p->memberPortMask  > RTL8651_PHYSICALPORTMASK) //this MAC is on extension port
-		entry.extMemberPort = (l2p->memberPortMask >>RTL8651_PORT_NUMBER);   
+		entry.extMemberPort = (l2p->memberPortMask >>RTL8651_PORT_NUMBER);
 #endif /* CONFIG_RTL8650B */
 
 	entry.memberPort = l2p->memberPortMask & RTL8651_PHYSICALPORTMASK;
@@ -626,14 +737,14 @@ int32 rtl8651_setAsicL2Table(uint32 row, uint32 column, rtl865x_tblAsicDrv_l2Par
 
 	/* RTL865xC: modification of age from ( 2 -> 3 -> 1 -> 0 ) to ( 3 -> 2 -> 1 -> 0 ). modification of granularity 100 sec to 150 sec. */
 	entry.agingTime = ( l2p->ageSec > 300 )? 0x03: ( l2p->ageSec <= 300 && l2p->ageSec > 150 )? 0x02: (l2p->ageSec <= 150 && l2p->ageSec > 0 )? 0x01: 0x00;
-	
+
 	entry.srcBlock = (l2p->srcBlk==TRUE)? 1: 0;
 	entry.fid=l2p->fid;
 	entry.auth=l2p->auth;
 	return _rtl8651_forceAddAsicEntry(TYPE_L2_SWITCH_TABLE, row<<2 | column, &entry);
 }
 
-int32 rtl8651_delAsicL2Table(uint32 row, uint32 column) 
+int32 rtl8651_delAsicL2Table(uint32 row, uint32 column)
 {
 	rtl865xc_tblAsic_l2Table_t entry;
 
@@ -651,7 +762,7 @@ unsigned int rtl8651_asicL2DAlookup(uint8 *dmac){
 	uint32 column;
 //	rtl8651_tblAsic_l2Table_t   entry;
 	rtl865xc_tblAsic_l2Table_t	entry;
-	
+
 //	unsigned int row = dmac[0]^dmac[1]^dmac[2]^dmac[3]^dmac[4]^dmac[5];
 	uint32 row = rtl8651_filterDbIndex((ether_addr_t *)dmac, 0);
 	//rtlglue_printf("mac %02x %02x %02x %02x %02x %02x \n",	mac[0],mac[1],mac[2],mac[3],mac[4],mac[5]);
@@ -691,7 +802,7 @@ unsigned int rtl8651_asicL2DAlookup(uint8 *dmac){
 
 int32 rtl8651_getAsicL2Table(uint32 row, uint32 column, rtl865x_tblAsicDrv_l2Param_t *l2p) {
 	rtl865xc_tblAsic_l2Table_t   entry;
- 
+
 	if((row >= RTL8651_L2TBL_ROW) || (column >= RTL8651_L2TBL_COLUMN) || (l2p == NULL))
 		return FAILED;
 
@@ -731,7 +842,7 @@ int32 rtl8651_setAsicPortMirror(uint32 mRxMask, uint32 mTxMask,uint32 mPortMask)
 	pmcr = ((mTxMask<<MirrorTxPrtMsk_OFFSET)&MirrorTxPrtMsk_MASK) |
 		((mRxMask << MirrorRxPrtMsk_OFFSET) & MirrorRxPrtMsk_MASK)|
 		((mPortMask<<MirrorPortMsk_OFFSET) & MirrorPortMsk_MASK);
-	
+
 	WRITE_MEM32(PMCR,pmcr);
 
 	return SUCCESS;
@@ -755,7 +866,7 @@ int32 rtl8651_getAsicPortMirror(uint32 *mRxMask, uint32 *mTxMask, uint32 *mPortM
 	{
 		*mTxMask = ( pmcr & MirrorTxPrtMsk_MASK ) >> MirrorTxPrtMsk_OFFSET;
 	}
-	
+
 	return SUCCESS;
 }
 
@@ -766,7 +877,7 @@ int32 rtl8651_clearAsicL2Table(void)
 	return SUCCESS;
 }
 
-inline int32 convert_setAsicL2Table(uint32 row, uint32 column, ether_addr_t * mac, int8 cpu, 
+inline int32 convert_setAsicL2Table(uint32 row, uint32 column, ether_addr_t * mac, int8 cpu,
 		int8 srcBlk, uint32 mbr, uint32 ageSec, int8 isStatic, int8 nhFlag,int8 fid, int8 auth)
 {
 	rtl865x_tblAsicDrv_l2Param_t l2;
@@ -775,15 +886,15 @@ inline int32 convert_setAsicL2Table(uint32 row, uint32 column, ether_addr_t * ma
 
 	l2.ageSec				= ageSec;
 	l2.cpu				= cpu;
-	l2.isStatic				= isStatic; 
+	l2.isStatic				= isStatic;
 	l2.memberPortMask		= mbr;
 	l2.nhFlag				= nhFlag;
 	l2.srcBlk				= srcBlk;
 //#ifdef RTL865XC_LAN_PORT_NUM_RESTRIT
 //	if(enable4LanPortNumRestrict == TRUE)
-	l2.fid=fid; 
+	l2.fid=fid;
 	l2.auth = auth;
-//#endif	
+//#endif
 	memcpy(&l2.macAddr, mac, 6);
 	return rtl8651_setAsicL2Table(row, column, &l2);
 }
@@ -791,21 +902,21 @@ inline int32 convert_setAsicL2Table(uint32 row, uint32 column, ether_addr_t * ma
 /*
  * <<RTL8651 version B Bug>>
  * RTL8651 L2 entry bug:
- *		For each L2 entry added by driver table as a static entry, the aging time 
+ *		For each L2 entry added by driver table as a static entry, the aging time
  *		will not be updated by ASIC
  * Bug fixed:
- *		To patch this bug, set the entry is a dynamic entry and turn on the 'nhFlag', 
+ *		To patch this bug, set the entry is a dynamic entry and turn on the 'nhFlag',
  *		then the aging time of this entry will be updated and once aging time expired,
  *		it won't be removed by ASIC automatically.
  */
-int32 rtl8651_setAsicL2Table_Patch(uint32 row, uint32 column, ether_addr_t * mac, int8 cpu, 
-		int8 srcBlk, uint32 mbr, uint32 ageSec, int8 isStatic, int8 nhFlag, int8 fid,int8 auth) 
+int32 rtl8651_setAsicL2Table_Patch(uint32 row, uint32 column, ether_addr_t * mac, int8 cpu,
+		int8 srcBlk, uint32 mbr, uint32 ageSec, int8 isStatic, int8 nhFlag, int8 fid,int8 auth)
 {
-#if 0	
+#if 0
 	ether_addr_t bcast_mac = { {0xff, 0xff, 0xff, 0xff, 0xff, 0xff} };
 	ether_addr_t cpu_mac = {{0x00,0x00,0x0a,0x00,0x00,0x0f}};
 
-	/* 
+	/*
 		In RTL865xC, we need to turn on the CPU bit of broadcast mac to let broadcast packets being trapped to CPU.
 	*/
 
@@ -840,7 +951,7 @@ int32 rtl8651_setAsicL2Table_Patch(uint32 row, uint32 column, ether_addr_t * mac
 				fid,
 				TRUE
 		);
-	}	
+	}
 	else {
 		int8 dStatic=isStatic/*, dnhFlag=(isStatic==TRUE? TRUE: FALSE)*/;
 		int8 dnhFlag = nhFlag;
@@ -851,7 +962,7 @@ int32 rtl8651_setAsicL2Table_Patch(uint32 row, uint32 column, ether_addr_t * mac
 			dStatic = TRUE;
 			dnhFlag = FALSE;
 		}
-#endif		
+#endif
 		return convert_setAsicL2Table(
 				row,
 				column,
@@ -874,16 +985,16 @@ int32 rtl8651_setAsicL2Table_Patch(uint32 row, uint32 column, ether_addr_t * mac
 /*
  * <<RTL8651 version B Bug>>
  * RTL8651 L2 entry bug:
- *		For each L2 entry added by driver table as a static entry, the aging time 
+ *		For each L2 entry added by driver table as a static entry, the aging time
  *		will not be updated by ASIC
  * Bug fixed:
- *		To patch this bug, set the entry as a dynamic entry and turn on the 'nhFlag', 
+ *		To patch this bug, set the entry as a dynamic entry and turn on the 'nhFlag',
  *		then the aging time of this entry will be updated and once aging time expired,
  *		it won't be removed by ASIC automatically.
  */
 #if 0
-int32 rtl8651_getAsicL2Table_Patch(uint32 row, uint32 column, ether_addr_t * mac, int8 * cpu, 
-	int8 * srcBlk, int8 * isStatic, uint32 * mbr, uint32 * ageSec, int8 *nhFlag) 
+int32 rtl8651_getAsicL2Table_Patch(uint32 row, uint32 column, ether_addr_t * mac, int8 * cpu,
+	int8 * srcBlk, int8 * isStatic, uint32 * mbr, uint32 * ageSec, int8 *nhFlag)
 {
 	rtl865x_tblAsicDrv_l2Param_t l2;
 
@@ -955,7 +1066,7 @@ static inline unsigned int rtl865x_probeP5GigaPHYChip(void)
 	/* Read */
 	rtl8651_getAsicEthernetPHYReg( CONFIG_EXTRTL8212_PHYID_P5, 0, &tmp );
 	rtl8651_setAsicEthernetPHYReg(CONFIG_EXTRTL8212_PHYID_P5,0x10,0x01FE);
-	
+
 	/* Read */
 	rtl8651_getAsicEthernetPHYReg( CONFIG_EXTRTL8212_PHYID_P5, 2, &tmp );
 	uid=tmp<<16;
@@ -965,7 +1076,7 @@ static inline unsigned int rtl865x_probeP5GigaPHYChip(void)
 	if( uid==0x001CC912 )  //0x001cc912 is 8212 two giga port , 0x001cc940 is 8214 four giga port
 	{	//printk("Find Port5   have 8211 PHY Chip! \r\n");
 		return 1;
-	}	
+	}
 
 	return 0;
 }
@@ -1115,13 +1226,13 @@ static int32 _rtl8651_initAsicPara( rtl8651_tblAsic_InitPara_t *para )
 	return SUCCESS;
 }
 
-#if defined(CONFIG_RTL8196C_REVISION_B) || defined(CONFIG_RTL_8198)
+#if defined(CONFIG_RTL8196C_REVISION_B) || defined(CONFIG_RTL_8198) || defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
 void Set_GPHYWB(unsigned int phyid, unsigned int page, unsigned int reg, unsigned int mask, unsigned int val)
 {
 	unsigned int data=0;
 	unsigned int wphyid=0;	//start
 	unsigned int wphyid_end=1;   //end
-	
+
 	if(phyid==999)
 	{	wphyid=0;
 		wphyid_end=5;    //total phyid=0~4
@@ -1133,7 +1244,7 @@ void Set_GPHYWB(unsigned int phyid, unsigned int page, unsigned int reg, unsigne
 
 	for(; wphyid<wphyid_end; wphyid++)
 	{
-		//change page 
+		//change page
 		if(page>=31)
 		{	rtl8651_setAsicEthernetPHYReg( wphyid, 31, 7  );
 			rtl8651_setAsicEthernetPHYReg( wphyid, 30, page  );
@@ -1163,20 +1274,20 @@ void Set_GPHYWB(unsigned int phyid, unsigned int page, unsigned int reg, unsigne
 void set_gray_code_by_port(int port)
 {
         uint32 val;
- 
+
         rtl8651_setAsicEthernetPHYReg( 4, 31, 1  );
- 
+
         rtl8651_getAsicEthernetPHYReg( 4, 20, &val  );
         rtl8651_setAsicEthernetPHYReg( 4, 20, val + (0x1 << port)  );
- 
+
         rtl8651_setAsicEthernetPHYReg( port, 31, 1  );
- 
+
         rtl8651_setAsicEthernetPHYReg( port, 19,  0x5400 );
         if (port<4) rtl8651_setAsicEthernetPHYReg( port, 19,  0x5440 );
         if (port<3) rtl8651_setAsicEthernetPHYReg( port, 19,  0x54c0 );
         if (port<2) rtl8651_setAsicEthernetPHYReg( port, 19,  0x5480 );
         if (port<1) rtl8651_setAsicEthernetPHYReg( port, 19,  0x5580 );
- 
+
         rtl8651_setAsicEthernetPHYReg( 4, 20, 0xb20  );
        rtl8651_setAsicEthernetPHYReg( port, 31, 0  );
 
@@ -1193,54 +1304,54 @@ void Setting_RTL8196C_PHY(void)
 	/*
 	  #=========ADC Bias Current =========================
 	  #RG1X_P4~0 [12:10] = Reg_pi_fix [2:0], 5 ->7
-	  phywb all 1 17 12-10 0x7      
+	  phywb all 1 17 12-10 0x7
 	*/
 	Set_GPHYWB(999, 1, 17, 0xffff-(7<<10), 0x7<<10);
 
 	/*
 	  #=========patch for eee============================
-	  #1. page4¡Breg24¡Glpi_rx_ti_timer_cnt change to f3     
+	  #1. page4¡Breg24¡Glpi_rx_ti_timer_cnt change to f3
 	  phywb all 4 24 7-0 0xf3
 
-	  #2. page4¡Breg16¡Grg_txqt_ps_sel change to 1    
-	  phywb all 4 16 3 1  
+	  #2. page4¡Breg16¡Grg_txqt_ps_sel change to 1
+	  phywb all 4 16 3 1
 	*/
 	Set_GPHYWB(999, 4, 24, 0xff00, 0xf3);
 	Set_GPHYWB(999, 4, 16, 0xffff-(1<<3), 1<<3);
-	/*	
-	  #=========patch for IOL Tx amp.=====================      
+	/*
+	  #=========patch for IOL Tx amp.=====================
 	  #<a>modify 100M DAC current default value:
 	  #Port#0~#4(per port control)
-	  #Page1,Reg19,bit[13:11]: 
+	  #Page1,Reg19,bit[13:11]:
 	  #original value 200uA(3b'100),--> change to 205u(3b'000)   => change to 205u(3b010)
 
 	  phywb all 1 19 13-11 0x2
 
-	  #<b>modify bandgap voltage default value:  
+	  #<b>modify bandgap voltage default value:
 	  #Port#0~#4 (Global all ports contorl setting),
-	  #Page1,Reg23,bit[8:6],                            
+	  #Page1,Reg23,bit[8:6],
 
-	  #original value 1.312V(3b'110),-->change to 1.212V(3b'100).       
+	  #original value 1.312V(3b'110),-->change to 1.212V(3b'100).
 
 	  phywb all 1 23 8-6 0x4
 
-	  #<c>modify TX CS cap default value:                                   
+	  #<c>modify TX CS cap default value:
 	  #Port#0~#4 (Global all ports contorl setting),
-	  #Page1,Reg18,bit[5:3],                                               
+	  #Page1,Reg18,bit[5:3],
 
-	  #original value Reserved bits(3b'000),-->change to 600fF(3b'011). =>change to 750fF(3b'110) 
+	  #original value Reserved bits(3b'000),-->change to 600fF(3b'011). =>change to 750fF(3b'110)
 	  phywb all 1 18 5-3 0x6
 	*/
 
 	Set_GPHYWB(999, 1, 19, 0xffff-(7<<11), 0x2<<11);
 	Set_GPHYWB(999, 1, 23, 0xffff-(7<<6)  , 0x4<<6);
 	Set_GPHYWB(999, 1, 18, 0xffff-(7<<3), 0x6<<3);
-	
+
 
 	/* 20100223 from Maxod: 100M half duplex enhancement */
  	REG32(MACCR)= (REG32(MACCR) & ~CF_RXIPG_MASK) | 0x05;
 
-	/* fix the link down / link up issue with SmartBit 3101B when DUT(8196c) set to Auto-negotiation 
+	/* fix the link down / link up issue with SmartBit 3101B when DUT(8196c) set to Auto-negotiation
 	    and SmartBit force to 100M Full-duplex */
  	REG32(MACCR)= (REG32(MACCR) & ~SELIPG_MASK) | SELIPG_11;
 
@@ -1257,7 +1368,7 @@ void Setting_RTL8196C_PHY(void)
 	*/
 	//rtl8651_setAsicEthernetPHYReg( i, 0x15, 0x232 );
 	//Set_GPHYWB(999, 0, 21, 0xffff-(0xff<<0), 0x32<<0);
-	// test 96C to 96C restart AN 100 times, result is pass ==> page0 reg21.14(disable the equlizar)=1      
+	// test 96C to 96C restart AN 100 times, result is pass ==> page0 reg21.14(disable the equlizar)=1
 #ifdef CONFIG_RTL8196C_ETH_IOT
 	// enable "equalizer reset", i.e. page 0 , reg21, bit14= 0
 	Set_GPHYWB(999, 0, 21, (~0x40ff), 0x0032);
@@ -1268,17 +1379,17 @@ void Setting_RTL8196C_PHY(void)
 	//rtl8651_setAsicEthernetPHYReg( i, 0x16, 0x5b45 );
 	//Set_GPHYWB(999, 0, 22, 0xffff-(7<<4), 0x4<<4);
 	Set_GPHYWB(999, 0, 22, 0xffff-(7<<4), 0x5<<4);
-	//rtl8651_setAsicEthernetPHYReg( i, 0x0, 0x1300 );						
+	//rtl8651_setAsicEthernetPHYReg( i, 0x0, 0x1300 );
 	Set_GPHYWB(999, 0, 0, 0xffff-(1<<9), 0x1<<9);
-	
+
 	/*20100225 from Anson:Switch Force cailibration
 	#change calibration update method for patch first pkt no update impedance
 	phywb all 1 29 1 0
-	#--------------Patch for impedance update fail cause rx crc error with long calbe-------- 
+	#--------------Patch for impedance update fail cause rx crc error with long calbe--------
 	#Froce cailibration
 	phywb all 1 29 2 1
 	#Force impedance value = 0x8888
-	phywb all 1 28 15-0 0x8888			 	
+	phywb all 1 28 15-0 0x8888
 	#-----------------------------------------------------------------------------------------
 	#Select clock (ckt125[4]) edge trigger mlt3[1:0] = negative for patch four corner fail issue(only tx timing)
 	phywb all 1 17 2-1 0x3
@@ -1287,21 +1398,21 @@ void Setting_RTL8196C_PHY(void)
 	//Set_GPHYWB(999, 1, 29, 0xffff-(1<<2), 0x1<<2);
 	//Set_GPHYWB(999, 1, 28, 0xffff-(0xffff), 0x8888);
 	Set_GPHYWB(999, 1, 17, 0xffff-(3<<1), 0x3<<1);
-		 		 
+
 	/*20100222 from Yozen:AOI TEST pass setting*/
-	Set_GPHYWB(999, 1, 18, 0xffff-(0xffff), 0x9004);				
-		
+	Set_GPHYWB(999, 1, 18, 0xffff-(0xffff), 0x9004);
+
 	// for "DSP recovery fail when link partner = force 100F"
-	Set_GPHYWB(999, 4, 26, 0xffff-(0xfff<<4), 0xff8<<4);	
-	
+	Set_GPHYWB(999, 4, 26, 0xffff-(0xfff<<4), 0xff8<<4);
+
 	for(i=0; i<5; i++)
 		REG32(PCRP0+i*4) &= ~(EnForceMode);
-	
+
 #ifdef CONFIG_RTL8196C_ETH_IOT
         for(i=0; i<5; i++) {
                 set_gray_code_by_port(i);
         }
-#endif	
+#endif
 	printk("  Set 8196C PHY Patch OK\n");
 
 }
@@ -1318,7 +1429,7 @@ static const unsigned int phy_para[]={
 	0x1f,0x0002,//Page 2
 	0x04,0x80C2,//Page 2 Reg 4 0x80C2, Fix 100M re-link fail issue (20100110)
 	0x05,0x0938,//Page 2 Reg 5 0x0938, Disable 10M standby mode (20100112)
-	
+
 	0x1F,0x0003,//Page 3
 	0x12,0xC4D2,//Page 3 Reg 18 = 0xC4D2, GAIN upper bond=24
 	0x0D,0x0207,//Page 3 Reg 13 = 0x0207 (20100112)
@@ -1326,20 +1437,20 @@ static const unsigned int phy_para[]={
 	0x02,0x63E8, //#Page 3 Reg  2 = 0x63E8 (20100423)
 	0x03,0x99C2, //#Page 3 Reg  3 = 0x99C2 (20100423)
 	0x04,0x0113, //#Page 3 Reg  4 = 0x0113 (20100423)
-	
+
 	0x1f,0x0001,//Page 1
 	0x07,0x267E,//Page 1 Reg  7 = 0x267E, Channel Gain offset (20100111)
 	0x1C,0xE5F7,//Page 1 Reg 28 = 0xE5F7, Cable length offset (20100111)
 	0x1B,0x0424,//Page 1 Reg 27 = 0x0424, SD threshold (20100111)
-	
-	//#Add by Gary for Channel Estimation fine tune 20100430            
+
+	//#Add by Gary for Channel Estimation fine tune 20100430
 	//0x1f,0x0002, //# change to Page 1 (Global)
 	//0x08,0x0574, //# Page1 Reg8 (CG_INITIAL_MASTER)
-	//0x09,0x2724, //# Page1 Reg9 (CB0_INITIAL_GIGA) 
+	//0x09,0x2724, //# Page1 Reg9 (CB0_INITIAL_GIGA)
 	//0x1f,0x0003, //# change to Page 3 (Global)
 	//0x1a,0x06f6, //# Page3 Reg26 (CG_INITIAL_SLAVE)
-	
-	//#Add by Gary for Channel Estimation fine tune 20100430            
+
+	//#Add by Gary for Channel Estimation fine tune 20100430
 	//#Page1 Reg8 (CG_INITIAL_MASTER)
 	//0x1f, 0x0005,
 	//0x05, 0x83dd,
@@ -1352,47 +1463,47 @@ static const unsigned int phy_para[]={
 	//0x1f, 0x0005,
 	//0x05, 0x843d,
 	//0x06, 0x06f6 ,
-	
+
 	//#NC FIFO
 	0x1f,0x0007,//ExtPage
 	0x1e,0x0042,//ExtPage 66
 	0x18,0x0000,//Page 66 Reg 24 = 0x0000, NC FIFO (20100111)
 	0x1e,0x002D,//ExtPage 45
 	0x18,0xF010,//Page 45 Reg 24 = 0xF010, Enable Giga Down Shift to 100M (20100118)
-	
+
 	0x1e,0x002c, //#ExtPage 44
 	0x18,0x008B, //#Page 44 Reg 24 = 0x008B, Enable deglitch circuit (20100426)
-		
+
 	//############################ EEE giga patch ################################
-	
-	//0x1f 0x0007;   
+
+	//0x1f 0x0007;
 	0x1e,0x0028,
 	0x16,0xf640,//phywb $phyID 40 22 15-0 0xF640
-	
-	0x1e,0x0021,	
+
+	0x1e,0x0021,
 	0x19,0x2929,//phywb $phyID 33 25 15-0 0x2929
-	
+
 	0x1a,0x1005,//phywb $phyID 33 26 15-0 0x1005
-	
-	0x1e,0x0020,	
+
+	0x1e,0x0020,
 	0x17,0x000a,//phywb $phyID 32 23 15-0 0x000a
-	
+
 	0x1b,0x2f4a,//Disable EEE PHY mode
 	0x15,0x0100,//EEE ability, Disable EEEP
-	
+
 	0x1e,0x0040,//
 	0x1a,0x5110,//	phywb $phyID 64 26 15-0 0x5110
 	0x18,0x0000,// programable mode
-	
+
 	0x1e,0x0041,//
 	0x15,0x0e02,//phywb $phyID 65 21 15-0 0x0e02
-	
+
 	0x16,0x2185,//phywb $phyID 65 22 15-0 0x2185
 	0x17,0x000c,//phywb $phyID 65 23 15-0 0x000c
 	0x1c,0x0008,//phywb $phyID 65 28 15-0 0x0008
 	0x1e,0x0042,//
 	0x15,0x0d00,//phywb $phyID 66 21 15-0 0x0d00
-		
+
 	#if 1
 	//############################ EEE Run code patch #################################
 	//###proc 67R_ram_code_20100211_inrx_uc_98_1
@@ -1403,18 +1514,18 @@ static const unsigned int phy_para[]={
 	6,  0x0080,
 	5,  0x8b6e,
 	6,  0x0000,
-	15, 0x0100,	
-	
+	15, 0x0100,
+
 	//### force MDI/MDIX
 	0x1f, 0x0007,
 	0x1e, 0x002d,
 	0x18, 0xf030,
-	
+
 	//### pcs nctl patch code (0423)
-	0x1f, 0x0007,  
-	0x1e, 0x0023,    
-	0x16, 0x0005,    
-	
+	0x1f, 0x0007,
+	0x1e, 0x0023,
+	0x16, 0x0005,
+
 	//### startpoint
 	 0x15, 0x005c,
 	0x19, 0x0068,
@@ -1447,10 +1558,10 @@ static const unsigned int phy_para[]={
 	0x15, 0x01e0,
 	0x19, 0x0080,
 	//### endpoint
-	
-	0x16, 0x0000,    
-	//### end of pcs nctl patch code  	
-	
+
+	0x16, 0x0000,
+	//### end of pcs nctl patch code
+
 	//inrx
 	0x1f, 0x0007,
 	0x1e, 0x0040,
@@ -1459,7 +1570,7 @@ static const unsigned int phy_para[]={
 	0x17,0x2160,
 	0x1f,0x0007,
 	0x1e,0x0040,
-	
+
 	//### startpoint
 	0x18,0x0004,
 	0x19,0x4000,
@@ -2828,14 +2939,14 @@ static const unsigned int phy_para[]={
 	0x18,0x2a84,
 	0x19,0x4800,
 	//### endpoint
-	
+
 	0x1f,0x0000,
 	0x17,0x2100,
 	0x1f,0x0007,
 	0x1e,0x0040,
 	0x18,0x0000,
 	//### end of inrx dspctl patch code
-	
+
 	//### inrx eyesch patch code
 	0x1f,0x0007,
 	0x1e,0x0042,
@@ -2846,7 +2957,7 @@ static const unsigned int phy_para[]={
 	0x10,0xf25e,
 	0x1f,0x0007,
 	0x1e,0x0042,
-	
+
 	//### startpoint
 	0x15,0x0f00,
 	0x16,0x7408,
@@ -2877,7 +2988,7 @@ static const unsigned int phy_para[]={
 	0x15,0x0e06,
 	0x15,0x0f06,
 	//### endpoint
-	
+
 	0x1f, 0x0001,
 	0x10, 0xf05e,
 	0x1f, 0x0007,
@@ -2887,16 +2998,16 @@ static const unsigned int phy_para[]={
 	0x1f,0x0000,
 	0x17,0x2100,
 	//### end of inrx eyesch patch code
-	  		
+
 	//### release MDI/MDIX force mode
 	0x1f, 0x0007,
 	0x1e, 0x002d,
-	0x18, 0xf010,	
-	
+	0x18, 0xf010,
+
 	//### uc patch code (20110103 add foce giga mode)
 	0x1f,0x0005,
-	//### startpoint	
-	
+	//### startpoint
+
 	5, 0x8000,
 	6, 0xeeff,
 	6, 0xfc8b,
@@ -3519,21 +3630,21 @@ static const unsigned int phy_para[]={
 	6, 0x6f7a,
 	6, 0x06a6,
 	//### endpoint
-	                                  
-	//#unlock uc ramcode version                 
+
+	//#unlock uc ramcode version
 	5,  0xe142,
 	6,  0x0701,
 	5,  0xe140,
 	6,  0x0405,
 	15, 0x0000,
-	
+
 	//### end of uc patch code
 	//#Enable negear EEE Nway ability autooff
 	0x1f,0x0005,
 	0x05,0x8b84,
 	0x06,0x0026,
 	0x1f,0x0000,
-	
+
 	//#lpi patch code-maxod-20110103
 	31, 0x0007,
 	30, 0x0023,
@@ -3546,7 +3657,7 @@ static const unsigned int phy_para[]={
 	25, 0x26,
 	22, 0x0002,
 	31, 0x0000,
-	
+
 	//#Add by Gary for Channel Estimation fine tune 20100430
 	//#Page1 Reg8 (CG_INITIAL_MASTER)
 	0x1f, 0x0005,
@@ -3560,16 +3671,16 @@ static const unsigned int phy_para[]={
 	0x1f, 0x0005,
 	0x05, 0x843d,
 	0x06, 0x06f6 ,
-	
+
 	0x1f, 0x0000,
-	#endif				
+	#endif
 };
 
 static const unsigned int default_val[]={
 	999,0x1f,0x0002,
-	
+
 	2,0x11,0x7e00,
-	
+
 	3,0x1f,0x0002,
 	3,0x17,0xff00,
 	3,0x18,0x0005,
@@ -3577,7 +3688,7 @@ static const unsigned int default_val[]={
 	3,0x1a,0x0005,
 	3,0x1b,0x0005,
 	3,0x1c,0x0005,
-	
+
 	4,0x1f,0x0002,
 	4,0x13,0x00aa,
 	4,0x14,0x00aa,
@@ -3588,18 +3699,18 @@ static const unsigned int default_val[]={
 	4,0x19,0x50ab,
 	4,0x1a,0x0000,
 	4,0x1b,0x0f0f,
-	
+
 	999,0x1f,0x0000,
 };
-  
+
 void Setting_RTL8198_GPHY(void)
 {
 	int i=0, port =0, len=0;
-    
+
 	for(i=0; i<5; i++)
 		REG32(PCRP0+i*4) |= (EnForceMode);
 
-	if (REG32(BSP_REVR) == BSP_RTL8198_REVISION_A) 	
+	if (REG32(BSP_REVR) == BSP_RTL8198_REVISION_A)
 	{
 		/*
 		 #Access command format: phywb {all: phyID=0,1,2,3,4} {page} {RegAddr} {Bit location} {Bit value}
@@ -3613,77 +3724,77 @@ void Setting_RTL8198_GPHY(void)
 		*/
 
 		//	Set_GPHYWB(3, 2, 20, 0, 0x2000);
-	
+
 		Set_GPHYWB(999, 72, 21, 0, 0x7092);
 		Set_GPHYWB(999, 72, 22, 0, 0x7092);
 		Set_GPHYWB(999, 72, 23, 0, 0x7092);
 		Set_GPHYWB(999, 72, 24, 0, 0x7092);
-		Set_GPHYWB(999, 72, 25, 0, 0x7092);	
-		Set_GPHYWB(999, 72, 26, 0, 0x7092);	
-	
+		Set_GPHYWB(999, 72, 25, 0, 0x7092);
+		Set_GPHYWB(999, 72, 26, 0, 0x7092);
+
 		/*
 		 set PageNum 2; #All of GPHY register in the Page#2
 		 #Array format = {{PhyID List1}  {RegAddr1 RegData1 RegAddr2 RegData2}, ...}
-	
+
 		set AFE_Reg     {{0 1 2 3 4} { 0 0x0000 1  0x065a 2 0x8c01  3  0x0428 4 0x80c8  5   0x0978  6  0x0678 7  0x3620 8 0x0000 9 0x0007 10 0x0000}
 	       	               {2}         {11 0x0063 12 0xeb65 13 0x51d1 14 0x5dcb 15 0x3044 16 0x1000 17  0x7e00 18 0x0000}
 	              	        {3}         {19 0x3d22 20 0x2000 21 0x6040 22 0x0000 23 0xff00 24 0x0005 25 0x0005 26 0x0005 27 0x0005 28 0x0005}
 	                     	 {4}         {19 0x00aa 20 0x00aa 21 0x00aa 22 0x00aa 23 0x00aa 24 0x0f0a 25 0x5050 26 0x0000 27 0x0f0f }}
 		*/
-	
+
 	       //phyid=all
-		Set_GPHYWB(999, 2,  0, 0, 0x0000);  
-		Set_GPHYWB(999, 2,  1, 0, 0x065a);  
-		Set_GPHYWB(999, 2,  2, 0, 0x8c01);  
-		Set_GPHYWB(999, 2,  3, 0, 0x0428);  
-		Set_GPHYWB(999, 2,  4, 0, 0x80c8);  
-		Set_GPHYWB(999, 2,  5, 0, 0x0978);  
+		Set_GPHYWB(999, 2,  0, 0, 0x0000);
+		Set_GPHYWB(999, 2,  1, 0, 0x065a);
+		Set_GPHYWB(999, 2,  2, 0, 0x8c01);
+		Set_GPHYWB(999, 2,  3, 0, 0x0428);
+		Set_GPHYWB(999, 2,  4, 0, 0x80c8);
+		Set_GPHYWB(999, 2,  5, 0, 0x0978);
 		Set_GPHYWB(999, 2,  6, 0, 0x0678);
-		Set_GPHYWB(999, 2,  7, 0, 0x3620);	
-		Set_GPHYWB(999, 2,  8, 0, 0x0000);  
-		Set_GPHYWB(999, 2,  9, 0, 0x0007);  
-		Set_GPHYWB(999, 2,  10, 0, 0x0000);  
-	
+		Set_GPHYWB(999, 2,  7, 0, 0x3620);
+		Set_GPHYWB(999, 2,  8, 0, 0x0000);
+		Set_GPHYWB(999, 2,  9, 0, 0x0007);
+		Set_GPHYWB(999, 2,  10, 0, 0x0000);
+
 	       //phyid=2
-		Set_GPHYWB( 2,   2, 11, 0, 0x0063);	
-		Set_GPHYWB( 2,   2, 12, 0, 0xeb65);	
-		Set_GPHYWB( 2,   2, 13, 0, 0x51d1);	
-		Set_GPHYWB( 2,   2, 14, 0, 0x5dcb);	
-		Set_GPHYWB( 2,   2, 15, 0, 0x3044);	
-		Set_GPHYWB( 2,   2, 16, 0, 0x1000);		
-		Set_GPHYWB( 2,   2, 17, 0, 0x7e00);		
-		Set_GPHYWB( 2,   2, 18, 0, 0x0000);	
-		
+		Set_GPHYWB( 2,   2, 11, 0, 0x0063);
+		Set_GPHYWB( 2,   2, 12, 0, 0xeb65);
+		Set_GPHYWB( 2,   2, 13, 0, 0x51d1);
+		Set_GPHYWB( 2,   2, 14, 0, 0x5dcb);
+		Set_GPHYWB( 2,   2, 15, 0, 0x3044);
+		Set_GPHYWB( 2,   2, 16, 0, 0x1000);
+		Set_GPHYWB( 2,   2, 17, 0, 0x7e00);
+		Set_GPHYWB( 2,   2, 18, 0, 0x0000);
+
 	       //phyid=3
-		Set_GPHYWB( 3,   2, 19, 0, 0x3d22);	
-		Set_GPHYWB( 3,   2, 20, 0, 0x2000);	
-	   	Set_GPHYWB( 3,   2, 21, 0, 0x6040);	
-		Set_GPHYWB( 3,   2, 22, 0, 0x0000);		   
-		Set_GPHYWB( 3,   2, 23, 0, 0xff00);	
-		Set_GPHYWB( 3,   2, 24, 0, 0x0005);	
-		Set_GPHYWB( 3,   2, 25, 0, 0x0005);		
-		Set_GPHYWB( 3,   2, 26, 0, 0x0005);	
-		Set_GPHYWB( 3,   2, 27, 0, 0x0005);	
-		Set_GPHYWB( 3,   2, 28, 0, 0x0005);		
-	
+		Set_GPHYWB( 3,   2, 19, 0, 0x3d22);
+		Set_GPHYWB( 3,   2, 20, 0, 0x2000);
+	   	Set_GPHYWB( 3,   2, 21, 0, 0x6040);
+		Set_GPHYWB( 3,   2, 22, 0, 0x0000);
+		Set_GPHYWB( 3,   2, 23, 0, 0xff00);
+		Set_GPHYWB( 3,   2, 24, 0, 0x0005);
+		Set_GPHYWB( 3,   2, 25, 0, 0x0005);
+		Set_GPHYWB( 3,   2, 26, 0, 0x0005);
+		Set_GPHYWB( 3,   2, 27, 0, 0x0005);
+		Set_GPHYWB( 3,   2, 28, 0, 0x0005);
+
 	       //phyid=4
-		Set_GPHYWB( 4,   2, 19, 0, 0x00aa);	
-		Set_GPHYWB( 4,   2, 20, 0, 0x00aa);	
-		Set_GPHYWB( 4,   2, 21, 0, 0x00aa);	
-		Set_GPHYWB( 4,   2, 22, 0, 0x00aa);	
-		Set_GPHYWB( 4,   2, 23, 0, 0x00aa);	
-		Set_GPHYWB( 4,   2, 24, 0, 0x0f0a);	
-		Set_GPHYWB( 4,   2, 25, 0, 0x5050);	
-		Set_GPHYWB( 4,   2, 26, 0, 0x0000);	
-		Set_GPHYWB( 4,   2, 27, 0, 0x0f0f);			
-	
+		Set_GPHYWB( 4,   2, 19, 0, 0x00aa);
+		Set_GPHYWB( 4,   2, 20, 0, 0x00aa);
+		Set_GPHYWB( 4,   2, 21, 0, 0x00aa);
+		Set_GPHYWB( 4,   2, 22, 0, 0x00aa);
+		Set_GPHYWB( 4,   2, 23, 0, 0x00aa);
+		Set_GPHYWB( 4,   2, 24, 0, 0x0f0a);
+		Set_GPHYWB( 4,   2, 25, 0, 0x5050);
+		Set_GPHYWB( 4,   2, 26, 0, 0x0000);
+		Set_GPHYWB( 4,   2, 27, 0, 0x0f0f);
+
 		/*
-		 #=========== INRX Para. =================================                                                                 
-	
+		 #=========== INRX Para. =================================
+
 		 phywb all 0 21 0x1006
 	           #dfse_mode[15:14]=3(full), Fine tune aagc_lvl_fnet[10:0]
 	           phywb all 1 12 15-0 0xdbf0
-	           
+
 	           #cb0_i_giga[12:0]
 	           phywb all 1 9  15-0 0x2576
 	           phywb all 1 7  15-0 0x287E
@@ -3697,68 +3808,70 @@ void Setting_RTL8198_GPHY(void)
 	           phywb all 1 16 15-0 0xF05E
 	           phywb all 1 27 15-0 0xB414
 		*/
-	           
-		Set_GPHYWB( 999,   1, 12, 0, 0xdbf0);	
-	
-		Set_GPHYWB( 999,   1, 9, 0, 0x2576);	
-		Set_GPHYWB( 999,   1, 7, 0, 0x287E);	
-		Set_GPHYWB( 999,   1, 10, 0, 0x68E5);	
-		Set_GPHYWB( 999,   1, 29, 0, 0x3DA4);		
-		Set_GPHYWB( 999,   1, 28, 0, 0xE7F7);	
-		Set_GPHYWB( 999,   1, 20, 0, 0x7F52);	
-		Set_GPHYWB( 999,   1, 24, 0, 0x7FCE);	
-		Set_GPHYWB( 999,   1, 8, 0, 0x04B7);		
-		Set_GPHYWB( 999,   1, 6, 0, 0x4072);	
-		Set_GPHYWB( 999,   1, 16, 0, 0xF05E);	
-		Set_GPHYWB( 999,   1, 27, 0, 0xB414);		
-	
+
+		Set_GPHYWB( 999,   1, 12, 0, 0xdbf0);
+
+		Set_GPHYWB( 999,   1, 9, 0, 0x2576);
+		Set_GPHYWB( 999,   1, 7, 0, 0x287E);
+		Set_GPHYWB( 999,   1, 10, 0, 0x68E5);
+		Set_GPHYWB( 999,   1, 29, 0, 0x3DA4);
+		Set_GPHYWB( 999,   1, 28, 0, 0xE7F7);
+		Set_GPHYWB( 999,   1, 20, 0, 0x7F52);
+		Set_GPHYWB( 999,   1, 24, 0, 0x7FCE);
+		Set_GPHYWB( 999,   1, 8, 0, 0x04B7);
+		Set_GPHYWB( 999,   1, 6, 0, 0x4072);
+		Set_GPHYWB( 999,   1, 16, 0, 0xF05E);
+		Set_GPHYWB( 999,   1, 27, 0, 0xB414);
+
 		/*
 		 #=========== Cable Test =================================
-	
+
 		  phywb all 3 26 15-0 0x06A6
 		  phywb all 3 16 15-0 0xF05E
 		  phywb all 3 19 15-0 0x06EB
 		  phywb all 3 18 15-0 0xF4D2
 		  phywb all 3 14 15-0 0xE120
 		  phywb all 3 0  15-0 0x7C00
-	
+
 		  phywb all 3 2  15-0 0x5FD0
 		  phywb all 3 13 15-0 0x0207
-		 
-		  #disable jabber detect         
+
+		  #disable jabber detect
 		   phywb all 0 16 15-0 0x05EF
-	
+
 		  #Patch for EEE GMII issue
 		  phywb all 32 26 15-0 0x0103
 		  phywb all 32 22 15-0 0x0004
 		*/
-		Set_GPHYWB( 999,   3, 26, 0, 0x06A6);	 
-		Set_GPHYWB( 999,   3, 16, 0, 0xF05E);	
-		Set_GPHYWB( 999,   3, 19, 0, 0x06EB);	
-		Set_GPHYWB( 999,   3, 18, 0, 0xF4D2);	
-		Set_GPHYWB( 999,   3, 14, 0, 0xE120);	
-		Set_GPHYWB( 999,   3, 00, 0, 0x7C00);	
-		
-		Set_GPHYWB( 999,   3, 02, 0, 0x5FD0);	
-		Set_GPHYWB( 999,   3, 13, 0, 0x0207);	
-		
-		Set_GPHYWB( 999,   0, 16, 0, 0x05EF);	
-		
-		Set_GPHYWB( 999,   3, 26, 0, 0x0103);	
-		Set_GPHYWB( 999,   3, 22, 0, 0x0004);	
-	
-		/*	
+		Set_GPHYWB( 999,   3, 26, 0, 0x06A6);
+		Set_GPHYWB( 999,   3, 16, 0, 0xF05E);
+		Set_GPHYWB( 999,   3, 19, 0, 0x06EB);
+		Set_GPHYWB( 999,   3, 18, 0, 0xF4D2);
+		Set_GPHYWB( 999,   3, 14, 0, 0xE120);
+		Set_GPHYWB( 999,   3, 00, 0, 0x7C00);
+
+		Set_GPHYWB( 999,   3, 02, 0, 0x5FD0);
+		Set_GPHYWB( 999,   3, 13, 0, 0x0207);
+
+		Set_GPHYWB( 999,   0, 16, 0, 0x05EF);
+
+		Set_GPHYWB( 999,   3, 26, 0, 0x0103);
+		Set_GPHYWB( 999,   3, 22, 0, 0x0004);
+
+		/*
 		 disable aldps_en, for power measurement
 		 hywb all 44 21 15-0 0x0350
 		*/
-		Set_GPHYWB( 999,   44, 21, 0, 0x0350);	
+		Set_GPHYWB( 999,   44, 21, 0, 0x0350);
 	}
 	else
 	{
+		Set_GPHYWB(999, 0, 0, 0xffff-POWER_DOWN, POWER_DOWN); // set power down
+	
 		len=sizeof(default_val)/sizeof(unsigned int);
 		for(i=0;i<len;i=i+3)
-		{	
-		
+		{
+
 			if(default_val[i]==999)
 			{
 				for(port=0; port<5; port++)
@@ -3774,25 +3887,25 @@ void Setting_RTL8198_GPHY(void)
 		for(port=0; port<5; port++)
 		{
 			for(i=0;i<len;i=i+2)
-			{	
+			{
 				rtl8651_setAsicEthernetPHYReg(port, phy_para[i], phy_para[i+1]);
 			}
 		}
-		Set_GPHYWB( 999,   5, 5, 0, 0x8b84);		
-		Set_GPHYWB( 999,   5, 6, 0, 0x0006);		
+		Set_GPHYWB( 999,   5, 5, 0, 0x8b84);
+		Set_GPHYWB( 999,   5, 6, 0, 0x0006);
 		Set_GPHYWB( 999,   2, 8, 0, 0x0020);
 
 		// for the IOT issue with IC+ when EEE N-way.
 		Set_GPHYWB( 999,   172, 24, 0, 0x0006);
 
 #ifdef CONFIG_RTL_8198_ESD
-		Set_GPHYWB(999, 44, 27, 0xffff-(0xf<<12), 0x4<<12);	
-#endif		
+		Set_GPHYWB(999, 44, 27, 0xffff-(0xf<<12), 0x4<<12);
+#endif
 	}
 
-	for(i=0; i<5; i++)
-		REG32(PCRP0+i*4) &= ~(EnForceMode);
-	
+//	for(i=0; i<5; i++)
+//		REG32(PCRP0+i*4) &= ~(EnForceMode);
+
 	printk("==Set GPHY Parameter OK\n");
 }
 
@@ -3800,14 +3913,14 @@ int rtl8198_force_giga(int port)
 {
 	if (port < 0 || port > 4)
 		return 0;
-	
+
 	REG32(PCRP0+ port*4) |= (EnForceMode);
 
 	rtl8651_setAsicEthernetPHYReg(port, 31, 0x5);
 	rtl8651_setAsicEthernetPHYReg(port, 5, 0x8b86);
 	rtl8651_setAsicEthernetPHYReg(port, 6, 0x0040);
 	rtl8651_setAsicEthernetPHYReg(port, 31, 0x0);
-	
+
 	REG32(PCRP0+ port*4) = REG32(PCRP0+ port*4) & ~(EnForceMode | NwayAbility100MF | NwayAbility100MH | NwayAbility10MF | NwayAbility10MH); // disable Nway 10/100 ability
 
 	rtl8651_restartAsicEthernetPHYNway(port);
@@ -3818,14 +3931,14 @@ int rtl8198_disable_force(int port)
 {
 	if (port < 0 || port > 4)
 		return 0;
-	
+
 	REG32(PCRP0+ port*4) |= (EnForceMode);
 
 	rtl8651_setAsicEthernetPHYReg(port, 31, 0x5);
 	rtl8651_setAsicEthernetPHYReg(port, 5, 0x8b86);
 	rtl8651_setAsicEthernetPHYReg(port, 6, 0x0000);
 	rtl8651_setAsicEthernetPHYReg(port, 31, 0x0);
-	
+
 	REG32(PCRP0+ port*4) = (REG32(PCRP0+ port*4) & ~(EnForceMode)) | NwayAbility100MF | NwayAbility100MH | NwayAbility10MF | NwayAbility10MH; // enable Nway 10/100 ability
 
 	rtl8651_restartAsicEthernetPHYNway(port);
@@ -3834,7 +3947,7 @@ int rtl8198_disable_force(int port)
 
 #if defined(PORT5_RGMII_GMII)
 unsigned int ExtP5GigaPhyMode=0;
-void ProbeP5GigaPHYChip(void)	
+void ProbeP5GigaPHYChip(void)
 {
 	unsigned int uid,tmp;
 	unsigned int i;
@@ -3861,49 +3974,325 @@ void ProbeP5GigaPHYChip(void)
 	uid=uid | tmp;
 
 	if( uid==0x001CC912 )  //0x001cc912 is 8212 two giga port , 0x001cc940 is 8214 four giga port
-	{	
+	{
 		//printk("Find Port5   have 8211 PHY Chip! \r\n");
 		ExtP5GigaPhyMode=1;
 		//return 1;
-	}	
+	}
 	else
-	{	
+	{
 		//printk("NO Find Port5 8211 PHY Chip! \r\n");
 		//ExtP5GigaPhyMode=0;
 		//return 1;
-	}	
+	}
 	for(i=0; i<=5; i++)
 		REG32(PCRP0+i*4) &= ~(EnForceMode);
 
 }
 #endif
+
+void disable_phy_power_down(void)
+{
+	int i;
+	uint32 statCtrlReg0;
+
+	for (i=0; i<5; i++)
+	{
+		rtl8651_getAsicEthernetPHYReg( i, 0, &statCtrlReg0 );
+ 
+		statCtrlReg0 &= (~POWER_DOWN);
+ 
+		/* write PHY reg 0 */
+		rtl8651_setAsicEthernetPHYReg( i, 0, statCtrlReg0 );
+
+		REG32(PCRP0+i*4) &= ~(EnForceMode);
+	}
+	mdelay(3000);
+}
 #endif
 
 int32 rtl865x_platform_check(void)
 {
-	uint32 bondOptReg=0;	
+	uint32 bondOptReg=0;
 	bondOptReg=REG32(0xB800000C);
 	if(((bondOptReg&0x0F)!=0x7) && ((bondOptReg&0x0F)!=0x9))
 	{
 		printk("current chip doesn't supported,system halt...\n");
-		while(1);		
+		while(1);
 	}
-	
+
 	return SUCCESS;
 }
 
+#if defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
+unsigned int Get_P0_PhyMode(void)
+{
+	/*
+		00: External  phy
+		01: embedded phy
+		10: olt
+		11: deb_sel
+	*/
+	#define GET_BITVAL(v,bitpos,pat) ((v& ((unsigned int)pat<<bitpos))>>bitpos)
+	#define RANG1 1
+	#define RANG2 3
+	#define RANG3  7
+	#define RANG4 0xf
+
+	#define SYS_HW_STRAP   (0xb8000000 +0x08)
+
+	unsigned int v=REG32(SYS_HW_STRAP);
+	unsigned int mode=GET_BITVAL(v, 6, RANG1) *2 + GET_BITVAL(v, 7, RANG1);
+
+	return (mode&3);
+}
+
+unsigned int Get_P0_MiiMode(void)
+{
+	/*
+		0: MII-PHY
+		1: MII-MAC
+		2: GMII-MAC
+		3: RGMII
+	*/
+	#define GET_BITVAL(v,bitpos,pat) ((v& ((unsigned int)pat<<bitpos))>>bitpos)
+	#define RANG1 1
+	#define RANG2 3
+	#define RANG3  7
+	#define RANG4 0xf
+
+	#define SYS_HW_STRAP   (0xb8000000 +0x08)
+
+	unsigned int v=REG32(SYS_HW_STRAP);
+	unsigned int mode=GET_BITVAL(v, 27, RANG2);
+
+	return mode;
+}
+
+unsigned int Get_P0_RxDelay(void)
+{
+	#define GET_BITVAL(v,bitpos,pat) ((v& ((unsigned int)pat<<bitpos))>>bitpos)
+	#define RANG1 1
+	#define RANG2 3
+	#define RANG3  7
+	#define RANG4 0xf
+
+	#define SYS_HW_STRAP   (0xb8000000 +0x08)
+
+	unsigned int v=REG32(SYS_HW_STRAP);
+	unsigned int val=GET_BITVAL(v, 29, RANG3);
+	return val;
+}
+
+unsigned int Get_P0_TxDelay(void)
+{
+	#define GET_BITVAL(v,bitpos,pat) ((v& ((unsigned int)pat<<bitpos))>>bitpos)
+	#define RANG1 1
+	#define RANG2 3
+	#define RANG3  7
+	#define RANG4 0xf
+
+	#define SYS_HW_STRAP   (0xb8000000 +0x08)
+
+	unsigned int v=REG32(SYS_HW_STRAP);
+	unsigned int val=GET_BITVAL(v, 17, RANG1);
+	return val;
+}
+
+int Setting_RTL8197D_PHY(void)
+{
+	int i;
+
+	for(i=0; i<5; i++)
+		REG32(PCRP0+i*4) |= (EnForceMode);
+
+	/*
+	  page 	addr rtl8197d-default 	rtl8197d-new value Purpose
+	  0 		21 	0x02c5 			0x0232 			Green: up/low bond to 3/2
+	  0 		22 	0x5b85 			0x5bd5 			Green: ad current from 2 to 3
+	  1 		18 	0x901c 			0x9004 			finetune AOI waveform
+	  1 		19 	0x4400 			0x5400 			finetune 100M DAC current
+	  1 		25 	0x00da 			0x00d0 			enable pwdn10rx at pwr saving enable snr threshold = 18dB
+
+	  4 		16 	0x4007 			0x737f 			enable EEE, fine tune EEE parameter
+	  4 		24 	0xc0a0 			0xc0f3 			change EEE wake idle to 10us
+	  4 		25 	0x0130 			0x0730 			turn off tx/rx pwr at LPI state
+	*/
+
+	// only do the PHY setting in this revision IC, no need for the new one.
+	if (REG32(REVR) == 0x8197C000) {
+
+		Set_GPHYWB(999, 0, 21, 0, 0x0232);
+
+		/* purpose: to avoid 100M N-way link fail issue Set_p="1" */
+		Set_GPHYWB(999, 0, 22, 0, 0x5bd5);
+
+	 	/* purpose: to adjust AOI waveform */
+		Set_GPHYWB(999, 1, 18, 0, 0x9004);
+
+		/* purpose: to enhance ethernet 100Mhz output voltage about 1.0(v) */
+		Set_GPHYWB(999, 1, 19, 0, 0x5400);
+
+		Set_GPHYWB(999, 1, 25, 0, 0x00d0);  //enable pwdn10rx at pwr saving enable snr threshold = 18dB
+
+		Set_GPHYWB(999, 4, 16, 0, 0x737f);// enable EEE, fine tune EEE parameter
+		Set_GPHYWB(999, 4, 24, 0, 0xc0f3);	//change EEE wake idle to 10us
+		Set_GPHYWB(999, 4, 25, 0, 0x0730);	 // turn off tx/rx pwr at LPI state
+	}
+
+	/* fine tune port on/off threshold to 160/148 */
+	REG32(PBFCR0) = 0x009400A0;
+	REG32(PBFCR1) = 0x009400A0;
+	REG32(PBFCR2) = 0x009400A0;
+	REG32(PBFCR3) = 0x009400A0;
+	REG32(PBFCR4) = 0x009400A0;
+	REG32(PBFCR6) = 0x009400A0;
+
+	/* modify egress leaky bucket parameter, default inaccuracy is 5~10%, the new one is 1~2% after modification */
+	REG32(ELBPCR) = 0x0000400B;
+	REG32(ELBTTCR) = 0x000000C0;
+
+	/* Fine tune minRx IPG from 6 to 5 byte */
+	REG32(MACCR) = 0x80420185;
+
+	/* default enable MAC EEE */
+	REG32(EEECR) = 0x28739ce7;
+
+	for(i=0; i<5; i++)
+		REG32(PCRP0+i*4) &= ~(EnForceMode);
+
+	return 0;
+}
+#endif
+
+#ifdef CONFIG_RTL_8196E
+int Setting_RTL8196E_PHY(void)
+{
+	int i;
+
+	for(i=0; i<5; i++)
+		REG32(PCRP0+i*4) |= (EnForceMode);
+
+	// write page1, reg16, bit[15:13] Iq Current 110:175uA (default 100: 125uA)
+	Set_GPHYWB(999, 1, 16, 0xffff-(0x7<<13), 0x6<<13);
+
+	// disable power saving mode in A-cut only
+	if (REG32(REVR) == 0x8196e000)
+		Set_GPHYWB(999, 0, 0x18, 0xffff-(1<<15), 0<<15);
+
+	/* B-cut and later,
+	    just increase a little power in long RJ45 cable case for Green Ethernet feature.
+	 */
+	else 
+	{
+		// adtune_lb setting
+		Set_GPHYWB(999, 0, 22, 0xffff-(0x7<<4), 0x4<<4);
+		//Setting SNR lb and hb
+		Set_GPHYWB(999, 0, 21, 0xffff-(0xff<<0), 0xc2<<0);
+		//auto bais current
+		Set_GPHYWB(999, 1, 19, 0xffff-(0x1<<0), 0x0<<0);
+		Set_GPHYWB(999, 0, 22, 0xffff-(0x1<<3), 0x0<<3);
+	}
+	
+	// fix Ethernet IOT issue
+	if ((REG32(BOND_OPTION) & BOND_ID_MASK) != BOND_8196ES) {
+		Set_GPHYWB(999, 0, 26, 0xffff-(0x1<<14), 0x0<<14);
+		Set_GPHYWB(999, 0, 17, 0xffff-(0xf<<8), 0xe<<8);
+	}
+	
+	/* 100M half duplex enhancement */
+	/* fix SmartBits half duplex backpressure IOT issue */
+ 	REG32(MACCR)= (REG32(MACCR) & ~(CF_RXIPG_MASK | SELIPG_MASK)) | (0x05 | SELIPG_11); 
+	
+	for(i=0; i<5; i++)
+		REG32(PCRP0+i*4) &= ~(EnForceMode);
+
+	return 0;
+}
+#endif
+
+void enable_EEE(void)
+{
+#if defined(CONFIG_RTL_8198)
+	eee_phy_enable_98();
+#else
+
+	int i;
+
+	for(i=0; i<RTL8651_PHY_NUMBER; i++)
+		REG32(PCRP0+i*4) |= (EnForceMode);
+
+#if defined(CONFIG_RTL_8196E) || defined(CONFIG_RTL_819XD)
+	//enable 100M EEE and 10M EEE
+	Set_GPHYWB(999, 4, 16, 0xffff-(0x3<<12), 0x3<<12);
+
+	//enable MAC EEE
+	REG32(EEECR) = 0x0E739CE7;
+	
+#elif defined(CONFIG_RTL_8196C)
+	for(i=0; i<RTL8651_PHY_NUMBER; i++) {
+		eee_phy_enable_by_port(i);
+	}
+
+	// set FRC_P0_EEE_100, EN_P0_TX_EEE and EN_P0_RX_EEE
+	//REG32(EEECR) = 0x0E739CE7;		// consult with Jim and Anson, we do not use this setting.
+	// set EN_P0_TX_EEE and EN_P0_RX_EEE
+	REG32(EEECR) = 0x06318C63;
+
+#ifdef CONFIG_POCKET_ROUTER_SUPPORT
+	#define ETH_PORT_START		4
+	#define ETH_PORT_END		4
+#else
+	#define ETH_PORT_START		0
+	#define ETH_PORT_END		4
+#endif
+	for ( i = ETH_PORT_START ; i <= ETH_PORT_END; i++ ) {
+		/* enable phy 100 eee ability */
+		mmd_write(i, 7, 60, 0x2);
+	}	
+#endif
+
+	for(i=0; i<RTL8651_PHY_NUMBER; i++)
+		REG32(PCRP0+i*4) &= ~(EnForceMode);
+#endif	
+}
+
+void disable_EEE(void)
+{
+#if defined(CONFIG_RTL_8198)
+	eee_phy_disable_98();
+
+#else
+	// for CONFIG_RTL_8196C, CONFIG_RTL_819XD and CONFIG_RTL_8196E
+	int i;
+
+	for(i=0; i<RTL8651_PHY_NUMBER; i++)
+		REG32(PCRP0+i*4) |= (EnForceMode);
+
+	//disable EEE MAC
+	REG32(EEECR) = 0;
+
+	//disable 100M EEE and 10M EEE
+	Set_GPHYWB(999, 4, 16, 0xffff-(0x3<<12), 0x0<<12);
+	
+	for(i=0; i<RTL8651_PHY_NUMBER; i++)
+		REG32(PCRP0+i*4) &= ~(EnForceMode);
+#endif
+}
+
 /*patch for LED showing*/
 #define BICOLOR_LED 1
+#define REG32_ANDOR(x,y,z)   (REG32(x)=(REG32(x)& (y))|(z))
 /*=========================================
   * init Layer2 Asic
   * rtl865x_initAsicL2 mainly configure basic&L2 Asic.
   * =========================================*/
-int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para) 
+int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 {
 	int32 index;
 
 #ifdef BICOLOR_LED
-#if defined (CONFIG_RTL_8196C) || defined(CONFIG_RTL_8198)
+#if defined (CONFIG_RTL_8196C) || defined(CONFIG_RTL_8198) || defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
 #else
 	unsigned int hw_val;
 #endif
@@ -4001,7 +4390,7 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 
 		/*
 			# According to Hardware SD: David & Maxod,
-			
+
 			Set Port5_GMII Configuration Register.
 			- RGMII Output Timing compensation control : 0 ns
 			- RGMII Input Timing compensation control : 0 ns
@@ -4018,7 +4407,7 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 
 		/*
 			# According to Hardware SD: David & Maxod,
-			
+
 			Set Port5_GMII Configuration Register.
 			- RGMII Output Timing compensation control : 0 ns
 			- RGMII Input Timing compensation control : 0 ns
@@ -4029,13 +4418,19 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 #ifdef CONFIG_RTL8196C_REVISION_B
 	if (REG32(REVR) == RTL8196C_REVISION_B)
 		Setting_RTL8196C_PHY();
-	
+
+#elif defined(CONFIG_RTL_8196E)
+	Setting_RTL8196E_PHY();
+
+#elif defined(CONFIG_RTL_819XD)
+	Setting_RTL8197D_PHY();
+
 #elif defined(CONFIG_RTL_8198)
 #if 0//def PORT5_RGMII_GMII
 	ProbeP5GigaPHYChip();
 #endif
+
 	Setting_RTL8198_GPHY();
-	
 #endif
 
 #ifdef CONFIG_8198_PORT5_RGMII
@@ -4054,7 +4449,7 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 	GPIO_PORT_I,
 	GPIO_PORT_MAX,
 	};
-	
+
 	#define REG_IOCFG_GPIO		0x00000018
 
 	extern int32 smi_init(uint32 port, uint32 pinSCK, uint32 pinSDA);
@@ -4068,13 +4463,13 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 	WRITE_MEM32(PABCD_DIR, READ_MEM32(PABCD_DIR) | ((0x0000000C))); //output pin
 
 	smi_init(GPIO_PORT_A, 3, 2);
-	
+
 	RTL8370_init();
 	}
 #endif
 
 	/* 	2006.12.12
-		We turn on bit.10 (ENATT2LOG). 
+		We turn on bit.10 (ENATT2LOG).
 
 		* Current implementation of unnumbered pppoe in multiple session
 		When wan type is multiple-session, and one session is unnumbered pppoe, WAN to unnumbered LAN is RP --> NPI.
@@ -4082,11 +4477,11 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 
 		However, when pktOpApp of this ACL rule is set, it seems that this toCPU ACL does not work.
 		Therefore, we turn on this bit (ENATT2LOG) to trap pkts (WAN --> unnumbered LAN) to CPU.
-		
+
 	*/
 	WRITE_MEM32( SWTCR1, READ_MEM32( SWTCR1 ) | EnNATT2LOG );
 
-	/* 
+	/*
 	  * Turn on ENFRAG2ACLPT for Rate Limit. For those packets which need to be trapped to CPU, we turn on
 	  * this bit to tell ASIC ACL and Protocol Trap to process these packets. If this bit is not turnned on, packets
 	  * which need to be trapped to CPU will not be processed by ASIC ACL and Protocol Trap.
@@ -4094,19 +4489,19 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 	  *																- chhuang
 	  */
 	WRITE_MEM32( SWTCR1, READ_MEM32( SWTCR1 ) | ENFRAGTOACLPT );
-	
+
 #ifdef CONFIG_RTL865X_LIGHT_ROMEDRV
 	WRITE_MEM32( SWTCR1, READ_MEM32( SWTCR1 ) | L4EnHash1 );    /*Turn on Napt Enhanced hash1*/
 #endif
 
-	/* 
+	/*
 	  * Cannot turn on EnNAP8651B due to:
 	  * If turn on, NAT/LP/ServerPort will reference nexthop. This will result in referecing wrong L2 entry when
 	  * the destination host is in the same subnet as WAN.
 	  */
 
 
-	/*Although chip is in 8650 compatible mode, 
+	/*Although chip is in 8650 compatible mode,
 	some 865XB features are independent to compatibility register*/
 	/*Initialize them here if needed*/
 
@@ -4114,7 +4509,7 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 		 int rev;
 		char chipVersion[16];
 		rtl8651_getChipVersion(chipVersion, sizeof(chipVersion), &rev);
-		if(chipVersion[strlen(chipVersion)-1]=='B' 
+		if(chipVersion[strlen(chipVersion)-1]=='B'
 			|| chipVersion[strlen(chipVersion) - 1] == 'C' )
 		{
 			rtl8651_totalExtPortNum=3; //this replaces all RTL8651_EXTPORT_NUMBER defines
@@ -4123,7 +4518,7 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 			rtl8651_asicEthernetCableMeterInit();
 #endif
 		}
-		
+
 	}
 	//Disable layer2, layer3 and layer4 function
 	//Layer 2 enabled automatically when a VLAN is added
@@ -4157,15 +4552,30 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 #if defined(CONFIG_RTL8186_KB) && defined(CONFIG_RTL8186_KB_N)
     hw_val = read_gpio_hw_setting();
     REG32(PIN_MUX_SEL) =0x0fffff80;/*For Belkin_n board, not for demo board*/
-    REG32(LEDCREG)=0; 
-#else
-#if defined (CONFIG_RTL_8196C) || defined(CONFIG_RTL_8198)
+    REG32(LEDCREG)=0;
+
+#elif defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
+	/*
+		#LED = direct mode
+		set mode 0x0
+		swwb 0xbb804300 21-20 0x2 19-18 $mode 17-16 $mode 15-14 $mode 13-12 $mode 11-10 $mode 9-8 $mode
+	*/
+	#ifdef 	CONFIG_RTK_VOIP_BOARD	
+	//for GMII/RGMII
+	//REG32(PIN_MUX_SEL) &= ~( (3<<8) | (3<<10) | (3<<3) | (1<<15) );  //let P0 to mii mode
+	REG32(PIN_MUX_SEL2) &= ~ ((3<<0) | (3<<3) | (3<<6) | (3<<9) | (3<<12) );  //LED0~LED4
+	#else
+	REG32(PIN_MUX_SEL) &= ~( (3<<8) | (3<<10) | (3<<3) | (1<<15) );  //let P0 to mii mode
+	REG32(PIN_MUX_SEL2) &= ~ ((3<<0) | (3<<3) | (3<<6) | (3<<9) | (3<<12) | (7<<15) );  //S0-S3, P0-P1
+	#endif
+	REG32(LEDCREG)  = (2<<20) | (0<<18) | (0<<16) | (0<<14) | (0<<12) | (0<<10) | (0<<8);  //P0-P5
+
+#elif defined (CONFIG_RTL_8196C) || defined(CONFIG_RTL_8198)
 #else
     hw_val = read_gpio_hw_setting();
     REG32(PIN_MUX_SEL) =0x00000380;
     REG32(LEDCREG)=0;
 #endif
-#endif
 #else
      hw_val = read_gpio_hw_setting();
     if (hw_val == 0x2 || hw_val == 0x3 || hw_val == 0x6 || hw_val == 0x7)  // LED in matrix mode
@@ -4195,12 +4605,15 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
     #endif /* BICOLOR_LED_VENDOR_BXXX */
 #endif
 #endif
+
+#if !defined(CONFIG_RTL_819XD) && !defined(CONFIG_RTL_8196E)
 	//MAC Control (0xBC803000)
 /*	WRITE_MEM32(MACCR,READ_MEM32(MACCR)&~DIS_IPG);//Set IFG range as 96+-4bit time*/
 	WRITE_MEM32(MACCR,READ_MEM32(MACCR)&~NORMAL_BACKOFF);//Normal backoff
 	WRITE_MEM32(MACCR,READ_MEM32(MACCR)&~BACKOFF_EXPONENTIAL_3);//Exponential parameter is 9
 	WRITE_MEM32(MACCR,READ_MEM32(MACCR)|INFINITE_PAUSE_FRAMES);//send pause frames infinitely.
 	WRITE_MEM32(MACCR,READ_MEM32(MACCR)|DIS_MASK_CGST);
+#endif
 
 	miiPhyAddress = -1;		/* not ready to use mii port 5 */
 
@@ -4260,7 +4673,7 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 	if(RTL865X_PHY6_DSP_BUG)
 		WRITE_MEM32(PCRP6, (6<<ExtPHYID_OFFSET)|AcptMaxLen_16K|EnablePHYIf );
 	/* Set PHYID 6 to PCRP6. (By default, PHYID of PCRP6 is 0. It will collide with PHYID of port 0. */
-#endif	
+#endif
 
 
 
@@ -4306,34 +4719,35 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 	WRITE_MEM32(MISCCR,READ_MEM32(MISCCR)&~MULTICAST_L2_MTU_MASK);
 	WRITE_MEM32(MISCCR,READ_MEM32(MISCCR)|(1522&MULTICAST_L2_MTU_MASK));//Multicast packet layer2 size 1522 at most*/
 	/* follow RTL865xB's convention, we use 1522 as default multicast MTU */
-	
+
 	/*FIXME:Hyking init in Layer3 1*/
 	//rtl8651_setAsicMulticastMTU(1522);
 
 	//Set all Protocol-Based Reg. to 0
 
 	for (index=0;index<32;index++)
-		WRITE_MEM32(PBVCR0+index*4,  0x00000000);	
+		WRITE_MEM32(PBVCR0+index*4,  0x00000000);
 	//Enable TTL-1
 	/*FIXME:Hyking init in Layer3 1*/
 	//WRITE_MEM32(TTLCR,READ_MEM32(TTLCR)|(uint32)EN_TTL1);//Don't hide this router. enable TTL-1 when routing on this gateway.
 
-	
+
 	for (index=0; index<RTL8651_PORT_NUMBER+rtl8651_totalExtPortNum; index++) {
 
-		
-		
+
+
 		if(	rtl8651_setAsicMulticastSpanningTreePortState(index, RTL8651_PORTSTA_FORWARDING))
 			return FAILED;
-		
+
 		rtl865xC_setAsicSpanningTreePortState(index, RTL8651_PORTSTA_FORWARDING);
 
 		rtl8651_setAsicEthernetBandwidthControl(index, TRUE, RTL8651_BC_FULL);
 		rtl8651_setAsicEthernetBandwidthControl(index, FALSE, RTL8651_BC_FULL);
 	}
 
+	// 08-15-2012, set TRXRDY bit in rtl865x_start() in rtl865x_asicCom.c
 	/* Enable TX/RX After ALL ASIC configurations are done */
-	WRITE_MEM32( SIRR, READ_MEM32(SIRR)| TRXRDY );
+	//WRITE_MEM32( SIRR, READ_MEM32(SIRR)| TRXRDY );
 
 	/* Initiate Bandwidth control backward compatible mode : Set all of them to FULL-Rate */
 	{
@@ -4349,11 +4763,11 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 		/* Sync the configuration to ASIC */
 		_rtl8651_syncToAsicEthernetBandwidthControl();
 	}
-	
+
 
 
 	/* ==================================================================================================
-		Embedded PHY patch -- According to the designer, internal PHY's parameters need to be adjusted. 
+		Embedded PHY patch -- According to the designer, internal PHY's parameters need to be adjusted.
 	 ================================================================================================== */
 	if(RTL865X_PHY6_DSP_BUG) /*modified by Mark*/
 	{
@@ -4372,52 +4786,32 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 					RTL8651_MAC_NUMBER:
 					RTL8651_PHY_NUMBER;
 
-#ifdef CONFIG_RTL_EEE_DISABLED
-		eee_enabled = 0;
-#endif
-
 		for ( port = 0 ; port < maxPort ; port ++ )
-		{	
-		
-#if defined(RTL8196C_EEE_MAC)
-			if (eee_enabled) {
-				uint32 reg; 		
-				eee_phy_enable_by_port(port);
-
-				if (REG32(REVR) == RTL8196C_REVISION_B) {
-					// enable EEE MAC
-					reg = REG32(EEECR);
-//					REG32(EEECR) = (reg & ~(0x1f << (port * 5)) )
-//						| ((FRC_P0_EEE_100|EN_P0_TX_EEE|EN_P0_RX_EEE) << (port * 5));
-					REG32(EEECR) = (reg & ~(0x1f << (port * 5)) )
-						| ((EN_P0_TX_EEE|EN_P0_RX_EEE) << (port * 5));
-
-				}
-			}
-#endif
+		{
 			rtl8651_setAsicFlowControlRegister(port, TRUE);
 			rtl865xC_setAsicPortPauseFlowControl(port, TRUE, TRUE);
 		}
-
-		#if defined(CONFIG_RTL_8196C)
-		if (eee_enabled == 0) {
-			REG32(EEECR) = 0;
-		}
-		#elif defined(CONFIG_RTL_8198)
-		if (eee_enabled) {
-			eee_phy_enable_98();
-		}
-		else {
-			eee_phy_disable_98();
-		}
-		#endif
 	}
 
+	/* ===============================
+	 	EEE setup
+	    =============================== */
+#if defined(CONFIG_RTL_EEE_DISABLED) ||defined(CONFIG_MP_PSD_SUPPORT)	
+	eee_enabled = 0;
+#else
+	eee_enabled = 1;
+#endif
+
+	if (eee_enabled) {	    
+		enable_EEE();
+	}
+	else {
+		disable_EEE();
+	}
 
-	
 	/* ===============================
 	 	(1) Handling port 0.
-	    =============================== */	
+	    =============================== */
 	rtl8651_restartAsicEthernetPHYNway(0);	/* Restart N-way of port 0 to let embedded phy patch take effect. */
 
 	/* ===============================
@@ -4425,7 +4819,7 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 	    =============================== */
 	if (rtl8651_tblAsicDrvPara.externalPHYProperty & RTL8651_TBLASIC_EXTPHYPROPERTY_PORT1234_RTL8212)
 	{
-		
+
 	} else
 	{
 		/* Restart N-way of port 1 - port 4 to let embedded phy patch take effect. */
@@ -4446,7 +4840,7 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 
 
 	/* =====================
-		QoS-related patch 
+		QoS-related patch
 	    ===================== */
 	{
 		#define DEFAULT_ILB_UBOUND 0x3FBE  /*added by Mark for suggested Leacky Bucket value*/
@@ -4460,7 +4854,7 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 		WRITE_MEM32( ILBPCR1, DEFAULT_ILB_UBOUND << UpperBound_OFFSET | DEFAULT_ILB_LBOUND << LowerBound_OFFSET );
 		for(i=0;i<=(RTL8651_PHY_NUMBER/2);i++) /*Current Token Register is 2 bytes per port*/
 			WRITE_MEM32( ILB_CURRENT_TOKEN + 4*i , DEFAULT_ILB_UBOUND << UpperBound_OFFSET | DEFAULT_ILB_UBOUND );
-		
+
 	}
 
 	#if defined(CONFIG_RTL_HW_QOS_SUPPORT)
@@ -4472,7 +4866,7 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 		Init QUEUE Number configuration for RTL865xC : For Port 0~5 and CPU Port - All ports have 1 queue for each.
 	*/
 	{
-#if 1
+#if !defined(CONFIG_RTL_819XD	) && !defined(CONFIG_RTL_8196E)
 	/*	The default value was just as same as what we want	*/
 		rtl865xC_lockSWCore();
 
@@ -4504,7 +4898,7 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 					*/
 					rtl8651_setAsicQueueFlowControlConfigureRegister( port, queue, FALSE);
 					#endif
-				}	
+				}
 			}
 		}
 
@@ -4512,7 +4906,6 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 		rtl8651_resetAsicOutputQueue();
 		rtl865xC_unLockSWCore();
 #endif
-		
 		/* DSP bug (PHY-ID for DSP controller is set same as PHY 0 ) in RTL865xC A-Cut */
 		if(RTL865X_PHY6_DSP_BUG)
 		/* correct the default value of input queue flow control threshold */
@@ -4532,8 +4925,11 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 		rtl8651_setAsicPriorityDecision(2, 1, 1, 1, 1);
 
 		WRITE_MEM32(PBPCR, 0);
+
+#if !defined(CONFIG_RTL_819XD	) && !defined(CONFIG_RTL_8196E)
 		/* Set the threshold value for qos sytem */
 		_rtl865x_setQosThresholdByQueueIdx(QNUM_IDX_123);
+#endif
 
 	/*	clear dscp priority assignment, otherwise, pkt with dscp value 0 will be assign priority 1		*/
 		WRITE_MEM32(DSCPCR0,0);
@@ -4542,9 +4938,9 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 		WRITE_MEM32(DSCPCR3,0);
 		WRITE_MEM32(DSCPCR4,0);
 		WRITE_MEM32(DSCPCR5,0);
-		WRITE_MEM32(DSCPCR6,0);			
+		WRITE_MEM32(DSCPCR6,0);
 	}
-			
+
 
 #if defined(RTL865X_TEST) || defined(RTL865X_MODEL_USER)
 #if defined(VERA)||defined(VSV)||defined(MIILIKE)
@@ -4554,12 +4950,12 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 	memset( &naptIcmp, 0, sizeof(naptIcmp) );
 	naptIcmp.isCollision = 1;
 	for(flowTblIdx=0; flowTblIdx<RTL8651_ICMPTBL_SIZE; flowTblIdx++)
-		rtl8651_setAsicNaptIcmpTable( TRUE, flowTblIdx, &naptIcmp );	
+		rtl8651_setAsicNaptIcmpTable( TRUE, flowTblIdx, &naptIcmp );
 #endif
 #endif
 
-#if 1  
-#if defined(CONFIG_RTL_8198_NFBI_BOARD) 
+#if 1
+#if defined(CONFIG_RTL_8198_NFBI_BOARD)
     //WRITE_MEM32(PIN_MUX_SEL_2, 0); //for led control
 
 	REG32(PCRP0) &= (0xFFFFFFFF-(0x00000000|MacSwReset));
@@ -4575,30 +4971,41 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 	REG32(PCRP3) = REG32(PCRP3) | (3 << ExtPHYID_OFFSET) | EnablePHYIf | MacSwReset;
 	REG32(PCRP4) = REG32(PCRP4) | (4 << ExtPHYID_OFFSET) | EnablePHYIf | MacSwReset;
 
-	//port5 STP forwarding?	
+	//port5 STP forwarding?
 	REG32(PITCR) = REG32(PITCR) & 0xFFFFF3FF; //configure port 5 to be a MII interface
-	rtl865xC_setAsicEthernetMIIMode(5, LINK_MII_PHY); //port 5 MII PHY mode
+
+	// for EMI issue, use "GMII/MII MAC auto mode" instead
+	//rtl865xC_setAsicEthernetMIIMode(5, LINK_MII_PHY); //port 5 MII PHY mode
+	rtl865xC_setAsicEthernetMIIMode(5, LINK_MII_MAC); //port 5 MII MAC mode
 	REG32(P5GMIICR) = REG32(P5GMIICR) | 0x40; //Conf_done=1
-	
+
 #if defined(RTL8198_NFBI_PORT5_GMII) //GMII mode
            #define GMII_PIN_MUX 0xc0
            REG32(PIN_MUX_SEL)= REG32(PIN_MUX_SEL)&(~(GMII_PIN_MUX));
 	REG32(PCRP5) = 0 | (0x10<<ExtPHYID_OFFSET) |
 			EnForceMode| ForceLink|ForceSpeed1000M|ForceDuplex |
-			MIIcfg_RXER | EnablePHYIf | MacSwReset; 
+			MIIcfg_RXER | EnablePHYIf | MacSwReset;
 #else //MII mode
 	REG32(PCRP5) = 0 | (0x10<<ExtPHYID_OFFSET) |
 			EnForceMode| ForceLink|ForceSpeed100M |ForceDuplex |
-			MIIcfg_RXER | EnablePHYIf | MacSwReset; 
+			MIIcfg_RXER | EnablePHYIf | MacSwReset;
 #endif
 
-#elif defined(CONFIG_RTL_8198) 
+#elif defined(CONFIG_RTL_8198) || defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
 	#define GMII_PIN_MUX 0xf00
        #if defined(CONFIG_8198_PORT5_GMII) || defined(CONFIG_8198_PORT5_RGMII)
            REG32(PIN_MUX_SEL)= REG32(PIN_MUX_SEL)&(~(GMII_PIN_MUX));
        #endif
 	//WRITE_MEM32(PIN_MUX_SEL_2, 0);
 
+#ifdef CONFIG_RTK_VOIP_GIGABYTE_PHY_LINK_MODE_100 // use 10/100 only
+       WRITE_MEM32(PCRP0, READ_MEM32(PCRP0) & ~(1<<22));
+       WRITE_MEM32(PCRP1, READ_MEM32(PCRP1) & ~(1<<22));
+       WRITE_MEM32(PCRP2, READ_MEM32(PCRP2) & ~(1<<22));
+       WRITE_MEM32(PCRP3, READ_MEM32(PCRP3) & ~(1<<22));
+       WRITE_MEM32(PCRP4, READ_MEM32(PCRP4) & ~(1<<22));
+#endif
+
        WRITE_MEM32(PCRP0, READ_MEM32(PCRP0) & ~MacSwReset);
        WRITE_MEM32(PCRP1, READ_MEM32(PCRP1) & ~MacSwReset);
        WRITE_MEM32(PCRP2, READ_MEM32(PCRP2) & ~MacSwReset);
@@ -4610,7 +5017,7 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
        WRITE_MEM32(PCRP2, READ_MEM32(PCRP2) | ((2<<ExtPHYID_OFFSET)|EnablePHYIf|MacSwReset) ); /* Jumbo Frame */
        WRITE_MEM32(PCRP3, READ_MEM32(PCRP3) | ((3<<ExtPHYID_OFFSET)|EnablePHYIf|MacSwReset) ); /* Jumbo Frame */
        WRITE_MEM32(PCRP4, READ_MEM32(PCRP4) | ((4<<ExtPHYID_OFFSET)|EnablePHYIf|MacSwReset) ); /* Jumbo Frame */
-		
+
 #if defined(PORT5_RGMII_GMII)
 		if(ExtP5GigaPhyMode)
 		{
@@ -4648,6 +5055,103 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 	Set_GPHYWB(999, 0, 9, 0, 0);
 #endif
 
+#if defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
+	{
+	#define REG32_ANDOR(x,y,z)   (REG32(x)=(REG32(x)& (y))|(z))
+
+#if defined(CONFIG_RTL_8211DS_SUPPORT)&&defined(CONFIG_RTL_8197D)
+	int i;
+	uint32 reg_tmp=0;
+#endif
+
+	//unsigned int rtl96d_P0phymode=Get_P0_PhyMode();
+	unsigned int rtl96d_P0phymode=1;
+
+#if defined(CONFIG_RTL_8211DS_SUPPORT)&&defined(CONFIG_RTL_8197D)
+	rtl96d_P0phymode = 3;
+
+#elif defined(CONFIG_RTL_8196E) //mark_es
+	if ((REG32(BOND_OPTION) & BOND_ID_MASK) == BOND_8196ES) 
+		rtl96d_P0phymode=Get_P0_PhyMode();	
+#endif
+
+	if(rtl96d_P0phymode==1)  //embedded phy
+	{
+		REG32(PCRP0) |=  (0 << ExtPHYID_OFFSET) | EnablePHYIf | MacSwReset;	//emabedded
+	}
+	else //external phy
+	{
+		unsigned int rtl96d_P0miimode=Get_P0_MiiMode();
+
+	#if defined(CONFIG_RTL_8211DS_SUPPORT)&&defined(CONFIG_RTL_8197D)
+		rtl96d_P0miimode = 3;
+	#endif
+
+		REG32(PCRP0) |=  (0x10 << ExtPHYID_OFFSET) | MIIcfg_RXER |  EnablePHYIf | MacSwReset;	//external
+
+		if(rtl96d_P0miimode==0)
+			REG32_ANDOR(P0GMIICR, ~(3<<23)  , LINK_MII_PHY<<23);
+		else if(rtl96d_P0miimode==1)
+			REG32_ANDOR(P0GMIICR, ~(3<<23)  , LINK_MII_MAC<<23);
+		else if(rtl96d_P0miimode==2)
+			REG32_ANDOR(P0GMIICR, ~(3<<23)  , LINK_MII_MAC<<23);  //GMII
+		else if(rtl96d_P0miimode==3)
+			REG32_ANDOR(P0GMIICR, ~(3<<23)  , LINK_RGMII<<23);
+
+		if(rtl96d_P0miimode==3)
+		{
+			unsigned int rtl96d_P0txdly=Get_P0_TxDelay();
+			unsigned int rtl96d_P0rxdly=Get_P0_RxDelay();
+			REG32_ANDOR(P0GMIICR, ~((1<<4)|(3<<0)) , (rtl96d_P0txdly<<4) | (rtl96d_P0rxdly<<0) );
+
+			#if defined(CONFIG_RTL_8211DS_SUPPORT)&&defined(CONFIG_RTL_8197D)
+				//Set GPIOC0 to PHY reset.
+				REG32_ANDOR(0xb8000044, 0xFFFFFFFF, (1<<20));
+				REG32_ANDOR(0xb8003500, ~(1<<16), 0);
+				REG32_ANDOR(0xb8003508, 0xFFFFFFFF, (1<<16));
+				REG32_ANDOR(0xb800350c, ~(1<<16), 0); //Set Reset to low
+
+				REG32(0xbb804104) &=0x3FFFFFF;
+				REG32(0xbb804104) |=0x18FF0000;	//set PCR0 phyid
+				REG32(0xbb80414c) =0x37d55;			//set port MII
+				REG32(0xbb804100) =0x1;
+
+				for(i=0; i<5; i++)
+					REG32(PCRP0+i*4) |= (EnForceMode);
+
+				REG32_ANDOR(0xb800350c, 0xFFFFFFFF, (1<<16));//Set Reset to high
+				mdelay(30);
+				//__delay(50000);
+
+				rtl8651_getAsicEthernetPHYReg(0x6, 0, &reg_tmp);//Learning PHY ID
+
+				for(i=0; i<5; i++)
+					REG32(PCRP0+i*4) &= ~(EnForceMode);
+			#endif
+		}
+
+		if(rtl96d_P0miimode==0)
+			REG32_ANDOR(PCRP0, ~AutoNegoSts_MASK, EnForceMode| ForceLink|ForceSpeed100M |ForceDuplex) ;
+		else if(rtl96d_P0miimode==1)
+			REG32_ANDOR(PCRP0, ~AutoNegoSts_MASK, EnForceMode| ForceLink|ForceSpeed100M |ForceDuplex) ;
+		else if(rtl96d_P0miimode==2)
+			REG32_ANDOR(PCRP0, ~AutoNegoSts_MASK, EnForceMode| ForceLink|ForceSpeed1000M |ForceDuplex );
+		#if !defined(CONFIG_RTL_8211DS_SUPPORT)
+		else if(rtl96d_P0miimode==3)
+			REG32_ANDOR(PCRP0, ~AutoNegoSts_MASK, EnForceMode| ForceLink|ForceSpeed1000M |ForceDuplex );
+		#endif
+
+		//REG32(P0GMIICR) |=(Conf_done); //mark_es
+		REG32(PITCR) |= (1<<0);   //00: embedded , 01L GMII/MII/RGMII
+
+		if((rtl96d_P0miimode==2)  ||(rtl96d_P0miimode==3)) {
+			REG32(MACCR) |= (1<<12);   //giga link
+		}
+		REG32(P0GMIICR) |=(Conf_done); //mark_es
+	}
+	}
+#endif
+
 #elif defined(CONFIG_RTL_8196C)
     WRITE_MEM32(PCRP0, (READ_MEM32(PCRP0)&(0xFFFFFFFF-(0x00400000|MacSwReset))) );
 	TOGGLE_BIT_IN_REG_TWICE(PCRP0,EnForceMode);
@@ -4677,14 +5181,14 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
     WRITE_MEM32(PCRP2, (READ_MEM32(PCRP2)|(rtl8651AsicEthernetTable[2].phyId<<ExtPHYID_OFFSET)|EnablePHYIf ) ); /* Jumbo Frame */
     WRITE_MEM32(PCRP3, (READ_MEM32(PCRP3)|(rtl8651AsicEthernetTable[3].phyId<<ExtPHYID_OFFSET)|EnablePHYIf ) ); /* Jumbo Frame */
     WRITE_MEM32(PCRP4, (READ_MEM32(PCRP4)|(rtl8651AsicEthernetTable[4].phyId<<ExtPHYID_OFFSET)|EnablePHYIf ) ); /* Jumbo Frame */
-#endif 
+#endif
+
 
-			
     if (rtl8651_tblAsicDrvPara.externalPHYProperty & RTL8651_TBLASIC_EXTPHYPROPERTY_PORT5_RTL8211B)
     {
         WRITE_MEM32(PCRP5, ( (READ_MEM32(PCRP5))|(rtl8651AsicEthernetTable[5].phyId<<ExtPHYID_OFFSET)|EnablePHYIf ) ); /* Jumbo Frame */
     }
- 
+
 #if 0  /* No need to set PHYID of port 6. Just use ASIC default value. */
        /*  Due to MSb of phyid has been added an inverter in b-cut,
          *  although we want to set 6(0b00110) as phyid, we have to write 22(0b10110) instead. */
@@ -4698,11 +5202,11 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 #if defined(CONFIG_RTL865X_DIAG_LED)
 	/* diagnosis led (gpio-porta-6) on */
 	/* pull high by set portA-0(bit 30) as gpio-output-1, meaning: diag led OFF */
-	REG32(PABDAT) |=  0x40000000; 
+	REG32(PABDAT) |=  0x40000000;
 #endif /* CONFIG_RTL865X_DIAG_LED */
 
 #if !defined(CONFIG_RTL_8196C) && !defined(CONFIG_RTL_8198)
-	REG32(MDCIOCR) = 0x96181441;	// enable Giga port 8211B LED
+	//REG32(MDCIOCR) = 0x96181441;	// enable Giga port 8211B LED
 #endif
 
 	/*disable pattern match*/
@@ -4735,7 +5239,7 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 	// Configure LED-SIG0/LED-SIG1/LED-SIG2/LED-SIG3/LED-PHASE0/LED-PHASE1/LED-PHASE2/LED-PHASE3 PAD as LED-SW
 
 #ifndef CONFIG_POCKET_ROUTER_SUPPORT
-	REG32(PIN_MUX_SEL) &= ~(0xFFFF);	
+	REG32(PIN_MUX_SEL) &= ~(0xFFFF);
 #endif
 
 #if defined(PATCH_GPIO_FOR_LED)
@@ -4744,10 +5248,51 @@ int32 rtl865x_initAsicL2(rtl8651_tblAsic_InitPara_t *para)
 
 #endif  // end of defined(CONFIG_RTL_8196C)
 
-#if defined(CONFIG_RTL_8198)
+#if defined(CONFIG_RTL_8198) && !defined(CONFIG_RTL_819XD)
         REG32(MPMR) |= PM_MODE_ENABLE_AUTOMATIC_POWER_DOWN;
 #endif
 
+
+#if defined(CONFIG_RTL_8196E) && defined(CONFIG_RTL_ULINKER) /* disable unused port for saving power */
+	{
+	uint32 statCtrlReg0;
+
+	for (index=0; index<4; index++) {
+		/* read current PHY reg 0 value */
+		rtl8651_getAsicEthernetPHYReg( index, 0, &statCtrlReg0 );
+
+		REG32(PCRP0+(index*4)) |= EnForceMode;
+		statCtrlReg0 |= POWER_DOWN;
+
+		/* write PHY reg 0 */
+		rtl8651_setAsicEthernetPHYReg( index, 0, statCtrlReg0 );
+	}
+	}
+#endif
+#if  defined(CONFIG_RTL_8196E) //mark_es
+	if ((REG32(BOND_OPTION) & BOND_ID_MASK) == BOND_8196ES) 
+	{
+		uint32 statCtrlReg0;
+
+		for (index=1; index<5; index++) {
+		/* read current PHY reg 0 value */
+		rtl8651_getAsicEthernetPHYReg( index, 0, &statCtrlReg0 );
+
+		REG32(PCRP0+(index*4)) |= EnForceMode;
+		statCtrlReg0 |= POWER_DOWN;
+
+		/* write PHY reg 0 */
+		rtl8651_setAsicEthernetPHYReg( index, 0, statCtrlReg0 );              
+		}
+	}
+#endif	
+
+#if defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
+#if defined(PATCH_GPIO_FOR_LED)
+	REG32(PIN_MUX_SEL2) |= (0x3FFF);
+#endif
+#endif
+
 	return SUCCESS;
 }
 
@@ -4767,7 +5312,7 @@ int32 rtl8651_setAsicPortPatternMatch(uint32 port, uint32 pattern, uint32 patter
 		return SUCCESS;
 	}
 	if(operation>3)
-		return FAILED; //valid operations: 0(drop), 1(mirror to cpu),2(fwd to cpu), 3(to mirror port) 
+		return FAILED; //valid operations: 0(drop), 1(mirror to cpu),2(fwd to cpu), 3(to mirror port)
 	WRITE_MEM32(PPMAR,READ_MEM32(PPMAR)|((1<<(port+26))|(1<<13))); //turn on system pattern match and turn on pattern match on indicated port.
 	WRITE_MEM32(PPMAR,(READ_MEM32(PPMAR) & (~(0x3<<(14+2*port))))|(operation<<(14+2*port)));   //specify operation
 	WRITE_MEM32(PATP0+4*port,pattern);
@@ -4775,7 +5320,7 @@ int32 rtl8651_setAsicPortPatternMatch(uint32 port, uint32 pattern, uint32 patter
 	return SUCCESS;
 }
 
-int32 rtl8651_getAsicPortPatternMatch(uint32 port, uint32 *pattern, uint32 *patternMask, int32 *operation) 
+int32 rtl8651_getAsicPortPatternMatch(uint32 port, uint32 *pattern, uint32 *patternMask, int32 *operation)
 {
 	//not for ext port
 	if(port>=RTL8651_PORT_NUMBER)
@@ -4788,7 +5333,7 @@ int32 rtl8651_getAsicPortPatternMatch(uint32 port, uint32 *pattern, uint32 *patt
 		*patternMask=READ_MEM32(MASKP0+4*port);
 	if(operation)
 		*operation=(READ_MEM32(PPMAR)>>(14+2*port))&0x3;
-	return SUCCESS;		
+	return SUCCESS;
 }
 
 /*
@@ -4807,7 +5352,7 @@ int32 rtl8651_setAsicSpanningEnable(int8 spanningTreeEnabled)
 	if(spanningTreeEnabled == TRUE)
 	{
 		WRITE_MEM32(MSCR,READ_MEM32(MSCR)|(EN_STP));
-		WRITE_MEM32(RMACR ,READ_MEM32(RMACR)|MADDR00);		
+		WRITE_MEM32(RMACR ,READ_MEM32(RMACR)|MADDR00);
 
 	}else
 	{
@@ -4848,7 +5393,7 @@ Config IEEE 802.1D spanning tree port sate into ASIC.
 int32 rtl865xC_setAsicSpanningTreePortState(uint32 port, uint32 portState)
 {
 	uint32 offset = port * 4;
-	
+
 	if ( port >= RTL865XC_PORT_NUMBER )
 		return FAILED;
 
@@ -4888,7 +5433,7 @@ int32 rtl865xC_getAsicSpanningTreePortState(uint32 port, uint32 *portState)
 {
 	uint32 reg;
 	uint32 offset = port * 4;
-	
+
 	if ( port >= RTL865XC_PORT_NUMBER || portState == NULL )
 		return FAILED;
 
@@ -4929,7 +5474,7 @@ int32 rtl8651_setAsicMulticastSpanningTreePortState(uint32 port, uint32 portStat
 {
 #if 0	//Note: 96C/98 have remove these bits!!!
 	uint32 offset = port * 4;
-	
+
 	if ( port >= RTL865XC_PORT_NUMBER )
 	{
 		return FAILED;
@@ -4974,12 +5519,12 @@ int32 rtl8651_getAsicMulticastSpanningTreePortState(uint32 port, uint32 *portSta
 {
 	uint32 reg;
 	uint32 offset = port * 4;
-	
+
 	if ( port >= RTL865XC_PORT_NUMBER || portState == NULL )
 		return FAILED;
 
 	reg = ( READ_MEM32( PCRP0 + offset ) & IPMSTP_PortST_MASK );
-	
+
 	switch(reg)
 	{
 		case IPMSTP_PortST_DISABLE:
@@ -5007,15 +5552,15 @@ int32 rtl8651_getAsicMulticastSpanningTreePortState(uint32 port, uint32 *portSta
 int32 rtl8651_getAsicEthernetPHYReg(uint32 phyId, uint32 regId, uint32 *rData)
 {
 	uint32 status;
-	
+
 	WRITE_MEM32( MDCIOCR, COMMAND_READ | ( phyId << PHYADD_OFFSET ) | ( regId << REGADD_OFFSET ) );
 
 #if defined(CONFIG_RTL_8198)
-	if (REG32(REVR) == BSP_RTL8198_REVISION_A) 
-		mdelay(10);   
+	if (REG32(REVR) == BSP_RTL8198_REVISION_A)
+		mdelay(10);
 #elif defined(CONFIG_RTL8196C_REVISION_B)
 	if (REG32(REVR) == RTL8196C_REVISION_A)
-		mdelay(10);	//wei add, for 8196C revision A. mdio data read will delay 1 mdc clock.	
+		mdelay(10);	//wei add, for 8196C revision A. mdio data read will delay 1 mdc clock.
 #endif
 
 	do { status = READ_MEM32( MDCIOSR ); } while ( ( status & MDC_STATUS ) != 0 );
@@ -5038,7 +5583,7 @@ int32 rtl8651_setAsicEthernetPHYReg(uint32 phyId, uint32 regId, uint32 wData)
 int32 rtl8651_getAsicEthernetPHYStatus(uint32 port, uint32 *rData)
 {
 	uint32 statCtrlReg1, phyid;
-	
+
 	/* port number validation */
 	if (rtl8651_tblAsicDrvPara.externalPHYProperty & RTL8651_TBLASIC_EXTPHYPROPERTY_PORT5_RTL8211B)
 	{
@@ -5167,7 +5712,7 @@ int32 rtl8651_setAsicEthernetPHYAdvCapality(uint32 port, uint32 capality)
 
 	/*Clear Duplex and Speed bits*/
 	statCtrlReg4 &= ~(0xF<<5);
-	
+
 	if (capality & (1<<DUPLEX_100M))
 	{
 		statCtrlReg4 |= (1<<8);
@@ -5189,7 +5734,7 @@ int32 rtl8651_setAsicEthernetPHYAdvCapality(uint32 port, uint32 capality)
 		/*Set All Duplex and Speed All Supported*/
 		statCtrlReg4 |=(0xF <<5);
 	}
-	
+
 	/* write PHY reg 4 */
 	rtl8651_setAsicEthernetPHYReg( phyid, 4, statCtrlReg4 );
 
@@ -5325,7 +5870,7 @@ int32 rtl865xC_setAsicPortPauseFlowControl(uint32 port, uint8 rxEn, uint8 txEn)
 		pauseFC |= PauseFlowControlDtxErx;
 	if(txEn!=0)
 		pauseFC |= PauseFlowControlEtxDrx;
-	
+
 	WRITE_MEM32(PCRP0+offset, (~(PauseFlowControl_MASK)&(READ_MEM32(PCRP0+offset)))|pauseFC);
 
 	TOGGLE_BIT_IN_REG_TWICE(PCRP0 + offset,EnForceMode);
@@ -5336,7 +5881,7 @@ int32 rtl865xC_getAsicPortPauseFlowControl(uint32 port, uint8 *rxEn, uint8 *txEn
 {
 	uint32 offset = port<<2;
 	uint32 pauseFC = 0;
-	
+
 	pauseFC = ((PauseFlowControl_MASK)&(READ_MEM32(PCRP0+offset)));
 
 	if (pauseFC&PauseFlowControlDtxErx)
@@ -5353,7 +5898,7 @@ int32 rtl8651_asicEthernetCableMeterInit(void)
 {
 	rtlglue_printf("NOT YET\n");
 
-#if 0	
+#if 0
 	uint32 old_value;
 	//set PHY6 Reg0 TxD latch internal clock phase
 	WRITE_MEM32(SWTAA, 0xbc8020c0);
@@ -5419,7 +5964,7 @@ int32 rtl865xC_setAsicEthernetRGMIITiming(uint32 port, uint32 Tcomp, uint32 Rcom
 		return FAILED;
 	if ( Tcomp < RGMII_TCOMP_0NS || Tcomp > RGMII_TCOMP_7NS || Rcomp < RGMII_RCOMP_0NS || Rcomp > RGMII_RCOMP_2DOT5NS )
 		return FAILED;
-	
+
 	if ( port == 0 )
 	{
 		WRITE_MEM32(P0GMIICR, ( ( ( READ_MEM32(P0GMIICR) & ~RGMII_TCOMP_MASK ) | Tcomp ) & ~RGMII_RCOMP_MASK ) | Rcomp );
@@ -5440,7 +5985,7 @@ int32 rtl8651_setAsicEthernetMII(uint32 phyAddress, int32 mode, int32 enabled)
 		return FAILED;
 	if ( mode != P5_LINK_RGMII && mode != P5_LINK_MII_MAC && mode != P5_LINK_MII_PHY )
 		return FAILED;
-	
+
 	/* Configure driver level information about mii port 5 */
 	if ( enabled )
 	{
@@ -5467,8 +6012,90 @@ int32 rtl8651_getAsicEthernetMII(uint32 *phyAddress)
 }
 
 
+/*vlan remark*/
+int32 rtl8651_setAsicVlanRemark(enum PORTID port, enum PRIORITYVALUE priority, int remark)
+{
+	int regValue;
+	if ((port < PHY0) || (port > CPU) || (priority < PRI0) || (priority > PRI7) ||(remark < PRI0) || (remark > PRI7))
+		return FAILED;
+
+	WRITE_MEM32(RLRC, (READ_MEM32(RLRC) &~(0x7)) |0x7);
+
+	regValue = READ_MEM32(RMCR1P) & ~((0x7<<(3*priority)) |(0x1 << (24 + port)));
+	regValue |= ((remark << (3*priority)) |(0x1 << (24 + port)));
+	WRITE_MEM32( RMCR1P, regValue);
+
+	return SUCCESS;
+}
+
+int32 rtl8651_getAsicVlanRemark(enum PORTID port, enum PRIORITYVALUE priority, int* remark)
+{
+	int regValue;
+	if ((port < PHY0) || (port > CPU) || (priority < PRI0) || (priority > PRI7) ||(remark == NULL))
+		return FAILED;
+
+	regValue = READ_MEM32(RMCR1P);
+	if(regValue & (0x1<<(24+port))){
+		*remark = (regValue>>(3*priority)) & 0x7;
+	}else{
+			return FAILED;
+	}
+
+	return SUCCESS;
+}
+
+
+/*vlan remark*/
+int32 rtl8651_setAsicDscpRemark(enum PORTID port, enum PRIORITYVALUE priority, int remark)
+{
+	int regValue0;
+	if ((port < PHY0) || (port > CPU) || (priority < PRI0) || (priority > PRI7) ||(remark < 0) ||(remark > 63))
+		return FAILED;
+
+	WRITE_MEM32(RLRC, (READ_MEM32(RLRC) &~(0x7<<3)) |(0x7<<3));
+
+	if(priority < 5){
+		regValue0 = READ_MEM32(DSCPRM0) &~(0x3f<<(6*priority));
+		regValue0 |= remark << (6*priority);
+		WRITE_MEM32( DSCPRM0, regValue0);
+
+		regValue0 = READ_MEM32(DSCPRM1) &~(0x1 << (23 + port));
+		regValue0 |= 0x1 << (23 + port);
+		WRITE_MEM32( DSCPRM1, regValue0);
+	}else{
+		regValue0 = READ_MEM32(DSCPRM1) & ~((0x3f<<(6*(priority -5))) |(0x1 << (23 + port)));
+		regValue0 |= ((remark<<(6*(priority -5))) |(0x1 << (23 + port)));
+		WRITE_MEM32( DSCPRM1, regValue0);
+	}
+
+	return SUCCESS;
+}
+
+int32 rtl8651_getAsicDscpRemark(enum PORTID port, enum PRIORITYVALUE priority, int* remark)
+{
+	int regValue0, regValue1;
+	if ((port < PHY0) || (port > CPU) || (priority < PRI0) || (priority > PRI7) || (remark == NULL))
+		return FAILED;
+
+	regValue1 = READ_MEM32(DSCPRM1);
+	if(regValue1 & (0x1 << (23 + port))){
+		if(priority < 5){
+			regValue0 = READ_MEM32(DSCPRM0);
+			*remark = (regValue0>>(6*priority)) & 0x3f;
+		}else{
+			*remark = (regValue1>>(6*(priority-5))) & 0x3f;
+		}
+	}else{
+			return FAILED;
+	}
+
+	return SUCCESS;
+}
+
+
+
 /*=========================================
-  * ASIC DRIVER API: Packet Scheduling Control Register 
+  * ASIC DRIVER API: Packet Scheduling Control Register
   *=========================================*/
 /*
 @func int32 | rtl8651_setAsicPriorityDecision | set priority selection
@@ -5477,25 +6104,42 @@ int32 rtl8651_getAsicEthernetMII(uint32 *phyAddress)
 @parm uint32 | dscppri | output queue decision priority assign for DSCP Based Priority
 @parm uint32 | aclpri | output queue decision priority assign for ACL Based Priority.
 @parm uint32 | natpri | output queue decision priority assign for NAT Based Priority.
-@rvalue SUCCESS | 
+@rvalue SUCCESS |
 @rvalue FAILED | invalid parameter
 @comm
- */ 
+ */
 int32 rtl8651_setAsicPriorityDecision( uint32 portpri, uint32 dot1qpri, uint32 dscppri, uint32 aclpri, uint32 natpri )
 {
 	/* Invalid input parameter */
-	if ((portpri < 0) || (portpri > 0xF) || (dot1qpri < 0) || (dot1qpri > 0xF) || 
+	if ((portpri < 0) || (portpri > 0xF) || (dot1qpri < 0) || (dot1qpri > 0xF) ||
 		(dscppri < 0) || (dscppri > 0xF) || (aclpri < 0) || (aclpri > 0xF) ||
-		(natpri < 0) || (natpri > 0xF)) 
+		(natpri < 0) || (natpri > 0xF))
 		return FAILED;
 
-	WRITE_MEM32(QIDDPCR, (portpri << PBP_PRI_OFFSET) | (dot1qpri << BP8021Q_PRI_OFFSET) | 
-		                 (dscppri << DSCP_PRI_OFFSET) | (aclpri << ACL_PRI_OFFSET) | 
+	WRITE_MEM32(QIDDPCR, (portpri << PBP_PRI_OFFSET) | (dot1qpri << BP8021Q_PRI_OFFSET) |
+		                 (dscppri << DSCP_PRI_OFFSET) | (aclpri << ACL_PRI_OFFSET) |
 		                 (natpri << NAPT_PRI_OFFSET));
 
 	return SUCCESS;
 }
 
+int32 rtl8651_getAsicPriorityDecision( uint32* portpri, uint32* dot1qpri, uint32* dscppri, uint32* aclpri, uint32* natpri )
+{
+	uint32 temp;
+	if((portpri == NULL) ||(dot1qpri == NULL) ||(dscppri == NULL) ||(aclpri == NULL) ||(natpri == NULL))
+		return FAILED;
+
+	temp = READ_MEM32(QIDDPCR);
+	*portpri = (temp >> PBP_PRI_OFFSET) & 0xf;
+	*dot1qpri = (temp >> BP8021Q_PRI_OFFSET) & 0xf;
+	*dscppri = (temp >> DSCP_PRI_OFFSET) & 0xf;
+	*aclpri = (temp >> ACL_PRI_OFFSET) & 0xf;
+	*natpri  = (temp >> NAPT_PRI_OFFSET) & 0xf;
+
+	return SUCCESS;
+}
+
+
 int32 rtl8651_setAsicQueueFlowControlConfigureRegister(enum PORTID port, enum QUEUEID queue, uint32 enable)
 {
 	switch (port)
@@ -5554,7 +6198,7 @@ int32 rtl8651_getAsicQueueFlowControlConfigureRegister(enum PORTID port, enum QU
 @parm uint32 | token | Token is used for adding budget in each time slot.
 @parm uint32 | tick | Tick is used for time slot size slot.
 @parm uint32 | hiThreshold | leaky bucket token high-threshold register
-@rvalue SUCCESS | 
+@rvalue SUCCESS |
 @rvalue FAILED | invalid parameter
 @comm
  */
@@ -5572,7 +6216,7 @@ int32 rtl8651_setAsicLBParameter( uint32 token, uint32 tick, uint32 hiThreshold
 @parm uint32* | pToken | pointer to return token
 @parm uint32* | pTick | pointer to return tick
 @parm uint32* | pHiThreshold | pointer to return hiThreshold
-@rvalue SUCCESS | 
+@rvalue SUCCESS |
 @rvalue FAILED | invalid parameter
 @comm
  */
@@ -5588,19 +6232,43 @@ int32 rtl8651_getAsicLBParameter( uint32* pToken, uint32* pTick, uint32* pHiThre
 		*pTick = (regValue & Tick_MASK) >> Tick_OFFSET;
 	if (pHiThreshold != NULL)
 		*pHiThreshold = (READ_MEM32(ELBTTCR) & 0xFF) >> L2_OFFSET;
-	
+
 	return SUCCESS;
 }
 
+/*port based priority*/
+int32 rtl8651_setAsicPortBasedPriority( enum PORTID port, enum PRIORITYVALUE priority )
+{
+	/* Invalid input parameter */
+	if ((priority < PRI0) || (priority > PRI7) ||(port < PHY0) || (port> EXT3) )
+		return FAILED;
+
+	WRITE_MEM32(PBPCR, (READ_MEM32(PBPCR) & ~(0x7 << (port*3))) | (priority << (port*3)));
+
+	return SUCCESS;
+}
+
+int32 rtl8651_getAsicPortBasedPriority( enum PORTID port, enum PRIORITYVALUE* priority )
+{
+	/* Invalid input parameter */
+	if ((port < PHY0) || (port> EXT3) ||(priority == NULL))
+		return FAILED;
+
+	*priority = (READ_MEM32(PBPCR) >> (3*port)) & 0x7;
+
+	return SUCCESS;
+}
+
+
 
 /*
 @func int32 | rtl8651_setAsicQueueRate | set per queue rate
 @parm enum PORTID | port | the port number
 @parm enum QUEUEID | queueid | the queue ID wanted to set
-@parm uint32 | pprTime | Peak Packet Rate (in times of APR). 0~6: PPR = (2^pprTime)*apr. 7: disable PPR 
+@parm uint32 | pprTime | Peak Packet Rate (in times of APR). 0~6: PPR = (2^pprTime)*apr. 7: disable PPR
 @parm uint32 | aprBurstSize | Bucket Burst Size of Average Packet Rate (unit: 1KByte). 0xFF: disable
 @parm uint32 | apr | Average Packet Rate (unit: 64Kbps). 0x3FFF: unlimited rate
-@rvalue SUCCESS | 
+@rvalue SUCCESS |
 @rvalue FAILED | invalid parameter
 @comm
  */
@@ -5624,10 +6292,10 @@ int32 rtl8651_setAsicQueueRate( enum PORTID port, enum QUEUEID queueid, uint32 p
 @func int32 | rtl8651_getAsicQueueRate | get per queue rate configuration
 @parm enum PORTID | port | the port number
 @parm enum QUEUEID | queueid | the queue ID wanted to set
-@parm uint32* | pPprTime | pointer to Peak Packet Rate (in times of APR). 0~6: PPR = (2^pprTime)*apr. 7: disable PPR 
+@parm uint32* | pPprTime | pointer to Peak Packet Rate (in times of APR). 0~6: PPR = (2^pprTime)*apr. 7: disable PPR
 @parm uint32* | pAprBurstSize | pointer to APR Burst Size (unit: 1KBytes). 0xff: disable
 @parm uint32* | pApr | pointer to Average Packet Rate (unit: 64Kbps). 0x3FFF: unlimited rate
-@rvalue SUCCESS | 
+@rvalue SUCCESS |
 @rvalue FAILED | invalid parameter
 @comm
  */
@@ -5654,7 +6322,7 @@ int32 rtl8651_getAsicQueueRate( enum PORTID port, enum QUEUEID queueid, uint32*
 @func int32 | rtl8651_setAsicPortIngressBandwidth | set per-port total ingress bandwidth
 @parm enum PORTID | port | the port number
 @parm uint32 | bandwidth | the total ingress bandwidth (unit: 16Kbps), 0:disable
-@rvalue SUCCESS | 
+@rvalue SUCCESS |
 @rvalue FAILED | invalid parameter
 @comm
  */
@@ -5684,7 +6352,7 @@ int32 rtl8651_setAsicPortIngressBandwidth( enum PORTID port, uint32 bandwidth)
 @func int32 | rtl8651_getAsicPortIngressBandwidth | get per-port total ingress bandwidth
 @parm enum PORTID | port | the port number
 @parm uint32* | pBandwidth | pointer to the returned total ingress bandwidth (unit: 16Kbps), 0:disable
-@rvalue SUCCESS | 
+@rvalue SUCCESS |
 @rvalue FAILED | invalid parameter
 @comm
  */
@@ -5714,8 +6382,8 @@ int32 rtl8651_getAsicPortIngressBandwidth( enum PORTID port, uint32* pBandwidth
 /*
 @func int32 | rtl8651_setAsicPortEgressBandwidth | set per-port total egress bandwidth
 @parm enum PORTID | port | the port number
-@parm uint32 | bandwidth | the total egress bandwidth (unit: 64kbps). 0x3FFF: disable 
-@rvalue SUCCESS | 
+@parm uint32 | bandwidth | the total egress bandwidth (unit: 64kbps). 0x3FFF: disable
+@rvalue SUCCESS |
 @rvalue FAILED | invalid parameter
 @comm
  */
@@ -5723,12 +6391,24 @@ int32 rtl8651_setAsicPortEgressBandwidth( enum PORTID port, uint32 bandwidth )
 {
 	uint32 reg1;
 
+#ifdef CONFIG_RTK_VOIP_QOS
+	extern int wan_port_check(int port);
+#endif
 	if ((port < PHY0) || (port > CPU))
 		return FAILED;
 
 	reg1 = WFQRCRP0 + (port * 0xC);  /* offset to get corresponding register */
 	WRITE_MEM32( reg1, (READ_MEM32(reg1) & ~(APR_MASK)) | (bandwidth << APR_OFFSET));
 
+#ifdef CONFIG_RTK_VOIP_QOS
+	if(wan_port_check(port))
+	{
+		if(bandwidth < 161 )
+			rtl8651_cpu_tx_fc(0);//disable flow control
+		else
+			rtl8651_cpu_tx_fc(1);//enable flow control
+	}
+#endif
 	return SUCCESS;
 }
 
@@ -5736,8 +6416,8 @@ int32 rtl8651_setAsicPortEgressBandwidth( enum PORTID port, uint32 bandwidth )
 /*
 @func int32 | rtl8651_getAsicPortEgressBandwidth | get per-port total egress bandwidth
 @parm enum PORTID | port | the port number
-@parm uint32* | pBandwidth | pointer to the returned total egress bandwidth (unit: 64kbps). 0x3FFF: disable 
-@rvalue SUCCESS | 
+@parm uint32* | pBandwidth | pointer to the returned total egress bandwidth (unit: 64kbps). 0x3FFF: disable
+@rvalue SUCCESS |
 @rvalue FAILED | invalid parameter
 @comm
  */
@@ -5757,6 +6437,42 @@ int32 rtl8651_getAsicPortEgressBandwidth( enum PORTID port, uint32* pBandwidth )
 	return SUCCESS;
 }
 
+/*set queue type as STRICT*/
+int32 rtl8651_setAsicQueueStrict( enum PORTID port, enum QUEUEID queueid, enum QUEUETYPE queueType)
+{
+	uint32 reg1, regOFFSET, regValue;
+
+	if ((port < PHY0) || (port > CPU) || (queueid < QUEUE0) || (queueid > QUEUE5))
+		return FAILED;
+	if ((queueType < STR_PRIO) || (queueType > WFQ_PRIO))
+		return FAILED;
+
+	reg1 = WFQWCR0P0 + (port * 0xC) + ((queueid >> 2) * 0x4);	/* offset to get corresponding register */
+	regOFFSET = (queueid % 4) * 0x8;	/* used to offset register value */
+
+	regValue = READ_MEM32(reg1) & ~((WEIGHT0_MASK | SCHE0_MASK) << regOFFSET);
+	regValue |= (queueType << (SCHE0_OFFSET + regOFFSET));
+	WRITE_MEM32( reg1, regValue);
+	return SUCCESS;
+}
+
+int32 rtl8651_getAsicQueueStrict( enum PORTID port, enum QUEUEID queueid, enum QUEUETYPE *pQueueType)
+{
+	uint32 reg1, regOFFSET, regValue;
+
+	if ((port < PHY0) || (port > CPU) || (queueid < QUEUE0) || (queueid > QUEUE5) ||(pQueueType == NULL))
+		return FAILED;
+
+	reg1 = WFQWCR0P0 + (port * 0xC) + ((queueid >> 2) * 0x4);  /* offset to get corresponding register */
+	regOFFSET = (queueid % 4) * 0x8;	/* used to offset register value */
+	regValue = READ_MEM32(reg1);
+
+	if (pQueueType != NULL)
+		*pQueueType = ((regValue & (SCHE0_MASK << regOFFSET)) >> SCHE0_OFFSET) >> regOFFSET;
+
+	return SUCCESS;
+}
+
 
 /*
 @func int32 | rtl8651_setAsicQueueWeight | set WFQ weighting
@@ -5764,21 +6480,23 @@ int32 rtl8651_getAsicPortEgressBandwidth( enum PORTID port, uint32* pBandwidth )
 @parm enum QUEUEID | queueid | the queue ID wanted to set
 @parm enum QUEUETYPE | queueType | the specified queue type
 @parm uint32 | weight | the weight value wanted to set (valid:0~127)
-@rvalue SUCCESS | 
+@rvalue SUCCESS |
 @rvalue FAILED | invalid parameter
 @comm
  */
 int32 rtl8651_setAsicQueueWeight( enum PORTID port, enum QUEUEID queueid, enum QUEUETYPE queueType, uint32 weight )
 {
 	uint32 reg1, regOFFSET, regValue;
-	
+
 	if ((port < PHY0) || (port > CPU) || (queueid < QUEUE0) || (queueid > QUEUE5))
 		return FAILED;
 	if ((queueType < STR_PRIO) || (queueType > WFQ_PRIO))
 		return FAILED;
+	if((weight < 0) && (weight > 127))
+		return FAILED;
 
 	reg1 = WFQWCR0P0 + (port * 0xC) + ((queueid >> 2) * 0x4);	/* offset to get corresponding register */
-	regOFFSET = (queueid % 4) * 0x8;	/* used to offset register value */	 	 
+	regOFFSET = (queueid % 4) * 0x8;	/* used to offset register value */
 
 	regValue = READ_MEM32(reg1) & ~((WEIGHT0_MASK | SCHE0_MASK) << regOFFSET);
 	regValue |= ((queueType << (SCHE0_OFFSET + regOFFSET)) | (weight << (WEIGHT0_OFFSET + regOFFSET)));
@@ -5793,19 +6511,19 @@ int32 rtl8651_setAsicQueueWeight( enum PORTID port, enum QUEUEID queueid, enum Q
 @parm enum QUEUEID | queueid | the queue ID wanted to set
 @parm enum QUEUETYPE* | pQueueType | pointer to the returned queue type
 @parm uint32* | pWeight | pointer to the returned weight value
-@rvalue SUCCESS | 
+@rvalue SUCCESS |
 @rvalue FAILED | invalid parameter
 @comm
  */
 int32 rtl8651_getAsicQueueWeight( enum PORTID port, enum QUEUEID queueid, enum QUEUETYPE *pQueueType, uint32 *pWeight )
 {
 	uint32 reg1, regOFFSET, regValue;
-	
+
 	if ((port < PHY0) || (port > CPU) || (queueid < QUEUE0) || (queueid > QUEUE5))
 		return FAILED;
 
 	reg1 = WFQWCR0P0 + (port * 0xC) + ((queueid >> 2) * 0x4);  /* offset to get corresponding register */
-	regOFFSET = (queueid % 4) * 0x8;	/* used to offset register value */	 
+	regOFFSET = (queueid % 4) * 0x8;	/* used to offset register value */
 	regValue = READ_MEM32(reg1);
 
 	if (pQueueType != NULL)
@@ -5820,7 +6538,7 @@ int32 rtl8651_getAsicQueueWeight( enum PORTID port, enum QUEUEID queueid, enum Q
 @func int32 | rtl8651_setAsicOutputQueueNumber | set output queue number for a specified port
 @parm enum PORTID | port | the port number (valid: physical ports(0~5) and CPU port(6) )
 @parm enum QUEUENUM | qnum | the output queue number
-@rvalue SUCCESS | 
+@rvalue SUCCESS |
 @rvalue FAILED | invalid parameter
 @comm
  */
@@ -5831,11 +6549,12 @@ int32 rtl8651_setAsicOutputQueueNumber( enum PORTID port, enum QUEUENUM qnum )
 	enum QUEUENUM orgQnum;
 
 	if ((port < PHY0) || (port > CPU) || (qnum < QNUM1) || (qnum > QNUM6))
-		return FAILED; 
+		return FAILED;
 
 	orgQnum = (READ_MEM32(QNUMCR) >> (3*port)) & 0x7;
 	WRITE_MEM32(QNUMCR, (READ_MEM32(QNUMCR) & ~(0x7 << (3*port))) | (qnum << (3*port)));
 
+#if !defined(CONFIG_RTL_819XD	) && !defined(CONFIG_RTL_8196E)
 	if (qnum==6)
 	{
 		if (orgQnum!=6)
@@ -5851,6 +6570,7 @@ int32 rtl8651_setAsicOutputQueueNumber( enum PORTID port, enum QUEUENUM qnum )
 		if(orgQnum>3)
 			_rtl865x_setQosThresholdByQueueIdx(QNUM_IDX_123);
 	}
+#endif
 
 	return SUCCESS;
 }
@@ -5860,7 +6580,7 @@ int32 rtl8651_setAsicOutputQueueNumber( enum PORTID port, enum QUEUENUM qnum )
 @func int32 | rtl8651_getAsicOutputQueueNumber | get output queue number for a specified port
 @parm enum PORTID | port | the port number (valid: physical ports(0~5) and CPU port(6) )
 @parm enum QUEUENUM | qnum | the output queue number
-@rvalue SUCCESS | 
+@rvalue SUCCESS |
 @rvalue FAILED | invalid parameter
 @comm
  */
@@ -5868,11 +6588,11 @@ int32 rtl8651_getAsicOutputQueueNumber( enum PORTID port, enum QUEUENUM *qnum )
 {
 	/* Invalid input parameter */
 	if ((port < PHY0) || (port > CPU))
-		return FAILED; 
+		return FAILED;
 
 	if (qnum != NULL)
 		*qnum = (READ_MEM32(QNUMCR) >> (3*port)) & 0x7;
-	
+
 	return SUCCESS;
 }
 
@@ -5881,19 +6601,19 @@ int32 rtl8651_getAsicOutputQueueNumber( enum PORTID port, enum QUEUENUM *qnum )
 @parm enum QUEUENUM | qnum | the output queue number
 @parm enum PRIORITYVALUE | priority | priority
 @parm enum QUEUEID | qid | queue ID
-@rvalue SUCCESS | 
+@rvalue SUCCESS |
 @rvalue FAILED | invalid parameter
 @comm
  */
 int32 rtl8651_setAsicPriorityToQIDMappingTable( enum QUEUENUM qnum, enum PRIORITYVALUE priority, enum QUEUEID qid )
 {
 	/* Invalid input parameter */
-	if ((priority < PRI0) || (priority > PRI7)) 
+	if ((priority < PRI0) || (priority > PRI7))
 		return FAILED;
-	if ((qid < QUEUE0) || (qid > QUEUE5)) 
+	if ((qid < QUEUE0) || (qid > QUEUE5))
 		return FAILED;
 
-	switch (qnum) 
+	switch (qnum)
 	{
 		case QNUM1:
 			WRITE_MEM32(UPTCMCR0, (READ_MEM32(UPTCMCR0) & ~(0x7 << (priority*3))) | (qid << (priority*3))); break;
@@ -5907,19 +6627,53 @@ int32 rtl8651_setAsicPriorityToQIDMappingTable( enum QUEUENUM qnum, enum PRIORIT
 			WRITE_MEM32(UPTCMCR4, (READ_MEM32(UPTCMCR4) & ~(0x7 << (priority*3))) | (qid << (priority*3))); break;
 		case QNUM6:
 			WRITE_MEM32(UPTCMCR5, (READ_MEM32(UPTCMCR5) & ~(0x7 << (priority*3))) | (qid << (priority*3))); break;
-		default: 
+		default:
 			return FAILED;
 	}
 
 	return SUCCESS;
 }
 
+int32 rtl8651_getAsicPriorityToQIDMappingTable( enum QUEUENUM qnum, enum PRIORITYVALUE priority, enum QUEUEID* qid )
+{
+	/* Invalid input parameter */
+	if ((priority < PRI0) || (priority > PRI7) ||(qid == NULL))
+		return FAILED;
+
+	switch (qnum)
+	{
+		case QNUM1:
+			*qid = (READ_MEM32(UPTCMCR0) >> (priority*3)) & 0x7;
+			break;
+		case QNUM2:
+			*qid = (READ_MEM32(UPTCMCR1) >> (priority*3)) & 0x7;
+			break;
+		case QNUM3:
+			*qid = (READ_MEM32(UPTCMCR2) >> (priority*3)) & 0x7;
+			break;
+		case QNUM4:
+			*qid = (READ_MEM32(UPTCMCR3) >> (priority*3)) & 0x7;
+			break;
+		case QNUM5:
+			*qid = (READ_MEM32(UPTCMCR4) >> (priority*3)) & 0x7;
+			break;
+		case QNUM6:
+			*qid = (READ_MEM32(UPTCMCR5) >> (priority*3)) & 0x7;
+			break;
+		default:
+			return FAILED;
+	}
+
+	return SUCCESS;
+}
+
+
 /*
 @func int32 | rtl8651_setAsicCPUPriorityToQIDMappingTable | set user priority to QID mapping table parameter based on destination port & priority information
 @parm enum PORTID | port | the destination port
 @parm enum PRIORITYVALUE | priority | priority
 @parm enum QUEUEID | qid | queue ID
-@rvalue SUCCESS | 
+@rvalue SUCCESS |
 @rvalue FAILED | invalid parameter
 @comm
  */
@@ -5927,16 +6681,16 @@ int32 rtl8651_setAsicCPUPriorityToQIDMappingTable( enum PORTID port, enum PRIORI
 {
 	uint32	reg;
 	/* Invalid input parameter */
-	if ((priority < PRI0) || (priority > PRI7)) 
+	if ((priority < PRI0) || (priority > PRI7))
 		return FAILED;
-	if ((qid < QUEUE0) || (qid > QUEUE5)) 
+	if ((qid < QUEUE0) || (qid > QUEUE5))
 		return FAILED;
 	if (port<CPU || port>MULTEXT)
 		return FAILED;
 
 	reg = (uint32)(((uint32*)CPUQIDMCR0) + (port-CPU));
 
-	WRITE_MEM32(reg, (READ_MEM32(reg) & ~(0x7 << (priority<<2))) | (qid << (priority<<2))); 
+	WRITE_MEM32(reg, (READ_MEM32(reg) & ~(0x7 << (priority<<2))) | (qid << (priority<<2)));
 
 	return SUCCESS;
 }
@@ -5945,12 +6699,12 @@ int32 rtl8651_setAsicCPUPriorityToQIDMappingTable( enum PORTID port, enum PRIORI
 int32 rtl8651_setAsicSystemBasedFlowControlRegister(uint32 sharedON, uint32 sharedOFF, uint32 fcON, uint32 fcOFF, uint32 drop)
 {
 	/* Invalid input parameter */
-	if ((sharedON > (SDC_FCON_MASK >> SDC_FCON_OFFSET)) || 
-		(sharedOFF > (S_DSC_FCOFF_MASK >> S_DSC_FCOFF_OFFSET)) || 
-		(fcON > ((S_Max_SBuf_FCON_MASK >> S_Max_SBuf_FCON_OFFSET))) || 
-		(fcOFF > (S_Max_SBuf_FCOFF_MASK >> S_Max_SBuf_FCOFF_OFFSET)) || 
+	if ((sharedON > (SDC_FCON_MASK >> SDC_FCON_OFFSET)) ||
+		(sharedOFF > (S_DSC_FCOFF_MASK >> S_DSC_FCOFF_OFFSET)) ||
+		(fcON > ((S_Max_SBuf_FCON_MASK >> S_Max_SBuf_FCON_OFFSET))) ||
+		(fcOFF > (S_Max_SBuf_FCOFF_MASK >> S_Max_SBuf_FCOFF_OFFSET)) ||
 		(drop > (S_DSC_RUNOUT_MASK >> S_DSC_RUNOUT_OFFSET)))
-		return FAILED; 
+		return FAILED;
 
 	WRITE_MEM32(SBFCR0, (READ_MEM32(SBFCR0) & ~(S_DSC_RUNOUT_MASK)) | (drop << S_DSC_RUNOUT_OFFSET));
 	WRITE_MEM32(SBFCR1, (READ_MEM32(SBFCR1) & ~(S_DSC_FCON_MASK | S_DSC_FCOFF_MASK)) | ( fcON<< S_DSC_FCON_OFFSET) | (fcOFF << S_DSC_FCOFF_OFFSET));
@@ -5962,26 +6716,26 @@ int32 rtl8651_setAsicQueueDescriptorBasedFlowControlRegister(enum PORTID port, e
 {
 	/* Invalid input parameter */
 	if ((port < PHY0) || (port > CPU))
-		return FAILED; 
+		return FAILED;
 
-	if ((fcON > (QG_DSC_FCON_MASK >> QG_DSC_FCON_OFFSET)) || 
+	if ((fcON > (QG_DSC_FCON_MASK >> QG_DSC_FCON_OFFSET)) ||
 		(fcOFF > (QG_DSC_FCOFF_MASK >> QG_DSC_FCOFF_OFFSET)))
-		return FAILED; 
+		return FAILED;
 
 
 	switch (queue)
 	{
 		case QUEUE0:
-			WRITE_MEM32((QDBFCRP0G0+(port*0xC)), (READ_MEM32(QDBFCRP0G0+(port*0xC)) & ~(QG_DSC_FCON_MASK | QG_DSC_FCOFF_MASK)) | (fcON << QG_DSC_FCON_OFFSET) | (fcOFF << QG_DSC_FCOFF_OFFSET)); 		
+			WRITE_MEM32((QDBFCRP0G0+(port*0xC)), (READ_MEM32(QDBFCRP0G0+(port*0xC)) & ~(QG_DSC_FCON_MASK | QG_DSC_FCOFF_MASK)) | (fcON << QG_DSC_FCON_OFFSET) | (fcOFF << QG_DSC_FCOFF_OFFSET));
 			break;
 		case QUEUE1:
 		case QUEUE2:
 		case QUEUE3:
-		case QUEUE4:		
-			WRITE_MEM32((QDBFCRP0G1+(port*0xC)), (READ_MEM32(QDBFCRP0G1+(port*0xC)) & ~(QG_DSC_FCON_MASK | QG_DSC_FCOFF_MASK)) | (fcON << QG_DSC_FCON_OFFSET) | (fcOFF << QG_DSC_FCOFF_OFFSET)); 		
+		case QUEUE4:
+			WRITE_MEM32((QDBFCRP0G1+(port*0xC)), (READ_MEM32(QDBFCRP0G1+(port*0xC)) & ~(QG_DSC_FCON_MASK | QG_DSC_FCOFF_MASK)) | (fcON << QG_DSC_FCON_OFFSET) | (fcOFF << QG_DSC_FCOFF_OFFSET));
 			break;
 		case QUEUE5:
-			WRITE_MEM32((QDBFCRP0G2+(port*0xC)), (READ_MEM32(QDBFCRP0G2+(port*0xC)) & ~(QG_DSC_FCON_MASK | QG_DSC_FCOFF_MASK)) | (fcON << QG_DSC_FCON_OFFSET) | (fcOFF << QG_DSC_FCOFF_OFFSET)); 		
+			WRITE_MEM32((QDBFCRP0G2+(port*0xC)), (READ_MEM32(QDBFCRP0G2+(port*0xC)) & ~(QG_DSC_FCON_MASK | QG_DSC_FCOFF_MASK)) | (fcON << QG_DSC_FCON_OFFSET) | (fcOFF << QG_DSC_FCOFF_OFFSET));
 			break;
 		default:
 			return FAILED;
@@ -5993,25 +6747,25 @@ int32 rtl8651_setAsicQueuePacketBasedFlowControlRegister(enum PORTID port, enum
 {
 	/* Invalid input parameter */
 	if ((port < PHY0) || (port > CPU))
-		return FAILED; 
+		return FAILED;
 
-	if ((fcON > (QG_QLEN_FCON_MASK>> QG_QLEN_FCON_OFFSET)) || 
+	if ((fcON > (QG_QLEN_FCON_MASK>> QG_QLEN_FCON_OFFSET)) ||
 		(fcOFF > (QG_QLEN_FCOFF_MASK >> QG_QLEN_FCOFF_OFFSET)))
-		return FAILED; 
+		return FAILED;
 
 	switch (queue)
 	{
 		case QUEUE0:
-			WRITE_MEM32((QPKTFCRP0G0+(port*0xC)), (READ_MEM32(QPKTFCRP0G0+(port*0xC)) & ~(QG_QLEN_FCON_MASK | QG_QLEN_FCOFF_MASK)) | (fcON << QG_QLEN_FCON_OFFSET) | (fcOFF << QG_QLEN_FCOFF_OFFSET)); 		
+			WRITE_MEM32((QPKTFCRP0G0+(port*0xC)), (READ_MEM32(QPKTFCRP0G0+(port*0xC)) & ~(QG_QLEN_FCON_MASK | QG_QLEN_FCOFF_MASK)) | (fcON << QG_QLEN_FCON_OFFSET) | (fcOFF << QG_QLEN_FCOFF_OFFSET));
 			break;
 		case QUEUE1:
 		case QUEUE2:
 		case QUEUE3:
-		case QUEUE4:		
-			WRITE_MEM32((QPKTFCRP0G1+(port*0xC)), (READ_MEM32(QPKTFCRP0G1+(port*0xC)) & ~(QG_QLEN_FCON_MASK | QG_QLEN_FCOFF_MASK)) | (fcON << QG_QLEN_FCON_OFFSET) | (fcOFF << QG_QLEN_FCOFF_OFFSET)); 
+		case QUEUE4:
+			WRITE_MEM32((QPKTFCRP0G1+(port*0xC)), (READ_MEM32(QPKTFCRP0G1+(port*0xC)) & ~(QG_QLEN_FCON_MASK | QG_QLEN_FCOFF_MASK)) | (fcON << QG_QLEN_FCON_OFFSET) | (fcOFF << QG_QLEN_FCOFF_OFFSET));
 			break;
 		case QUEUE5:
-			WRITE_MEM32((QPKTFCRP0G2+(port*0xC)), (READ_MEM32(QPKTFCRP0G2+(port*0xC)) & ~(QG_QLEN_FCON_MASK | QG_QLEN_FCOFF_MASK)) | (fcON << QG_QLEN_FCON_OFFSET) | (fcOFF << QG_QLEN_FCOFF_OFFSET)); 
+			WRITE_MEM32((QPKTFCRP0G2+(port*0xC)), (READ_MEM32(QPKTFCRP0G2+(port*0xC)) & ~(QG_QLEN_FCON_MASK | QG_QLEN_FCOFF_MASK)) | (fcON << QG_QLEN_FCON_OFFSET) | (fcOFF << QG_QLEN_FCOFF_OFFSET));
 			break;
 		default:
 			return FAILED;
@@ -6023,9 +6777,9 @@ int32 rtl8651_setAsicQueuePacketBasedFlowControlRegister(enum PORTID port, enum
 int32 rtl8651_setAsicPortBasedFlowControlRegister(enum PORTID port, uint32 fcON, uint32 fcOFF)
 {
 	/* Invalid input parameter */
-	if ((fcON > (P_MaxDSC_FCON_MASK >> P_MaxDSC_FCON_OFFSET)) || 
+	if ((fcON > (P_MaxDSC_FCON_MASK >> P_MaxDSC_FCON_OFFSET)) ||
 		(fcOFF > (P_MaxDSC_FCOFF_MASK >> P_MaxDSC_FCOFF_OFFSET)))
-		return FAILED; 
+		return FAILED;
 
 	switch (port)
 	{
@@ -6056,11 +6810,54 @@ int32 rtl8651_setAsicPerQueuePhysicalLengthGapRegister(uint32 gap)
 	if (gap > (QLEN_GAP_MASK >> QLEN_GAP_OFFSET))
 		return FAILED;
 
-	WRITE_MEM32(PQPLGR, (READ_MEM32(PQPLGR) & ~(QLEN_GAP_MASK)) | (gap << QLEN_GAP_OFFSET)); 		
+	WRITE_MEM32(PQPLGR, (READ_MEM32(PQPLGR) & ~(QLEN_GAP_MASK)) | (gap << QLEN_GAP_OFFSET));
 	return SUCCESS;
 }
 
+/* 	note: the dynamic mechanism: adjust the flow control threshold value according to the number of Ethernet link up ports.
+	buffer threshold setting:
+	sys on = 208, share on = 192 for link port <=3
+	0xbb804504 = 0x00c000d0
+	0xbb804508 = 0x00b000c0
+
+	sys on = 172, share on = 98 , for link port > 3
+	0xbb804504 = 0x00A000AC
+	0xbb804508 = 0x004A0062
 
+	1. default threshold setting is link port <=3
+	2. got link change interrupt and link port > 3, then change threhosld for link port > 3
+	3. got link change interrupt and link port <= 3, then change threhosld for link port <= 3
+ */
+
+#ifdef CONFIG_RTL_8197D_DYN_THR
+int32 rtl819x_setQosThreshold(uint32 old_sts, uint32 new_sts)
+{
+	int32 i, link_up_ports=0;
+	uint32 j = new_sts;
+
+	if (old_sts == new_sts)
+		return SUCCESS;
+
+	for (i=0; i<5; i++) {
+		if ((j & 0x1) == 1)
+			link_up_ports++;
+		j = j >> 1;
+	}
+
+	if (link_up_ports <= DYN_THR_LINK_UP_PORTS) {
+		WRITE_MEM32(SBFCR1, (READ_MEM32(SBFCR1) & ~(S_DSC_FCON_MASK | S_DSC_FCOFF_MASK)) | ( DYN_THR_AGG_fcON<< S_DSC_FCON_OFFSET) | (DYN_THR_AGG_fcOFF << S_DSC_FCOFF_OFFSET));
+		WRITE_MEM32(SBFCR2, (READ_MEM32(SBFCR2) & ~(S_Max_SBuf_FCON_MASK | S_Max_SBuf_FCOFF_MASK)) | (DYN_THR_AGG_sharedON << S_Max_SBuf_FCON_OFFSET) | (DYN_THR_AGG_sharedOFF << S_Max_SBuf_FCOFF_OFFSET));
+	}
+	else {
+		WRITE_MEM32(SBFCR1, (READ_MEM32(SBFCR1) & ~(S_DSC_FCON_MASK | S_DSC_FCOFF_MASK)) | ( DYN_THR_DEF_fcON<< S_DSC_FCON_OFFSET) | (DYN_THR_DEF_fcOFF << S_DSC_FCOFF_OFFSET));
+		WRITE_MEM32(SBFCR2, (READ_MEM32(SBFCR2) & ~(S_Max_SBuf_FCON_MASK | S_Max_SBuf_FCOFF_MASK)) | (DYN_THR_DEF_sharedON << S_Max_SBuf_FCON_OFFSET) | (DYN_THR_DEF_sharedOFF << S_Max_SBuf_FCOFF_OFFSET));
+	}
+
+	return SUCCESS;
+}
+#endif
+
+#if !defined(CONFIG_RTL_819XD	) && !defined(CONFIG_RTL_8196E)
 static int32 _rtl865x_setQosThresholdByQueueIdx(uint32 qidx)
 {
 	/* Set the threshold value for qos sytem */
@@ -6074,7 +6871,7 @@ static int32 _rtl865x_setQosThresholdByQueueIdx(uint32 qidx)
 		rtlglue_printf("Set System Base Flow Control Para Error.\n");
 		return retval;
 	}
-	
+
 	for(i =0; i < RTL8651_OUTPUTQUEUE_SIZE; i++)
 	{
 		retval = rtl8651_setAsicQueueDescriptorBasedFlowControlRegister(0, i, outputQueuePara[qidx].queueDescFCON, outputQueuePara[qidx].queueDescFCOFF);
@@ -6106,7 +6903,7 @@ static int32 _rtl865x_setQosThresholdByQueueIdx(uint32 qidx)
 	}
 	for(j=1;j<=CPU;j++)
 		rtl8651_setAsicPortBasedFlowControlRegister(PHY0+j, outputQueuePara[qidx].portFCON, outputQueuePara[qidx].portFCOFF);
-	
+
 	retval = rtl8651_setAsicPerQueuePhysicalLengthGapRegister(outputQueuePara[qidx].gap);
 	if (retval!= SUCCESS)
 	{
@@ -6116,6 +6913,7 @@ static int32 _rtl865x_setQosThresholdByQueueIdx(uint32 qidx)
 
 	return SUCCESS;
 }
+#endif
 
 #if defined(CONFIG_RTL_HW_QOS_SUPPORT)
 static int32 _rtl865xC_QM_init( void )
@@ -6130,17 +6928,18 @@ static int32 _rtl865xC_QM_init( void )
 	/*
 	   1. Get the original decriptor usage for QM.
 	 */
+	WRITE_MEM32( SIRR, READ_MEM32(SIRR)| TRXRDY );
  	rtl865xC_lockSWCore();
 
 	_rtl865xC_QM_orgDescUsage = 0;	/* by default, set it to 0 */
-	
+
 	do
 	{
 		int32 idx;
 
 		originalDescGetReady = TRUE;	/* by default, set it to TRUE */
 		cnt = 0;
-		
+
 		for ( idx = 0 ; idx < RTL865XC_QM_DESC_READROBUSTPARAMETER ; idx ++ )
 		{
 			uint32 currentDescUsage;
@@ -6164,14 +6963,14 @@ static int32 _rtl865xC_QM_init( void )
 		_rtl865xC_QM_orgDescUsage = 12;
 
 	rtl865xC_unLockSWCore();
-#endif	
+#endif
 	return SUCCESS;
 }
 #endif
 
 /*
 @func int32 | rtl865xC_waitForOutputQueueEmpty | wait until output queue empty
-@rvalue SUCCESS | 
+@rvalue SUCCESS |
 @comm
 	The function will not return until all the output queue is empty.
  */
@@ -6187,7 +6986,7 @@ int32 rtl865xC_waitForOutputQueueEmpty(void)
 	while ( ((READ_MEM32(LAGCR0)&OUTPUTQUEUE_STAT_MASK_CR0)^OUTPUTQUEUE_STAT_MASK_CR0) ||
        	((READ_MEM32(LAGCR1)&OUTPUTQUEUE_STAT_MASK_CR1)^OUTPUTQUEUE_STAT_MASK_CR1) );
 	#endif
-	
+
 	/*	There are something wrong when check the input queue is empty or not	*/
 	currentDescUsage = (READ_MEM32( GDSR0 ) & USEDDSC_MASK) >> USEDDSC_OFFSET;
 	cnt = (currentDescUsage-_rtl865xC_QM_orgDescUsage)<<10;
@@ -6205,10 +7004,10 @@ int32 rtl865xC_waitForOutputQueueEmpty(void)
 #endif
        return SUCCESS;
 }
- 
+
 /*
 @func int32 | rtl8651_resetAsicOutputQueue | reset output queue
-@rvalue SUCCESS | 
+@rvalue SUCCESS |
 @comm
 	When reset is done, all queue pointer will be reset to the initial base address.
  */
@@ -6217,7 +7016,7 @@ int32 rtl865xC_waitForOutputQueueEmpty(void)
  {
 	uint32	i;
 	uint32	scr, pauseTicks;
-	
+
  	WRITE_MEM32(QRR, 0x0);
 	scr = (REG32(SCCR) & 0x00000070) >> 4;
 	switch( scr )
@@ -6235,14 +7034,14 @@ int32 rtl865xC_waitForOutputQueueEmpty(void)
 	/* waiting 500ms */
 
 	pauseTicks = pauseTicks<<2;
-	
+
 	for(i=pauseTicks;i<0;i--)
 	{
 		i = i;
 	}
-	
+
  	WRITE_MEM32(QRR, 0x1);
-	
+
 	for(i=pauseTicks;i<0;i--)
 	{
 		i = i;
@@ -6288,7 +7087,7 @@ static void _rtl8651_syncToAsicEthernetBandwidthControl(void)
 			uint32 currentSwBandwidthCtrlMultiplier;
 			uint32 currentSwBandwidthCtrlSetting;
 			uint32 currentAsicBandwidthCtrlSetting;
-			
+
 			/*
 				We would check for rate and _rtl865xB_BandwidthCtrlMultiplier for the rate-multiply.
 
@@ -6343,20 +7142,20 @@ static void _rtl8651_syncToAsicEthernetBandwidthControl(void)
 			if (	(!( (currentSwBandwidthCtrlSetting) == 0 && (currentAsicBandwidthCtrlSetting == 0x3fff) ) /* for FULL Rate case */) ||
 				( currentSwBandwidthCtrlSetting != currentAsicBandwidthCtrlSetting ))
 			{
-#if 0			
+#if 0
 				if (cfgTypeIdx==0)
 				{
 					rtlglue_printf("set ingress bandwidth port %d, %d.\n", port, (currentSwBandwidthCtrlSetting == 0)?
 																(0	/* For Ingress Bandwidth control, 0 means "disabled" */):
 																(currentSwBandwidthCtrlSetting));
 				}
-				else 
+				else
 				{
 					rtlglue_printf("set ingress bandwidth port %d, %d.\n", port, (currentSwBandwidthCtrlSetting == 0)?
 																(0x3fff	/* For Egress Bandwidth control, 0x3fff means "disabled" */):
 																(currentSwBandwidthCtrlSetting));
 				}
-#endif				
+#endif
 				retval = (cfgTypeIdx == 0)?
 					/* Ingress */
 					(rtl8651_setAsicPortIngressBandwidth(	port,
@@ -6388,7 +7187,7 @@ out:
 @parm uint32 | port | the port number
 @parm int8 | input | Ingress or egress control to <p port>
 @parm uint32 | rate | rate to set.
-@rvalue SUCCESS | 
+@rvalue SUCCESS |
 @rvalue FAILED | invalid parameter
 @comm
 The <p rate> can be set to several different values:
@@ -6470,7 +7269,7 @@ int32 rtl8651_setAsicFlowControlRegister(uint32 port, uint32 enable)
 
 	if ( enable && ( statCtrlReg4 & CAPABLE_PAUSE ) == 0 )
 	{
-		statCtrlReg4 |= CAPABLE_PAUSE;		
+		statCtrlReg4 |= CAPABLE_PAUSE;
 	}
 	else if ( enable == 0 && ( statCtrlReg4 & CAPABLE_PAUSE ) )
 	{
@@ -6480,7 +7279,7 @@ int32 rtl8651_setAsicFlowControlRegister(uint32 port, uint32 enable)
 		return SUCCESS;	/* The configuration does not change. Do nothing. */
 
 	rtl8651_setAsicEthernetPHYReg( phyid, 4, statCtrlReg4 );
-	
+
 	/* restart N-way. */
 	rtl8651_restartAsicEthernetPHYNway(port);
 
@@ -6519,7 +7318,7 @@ int32 rtl8651_getAsicFlowControlRegister(uint32 port, uint32 *enable)
 @func int32 | rtl8651_setAsicSystemInputFlowControlRegister | Set System input queue flow control register
 @parm uint32 | fcON		| Threshold for Flow control OFF
 @parm uint32 | fcOFF		| Threshold for Flow control ON
-@rvalue SUCCESS | 
+@rvalue SUCCESS |
 @comm
 Set input-queue flow control threshold on RTL865xC platform.
  */
@@ -6543,7 +7342,7 @@ int32 rtl8651_setAsicSystemInputFlowControlRegister(uint32 fcON, uint32 fcOFF)
 @func int32 | rtl8651_getAsicSystemInputFlowControlRegister | Get System input queue flow control register
 @parm uint32* | fcON		| pointer to get Threshold for Flow control OFF
 @parm uint32* | fcOFF		| pointer to get Threshold for Flow control ON
-@rvalue SUCCESS | 
+@rvalue SUCCESS |
 @comm
 Set input-queue flow control threshold on RTL865xC platform.
  */
@@ -6571,7 +7370,7 @@ int32 rtl865xC_setAsicEthernetForceModeRegs(uint32 port, uint32 enForceMode, uin
 {
 	uint32 offset = port * 4;
 	uint32 PCR = READ_MEM32( PCRP0 + offset );
-	
+
 	if (rtl8651_tblAsicDrvPara.externalPHYProperty & RTL8651_TBLASIC_EXTPHYPROPERTY_PORT5_RTL8211B)
 	{
 		if ( port > RTL8651_MAC_NUMBER )
@@ -6595,7 +7394,7 @@ int32 rtl865xC_setAsicEthernetForceModeRegs(uint32 port, uint32 enForceMode, uin
 	{
 		PCR |= EnForceMode;
 
-#if defined(CONFIG_RTL_8196C) || defined(CONFIG_RTL_8198)
+#if defined(CONFIG_RTL_8196C) || defined(CONFIG_RTL_8198) || defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
                 //ForceMode with polling link status, disable Auto-Negotiation but polling phy's link status
                 PCR |= PollLinkStatus;
 #endif
@@ -6613,7 +7412,7 @@ int32 rtl865xC_setAsicEthernetForceModeRegs(uint32 port, uint32 enForceMode, uin
 		if ( forceDuplex )
 			PCR |= ForceDuplex;
 	}
-	
+
 	WRITE_MEM32( PCRP0 + offset, PCR );
 	mdelay(10);
 	TOGGLE_BIT_IN_REG_TWICE(PCRP0 + offset,EnForceMode);
@@ -6641,19 +7440,19 @@ static int32 rtl8651_perPortStormControl(uint32 type, uint32 portNum, uint32 ena
 	uint32 oldRegValue;
 	uint32 newRegValue;
 	uint32 totalExtPortNum=3;
-	
+
 	if(portNum>=RTL8651_PORT_NUMBER + totalExtPortNum)
 	{
 		rtlglue_printf("wrong port number\n");
 		return FAILED;
 	}
-	
+
 	regAddress=PCRP0 + portNum * 4;
-		
+
 	oldRegValue=READ_MEM32(regAddress);
-	
+
 	newRegValue=oldRegValue;
-	
+
 	if((type & BROADCAST_STORM_CONTROL) !=0)
 	{
 		if(enable == TRUE)
@@ -6665,9 +7464,9 @@ static int32 rtl8651_perPortStormControl(uint32 type, uint32 portNum, uint32 ena
 		{
 			newRegValue = newRegValue & (~BCSC_ENBROADCAST);
 		}
-		
+
 	}
-	
+
 	if((type & MULTICAST_STORM_CONTROL) !=0)
 	{
 		if(enable == TRUE)
@@ -6679,7 +7478,7 @@ static int32 rtl8651_perPortStormControl(uint32 type, uint32 portNum, uint32 ena
 		{
 			newRegValue = newRegValue & (~BCSC_ENMULTICAST);
 		}
-		
+
 	}
 
 	if((newRegValue & (BCSC_ENMULTICAST |BCSC_ENBROADCAST ))==0)
@@ -6687,17 +7486,17 @@ static int32 rtl8651_perPortStormControl(uint32 type, uint32 portNum, uint32 ena
 		/*no needn't storm control*/
 		newRegValue = newRegValue & (~ENBCSC);
 	}
-		
+
 	if(newRegValue!=oldRegValue)
 	{
 		WRITE_MEM32(regAddress, newRegValue);
 	}
-	
+
 	TOGGLE_BIT_IN_REG_TWICE(regAddress,EnForceMode);
-	
+
 	return SUCCESS;
 
-	
+
 }
 
 int32 rtl865x_setStormControl(uint32 type,uint32 enable,uint32 percentage)
@@ -6717,18 +7516,18 @@ int32 rtl865x_setStormControl(uint32 type,uint32 enable,uint32 percentage)
 			{
 				rtl865xC_setBrdcstStormCtrlRate(percentage);
 			}
-			
+
 			rtl8651_perPortStormControl(type, port, TRUE);
 		}
 		else
 		{
 			rtl865xC_setBrdcstStormCtrlRate(100);
 			rtl8651_perPortStormControl(type, port, FALSE);
-			
+
 		}
-			
+
 	}
-	
+
 	return SUCCESS;
 }
 
@@ -6736,73 +7535,73 @@ int32 rtl865x_setStormControl(uint32 type,uint32 enable,uint32 percentage)
 @func int32 | rtl8651_setAsic802D1xMacBaseAbility | set 802.1x mac based ability
 @parm enum PORTID | port | the port number (physical port: 0~5, extension port: 6~8)
 @parm uint32* | isEnable | 1: enabled, 0: disabled.
-@rvalue SUCCESS | 
+@rvalue SUCCESS |
 @rvalue FAILED | invalid parameter
 @comm
  */
 int32 rtl8651_setAsic802D1xMacBaseAbility( enum PORTID port, uint32 isEnable )
 {
 	/* Invalid input parameter */
-	if ((port < PHY0) || (port > EXT2)) 
+	if ((port < PHY0) || (port > EXT2))
 		return FAILED;
-	
+
 	/* Invalid input parameter */
 	if ((isEnable != TRUE) && (isEnable != FALSE))
-		return FAILED; 
+		return FAILED;
 
 	switch (port)
 	{
 		case PHY0:
 		WRITE_MEM32(DOT1XMACCR, isEnable == TRUE ? (READ_MEM32(DOT1XMACCR) | ( Dot1xMAC_P0En)):
-											(READ_MEM32(DOT1XMACCR) & ~( Dot1xMAC_P0En))); 
+											(READ_MEM32(DOT1XMACCR) & ~( Dot1xMAC_P0En)));
 			break;
-			
+
 		case PHY1:
 		WRITE_MEM32(DOT1XMACCR, isEnable == TRUE ? (READ_MEM32(DOT1XMACCR) | ( Dot1xMAC_P1En)):
-											(READ_MEM32(DOT1XMACCR) & ~( Dot1xMAC_P1En)));  
-			break;	
-			
+											(READ_MEM32(DOT1XMACCR) & ~( Dot1xMAC_P1En)));
+			break;
+
 		case PHY2:
 		WRITE_MEM32(DOT1XMACCR, isEnable == TRUE ? (READ_MEM32(DOT1XMACCR) | ( Dot1xMAC_P2En)):
 											(READ_MEM32(DOT1XMACCR) & ~( Dot1xMAC_P2En)));
 			break;
-			
+
 		case PHY3:
 		WRITE_MEM32(DOT1XMACCR, isEnable == TRUE ? (READ_MEM32(DOT1XMACCR) | ( Dot1xMAC_P3En)):
-											(READ_MEM32(DOT1XMACCR) & ~( Dot1xMAC_P3En)));  
+											(READ_MEM32(DOT1XMACCR) & ~( Dot1xMAC_P3En)));
 			break;
-			
+
 		case PHY4:
 		WRITE_MEM32(DOT1XMACCR, isEnable == TRUE ? (READ_MEM32(DOT1XMACCR) | ( Dot1xMAC_P4En)):
-											(READ_MEM32(DOT1XMACCR) & ~( Dot1xMAC_P4En)));  											
-			break;		
-			
+											(READ_MEM32(DOT1XMACCR) & ~( Dot1xMAC_P4En)));
+			break;
+
 		case PHY5:
 		WRITE_MEM32(DOT1XMACCR, isEnable == TRUE ? (READ_MEM32(DOT1XMACCR) | ( Dot1xMAC_P5En)):
 											(READ_MEM32(DOT1XMACCR) & ~( Dot1xMAC_P5En)));
 			break;
-											
+
 		case CPU:
 		WRITE_MEM32(DOT1XMACCR, isEnable == TRUE ? (READ_MEM32(DOT1XMACCR) | ( Dot1xMAC_P6En)):
 											(READ_MEM32(DOT1XMACCR) & ~( Dot1xMAC_P6En)));
 			break;
-			
+
 		case EXT1:
 		WRITE_MEM32(DOT1XMACCR, isEnable == TRUE ? (READ_MEM32(DOT1XMACCR) | ( Dot1xMAC_P7En)):
-											(READ_MEM32(DOT1XMACCR) & ~( Dot1xMAC_P7En)));  
+											(READ_MEM32(DOT1XMACCR) & ~( Dot1xMAC_P7En)));
 			break;
-			
+
 		case EXT2:
 		WRITE_MEM32(DOT1XMACCR, isEnable == TRUE ? (READ_MEM32(DOT1XMACCR) | ( Dot1xMAC_P8En)):
-											(READ_MEM32(DOT1XMACCR) & ~( Dot1xMAC_P8En))); 
+											(READ_MEM32(DOT1XMACCR) & ~( Dot1xMAC_P8En)));
 			break;
-			
+
 		case EXT3:
 			return FAILED;
 
 		case MULTEXT:
 			return FAILED;
-											
+
 	}
 	return SUCCESS;
 }
@@ -6810,7 +7609,7 @@ int32 rtl8651_setAsic802D1xMacBaseAbility( enum PORTID port, uint32 isEnable )
 /*
 @func int32 | rtl8651_setAsic802D1xMacBaseDirection | set 802.1x mac based direction
 @parm enum uint32 | dir | OperCOnntrolledDirections for MAC-Based ACCESS Control. 0:BOTH, 1:IN
-@rvalue SUCCESS | 
+@rvalue SUCCESS |
 @rvalue FAILED | invalid parameter
 @comm
  */
@@ -6830,7 +7629,7 @@ int32 rtl8651_setAsic802D1xMacBaseDirection(int32 dir)
 	}
 	else
 	{
-		WRITE_MEM32(DOT1XMACCR,(READ_MEM32(DOT1XMACCR)) &(~Dot1xMAC_OPDIR));	
+		WRITE_MEM32(DOT1XMACCR,(READ_MEM32(DOT1XMACCR)) &(~Dot1xMAC_OPDIR));
 	}
 	return SUCCESS;
 }
@@ -6838,34 +7637,34 @@ int32 rtl8651_setAsic802D1xMacBaseDirection(int32 dir)
 /*
 @func int32 | rtl8651_setAsicGuestVlanProcessControl | set guest vlan process control
 @parm enum  uint32 | process |default process for unauthenticated client  (00~11)<<12
-@rvalue SUCCESS 
+@rvalue SUCCESS
 @rvalue FAILED | invalid parameter
 @comm
  */
 int32 rtl8651_setAsicGuestVlanProcessControl( uint32 process)
 {
-	/* Invalid input parameter */	
+	/* Invalid input parameter */
 	if((process < Dot1xUNAUTHBH_DROP) || (process > Dot1xUNAUTHBH_RESERVED))
-		return FAILED;	
-	
+		return FAILED;
+
 	WRITE_MEM32(GVGCR, ((READ_MEM32(GVGCR)) & 0x0fff) | process);
 
 	return SUCCESS;
 }
 
-#if	defined(CONFIG_RTL_QOS_8021P_SUPPORT)
+
 /*
 @func int32 | rtl8651_setAsicDot1qAbsolutelyPriority | set 802.1Q absolutely priority
 @parm enum PRIORITYVALUE | srcpriority | priority value
-@parm enum PRIORITYVALUE | priority | absolute priority value 
-@rvalue SUCCESS | 
+@parm enum PRIORITYVALUE | priority | absolute priority value
+@rvalue SUCCESS |
 @rvalue FAILED | invalid parameter
 @comm
  */
 int32 rtl8651_setAsicDot1qAbsolutelyPriority( enum PRIORITYVALUE srcpriority, enum PRIORITYVALUE priority )
 {
 	/* Invalid input parameter */
-	if ((srcpriority < PRI0) || (srcpriority > PRI7) || (priority < PRI0) || (priority > PRI7)) 
+	if ((srcpriority < PRI0) || (srcpriority > PRI7) || (priority < PRI0) || (priority > PRI7))
 		return FAILED;
 
 	switch (srcpriority)
@@ -6893,10 +7692,10 @@ int32 rtl8651_setAsicDot1qAbsolutelyPriority( enum PRIORITYVALUE srcpriority, en
 
 
 /*
-@func int32 | rtl8651_getAsicDot1qAbsolutelyPriority | get 802.1Q absolutely priority 
+@func int32 | rtl8651_getAsicDot1qAbsolutelyPriority | get 802.1Q absolutely priority
 @parm enum PRIORITYVALUE | srcpriority | priority value
-@parm enum PRIORITYVALUE* | pPriority | pPriority will return the absolute priority value 
-@rvalue SUCCESS | 
+@parm enum PRIORITYVALUE* | pPriority | pPriority will return the absolute priority value
+@rvalue SUCCESS |
 @rvalue FAILED | invalid parameter
 @comm
  */
@@ -6904,7 +7703,7 @@ int32 rtl8651_getAsicDot1qAbsolutelyPriority( enum PRIORITYVALUE srcpriority, en
 {
 
 	/* Invalid input parameter */
-	if ((srcpriority < PRI0) || (srcpriority > PRI7)) 
+	if ((srcpriority < PRI0) || (srcpriority > PRI7))
 		return FAILED;
 
 	if (pPriority != NULL)
@@ -6934,10 +7733,12 @@ int32 rtl8651_getAsicDot1qAbsolutelyPriority( enum PRIORITYVALUE srcpriority, en
 }
 
 
+#if	defined(CONFIG_RTL_QOS_8021P_SUPPORT)
+
 /*
 @func int32 | rtl8651_flushAsicDot1qAbsolutelyPriority | set 802.1Q absolutely priority the default value 0
 @parm void
-@rvalue SUCCESS | 
+@rvalue SUCCESS |
 @comm
  */
 int32 rtl8651_flushAsicDot1qAbsolutelyPriority(void)
@@ -6958,22 +7759,22 @@ RTL865x provides an aggregator port. This API updates the table which maps hashe
 If portmask = 0: clear all aggregation port mappings.
 Rearrange policy is round-robin. ie. if port a,b,c is in portmask, then hash block 0~7's port number is a,b,c,a,b,c,a,b
 */
-int32  rtl8651_updateAsicLinkAggregatorLMPR(int32 portmask) 
+int32  rtl8651_updateAsicLinkAggregatorLMPR(int32 portmask)
 {
 	uint32 hIdx, portIdx, reg;
 
 	/* Clear all mappings */
-	WRITE_MEM32( LAGHPMR0, 0 );	
-	
+	WRITE_MEM32( LAGHPMR0, 0 );
+
 	if ( portmask == 0 )
 	{
 		return SUCCESS;
 	}
 
 	reg = 0;
-	portIdx = 0;	
+	portIdx = 0;
 	for ( hIdx = 0; hIdx < RTL865XC_LAGHASHIDX_NUMBER; hIdx++ )
-	{		
+	{
 		while ( ( ( 1 << portIdx ) & portmask ) == 0 )	/* Don't worry about infinite loop because portmask won't be 0. */
 		{
 			portIdx = ( portIdx + 1 ) % ( RTL8651_PORT_NUMBER + rtl8651_totalExtPortNum );
@@ -6989,6 +7790,7 @@ int32  rtl8651_updateAsicLinkAggregatorLMPR(int32 portmask)
 
 
 
+#if 0
 static uint32 _rtl8651_findAsicLinkupPortmask(uint32 portMask)
 {
 	uint32 port, lnkUp_portMask = portMask;
@@ -7001,11 +7803,12 @@ static uint32 _rtl8651_findAsicLinkupPortmask(uint32 portMask)
 	}
 	return lnkUp_portMask;
 }
+#endif
 
 int32 rtl8651_setAsicEthernetLinkStatus(uint32 port, int8 linkUp)
 {
 	int8 notify;
-	uint32 portmask;
+//	uint32 portmask;
 
 	if (port >= (RTL8651_PORT_NUMBER+rtl8651_totalExtPortNum))
 	{
@@ -7017,8 +7820,9 @@ int32 rtl8651_setAsicEthernetLinkStatus(uint32 port, int8 linkUp)
 
 	rtl8651AsicEthernetTable[port].linkUp = (linkUp == TRUE)? TRUE: FALSE;
 
-	/* 
-		If the specified port is a member of the aggregator, 
+#if 0
+	/*
+		If the specified port is a member of the aggregator,
 		update the table which maps hashed index to the port
 		because the member port of the aggregator link changes.
 	*/
@@ -7029,6 +7833,7 @@ int32 rtl8651_setAsicEthernetLinkStatus(uint32 port, int8 linkUp)
 		uint32 lnkUp_portMask = _rtl8651_findAsicLinkupPortmask(portmask);
 		rtl8651_updateAsicLinkAggregatorLMPR( lnkUp_portMask );
 	}
+#endif
 
 	return SUCCESS;
 }
@@ -7054,7 +7859,7 @@ int32 rtl8651_setAsicRateLimitTable(uint32 index, rtl865x_tblAsicDrv_rateLimitPa
 int32 rtl8651_delAsicRateLimitTable(uint32 index)
 {
 	rtl8651_tblAsic_rateLimitTable_t entry;
-	
+
 	if (index >= RTL8651_RATELIMITTBL_SIZE)
 		return FAILED;
 	memset(&entry,0,sizeof(rtl8651_tblAsic_rateLimitTable_t));
@@ -7079,54 +7884,165 @@ int32 rtl8651_getAsicRateLimitTable(uint32 index, rtl865x_tblAsicDrv_rateLimitPa
 	return SUCCESS;
 }
 #endif
-#ifdef CONFIG_RTK_VOIP_QOS
+
+
+int32 rtl8651_setPortFlowControlConfigureRegister(enum PORTID port,uint32 enable)
+{
+	int enable_port = 0;
+	if(enable)
+		enable_port = 0x3f;
+		
+	switch (port)
+	{
+		case PHY0:
+			WRITE_MEM32(FCCR0, (READ_MEM32(FCCR0) & ~(0x3F<<Q_P0_EN_FC_OFFSET)) | (enable_port << Q_P0_EN_FC_OFFSET));  break;
+		case PHY1:
+			WRITE_MEM32(FCCR0, (READ_MEM32(FCCR0) & ~(0x3F<<Q_P1_EN_FC_OFFSET)) | (enable_port << Q_P1_EN_FC_OFFSET));  break;
+		case PHY2:
+			WRITE_MEM32(FCCR0, (READ_MEM32(FCCR0) & ~(0x3F<<Q_P2_EN_FC_OFFSET)) | (enable_port << Q_P2_EN_FC_OFFSET));  break;
+		case PHY3:
+			WRITE_MEM32(FCCR0, (READ_MEM32(FCCR0) & ~(0x3F<<Q_P3_EN_FC_OFFSET)) | (enable_port << Q_P3_EN_FC_OFFSET));  break;
+		case PHY4:
+			WRITE_MEM32(FCCR1, (READ_MEM32(FCCR1) & ~(0x3F<<Q_P4_EN_FC_OFFSET)) | (enable_port << Q_P4_EN_FC_OFFSET));  break;
+		case PHY5:
+			WRITE_MEM32(FCCR1, (READ_MEM32(FCCR1) & ~(0x3F<<Q_P5_EN_FC_OFFSET)) | (enable_port << Q_P5_EN_FC_OFFSET));  break;
+		case CPU:
+			WRITE_MEM32(FCCR1, (READ_MEM32(FCCR1) & ~(0x1<<Q_P6_EN_FC_OFFSET)) | (enable_port << Q_P6_EN_FC_OFFSET));  break;
+		default:
+			return FAILED;
+	}
+
+	return SUCCESS;
+}
+/*
+@func int32 | rtl8651_setAsicPortPriority | set port based priority
+@parm enum PORTID | port | the port number (valid: physical ports(0~5) and extension ports(7~9) )
+@parm enum PRIORITYVALUE | priority | priority value.
+@rvalue SUCCESS | 
+@rvalue FAILED | invalid parameter
+@comm
+ */
+int32 rtl8651_setAsicPortPriority( enum PORTID port, enum PRIORITYVALUE priority )
+{
+	/* Invalid input parameter */
+	if ((priority < PRI0) || (priority > PRI7))
+		return FAILED; 
+
+	switch (port)
+	{
+		case PHY0:
+			WRITE_MEM32(PBPCR, (READ_MEM32(PBPCR) & ~(PBPRI_P0_MASK)) | (priority << PBPRI_P0_OFFSET)); break;
+		case PHY1:
+			WRITE_MEM32(PBPCR, (READ_MEM32(PBPCR) & ~(PBPRI_P1_MASK)) | (priority << PBPRI_P1_OFFSET)); break;
+		case PHY2:
+			WRITE_MEM32(PBPCR, (READ_MEM32(PBPCR) & ~(PBPRI_P2_MASK)) | (priority << PBPRI_P2_OFFSET)); break;
+		case PHY3:
+			WRITE_MEM32(PBPCR, (READ_MEM32(PBPCR) & ~(PBPRI_P3_MASK)) | (priority << PBPRI_P3_OFFSET)); break;
+		case PHY4:
+			WRITE_MEM32(PBPCR, (READ_MEM32(PBPCR) & ~(PBPRI_P4_MASK)) | (priority << PBPRI_P4_OFFSET)); break;
+		case PHY5:
+			WRITE_MEM32(PBPCR, (READ_MEM32(PBPCR) & ~(PBPRI_P5_MASK)) | (priority << PBPRI_P5_OFFSET)); break;
+		case EXT1:
+			WRITE_MEM32(PBPCR, (READ_MEM32(PBPCR) & ~(PBPRI_P6_MASK)) | (priority << PBPRI_P6_OFFSET)); break;
+		case EXT2:
+			WRITE_MEM32(PBPCR, (READ_MEM32(PBPCR) & ~(PBPRI_P7_MASK)) | (priority << PBPRI_P7_OFFSET)); break;
+		case EXT3:
+			WRITE_MEM32(PBPCR, (READ_MEM32(PBPCR) & ~(PBPRI_P8_MASK)) | (priority << PBPRI_P8_OFFSET)); break;
+		case CPU: /* fall thru */
+		default:
+			return FAILED;
+	}
+
+	return SUCCESS;
+}
 /*
- * @func int32 | rtl8651_setAsicDscpPriority | set DSCP-based priority 
+@func int32 | rtl8651_getAsicPortPriority | get port based priority
+@parm enum PORTID | port | the port number (valid: physical ports(0~5) and extension ports(7~9) )
+@parm enum PRIORITYVALUE* | pPriority | pPriority will return the priority of the specified port.
+@rvalue SUCCESS |
+@rvalue FAILED | invalid parameter
+@comm
+ */
+int32 rtl8651_getAsicPortPriority( enum PORTID port, enum PRIORITYVALUE *pPriority )
+{
+        if (pPriority != NULL)
+        {
+                switch (port)
+                {
+                        case PHY0:
+                                *pPriority = (READ_MEM32(PBPCR) & PBPRI_P0_MASK) >> PBPRI_P0_OFFSET;  break;
+                        case PHY1:
+                                *pPriority = (READ_MEM32(PBPCR) & PBPRI_P1_MASK) >> PBPRI_P1_OFFSET;  break;
+                        case PHY2:
+                                *pPriority = (READ_MEM32(PBPCR) & PBPRI_P2_MASK) >> PBPRI_P2_OFFSET;  break;
+                        case PHY3:
+                                *pPriority = (READ_MEM32(PBPCR) & PBPRI_P3_MASK) >> PBPRI_P3_OFFSET;  break;
+                        case PHY4:
+                                *pPriority = (READ_MEM32(PBPCR) & PBPRI_P4_MASK) >> PBPRI_P4_OFFSET;  break;
+                        case PHY5:
+                                *pPriority = (READ_MEM32(PBPCR) & PBPRI_P5_MASK) >> PBPRI_P5_OFFSET;  break;
+                        case EXT1:
+                                *pPriority = (READ_MEM32(PBPCR) & PBPRI_P6_MASK) >> PBPRI_P6_OFFSET;  break;
+                        case EXT2:
+                                *pPriority = (READ_MEM32(PBPCR) & PBPRI_P7_MASK) >> PBPRI_P7_OFFSET;  break;
+                        case EXT3:
+                                *pPriority = (READ_MEM32(PBPCR) & PBPRI_P8_MASK) >> PBPRI_P8_OFFSET;  break;
+                        case CPU: /* fall thru */
+                        default:
+                                return FAILED;
+                }
+        }
+
+        return SUCCESS;
+}
+
+/*
+ * @func int32 | rtl8651_setAsicDscpPriority | set DSCP-based priority
  * @parm uint32 | dscp | DSCP value
- * @parm enum PRIORITYVALUE | priority | priority value 
- * @rvalue SUCCESS | 
+ * @parm enum PRIORITYVALUE | priority | priority value
+ * @rvalue SUCCESS |
  * @rvalue FAILED | invalid parameter
  * @comm
  *  */
 int32 rtl8651_setAsicDscpPriority( uint32 dscp, enum PRIORITYVALUE priority )
 {
 	/* Invalid input parameter */
-	if ((dscp < 0) || (dscp > 63)) 
+	if ((dscp < 0) || (dscp > 63))
 		return FAILED;
-	if ((priority < PRI0) || (priority > PRI7)) 
+	if ((priority < PRI0) || (priority > PRI7))
 		return FAILED;
 
 	if ((0 <= dscp) && (dscp <= 9))
-		WRITE_MEM32(DSCPCR0, (READ_MEM32(DSCPCR0) & ~(0x7 << (dscp*3))) | (priority << (dscp*3))); 
+		WRITE_MEM32(DSCPCR0, (READ_MEM32(DSCPCR0) & ~(0x7 << (dscp*3))) | (priority << (dscp*3)));
 	else if ((10 <= dscp) && (dscp <= 19))
-		WRITE_MEM32(DSCPCR1, (READ_MEM32(DSCPCR1) & ~(0x7 << ((dscp%10)*3))) | (priority << ((dscp%10)*3))); 
+		WRITE_MEM32(DSCPCR1, (READ_MEM32(DSCPCR1) & ~(0x7 << ((dscp%10)*3))) | (priority << ((dscp%10)*3)));
 	else if ((20 <= dscp) && (dscp <= 29))
-		WRITE_MEM32(DSCPCR2, (READ_MEM32(DSCPCR2) & ~(0x7 << ((dscp%10)*3))) | (priority << ((dscp%10)*3))); 
+		WRITE_MEM32(DSCPCR2, (READ_MEM32(DSCPCR2) & ~(0x7 << ((dscp%10)*3))) | (priority << ((dscp%10)*3)));
 	else if ((30 <= dscp) && (dscp <= 39))
-		WRITE_MEM32(DSCPCR3, (READ_MEM32(DSCPCR3) & ~(0x7 << ((dscp%10)*3))) | (priority << ((dscp%10)*3))); 
+		WRITE_MEM32(DSCPCR3, (READ_MEM32(DSCPCR3) & ~(0x7 << ((dscp%10)*3))) | (priority << ((dscp%10)*3)));
 	else if ((40 <= dscp) && (dscp <= 49))
-		WRITE_MEM32(DSCPCR4, (READ_MEM32(DSCPCR4) & ~(0x7 << ((dscp%10)*3))) | (priority << ((dscp%10)*3))); 
+		WRITE_MEM32(DSCPCR4, (READ_MEM32(DSCPCR4) & ~(0x7 << ((dscp%10)*3))) | (priority << ((dscp%10)*3)));
 	else if ((50 <= dscp) && (dscp <= 59))
-		WRITE_MEM32(DSCPCR5, (READ_MEM32(DSCPCR5) & ~(0x7 << ((dscp%10)*3))) | (priority << ((dscp%10)*3))); 
+		WRITE_MEM32(DSCPCR5, (READ_MEM32(DSCPCR5) & ~(0x7 << ((dscp%10)*3))) | (priority << ((dscp%10)*3)));
 	else if ((60 <= dscp) && (dscp <= 63))
-		WRITE_MEM32(DSCPCR6, (READ_MEM32(DSCPCR6) & ~(0x7 << ((dscp%10)*3))) | (priority << ((dscp%10)*3))); 
+		WRITE_MEM32(DSCPCR6, (READ_MEM32(DSCPCR6) & ~(0x7 << ((dscp%10)*3))) | (priority << ((dscp%10)*3)));
 
 	return SUCCESS;
 }
 
 
 /*
- * @func int32 | rtl8651_setAsicDscpPriority | set DSCP-based priority 
+ * @func int32 | rtl8651_setAsicDscpPriority | set DSCP-based priority
  * @parm uint32 | dscp | DSCP value
  * @parm enum PRIORITYVALUE* | pPriority | pPriority will return the priority of the specified DSCP
- * @rvalue SUCCESS | 
+ * @rvalue SUCCESS |
  * @rvalue FAILED | invalid parameter
  * @comm
  *  */
 int32 rtl8651_getAsicDscpPriority( uint32 dscp, enum PRIORITYVALUE *pPriority )
 {
 	/* Invalid input parameter */
-	if ((dscp < 0) || (dscp > 63)) 
+	if ((dscp < 0) || (dscp > 63))
 		return FAILED;
 
 	if (pPriority != NULL)
@@ -7150,17 +8066,19 @@ int32 rtl8651_getAsicDscpPriority( uint32 dscp, enum PRIORITYVALUE *pPriority )
 	return SUCCESS;
 }
 
-int32 rtl8651_reset_dscp_priority(void) 
+#ifdef CONFIG_RTK_VOIP_QOS
+
+int32 rtl8651_reset_dscp_priority(void)
 {
-	//clear dscp priority assignment, otherwise pkt with dscp value 0 will be assign priority 1		
+	//clear dscp priority assignment, otherwise pkt with dscp value 0 will be assign priority 1
 	WRITE_MEM32(DSCPCR0,0);
 	WRITE_MEM32(DSCPCR1,0);
 	WRITE_MEM32(DSCPCR2,0);
 	WRITE_MEM32(DSCPCR3,0);
 	WRITE_MEM32(DSCPCR4,0);
 	WRITE_MEM32(DSCPCR5,0);
-	WRITE_MEM32(DSCPCR6,0);	       
-	return 0; 
+	WRITE_MEM32(DSCPCR6,0);
+	return 0;
 }
 int32 rtl8651_cpu_tx_fc(int enable)
 {
@@ -7179,7 +8097,7 @@ int32 rtl8651_setQueueNumber(int port, int qnum)
         rtl865xC_unLockSWCore();
         return SUCCESS;
 }
-#endif 
+#endif
 #ifdef CONFIG_RTK_VOIP_PORT_LINK
 int rtl8651_getAsicEthernetLinkStatus(uint32 port, int8 *linkUp)
 {
diff --git a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865x_asicL2.h b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865x_asicL2.h
index abe8978bd..9005829ac 100644
--- a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865x_asicL2.h
+++ b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865x_asicL2.h
@@ -88,6 +88,12 @@ enum PORTID
 	EXT3 = 9,
 	MULTEXT = 10,
 };
+enum GROUP
+{
+	GR0 = 0,
+	GR1 = 1,
+	GR2 = 2,
+};
 
 /* enum for queue ID */
 enum QUEUEID
@@ -341,11 +347,26 @@ int32 rtl865xC_waitForOutputQueueEmpty(void);
 int32 rtl8651_setAsic802D1xMacBaseAbility( enum PORTID port, uint32 isEnable );
 int32 rtl8651_setAsic802D1xMacBaseDirection(int32 dir);
 int32 rtl8651_setAsicGuestVlanProcessControl( uint32 process);
+int32 rtl8651_setAsicPortBasedPriority( enum PORTID port, enum PRIORITYVALUE priority );
+int32 rtl8651_setAsicQueueStrict( enum PORTID port, enum QUEUEID queueid, enum QUEUETYPE queueType);
+int32 rtl8651_setAsicVlanRemark(enum PORTID port, enum PRIORITYVALUE priority, int remark);
+int32 rtl8651_setAsicDscpRemark(enum PORTID port, enum PRIORITYVALUE priority, int remark);
+int32 rtl8651_getAsicPortBasedPriority( enum PORTID port, enum PRIORITYVALUE* priority );
+int32 rtl8651_getAsicQueueStrict( enum PORTID port, enum QUEUEID queueid, enum QUEUETYPE *pQueueType);
+int32 rtl8651_getAsicPriorityToQIDMappingTable( enum QUEUENUM qnum, enum PRIORITYVALUE priority, enum QUEUEID* qid );
+int32 rtl8651_getAsicPriorityDecision( uint32* portpri, uint32* dot1qpri, uint32* dscppri, uint32* aclpri, uint32* natpri );
+int32 rtl8651_getAsicVlanRemark(enum PORTID port, enum PRIORITYVALUE priority, int* remark);
+int32 rtl8651_getAsicDscpRemark(enum PORTID port, enum PRIORITYVALUE priority, int* remark);
+
 
-#if	defined(CONFIG_RTL_QOS_8021P_SUPPORT)
 /* for vlan base qos */
 int32 rtl8651_setAsicDot1qAbsolutelyPriority( enum PRIORITYVALUE srcpriority, enum PRIORITYVALUE priority );
 int32 rtl8651_getAsicDot1qAbsolutelyPriority( enum PRIORITYVALUE srcpriority, enum PRIORITYVALUE *pPriority );
+
+#if	defined(CONFIG_RTL_QOS_8021P_SUPPORT)
+/* for vlan base qos */
+//int32 rtl8651_setAsicDot1qAbsolutelyPriority( enum PRIORITYVALUE srcpriority, enum PRIORITYVALUE priority );
+//int32 rtl8651_getAsicDot1qAbsolutelyPriority( enum PRIORITYVALUE srcpriority, enum PRIORITYVALUE *pPriority );
 int32 rtl8651_flushAsicDot1qAbsolutelyPriority(void);
 #endif
 
@@ -355,11 +376,21 @@ int32 rtl8651_delAsicRateLimitTable(uint32 index);
 int32 rtl8651_getAsicRateLimitTable(uint32 index, rtl865x_tblAsicDrv_rateLimitParam_t *rateLimit_t);
 #endif
 
-#ifdef CONFIG_RTK_VOIP_QOS
+int32 rtl8651_setPortFlowControlConfigureRegister(enum PORTID port,uint32 enable);
+int32 rtl8651_setAsicPortPriority( enum PORTID port, enum PRIORITYVALUE priority);
+int32 rtl8651_getAsicPortPriority( enum PORTID port, enum PRIORITYVALUE *pPriority );
+
 int32 rtl8651_setAsicDscpPriority( uint32 dscp, enum PRIORITYVALUE priority );
 int32 rtl8651_getAsicDscpPriority( uint32 dscp, enum PRIORITYVALUE *pPriority );
+
+#ifdef CONFIG_RTK_VOIP_QOS
 int32 rtl8651_reset_dscp_priority(void);
 int32 rtl8651_cpu_tx_fc(int enable);
+int32 rtl8651_setQueueNumber(int port, int qnum);
+#endif
+
+#ifdef CONFIG_RTL_8197D_DYN_THR	
+int32 rtl819x_setQosThreshold(uint32 old_sts, uint32 new_sts);
 #endif
 
 #endif
diff --git a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865x_asicL3.h b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865x_asicL3.h
index 5afd2da21..3dbf8eecf 100644
--- a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865x_asicL3.h
+++ b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865x_asicL3.h
@@ -17,9 +17,17 @@
 #define RTL8651_ROUTINGTBL_SIZE		8
 #define RTL8651_ARPTBL_SIZE			512
 
-#if defined (CONFIG_RTL8196C_REVISION_B) || defined (CONFIG_RTL8198_REVISION_B) 
+#if defined(CONFIG_RTL_8196E)
+
+#define RTL8651_MULTICASTTBL_SIZE			256
+#define RTL8651_IPMULTICASTTBL_SIZE		256
+#define RTL8651_IPMCAST_CAM_SIZE			32
+
+#elif defined (CONFIG_RTL8196C_REVISION_B) || defined (CONFIG_RTL8198_REVISION_B) || defined(CONFIG_RTL_819XD)
 #define RTL8651_IPMULTICASTTBL_SIZE		128
 #define RTL8651_MULTICASTTBL_SIZE		128
+#define RTL8651_IPMCAST_CAM_SIZE			32
+
 #else
 #define RTL8651_IPMULTICASTTBL_SIZE		64
 #define RTL8651_MULTICASTTBL_SIZE		64
@@ -321,7 +329,7 @@ typedef struct {
 } rtl865xc_tblAsic_arpTable_t;
 
 typedef struct {
-#if defined (CONFIG_RTL8196C_REVISION_B) || defined (CONFIG_RTL8198_REVISION_B)
+#if defined (CONFIG_RTL8196C_REVISION_B) || defined (CONFIG_RTL8198_REVISION_B) || defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
     /* word 0 */
     ipaddr_t        srcIPAddr;
     /* word 1 */
diff --git a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865xc_asicregs.h b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865xc_asicregs.h
index 5454a4622..ec55a0bba 100644
--- a/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865xc_asicregs.h
+++ b/target/linux/realtek/files/drivers/net/rtl819x/AsicDriver/rtl865xc_asicregs.h
@@ -174,7 +174,7 @@ extern int8						*pVirtualSWTable;
 #define HSA_BASE			REAL_HSA_BASE
 #endif
 
-#ifdef CONFIG_RTL_8198
+#if defined(CONFIG_RTL_8198) || defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
 #define PIN_MUX_SEL_2                           (SYSTEM_BASE + 0x0044)
 #endif
 
@@ -504,6 +504,11 @@ link partner ability registers field definitions
 #define CPUTPDCR1					(0x024 + CPU_IFACE_BASE)		/* Tx pkthdr descriptor control High */
 #define CPUTPDCR(idx)				(CPUTPDCR0 + (idx << 2))		/* Tx pkthdr descriptor control with index */
 
+#if defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
+#define CPUTPDCR2					(0x060 + CPU_IFACE_BASE)    /* Tx Pkthdr Descriptor 2 Control Register */
+#define CPUTPDCR3					(0x064 + CPU_IFACE_BASE)    /* Tx Pkthdr Descriptor 3 Control Register */
+#endif
+
 #define CPUIIMR						(0x028 + CPU_IFACE_BASE)		/* Interrupt mask control */
 #define CPUIISR						(0x02c + CPU_IFACE_BASE)		/* Interrupt status control */
 #define CPUQDM0						(0x030 + CPU_IFACE_BASE)		/* Queue ID 0 and Descriptor Ring Mapping Register */
@@ -622,8 +627,8 @@ link partner ability registers field definitions
 #define PKTHDR_DESC_RUNOUT_IP_ALL	(0x3f << 17)			/* Run out anyone pkthdr descriptor interrupt pending */
 #define PKTHDR_DESC_RUNOUT_IP(idx)		(1 << (17+(idx)))		/* Run out pkthdr descriptor [IDX] interrupt pending */
 
-#define MBUF_DESC_RUNOUT_IP_ALL		(1 << 11)			/* Run out anyone mbuf interrupt pending */
-#define MBUF_DESC_RUNOUT_IP0			(1 << 11)			/* Run out mbuf descriptor 0 interrupt pending */
+#define MBUF_DESC_RUNOUT_IP_ALL		(1 << 16)			/* Run out anyone mbuf interrupt pending */
+#define MBUF_DESC_RUNOUT_IP0			(1 << 16)			/* Run out mbuf descriptor 0 interrupt pending */
 
 #define TX_DONE_IP0						(1 << 9)				/* Tx one packet done interrupt for descriptor 0 pending */
 #define TX_DONE_IP1						(1 << 10)			/* Tx one packet done interrupt for descriptor 1 pending */
@@ -760,7 +765,7 @@ link partner ability registers field definitions
 #define USEDDSC_MASK				(0x3ff<<16)				/* Total Used Descriptor */
 #define SharedBufFCON_Flag			(1<<14)					/* SharedBufFCON threshold triggerred flag */
 #define MaxUsedDsc_OFFSET			0						/* Max Used Descriptor Count History */
-#define MaxUsedDsc_MASK			(0x3ff<<0)				/* Max Used Descriptor Count History */
+#define MaxUsedDsc_MASK			(0x3fff<<0)				/* Max Used Descriptor Count History */
 
 /* PCSR0 - Port Congestion Status Register 0 */
 #define P3OQCgst_OFFSET			(24)
@@ -1011,6 +1016,10 @@ to forward packet to the unauthorized node. Otherwise, it is not allowed.
 #define CCR                                 (0x048+SWMACCR_BASE)     /* Checksum Control Register */
 #define EPOCR					(0x04C+SWMACCR_BASE)	/* Embedded PHY Operation Control Register */
 #define EPIDR					(0x050+SWMACCR_BASE)	/* Embedded PHY ID Register */
+#define MACCR1					(0x058+SWMACCR_BASE)	/* Embedded PHY ID Register */
+
+/* MACCR1 - MAC control register 1 field definitions */
+#define PORT0_ROUTER_MODE                      (1 << 0)                /* 1: enable Port0 as router mode,  0: normal mode */
 
 /* MACCR - MAC control register field definitions */
 #define NORMAL_BACKOFF                      (1 << 28)                /* Normal back off slot timer */
@@ -1028,6 +1037,8 @@ to forward packet to the unauthorized node. Otherwise, it is not allowed.
 #define SELIPG_11                    		(2<<18)     			/* 11, unit: byte-time */
 #define SELIPG_12                    		(3<<18)     			/* 12, unit: byte-time */
 #define SPDUP_100       	                    (1 << 16)                /* 10000 times speed up aging timer */
+#define CF_SYSCLK_SEL_MASK					(0x3 << 12)
+#define CF_SYSCLK_SEL_OFFSET				(12)
 #define CF_FCDSC_OFFSET                    (4)                     	/* Flow control DSC tolerance, default: 24 pages ( also minimum value ) */
 #define CF_FCDSC_MASK                       (0x7f << 4)                /* Flow control DSC tolerance, default: 24 pages ( also minimum value ) */
 #define CF_RXIPG_MASK                       (0xf << 0)                /* Min. IPG limitation for RX receiving packetMinimum value is 6. Maximum value is 12. */
@@ -1107,7 +1118,7 @@ to forward packet to the unauthorized node. Otherwise, it is not allowed.
 /* CSCR, CCR - Checksum Control Register */
 #define EnL4ChkCal                          (1<<5)                   /* Enable L4 Checksum Re-calculation */
 #define EnL3ChkCal                          (1<<4)                   /* Enable L3 Checksum Re-calculation */
-#if defined(CONFIG_RTL_8196C) || defined(CONFIG_RTL_8198)
+#if defined(CONFIG_RTL_8196C) || defined(CONFIG_RTL_8198) || defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
 #define AcceptL2Err                          (1<<3)                   /* CPU port L2 CRC Error Allow; 0: Not Allowed, 1: Allowed (default) */
 #endif
 #define L4ChkSErrAllow                      (1<<2)                   /* L4 Checksum Error Allow */
@@ -1154,7 +1165,7 @@ to forward packet to the unauthorized node. Otherwise, it is not allowed.
 #define Port0_TypeCfg_UTP                   (0<< 0)
 #define Port0_TypeCfg_GMII_MII_RGMII        (1<< 0)
 
-#if defined(CONFIG_RTL_8196C) || defined(CONFIG_RTL_8198)
+#if defined(CONFIG_RTL_8196C) || defined(CONFIG_RTL_8198) || defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
 
 #define GIGA_P5_PHYID	0x16
 /* 0xBB804104 ~ 0xBB804124 */
@@ -1312,6 +1323,8 @@ to forward packet to the unauthorized node. Otherwise, it is not allowed.
 #endif
 
 /* PSRP0,PSRP1,PSRP2,PSRP3,PSRP4,PSRP5,PSRP6,PSRP7,PSRP8 - Port Status Register Port 0~8 */
+#define PortEEEStatus_MASK					(3<<12)
+#define PortEEEStatus_OFFSET				12
 #define LinkDownEventFlag                   (1<<8)      /* Port Link Down Event detecting monitor flag */
 #define PortStatusNWayEnable                (1<<7)      /* N-Way Enable */
 #define PortStatusRXPAUSE                   (1<<6)      /* Rx PAUSE */
@@ -1326,6 +1339,8 @@ to forward packet to the unauthorized node. Otherwise, it is not allowed.
 #define PortStatusLinkSpeedReserved		(3<<0)	/* Reserved Speed */
 
 /* P0GMIICR Port-0 / Port-5 GMII Configuration Register */
+#define CFG_TX_CPUC_TAG                      (1<<26)                  /* Enable Tx CPU tag */
+#define CFG_CPUC_TAG                      	(1<<25)                  /* Enable CPU tag */
 #define CFG_GMAC_MASK                       (3<<23)                  /* The register default reflect the HW power on strapping value of H/W pin. */
 #define CFG_GMAC_RGMII                      (0<<23)                  /* RGMII mode */
 #define CFG_GMAC_GMII_MII_MAC               (1<<23)                  /* GMII/MII MAC mode */
@@ -1386,7 +1401,7 @@ to forward packet to the unauthorized node. Otherwise, it is not allowed.
 #define CVIDR                               (0x00+SWMISC_BASE)     /* Chip Version ID Register */
 #define SSIR						        (0x04+SWMISC_BASE)     /* System Initial and Reset Registe*/
 
-#if defined(CONFIG_RTL_8196C) || defined(CONFIG_RTL_8198)
+#if defined(CONFIG_RTL_8196C) || defined(CONFIG_RTL_8198) || defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
 #define CRMR                                (0x08+SWMISC_BASE)     /* Chip Revision Management Register */
 #define BISTCR                              (0x0C+SWMISC_BASE)     /* BIST control */
 #define BISTTSDR0                           (0x38+SWMISC_BASE)     /* BIST Test Status Diagnostic Register 0 */
@@ -1419,6 +1434,10 @@ to forward packet to the unauthorized node. Otherwise, it is not allowed.
 #endif
 #define SIRR						        (SSIR)                 /* Alias Name */
 
+#if defined(CONFIG_RTL_819XD) || defined(CONFIG_RTL_8196E)
+#define MEMCR                              (0x34+SWMISC_BASE)     /* MEM CTRL Register */
+#endif
+
 /* 	SIRR, SSIR - System Initial and Reset Register*/
 #define SwitchFullRst                       (1 << 2)   /* Reset all tables & queues */
 #define SwitchSemiRst                       (1 << 1)   /* Reset queues */
@@ -1701,6 +1720,10 @@ to forward packet to the unauthorized node. Otherwise, it is not allowed.
 #define QPKTFCRP5G0                         (0x0B8+SBFCTR)        /* Queue-Packet-Based Flow Control Register for Port 5 Group 0 */
 #define QPKTFCRP5G1                         (0x0BC+SBFCTR)        /* Queue-Packet-Based Flow Control Register for Port 5 Group 1 */
 #define QPKTFCRP5G2                         (0x0C0+SBFCTR)        /* Queue-Packet-Based Flow Control Register for Port 5 Group 2 */
+#define QPKTFCRP6G0							(0x0C4+SBFCTR)		  /* Queue-Packet-Based Flow Control Register for Port 6 Group 0 */
+#define QPKTFCRP6G1							(0x0C8+SBFCTR)		  /* Queue-Packet-Based Flow Control Register for Port 6 Group 1 */
+#define QPKTFCRP6G2							(0x0CC+SBFCTR)		  /* Queue-Packet-Based Flow Control Register for Port 6 Group 2 */
+
 #define FCCR0                               (0x0d0+SBFCTR)        /* Flow Control Configuration Register 0 */
 #define FCCR1                               (0x0d4+SBFCTR)        /* Flow Control Configuration Register 1 */
 #define PQPLGR                              (0x0d8+SBFCTR)        /* Per Queue Physical Length Gap Register */
@@ -3090,6 +3113,7 @@ enum FDB_FLAGS
 	#define PIN_MUX_SEL     (SYSTEM_BASE+0x30)      /* 0xB8000030 - 0xB8000033 */
 #else
 	#define PIN_MUX_SEL 0xb8000040
+	#define PIN_MUX_SEL2 	0xb8000044
 #endif
 
 /* Shared Pin Register field definitions */
@@ -3493,9 +3517,21 @@ void rtl865x_wireCompBlinkAmber(void);
 #define BSP_REVR		0xB8000000
 #define BSP_RTL8198_REVISION_A	0xC0000000
 #define BSP_RTL8198_REVISION_B	0xC0000001   
-#define SYS_CLK_MAG 0xb8000010
+
+#define SYS_CLK_MAG			(SYSTEM_BASE+0x0010)
 #define SYS_SW_CLK_ENABLE  0x200
 #define SYS_SW_RESET 0x800
+#define CM_ACTIVE_SWCORE			(1<<11)
+#define CM_PROTECT					(1<<27)
+
+#define 	HW_STRAP		(SYSTEM_BASE+0x0008)
+#define 	BOND_OPTION	(SYSTEM_BASE+0x000C)
+
+#define 	BOND_ID_MASK	(0xF)
+#define 	BOND_8196E		(0xF)
+#define 	BOND_8196EU	(0xC)
+#define 	BOND_8196ES	(0xD)
+
 #endif   /* _ASICREGS_H */