original files of m241
This commit is contained in:
neelimap
@@ -0,0 +1,644 @@
|
||||
/******************************************************************************
|
||||
|
||||
© 1995-2003, 2004, 2005-2010 Freescale Semiconductor, Inc.
|
||||
All rights reserved.
|
||||
|
||||
This is proprietary source code of Freescale Semiconductor Inc.,
|
||||
and its use is subject to the NetComm Device Drivers EULA.
|
||||
The copyright notice above does not evidence any actual or intended
|
||||
publication of such source code.
|
||||
|
||||
**************************************************************************/
|
||||
|
||||
#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(((uint32_t)(p_Src8) & 3) && size) /* (pSrc mod 4) > 0 and size > 0 */
|
||||
{
|
||||
*p_Dst8++ = *p_Src8++;
|
||||
size--;
|
||||
}
|
||||
|
||||
/* align destination (possibly disaligning source)*/
|
||||
while(((uint32_t)(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)(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(((uint32_t)(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(((uint32_t)(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)(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(((uint32_t)(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(((uint32_t)(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)(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(((uint32_t)(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(((uint32_t)(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)(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(((uint32_t)(p_Src8) & 7) && size) /* (pSrc mod 8) > 0 and size > 0 */
|
||||
{
|
||||
*p_Dst8++ = *p_Src8++;
|
||||
size--;
|
||||
}
|
||||
|
||||
/* align destination (possibly disaligning source)*/
|
||||
while(((uint32_t)(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)(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(((uint32_t)(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(((uint32_t)(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(((uint32_t)(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)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
Block a user