Kernel/arch/armv7/lib.c

   1 /*
   2  * Acess2 ARM7 Port
   3  *
   4  * lib.c - Library Functions
   5  */
   6 #include <acess.h>
   7
   8 // === IMPORTS ===
   9 extern void     __memcpy_align4(void *_dest, const void *_src, size_t _length);
  10 extern void     __memcpy_byte(void *_dest, const void *_src, size_t _length);
  11 extern Uint32   __divmod32_asm(Uint32 Num, Uint32 Den, Uint32 *Rem);
  12
  13 // === PROTOTYPES ===
  14 Uint64  __divmod64(Uint64 Num, Uint64 Den, Uint64 *Rem);
  15 Uint32  __divmod32(Uint32 Num, Uint32 Den, Uint32 *Rem);
  16 Uint64  __udivdi3(Uint64 Num, Uint64 Den);
  17 Uint64  __umoddi3(Uint64 Num, Uint64 Den);
  18 Uint32  __udivsi3(Uint32 Num, Uint32 Den);
  19 Uint32  __umodsi3(Uint32 Num, Uint32 Den);
  20 Sint32  __divsi3(Sint32 Num, Sint32 Den);
  21 Sint32  __modsi3(Sint32 Num, Sint32 Den);
  22
  23 // === CODE ===
  24 void *memcpy(void *_dest, const void *_src, size_t _length)
  25 {
  26         Uint8   *dst8 = _dest;
  27         const Uint8     *src8 = _src;
  28
  29         if( ((tVAddr)_dest & 3) == 0 && ((tVAddr)_src & 3) == 0 )
  30         {
  31                 __memcpy_align4(_dest, _src, _length);
  32                 return _dest;
  33         }
  34
  35         // Handle small copies / Non-aligned
  36         if( _length < 4 || ((tVAddr)_dest & 3) != ((tVAddr)_src & 3) )
  37         {
  38                 __memcpy_byte(_dest, _src, _length);
  39                 return _dest;
  40         }
  41
  42         // Force alignment
  43         while( (tVAddr)dst8 & 3 ) *dst8 ++ = *src8++, _length --;
  44
  45         __memcpy_align4(dst8, src8, _length);
  46
  47         return _dest;
  48 }
  49
  50 int memcmp(const void *_m1, const void *_m2, size_t _length)
  51 {
  52         const Uint32    *m1, *m2;
  53         const Uint8     *m1_8 = _m1, *m2_8 = _m2;
  54
  55         // Handle small copies / Non-aligned
  56         if( _length < 4 || ((tVAddr)_m1 & 3) != ((tVAddr)_m1 & 3) )
  57         {
  58                 for( ; _length--; m1_8++,m2_8++ ) {
  59                         if(*m1_8 != *m2_8)      return *m1_8 - *m2_8;
  60                 }
  61                 return 0;
  62         }
  63
  64         // Force alignment
  65         for( ; (tVAddr)m1_8 & 3; m1_8 ++, m2_8 ++) {
  66                 if(*m1_8 != *m2_8)      return *m1_8 - *m2_8;
  67         }
  68         m1 = (void *)m1_8;      m2 = (void *)m2_8;
  69
  70         // DWORD copies
  71         for( ; _length > 3; _length -= 4, m1++, m2++)
  72                 if(*m1 != *m2)  return *m1 - *m2;
  73
  74         // Trailing bytes
  75         m1_8 = (void*)m1;       m2_8 = (void*)m2;
  76         for( ; _length; _length --, m1_8++, m2_8++ )
  77                 if(*m1_8 != *m2_8)      return *m1_8 - *m2_8;
  78
  79         return 0;
  80 }
  81
  82 void *memset(void *_dest, int _value, size_t _length)
  83 {
  84         Uint32  *dst, val32;
  85         Uint8   *dst8 = _dest;
  86
  87         _value = (Uint8)_value;
  88
  89         // Handle small copies
  90         if( _length < 4 )
  91         {
  92                 for( ; _length--; dst8++ )
  93                         *dst8 = _value;
  94                 return _dest;
  95         }
  96
  97         val32 = _value;
  98         val32 |= val32 << 8;
  99         val32 |= val32 << 16;
 100
 101         // Force alignment
 102         while( (tVAddr)dst8 & 3 ) *dst8 ++ = _value;
 103         dst = (void *)dst8;
 104
 105         // DWORD copies
 106         for( ; _length > 3; _length -= 4)
 107                 *dst++ = val32;
 108
 109         // Trailing bytes
 110         dst8 = (void*)dst;
 111         for( ; _length; _length -- )
 112                 *dst8 ++ = _value;
 113
 114         return _dest;
 115 }
 116
 117 // Divide
 118 // - Find what power of two times Den is > Num
 119 // - Iterate down in bit significance
 120 //  > If the `N` value is greater than `D`, we can set this bit
 121 #define DEF_DIVMOD(s) Uint##s __divmod##s(Uint##s N, Uint##s D, Uint##s*Rem){\
 122         Uint##s ret=0,add=1;\
 123         while(N>=D&&add) {D<<=1;add<<=1;}\
 124         while(add>1){\
 125                 add>>=1;D>>=1;\
 126                 if(N>=D){ret+=add;N-=D;}\
 127         }\
 128         if(Rem)*Rem = N;\
 129         return ret;\
 130 }
 131
 132 DEF_DIVMOD(64)
 133 DEF_DIVMOD(32)
 134
 135
 136 Uint64 DivMod64U(Uint64 Num, Uint64 Den, Uint64 *Rem)
 137 {
 138         Uint64  ret;
 139         if(Den == 0)    return 0;       // TODO: #div0
 140         if(Num < Den) {
 141                 if(Rem) *Rem = Num;
 142                 return 0;
 143         }
 144         if(Num == 0) {
 145                 if(Rem) *Rem = 0;
 146                 return 0;
 147         }
 148         if(Den == 1) {
 149                 if(Rem) *Rem = 0;
 150                 return Num;
 151         }
 152         if(Den == 2) {
 153                 if(Rem) *Rem = Num & 1;
 154                 return Num >> 1;
 155         }
 156         if(Den == 16) {
 157                 if(Rem) *Rem = Num & 0xF;
 158                 return Num >> 4;
 159         }
 160         if(Den == 32) {
 161                 if(Rem) *Rem = Num & 0x1F;
 162                 return Num >> 5;
 163         }
 164         if(Den == 0x1000) {
 165                 if(Rem) *Rem = Num & 0xFFF;
 166                 return Num >> 12;
 167         }
 168
 169         if( !(Den >> 32) && !(Num >> 32) ) {
 170                 if(Rem) *Rem = 0;       // Clear high bits
 171                 return __divmod32_asm(Num, Den, (Uint32*)Rem);
 172         }
 173
 174         ret = __divmod64(Num, Den, Rem);
 175         return ret;
 176 }
 177
 178 // Unsigned Divide 64-bit Integer
 179 Uint64 __udivdi3(Uint64 Num, Uint64 Den)
 180 {
 181         return DivMod64U(Num, Den, NULL);
 182 }
 183
 184 // Unsigned Modulus 64-bit Integer
 185 Uint64 __umoddi3(Uint64 Num, Uint64 Den)
 186 {
 187         Uint64  ret = 0;
 188         DivMod64U(Num, Den, &ret);
 189         return ret;
 190 }
 191
 192 Uint32 __udivsi3(Uint32 Num, Uint32 Den)
 193 {
 194         return __divmod32_asm(Num, Den, NULL);
 195 }
 196
 197 Uint32 __umodsi3(Uint32 Num, Uint32 Den)
 198 {
 199         Uint32  rem;
 200         __divmod32_asm(Num, Den, &rem);
 201         return rem;
 202 }
 203
 204 static inline Sint32 DivMod32S(Sint32 Num, Sint32 Den, Sint32 *Rem)
 205 {
 206         Sint32  ret = 1;
 207         if( Num < 0 ) {
 208                 ret = -ret;
 209                 Num = -Num;
 210         }
 211         if( Den < 0 ) {
 212                 ret = -ret;
 213                 Den = -Den;
 214         }
 215         if(ret < 0)
 216                 ret = -__divmod32(Num, Den, (Uint32*)Rem);
 217         else
 218                 ret = __divmod32(Num, Den, (Uint32*)Rem);
 219         return ret;
 220 }
 221
 222 Sint32 __divsi3(Sint32 Num, Sint32 Den)
 223 {
 224         return DivMod32S(Num, Den, NULL);
 225 }
 226
 227 Sint32 __modsi3(Sint32 Num, Sint32 Den)
 228 {
 229         Sint32  rem;
 230         DivMod32S(Num, Den, &rem);
 231         return rem;
 232 }