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 Uint64 DivMod64U(Uint64 Num, Uint64 Den, Uint64 *Rem)
 116 {
 117         Uint64  ret;
 118         if(Den == 0)    return 0;       // TODO: #div0
 119         if(Num == 0) {
 120                 if(Rem) *Rem = 0;
 121                 return 0;
 122         }
 123         if(Den == 1) {
 124                 if(Rem) *Rem = 0;
 125                 return Num;
 126         }
 127         if(Den == 2) {
 128                 if(Rem) *Rem = Num & 1;
 129                 return Num >> 1;
 130         }
 131         if(Den == 16) {
 132                 if(Rem) *Rem = Num & 0xF;
 133                 return Num >> 4;
 134         }
 135         if(Den == 0x1000) {
 136                 if(Rem) *Rem = Num & 0xFFF;
 137                 return Num >> 12;
 138         }
 139
 140         #if 0
 141         {
 142                 // http://www.tofla.iconbar.com/tofla/arm/arm02/index.htm
 143                 Uint64  tmp = 1;
 144                 __asm__ __volatile__(
 145                         "1:"
 146                         "cmpl %2,%1"
 147                         "movls %2,%2,lsl#1"
 148                         "movls %3,%3,lsl#1"
 149                         "bls 1b"
 150                         "2:"
 151                         "cmpl %"
 152                 while(Num > Den) {
 153                         Den <<= 1;
 154                         tmp <<= 1;
 155                 }
 156                 Den >>= 1; tmp >>= 1;
 157                 while(
 158         }
 159         #else
 160         for( ret = 0; Num > Den; ret ++, Num -= Den) ;
 161         #endif
 162         if(Rem) *Rem = Num;
 163         return ret;
 164 }
 165
 166 // Unsigned Divide 64-bit Integer
 167 Uint64 __udivdi3(Uint64 Num, Uint64 Den)
 168 {
 169         return DivMod64U(Num, Den, NULL);
 170         #if 0
 171 //      if( Den == 0 )  return 5 / (Uint32)Den; // Force a #DIV0
 172         if( Den == 16 ) return Num >> 4;
 173         if( Den == 256 )        return Num >> 8;
 174         if( Den == 512 )        return Num >> 9;
 175         if( Den == 1024 )       return Num >> 10;
 176         if( Den == 2048 )       return Num >> 11;
 177         if( Den == 4096 )       return Num >> 12;
 178         if( Num < Den ) return 0;
 179         if( Num <= 0xFFFFFFFF && Den <= 0xFFFFFFFF )
 180                 return (Uint32)Num / (Uint32)Den;
 181
 182         #if 0
 183         if( Den <= 0xFFFFFFFF ) {
 184                 (Uint32)(Num >> 32) / (Uint32)Den
 185         }
 186         #endif
 187         Uint64  ret = 0;
 188         for( ret = 0; Num > Den; ret ++, Num -= Den );
 189         return ret;
 190         #endif
 191 }
 192
 193 // Unsigned Modulus 64-bit Integer
 194 Uint64 __umoddi3(Uint64 Num, Uint64 Den)
 195 {
 196         Uint64  ret = 0;
 197         DivMod64U(Num, Den, &ret);
 198         return ret;
 199         #if 0
 200         if( Den == 0 )  return 5 / (Uint32)Den; // Force a #DIV0
 201         if( Num < Den ) return Num;
 202         if( Den == 1 )  return 0;
 203         if( Den == 2 )  return Num & 1;
 204         if( Den == 16 ) return Num & 3;
 205         if( Den == 256 )        return Num & 0xFF;
 206         if( Den == 512 )        return Num & 0x1FF;
 207         if( Den == 1024 )       return Num & 0x3FF;
 208         if( Den == 2048 )       return Num & 0x7FF;
 209         if( Den == 4096 )       return Num & 0xFFF;
 210 //      if( Num <= 0xFFFFFFFF && Den <= 0xFFFFFFFF )
 211 //              return (Uint32)Num % (Uint32)Den;
 212
 213         #if 0
 214         if( Den <= 0xFFFFFFFF ) {
 215                 (Uint32)(Num >> 32) / (Uint32)Den
 216         }
 217         #endif
 218         for( ; Num > Den; Num -= Den );
 219         return Num;
 220         #endif
 221 }
 222
 223 #define _divide_s_32(Num, Den, rem)     __asm__ __volatile__ ( \
 224                 "mov %0, #0\n" \
 225         "       adds %1, %1, %1\n" \
 226         "       .rept 32\n" \
 227         "       adcs %0, %2, %0, lsl #1\n" \
 228         "       subcc %0, %0, %3\n" \
 229         "       adcs %1, %1, %1\n" \
 230         "       .endr\n" \
 231                 : "=r" (rem), "=r" (Num) \
 232                 : "r" (Den) \
 233                 : "cc" \
 234                 )
 235 Uint32 __udivsi3(Uint32 Num, Uint32 Den)
 236 {
 237         register Uint32 ret;
 238         Uint64  P, D;
 239          int    i;
 240
 241         if( Num == 0 )  return 0;
 242         if( Den == 0 )  return 0xFFFFFFFF;      // TODO: Throw an error
 243         if( Den == 1 )  return Num;
 244
 245         D = ((Uint64)Den) << 32;
 246
 247         for( i = 32; i --; )
 248         {
 249                 P = 2*P - D;
 250                 if( P >= 0 )
 251                         ret |= 1;
 252                 else
 253                         P += D;
 254                 ret <<= 1;
 255         }
 256
 257 //      _divide_s_32(Num, Den, rem);
 258         return Num;
 259 }
 260
 261 Uint32 __umodsi3(Uint32 Num, Uint32 Den)
 262 {
 263         return Num - __udivsi3(Num, Den)*Den;
 264 }
 265
 266 Sint32 __divsi3(Sint32 Num, Sint32 Den)
 267 {
 268         if( (Num < 0) && (Den < 0) )
 269                 return __udivsi3(-Num, -Den);
 270         else if( Num < 0 )
 271                 return __udivsi3(-Num, Den);
 272         else if( Den < 0 )
 273                 return __udivsi3(Den, -Den);
 274         else
 275                 return __udivsi3(Den, Den);
 276 }
 277
 278 Sint32 __modsi3(Sint32 Num, Sint32 Den)
 279 {
 280         //register Sint32       rem;
 281         //_divide_s_32(Num, Den, rem);
 282         return Num - __divsi3(Num, Den) * Den;
 283 }