cmhi
/
f-stack
mirror of https://github.com/F-Stack/f-stack.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
f-stack/freebsd/contrib/ncsw/etc/memcpy.c

666 lines
18 KiB
C
Raw Permalink Blame History

/* Copyright (c) 2008-2011 Freescale Semiconductor, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Freescale Semiconductor nor the
* names of its contributors may be used to endorse or promote products
* derived from this software without specific prior written permission.
*
*
* ALTERNATIVELY, this software may be distributed under the terms of the
* GNU General Public License ("GPL") as published by the Free Software
* Foundation, either version 2 of that License or (at your option) any
* later version.
*
* THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``AS IS'' AND ANY
* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL Freescale Semiconductor BE LIABLE FOR ANY
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "std_ext.h"
#include "xx_ext.h"
#include "memcpy_ext.h"
#ifdef CORE_8BIT_ACCESS_ERRATA
static void MY_MY_WRITE_UINT8(uint8_t *addr, uint8_t val)
{
uint32_t newAddr, newVal;
newAddr = (uint32_t)addr & ~0x3L;
switch ((uint32_t)addr%4)
{
case (0):
newVal = GET_UINT32(*(uint32_t*)newAddr);
newVal = (newVal & 0x00ffffff) | (((uint32_t)val)<<24);
WRITE_UINT32(*(uint32_t*)newAddr, newVal);
break;
case (1):
newVal = GET_UINT32(*(uint32_t*)newAddr);
newVal = (newVal & 0xff00ffff) | (((uint32_t)val)<<16);
WRITE_UINT32(*(uint32_t*)newAddr, newVal);
break;
case (2):
newVal = GET_UINT32(*(uint32_t*)newAddr);
newVal = (newVal & 0xffff00ff) | (((uint32_t)val)<<8);
WRITE_UINT32(*(uint32_t*)newAddr, newVal);
break;
case (3):
newVal = GET_UINT32(*(uint32_t*)newAddr);
newVal = (newVal & 0xffffff00) | val;
WRITE_UINT32(*(uint32_t*)newAddr, newVal);
break;
}
}
static uint8_t MY_MY_GET_UINT8(uint8_t *addr)
{
uint32_t newAddr, newVal=0;
newAddr = (uint32_t)addr & ~0x3L;
switch ((uint32_t)addr%4)
{
case (0):
newVal = GET_UINT32(*(uint32_t*)newAddr);
newVal = (newVal & 0xff000000)>>24;
break;
case (1):
newVal = GET_UINT32(*(uint32_t*)newAddr);
newVal = (newVal & 0x00ff0000)>>16;
break;
case (2):
newVal = GET_UINT32(*(uint32_t*)newAddr);
newVal = (newVal & 0x0000ff00)>>8;
break;
case (3):
newVal = GET_UINT32(*(uint32_t*)newAddr);
newVal = (newVal & 0x000000ff);
break;
}
return (uint8_t)newVal;
}
#define MY_WRITE_UINT8(addr,val) MY_MY_WRITE_UINT8(&addr,val)
#define MY_GET_UINT8(addr) MY_MY_GET_UINT8(&addr)
#else
#define MY_WRITE_UINT8 WRITE_UINT8
#define MY_GET_UINT8 GET_UINT8
#endif /* CORE_8BIT_ACCESS_ERRATA */
void * MemCpy32(void* pDst,void* pSrc, uint32_t size)
{
uint32_t leftAlign;
uint32_t rightAlign;
uint32_t lastWord;
uint32_t currWord;
uint32_t *p_Src32;
uint32_t *p_Dst32;
uint8_t *p_Src8;
uint8_t *p_Dst8;
p_Src8 = (uint8_t*)(pSrc);
p_Dst8 = (uint8_t*)(pDst);
/* first copy byte by byte till the source first alignment
* this step is necessary to ensure we do not even try to access
* data which is before the source buffer, hence it is not ours.
*/
while((PTR_TO_UINT(p_Src8) & 3) && size) /* (pSrc mod 4) > 0 and size > 0 */
{
*p_Dst8++ = *p_Src8++;
size--;
}
/* align destination (possibly disaligning source)*/
while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
{
*p_Dst8++ = *p_Src8++;
size--;
}
/* dest is aligned and source is not necessarily aligned */
leftAlign = (uint32_t)((PTR_TO_UINT(p_Src8) & 3) << 3); /* leftAlign = (pSrc mod 4)*8 */
rightAlign = 32 - leftAlign;
if (leftAlign == 0)
{
/* source is also aligned */
p_Src32 = (uint32_t*)(p_Src8);
p_Dst32 = (uint32_t*)(p_Dst8);
while (size >> 2) /* size >= 4 */
{
*p_Dst32++ = *p_Src32++;
size -= 4;
}
p_Src8 = (uint8_t*)(p_Src32);
p_Dst8 = (uint8_t*)(p_Dst32);
}
else
{
/* source is not aligned (destination is aligned)*/
p_Src32 = (uint32_t*)(p_Src8 - (leftAlign >> 3));
p_Dst32 = (uint32_t*)(p_Dst8);
lastWord = *p_Src32++;
while(size >> 3) /* size >= 8 */
{
currWord = *p_Src32;
*p_Dst32 = (lastWord << leftAlign) | (currWord >> rightAlign);
lastWord = currWord;
p_Src32++;
p_Dst32++;
size -= 4;
}
p_Dst8 = (uint8_t*)(p_Dst32);
p_Src8 = (uint8_t*)(p_Src32) - 4 + (leftAlign >> 3);
}
/* complete the left overs */
while (size--)
*p_Dst8++ = *p_Src8++;
return pDst;
}
void * IO2IOCpy32(void* pDst,void* pSrc, uint32_t size)
{
uint32_t leftAlign;
uint32_t rightAlign;
uint32_t lastWord;
uint32_t currWord;
uint32_t *p_Src32;
uint32_t *p_Dst32;
uint8_t *p_Src8;
uint8_t *p_Dst8;
p_Src8 = (uint8_t*)(pSrc);
p_Dst8 = (uint8_t*)(pDst);
/* first copy byte by byte till the source first alignment
* this step is necessary to ensure we do not even try to access
* data which is before the source buffer, hence it is not ours.
*/
while((PTR_TO_UINT(p_Src8) & 3) && size) /* (pSrc mod 4) > 0 and size > 0 */
{
MY_WRITE_UINT8(*p_Dst8, MY_GET_UINT8(*p_Src8));
p_Dst8++;p_Src8++;
size--;
}
/* align destination (possibly disaligning source)*/
while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
{
MY_WRITE_UINT8(*p_Dst8, MY_GET_UINT8(*p_Src8));
p_Dst8++;p_Src8++;
size--;
}
/* dest is aligned and source is not necessarily aligned */
leftAlign = (uint32_t)((PTR_TO_UINT(p_Src8) & 3) << 3); /* leftAlign = (pSrc mod 4)*8 */
rightAlign = 32 - leftAlign;
if (leftAlign == 0)
{
/* source is also aligned */
p_Src32 = (uint32_t*)(p_Src8);
p_Dst32 = (uint32_t*)(p_Dst8);
while (size >> 2) /* size >= 4 */
{
WRITE_UINT32(*p_Dst32, GET_UINT32(*p_Src32));
p_Dst32++;p_Src32++;
size -= 4;
}
p_Src8 = (uint8_t*)(p_Src32);
p_Dst8 = (uint8_t*)(p_Dst32);
}
else
{
/* source is not aligned (destination is aligned)*/
p_Src32 = (uint32_t*)(p_Src8 - (leftAlign >> 3));
p_Dst32 = (uint32_t*)(p_Dst8);
lastWord = GET_UINT32(*p_Src32);
p_Src32++;
while(size >> 3) /* size >= 8 */
{
currWord = GET_UINT32(*p_Src32);
WRITE_UINT32(*p_Dst32, (lastWord << leftAlign) | (currWord >> rightAlign));
lastWord = currWord;
p_Src32++;p_Dst32++;
size -= 4;
}
p_Dst8 = (uint8_t*)(p_Dst32);
p_Src8 = (uint8_t*)(p_Src32) - 4 + (leftAlign >> 3);
}
/* complete the left overs */
while (size--)
{
MY_WRITE_UINT8(*p_Dst8, MY_GET_UINT8(*p_Src8));
p_Dst8++;p_Src8++;
}
return pDst;
}
void * Mem2IOCpy32(void* pDst,void* pSrc, uint32_t size)
{
uint32_t leftAlign;
uint32_t rightAlign;
uint32_t lastWord;
uint32_t currWord;
uint32_t *p_Src32;
uint32_t *p_Dst32;
uint8_t *p_Src8;
uint8_t *p_Dst8;
p_Src8 = (uint8_t*)(pSrc);
p_Dst8 = (uint8_t*)(pDst);
/* first copy byte by byte till the source first alignment
* this step is necessary to ensure we do not even try to access
* data which is before the source buffer, hence it is not ours.
*/
while((PTR_TO_UINT(p_Src8) & 3) && size) /* (pSrc mod 4) > 0 and size > 0 */
{
MY_WRITE_UINT8(*p_Dst8, *p_Src8);
p_Dst8++;p_Src8++;
size--;
}
/* align destination (possibly disaligning source)*/
while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
{
MY_WRITE_UINT8(*p_Dst8, *p_Src8);
p_Dst8++;p_Src8++;
size--;
}
/* dest is aligned and source is not necessarily aligned */
leftAlign = (uint32_t)((PTR_TO_UINT(p_Src8) & 3) << 3); /* leftAlign = (pSrc mod 4)*8 */
rightAlign = 32 - leftAlign;
if (leftAlign == 0)
{
/* source is also aligned */
p_Src32 = (uint32_t*)(p_Src8);
p_Dst32 = (uint32_t*)(p_Dst8);
while (size >> 2) /* size >= 4 */
{
WRITE_UINT32(*p_Dst32, *p_Src32);
p_Dst32++;p_Src32++;
size -= 4;
}
p_Src8 = (uint8_t*)(p_Src32);
p_Dst8 = (uint8_t*)(p_Dst32);
}
else
{
/* source is not aligned (destination is aligned)*/
p_Src32 = (uint32_t*)(p_Src8 - (leftAlign >> 3));
p_Dst32 = (uint32_t*)(p_Dst8);
lastWord = *p_Src32++;
while(size >> 3) /* size >= 8 */
{
currWord = *p_Src32;
WRITE_UINT32(*p_Dst32, (lastWord << leftAlign) | (currWord >> rightAlign));
lastWord = currWord;
p_Src32++;p_Dst32++;
size -= 4;
}
p_Dst8 = (uint8_t*)(p_Dst32);
p_Src8 = (uint8_t*)(p_Src32) - 4 + (leftAlign >> 3);
}
/* complete the left overs */
while (size--)
{
MY_WRITE_UINT8(*p_Dst8, *p_Src8);
p_Dst8++;p_Src8++;
}
return pDst;
}
void * IO2MemCpy32(void* pDst,void* pSrc, uint32_t size)
{
uint32_t leftAlign;
uint32_t rightAlign;
uint32_t lastWord;
uint32_t currWord;
uint32_t *p_Src32;
uint32_t *p_Dst32;
uint8_t *p_Src8;
uint8_t *p_Dst8;
p_Src8 = (uint8_t*)(pSrc);
p_Dst8 = (uint8_t*)(pDst);
/* first copy byte by byte till the source first alignment
* this step is necessary to ensure we do not even try to access
* data which is before the source buffer, hence it is not ours.
*/
while((PTR_TO_UINT(p_Src8) & 3) && size) /* (pSrc mod 4) > 0 and size > 0 */
{
*p_Dst8 = MY_GET_UINT8(*p_Src8);
p_Dst8++;p_Src8++;
size--;
}
/* align destination (possibly disaligning source)*/
while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
{
*p_Dst8 = MY_GET_UINT8(*p_Src8);
p_Dst8++;p_Src8++;
size--;
}
/* dest is aligned and source is not necessarily aligned */
leftAlign = (uint32_t)((PTR_TO_UINT(p_Src8) & 3) << 3); /* leftAlign = (pSrc mod 4)*8 */
rightAlign = 32 - leftAlign;
if (leftAlign == 0)
{
/* source is also aligned */
p_Src32 = (uint32_t*)(p_Src8);
p_Dst32 = (uint32_t*)(p_Dst8);
while (size >> 2) /* size >= 4 */
{
*p_Dst32 = GET_UINT32(*p_Src32);
p_Dst32++;p_Src32++;
size -= 4;
}
p_Src8 = (uint8_t*)(p_Src32);
p_Dst8 = (uint8_t*)(p_Dst32);
}
else
{
/* source is not aligned (destination is aligned)*/
p_Src32 = (uint32_t*)(p_Src8 - (leftAlign >> 3));
p_Dst32 = (uint32_t*)(p_Dst8);
lastWord = GET_UINT32(*p_Src32);
p_Src32++;
while(size >> 3) /* size >= 8 */
{
currWord = GET_UINT32(*p_Src32);
*p_Dst32 = (lastWord << leftAlign) | (currWord >> rightAlign);
lastWord = currWord;
p_Src32++;p_Dst32++;
size -= 4;
}
p_Dst8 = (uint8_t*)(p_Dst32);
p_Src8 = (uint8_t*)(p_Src32) - 4 + (leftAlign >> 3);
}
/* complete the left overs */
while (size--)
{
*p_Dst8 = MY_GET_UINT8(*p_Src8);
p_Dst8++;p_Src8++;
}
return pDst;
}
void * MemCpy64(void* pDst,void* pSrc, uint32_t size)
{
uint32_t leftAlign;
uint32_t rightAlign;
uint64_t lastWord;
uint64_t currWord;
uint64_t *pSrc64;
uint64_t *pDst64;
uint8_t *p_Src8;
uint8_t *p_Dst8;
p_Src8 = (uint8_t*)(pSrc);
p_Dst8 = (uint8_t*)(pDst);
/* first copy byte by byte till the source first alignment
* this step is necessarily to ensure we do not even try to access
* data which is before the source buffer, hence it is not ours.
*/
while((PTR_TO_UINT(p_Src8) & 7) && size) /* (pSrc mod 8) > 0 and size > 0 */
{
*p_Dst8++ = *p_Src8++;
size--;
}
/* align destination (possibly disaligning source)*/
while((PTR_TO_UINT(p_Dst8) & 7) && size) /* (pDst mod 8) > 0 and size > 0 */
{
*p_Dst8++ = *p_Src8++;
size--;
}
/* dest is aligned and source is not necessarily aligned */
leftAlign = (uint32_t)((PTR_TO_UINT(p_Src8) & 7) << 3); /* leftAlign = (pSrc mod 8)*8 */
rightAlign = 64 - leftAlign;
if (leftAlign == 0)
{
/* source is also aligned */
pSrc64 = (uint64_t*)(p_Src8);
pDst64 = (uint64_t*)(p_Dst8);
while (size >> 3) /* size >= 8 */
{
*pDst64++ = *pSrc64++;
size -= 8;
}
p_Src8 = (uint8_t*)(pSrc64);
p_Dst8 = (uint8_t*)(pDst64);
}
else
{
/* source is not aligned (destination is aligned)*/
pSrc64 = (uint64_t*)(p_Src8 - (leftAlign >> 3));
pDst64 = (uint64_t*)(p_Dst8);
lastWord = *pSrc64++;
while(size >> 4) /* size >= 16 */
{
currWord = *pSrc64;
*pDst64 = (lastWord << leftAlign) | (currWord >> rightAlign);
lastWord = currWord;
pSrc64++;
pDst64++;
size -= 8;
}
p_Dst8 = (uint8_t*)(pDst64);
p_Src8 = (uint8_t*)(pSrc64) - 8 + (leftAlign >> 3);
}
/* complete the left overs */
while (size--)
*p_Dst8++ = *p_Src8++;
return pDst;
}
void * MemSet32(void* pDst, uint8_t val, uint32_t size)
{
uint32_t val32;
uint32_t *p_Dst32;
uint8_t *p_Dst8;
p_Dst8 = (uint8_t*)(pDst);
/* generate four 8-bit val's in 32-bit container */
val32 = (uint32_t) val;
val32 |= (val32 << 8);
val32 |= (val32 << 16);
/* align destination to 32 */
while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
{
*p_Dst8++ = val;
size--;
}
/* 32-bit chunks */
p_Dst32 = (uint32_t*)(p_Dst8);
while (size >> 2) /* size >= 4 */
{
*p_Dst32++ = val32;
size -= 4;
}
/* complete the leftovers */
p_Dst8 = (uint8_t*)(p_Dst32);
while (size--)
*p_Dst8++ = val;
return pDst;
}
void * IOMemSet32(void* pDst, uint8_t val, uint32_t size)
{
uint32_t val32;
uint32_t *p_Dst32;
uint8_t *p_Dst8;
p_Dst8 = (uint8_t*)(pDst);
/* generate four 8-bit val's in 32-bit container */
val32 = (uint32_t) val;
val32 |= (val32 << 8);
val32 |= (val32 << 16);
/* align destination to 32 */
while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
{
MY_WRITE_UINT8(*p_Dst8, val);
p_Dst8++;
size--;
}
/* 32-bit chunks */
p_Dst32 = (uint32_t*)(p_Dst8);
while (size >> 2) /* size >= 4 */
{
WRITE_UINT32(*p_Dst32, val32);
p_Dst32++;
size -= 4;
}
/* complete the leftovers */
p_Dst8 = (uint8_t*)(p_Dst32);
while (size--)
{
MY_WRITE_UINT8(*p_Dst8, val);
p_Dst8++;
}
return pDst;
}
void * MemSet64(void* pDst, uint8_t val, uint32_t size)
{
uint64_t val64;
uint64_t *pDst64;
uint8_t *p_Dst8;
p_Dst8 = (uint8_t*)(pDst);
/* generate four 8-bit val's in 32-bit container */
val64 = (uint64_t) val;
val64 |= (val64 << 8);
val64 |= (val64 << 16);
val64 |= (val64 << 24);
val64 |= (val64 << 32);
/* align destination to 64 */
while((PTR_TO_UINT(p_Dst8) & 7) && size) /* (pDst mod 8) > 0 and size > 0 */
{
*p_Dst8++ = val;
size--;
}
/* 64-bit chunks */
pDst64 = (uint64_t*)(p_Dst8);
while (size >> 4) /* size >= 8 */
{
*pDst64++ = val64;
size -= 8;
}
/* complete the leftovers */
p_Dst8 = (uint8_t*)(pDst64);
while (size--)
*p_Dst8++ = val;
return pDst;
}
void MemDisp(uint8_t *p, int size)
{
uint32_t space = (uint32_t)(PTR_TO_UINT(p) & 0x3);
uint8_t *p_Limit;
if (space)
{
p_Limit = (p - space + 4);
XX_Print("0x%08X: ", (p - space));
while (space--)
{
XX_Print("--");
}
while (size && (p < p_Limit))
{
XX_Print("%02x", *(uint8_t*)p);
size--;
p++;
}
XX_Print(" ");
p_Limit += 12;
while ((size > 3) && (p < p_Limit))
{
XX_Print("%08x ", *(uint32_t*)p);
size -= 4;
p += 4;
}
XX_Print("\r\n");
}
while (size > 15)
{
XX_Print("0x%08X: %08x %08x %08x %08x\r\n",
p, *(uint32_t *)p, *(uint32_t *)(p + 4),
*(uint32_t *)(p + 8), *(uint32_t *)(p + 12));
size -= 16;
p += 16;
}
if (size)
{
XX_Print("0x%08X: ", p);
while (size > 3)
{
XX_Print("%08x ", *(uint32_t *)p);
size -= 4;
p += 4;
}
while (size)
{
XX_Print("%02x", *(uint8_t *)p);
size--;
p++;
}
XX_Print("\r\n");
}
}
Reference in New Issue View Git Blame Copy Permalink