Kernel/arch/armv7/lib.c

   1 /*
   2  * Acess2 ARM7 Port
   3  *
   4  * lib.c - Library Functions
   5  */
   6 #include <acess.h>
   7
   8 // === PROTOTYPES ===
   9 Uint64  __divmod64(Uint64 Num, Uint64 Den, Uint64 *Rem);
  10 Uint64  __udivdi3(Uint64 Num, Uint64 Den);
  11 Uint64  __umoddi3(Uint64 Num, Uint64 Den);
  12 Uint32  __udivsi3(Uint32 Num, Uint32 Den);
  13 Uint32  __umodsi3(Uint32 Num, Uint32 Den);
  14 Sint32  __divsi3(Sint32 Num, Sint32 Den);
  15 Sint32  __modsi3(Sint32 Num, Sint32 Den);
  16
  17 // === CODE ===
  18 void *memcpy(void *_dest, const void *_src, size_t _length)
  19 {
  20         Uint32  *dst;
  21         const Uint32    *src;
  22         Uint8   *dst8 = _dest;
  23         const Uint8     *src8 = _src;
  24
  25         // Handle small copies / Non-aligned
  26         if( _length < 4 || ((tVAddr)_dest & 3) != ((tVAddr)_src & 3) )
  27         {
  28                 for( ; _length--; dst8++,src8++ )
  29                         *dst8 = *src8;
  30                 return _dest;
  31         }
  32
  33         // Force alignment
  34         while( (tVAddr)dst8 & 3 ) *dst8 ++ = *src8++;
  35         dst = (void *)dst8;     src = (void *)src8;
  36
  37         // DWORD copies
  38         for( ; _length > 3; _length -= 4)
  39                 *dst++ = *src++;
  40
  41         // Trailing bytes
  42         dst8 = (void*)dst;      src8 = (void*)src;
  43         for( ; _length; _length -- )
  44                 *dst8 ++ = *src8 ++;
  45
  46         return _dest;
  47 }
  48
  49 int memcmp(const void *_m1, const void *_m2, size_t _length)
  50 {
  51         const Uint32    *m1, *m2;
  52         const Uint8     *m1_8 = _m1, *m2_8 = _m2;
  53
  54         // Handle small copies / Non-aligned
  55         if( _length < 4 || ((tVAddr)_m1 & 3) != ((tVAddr)_m1 & 3) )
  56         {
  57                 for( ; _length--; m1_8++,m2_8++ ) {
  58                         if(*m1_8 != *m2_8)      return *m1_8 - *m2_8;
  59                 }
  60                 return 0;
  61         }
  62
  63         // Force alignment
  64         for( ; (tVAddr)m1_8 & 3; m1_8 ++, m2_8 ++) {
  65                 if(*m1_8 != *m2_8)      return *m1_8 - *m2_8;
  66         }
  67         m1 = (void *)m1_8;      m2 = (void *)m2_8;
  68
  69         // DWORD copies
  70         for( ; _length > 3; _length -= 4, m1++, m2++)
  71                 if(*m1 != *m2)  return *m1 - *m2;
  72
  73         // Trailing bytes
  74         m1_8 = (void*)m1;       m2_8 = (void*)m2;
  75         for( ; _length; _length --, m1_8++, m2_8++ )
  76                 if(*m1_8 != *m2_8)      return *m1_8 - *m2_8;
  77
  78         return 0;
  79 }
  80
  81 void *memset(void *_dest, int _value, size_t _length)
  82 {
  83         Uint32  *dst, val32;
  84         Uint8   *dst8 = _dest;
  85
  86         _value = (Uint8)_value;
  87
  88         // Handle small copies / Non-aligned
  89         if( _length < 4 )
  90         {
  91                 for( ; _length--; dst8++ )
  92                         *dst8 = _value;
  93                 return _dest;
  94         }
  95
  96         val32 = _value;
  97         val32 |= val32 << 8;
  98         val32 |= val32 << 16;
  99
 100         // Force alignment
 101         while( (tVAddr)dst8 & 3 ) *dst8 ++ = _value;
 102         dst = (void *)dst8;
 103
 104         // DWORD copies
 105         for( ; _length > 3; _length -= 4)
 106                 *dst++ = val32;
 107
 108         // Trailing bytes
 109         dst8 = (void*)dst;
 110         for( ; _length; _length -- )
 111                 *dst8 ++ = _value;
 112
 113         return _dest;
 114 }
 115
 116 Uint64 __divmod64(Uint64 Num, Uint64 Den, Uint64 *Rem)
 117 {
 118         Uint64  ret, add;
 119
 120         ret = 0;
 121         add = 1;
 122
 123         // Find what power of two times Den is > Num
 124         while( Num >= Den )
 125         {
 126                 Den <<= 1;
 127                 add <<= 1;
 128         }
 129
 130         // Search backwards
 131         while( add > 1 )
 132         {
 133                 add >>= 1;
 134                 Den >>= 1;
 135                 // If the numerator is > Den, subtract and add to return value
 136                 if( Num > Den )
 137                 {
 138                         ret += add;
 139                         Num -= Den;
 140                 }
 141         }
 142         if(Rem) *Rem = Num;
 143         return ret;
 144 }
 145
 146 Uint64 DivMod64U(Uint64 Num, Uint64 Den, Uint64 *Rem)
 147 {
 148         Uint64  ret;
 149         if(Den == 0)    return 0;       // TODO: #div0
 150         if(Num == 0) {
 151                 if(Rem) *Rem = 0;
 152                 return 0;
 153         }
 154         if(Den == 1) {
 155                 if(Rem) *Rem = 0;
 156                 return Num;
 157         }
 158         if(Den == 2) {
 159                 if(Rem) *Rem = Num & 1;
 160                 return Num >> 1;
 161         }
 162         if(Den == 16) {
 163                 if(Rem) *Rem = Num & 0xF;
 164                 return Num >> 4;
 165         }
 166         if(Den == 32) {
 167                 if(Rem) *Rem = Num & 0x1F;
 168                 return Num >> 5;
 169         }
 170         if(Den == 0x1000) {
 171                 if(Rem) *Rem = Num & 0xFFF;
 172                 return Num >> 12;
 173         }
 174
 175         #if 0
 176         {
 177                 // http://www.tofla.iconbar.com/tofla/arm/arm02/index.htm
 178                 Uint64  tmp = 1;
 179                 __asm__ __volatile__(
 180                         "1:"
 181                         "cmpl %2,%1"
 182                         "movls %2,%2,lsl#1"
 183                         "movls %3,%3,lsl#1"
 184                         "bls 1b"
 185                         "2:"
 186                         "cmpl %"
 187                 while(Num > Den) {
 188                         Den <<= 1;
 189                         tmp <<= 1;
 190                 }
 191                 Den >>= 1; tmp >>= 1;
 192                 while(
 193         }
 194         if(Rem) *Rem = Num;
 195         return ret;
 196         #elif 0
 197         for( ret = 0; Num > Den; ret ++, Num -= Den) ;
 198         if(Rem) *Rem = Num;
 199         return ret;
 200         #else
 201         ret = __divmod64(Num, Den, Rem);
 202         return ret;
 203         #endif
 204 }
 205
 206 // Unsigned Divide 64-bit Integer
 207 Uint64 __udivdi3(Uint64 Num, Uint64 Den)
 208 {
 209         return DivMod64U(Num, Den, NULL);
 210         #if 0
 211 //      if( Den == 0 )  return 5 / (Uint32)Den; // Force a #DIV0
 212         if( Den == 16 ) return Num >> 4;
 213         if( Den == 256 )        return Num >> 8;
 214         if( Den == 512 )        return Num >> 9;
 215         if( Den == 1024 )       return Num >> 10;
 216         if( Den == 2048 )       return Num >> 11;
 217         if( Den == 4096 )       return Num >> 12;
 218         if( Num < Den ) return 0;
 219         if( Num <= 0xFFFFFFFF && Den <= 0xFFFFFFFF )
 220                 return (Uint32)Num / (Uint32)Den;
 221
 222         #if 0
 223         if( Den <= 0xFFFFFFFF ) {
 224                 (Uint32)(Num >> 32) / (Uint32)Den
 225         }
 226         #endif
 227         Uint64  ret = 0;
 228         for( ret = 0; Num > Den; ret ++, Num -= Den );
 229         return ret;
 230         #endif
 231 }
 232
 233 // Unsigned Modulus 64-bit Integer
 234 Uint64 __umoddi3(Uint64 Num, Uint64 Den)
 235 {
 236         Uint64  ret = 0;
 237         DivMod64U(Num, Den, &ret);
 238         return ret;
 239         #if 0
 240         if( Den == 0 )  return 5 / (Uint32)Den; // Force a #DIV0
 241         if( Num < Den ) return Num;
 242         if( Den == 1 )  return 0;
 243         if( Den == 2 )  return Num & 1;
 244         if( Den == 16 ) return Num & 3;
 245         if( Den == 256 )        return Num & 0xFF;
 246         if( Den == 512 )        return Num & 0x1FF;
 247         if( Den == 1024 )       return Num & 0x3FF;
 248         if( Den == 2048 )       return Num & 0x7FF;
 249         if( Den == 4096 )       return Num & 0xFFF;
 250 //      if( Num <= 0xFFFFFFFF && Den <= 0xFFFFFFFF )
 251 //              return (Uint32)Num % (Uint32)Den;
 252
 253         #if 0
 254         if( Den <= 0xFFFFFFFF ) {
 255                 (Uint32)(Num >> 32) / (Uint32)Den
 256         }
 257         #endif
 258         for( ; Num > Den; Num -= Den );
 259         return Num;
 260         #endif
 261 }
 262
 263 #define _divide_s_32(Num, Den, rem)     __asm__ __volatile__ ( \
 264                 "mov %0, #0\n" \
 265         "       adds %1, %1, %1\n" \
 266         "       .rept 32\n" \
 267         "       adcs %0, %2, %0, lsl #1\n" \
 268         "       subcc %0, %0, %3\n" \
 269         "       adcs %1, %1, %1\n" \
 270         "       .endr\n" \
 271                 : "=r" (rem), "=r" (Num) \
 272                 : "r" (Den) \
 273                 : "cc" \
 274                 )
 275 Uint32 __udivsi3(Uint32 Num, Uint32 Den)
 276 {
 277         register Uint32 ret;
 278         Uint64  P, D;
 279          int    i;
 280
 281         if( Num == 0 )  return 0;
 282         if( Den == 0 )  return 0xFFFFFFFF;      // TODO: Throw an error
 283         if( Den == 1 )  return Num;
 284
 285         D = ((Uint64)Den) << 32;
 286
 287         for( i = 32; i --; )
 288         {
 289                 P = 2*P - D;
 290                 if( P >= 0 )
 291                         ret |= 1;
 292                 else
 293                         P += D;
 294                 ret <<= 1;
 295         }
 296
 297 //      _divide_s_32(Num, Den, rem);
 298         return Num;
 299 }
 300
 301 Uint32 __umodsi3(Uint32 Num, Uint32 Den)
 302 {
 303         return Num - __udivsi3(Num, Den)*Den;
 304 }
 305
 306 Sint32 __divsi3(Sint32 Num, Sint32 Den)
 307 {
 308         if( (Num < 0) && (Den < 0) )
 309                 return __udivsi3(-Num, -Den);
 310         else if( Num < 0 )
 311                 return __udivsi3(-Num, Den);
 312         else if( Den < 0 )
 313                 return __udivsi3(Den, -Den);
 314         else
 315                 return __udivsi3(Den, Den);
 316 }
 317
 318 Sint32 __modsi3(Sint32 Num, Sint32 Den)
 319 {
 320         //register Sint32       rem;
 321         //_divide_s_32(Num, Den, rem);
 322         return Num - __divsi3(Num, Den) * Den;
 323 }