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

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