Kernel - Working on allowing \0 out debug
[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) )     continue;       // No #PFs please
395                         if(!p)  goto printString;
396                         while(minSize--) {
397                                 if(*p == '\0') {
398                                         PUTCH('\\');
399                                         PUTCH('0');
400                                 }
401                                 else {
402                                         PUTCH(*p);
403                                 }
404                                 p ++;
405                         }
406                         break;
407                 
408                 // Single Character
409                 case 'c':
410                 default:
411                         GETVAL();
412                         PUTCH( (Uint8)val );
413                         break;
414                 }
415         }
416         
417         if(__s && pos < __maxlen)
418                 __s[pos] = '\0';
419         
420         return pos;
421 }
422 #undef PUTCH
423
424 /**
425  */
426 int snprintf(char *__s, size_t __n, const char *__format, ...)
427 {
428         va_list args;
429          int    ret;
430         
431         va_start(args, __format);
432         ret = vsnprintf(__s, __n, __format, args);
433         va_end(args);
434         
435         return ret;
436 }
437
438 /**
439  */
440 int sprintf(char *__s, const char *__format, ...)
441 {
442         va_list args;
443          int    ret;
444         
445         va_start(args, __format);
446         ret = vsnprintf(__s, -1, __format, args);
447         va_end(args);
448         
449         return ret;
450 }
451
452 /*
453  * ==================
454  * ctype.h
455  * ==================
456  */
457 int isalnum(int c)
458 {
459         return isalpha(c) || isdigit(c);
460 }
461 int isalpha(int c)
462 {
463         return isupper(c) || islower(c);
464 }
465 int isascii(int c)
466 {
467         return (0 <= c && c < 128);
468 }
469 int isblank(int c)
470 {
471         if(c == '\t')   return 1;
472         if(c == ' ')    return 1;
473         return 0;
474 }
475 int iscntrl(int c)
476 {
477         // TODO: Check iscntrl
478         if(c < ' ')     return 1;
479         return 0;
480 }
481 int isdigit(int c)
482 {
483         return ('0' <= c && c <= '9');
484 }
485 int isgraph(int c)
486 {
487         // TODO: Check isgraph
488         return 0;
489 }
490 int islower(int c)
491 {
492         return ('a' <= c && c <= 'z');
493 }
494 int isprint(int c)
495 {
496         if( ' ' <= c && c <= 0x7F )     return 1;
497         return 0;
498 }
499 int ispunct(int c)
500 {
501         switch(c)
502         {
503         case '.':       case ',':
504         case '?':       case '!':
505                 return 1;
506         default:
507                 return 0;
508         }
509 }
510 int isspace(int c)
511 {
512         if(c == ' ')    return 1;
513         if(c == '\t')   return 1;
514         if(c == '\v')   return 1;
515         if(c == '\n')   return 1;
516         if(c == '\r')   return 1;
517         return 0;
518 }
519 int isupper(int c)
520 {
521         return ('A' <= c && c <= 'Z');
522 }
523 int isxdigit(int c)
524 {
525         return isdigit(c) || ('a' <= c && c <= 'f') || ('A' <= c && c <= 'F');
526 }
527
528 int toupper(int c)
529 {
530         if( islower(c) )
531                 return c - 0x20;
532         else
533                 return c;
534 }
535 int tolower(int c)
536 {
537         if( isupper(c) )
538                 return c + 0x20;
539         else
540                 return c;
541 }
542
543 /**
544  * \fn int strucmp(const char *Str1, const char *Str2)
545  * \brief Compare \a Str1 and \a Str2 case-insensitively
546  */
547 int strucmp(const char *Str1, const char *Str2)
548 {
549         while(*Str1 && tolower(*Str1) == tolower(*Str2))
550                 Str1++, Str2++;
551         return tolower(*Str1) - tolower(*Str2);
552 }
553
554 /**
555  * \brief Locate a byte in a string
556  */
557 char *strchr(const char *__s, int __c)
558 {
559         for( ; *__s; __s ++ )
560         {
561                 if( *__s == __c )       return (char*)__s;
562         }
563         return NULL;
564 }
565
566 char *strrchr(const char *__s, int __c)
567 {
568         size_t ofs = strlen(__s);
569         while(--ofs && __s[ofs] != __c);
570         if( __s[ofs] == __c )
571                 return (char*)__s + ofs;
572         return NULL;
573 }
574
575 /**
576  * \fn int strpos(const char *Str, char Ch)
577  * \brief Search a string for an ascii character
578  */
579 int strpos(const char *Str, char Ch)
580 {
581          int    pos;
582         for(pos=0;Str[pos];pos++)
583         {
584                 if(Str[pos] == Ch)      return pos;
585         }
586         return -1;
587 }
588
589 /**
590  * \fn size_t strlen(const char *__str)
591  * \brief Get the length of string
592  */
593 size_t strlen(const char *__str)
594 {
595         size_t  ret = 0;
596         while(*__str++) ret++;
597         return ret;
598 }
599
600 /**
601  * \brief Copy a string to a new location
602  */
603 char *strcpy(char *__str1, const char *__str2)
604 {
605         while(*__str2)
606                 *__str1++ = *__str2++;
607         *__str1 = '\0'; // Terminate String
608         return __str1;
609 }
610
611 /**
612  * \brief Copy a string to a new location
613  * \note Copies at most `max` chars
614  */
615 char *strncpy(char *__str1, const char *__str2, size_t __max)
616 {
617         while(*__str2 && __max-- >= 1)
618                 *__str1++ = *__str2++;
619         if(__max)
620                 *__str1 = '\0'; // Terminate String
621         return __str1;
622 }
623
624 /**
625  * \brief Append a string to another
626  */
627 char *strcat(char *__dest, const char *__src)
628 {
629         while(*__dest++);
630         __dest--;
631         while(*__src)
632                 *__dest++ = *__src++;
633         *__dest = '\0';
634         return __dest;
635 }
636
637 /**
638  * \brief Append at most \a n chars to a string from another
639  * \note At most n+1 chars are written (the dest is always zero terminated)
640  */
641 char *strncat(char *__dest, const char *__src, size_t n)
642 {
643         while(*__dest++);
644         while(*__src && n-- >= 1)
645                 *__dest++ = *__src++;
646         *__dest = '\0';
647         return __dest;
648 }
649
650 /**
651  * \fn int strcmp(const char *str1, const char *str2)
652  * \brief Compare two strings return the difference between
653  *        the first non-matching characters.
654  */
655 int strcmp(const char *str1, const char *str2)
656 {
657         while(*str1 && *str1 == *str2)
658                 str1++, str2++;
659         return *str1 - *str2;
660 }
661
662 /**
663  * \fn int strncmp(const char *Str1, const char *Str2, size_t num)
664  * \brief Compare strings \a Str1 and \a Str2 to a maximum of \a num characters
665  */
666 int strncmp(const char *Str1, const char *Str2, size_t num)
667 {
668         if(num == 0)    return 0;       // TODO: Check what should officially happen here
669         while(--num && *Str1 && *Str1 == *Str2)
670                 Str1++, Str2++;
671         return *Str1-*Str2;
672 }
673
674 #if 0
675 /**
676  * \fn char *strdup(const char *Str)
677  * \brief Duplicates a string
678  */
679 char *strdup(const char *Str)
680 {
681         char    *ret;
682         ret = malloc(strlen(Str)+1);
683         if( !ret )      return NULL;
684         strcpy(ret, Str);
685         return ret;
686 }
687 #else
688
689 /**
690  * \fn char *_strdup(const char *File, int Line, const char *Str)
691  * \brief Duplicates a string
692  */
693 char *_strdup(const char *File, int Line, const char *Str)
694 {
695         char    *ret;
696         ret = Heap_Allocate(File, Line, strlen(Str)+1);
697         if( !ret )      return NULL;
698         strcpy(ret, Str);
699         return ret;
700 }
701 #endif
702
703 /**
704  * \fn int rand()
705  * \brief Pseudo random number generator
706  */
707 int rand(void)
708 {
709         #if 0
710         static Uint     state = RANDOM_SEED;
711         Uint    old = state;
712         // Get the next state value
713         giRandomState = (RANDOM_A*state + RANDOM_C);
714         // Check if it has changed, and if it hasn't, change it
715         if(state == old)        state += RANDOM_SPRUCE;
716         return state;
717         #else
718         // http://en.wikipedia.org/wiki/Xorshift
719         // 2010-10-03
720         static Uint32   x = 123456789;
721         static Uint32   y = 362436069;
722         static Uint32   z = 521288629;
723         static Uint32   w = 88675123; 
724         Uint32  t;
725  
726         t = x ^ (x << 11);
727         x = y; y = z; z = w;
728         return w = w ^ (w >> 19) ^ t ^ (t >> 8); 
729         #endif
730 }
731
732 void *memmove(void *__dest, const void *__src, size_t len)
733 {
734         char    *dest = __dest;
735         const char      *src = __src;
736         void    *ret = __dest;
737
738         if( len == 0 || dest == src )
739                 return dest;
740         
741         if( (tVAddr)dest > (tVAddr)src + len )
742                 return memcpy(dest, src, len);
743         if( (tVAddr)dest + len < (tVAddr)src )
744                 return memcpy(dest, src, len);
745         
746         // NOTE: Assumes memcpy works forward
747         if( (tVAddr)dest < (tVAddr)src )
748                 return memcpy(dest, src, len);
749
750         #if 0
751         size_t  block_size;
752         if( (tVAddr)dest < (tVAddr)src )
753                 block_size = (tVAddr)src - (tVAddr)dest;
754         else
755                 block_size = (tVAddr)dest - (tVAddr)src;
756         
757         block_size &= ~0xF;
758         
759         while(len >= block_size)
760         {
761                 memcpy(dest, src, block_size);
762                 len -= block_size;
763                 dest += block_size;
764                 src += block_size;
765         }
766         memcpy(dest, src, len);
767         return ret;
768         #else
769         for( int i = len; i--; )
770         {
771                 dest[i] = src[i];
772         }
773         return ret;
774         #endif
775         
776 }
777
778 // NOTE: Strictly not libc, but lib.c is used by userland code too and hence these two
779 // can't be in it.
780 /**
781  * \name Memory Validation
782  * \{
783  */
784 /**
785  * \brief Checks if a string resides fully in valid memory
786  */
787 int CheckString(const char *String)
788 {
789         tVAddr  addr;
790          int    bUser;
791
792         addr = (tVAddr)String;
793
794         if( !MM_GetPhysAddr( (void*)addr ) )
795                 return 0;
796         
797         // Check 1st page
798         bUser = MM_IsUser( addr );
799         
800         while( *(char*)addr )
801         {
802                 if( (addr & (PAGE_SIZE-1)) == 0 )
803                 {
804                         if(bUser && !MM_IsUser(addr) )
805                                 return 0;
806                         if(!bUser && !MM_GetPhysAddr((void*)addr) )
807                                 return 0;
808                 }
809                 addr ++;
810         }
811         return 1;
812 }
813
814 /**
815  * \brief Check if a sized memory region is valid memory
816  * \return Boolean success
817  */
818 int CheckMem(const void *Mem, int NumBytes)
819 {
820         return MM_IsValidBuffer( (tVAddr)Mem, NumBytes );
821 }
822 /* *
823  * \}
824  */
825

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