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