1 files changed, 54 insertions, 0 deletions

diff --git a/firmware/rolo.c b/firmware/rolo.c
index 08223ae889..46819d82fb 100644
--- a/firmware/rolo.c
+++ b/firmware/rolo.c
@@ -30,6 +30,54 @@
 
 #define IRQ0_EDGE_TRIGGER 0x80
 
+/* test code, to be removed later */
+extern union /* defined in main.c */
+{
+    unsigned char  port8 [512];
+    unsigned short port16[256];
+    unsigned       port32[128];
+} startup_io;
+
+
+bool rolo_io_load(char* filename)
+{
+    int fd;
+    fd = open(filename, O_RDONLY);
+    if(fd >= 0) /* no complaint if it doesn't exit */
+    {
+        read(fd, (void *)&startup_io, sizeof(startup_io));
+        close(fd);
+        return true; /* success */
+    }
+    return false; /* not found */
+}
+
+void rolo_io_restore(void)
+{
+    int i;
+    for (i = 0; i < 512; i++)
+    {   /* most can be written with 8 bit access */
+        *((volatile unsigned char*)0x5FFFE00 + i) = startup_io.port8[i];
+    }
+    /* some don't allow that, write with 32 bit if aligned */
+    *((volatile unsigned char*)0x5FFFF40) = startup_io.port32[0x140/4];
+    *((volatile unsigned char*)0x5FFFF44) = startup_io.port32[0x144/4];
+    *((volatile unsigned char*)0x5FFFF50) = startup_io.port32[0x150/4];
+    *((volatile unsigned char*)0x5FFFF54) = startup_io.port32[0x154/4];
+    *((volatile unsigned char*)0x5FFFF60) = startup_io.port32[0x160/4];
+    *((volatile unsigned char*)0x5FFFF70) = startup_io.port32[0x170/4];
+    *((volatile unsigned char*)0x5FFFF74) = startup_io.port32[0x174/4];
+
+    /* write the rest with 16 bit */
+    *((volatile unsigned short*)0x5FFFF4A) = startup_io.port16[0x14A/2];
+    *((volatile unsigned short*)0x5FFFF5A) = startup_io.port16[0x15A/2];
+    *((volatile unsigned short*)0x5FFFF6A) = startup_io.port16[0x16A/2];
+    *((volatile unsigned short*)0x5FFFF7A) = startup_io.port16[0x17A/2];
+}
+/* end of test code */
+
+
+
 static void rolo_error(char *text)
 {
     lcd_clear_display();
@@ -55,6 +103,7 @@ int rolo_load(char* filename)
     unsigned short checksum,file_checksum;
     unsigned char* ramstart = (void*)0x09000000;
     void (*start_func)(void) = (void*)ramstart + 0x200;
+    bool restore_io; /* debug value */
 
     lcd_clear_display();
     lcd_puts(0, 0, "ROLO...");
@@ -122,6 +171,8 @@ int rolo_load(char* filename)
         return -1;
     }
 
+    restore_io = rolo_io_load("/startup_io.bin"); /* test code, recycle a variable */
+
     lcd_puts(0, 1, "Executing     ");
     lcd_update();
 
@@ -135,6 +186,9 @@ int rolo_load(char* filename)
     i2c_init();              /* Init i2c bus - it seems like a good idea */
     ICR = IRQ0_EDGE_TRIGGER; /* Make IRQ0 edge triggered */
 
+    if (restore_io) /* test code */
+        rolo_io_restore(); /* restore the I/Os from the file content */
+
     /* move firmware to start of ram */
     for ( i=0; i < length/4+1; i++ )
         ((unsigned int*)ramstart)[i] = ((unsigned int*)mp3buf)[i];