From 2c547292fa1e668488224d3b03edd5a7dd1b2dc2 Mon Sep 17 00:00:00 2001
From: David Banks <dave@hoglet.com>
Date: Mon, 24 Apr 2017 22:34:39 +0100
Subject: [PATCH] Added code to copy RGB to framebuffer

Change-Id: Id738bdab509091155d4aeff5cf652ca4334e93a6
---
 src/CMakeLists.txt |   1 +
 src/defs.h         |   5 ++
 src/rgb_to_fb.S    | 127 +++++++++++++++++++++++++++++++++++++++++++++
 src/rgb_to_hdmi.c  |  14 +++--
 4 files changed, 142 insertions(+), 5 deletions(-)
 create mode 100644 src/rgb_to_fb.S

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index b7616213..2aee9b17 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -61,6 +61,7 @@ file( GLOB core_files
     armc-cstartup.c
     armc-cstubs.c
     rgb_to_hdmi.c
+    rgb_to_fb.S
     defs.h
     arm-exception.c
     cache.c
diff --git a/src/defs.h b/src/defs.h
index 42482e9c..7c3b76fa 100644
--- a/src/defs.h
+++ b/src/defs.h
@@ -27,4 +27,9 @@
 
 #define LED_PIN      (47)
 
+#define PSYNC_MASK    (1 << PSYNC_PIN)
+#define HSYNC_MASK    (1 << HSYNC_PIN)
+#define VSYNC_MASK    (1 << VSYNC_PIN)
+#define FIELD_MASK    (1 << FIELD_PIN)
+
 #endif
diff --git a/src/rgb_to_fb.S b/src/rgb_to_fb.S
new file mode 100644
index 00000000..c17c5377
--- /dev/null
+++ b/src/rgb_to_fb.S
@@ -0,0 +1,127 @@
+#include "rpi-base.h"
+#include "defs.h"
+
+        
+.text
+.global rgb_to_fb
+        
+//  r0 = pointer to frame buffer
+//  r1
+//  r2
+//  r3
+//  r4 = GPLEV0
+//  r5 = line counter
+//  r6 = pixel counter
+//  r8 = value read from GPLEV0
+//  r9 = extracted pixel
+// r10 = block of 8 pixels, to be written to FB 
+        
+rgb_to_fb:
+
+        push    {r4-r12, lr}
+
+loop0:
+        mov     r1, r0
+        
+        ldr     r4, =GPLEV0
+        mov     r5, #256
+                
+loop1:  
+        
+        // Wait for active high hsync
+wait_for_hsync:
+        ldr    r8, [r4]
+        tst    r8, #HSYNC_MASK
+        beq    wait_for_hsync
+
+        // 80 8-bit characters per line
+        mov    r6, #80
+loop2:
+
+        // Initialize 8 pixel block
+        mov    r10, #0
+        
+        // Wait for 0-1 edge on PSYNC
+wait_psync_01:
+        ldr    r8, [r4]
+        tst    r8, #PSYNC_MASK
+        beq    wait_psync_01
+
+        // Pixel 0 in GPIO  4.. 2 ->  3.. 0
+        // Pixel 1 in GPIO  7.. 5 ->  7.. 4
+        // Pixel 2 in GPIO 10.. 8 -> 11.. 8
+        // Pixel 3 in GPIO 13..11 -> 15..12
+        
+        and    r9, r8, #(7 << PIXEL_BASE)
+        orr    r10, r10, r9, lsr #2
+        
+        and    r9, r8, #(7 << (PIXEL_BASE + 3))
+        orr    r10, r10, r9, lsr #1
+
+        and    r9, r8, #(7 << (PIXEL_BASE + 6))
+        orr    r10, r10, r9
+
+        and    r9, r8, #(7 << (PIXEL_BASE + 9))
+        orr    r10, r10, r9, lsl #1
+
+        // Wait for 1-0 edge on PSYNC
+wait_psync_10:
+        ldr    r8, [r4]
+        tst    r8, #PSYNC_MASK
+        bne    wait_psync_10
+
+        // Pixel 4 in GPIO  4.. 2 -> 19..16
+        // Pixel 5 in GPIO  7.. 5 -> 23..20
+        // Pixel 6 in GPIO 10.. 8 -> 27..24
+        // Pixel 7 in GPIO 13..11 -> 31..28
+        
+        and    r9, r8, #(7 << PIXEL_BASE)
+        orr    r10, r10, r9, lsl #14
+        
+        and    r9, r8, #(7 << (PIXEL_BASE + 3))
+        orr    r10, r10, r9, lsl #15
+
+        and    r9, r8, #(7 << (PIXEL_BASE + 6))
+        orr    r10, r10, r9, lsl #16
+
+        and    r9, r8, #(7 << (PIXEL_BASE + 9))
+        orr    r10, r10, r9, lsl #17
+
+
+        str    r10, [r1], #4
+        
+        
+        subs    r6, r6, #1
+        bne     loop2
+
+
+        subs    r5, r5, #1
+        bne     loop1
+
+
+// Temporary hack - skip inactive lines
+        ldr     r5, =(312-256)
+                
+loop3:  
+        
+wait_for_hsync_a:
+        ldr    r8, [r4]
+        tst    r8, #HSYNC_MASK
+        beq    wait_for_hsync_a
+
+wait_for_hsync_b:
+        ldr    r8, [r4]
+        tst    r8, #HSYNC_MASK
+        bne    wait_for_hsync_b
+
+        subs    r5, r5, #1
+        bne     loop3
+
+
+        b       loop0
+        
+// Return        
+        
+        pop     {r4-r12, lr}
+        mov     pc, lr
+        
diff --git a/src/rgb_to_hdmi.c b/src/rgb_to_hdmi.c
index 88c83854..6e21189b 100644
--- a/src/rgb_to_hdmi.c
+++ b/src/rgb_to_hdmi.c
@@ -11,17 +11,13 @@
 #include "rpi-mailbox-interface.h"
 #include "startup.h"
 
-
-
 #define GZ_CLK_BUSY    (1 << 7)
 
 #define GP_CLK1_CTL (uint32_t *)(PERIPHERAL_BASE + 0x101078)
 #define GP_CLK1_DIV (uint32_t *)(PERIPHERAL_BASE + 0x10107C)
 
-
-
 #define SCREEN_WIDTH    640
-#define SCREEN_HEIGHT   480
+#define SCREEN_HEIGHT   512
 
 #define BPP4
 
@@ -45,6 +41,8 @@
 #define COLBITS         4
 #endif
 
+extern void rgb_to_fb(unsigned char *fb);
+
 
 void init_gpclk(int source, int divisor) {
 
@@ -257,6 +255,12 @@ void kernel_main(unsigned int r0, unsigned int r1, unsigned int atags)
    enable_MMU_and_IDCaches();
    _enable_unaligned_access();
 
+
+   while (1) {
+      rgb_to_fb(fb);
+   }
+
+
    log_info("RGB to HDMI finished\r\n");
   
    while (1);