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