Kernel - Expose thread timer for EVENT_TIMER, multiple sleepers for workqueue, cleanu...
[tpg/acess2.git] / KernelLand / Kernel / libc.c
1 /*
2  * Acess2 Kernel
3  * - By John Hodge (thePowersGang)
4  *
5  * libc.c
6  * - Kernel-land C Library
7  */
8 #include <acess.h>
9 #include <hal_proc.h>   // For MM_*
10 #include <utf16.h>
11
12 // === CONSTANTS ===
13 #define RANDOM_SEED     0xACE55052
14 #define RANDOM_A        0x00731ADE
15 #define RANDOM_C        12345
16 #define RANDOM_SPRUCE   0xf12b039
17
18 // === PROTOTYPES ===
19 #if 0
20 unsigned long long      strtoull(const char *str, char **end, int base);
21 unsigned long   strtoul(const char *str, char **end, int base);
22 signed long long        strtoll(const char *str, char **end, int base);
23 signed long     strtol(const char *str, char **end, int base);
24  int    atoi(const char *string);
25  int    ParseInt(const char *string, int *Val);
26 void    itoa(char *buf, Uint64 num, int base, int minLength, char pad);
27  int    vsnprintf(char *__s, size_t __maxlen, const char *__format, va_list args);
28  int    snprintf(char *__s, size_t __n, const char *__format, ...);
29  int    sprintf(char *__s, const char *__format, ...);
30  int    strucmp(const char *Str1, const char *Str2);
31 char    *strchr(const char *__s, int __c);
32  int    strpos(const char *Str, char Ch);
33 size_t  strlen(const char *__s);
34 char    *strcpy(char *__str1, const char *__str2);
35 char    *strncpy(char *__str1, const char *__str2, size_t max);
36 char    *strcat(char *dest, const char *source);
37  int    strcmp(const char *str1, const char *str2);
38  int    strncmp(const char *str1, const char *str2, size_t num);
39 char    *_strdup(const char *File, int Line, const char *Str);
40  int    rand(void);
41 void    *memmove(void *__dest, const void *__src, size_t len);
42
43  int    CheckString(char *String);
44  int    CheckMem(void *Mem, int NumBytes); 
45 #endif
46
47 // === EXPORTS ===
48 EXPORT(atoi);
49 EXPORT(itoa);
50 EXPORT(vsnprintf);
51 EXPORT(snprintf);
52 EXPORT(sprintf);
53 EXPORT(tolower);
54
55 EXPORT(strucmp);
56 EXPORT(strchr);
57 EXPORT(strrchr);
58 EXPORT(strpos);
59 EXPORT(strlen);
60 EXPORT(strcpy);
61 EXPORT(strncpy);
62 EXPORT(strcat);
63 EXPORT(strncat);
64 EXPORT(strcmp);
65 EXPORT(strncmp);
66 //EXPORT(strdup);
67 EXPORT(_strdup);        // Takes File/Line too
68 EXPORT(rand);
69 EXPORT(memmove);
70
71 EXPORT(CheckString);
72 EXPORT(CheckMem);
73
74 // === CODE ===
75 // - Import userland stroi.c file
76 #define _LIB_H_
77 #include "../../Usermode/Libraries/libc.so_src/strtoi.c"
78
79 int ParseInt(const char *string, int *Val)
80 {
81          int    ret = 0;
82          int    bNeg = 0;
83         const char *orig_string = string;
84         
85         //Log("atoi: (string='%s')", string);
86         
87         // Clear non-numeric characters
88         while( !('0' <= *string && *string <= '9') && *string != '-' )  string++;
89         if( *string == '-' ) {
90                 bNeg = 1;
91                 while( !('0' <= *string && *string <= '9') )    string++;
92         }
93         
94         if(*string == '0')
95         {
96                 string ++;
97                 if(*string == 'x')
98                 {
99                         // Hex
100                         string ++;
101                         for( ;; string ++ )
102                         {
103                                 if('0' <= *string && *string <= '9') {
104                                         ret *= 16;
105                                         ret += *string - '0';
106                                 }
107                                 else if('A' <= *string && *string <= 'F') {
108                                         ret *= 16;
109                                         ret += *string - 'A' + 10;
110                                 }
111                                 else if('a' <= *string && *string <= 'f') {
112                                         ret *= 16;
113                                         ret += *string - 'a' + 10;
114                                 }
115                                 else
116                                         break;
117                         }
118                 }
119                 else    // Octal
120                 {
121                         for( ; '0' <= *string && *string <= '7'; string ++ )
122                         {
123                                 ret *= 8;
124                                 ret += *string - '0';
125                         }
126                 }
127         }
128         else    // Decimal
129         {
130                 for( ; '0' <= *string && *string <= '9'; string++)
131                 {
132                         ret *= 10;
133                         ret += *string - '0';
134                 }
135                 // Error check
136                 if( ret == 0 )  return 0;
137         }
138         
139         if(bNeg)        ret = -ret;
140         
141         //Log("atoi: RETURN %i", ret);
142         
143         if(Val) *Val = ret;
144         
145         return string - orig_string;
146 }
147
148 static const char cUCDIGITS[] = "0123456789ABCDEF";
149 /**
150  * \fn void itoa(char *buf, Uint64 num, int base, int minLength, char pad)
151  * \brief Convert an integer into a character string
152  */
153 void itoa(char *buf, Uint64 num, int base, int minLength, char pad)
154 {
155         char    tmpBuf[64+1];
156          int    pos=0, i;
157         Uint64  rem;
158
159         buf[0] = 0;
160         ASSERTR(base >= 2, );
161         ASSERTR(base <= 16, );
162
163         // Sanity check
164         if(!buf)        return;
165         
166         // Convert 
167         while(num > base-1) {
168                 num = DivMod64U(num, base, &rem);       // Shift `num` and get remainder
169                 ASSERT(rem >= 0);
170                 if( rem >= base && base != 16 ) {
171                         Debug("rem(%llx) >= base(%x), num=%llx", rem, base, num);
172                 }
173                 ASSERT(rem < base);
174                 tmpBuf[pos] = cUCDIGITS[ rem ];
175                 pos++;
176         }
177         tmpBuf[pos++] = cUCDIGITS[ num ];               // Last digit of `num`
178         
179         // Put in reverse
180         i = 0;
181         minLength -= pos;
182         while(minLength-- > 0)  buf[i++] = pad;
183         while(pos-- > 0)                buf[i++] = tmpBuf[pos]; // Reverse the order of characters
184         buf[i] = 0;
185 }
186
187 /**
188  * \brief Append a character the the vsnprintf output
189  */
190 #define PUTCH(ch)       do { \
191                 if(pos < __maxlen && __s) { \
192                         __s[pos] = ch; \
193                 } else { \
194                         (void)ch;\
195                 } \
196                 pos ++; \
197         } while(0)
198 #define GETVAL()        do {\
199         if(isLongLong)  val = va_arg(args, Uint64);\
200         else    val = va_arg(args, unsigned int);\
201         }while(0)
202 /**
203  * \brief VArg String Number Print Formatted
204  */
205 int vsnprintf(char *__s, const size_t __maxlen, const char *__format, va_list args)
206 {
207         char    c, pad = ' ';
208          int    minSize = 0, precision = -1, len;
209         char    tmpBuf[34];     // For Integers
210         const char      *p = NULL;
211          int    isLongLong = 0, isLong;
212         Uint64  val;
213         size_t  pos = 0;
214         // Flags
215          int    bPadLeft = 0;
216
217         while((c = *__format++) != 0)
218         {
219                 // Non control character
220                 if(c != '%') { PUTCH(c); continue; }
221
222                 c = *__format++;
223                 if(c == '\0')   break;
224                 
225                 // Literal %
226                 if(c == '%') { PUTCH('%'); continue; }
227                 
228                 // Pointer - Done first for debugging
229                 if(c == 'p') {
230                         Uint    ptr = va_arg(args, Uint);
231                         PUTCH('*');     PUTCH('0');     PUTCH('x');
232                         for( len = BITS/4; len -- && ((ptr>>(len*4))&15) == 0; )
233                                 ;
234                         len ++;
235                         if( len == 0 )
236                                 PUTCH( '0' );
237                         else
238                                 while( len -- )
239                                         PUTCH( cUCDIGITS[ (ptr>>(len*4))&15 ] );
240                         continue ;
241                 }
242
243                 isLongLong = 0;
244                 isLong = 0;
245         
246                 // - Padding Side Flag
247                 if(c == '-') {
248                         bPadLeft = 1;
249                         c = *__format++;
250                 }
251                 
252                 // - Padding
253                 if(c == '0') {
254                         pad = '0';
255                         c = *__format++;
256                 }
257                 else
258                         pad = ' ';
259                 
260                 // - Minimum length
261                 if(c == '*') {  // Dynamic length
262                         minSize = va_arg(args, unsigned int);
263                         c = *__format++;
264                 }
265                 else if('1' <= c && c <= '9')
266                 {
267                         minSize = 0;
268                         while('0' <= c && c <= '9')
269                         {
270                                 minSize *= 10;
271                                 minSize += c - '0';
272                                 c = *__format++;
273                         }
274                 }
275                 else
276                         minSize = 0;
277                 
278                 // - Precision
279                 precision = -1;
280                 if( c == '.' ) {
281                         c = *__format++;
282                         
283                         if(c == '*') {  // Dynamic length
284                                 precision = va_arg(args, unsigned int);
285                                 c = *__format++;
286                         }
287                         else if('1' <= c && c <= '9')
288                         {
289                                 precision = 0;
290                                 while('0' <= c && c <= '9')
291                                 {
292                                         precision *= 10;
293                                         precision += c - '0';
294                                         c = *__format++;
295                                 }
296                         }
297                 }
298                 
299                 // - Default, Long or LongLong?
300                 isLongLong = 0;
301                 if(c == 'l')    // Long is actually the default on x86
302                 {
303                         isLong = 1;
304                         c = *__format++;
305                         if(c == 'l') {
306                                 c = *__format++;
307                                 isLongLong = 1;
308                         }
309                 }
310                 
311                 // - Now get the format code
312                 p = tmpBuf;
313                 switch(c)
314                 {
315                 case 'd':
316                 case 'i':
317                         GETVAL();
318                         if( isLongLong && val >> 63 ) {
319                                 PUTCH('-');
320                                 if( val == LLONG_MIN )
321                                         val = LLONG_MAX;
322                                 else
323                                         val = -val;
324                         }
325                         else if( !isLongLong && (val >> 31) ) {
326                                 PUTCH('-');
327                                 val = (~val & 0xFFFFFFFF)+1;
328                         }
329                         itoa(tmpBuf, val, 10, minSize, pad);
330                         goto printString;
331                 case 'u':       // Unsigned
332                         GETVAL();
333                         itoa(tmpBuf, val, 10, minSize, pad);
334                         goto printString;
335                 case 'P':       // Physical Address
336                         PUTCH('0');
337                         PUTCH('x');
338                         if(sizeof(tPAddr) > 4)  isLongLong = 1;
339                         GETVAL();
340                         itoa(tmpBuf, val, 16, minSize, pad);
341                         goto printString;
342                 case 'X':       // Hex
343                         if(BITS == 64)
344                                 isLongLong = 1; // TODO: Handle non-x86 64-bit archs
345                         GETVAL();
346                         itoa(tmpBuf, val, 16, minSize, pad);
347                         goto printString;
348                         
349                 case 'x':       // Lower case hex
350                         GETVAL();
351                         itoa(tmpBuf, val, 16, minSize, pad);
352                         goto printString;
353                 case 'o':       // Octal
354                         GETVAL();
355                         itoa(tmpBuf, val, 8, minSize, pad);
356                         goto printString;
357                 case 'b':
358                         GETVAL();
359                         itoa(tmpBuf, val, 2, minSize, pad);
360                         goto printString;
361
362                 case 'B':       //Boolean
363                         val = va_arg(args, unsigned int);
364                         if(val) p = "True";
365                         else    p = "False";
366                         goto printString;
367                 
368                 // String - Null Terminated Array
369                 case 's':
370                         if( isLong ) {
371                                 Uint16  *p16 = va_arg(args, Uint16*);
372                                 Uint8   tmp[5];
373                                 while( *p16 && precision-- ) {
374                                         Uint32  cp;
375                                         p16 += ReadUTF16(p16, &cp);
376                                         tmp[WriteUTF8(tmp, cp)] = 0;
377                                         for(int i = 0; tmp[i] && i<5; i ++)
378                                                 PUTCH(tmp[i]);
379                                 }
380                                 break;
381                         }
382                         p = va_arg(args, char*);        // Get Argument
383                         if( !p || !CheckString(p) )     p = "(inval)";  // Avoid #PFs  
384                 printString:
385                         if(!p)          p = "(null)";
386                         len = strlen(p);
387                         if( !bPadLeft ) while(len++ < minSize)  PUTCH(pad);
388                         while(*p && precision--) { PUTCH(*p); p++;} 
389                         if( bPadLeft )  while(len++ < minSize)  PUTCH(pad);
390                         break;
391                 
392                 case 'C':       // Non-Null Terminated Character Array
393                         p = va_arg(args, char*);
394                         if( !CheckMem(p, minSize) ) {
395                                 p = "(inval)";
396                                 goto printString;
397                         }
398                         if(!p)  goto printString;
399                         while(minSize--) {
400                                 if(*p == '\0') {
401                                         PUTCH('\\');
402                                         PUTCH('0');
403                                 }
404                                 else if(*p == '\\') {
405                                         PUTCH('\\');
406                                         PUTCH('\\');
407                                 }
408                                 else {
409                                         PUTCH(*p);
410                                 }
411                                 p ++;
412                         }
413                         break;
414                 
415                 // Single Character
416                 case 'c':
417                 default:
418                         GETVAL();
419                         PUTCH( (Uint8)val );
420                         break;
421                 }
422         }
423         
424         if(__s && pos < __maxlen)
425                 __s[pos] = '\0';
426         
427         return pos;
428 }
429 #undef PUTCH
430
431 /**
432  */
433 int snprintf(char *__s, size_t __n, const char *__format, ...)
434 {
435         va_list args;
436          int    ret;
437         
438         va_start(args, __format);
439         ret = vsnprintf(__s, __n, __format, args);
440         va_end(args);
441         
442         return ret;
443 }
444
445 /**
446  */
447 int sprintf(char *__s, const char *__format, ...)
448 {
449         va_list args;
450          int    ret;
451         
452         va_start(args, __format);
453         ret = vsnprintf(__s, -1, __format, args);
454         va_end(args);
455         
456         return ret;
457 }
458
459 /*
460  * ==================
461  * ctype.h
462  * ==================
463  */
464 int isalnum(int c)
465 {
466         return isalpha(c) || isdigit(c);
467 }
468 int isalpha(int c)
469 {
470         return isupper(c) || islower(c);
471 }
472 int isascii(int c)
473 {
474         return (0 <= c && c < 128);
475 }
476 int isblank(int c)
477 {
478         if(c == '\t')   return 1;
479         if(c == ' ')    return 1;
480         return 0;
481 }
482 int iscntrl(int c)
483 {
484         // TODO: Check iscntrl
485         if(c < ' ')     return 1;
486         return 0;
487 }
488 int isdigit(int c)
489 {
490         return ('0' <= c && c <= '9');
491 }
492 int isgraph(int c)
493 {
494         // TODO: Check isgraph
495         return 0;
496 }
497 int islower(int c)
498 {
499         return ('a' <= c && c <= 'z');
500 }
501 int isprint(int c)
502 {
503         if( ' ' <= c && c <= 0x7F )     return 1;
504         return 0;
505 }
506 int ispunct(int c)
507 {
508         switch(c)
509         {
510         case '.':       case ',':
511         case '?':       case '!':
512                 return 1;
513         default:
514                 return 0;
515         }
516 }
517 int isspace(int c)
518 {
519         if(c == ' ')    return 1;
520         if(c == '\t')   return 1;
521         if(c == '\v')   return 1;
522         if(c == '\n')   return 1;
523         if(c == '\r')   return 1;
524         return 0;
525 }
526 int isupper(int c)
527 {
528         return ('A' <= c && c <= 'Z');
529 }
530 int isxdigit(int c)
531 {
532         return isdigit(c) || ('a' <= c && c <= 'f') || ('A' <= c && c <= 'F');
533 }
534
535 int toupper(int c)
536 {
537         if( islower(c) )
538                 return c - 0x20;
539         else
540                 return c;
541 }
542 int tolower(int c)
543 {
544         if( isupper(c) )
545                 return c + 0x20;
546         else
547                 return c;
548 }
549
550 /**
551  * \fn int strucmp(const char *Str1, const char *Str2)
552  * \brief Compare \a Str1 and \a Str2 case-insensitively
553  */
554 int strucmp(const char *Str1, const char *Str2)
555 {
556         while(*Str1 && tolower(*Str1) == tolower(*Str2))
557                 Str1++, Str2++;
558         return tolower(*Str1) - tolower(*Str2);
559 }
560
561 /**
562  * \brief Locate a byte in a string
563  */
564 char *strchr(const char *__s, int __c)
565 {
566         for( ; *__s; __s ++ )
567         {
568                 if( *__s == __c )       return (char*)__s;
569         }
570         return NULL;
571 }
572
573 char *strrchr(const char *__s, int __c)
574 {
575         size_t ofs = strlen(__s);
576         while(--ofs && __s[ofs] != __c);
577         if( __s[ofs] == __c )
578                 return (char*)__s + ofs;
579         return NULL;
580 }
581
582 /**
583  * \fn int strpos(const char *Str, char Ch)
584  * \brief Search a string for an ascii character
585  */
586 int strpos(const char *Str, char Ch)
587 {
588          int    pos;
589         for(pos=0;Str[pos];pos++)
590         {
591                 if(Str[pos] == Ch)      return pos;
592         }
593         return -1;
594 }
595
596 /**
597  * \fn size_t strlen(const char *__str)
598  * \brief Get the length of string
599  */
600 size_t strlen(const char *__str)
601 {
602         size_t  ret = 0;
603         while(*__str++) ret++;
604         return ret;
605 }
606
607 /**
608  * \brief Copy a string to a new location
609  */
610 char *strcpy(char *__str1, const char *__str2)
611 {
612         while(*__str2)
613                 *__str1++ = *__str2++;
614         *__str1 = '\0'; // Terminate String
615         return __str1;
616 }
617
618 /**
619  * \brief Copy a string to a new location
620  * \note Copies at most `max` chars
621  */
622 char *strncpy(char *__str1, const char *__str2, size_t __max)
623 {
624         while(*__str2 && __max-- >= 1)
625                 *__str1++ = *__str2++;
626         if(__max)
627                 *__str1 = '\0'; // Terminate String
628         return __str1;
629 }
630
631 /**
632  * \brief Append a string to another
633  */
634 char *strcat(char *__dest, const char *__src)
635 {
636         while(*__dest++);
637         __dest--;
638         while(*__src)
639                 *__dest++ = *__src++;
640         *__dest = '\0';
641         return __dest;
642 }
643
644 /**
645  * \brief Append at most \a n chars to a string from another
646  * \note At most n+1 chars are written (the dest is always zero terminated)
647  */
648 char *strncat(char *__dest, const char *__src, size_t n)
649 {
650         while(*__dest++);
651         while(*__src && n-- >= 1)
652                 *__dest++ = *__src++;
653         *__dest = '\0';
654         return __dest;
655 }
656
657 /**
658  * \fn int strcmp(const char *str1, const char *str2)
659  * \brief Compare two strings return the difference between
660  *        the first non-matching characters.
661  */
662 int strcmp(const char *str1, const char *str2)
663 {
664         while(*str1 && *str1 == *str2)
665                 str1++, str2++;
666         return *str1 - *str2;
667 }
668
669 /**
670  * \fn int strncmp(const char *Str1, const char *Str2, size_t num)
671  * \brief Compare strings \a Str1 and \a Str2 to a maximum of \a num characters
672  */
673 int strncmp(const char *Str1, const char *Str2, size_t num)
674 {
675         if(num == 0)    return 0;       // TODO: Check what should officially happen here
676         while(--num && *Str1 && *Str1 == *Str2)
677                 Str1++, Str2++;
678         return *Str1-*Str2;
679 }
680
681 #if 0
682 /**
683  * \fn char *strdup(const char *Str)
684  * \brief Duplicates a string
685  */
686 char *strdup(const char *Str)
687 {
688         char    *ret;
689         ret = malloc(strlen(Str)+1);
690         if( !ret )      return NULL;
691         strcpy(ret, Str);
692         return ret;
693 }
694 #else
695
696 /**
697  * \fn char *_strdup(const char *File, int Line, const char *Str)
698  * \brief Duplicates a string
699  */
700 char *_strdup(const char *File, int Line, const char *Str)
701 {
702         char    *ret;
703         ret = Heap_Allocate(File, Line, strlen(Str)+1);
704         if( !ret )      return NULL;
705         strcpy(ret, Str);
706         return ret;
707 }
708 #endif
709
710 /**
711  * \fn int rand()
712  * \brief Pseudo random number generator
713  */
714 int rand(void)
715 {
716         #if 0
717         static Uint     state = RANDOM_SEED;
718         Uint    old = state;
719         // Get the next state value
720         giRandomState = (RANDOM_A*state + RANDOM_C);
721         // Check if it has changed, and if it hasn't, change it
722         if(state == old)        state += RANDOM_SPRUCE;
723         return state;
724         #else
725         // http://en.wikipedia.org/wiki/Xorshift
726         // 2010-10-03
727         static Uint32   x = 123456789;
728         static Uint32   y = 362436069;
729         static Uint32   z = 521288629;
730         static Uint32   w = 88675123; 
731         Uint32  t;
732  
733         t = x ^ (x << 11);
734         x = y; y = z; z = w;
735         return w = w ^ (w >> 19) ^ t ^ (t >> 8); 
736         #endif
737 }
738
739 void *memmove(void *__dest, const void *__src, size_t len)
740 {
741         char    *dest = __dest;
742         const char      *src = __src;
743         void    *ret = __dest;
744
745         if( len == 0 || dest == src )
746                 return dest;
747         
748         if( (tVAddr)dest > (tVAddr)src + len )
749                 return memcpy(dest, src, len);
750         if( (tVAddr)dest + len < (tVAddr)src )
751                 return memcpy(dest, src, len);
752         
753         // NOTE: Assumes memcpy works forward
754         if( (tVAddr)dest < (tVAddr)src )
755                 return memcpy(dest, src, len);
756
757         #if 0
758         size_t  block_size;
759         if( (tVAddr)dest < (tVAddr)src )
760                 block_size = (tVAddr)src - (tVAddr)dest;
761         else
762                 block_size = (tVAddr)dest - (tVAddr)src;
763         
764         block_size &= ~0xF;
765         
766         while(len >= block_size)
767         {
768                 memcpy(dest, src, block_size);
769                 len -= block_size;
770                 dest += block_size;
771                 src += block_size;
772         }
773         memcpy(dest, src, len);
774         return ret;
775         #else
776         for( int i = len; i--; )
777         {
778                 dest[i] = src[i];
779         }
780         return ret;
781         #endif
782         
783 }
784
785 // NOTE: Strictly not libc, but lib.c is used by userland code too and hence these two
786 // can't be in it.
787 /**
788  * \name Memory Validation
789  * \{
790  */
791 /**
792  * \brief Checks if a string resides fully in valid memory
793  */
794 int CheckString(const char *String)
795 {
796         tVAddr  addr;
797          int    bUser;
798
799         addr = (tVAddr)String;
800
801         if( !MM_GetPhysAddr( (void*)addr ) )
802                 return 0;
803         
804         // Check 1st page
805         bUser = MM_IsUser( addr );
806         
807         while( *(char*)addr )
808         {
809                 if( (addr & (PAGE_SIZE-1)) == 0 )
810                 {
811                         if(bUser && !MM_IsUser(addr) )
812                                 return 0;
813                         if(!bUser && !MM_GetPhysAddr((void*)addr) )
814                                 return 0;
815                 }
816                 addr ++;
817         }
818         return 1;
819 }
820
821 /**
822  * \brief Check if a sized memory region is valid memory
823  * \return Boolean success
824  */
825 int CheckMem(const void *Mem, int NumBytes)
826 {
827         return MM_IsValidBuffer( (tVAddr)Mem, NumBytes );
828 }
829 /* *
830  * \}
831  */
832

UCC git Repository :: git.ucc.asn.au