original files of m241
This commit is contained in:
neelimap
@@ -0,0 +1,97 @@
|
||||
/******************************************************************************
|
||||
|
||||
© 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.
|
||||
|
||||
**************************************************************************/
|
||||
/*
|
||||
|
||||
@File error.c
|
||||
|
||||
@Description General errors and events reporting utilities.
|
||||
*//***************************************************************************/
|
||||
|
||||
#if (defined(DEBUG_ERRORS) && (DEBUG_ERRORS > 0))
|
||||
|
||||
const char *dbgLevelStrings[] =
|
||||
{
|
||||
"CRITICAL"
|
||||
,"MAJOR"
|
||||
,"MINOR"
|
||||
,"WARNING"
|
||||
,"INFO"
|
||||
,"TRACE"
|
||||
};
|
||||
|
||||
const char *errTypeStrings[] =
|
||||
{
|
||||
"Invalid State" /* E_INVALID_STATE */
|
||||
,"Invalid Operation" /* E_INVALID_OPERATION */
|
||||
,"Unsupported Operation" /* E_NOT_SUPPORTED */
|
||||
,"No Device" /* E_NO_DEVICE */
|
||||
,"Invalid Handle" /* E_INVALID_HANDLE */
|
||||
,"Invalid ID" /* E_INVALID_ID */
|
||||
,"Unexpected NULL Pointer" /* E_NULL_POINTER */
|
||||
,"Invalid Value" /* E_INVALID_VALUE */
|
||||
,"Invalid Selection" /* E_INVALID_SELECTION */
|
||||
,"Invalid Communication Mode" /* E_INVALID_COMM_MODE */
|
||||
,"Invalid Byte Order" /* E_INVALID_BYTE_ORDER */
|
||||
,"Invalid Memory Type" /* E_INVALID_MEMORY_TYPE */
|
||||
,"Invalid Interrupt Queue" /* E_INVALID_INTR_QUEUE */
|
||||
,"Invalid Priority" /* E_INVALID_PRIORITY */
|
||||
,"Invalid Clock" /* E_INVALID_CLOCK */
|
||||
,"Invalid Rate" /* E_INVALID_RATE */
|
||||
,"Invalid Address" /* E_INVALID_ADDRESS */
|
||||
,"Invalid Bus" /* E_INVALID_BUS */
|
||||
,"Conflict In Bus Selection" /* E_BUS_CONFLICT */
|
||||
,"Conflict In Settings" /* E_CONFLICT */
|
||||
,"Incorrect Alignment" /* E_NOT_ALIGNED */
|
||||
,"Value Out Of Range" /* E_NOT_IN_RANGE */
|
||||
,"Invalid Frame" /* E_INVALID_FRAME */
|
||||
,"Frame Is Empty" /* E_EMPTY_FRAME */
|
||||
,"Buffer Is Empty" /* E_EMPTY_BUFFER */
|
||||
,"Memory Allocation Failed" /* E_NO_MEMORY */
|
||||
,"Resource Not Found" /* E_NOT_FOUND */
|
||||
,"Resource Is Unavailable" /* E_NOT_AVAILABLE */
|
||||
,"Resource Already Exists" /* E_ALREADY_EXISTS */
|
||||
,"Resource Is Full" /* E_FULL */
|
||||
,"Resource Is Empty" /* E_EMPTY */
|
||||
,"Resource Is Busy" /* E_BUSY */
|
||||
,"Resource Already Free" /* E_ALREADY_FREE */
|
||||
,"Read Access Failed" /* E_READ_FAILED */
|
||||
,"Write Access Failed" /* E_WRITE_FAILED */
|
||||
,"Send Operation Failed" /* E_SEND_FAILED */
|
||||
,"Receive Operation Failed" /* E_RECEIVE_FAILED */
|
||||
,"Operation Timed Out" /* E_TIMEOUT */
|
||||
};
|
||||
|
||||
|
||||
#if (defined(REPORT_EVENTS) && (REPORT_EVENTS > 0))
|
||||
|
||||
const char *eventStrings[] =
|
||||
{
|
||||
"Rx Discard" /* EV_RX_DISCARD */
|
||||
,"Rx Error" /* EV_RX_ERROR */
|
||||
,"Tx Error" /* EV_TX_ERROR */
|
||||
,"No Buffer Objects" /* EV_NO_BUFFERS */
|
||||
,"No MB-Frame Objects" /* EV_NO_MB_FRAMES */
|
||||
,"No SB-Frame Objects" /* EV_NO_SB_FRAMES */
|
||||
,"Tx Queue Is Full" /* EV_TX_QUEUE_FULL */
|
||||
,"Rx Queue Is Full" /* EV_RX_QUEUE_FULL */
|
||||
,"Interrupts Queue Is Full" /* EV_INTR_QUEUE_FULL */
|
||||
,"Data Buffer Is Unavailable" /* EV_NO_DATA_BUFFER */
|
||||
,"Objects Pool Is Empty" /* EV_OBJ_POOL_EMPTY */
|
||||
,"Illegal bus access" /* EV_BUS_ERROR */
|
||||
,"PTP Tx Timestamps Queue Is Full" /* EV_PTP_TXTS_QUEUE_FULL */
|
||||
,"PTP Rx Timestamps Queue Is Full" /* EV_PTP_RXTS_QUEUE_FULL */
|
||||
};
|
||||
|
||||
#endif /* (defined(REPORT_EVENTS) && (REPORT_EVENTS > 0)) */
|
||||
|
||||
#endif /* (defined(DEBUG_ERRORS) && (DEBUG_ERRORS > 0)) */
|
||||
|
||||
@@ -0,0 +1,739 @@
|
||||
/******************************************************************************
|
||||
|
||||
© 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 "error_ext.h"
|
||||
#include "part_ext.h"
|
||||
#include "std_ext.h"
|
||||
#include "string_ext.h"
|
||||
#include "mem_ext.h"
|
||||
#include "hash_ext.h"
|
||||
#include "mem.h"
|
||||
#include "xx_ext.h"
|
||||
|
||||
#define PAD_ALIGNMENT(align, x) (((x)%(align)) ? ((align)-((x)%(align))) : 0)
|
||||
|
||||
#define ALIGN_BLOCK(p_Block, prefixSize, alignment) \
|
||||
do { \
|
||||
p_Block += (prefixSize); \
|
||||
p_Block += PAD_ALIGNMENT((alignment), (uint32_t)(p_Block)); \
|
||||
} while (0)
|
||||
|
||||
#if defined(__GNUC__)
|
||||
#define GET_CALLER_ADDR \
|
||||
__asm__ ("mflr %0" : "=r" (callerAddr))
|
||||
#elif defined(__MWERKS__)
|
||||
/* NOTE: This implementation is only valid for CodeWarrior for PowerPC */
|
||||
#define GET_CALLER_ADDR \
|
||||
__asm__("add %0, 0, %0" : : "r" (callerAddr))
|
||||
#endif /* defined(__GNUC__) */
|
||||
|
||||
|
||||
/*****************************************************************************/
|
||||
static __inline__ void * MemGet(t_MemorySegment *p_Mem)
|
||||
{
|
||||
uint8_t *p_Block;
|
||||
|
||||
/* check if there is an available block */
|
||||
if (p_Mem->current == p_Mem->num)
|
||||
{
|
||||
p_Mem->getFailures++;
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/* get the block */
|
||||
p_Block = p_Mem->p_BlocksStack[p_Mem->current];
|
||||
#ifdef DEBUG
|
||||
p_Mem->p_BlocksStack[p_Mem->current] = NULL;
|
||||
#endif /* DEBUG */
|
||||
/* advance current index */
|
||||
p_Mem->current++;
|
||||
|
||||
return (void *)p_Block;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
static __inline__ t_Error MemPut(t_MemorySegment *p_Mem, void *p_Block)
|
||||
{
|
||||
/* check if blocks stack is full */
|
||||
if (p_Mem->current > 0)
|
||||
{
|
||||
/* decrease current index */
|
||||
p_Mem->current--;
|
||||
/* put the block */
|
||||
p_Mem->p_BlocksStack[p_Mem->current] = (uint8_t *)p_Block;
|
||||
return E_OK;
|
||||
}
|
||||
|
||||
RETURN_ERROR(MAJOR, E_FULL, NO_MSG);
|
||||
}
|
||||
|
||||
|
||||
#ifdef DEBUG_MEM_LEAKS
|
||||
|
||||
/*****************************************************************************/
|
||||
static t_Error InitMemDebugDatabase(t_MemorySegment *p_Mem)
|
||||
{
|
||||
p_Mem->p_MemDbg = (void *)XX_Malloc(sizeof(t_MemDbg) * p_Mem->num);
|
||||
if (!p_Mem->p_MemDbg)
|
||||
{
|
||||
RETURN_ERROR(MAJOR, E_NO_MEMORY, ("Memory debug object"));
|
||||
}
|
||||
|
||||
memset(p_Mem->p_MemDbg, ILLEGAL_BASE, sizeof(t_MemDbg) * p_Mem->num);
|
||||
|
||||
return E_OK;
|
||||
}
|
||||
|
||||
|
||||
/*****************************************************************************/
|
||||
static t_Error DebugMemGet(t_Handle h_Mem, void *p_Block, uint32_t ownerAddress)
|
||||
{
|
||||
t_MemorySegment *p_Mem = (t_MemorySegment *)h_Mem;
|
||||
t_MemDbg *p_MemDbg = (t_MemDbg *)p_Mem->p_MemDbg;
|
||||
uint32_t blockIndex;
|
||||
|
||||
ASSERT_COND(ownerAddress != ILLEGAL_BASE);
|
||||
|
||||
/* Find block num */
|
||||
if (p_Mem->consecutiveMem)
|
||||
{
|
||||
blockIndex =
|
||||
(((uint8_t *)p_Block - (p_Mem->p_Bases[0] + p_Mem->blockOffset)) / p_Mem->blockSize);
|
||||
}
|
||||
else
|
||||
{
|
||||
blockIndex = *(uint32_t *)((uint8_t *)p_Block - 4);
|
||||
}
|
||||
|
||||
ASSERT_COND(blockIndex < p_Mem->num);
|
||||
ASSERT_COND(p_MemDbg[blockIndex].ownerAddress == ILLEGAL_BASE);
|
||||
|
||||
p_MemDbg[blockIndex].ownerAddress = ownerAddress;
|
||||
|
||||
return E_OK;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
static t_Error DebugMemPut(t_Handle h_Mem, void *p_Block)
|
||||
{
|
||||
t_MemorySegment *p_Mem = (t_MemorySegment *)h_Mem;
|
||||
t_MemDbg *p_MemDbg = (t_MemDbg *)p_Mem->p_MemDbg;
|
||||
uint32_t blockIndex;
|
||||
uint8_t *p_Temp;
|
||||
|
||||
/* Find block num */
|
||||
if (p_Mem->consecutiveMem)
|
||||
{
|
||||
blockIndex =
|
||||
(((uint8_t *)p_Block - (p_Mem->p_Bases[0] + p_Mem->blockOffset)) / p_Mem->blockSize);
|
||||
|
||||
if (blockIndex >= p_Mem->num)
|
||||
{
|
||||
RETURN_ERROR(MAJOR, E_INVALID_ADDRESS,
|
||||
("Freed address (0x%08x) does not belong to this pool", p_Block));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
blockIndex = *(uint32_t *)((uint8_t *)p_Block - 4);
|
||||
|
||||
if (blockIndex >= p_Mem->num)
|
||||
{
|
||||
RETURN_ERROR(MAJOR, E_INVALID_ADDRESS,
|
||||
("Freed address (0x%08x) does not belong to this pool", p_Block));
|
||||
}
|
||||
|
||||
/* Verify that the block matches the corresponding base */
|
||||
p_Temp = p_Mem->p_Bases[blockIndex];
|
||||
|
||||
ALIGN_BLOCK(p_Temp, p_Mem->prefixSize, p_Mem->alignment);
|
||||
|
||||
if (p_Temp == p_Mem->p_Bases[blockIndex])
|
||||
p_Temp += p_Mem->alignment;
|
||||
|
||||
if (p_Temp != p_Block)
|
||||
{
|
||||
RETURN_ERROR(MAJOR, E_INVALID_ADDRESS,
|
||||
("Freed address (0x%08x) does not belong to this pool", p_Block));
|
||||
}
|
||||
}
|
||||
|
||||
if (p_MemDbg[blockIndex].ownerAddress == ILLEGAL_BASE)
|
||||
{
|
||||
RETURN_ERROR(MAJOR, E_ALREADY_FREE,
|
||||
("Attempt to free unallocated address (0x%08x)", p_Block));
|
||||
}
|
||||
|
||||
p_MemDbg[blockIndex].ownerAddress = (uint32_t)ILLEGAL_BASE;
|
||||
|
||||
return E_OK;
|
||||
}
|
||||
|
||||
#endif /* DEBUG_MEM_LEAKS */
|
||||
|
||||
|
||||
/*****************************************************************************/
|
||||
uint32_t MEM_ComputePartitionSize(uint32_t num,
|
||||
uint16_t dataSize,
|
||||
uint16_t prefixSize,
|
||||
uint16_t postfixSize,
|
||||
uint16_t alignment)
|
||||
{
|
||||
uint32_t blockSize = 0, pad1 = 0, pad2 = 0;
|
||||
|
||||
/* Make sure that the alignment is at least 4 */
|
||||
if (alignment < 4)
|
||||
{
|
||||
alignment = 4;
|
||||
}
|
||||
|
||||
pad1 = (uint32_t)PAD_ALIGNMENT(4, prefixSize);
|
||||
/* Block size not including 2nd padding */
|
||||
blockSize = pad1 + prefixSize + dataSize + postfixSize;
|
||||
pad2 = PAD_ALIGNMENT(alignment, blockSize);
|
||||
/* Block size including 2nd padding */
|
||||
blockSize += pad2;
|
||||
|
||||
return ((num * blockSize) + alignment);
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
t_Error MEM_Init(char name[],
|
||||
t_Handle *p_Handle,
|
||||
uint32_t num,
|
||||
uint16_t dataSize,
|
||||
uint16_t prefixSize,
|
||||
uint16_t postfixSize,
|
||||
uint16_t alignment)
|
||||
{
|
||||
uint8_t *p_Memory;
|
||||
uint32_t allocSize;
|
||||
t_Error errCode;
|
||||
|
||||
allocSize = MEM_ComputePartitionSize(num,
|
||||
dataSize,
|
||||
prefixSize,
|
||||
postfixSize,
|
||||
alignment);
|
||||
|
||||
p_Memory = (uint8_t *)XX_Malloc(allocSize);
|
||||
if (!p_Memory)
|
||||
{
|
||||
RETURN_ERROR(MAJOR, E_NO_MEMORY, ("Memory segment"));
|
||||
}
|
||||
|
||||
errCode = MEM_InitByAddress(name,
|
||||
p_Handle,
|
||||
num,
|
||||
dataSize,
|
||||
prefixSize,
|
||||
postfixSize,
|
||||
alignment,
|
||||
p_Memory);
|
||||
if (errCode != E_OK)
|
||||
{
|
||||
RETURN_ERROR(MAJOR, errCode, NO_MSG);
|
||||
}
|
||||
|
||||
((t_MemorySegment *)(*p_Handle))->allocOwner = e_MEM_ALLOC_OWNER_LOCAL;
|
||||
|
||||
return E_OK;
|
||||
}
|
||||
|
||||
|
||||
/*****************************************************************************/
|
||||
t_Error MEM_InitByAddress(char name[],
|
||||
t_Handle *p_Handle,
|
||||
uint32_t num,
|
||||
uint16_t dataSize,
|
||||
uint16_t prefixSize,
|
||||
uint16_t postfixSize,
|
||||
uint16_t alignment,
|
||||
uint8_t *p_Memory)
|
||||
{
|
||||
t_MemorySegment *p_Mem;
|
||||
uint32_t i, blockSize;
|
||||
uint16_t alignPad, endPad;
|
||||
uint8_t *p_Blocks;
|
||||
|
||||
/* prepare in case of error */
|
||||
*p_Handle = NULL;
|
||||
|
||||
if (!p_Memory)
|
||||
{
|
||||
RETURN_ERROR(MAJOR, E_NULL_POINTER, ("Memory blocks"));
|
||||
}
|
||||
|
||||
p_Blocks = p_Memory;
|
||||
|
||||
/* make sure that the alignment is at least 4 and power of 2 */
|
||||
if (alignment < 4)
|
||||
{
|
||||
alignment = 4;
|
||||
}
|
||||
else if (!POWER_OF_2(alignment))
|
||||
{
|
||||
RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("Alignment (should be power of 2)"));
|
||||
}
|
||||
|
||||
/* first allocate the segment descriptor */
|
||||
p_Mem = (t_MemorySegment *)XX_Malloc(sizeof(t_MemorySegment));
|
||||
if (!p_Mem)
|
||||
{
|
||||
RETURN_ERROR(MAJOR, E_NO_MEMORY, ("Memory segment structure"));
|
||||
}
|
||||
|
||||
/* allocate the blocks stack */
|
||||
p_Mem->p_BlocksStack = (uint8_t **)XX_Malloc(num * sizeof(uint8_t*));
|
||||
if (!p_Mem->p_BlocksStack)
|
||||
{
|
||||
XX_Free(p_Mem);
|
||||
RETURN_ERROR(MAJOR, E_NO_MEMORY, ("Memory segment block pointers stack"));
|
||||
}
|
||||
|
||||
/* allocate the blocks bases array */
|
||||
p_Mem->p_Bases = (uint8_t **)XX_Malloc(sizeof(uint8_t*));
|
||||
if (!p_Mem->p_Bases)
|
||||
{
|
||||
MEM_Free(p_Mem);
|
||||
RETURN_ERROR(MAJOR, E_NO_MEMORY, ("Memory segment base pointers array"));
|
||||
}
|
||||
memset(p_Mem->p_Bases, 0, sizeof(uint8_t*));
|
||||
|
||||
/* store info about this segment */
|
||||
p_Mem->num = num;
|
||||
p_Mem->current = 0;
|
||||
p_Mem->dataSize = dataSize;
|
||||
p_Mem->p_Bases[0] = p_Blocks;
|
||||
p_Mem->getFailures = 0;
|
||||
p_Mem->allocOwner = e_MEM_ALLOC_OWNER_EXTERNAL;
|
||||
p_Mem->consecutiveMem = TRUE;
|
||||
p_Mem->prefixSize = prefixSize;
|
||||
p_Mem->postfixSize = postfixSize;
|
||||
p_Mem->alignment = alignment;
|
||||
/* store name */
|
||||
strncpy(p_Mem->name, name, MEM_MAX_NAME_LENGTH);
|
||||
|
||||
p_Mem->h_Spinlock = XX_InitSpinlock();
|
||||
if (!p_Mem->h_Spinlock)
|
||||
{
|
||||
MEM_Free(p_Mem);
|
||||
RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Can't create spinlock!"));
|
||||
}
|
||||
|
||||
alignPad = (uint16_t)PAD_ALIGNMENT(4, prefixSize);
|
||||
/* Make sure the entire size is a multiple of alignment */
|
||||
endPad = (uint16_t)PAD_ALIGNMENT(alignment, (alignPad + prefixSize + dataSize + postfixSize));
|
||||
|
||||
/* The following manipulation places the data of block[0] in an aligned address,
|
||||
since block size is aligned the following block datas will all be aligned */
|
||||
ALIGN_BLOCK(p_Blocks, prefixSize, alignment);
|
||||
|
||||
blockSize = (uint32_t)(alignPad + prefixSize + dataSize + postfixSize + endPad);
|
||||
|
||||
/* initialize the blocks */
|
||||
for (i=0; i < num; i++)
|
||||
{
|
||||
p_Mem->p_BlocksStack[i] = p_Blocks;
|
||||
p_Blocks += blockSize;
|
||||
}
|
||||
|
||||
/* return handle to caller */
|
||||
*p_Handle = (t_Handle)p_Mem;
|
||||
|
||||
#ifdef DEBUG_MEM_LEAKS
|
||||
{
|
||||
t_Error errCode = InitMemDebugDatabase(p_Mem);
|
||||
|
||||
if (errCode != E_OK)
|
||||
RETURN_ERROR(MAJOR, errCode, NO_MSG);
|
||||
|
||||
p_Mem->blockOffset = (uint32_t)(p_Mem->p_BlocksStack[0] - p_Mem->p_Bases[0]);
|
||||
p_Mem->blockSize = blockSize;
|
||||
}
|
||||
#endif /* DEBUG_MEM_LEAKS */
|
||||
|
||||
return E_OK;
|
||||
}
|
||||
|
||||
|
||||
/*****************************************************************************/
|
||||
t_Error MEM_InitSmart(char name[],
|
||||
t_Handle *p_Handle,
|
||||
uint32_t num,
|
||||
uint16_t dataSize,
|
||||
uint16_t prefixSize,
|
||||
uint16_t postfixSize,
|
||||
uint16_t alignment,
|
||||
uint8_t memPartitionId,
|
||||
bool consecutiveMem)
|
||||
{
|
||||
t_MemorySegment *p_Mem;
|
||||
uint32_t i, blockSize;
|
||||
uint16_t alignPad, endPad;
|
||||
|
||||
/* prepare in case of error */
|
||||
*p_Handle = NULL;
|
||||
|
||||
/* make sure that size is always a multiple of 4 */
|
||||
if (dataSize & 3)
|
||||
{
|
||||
dataSize &= ~3;
|
||||
dataSize += 4;
|
||||
}
|
||||
|
||||
/* make sure that the alignment is at least 4 and power of 2 */
|
||||
if (alignment < 4)
|
||||
{
|
||||
alignment = 4;
|
||||
}
|
||||
else if (!POWER_OF_2(alignment))
|
||||
{
|
||||
RETURN_ERROR(MAJOR, E_INVALID_VALUE, ("Alignment (should be power of 2)"));
|
||||
}
|
||||
|
||||
/* first allocate the segment descriptor */
|
||||
p_Mem = (t_MemorySegment *)XX_Malloc(sizeof(t_MemorySegment));
|
||||
if (!p_Mem)
|
||||
{
|
||||
RETURN_ERROR(MAJOR, E_NO_MEMORY, ("Memory segment structure"));
|
||||
}
|
||||
|
||||
/* allocate the blocks stack */
|
||||
p_Mem->p_BlocksStack = (uint8_t **)XX_Malloc(num * sizeof(uint8_t*));
|
||||
if (!p_Mem->p_BlocksStack)
|
||||
{
|
||||
MEM_Free(p_Mem);
|
||||
RETURN_ERROR(MAJOR, E_NO_MEMORY, ("Memory segment block pointers stack"));
|
||||
}
|
||||
|
||||
/* allocate the blocks bases array */
|
||||
p_Mem->p_Bases = (uint8_t **)XX_Malloc((consecutiveMem ? 1 : num) * sizeof(uint8_t*));
|
||||
if (!p_Mem->p_Bases)
|
||||
{
|
||||
MEM_Free(p_Mem);
|
||||
RETURN_ERROR(MAJOR, E_NO_MEMORY, ("Memory segment base pointers array"));
|
||||
}
|
||||
memset(p_Mem->p_Bases, 0, (consecutiveMem ? 1 : num) * sizeof(uint8_t*));
|
||||
|
||||
/* store info about this segment */
|
||||
p_Mem->num = num;
|
||||
p_Mem->current = 0;
|
||||
p_Mem->dataSize = dataSize;
|
||||
p_Mem->getFailures = 0;
|
||||
p_Mem->allocOwner = e_MEM_ALLOC_OWNER_LOCAL_SMART;
|
||||
p_Mem->consecutiveMem = consecutiveMem;
|
||||
p_Mem->prefixSize = prefixSize;
|
||||
p_Mem->postfixSize = postfixSize;
|
||||
p_Mem->alignment = alignment;
|
||||
|
||||
p_Mem->h_Spinlock = XX_InitSpinlock();
|
||||
if (!p_Mem->h_Spinlock)
|
||||
{
|
||||
MEM_Free(p_Mem);
|
||||
RETURN_ERROR(MAJOR, E_INVALID_STATE, ("Can't create spinlock!"));
|
||||
}
|
||||
|
||||
alignPad = (uint16_t)PAD_ALIGNMENT(4, prefixSize);
|
||||
/* Make sure the entire size is a multiple of alignment */
|
||||
endPad = (uint16_t)PAD_ALIGNMENT(alignment, alignPad + prefixSize + dataSize + postfixSize);
|
||||
|
||||
/* Calculate blockSize */
|
||||
blockSize = (uint32_t)(alignPad + prefixSize + dataSize + postfixSize + endPad);
|
||||
|
||||
/* Now allocate the blocks */
|
||||
if (p_Mem->consecutiveMem)
|
||||
{
|
||||
/* |alignment - 1| bytes at most will be discarded in the beginning of the
|
||||
received segment for alignment reasons, therefore the allocation is of:
|
||||
(alignment + (num * block size)). */
|
||||
uint8_t *p_Blocks = (uint8_t *)
|
||||
XX_MallocSmart((uint32_t)((num * blockSize) + alignment), memPartitionId, 1);
|
||||
if (!p_Blocks)
|
||||
{
|
||||
MEM_Free(p_Mem);
|
||||
RETURN_ERROR(MAJOR, E_NO_MEMORY, ("Memory segment blocks"));
|
||||
}
|
||||
|
||||
/* Store the memory segment address */
|
||||
p_Mem->p_Bases[0] = p_Blocks;
|
||||
|
||||
/* The following manipulation places the data of block[0] in an aligned address,
|
||||
since block size is aligned the following block datas will all be aligned.*/
|
||||
ALIGN_BLOCK(p_Blocks, prefixSize, alignment);
|
||||
|
||||
/* initialize the blocks */
|
||||
for (i = 0; i < num; i++)
|
||||
{
|
||||
p_Mem->p_BlocksStack[i] = p_Blocks;
|
||||
p_Blocks += blockSize;
|
||||
}
|
||||
|
||||
#ifdef DEBUG_MEM_LEAKS
|
||||
p_Mem->blockOffset = (uint32_t)(p_Mem->p_BlocksStack[0] - p_Mem->p_Bases[0]);
|
||||
p_Mem->blockSize = blockSize;
|
||||
#endif /* DEBUG_MEM_LEAKS */
|
||||
}
|
||||
else
|
||||
{
|
||||
/* |alignment - 1| bytes at most will be discarded in the beginning of the
|
||||
received segment for alignment reasons, therefore the allocation is of:
|
||||
(alignment + block size). */
|
||||
for (i = 0; i < num; i++)
|
||||
{
|
||||
uint8_t *p_Block = (uint8_t *)
|
||||
XX_MallocSmart((uint32_t)(blockSize + alignment), memPartitionId, 1);
|
||||
if (!p_Block)
|
||||
{
|
||||
MEM_Free(p_Mem);
|
||||
RETURN_ERROR(MAJOR, E_NO_MEMORY, ("Memory segment blocks"));
|
||||
}
|
||||
|
||||
/* Store the memory segment address */
|
||||
p_Mem->p_Bases[i] = p_Block;
|
||||
|
||||
/* The following places the data of each block in an aligned address */
|
||||
ALIGN_BLOCK(p_Block, prefixSize, alignment);
|
||||
|
||||
#ifdef DEBUG_MEM_LEAKS
|
||||
/* Need 4 bytes before the meaningful bytes to store the block index.
|
||||
We know we have them because alignment is at least 4 bytes. */
|
||||
if (p_Block == p_Mem->p_Bases[i])
|
||||
p_Block += alignment;
|
||||
|
||||
*(uint32_t *)(p_Block - 4) = i;
|
||||
#endif /* DEBUG_MEM_LEAKS */
|
||||
|
||||
p_Mem->p_BlocksStack[i] = p_Block;
|
||||
}
|
||||
}
|
||||
|
||||
/* store name */
|
||||
strncpy(p_Mem->name, name, MEM_MAX_NAME_LENGTH);
|
||||
|
||||
/* return handle to caller */
|
||||
*p_Handle = (t_Handle)p_Mem;
|
||||
|
||||
#ifdef DEBUG_MEM_LEAKS
|
||||
{
|
||||
t_Error errCode = InitMemDebugDatabase(p_Mem);
|
||||
|
||||
if (errCode != E_OK)
|
||||
RETURN_ERROR(MAJOR, errCode, NO_MSG);
|
||||
}
|
||||
#endif /* DEBUG_MEM_LEAKS */
|
||||
|
||||
return E_OK;
|
||||
}
|
||||
|
||||
|
||||
/*****************************************************************************/
|
||||
void MEM_Free(t_Handle h_Mem)
|
||||
{
|
||||
t_MemorySegment *p_Mem = (t_MemorySegment*)h_Mem;
|
||||
uint32_t num, i;
|
||||
|
||||
/* Check MEM leaks */
|
||||
MEM_CheckLeaks(h_Mem);
|
||||
|
||||
if (p_Mem)
|
||||
{
|
||||
num = p_Mem->consecutiveMem ? 1 : p_Mem->num;
|
||||
|
||||
if (p_Mem->allocOwner == e_MEM_ALLOC_OWNER_LOCAL_SMART)
|
||||
{
|
||||
for (i=0; i < num; i++)
|
||||
{
|
||||
if (p_Mem->p_Bases[i])
|
||||
{
|
||||
XX_FreeSmart(p_Mem->p_Bases[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (p_Mem->allocOwner == e_MEM_ALLOC_OWNER_LOCAL)
|
||||
{
|
||||
for (i=0; i < num; i++)
|
||||
{
|
||||
if (p_Mem->p_Bases[i])
|
||||
{
|
||||
XX_Free(p_Mem->p_Bases[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (p_Mem->h_Spinlock)
|
||||
XX_FreeSpinlock(p_Mem->h_Spinlock);
|
||||
|
||||
if (p_Mem->p_Bases)
|
||||
XX_Free(p_Mem->p_Bases);
|
||||
|
||||
if (p_Mem->p_BlocksStack)
|
||||
XX_Free(p_Mem->p_BlocksStack);
|
||||
|
||||
#ifdef DEBUG_MEM_LEAKS
|
||||
if (p_Mem->p_MemDbg)
|
||||
XX_Free(p_Mem->p_MemDbg);
|
||||
#endif /* DEBUG_MEM_LEAKS */
|
||||
|
||||
XX_Free(p_Mem);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*****************************************************************************/
|
||||
void * MEM_Get(t_Handle h_Mem)
|
||||
{
|
||||
t_MemorySegment *p_Mem = (t_MemorySegment *)h_Mem;
|
||||
uint8_t *p_Block;
|
||||
uint32_t intFlags;
|
||||
#ifdef DEBUG_MEM_LEAKS
|
||||
uint32_t callerAddr = 0;
|
||||
|
||||
GET_CALLER_ADDR;
|
||||
#endif /* DEBUG_MEM_LEAKS */
|
||||
|
||||
ASSERT_COND(h_Mem);
|
||||
|
||||
intFlags = XX_LockIntrSpinlock(p_Mem->h_Spinlock);
|
||||
/* check if there is an available block */
|
||||
if ((p_Block = (uint8_t *)MemGet(p_Mem)) == NULL)
|
||||
{
|
||||
XX_UnlockIntrSpinlock(p_Mem->h_Spinlock, intFlags);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
#ifdef DEBUG_MEM_LEAKS
|
||||
DebugMemGet(p_Mem, p_Block, callerAddr);
|
||||
#endif /* DEBUG_MEM_LEAKS */
|
||||
XX_UnlockIntrSpinlock(p_Mem->h_Spinlock, intFlags);
|
||||
|
||||
return (void *)p_Block;
|
||||
}
|
||||
|
||||
|
||||
/*****************************************************************************/
|
||||
uint16_t MEM_GetN(t_Handle h_Mem, uint32_t num, void *array[])
|
||||
{
|
||||
t_MemorySegment *p_Mem = (t_MemorySegment *)h_Mem;
|
||||
uint32_t availableBlocks;
|
||||
register uint32_t i;
|
||||
uint32_t intFlags;
|
||||
#ifdef DEBUG_MEM_LEAKS
|
||||
uint32_t callerAddr = 0;
|
||||
|
||||
GET_CALLER_ADDR;
|
||||
#endif /* DEBUG_MEM_LEAKS */
|
||||
|
||||
ASSERT_COND(h_Mem);
|
||||
|
||||
intFlags = XX_LockIntrSpinlock(p_Mem->h_Spinlock);
|
||||
/* check how many blocks are available */
|
||||
availableBlocks = (uint32_t)(p_Mem->num - p_Mem->current);
|
||||
if (num > availableBlocks)
|
||||
{
|
||||
num = availableBlocks;
|
||||
}
|
||||
|
||||
for (i=0; i < num; i++)
|
||||
{
|
||||
/* get pointer to block */
|
||||
if ((array[i] = MemGet(p_Mem)) == NULL)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
#ifdef DEBUG_MEM_LEAKS
|
||||
DebugMemGet(p_Mem, array[i], callerAddr);
|
||||
#endif /* DEBUG_MEM_LEAKS */
|
||||
}
|
||||
XX_UnlockIntrSpinlock(p_Mem->h_Spinlock, intFlags);
|
||||
|
||||
return (uint16_t)i;
|
||||
}
|
||||
|
||||
|
||||
/*****************************************************************************/
|
||||
t_Error MEM_Put(t_Handle h_Mem, void *p_Block)
|
||||
{
|
||||
t_MemorySegment *p_Mem = (t_MemorySegment *)h_Mem;
|
||||
t_Error rc;
|
||||
uint32_t intFlags;
|
||||
|
||||
ASSERT_COND(h_Mem);
|
||||
|
||||
intFlags = XX_LockIntrSpinlock(p_Mem->h_Spinlock);
|
||||
/* check if blocks stack is full */
|
||||
if ((rc = MemPut(p_Mem, p_Block)) != E_OK)
|
||||
{
|
||||
XX_UnlockIntrSpinlock(p_Mem->h_Spinlock, intFlags);
|
||||
RETURN_ERROR(MAJOR, rc, NO_MSG);
|
||||
}
|
||||
|
||||
#ifdef DEBUG_MEM_LEAKS
|
||||
DebugMemPut(p_Mem, p_Block);
|
||||
#endif /* DEBUG_MEM_LEAKS */
|
||||
XX_UnlockIntrSpinlock(p_Mem->h_Spinlock, intFlags);
|
||||
|
||||
return E_OK;
|
||||
}
|
||||
|
||||
|
||||
#ifdef DEBUG_MEM_LEAKS
|
||||
|
||||
/*****************************************************************************/
|
||||
void MEM_CheckLeaks(t_Handle h_Mem)
|
||||
{
|
||||
t_MemorySegment *p_Mem = (t_MemorySegment *)h_Mem;
|
||||
t_MemDbg *p_MemDbg = (t_MemDbg *)p_Mem->p_MemDbg;
|
||||
uint8_t *p_Block;
|
||||
int i;
|
||||
|
||||
ASSERT_COND(h_Mem);
|
||||
|
||||
if (p_Mem->consecutiveMem)
|
||||
{
|
||||
for (i=0; i < p_Mem->num; i++)
|
||||
{
|
||||
if (p_MemDbg[i].ownerAddress != ILLEGAL_BASE)
|
||||
{
|
||||
/* Find the block address */
|
||||
p_Block = ((p_Mem->p_Bases[0] + p_Mem->blockOffset) +
|
||||
(i * p_Mem->blockSize));
|
||||
|
||||
XX_Print("MEM leak: 0x%08x, Caller address: 0x%08x\n",
|
||||
p_Block, p_MemDbg[i].ownerAddress);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
for (i=0; i < p_Mem->num; i++)
|
||||
{
|
||||
if (p_MemDbg[i].ownerAddress != ILLEGAL_BASE)
|
||||
{
|
||||
/* Find the block address */
|
||||
p_Block = p_Mem->p_Bases[i];
|
||||
|
||||
ALIGN_BLOCK(p_Block, p_Mem->prefixSize, p_Mem->alignment);
|
||||
|
||||
if (p_Block == p_Mem->p_Bases[i])
|
||||
p_Block += p_Mem->alignment;
|
||||
|
||||
XX_Print("MEM leak: 0x%08x, Caller address: 0x%08x\n",
|
||||
p_Block, p_MemDbg[i].ownerAddress);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif /* DEBUG_MEM_LEAKS */
|
||||
|
||||
|
||||
@@ -0,0 +1,50 @@
|
||||
/******************************************************************************
|
||||
|
||||
© 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.
|
||||
|
||||
**************************************************************************/
|
||||
#ifndef __MEM_H
|
||||
#define __MEM_H
|
||||
|
||||
#include "error_ext.h"
|
||||
#include "std_ext.h"
|
||||
#include "list_ext.h"
|
||||
|
||||
|
||||
#define __ERR_MODULE__ MODULE_MEM
|
||||
|
||||
|
||||
|
||||
|
||||
/**************************************************************************//**
|
||||
@Description Memory allocation owner.
|
||||
*//***************************************************************************/
|
||||
typedef enum e_MemAllocOwner
|
||||
{
|
||||
e_MEM_ALLOC_OWNER_LOCAL,
|
||||
e_MEM_ALLOC_OWNER_LOCAL_SMART,
|
||||
e_MEM_ALLOC_OWNER_EXTERNAL
|
||||
} e_MemAllocOwner;
|
||||
|
||||
|
||||
#ifdef DEBUG_MEM_LEAKS
|
||||
|
||||
/**************************************************************************//**
|
||||
@Description MEM block information for leaks detection.
|
||||
*//***************************************************************************/
|
||||
typedef struct t_MemDbg
|
||||
{
|
||||
uint32_t ownerAddress;
|
||||
|
||||
} t_MemDbg;
|
||||
|
||||
#endif /* DEBUG_MEM_LEAKS */
|
||||
|
||||
|
||||
#endif /* __MEM_H */
|
||||
@@ -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