7 files changed, 582 insertions, 32 deletions

diff --git a/bootloader/SOURCES b/bootloader/SOURCES
index 12347c57c9..a9c6aebd37 100644
--- a/bootloader/SOURCES
+++ b/bootloader/SOURCES
@@ -2,6 +2,8 @@ common.c
 
 #if defined(IPOD_NANO2G)
 ipodnano2g.c
+#elif defined(IPOD_6G)
+ipod6g.c
 #elif defined(IPOD_ARCH)
 ipod.c
 #elif defined(GIGABEAT_F)
diff --git a/bootloader/ipod6g.c b/bootloader/ipod6g.c
new file mode 100644
index 0000000000..0ab9444578
--- /dev/null
+++ b/bootloader/ipod6g.c
@@ -0,0 +1,462 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2005 by Dave Chapman
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdarg.h>
+#include <string.h>
+
+#include "config.h"
+
+#include "inttypes.h"
+#include "cpu.h"
+#include "system.h"
+#include "lcd.h"
+#include "../kernel-internal.h"
+#include "file_internal.h"
+#include "storage.h"
+#include "fat.h"
+#include "disk.h"
+#include "font.h"
+#include "backlight.h"
+#include "backlight-target.h"
+#include "button.h"
+#include "panic.h"
+#include "power.h"
+#include "file.h"
+#include "common.h"
+#include "rb-loader.h"
+#include "loader_strerror.h"
+#include "version.h"
+#include "powermgmt.h"
+#include "usb.h"
+#ifdef HAVE_SERIAL
+#include "serial.h"
+#endif
+
+#include "s5l8702.h"
+#include "clocking-s5l8702.h"
+#include "spi-s5l8702.h"
+#include "i2c-s5l8702.h"
+#include "gpio-s5l8702.h"
+#include "pmu-target.h"
+#include "nor-target.h"
+
+
+#define FW_ROCKBOX  0
+#define FW_APPLE    1
+
+#define ERR_RB  0
+#define ERR_OF  1
+#define ERR_HDD 2
+
+/* Safety measure - maximum allowed firmware image size.
+   The largest known current (October 2009) firmware is about 6.2MB so
+   we set this to 8MB.
+*/
+#define MAX_LOADSIZE (8*1024*1024)
+
+#define LCD_RBYELLOW    LCD_RGBPACK(255,192,0)
+#define LCD_REDORANGE   LCD_RGBPACK(255,70,0)
+
+extern void bss_init(void);
+extern uint32_t _movestart;
+extern uint32_t start_loc;
+
+extern int line;
+
+#ifdef HAVE_BOOTLOADER_USB_MODE
+static void usb_mode(void)
+{
+    int button;
+
+    verbose = true;
+
+    printf("Entering USB mode...");
+
+    powermgmt_init();
+
+    /* The code will ask for the maximum possible value */
+    usb_charging_enable(USB_CHARGING_ENABLE);
+
+    usb_init();
+    usb_start_monitoring();
+
+    /* Wait until USB is plugged */
+    while (usb_detect() != USB_INSERTED)
+    {
+        printf("Plug USB cable");
+        line--;
+        sleep(HZ/10);
+    }
+
+    while(1)
+    {
+        button = button_get_w_tmo(HZ/10);
+
+        if (button == SYS_USB_CONNECTED)
+            break; /* Hit */
+
+        if (usb_detect() == USB_EXTRACTED)
+            break; /* Cable pulled */
+
+        /* Wait for threads to connect or cable is pulled */
+        printf("USB: Connecting...");
+        line--;
+    }
+
+    if (button == SYS_USB_CONNECTED)
+    {
+        /* Got the message - wait for disconnect */
+        printf("Bootloader USB mode");
+
+        /* Ack the SYS_USB_CONNECTED polled from the button queue */
+        usb_acknowledge(SYS_USB_CONNECTED_ACK);
+
+        while(1)
+        {
+            button = button_get_w_tmo(HZ/2);
+            if (button == SYS_USB_DISCONNECTED)
+                break;
+        }
+    }
+
+    /* We don't want the HDD to spin up if the USB is attached again */
+    usb_close();
+    printf("USB mode exit     ");
+}
+#endif /* HAVE_BOOTLOADER_USB_MODE */
+
+void fatal_error(int err)
+{
+    verbose = true;
+
+    /* System font is 6 pixels wide */
+    line++;
+    switch (err)
+    {
+        case ERR_RB:
+#ifdef HAVE_BOOTLOADER_USB_MODE
+            usb_mode();
+            printf("Hold MENU+SELECT to reboot");
+            break;
+#endif
+        case ERR_HDD:
+            printf("Hold MENU+SELECT to reboot");
+            printf("then SELECT+PLAY for disk mode");
+            break;
+        case ERR_OF:
+            printf("Hold MENU+SELECT to reboot");
+            printf("and enter Rockbox firmware");
+            break;
+    }
+
+    if (ide_powered())
+        ata_sleepnow(); /* Immediately spindown the disk. */
+
+    line++;
+    lcd_set_foreground(LCD_REDORANGE);
+    while (1) {
+        lcd_puts(0, line, button_hold() ? "Hold switch on!"
+                                        : "               ");
+        lcd_update();
+    }
+}
+
+static void battery_trap(void)
+{
+    int vbat, old_verb;
+    int th = 50;
+
+    old_verb = verbose;
+    verbose = true;
+
+    usb_charging_maxcurrent_change(100);
+
+    while (1)
+    {
+        vbat = _battery_voltage();
+
+        /*  Two reasons to use this threshold (may require adjustments):
+         *  - when USB (or wall adaptor) is plugged/unplugged, Vbat readings
+         *    differ as much as more than 200 mV when charge current is at
+         *    maximum (~340 mA).
+         *  - RB uses some sort of average/compensation for battery voltage
+         *    measurements, battery icon blinks at battery_level_dangerous,
+         *    when the HDD is used heavily (large database) the level drops
+         *    to battery_level_shutoff quickly.
+         */
+        if (vbat >= battery_level_dangerous[0] + th)
+            break;
+        th = 200;
+
+        if (power_input_status() != POWER_INPUT_NONE) {
+            lcd_set_foreground(LCD_RBYELLOW);
+            printf("Low battery: %d mV, charging...     ", vbat);
+            sleep(HZ*3);
+        }
+        else {
+            /* Wait for the user to insert a charger */
+            int tmo = 10;
+            lcd_set_foreground(LCD_REDORANGE);
+            while (1) {
+                vbat = _battery_voltage();
+                printf("Low battery: %d mV, power off in %d ", vbat, tmo);
+                if (!tmo--) {
+                    /* Raise Vsysok (hyst=0.02*Vsysok) to avoid PMU
+                       standby<->active looping */
+                    if (vbat < 3200)
+                        pmu_write(PCF5063X_REG_SVMCTL, 0xA /*3200mV*/);
+                    power_off();
+                }
+                sleep(HZ*1);
+                if (power_input_status() != POWER_INPUT_NONE)
+                    break;
+                line--;
+            }
+        }
+        line--;
+    }
+
+    verbose = old_verb;
+    lcd_set_foreground(LCD_WHITE);
+    printf("Battery status ok: %d mV            ", vbat);
+}
+
+static int launch_onb(int clkdiv)
+{
+    /* SPI clock = PClk/(clkdiv+1) */
+    spi_clkdiv(SPI_PORT, clkdiv);
+
+    /* Actually IRAM1_ORIG contains current RB bootloader IM3 header,
+       it will be replaced by ONB IM3 header, so this function must
+       be called once!!! */
+    struct Im3Info *hinfo = (struct Im3Info*)IRAM1_ORIG;
+
+    /* Loads ONB in IRAM0, exception vector table is destroyed !!! */
+    int rc = im3_read(
+            NORBOOT_OFF + im3_nor_sz(hinfo), hinfo, (void*)IRAM0_ORIG);
+
+    if (rc != 0) {
+        /* Restore exception vector table */
+        memcpy((void*)IRAM0_ORIG, &_movestart, 4*(&start_loc-&_movestart));
+        commit_discard_idcache();
+        return rc;
+    }
+
+    /* Disable all external interrupts */
+    eint_init();
+
+    commit_discard_idcache();
+
+    /* Branch to start of IRAM */
+    asm volatile("mov pc, %0"::"r"(IRAM0_ORIG));
+    while(1);
+}
+
+/* Launch OF when kernel mode is running */
+static int kernel_launch_onb(void)
+{
+    disable_irq();
+    int rc = launch_onb(3); /* 54/4 = 13.5 MHz. */
+    enable_irq();
+    return rc;
+}
+
+static bool pmu_is_hibernated(void)
+{
+    /* OF sets GPIO3 to low when SDRAM is hibernated */
+    return !(pmu_rd(PCF5063X_REG_GPIO3CFG) & 7) &&
+        !(pmu_rd(PCF5063X_REG_OOCSHDWN) & PCF5063X_OOCSHDWN_COLDBOOT);
+}
+
+/*  The boot sequence is executed on power-on or reset. After power-up
+ *  the device could come from a state of hibernation, OF hibernates
+ *  the iPod after an inactive period of ~30 minutes (FW 1.1.2), on
+ *  this state the SDRAM is in self-refresh mode.
+ *
+ *  t0 = 0
+ *     S5L8702 BOOTROM loads an IM3 image located at NOR:
+ *     - IM3 header (first 0x800 bytes) is loaded at IRAM1_ORIG
+ *     - IM3 body (decrypted RB bootloader) is loaded at IRAM0_ORIG
+ *     The time needed to load the RB bootloader (~90 Kb) is estimated
+ *     on 200~250 ms. Once executed, RB booloader moves itself from
+ *     IRAM0_ORIG to IRAM1_ORIG+0x800, preserving current IM3 header
+ *     that contains the NOR offset where the ONB (original NOR boot),
+ *     is located (see dualboot.c for details).
+ *
+ *  t1 = ~250 ms.
+ *     If the PMU is hibernated, decrypted ONB (size 128Kb) is loaded
+ *       and executed, it takes ~120 ms. Then the ONB restores the
+ *       iPod to the state prior to hibernation.
+ *     If not, initialize system and RB kernel, wait for t2.
+ *
+ *  t2 = ~650 ms.
+ *     Check user button selection.
+ *     If OF, diagmode, or diskmode is selected then launch ONB.
+ *     If not, wait for LCD initialization.
+ *
+ *  t3 = ~700,~900 ms. (lcd_type_01,lcd_type_23)
+ *     LCD is initialized, baclight ON.
+ *     Wait for HDD spin-up.
+ *
+ *  t4 = ~2600,~2800 ms.
+ *     HDD is ready.
+ *     If hold switch is locked, then load and launch ONB.
+ *     If not, load rockbox.ipod file from HDD.
+ *
+ *  t5 = ~2800,~3000 ms.
+ *     rockbox.ipod is executed.
+ */
+void main(void)
+{
+    int fw = FW_ROCKBOX;
+    int rc = 0;
+    unsigned char *loadbuffer;
+    int (*kernel_entry)(void);
+
+    usec_timer_init();
+
+    /* Configure I2C0 */
+    i2c_preinit(0);
+
+    if (pmu_is_hibernated()) {
+        fw = FW_APPLE;
+        rc = launch_onb(1); /* 27/2 = 13.5 MHz. */
+    }
+
+    system_preinit();
+    memory_init();
+    /*
+     * XXX: BSS is initialized here, do not use .bss before this line
+     */
+    bss_init();
+
+    system_init();
+    kernel_init();
+    i2c_init();
+    power_init();
+
+    enable_irq();
+
+#ifdef HAVE_SERIAL
+    serial_setup();
+#endif
+
+    button_init();
+    if (rc == 0) {
+        /* User button selection timeout */
+        while (USEC_TIMER < 400000);
+        int btn = button_read_device();
+        /* This prevents HDD spin-up when the user enters DFU */
+        if (btn == (BUTTON_SELECT|BUTTON_MENU)) {
+            while (button_read_device() == (BUTTON_SELECT|BUTTON_MENU))
+                sleep(HZ/10);
+            sleep(HZ);
+            btn = button_read_device();
+        }
+        /* Enter OF, diagmode and diskmode using ONB */
+        if ((btn == BUTTON_MENU)
+                || (btn == (BUTTON_SELECT|BUTTON_LEFT))
+                || (btn == (BUTTON_SELECT|BUTTON_PLAY))) {
+            fw = FW_APPLE;
+            rc = kernel_launch_onb();
+        }
+    }
+
+    lcd_init();
+    lcd_set_foreground(LCD_WHITE);
+    lcd_set_background(LCD_BLACK);
+    lcd_clear_display();
+    font_init();
+    lcd_setfont(FONT_SYSFIXED);
+    lcd_update();
+    sleep(HZ/40);
+
+    verbose = true;
+
+    printf("Rockbox boot loader");
+    printf("Version: %s", rbversion);
+
+    backlight_init(); /* Turns on the backlight */
+
+    if (rc == 0) {
+        /* Wait until there is enought power to spin-up HDD */
+        battery_trap();
+
+        rc = storage_init();
+        if (rc != 0) {
+            printf("ATA error: %d", rc);
+            fatal_error(ERR_HDD);
+        }
+
+        filesystem_init();
+
+        /* We wait until HDD spins up to check for hold button */
+        if (button_hold()) {
+            fw = FW_APPLE;
+            printf("Executing OF...");
+            ata_sleepnow();
+            rc = kernel_launch_onb();
+        }
+    }
+
+    if (rc != 0) {
+        printf("Load OF error: %d", rc);
+        fatal_error(ERR_OF);
+    }
+
+#ifdef HAVE_BOOTLOADER_USB_MODE
+    /* Enter USB mode if SELECT+RIGHT are pressed */
+    if (button_read_device() == (BUTTON_SELECT|BUTTON_RIGHT))
+        usb_mode();
+#endif
+
+    rc = disk_mount_all();
+    if (rc <= 0) {
+        printf("No partition found");
+        fatal_error(ERR_RB);
+    }
+
+    printf("Loading Rockbox...");
+    loadbuffer = (unsigned char *)DRAM_ORIG;
+    rc = load_firmware(loadbuffer, BOOTFILE, MAX_LOADSIZE);
+
+    if (rc <= EFILE_EMPTY) {
+        printf("Error!");
+        printf("Can't load " BOOTFILE ": ");
+        printf(loader_strerror(rc));
+        fatal_error(ERR_RB);
+    }
+
+    printf("Rockbox loaded.");
+
+    /* If we get here, we have a new firmware image at 0x08000000, run it */
+    disable_irq();
+
+    kernel_entry = (void*) loadbuffer;
+    commit_discard_idcache();
+    rc = kernel_entry();
+
+    /* End stop - should not get here */
+    enable_irq();
+    printf("ERR: Failed to boot");
+    while(1);
+}
diff --git a/firmware/SOURCES b/firmware/SOURCES
index 88e40fa74f..09716be40e 100644
--- a/firmware/SOURCES
+++ b/firmware/SOURCES
@@ -1597,12 +1597,12 @@ target/arm/s5l8700/ipodnano2g/piezo-nano2g.c
 #ifdef IPOD_6G
 target/arm/ipod/button-clickwheel.c
 target/arm/s5l8702/ipod6g/storage_ata-ipod6g.c
-target/arm/s5l8702/ipod6g/cscodec-ipod6g.c
 target/arm/s5l8702/ipod6g/backlight-ipod6g.c
 target/arm/s5l8702/ipod6g/powermgmt-ipod6g.c
 target/arm/s5l8702/ipod6g/power-ipod6g.c
 target/arm/s5l8702/kernel-s5l8702.c
 target/arm/s5l8702/system-s5l8702.c
+target/arm/s5l8702/timer-s5l8702.c
 target/arm/s5l8702/gpio-s5l8702.c
 target/arm/s5l8702/pl080.c
 target/arm/s5l8702/dma-s5l8702.c
@@ -1615,24 +1615,26 @@ target/arm/s5l8702/postmortemstub.S
 #endif
 target/arm/s5l8702/ipod6g/pmu-ipod6g.c
 target/arm/s5l8702/ipod6g/rtc-ipod6g.c
+target/arm/s5l8702/ipod6g/adc-ipod6g.c
+#if !defined(BOOTLOADER) || defined(HAVE_BOOTLOADER_USB_MODE)
 target/arm/s5l8702/usb-s5l8702.c
+#endif
 #ifdef HAVE_SERIAL
 target/arm/uc870x.c
 target/arm/s5l8702/uart-s5l8702.c
 target/arm/s5l8702/ipod6g/serial-ipod6g.c
 #endif
 #ifndef BOOTLOADER
-target/arm/s5l8702/timer-s5l8702.c
 target/arm/s5l8702/debug-s5l8702.c
 target/arm/s5l8702/pcm-s5l8702.c
 target/arm/s5l8702/ipod6g/audio-ipod6g.c
-target/arm/s5l8702/ipod6g/adc-ipod6g.c
+target/arm/s5l8702/ipod6g/cscodec-ipod6g.c
 #else
 target/arm/s5l8702/spi-s5l8702.c
 target/arm/s5l8702/crypto-s5l8702.c
 target/arm/s5l8702/nor-s5l8702.c
-#endif
-#endif
+#endif /* BOOTLOADER */
+#endif /* IPOD_6G */
 
 #if CONFIG_CPU == RK27XX
 target/arm/rk27xx/audio-rk27xx.c
diff --git a/firmware/export/config.h b/firmware/export/config.h
index e7cfc698df..efad75f1b2 100644
--- a/firmware/export/config.h
+++ b/firmware/export/config.h
@@ -872,8 +872,12 @@ Lyre prototype 1 */
 #ifdef HAVE_BOOTLOADER_USB_MODE
 /* Priority in bootloader is wanted */
 #define HAVE_PRIORITY_SCHEDULING
+#if (CONFIG_CPU == S5L8702)
+#define USB_DRIVER_CLOSE
+#else
 #define USB_STATUS_BY_EVENT
 #define USB_DETECT_BY_REQUEST
+#endif
 #if defined(HAVE_USBSTACK) && CONFIG_USBOTG == USBOTG_ARC
 #define INCLUDE_TIMEOUT_API
 #define USB_DRIVER_CLOSE
diff --git a/firmware/export/config/ipod6g.h b/firmware/export/config/ipod6g.h
index 01934a0cc2..5494cf387a 100644
--- a/firmware/export/config/ipod6g.h
+++ b/firmware/export/config/ipod6g.h
@@ -248,14 +248,27 @@
 #define USB_VENDOR_ID 0x05AC
 #define USB_PRODUCT_ID 0x1261
 #define USB_DEVBSS_ATTR __attribute__((aligned(32)))
+#define HAVE_BOOTLOADER_USB_MODE
+#ifdef BOOTLOADER
+#define USBPOWER_BTN_IGNORE (~0)
+#endif
 
 #define USB_READ_BUFFER_SIZE (1024*24)
 
+/* Serial */
+#ifdef BOOTLOADER
+#if 0 /* Enable/disable LOGF_SERIAL for bootloader */
+#define HAVE_SERIAL
+#define ROCKBOX_HAS_LOGF
+#define LOGF_SERIAL
+#endif
+#else /* !BOOTLOADER */
 #define HAVE_SERIAL
 /* Disable iAP when LOGF_SERIAL is enabled to avoid conflicts */
 #ifndef LOGF_SERIAL
 #define IPOD_ACCESSORY_PROTOCOL
 #endif
+#endif
 
 /* Define this if you can switch on/off the accessory power supply */
 #define HAVE_ACCESSORY_SUPPLY
diff --git a/firmware/target/arm/s5l8702/boot.lds b/firmware/target/arm/s5l8702/boot.lds
index 2885f77eda..61f23b9c22 100644
--- a/firmware/target/arm/s5l8702/boot.lds
+++ b/firmware/target/arm/s5l8702/boot.lds
@@ -1,5 +1,6 @@
 #define ASM
 #include "config.h"
+#include "cpu.h"
 
 ENTRY(start)
 #ifdef ROCKBOX_LITTLE_ENDIAN
@@ -10,13 +11,11 @@ OUTPUT_FORMAT(elf32-bigarm)
 OUTPUT_ARCH(arm)
 STARTUP(target/arm/s5l8702/crt0.o)
 
+#define MAX_LOADSIZE 8M  /* reserved for loading Rockbox binary */
+
 #ifdef IPOD_NANO2G
 #define DRAMORIG 0x08000000 + ((MEMORYSIZE - 1) * 0x100000)
 #define DRAMSIZE 0x00100000
-#else
-#define DRAMORIG 0x08000000
-#define DRAMSIZE (DRAM_SIZE - TTB_SIZE)
-#endif
 
 #define IRAMORIG 0x22000000
 #define IRAMSIZE 256K
@@ -26,17 +25,47 @@ MEMORY
     DRAM  : ORIGIN = DRAMORIG, LENGTH = DRAMSIZE
     IRAM  : ORIGIN = IRAMORIG, LENGTH = IRAMSIZE
 }
-
 #define LOAD_AREA IRAM
+#define VECT_AREA IRAM
+#define BSS_AREA  DRAM
+
+#elif defined(IPOD_6G)
+MEMORY
+{
+    DRAM  : ORIGIN = DRAM_ORIG, LENGTH = DRAM_SIZE
+    IRAM  : ORIGIN = IRAM_ORIG, LENGTH = IRAM_SIZE
+
+    /* s5l8702 maps address 0 to ROM, IRAM or DRAM */
+    VECT_AREA : ORIGIN = 0, LENGTH = 1K
+
+    /* IRAM region where loaded IM3 body will be moved and executed,
+       preserving the loaded IM3 header (0x800 bytes) at IRAM1_ORIG */
+    MOVE_AREA : ORIGIN = IRAM1_ORIG + 0x800,
+                LENGTH = IRAM1_SIZE - 0x800
+
+    /* DRAM region for BSS */
+    BSS_AREA  : ORIGIN = DRAM_ORIG + MAX_LOADSIZE,
+                LENGTH = DRAM_SIZE - MAX_LOADSIZE - TTB_SIZE
+}
+#define LOAD_AREA   MOVE_AREA
+
+#else
+#error No target defined!
+#endif
+
 
 SECTIONS
 {
+  _dfuloadaddr = IRAM0_ORIG ;
+  _movestart = LOADADDR(.text) ;
+  _moveend = LOADADDR(.data) + SIZEOF(.data) ;
+
 #ifdef NEEDS_INTVECT_COPYING
   .intvect : {
     _intvectstart = . ;
     *(.intvect)
-    _intvectend = _newstart ;  
-  } >IRAM AT> LOAD_AREA
+    _intvectend = _newstart ;
+  } >VECT_AREA AT> LOAD_AREA
   _intvectcopy = LOADADDR(.intvect) ;
 #endif
 
@@ -61,10 +90,9 @@ SECTIONS
     *(.idata*)
     *(.data*)
     *(.ncdata*);
-    . = ALIGN(0x4);
+    . = ALIGN(0x20);  /* align move size */
     _dataend = . ;
-   } > IRAM AT> LOAD_AREA
-   _datacopy = LOADADDR(.data) ;
+  } > LOAD_AREA
 
   .stack (NOLOAD) :
   {
@@ -80,7 +108,7 @@ SECTIONS
      _fiqstackbegin = .;
      . += 0x400;
      _fiqstackend = .;
-  } > IRAM
+  } > LOAD_AREA
 
   .bss (NOLOAD) : {
      _edata = .;
@@ -88,7 +116,7 @@ SECTIONS
      *(.ibss*);
      *(.ncbss*);
      *(COMMON);
-    . = ALIGN(0x4);
+    . = ALIGN(0x20);  /* align bzero size */
      _end = .;
-  } > IRAM
+  } > BSS_AREA
 }
diff --git a/firmware/target/arm/s5l8702/crt0.S b/firmware/target/arm/s5l8702/crt0.S
index 3d1ee2bdfd..915c3f680b 100644
--- a/firmware/target/arm/s5l8702/crt0.S
+++ b/firmware/target/arm/s5l8702/crt0.S
@@ -46,17 +46,20 @@ newstart2:
     msr     cpsr_c, #0xd3 /* enter supervisor mode, disable IRQ/FIQ */
 
 #ifdef BOOTLOADER
-    /* Relocate ourself to IRAM - we have been loaded to DRAM */
-    mov     r0, #0x08000000   /* source (DRAM) */
-    mov     r1, #0x22000000   /* dest (IRAM) */
-    ldr     r2, =_dataend
+    /* Relocate ourself to IRAM1 - we have been loaded to IRAM0 */
+    ldr     r0, =_dfuloadaddr
+    ldr     r1, =_movestart
+    ldr     r2, =_moveend
 1:
-    cmp     r2, r1
-    ldrhi   r3, [r0], #4
-    strhi   r3, [r1], #4
-    bhi     1b
+    ldmia   r0!, {r3-r10}
+    stmia   r1!, {r3-r10}
+    cmp     r1, r2
+    blt     1b
 
     ldr     pc, =start_loc    /* jump to the relocated start_loc: */
+
+    .section .init.text,"ax",%progbits
+    .global  start_loc
 start_loc:
 #endif
 
@@ -66,11 +69,11 @@ start_loc:
     mcr     p15, 0, r0, c1, c0, 0   /* disable caches and protection unit */
 
 .cleancache:
-    mrc     p15, 0, r15,c7,c10,3
+    mrc     p15, 0, r15, c7, c10, 3 /* test and clean dcache */
     bne     .cleancache
     mov     r0, #0
-    mcr     p15, 0, r0,c7,c10,4
-    mcr     p15, 0, r0,c7,c5,0
+    mcr     p15, 0, r0, c7, c10, 4  /* drain write buffer */
+    mcr     p15, 0, r0, c7, c5, 0   /* invalidate icache */
 
     /* reset VIC controller */
     ldr     r1, =0x38e00000
@@ -86,7 +89,15 @@ start_loc:
     str     r0, [r1,#0x14]
     str     r0, [r2,#0x14]
 
-#if !defined(BOOTLOADER)
+#if defined(BOOTLOADER)
+    /* SPI speed is limited when icache is not active. Not worth
+     * activating dcache, it is almost useless on pre-init stage
+     * and the TLB needs 16Kb in detriment of the bootloader.
+     */
+    mrc     p15, 0, r0, c1, c0, 0
+    orr     r0, r0, #1<<12           /* enable icache */
+    mcr     p15, 0, r0, c1, c0, 0
+#else
     bl      memory_init
 
     /* Copy interrupt vectors to iram */
@@ -98,7 +109,6 @@ start_loc:
     ldrhi   r1, [r4], #4
     strhi   r1, [r2], #4
     bhi     1b
-#endif
 
     /* Initialise bss section to zero */
     ldr     r2, =_edata
@@ -109,7 +119,6 @@ start_loc:
     strhi   r4, [r2], #4
     bhi     1b
 
-#ifndef BOOTLOADER
     /* Copy icode and data to ram */
     ldr     r2, =_iramstart
     ldr     r3, =_iramend
@@ -159,3 +168,33 @@ start_loc:
     bhi     1b
 
     b       main
+
+#ifdef BOOTLOADER
+    /* Initialise bss section to zero */
+    .global bss_init
+    .type   bss_init, %function
+
+bss_init:
+    stmfd   sp!, {r4-r9,lr}
+
+    ldr     r0, =_edata
+    ldr     r1, =_end
+    mov     r2, #0
+    mov     r3, #0
+    mov     r4, #0
+    mov     r5, #0
+    mov     r6, #0
+    mov     r7, #0
+    mov     r8, #0
+    mov     r9, #0
+    b       2f
+    .align  5   /* cache line size */
+1:
+    stmia   r0!, {r2-r9}
+2:
+    cmp     r0, r1
+    blt     1b
+
+    ldmpc   regs=r4-r9
+    .ltorg
+#endif