1 /* Copyright (c) 2008-2011 Freescale Semiconductor, Inc.
4 * Redistribution and use in source and binary forms, with or without
5 * modification, are permitted provided that the following conditions are met:
6 * * Redistributions of source code must retain the above copyright
7 * notice, this list of conditions and the following disclaimer.
8 * * Redistributions in binary form must reproduce the above copyright
9 * notice, this list of conditions and the following disclaimer in the
10 * documentation and/or other materials provided with the distribution.
11 * * Neither the name of Freescale Semiconductor nor the
12 * names of its contributors may be used to endorse or promote products
13 * derived from this software without specific prior written permission.
16 * ALTERNATIVELY, this software may be distributed under the terms of the
17 * GNU General Public License ("GPL") as published by the Free Software
18 * Foundation, either version 2 of that License or (at your option) any
21 * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``AS IS'' AND ANY
22 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
23 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
24 * DISCLAIMED. IN NO EVENT SHALL Freescale Semiconductor BE LIABLE FOR ANY
25 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
26 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
28 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
29 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
30 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
36 #include "memcpy_ext.h"
39 #ifdef CORE_8BIT_ACCESS_ERRATA
40 static void MY_MY_WRITE_UINT8(uint8_t *addr, uint8_t val)
42 uint32_t newAddr, newVal;
43 newAddr = (uint32_t)addr & ~0x3L;
44 switch ((uint32_t)addr%4)
47 newVal = GET_UINT32(*(uint32_t*)newAddr);
48 newVal = (newVal & 0x00ffffff) | (((uint32_t)val)<<24);
49 WRITE_UINT32(*(uint32_t*)newAddr, newVal);
52 newVal = GET_UINT32(*(uint32_t*)newAddr);
53 newVal = (newVal & 0xff00ffff) | (((uint32_t)val)<<16);
54 WRITE_UINT32(*(uint32_t*)newAddr, newVal);
57 newVal = GET_UINT32(*(uint32_t*)newAddr);
58 newVal = (newVal & 0xffff00ff) | (((uint32_t)val)<<8);
59 WRITE_UINT32(*(uint32_t*)newAddr, newVal);
62 newVal = GET_UINT32(*(uint32_t*)newAddr);
63 newVal = (newVal & 0xffffff00) | val;
64 WRITE_UINT32(*(uint32_t*)newAddr, newVal);
69 static uint8_t MY_MY_GET_UINT8(uint8_t *addr)
71 uint32_t newAddr, newVal=0;
72 newAddr = (uint32_t)addr & ~0x3L;
73 switch ((uint32_t)addr%4)
76 newVal = GET_UINT32(*(uint32_t*)newAddr);
77 newVal = (newVal & 0xff000000)>>24;
80 newVal = GET_UINT32(*(uint32_t*)newAddr);
81 newVal = (newVal & 0x00ff0000)>>16;
84 newVal = GET_UINT32(*(uint32_t*)newAddr);
85 newVal = (newVal & 0x0000ff00)>>8;
88 newVal = GET_UINT32(*(uint32_t*)newAddr);
89 newVal = (newVal & 0x000000ff);
93 return (uint8_t)newVal;
96 #define MY_WRITE_UINT8(addr,val) MY_MY_WRITE_UINT8(&addr,val)
97 #define MY_GET_UINT8(addr) MY_MY_GET_UINT8(&addr)
99 #define MY_WRITE_UINT8 WRITE_UINT8
100 #define MY_GET_UINT8 GET_UINT8
101 #endif /* CORE_8BIT_ACCESS_ERRATA */
104 void * MemCpy32(void* pDst,void* pSrc, uint32_t size)
115 p_Src8 = (uint8_t*)(pSrc);
116 p_Dst8 = (uint8_t*)(pDst);
117 /* first copy byte by byte till the source first alignment
118 * this step is necessary to ensure we do not even try to access
119 * data which is before the source buffer, hence it is not ours.
121 while((PTR_TO_UINT(p_Src8) & 3) && size) /* (pSrc mod 4) > 0 and size > 0 */
123 *p_Dst8++ = *p_Src8++;
127 /* align destination (possibly disaligning source)*/
128 while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
130 *p_Dst8++ = *p_Src8++;
134 /* dest is aligned and source is not necessarily aligned */
135 leftAlign = (uint32_t)((PTR_TO_UINT(p_Src8) & 3) << 3); /* leftAlign = (pSrc mod 4)*8 */
136 rightAlign = 32 - leftAlign;
141 /* source is also aligned */
142 p_Src32 = (uint32_t*)(p_Src8);
143 p_Dst32 = (uint32_t*)(p_Dst8);
144 while (size >> 2) /* size >= 4 */
146 *p_Dst32++ = *p_Src32++;
149 p_Src8 = (uint8_t*)(p_Src32);
150 p_Dst8 = (uint8_t*)(p_Dst32);
154 /* source is not aligned (destination is aligned)*/
155 p_Src32 = (uint32_t*)(p_Src8 - (leftAlign >> 3));
156 p_Dst32 = (uint32_t*)(p_Dst8);
157 lastWord = *p_Src32++;
158 while(size >> 3) /* size >= 8 */
161 *p_Dst32 = (lastWord << leftAlign) | (currWord >> rightAlign);
167 p_Dst8 = (uint8_t*)(p_Dst32);
168 p_Src8 = (uint8_t*)(p_Src32) - 4 + (leftAlign >> 3);
171 /* complete the left overs */
173 *p_Dst8++ = *p_Src8++;
178 void * IO2IOCpy32(void* pDst,void* pSrc, uint32_t size)
189 p_Src8 = (uint8_t*)(pSrc);
190 p_Dst8 = (uint8_t*)(pDst);
191 /* first copy byte by byte till the source first alignment
192 * this step is necessary to ensure we do not even try to access
193 * data which is before the source buffer, hence it is not ours.
195 while((PTR_TO_UINT(p_Src8) & 3) && size) /* (pSrc mod 4) > 0 and size > 0 */
197 MY_WRITE_UINT8(*p_Dst8, MY_GET_UINT8(*p_Src8));
202 /* align destination (possibly disaligning source)*/
203 while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
205 MY_WRITE_UINT8(*p_Dst8, MY_GET_UINT8(*p_Src8));
210 /* dest is aligned and source is not necessarily aligned */
211 leftAlign = (uint32_t)((PTR_TO_UINT(p_Src8) & 3) << 3); /* leftAlign = (pSrc mod 4)*8 */
212 rightAlign = 32 - leftAlign;
216 /* source is also aligned */
217 p_Src32 = (uint32_t*)(p_Src8);
218 p_Dst32 = (uint32_t*)(p_Dst8);
219 while (size >> 2) /* size >= 4 */
221 WRITE_UINT32(*p_Dst32, GET_UINT32(*p_Src32));
225 p_Src8 = (uint8_t*)(p_Src32);
226 p_Dst8 = (uint8_t*)(p_Dst32);
230 /* source is not aligned (destination is aligned)*/
231 p_Src32 = (uint32_t*)(p_Src8 - (leftAlign >> 3));
232 p_Dst32 = (uint32_t*)(p_Dst8);
233 lastWord = GET_UINT32(*p_Src32);
235 while(size >> 3) /* size >= 8 */
237 currWord = GET_UINT32(*p_Src32);
238 WRITE_UINT32(*p_Dst32, (lastWord << leftAlign) | (currWord >> rightAlign));
243 p_Dst8 = (uint8_t*)(p_Dst32);
244 p_Src8 = (uint8_t*)(p_Src32) - 4 + (leftAlign >> 3);
247 /* complete the left overs */
250 MY_WRITE_UINT8(*p_Dst8, MY_GET_UINT8(*p_Src8));
257 void * Mem2IOCpy32(void* pDst,void* pSrc, uint32_t size)
268 p_Src8 = (uint8_t*)(pSrc);
269 p_Dst8 = (uint8_t*)(pDst);
270 /* first copy byte by byte till the source first alignment
271 * this step is necessary to ensure we do not even try to access
272 * data which is before the source buffer, hence it is not ours.
274 while((PTR_TO_UINT(p_Src8) & 3) && size) /* (pSrc mod 4) > 0 and size > 0 */
276 MY_WRITE_UINT8(*p_Dst8, *p_Src8);
281 /* align destination (possibly disaligning source)*/
282 while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
284 MY_WRITE_UINT8(*p_Dst8, *p_Src8);
289 /* dest is aligned and source is not necessarily aligned */
290 leftAlign = (uint32_t)((PTR_TO_UINT(p_Src8) & 3) << 3); /* leftAlign = (pSrc mod 4)*8 */
291 rightAlign = 32 - leftAlign;
295 /* source is also aligned */
296 p_Src32 = (uint32_t*)(p_Src8);
297 p_Dst32 = (uint32_t*)(p_Dst8);
298 while (size >> 2) /* size >= 4 */
300 WRITE_UINT32(*p_Dst32, *p_Src32);
304 p_Src8 = (uint8_t*)(p_Src32);
305 p_Dst8 = (uint8_t*)(p_Dst32);
309 /* source is not aligned (destination is aligned)*/
310 p_Src32 = (uint32_t*)(p_Src8 - (leftAlign >> 3));
311 p_Dst32 = (uint32_t*)(p_Dst8);
312 lastWord = *p_Src32++;
313 while(size >> 3) /* size >= 8 */
316 WRITE_UINT32(*p_Dst32, (lastWord << leftAlign) | (currWord >> rightAlign));
321 p_Dst8 = (uint8_t*)(p_Dst32);
322 p_Src8 = (uint8_t*)(p_Src32) - 4 + (leftAlign >> 3);
325 /* complete the left overs */
328 MY_WRITE_UINT8(*p_Dst8, *p_Src8);
335 void * IO2MemCpy32(void* pDst,void* pSrc, uint32_t size)
346 p_Src8 = (uint8_t*)(pSrc);
347 p_Dst8 = (uint8_t*)(pDst);
348 /* first copy byte by byte till the source first alignment
349 * this step is necessary to ensure we do not even try to access
350 * data which is before the source buffer, hence it is not ours.
352 while((PTR_TO_UINT(p_Src8) & 3) && size) /* (pSrc mod 4) > 0 and size > 0 */
354 *p_Dst8 = MY_GET_UINT8(*p_Src8);
359 /* align destination (possibly disaligning source)*/
360 while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
362 *p_Dst8 = MY_GET_UINT8(*p_Src8);
367 /* dest is aligned and source is not necessarily aligned */
368 leftAlign = (uint32_t)((PTR_TO_UINT(p_Src8) & 3) << 3); /* leftAlign = (pSrc mod 4)*8 */
369 rightAlign = 32 - leftAlign;
373 /* source is also aligned */
374 p_Src32 = (uint32_t*)(p_Src8);
375 p_Dst32 = (uint32_t*)(p_Dst8);
376 while (size >> 2) /* size >= 4 */
378 *p_Dst32 = GET_UINT32(*p_Src32);
382 p_Src8 = (uint8_t*)(p_Src32);
383 p_Dst8 = (uint8_t*)(p_Dst32);
387 /* source is not aligned (destination is aligned)*/
388 p_Src32 = (uint32_t*)(p_Src8 - (leftAlign >> 3));
389 p_Dst32 = (uint32_t*)(p_Dst8);
390 lastWord = GET_UINT32(*p_Src32);
392 while(size >> 3) /* size >= 8 */
394 currWord = GET_UINT32(*p_Src32);
395 *p_Dst32 = (lastWord << leftAlign) | (currWord >> rightAlign);
400 p_Dst8 = (uint8_t*)(p_Dst32);
401 p_Src8 = (uint8_t*)(p_Src32) - 4 + (leftAlign >> 3);
404 /* complete the left overs */
407 *p_Dst8 = MY_GET_UINT8(*p_Src8);
414 void * MemCpy64(void* pDst,void* pSrc, uint32_t size)
425 p_Src8 = (uint8_t*)(pSrc);
426 p_Dst8 = (uint8_t*)(pDst);
427 /* first copy byte by byte till the source first alignment
428 * this step is necessarily to ensure we do not even try to access
429 * data which is before the source buffer, hence it is not ours.
431 while((PTR_TO_UINT(p_Src8) & 7) && size) /* (pSrc mod 8) > 0 and size > 0 */
433 *p_Dst8++ = *p_Src8++;
437 /* align destination (possibly disaligning source)*/
438 while((PTR_TO_UINT(p_Dst8) & 7) && size) /* (pDst mod 8) > 0 and size > 0 */
440 *p_Dst8++ = *p_Src8++;
444 /* dest is aligned and source is not necessarily aligned */
445 leftAlign = (uint32_t)((PTR_TO_UINT(p_Src8) & 7) << 3); /* leftAlign = (pSrc mod 8)*8 */
446 rightAlign = 64 - leftAlign;
451 /* source is also aligned */
452 pSrc64 = (uint64_t*)(p_Src8);
453 pDst64 = (uint64_t*)(p_Dst8);
454 while (size >> 3) /* size >= 8 */
456 *pDst64++ = *pSrc64++;
459 p_Src8 = (uint8_t*)(pSrc64);
460 p_Dst8 = (uint8_t*)(pDst64);
464 /* source is not aligned (destination is aligned)*/
465 pSrc64 = (uint64_t*)(p_Src8 - (leftAlign >> 3));
466 pDst64 = (uint64_t*)(p_Dst8);
467 lastWord = *pSrc64++;
468 while(size >> 4) /* size >= 16 */
471 *pDst64 = (lastWord << leftAlign) | (currWord >> rightAlign);
477 p_Dst8 = (uint8_t*)(pDst64);
478 p_Src8 = (uint8_t*)(pSrc64) - 8 + (leftAlign >> 3);
481 /* complete the left overs */
483 *p_Dst8++ = *p_Src8++;
488 void * MemSet32(void* pDst, uint8_t val, uint32_t size)
494 p_Dst8 = (uint8_t*)(pDst);
496 /* generate four 8-bit val's in 32-bit container */
497 val32 = (uint32_t) val;
498 val32 |= (val32 << 8);
499 val32 |= (val32 << 16);
501 /* align destination to 32 */
502 while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
509 p_Dst32 = (uint32_t*)(p_Dst8);
510 while (size >> 2) /* size >= 4 */
516 /* complete the leftovers */
517 p_Dst8 = (uint8_t*)(p_Dst32);
524 void * IOMemSet32(void* pDst, uint8_t val, uint32_t size)
530 p_Dst8 = (uint8_t*)(pDst);
532 /* generate four 8-bit val's in 32-bit container */
533 val32 = (uint32_t) val;
534 val32 |= (val32 << 8);
535 val32 |= (val32 << 16);
537 /* align destination to 32 */
538 while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
540 MY_WRITE_UINT8(*p_Dst8, val);
546 p_Dst32 = (uint32_t*)(p_Dst8);
547 while (size >> 2) /* size >= 4 */
549 WRITE_UINT32(*p_Dst32, val32);
554 /* complete the leftovers */
555 p_Dst8 = (uint8_t*)(p_Dst32);
558 MY_WRITE_UINT8(*p_Dst8, val);
565 void * MemSet64(void* pDst, uint8_t val, uint32_t size)
571 p_Dst8 = (uint8_t*)(pDst);
573 /* generate four 8-bit val's in 32-bit container */
574 val64 = (uint64_t) val;
575 val64 |= (val64 << 8);
576 val64 |= (val64 << 16);
577 val64 |= (val64 << 24);
578 val64 |= (val64 << 32);
580 /* align destination to 64 */
581 while((PTR_TO_UINT(p_Dst8) & 7) && size) /* (pDst mod 8) > 0 and size > 0 */
588 pDst64 = (uint64_t*)(p_Dst8);
589 while (size >> 4) /* size >= 8 */
595 /* complete the leftovers */
596 p_Dst8 = (uint8_t*)(pDst64);
603 void MemDisp(uint8_t *p, int size)
605 uint32_t space = (uint32_t)(PTR_TO_UINT(p) & 0x3);
610 p_Limit = (p - space + 4);
612 XX_Print("0x%08X: ", (p - space));
618 while (size && (p < p_Limit))
620 XX_Print("%02x", *(uint8_t*)p);
628 while ((size > 3) && (p < p_Limit))
630 XX_Print("%08x ", *(uint32_t*)p);
639 XX_Print("0x%08X: %08x %08x %08x %08x\r\n",
640 p, *(uint32_t *)p, *(uint32_t *)(p + 4),
641 *(uint32_t *)(p + 8), *(uint32_t *)(p + 12));
648 XX_Print("0x%08X: ", p);
652 XX_Print("%08x ", *(uint32_t *)p);
658 XX_Print("%02x", *(uint8_t *)p);