2 * Copyright 2008-2012 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.
37 #include "memcpy_ext.h"
39 void * MemCpy8(void* pDst, void* pSrc, uint32_t size)
43 for(i = 0; i < size; ++i)
44 *(((uint8_t*)(pDst)) + i) = *(((uint8_t*)(pSrc)) + i);
49 void * MemSet8(void* pDst, int c, uint32_t size)
53 for(i = 0; i < size; ++i)
54 *(((uint8_t*)(pDst)) + i) = (uint8_t)(c);
59 void * MemCpy32(void* pDst,void* pSrc, uint32_t size)
70 p_Src8 = (uint8_t*)(pSrc);
71 p_Dst8 = (uint8_t*)(pDst);
72 /* first copy byte by byte till the source first alignment
73 * this step is necessary to ensure we do not even try to access
74 * data which is before the source buffer, hence it is not ours.
76 while((PTR_TO_UINT(p_Src8) & 3) && size) /* (pSrc mod 4) > 0 and size > 0 */
78 *p_Dst8++ = *p_Src8++;
82 /* align destination (possibly disaligning source)*/
83 while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
85 *p_Dst8++ = *p_Src8++;
89 /* dest is aligned and source is not necessarily aligned */
90 leftAlign = (uint32_t)((PTR_TO_UINT(p_Src8) & 3) << 3); /* leftAlign = (pSrc mod 4)*8 */
91 rightAlign = 32 - leftAlign;
96 /* source is also aligned */
97 p_Src32 = (uint32_t*)(p_Src8);
98 p_Dst32 = (uint32_t*)(p_Dst8);
99 while (size >> 2) /* size >= 4 */
101 *p_Dst32++ = *p_Src32++;
104 p_Src8 = (uint8_t*)(p_Src32);
105 p_Dst8 = (uint8_t*)(p_Dst32);
109 /* source is not aligned (destination is aligned)*/
110 p_Src32 = (uint32_t*)(p_Src8 - (leftAlign >> 3));
111 p_Dst32 = (uint32_t*)(p_Dst8);
112 lastWord = *p_Src32++;
113 while(size >> 3) /* size >= 8 */
116 *p_Dst32 = (lastWord << leftAlign) | (currWord >> rightAlign);
122 p_Dst8 = (uint8_t*)(p_Dst32);
123 p_Src8 = (uint8_t*)(p_Src32) - 4 + (leftAlign >> 3);
126 /* complete the left overs */
128 *p_Dst8++ = *p_Src8++;
133 void * IO2IOCpy32(void* pDst,void* pSrc, uint32_t size)
144 p_Src8 = (uint8_t*)(pSrc);
145 p_Dst8 = (uint8_t*)(pDst);
146 /* first copy byte by byte till the source first alignment
147 * this step is necessary to ensure we do not even try to access
148 * data which is before the source buffer, hence it is not ours.
150 while((PTR_TO_UINT(p_Src8) & 3) && size) /* (pSrc mod 4) > 0 and size > 0 */
152 WRITE_UINT8(*p_Dst8, GET_UINT8(*p_Src8));
157 /* align destination (possibly disaligning source)*/
158 while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
160 WRITE_UINT8(*p_Dst8, GET_UINT8(*p_Src8));
165 /* dest is aligned and source is not necessarily aligned */
166 leftAlign = (uint32_t)((PTR_TO_UINT(p_Src8) & 3) << 3); /* leftAlign = (pSrc mod 4)*8 */
167 rightAlign = 32 - leftAlign;
171 /* source is also aligned */
172 p_Src32 = (uint32_t*)(p_Src8);
173 p_Dst32 = (uint32_t*)(p_Dst8);
174 while (size >> 2) /* size >= 4 */
176 WRITE_UINT32(*p_Dst32, GET_UINT32(*p_Src32));
180 p_Src8 = (uint8_t*)(p_Src32);
181 p_Dst8 = (uint8_t*)(p_Dst32);
185 /* source is not aligned (destination is aligned)*/
186 p_Src32 = (uint32_t*)(p_Src8 - (leftAlign >> 3));
187 p_Dst32 = (uint32_t*)(p_Dst8);
188 lastWord = GET_UINT32(*p_Src32);
190 while(size >> 3) /* size >= 8 */
192 currWord = GET_UINT32(*p_Src32);
193 WRITE_UINT32(*p_Dst32, (lastWord << leftAlign) | (currWord >> rightAlign));
198 p_Dst8 = (uint8_t*)(p_Dst32);
199 p_Src8 = (uint8_t*)(p_Src32) - 4 + (leftAlign >> 3);
202 /* complete the left overs */
205 WRITE_UINT8(*p_Dst8, GET_UINT8(*p_Src8));
212 void * Mem2IOCpy32(void* pDst,void* pSrc, uint32_t size)
223 p_Src8 = (uint8_t*)(pSrc);
224 p_Dst8 = (uint8_t*)(pDst);
225 /* first copy byte by byte till the source first alignment
226 * this step is necessary to ensure we do not even try to access
227 * data which is before the source buffer, hence it is not ours.
229 while((PTR_TO_UINT(p_Src8) & 3) && size) /* (pSrc mod 4) > 0 and size > 0 */
231 WRITE_UINT8(*p_Dst8, *p_Src8);
236 /* align destination (possibly disaligning source)*/
237 while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
239 WRITE_UINT8(*p_Dst8, *p_Src8);
244 /* dest is aligned and source is not necessarily aligned */
245 leftAlign = (uint32_t)((PTR_TO_UINT(p_Src8) & 3) << 3); /* leftAlign = (pSrc mod 4)*8 */
246 rightAlign = 32 - leftAlign;
250 /* source is also aligned */
251 p_Src32 = (uint32_t*)(p_Src8);
252 p_Dst32 = (uint32_t*)(p_Dst8);
253 while (size >> 2) /* size >= 4 */
255 WRITE_UINT32(*p_Dst32, *p_Src32);
259 p_Src8 = (uint8_t*)(p_Src32);
260 p_Dst8 = (uint8_t*)(p_Dst32);
264 /* source is not aligned (destination is aligned)*/
265 p_Src32 = (uint32_t*)(p_Src8 - (leftAlign >> 3));
266 p_Dst32 = (uint32_t*)(p_Dst8);
267 lastWord = *p_Src32++;
268 while(size >> 3) /* size >= 8 */
271 WRITE_UINT32(*p_Dst32, (lastWord << leftAlign) | (currWord >> rightAlign));
276 p_Dst8 = (uint8_t*)(p_Dst32);
277 p_Src8 = (uint8_t*)(p_Src32) - 4 + (leftAlign >> 3);
280 /* complete the left overs */
283 WRITE_UINT8(*p_Dst8, *p_Src8);
290 void * IO2MemCpy32(void* pDst,void* pSrc, uint32_t size)
301 p_Src8 = (uint8_t*)(pSrc);
302 p_Dst8 = (uint8_t*)(pDst);
303 /* first copy byte by byte till the source first alignment
304 * this step is necessary to ensure we do not even try to access
305 * data which is before the source buffer, hence it is not ours.
307 while((PTR_TO_UINT(p_Src8) & 3) && size) /* (pSrc mod 4) > 0 and size > 0 */
309 *p_Dst8 = GET_UINT8(*p_Src8);
314 /* align destination (possibly disaligning source)*/
315 while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
317 *p_Dst8 = GET_UINT8(*p_Src8);
322 /* dest is aligned and source is not necessarily aligned */
323 leftAlign = (uint32_t)((PTR_TO_UINT(p_Src8) & 3) << 3); /* leftAlign = (pSrc mod 4)*8 */
324 rightAlign = 32 - leftAlign;
328 /* source is also aligned */
329 p_Src32 = (uint32_t*)(p_Src8);
330 p_Dst32 = (uint32_t*)(p_Dst8);
331 while (size >> 2) /* size >= 4 */
333 *p_Dst32 = GET_UINT32(*p_Src32);
337 p_Src8 = (uint8_t*)(p_Src32);
338 p_Dst8 = (uint8_t*)(p_Dst32);
342 /* source is not aligned (destination is aligned)*/
343 p_Src32 = (uint32_t*)(p_Src8 - (leftAlign >> 3));
344 p_Dst32 = (uint32_t*)(p_Dst8);
345 lastWord = GET_UINT32(*p_Src32);
347 while(size >> 3) /* size >= 8 */
349 currWord = GET_UINT32(*p_Src32);
350 *p_Dst32 = (lastWord << leftAlign) | (currWord >> rightAlign);
355 p_Dst8 = (uint8_t*)(p_Dst32);
356 p_Src8 = (uint8_t*)(p_Src32) - 4 + (leftAlign >> 3);
359 /* complete the left overs */
362 *p_Dst8 = GET_UINT8(*p_Src8);
369 void * MemCpy64(void* pDst,void* pSrc, uint32_t size)
380 p_Src8 = (uint8_t*)(pSrc);
381 p_Dst8 = (uint8_t*)(pDst);
382 /* first copy byte by byte till the source first alignment
383 * this step is necessarily to ensure we do not even try to access
384 * data which is before the source buffer, hence it is not ours.
386 while((PTR_TO_UINT(p_Src8) & 7) && size) /* (pSrc mod 8) > 0 and size > 0 */
388 *p_Dst8++ = *p_Src8++;
392 /* align destination (possibly disaligning source)*/
393 while((PTR_TO_UINT(p_Dst8) & 7) && size) /* (pDst mod 8) > 0 and size > 0 */
395 *p_Dst8++ = *p_Src8++;
399 /* dest is aligned and source is not necessarily aligned */
400 leftAlign = (uint32_t)((PTR_TO_UINT(p_Src8) & 7) << 3); /* leftAlign = (pSrc mod 8)*8 */
401 rightAlign = 64 - leftAlign;
406 /* source is also aligned */
407 pSrc64 = (uint64_t*)(p_Src8);
408 pDst64 = (uint64_t*)(p_Dst8);
409 while (size >> 3) /* size >= 8 */
411 *pDst64++ = *pSrc64++;
414 p_Src8 = (uint8_t*)(pSrc64);
415 p_Dst8 = (uint8_t*)(pDst64);
419 /* source is not aligned (destination is aligned)*/
420 pSrc64 = (uint64_t*)(p_Src8 - (leftAlign >> 3));
421 pDst64 = (uint64_t*)(p_Dst8);
422 lastWord = *pSrc64++;
423 while(size >> 4) /* size >= 16 */
426 *pDst64 = (lastWord << leftAlign) | (currWord >> rightAlign);
432 p_Dst8 = (uint8_t*)(pDst64);
433 p_Src8 = (uint8_t*)(pSrc64) - 8 + (leftAlign >> 3);
436 /* complete the left overs */
438 *p_Dst8++ = *p_Src8++;
443 void * MemSet32(void* pDst, uint8_t val, uint32_t size)
449 p_Dst8 = (uint8_t*)(pDst);
451 /* generate four 8-bit val's in 32-bit container */
452 val32 = (uint32_t) val;
453 val32 |= (val32 << 8);
454 val32 |= (val32 << 16);
456 /* align destination to 32 */
457 while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
464 p_Dst32 = (uint32_t*)(p_Dst8);
465 while (size >> 2) /* size >= 4 */
471 /* complete the leftovers */
472 p_Dst8 = (uint8_t*)(p_Dst32);
479 void * IOMemSet32(void* pDst, uint8_t val, uint32_t size)
485 p_Dst8 = (uint8_t*)(pDst);
487 /* generate four 8-bit val's in 32-bit container */
488 val32 = (uint32_t) val;
489 val32 |= (val32 << 8);
490 val32 |= (val32 << 16);
492 /* align destination to 32 */
493 while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
495 WRITE_UINT8(*p_Dst8, val);
501 p_Dst32 = (uint32_t*)(p_Dst8);
502 while (size >> 2) /* size >= 4 */
504 WRITE_UINT32(*p_Dst32, val32);
509 /* complete the leftovers */
510 p_Dst8 = (uint8_t*)(p_Dst32);
513 WRITE_UINT8(*p_Dst8, val);
520 void * MemSet64(void* pDst, uint8_t val, uint32_t size)
526 p_Dst8 = (uint8_t*)(pDst);
528 /* generate four 8-bit val's in 32-bit container */
529 val64 = (uint64_t) val;
530 val64 |= (val64 << 8);
531 val64 |= (val64 << 16);
532 val64 |= (val64 << 24);
533 val64 |= (val64 << 32);
535 /* align destination to 64 */
536 while((PTR_TO_UINT(p_Dst8) & 7) && size) /* (pDst mod 8) > 0 and size > 0 */
543 pDst64 = (uint64_t*)(p_Dst8);
544 while (size >> 4) /* size >= 8 */
550 /* complete the leftovers */
551 p_Dst8 = (uint8_t*)(pDst64);
558 void MemDisp(uint8_t *p, int size)
560 uint32_t space = (uint32_t)(PTR_TO_UINT(p) & 0x3);
565 p_Limit = (p - space + 4);
567 XX_Print("0x%08X: ", (p - space));
573 while (size && (p < p_Limit))
575 XX_Print("%02x", *(uint8_t*)p);
583 while ((size > 3) && (p < p_Limit))
585 XX_Print("%08x ", *(uint32_t*)p);
594 XX_Print("0x%08X: %08x %08x %08x %08x\r\n",
595 p, *(uint32_t *)p, *(uint32_t *)(p + 4),
596 *(uint32_t *)(p + 8), *(uint32_t *)(p + 12));
603 XX_Print("0x%08X: ", p);
607 XX_Print("%08x ", *(uint32_t *)p);
613 XX_Print("%02x", *(uint8_t *)p);