4 * lib.c - Library Functions
9 Uint64 __divmod64(Uint64 Num, Uint64 Den, Uint64 *Rem);
10 Uint64 __udivdi3(Uint64 Num, Uint64 Den);
11 Uint64 __umoddi3(Uint64 Num, Uint64 Den);
12 Uint32 __udivsi3(Uint32 Num, Uint32 Den);
13 Uint32 __umodsi3(Uint32 Num, Uint32 Den);
14 Sint32 __divsi3(Sint32 Num, Sint32 Den);
15 Sint32 __modsi3(Sint32 Num, Sint32 Den);
18 void *memcpy(void *_dest, const void *_src, size_t _length)
23 const Uint8 *src8 = _src;
25 // Handle small copies / Non-aligned
26 if( _length < 4 || ((tVAddr)_dest & 3) != ((tVAddr)_src & 3) )
28 for( ; _length--; dst8++,src8++ )
34 while( (tVAddr)dst8 & 3 ) *dst8 ++ = *src8++;
35 dst = (void *)dst8; src = (void *)src8;
38 for( ; _length > 3; _length -= 4)
42 dst8 = (void*)dst; src8 = (void*)src;
43 for( ; _length; _length -- )
49 int memcmp(const void *_m1, const void *_m2, size_t _length)
51 const Uint32 *m1, *m2;
52 const Uint8 *m1_8 = _m1, *m2_8 = _m2;
54 // Handle small copies / Non-aligned
55 if( _length < 4 || ((tVAddr)_m1 & 3) != ((tVAddr)_m1 & 3) )
57 for( ; _length--; m1_8++,m2_8++ ) {
58 if(*m1_8 != *m2_8) return *m1_8 - *m2_8;
64 for( ; (tVAddr)m1_8 & 3; m1_8 ++, m2_8 ++) {
65 if(*m1_8 != *m2_8) return *m1_8 - *m2_8;
67 m1 = (void *)m1_8; m2 = (void *)m2_8;
70 for( ; _length > 3; _length -= 4, m1++, m2++)
71 if(*m1 != *m2) return *m1 - *m2;
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;
81 void *memset(void *_dest, int _value, size_t _length)
86 _value = (Uint8)_value;
88 // Handle small copies / Non-aligned
91 for( ; _length--; dst8++ )
101 while( (tVAddr)dst8 & 3 ) *dst8 ++ = _value;
105 for( ; _length > 3; _length -= 4)
110 for( ; _length; _length -- )
116 Uint64 __divmod64(Uint64 Num, Uint64 Den, Uint64 *Rem)
123 // Find what power of two times Den is > Num
135 // If the numerator is > Den, subtract and add to return value
146 Uint64 DivMod64U(Uint64 Num, Uint64 Den, Uint64 *Rem)
149 if(Den == 0) return 0; // TODO: #div0
159 if(Rem) *Rem = Num & 1;
163 if(Rem) *Rem = Num & 0xF;
167 if(Rem) *Rem = Num & 0x1F;
171 if(Rem) *Rem = Num & 0xFFF;
177 // http://www.tofla.iconbar.com/tofla/arm/arm02/index.htm
179 __asm__ __volatile__(
191 Den >>= 1; tmp >>= 1;
197 for( ret = 0; Num > Den; ret ++, Num -= Den) ;
201 ret = __divmod64(Num, Den, Rem);
206 // Unsigned Divide 64-bit Integer
207 Uint64 __udivdi3(Uint64 Num, Uint64 Den)
209 return DivMod64U(Num, Den, NULL);
211 // if( Den == 0 ) return 5 / (Uint32)Den; // Force a #DIV0
212 if( Den == 16 ) return Num >> 4;
213 if( Den == 256 ) return Num >> 8;
214 if( Den == 512 ) return Num >> 9;
215 if( Den == 1024 ) return Num >> 10;
216 if( Den == 2048 ) return Num >> 11;
217 if( Den == 4096 ) return Num >> 12;
218 if( Num < Den ) return 0;
219 if( Num <= 0xFFFFFFFF && Den <= 0xFFFFFFFF )
220 return (Uint32)Num / (Uint32)Den;
223 if( Den <= 0xFFFFFFFF ) {
224 (Uint32)(Num >> 32) / (Uint32)Den
228 for( ret = 0; Num > Den; ret ++, Num -= Den );
233 // Unsigned Modulus 64-bit Integer
234 Uint64 __umoddi3(Uint64 Num, Uint64 Den)
237 DivMod64U(Num, Den, &ret);
240 if( Den == 0 ) return 5 / (Uint32)Den; // Force a #DIV0
241 if( Num < Den ) return Num;
242 if( Den == 1 ) return 0;
243 if( Den == 2 ) return Num & 1;
244 if( Den == 16 ) return Num & 3;
245 if( Den == 256 ) return Num & 0xFF;
246 if( Den == 512 ) return Num & 0x1FF;
247 if( Den == 1024 ) return Num & 0x3FF;
248 if( Den == 2048 ) return Num & 0x7FF;
249 if( Den == 4096 ) return Num & 0xFFF;
250 // if( Num <= 0xFFFFFFFF && Den <= 0xFFFFFFFF )
251 // return (Uint32)Num % (Uint32)Den;
254 if( Den <= 0xFFFFFFFF ) {
255 (Uint32)(Num >> 32) / (Uint32)Den
258 for( ; Num > Den; Num -= Den );
263 #define _divide_s_32(Num, Den, rem) __asm__ __volatile__ ( \
265 " adds %1, %1, %1\n" \
267 " adcs %0, %2, %0, lsl #1\n" \
268 " subcc %0, %0, %3\n" \
269 " adcs %1, %1, %1\n" \
271 : "=r" (rem), "=r" (Num) \
275 Uint32 __udivsi3(Uint32 Num, Uint32 Den)
281 if( Num == 0 ) return 0;
282 if( Den == 0 ) return 0xFFFFFFFF; // TODO: Throw an error
283 if( Den == 1 ) return Num;
285 D = ((Uint64)Den) << 32;
297 // _divide_s_32(Num, Den, rem);
301 Uint32 __umodsi3(Uint32 Num, Uint32 Den)
303 return Num - __udivsi3(Num, Den)*Den;
306 Sint32 __divsi3(Sint32 Num, Sint32 Den)
308 if( (Num < 0) && (Den < 0) )
309 return __udivsi3(-Num, -Den);
311 return __udivsi3(-Num, Den);
313 return __udivsi3(Den, -Den);
315 return __udivsi3(Den, Den);
318 Sint32 __modsi3(Sint32 Num, Sint32 Den)
320 //register Sint32 rem;
321 //_divide_s_32(Num, Den, rem);
322 return Num - __divsi3(Num, Den) * Den;