diff options
Diffstat (limited to 'drivers/net/ethernet/freescale/sdk_fman/etc/memcpy.c')
| -rw-r--r-- | drivers/net/ethernet/freescale/sdk_fman/etc/memcpy.c | 620 |
1 files changed, 620 insertions, 0 deletions
diff --git a/drivers/net/ethernet/freescale/sdk_fman/etc/memcpy.c b/drivers/net/ethernet/freescale/sdk_fman/etc/memcpy.c new file mode 100644 index 0000000..fa203ec --- /dev/null +++ b/drivers/net/ethernet/freescale/sdk_fman/etc/memcpy.c @@ -0,0 +1,620 @@ +/* + * Copyright 2008-2012 Freescale Semiconductor Inc. + * + * 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" + +void * MemCpy8(void* pDst, void* pSrc, uint32_t size) +{ + int i; + + for(i = 0; i < size; ++i) + *(((uint8_t*)(pDst)) + i) = *(((uint8_t*)(pSrc)) + i); + + return pDst; +} + +void * MemSet8(void* pDst, int c, uint32_t size) +{ + int i; + + for(i = 0; i < size; ++i) + *(((uint8_t*)(pDst)) + i) = (uint8_t)(c); + + return pDst; +} + +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 */ + { + WRITE_UINT8(*p_Dst8, 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 */ + { + WRITE_UINT8(*p_Dst8, 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--) + { + WRITE_UINT8(*p_Dst8, 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 */ + { + 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 */ + { + 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--) + { + 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 = 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 = 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 = 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 */ + { + 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--) + { + 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"); + } +} |