From b16cca5a8c10d41da5b07f66de6a6710c88bb859 Mon Sep 17 00:00:00 2001
From: rw-r-r-0644 <r.r.qwertyuiop.r.r@gmail.com>
Date: Sun, 30 May 2021 20:12:38 +0200
Subject: [PATCH] lolserial_asm: fix accidental register corruption in debug
 builds, code improvements

oof
---
 stage2/lolserial_asm.S | 97 ++++++++++++++++++++++--------------------
 1 file changed, 51 insertions(+), 46 deletions(-)

diff --git a/stage2/lolserial_asm.S b/stage2/lolserial_asm.S
index 9ccd8f0..05e4416 100644
--- a/stage2/lolserial_asm.S
+++ b/stage2/lolserial_asm.S
@@ -6,7 +6,7 @@
  *  This code is licensed to you under the terms of the GNU GPL, version 2;
  *  see file COPYING or http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt
  */
- 
+
 #ifdef LOLSERIAL_DEBUG
 
 #include "latte.h"
@@ -27,72 +27,77 @@
 
 .equ LOLSERIAL_WAIT_TICKS, 200
 .equ GP_SENSORBAR, 0x00000100
-.equ GP_SENSORBAR_SHIFT, 8
 
+@ arg:r0 string address
 lolserial_print:
     mov r1, #-1
+
+@ arg:r0 string address
+@ arg:r1 max length
 lolserial_lprint:
-    push {r5-r6}
-    add r1, r1, r0
+    push {r4-r5}
+    add r1, r0                                          @ end address in r1
+    ldr r5, =LT_REG_BASE                                @ latte registers base in r5
 
-    ldr r6, =0x0D800000
+    @ clear32(LT_GPIO_OWNER, GP_SENSORBAR)
+    ldr r2, [r5, #0x0FC]
+    bic r2, r2, #GP_SENSORBAR
+    str r2, [r5, #0x0FC]
 
-    /* clear32(LT_GPIO_OWNER, GP_SENSORBAR) */
-    ldr r5, [r6, #0x0FC]
-    bic r5, r5, #GP_SENSORBAR
-    str r5, [r6, #0x0FC]
+    @ set32(LT_GPIO_ENABLE, GP_SENSORBAR)
+    ldr r2, [r5, #0x0DC]
+    orr r2, r2, #GP_SENSORBAR
+    str r2, [r5, #0x0DC]
 
-    /* set32(LT_GPIO_ENABLE, GP_SENSORBAR) */
-    ldr r5, [r6, #0x0DC]
-    orr r5, r5, #GP_SENSORBAR
-    str r5, [r6, #0x0DC]
+    @ set32(LT_GPIO_DIR, GP_SENSORBAR)
+    ldr r2, [r5, #0x0E4]
+    orr r2, r2, #GP_SENSORBAR
+    str r2, [r5, #0x0E4]
 
-    /* set32(LT_GPIO_DIR, GP_SENSORBAR) */
-    ldr r5, [r6, #0x0E4]
-    orr r5, r5, #GP_SENSORBAR
-    str r5, [r6, #0x0E4]
-
-    /* set32(LT_GPIO_OUT, GP_SENSORBAR) */
-    ldr r5, [r6, #0x0E0]
-    orr r5, r5, #GP_SENSORBAR
-    str r5, [r6, #0x0E0]
+    @ set32(LT_GPIO_OUT, GP_SENSORBAR)
+    ldr r2, [r5, #0x0E0]
+    orr r2, r2, #GP_SENSORBAR
+    str r2, [r5, #0x0E0]
 
     lolserial_send_string_loop:
-        cmp r0, r1
-        ldrneb r5, [r0], #1
-        cmpne r5, #0
-        beq lolserial_send_string_end    
+        cmp r0, r1                                      @ ensure end address was not reached
+        ldrneb r3, [r0], #1                             @   load cur char in r3, increment r0 string ptr
+        cmpne r3, #0                                    @   check for string null termination
+        beq lolserial_send_string_end
 
-        mov r3, #0x200
-        orr r3, r3, r5, lsl #1
+        @ r3: bits to send (serial end bit, data byte, serial start bit)
+        @ r3 = (1 << 9) | (r3:chr << 1) | (0 << 0)
+        lsl r3, #1
+        orr r3, #0x200
 
+        @ loop over bits to send
         lolserial_send_char_loop:
-            and r4, r3, #1
+            @ mask32(LT_GPIO_OUT, GP_SENSORBAR, bit ? GP_SENSORBAR : 0)
+            tst r3, #1
+            ldr r2, [r5, #0x0E0]
+            biceq r2, #GP_SENSORBAR
+            orrne r2, #GP_SENSORBAR
+            str r2, [r5, #0x0E0]
 
-            ldr r5, [r6, #0x0E0]
-            bic r5, r5, #GP_SENSORBAR
-            orr r5, r5, r4, lsl #GP_SENSORBAR_SHIFT
-            str r5, [r6, #0x0E0]
-
-            ldr r5, [r6, #0x010]
-            adds r4, r5, #LOLSERIAL_WAIT_TICKS
-            bcc timer_wait_loop
+            @ wait for LOLSERIAL_WAIT_TICKS timer ticks
+            ldr r2, [r5, #0x010]                        @ r2: current ticks
+            adds r4, r2, #LOLSERIAL_WAIT_TICKS          @ r4: wait time end
             timer_wait_overflow_loop:
-                ldr r2, [r6, #0x010]
-                cmp r2, r5
-                bhs timer_wait_overflow_loop
+                cmp r4, r2
+                ldr r2, [r5, #0x010]
+                blo timer_wait_overflow_loop
             timer_wait_loop:
-                ldr r5, [r6, #0x010]
-                cmp r5, r4
-                blo timer_wait_loop
+                cmp r4, r2
+                ldr r2, [r5, #0x010]
+                bhi timer_wait_loop
 
-            movs r3, r3, lsr #1
-            bne lolserial_send_char_loop            
+            lsrs r3, #1                                 @ go to next bit
+            bne lolserial_send_char_loop                @ unless r3 is 0 (end bit sent)
         
         b lolserial_send_string_loop
     lolserial_send_string_end:
 
-    pop {r5-r6}
+    pop {r4-r5}
     bx lr
 
 #endif /* LOLSERIAL_DEBUG */