X-Git-Url: https://git.ucc.asn.au/?a=blobdiff_plain;f=Kernel%2Flib.c;h=62500fe9d40de9e06938bf5f7605767fbae3edb9;hb=bd8dc898108f10c0498f4dc5d0164a50b5ff2e5c;hp=0dbb9b96afc3e0a1b02e09a0628803c608bd1a0f;hpb=ef2ef0b76dca5bd5ee3b80fb7f831f89fc81f80d;p=tpg%2Facess2.git

diff --git a/Kernel/lib.c b/Kernel/lib.c
index 0dbb9b96..62500fe9 100644
--- a/Kernel/lib.c
+++ b/Kernel/lib.c
@@ -2,23 +2,128 @@
  * Acess2
  * Common Library Functions
  */
-#include <common.h>
+#include <acess.h>
 
 // === CONSTANTS ===
 #define	RANDOM_SEED	0xACE55052
-#define	RANDOM_A	0x12231ADE
-#define	RANDOM_C	0x1BADBEEF
+#define	RANDOM_A	0x00731ADE
+#define	RANDOM_C	12345
+#define	RANDOM_SPRUCE	0xf12b02b
 //                          Jan Feb Mar Apr May  Jun  Jul  Aug  Sept Oct  Nov  Dec
 const short DAYS_BEFORE[] = {0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334};
 #define UNIX_TO_2K	((30*365*3600*24) + (7*3600*24))	//Normal years + leap years
 
 // === PROTOTYPES ===
+ int	atoi(const char *string);
+void	itoa(char *buf, Uint num, int base, int minLength, char pad);
+ int	vsnprintf(char *__s, size_t __maxlen, const char *__format, va_list args);
+ int	sprintf(char *__s, const char *__format, ...);
+ int	tolower(int c);
+ int	strucmp(const char *Str1, const char *Str2);
+ int	strpos(const char *Str, char Ch);
+ Uint8	ByteSum(void *Ptr, int Size);
+size_t	strlen(const char *__s);
+char	*strcpy(char *__str1, const char *__str2);
+char	*strncpy(char *__str1, const char *__str2, size_t max);
+ int	strcmp(const char *str1, const char *str2);
+ int	strncmp(const char *str1, const char *str2, size_t num);
+char	*strdup(const char *Str);
+ int	DivUp(int num, int dem);
+ int	strpos8(const char *str, Uint32 Search);
  int	ReadUTF8(Uint8 *str, Uint32 *Val);
+ int	WriteUTF8(Uint8 *str, Uint32 Val);
+Sint64	timestamp(int sec, int mins, int hrs, int day, int month, int year);
+Uint	rand(void);
+ int	CheckString(char *String);
+ int	CheckMem(void *Mem, int NumBytes);
+ int	ModUtil_LookupString(char **Array, char *Needle);
+ int	ModUtil_SetIdent(char *Dest, char *Value);
+
+// === EXPORTS ===
+EXPORT(atoi);
+EXPORT(itoa);
+EXPORT(vsnprintf);
+EXPORT(sprintf);
+EXPORT(tolower);
+EXPORT(strucmp);
+EXPORT(strpos);
+EXPORT(ByteSum);
+EXPORT(strlen);
+EXPORT(strcpy);
+EXPORT(strncpy);
+EXPORT(strcmp);
+EXPORT(strncmp);
+EXPORT(strdup);
+EXPORT(DivUp);
+EXPORT(strpos8);
+EXPORT(ReadUTF8);
+EXPORT(WriteUTF8);
+EXPORT(timestamp);
+EXPORT(CheckString);
+EXPORT(CheckMem);
+EXPORT(ModUtil_LookupString);
+EXPORT(ModUtil_SetIdent);
+
+// === GLOBALS ===
+static Uint	giRandomState = RANDOM_SEED;
 
 // === CODE ===
+/**
+ * \brief Convert a string into an integer
+ */
+int atoi(const char *string)
+{
+	 int	ret = 0;
+	
+	// Clear non-numeric characters
+	while( !('0' <= *string && *string <= '9') )	string++;
+	
+	if(*string == '0')
+	{
+		string ++;
+		if(*string == 'x')
+		{
+			// Hex
+			string ++;
+			for( ;; ) {
+				ret *= 16;
+				if('0' <= *string && *string <= '9')
+					ret += *string - '0';
+				else if('A' <= *string && *string <= 'F')
+					ret += *string - 'A' + 10;
+				else if('a' <= *string && *string <= 'f')
+					ret += *string - 'a' + 10;
+				else
+					break;
+				string ++;
+			}
+		}
+		else
+		{
+			for( ;; )
+			{
+				ret *= 8;
+				if('0' <= *string && *string <= '7')
+					ret += *string - '0';
+				else
+					break;
+			}
+		}
+	}
+	else
+	{
+		for( ; '0' <= *string && *string <= '9'; string++)
+		{
+			ret *= 10;
+			ret += *string - '0';
+		}
+	}
+	return ret;
+}
+
 static const char cUCDIGITS[] = "0123456789ABCDEF";
 /**
- * \fn static void itoa(char *buf, Uint num, int base, int minLength, char pad)
+ * \fn void itoa(char *buf, Uint num, int base, int minLength, char pad)
  * \brief Convert an integer into a character string
  */
 void itoa(char *buf, Uint num, int base, int minLength, char pad)
@@ -51,8 +156,153 @@ void itoa(char *buf, Uint num, int base, int minLength, char pad)
 	buf[i] = 0;
 }
 
+#define PUTCH(c)	do{if(pos==__maxlen)break;if(__s){__s[pos++]=(c);}else{pos++;}}while(0)
+int vsnprintf(char *__s, size_t __maxlen, const char *__format, va_list args)
+{
+	char	c, pad = ' ';
+	 int	minSize = 0;
+	char	tmpBuf[34];	// For Integers
+	char	*p = NULL;
+	 int	isLongLong = 0;
+	Uint64	val;
+	size_t	pos = 0;
+	
+	while((c = *__format++) != 0)
+	{
+		// Non control character
+		if(c != '%') { PUTCH(c); continue; }
+		
+		c = *__format++;
+		
+		// Literal %
+		if(c == '%') { PUTCH('%'); continue; }
+		
+		// Pointer - Done first for debugging
+		if(c == 'p') {
+			Uint	ptr = va_arg(args, Uint);
+			PUTCH('*');	PUTCH('0');	PUTCH('x');
+			p = tmpBuf;
+			itoa(p, ptr, 16, BITS/4, '0');
+			goto printString;
+		}
+		
+		// Get Argument
+		val = va_arg(args, Uint);
+		
+		// - Padding
+		if(c == '0') {
+			pad = '0';
+			c = *__format++;
+		}
+		else
+			pad = ' ';
+		
+		// - Minimum length
+		minSize = 1;
+		if('1' <= c && c <= '9')
+		{
+			minSize = 0;
+			while('0' <= c && c <= '9')
+			{
+				minSize *= 10;
+				minSize += c - '0';
+				c = *__format++;
+			}
+		}
+		
+		// - Default, Long or LongLong?
+		isLongLong = 0;
+		if(c == 'l')	// Long is actually the default on x86
+		{
+			c = *__format++;
+			if(c == 'l') {
+				#if BITS == 32
+				val |= (Uint64)va_arg(args, Uint) << 32;
+				#endif
+				c = *__format++;
+				isLongLong = 1;
+			}
+		}
+		
+		// - Now get the format code
+		p = tmpBuf;
+		switch(c)
+		{
+		case 'd':
+		case 'i':
+			if( (isLongLong && val >> 63) || (!isLongLong && val >> 31) ) {
+				PUTCH('-');
+				val = -val;
+			}
+			itoa(p, val, 10, minSize, pad);
+			goto printString;
+		case 'u':
+			itoa(p, val, 10, minSize, pad);
+			goto printString;
+		case 'x':
+			itoa(p, val, 16, minSize, pad);
+			goto printString;
+		case 'o':
+			itoa(p, val, 8, minSize, pad);
+			goto printString;
+		case 'b':
+			itoa(p, val, 2, minSize, pad);
+			goto printString;
+
+		case 'B':	//Boolean
+			if(val)	p = "True";
+			else	p = "False";
+			goto printString;
+		
+		// String - Null Terminated Array
+		case 's':
+			p = (char*)(Uint)val;
+		printString:
+			if(!p)		p = "(null)";
+			while(*p)	PUTCH(*p++);
+			break;
+		
+		case 'C':	// Non-Null Terminated Character Array
+			p = (char*)(Uint)val;
+			if(!p)	goto printString;
+			while(minSize--)	PUTCH(*p++);
+			break;
+		
+		// Single Character
+		case 'c':
+		default:
+			PUTCH( (Uint8)val );
+			break;
+		}
+		
+		if(pos == __maxlen)
+			break;
+	}
+	
+	if(__s && pos != __maxlen)
+		__s[pos] = '\0';
+		
+	
+	return pos;
+}
+#undef PUTCH
+
+/**
+ */
+int sprintf(char *__s, const char *__format, ...)
+{
+	va_list	args;
+	 int	ret;
+	
+	va_start(args, __format);
+	ret = vsnprintf(__s, -1, __format, args);
+	va_end(args);
+	
+	return ret;
+}
+
 /**
- * \fn int tolower(int __c)
+ * \fn int tolower(int c)
  * \brief Converts a character to lower case
  */
 int tolower(int c)
@@ -63,10 +313,10 @@ int tolower(int c)
 }
 
 /**
- * \fn int strucmp(char *Str1, char *Str2)
+ * \fn int strucmp(const char *Str1, const char *Str2)
  * \brief Compare \a Str1 and \a Str2 case-insensitively
  */
-int strucmp(char *Str1, char *Str2)
+int strucmp(const char *Str1, const char *Str2)
 {
 	while(*Str1 && tolower(*Str1) == tolower(*Str2))
 		Str1++, Str2++;
@@ -74,10 +324,10 @@ int strucmp(char *Str1, char *Str2)
 }
 
 /**
- * \fn int strpos(char *Str, char Ch)
+ * \fn int strpos(const char *Str, char Ch)
  * \brief Search a string for an ascii character
  */
-int strpos(char *Str, char Ch)
+int strpos(const char *Str, char Ch)
 {
 	 int	pos;
 	for(pos=0;Str[pos];pos++)
@@ -88,19 +338,21 @@ int strpos(char *Str, char Ch)
 }
 
 /**
+ * \fn Uint8 ByteSum(void *Ptr, int Size)
+ * \brief Adds the bytes in a memory region and returns the sum
  */
-int ByteSum(void *Ptr, int Size)
+Uint8 ByteSum(void *Ptr, int Size)
 {
-	 int	sum = 0;
+	Uint8	sum = 0;
 	while(Size--)	sum += *(Uint8*)Ptr++;
 	return sum;
 }
 
 /**
- * \fn Uint strlen(char *__str)
+ * \fn Uint strlen(const char *__str)
  * \brief Get the length of string
  */
-Uint strlen(char *__str)
+Uint strlen(const char *__str)
 {
 	Uint	ret = 0;
 	while(*__str++)	ret++;
@@ -108,10 +360,10 @@ Uint strlen(char *__str)
 }
 
 /**
- * \fn char *strcpy(char *__str1, char *__str2)
+ * \fn char *strcpy(char *__str1, const char *__str2)
  * \brief Copy a string to a new location
  */
-char *strcpy(char *__str1, char *__str2)
+char *strcpy(char *__str1, const char *__str2)
 {
 	while(*__str2)
 		*__str1++ = *__str2++;
@@ -120,11 +372,24 @@ char *strcpy(char *__str1, char *__str2)
 }
 
 /**
- * \fn int strcmp(char *str1, char *str2)
+ * \fn char *strncpy(char *__str1, const char *__str2, size_t max)
+ * \brief Copy a string to a new location
+ */
+char *strncpy(char *__str1, const char *__str2, size_t max)
+{
+	while(*__str2 && max-- >= 1)
+		*__str1++ = *__str2++;
+	if(max)
+		*__str1 = '\0';	// Terminate String
+	return __str1;
+}
+
+/**
+ * \fn int strcmp(const char *str1, const char *str2)
  * \brief Compare two strings return the difference between
  *        the first non-matching characters.
  */
-int strcmp(char *str1, char *str2)
+int strcmp(const char *str1, const char *str2)
 {
 	while(*str1 && *str1 == *str2)
 		str1++, str2++;
@@ -132,21 +397,45 @@ int strcmp(char *str1, char *str2)
 }
 
 /**
- * \fn int strncmp(char *Str1, char *Str2, size_t num)
+ * \fn int strncmp(const char *Str1, const char *Str2, size_t num)
  * \brief Compare strings \a Str1 and \a Str2 to a maximum of \a num characters
  */
-int strncmp(char *Str1, char *Str2, size_t num)
+int strncmp(const char *Str1, const char *Str2, size_t num)
 {
-	while(num-- && *Str1 && *Str1 == *Str2)
+	if(num == 0)	return 0;	// TODO: Check what should officially happen here
+	while(--num && *Str1 && *Str1 == *Str2)
 		Str1++, Str2++;
 	return *Str1-*Str2;
 }
 
 /**
- * \fn int strpos8(char *str, Uint32 search)
+ * \fn char *strdup(const char *Str)
+ * \brief Duplicates a string
+ */
+char *strdup(const char *Str)
+{
+	char	*ret;
+	ret = malloc(strlen(Str)+1);
+	strcpy(ret, Str);
+	return ret;
+}
+
+/**
+ * \fn int DivUp(int num, int dem)
+ * \brief Divide two numbers, rounding up
+ * \param num	Numerator
+ * \param dem	Denominator
+ */
+int DivUp(int num, int dem)
+{
+	return (num+dem-1)/dem;
+}
+
+/**
+ * \fn int strpos8(const char *str, Uint32 search)
  * \brief Search a string for a UTF-8 character
  */
-int strpos8(char *str, Uint32 Search)
+int strpos8(const char *str, Uint32 Search)
 {
 	 int	pos;
 	Uint32	val = 0;
@@ -301,20 +590,100 @@ Sint64 timestamp(int sec, int mins, int hrs, int day, int month, int year)
  * \brief Pseudo random number generator
  * \note Unknown effectiveness (made up on the spot)
  */
-Uint rand()
+Uint rand(void)
 {
-	#if 0
-	static Uint	randomState = RANDOM_SEED;
-	Uint	ret = randomState;
-	 int	roll = randomState & 31;
-	randomState = (randomState << roll) | (randomState >> (32-roll));
-	randomState ^= 0x9A3C5E78;
-	return ret;
-	#else
-	static Uint	randomState = RANDOM_SEED;
-	return randomState = (RANDOM_A*randomState + RANDOM_C) & 0xFFFFFFFF;
-	#endif
+	Uint	old = giRandomState;
+	// Get the next state value
+	giRandomState = (RANDOM_A*giRandomState + RANDOM_C) & 0xFFFFFFFF;
+	// Check if it has changed, and if it hasn't, change it
+	if(giRandomState == old)	giRandomState += RANDOM_SPRUCE;
+	return giRandomState;
 }
 
-EXPORT(timestamp);
-EXPORT(ReadUTF8);
+/// \name Memory Validation
+/// \{
+/**
+ * \brief Checks if a string resides fully in valid memory
+ */
+int CheckString(char *String)
+{
+	// Check 1st page
+	if( MM_IsUser( (tVAddr)String ) )
+	{
+		// Traverse String
+		while( *String )
+		{
+			if( !MM_IsUser( (tVAddr)String ) )
+				return 0;
+			// Increment string pointer
+			String ++;
+		}
+		return 1;
+	}
+	else if( MM_GetPhysAddr( (tVAddr)String ) )
+	{
+		// Traverse String
+		while( *String )
+		{
+			if( !MM_GetPhysAddr( (tVAddr)String ) )
+				return 0;
+			// Increment string pointer
+			String ++;
+		}
+		return 1;
+	}
+	return 0;
+}
+
+/**
+ * \brief Check if a sized memory region is valid memory
+ */
+int CheckMem(void *Mem, int NumBytes)
+{
+	tVAddr	addr = (tVAddr)Mem;
+	
+	if( MM_IsUser( addr ) )
+	{
+		while( NumBytes-- )
+		{
+			if( !MM_IsUser( addr ) )
+				return 0;
+			addr ++;
+		}
+		return 1;
+	}
+	else if( MM_GetPhysAddr( addr ) )
+	{
+		while( NumBytes-- )
+		{
+			if( !MM_GetPhysAddr( addr ) )
+				return 0;
+			addr ++;
+		}
+		return 1;
+	}
+	return 0;
+}
+/// \}
+
+/**
+ * \brief Search a string array for \a Needle
+ * \note Helper function for eTplDrv_IOCtl::DRV_IOCTL_LOOKUP
+ */
+int ModUtil_LookupString(char **Array, char *Needle)
+{
+	 int	i;
+	if( !CheckString(Needle) )	return -1;
+	for( i = 0; Array[i]; i++ )
+	{
+		if(strcmp(Array[i], Needle) == 0)	return i;
+	}
+	return -1;
+}
+
+int ModUtil_SetIdent(char *Dest, char *Value)
+{
+	if( !CheckMem(Dest, 32) )	return -1;
+	strncpy(Dest, Value, 32);
+	return 1;
+}