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
  12 // === PROTOTYPES ===
  13 Uint64  __divmod64(Uint64 Num, Uint64 Den, Uint64 *Rem);
  14 Uint32  __divmod32(Uint32 Num, Uint32 Den, Uint32 *Rem);
  15 Uint64  __udivdi3(Uint64 Num, Uint64 Den);
  16 Uint64  __umoddi3(Uint64 Num, Uint64 Den);
  17 Uint32  __udivsi3(Uint32 Num, Uint32 Den);
  18 Uint32  __umodsi3(Uint32 Num, Uint32 Den);
  19 Sint32  __divsi3(Sint32 Num, Sint32 Den);
  20 Sint32  __modsi3(Sint32 Num, Sint32 Den);
  21
  22 // === CODE ===
  23 void *memcpy(void *_dest, const void *_src, size_t _length)
  24 {
  25         Uint8   *dst8 = _dest;
  26         const Uint8     *src8 = _src;
  27
  28         if( ((tVAddr)_dest & 3) == 0 && ((tVAddr)_src & 3) == 0 )
  29         {
  30                 __memcpy_align4(_dest, _src, _length);
  31                 return _dest;
  32         }
  33
  34         // Handle small copies / Non-aligned
  35         if( _length < 4 || ((tVAddr)_dest & 3) != ((tVAddr)_src & 3) )
  36         {
  37                 __memcpy_byte(_dest, _src, _length);
  38                 return _dest;
  39         }
  40
  41         // Force alignment
  42         while( (tVAddr)dst8 & 3 ) *dst8 ++ = *src8++, _length --;
  43
  44         __memcpy_align32(dst8, src8, _length);
  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
  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 // Divide
 117 // - Find what power of two times Den is > Num
 118 // - Iterate down in bit significance
 119 //  > If the `N` value is greater than `D`, we can set this bit
 120 #define DEF_DIVMOD(s) Uint##s __divmod##s(Uint##s N, Uint##s D, Uint##s*Rem){\
 121         Uint##s ret=0,add=1;\
 122         while(N>=D&&add) {D<<=1;add<<=1;}\
 123         while(add>1){\
 124                 add>>=1;D>>=1;\
 125                 if(N>=D){ret+=add;N-=D;}\
 126         }\
 127         if(Rem)*Rem = N;\
 128         return ret;\
 129 }
 130
 131 DEF_DIVMOD(64)
 132 DEF_DIVMOD(32)
 133
 134
 135 Uint64 DivMod64U(Uint64 Num, Uint64 Den, Uint64 *Rem)
 136 {
 137         Uint64  ret;
 138         if(Den == 0)    return 0;       // TODO: #div0
 139         if(Num == 0) {
 140                 if(Rem) *Rem = 0;
 141                 return 0;
 142         }
 143         if(Den == 1) {
 144                 if(Rem) *Rem = 0;
 145                 return Num;
 146         }
 147         if(Den == 2) {
 148                 if(Rem) *Rem = Num & 1;
 149                 return Num >> 1;
 150         }
 151         if(Den == 16) {
 152                 if(Rem) *Rem = Num & 0xF;
 153                 return Num >> 4;
 154         }
 155         if(Den == 32) {
 156                 if(Rem) *Rem = Num & 0x1F;
 157                 return Num >> 5;
 158         }
 159         if(Den == 0x1000) {
 160                 if(Rem) *Rem = Num & 0xFFF;
 161                 return Num >> 12;
 162         }
 163
 164         ret = __divmod64(Num, Den, Rem);
 165         return ret;
 166 }
 167
 168 // Unsigned Divide 64-bit Integer
 169 Uint64 __udivdi3(Uint64 Num, Uint64 Den)
 170 {
 171         return DivMod64U(Num, Den, NULL);
 172 }
 173
 174 // Unsigned Modulus 64-bit Integer
 175 Uint64 __umoddi3(Uint64 Num, Uint64 Den)
 176 {
 177         Uint64  ret = 0;
 178         DivMod64U(Num, Den, &ret);
 179         return ret;
 180 }
 181
 182 Uint32 __udivsi3(Uint32 Num, Uint32 Den)
 183 {
 184         return __divmod32(Num, Den, NULL);
 185 }
 186
 187 Uint32 __umodsi3(Uint32 Num, Uint32 Den)
 188 {
 189         Uint32  rem;
 190         __divmod32(Num, Den, &rem);
 191         return rem;
 192 }
 193
 194 static inline Sint32 DivMod32S(Sint32 Num, Sint32 Den, Sint32 *Rem)
 195 {
 196         Sint32  ret = 1;
 197         if( Num < 0 ) {
 198                 ret = -ret;
 199                 Num = -Num;
 200         }
 201         if( Den < 0 ) {
 202                 ret = -ret;
 203                 Den = -Den;
 204         }
 205         if(ret < 0)
 206                 ret = -__divmod32(Num, Den, (Uint32*)Rem);
 207         else
 208                 ret = __divmod32(Num, Den, (Uint32*)Rem);
 209         return ret;
 210 }
 211
 212 Sint32 __divsi3(Sint32 Num, Sint32 Den)
 213 {
 214         return DivMod32S(Num, Den, NULL);
 215 }
 216
 217 Sint32 __modsi3(Sint32 Num, Sint32 Den)
 218 {
 219         Sint32  rem;
 220         DivMod32S(Num, Den, &rem);
 221         return rem;
 222 }