diff mbox series

[v1,1/1] arm64: zynqmp: Fix k24 psu_init_gpl.c clocking

Message ID 20231213134007.2818069-1-neal.frager@amd.com
State New
Headers show
Series [v1,1/1] arm64: zynqmp: Fix k24 psu_init_gpl.c clocking | expand

Commit Message

Neal Frager Dec. 13, 2023, 1:40 p.m. UTC 
This patch corrects the k24 som clocking configuration such that the serdes
clocks are correctly enabled and the usb0 is configured to use the psgtr
refclk2 for usb3 mode.

This patch also corrects the configuration of mio pins mio76 and mio77
which should be configured as gpio pins.

Signed-off-by: Neal Frager <neal.frager@amd.com>
---
 .../zynqmp/zynqmp-sm-k24-revA/psu_init_gpl.c  | 266 +++++++++++++-----
 1 file changed, 200 insertions(+), 66 deletions(-)

Comments

Michal Simek Dec. 13, 2023, 4:07 p.m. UTC | #1 
On 12/13/23 14:40, Neal Frager wrote:
> This patch corrects the k24 som clocking configuration such that the serdes

Corrects the ...

> clocks are correctly enabled and the usb0 is configured to use the psgtr
> refclk2 for usb3 mode.

I sort of have no problem with this patch but it should be better described.
Pretty much serdes configuration should be called from u-boot because we already 
have psgtr driver and usb3.0 can be configured out of it.
But we can't touch dwc3 driver now that's why the only way to do it is to 
configure via psu_init.
But it should be clear from commit message that it is workaround not really 
something what should be here.


> This patch also corrects the configuration of mio pins mio76 and mio77
> which should be configured as gpio pins.

This is definitely fine.

> 
> Signed-off-by: Neal Frager <neal.frager@amd.com>

You mentioned that SPL didn't work before. Is it because of incorrect serdes 
configuration? Because that's sounds weird because you don't need serdes 
initialization for booting out of qspi. That's why looking for which is that bit 
which make SPL work on KD240.

> ---
>   .../zynqmp/zynqmp-sm-k24-revA/psu_init_gpl.c  | 266 +++++++++++++-----
>   1 file changed, 200 insertions(+), 66 deletions(-)

This patch is not based on the latest tree.
There is 0a965b10e2413989e351b5a5b49737babdf5b751 in the middle that's why 
please rebase.

Thanks,
Michal
Neal Frager Dec. 14, 2023, 4:30 p.m. UTC | #2 
Hi Michal,

> This patch corrects the k24 som clocking configuration such that the serdes

> Corrects the ...

Ok.

> clocks are correctly enabled and the usb0 is configured to use the psgtr
> refclk2 for usb3 mode.

> I sort of have no problem with this patch but it should be better described.
> Pretty much serdes configuration should be called from u-boot because we already 
> have psgtr driver and usb3.0 can be configured out of it.
> But we can't touch dwc3 driver now that's why the only way to do it is to 
> configure via psu_init.
> But it should be clear from commit message that it is workaround not really 
> something what should be here.

I will re-test this without the serdes additions to see if they are
really necessary.

> This patch also corrects the configuration of mio pins mio76 and mio77
> which should be configured as gpio pins.

> This is definitely fine.

Good.

> 
> Signed-off-by: Neal Frager <neal.frager@amd.com>

> You mentioned that SPL didn't work before. Is it because of incorrect serdes 
> configuration? Because that's sounds weird because you don't need serdes 
> initialization for booting out of qspi. That's why looking for which is that bit 
> which make SPL work on KD240.

The SPL works.  The issue is the same as I had with the kr260
board.  The sd card interface is over usb, so unless the usb3
works correctly, u-boot has no sd card interface and cannot
find the Linux kernel and file system.

So what I meant was that I cannot boot up properly
when starting from the SPL.  This is what this patch is
meant to fix.

> ---
>   .../zynqmp/zynqmp-sm-k24-revA/psu_init_gpl.c  | 266 +++++++++++++-----
>   1 file changed, 200 insertions(+), 66 deletions(-)

> This patch is not based on the latest tree.
> There is 0a965b10e2413989e351b5a5b49737babdf5b751 in the middle that's why 
> please rebase.

Yes, I will switch over, as I did not realize that this file was not
the same in the u-boot-xlnx and upstream trees.

Best regards,
Neal Frager
AMD
Michal Simek Dec. 14, 2023, 5:05 p.m. UTC | #3 
Hi Neal,

On 12/14/23 17:30, Frager, Neal wrote:
> Hi Michal,
> 
>> This patch corrects the k24 som clocking configuration such that the serdes
> 
>> Corrects the ...
> 
> Ok.
> 
>> clocks are correctly enabled and the usb0 is configured to use the psgtr
>> refclk2 for usb3 mode.
> 
>> I sort of have no problem with this patch but it should be better described.
>> Pretty much serdes configuration should be called from u-boot because we already
>> have psgtr driver and usb3.0 can be configured out of it.
>> But we can't touch dwc3 driver now that's why the only way to do it is to
>> configure via psu_init.
>> But it should be clear from commit message that it is workaround not really
>> something what should be here.
> 
> I will re-test this without the serdes additions to see if they are
> really necessary.
> 
>> This patch also corrects the configuration of mio pins mio76 and mio77
>> which should be configured as gpio pins.
> 
>> This is definitely fine.
> 
> Good.
> 
>>
>> Signed-off-by: Neal Frager <neal.frager@amd.com>
> 
>> You mentioned that SPL didn't work before. Is it because of incorrect serdes
>> configuration? Because that's sounds weird because you don't need serdes
>> initialization for booting out of qspi. That's why looking for which is that bit
>> which make SPL work on KD240.
> 
> The SPL works.  The issue is the same as I had with the kr260
> board.  The sd card interface is over usb, so unless the usb3
> works correctly, u-boot has no sd card interface and cannot
> find the Linux kernel and file system.
> 
> So what I meant was that I cannot boot up properly
> when starting from the SPL.  This is what this patch is
> meant to fix.

What I have seen on KD240 was that I could get to u-boot prompt. And for that 
you don't need any serdes initialization at all.

For booting OS you need it if you want to use USB3.0.

That's why I want to make it clear what makes kd240 to boot u-boot.
And what makes to access sd over usb to boot OS.

And no issue to add serdes initialization to psu_init to get boot OS part to 
work till someone has time to wire psgtr driver with dwc3 to configure it at run 
time.

Thanks,
Michal
Neal Frager Dec. 14, 2023, 5:11 p.m. UTC | #4 
Hi Michal,

> 
>> This patch corrects the k24 som clocking configuration such that the serdes
> 
>> Corrects the ...
> 
> Ok.
> 
>> clocks are correctly enabled and the usb0 is configured to use the psgtr
>> refclk2 for usb3 mode.
> 
>> I sort of have no problem with this patch but it should be better described.
>> Pretty much serdes configuration should be called from u-boot because we already
>> have psgtr driver and usb3.0 can be configured out of it.
>> But we can't touch dwc3 driver now that's why the only way to do it is to
>> configure via psu_init.
>> But it should be clear from commit message that it is workaround not really
>> something what should be here.
> 
> I will re-test this without the serdes additions to see if they are
> really necessary.
> 
>> This patch also corrects the configuration of mio pins mio76 and mio77
>> which should be configured as gpio pins.
> 
>> This is definitely fine.
> 
> Good.
> 
>>
>> Signed-off-by: Neal Frager <neal.frager@amd.com>
> 
>> You mentioned that SPL didn't work before. Is it because of incorrect serdes
>> configuration? Because that's sounds weird because you don't need serdes
>> initialization for booting out of qspi. That's why looking for which is that bit
>> which make SPL work on KD240.
> 
> The SPL works.  The issue is the same as I had with the kr260
> board.  The sd card interface is over usb, so unless the usb3
> works correctly, u-boot has no sd card interface and cannot
> find the Linux kernel and file system.
> 
> So what I meant was that I cannot boot up properly
> when starting from the SPL.  This is what this patch is
> meant to fix.

> What I have seen on KD240 was that I could get to u-boot prompt. And for that 
> you don't need any serdes initialization at all.

Yes, agreed.  I can get to u-boot prompt too without
any additional patches.

> For booting OS you need it if you want to use USB3.0.

And for the kd240, sd card requires usb3.0 also.

> That's why I want to make it clear what makes kd240 to boot u-boot.
> And what makes to access sd over usb to boot OS.

Understood.  The issue I am trying to solve is enabling
sd over usb to boot OS.

> And no issue to add serdes initialization to psu_init to get boot OS part to 
> work till someone has time to wire psgtr driver with dwc3 to configure it at run 
> time.

Is someone currently working on wiring psgtr driver with dwc3?

If this were to happen soon, than this psu_init_gpl.c patch
would become unnecessary.

Best regards,
Neal Frager
AMD
Michal Simek Dec. 15, 2023, 7:42 a.m. UTC | #5 
Hi Neal,

On 12/14/23 18:11, Frager, Neal wrote:
> Hi Michal,
> 
>>
>>> This patch corrects the k24 som clocking configuration such that the serdes
>>
>>> Corrects the ...
>>
>> Ok.
>>
>>> clocks are correctly enabled and the usb0 is configured to use the psgtr
>>> refclk2 for usb3 mode.
>>
>>> I sort of have no problem with this patch but it should be better described.
>>> Pretty much serdes configuration should be called from u-boot because we already
>>> have psgtr driver and usb3.0 can be configured out of it.
>>> But we can't touch dwc3 driver now that's why the only way to do it is to
>>> configure via psu_init.
>>> But it should be clear from commit message that it is workaround not really
>>> something what should be here.
>>
>> I will re-test this without the serdes additions to see if they are
>> really necessary.
>>
>>> This patch also corrects the configuration of mio pins mio76 and mio77
>>> which should be configured as gpio pins.
>>
>>> This is definitely fine.
>>
>> Good.
>>
>>>
>>> Signed-off-by: Neal Frager <neal.frager@amd.com>
>>
>>> You mentioned that SPL didn't work before. Is it because of incorrect serdes
>>> configuration? Because that's sounds weird because you don't need serdes
>>> initialization for booting out of qspi. That's why looking for which is that bit
>>> which make SPL work on KD240.
>>
>> The SPL works.  The issue is the same as I had with the kr260
>> board.  The sd card interface is over usb, so unless the usb3
>> works correctly, u-boot has no sd card interface and cannot
>> find the Linux kernel and file system.
>>
>> So what I meant was that I cannot boot up properly
>> when starting from the SPL.  This is what this patch is
>> meant to fix.
> 
>> What I have seen on KD240 was that I could get to u-boot prompt. And for that
>> you don't need any serdes initialization at all.
> 
> Yes, agreed.  I can get to u-boot prompt too without
> any additional patches.

ok. Good.

> 
>> For booting OS you need it if you want to use USB3.0.
> 
> And for the kd240, sd card requires usb3.0 also.
> 
>> That's why I want to make it clear what makes kd240 to boot u-boot.
>> And what makes to access sd over usb to boot OS.
> 
> Understood.  The issue I am trying to solve is enabling
> sd over usb to boot OS.

good.

> 
>> And no issue to add serdes initialization to psu_init to get boot OS part to
>> work till someone has time to wire psgtr driver with dwc3 to configure it at run
>> time.
> 
> Is someone currently working on wiring psgtr driver with dwc3?

I don't think so.

> 
> If this were to happen soon, than this psu_init_gpl.c patch
> would become unnecessary.

I will take some time to get there that's why it is fine to enable serdes lines 
via psu_init. You can put k24 to kv260/kr260 carrier cards but that's not normal 
mode of operations that's why it shouldn't be a problem.
Maybe it is nice to test it to see but definitely not requirement.

Thanks,
Michal
diff mbox series

Patch

diff --git a/board/xilinx/zynqmp/zynqmp-sm-k24-revA/psu_init_gpl.c b/board/xilinx/zynqmp/zynqmp-sm-k24-revA/psu_init_gpl.c
index 45102302dc..4c904c6c81 100644
--- a/board/xilinx/zynqmp/zynqmp-sm-k24-revA/psu_init_gpl.c
+++ b/board/xilinx/zynqmp/zynqmp-sm-k24-revA/psu_init_gpl.c
@@ -72,6 +72,18 @@  static void dpll_prog(int div2, int ddr_pll_fbdiv, int d_lock_dly,
 	Xil_Out32(((0xFD1A0000U) + 0x0000002C), pll_ctrl_regval);
 }
 
+static int serdes_illcalib(u32 lane3_protocol, u32 lane3_rate,
+			   u32 lane2_protocol, u32 lane2_rate,
+			   u32 lane1_protocol, u32 lane1_rate,
+			   u32 lane0_protocol, u32 lane0_rate)
+{
+	Xil_Out32(0xFD409914, 0xF3);
+	Xil_Out32(0xFD409940, 0xF3);
+	Xil_Out32(0xFD409990, 0x20);
+	Xil_Out32(0xFD409924, 0x37);
+	return 1;
+}
+
 static unsigned long psu_pll_init_data(void)
 {
 	psu_mask_write(0xFF5E0034, 0xFE7FEDEFU, 0x7E4B0C62U);
@@ -490,20 +502,20 @@  static unsigned long psu_mio_init_data(void)
 	psu_mask_write(0xFF18008C, 0x000000FEU, 0x00000000U);
 	psu_mask_write(0xFF180090, 0x000000FEU, 0x000000C0U);
 	psu_mask_write(0xFF180094, 0x000000FEU, 0x000000C0U);
-	psu_mask_write(0xFF180098, 0x000000FEU, 0x00000000U);
-	psu_mask_write(0xFF18009C, 0x000000FEU, 0x00000010U);
-	psu_mask_write(0xFF1800A0, 0x000000FEU, 0x00000010U);
-	psu_mask_write(0xFF1800A4, 0x000000FEU, 0x00000010U);
-	psu_mask_write(0xFF1800A8, 0x000000FEU, 0x00000010U);
-	psu_mask_write(0xFF1800AC, 0x000000FEU, 0x00000010U);
-	psu_mask_write(0xFF1800B0, 0x000000FEU, 0x00000000U);
-	psu_mask_write(0xFF1800B4, 0x000000FEU, 0x00000010U);
-	psu_mask_write(0xFF1800B8, 0x000000FEU, 0x00000010U);
-	psu_mask_write(0xFF1800BC, 0x000000FEU, 0x00000010U);
-	psu_mask_write(0xFF1800C0, 0x000000FEU, 0x00000010U);
-	psu_mask_write(0xFF1800C4, 0x000000FEU, 0x00000010U);
-	psu_mask_write(0xFF1800C8, 0x000000FEU, 0x00000010U);
-	psu_mask_write(0xFF1800CC, 0x000000FEU, 0x00000010U);
+	psu_mask_write(0xFF180098, 0x000000FEU, 0x00000002U);
+	psu_mask_write(0xFF18009C, 0x000000FEU, 0x00000002U);
+	psu_mask_write(0xFF1800A0, 0x000000FEU, 0x00000002U);
+	psu_mask_write(0xFF1800A4, 0x000000FEU, 0x00000002U);
+	psu_mask_write(0xFF1800A8, 0x000000FEU, 0x00000002U);
+	psu_mask_write(0xFF1800AC, 0x000000FEU, 0x00000002U);
+	psu_mask_write(0xFF1800B0, 0x000000FEU, 0x00000002U);
+	psu_mask_write(0xFF1800B4, 0x000000FEU, 0x00000002U);
+	psu_mask_write(0xFF1800B8, 0x000000FEU, 0x00000002U);
+	psu_mask_write(0xFF1800BC, 0x000000FEU, 0x00000002U);
+	psu_mask_write(0xFF1800C0, 0x000000FEU, 0x00000002U);
+	psu_mask_write(0xFF1800C4, 0x000000FEU, 0x00000002U);
+	psu_mask_write(0xFF1800C8, 0x000000FEU, 0x00000080U);
+	psu_mask_write(0xFF1800CC, 0x000000FEU, 0x00000080U);
 	psu_mask_write(0xFF1800D0, 0x000000FEU, 0x00000004U);
 	psu_mask_write(0xFF1800D4, 0x000000FEU, 0x00000004U);
 	psu_mask_write(0xFF1800D8, 0x000000FEU, 0x00000004U);
@@ -528,8 +540,8 @@  static unsigned long psu_mio_init_data(void)
 	psu_mask_write(0xFF180124, 0x000000FEU, 0x00000002U);
 	psu_mask_write(0xFF180128, 0x000000FEU, 0x00000002U);
 	psu_mask_write(0xFF18012C, 0x000000FEU, 0x00000002U);
-	psu_mask_write(0xFF180130, 0x000000FEU, 0x000000C0U);
-	psu_mask_write(0xFF180134, 0x000000FEU, 0x000000C0U);
+	psu_mask_write(0xFF180130, 0x000000FEU, 0x00000000U);
+	psu_mask_write(0xFF180134, 0x000000FEU, 0x00000000U);
 	psu_mask_write(0xFF180204, 0xFFFFFFFFU, 0x50000000U);
 	psu_mask_write(0xFF180208, 0xFFFFFFFFU, 0x00B02020U);
 	psu_mask_write(0xFF18020C, 0x00003FFFU, 0x00000FC0U);
@@ -539,18 +551,18 @@  static unsigned long psu_mio_init_data(void)
 	psu_mask_write(0xFF180144, 0x03FFFFFFU, 0x03FFFFFFU);
 	psu_mask_write(0xFF180148, 0x03FFFFFFU, 0x03FFFFFFU);
 	psu_mask_write(0xFF18014C, 0x03FFFFFFU, 0x03FFFFFFU);
-	psu_mask_write(0xFF180154, 0x03FFFFFFU, 0x00080814U);
+	psu_mask_write(0xFF180154, 0x03FFFFFFU, 0x00FC0814U);
 	psu_mask_write(0xFF180158, 0x03FFFFFFU, 0x03FFFFFFU);
 	psu_mask_write(0xFF18015C, 0x03FFFFFFU, 0x00000000U);
-	psu_mask_write(0xFF180160, 0x03FFFFFFU, 0x03FFFFFFU);
-	psu_mask_write(0xFF180164, 0x03FFFFFFU, 0x03FFFFFFU);
-	psu_mask_write(0xFF180168, 0x03FFFFFFU, 0x03F7F7EBU);
-	psu_mask_write(0xFF180170, 0x03FFFFFFU, 0x00FC000BU);
+	psu_mask_write(0xFF180160, 0x0387FFFFU, 0x0387FFFFU);
+	psu_mask_write(0xFF180164, 0x03FFFFFFU, 0x03FFF87FU);
+	psu_mask_write(0xFF180168, 0x03FFFFFFU, 0x0303F7EBU);
+	psu_mask_write(0xFF180170, 0x03FFFFFFU, 0x0004400BU);
 	psu_mask_write(0xFF180174, 0x03FFFFFFU, 0x03FFFFFFU);
 	psu_mask_write(0xFF180178, 0x03FFFFFFU, 0x00000000U);
-	psu_mask_write(0xFF18017C, 0x0357FFFFU, 0x0357FFFFU);
-	psu_mask_write(0xFF180180, 0x03FFFFFFU, 0x0357FFFFU);
-	psu_mask_write(0xFF180184, 0x03FFFFFFU, 0x0303FFF4U);
+	psu_mask_write(0xFF18017C, 0x03FFFFFFU, 0x03FFFFFFU);
+	psu_mask_write(0xFF180180, 0x03FFFFFFU, 0x03FFFFFFU);
+	psu_mask_write(0xFF180184, 0x03FFFFFFU, 0x03FBBFF4U);
 	psu_mask_write(0xFF180200, 0x0000000FU, 0x00000000U);
 
 	return 1;
@@ -569,21 +581,16 @@  static unsigned long psu_peripherals_init_data(void)
 	psu_mask_write(0xFD1A0100, 0x0001807CU, 0x00000000U);
 	psu_mask_write(0xFF5E0238, 0x001A0000U, 0x00000000U);
 	psu_mask_write(0xFF5E023C, 0x0093C018U, 0x00000000U);
-	psu_mask_write(0xFF5E0230, 0x00000008U, 0x00000000U);
+	psu_mask_write(0xFF5E0230, 0x00000002U, 0x00000000U);
 	psu_mask_write(0xFF5E0238, 0x00000001U, 0x00000000U);
 	psu_mask_write(0xFF180390, 0x00000004U, 0x00000004U);
 	psu_mask_write(0xFF5E023C, 0x00000400U, 0x00000000U);
-	psu_mask_write(0xFF5E0238, 0x00000040U, 0x00000000U);
-	psu_mask_write(0xFF180310, 0x00008000U, 0x00000000U);
-	psu_mask_write(0xFF180320, 0x33840000U, 0x02840000U);
-	psu_mask_write(0xFF18031C, 0x7FFE0000U, 0x64500000U);
-	psu_mask_write(0xFF180358, 0x00000008U, 0x00000008U);
-	psu_mask_write(0xFF180324, 0x03C00000U, 0x00000000U);
+	psu_mask_write(0xFF5E0238, 0x00000080U, 0x00000000U);
 	psu_mask_write(0xFF5E0238, 0x00000400U, 0x00000000U);
 	psu_mask_write(0xFF5E0238, 0x00008000U, 0x00000000U);
 	psu_mask_write(0xFF5E0238, 0x00000010U, 0x00000000U);
 	psu_mask_write(0xFF5E0238, 0x00007800U, 0x00000000U);
-	psu_mask_write(0xFF5E0238, 0x00000004U, 0x00000000U);
+	psu_mask_write(0xFF5E0238, 0x00000006U, 0x00000000U);
 	psu_mask_write(0xFF5E0238, 0x00040000U, 0x00000000U);
 	psu_mask_write(0xFF4B0024, 0x000000FFU, 0x000000FFU);
 	psu_mask_write(0xFFCA5000, 0x00001FFFU, 0x00000000U);
@@ -591,40 +598,26 @@  static unsigned long psu_peripherals_init_data(void)
 	psu_mask_write(0xFFA60040, 0x80000000U, 0x80000000U);
 	psu_mask_write(0xFF260020, 0xFFFFFFFFU, 0x05F5DD18U);
 	psu_mask_write(0xFF260000, 0x00000001U, 0x00000001U);
-	psu_mask_write(0xFF5E0250, 0x00000F0FU, 0x00000202U);
+	psu_mask_write(0xFF0A0284, 0x03FFFFFFU, 0x01000000U);
+	psu_mask_write(0xFF0A0288, 0x03FFFFFFU, 0x01000000U);
+	psu_mask_write(0xFF0A0014, 0x03FF03FFU, 0x02FF0100U);
 
 	mask_delay(1);
-	psu_mask_write(0xFF5E0250, 0x00000F0FU, 0x00000002U);
+	psu_mask_write(0xFF0A0014, 0x03FF03FFU, 0x02FF0000U);
 
 	mask_delay(5);
-	psu_mask_write(0xFF5E0250, 0x00000F0FU, 0x00000202U);
+	psu_mask_write(0xFF0A0014, 0x03FF03FFU, 0x02FF0100U);
 
 	return 1;
 }
 
 static unsigned long psu_serdes_init_data(void)
 {
-	psu_mask_write(0xFD410000, 0x0000001FU, 0x00000009U);
-	psu_mask_write(0xFD410004, 0x0000001FU, 0x00000009U);
 	psu_mask_write(0xFD410008, 0x0000001FU, 0x00000008U);
-	psu_mask_write(0xFD402860, 0x00000080U, 0x00000080U);
-	psu_mask_write(0xFD402864, 0x00000081U, 0x00000001U);
-	psu_mask_write(0xFD402868, 0x00000082U, 0x00000002U);
+	psu_mask_write(0xFD402868, 0x00000080U, 0x00000080U);
 	psu_mask_write(0xFD40A094, 0x00000010U, 0x00000010U);
 	psu_mask_write(0xFD40A368, 0x000000FFU, 0x00000038U);
 	psu_mask_write(0xFD40A36C, 0x00000007U, 0x00000003U);
-	psu_mask_write(0xFD402368, 0x000000FFU, 0x00000058U);
-	psu_mask_write(0xFD40236C, 0x00000007U, 0x00000003U);
-	psu_mask_write(0xFD406368, 0x000000FFU, 0x00000058U);
-	psu_mask_write(0xFD40636C, 0x00000007U, 0x00000003U);
-	psu_mask_write(0xFD402370, 0x000000FFU, 0x0000007CU);
-	psu_mask_write(0xFD402374, 0x000000FFU, 0x00000033U);
-	psu_mask_write(0xFD402378, 0x000000FFU, 0x00000002U);
-	psu_mask_write(0xFD40237C, 0x00000033U, 0x00000030U);
-	psu_mask_write(0xFD406370, 0x000000FFU, 0x0000007CU);
-	psu_mask_write(0xFD406374, 0x000000FFU, 0x00000033U);
-	psu_mask_write(0xFD406378, 0x000000FFU, 0x00000002U);
-	psu_mask_write(0xFD40637C, 0x00000033U, 0x00000030U);
 	psu_mask_write(0xFD40A370, 0x000000FFU, 0x000000F4U);
 	psu_mask_write(0xFD40A374, 0x000000FFU, 0x00000031U);
 	psu_mask_write(0xFD40A378, 0x000000FFU, 0x00000002U);
@@ -678,18 +671,8 @@  static unsigned long psu_serdes_init_data(void)
 	psu_mask_write(0xFD409978, 0x00000010U, 0x00000010U);
 	psu_mask_write(0xFD40D978, 0x00000010U, 0x00000010U);
 
-	serdes_illcalib(0, 0, 3, 0, 4, 0, 4, 0);
-	psu_mask_write(0xFD410010, 0x00000077U, 0x00000044U);
+	serdes_illcalib(0, 0, 3, 0, 0, 0, 0, 0);
 	psu_mask_write(0xFD410014, 0x00000007U, 0x00000003U);
-	psu_mask_write(0xFD400CB4, 0x00000037U, 0x00000037U);
-	psu_mask_write(0xFD404CB4, 0x00000037U, 0x00000037U);
-	psu_mask_write(0xFD4001D8, 0x00000001U, 0x00000001U);
-	psu_mask_write(0xFD4041D8, 0x00000001U, 0x00000001U);
-	psu_mask_write(0xFD404CC0, 0x0000001FU, 0x00000000U);
-	psu_mask_write(0xFD400CC0, 0x0000001FU, 0x00000000U);
-	psu_mask_write(0xFD404048, 0x000000FFU, 0x00000000U);
-	psu_mask_write(0xFD400048, 0x000000FFU, 0x00000000U);
-
 	return 1;
 }
 
@@ -699,7 +682,7 @@  static unsigned long psu_resetout_init_data(void)
 	psu_mask_write(0xFF9D0080, 0x00000001U, 0x00000001U);
 	psu_mask_write(0xFF9D007C, 0x00000001U, 0x00000000U);
 	psu_mask_write(0xFF5E023C, 0x00000140U, 0x00000000U);
-	psu_mask_write(0xFF5E0230, 0x00000008U, 0x00000000U);
+	psu_mask_write(0xFF5E0230, 0x00000002U, 0x00000000U);
 	psu_mask_write(0xFD1A0100, 0x00010000U, 0x00000000U);
 	psu_mask_write(0xFD4A0200, 0x00000002U, 0x00000000U);
 	psu_mask_write(0xFD4A0238, 0x0000000FU, 0x00000000U);
@@ -708,7 +691,6 @@  static unsigned long psu_resetout_init_data(void)
 	psu_mask_write(0xFE20C11C, 0x00000600U, 0x00000600U);
 	psu_mask_write(0xFE20C12C, 0x00004000U, 0x00004000U);
 	psu_mask_write(0xFD480064, 0x00000200U, 0x00000200U);
-	mask_poll(0xFD4063E4, 0x00000010U);
 	mask_poll(0xFD40A3E4, 0x00000010U);
 
 	return 1;
@@ -717,7 +699,7 @@  static unsigned long psu_resetout_init_data(void)
 static unsigned long psu_resetin_init_data(void)
 {
 	psu_mask_write(0xFF5E023C, 0x00000540U, 0x00000540U);
-	psu_mask_write(0xFF5E0230, 0x00000008U, 0x00000008U);
+	psu_mask_write(0xFF5E0230, 0x00000002U, 0x00000002U);
 	psu_mask_write(0xFD4A0238, 0x0000000FU, 0x0000000AU);
 	psu_mask_write(0xFD4A0200, 0x00000002U, 0x00000002U);
 	psu_mask_write(0xFD1A0100, 0x00010000U, 0x00010000U);
@@ -1034,6 +1016,157 @@  static unsigned long psu_ddr_phybringup_data(void)
 	return 1;
 }
 
+static int serdes_enb_coarse_saturation(void)
+{
+	Xil_Out32(0xFD402094, 0x00000010);
+	Xil_Out32(0xFD406094, 0x00000010);
+	Xil_Out32(0xFD40A094, 0x00000010);
+	Xil_Out32(0xFD40E094, 0x00000010);
+	return 1;
+}
+
+static int serdes_fixcal_code(void)
+{
+	int maskstatus = 1;
+	unsigned int rdata = 0;
+	unsigned int match_pmos_code[23];
+	unsigned int match_nmos_code[23];
+	unsigned int match_ical_code[7];
+	unsigned int match_rcal_code[7];
+	unsigned int p_code = 0;
+	unsigned int n_code = 0;
+	unsigned int i_code = 0;
+	unsigned int r_code = 0;
+	unsigned int repeat_count = 0;
+	unsigned int L3_TM_CALIB_DIG20 = 0;
+	unsigned int L3_TM_CALIB_DIG19 = 0;
+	unsigned int L3_TM_CALIB_DIG18 = 0;
+	unsigned int L3_TM_CALIB_DIG16 = 0;
+	unsigned int L3_TM_CALIB_DIG15 = 0;
+	unsigned int L3_TM_CALIB_DIG14 = 0;
+	int i = 0;
+
+	rdata = Xil_In32(0xFD40289C);
+	rdata = rdata & ~0x03;
+	rdata = rdata | 0x1;
+	Xil_Out32(0xFD40289C, rdata);
+	int count = 0;
+	do {
+		if (count == 1100000)
+			break;
+		rdata = Xil_In32(0xFD402B1C);
+		count++;
+	} while ((rdata & 0x0000000E) != 0x0000000E);
+
+	for (i = 0; i < 23; i++) {
+		match_pmos_code[i] = 0;
+		match_nmos_code[i] = 0;
+	}
+	for (i = 0; i < 7; i++) {
+		match_ical_code[i] = 0;
+		match_rcal_code[i] = 0;
+	}
+
+	do {
+		Xil_Out32(0xFD410010, 0x00000000);
+		Xil_Out32(0xFD410014, 0x00000000);
+
+		Xil_Out32(0xFD410010, 0x00000001);
+		Xil_Out32(0xFD410014, 0x00000000);
+
+		maskstatus = mask_poll(0xFD40EF14, 0x2);
+		if (maskstatus == 0) {
+			xil_printf("#SERDES initialization timed out\n\r");
+			return maskstatus;
+		}
+
+		p_code = mask_read(0xFD40EF18, 0xFFFFFFFF);
+		n_code = mask_read(0xFD40EF1C, 0xFFFFFFFF);
+		;
+		i_code = mask_read(0xFD40EF24, 0xFFFFFFFF);
+		r_code = mask_read(0xFD40EF28, 0xFFFFFFFF);
+		;
+
+		if (p_code >= 0x26 && p_code <= 0x3C)
+			match_pmos_code[p_code - 0x26] += 1;
+
+		if (n_code >= 0x26 && n_code <= 0x3C)
+			match_nmos_code[n_code - 0x26] += 1;
+
+		if (i_code >= 0xC && i_code <= 0x12)
+			match_ical_code[i_code - 0xC] += 1;
+
+		if (r_code >= 0x6 && r_code <= 0xC)
+			match_rcal_code[r_code - 0x6] += 1;
+
+	} while (repeat_count++ < 10);
+
+	for (i = 0; i < 23; i++) {
+		if (match_pmos_code[i] >= match_pmos_code[0]) {
+			match_pmos_code[0] = match_pmos_code[i];
+			p_code = 0x26 + i;
+		}
+		if (match_nmos_code[i] >= match_nmos_code[0]) {
+			match_nmos_code[0] = match_nmos_code[i];
+			n_code = 0x26 + i;
+		}
+	}
+
+	for (i = 0; i < 7; i++) {
+		if (match_ical_code[i] >= match_ical_code[0]) {
+			match_ical_code[0] = match_ical_code[i];
+			i_code = 0xC + i;
+		}
+		if (match_rcal_code[i] >= match_rcal_code[0]) {
+			match_rcal_code[0] = match_rcal_code[i];
+			r_code = 0x6 + i;
+		}
+	}
+
+	L3_TM_CALIB_DIG20 = mask_read(0xFD40EC50, 0xFFFFFFF0);
+	L3_TM_CALIB_DIG20 = L3_TM_CALIB_DIG20 | 0x8 | ((p_code >> 2) & 0x7);
+
+	L3_TM_CALIB_DIG19 = mask_read(0xFD40EC4C, 0xFFFFFF18);
+	L3_TM_CALIB_DIG19 = L3_TM_CALIB_DIG19 | ((p_code & 0x3) << 6)
+	    | 0x20 | 0x4 | ((n_code >> 3) & 0x3);
+
+	L3_TM_CALIB_DIG18 = mask_read(0xFD40EC48, 0xFFFFFF0F);
+	L3_TM_CALIB_DIG18 = L3_TM_CALIB_DIG18 | ((n_code & 0x7) << 5) | 0x10;
+
+	L3_TM_CALIB_DIG16 = mask_read(0xFD40EC40, 0xFFFFFFF8);
+	L3_TM_CALIB_DIG16 = L3_TM_CALIB_DIG16 | ((r_code >> 1) & 0x7);
+
+	L3_TM_CALIB_DIG15 = mask_read(0xFD40EC3C, 0xFFFFFF30);
+	L3_TM_CALIB_DIG15 = L3_TM_CALIB_DIG15 | ((r_code & 0x1) << 7)
+	    | 0x40 | 0x8 | ((i_code >> 1) & 0x7);
+
+	L3_TM_CALIB_DIG14 = mask_read(0xFD40EC38, 0xFFFFFF3F);
+	L3_TM_CALIB_DIG14 = L3_TM_CALIB_DIG14 | ((i_code & 0x1) << 7) | 0x40;
+
+	Xil_Out32(0xFD40EC50, L3_TM_CALIB_DIG20);
+	Xil_Out32(0xFD40EC4C, L3_TM_CALIB_DIG19);
+	Xil_Out32(0xFD40EC48, L3_TM_CALIB_DIG18);
+	Xil_Out32(0xFD40EC40, L3_TM_CALIB_DIG16);
+	Xil_Out32(0xFD40EC3C, L3_TM_CALIB_DIG15);
+	Xil_Out32(0xFD40EC38, L3_TM_CALIB_DIG14);
+	return maskstatus;
+}
+
+static int init_serdes(void)
+{
+	int status = 1;
+
+	status &= psu_resetin_init_data();
+
+	status &= serdes_fixcal_code();
+	status &= serdes_enb_coarse_saturation();
+
+	status &= psu_serdes_init_data();
+	status &= psu_resetout_init_data();
+
+	return status;
+}
+
 static void init_peripheral(void)
 {
 	psu_mask_write(0xFD5F0018, 0x8000001FU, 0x8000001FU);
@@ -1050,6 +1183,7 @@  int psu_init(void)
 	status &= psu_ddr_init_data();
 	status &= psu_ddr_phybringup_data();
 	status &= psu_peripherals_init_data();
+	status &= init_serdes();
 	init_peripheral();
 
 	status &= psu_afi_config();