1/* Copyright (c) 2008-2011 Freescale Semiconductor, Inc.
2 * All rights reserved.
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#include "std_ext.h"
35#include "xx_ext.h"
36#include "memcpy_ext.h"
37
38
39#ifdef CORE_8BIT_ACCESS_ERRATA
40static void MY_MY_WRITE_UINT8(uint8_t *addr, uint8_t val)
41{
42    uint32_t newAddr, newVal;
43    newAddr = (uint32_t)addr & ~0x3L;
44    switch ((uint32_t)addr%4)
45    {
46    case (0):
47        newVal = GET_UINT32(*(uint32_t*)newAddr);
48        newVal = (newVal & 0x00ffffff) | (((uint32_t)val)<<24);
49        WRITE_UINT32(*(uint32_t*)newAddr, newVal);
50        break;
51    case (1):
52         newVal = GET_UINT32(*(uint32_t*)newAddr);
53        newVal = (newVal & 0xff00ffff) | (((uint32_t)val)<<16);
54        WRITE_UINT32(*(uint32_t*)newAddr, newVal);
55        break;
56    case (2):
57        newVal = GET_UINT32(*(uint32_t*)newAddr);
58        newVal = (newVal & 0xffff00ff) | (((uint32_t)val)<<8);
59        WRITE_UINT32(*(uint32_t*)newAddr, newVal);
60        break;
61    case (3):
62        newVal = GET_UINT32(*(uint32_t*)newAddr);
63        newVal = (newVal & 0xffffff00) | val;
64        WRITE_UINT32(*(uint32_t*)newAddr, newVal);
65        break;
66    }
67}
68
69static uint8_t MY_MY_GET_UINT8(uint8_t *addr)
70{
71    uint32_t newAddr, newVal=0;
72    newAddr = (uint32_t)addr & ~0x3L;
73    switch ((uint32_t)addr%4)
74    {
75    case (0):
76        newVal = GET_UINT32(*(uint32_t*)newAddr);
77        newVal = (newVal & 0xff000000)>>24;
78        break;
79    case (1):
80        newVal = GET_UINT32(*(uint32_t*)newAddr);
81        newVal = (newVal & 0x00ff0000)>>16;
82        break;
83    case (2):
84        newVal = GET_UINT32(*(uint32_t*)newAddr);
85        newVal = (newVal & 0x0000ff00)>>8;
86        break;
87    case (3):
88        newVal = GET_UINT32(*(uint32_t*)newAddr);
89        newVal = (newVal & 0x000000ff);
90        break;
91    }
92
93    return (uint8_t)newVal;
94}
95
96#define MY_WRITE_UINT8(addr,val) MY_MY_WRITE_UINT8(&addr,val)
97#define MY_GET_UINT8(addr) MY_MY_GET_UINT8(&addr)
98#else
99#define MY_WRITE_UINT8 WRITE_UINT8
100#define MY_GET_UINT8   GET_UINT8
101#endif /* CORE_8BIT_ACCESS_ERRATA */
102
103
104void * MemCpy32(void* pDst,void* pSrc, uint32_t size)
105{
106    uint32_t leftAlign;
107    uint32_t rightAlign;
108    uint32_t lastWord;
109    uint32_t currWord;
110    uint32_t *p_Src32;
111    uint32_t *p_Dst32;
112    uint8_t  *p_Src8;
113    uint8_t  *p_Dst8;
114
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.
120     */
121    while((PTR_TO_UINT(p_Src8) & 3) && size) /* (pSrc mod 4) > 0 and size > 0 */
122    {
123        *p_Dst8++ = *p_Src8++;
124        size--;
125    }
126
127    /* align destination (possibly disaligning source)*/
128    while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
129    {
130        *p_Dst8++ = *p_Src8++;
131        size--;
132    }
133
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;
137
138
139    if (leftAlign == 0)
140    {
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 */
145        {
146            *p_Dst32++ = *p_Src32++;
147            size -= 4;
148        }
149        p_Src8 = (uint8_t*)(p_Src32);
150        p_Dst8 = (uint8_t*)(p_Dst32);
151    }
152    else
153    {
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 */
159        {
160            currWord = *p_Src32;
161            *p_Dst32 = (lastWord << leftAlign) | (currWord >> rightAlign);
162            lastWord = currWord;
163            p_Src32++;
164            p_Dst32++;
165            size -= 4;
166        }
167        p_Dst8 = (uint8_t*)(p_Dst32);
168        p_Src8 = (uint8_t*)(p_Src32) - 4 + (leftAlign >> 3);
169    }
170
171    /* complete the left overs */
172    while (size--)
173        *p_Dst8++ = *p_Src8++;
174
175    return pDst;
176}
177
178void * IO2IOCpy32(void* pDst,void* pSrc, uint32_t size)
179{
180    uint32_t leftAlign;
181    uint32_t rightAlign;
182    uint32_t lastWord;
183    uint32_t currWord;
184    uint32_t *p_Src32;
185    uint32_t *p_Dst32;
186    uint8_t  *p_Src8;
187    uint8_t  *p_Dst8;
188
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.
194     */
195    while((PTR_TO_UINT(p_Src8) & 3) && size) /* (pSrc mod 4) > 0 and size > 0 */
196    {
197        MY_WRITE_UINT8(*p_Dst8, MY_GET_UINT8(*p_Src8));
198        p_Dst8++;p_Src8++;
199        size--;
200    }
201
202    /* align destination (possibly disaligning source)*/
203    while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
204    {
205        MY_WRITE_UINT8(*p_Dst8, MY_GET_UINT8(*p_Src8));
206        p_Dst8++;p_Src8++;
207        size--;
208    }
209
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;
213
214    if (leftAlign == 0)
215    {
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 */
220        {
221            WRITE_UINT32(*p_Dst32, GET_UINT32(*p_Src32));
222            p_Dst32++;p_Src32++;
223            size -= 4;
224        }
225        p_Src8 = (uint8_t*)(p_Src32);
226        p_Dst8 = (uint8_t*)(p_Dst32);
227    }
228    else
229    {
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);
234        p_Src32++;
235        while(size >> 3) /* size >= 8 */
236        {
237            currWord = GET_UINT32(*p_Src32);
238            WRITE_UINT32(*p_Dst32, (lastWord << leftAlign) | (currWord >> rightAlign));
239            lastWord = currWord;
240            p_Src32++;p_Dst32++;
241            size -= 4;
242        }
243        p_Dst8 = (uint8_t*)(p_Dst32);
244        p_Src8 = (uint8_t*)(p_Src32) - 4 + (leftAlign >> 3);
245    }
246
247    /* complete the left overs */
248    while (size--)
249    {
250        MY_WRITE_UINT8(*p_Dst8, MY_GET_UINT8(*p_Src8));
251        p_Dst8++;p_Src8++;
252    }
253
254    return pDst;
255}
256
257void * Mem2IOCpy32(void* pDst,void* pSrc, uint32_t size)
258{
259    uint32_t leftAlign;
260    uint32_t rightAlign;
261    uint32_t lastWord;
262    uint32_t currWord;
263    uint32_t *p_Src32;
264    uint32_t *p_Dst32;
265    uint8_t  *p_Src8;
266    uint8_t  *p_Dst8;
267
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.
273     */
274    while((PTR_TO_UINT(p_Src8) & 3) && size) /* (pSrc mod 4) > 0 and size > 0 */
275    {
276        MY_WRITE_UINT8(*p_Dst8, *p_Src8);
277        p_Dst8++;p_Src8++;
278        size--;
279    }
280
281    /* align destination (possibly disaligning source)*/
282    while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
283    {
284        MY_WRITE_UINT8(*p_Dst8, *p_Src8);
285        p_Dst8++;p_Src8++;
286        size--;
287    }
288
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;
292
293    if (leftAlign == 0)
294    {
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 */
299        {
300            WRITE_UINT32(*p_Dst32, *p_Src32);
301            p_Dst32++;p_Src32++;
302            size -= 4;
303        }
304        p_Src8 = (uint8_t*)(p_Src32);
305        p_Dst8 = (uint8_t*)(p_Dst32);
306    }
307    else
308    {
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 */
314        {
315            currWord = *p_Src32;
316            WRITE_UINT32(*p_Dst32, (lastWord << leftAlign) | (currWord >> rightAlign));
317            lastWord = currWord;
318            p_Src32++;p_Dst32++;
319            size -= 4;
320        }
321        p_Dst8 = (uint8_t*)(p_Dst32);
322        p_Src8 = (uint8_t*)(p_Src32) - 4 + (leftAlign >> 3);
323    }
324
325    /* complete the left overs */
326    while (size--)
327    {
328        MY_WRITE_UINT8(*p_Dst8, *p_Src8);
329        p_Dst8++;p_Src8++;
330    }
331
332    return pDst;
333}
334
335void * IO2MemCpy32(void* pDst,void* pSrc, uint32_t size)
336{
337    uint32_t leftAlign;
338    uint32_t rightAlign;
339    uint32_t lastWord;
340    uint32_t currWord;
341    uint32_t *p_Src32;
342    uint32_t *p_Dst32;
343    uint8_t  *p_Src8;
344    uint8_t  *p_Dst8;
345
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.
351     */
352    while((PTR_TO_UINT(p_Src8) & 3) && size) /* (pSrc mod 4) > 0 and size > 0 */
353    {
354        *p_Dst8 = MY_GET_UINT8(*p_Src8);
355        p_Dst8++;p_Src8++;
356        size--;
357    }
358
359    /* align destination (possibly disaligning source)*/
360    while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
361    {
362        *p_Dst8 = MY_GET_UINT8(*p_Src8);
363        p_Dst8++;p_Src8++;
364        size--;
365    }
366
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;
370
371    if (leftAlign == 0)
372    {
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 */
377        {
378            *p_Dst32 = GET_UINT32(*p_Src32);
379            p_Dst32++;p_Src32++;
380            size -= 4;
381        }
382        p_Src8 = (uint8_t*)(p_Src32);
383        p_Dst8 = (uint8_t*)(p_Dst32);
384    }
385    else
386    {
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);
391        p_Src32++;
392        while(size >> 3) /* size >= 8 */
393        {
394            currWord = GET_UINT32(*p_Src32);
395            *p_Dst32 = (lastWord << leftAlign) | (currWord >> rightAlign);
396            lastWord = currWord;
397            p_Src32++;p_Dst32++;
398            size -= 4;
399        }
400        p_Dst8 = (uint8_t*)(p_Dst32);
401        p_Src8 = (uint8_t*)(p_Src32) - 4 + (leftAlign >> 3);
402    }
403
404    /* complete the left overs */
405    while (size--)
406    {
407        *p_Dst8 = MY_GET_UINT8(*p_Src8);
408        p_Dst8++;p_Src8++;
409    }
410
411    return pDst;
412}
413
414void * MemCpy64(void* pDst,void* pSrc, uint32_t size)
415{
416    uint32_t leftAlign;
417    uint32_t rightAlign;
418    uint64_t lastWord;
419    uint64_t currWord;
420    uint64_t *pSrc64;
421    uint64_t *pDst64;
422    uint8_t  *p_Src8;
423    uint8_t  *p_Dst8;
424
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.
430     */
431    while((PTR_TO_UINT(p_Src8) & 7) && size) /* (pSrc mod 8) > 0 and size > 0 */
432    {
433        *p_Dst8++ = *p_Src8++;
434        size--;
435    }
436
437    /* align destination (possibly disaligning source)*/
438    while((PTR_TO_UINT(p_Dst8) & 7) && size) /* (pDst mod 8) > 0 and size > 0 */
439    {
440        *p_Dst8++ = *p_Src8++;
441        size--;
442    }
443
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;
447
448
449    if (leftAlign == 0)
450    {
451        /* source is also aligned */
452        pSrc64 = (uint64_t*)(p_Src8);
453        pDst64 = (uint64_t*)(p_Dst8);
454        while (size >> 3) /* size >= 8 */
455        {
456            *pDst64++ = *pSrc64++;
457            size -= 8;
458        }
459        p_Src8 = (uint8_t*)(pSrc64);
460        p_Dst8 = (uint8_t*)(pDst64);
461    }
462    else
463    {
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 */
469        {
470            currWord = *pSrc64;
471            *pDst64 = (lastWord << leftAlign) | (currWord >> rightAlign);
472            lastWord = currWord;
473            pSrc64++;
474            pDst64++;
475            size -= 8;
476        }
477        p_Dst8 = (uint8_t*)(pDst64);
478        p_Src8 = (uint8_t*)(pSrc64) - 8 + (leftAlign >> 3);
479    }
480
481    /* complete the left overs */
482    while (size--)
483        *p_Dst8++ = *p_Src8++;
484
485    return pDst;
486}
487
488void * MemSet32(void* pDst, uint8_t val, uint32_t size)
489{
490    uint32_t val32;
491    uint32_t *p_Dst32;
492    uint8_t  *p_Dst8;
493
494    p_Dst8 = (uint8_t*)(pDst);
495
496    /* generate four 8-bit val's in 32-bit container */
497    val32  = (uint32_t) val;
498    val32 |= (val32 <<  8);
499    val32 |= (val32 << 16);
500
501    /* align destination to 32 */
502    while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
503    {
504        *p_Dst8++ = val;
505        size--;
506    }
507
508    /* 32-bit chunks */
509    p_Dst32 = (uint32_t*)(p_Dst8);
510    while (size >> 2) /* size >= 4 */
511    {
512        *p_Dst32++ = val32;
513        size -= 4;
514    }
515
516    /* complete the leftovers */
517    p_Dst8 = (uint8_t*)(p_Dst32);
518    while (size--)
519        *p_Dst8++ = val;
520
521    return pDst;
522}
523
524void * IOMemSet32(void* pDst, uint8_t val, uint32_t size)
525{
526    uint32_t val32;
527    uint32_t *p_Dst32;
528    uint8_t  *p_Dst8;
529
530    p_Dst8 = (uint8_t*)(pDst);
531
532    /* generate four 8-bit val's in 32-bit container */
533    val32  = (uint32_t) val;
534    val32 |= (val32 <<  8);
535    val32 |= (val32 << 16);
536
537    /* align destination to 32 */
538    while((PTR_TO_UINT(p_Dst8) & 3) && size) /* (pDst mod 4) > 0 and size > 0 */
539    {
540        MY_WRITE_UINT8(*p_Dst8, val);
541        p_Dst8++;
542        size--;
543    }
544
545    /* 32-bit chunks */
546    p_Dst32 = (uint32_t*)(p_Dst8);
547    while (size >> 2) /* size >= 4 */
548    {
549        WRITE_UINT32(*p_Dst32, val32);
550        p_Dst32++;
551        size -= 4;
552    }
553
554    /* complete the leftovers */
555    p_Dst8 = (uint8_t*)(p_Dst32);
556    while (size--)
557    {
558        MY_WRITE_UINT8(*p_Dst8, val);
559        p_Dst8++;
560    }
561
562    return pDst;
563}
564
565void * MemSet64(void* pDst, uint8_t val, uint32_t size)
566{
567    uint64_t val64;
568    uint64_t *pDst64;
569    uint8_t  *p_Dst8;
570
571    p_Dst8 = (uint8_t*)(pDst);
572
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);
579
580    /* align destination to 64 */
581    while((PTR_TO_UINT(p_Dst8) & 7) && size) /* (pDst mod 8) > 0 and size > 0 */
582    {
583        *p_Dst8++ = val;
584        size--;
585    }
586
587    /* 64-bit chunks */
588    pDst64 = (uint64_t*)(p_Dst8);
589    while (size >> 4) /* size >= 8 */
590    {
591        *pDst64++ = val64;
592        size -= 8;
593    }
594
595    /* complete the leftovers */
596    p_Dst8 = (uint8_t*)(pDst64);
597    while (size--)
598        *p_Dst8++ = val;
599
600    return pDst;
601}
602
603void MemDisp(uint8_t *p, int size)
604{
605    uint32_t    space = (uint32_t)(PTR_TO_UINT(p) & 0x3);
606    uint8_t     *p_Limit;
607
608    if (space)
609    {
610        p_Limit = (p - space + 4);
611
612        XX_Print("0x%08X: ", (p - space));
613
614        while (space--)
615        {
616            XX_Print("--");
617        }
618        while (size  && (p < p_Limit))
619        {
620            XX_Print("%02x", *(uint8_t*)p);
621            size--;
622            p++;
623        }
624
625        XX_Print(" ");
626        p_Limit += 12;
627
628        while ((size > 3) && (p < p_Limit))
629        {
630            XX_Print("%08x ", *(uint32_t*)p);
631            size -= 4;
632            p += 4;
633        }
634        XX_Print("\r\n");
635    }
636
637    while (size > 15)
638    {
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));
642        size -= 16;
643        p += 16;
644    }
645
646    if (size)
647    {
648        XX_Print("0x%08X: ", p);
649
650        while (size > 3)
651        {
652            XX_Print("%08x ", *(uint32_t *)p);
653            size -= 4;
654            p += 4;
655        }
656        while (size)
657        {
658            XX_Print("%02x", *(uint8_t *)p);
659            size--;
660            p++;
661        }
662
663        XX_Print("\r\n");
664    }
665}
666