Kernel/arm7 - Fiddling on process switching

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

diff --git a/Kernel/arch/arm7/lib.c b/Kernel/arch/arm7/lib.c
index bfcfeb6..94c1a3a 100644 (file)
--- a/Kernel/arch/arm7/lib.c
+++ b/Kernel/arch/arm7/lib.c
@@ -6,6 +6,7 @@
 #include <acess.h>
 
 // === PROTOTYPES ===
+Uint64 __divmod64(Uint64 Num, Uint64 Den, Uint64 *Rem);
 Uint64 __udivdi3(Uint64 Num, Uint64 Den);
 Uint64 __umoddi3(Uint64 Num, Uint64 Den);
 Uint32 __udivsi3(Uint32 Num, Uint32 Den);
@@ -112,13 +113,102 @@ void *memset(void *_dest, int _value, size_t _length)
        return _dest;
 }
 
+Uint64 __divmod64(Uint64 Num, Uint64 Den, Uint64 *Rem)
+{
+       Uint64  ret, add;
+
+       ret = 0;
+       add = 1;
+
+       // Find what power of two times Den is > Num
+       while( Num >= Den )
+       {
+               Den <<= 1;
+               add <<= 1;
+       }
+
+       // Search backwards
+       while( add > 1 )
+       {
+               add >>= 1;
+               Den >>= 1;
+               // If the numerator is > Den, subtract and add to return value
+               if( Num > Den )
+               {
+                       ret += add;
+                       Num -= Den;
+               }
+       }
+       if(Rem) *Rem = Num;
+       return ret;
+}
+
+Uint64 DivMod64U(Uint64 Num, Uint64 Den, Uint64 *Rem)
+{
+       Uint64  ret;
+       if(Den == 0)    return 0;       // TODO: #div0
+       if(Num == 0) {
+               if(Rem) *Rem = 0;
+               return 0;
+       }
+       if(Den == 1) {
+               if(Rem) *Rem = 0;
+               return Num;
+       }
+       if(Den == 2) {
+               if(Rem) *Rem = Num & 1;
+               return Num >> 1;
+       }
+       if(Den == 16) {
+               if(Rem) *Rem = Num & 0xF;
+               return Num >> 4;
+       }
+       if(Den == 32) {
+               if(Rem) *Rem = Num & 0x1F;
+               return Num >> 5;
+       }
+       if(Den == 0x1000) {
+               if(Rem) *Rem = Num & 0xFFF;
+               return Num >> 12;
+       }
+
+       #if 0
+       {
+               // http://www.tofla.iconbar.com/tofla/arm/arm02/index.htm
+               Uint64  tmp = 1;
+               __asm__ __volatile__(
+                       "1:"
+                       "cmpl %2,%1"
+                       "movls %2,%2,lsl#1"
+                       "movls %3,%3,lsl#1"
+                       "bls 1b"
+                       "2:"
+                       "cmpl %"
+               while(Num > Den) {
+                       Den <<= 1;
+                       tmp <<= 1;
+               }
+               Den >>= 1; tmp >>= 1;
+               while(
+       }
+       if(Rem) *Rem = Num;
+       return ret;
+       #elif 0
+       for( ret = 0; Num > Den; ret ++, Num -= Den) ;
+       if(Rem) *Rem = Num;
+       return ret;
+       #else
+       ret = __divmod64(Num, Den, Rem);
+       return ret;
+       #endif
+}
+
 // Unsigned Divide 64-bit Integer
 Uint64 __udivdi3(Uint64 Num, Uint64 Den)
 {
-       if( Num == 0 )  return 0;
-       if( Den == 0 )  return 5 / (Uint32)Den; // Force a #DIV0
-       if( Den == 1 )  return Num;
-       if( Den == 2 )  return Num >> 1;
+       return DivMod64U(Num, Den, NULL);
+       #if 0
+//     if( Den == 0 )  return 5 / (Uint32)Den; // Force a #DIV0
        if( Den == 16 ) return Num >> 4;
        if( Den == 256 )        return Num >> 8;
        if( Den == 512 )        return Num >> 9;
@@ -137,13 +227,18 @@ Uint64 __udivdi3(Uint64 Num, Uint64 Den)
        Uint64  ret = 0;
        for( ret = 0; Num > Den; ret ++, Num -= Den );
        return ret;
+       #endif
 }
 
 // Unsigned Modulus 64-bit Integer
 Uint64 __umoddi3(Uint64 Num, Uint64 Den)
 {
-       if( Num == 0 )  return 0;
+       Uint64  ret = 0;
+       DivMod64U(Num, Den, &ret);
+       return ret;
+       #if 0
        if( Den == 0 )  return 5 / (Uint32)Den; // Force a #DIV0
+       if( Num < Den ) return Num;
        if( Den == 1 )  return 0;
        if( Den == 2 )  return Num & 1;
        if( Den == 16 ) return Num & 3;
@@ -152,9 +247,8 @@ Uint64 __umoddi3(Uint64 Num, Uint64 Den)
        if( Den == 1024 )       return Num & 0x3FF;
        if( Den == 2048 )       return Num & 0x7FF;
        if( Den == 4096 )       return Num & 0xFFF;
-       if( Num < Den ) return 0;
-       if( Num <= 0xFFFFFFFF && Den <= 0xFFFFFFFF )
-               return (Uint32)Num % (Uint32)Den;
+//     if( Num <= 0xFFFFFFFF && Den <= 0xFFFFFFFF )
+//             return (Uint32)Num % (Uint32)Den;
 
        #if 0
        if( Den <= 0xFFFFFFFF ) {
@@ -163,6 +257,7 @@ Uint64 __umoddi3(Uint64 Num, Uint64 Den)
        #endif
        for( ; Num > Den; Num -= Den );
        return Num;
+       #endif
 }
 
 #define _divide_s_32(Num, Den, rem)    __asm__ __volatile__ ( \