Kernel - Slight reworks to timer code

[tpg/acess2.git] / Kernel / arch / armv7 / lib.c

diff --git a/Kernel/arch/armv7/lib.c b/Kernel/arch/armv7/lib.c
index a44489a..5a6928e 100644 (file)
--- a/Kernel/arch/armv7/lib.c
+++ b/Kernel/arch/armv7/lib.c
@@ -4,6 +4,12 @@
  * lib.c - Library Functions
  */
 #include <acess.h>
+#include "../helpers.h"
+
+// === IMPORTS ===
+extern void    __memcpy_align4(void *_dest, const void *_src, size_t _length);
+extern void    __memcpy_byte(void *_dest, const void *_src, size_t _length);
+extern Uint32  __divmod32_asm(Uint32 Num, Uint32 Den, Uint32 *Rem);
 
 // === PROTOTYPES ===
 Uint64 __divmod64(Uint64 Num, Uint64 Den, Uint64 *Rem);
@@ -18,31 +24,26 @@ Sint32      __modsi3(Sint32 Num, Sint32 Den);
 // === CODE ===
 void *memcpy(void *_dest, const void *_src, size_t _length)
 {
-       Uint32  *dst;
-       const Uint32    *src;
        Uint8   *dst8 = _dest;
        const Uint8     *src8 = _src;
 
+       if( ((tVAddr)_dest & 3) == 0 && ((tVAddr)_src & 3) == 0 )
+       {
+               __memcpy_align4(_dest, _src, _length);
+               return _dest;
+       }
+
        // Handle small copies / Non-aligned
        if( _length < 4 || ((tVAddr)_dest & 3) != ((tVAddr)_src & 3) )
        {
-               for( ; _length--; dst8++,src8++ )
-                       *dst8 = *src8;
+               __memcpy_byte(_dest, _src, _length);
                return _dest;
        }
 
        // Force alignment
-       while( (tVAddr)dst8 & 3 ) *dst8 ++ = *src8++;
-       dst = (void *)dst8;     src = (void *)src8;
-
-       // DWORD copies
-       for( ; _length > 3; _length -= 4)
-               *dst++ = *src++;
+       while( (tVAddr)dst8 & 3 ) *dst8 ++ = *src8++, _length --;
 
-       // Trailing bytes
-       dst8 = (void*)dst;      src8 = (void*)src;
-       for( ; _length; _length -- )
-               *dst8 ++ = *src8 ++;
+       __memcpy_align4(dst8, src8, _length);
        
        return _dest;
 }
@@ -86,7 +87,7 @@ void *memset(void *_dest, int _value, size_t _length)
 
        _value = (Uint8)_value;
 
-       // Handle small copies / Non-aligned
+       // Handle small copies
        if( _length < 4 )
        {
                for( ; _length--; dst8++ )
@@ -114,29 +115,17 @@ void *memset(void *_dest, int _value, size_t _length)
        return _dest;
 }
 
-// Divide
-// - Find what power of two times Den is > Num
-// - Iterate down in bit significance
-//  > If the `N` value is greater than `D`, we can set this bit
-#define DEF_DIVMOD(s) Uint##s __divmod##s(Uint##s N, Uint##s D, Uint##s*Rem){\
-       Uint##s ret=0,add=1;\
-       while(N>=D&&add) {D<<=1;add<<=1;}\
-       while(add>1){\
-               add>>=1;D>>=1;\
-               if(N>=D){ret+=add;N-=D;}\
-       }\
-       if(Rem)*Rem = N;\
-       return ret;\
-}
-
 DEF_DIVMOD(64)
 DEF_DIVMOD(32)
 
-
 Uint64 DivMod64U(Uint64 Num, Uint64 Den, Uint64 *Rem)
 {
        Uint64  ret;
        if(Den == 0)    return 0;       // TODO: #div0
+       if(Num < Den) {
+               if(Rem) *Rem = Num;
+               return 0;
+       }
        if(Num == 0) {
                if(Rem) *Rem = 0;
                return 0;
@@ -161,6 +150,11 @@ Uint64 DivMod64U(Uint64 Num, Uint64 Den, Uint64 *Rem)
                if(Rem) *Rem = Num & 0xFFF;
                return Num >> 12;
        }
+       
+       if( !(Den >> 32) && !(Num >> 32) ) {
+               if(Rem) *Rem = 0;       // Clear high bits
+               return __divmod32_asm(Num, Den, (Uint32*)Rem);
+       }
 
        ret = __divmod64(Num, Den, Rem);
        return ret;
@@ -180,50 +174,44 @@ Uint64 __umoddi3(Uint64 Num, Uint64 Den)
        return ret;
 }
 
-#define _divide_s_32(Num, Den, rem)    __asm__ __volatile__ ( \
-               "mov %0, #0\n" \
-       "       adds %1, %1, %1\n" \
-       "       .rept 32\n" \
-       "       adcs %0, %2, %0, lsl #1\n" \
-       "       subcc %0, %0, %3\n" \
-       "       adcs %1, %1, %1\n" \
-       "       .endr\n" \
-               : "=r" (rem), "=r" (Num) \
-               : "r" (Den) \
-               : "cc" \
-               )
 Uint32 __udivsi3(Uint32 Num, Uint32 Den)
 {
-       return __divmod32(Num, Den, NULL);
+       return __divmod32_asm(Num, Den, NULL);
 }
 
 Uint32 __umodsi3(Uint32 Num, Uint32 Den)
 {
        Uint32  rem;
-       __divmod32(Num, Den, &rem);
+       __divmod32_asm(Num, Den, &rem);
        return rem;
 }
 
-Sint32 __divsi3(Sint32 Num, Sint32 Den)
+static inline Sint32 DivMod32S(Sint32 Num, Sint32 Den, Sint32 *Rem)
 {
-       if( (Num < 0) && (Den < 0) )
-               return __udivsi3(-Num, -Den);
-       else if( Num < 0 )
-               return __udivsi3(-Num, Den);
-       else if( Den < 0 )
-               return __udivsi3(Den, -Den);
+       Sint32  ret = 1;
+       if( Num < 0 ) {
+               ret = -ret;
+               Num = -Num;
+       }
+       if( Den < 0 ) {
+               ret = -ret;
+               Den = -Den;
+       }
+       if(ret < 0)
+               ret = -__divmod32(Num, Den, (Uint32*)Rem);
        else
-               return __udivsi3(Den, Den);
+               ret = __divmod32(Num, Den, (Uint32*)Rem);
+       return ret;
+}
+
+Sint32 __divsi3(Sint32 Num, Sint32 Den)
+{
+       return DivMod32S(Num, Den, NULL);
 }
 
 Sint32 __modsi3(Sint32 Num, Sint32 Den)
 {
-       if( (Num < 0) && (Den < 0) )
-               return __umodsi3(-Num, -Den);
-       else if( Num < 0 )
-               return __umodsi3(-Num, Den);
-       else if( Den < 0 )
-               return __umodsi3(Den, -Den);
-       else
-               return __umodsi3(Den, Den);
+       Sint32  rem;
+       DivMod32S(Num, Den, &rem);
+       return rem;
 }