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