dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity
px4-firmware/nuttx/libc/string/lib_vikmemcpy.c

349 lines
14 KiB
C
Raw Normal View History

OK.. I think the directory has been recovered and renamed git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5331 42af7a65-404d-4744-a932-0658087f49c3
2012-11-10 12:34:46 -04:00
/****************************************************************************
* File: libc/string/lib_vikmemcpy.c
*
* This is version of the optimized memcpy by Daniel Vik, adapted to the
* NuttX environment.
*
* Copyright (C) 1999-2010 Daniel Vik
*
* Adaptations include:
* - File name change
* - Use of types defined in stdint.h
* - Integration with the NuttX configuration system
* - Other cosmetic changes for consistency with NuttX coding standards
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any
* damages arising from the use of this software.
* Permission is granted to anyone to use this software for any
* purpose, including commercial applications, and to alter it and
* redistribute it freely, subject to the following restrictions:
*
* 1. The origin of this software must not be misrepresented; you
* must not claim that you wrote the original software. If you
* use this software in a product, an acknowledgment in the
* use this software in a product, an acknowledgment in the
* product documentation would be appreciated but is not
* required.
*
* 2. Altered source versions must be plainly marked as such, and
* must not be misrepresented as being the original software.
*
* 3. This notice may not be removed or altered from any source
* distribution.
*
* Description: Implementation of the standard library function memcpy.
* This implementation of memcpy() is ANSI-C89 compatible.
*
* The following configuration options can be set:
*
* CONFIG_ENDIAN_BIG
* Uses processor with big endian addressing. Default is little endian.
*
* CONFIG_MEMCPY_PRE_INC_PTRS
* Use pre increment of pointers. Default is post increment of pointers.
*
* CONFIG_MEMCPY_INDEXED_COPY
* Copying data using array indexing. Using this option, disables the
* CONFIG_MEMCPY_PRE_INC_PTRS option.
*
* CONFIG_MEMCPY_64BIT - Compiles memcpy for 64 bit architectures
*
****************************************************************************/
/****************************************************************************
* Configuration definitions.
****************************************************************************/
#define CONFIG_MEMCPY_INDEXED_COPY
/********************************************************************
* Included Files
*******************************************************************/
#include <nuttx/config.h>
#include <nuttx/compiler.h>
#include <stddef.h>
#include <stdint.h>
#include <string.h>
/********************************************************************
* Pre-processor Definitions
*******************************************************************/
/* Can't support CONFIG_MEMCPY_64BIT if the platform does not have 64-bit
* integer types.
*/
#ifndef CONFIG_HAVE_LONG_LONG
# undef CONFIG_MEMCPY_64BIT
#endif
/* Remove definitions when CONFIG_MEMCPY_INDEXED_COPY is defined */
#if defined (CONFIG_MEMCPY_INDEXED_COPY)
# if defined (CONFIG_MEMCPY_PRE_INC_PTRS)
# undef CONFIG_MEMCPY_PRE_INC_PTRS
# endif /* CONFIG_MEMCPY_PRE_INC_PTRS */
#endif /* CONFIG_MEMCPY_INDEXED_COPY */
/* Definitions for pre and post increment of pointers */
#if defined (CONFIG_MEMCPY_PRE_INC_PTRS)
# define START_VAL(x) (x)--
# define INC_VAL(x) *++(x)
# define CAST_TO_U8(p, o) ((uint8_t*)p + o + TYPE_WIDTH)
# define WHILE_DEST_BREAK (TYPE_WIDTH - 1)
# define PRE_LOOP_ADJUST - (TYPE_WIDTH - 1)
# define PRE_SWITCH_ADJUST + 1
#else /* CONFIG_MEMCPY_PRE_INC_PTRS */
# define START_VAL(x)
# define INC_VAL(x) *(x)++
# define CAST_TO_U8(p, o) ((uint8_t*)p + o)
# define WHILE_DEST_BREAK 0
# define PRE_LOOP_ADJUST
# define PRE_SWITCH_ADJUST
#endif /* CONFIG_MEMCPY_PRE_INC_PTRS */
/* Definitions for endian-ness */
#ifdef CONFIG_ENDIAN_BIG
# define SHL <<
# define SHR >>
#else /* CONFIG_ENDIAN_BIG */
# define SHL >>
# define SHR <<
#endif /* CONFIG_ENDIAN_BIG */
/********************************************************************
* Macros for copying words of different alignment.
* Uses incremening pointers.
*******************************************************************/
#define CP_INCR() \
{ \
INC_VAL(dstN) = INC_VAL(srcN); \
}
#define CP_INCR_SH(shl, shr) \
{ \
dstWord = srcWord SHL shl; \
srcWord = INC_VAL(srcN); \
dstWord |= srcWord SHR shr; \
INC_VAL(dstN) = dstWord; \
}
/********************************************************************
* Macros for copying words of different alignment.
* Uses array indexes.
*******************************************************************/
#define CP_INDEX(idx) \
{ \
dstN[idx] = srcN[idx]; \
}
#define CP_INDEX_SH(x, shl, shr) \
{ \
dstWord = srcWord SHL shl; \
srcWord = srcN[x]; \
dstWord |= srcWord SHR shr; \
dstN[x] = dstWord; \
}
/********************************************************************
* Macros for copying words of different alignment.
* Uses incremening pointers or array indexes depending on
* configuration.
*******************************************************************/
#if defined (CONFIG_MEMCPY_INDEXED_COPY)
# define CP(idx) CP_INDEX(idx)
# define CP_SH(idx, shl, shr) CP_INDEX_SH(idx, shl, shr)
# define INC_INDEX(p, o) ((p) += (o))
#else /* CONFIG_MEMCPY_INDEXED_COPY */
# define CP(idx) CP_INCR()
# define CP_SH(idx, shl, shr) CP_INCR_SH(shl, shr)
# define INC_INDEX(p, o)
#endif /* CONFIG_MEMCPY_INDEXED_COPY */
#define COPY_REMAINING(count) \
{ \
START_VAL(dst8); \
START_VAL(src8); \
\
switch (count) \
{ \
case 7: INC_VAL(dst8) = INC_VAL(src8); \
case 6: INC_VAL(dst8) = INC_VAL(src8); \
case 5: INC_VAL(dst8) = INC_VAL(src8); \
case 4: INC_VAL(dst8) = INC_VAL(src8); \
case 3: INC_VAL(dst8) = INC_VAL(src8); \
case 2: INC_VAL(dst8) = INC_VAL(src8); \
case 1: INC_VAL(dst8) = INC_VAL(src8); \
case 0: \
default: break; \
} \
}
#define COPY_NO_SHIFT() \
{ \
UIntN* dstN = (UIntN*)(dst8 PRE_LOOP_ADJUST); \
UIntN* srcN = (UIntN*)(src8 PRE_LOOP_ADJUST); \
size_t length = count / TYPE_WIDTH; \
\
while (length & 7) \
{ \
CP_INCR(); \
length--; \
} \
\
length /= 8; \
\
while (length--) \
{ \
CP(0); \
CP(1); \
CP(2); \
CP(3); \
CP(4); \
CP(5); \
CP(6); \
CP(7); \
\
INC_INDEX(dstN, 8); \
INC_INDEX(srcN, 8); \
} \
\
src8 = CAST_TO_U8(srcN, 0); \
dst8 = CAST_TO_U8(dstN, 0); \
\
COPY_REMAINING(count & (TYPE_WIDTH - 1)); \
\
return dest; \
}
#define COPY_SHIFT(shift) \
{ \
UIntN* dstN = (UIntN*)((((UIntN)dst8) PRE_LOOP_ADJUST) & \
~(TYPE_WIDTH - 1)); \
UIntN* srcN = (UIntN*)((((UIntN)src8) PRE_LOOP_ADJUST) & \
~(TYPE_WIDTH - 1)); \
size_t length = count / TYPE_WIDTH; \
UIntN srcWord = INC_VAL(srcN); \
UIntN dstWord; \
\
while (length & 7) \
{ \
CP_INCR_SH(8 * shift, 8 * (TYPE_WIDTH - shift)); \
length--; \
} \
\
length /= 8; \
\
while (length--) \
{ \
CP_SH(0, 8 * shift, 8 * (TYPE_WIDTH - shift)); \
CP_SH(1, 8 * shift, 8 * (TYPE_WIDTH - shift)); \
CP_SH(2, 8 * shift, 8 * (TYPE_WIDTH - shift)); \
CP_SH(3, 8 * shift, 8 * (TYPE_WIDTH - shift)); \
CP_SH(4, 8 * shift, 8 * (TYPE_WIDTH - shift)); \
CP_SH(5, 8 * shift, 8 * (TYPE_WIDTH - shift)); \
CP_SH(6, 8 * shift, 8 * (TYPE_WIDTH - shift)); \
CP_SH(7, 8 * shift, 8 * (TYPE_WIDTH - shift)); \
\
INC_INDEX(dstN, 8); \
INC_INDEX(srcN, 8); \
} \
\
src8 = CAST_TO_U8(srcN, (shift - TYPE_WIDTH)); \
dst8 = CAST_TO_U8(dstN, 0); \
\
COPY_REMAINING(count & (TYPE_WIDTH - 1)); \
\
return dest; \
}
/********************************************************************
* Type Definitions
*******************************************************************/
#ifdef CONFIG_MEMCPY_64BIT
typedef uint64_t UIntN;
# define TYPE_WIDTH 8L
#else
typedef uint32_t UIntN;
# define TYPE_WIDTH 4L
#endif
/********************************************************************
* Public Functions
*******************************************************************/
/********************************************************************
* Name: memcpy
*
* Description:
* Copies count bytes from src to dest. No overlap check is performed.
*
* Input Parameters:
* dest - pointer to destination buffer
* src - pointer to source buffer
* count - number of bytes to copy
*
* Returned Value:
* A pointer to destination buffer
*
*******************************************************************/
void *memcpy(void *dest, const void *src, size_t count)
{
uint8_t *dst8 = (uint8_t*)dest;
uint8_t *src8 = (uint8_t*)src;
if (count < 8)
{
COPY_REMAINING(count);
return dest;
}
START_VAL(dst8);
START_VAL(src8);
while (((UIntN)dst8 & (TYPE_WIDTH - 1)) != WHILE_DEST_BREAK)
{
INC_VAL(dst8) = INC_VAL(src8);
count--;
}
switch ((((UIntN)src8) PRE_SWITCH_ADJUST) & (TYPE_WIDTH - 1))
{
case 0: COPY_NO_SHIFT(); break;
case 1: COPY_SHIFT(1); break;
case 2: COPY_SHIFT(2); break;
case 3: COPY_SHIFT(3); break;
#if TYPE_WIDTH > 4
case 4: COPY_SHIFT(4); break;
case 5: COPY_SHIFT(5); break;
case 6: COPY_SHIFT(6); break;
case 7: COPY_SHIFT(7); break;
#endif
}
return dest;
}