KernelLand/Kernel/lib.c

   1 /*
   2  * Acess2
   3  * Common Library Functions
   4  */
   5 #include <acess.h>
   6
   7 // === CONSTANTS ===
   8 //                          Jan Feb Mar Apr May  Jun  Jul  Aug  Sept Oct  Nov  Dec
   9 const short DAYS_BEFORE[] = {0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334};
  10 #define UNIX_TO_2K      ((30*365*3600*24) + (7*3600*24))        //Normal years + leap years
  11
  12 // === PROTOTYPES ===
  13 #if 0
  14  Uint8  ByteSum(void *Ptr, int Size);
  15 char    **str_split(const char *__str, char __ch);
  16  int    strpos8(const char *str, Uint32 Search);
  17  int    ReadUTF8(Uint8 *str, Uint32 *Val);
  18  int    WriteUTF8(Uint8 *str, Uint32 Val);
  19  int    DivUp(int num, int dem);
  20 Sint64  timestamp(int sec, int mins, int hrs, int day, int month, int year);
  21 void    format_date(tTime TS, int *year, int *month, int *day, int *hrs, int *mins, int *sec, int *ms);
  22
  23  int    ModUtil_LookupString(char **Array, char *Needle);
  24  int    ModUtil_SetIdent(char *Dest, char *Value);
  25
  26  int    Hex(char *Dest, size_t Size, const Uint8 *SourceData);
  27  int    UnHex(Uint8 *Dest, size_t DestSize, const char *SourceString);
  28
  29 Uint16  SwapEndian16(Uint16 Val);
  30 Uint32  SwapEndian32(Uint16 Val);
  31 Uint64  SwapEndian64(Uint16 Val);
  32 #endif
  33
  34 // === EXPORTS ===
  35 EXPORT(ByteSum);
  36 EXPORT(str_split);
  37 EXPORT(strpos8);
  38 EXPORT(DivUp);
  39 EXPORT(ReadUTF8);
  40 EXPORT(WriteUTF8);
  41 EXPORT(timestamp);
  42 EXPORT(ModUtil_LookupString);
  43 EXPORT(ModUtil_SetIdent);
  44 EXPORT(Hex);
  45 EXPORT(UnHex);
  46 EXPORT(SwapEndian16);
  47 EXPORT(SwapEndian32);
  48 EXPORT(SwapEndian64);
  49
  50 // === CODE ===
  51 /**
  52  * \fn Uint8 ByteSum(void *Ptr, int Size)
  53  * \brief Adds the bytes in a memory region and returns the sum
  54  */
  55 Uint8 ByteSum(const void *Ptr, int Size)
  56 {
  57         Uint8   sum = 0;
  58         const Uint8     *data = Ptr;
  59         while(Size--)   sum += *(data++);
  60         return sum;
  61 }
  62
  63 /**
  64  * \brief Split a string using the passed character
  65  * \return NULL terminated array of strings on the heap
  66  * \param __str String to split
  67  * \param __ch  Character to split by
  68  */
  69 char **str_split(const char *__str, char __ch)
  70 {
  71          int    i, j;
  72          int    len = 1;
  73         char    **ret;
  74         char    *start;
  75
  76         for( i = 0; __str[i]; i++ )
  77         {
  78                 if(__str[i] == __ch)
  79                         len ++;
  80         }
  81
  82         ret = malloc( sizeof(char*)*(len+1) + (i + 1) );
  83         if( !ret )      return NULL;
  84
  85         j = 1;
  86         start = (char *)&ret[len+1];
  87         ret[0] = start;
  88         for( i = 0; __str[i]; i++ )
  89         {
  90                 if(__str[i] == __ch) {
  91                         *start++ = '\0';
  92                         ret[j++] = start;
  93                 }
  94                 else {
  95                         *start++ = __str[i];
  96                 }
  97         }
  98         *start = '\0';
  99         ret[j] = NULL;
 100
 101         return ret;
 102 }
 103
 104 /**
 105  * \fn int DivUp(int num, int dem)
 106  * \brief Divide two numbers, rounding up
 107  * \param num   Numerator
 108  * \param dem   Denominator
 109  */
 110 int DivUp(int num, int dem)
 111 {
 112         return (num+dem-1)/dem;
 113 }
 114
 115 /**
 116  * \fn int strpos8(const char *str, Uint32 search)
 117  * \brief Search a string for a UTF-8 character
 118  */
 119 int strpos8(const char *str, Uint32 Search)
 120 {
 121          int    pos;
 122         Uint32  val = 0;
 123         for(pos=0;str[pos];pos++)
 124         {
 125                 // ASCII Range
 126                 if(Search < 128) {
 127                         if(str[pos] == (char)Search)    return pos;
 128                         continue;
 129                 }
 130                 if(*(Uint8*)(str+pos) < 128)    continue;
 131
 132                 pos += ReadUTF8( (Uint8*)&str[pos], &val );
 133                 if(val == Search)       return pos;
 134         }
 135         return -1;
 136 }
 137
 138 /**
 139  * \fn int ReadUTF8(Uint8 *str, Uint32 *Val)
 140  * \brief Read a UTF-8 character from a string
 141  */
 142 int ReadUTF8(const Uint8 *str, Uint32 *Val)
 143 {
 144         Uint32  outval;
 145
 146         *Val = 0xFFFD;  // Assume invalid character
 147
 148         // ASCII
 149         if( !(*str & 0x80) ) {
 150                 *Val = *str;
 151                 return 1;
 152         }
 153
 154         // Middle of a sequence
 155         if( (*str & 0xC0) == 0x80 ) {
 156                 return 1;
 157         }
 158
 159         // Two Byte
 160         if( (*str & 0xE0) == 0xC0 ) {
 161                 outval = (*str & 0x1F) << 6;    // Upper 6 Bits
 162                 str ++;
 163                 if( (*str & 0xC0) != 0x80)      return 2;       // Validity check
 164                 outval |= (*str & 0x3F);        // Lower 6 Bits
 165                 *Val = outval;
 166                 return 2;
 167         }
 168
 169         // Three Byte
 170         if( (*str & 0xF0) == 0xE0 ) {
 171                 outval = (*str & 0x0F) << 12;   // Upper 4 Bits
 172                 str ++;
 173                 if( (*str & 0xC0) != 0x80)      return 2;       // Validity check
 174                 outval |= (*str & 0x3F) << 6;   // Middle 6 Bits
 175                 str ++;
 176                 if( (*str & 0xC0) != 0x80)      return 3;       // Validity check
 177                 outval |= (*str & 0x3F);        // Lower 6 Bits
 178                 *Val = outval;
 179                 return 3;
 180         }
 181
 182         // Four Byte
 183         if( (*str & 0xF8) == 0xF0 ) {
 184                 outval = (*str & 0x07) << 18;   // Upper 3 Bits
 185                 str ++;
 186                 if( (*str & 0xC0) != 0x80)      return 2;       // Validity check
 187                 outval |= (*str & 0x3F) << 12;  // Middle-upper 6 Bits
 188                 str ++;
 189                 if( (*str & 0xC0) != 0x80)      return 3;       // Validity check
 190                 outval |= (*str & 0x3F) << 6;   // Middle-lower 6 Bits
 191                 str ++;
 192                 if( (*str & 0xC0) != 0x80)      return 4;       // Validity check
 193                 outval |= (*str & 0x3F);        // Lower 6 Bits
 194                 *Val = outval;
 195                 return 4;
 196         }
 197
 198         // UTF-8 Doesn't support more than four bytes
 199         return 4;
 200 }
 201
 202 /**
 203  * \fn int WriteUTF8(Uint8 *str, Uint32 Val)
 204  * \brief Write a UTF-8 character sequence to a string
 205  */
 206 int WriteUTF8(Uint8 *str, Uint32 Val)
 207 {
 208         // ASCII
 209         if( Val < 128 ) {
 210                 if( str ) {
 211                         *str = Val;
 212                 }
 213                 return 1;
 214         }
 215
 216         // Two Byte
 217         if( Val < 0x8000 ) {
 218                 if( str ) {
 219                         *str++ = 0xC0 | (Val >> 6);
 220                         *str++ = 0x80 | (Val & 0x3F);
 221                 }
 222                 return 2;
 223         }
 224
 225         // Three Byte
 226         if( Val < 0x10000 ) {
 227                 if( str ) {
 228                         *str++ = 0xE0 | (Val >> 12);
 229                         *str++ = 0x80 | ((Val >> 6) & 0x3F);
 230                         *str++ = 0x80 | (Val & 0x3F);
 231                 }
 232                 return 3;
 233         }
 234
 235         // Four Byte
 236         if( Val < 0x110000 ) {
 237                 if( str ) {
 238                         *str++ = 0xF0 | (Val >> 18);
 239                         *str++ = 0x80 | ((Val >> 12) & 0x3F);
 240                         *str++ = 0x80 | ((Val >> 6) & 0x3F);
 241                         *str++ = 0x80 | (Val & 0x3F);
 242                 }
 243                 return 4;
 244         }
 245
 246         // UTF-8 Doesn't support more than four bytes
 247         return 0;
 248 }
 249
 250 /**
 251  * \fn Uint64 timestamp(int sec, int mins, int hrs, int day, int month, int year)
 252  * \brief Converts a date into an Acess Timestamp
 253  */
 254 Sint64 timestamp(int sec, int min, int hrs, int day, int month, int year)
 255 {
 256          int    is_leap;
 257         Sint64  stamp;
 258
 259         if( !(0 <= sec && sec < 60) )   return 0;
 260         if( !(0 <= min && min < 60) )   return 0;
 261         if( !(0 <= hrs && hrs < 24) )   return 0;
 262         if( !(0 <= day && day < 31) )   return 0;
 263         if( !(0 <= month && month < 12) )       return 0;
 264
 265         stamp = DAYS_BEFORE[month] + day;
 266
 267         // Every 4 years
 268         // - every 100 years
 269         // + every 400 years
 270         is_leap = (year % 4 == 0) - (year % 100 == 0) + (year % 400 == 0);
 271         ASSERT(is_leap == 0 || is_leap == 1);
 272
 273         if( is_leap && month > 1 )      // Leap year and after feb
 274                 stamp += 1;
 275
 276         // Get seconds before the first of specified year
 277         year -= 2000;   // Base off Y2K
 278         // base year days + total leap year days
 279         stamp += year*365 + (year/400) - (year/100) + (year/4);
 280
 281         stamp *= 3600*24;
 282         stamp += UNIX_TO_2K;
 283         stamp += sec;
 284         stamp += min*60;
 285         stamp += hrs*3600;
 286
 287         return stamp * 1000;
 288 }
 289
 290 static Sint64 DivMod64(Sint64 N, Sint64 D, Sint64 *R);
 291
 292 static Sint64 DivMod64(Sint64 N, Sint64 D, Sint64 *R)
 293 {
 294         int sign = (N < 0) != (D < 0);
 295         if(N < 0)       N = -N;
 296         if(D < 0)       D = -D;
 297         if(sign)
 298                 return -DivMod64U(N, D, (Uint64*)R);
 299         else
 300                 return DivMod64U(N, D, (Uint64*)R);
 301 }
 302
 303 void format_date(tTime TS, int *year, int *month, int *day, int *hrs, int *mins, int *sec, int *ms)
 304 {
 305          int    is_leap = 0, i;
 306
 307         // Get time
 308         // TODO: Leap-seconds?
 309         {
 310                 Sint64  rem;
 311                 TS = DivMod64( TS, 1000, &rem );
 312                 *ms = rem;
 313                 TS = DivMod64( TS, 60, &rem );
 314                 *sec = rem;
 315                 TS = DivMod64( TS, 60, &rem );
 316                 *mins = rem;
 317                 TS = DivMod64( TS, 24, &rem );
 318                 *hrs = rem;
 319         }
 320
 321         // Adjust to Y2K
 322         TS -= UNIX_TO_2K/(3600*24);
 323
 324         // Year (400 yr blocks) - (400/4-3) leap years
 325         *year = 400 * DivMod64( TS, 365*400 + (400/4-3), &TS );
 326         if( TS < 366 )  // First year in 400 is a leap
 327                 is_leap = 1;
 328         else
 329         {
 330                 // 100 yr blocks - 100/4-1 leap years
 331                 *year += 100 * DivMod64( TS, 365*100 + (100/4-1), &TS );
 332                 if( TS < 366 )  // First year in 100 isn't a leap
 333                         is_leap = 0;
 334                 else
 335                 {
 336                         *year += 4 * DivMod64( TS, 365*4 + 1, &TS );
 337                         if( TS < 366 )  // First year in 4 is a leap
 338                                 is_leap = 1;
 339                         else
 340                         {
 341                                 *year += DivMod64( TS, 356, &TS );
 342                         }
 343                 }
 344         }
 345         *year += 2000;
 346
 347         ASSERT(TS >= 0);
 348
 349         *day = 0;
 350         // Month (if after the first of march, which is 29 Feb in a leap year)
 351         if( is_leap && TS > DAYS_BEFORE[2] ) {
 352                 TS -= 1;        // Shifts 29 Feb to 28 Feb
 353                 *day = 1;
 354         }
 355         // Get what month it is
 356         for( i = 0; i < 12; i ++ ) {
 357                 if( TS < DAYS_BEFORE[i] )
 358                         break;
 359         }
 360         *month = i - 1;
 361         // Get day
 362         TS -= DAYS_BEFORE[i-1];
 363         *day += TS;     // Plus offset from leap handling above
 364 }
 365
 366 /**
 367  * \brief Search a string array for \a Needle
 368  * \note Helper function for eTplDrv_IOCtl::DRV_IOCTL_LOOKUP
 369  */
 370 int ModUtil_LookupString(const char **Array, const char *Needle)
 371 {
 372          int    i;
 373         if( !CheckString(Needle) )      return -1;
 374         for( i = 0; Array[i]; i++ )
 375         {
 376                 if(strcmp(Array[i], Needle) == 0)       return i;
 377         }
 378         return -1;
 379 }
 380
 381 int ModUtil_SetIdent(char *Dest, const char *Value)
 382 {
 383         if( !CheckMem(Dest, 32) )       return -1;
 384         strncpy(Dest, Value, 32);
 385         return 1;
 386 }
 387
 388 int Hex(char *Dest, size_t Size, const Uint8 *SourceData)
 389 {
 390         size_t  i;
 391         for( i = 0; i < Size; i ++ )
 392         {
 393                 sprintf(Dest + i*2, "%02x", SourceData[i]);
 394         }
 395         return i*2;
 396 }
 397
 398 /**
 399  * \brief Convert a string of hexadecimal digits into a byte stream
 400  */
 401 int UnHex(Uint8 *Dest, size_t DestSize, const char *SourceString)
 402 {
 403         size_t  i;
 404         for( i = 0; i < DestSize*2; i += 2 )
 405         {
 406                 Uint8   val = 0;
 407
 408                 if(SourceString[i] == '\0')     break;
 409
 410                 if('0' <= SourceString[i] && SourceString[i] <= '9')
 411                         val |= (SourceString[i]-'0') << 4;
 412                 else if('A' <= SourceString[i] && SourceString[i] <= 'F')
 413                         val |= (SourceString[i]-'A'+10) << 4;
 414                 else if('a' <= SourceString[i] && SourceString[i] <= 'f')
 415                         val |= (SourceString[i]-'a'+10) << 4;
 416
 417                 if(SourceString[i+1] == '\0')   break;
 418
 419                 if('0' <= SourceString[i+1] && SourceString[i+1] <= '9')
 420                         val |= (SourceString[i+1] - '0');
 421                 else if('A' <= SourceString[i+1] && SourceString[i+1] <= 'F')
 422                         val |= (SourceString[i+1] - 'A' + 10);
 423                 else if('a' <= SourceString[i+1] && SourceString[i+1] <= 'f')
 424                         val |= (SourceString[i+1] - 'a' + 10);
 425
 426                 Dest[i/2] = val;
 427         }
 428         return i/2;
 429 }
 430
 431 Uint16 SwapEndian16(Uint16 Val)
 432 {
 433         return ((Val&0xFF)<<8) | ((Val>>8)&0xFF);
 434 }
 435 Uint32 SwapEndian32(Uint32 Val)
 436 {
 437         return ((Val&0xFF)<<24) | ((Val&0xFF00)<<8) | ((Val>>8)&0xFF00) | ((Val>>24)&0xFF);
 438 }
 439 Uint64 SwapEndian64(Uint64 Val)
 440 {
 441         return SwapEndian32(Val >> 32) | ((Uint64)SwapEndian32(Val) << 32);
 442 }