Many changes, bugfixes to user vsnprintf and to escape code handling
[tpg/acess2.git] / Kernel / lib.c
1 /*
2  * Acess2
3  * Common Library Functions
4  */
5 #include <acess.h>
6
7 // === CONSTANTS ===
8 #define RANDOM_SEED     0xACE55052
9 #define RANDOM_A        0x00731ADE
10 #define RANDOM_C        12345
11 #define RANDOM_SPRUCE   0xf12b02b
12 //                          Jan Feb Mar Apr May  Jun  Jul  Aug  Sept Oct  Nov  Dec
13 const short DAYS_BEFORE[] = {0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334};
14 #define UNIX_TO_2K      ((30*365*3600*24) + (7*3600*24))        //Normal years + leap years
15
16 // === PROTOTYPES ===
17  int    atoi(const char *string);
18 void    itoa(char *buf, Uint num, int base, int minLength, char pad);
19  int    vsnprintf(char *__s, size_t __maxlen, const char *__format, va_list args);
20  int    sprintf(char *__s, const char *__format, ...);
21  int    tolower(int c);
22  int    strucmp(const char *Str1, const char *Str2);
23  int    strpos(const char *Str, char Ch);
24  Uint8  ByteSum(void *Ptr, int Size);
25 size_t  strlen(const char *__s);
26 char    *strcpy(char *__str1, const char *__str2);
27 char    *strncpy(char *__str1, const char *__str2, size_t max);
28  int    strcmp(const char *str1, const char *str2);
29  int    strncmp(const char *str1, const char *str2, size_t num);
30 char    *strdup(const char *Str);
31 char    **str_split(const char *__str, char __ch);
32  int    strpos8(const char *str, Uint32 Search);
33  int    ReadUTF8(Uint8 *str, Uint32 *Val);
34  int    WriteUTF8(Uint8 *str, Uint32 Val);
35  int    DivUp(int num, int dem);
36 Sint64  timestamp(int sec, int mins, int hrs, int day, int month, int year);
37 Uint    rand(void);
38  int    CheckString(char *String);
39  int    CheckMem(void *Mem, int NumBytes);
40  int    ModUtil_LookupString(char **Array, char *Needle);
41  int    ModUtil_SetIdent(char *Dest, char *Value);
42
43 // === EXPORTS ===
44 EXPORT(atoi);
45 EXPORT(itoa);
46 EXPORT(vsnprintf);
47 EXPORT(sprintf);
48 EXPORT(tolower);
49 EXPORT(strucmp);
50 EXPORT(strpos);
51 EXPORT(ByteSum);
52 EXPORT(strlen);
53 EXPORT(strcpy);
54 EXPORT(strncpy);
55 EXPORT(strcmp);
56 EXPORT(strncmp);
57 EXPORT(strdup);
58 EXPORT(str_split);
59 EXPORT(strpos8);
60 EXPORT(DivUp);
61 EXPORT(ReadUTF8);
62 EXPORT(WriteUTF8);
63 EXPORT(timestamp);
64 EXPORT(CheckString);
65 EXPORT(CheckMem);
66 EXPORT(ModUtil_LookupString);
67 EXPORT(ModUtil_SetIdent);
68
69 // === GLOBALS ===
70 static Uint     giRandomState = RANDOM_SEED;
71
72 // === CODE ===
73 /**
74  * \brief Convert a string into an integer
75  */
76 int atoi(const char *string)
77 {
78          int    ret = 0;
79          int    bNeg = 0;
80         
81         //Log("atoi: (string='%s')", string);
82         
83         // Clear non-numeric characters
84         while( !('0' <= *string && *string <= '9') && *string != '-' )  string++;
85         if( *string == '-' ) {
86                 bNeg = 1;
87                 while( !('0' <= *string && *string <= '9') )    string++;
88         }
89         
90         if(*string == '0')
91         {
92                 string ++;
93                 if(*string == 'x')
94                 {
95                         // Hex
96                         string ++;
97                         for( ;; string ++ )
98                         {
99                                 if('0' <= *string && *string <= '9') {
100                                         ret *= 16;
101                                         ret += *string - '0';
102                                 }
103                                 else if('A' <= *string && *string <= 'F') {
104                                         ret *= 16;
105                                         ret += *string - 'A' + 10;
106                                 }
107                                 else if('a' <= *string && *string <= 'f') {
108                                         ret *= 16;
109                                         ret += *string - 'a' + 10;
110                                 }
111                                 else
112                                         break;
113                         }
114                 }
115                 else    // Octal
116                 {
117                         for( ; '0' <= *string && *string <= '7'; string ++ )
118                         {
119                                 ret *= 8;
120                                 ret += *string - '0';
121                         }
122                 }
123         }
124         else    // Decimal
125         {
126                 for( ; '0' <= *string && *string <= '9'; string++)
127                 {
128                         ret *= 10;
129                         ret += *string - '0';
130                 }
131         }
132         
133         if(bNeg)        ret = -ret;
134         
135         //Log("atoi: RETURN %i", ret);
136         
137         return ret;
138 }
139
140 static const char cUCDIGITS[] = "0123456789ABCDEF";
141 /**
142  * \fn void itoa(char *buf, Uint num, int base, int minLength, char pad)
143  * \brief Convert an integer into a character string
144  */
145 void itoa(char *buf, Uint num, int base, int minLength, char pad)
146 {
147         char    tmpBuf[BITS];
148          int    pos=0, i;
149
150         // Sanity check
151         if(!buf)        return;
152         
153         // Sanity Check
154         if(base > 16 || base < 2) {
155                 buf[0] = 0;
156                 return;
157         }
158         
159         // Convert 
160         while(num > base-1) {
161                 tmpBuf[pos] = cUCDIGITS[ num % base ];
162                 num /= (Uint)base;              // Shift `num` right 1 digit
163                 pos++;
164         }
165         tmpBuf[pos++] = cUCDIGITS[ num % base ];                // Last digit of `num`
166         
167         // Put in reverse
168         i = 0;
169         minLength -= pos;
170         while(minLength-- > 0)  buf[i++] = pad;
171         while(pos-- > 0)                buf[i++] = tmpBuf[pos]; // Reverse the order of characters
172         buf[i] = 0;
173 }
174
175 /**
176  * \brief Append a character the the vsnprintf output
177  */
178 #define PUTCH(c)        do{\
179         char ch=(c);\
180         if(pos==__maxlen){return pos;}\
181         if(__s){__s[pos++]=ch;}else{pos++;}\
182         }while(0)
183 int vsnprintf(char *__s, size_t __maxlen, const char *__format, va_list args)
184 {
185         char    c, pad = ' ';
186          int    minSize = 0, len;
187         char    tmpBuf[34];     // For Integers
188         char    *p = NULL;
189          int    isLongLong = 0;
190         Uint64  val;
191         size_t  pos = 0;
192         // Flags
193          int    bPadLeft = 0;
194         
195         //Log("vsnprintf: (__s=%p, __maxlen=%i, __format='%s', ...)", __s, __maxlen, __format);
196         
197         while((c = *__format++) != 0)
198         {
199                 // Non control character
200                 if(c != '%') { PUTCH(c); continue; }
201                 
202                 c = *__format++;
203                 //Log("pos = %i", pos);
204                 
205                 // Literal %
206                 if(c == '%') { PUTCH('%'); continue; }
207                 
208                 // Pointer - Done first for debugging
209                 if(c == 'p') {
210                         Uint    ptr = va_arg(args, Uint);
211                         PUTCH('*');     PUTCH('0');     PUTCH('x');
212                         p = tmpBuf;
213                         itoa(p, ptr, 16, BITS/4, '0');
214                         goto printString;
215                 }
216                 
217                 // Get Argument
218                 val = va_arg(args, Uint);
219                 
220                 // - Padding Side Flag
221                 if(c == '+') {
222                         bPadLeft = 1;
223                         c = *__format++;
224                 }
225                 
226                 // - Padding
227                 if(c == '0') {
228                         pad = '0';
229                         c = *__format++;
230                 }
231                 else
232                         pad = ' ';
233                 
234                 // - Minimum length
235                 if(c == '*') {
236                         minSize = val;
237                         val = va_arg(args, Uint);
238                         c = *__format++;
239                 }
240                 else if('1' <= c && c <= '9')
241                 {
242                         minSize = 0;
243                         while('0' <= c && c <= '9')
244                         {
245                                 minSize *= 10;
246                                 minSize += c - '0';
247                                 c = *__format++;
248                         }
249                 }
250                 else
251                         minSize = 1;
252                 
253                 // - Default, Long or LongLong?
254                 isLongLong = 0;
255                 if(c == 'l')    // Long is actually the default on x86
256                 {
257                         c = *__format++;
258                         if(c == 'l') {
259                                 #if BITS == 32
260                                 val |= (Uint64)va_arg(args, Uint) << 32;
261                                 #endif
262                                 c = *__format++;
263                                 isLongLong = 1;
264                         }
265                 }
266                 
267                 // - Now get the format code
268                 p = tmpBuf;
269                 switch(c)
270                 {
271                 case 'd':
272                 case 'i':
273                         if( (isLongLong && val >> 63) || (!isLongLong && val >> 31) ) {
274                                 PUTCH('-');
275                                 val = -val;
276                         }
277                         itoa(p, val, 10, minSize, pad);
278                         goto printString;
279                 case 'u':
280                         itoa(p, val, 10, minSize, pad);
281                         goto printString;
282                 case 'x':
283                         itoa(p, val, 16, minSize, pad);
284                         goto printString;
285                 case 'o':
286                         itoa(p, val, 8, minSize, pad);
287                         goto printString;
288                 case 'b':
289                         itoa(p, val, 2, minSize, pad);
290                         goto printString;
291
292                 case 'B':       //Boolean
293                         if(val) p = "True";
294                         else    p = "False";
295                         goto printString;
296                 
297                 // String - Null Terminated Array
298                 case 's':
299                         p = (char*)(Uint)val;
300                 printString:
301                         if(!p)          p = "(null)";
302                         len = strlen(p);
303                         if( !bPadLeft ) while(len++ < minSize)  PUTCH(pad);
304                         while(*p)       PUTCH(*p++);
305                         if( bPadLeft )  while(len++ < minSize)  PUTCH(pad);
306                         break;
307                 
308                 case 'C':       // Non-Null Terminated Character Array
309                         p = (char*)(Uint)val;
310                         if(!p)  goto printString;
311                         //while(minSize--)      PUTCH(*p++);
312                         while(minSize--)        PUTCH(*p++);
313                         break;
314                 
315                 // Single Character
316                 case 'c':
317                 default:
318                         PUTCH( (Uint8)val );
319                         break;
320                 }
321         }
322         
323         if(__s && pos != __maxlen)
324                 __s[pos] = '\0';
325         
326         return pos;
327 }
328 #undef PUTCH
329
330 /**
331  */
332 int sprintf(char *__s, const char *__format, ...)
333 {
334         va_list args;
335          int    ret;
336         
337         va_start(args, __format);
338         ret = vsnprintf(__s, -1, __format, args);
339         va_end(args);
340         
341         return ret;
342 }
343
344 /**
345  * \fn int tolower(int c)
346  * \brief Converts a character to lower case
347  */
348 int tolower(int c)
349 {
350         if('A' <= c && c <= 'Z')
351                 return c - 'A' + 'a';
352         return c;
353 }
354
355 /**
356  * \fn int strucmp(const char *Str1, const char *Str2)
357  * \brief Compare \a Str1 and \a Str2 case-insensitively
358  */
359 int strucmp(const char *Str1, const char *Str2)
360 {
361         while(*Str1 && tolower(*Str1) == tolower(*Str2))
362                 Str1++, Str2++;
363         return tolower(*Str1) - tolower(*Str2);
364 }
365
366 /**
367  * \fn int strpos(const char *Str, char Ch)
368  * \brief Search a string for an ascii character
369  */
370 int strpos(const char *Str, char Ch)
371 {
372          int    pos;
373         for(pos=0;Str[pos];pos++)
374         {
375                 if(Str[pos] == Ch)      return pos;
376         }
377         return -1;
378 }
379
380 /**
381  * \fn Uint8 ByteSum(void *Ptr, int Size)
382  * \brief Adds the bytes in a memory region and returns the sum
383  */
384 Uint8 ByteSum(void *Ptr, int Size)
385 {
386         Uint8   sum = 0;
387         while(Size--)   sum += *(Uint8*)Ptr++;
388         return sum;
389 }
390
391 /**
392  * \fn Uint strlen(const char *__str)
393  * \brief Get the length of string
394  */
395 Uint strlen(const char *__str)
396 {
397         Uint    ret = 0;
398         while(*__str++) ret++;
399         return ret;
400 }
401
402 /**
403  * \fn char *strcpy(char *__str1, const char *__str2)
404  * \brief Copy a string to a new location
405  */
406 char *strcpy(char *__str1, const char *__str2)
407 {
408         while(*__str2)
409                 *__str1++ = *__str2++;
410         *__str1 = '\0'; // Terminate String
411         return __str1;
412 }
413
414 /**
415  * \fn char *strncpy(char *__str1, const char *__str2, size_t max)
416  * \brief Copy a string to a new location
417  */
418 char *strncpy(char *__str1, const char *__str2, size_t max)
419 {
420         while(*__str2 && max-- >= 1)
421                 *__str1++ = *__str2++;
422         if(max)
423                 *__str1 = '\0'; // Terminate String
424         return __str1;
425 }
426
427 /**
428  * \fn int strcmp(const char *str1, const char *str2)
429  * \brief Compare two strings return the difference between
430  *        the first non-matching characters.
431  */
432 int strcmp(const char *str1, const char *str2)
433 {
434         while(*str1 && *str1 == *str2)
435                 str1++, str2++;
436         return *str1 - *str2;
437 }
438
439 /**
440  * \fn int strncmp(const char *Str1, const char *Str2, size_t num)
441  * \brief Compare strings \a Str1 and \a Str2 to a maximum of \a num characters
442  */
443 int strncmp(const char *Str1, const char *Str2, size_t num)
444 {
445         if(num == 0)    return 0;       // TODO: Check what should officially happen here
446         while(--num && *Str1 && *Str1 == *Str2)
447                 Str1++, Str2++;
448         return *Str1-*Str2;
449 }
450
451 /**
452  * \fn char *strdup(const char *Str)
453  * \brief Duplicates a string
454  */
455 char *strdup(const char *Str)
456 {
457         char    *ret;
458         ret = malloc(strlen(Str)+1);
459         strcpy(ret, Str);
460         return ret;
461 }
462
463 /**
464  * \brief Split a string using the passed character
465  * \return NULL terminated array of strings on the heap
466  * \param __str String to split
467  * \param __ch  Character to split by
468  */
469 char **str_split(const char *__str, char __ch)
470 {
471          int    i, j;
472          int    len = 1;
473         char    **ret;
474         char    *start;
475         
476         for( i = 0; __str[i]; i++ )
477         {
478                 if(__str[i] == __ch)
479                         len ++;
480         }
481         
482         ret = malloc( sizeof(char*)*(len+1) + (i + 1) );
483         if( !ret )      return NULL;
484         
485         j = 1;
486         start = (char *)&ret[len+1];
487         ret[0] = start;
488         for( i = 0; __str[i]; i++ )
489         {
490                 if(__str[i] == __ch) {
491                         *start++ = '\0';
492                         Log_Debug("Lib", "str_split: ret[%i] = '%s'", j-1, ret[j-1]);
493                         ret[j++] = start;
494                 }
495                 else {
496                         *start++ = __str[i]; 
497                 }
498         }
499         *start = '\0';
500         ret[j] = NULL;
501         Log_Debug("Lib", "str_split: ret[%i] = '%s'", j-1, ret[j-1]);
502         
503         for( j = 0; j < len; j++ )
504                 Log_Debug("Lib", "str_split: ret[%i] = '%s'", j, ret[j]);
505         
506         return ret;
507 }
508
509 /**
510  * \fn int DivUp(int num, int dem)
511  * \brief Divide two numbers, rounding up
512  * \param num   Numerator
513  * \param dem   Denominator
514  */
515 int DivUp(int num, int dem)
516 {
517         return (num+dem-1)/dem;
518 }
519
520 /**
521  * \fn int strpos8(const char *str, Uint32 search)
522  * \brief Search a string for a UTF-8 character
523  */
524 int strpos8(const char *str, Uint32 Search)
525 {
526          int    pos;
527         Uint32  val = 0;
528         for(pos=0;str[pos];pos++)
529         {
530                 // ASCII Range
531                 if(Search < 128) {
532                         if(str[pos] == Search)  return pos;
533                         continue;
534                 }
535                 if(*(Uint8*)(str+pos) < 128)    continue;
536                 
537                 pos += ReadUTF8( (Uint8*)&str[pos], &val );
538                 if(val == Search)       return pos;
539         }
540         return -1;
541 }
542
543 /**
544  * \fn int ReadUTF8(Uint8 *str, Uint32 *Val)
545  * \brief Read a UTF-8 character from a string
546  */
547 int ReadUTF8(Uint8 *str, Uint32 *Val)
548 {
549         *Val = 0xFFFD;  // Assume invalid character
550         
551         // ASCII
552         if( !(*str & 0x80) ) {
553                 *Val = *str;
554                 return 1;
555         }
556         
557         // Middle of a sequence
558         if( (*str & 0xC0) == 0x80 ) {
559                 return 1;
560         }
561         
562         // Two Byte
563         if( (*str & 0xE0) == 0xC0 ) {
564                 *Val = (*str & 0x1F) << 6;      // Upper 6 Bits
565                 str ++;
566                 if( (*str & 0xC0) != 0x80)      return -1;      // Validity check
567                 *Val |= (*str & 0x3F);  // Lower 6 Bits
568                 return 2;
569         }
570         
571         // Three Byte
572         if( (*str & 0xF0) == 0xE0 ) {
573                 *Val = (*str & 0x0F) << 12;     // Upper 4 Bits
574                 str ++;
575                 if( (*str & 0xC0) != 0x80)      return -1;      // Validity check
576                 *Val |= (*str & 0x3F) << 6;     // Middle 6 Bits
577                 str ++;
578                 if( (*str & 0xC0) != 0x80)      return -1;      // Validity check
579                 *Val |= (*str & 0x3F);  // Lower 6 Bits
580                 return 3;
581         }
582         
583         // Four Byte
584         if( (*str & 0xF1) == 0xF0 ) {
585                 *Val = (*str & 0x07) << 18;     // Upper 3 Bits
586                 str ++;
587                 if( (*str & 0xC0) != 0x80)      return -1;      // Validity check
588                 *Val |= (*str & 0x3F) << 12;    // Middle-upper 6 Bits
589                 str ++;
590                 if( (*str & 0xC0) != 0x80)      return -1;      // Validity check
591                 *Val |= (*str & 0x3F) << 6;     // Middle-lower 6 Bits
592                 str ++;
593                 if( (*str & 0xC0) != 0x80)      return -1;      // Validity check
594                 *Val |= (*str & 0x3F);  // Lower 6 Bits
595                 return 4;
596         }
597         
598         // UTF-8 Doesn't support more than four bytes
599         return 4;
600 }
601
602 /**
603  * \fn int WriteUTF8(Uint8 *str, Uint32 Val)
604  * \brief Write a UTF-8 character sequence to a string
605  */
606 int WriteUTF8(Uint8 *str, Uint32 Val)
607 {
608         // ASCII
609         if( Val < 128 ) {
610                 *str = Val;
611                 return 1;
612         }
613         
614         // Two Byte
615         if( Val < 0x8000 ) {
616                 *str = 0xC0 | (Val >> 6);
617                 str ++;
618                 *str = 0x80 | (Val & 0x3F);
619                 return 2;
620         }
621         
622         // Three Byte
623         if( Val < 0x10000 ) {
624                 *str = 0xE0 | (Val >> 12);
625                 str ++;
626                 *str = 0x80 | ((Val >> 6) & 0x3F);
627                 str ++;
628                 *str = 0x80 | (Val & 0x3F);
629                 return 3;
630         }
631         
632         // Four Byte
633         if( Val < 0x110000 ) {
634                 *str = 0xF0 | (Val >> 18);
635                 str ++;
636                 *str = 0x80 | ((Val >> 12) & 0x3F);
637                 str ++;
638                 *str = 0x80 | ((Val >> 6) & 0x3F);
639                 str ++;
640                 *str = 0x80 | (Val & 0x3F);
641                 return 4;
642         }
643         
644         // UTF-8 Doesn't support more than four bytes
645         return 0;
646 }
647
648 /**
649  * \fn Uint64 timestamp(int sec, int mins, int hrs, int day, int month, int year)
650  * \brief Converts a date into an Acess Timestamp
651  */
652 Sint64 timestamp(int sec, int mins, int hrs, int day, int month, int year)
653 {
654         Sint64  stamp;
655         stamp = sec;
656         stamp += mins*60;
657         stamp += hrs*3600;
658         
659         stamp += day*3600*24;
660         stamp += month*DAYS_BEFORE[month]*3600*24;
661         if(     (
662                 ((year&3) == 0 || year%100 != 0)
663                 || (year%100 == 0 && ((year/100)&3) == 0)
664                 ) && month > 1) // Leap year and after feb
665                 stamp += 3600*24;
666         
667         stamp += ((365*4+1) * ((year-2000)&~3)) * 3600*24;      // Foour Year Segments
668         stamp += ((year-2000)&3) * 365*3600*24; // Inside four year segment
669         stamp += UNIX_TO_2K;
670         
671         return stamp * 1000;
672 }
673
674 /**
675  * \fn Uint rand()
676  * \brief Pseudo random number generator
677  * \note Unknown effectiveness (made up on the spot)
678  */
679 Uint rand(void)
680 {
681         Uint    old = giRandomState;
682         // Get the next state value
683         giRandomState = (RANDOM_A*giRandomState + RANDOM_C) & 0xFFFFFFFF;
684         // Check if it has changed, and if it hasn't, change it
685         if(giRandomState == old)        giRandomState += RANDOM_SPRUCE;
686         return giRandomState;
687 }
688
689 /// \name Memory Validation
690 /// \{
691 /**
692  * \brief Checks if a string resides fully in valid memory
693  */
694 int CheckString(char *String)
695 {
696         // Check 1st page
697         if( MM_IsUser( (tVAddr)String ) )
698         {
699                 // Traverse String
700                 while( *String )
701                 {
702                         if( !MM_IsUser( (tVAddr)String ) )
703                                 return 0;
704                         // Increment string pointer
705                         String ++;
706                 }
707                 return 1;
708         }
709         else if( MM_GetPhysAddr( (tVAddr)String ) )
710         {
711                 // Traverse String
712                 while( *String )
713                 {
714                         if( !MM_GetPhysAddr( (tVAddr)String ) )
715                                 return 0;
716                         // Increment string pointer
717                         String ++;
718                 }
719                 return 1;
720         }
721         return 0;
722 }
723
724 /**
725  * \brief Check if a sized memory region is valid memory
726  */
727 int CheckMem(void *Mem, int NumBytes)
728 {
729         tVAddr  addr = (tVAddr)Mem;
730         
731         if( MM_IsUser( addr ) )
732         {
733                 while( NumBytes-- )
734                 {
735                         if( !MM_IsUser( addr ) )
736                                 return 0;
737                         addr ++;
738                 }
739                 return 1;
740         }
741         else if( MM_GetPhysAddr( addr ) )
742         {
743                 while( NumBytes-- )
744                 {
745                         if( !MM_GetPhysAddr( addr ) )
746                                 return 0;
747                         addr ++;
748                 }
749                 return 1;
750         }
751         return 0;
752 }
753 /// \}
754
755 /**
756  * \brief Search a string array for \a Needle
757  * \note Helper function for eTplDrv_IOCtl::DRV_IOCTL_LOOKUP
758  */
759 int ModUtil_LookupString(char **Array, char *Needle)
760 {
761          int    i;
762         if( !CheckString(Needle) )      return -1;
763         for( i = 0; Array[i]; i++ )
764         {
765                 if(strcmp(Array[i], Needle) == 0)       return i;
766         }
767         return -1;
768 }
769
770 int ModUtil_SetIdent(char *Dest, char *Value)
771 {
772         if( !CheckMem(Dest, 32) )       return -1;
773         strncpy(Dest, Value, 32);
774         return 1;
775 }

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