Kernel/arch/armv7/lib.c

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