1 /*
2 * Copyright 2008-2012 Freescale Semiconductor Inc.
3 *
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.
14 *
15 *
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
19 * later version.
20 *
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.
31 */
32
33
34
35 #include "std_ext.h"
36 #include "xx_ext.h"
37 #include "memcpy_ext.h"
38
MemCpy8(void * pDst,void * pSrc,uint32_t size)39 void * MemCpy8(void* pDst, void* pSrc, uint32_t size)
40 {
41 uint32_t i;
42
43 for(i = 0; i < size; ++i)
44 *(((uint8_t*)(pDst)) + i) = *(((uint8_t*)(pSrc)) + i);
45
46 return pDst;
47 }
48
MemSet8(void * pDst,int c,uint32_t size)49 void * MemSet8(void* pDst, int c, uint32_t size)
50 {
51 uint32_t i;
52
53 for(i = 0; i < size; ++i)
54 *(((uint8_t*)(pDst)) + i) = (uint8_t)(c);
55
56 return pDst;
57 }
58
MemCpy32(void * pDst,void * pSrc,uint32_t size)59 void * MemCpy32(void* pDst,void* pSrc, uint32_t size)
60 {
61 uint32_t leftAlign;
62 uint32_t rightAlign;
63 uint32_t lastWord;
64 uint32_t currWord;
65 uint32_t *p_Src32;
66 uint32_t *p_Dst32;
67 uint8_t *p_Src8;
68 uint8_t *p_Dst8;
69
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.
75 */
76 while((PTR_TO_UINT(p_Src8) & 3) && size) /* (pSrc mod 4) > 0 and size > 0 */
77 {
78 *p_Dst8++ = *p_Src8++;
79 size--;
80 }
81
82 /* align destination (possibly disaligning source)*/
83 while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
84 {
85 *p_Dst8++ = *p_Src8++;
86 size--;
87 }
88
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;
92
93
94 if (leftAlign == 0)
95 {
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 */
100 {
101 *p_Dst32++ = *p_Src32++;
102 size -= 4;
103 }
104 p_Src8 = (uint8_t*)(p_Src32);
105 p_Dst8 = (uint8_t*)(p_Dst32);
106 }
107 else
108 {
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 */
114 {
115 currWord = *p_Src32;
116 *p_Dst32 = (lastWord << leftAlign) | (currWord >> rightAlign);
117 lastWord = currWord;
118 p_Src32++;
119 p_Dst32++;
120 size -= 4;
121 }
122 p_Dst8 = (uint8_t*)(p_Dst32);
123 p_Src8 = (uint8_t*)(p_Src32) - 4 + (leftAlign >> 3);
124 }
125
126 /* complete the left overs */
127 while (size--)
128 *p_Dst8++ = *p_Src8++;
129
130 return pDst;
131 }
132
IO2IOCpy32(void * pDst,void * pSrc,uint32_t size)133 void * IO2IOCpy32(void* pDst,void* pSrc, uint32_t size)
134 {
135 uint32_t leftAlign;
136 uint32_t rightAlign;
137 uint32_t lastWord;
138 uint32_t currWord;
139 uint32_t *p_Src32;
140 uint32_t *p_Dst32;
141 uint8_t *p_Src8;
142 uint8_t *p_Dst8;
143
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.
149 */
150 while((PTR_TO_UINT(p_Src8) & 3) && size) /* (pSrc mod 4) > 0 and size > 0 */
151 {
152 WRITE_UINT8(*p_Dst8, GET_UINT8(*p_Src8));
153 p_Dst8++;p_Src8++;
154 size--;
155 }
156
157 /* align destination (possibly disaligning source)*/
158 while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
159 {
160 WRITE_UINT8(*p_Dst8, GET_UINT8(*p_Src8));
161 p_Dst8++;p_Src8++;
162 size--;
163 }
164
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;
168
169 if (leftAlign == 0)
170 {
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 */
175 {
176 WRITE_UINT32(*p_Dst32, GET_UINT32(*p_Src32));
177 p_Dst32++;p_Src32++;
178 size -= 4;
179 }
180 p_Src8 = (uint8_t*)(p_Src32);
181 p_Dst8 = (uint8_t*)(p_Dst32);
182 }
183 else
184 {
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);
189 p_Src32++;
190 while(size >> 3) /* size >= 8 */
191 {
192 currWord = GET_UINT32(*p_Src32);
193 WRITE_UINT32(*p_Dst32, (lastWord << leftAlign) | (currWord >> rightAlign));
194 lastWord = currWord;
195 p_Src32++;p_Dst32++;
196 size -= 4;
197 }
198 p_Dst8 = (uint8_t*)(p_Dst32);
199 p_Src8 = (uint8_t*)(p_Src32) - 4 + (leftAlign >> 3);
200 }
201
202 /* complete the left overs */
203 while (size--)
204 {
205 WRITE_UINT8(*p_Dst8, GET_UINT8(*p_Src8));
206 p_Dst8++;p_Src8++;
207 }
208
209 return pDst;
210 }
211
Mem2IOCpy32(void * pDst,void * pSrc,uint32_t size)212 void * Mem2IOCpy32(void* pDst,void* pSrc, uint32_t size)
213 {
214 uint32_t leftAlign;
215 uint32_t rightAlign;
216 uint32_t lastWord;
217 uint32_t currWord;
218 uint32_t *p_Src32;
219 uint32_t *p_Dst32;
220 uint8_t *p_Src8;
221 uint8_t *p_Dst8;
222
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.
228 */
229 while((PTR_TO_UINT(p_Src8) & 3) && size) /* (pSrc mod 4) > 0 and size > 0 */
230 {
231 WRITE_UINT8(*p_Dst8, *p_Src8);
232 p_Dst8++;p_Src8++;
233 size--;
234 }
235
236 /* align destination (possibly disaligning source)*/
237 while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
238 {
239 WRITE_UINT8(*p_Dst8, *p_Src8);
240 p_Dst8++;p_Src8++;
241 size--;
242 }
243
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;
247
248 if (leftAlign == 0)
249 {
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 */
254 {
255 WRITE_UINT32(*p_Dst32, *p_Src32);
256 p_Dst32++;p_Src32++;
257 size -= 4;
258 }
259 p_Src8 = (uint8_t*)(p_Src32);
260 p_Dst8 = (uint8_t*)(p_Dst32);
261 }
262 else
263 {
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 */
269 {
270 currWord = *p_Src32;
271 WRITE_UINT32(*p_Dst32, (lastWord << leftAlign) | (currWord >> rightAlign));
272 lastWord = currWord;
273 p_Src32++;p_Dst32++;
274 size -= 4;
275 }
276 p_Dst8 = (uint8_t*)(p_Dst32);
277 p_Src8 = (uint8_t*)(p_Src32) - 4 + (leftAlign >> 3);
278 }
279
280 /* complete the left overs */
281 while (size--)
282 {
283 WRITE_UINT8(*p_Dst8, *p_Src8);
284 p_Dst8++;p_Src8++;
285 }
286
287 return pDst;
288 }
289
IO2MemCpy32(void * pDst,void * pSrc,uint32_t size)290 void * IO2MemCpy32(void* pDst,void* pSrc, uint32_t size)
291 {
292 uint32_t leftAlign;
293 uint32_t rightAlign;
294 uint32_t lastWord;
295 uint32_t currWord;
296 uint32_t *p_Src32;
297 uint32_t *p_Dst32;
298 uint8_t *p_Src8;
299 uint8_t *p_Dst8;
300
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.
306 */
307 while((PTR_TO_UINT(p_Src8) & 3) && size) /* (pSrc mod 4) > 0 and size > 0 */
308 {
309 *p_Dst8 = GET_UINT8(*p_Src8);
310 p_Dst8++;p_Src8++;
311 size--;
312 }
313
314 /* align destination (possibly disaligning source)*/
315 while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
316 {
317 *p_Dst8 = GET_UINT8(*p_Src8);
318 p_Dst8++;p_Src8++;
319 size--;
320 }
321
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;
325
326 if (leftAlign == 0)
327 {
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 */
332 {
333 *p_Dst32 = GET_UINT32(*p_Src32);
334 p_Dst32++;p_Src32++;
335 size -= 4;
336 }
337 p_Src8 = (uint8_t*)(p_Src32);
338 p_Dst8 = (uint8_t*)(p_Dst32);
339 }
340 else
341 {
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);
346 p_Src32++;
347 while(size >> 3) /* size >= 8 */
348 {
349 currWord = GET_UINT32(*p_Src32);
350 *p_Dst32 = (lastWord << leftAlign) | (currWord >> rightAlign);
351 lastWord = currWord;
352 p_Src32++;p_Dst32++;
353 size -= 4;
354 }
355 p_Dst8 = (uint8_t*)(p_Dst32);
356 p_Src8 = (uint8_t*)(p_Src32) - 4 + (leftAlign >> 3);
357 }
358
359 /* complete the left overs */
360 while (size--)
361 {
362 *p_Dst8 = GET_UINT8(*p_Src8);
363 p_Dst8++;p_Src8++;
364 }
365
366 return pDst;
367 }
368
MemCpy64(void * pDst,void * pSrc,uint32_t size)369 void * MemCpy64(void* pDst,void* pSrc, uint32_t size)
370 {
371 uint32_t leftAlign;
372 uint32_t rightAlign;
373 uint64_t lastWord;
374 uint64_t currWord;
375 uint64_t *pSrc64;
376 uint64_t *pDst64;
377 uint8_t *p_Src8;
378 uint8_t *p_Dst8;
379
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.
385 */
386 while((PTR_TO_UINT(p_Src8) & 7) && size) /* (pSrc mod 8) > 0 and size > 0 */
387 {
388 *p_Dst8++ = *p_Src8++;
389 size--;
390 }
391
392 /* align destination (possibly disaligning source)*/
393 while((PTR_TO_UINT(p_Dst8) & 7) && size) /* (pDst mod 8) > 0 and size > 0 */
394 {
395 *p_Dst8++ = *p_Src8++;
396 size--;
397 }
398
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;
402
403
404 if (leftAlign == 0)
405 {
406 /* source is also aligned */
407 pSrc64 = (uint64_t*)(p_Src8);
408 pDst64 = (uint64_t*)(p_Dst8);
409 while (size >> 3) /* size >= 8 */
410 {
411 *pDst64++ = *pSrc64++;
412 size -= 8;
413 }
414 p_Src8 = (uint8_t*)(pSrc64);
415 p_Dst8 = (uint8_t*)(pDst64);
416 }
417 else
418 {
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 */
424 {
425 currWord = *pSrc64;
426 *pDst64 = (lastWord << leftAlign) | (currWord >> rightAlign);
427 lastWord = currWord;
428 pSrc64++;
429 pDst64++;
430 size -= 8;
431 }
432 p_Dst8 = (uint8_t*)(pDst64);
433 p_Src8 = (uint8_t*)(pSrc64) - 8 + (leftAlign >> 3);
434 }
435
436 /* complete the left overs */
437 while (size--)
438 *p_Dst8++ = *p_Src8++;
439
440 return pDst;
441 }
442
MemSet32(void * pDst,uint8_t val,uint32_t size)443 void * MemSet32(void* pDst, uint8_t val, uint32_t size)
444 {
445 uint32_t val32;
446 uint32_t *p_Dst32;
447 uint8_t *p_Dst8;
448
449 p_Dst8 = (uint8_t*)(pDst);
450
451 /* generate four 8-bit val's in 32-bit container */
452 val32 = (uint32_t) val;
453 val32 |= (val32 << 8);
454 val32 |= (val32 << 16);
455
456 /* align destination to 32 */
457 while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
458 {
459 *p_Dst8++ = val;
460 size--;
461 }
462
463 /* 32-bit chunks */
464 p_Dst32 = (uint32_t*)(p_Dst8);
465 while (size >> 2) /* size >= 4 */
466 {
467 *p_Dst32++ = val32;
468 size -= 4;
469 }
470
471 /* complete the leftovers */
472 p_Dst8 = (uint8_t*)(p_Dst32);
473 while (size--)
474 *p_Dst8++ = val;
475
476 return pDst;
477 }
478
IOMemSet32(void * pDst,uint8_t val,uint32_t size)479 void * IOMemSet32(void* pDst, uint8_t val, uint32_t size)
480 {
481 uint32_t val32;
482 uint32_t *p_Dst32;
483 uint8_t *p_Dst8;
484
485 p_Dst8 = (uint8_t*)(pDst);
486
487 /* generate four 8-bit val's in 32-bit container */
488 val32 = (uint32_t) val;
489 val32 |= (val32 << 8);
490 val32 |= (val32 << 16);
491
492 /* align destination to 32 */
493 while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
494 {
495 WRITE_UINT8(*p_Dst8, val);
496 p_Dst8++;
497 size--;
498 }
499
500 /* 32-bit chunks */
501 p_Dst32 = (uint32_t*)(p_Dst8);
502 while (size >> 2) /* size >= 4 */
503 {
504 WRITE_UINT32(*p_Dst32, val32);
505 p_Dst32++;
506 size -= 4;
507 }
508
509 /* complete the leftovers */
510 p_Dst8 = (uint8_t*)(p_Dst32);
511 while (size--)
512 {
513 WRITE_UINT8(*p_Dst8, val);
514 p_Dst8++;
515 }
516
517 return pDst;
518 }
519
MemSet64(void * pDst,uint8_t val,uint32_t size)520 void * MemSet64(void* pDst, uint8_t val, uint32_t size)
521 {
522 uint64_t val64;
523 uint64_t *pDst64;
524 uint8_t *p_Dst8;
525
526 p_Dst8 = (uint8_t*)(pDst);
527
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);
534
535 /* align destination to 64 */
536 while((PTR_TO_UINT(p_Dst8) & 7) && size) /* (pDst mod 8) > 0 and size > 0 */
537 {
538 *p_Dst8++ = val;
539 size--;
540 }
541
542 /* 64-bit chunks */
543 pDst64 = (uint64_t*)(p_Dst8);
544 while (size >> 4) /* size >= 8 */
545 {
546 *pDst64++ = val64;
547 size -= 8;
548 }
549
550 /* complete the leftovers */
551 p_Dst8 = (uint8_t*)(pDst64);
552 while (size--)
553 *p_Dst8++ = val;
554
555 return pDst;
556 }
557
MemDisp(uint8_t * p,int size)558 void MemDisp(uint8_t *p, int size)
559 {
560 uint32_t space = (uint32_t)(PTR_TO_UINT(p) & 0x3);
561 uint8_t *p_Limit;
562
563 if (space)
564 {
565 p_Limit = (p - space + 4);
566
567 XX_Print("0x%08X: ", (p - space));
568
569 while (space--)
570 {
571 XX_Print("--");
572 }
573 while (size && (p < p_Limit))
574 {
575 XX_Print("%02x", *(uint8_t*)p);
576 size--;
577 p++;
578 }
579
580 XX_Print(" ");
581 p_Limit += 12;
582
583 while ((size > 3) && (p < p_Limit))
584 {
585 XX_Print("%08x ", *(uint32_t*)p);
586 size -= 4;
587 p += 4;
588 }
589 XX_Print("\r\n");
590 }
591
592 while (size > 15)
593 {
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));
597 size -= 16;
598 p += 16;
599 }
600
601 if (size)
602 {
603 XX_Print("0x%08X: ", p);
604
605 while (size > 3)
606 {
607 XX_Print("%08x ", *(uint32_t *)p);
608 size -= 4;
609 p += 4;
610 }
611 while (size)
612 {
613 XX_Print("%02x", *(uint8_t *)p);
614 size--;
615 p++;
616 }
617
618 XX_Print("\r\n");
619 }
620 }
621