X-Git-Url: https://git.ucc.asn.au/?a=blobdiff_plain;f=Kernel%2Flib.c;h=73b6432e00b616f56f44ce84148fbd1ef7e921b3;hb=066ea7cb8cd5c873734405d0b713cb330c79ed49;hp=f99f58ac705bcb821d03ebe13a3a51ed1e8ad4ac;hpb=6ea2f6040da80c963882f6ccfe4bf21ff048eaff;p=tpg%2Facess2.git diff --git a/Kernel/lib.c b/Kernel/lib.c index f99f58ac..73b6432e 100644 --- a/Kernel/lib.c +++ b/Kernel/lib.c @@ -8,27 +8,159 @@ #define RANDOM_SEED 0xACE55052 #define RANDOM_A 0x00731ADE #define RANDOM_C 12345 -#define RANDOM_SPRUCE 0xf12b02b +#define RANDOM_SPRUCE 0xf12b039 // 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 === +#if 0 + int atoi(const char *string); +void itoa(char *buf, Uint64 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, ...); +#endif + int tolower(int c); +#if 0 + int strucmp(const char *Str1, const char *Str2); +char *strchr(const char *__s, int __c); + 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 *File, int Line, const char *Str); +char **str_split(const char *__str, char __ch); + int strpos8(const char *str, Uint32 Search); int ReadUTF8(Uint8 *str, Uint32 *Val); + int WriteUTF8(Uint8 *str, Uint32 Val); + int DivUp(int num, int dem); +Sint64 timestamp(int sec, int mins, int hrs, int day, int month, int year); + int 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); + + int UnHex(Uint8 *Dest, size_t DestSize, const char *SourceString); +#endif -// === GLOBALS === -static Uint giRandomState = RANDOM_SEED; +// === EXPORTS === +EXPORT(atoi); +EXPORT(itoa); +EXPORT(vsnprintf); +EXPORT(sprintf); +EXPORT(tolower); +EXPORT(strucmp); +EXPORT(strchr); +EXPORT(strpos); +EXPORT(ByteSum); +EXPORT(strlen); +EXPORT(strcpy); +EXPORT(strncpy); +EXPORT(strcat); +EXPORT(strncat); +EXPORT(strcmp); +EXPORT(strncmp); +//EXPORT(strdup); +EXPORT(_strdup); // Takes File/Line too +EXPORT(str_split); +EXPORT(strpos8); +EXPORT(DivUp); +EXPORT(ReadUTF8); +EXPORT(WriteUTF8); +EXPORT(timestamp); +EXPORT(CheckString); +EXPORT(CheckMem); +EXPORT(ModUtil_LookupString); +EXPORT(ModUtil_SetIdent); +EXPORT(UnHex); +EXPORT(SwapEndian16); +EXPORT(SwapEndian32); +EXPORT(memmove); // === CODE === +/** + * \brief Convert a string into an integer + */ +int atoi(const char *string) +{ + int ret = 0; + int bNeg = 0; + + //Log("atoi: (string='%s')", string); + + // Clear non-numeric characters + while( !('0' <= *string && *string <= '9') && *string != '-' ) string++; + if( *string == '-' ) { + bNeg = 1; + while( !('0' <= *string && *string <= '9') ) string++; + } + + if(*string == '0') + { + string ++; + if(*string == 'x') + { + // Hex + string ++; + for( ;; string ++ ) + { + if('0' <= *string && *string <= '9') { + ret *= 16; + ret += *string - '0'; + } + else if('A' <= *string && *string <= 'F') { + ret *= 16; + ret += *string - 'A' + 10; + } + else if('a' <= *string && *string <= 'f') { + ret *= 16; + ret += *string - 'a' + 10; + } + else + break; + } + } + else // Octal + { + for( ; '0' <= *string && *string <= '7'; string ++ ) + { + ret *= 8; + ret += *string - '0'; + } + } + } + else // Decimal + { + for( ; '0' <= *string && *string <= '9'; string++) + { + ret *= 10; + ret += *string - '0'; + } + } + + if(bNeg) ret = -ret; + + //Log("atoi: RETURN %i", ret); + + 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, Uint64 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) +void itoa(char *buf, Uint64 num, int base, int minLength, char pad) { - char tmpBuf[BITS]; + char tmpBuf[64+1]; int pos=0, i; + Uint64 rem; // Sanity check if(!buf) return; @@ -41,11 +173,11 @@ void itoa(char *buf, Uint num, int base, int minLength, char pad) // Convert while(num > base-1) { - tmpBuf[pos] = cUCDIGITS[ num % base ]; - num /= (Uint)base; // Shift `num` right 1 digit + num = DivMod64U(num, base, &rem); // Shift `num` and get remainder + tmpBuf[pos] = cUCDIGITS[ rem ]; pos++; } - tmpBuf[pos++] = cUCDIGITS[ num % base ]; // Last digit of `num` + tmpBuf[pos++] = cUCDIGITS[ num ]; // Last digit of `num` // Put in reverse i = 0; @@ -55,16 +187,34 @@ 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) +/** + * \brief Append a character the the vsnprintf output + */ +#define PUTCH(c) do{\ + char ch=(c);\ + if(pos==__maxlen){return pos;}\ + if(__s){__s[pos++]=ch;}else{pos++;}\ + }while(0) +#define GETVAL() do {\ + if(isLongLong) val = va_arg(args, Uint64);\ + else val = va_arg(args, unsigned int);\ + }while(0) +/** + * \brief VArg String Number Print Formatted + */ int vsnprintf(char *__s, size_t __maxlen, const char *__format, va_list args) { char c, pad = ' '; - int minSize = 0; + int minSize = 0, precision = -1, len; char tmpBuf[34]; // For Integers - char *p = NULL; + const char *p = NULL; int isLongLong = 0; Uint64 val; size_t pos = 0; + // Flags + int bPadLeft = 0; + + //Log("vsnprintf: (__s=%p, __maxlen=%i, __format='%s', ...)", __s, __maxlen, __format); while((c = *__format++) != 0) { @@ -72,6 +222,7 @@ int vsnprintf(char *__s, size_t __maxlen, const char *__format, va_list args) if(c != '%') { PUTCH(c); continue; } c = *__format++; + //Log("pos = %i", pos); // Literal % if(c == '%') { PUTCH('%'); continue; } @@ -80,13 +231,16 @@ int vsnprintf(char *__s, size_t __maxlen, const char *__format, va_list args) if(c == 'p') { Uint ptr = va_arg(args, Uint); PUTCH('*'); PUTCH('0'); PUTCH('x'); + itoa(tmpBuf, ptr, 16, BITS/4, '0'); p = tmpBuf; - itoa(p, ptr, 16, BITS/4, '0'); goto printString; } - // Get Argument - val = va_arg(args, Uint); + // - Padding Side Flag + if(c == '-') { + bPadLeft = 1; + c = *__format++; + } // - Padding if(c == '0') { @@ -97,8 +251,11 @@ int vsnprintf(char *__s, size_t __maxlen, const char *__format, va_list args) pad = ' '; // - Minimum length - minSize = 1; - if('1' <= c && c <= '9') + if(c == '*') { // Dynamic length + minSize = va_arg(args, unsigned int); + c = *__format++; + } + else if('1' <= c && c <= '9') { minSize = 0; while('0' <= c && c <= '9') @@ -108,6 +265,29 @@ int vsnprintf(char *__s, size_t __maxlen, const char *__format, va_list args) c = *__format++; } } + else + minSize = 0; + + // - Precision + precision = -1; + if( c == '.' ) { + c = *__format++; + + if(c == '*') { // Dynamic length + precision = va_arg(args, unsigned int); + c = *__format++; + } + else if('1' <= c && c <= '9') + { + precision = 0; + while('0' <= c && c <= '9') + { + precision *= 10; + precision += c - '0'; + c = *__format++; + } + } + } // - Default, Long or LongLong? isLongLong = 0; @@ -115,9 +295,6 @@ int vsnprintf(char *__s, size_t __maxlen, const char *__format, va_list args) { c = *__format++; if(c == 'l') { - #if BITS == 32 - val |= (Uint64)va_arg(args, Uint) << 32; - #endif c = *__format++; isLongLong = 1; } @@ -129,40 +306,69 @@ int vsnprintf(char *__s, size_t __maxlen, const char *__format, va_list args) { case 'd': case 'i': - if( (isLongLong && val >> 63) || (!isLongLong && val >> 31) ) { + GETVAL(); + if( isLongLong && val >> 63 ) { PUTCH('-'); val = -val; } - itoa(p, val, 10, minSize, pad); + else if( !isLongLong && val >> 31 ) { + PUTCH('-'); + val = -(Sint32)val; + } + itoa(tmpBuf, val, 10, minSize, pad); + goto printString; + case 'u': // Unsigned + GETVAL(); + itoa(tmpBuf, val, 10, minSize, pad); goto printString; - case 'u': - itoa(p, val, 10, minSize, pad); + case 'P': // Physical Address + PUTCH('0'); + PUTCH('x'); + if(sizeof(tPAddr) > 4) isLongLong = 1; + GETVAL(); + itoa(tmpBuf, val, 16, minSize, pad); goto printString; - case 'x': - itoa(p, val, 16, minSize, pad); + case 'X': // Hex + if(BITS == 64) + isLongLong = 1; // TODO: Handle non-x86 64-bit archs + GETVAL(); + itoa(tmpBuf, val, 16, minSize, pad); goto printString; - case 'o': - itoa(p, val, 8, minSize, pad); + + case 'x': // Lower case hex + GETVAL(); + itoa(tmpBuf, val, 16, minSize, pad); + goto printString; + case 'o': // Octal + GETVAL(); + itoa(tmpBuf, val, 8, minSize, pad); goto printString; case 'b': - itoa(p, val, 2, minSize, pad); + GETVAL(); + itoa(tmpBuf, val, 2, minSize, pad); goto printString; case 'B': //Boolean + val = va_arg(args, unsigned int); if(val) p = "True"; else p = "False"; goto printString; // String - Null Terminated Array case 's': - p = (char*)(Uint)val; + p = va_arg(args, char*); // Get Argument + if( !p || !CheckString(p) ) p = "(inval)"; // Avoid #PFs printString: if(!p) p = "(null)"; - while(*p) PUTCH(*p++); + len = strlen(p); + if( !bPadLeft ) while(len++ < minSize) PUTCH(pad); + while(*p && precision--) PUTCH(*p++); + if( bPadLeft ) while(len++ < minSize) PUTCH(pad); break; case 'C': // Non-Null Terminated Character Array - p = (char*)(Uint)val; + p = va_arg(args, char*); + if( !CheckMem(p, minSize) ) continue; // No #PFs please if(!p) goto printString; while(minSize--) PUTCH(*p++); break; @@ -170,6 +376,7 @@ int vsnprintf(char *__s, size_t __maxlen, const char *__format, va_list args) // Single Character case 'c': default: + GETVAL(); PUTCH( (Uint8)val ); break; } @@ -177,7 +384,6 @@ int vsnprintf(char *__s, size_t __maxlen, const char *__format, va_list args) if(__s && pos != __maxlen) __s[pos] = '\0'; - return pos; } @@ -209,7 +415,7 @@ 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(const char *Str1, const char *Str2) @@ -219,6 +425,18 @@ int strucmp(const char *Str1, const char *Str2) return tolower(*Str1) - tolower(*Str2); } +/** + * \brief Locate a byte in a string + */ +char *strchr(const char *__s, int __c) +{ + for( ; *__s; __s ++ ) + { + if( *__s == __c ) return (char*)__s; + } + return NULL; +} + /** * \fn int strpos(const char *Str, char Ch) * \brief Search a string for an ascii character @@ -237,26 +455,26 @@ int strpos(const char *Str, char Ch) * \fn Uint8 ByteSum(void *Ptr, int Size) * \brief Adds the bytes in a memory region and returns the sum */ -Uint8 ByteSum(void *Ptr, int Size) +Uint8 ByteSum(const void *Ptr, int Size) { Uint8 sum = 0; - while(Size--) sum += *(Uint8*)Ptr++; + const Uint8 *data = Ptr; + while(Size--) sum += *(data++); return sum; } /** - * \fn Uint strlen(const char *__str) + * \fn size_t strlen(const char *__str) * \brief Get the length of string */ -Uint strlen(const char *__str) +size_t strlen(const char *__str) { - Uint ret = 0; + size_t ret = 0; while(*__str++) ret++; return ret; } /** - * \fn char *strcpy(const char *__str1, const char *__str2) * \brief Copy a string to a new location */ char *strcpy(char *__str1, const char *__str2) @@ -268,8 +486,8 @@ char *strcpy(char *__str1, const char *__str2) } /** - * \fn char *strcpy(const char *__str1, const char *__str2) * \brief Copy a string to a new location + * \note Copies at most `max` chars */ char *strncpy(char *__str1, const char *__str2, size_t max) { @@ -280,6 +498,32 @@ char *strncpy(char *__str1, const char *__str2, size_t max) return __str1; } +/** + * \brief Append a string to another + */ +char *strcat(char *__dest, const char *__src) +{ + while(*__dest++); + __dest--; + while(*__src) + *__dest++ = *__src++; + *__dest = '\0'; + return __dest; +} + +/** + * \brief Append at most \a n chars to a string from another + * \note At most n+1 chars are written (the dest is always zero terminated) + */ +char *strncat(char *__dest, const char *__src, size_t n) +{ + while(*__dest++); + while(*__src && n-- >= 1) + *__dest++ = *__src++; + *__dest = '\0'; + return __dest; +} + /** * \fn int strcmp(const char *str1, const char *str2) * \brief Compare two strings return the difference between @@ -304,6 +548,7 @@ int strncmp(const char *Str1, const char *Str2, size_t num) return *Str1-*Str2; } +#if 0 /** * \fn char *strdup(const char *Str) * \brief Duplicates a string @@ -312,9 +557,66 @@ char *strdup(const char *Str) { char *ret; ret = malloc(strlen(Str)+1); + if( !ret ) return NULL; strcpy(ret, Str); return ret; } +#else + +/** + * \fn char *_strdup(const char *File, int Line, const char *Str) + * \brief Duplicates a string + */ +char *_strdup(const char *File, int Line, const char *Str) +{ + char *ret; + ret = Heap_Allocate(File, Line, strlen(Str)+1); + if( !ret ) return NULL; + strcpy(ret, Str); + return ret; +} +#endif + +/** + * \brief Split a string using the passed character + * \return NULL terminated array of strings on the heap + * \param __str String to split + * \param __ch Character to split by + */ +char **str_split(const char *__str, char __ch) +{ + int i, j; + int len = 1; + char **ret; + char *start; + + for( i = 0; __str[i]; i++ ) + { + if(__str[i] == __ch) + len ++; + } + + ret = malloc( sizeof(char*)*(len+1) + (i + 1) ); + if( !ret ) return NULL; + + j = 1; + start = (char *)&ret[len+1]; + ret[0] = start; + for( i = 0; __str[i]; i++ ) + { + if(__str[i] == __ch) { + *start++ = '\0'; + ret[j++] = start; + } + else { + *start++ = __str[i]; + } + } + *start = '\0'; + ret[j] = NULL; + + return ret; +} /** * \fn int DivUp(int num, int dem) @@ -354,7 +656,7 @@ int strpos8(const char *str, Uint32 Search) * \fn int ReadUTF8(Uint8 *str, Uint32 *Val) * \brief Read a UTF-8 character from a string */ -int ReadUTF8(Uint8 *str, Uint32 *Val) +int ReadUTF8(const Uint8 *str, Uint32 *Val) { *Val = 0xFFFD; // Assume invalid character @@ -474,7 +776,7 @@ Sint64 timestamp(int sec, int mins, int hrs, int day, int month, int year) ) && month > 1) // Leap year and after feb stamp += 3600*24; - stamp += ((365*4+1) * ((year-2000)&~3)) * 3600*24; // Foour Year Segments + stamp += ((365*4+1) * ((year-2000)&~3)) * 3600*24; // Four Year Segments stamp += ((year-2000)&3) * 365*3600*24; // Inside four year segment stamp += UNIX_TO_2K; @@ -482,27 +784,46 @@ Sint64 timestamp(int sec, int mins, int hrs, int day, int month, int year) } /** - * \fn Uint rand() + * \fn int rand() * \brief Pseudo random number generator - * \note Unknown effectiveness (made up on the spot) */ -Uint rand() +int rand(void) { - Uint old = giRandomState; + #if 0 + static Uint state = RANDOM_SEED; + Uint old = state; // Get the next state value - giRandomState = (RANDOM_A*giRandomState + RANDOM_C) & 0xFFFFFFFF; + giRandomState = (RANDOM_A*state + RANDOM_C); // Check if it has changed, and if it hasn't, change it - if(giRandomState == old) giRandomState += RANDOM_SPRUCE; - return giRandomState; + if(state == old) state += RANDOM_SPRUCE; + return state; + #else + // http://en.wikipedia.org/wiki/Xorshift + // 2010-10-03 + static Uint32 x = 123456789; + static Uint32 y = 362436069; + static Uint32 z = 521288629; + static Uint32 w = 88675123; + Uint32 t; + + t = x ^ (x << 11); + x = y; y = z; z = w; + return w = w ^ (w >> 19) ^ t ^ (t >> 8); + #endif } -/// \name Memory Validation -/// \{ +/* * + * \name Memory Validation + * \{ + */ /** * \brief Checks if a string resides fully in valid memory */ -int CheckString(char *String) +int CheckString(const char *String) { + if( !MM_GetPhysAddr( (tVAddr)String ) ) + return 0; + // Check 1st page if( MM_IsUser( (tVAddr)String ) ) { @@ -533,10 +854,14 @@ int CheckString(char *String) /** * \brief Check if a sized memory region is valid memory + * \return Boolean success */ -int CheckMem(void *Mem, int NumBytes) +int CheckMem(const void *Mem, int NumBytes) { tVAddr addr = (tVAddr)Mem; + + if( !MM_GetPhysAddr( addr ) ) + return 0; if( MM_IsUser( addr ) ) { @@ -560,13 +885,15 @@ int CheckMem(void *Mem, int NumBytes) } 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 ModUtil_LookupString(const char **Array, const char *Needle) { int i; if( !CheckString(Needle) ) return -1; @@ -577,23 +904,91 @@ int ModUtil_LookupString(char **Array, char *Needle) return -1; } -int ModUtil_SetIdent(char *Dest, char *Value) +int ModUtil_SetIdent(char *Dest, const char *Value) { if( !CheckMem(Dest, 32) ) return -1; strncpy(Dest, Value, 32); return 1; } -EXPORT(strlen); -EXPORT(strdup); -EXPORT(strcmp); -EXPORT(strncmp); -EXPORT(strcpy); -EXPORT(strncpy); +/** + * \brief Convert a string of hexadecimal digits into a byte stream + */ +int UnHex(Uint8 *Dest, size_t DestSize, const char *SourceString) +{ + int i; + + for( i = 0; i < DestSize*2; i += 2 ) + { + Uint8 val = 0; + + if(SourceString[i] == '\0') break; + + if('0' <= SourceString[i] && SourceString[i] <= '9') + val |= (SourceString[i]-'0') << 4; + else if('A' <= SourceString[i] && SourceString[i] <= 'F') + val |= (SourceString[i]-'A'+10) << 4; + else if('a' <= SourceString[i] && SourceString[i] <= 'f') + val |= (SourceString[i]-'a'+10) << 4; + + if(SourceString[i+1] == '\0') break; + + if('0' <= SourceString[i+1] && SourceString[i+1] <= '9') + val |= (SourceString[i+1] - '0'); + else if('A' <= SourceString[i+1] && SourceString[i+1] <= 'F') + val |= (SourceString[i+1] - 'A' + 10); + else if('a' <= SourceString[i+1] && SourceString[i+1] <= 'f') + val |= (SourceString[i+1] - 'a' + 10); + + Dest[i/2] = val; + } + return i/2; +} + +Uint16 SwapEndian16(Uint16 Val) +{ + return ((Val&0xFF)<<8) | ((Val>>8)&0xFF); +} +Uint32 SwapEndian32(Uint32 Val) +{ + return ((Val&0xFF)<<24) | ((Val&0xFF00)<<8) | ((Val>>8)&0xFF00) | ((Val>>24)&0xFF); +} + +void *memmove(void *__dest, const void *__src, size_t len) +{ + size_t block_size; + char *dest = __dest; + const char *src = __src; + void *ret = __dest; + + if( len == 0 || dest == src ) + return dest; + + if( (tVAddr)dest > (tVAddr)src + len ) + return memcpy(dest, src, len); + if( (tVAddr)dest + len < (tVAddr)src ) + return memcpy(dest, src, len); + + // NOTE: Assumes memcpy works forward + if( (tVAddr)dest < (tVAddr)src ) + return memcpy(dest, src, len); + + if( (tVAddr)dest < (tVAddr)src ) + block_size = (tVAddr)src - (tVAddr)dest; + else + block_size = (tVAddr)dest - (tVAddr)src; + + block_size &= ~0xF; + + while(len >= block_size) + { + memcpy(dest, src, block_size); + len -= block_size; + dest += block_size; + src += block_size; + } + memcpy(dest, src, len); + return ret; + +} -EXPORT(timestamp); -EXPORT(ReadUTF8); -EXPORT(CheckMem); -EXPORT(CheckString); -EXPORT(ModUtil_LookupString); -EXPORT(ModUtil_SetIdent);