]> git.sur5r.net Git - freertos/blob - FreeRTOS-Plus/Demo/FreeRTOS_IoT_Libraries/mqtt/printf-stdarg.c
5505535c171d191de3d0a33fe51801519f998cfa
[freertos] / FreeRTOS-Plus / Demo / FreeRTOS_IoT_Libraries / mqtt / printf-stdarg.c
1 /*\r
2         Copyright 2001, 2002 Georges Menie (www.menie.org)\r
3         stdarg version contributed by Christian Ettinger\r
4 \r
5         This program is free software; you can redistribute it and/or modify\r
6         it under the terms of the GNU Lesser General Public License as published by\r
7         the Free Software Foundation; either version 2 of the License, or\r
8         (at your option) any later version.\r
9 \r
10         Changes for the FreeRTOS ports:\r
11 \r
12         - The dot in "%-8.8s"\r
13         - The specifiers 'l' (long) and 'L' (long long)\r
14         - The specifier 'u' for unsigned\r
15         - Dot notation for IP addresses:\r
16           sprintf("IP = %xip\n", 0xC0A80164);\r
17       will produce "IP = 192.168.1.100\n"\r
18 */\r
19 \r
20 #include <stdarg.h>\r
21 #include <stdio.h>\r
22 #include <stdlib.h>\r
23 #include <string.h>\r
24 \r
25 #include "FreeRTOS.h"\r
26 \r
27 #define PAD_RIGHT 1\r
28 #define PAD_ZERO 2\r
29 \r
30 /*\r
31  * Return 1 for readable, 2 for writeable, 3 for both.\r
32  * Function must be provided by the application.\r
33  */\r
34 extern BaseType_t xApplicationMemoryPermissions( uint32_t aAddress );\r
35 \r
36 extern void vOutputChar( const char cChar, const TickType_t xTicksToWait  );\r
37 static const TickType_t xTicksToWait = pdMS_TO_TICKS( 20 );\r
38 \r
39 struct xPrintFlags\r
40 {\r
41         int base;\r
42         int width;\r
43         int printLimit;\r
44         unsigned\r
45                 pad : 8,\r
46                 letBase : 8,\r
47                 isSigned : 1,\r
48                 isNumber : 1,\r
49                 long32 : 1,\r
50                 long64 : 1;\r
51 };\r
52 \r
53 struct SStringBuf\r
54 {\r
55         char *str;\r
56         const char *orgStr;\r
57         const char *nulPos;\r
58         int curLen;\r
59         struct xPrintFlags flags;\r
60 };\r
61 \r
62 static void strbuf_init( struct SStringBuf *apStr, char *apBuf, const char *apMaxStr )\r
63 {\r
64         apStr->str = apBuf;\r
65         apStr->orgStr = apBuf;\r
66         apStr->nulPos = apMaxStr-1;\r
67         apStr->curLen = 0;\r
68 \r
69         memset( &apStr->flags, '\0', sizeof( apStr->flags ) );\r
70 }\r
71 /*-----------------------------------------------------------*/\r
72 \r
73 static BaseType_t strbuf_printchar( struct SStringBuf *apStr, int c )\r
74 {\r
75         if( apStr->str == NULL )\r
76         {\r
77                 vOutputChar( ( char ) c, xTicksToWait );\r
78                 apStr->curLen++;\r
79                 return pdTRUE;\r
80         }\r
81         if( apStr->str < apStr->nulPos )\r
82         {\r
83                 *( apStr->str++ ) = c;\r
84                 apStr->curLen++;\r
85                 return pdTRUE;\r
86         }\r
87         if( apStr->str == apStr->nulPos )\r
88         {\r
89                 *( apStr->str++ ) = '\0';\r
90         }\r
91         return pdFALSE;\r
92 }\r
93 /*-----------------------------------------------------------*/\r
94 \r
95 static portINLINE BaseType_t strbuf_printchar_inline( struct SStringBuf *apStr, int c )\r
96 {\r
97         if( apStr->str == NULL )\r
98         {\r
99                 vOutputChar( ( char ) c, xTicksToWait );\r
100                 if( c == 0 )\r
101                 {\r
102                         return pdFALSE;\r
103                 }\r
104                 apStr->curLen++;\r
105                 return pdTRUE;\r
106         }\r
107         if( apStr->str < apStr->nulPos )\r
108         {\r
109                 *(apStr->str++) = c;\r
110                 if( c == 0 )\r
111                 {\r
112                         return pdFALSE;\r
113                 }\r
114                 apStr->curLen++;\r
115                 return pdTRUE;\r
116         }\r
117         if( apStr->str == apStr->nulPos )\r
118         {\r
119                 *( apStr->str++ ) = '\0';\r
120         }\r
121         return pdFALSE;\r
122 }\r
123 /*-----------------------------------------------------------*/\r
124 \r
125 static portINLINE int i2hex( int aCh )\r
126 {\r
127 int iResult;\r
128 \r
129         if( aCh < 10 )\r
130         {\r
131                 iResult = '0' + aCh;\r
132         }\r
133         else\r
134         {\r
135                 iResult = 'A' + aCh - 10;\r
136         }\r
137 \r
138         return iResult;\r
139 }\r
140 /*-----------------------------------------------------------*/\r
141 \r
142 static BaseType_t prints(struct SStringBuf *apBuf, const char *apString )\r
143 {\r
144         register int padchar = ' ';\r
145         int i,len;\r
146 \r
147         if( xApplicationMemoryPermissions( ( uint32_t )apString ) == 0 )\r
148         {\r
149                 /* The user has probably made a mistake with the parameter\r
150                 for '%s', the memory is not readbale. */\r
151                 apString = "INV_MEM";\r
152         }\r
153 \r
154         if( apBuf->flags.width > 0 )\r
155         {\r
156                 register int len = 0;\r
157                 register const char *ptr;\r
158                 for( ptr = apString; *ptr; ++ptr )\r
159                 {\r
160                         ++len;\r
161                 }\r
162 \r
163                 if( len >= apBuf->flags.width )\r
164                 {\r
165                         apBuf->flags.width = 0;\r
166                 }\r
167                 else\r
168                 {\r
169                         apBuf->flags.width -= len;\r
170                 }\r
171 \r
172                 if( apBuf->flags.pad & PAD_ZERO )\r
173                 {\r
174                         padchar = '0';\r
175                 }\r
176         }\r
177         if( ( apBuf->flags.pad & PAD_RIGHT ) == 0 )\r
178         {\r
179                 for( ; apBuf->flags.width > 0; --apBuf->flags.width )\r
180                 {\r
181                         if( strbuf_printchar( apBuf, padchar ) == 0 )\r
182                         {\r
183                                 return pdFALSE;\r
184                         }\r
185                 }\r
186         }\r
187         if( ( apBuf->flags.isNumber == pdTRUE ) && ( apBuf->flags.pad == pdTRUE ) )\r
188         {\r
189                 /* The string to print represents an integer number.\r
190                  * In this case, printLimit is the min number of digits to print\r
191                  * If the length of the number to print is less than the min nb of i\r
192                  * digits to display, we add 0 before printing the number\r
193                  */\r
194                 len = strlen( apString );\r
195 \r
196                 if( len < apBuf->flags.printLimit )\r
197                 {\r
198                         i = apBuf->flags.printLimit - len;\r
199                         for( ; i; i-- )\r
200                         {\r
201                                 if( strbuf_printchar( apBuf, '0' )  == 0 )\r
202                                 {\r
203                                         return pdFALSE;\r
204                                 }\r
205                         }\r
206                 }\r
207         }\r
208         /* The string to print is not the result of a number conversion to ascii.\r
209          * For a string, printLimit is the max number of characters to display\r
210          */\r
211         for( ; apBuf->flags.printLimit && *apString ; ++apString, --apBuf->flags.printLimit )\r
212         {\r
213                 if( !strbuf_printchar( apBuf, *apString ) )\r
214                 {\r
215                         return pdFALSE;\r
216                 }\r
217         }\r
218 \r
219         for( ; apBuf->flags.width > 0; --apBuf->flags.width )\r
220         {\r
221                 if( !strbuf_printchar( apBuf, padchar ) )\r
222                 {\r
223                         return pdFALSE;\r
224                 }\r
225         }\r
226 \r
227         return pdTRUE;\r
228 }\r
229 /*-----------------------------------------------------------*/\r
230 \r
231 /* the following should be enough for 32 bit int */\r
232 #define PRINT_BUF_LEN 12        /* to print 4294967296 */\r
233 \r
234 #if     SPRINTF_LONG_LONG\r
235 #warning 64-bit libraries will be included as well\r
236 static BaseType_t printll( struct SStringBuf *apBuf, long long i )\r
237 {\r
238         char print_buf[ 2 * PRINT_BUF_LEN ];\r
239         register char *s;\r
240         register int t, neg = 0;\r
241         register unsigned long long u = i;\r
242         lldiv_t lldiv_result;\r
243 \r
244 /* typedef struct\r
245  * {\r
246  *      long long int quot; // quotient\r
247  *      long long int rem;  // remainder\r
248  * } lldiv_t;\r
249  */\r
250 \r
251         apBuf->flags.isNumber = pdTRUE; /* Parameter for prints */\r
252         if( i == 0LL )\r
253         {\r
254                 print_buf[ 0 ] = '0';\r
255                 print_buf[ 1 ] = '\0';\r
256                 return prints( apBuf, print_buf );\r
257         }\r
258 \r
259         if( ( apBuf->flags.isSigned == pdTRUE ) && ( apBuf->flags.base == 10 ) && ( i < 0LL ) )\r
260         {\r
261                 neg = 1;\r
262                 u = -i;\r
263         }\r
264 \r
265         s = print_buf + sizeof( print_buf ) - 1;\r
266 \r
267         *s = '\0';\r
268         /* 18446744073709551616 */\r
269         while( u != 0 )\r
270         {\r
271                 lldiv_result = lldiv( u, ( unsigned long long ) apBuf->flags.base );\r
272                 t = lldiv_result.rem;\r
273                 if( t >= 10 )\r
274                 {\r
275                         t += apBuf->flags.letBase - '0' - 10;\r
276                 }\r
277                 *( --s ) = t + '0';\r
278                 u = lldiv_result.quot;\r
279         }\r
280 \r
281         if( neg != 0 )\r
282         {\r
283                 if( ( apBuf->flags.width != 0 ) && ( apBuf->flags.pad & PAD_ZERO ) )\r
284                 {\r
285                         if( !strbuf_printchar( apBuf, '-' ) )\r
286                         {\r
287                                 return pdFALSE;\r
288                         }\r
289                         --apBuf->flags.width;\r
290                 }\r
291                 else\r
292                 {\r
293                         *( --s ) = '-';\r
294                 }\r
295         }\r
296 \r
297         return prints( apBuf, s );\r
298 }\r
299 #endif  /* SPRINTF_LONG_LONG */\r
300 /*-----------------------------------------------------------*/\r
301 \r
302 static BaseType_t printi( struct SStringBuf *apBuf, int i )\r
303 {\r
304         char print_buf[ PRINT_BUF_LEN ];\r
305         register char *s;\r
306         register int t, neg = 0;\r
307         register unsigned int u = i;\r
308         register unsigned base = apBuf->flags.base;\r
309 \r
310         apBuf->flags.isNumber = pdTRUE; /* Parameter for prints */\r
311 \r
312         if( i == 0 )\r
313         {\r
314                 print_buf[ 0 ] = '0';\r
315                 print_buf[ 1 ] = '\0';\r
316                 return prints( apBuf, print_buf );\r
317         }\r
318 \r
319         if( ( apBuf->flags.isSigned == pdTRUE ) && ( base == 10 ) && ( i < 0 ) )\r
320         {\r
321                 neg = 1;\r
322                 u = -i;\r
323         }\r
324 \r
325         s = print_buf + sizeof( print_buf ) - 1;\r
326 \r
327         *s = '\0';\r
328         switch( base )\r
329         {\r
330         case 16:\r
331                 while( u != 0 )\r
332                 {\r
333                         t = u & 0xF;\r
334                         if( t >= 10 )\r
335                         {\r
336                                 t += apBuf->flags.letBase - '0' - 10;\r
337                         }\r
338                         *( --s ) = t + '0';\r
339                         u >>= 4;\r
340                 }\r
341                 break;\r
342 \r
343         case 8:\r
344         case 10:\r
345                 /* GCC compiles very efficient */\r
346                 while( u )\r
347                 {\r
348                         t = u % base;\r
349                         *( --s ) = t + '0';\r
350                         u /= base;\r
351                 }\r
352                 break;\r
353 /*\r
354         // The generic case, not yet in use\r
355         default:\r
356                 while( u )\r
357                 {\r
358                         t = u % base;\r
359                         if( t >= 10)\r
360                         {\r
361                                 t += apBuf->flags.letBase - '0' - 10;\r
362                         }\r
363                         *( --s ) = t + '0';\r
364                         u /= base;\r
365                 }\r
366                 break;\r
367 */\r
368         }\r
369 \r
370         if( neg != 0 )\r
371         {\r
372                 if( apBuf->flags.width && (apBuf->flags.pad & PAD_ZERO ) )\r
373                 {\r
374                         if( strbuf_printchar( apBuf, '-' ) == 0 )\r
375                         {\r
376                                 return pdFALSE;\r
377                         }\r
378                         --apBuf->flags.width;\r
379                 }\r
380                 else\r
381                 {\r
382                         *( --s ) = '-';\r
383                 }\r
384         }\r
385 \r
386         return prints( apBuf, s );\r
387 }\r
388 /*-----------------------------------------------------------*/\r
389 \r
390 static BaseType_t printIp(struct SStringBuf *apBuf, unsigned i )\r
391 {\r
392         char print_buf[16];\r
393 \r
394         sprintf( print_buf, "%u.%u.%u.%u",\r
395                 i >> 24,\r
396                 ( i >> 16 ) & 0xff,\r
397                 ( i >> 8 ) & 0xff,\r
398                 i & 0xff );\r
399         apBuf->flags.isNumber = pdTRUE; /* Parameter for prints */\r
400         prints( apBuf, print_buf );\r
401 \r
402         return pdTRUE;\r
403 }\r
404 /*-----------------------------------------------------------*/\r
405 \r
406 static void tiny_print( struct SStringBuf *apBuf, const char *format, va_list args )\r
407 {\r
408         char scr[2];\r
409 \r
410         for( ; ; )\r
411         {\r
412                 int ch = *( format++ );\r
413 \r
414                 if( ch != '%' )\r
415                 {\r
416                         do\r
417                         {\r
418                                 /* Put the most like flow in a small loop */\r
419                                 if( strbuf_printchar_inline( apBuf, ch ) == 0 )\r
420                                 {\r
421                                         return;\r
422                                 }\r
423                                 ch = *( format++ );\r
424                         } while( ch != '%' );\r
425                 }\r
426                 ch = *( format++ );\r
427                 /* Now ch has character after '%', format pointing to next */\r
428 \r
429                 if( ch == '\0' )\r
430                 {\r
431                         break;\r
432                 }\r
433                 if( ch == '%' )\r
434                 {\r
435                         if( strbuf_printchar( apBuf, ch ) == 0 )\r
436                         {\r
437                                 return;\r
438                         }\r
439                         continue;\r
440                 }\r
441                 memset( &apBuf->flags, '\0', sizeof( apBuf->flags ) );\r
442 \r
443                 if( ch == '-' )\r
444                 {\r
445                         ch = *( format++ );\r
446                         apBuf->flags.pad = PAD_RIGHT;\r
447                 }\r
448                 while( ch == '0' )\r
449                 {\r
450                         ch = *( format++ );\r
451                         apBuf->flags.pad |= PAD_ZERO;\r
452                 }\r
453                 if( ch == '*' )\r
454                 {\r
455                         ch = *( format++ );\r
456                         apBuf->flags.width = va_arg( args, int );\r
457                 }\r
458                 else\r
459                 {\r
460                         while( ch >= '0' && ch <= '9' )\r
461                         {\r
462                                 apBuf->flags.width *= 10;\r
463                                 apBuf->flags.width += ch - '0';\r
464                                 ch = *( format++ );\r
465                         }\r
466                 }\r
467                 if( ch == '.' )\r
468                 {\r
469                         ch = *( format++ );\r
470                         if( ch == '*' )\r
471                         {\r
472                                 apBuf->flags.printLimit = va_arg( args, int );\r
473                                 ch = *( format++ );\r
474                         }\r
475                         else\r
476                         {\r
477                                 while( ch >= '0' && ch <= '9' )\r
478                                 {\r
479                                         apBuf->flags.printLimit *= 10;\r
480                                         apBuf->flags.printLimit += ch - '0';\r
481                                         ch = *( format++ );\r
482                                 }\r
483                         }\r
484                 }\r
485                 if( apBuf->flags.printLimit == 0 )\r
486                 {\r
487                         apBuf->flags.printLimit--;  /* -1: make it unlimited */\r
488                 }\r
489                 if( ch == 's' )\r
490                 {\r
491                         register char *s = ( char * )va_arg( args, int );\r
492                         if( prints( apBuf, s ? s : "(null)" ) == 0 )\r
493                         {\r
494                                 break;\r
495                         }\r
496                         continue;\r
497                 }\r
498                 if( ch == 'c' )\r
499                 {\r
500                         /* char are converted to int then pushed on the stack */\r
501                         scr[0] = ( char ) va_arg( args, int );\r
502 \r
503                         if( strbuf_printchar( apBuf, scr[0] )  == 0 )\r
504                         {\r
505                                 return;\r
506                         }\r
507 \r
508                         continue;\r
509                 }\r
510                 if( ch == 'l' )\r
511                 {\r
512                         ch = *( format++ );\r
513                         apBuf->flags.long32 = 1;\r
514                         /* Makes not difference as u32 == long */\r
515                 }\r
516                 if( ch == 'L' )\r
517                 {\r
518                         ch = *( format++ );\r
519                         apBuf->flags.long64 = 1;\r
520                         /* Does make a difference */\r
521                 }\r
522                 apBuf->flags.base = 10;\r
523                 apBuf->flags.letBase = 'a';\r
524 \r
525                 if( ch == 'd' || ch == 'u' )\r
526                 {\r
527                         apBuf->flags.isSigned = ( ch == 'd' );\r
528 #if     SPRINTF_LONG_LONG\r
529                         if( apBuf->flags.long64 != pdFALSE )\r
530                         {\r
531                                 if( printll( apBuf, va_arg( args, long long ) ) == 0 )\r
532                                 {\r
533                                         break;\r
534                                 }\r
535                         } else\r
536 #endif  /* SPRINTF_LONG_LONG */\r
537                         if( printi( apBuf, va_arg( args, int ) ) == 0 )\r
538                         {\r
539                                 break;\r
540                         }\r
541                         continue;\r
542                 }\r
543 \r
544                 apBuf->flags.base = 16;         /* From here all hexadecimal */\r
545 \r
546                 if( ch == 'x' && format[0] == 'i' && format[1] == 'p' )\r
547                 {\r
548                         format += 2;    /* eat the "xi" of "xip" */\r
549                         /* Will use base 10 again */\r
550                         if( printIp( apBuf, va_arg( args, int ) ) == 0 )\r
551                         {\r
552                                 break;\r
553                         }\r
554                         continue;\r
555                 }\r
556                 if( ch == 'x' || ch == 'X' || ch == 'p' || ch == 'o' )\r
557                 {\r
558                         if( ch == 'X' )\r
559                         {\r
560                                 apBuf->flags.letBase = 'A';\r
561                         }\r
562                         else if( ch == 'o' )\r
563                         {\r
564                                 apBuf->flags.base = 8;\r
565                         }\r
566 #if     SPRINTF_LONG_LONG\r
567                         if( apBuf->flags.long64 != pdFALSE )\r
568                         {\r
569                                 if( printll( apBuf, va_arg( args, long long ) ) == 0 )\r
570                                 {\r
571                                         break;\r
572                                 }\r
573                         } else\r
574 #endif  /* SPRINTF_LONG_LONG */\r
575                         if( printi( apBuf, va_arg( args, int ) ) == 0 )\r
576                         {\r
577                                 break;\r
578                         }\r
579                         continue;\r
580                 }\r
581         }\r
582         strbuf_printchar( apBuf, '\0' );\r
583 }\r
584 /*-----------------------------------------------------------*/\r
585 \r
586 int vsnprintf( char *apBuf, size_t aMaxLen, const char *apFmt, va_list args )\r
587 {\r
588         struct SStringBuf strBuf;\r
589         strbuf_init( &strBuf, apBuf, ( const char* )apBuf + aMaxLen );\r
590         tiny_print( &strBuf, apFmt, args );\r
591 \r
592         return strBuf.curLen;\r
593 }\r
594 /*-----------------------------------------------------------*/\r
595 \r
596 int snprintf( char *apBuf, size_t aMaxLen, const char *apFmt, ... )\r
597 {\r
598         va_list args;\r
599 \r
600         va_start( args,  apFmt );\r
601         struct SStringBuf strBuf;\r
602         strbuf_init( &strBuf, apBuf, ( const char* )apBuf + aMaxLen );\r
603         tiny_print( &strBuf, apFmt, args );\r
604         va_end( args );\r
605 \r
606         return strBuf.curLen;\r
607 }\r
608 /*-----------------------------------------------------------*/\r
609 \r
610 int sprintf( char *apBuf, const char *apFmt, ... )\r
611 {\r
612         va_list args;\r
613 \r
614         va_start( args,  apFmt );\r
615         struct SStringBuf strBuf;\r
616         strbuf_init( &strBuf, apBuf, ( const char * )apBuf + 1024 );\r
617         tiny_print( &strBuf, apFmt, args );\r
618         va_end( args );\r
619 \r
620         return strBuf.curLen;\r
621 }\r
622 /*-----------------------------------------------------------*/\r
623 \r
624 int vsprintf( char *apBuf, const char *apFmt, va_list args )\r
625 {\r
626         struct SStringBuf strBuf;\r
627         strbuf_init( &strBuf, apBuf, ( const char* ) apBuf + 1024 );\r
628         tiny_print( &strBuf, apFmt, args );\r
629 \r
630         return strBuf.curLen;\r
631 }\r
632 /*-----------------------------------------------------------*/\r
633 \r
634 const char *mkSize (unsigned long long aSize, char *apBuf, int aLen)\r
635 {\r
636 static char retString[33];\r
637 size_t gb, mb, kb, sb;\r
638 \r
639         if (apBuf == NULL) {\r
640                 apBuf = retString;\r
641                 aLen = sizeof( retString );\r
642         }\r
643         gb = aSize / (1024*1024*1024);\r
644         aSize -= gb * (1024*1024*1024);\r
645         mb = aSize / (1024*1024);\r
646         aSize -= mb * (1024*1024);\r
647         kb = aSize / (1024);\r
648         aSize -= kb * (1024);\r
649         sb = aSize;\r
650         if( gb )\r
651         {\r
652                 snprintf (apBuf, aLen, "%u.%02u GB", ( unsigned ) gb, ( unsigned ) ( ( 100 * mb ) / 1024ul ) );\r
653         }\r
654         else if( mb )\r
655         {\r
656                 snprintf (apBuf, aLen, "%u.%02u MB", ( unsigned ) mb, ( unsigned ) ( ( 100 * kb) / 1024ul ) );\r
657         }\r
658         else if( kb != 0ul )\r
659         {\r
660                 snprintf (apBuf, aLen, "%u.%02u KB", ( unsigned ) kb, ( unsigned ) ( ( 100 * sb) / 1024ul ) );\r
661         }\r
662         else\r
663         {\r
664                 snprintf (apBuf, aLen, "%u bytes", ( unsigned ) sb);\r
665         }\r
666         return apBuf;\r
667 }\r