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
39void * 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
49void * 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
59void * 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
133void * 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
212void * 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
290void * 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
369void * 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
443void * 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
479void * 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
520void * 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
558void 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