arch/cris/arch-v10/boot/rescue/head.S - kernel/msm - Gitiles

 /*
  * Rescue code, made to reside at the beginning of the
  * flash-memory. when it starts, it checks a partition
  * table at the first sector after the rescue sector.
  * the partition table was generated by the product builder
  * script and contains offsets, lengths, types and checksums
  * for each partition that this code should check.
  *
  * If any of the checksums fail, we assume the flash is so
  * corrupt that we cant use it to boot into the ftp flash
  * loader, and instead we initialize the serial port to
  * receive a flash-loader and new flash image. we dont include
  * any flash code here, but just accept a certain amount of
  * bytes from the serial port and jump into it. the downloaded
  * code is put in the cache.
  *
  * The partitiontable is designed so that it is transparent to
  * code execution - it has a relative branch opcode in the
  * beginning that jumps over it. each entry contains extra
  * data so we can add stuff later.
  *
  * Partition table format:
  *
  *     Code transparency:
  *
  *     2 bytes    [opcode 'nop']
  *     2 bytes    [opcode 'di']
  *     4 bytes    [opcode 'ba <offset>', 8-bit or 16-bit version]
  *     2 bytes    [opcode 'nop', delay slot]
  *
  *     Table validation (at +10):
  *
  *     2 bytes    [magic/version word for partitiontable - 0xef, 0xbe]
  *     2 bytes    [length of all entries plus the end marker]
  *     4 bytes    [checksum for the partitiontable itself]
  *
  *     Entries, each with the following format, last has offset -1:
  *
  *        4 bytes    [offset in bytes, from start of flash]
  *        4 bytes    [length in bytes of partition]
  *        4 bytes    [checksum, simple longword sum]
  *        2 bytes    [partition type]
  *        2 bytes    [flags, only bit 0 used, ro/rw = 1/0]
  *        16 bytes   [reserved for future use]
  *
  *     End marker
  *
  *        4 bytes    [-1]
  *
  *	 10 bytes    [0, padding]
  *
  * Bit 0 in flags signifies RW or RO. The rescue code only bothers
  * to check the checksum for RO partitions, since the others will
  * change their data without updating the checksums. A 1 in bit 0
  * means RO, 0 means RW. That way, it is possible to set a partition
  * in RO mode initially, and later mark it as RW, since you can always
  * write 0's to the flash.
  *
  * During the wait for serial input, the status LED will flash so the
  * user knows something went wrong.
  *
  * Copyright (C) 1999-2007 Axis Communications AB
  */

 #ifdef CONFIG_ETRAX_AXISFLASHMAP

 #define ASSEMBLER_MACROS_ONLY
 #include <asm/arch/sv_addr_ag.h>

 	;; The partitiontable is looked for at the first sector after the boot
 	;; sector. Sector size is 65536 bytes in all flashes we use.

 #define PTABLE_START CONFIG_ETRAX_PTABLE_SECTOR
 #define PTABLE_MAGIC 0xbeef

 	;; The normal Etrax100 on-chip boot ROM does serial boot at 0x380000f0.
 	;; That is not where we put our downloaded serial boot-code.
 	;; The length is enough for downloading code that loads the rest
 	;; of itself (after having setup the DRAM etc).
 	;; It is the same length as the on-chip ROM loads, so the same
 	;; host loader can be used to load a rescued product as well as
 	;; one booted through the Etrax serial boot code.

 #define CODE_START 0x40000000
 #define CODE_LENGTH 784

 #ifdef CONFIG_ETRAX_RESCUE_SER0
 #define SERXOFF R_SERIAL0_XOFF
 #define SERBAUD R_SERIAL0_BAUD
 #define SERRECC R_SERIAL0_REC_CTRL
 #define SERRDAT R_SERIAL0_REC_DATA
 #define SERSTAT R_SERIAL0_STATUS
 #endif
 #ifdef CONFIG_ETRAX_RESCUE_SER1
 #define SERXOFF R_SERIAL1_XOFF
 #define SERBAUD R_SERIAL1_BAUD
 #define SERRECC R_SERIAL1_REC_CTRL
 #define SERRDAT R_SERIAL1_REC_DATA
 #define SERSTAT R_SERIAL1_STATUS
 #endif
 #ifdef CONFIG_ETRAX_RESCUE_SER2
 #define SERXOFF R_SERIAL2_XOFF
 #define SERBAUD R_SERIAL2_BAUD
 #define SERRECC R_SERIAL2_REC_CTRL
 #define SERRDAT R_SERIAL2_REC_DATA
 #define SERSTAT R_SERIAL2_STATUS
 #endif
 #ifdef CONFIG_ETRAX_RESCUE_SER3
 #define SERXOFF R_SERIAL3_XOFF
 #define SERBAUD R_SERIAL3_BAUD
 #define SERRECC R_SERIAL3_REC_CTRL
 #define SERRDAT R_SERIAL3_REC_DATA
 #define SERSTAT R_SERIAL3_STATUS
 #endif

 #define NOP_DI 0xf025050f
 #define RAM_INIT_MAGIC 0x56902387

 	.text

 	;; This is the entry point of the rescue code
 	;; 0x80000000 if loaded in flash (as it should be)
 	;; Since etrax actually starts at address 2 when booting from flash, we
 	;; put a nop (2 bytes) here first so we dont accidentally skip the di

 	nop
 	di

 	jump	in_cache	; enter cached area instead
 in_cache:


 	;; First put a jump test to give a possibility of upgrading the
 	;; rescue code without erasing/reflashing the sector.
 	;; We put a longword of -1 here and if it is not -1, we jump using
 	;; the value as jump target. Since we can always change 1's to 0's
 	;; without erasing the sector, it is possible to add new
 	;; code after this and altering the jumptarget in an upgrade.

 jtcd:	move.d	[jumptarget], $r0
 	cmp.d	0xffffffff, $r0
 	beq	no_newjump
 	nop

 	jump	[$r0]

 jumptarget:
 	.dword	0xffffffff	; can be overwritten later to insert new code

 no_newjump:
 #ifdef CONFIG_ETRAX_ETHERNET
 	;; Start MII clock to make sure it is running when tranceiver is reset
 	move.d 0x3, $r0    ; enable = on, phy = mii_clk
 	move.d $r0, [R_NETWORK_GEN_CONFIG]
 #endif

 	;; We need to setup the bus registers before we start using the DRAM
 #include "../../lib/dram_init.S"

 	;; we now should go through the checksum-table and check the listed
 	;; partitions for errors.

 	move.d	PTABLE_START, $r3
 	move.d	[$r3], $r0
 	cmp.d	NOP_DI, $r0	; make sure the nop/di is there...
 	bne	do_rescue
 	nop

 	;; skip the code transparency block (10 bytes).

 	addq	10, $r3

 	;; check for correct magic

 	move.w	[$r3+], $r0
 	cmp.w	PTABLE_MAGIC, $r0
 	bne	do_rescue	; didn't recognize - trig rescue
 	nop

 	;; check for correct ptable checksum

 	movu.w	[$r3+], $r2	; ptable length
 	move.d	$r2, $r8	; save for later, length of total ptable
 	addq	28, $r8		; account for the rest
 	move.d	[$r3+], $r4	; ptable checksum
 	move.d	$r3, $r1
 	jsr	checksum	; r1 source, r2 length, returns in r0

 	cmp.d	$r0, $r4
 	bne	do_rescue	; didn't match - trig rescue
 	nop

 	;; ptable is ok. validate each entry.

 	moveq	-1, $r7

 ploop:	move.d	[$r3+], $r1	; partition offset (from ptable start)
 	bne	notfirst	; check if it is the partition containing ptable
 	nop			; yes..
 	move.d	$r8, $r1	; for its checksum check, skip the ptable
 	move.d	[$r3+], $r2	; partition length
 	sub.d	$r8, $r2	; minus the ptable length
 	ba	bosse
 	nop
 notfirst:
 	cmp.d	-1, $r1		; the end of the ptable ?
 	beq	flash_ok	;   if so, the flash is validated
 	move.d	[$r3+], $r2	; partition length
 bosse:	move.d	[$r3+], $r5	; checksum
 	move.d	[$r3+], $r4	; type and flags
 	addq	16, $r3		; skip the reserved bytes
 	btstq	16, $r4		; check ro flag
 	bpl	ploop		;   rw partition, skip validation
 	nop
 	btstq	17, $r4		; check bootable flag
 	bpl	1f
 	nop
 	move.d	$r1, $r7	; remember boot partition offset
 1:
 	add.d	PTABLE_START, $r1

 	jsr	checksum	; checksum the partition

 	cmp.d	$r0, $r5
 	beq	ploop		; checksums matched, go to next entry
 	nop

 	;; otherwise fall through to the rescue code.

 do_rescue:
 	;; setup port PA and PB default initial directions and data
 	;; (so we can flash LEDs, and so that DTR and others are set)

 	move.b	CONFIG_ETRAX_DEF_R_PORT_PA_DIR, $r0
 	move.b	$r0, [R_PORT_PA_DIR]
 	move.b	CONFIG_ETRAX_DEF_R_PORT_PA_DATA, $r0
 	move.b	$r0, [R_PORT_PA_DATA]

 	move.b	CONFIG_ETRAX_DEF_R_PORT_PB_DIR, $r0
 	move.b	$r0, [R_PORT_PB_DIR]
 	move.b	CONFIG_ETRAX_DEF_R_PORT_PB_DATA, $r0
 	move.b	$r0, [R_PORT_PB_DATA]

 	;; setup the serial port at 115200 baud

 	moveq	0, $r0
 	move.d	$r0, [SERXOFF]

 	move.b	0x99, $r0
 	move.b	$r0, [SERBAUD]	; 115.2kbaud for both transmit and receive

 	move.b	0x40, $r0	; rec enable
 	move.b	$r0, [SERRECC]

 	moveq	0, $r1		; "timer" to clock out a LED red flash
 	move.d	CODE_START, $r3	; destination counter
 	movu.w	CODE_LENGTH, $r4; length

 wait_ser:
 	addq	1, $r1
 #ifndef CONFIG_ETRAX_NO_LEDS
 #ifdef CONFIG_ETRAX_PA_LEDS
 	move.b	CONFIG_ETRAX_DEF_R_PORT_PA_DATA, $r2
 #endif
 #ifdef CONFIG_ETRAX_PB_LEDS
 	move.b	CONFIG_ETRAX_DEF_R_PORT_PB_DATA, $r2
 #endif
 	move.d	(1 << CONFIG_ETRAX_LED1R) | (1 << CONFIG_ETRAX_LED2R), $r0
 	btstq	16, $r1
 	bpl	1f
 	nop
 	or.d	$r0, $r2	; set bit
 	ba	2f
 	nop
 1:	not	$r0		; clear bit
 	and.d	$r0, $r2
 2:
 #ifdef CONFIG_ETRAX_PA_LEDS
 	move.b	$r2, [R_PORT_PA_DATA]
 #endif
 #ifdef CONFIG_ETRAX_PB_LEDS
 	move.b	$r2, [R_PORT_PB_DATA]
 #endif
 #ifdef CONFIG_ETRAX_90000000_LEDS
 	move.b	$r2, [0x90000000]
 #endif
 #endif

 	;; check if we got something on the serial port

 	move.b	[SERSTAT], $r0
 	btstq	0, $r0		; data_avail
 	bpl	wait_ser
 	nop

 	;; got something - copy the byte and loop

 	move.b	[SERRDAT], $r0
 	move.b	$r0, [$r3+]

 	subq	1, $r4		; decrease length
 	bne	wait_ser
 	nop

 	;; jump into downloaded code

 	move.d	RAM_INIT_MAGIC, $r8	; Tell next product that DRAM is
 					; initialized
 	jump	CODE_START

 flash_ok:
 	;; check r7, which contains either -1 or the partition to boot from

 	cmp.d	-1, $r7
 	bne	1f
 	nop
 	move.d	PTABLE_START, $r7; otherwise use the ptable start
 1:
 	move.d	RAM_INIT_MAGIC, $r8	; Tell next product that DRAM is
 					; initialized
 	jump	$r7		; boot!


 	;; Helper subroutines

 	;; Will checksum by simple addition
 	;; r1 - source
 	;; r2 - length in bytes
 	;; result will be in r0
 checksum:
 	moveq	0, $r0
 	moveq   CONFIG_ETRAX_FLASH1_SIZE, $r6

 	;; If the first physical flash memory is exceeded wrap to the
 	;; second one
 	btstq	26, $r1		; Are we addressing first flash?
 	bpl	1f
 	nop
 	clear.d	$r6

 1:	test.d  $r6		; 0 = no wrapping
 	beq	2f
 	nop
 	lslq	20, $r6		; Convert MB to bytes
 	sub.d	$r1, $r6

 2:	addu.b	[$r1+], $r0
 	subq	1, $r6		; Flash memory left
 	beq	3f
 	subq	1, $r2		; Length left
 	bne	2b
 	nop
 	ret
 	nop

 3:	move.d	MEM_CSE1_START, $r1 ; wrap to second flash
 	ba	2b
 	nop

 #endif
	/*
	* Rescue code, made to reside at the beginning of the
	* flash-memory. when it starts, it checks a partition
	* table at the first sector after the rescue sector.
	* the partition table was generated by the product builder
	* script and contains offsets, lengths, types and checksums
	* for each partition that this code should check.
	*
	* If any of the checksums fail, we assume the flash is so
	* corrupt that we cant use it to boot into the ftp flash
	* loader, and instead we initialize the serial port to
	* receive a flash-loader and new flash image. we dont include
	* any flash code here, but just accept a certain amount of
	* bytes from the serial port and jump into it. the downloaded
	* code is put in the cache.
	*
	* The partitiontable is designed so that it is transparent to
	* code execution - it has a relative branch opcode in the
	* beginning that jumps over it. each entry contains extra
	* data so we can add stuff later.
	*
	* Partition table format:
	*
	* Code transparency:
	*
	* 2 bytes [opcode 'nop']
	* 2 bytes [opcode 'di']
	* 4 bytes [opcode 'ba <offset>', 8-bit or 16-bit version]
	* 2 bytes [opcode 'nop', delay slot]
	*
	* Table validation (at +10):
	*
	* 2 bytes [magic/version word for partitiontable - 0xef, 0xbe]
	* 2 bytes [length of all entries plus the end marker]
	* 4 bytes [checksum for the partitiontable itself]
	*
	* Entries, each with the following format, last has offset -1:
	*
	* 4 bytes [offset in bytes, from start of flash]
	* 4 bytes [length in bytes of partition]
	* 4 bytes [checksum, simple longword sum]
	* 2 bytes [partition type]
	* 2 bytes [flags, only bit 0 used, ro/rw = 1/0]
	* 16 bytes [reserved for future use]
	*
	* End marker
	*
	* 4 bytes [-1]
	*
	* 10 bytes [0, padding]
	*
	* Bit 0 in flags signifies RW or RO. The rescue code only bothers
	* to check the checksum for RO partitions, since the others will
	* change their data without updating the checksums. A 1 in bit 0
	* means RO, 0 means RW. That way, it is possible to set a partition
	* in RO mode initially, and later mark it as RW, since you can always
	* write 0's to the flash.
	*
	* During the wait for serial input, the status LED will flash so the
	* user knows something went wrong.
	*
	* Copyright (C) 1999-2007 Axis Communications AB
	*/

	#ifdef CONFIG_ETRAX_AXISFLASHMAP

	#define ASSEMBLER_MACROS_ONLY
	#include <asm/arch/sv_addr_ag.h>

	;; The partitiontable is looked for at the first sector after the boot
	;; sector. Sector size is 65536 bytes in all flashes we use.

	#define PTABLE_START CONFIG_ETRAX_PTABLE_SECTOR
	#define PTABLE_MAGIC 0xbeef

	;; The normal Etrax100 on-chip boot ROM does serial boot at 0x380000f0.
	;; That is not where we put our downloaded serial boot-code.
	;; The length is enough for downloading code that loads the rest
	;; of itself (after having setup the DRAM etc).
	;; It is the same length as the on-chip ROM loads, so the same
	;; host loader can be used to load a rescued product as well as
	;; one booted through the Etrax serial boot code.

	#define CODE_START 0x40000000
	#define CODE_LENGTH 784

	#ifdef CONFIG_ETRAX_RESCUE_SER0
	#define SERXOFF R_SERIAL0_XOFF
	#define SERBAUD R_SERIAL0_BAUD
	#define SERRECC R_SERIAL0_REC_CTRL
	#define SERRDAT R_SERIAL0_REC_DATA
	#define SERSTAT R_SERIAL0_STATUS
	#endif
	#ifdef CONFIG_ETRAX_RESCUE_SER1
	#define SERXOFF R_SERIAL1_XOFF
	#define SERBAUD R_SERIAL1_BAUD
	#define SERRECC R_SERIAL1_REC_CTRL
	#define SERRDAT R_SERIAL1_REC_DATA
	#define SERSTAT R_SERIAL1_STATUS
	#endif
	#ifdef CONFIG_ETRAX_RESCUE_SER2
	#define SERXOFF R_SERIAL2_XOFF
	#define SERBAUD R_SERIAL2_BAUD
	#define SERRECC R_SERIAL2_REC_CTRL
	#define SERRDAT R_SERIAL2_REC_DATA
	#define SERSTAT R_SERIAL2_STATUS
	#endif
	#ifdef CONFIG_ETRAX_RESCUE_SER3
	#define SERXOFF R_SERIAL3_XOFF
	#define SERBAUD R_SERIAL3_BAUD
	#define SERRECC R_SERIAL3_REC_CTRL
	#define SERRDAT R_SERIAL3_REC_DATA
	#define SERSTAT R_SERIAL3_STATUS
	#endif

	#define NOP_DI 0xf025050f
	#define RAM_INIT_MAGIC 0x56902387

	.text

	;; This is the entry point of the rescue code
	;; 0x80000000 if loaded in flash (as it should be)
	;; Since etrax actually starts at address 2 when booting from flash, we
	;; put a nop (2 bytes) here first so we dont accidentally skip the di

	nop
	di

	jump in_cache ; enter cached area instead
	in_cache:


	;; First put a jump test to give a possibility of upgrading the
	;; rescue code without erasing/reflashing the sector.
	;; We put a longword of -1 here and if it is not -1, we jump using
	;; the value as jump target. Since we can always change 1's to 0's
	;; without erasing the sector, it is possible to add new
	;; code after this and altering the jumptarget in an upgrade.

	jtcd: move.d [jumptarget], $r0
	cmp.d 0xffffffff, $r0
	beq no_newjump
	nop

	jump [$r0]

	jumptarget:
	.dword 0xffffffff ; can be overwritten later to insert new code

	no_newjump:
	#ifdef CONFIG_ETRAX_ETHERNET
	;; Start MII clock to make sure it is running when tranceiver is reset
	move.d 0x3, $r0 ; enable = on, phy = mii_clk
	move.d $r0, [R_NETWORK_GEN_CONFIG]
	#endif

	;; We need to setup the bus registers before we start using the DRAM
	#include "../../lib/dram_init.S"

	;; we now should go through the checksum-table and check the listed
	;; partitions for errors.

	move.d PTABLE_START, $r3
	move.d [$r3], $r0
	cmp.d NOP_DI, $r0 ; make sure the nop/di is there...
	bne do_rescue
	nop

	;; skip the code transparency block (10 bytes).

	addq 10, $r3

	;; check for correct magic

	move.w [$r3+], $r0
	cmp.w PTABLE_MAGIC, $r0
	bne do_rescue ; didn't recognize - trig rescue
	nop

	;; check for correct ptable checksum

	movu.w [$r3+], $r2 ; ptable length
	move.d $r2, $r8 ; save for later, length of total ptable
	addq 28, $r8 ; account for the rest
	move.d [$r3+], $r4 ; ptable checksum
	move.d $r3, $r1
	jsr checksum ; r1 source, r2 length, returns in r0

	cmp.d $r0, $r4
	bne do_rescue ; didn't match - trig rescue
	nop

	;; ptable is ok. validate each entry.

	moveq -1, $r7

	ploop: move.d [$r3+], $r1 ; partition offset (from ptable start)
	bne notfirst ; check if it is the partition containing ptable
	nop ; yes..
	move.d $r8, $r1 ; for its checksum check, skip the ptable
	move.d [$r3+], $r2 ; partition length
	sub.d $r8, $r2 ; minus the ptable length
	ba bosse
	nop
	notfirst:
	cmp.d -1, $r1 ; the end of the ptable ?
	beq flash_ok ; if so, the flash is validated
	move.d [$r3+], $r2 ; partition length
	bosse: move.d [$r3+], $r5 ; checksum
	move.d [$r3+], $r4 ; type and flags
	addq 16, $r3 ; skip the reserved bytes
	btstq 16, $r4 ; check ro flag
	bpl ploop ; rw partition, skip validation
	nop
	btstq 17, $r4 ; check bootable flag
	bpl 1f
	nop
	move.d $r1, $r7 ; remember boot partition offset
	1:
	add.d PTABLE_START, $r1

	jsr checksum ; checksum the partition

	cmp.d $r0, $r5
	beq ploop ; checksums matched, go to next entry
	nop

	;; otherwise fall through to the rescue code.

	do_rescue:
	;; setup port PA and PB default initial directions and data
	;; (so we can flash LEDs, and so that DTR and others are set)

	move.b CONFIG_ETRAX_DEF_R_PORT_PA_DIR, $r0
	move.b $r0, [R_PORT_PA_DIR]
	move.b CONFIG_ETRAX_DEF_R_PORT_PA_DATA, $r0
	move.b $r0, [R_PORT_PA_DATA]

	move.b CONFIG_ETRAX_DEF_R_PORT_PB_DIR, $r0
	move.b $r0, [R_PORT_PB_DIR]
	move.b CONFIG_ETRAX_DEF_R_PORT_PB_DATA, $r0
	move.b $r0, [R_PORT_PB_DATA]

	;; setup the serial port at 115200 baud

	moveq 0, $r0
	move.d $r0, [SERXOFF]

	move.b 0x99, $r0
	move.b $r0, [SERBAUD] ; 115.2kbaud for both transmit and receive

	move.b 0x40, $r0 ; rec enable
	move.b $r0, [SERRECC]

	moveq 0, $r1 ; "timer" to clock out a LED red flash
	move.d CODE_START, $r3 ; destination counter
	movu.w CODE_LENGTH, $r4; length

	wait_ser:
	addq 1, $r1
	#ifndef CONFIG_ETRAX_NO_LEDS
	#ifdef CONFIG_ETRAX_PA_LEDS
	move.b CONFIG_ETRAX_DEF_R_PORT_PA_DATA, $r2
	#endif
	#ifdef CONFIG_ETRAX_PB_LEDS
	move.b CONFIG_ETRAX_DEF_R_PORT_PB_DATA, $r2
	#endif
	move.d (1 << CONFIG_ETRAX_LED1R) \| (1 << CONFIG_ETRAX_LED2R), $r0
	btstq 16, $r1
	bpl 1f
	nop
	or.d $r0, $r2 ; set bit
	ba 2f
	nop
	1: not $r0 ; clear bit
	and.d $r0, $r2
	2:
	#ifdef CONFIG_ETRAX_PA_LEDS
	move.b $r2, [R_PORT_PA_DATA]
	#endif
	#ifdef CONFIG_ETRAX_PB_LEDS
	move.b $r2, [R_PORT_PB_DATA]
	#endif
	#ifdef CONFIG_ETRAX_90000000_LEDS
	move.b $r2, [0x90000000]
	#endif
	#endif

	;; check if we got something on the serial port

	move.b [SERSTAT], $r0
	btstq 0, $r0 ; data_avail
	bpl wait_ser
	nop

	;; got something - copy the byte and loop

	move.b [SERRDAT], $r0
	move.b $r0, [$r3+]

	subq 1, $r4 ; decrease length
	bne wait_ser
	nop

	;; jump into downloaded code

	move.d RAM_INIT_MAGIC, $r8 ; Tell next product that DRAM is
	; initialized
	jump CODE_START

	flash_ok:
	;; check r7, which contains either -1 or the partition to boot from

	cmp.d -1, $r7
	bne 1f
	nop
	move.d PTABLE_START, $r7; otherwise use the ptable start
	1:
	move.d RAM_INIT_MAGIC, $r8 ; Tell next product that DRAM is
	; initialized
	jump $r7 ; boot!


	;; Helper subroutines

	;; Will checksum by simple addition
	;; r1 - source
	;; r2 - length in bytes
	;; result will be in r0
	checksum:
	moveq 0, $r0
	moveq CONFIG_ETRAX_FLASH1_SIZE, $r6

	;; If the first physical flash memory is exceeded wrap to the
	;; second one
	btstq 26, $r1 ; Are we addressing first flash?
	bpl 1f
	nop
	clear.d $r6

	1: test.d $r6 ; 0 = no wrapping
	beq 2f
	nop
	lslq 20, $r6 ; Convert MB to bytes
	sub.d $r1, $r6

	2: addu.b [$r1+], $r0
	subq 1, $r6 ; Flash memory left
	beq 3f
	subq 1, $r2 ; Length left
	bne 2b
	nop
	ret
	nop

	3: move.d MEM_CSE1_START, $r1 ; wrap to second flash
	ba 2b
	nop

	#endif