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