From c2e01a4cc220b9a1c1a6bb52e5f58bcc8d2edc55 Mon Sep 17 00:00:00 2001
From: manikandan-e <manikandan.hcl.ers.epl@gmail.com>
Date: Wed, 30 Oct 2019 19:43:51 +0530
Subject: [PATCH 2/2] board-aspeed-Add-Mux-for-yosemitev2

Signed-off-by: manikandan-e <manikandan.hcl.ers.epl@gmail.com>

---
 arch/arm/mach-aspeed/platform_g5.S | 213 +++++++++++++++++++++++++++++++------
 1 file changed, 183 insertions(+), 30 deletions(-)

diff --git a/arch/arm/mach-aspeed/platform_g5.S b/arch/arm/mach-aspeed/platform_g5.S
index 2ac1ca4..c3ce077 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, #0x0D                              @ Baudrate 115200
+    movne r1, #0x01                              @ Baudrate 115200, 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,36 @@ init_dram:
     /********************************************
        Initial Reset Procedure : Begin
      *******************************************/
+    /* save into SRAM */
+    ldr r0, =0x1e720200 /* vbs.uboot_exec_address */
+    str r4, [r0]
+
+    uart_console_setup
+
+    /* Debug - UART console message */
+    ldr r0, =0x1E784000
+    mov r1, #0x0D                                @ '\r'
+    str r1, [r0]
+    mov r1, #0x0A                                @ '\n'
+    str r1, [r0]
+    mov r1, #0x54                                @ 'S'
+    str r1, [r0]
+    mov r1, #0x50                                @ 'P'
+    str r1, [r0]
+    mov r1, #0x4C                                @ 'L'
+    str r1, [r0]
+    mov r1, #0x0D                                @ '\r'
+    str r1, [r0]
+    mov r1, #0x0A                                @ '\n'
+    str r1, [r0]
+    /* End Debug - UART console message */
+
+
+
+    ldr   r0, =0x1E720204
+    mov   r1, #0x0
+    str   r1, [r0]
+
     /* Clear AHB bus lock condition */
     ldr   r0, =0x1e600000
     ldr   r1, =0xAEED1A03
@@ -793,7 +973,9 @@ wait_ddr_reset:
     clear_delay_timer
     /* end delay 10ms */
 
-/* Debug - UART console message */
+    uart_console_setup
+
+  /* Debug - UART console message */
 #ifdef CONFIG_DRAM_UART_TO_UART1
     ldr   r0, =0x1e78909c                        @ route UART5 to UART Port1, 2016.08.29
     ldr   r1, =0x10000004
@@ -806,35 +988,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.7.4