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