From 4335d4e5311d38f87e2b1aee8a3fad98570cb40d Mon Sep 17 00:00:00 2001
From: David Banks <dave@hoglet.com>
Date: Tue, 25 Apr 2017 18:24:49 +0100
Subject: [PATCH] Early work on Mode 7

Change-Id: I0cea302fd7ea9ea8cb3649721185c2351b7084dc
---
 src/rgb_to_fb.S     | 37 +++++++++++++++++++++++--------------
 src/rgb_to_hdmi.c   | 29 +++++++++++++++++++++++------
 vhdl/RGBtoHDMI.vhdl |  8 +++++++-
 3 files changed, 53 insertions(+), 21 deletions(-)

diff --git a/src/rgb_to_fb.S b/src/rgb_to_fb.S
index 0b63a3ab..cd65e382 100644
--- a/src/rgb_to_fb.S
+++ b/src/rgb_to_fb.S
@@ -14,31 +14,38 @@
 .text
 .global rgb_to_fb
 
-//  r0 = pointer to frame buffer
-//  r1
-//  r2
-//  r3 = field offset
+// Parameter registers:
+//
+//  r0 = frame buffer base
+//  r1 = 8-pixel characters per line (normally 80, but 62.5 (63) in Mode 7
+//  r2 = bytes per line
+//
+// Working registers:
+//
+//  r3 = field offsset
 //  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
-
+// r11 = pointer into frame buffer (start of line)
+// r12 = pointer into frame buffer (moves within line)
+        
 rgb_to_fb:
 
         push   {r4-r12, lr}
 
-        mov    r3, #320
+        mov    r3, r2
         ldr    r4, =GPLEV0
 
 frame:
         // Set framebuffer address
 #ifdef LINE_DOUBLE        
-        mov    r1, r0
+        mov    r11, r0
 #else
-        add    r1, r0, r3
-        eor    r3, r3, #320
+        add    r11, r0, r3
+        eor    r3, r3, r2
 #endif
         
         // Wait for start of vsync
@@ -97,8 +104,10 @@ wait_for_hsync:
         tst    r8, #HSYNC_MASK
         beq    wait_for_hsync
 
-        // 80 8-bit characters per line
-        mov    r6, #80
+        // number of 8-bit characters per line
+        mov    r6, r1
+        // pointer into frame buffer
+        mov    r12, r11
 loop2:
 
         // Initialize 8 pixel block
@@ -157,15 +166,15 @@ wait_psync_10:
         orr    r10, r10, r9, lsl #13
 
 #ifdef LINE_DOUBLE        
-        str    r10, [r1, r3]
+        str    r10, [r12, r2]
 #endif        
-        str    r10, [r1], #4
+        str    r10, [r12], #4
 
         subs   r6, r6, #1
         bne    loop2
 
         // Skip a whole line to maintain aspect ratio
-        add    r1, r1, #320
+        add    r11, r11, r2, lsl #1
 
         subs   r5, r5, #1
         bne    loop1
diff --git a/src/rgb_to_hdmi.c b/src/rgb_to_hdmi.c
index b0d5f7d4..6a65c25a 100644
--- a/src/rgb_to_hdmi.c
+++ b/src/rgb_to_hdmi.c
@@ -11,6 +11,20 @@
 #include "rpi-mailbox-interface.h"
 #include "startup.h"
 
+#define CORE_FREQ        383900000
+
+// #define MODE7
+
+#ifdef MODE7
+#define GPCLK_DIVISOR    24      // 48MHz
+#define CHARS_PER_LINE   63
+#define BYTES_PER_LINE   320
+#else
+#define GPCLK_DIVISOR    18      // 64MHz
+#define CHARS_PER_LINE   80
+#define BYTES_PER_LINE   320
+#endif
+
 #define GZ_CLK_BUSY    (1 << 7)
 
 #define GP_CLK1_CTL (uint32_t *)(PERIPHERAL_BASE + 0x101078)
@@ -41,23 +55,26 @@
 #define COLBITS         4
 #endif
 
-extern void rgb_to_fb(unsigned char *fb);
+extern void rgb_to_fb(unsigned char *fb, int chars_per_line, int bytes_per_line);
 
 
 void init_gpclk(int source, int divisor) {
 
 
    RPI_PropertyInit();
-   RPI_PropertyAddTag( TAG_SET_CLOCK_RATE, CORE_CLK_ID, 383900000, 1);
+   RPI_PropertyAddTag( TAG_SET_CLOCK_RATE, CORE_CLK_ID, CORE_FREQ, 1);
    RPI_PropertyProcess();
 
+   // Re-initialize UART, as system clock rate changed
+   RPI_AuxMiniUartInit( 115200, 8 );
+
    log_info("GP_CLK1_CTL = %08"PRIx32, *GP_CLK1_CTL);
    log_info("GP_CLK1_DIV = %08"PRIx32, *GP_CLK1_DIV);
 
    *GP_CLK1_CTL = 0x5A000000 + source;
 
    while ((*GP_CLK1_CTL) & GZ_CLK_BUSY) {}    // Wait for BUSY low
-   *GP_CLK1_DIV = 0x5A002000 | (divisor << 12); // set DIVI
+   *GP_CLK1_DIV = 0x5A000000 | (divisor << 12); // set DIVI
    *GP_CLK1_CTL = 0x5A000010 | source;    // GPCLK0 on
 
    log_info("GP_CLK1_CTL = %08"PRIx32, *GP_CLK1_CTL);
@@ -208,10 +225,10 @@ void init_hardware() {
 
    // Source 1 = OSC = 19.2MHz
    // Source 4 = PLLA = 0MHz
-   // Source 5 = PLLC = core_freq * 3 (
+   // Source 5 = PLLC = core_freq * 3 = (384 * 3) = 1152
    // Source 6 = PLLD = 500MHz
 
-   init_gpclk(5, 18);
+   init_gpclk(5, GPCLK_DIVISOR);
 
 
   // Initialise the info system with cached values (as we break the GPU property interface)
@@ -254,7 +271,7 @@ void kernel_main(unsigned int r0, unsigned int r1, unsigned int atags)
 
 
    while (1) {
-      rgb_to_fb(fb);
+      rgb_to_fb(fb, CHARS_PER_LINE, BYTES_PER_LINE);
    }
 
 
diff --git a/vhdl/RGBtoHDMI.vhdl b/vhdl/RGBtoHDMI.vhdl
index 935b0689..ae6b4b35 100644
--- a/vhdl/RGBtoHDMI.vhdl
+++ b/vhdl/RGBtoHDMI.vhdl
@@ -40,6 +40,12 @@ end RGBtoHDMI;
 
 architecture Behavorial of RGBtoHDMI is
 
+    -- For Mode 7
+    -- constant sample_point : unsigned(10 downto 0) := to_unsigned(2048 - 48 * 2 + 3, 11);
+    
+    -- For Modes 6..0
+    constant sample_point : unsigned(10 downto 0) := to_unsigned(2048 - 32 * 23 + 3, 11);
+
     signal shift : std_logic_vector(11 downto 0);
 
     signal nCSYNC1 : std_logic;
@@ -104,7 +110,7 @@ begin
                 end if;
                     
                 -- counter is used to find sampling point for first pixel
-                counter <= to_unsigned(1311, counter'length);
+                counter <= sample_point;
 
             elsif nCSYNC1 = '0' then
                 -- within the line sync pulse