memmove.c

T-kernel 的extension源代码

/*
 *----------------------------------------------------------------------
 *    T-Kernel / Standard Extension
 *
 *    Copyright (C) 2006 by Ken Sakamura. All rights reserved.
 *    T-Kernel / Standard Extension is distributed 
 *      under the T-License for T-Kernel / Standard Extension.
 *----------------------------------------------------------------------
 *
 *    Version:   1.00.00
 *    Released by T-Engine Forum(http://www.t-engine.org) at 2006/8/11.
 *
 *----------------------------------------------------------------------
 */

/*
 *	@(#)memmove.c (libg) 
 *
 *
 *	Speeding-up
 *	 Execute an access in CW unit wherever possible.
 *	 Execute a copy in multiple CW unit.
 */

#include <basic.h>
#include <string.h>

#define NXT_ALIGN_PNT		8	/* next align pointer set */
#define NXT_ALIGN_PNT_U		8U	/* next align pointer set */

/*
 * Copy unit CW
 */
typedef	UW			CW;

#define	BitWid		( sizeof(CW) * 8U )	/* Bit width of CW */

#define	ALIGN(p)	( (UINT)(p) & (UW)(sizeof(CW)-1) )
#define	ALIGN_L(p)	(CW*)((UINT)(p) & ~(UW)(sizeof(CW)-1))
#define	ALIGN_U(p)	(CW*)(((UINT)(p) + (sizeof(CW)-1)) & ~(UW)(sizeof(CW)-1))

#define	MSB_SEC(w, n)	( (_w = w), (w <<= n), _w >> (BitWid - n) )
#define	MSB_BYTE(w)	( (_w = w), (w <<= 8U), (UB)(_w >> (BitWid - 8U)) )
#define	LSB_SEC(w, n)	( (_w = w), (w >>= n), _w << (BitWid - n) )
#define	LSB_BYTE(w)	( (_w = w), (w >>= 8U), (UB)_w )

#if BIGENDIAN
#define	SEC(w, n)	MSB_SEC(w, n)
#define	BYTE(w)		MSB_BYTE(w)
#define	RSEC(w, n)	LSB_SEC(w, n)
#define	RBYTE(w)	LSB_BYTE(w)
#else
#define	SEC(w, n)	LSB_SEC(w, n)
#define	BYTE(w)		LSB_BYTE(w)
#define	RSEC(w, n)	MSB_SEC(w, n)
#define	RBYTE(w)	MSB_BYTE(w)
#endif

LOCAL void* _memmove_i( void *dest, const void *src, size_t len )
{
	register CW	_w, w1, w2, w3, w4;
	const CW	*sw = src;
	CW		*dw = dest;
	const CW	*se;
	size_t		align;

	if ( len < (sizeof(CW) * 4) ) {
		/* Execute a simple byte-copy when the bit width of CW is small-scaled */
		while ( len-- > 0U ) {
			*(UB*)dw = *(const UB*)sw;
			dw = (CW*)((UB*)dw + 1);
			sw = (CW*)((UB*)sw + 1);
		}
		return dest;
	}

	/* Advance "src" up to the CW alignment */
	while ( ALIGN((const UB*)sw) != 0U ) {
		*(UB*)dw = *(const UB*)sw;
		dw = (CW*)((UB*)dw + 1);
		sw = (CW*)((UB*)sw + 1);
	}
	w1 = *(sw++);

	/* Advance "dst" up to the CW alignment */
	align = 0;
	while ( ALIGN((UB*)dw) != 0U ) {
		*(UB*)dw = BYTE(w1);
		dw = (CW*)((UB*)dw + 1);
		align += NXT_ALIGN_PNT;
	}

	/* Copy in 3 CW units
 *	When the CW count(copy unit) is increased, the processing speed may grow late all the
 *      more on the models with small register count.  Adjust the range of CW count so that
 *      the register count can fully hold it. 
 */
	se = (VP)(ALIGN_L((UB*)src + len) - 2);
	if ( align == 0 ) {
		while ( sw < se ) {
			w2 = sw[0];
			w3 = sw[1];
			w4 = sw[2];
			sw += 3;
			dw[0] = w1;
			dw[1] = w2;
			dw[2] = w3;
			dw += 3;
			w1 = w4;
		}
	} else {
		while ( sw < se ) {
			w2 = sw[0];
			w3 = sw[1];
			w4 = sw[2];
			sw += 3;
			dw[0] = (w1 | SEC(w2, align));
			dw[1] = (w2 | SEC(w3, align));
			dw[2] = (w3 | SEC(w4, align));
			dw += 3;
			w1 = w4;
		}
	}

	/* Copy in 1 CW unit */
	se = (VP)ALIGN_L((UB*)src + len);
	if ( align == 0 ) {
		while ( sw < se ) {
			w2 = *(sw++);
			*(dw++) = w1;
			w1 = w2;
		}
		*(dw++) = w1;
		align = BitWid;
	} else {
		while ( sw < se ) {
			w2 = *(sw++);
			*(dw++) = w1 | SEC(w2, align);
			w1 = w2;
		}
	}

	/* Copy of the remaining half amount */
	for ( ; align < BitWid; align += NXT_ALIGN_PNT_U ) {
		*(UB*)dw = BYTE(w1);
		dw = (CW*)((UB*)dw + 1);
	}
	se = (VP)((UB*)src + len);
	while ( (const UB*)sw < (const UB*)se ) {
		*(UB*)dw = *(const UB*)sw;
		dw = (CW*)((UB*)dw + 1);
		sw = (CW*)((UB*)sw + 1);
	}

	return dest;
}

LOCAL void* _memmove_d( void *dest, const void *src, size_t len )
{
	register CW	_w, w1, w2, w3, w4;
	const CW	*sw = src;
	CW		*dw = dest;
	const CW	*se;
	size_t		align, n;

	sw = (CW*)((UB*)sw + len);
	dw = (CW*)((UB*)dw + len);

	if ( len < (sizeof(CW) * 4 )) {
		/* Execute a simple byte-copy when the bit width of CW is small-scaled */
		while ( len-- > 0U ) {
			dw = (CW*)((UB*)dw - 1);
			sw = (CW*)((UB*)sw - 1);
			*(UB*)dw = *(const UB*)sw;
		}
		return dest;
	}

	/* Advance "src" up to the CW alignment */
	n = ALIGN((const UB*)sw);
	while ( n-- > 0 ) {
		dw = (CW*)((UB*)dw - 1);
		sw = (CW*)((UB*)sw - 1);
		*(UB*)dw = *(const UB*)sw;
	}
	w1 = *(--sw);

	/* Advance "dst" up to the CW alignment */
	n = ALIGN((UB*)dw);
	align = 0;
	while ( n-- > 0 ) {
		dw = (CW*)((UB*)dw - 1);
		*(UB*)dw = RBYTE(w1);
		align += NXT_ALIGN_PNT;
	}

	/* Copy in 3 CW units
 *	When the CW count(copy unit) is increased, the processing speed may grow late all the
 *      more on the models with small register count.  Adjust the range of CW count so that
 *      the register count can fully hold it. 
 */
	se = ALIGN_U(src) + 2;
	if ( align == 0 ) {
		while ( sw > se ) {
			sw -= 3;
			w2 = sw[2];
			w3 = sw[1];
			w4 = sw[0];
			dw -= 3;
			dw[2] = w1;
			dw[1] = w2;
			dw[0] = w3;
			w1 = w4;
		}
	} else {
		while ( sw > se ) {
			sw -= 3;
			w2 = sw[2];
			w3 = sw[1];
			w4 = sw[0];
			dw -= 3;
			dw[2] = (w1 | RSEC(w2, align));
			dw[1] = (w2 | RSEC(w3, align));
			dw[0] = (w3 | RSEC(w4, align));
			w1 = w4;
		}
	}

	/* Copy in 1 CW unit */
	se = ALIGN_U(src);
	if ( align == 0 ) {
		while ( sw > se ) {
			w2 = *(--sw);
			*(--dw) = w1;
			w1 = w2;
		}
		*(--dw) = w1;
		align = BitWid;
	} else {
		while ( sw > se ) {
			w2 = *(--sw);
			*(--dw) = (w1 | RSEC(w2, align));
			w1 = w2;
		}
	}

	/* Copy of the remaining half amount */
	for ( ; align < BitWid; align += NXT_ALIGN_PNT_U ) {
		dw = (CW*)((UB*)dw - 1);
		*(UB*)dw = RBYTE(w1);
	}
	se = src;
	while ( (const UB*)sw > (const UB*)se ) {
		dw = (CW*)((UB*)dw - 1);
		sw = (CW*)((UB*)sw - 1);
		*(UB*)dw = *(const UB*)sw;
	}

	return dest;
}

EXPORT void* memmove( void *dest, const void *src, size_t len )
{
	return ( dest <= src )?
		_memmove_i(dest, src, len):
		_memmove_d(dest, src, len);
}