kernel/libc.c

   1 /*--------------------------------------------------------------------
   2  * TITLE: ANSI C Library
   3  * AUTHOR: Steve Rhoads (rhoadss@yahoo.com)
   4  * DATE CREATED: 12/17/05
   5  * FILENAME: libc.c
   6  * PROJECT: Plasma CPU core
   7  * COPYRIGHT: Software placed into the public domain by the author.
   8  *    Software 'as is' without warranty.  Author liable for nothing.
   9  * DESCRIPTION:
  10  *    Subset of the ANSI C library
  11  *--------------------------------------------------------------------*/
  12 #define NO_ELLIPSIS
  13 #include "rtos.h"
  14
  15
  16 char *strcpy(char *dst, const char *src)
  17 {
  18    char *dstSave=dst;
  19    int c;
  20    do
  21    {
  22       c = *dst++ = *src++;
  23    } while(c);
  24    return dstSave;
  25 }
  26
  27
  28 char *strncpy(char *dst, const char *src, int count)
  29 {
  30    int c=1;
  31    char *dstSave=dst;
  32    while(count-- > 0 && c)
  33       c = *dst++ = *src++;
  34    *dst = 0;
  35    return dstSave;
  36 }
  37
  38
  39 char *strcat(char *dst, const char *src)
  40 {
  41    int c;
  42    char *dstSave=dst;
  43    while(*dst)
  44       ++dst;
  45    do
  46    {
  47       c = *dst++ = *src++;
  48    } while(c);
  49    return dstSave;
  50 }
  51
  52
  53 char *strncat(char *dst, const char *src, int count)
  54 {
  55    int c=1;
  56    char *dstSave=dst;
  57    while(*dst && --count > 0)
  58       ++dst;
  59    while(--count > 0 && c)
  60       c = *dst++ = *src++;
  61    *dst = 0;
  62    return dstSave;
  63 }
  64
  65
  66 int strcmp(const char *string1, const char *string2)
  67 {
  68    int diff, c;
  69    for(;;)
  70    {
  71       diff = *string1++ - (c = *string2++);
  72       if(diff)
  73          return diff;
  74       if(c == 0)
  75          return 0;
  76    }
  77 }
  78
  79
  80 int strncmp(const char *string1, const char *string2, int count)
  81 {
  82    int diff, c;
  83    while(count-- > 0)
  84    {
  85       diff = *string1++ - (c = *string2++);
  86       if(diff)
  87          return diff;
  88       if(c == 0)
  89          return 0;
  90    }
  91    return 0;
  92 }
  93
  94
  95 char *strstr(const char *string, const char *find)
  96 {
  97    int i;
  98    for(;;)
  99    {
 100       for(i = 0; string[i] == find[i] && find[i]; ++i) ;
 101       if(find[i] == 0)
 102          return (char*)string;
 103       if(*string++ == 0)
 104          return NULL;
 105    }
 106 }
 107
 108
 109 int strlen(const char *string)
 110 {
 111    const char *base=string;
 112    while(*string++) ;
 113    return string - base - 1;
 114 }
 115
 116
 117 void *memcpy(void *dst, const void *src, unsigned long bytes)
 118 {
 119    if(((uint32)dst | (uint32)src | bytes) & 3)
 120    {
 121       uint8 *Dst = (uint8*)dst, *Src = (uint8*)src;
 122       while((int)bytes-- > 0)
 123          *Dst++ = *Src++;
 124    }
 125    else
 126    {
 127       uint32 *Dst32 = (uint32*)dst, *Src32 = (uint32*)src;
 128       bytes >>= 2;
 129       while((int)bytes-- > 0)
 130          *Dst32++ = *Src32++;
 131    }
 132    return dst;
 133 }
 134
 135
 136 void *memmove(void *dst, const void *src, unsigned long bytes)
 137 {
 138    uint8 *Dst = (uint8*)dst;
 139    uint8 *Src = (uint8*)src;
 140    if(Dst < Src)
 141    {
 142       while((int)bytes-- > 0)
 143          *Dst++ = *Src++;
 144    }
 145    else
 146    {
 147       Dst += bytes;
 148       Src += bytes;
 149       while((int)bytes-- > 0)
 150          *--Dst = *--Src;
 151    }
 152    return dst;
 153 }
 154
 155
 156 int memcmp(const void *cs, const void *ct, unsigned long bytes)
 157 {
 158    uint8 *Dst = (uint8*)cs;
 159    uint8 *Src = (uint8*)ct;
 160    int diff;
 161    while((int)bytes-- > 0)
 162    {
 163       diff = *Dst++ - *Src++;
 164       if(diff)
 165          return diff;
 166    }
 167    return 0;
 168 }
 169
 170
 171 void *memset(void *dst, int c, unsigned long bytes)
 172 {
 173    uint8 *Dst = (uint8*)dst;
 174    while((int)bytes-- > 0)
 175       *Dst++ = (uint8)c;
 176    return dst;
 177 }
 178
 179
 180 int abs(int n)
 181 {
 182    return n>=0 ? n : -n;
 183 }
 184
 185
 186 static uint32 Rand1=0x1f2bcda3;
 187 int rand(void)
 188 {
 189    Rand1 = 1664525 * Rand1 + 1013904223;  //from D.E. Knuth and H.W. Lewis
 190    return Rand1;
 191 }
 192
 193
 194 void srand(unsigned int seed)
 195 {
 196    Rand1 = seed;
 197 }
 198
 199
 200 long strtol(const char *s, char **end, int base)
 201 {
 202    int i;
 203    unsigned long ch, value=0, neg=0;
 204
 205    if(s[0] == '-')
 206    {
 207       neg = 1;
 208       ++s;
 209    }
 210    if(s[0] == '0' && s[1] == 'x')
 211    {
 212       base = 16;
 213       s += 2;
 214    }
 215    for(i = 0; i <= 8; ++i)
 216    {
 217       ch = *s++;
 218       if('0' <= ch && ch <= '9')
 219          ch -= '0';
 220       else if('A' <= ch && ch <= 'Z')
 221          ch = ch - 'A' + 10;
 222       else if('a' <= ch && ch <= 'z')
 223          ch = ch - 'a' + 10;
 224       else
 225          break;
 226       value = value * base + ch;
 227    }
 228    if(end)
 229       *end = (char*)s - 1;
 230    if(neg)
 231       value = -(int)value;
 232    return value;
 233 }
 234
 235
 236 int atoi(const char *s)
 237 {
 238    return strtol(s, NULL, 10);
 239 }
 240
 241
 242 char *itoa(int num, char *dst, int base)
 243 {
 244    int digit, negate=0, place;
 245    char c, text[20];
 246
 247    if(base == 10 && num < 0)
 248    {
 249       num = -num;
 250       negate = 1;
 251    }
 252    text[16] = 0;
 253    for(place = 15; place >= 0; --place)
 254    {
 255       digit = (unsigned int)num % (unsigned int)base;
 256       if(num == 0 && place < 15 && base == 10 && negate)
 257       {
 258          c = '-';
 259          negate = 0;
 260       }
 261       else if(digit < 10)
 262          c = (char)('0' + digit);
 263       else
 264          c = (char)('a' + digit - 10);
 265       text[place] = c;
 266       num = (unsigned int)num / (unsigned int)base;
 267       if(num == 0 && negate == 0)
 268          break;
 269    }
 270    strcpy(dst, text + place);
 271    return dst;
 272 }
 273
 274
 275 int sprintf(char *s, const char *format,
 276             int arg0, int arg1, int arg2, int arg3,
 277             int arg4, int arg5, int arg6, int arg7)
 278 {
 279    int argv[8];
 280    int argc=0, width, length;
 281    char f, text[20], fill;
 282
 283    argv[0] = arg0; argv[1] = arg1; argv[2] = arg2; argv[3] = arg3;
 284    argv[4] = arg4; argv[5] = arg5; argv[6] = arg6; argv[7] = arg7;
 285
 286    for(;;)
 287    {
 288       f = *format++;
 289       if(f == 0)
 290          return argc;
 291       else if(f == '%')
 292       {
 293          width = 0;
 294          fill = ' ';
 295          f = *format++;
 296          while('0' <= f && f <= '9')
 297          {
 298             width = width * 10 + f - '0';
 299             f = *format++;
 300          }
 301          if(f == '.')
 302          {
 303             fill = '0';
 304             f = *format++;
 305          }
 306          if(f == 0)
 307             return argc;
 308
 309          if(f == 'd')
 310          {
 311             memset(s, fill, width);
 312             itoa(argv[argc++], text, 10);
 313             length = (int)strlen(text);
 314             if(width < length)
 315                width = length;
 316             strcpy(s + width - length, text);
 317          }
 318          else if(f == 'x' || f == 'f')
 319          {
 320             memset(s, '0', width);
 321             itoa(argv[argc++], text, 16);
 322             length = (int)strlen(text);
 323             if(width < length)
 324                width = length;
 325             strcpy(s + width - length, text);
 326          }
 327          else if(f == 'c')
 328          {
 329             *s++ = (char)argv[argc++];
 330             *s = 0;
 331          }
 332          else if(f == 's')
 333          {
 334             length = strlen((char*)argv[argc]);
 335             if(width > length)
 336             {
 337                memset(s, ' ', width - length);
 338                s += width - length;
 339             }
 340             strcpy(s, (char*)argv[argc++]);
 341          }
 342          s += strlen(s);
 343       }
 344       else
 345       {
 346          if(f == '\n')
 347             *s++ = '\r';
 348          *s++ = f;
 349          if(f == '\r' && *format == '\n')
 350             *s++ = *format++;
 351       }
 352       *s = 0;
 353    }
 354 }
 355
 356
 357 int sscanf(const char *s, const char *format,
 358            int arg0, int arg1, int arg2, int arg3,
 359            int arg4, int arg5, int arg6, int arg7)
 360 {
 361    int argv[8];
 362    int argc=0;
 363    char f, *ptr;
 364
 365    argv[0] = arg0; argv[1] = arg1; argv[2] = arg2; argv[3] = arg3;
 366    argv[4] = arg4; argv[5] = arg5; argv[6] = arg6; argv[7] = arg7;
 367
 368    for(;;)
 369    {
 370       if(*s == 0)
 371          return argc;
 372       f = *format++;
 373       if(f == 0)
 374          return argc;
 375       else if(f == '%')
 376       {
 377          while(isspace(*s))
 378             ++s;
 379          f = *format++;
 380          if(f == 0)
 381             return argc;
 382          if(f == 'd')
 383             *(int*)argv[argc++] = strtol(s, (char**)&s, 10);
 384          else if(f == 'x')
 385             *(int*)argv[argc++] = strtol(s, (char**)&s, 16);
 386          else if(f == 'c')
 387             *(char*)argv[argc++] = *s++;
 388          else if(f == 's')
 389          {
 390             ptr = (char*)argv[argc++];
 391             while(!isspace(*s))
 392                *ptr++ = *s++;
 393             *ptr = 0;
 394          }
 395       }
 396       else
 397       {
 398          while(*s && *s != f)
 399             ++s;
 400          if(*s)
 401             ++s;
 402       }
 403    }
 404 }
 405
 406
 407 #ifdef INCLUDE_DUMP
 408 /*********************** dump ***********************/
 409 void dump(const unsigned char *data, int length)
 410 {
 411    int i, index=0, value;
 412    char string[80];
 413    memset(string, 0, sizeof(string));
 414    for(i = 0; i < length; ++i)
 415    {
 416       if((i & 15) == 0)
 417       {
 418          if(strlen(string))
 419             printf("%s\n", string);
 420          printf("%4x ", i);
 421          memset(string, 0, sizeof(string));
 422          index = 0;
 423       }
 424       value = data[i];
 425       printf("%2x ", value);
 426       if(isprint(value))
 427          string[index] = (char)value;
 428       else
 429          string[index] = '.';
 430       ++index;
 431    }
 432    for(; index < 16; ++index)
 433       printf("   ");
 434    printf("%s\n", string);
 435 }
 436 #endif //INCLUDE_DUMP
 437
 438
 439 #ifdef INCLUDE_QSORT
 440 /*********************** qsort ***********************/
 441 static void QsortSwap(char *base, long left, long right, long size)
 442 {
 443    int temp, i;
 444    char *ptrLeft, *ptrRight;
 445    ptrLeft = base + left * size;
 446    ptrRight = base + right * size;
 447    for(i = 0; i < size; ++i)
 448    {
 449       temp = ptrLeft[i];
 450       ptrLeft[i] = ptrRight[i];
 451       ptrRight[i] = (char)temp;
 452    }
 453 }
 454
 455
 456 //Modified from K&R
 457 static void qsort2(void *base, long left, long right, long size,
 458       int (*cmp)(const void *,const void *))
 459 {
 460    int i, last;
 461    char *base2=(char*)base, *pivot;
 462    if(left >= right)
 463       return;
 464    QsortSwap(base2, left, (left + right)/2, size);
 465    last = left;
 466    pivot = &base2[left*size];
 467    for(i = left + 1; i <= right; ++i)
 468    {
 469       if(cmp(&base2[i*size], pivot) < 0)
 470          QsortSwap(base2, ++last, i, size);
 471    }
 472    QsortSwap(base2, left, last, size);
 473    qsort2(base, left, last-1, size, cmp);
 474    qsort2(base, last+1, right, size, cmp);
 475 }
 476
 477
 478 void qsort(void *base,
 479            long n,
 480            long size,
 481            int (*cmp)(const void *,const void *))
 482 {
 483    qsort2(base, 0, n-1, size, cmp);
 484 }
 485
 486
 487 void *bsearch(const void *key,
 488               const void *base,
 489               long n,
 490               long size,
 491               int (*cmp)(const void *,const void *))
 492 {
 493    long cond, low=0, high=n-1, mid;
 494    char *base2=(char*)base;
 495    while(low <= high)
 496    {
 497       mid = (low + high)/2;
 498       cond = cmp(key, &base2[mid*size]);
 499       if(cond < 0)
 500          high = mid - 1;
 501       else if(cond > 0)
 502          low = mid + 1;
 503       else
 504          return &base2[mid * size];
 505    }
 506    return NULL;
 507 }
 508 #endif //INCLUDE_QSORT
 509
 510
 511 #ifdef INCLUDE_TIMELIB
 512 /************************* time.h ***********************/
 513 #define SEC_PER_YEAR (365L*24*60*60)
 514 #define SEC_PER_DAY (24L*60*60)
 515 //typedef unsigned long time_t;  //start at 1/1/80
 516 //struct tm {
 517 //   int tm_sec;      //(0,59)
 518 //   int tm_min;      //(0,59)
 519 //   int tm_hour;     //(0,23)
 520 //   int tm_mday;     //(1,31)
 521 //   int tm_mon;      //(0,11)
 522 //   int tm_year;     //(0,n) from 1900
 523 //   int tm_wday;     //(0,6)     calculated
 524 //   int tm_yday;     //(0,365)   calculated
 525 //   int tm_isdst;    //hour adjusted for day light savings
 526 //};
 527 static const unsigned short DaysUntilMonth[]=
 528    {0,31,59,90,120,151,181,212,243,273,304,334,365};
 529 static const unsigned short DaysInMonth[]=
 530    {31,28,31,30,31,30,31,31,30,31,30,31};
 531 static time_t DstTimeIn, DstTimeOut;
 532
 533
 534 /* Leap year if divisible by 4.  Centenary years should only be
 535    leap-years if they were divisible by 400. */
 536 static int IsLeapYear(int year)
 537 {
 538    return(((year % 4 == 0) && (year % 100 != 0)) || (year % 400 == 0));
 539 }
 540
 541 time_t mktime(struct tm *tp)
 542 {
 543    time_t seconds;
 544    unsigned long days, y, year;
 545
 546    days = tp->tm_mday - 1 + DaysUntilMonth[tp->tm_mon] +
 547       365 * (tp->tm_year - 80);
 548    seconds = (unsigned long)tp->tm_sec + 60L * (tp->tm_min +
 549       60L * (tp->tm_hour + 24L * days));
 550    if(tp->tm_isdst)
 551       seconds -= 60 * 60;
 552    year = 1900 + tp->tm_year - (tp->tm_mon < 2);
 553    for(y = 1980; y <= year; y += 4)
 554    {
 555       if(y % 100 != 0 || y % 400 == 0)
 556          seconds += SEC_PER_DAY;
 557    }
 558    return seconds;
 559 }
 560
 561
 562 void gmtime_r(const time_t *tp, struct tm *out)
 563 {
 564    time_t seconds, delta, secondsIn=*tp;
 565    int isLeapYear;
 566    unsigned long year, month;
 567
 568    out->tm_isdst = 0;
 569    if(DstTimeIn <= secondsIn && secondsIn < DstTimeOut)
 570    {
 571       secondsIn += 60 * 60;
 572       out->tm_isdst = 1;
 573    }
 574    seconds = secondsIn;
 575    for(year = 0; ; ++year)
 576    {
 577       delta = SEC_PER_YEAR + IsLeapYear(1980 + year) * SEC_PER_DAY;
 578       if(seconds >= delta)
 579          seconds -= delta;
 580       else
 581          break;
 582    }
 583    out->tm_year = year + 80;
 584    isLeapYear = IsLeapYear(1980 + year);
 585    for(month = 0; ; ++month)
 586    {
 587       delta = SEC_PER_DAY * (DaysInMonth[month] + (isLeapYear && (month == 1)));
 588       if(seconds >= delta)
 589          seconds -= delta;
 590       else
 591          break;
 592    }
 593    out->tm_mon = month;
 594    out->tm_mday = seconds / SEC_PER_DAY;
 595    out->tm_yday = DaysUntilMonth[month] + out->tm_mday;
 596    seconds -= out->tm_mday * SEC_PER_DAY;
 597    ++out->tm_mday;
 598    out->tm_hour = seconds / (60 * 60);
 599    seconds -= out->tm_hour * (60 * 60);
 600    out->tm_min = seconds / 60;
 601    seconds -= out->tm_min * 60;
 602    out->tm_sec = seconds;
 603    seconds = secondsIn % (SEC_PER_DAY * 7);
 604    out->tm_wday = (seconds / SEC_PER_DAY + 2) % 7; /* 1/1/80 is a Tue */
 605    //printf("%4.d/%2.d/%2.d:%2.d:%2.d:%2.d\n",
 606    //         out->tm_year+1900, out->tm_mon+1, out->tm_mday,
 607    //         out->tm_hour, out->tm_min, out->tm_sec);
 608 }
 609
 610
 611 void gmtimeDst(time_t dstTimeIn, time_t dstTimeOut)
 612 {
 613    DstTimeIn = dstTimeIn;
 614    DstTimeOut = dstTimeOut;
 615 }
 616
 617
 618 //DST from 2am on the second Sunday in March to 2am first Sunday in November
 619 void gmtimeDstSet(time_t *tp, time_t *dstTimeIn, time_t *dstTimeOut)
 620 {
 621    time_t seconds, timeIn, timeOut;
 622    struct tm tmDate;
 623    int year, days;
 624
 625    DstTimeIn = 0;
 626    DstTimeOut = 0;
 627    gmtime_r(tp, &tmDate);
 628    year = tmDate.tm_year;
 629
 630    //March 1, year, 2AM -> second Sunday
 631    tmDate.tm_year = year;
 632    tmDate.tm_mon = 2;
 633    tmDate.tm_mday = 1;
 634    tmDate.tm_hour = 2;
 635    tmDate.tm_min = 0;
 636    tmDate.tm_sec = 0;
 637    seconds = mktime(&tmDate);
 638    gmtime_r(&seconds, &tmDate);
 639    days = 7 - tmDate.tm_wday + 7;
 640    *dstTimeIn = timeIn = seconds + days * SEC_PER_DAY;
 641
 642    //November 1, year, 2AM -> first Sunday
 643    tmDate.tm_year = year;
 644    tmDate.tm_mon = 10;
 645    tmDate.tm_mday = 1;
 646    tmDate.tm_hour = 2;
 647    tmDate.tm_min = 0;
 648    tmDate.tm_sec = 0;
 649    seconds = mktime(&tmDate);
 650    gmtime_r(&seconds, &tmDate);
 651    days = 7 - tmDate.tm_wday;
 652    *dstTimeOut = timeOut = seconds + days * SEC_PER_DAY;
 653
 654    DstTimeIn = timeIn;
 655    DstTimeOut = timeOut;
 656 }
 657 #endif //INCLUDE_TIMELIB
 658