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 Uint32  __divmod32(Uint32 Num, Uint32 Den, Uint32 *Rem);
  11 Uint64  __udivdi3(Uint64 Num, Uint64 Den);
  12 Uint64  __umoddi3(Uint64 Num, Uint64 Den);
  13 Uint32  __udivsi3(Uint32 Num, Uint32 Den);
  14 Uint32  __umodsi3(Uint32 Num, Uint32 Den);
  15 Sint32  __divsi3(Sint32 Num, Sint32 Den);
  16 Sint32  __modsi3(Sint32 Num, Sint32 Den);
  17
  18 // === CODE ===
  19 void *memcpy(void *_dest, const void *_src, size_t _length)
  20 {
  21         Uint32  *dst;
  22         const Uint32    *src;
  23         Uint8   *dst8 = _dest;
  24         const Uint8     *src8 = _src;
  25
  26         // Handle small copies / Non-aligned
  27         if( _length < 4 || ((tVAddr)_dest & 3) != ((tVAddr)_src & 3) )
  28         {
  29                 for( ; _length--; dst8++,src8++ )
  30                         *dst8 = *src8;
  31                 return _dest;
  32         }
  33
  34         // Force alignment
  35         while( (tVAddr)dst8 & 3 ) *dst8 ++ = *src8++;
  36         dst = (void *)dst8;     src = (void *)src8;
  37
  38         // DWORD copies
  39         for( ; _length > 3; _length -= 4)
  40                 *dst++ = *src++;
  41
  42         // Trailing bytes
  43         dst8 = (void*)dst;      src8 = (void*)src;
  44         for( ; _length; _length -- )
  45                 *dst8 ++ = *src8 ++;
  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 / Non-aligned
  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 == 0) {
 141                 if(Rem) *Rem = 0;
 142                 return 0;
 143         }
 144         if(Den == 1) {
 145                 if(Rem) *Rem = 0;
 146                 return Num;
 147         }
 148         if(Den == 2) {
 149                 if(Rem) *Rem = Num & 1;
 150                 return Num >> 1;
 151         }
 152         if(Den == 16) {
 153                 if(Rem) *Rem = Num & 0xF;
 154                 return Num >> 4;
 155         }
 156         if(Den == 32) {
 157                 if(Rem) *Rem = Num & 0x1F;
 158                 return Num >> 5;
 159         }
 160         if(Den == 0x1000) {
 161                 if(Rem) *Rem = Num & 0xFFF;
 162                 return Num >> 12;
 163         }
 164
 165         ret = __divmod64(Num, Den, Rem);
 166         return ret;
 167 }
 168
 169 // Unsigned Divide 64-bit Integer
 170 Uint64 __udivdi3(Uint64 Num, Uint64 Den)
 171 {
 172         return DivMod64U(Num, Den, NULL);
 173 }
 174
 175 // Unsigned Modulus 64-bit Integer
 176 Uint64 __umoddi3(Uint64 Num, Uint64 Den)
 177 {
 178         Uint64  ret = 0;
 179         DivMod64U(Num, Den, &ret);
 180         return ret;
 181 }
 182
 183 #define _divide_s_32(Num, Den, rem)     __asm__ __volatile__ ( \
 184                 "mov %0, #0\n" \
 185         "       adds %1, %1, %1\n" \
 186         "       .rept 32\n" \
 187         "       adcs %0, %2, %0, lsl #1\n" \
 188         "       subcc %0, %0, %3\n" \
 189         "       adcs %1, %1, %1\n" \
 190         "       .endr\n" \
 191                 : "=r" (rem), "=r" (Num) \
 192                 : "r" (Den) \
 193                 : "cc" \
 194                 )
 195 Uint32 __udivsi3(Uint32 Num, Uint32 Den)
 196 {
 197         return __divmod32(Num, Den, NULL);
 198 }
 199
 200 Uint32 __umodsi3(Uint32 Num, Uint32 Den)
 201 {
 202         Uint32  rem;
 203         __divmod32(Num, Den, &rem);
 204         return rem;
 205 }
 206
 207 Sint32 __divsi3(Sint32 Num, Sint32 Den)
 208 {
 209         if( (Num < 0) && (Den < 0) )
 210                 return __udivsi3(-Num, -Den);
 211         else if( Num < 0 )
 212                 return __udivsi3(-Num, Den);
 213         else if( Den < 0 )
 214                 return __udivsi3(Den, -Den);
 215         else
 216                 return __udivsi3(Den, Den);
 217 }
 218
 219 Sint32 __modsi3(Sint32 Num, Sint32 Den)
 220 {
 221         if( (Num < 0) && (Den < 0) )
 222                 return __umodsi3(-Num, -Den);
 223         else if( Num < 0 )
 224                 return __umodsi3(-Num, Den);
 225         else if( Den < 0 )
 226                 return __umodsi3(Den, -Den);
 227         else
 228                 return __umodsi3(Den, Den);
 229 }