sys/contrib/ncsw/etc/memcpy.c

   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
  40 static 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
  69 static 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
 104 void * 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
 178 void * 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
 257 void * 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
 335 void * 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
 414 void * 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
 488 void * 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
 524 void * 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
 565 void * 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
 603 void 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 }