Kernel/arch/arm7/lib.c

   1 /*
   2  * Acess2 ARM7 Port
   3  *
   4  * lib.c - Library Functions
   5  */
   6 #include <acess.h>
   7
   8 // === PROTOTYPES ===
   9 Uint64  __udivdi3(Uint64 Num, Uint64 Den);
  10 Uint64  __umoddi3(Uint64 Num, Uint64 Den);
  11 Uint32  __udivsi3(Uint32 Num, Uint32 Den);
  12 Uint32  __umodsi3(Uint32 Num, Uint32 Den);
  13 Sint32  __divsi3(Sint32 Num, Sint32 Den);
  14 Sint32  __modsi3(Sint32 Num, Sint32 Den);
  15
  16 // === CODE ===
  17 void *memcpy(void *_dest, const void *_src, size_t _length)
  18 {
  19         Uint32  *dst;
  20         const Uint32    *src;
  21         Uint8   *dst8 = _dest;
  22         const Uint8     *src8 = _src;
  23
  24         // Handle small copies / Non-aligned
  25         if( _length < 4 || ((tVAddr)_dest & 3) != ((tVAddr)_src & 3) )
  26         {
  27                 for( ; _length--; dst8++,src8++ )
  28                         *dst8 = *src8;
  29                 return _dest;
  30         }
  31
  32         // Force alignment
  33         while( (tVAddr)dst8 & 3 ) *dst8 ++ = *src8++;
  34         dst = (void *)dst8;     src = (void *)src8;
  35
  36         // DWORD copies
  37         for( ; _length > 3; _length -= 4)
  38                 *dst++ = *src++;
  39
  40         // Trailing bytes
  41         dst8 = (void*)dst;      src8 = (void*)src;
  42         for( ; _length; _length -- )
  43                 *dst8 ++ = *src8 ++;
  44
  45         return _dest;
  46 }
  47
  48 int memcmp(const void *_m1, const void *_m2, size_t _length)
  49 {
  50         const Uint32    *m1, *m2;
  51         const Uint8     *m1_8 = _m1, *m2_8 = _m2;
  52
  53         // Handle small copies / Non-aligned
  54         if( _length < 4 || ((tVAddr)_m1 & 3) != ((tVAddr)_m1 & 3) )
  55         {
  56                 for( ; _length--; m1_8++,m2_8++ ) {
  57                         if(*m1_8 != *m2_8)      return *m1_8 - *m2_8;
  58                 }
  59                 return 0;
  60         }
  61
  62         // Force alignment
  63         for( ; (tVAddr)m1_8 & 3; m1_8 ++, m2_8 ++) {
  64                 if(*m1_8 != *m2_8)      return *m1_8 - *m2_8;
  65         }
  66         m1 = (void *)m1_8;      m2 = (void *)m2_8;
  67
  68         // DWORD copies
  69         for( ; _length > 3; _length -= 4, m1++, m2++)
  70                 if(*m1 != *m2)  return *m1 - *m2;
  71
  72         // Trailing bytes
  73         m1_8 = (void*)m1;       m2_8 = (void*)m2;
  74         for( ; _length; _length --, m1_8++, m2_8++ )
  75                 if(*m1_8 != *m2_8)      return *m1_8 - *m2_8;
  76
  77         return 0;
  78 }
  79
  80 void *memset(void *_dest, int _value, size_t _length)
  81 {
  82         Uint32  *dst, val32;
  83         Uint8   *dst8 = _dest;
  84
  85         _value = (Uint8)_value;
  86
  87         // Handle small copies / Non-aligned
  88         if( _length < 4 )
  89         {
  90                 for( ; _length--; dst8++ )
  91                         *dst8 = _value;
  92                 return _dest;
  93         }
  94
  95         val32 = _value;
  96         val32 |= val32 << 8;
  97         val32 |= val32 << 16;
  98
  99         // Force alignment
 100         while( (tVAddr)dst8 & 3 ) *dst8 ++ = _value;
 101         dst = (void *)dst8;
 102
 103         // DWORD copies
 104         for( ; _length > 3; _length -= 4)
 105                 *dst++ = val32;
 106
 107         // Trailing bytes
 108         dst8 = (void*)dst;
 109         for( ; _length; _length -- )
 110                 *dst8 ++ = _value;
 111
 112         return _dest;
 113 }
 114
 115 // Unsigned Divide 64-bit Integer
 116 Uint64 __udivdi3(Uint64 Num, Uint64 Den)
 117 {
 118         if( Num == 0 )  return 0;
 119         if( Den == 0 )  return 5 / (Uint32)Den; // Force a #DIV0
 120         if( Den == 1 )  return Num;
 121         if( Den == 2 )  return Num >> 1;
 122         if( Den == 16 ) return Num >> 4;
 123         if( Den == 256 )        return Num >> 8;
 124         if( Den == 512 )        return Num >> 9;
 125         if( Den == 1024 )       return Num >> 10;
 126         if( Den == 2048 )       return Num >> 11;
 127         if( Den == 4096 )       return Num >> 12;
 128         if( Num < Den ) return 0;
 129         if( Num <= 0xFFFFFFFF && Den <= 0xFFFFFFFF )
 130                 return (Uint32)Num / (Uint32)Den;
 131
 132         #if 0
 133         if( Den <= 0xFFFFFFFF ) {
 134                 (Uint32)(Num >> 32) / (Uint32)Den
 135         }
 136         #endif
 137         Uint64  ret = 0;
 138         for( ret = 0; Num > Den; ret ++, Num -= Den );
 139         return ret;
 140 }
 141
 142 // Unsigned Modulus 64-bit Integer
 143 Uint64 __umoddi3(Uint64 Num, Uint64 Den)
 144 {
 145         if( Num == 0 )  return 0;
 146         if( Den == 0 )  return 5 / (Uint32)Den; // Force a #DIV0
 147         if( Den == 1 )  return 0;
 148         if( Den == 2 )  return Num & 1;
 149         if( Den == 16 ) return Num & 3;
 150         if( Den == 256 )        return Num & 0xFF;
 151         if( Den == 512 )        return Num & 0x1FF;
 152         if( Den == 1024 )       return Num & 0x3FF;
 153         if( Den == 2048 )       return Num & 0x7FF;
 154         if( Den == 4096 )       return Num & 0xFFF;
 155         if( Num < Den ) return 0;
 156         if( Num <= 0xFFFFFFFF && Den <= 0xFFFFFFFF )
 157                 return (Uint32)Num % (Uint32)Den;
 158
 159         #if 0
 160         if( Den <= 0xFFFFFFFF ) {
 161                 (Uint32)(Num >> 32) / (Uint32)Den
 162         }
 163         #endif
 164         for( ; Num > Den; Num -= Den );
 165         return Num;
 166 }
 167
 168 #define _divide_s_32(Num, Den, rem)     __asm__ __volatile__ ( \
 169                 "mov %0, #0\n" \
 170         "       adds %1, %1, %1\n" \
 171         "       .rept 32\n" \
 172         "       adcs %0, %2, %0, lsl #1\n" \
 173         "       subcc %0, %0, %3\n" \
 174         "       adcs %1, %1, %1\n" \
 175         "       .endr\n" \
 176                 : "=r" (rem), "=r" (Num) \
 177                 : "r" (Den) \
 178                 : "cc" \
 179                 )
 180 Uint32 __udivsi3(Uint32 Num, Uint32 Den)
 181 {
 182         register Uint32 ret;
 183         Uint64  P, D;
 184          int    i;
 185
 186         if( Num == 0 )  return 0;
 187         if( Den == 0 )  return 0xFFFFFFFF;      // TODO: Throw an error
 188         if( Den == 1 )  return Num;
 189
 190         D = ((Uint64)Den) << 32;
 191
 192         for( i = 32; i --; )
 193         {
 194                 P = 2*P - D;
 195                 if( P >= 0 )
 196                         ret |= 1;
 197                 else
 198                         P += D;
 199                 ret <<= 1;
 200         }
 201
 202 //      _divide_s_32(Num, Den, rem);
 203         return Num;
 204 }
 205
 206 Uint32 __umodsi3(Uint32 Num, Uint32 Den)
 207 {
 208         return Num - __udivsi3(Num, Den)*Den;
 209 }
 210
 211 Sint32 __divsi3(Sint32 Num, Sint32 Den)
 212 {
 213         if( (Num < 0) && (Den < 0) )
 214                 return __udivsi3(-Num, -Den);
 215         else if( Num < 0 )
 216                 return __udivsi3(-Num, Den);
 217         else if( Den < 0 )
 218                 return __udivsi3(Den, -Den);
 219         else
 220                 return __udivsi3(Den, Den);
 221 }
 222
 223 Sint32 __modsi3(Sint32 Num, Sint32 Den)
 224 {
 225         //register Sint32       rem;
 226         //_divide_s_32(Num, Den, rem);
 227         return Num - __divsi3(Num, Den) * Den;
 228 }