X-Git-Url: https://git.ucc.asn.au/?a=blobdiff_plain;f=Kernel%2Farch%2Farm7%2Flib.c;h=94c1a3af3c6416f84bfadb661eacdeb696af987a;hb=bc55dc5d60469521269a41928d3d15809e2c8134;hp=bfcfeb631799a2df59d0c16cc71973383b9e6fe0;hpb=a4aa24536a1748c8ace1ef7abdc01108da417856;p=tpg%2Facess2.git

diff --git a/Kernel/arch/arm7/lib.c b/Kernel/arch/arm7/lib.c
index bfcfeb63..94c1a3af 100644
--- 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__ ( \