From f09b84e61569f2b991cc3e257c34517a222922d3 Mon Sep 17 00:00:00 2001
From: Logananth Sundararaj <logananth.s@hcl.com>
Date: Fri, 3 Dec 2021 15:21:50 +0530
Subject: [PATCH] board-aspeed-Add-Mux-for-yosemitev2

Signed-off-by: Logananth Sundararaj <logananth.s@hcl.com>
---
 arch/arm/mach-aspeed/platform_g5.S | 191 ++++++++++++++++++++++++-----
 1 file changed, 162 insertions(+), 29 deletions(-)

diff --git a/arch/arm/mach-aspeed/platform_g5.S b/arch/arm/mach-aspeed/platform_g5.S
index 2ac1ca4721..e1c17ae4a3 100644
--- a/arch/arm/mach-aspeed/platform_g5.S
+++ b/arch/arm/mach-aspeed/platform_g5.S
@@ -302,6 +302,156 @@ TIME_TABLE_DDR4_1600:
     ldr   r2, =0x00000800
     .endm

+    .macro console_bmc
+    ldr r0, =0x1e780024
+ldr r1, [r0]
+orr r1, r1, #0xF
+str r1, [r0]
+
+ldr r0, =0x1e780020
+ldr r1, [r0]
+and r1, r1, #0xFFFFFFF0
+orr r1, r1, #0xC
+str r1, [r0]
+.endm
+
+.macro console_sel
+
+  // Disable SoL UARTs[1-4]
+  ldr r0, =0x1e6e2080
+  ldr r1, [r0]
+  ldr r2, =0xBFBFFFFF
+  and r1, r1, r2
+  str r1, [r0]
+
+  ldr r0, =0x1e6e2084
+  ldr r1, [r0]
+  and r1, r1, r2
+  str r1, [r0]
+  // Enable GPIOE[0-3] Tolerant
+  ldr r0, =0x1e78003c
+  ldr r1, [r0]
+  orr r1, r1, #0xF
+  str r1, [r0]
+
+  // Read debug card present
+  ldr r2, =0x1e780080
+  ldr r0, [r2]
+  and r0, r0, #0x00000800
+  ldr r1, =0x0800
+  cmp r0, r1
+  bne dbg_card_pres\@
+  console_bmc
+  b case_end\@
+
+dbg_card_pres\@:
+  // Read key position
+  ldr r2, =0x1e7801e0
+  ldr r0, [r2]
+  bic r1, r0, #0xFF0FFFFF
+  mov r0, r1, lsr #20
+  //Test for position#1
+  ldr r1, =0x00
+  cmp r0, r1
+  bne case_pos2\@
+ console_bmc
+  b case_end\@
+case_pos2\@:
+  //Test for position#2
+  ldr r1, =0x01
+  cmp r0, r1
+  bne case_pos3\@
+ console_bmc
+  b case_end\@
+case_pos3\@:
+ //Test for position#3
+  ldr r1, =0x02
+  cmp r0, r1
+  bne case_pos4\@
+  console_bmc
+  b case_end\@
+case_pos4\@:
+//Test for position#4
+  ldr r1, =0x03
+  cmp r0, r1
+  bne case_pos5\@
+  console_bmc
+  b case_end\@
+case_pos5\@:
+  //Test for position#5
+  ldr r1, =0x04
+  cmp r0, r1
+  bne case_pos6\@
+  console_bmc
+  b case_end\@
+case_pos6\@:
+  //Test for position#6
+  ldr r1, =0x05
+  cmp r0, r1
+  bne case_pos7\@
+  console_bmc
+  b case_end\@
+case_pos7\@:
+ //Test for position#7
+   ldr r1, =0x06
+   cmp r0, r1
+   bne case_pos8\@
+   console_bmc
+   b case_end\@
+case_pos8\@:
+  //Test for position#8
+   ldr r1, =0x07
+   cmp r0, r1
+   bne case_pos9\@
+   console_bmc
+   b case_end\@
+case_pos9\@:
+    //Test for position#9
+   ldr r1, =0x08
+   cmp r0, r1
+   bne case_pos10\@
+   console_bmc
+   b case_end\@
+case_pos10\@:
+   //Test for position#10
+   ldr r1, =0x09
+   cmp r0, r1
+   bne case_end\@
+   console_bmc
+   b case_end\@
+case_end\@:
+.endm
+
+    .macro uart_console_setup
+    console_sel
+    /* setup UART console */
+    ldr   r0, =0x1E78400C
+    mov   r1, #0x83
+    str   r1, [r0]
+
+    ldr   r0, =0x1e6e202c
+    ldr   r2, [r0]
+    mov   r2, r2, lsr #12
+    tst   r2, #0x01
+    ldr   r0, =0x1E784000
+    moveq r1, #0x1A                              @ Baudrate 57600
+    movne r1, #0x02                              @ Baudrate 57600, div13
+
+    str   r1, [r0]
+
+    ldr   r0, =0x1E784004
+    mov   r1, #0x00
+    str   r1, [r0]
+
+    ldr   r0, =0x1E78400C
+    mov   r1, #0x03
+    str   r1, [r0]
+
+    ldr   r0, =0x1E784008
+    mov   r1, #0x07
+    str   r1, [r0]
+    .endm
+
     .macro print_hex_char
     and   r1, r1, #0xF
     cmp   r1, #9
@@ -324,6 +474,16 @@ init_dram:
     /********************************************
        Initial Reset Procedure : Begin
      *******************************************/
+    /* save into SRAM */
+    ldr r0, =0x1e720200 /* vbs.uboot_exec_address */
+    str r4, [r0]
+
+    uart_console_setup
+
+    ldr   r0, =0x1E720204
+    mov   r1, #0x0
+    str   r1, [r0]
+
     /* Clear AHB bus lock condition */
     ldr   r0, =0x1e600000
     ldr   r1, =0xAEED1A03
@@ -793,6 +953,8 @@ wait_ddr_reset:
     clear_delay_timer
     /* end delay 10ms */

+    uart_console_setup
+
 /* Debug - UART console message */
 #ifdef CONFIG_DRAM_UART_TO_UART1
     ldr   r0, =0x1e78909c                        @ route UART5 to UART Port1, 2016.08.29
@@ -806,35 +968,6 @@ wait_ddr_reset:
     str   r1, [r0]
 #endif

-    ldr   r0, =0x1e78400c
-    mov   r1, #0x83
-    str   r1, [r0]
-
-    ldr   r0, =0x1e6e202c
-    ldr   r2, [r0]
-    mov   r2, r2, lsr #12
-    tst   r2, #0x01
-    ldr   r0, =0x1e784000
-    moveq r1, #0x0D                              @ Baudrate 115200
-    movne r1, #0x01                              @ Baudrate 115200, div13
-#ifdef CONFIG_DRAM_UART_38400
-    moveq r1, #0x27                              @ Baudrate 38400
-    movne r1, #0x03                              @ Baudrate 38400 , div13
-#endif
-    str   r1, [r0]
-
-    ldr   r0, =0x1e784004
-    mov   r1, #0x00
-    str   r1, [r0]
-
-    ldr   r0, =0x1e78400c
-    mov   r1, #0x03
-    str   r1, [r0]
-
-    ldr   r0, =0x1e784008
-    mov   r1, #0x07
-    str   r1, [r0]
-
     ldr   r0, =0x1e784000
     mov   r1, #0x0D                              @ '\r'
     str   r1, [r0]
--
2.17.1