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( Den == 0 )  return 5 / (Uint32)Den; // Force a #DIV0
 171         if( Den == 16 ) return Num >> 4;
 172         if( Den == 256 )        return Num >> 8;
 173         if( Den == 512 )        return Num >> 9;
 174         if( Den == 1024 )       return Num >> 10;
 175         if( Den == 2048 )       return Num >> 11;
 176         if( Den == 4096 )       return Num >> 12;
 177         if( Num < Den ) return 0;
 178         if( Num <= 0xFFFFFFFF && Den <= 0xFFFFFFFF )
 179                 return (Uint32)Num / (Uint32)Den;
 180
 181         #if 0
 182         if( Den <= 0xFFFFFFFF ) {
 183                 (Uint32)(Num >> 32) / (Uint32)Den
 184         }
 185         #endif
 186         Uint64  ret = 0;
 187         for( ret = 0; Num > Den; ret ++, Num -= Den );
 188         return ret;
 189 }
 190
 191 // Unsigned Modulus 64-bit Integer
 192 Uint64 __umoddi3(Uint64 Num, Uint64 Den)
 193 {
 194         if( Den == 0 )  return 5 / (Uint32)Den; // Force a #DIV0
 195         if( Num < Den ) return Num;
 196         if( Den == 1 )  return 0;
 197         if( Den == 2 )  return Num & 1;
 198         if( Den == 16 ) return Num & 3;
 199         if( Den == 256 )        return Num & 0xFF;
 200         if( Den == 512 )        return Num & 0x1FF;
 201         if( Den == 1024 )       return Num & 0x3FF;
 202         if( Den == 2048 )       return Num & 0x7FF;
 203         if( Den == 4096 )       return Num & 0xFFF;
 204 //      if( Num <= 0xFFFFFFFF && Den <= 0xFFFFFFFF )
 205 //              return (Uint32)Num % (Uint32)Den;
 206
 207         #if 0
 208         if( Den <= 0xFFFFFFFF ) {
 209                 (Uint32)(Num >> 32) / (Uint32)Den
 210         }
 211         #endif
 212         for( ; Num > Den; Num -= Den );
 213         return Num;
 214 }
 215
 216 #define _divide_s_32(Num, Den, rem)     __asm__ __volatile__ ( \
 217                 "mov %0, #0\n" \
 218         "       adds %1, %1, %1\n" \
 219         "       .rept 32\n" \
 220         "       adcs %0, %2, %0, lsl #1\n" \
 221         "       subcc %0, %0, %3\n" \
 222         "       adcs %1, %1, %1\n" \
 223         "       .endr\n" \
 224                 : "=r" (rem), "=r" (Num) \
 225                 : "r" (Den) \
 226                 : "cc" \
 227                 )
 228 Uint32 __udivsi3(Uint32 Num, Uint32 Den)
 229 {
 230         register Uint32 ret;
 231         Uint64  P, D;
 232          int    i;
 233
 234         if( Num == 0 )  return 0;
 235         if( Den == 0 )  return 0xFFFFFFFF;      // TODO: Throw an error
 236         if( Den == 1 )  return Num;
 237
 238         D = ((Uint64)Den) << 32;
 239
 240         for( i = 32; i --; )
 241         {
 242                 P = 2*P - D;
 243                 if( P >= 0 )
 244                         ret |= 1;
 245                 else
 246                         P += D;
 247                 ret <<= 1;
 248         }
 249
 250 //      _divide_s_32(Num, Den, rem);
 251         return Num;
 252 }
 253
 254 Uint32 __umodsi3(Uint32 Num, Uint32 Den)
 255 {
 256         return Num - __udivsi3(Num, Den)*Den;
 257 }
 258
 259 Sint32 __divsi3(Sint32 Num, Sint32 Den)
 260 {
 261         if( (Num < 0) && (Den < 0) )
 262                 return __udivsi3(-Num, -Den);
 263         else if( Num < 0 )
 264                 return __udivsi3(-Num, Den);
 265         else if( Den < 0 )
 266                 return __udivsi3(Den, -Den);
 267         else
 268                 return __udivsi3(Den, Den);
 269 }
 270
 271 Sint32 __modsi3(Sint32 Num, Sint32 Den)
 272 {
 273         //register Sint32       rem;
 274         //_divide_s_32(Num, Den, rem);
 275         return Num - __divsi3(Num, Den) * Den;
 276 }