/****************************************************************************/
/*	Copyright (c) 2009, Bendor Research Pty. Ltd. All rights reserved.		*/
/*	This software is released under the GNU General Public License, version	*/
/*	3, with the exemptions that 1) the header files installed by this 	 	*/
/*	package can be included in closed-source code and 2) the libraries		*/
/*	built by this package can be statically linked into a closed-source 	*/
/*	binary. See the file LICENSE for the details.							*/
/*	This software is provided "AS IS", without any warranty or guarantee.	*/
/*	By using this software you acknowledge that neither Bendor Research nor	*/
/*	any of its personnel or affiliates can be held liable for any damages,	*/
/*	including, but not limited to, loss of business, loss of money, loss of */
/*	life, loss of reputation, arising out of this software being faulty or	*/
/*	not performing as advertised, even if Bendor Research was aware of such	*/
/*	faults in the software. You use this software at your own risk, any and */
/*	all liabilities that arise out of your using this software are strictly	*/
/*	yours.																	*/
/****************************************************************************/

#include <arm_func.h>

/*
*	Shift amounts depending on the address mismatch size (1-3)
*/

#define	N(x)	(8*(x))
#define	M(x)	(32-N(x))

/*
*	Elementary step for forward case shift
*/

	.macro	SHFS r1 r2 n
	mov		\r1,\r1,lsr #M(\n)
	orr		\r1,\r1,\r2,lsl #N(\n)
	.endm

/*
*	Forward case shift
*/

	.macro	SHF n
	orr		r3,r3,r4,lsl #N(\n)
	SHFS	r4,r5,\n
	SHFS	r5,r6,\n
	SHFS	r6,r7,\n
	SHFS	r7,r8,\n
	SHFS	r8,r9,\n
	SHFS	r9,r10,\n
	SHFS	r10,r11,\n
	SHFS	r11,r12,\n
	SHFS	r12,r14,\n
	.endm

/*
*	Elementary step for backward case shift
*/

	.macro	SHBS r1 r2 n
	orr		\r1,\r1,\r2,lsr #N(\n)
	mov		\r2,\r2,lsl #M(\n)
	.endm

/*
*	Backward case shift
*/

	.macro	SHB n
	SHBS	r14,r12,\n
	SHBS	r12,r11,\n
	SHBS	r11,r10,\n
	SHBS	r10,r9,\n
	SHBS	r9,r8,\n
	SHBS	r8,r7,\n
	SHBS	r7,r6,\n
	SHBS	r6,r5,\n
	SHBS	r5,r4,\n
	orr		r4,r4,r3,lsr #N(\n)
	.endm

/*
*	Backward move, misaligned case
*/

	.macro	BACK n
	subs	r2,r2,#44				// Do ve have at least 40 bytes?
	bmi		1f						// If not, skip it
	stmdb	sp!,{r4-r11,r14}		// Save work registers
	mov		r14,r12					// Residual R14-ben
0:
	ldmdb	r1!,{r3-r12}			// Fetch 40 bytes
	SHB		\n						// Shift them to place
	stmdb	r0!,{r4-r12,r14}		// Store the lot
	mov		r14,r3,lsl #M(\n)		// Move the new residual in place
	subs	r2,r2,#40				// Check if we have 40 more bytes
	bpl		0b						// If yes, keep going
	mov		r12,r14					// Move the residual back to R12
	ldmia	sp!,{r4-r11,r14}		// Restore work registers
1:
	adds	r2,r2,#36				// Restore byte count and check if we
	bmi		1f						// have at least 4 bytes, skip if not
0:
	ldr		r3,[r1,#-4]!			// Fetch a word
	orr		r12,r12,r3,lsr #N(\n)	// Concatenate with the residual
	str		r12,[r0,#-4]!			// Store the word
	mov		r12,r3,lsl #M(\n)		// Calculate the new residual
	subs	r2,r2,#4				// See if we have an other word
	bpl		0b						// and if yes, keep going
1:
	mov		r3,#\n					// Load the residual count
	add		r2,r2,#4				// Restore the byte count
	b		back_byte				// and go to common byte store
	.endm

/*
*	Forward move, misaligned case
*/

	.macro	FORW n
	subs	r2,r2,#44				// Do ve have at least 40 bytes?
	bmi		1f						// If not, skip it
	stmdb	sp!,{r4-r11,r14}		// Save work registers
0:
	ldmia	r1!,{r4-r12,r14}		// Fetch 40 bytes
	SHF		\n						// Shift them to place
	stmia	r0!,{r3-r12}			// Store the lot
	mov		r3,r14,lsr #M(\n)		// Move the new residual in place
	subs	r2,r2,#40				// Check if we have 40 more bytes
	bpl		0b						// If yes, keep going
	ldmia	sp!,{r4-r11,r14}		// Restore work registers
1:
	adds	r2,r2,#36				// Restore byte count and check if we
	bmi		1f						// have at least 4 bytes, skip if not
0:
	ldr		r12,[r1],#4				// Fetch a word
	orr		r3,r3,r12,lsl #N(\n)	// Concatenate with the residual
	str		r3,[r0],#4				// Store the word
	mov		r3,r12,lsr #M(\n)		// Calculate the new residual
	subs	r2,r2,#4				// See if we have an other word
	bpl		0b						// and if yes, keep going
1:
	mov		r12,#\n					// Load the residual count
	add		r2,r2,#4				// Restore the byte count
	b		forw_byte				// and go to common byte store
	.endm

/*
*	Function entry point
*	~~~~~~~~~~~~~~~~~~~~
*/

	afunc	memmove_fast
	cmp		r2,#0					// Copy length is nonzero?
	cmpne	r0,r1					// See if we have to copy forward or back
	bxeq	lr						// We don't have to copy at all
	bhi		back					// If back, then go there.

	// Forward copy. Do a byte by byte copy until the
	// destination becomes word aligned. If we have less
	// that 6 bytes to copy, we just do a byte copy anyway

	cmp		r2,#6					// Do we have at least 6 bytes?
	bhs		1f						// If yes, then start working
0:
	ldrb	r12,[r1],#1				// Otherwise fetch the next byte
	strb	r12,[r0],#1				// then store it and
	subs	r2,r2,#1				// decrement the byte count
	bne		0b						// and keep going if any more left
	bx		lr						// or return if not
1:
	tst		r0,#3					// Is the destination aligned?
	beq		1f						// If yes, we're in business
	subs	r2,r2,#1				// Otherwise, any more bytes left?
	bxmi	lr						// If not, just return
	ldrb	r12,[r1],#1				// If yes, fetch the next byte
	strb	r12,[r0],#1				// then store it and
	b		1b						// go to test the alignment again

	// Now the destination is aligned. Align the source as well.
	// The bytes we fetch are placed into R3 and not yet stored.

1:
	tst		r1,#3					// See if the source is also aligned
	beq		forw_off0				// Yes, start block copy, no offset
	ldrb	r3,[r1],#1				// No, load one byte
	tst		r1,#3					// See if the source is now aligned
	beq		forw_off1				// Yes, start copying, the offset is 1
	ldrb	r12,[r1],#1				// Not yet, load one byte
	orr		r3,r3,r12,lsl #8		// and shift in into the residual
	tst		r1,#3					// See if the source is now aligned
	beq		forw_off2				// Yes, start copying, the offset is 2
	ldrb	r12,[r1],#1				// Still not, load one byte and
	orr		r3,r3,r12,lsl #16		// put into the residual

	// Forward copy, both destination and source are aligned and
	// R3 contains the given number of residual bytes

forw_off3:
	FORW	3						// This copies with offset 3
forw_off2:
	FORW	2						// This copies with offset 2
forw_off1:
	FORW	1						// This copies with offset 1

	// Forward copy, both destination and source are aligned and
	// there is no residual. Just copy as fast as possible.

forw_off0:
	subs	r2,r2,#44				// Do we have at least 44 bytes to copy?
	bmi		1f						// If not, try words
	stmdb	sp!,{r4-r11,lr}			// Save the work registers
0:
	ldmia	r1!,{r3-r12,lr}			// Fetch 44 bytes
	stmia	r0!,{r3-r12,lr}			// Store them
	subs	r2,r2,#44				// Decrement the count
	bpl		0b						// Keep going if there is an other block
	ldmia	sp!,{r4-r11,lr}			// Restore the work registers
1:
	adds	r2,r2,#44-4				// Restore the byte count and check if we
	bmi		1f						// have a word left. If not, try byte move
0:
	ldr		r12,[r1],#4				// Load a word
	str		r12,[r0],#4				// Store it
	subs	r2,r2,#4				// Do we have at least a word left?
	bpl		0b						// If yes, keep going
1:
	mov		r12,#0					// In aligned case there's no residual

	// This is the end of all forward copies. When we get here R12 contains
	// how many residual bytes we have and R3 contains the residual bytes.
	// R0, R1 and R2 are still the destination, source and byte count.

forw_byte:
	add		r2,r2,#4				// Restore the length count
0:
	subs	r2,r2,#1				// Any more bytes left
	bxmi	lr						// If not, we're done
	subs	r12,r12,#1				// Do we have a residual?
	ldrmib	r3,[r1],#1				// If not, load a byte
	strb	r3,[r0],#1				// Store the residual or the loaded byte
	mov		r3,r3,lsr #8			// Adjust the residual
	b		0b						// Rinse and repeat :-)

	// Backward copy

back:
	add		r0,r0,r2				// Move destination to the end
	add		r1,r1,r2				// Move source to the end

	// Do a byte by byte copy until the destination get aligned

	cmp		r2,#6					// Do we have at least 6 bytes?
	bhs		1f						// If yes, then start working
0:
	ldrb	r12,[r1,#-1]!			// Otherwise fetch the next byte
	strb	r12,[r0,#-1]!			// then store it and
	subs	r2,r2,#1				// decrement the byte count
	bne		0b						// and keep going if any more left
	bx		lr						// or return if not
1:
	tst		r0,#3					// Is the destination aligned?
	beq		1f						// If yes, we're in business
	subs	r2,r2,#1				// Otherwise, any more bytes left?
	bxmi	lr						// If not, just return
	ldrb	r12,[r1,#-1]!			// If yes, fetch the next byte
	strb	r12,[r0,#-1]!			// then store it and
	b		1b						// go to test the alignment again

	// Destination is aligned. Align the source as well. The residual,
	// that is, the bytes we fetch to make the source aligned are built
	// in R12, shifting to the left.

1:
	tst		r1,#3					// See if the source is also aligned
	beq		back_off0				// Yes, start block copy, no offset
	ldrb	r12,[r1,#-1]!			// No, load one byte to the residual
	mov		r12,r12,lsl #24			// and put it in place
	tst		r1,#3					// See if the source is now aligned
	beq		back_off1				// Yes, start copying, the offset is 1
	ldrb	r3,[r1,#-1]!			// Not yet, load one byte
	orr		r12,r12,r3,lsl #16		// and shift in into the residual
	tst		r1,#3					// See if the source is now aligned
	beq		back_off2				// Yes, start copying, the offset is 2
	ldrb	r3,[r1,#-1]!			// Still not, load one byte
	orr		r12,r12,r3,lsl #8		// and shift in into the residual

	// Copying backward. Both the source and destination pointers are
	// now aligned and there are 1, 2 or 3 residual bytes in R12.

back_off3:
	BACK	3
back_off2:
	BACK	2
back_off1:
	BACK	1

	// We are copying backward and both the source and destination
	// are aligned and there's no residual.

back_off0:
	subs	r2,r2,#44				// Do we have at least 40 bytes to copy?
	bmi		1f						// If not, try words
	stmdb	sp!,{r4-r11,lr}			// Save the work registers
0:
	ldmdb	r1!,{r3-r12,lr}			// Fetch 44 bytes in one hit
	stmdb	r0!,{r3-r12,lr}			// Store them in one hit
	subs	r2,r2,#44				// Decrement the count
	bpl		0b						// Keep going if there is an other block
	ldmia	sp!,{r4-r11,lr}			// Restore the work registers
1:
	adds	r2,r2,#44-4				// Do we have at least a word left?
	bmi		1f						// If not, try byte move
0:
	ldr		r12,[r1,#-4]!			// Load a word
	str		r12,[r0,#-4]!			// Store it
	subs	r2,r2,#4				// Do we have at least a word left?
	bpl		0b						// If yes, keep going
1:
	mov		r12,#0					// In aligned case there's no residual
	mov		r3,#0

	// Backward copy, complete the transfer. When we get here R12 contains
	// the residual and R3 tells us how many residual bytes there are.

back_byte:
	add		r2,r2,#4				// Restore the length count
0:
	subs	r2,r2,#1				// Any more bytes left
	bxmi	lr						// If not, we're done
	mov		r12,r12,ror #24			// Adjust the residual
	subs	r3,r3,#1				// Do we have a residual?
	ldrmib	r12,[r1,#-1]!			// If not, load a byte
	strb	r12,[r0,#-1]!			// Store the residual or the loaded byte
	b		0b						// Rinse and repeat :-)

