git.mutantstargoat.com Git - psx_test1/blob - src/libc/string.c

   1 #include <string.h>
   2 #include <stdint.h>
   3
   4 static inline void memcpy_words(uint32_t *dest, const uint32_t *src, size_t n)
   5 {
   6         while(n-- > 0) {
   7                 *dest++ = *src++;
   8         }
   9 }
  10
  11 static inline void memcpy_bytes(unsigned char *dest, const unsigned char *src, size_t n)
  12 {
  13         while(n-- > 0) {
  14                 *dest++ = *src++;
  15         }
  16 }
  17
  18 void *memcpy(void *dest, const void *src, size_t n)
  19 {
  20         int pre;
  21         size_t nwords;
  22         int dalign = (uintptr_t)dest & 3;
  23         int salign = (uintptr_t)src & 3;
  24         unsigned char *bdest;
  25         const unsigned char *bsrc;
  26         uint32_t *wdest;
  27         const uint32_t *wsrc;
  28
  29         if(n < 4) {
  30                 memcpy_bytes(dest, src, n);
  31                 return dest;
  32         }
  33
  34         if(dalign != 0 || salign != 0) {
  35                 if(dalign != salign) {
  36                         /* alignment differs, copy bytes */
  37                         memcpy_bytes(dest, src, n);
  38                         return dest;
  39                 }
  40
  41                 /* unaligned but on the same alignment, copy bytes first */
  42                 pre = ~dalign & 3;
  43                 bdest = dest;
  44                 bsrc = src;
  45                 memcpy_bytes(dest, src, pre);
  46
  47                 wdest = (uint32_t*)((char*)dest + pre);
  48                 wsrc = (const uint32_t*)((char*)src + pre);
  49                 n -= pre;
  50         } else {
  51                 /* both aligned */
  52                 wdest = dest;
  53                 wsrc = src;
  54         }
  55
  56         nwords = n >> 2;
  57         if(nwords) {
  58                 memcpy_words(wdest, wsrc, nwords);
  59         }
  60
  61         n &= 3;
  62         if(n) {
  63                 bdest = (unsigned char*)(wdest + nwords);
  64                 bsrc = (const unsigned char*)(wsrc + nwords);
  65                 memcpy_bytes(bdest, bsrc, n);
  66         }
  67         return dest;
  68 }