Kernel/lib.c

   1 /*
   2  * Acess2
   3  * Common Library Functions
   4  */
   5 #include <acess.h>
   6
   7 // === CONSTANTS ===
   8 #define RANDOM_SEED     0xACE55052
   9 #define RANDOM_A        0x00731ADE
  10 #define RANDOM_C        12345
  11 #define RANDOM_SPRUCE   0xf12b02b
  12 //                          Jan Feb Mar Apr May  Jun  Jul  Aug  Sept Oct  Nov  Dec
  13 const short DAYS_BEFORE[] = {0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334};
  14 #define UNIX_TO_2K      ((30*365*3600*24) + (7*3600*24))        //Normal years + leap years
  15
  16 // === PROTOTYPES ===
  17  int    atoi(const char *string);
  18 void    itoa(char *buf, Uint num, int base, int minLength, char pad);
  19  int    vsnprintf(char *__s, size_t __maxlen, const char *__format, va_list args);
  20  int    sprintf(char *__s, const char *__format, ...);
  21  int    tolower(int c);
  22  int    strucmp(const char *Str1, const char *Str2);
  23  int    strpos(const char *Str, char Ch);
  24  Uint8  ByteSum(void *Ptr, int Size);
  25 size_t  strlen(const char *__s);
  26 char    *strcpy(char *__str1, const char *__str2);
  27 char    *strncpy(char *__str1, const char *__str2, size_t max);
  28  int    strcmp(const char *str1, const char *str2);
  29  int    strncmp(const char *str1, const char *str2, size_t num);
  30 char    *strdup(const char *Str);
  31  int    DivUp(int num, int dem);
  32  int    strpos8(const char *str, Uint32 Search);
  33  int    ReadUTF8(Uint8 *str, Uint32 *Val);
  34  int    WriteUTF8(Uint8 *str, Uint32 Val);
  35 Sint64  timestamp(int sec, int mins, int hrs, int day, int month, int year);
  36 Uint    rand(void);
  37  int    CheckString(char *String);
  38  int    CheckMem(void *Mem, int NumBytes);
  39  int    ModUtil_LookupString(char **Array, char *Needle);
  40  int    ModUtil_SetIdent(char *Dest, char *Value);
  41
  42 // === EXPORTS ===
  43 EXPORT(atoi);
  44 EXPORT(itoa);
  45 EXPORT(vsnprintf);
  46 EXPORT(sprintf);
  47 EXPORT(tolower);
  48 EXPORT(strucmp);
  49 EXPORT(strpos);
  50 EXPORT(ByteSum);
  51 EXPORT(strlen);
  52 EXPORT(strcpy);
  53 EXPORT(strncpy);
  54 EXPORT(strcmp);
  55 EXPORT(strncmp);
  56 EXPORT(strdup);
  57 EXPORT(DivUp);
  58 EXPORT(strpos8);
  59 EXPORT(ReadUTF8);
  60 EXPORT(WriteUTF8);
  61 EXPORT(timestamp);
  62 EXPORT(CheckString);
  63 EXPORT(CheckMem);
  64 EXPORT(ModUtil_LookupString);
  65 EXPORT(ModUtil_SetIdent);
  66
  67 // === GLOBALS ===
  68 static Uint     giRandomState = RANDOM_SEED;
  69
  70 // === CODE ===
  71 /**
  72  * \brief Convert a string into an integer
  73  */
  74 int atoi(const char *string)
  75 {
  76          int    ret = 0;
  77          int    bNeg = 0;
  78
  79         //Log("atoi: (string='%s')", string);
  80
  81         // Clear non-numeric characters
  82         while( !('0' <= *string && *string <= '9') && *string != '-' )  string++;
  83         if( *string == '-' ) {
  84                 bNeg = 1;
  85                 while( !('0' <= *string && *string <= '9') )    string++;
  86         }
  87
  88         if(*string == '0')
  89         {
  90                 string ++;
  91                 if(*string == 'x')
  92                 {
  93                         // Hex
  94                         string ++;
  95                         for( ;; string ++ )
  96                         {
  97                                 if('0' <= *string && *string <= '9') {
  98                                         ret *= 16;
  99                                         ret += *string - '0';
 100                                 }
 101                                 else if('A' <= *string && *string <= 'F') {
 102                                         ret *= 16;
 103                                         ret += *string - 'A' + 10;
 104                                 }
 105                                 else if('a' <= *string && *string <= 'f') {
 106                                         ret *= 16;
 107                                         ret += *string - 'a' + 10;
 108                                 }
 109                                 else
 110                                         break;
 111                         }
 112                 }
 113                 else    // Octal
 114                 {
 115                         for( ; '0' <= *string && *string <= '7'; string ++ )
 116                         {
 117                                 ret *= 8;
 118                                 ret += *string - '0';
 119                         }
 120                 }
 121         }
 122         else    // Decimal
 123         {
 124                 for( ; '0' <= *string && *string <= '9'; string++)
 125                 {
 126                         ret *= 10;
 127                         ret += *string - '0';
 128                 }
 129         }
 130
 131         if(bNeg)        ret = -ret;
 132
 133         //Log("atoi: RETURN %i", ret);
 134
 135         return ret;
 136 }
 137
 138 static const char cUCDIGITS[] = "0123456789ABCDEF";
 139 /**
 140  * \fn void itoa(char *buf, Uint num, int base, int minLength, char pad)
 141  * \brief Convert an integer into a character string
 142  */
 143 void itoa(char *buf, Uint num, int base, int minLength, char pad)
 144 {
 145         char    tmpBuf[BITS];
 146          int    pos=0, i;
 147
 148         // Sanity check
 149         if(!buf)        return;
 150
 151         // Sanity Check
 152         if(base > 16 || base < 2) {
 153                 buf[0] = 0;
 154                 return;
 155         }
 156
 157         // Convert
 158         while(num > base-1) {
 159                 tmpBuf[pos] = cUCDIGITS[ num % base ];
 160                 num /= (Uint)base;              // Shift `num` right 1 digit
 161                 pos++;
 162         }
 163         tmpBuf[pos++] = cUCDIGITS[ num % base ];                // Last digit of `num`
 164
 165         // Put in reverse
 166         i = 0;
 167         minLength -= pos;
 168         while(minLength-- > 0)  buf[i++] = pad;
 169         while(pos-- > 0)                buf[i++] = tmpBuf[pos]; // Reverse the order of characters
 170         buf[i] = 0;
 171 }
 172
 173 /**
 174  * \brief Append a character the the vsnprintf output
 175  */
 176 #define PUTCH(c)        do{\
 177         char ch=(c);\
 178         if(pos==__maxlen){return pos;}\
 179         if(__s){__s[pos++]=ch;}else{pos++;}\
 180         }while(0)
 181 int vsnprintf(char *__s, size_t __maxlen, const char *__format, va_list args)
 182 {
 183         char    c, pad = ' ';
 184          int    minSize = 0;
 185         char    tmpBuf[34];     // For Integers
 186         char    *p = NULL;
 187          int    isLongLong = 0;
 188         Uint64  val;
 189         size_t  pos = 0;
 190         // Flags
 191         // int  bPadLeft = 0;
 192
 193         //Log("vsnprintf: (__s=%p, __maxlen=%i, __format='%s', ...)", __s, __maxlen, __format);
 194
 195         while((c = *__format++) != 0)
 196         {
 197                 // Non control character
 198                 if(c != '%') { PUTCH(c); continue; }
 199
 200                 c = *__format++;
 201                 //Log("pos = %i", pos);
 202
 203                 // Literal %
 204                 if(c == '%') { PUTCH('%'); continue; }
 205
 206                 // Pointer - Done first for debugging
 207                 if(c == 'p') {
 208                         Uint    ptr = va_arg(args, Uint);
 209                         PUTCH('*');     PUTCH('0');     PUTCH('x');
 210                         p = tmpBuf;
 211                         itoa(p, ptr, 16, BITS/4, '0');
 212                         goto printString;
 213                 }
 214
 215                 // Get Argument
 216                 val = va_arg(args, Uint);
 217                 //Log("val = %x", val);
 218
 219                 // - Padding
 220                 if(c == '0') {
 221                         pad = '0';
 222                         c = *__format++;
 223                 }
 224                 else
 225                         pad = ' ';
 226
 227                 // - Minimum length
 228                 minSize = 1;
 229                 if('1' <= c && c <= '9')
 230                 {
 231                         minSize = 0;
 232                         while('0' <= c && c <= '9')
 233                         {
 234                                 minSize *= 10;
 235                                 minSize += c - '0';
 236                                 c = *__format++;
 237                         }
 238                 }
 239
 240                 // - Default, Long or LongLong?
 241                 isLongLong = 0;
 242                 if(c == 'l')    // Long is actually the default on x86
 243                 {
 244                         c = *__format++;
 245                         if(c == 'l') {
 246                                 #if BITS == 32
 247                                 val |= (Uint64)va_arg(args, Uint) << 32;
 248                                 #endif
 249                                 c = *__format++;
 250                                 isLongLong = 1;
 251                         }
 252                 }
 253
 254                 // - Now get the format code
 255                 p = tmpBuf;
 256                 switch(c)
 257                 {
 258                 case 'd':
 259                 case 'i':
 260                         if( (isLongLong && val >> 63) || (!isLongLong && val >> 31) ) {
 261                                 PUTCH('-');
 262                                 val = -val;
 263                         }
 264                         itoa(p, val, 10, minSize, pad);
 265                         goto printString;
 266                 case 'u':
 267                         itoa(p, val, 10, minSize, pad);
 268                         goto printString;
 269                 case 'x':
 270                         itoa(p, val, 16, minSize, pad);
 271                         goto printString;
 272                 case 'o':
 273                         itoa(p, val, 8, minSize, pad);
 274                         goto printString;
 275                 case 'b':
 276                         itoa(p, val, 2, minSize, pad);
 277                         goto printString;
 278
 279                 case 'B':       //Boolean
 280                         if(val) p = "True";
 281                         else    p = "False";
 282                         goto printString;
 283
 284                 // String - Null Terminated Array
 285                 case 's':
 286                         p = (char*)(Uint)val;
 287                 printString:
 288                         //Log("p = '%s'", p);
 289                         if(!p)          p = "(null)";
 290                         while(*p)       PUTCH(*p++);
 291                         break;
 292
 293                 case 'C':       // Non-Null Terminated Character Array
 294                         p = (char*)(Uint)val;
 295                         if(!p)  goto printString;
 296                         while(minSize--)        PUTCH(*p++);
 297                         break;
 298
 299                 // Single Character
 300                 case 'c':
 301                 default:
 302                         PUTCH( (Uint8)val );
 303                         break;
 304                 }
 305         }
 306
 307         if(__s && pos != __maxlen)
 308                 __s[pos] = '\0';
 309
 310         return pos;
 311 }
 312 #undef PUTCH
 313
 314 /**
 315  */
 316 int sprintf(char *__s, const char *__format, ...)
 317 {
 318         va_list args;
 319          int    ret;
 320
 321         va_start(args, __format);
 322         ret = vsnprintf(__s, -1, __format, args);
 323         va_end(args);
 324
 325         return ret;
 326 }
 327
 328 /**
 329  * \fn int tolower(int c)
 330  * \brief Converts a character to lower case
 331  */
 332 int tolower(int c)
 333 {
 334         if('A' <= c && c <= 'Z')
 335                 return c - 'A' + 'a';
 336         return c;
 337 }
 338
 339 /**
 340  * \fn int strucmp(const char *Str1, const char *Str2)
 341  * \brief Compare \a Str1 and \a Str2 case-insensitively
 342  */
 343 int strucmp(const char *Str1, const char *Str2)
 344 {
 345         while(*Str1 && tolower(*Str1) == tolower(*Str2))
 346                 Str1++, Str2++;
 347         return tolower(*Str1) - tolower(*Str2);
 348 }
 349
 350 /**
 351  * \fn int strpos(const char *Str, char Ch)
 352  * \brief Search a string for an ascii character
 353  */
 354 int strpos(const char *Str, char Ch)
 355 {
 356          int    pos;
 357         for(pos=0;Str[pos];pos++)
 358         {
 359                 if(Str[pos] == Ch)      return pos;
 360         }
 361         return -1;
 362 }
 363
 364 /**
 365  * \fn Uint8 ByteSum(void *Ptr, int Size)
 366  * \brief Adds the bytes in a memory region and returns the sum
 367  */
 368 Uint8 ByteSum(void *Ptr, int Size)
 369 {
 370         Uint8   sum = 0;
 371         while(Size--)   sum += *(Uint8*)Ptr++;
 372         return sum;
 373 }
 374
 375 /**
 376  * \fn Uint strlen(const char *__str)
 377  * \brief Get the length of string
 378  */
 379 Uint strlen(const char *__str)
 380 {
 381         Uint    ret = 0;
 382         while(*__str++) ret++;
 383         return ret;
 384 }
 385
 386 /**
 387  * \fn char *strcpy(char *__str1, const char *__str2)
 388  * \brief Copy a string to a new location
 389  */
 390 char *strcpy(char *__str1, const char *__str2)
 391 {
 392         while(*__str2)
 393                 *__str1++ = *__str2++;
 394         *__str1 = '\0'; // Terminate String
 395         return __str1;
 396 }
 397
 398 /**
 399  * \fn char *strncpy(char *__str1, const char *__str2, size_t max)
 400  * \brief Copy a string to a new location
 401  */
 402 char *strncpy(char *__str1, const char *__str2, size_t max)
 403 {
 404         while(*__str2 && max-- >= 1)
 405                 *__str1++ = *__str2++;
 406         if(max)
 407                 *__str1 = '\0'; // Terminate String
 408         return __str1;
 409 }
 410
 411 /**
 412  * \fn int strcmp(const char *str1, const char *str2)
 413  * \brief Compare two strings return the difference between
 414  *        the first non-matching characters.
 415  */
 416 int strcmp(const char *str1, const char *str2)
 417 {
 418         while(*str1 && *str1 == *str2)
 419                 str1++, str2++;
 420         return *str1 - *str2;
 421 }
 422
 423 /**
 424  * \fn int strncmp(const char *Str1, const char *Str2, size_t num)
 425  * \brief Compare strings \a Str1 and \a Str2 to a maximum of \a num characters
 426  */
 427 int strncmp(const char *Str1, const char *Str2, size_t num)
 428 {
 429         if(num == 0)    return 0;       // TODO: Check what should officially happen here
 430         while(--num && *Str1 && *Str1 == *Str2)
 431                 Str1++, Str2++;
 432         return *Str1-*Str2;
 433 }
 434
 435 /**
 436  * \fn char *strdup(const char *Str)
 437  * \brief Duplicates a string
 438  */
 439 char *strdup(const char *Str)
 440 {
 441         char    *ret;
 442         ret = malloc(strlen(Str)+1);
 443         strcpy(ret, Str);
 444         return ret;
 445 }
 446
 447 /**
 448  * \fn int DivUp(int num, int dem)
 449  * \brief Divide two numbers, rounding up
 450  * \param num   Numerator
 451  * \param dem   Denominator
 452  */
 453 int DivUp(int num, int dem)
 454 {
 455         return (num+dem-1)/dem;
 456 }
 457
 458 /**
 459  * \fn int strpos8(const char *str, Uint32 search)
 460  * \brief Search a string for a UTF-8 character
 461  */
 462 int strpos8(const char *str, Uint32 Search)
 463 {
 464          int    pos;
 465         Uint32  val = 0;
 466         for(pos=0;str[pos];pos++)
 467         {
 468                 // ASCII Range
 469                 if(Search < 128) {
 470                         if(str[pos] == Search)  return pos;
 471                         continue;
 472                 }
 473                 if(*(Uint8*)(str+pos) < 128)    continue;
 474
 475                 pos += ReadUTF8( (Uint8*)&str[pos], &val );
 476                 if(val == Search)       return pos;
 477         }
 478         return -1;
 479 }
 480
 481 /**
 482  * \fn int ReadUTF8(Uint8 *str, Uint32 *Val)
 483  * \brief Read a UTF-8 character from a string
 484  */
 485 int ReadUTF8(Uint8 *str, Uint32 *Val)
 486 {
 487         *Val = 0xFFFD;  // Assume invalid character
 488
 489         // ASCII
 490         if( !(*str & 0x80) ) {
 491                 *Val = *str;
 492                 return 1;
 493         }
 494
 495         // Middle of a sequence
 496         if( (*str & 0xC0) == 0x80 ) {
 497                 return 1;
 498         }
 499
 500         // Two Byte
 501         if( (*str & 0xE0) == 0xC0 ) {
 502                 *Val = (*str & 0x1F) << 6;      // Upper 6 Bits
 503                 str ++;
 504                 if( (*str & 0xC0) != 0x80)      return -1;      // Validity check
 505                 *Val |= (*str & 0x3F);  // Lower 6 Bits
 506                 return 2;
 507         }
 508
 509         // Three Byte
 510         if( (*str & 0xF0) == 0xE0 ) {
 511                 *Val = (*str & 0x0F) << 12;     // Upper 4 Bits
 512                 str ++;
 513                 if( (*str & 0xC0) != 0x80)      return -1;      // Validity check
 514                 *Val |= (*str & 0x3F) << 6;     // Middle 6 Bits
 515                 str ++;
 516                 if( (*str & 0xC0) != 0x80)      return -1;      // Validity check
 517                 *Val |= (*str & 0x3F);  // Lower 6 Bits
 518                 return 3;
 519         }
 520
 521         // Four Byte
 522         if( (*str & 0xF1) == 0xF0 ) {
 523                 *Val = (*str & 0x07) << 18;     // Upper 3 Bits
 524                 str ++;
 525                 if( (*str & 0xC0) != 0x80)      return -1;      // Validity check
 526                 *Val |= (*str & 0x3F) << 12;    // Middle-upper 6 Bits
 527                 str ++;
 528                 if( (*str & 0xC0) != 0x80)      return -1;      // Validity check
 529                 *Val |= (*str & 0x3F) << 6;     // Middle-lower 6 Bits
 530                 str ++;
 531                 if( (*str & 0xC0) != 0x80)      return -1;      // Validity check
 532                 *Val |= (*str & 0x3F);  // Lower 6 Bits
 533                 return 4;
 534         }
 535
 536         // UTF-8 Doesn't support more than four bytes
 537         return 4;
 538 }
 539
 540 /**
 541  * \fn int WriteUTF8(Uint8 *str, Uint32 Val)
 542  * \brief Write a UTF-8 character sequence to a string
 543  */
 544 int WriteUTF8(Uint8 *str, Uint32 Val)
 545 {
 546         // ASCII
 547         if( Val < 128 ) {
 548                 *str = Val;
 549                 return 1;
 550         }
 551
 552         // Two Byte
 553         if( Val < 0x8000 ) {
 554                 *str = 0xC0 | (Val >> 6);
 555                 str ++;
 556                 *str = 0x80 | (Val & 0x3F);
 557                 return 2;
 558         }
 559
 560         // Three Byte
 561         if( Val < 0x10000 ) {
 562                 *str = 0xE0 | (Val >> 12);
 563                 str ++;
 564                 *str = 0x80 | ((Val >> 6) & 0x3F);
 565                 str ++;
 566                 *str = 0x80 | (Val & 0x3F);
 567                 return 3;
 568         }
 569
 570         // Four Byte
 571         if( Val < 0x110000 ) {
 572                 *str = 0xF0 | (Val >> 18);
 573                 str ++;
 574                 *str = 0x80 | ((Val >> 12) & 0x3F);
 575                 str ++;
 576                 *str = 0x80 | ((Val >> 6) & 0x3F);
 577                 str ++;
 578                 *str = 0x80 | (Val & 0x3F);
 579                 return 4;
 580         }
 581
 582         // UTF-8 Doesn't support more than four bytes
 583         return 0;
 584 }
 585
 586 /**
 587  * \fn Uint64 timestamp(int sec, int mins, int hrs, int day, int month, int year)
 588  * \brief Converts a date into an Acess Timestamp
 589  */
 590 Sint64 timestamp(int sec, int mins, int hrs, int day, int month, int year)
 591 {
 592         Sint64  stamp;
 593         stamp = sec;
 594         stamp += mins*60;
 595         stamp += hrs*3600;
 596
 597         stamp += day*3600*24;
 598         stamp += month*DAYS_BEFORE[month]*3600*24;
 599         if(     (
 600                 ((year&3) == 0 || year%100 != 0)
 601                 || (year%100 == 0 && ((year/100)&3) == 0)
 602                 ) && month > 1) // Leap year and after feb
 603                 stamp += 3600*24;
 604
 605         stamp += ((365*4+1) * ((year-2000)&~3)) * 3600*24;      // Foour Year Segments
 606         stamp += ((year-2000)&3) * 365*3600*24; // Inside four year segment
 607         stamp += UNIX_TO_2K;
 608
 609         return stamp * 1000;
 610 }
 611
 612 /**
 613  * \fn Uint rand()
 614  * \brief Pseudo random number generator
 615  * \note Unknown effectiveness (made up on the spot)
 616  */
 617 Uint rand(void)
 618 {
 619         Uint    old = giRandomState;
 620         // Get the next state value
 621         giRandomState = (RANDOM_A*giRandomState + RANDOM_C) & 0xFFFFFFFF;
 622         // Check if it has changed, and if it hasn't, change it
 623         if(giRandomState == old)        giRandomState += RANDOM_SPRUCE;
 624         return giRandomState;
 625 }
 626
 627 /// \name Memory Validation
 628 /// \{
 629 /**
 630  * \brief Checks if a string resides fully in valid memory
 631  */
 632 int CheckString(char *String)
 633 {
 634         // Check 1st page
 635         if( MM_IsUser( (tVAddr)String ) )
 636         {
 637                 // Traverse String
 638                 while( *String )
 639                 {
 640                         if( !MM_IsUser( (tVAddr)String ) )
 641                                 return 0;
 642                         // Increment string pointer
 643                         String ++;
 644                 }
 645                 return 1;
 646         }
 647         else if( MM_GetPhysAddr( (tVAddr)String ) )
 648         {
 649                 // Traverse String
 650                 while( *String )
 651                 {
 652                         if( !MM_GetPhysAddr( (tVAddr)String ) )
 653                                 return 0;
 654                         // Increment string pointer
 655                         String ++;
 656                 }
 657                 return 1;
 658         }
 659         return 0;
 660 }
 661
 662 /**
 663  * \brief Check if a sized memory region is valid memory
 664  */
 665 int CheckMem(void *Mem, int NumBytes)
 666 {
 667         tVAddr  addr = (tVAddr)Mem;
 668
 669         if( MM_IsUser( addr ) )
 670         {
 671                 while( NumBytes-- )
 672                 {
 673                         if( !MM_IsUser( addr ) )
 674                                 return 0;
 675                         addr ++;
 676                 }
 677                 return 1;
 678         }
 679         else if( MM_GetPhysAddr( addr ) )
 680         {
 681                 while( NumBytes-- )
 682                 {
 683                         if( !MM_GetPhysAddr( addr ) )
 684                                 return 0;
 685                         addr ++;
 686                 }
 687                 return 1;
 688         }
 689         return 0;
 690 }
 691 /// \}
 692
 693 /**
 694  * \brief Search a string array for \a Needle
 695  * \note Helper function for eTplDrv_IOCtl::DRV_IOCTL_LOOKUP
 696  */
 697 int ModUtil_LookupString(char **Array, char *Needle)
 698 {
 699          int    i;
 700         if( !CheckString(Needle) )      return -1;
 701         for( i = 0; Array[i]; i++ )
 702         {
 703                 if(strcmp(Array[i], Needle) == 0)       return i;
 704         }
 705         return -1;
 706 }
 707
 708 int ModUtil_SetIdent(char *Dest, char *Value)
 709 {
 710         if( !CheckMem(Dest, 32) )       return -1;
 711         strncpy(Dest, Value, 32);
 712         return 1;
 713 }