Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1471 Nick666 1
// Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist nicht von der Lizenz für den MikroKopter-Teil unterstellt
2
 
3
/*
4
Copyright (C) 1993 Free Software Foundation
5
 
6
This file is part of the GNU IO Library.  This library is free
7
software; you can redistribute it and/or modify it under the
8
terms of the GNU General Public License as published by the
9
Free Software Foundation; either version 2, or (at your option)
10
any later version.
11
 
12
This library is distributed in the hope that it will be useful,
13
but WITHOUT ANY WARRANTY; without even the implied warranty of
14
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15
GNU General Public License for more details.
16
 
17
You should have received a copy of the GNU General Public License
18
along with this library; see the file COPYING.  If not, write to the Free
19
Software Foundation, 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
20
 
21
As a special exception, if you link this library with files
22
compiled with a GNU compiler to produce an executable, this does not cause
23
the resulting executable to be covered by the GNU General Public License.
24
This exception does not however invalidate any other reasons why
25
the executable file might be covered by the GNU General Public License. */
26
 
27
/*
28
 * Copyright (c) 1990 Regents of the University of California.
29
 * All rights reserved.
30
 *
31
 * Redistribution and use in source and binary forms, with or without
32
 * modification, are permitted provided that the following conditions
33
 * are met:
34
 * 1. Redistributions of source code must retain the above copyright
35
 *    notice, this list of conditions and the following disclaimer.
36
 * 2. Redistributions in binary form must reproduce the above copyright
37
 *    notice, this list of conditions and the following disclaimer in the
38
 *    documentation and/or other materials provided with the distribution.
39
 * 3. [rescinded 22 July 1999]
40
 * 4. Neither the name of the University nor the names of its contributors
41
 *    may be used to endorse or promote products derived from this software
42
 *    without specific prior written permission.
43
 *
44
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
45
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
46
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
47
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
48
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
49
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
50
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
51
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
52
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
53
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
54
 * SUCH DAMAGE.
55
 */
56
 
57
/******************************************************************************
58
 This file is a patched version of printf called _printf_P
59
 It is made to work with avr-gcc for Atmel AVR MCUs.
60
 There are some differences from standard printf:
61
        1. There is no floating point support (with fp the code is about 8K!)
62
        2. Return type is void
63
        3. Format string must be in program memory (by using macro printf this is
64
           done automaticaly)
65
        4. %n is not implemented (just remove the comment around it if you need it)
66
        5. If LIGHTPRINTF is defined, the code is about 550 bytes smaller and the
67
           folowing specifiers are disabled :
68
                space # * . - + p s o O
69
        6. A function void uart_sendchar(char c) is used for output. The UART must
70
                be initialized before using printf.
71
 
72
 Alexander Popov
73
 sasho@vip.orbitel.bg
74
******************************************************************************/
75
 
76
/*
77
 * Actual printf innards.
78
 *
79
 * This code is large and complicated...
80
 */
81
 
82
#include <string.h>
83
#ifdef __STDC__
84
#include <stdarg.h>
85
#else
86
#include <varargs.h>
87
#endif
88
 
89
#include "old_macros.h"
90
#include "printf_P.h"
91
#include "menu.h"
92
#include "uart0.h"
93
 
94
 
95
//#define LIGHTPRINTF
96
char PrintZiel;
97
 
98
 
99
char Putchar(char zeichen)
100
{
101
 if(PrintZiel == OUT_LCD) { DisplayBuff[DispPtr++] = zeichen; return(1);}
102
 else                       return(uart_putchar(zeichen));
103
}
104
 
105
 
106
void PRINT(const char * ptr, unsigned int len)
107
{
108
 for(;len;len--) Putchar(*ptr++);
109
}
110
 
111
void PRINTP(const char * ptr, unsigned int len)
112
{
113
 for(;len;len--) Putchar(pgm_read_byte(ptr++));
114
}
115
 
116
void PAD_SP(signed char howmany)
117
{
118
        for(;howmany>0;howmany--) Putchar(' ');
119
}
120
 
121
void PAD_0(signed char howmany)
122
{
123
        for(;howmany>0;howmany--) Putchar('0');
124
}
125
 
126
#define BUF             40
127
 
128
/*
129
 * Macros for converting digits to letters and vice versa
130
 */
131
#define to_digit(c)     ((c) - '0')
132
#define  is_digit(c)    ((c)<='9' && (c)>='0')
133
#define to_char(n)      ((n) + '0')
134
 
135
/*
136
 * Flags used during conversion.
137
 */
138
#define LONGINT         0x01            /* long integer */
139
#define LONGDBL         0x02            /* long double; unimplemented */
140
#define SHORTINT                0x04            /* short integer */
141
#define ALT                     0x08            /* alternate form */
142
#define LADJUST         0x10            /* left adjustment */
143
#define ZEROPAD         0x20            /* zero (as opposed to blank) pad */
144
#define HEXPREFIX       0x40            /* add 0x or 0X prefix */
145
 
146
void _printf_P (char ziel,char const *fmt0, ...)      /* Works with string from FLASH */
147
{
148
        va_list ap;
149
        register const char *fmt; /* format string */
150
        register char ch;       /* character from fmt */
151
        register int n;         /* handy integer (short term usage) */
152
        register char *cp;      /* handy char pointer (short term usage) */
153
        const char *fmark;      /* for remembering a place in fmt */
154
        register unsigned char flags;   /* flags as above */
155
        signed char width;              /* width from format (%8d), or 0 */
156
        signed char prec;               /* precision from format (%.3d), or -1 */
157
        char sign;                              /* sign prefix (' ', '+', '-', or \0) */
158
        unsigned long _ulong=0; /* integer arguments %[diouxX] */
159
#define OCT 8
160
#define DEC 10
161
#define HEX 16
162
        unsigned char base;             /* base for [diouxX] conversion */
163
        signed char dprec;              /* a copy of prec if [diouxX], 0 otherwise */
164
        signed char dpad;                       /* extra 0 padding needed for integers */
165
        signed char fieldsz;            /* field size expanded by sign, dpad etc */
166
        /* The initialization of 'size' is to suppress a warning that
167
           'size' might be used unitialized.  It seems gcc can't
168
           quite grok this spaghetti code ... */
169
        signed char size = 0;           /* size of converted field or string */
170
        char buf[BUF];          /* space for %c, %[diouxX], %[eEfgG] */
171
        char ox[2];                     /* space for 0x hex-prefix */
172
 
173
    PrintZiel = ziel;  // bestimmt, LCD oder UART
174
        va_start(ap, fmt0);
175
 
176
        fmt = fmt0;
177
 
178
        /*
179
         * Scan the format for conversions (`%' character).
180
         */
181
        for (;;) {
182
                for (fmark = fmt; (ch = pgm_read_byte(fmt)) != '\0' && ch != '%'; fmt++)
183
                        /* void */;
184
                if ((n = fmt - fmark) != 0) {
185
                        PRINTP(fmark, n);
186
                }
187
                if (ch == '\0')
188
                        goto done;
189
                fmt++;          /* skip over '%' */
190
 
191
                flags = 0;
192
                dprec = 0;
193
                width = 0;
194
                prec = -1;
195
                sign = '\0';
196
 
197
rflag:          ch = PRG_RDB(fmt++);
198
reswitch:
199
#ifdef LIGHTPRINTF
200
        if (ch=='o' || ch=='u' || (ch|0x20)=='x') {
201
#else
202
        if (ch=='u' || (ch|0x20)=='x') {
203
#endif
204
                if (flags&LONGINT) {
205
                        _ulong=va_arg(ap, unsigned long);
206
                } else {
207
                        register unsigned int _d;
208
                        _d=va_arg(ap, unsigned int);
209
                        _ulong = flags&SHORTINT ? (unsigned long)(unsigned short)_d : (unsigned long)_d;
210
                }
211
        }
212
 
213
#ifndef LIGHTPRINTF
214
                if(ch==' ') {
215
                        /*
216
                         * ``If the space and + flags both appear, the space
217
                         * flag will be ignored.''
218
                         *      -- ANSI X3J11
219
                         */
220
                        if (!sign)
221
                                sign = ' ';
222
                        goto rflag;
223
                } else if (ch=='#') {
224
                        flags |= ALT;
225
                        goto rflag;
226
                } else if (ch=='*'||ch=='-') {
227
                        if (ch=='*') {
228
                                /*
229
                                 * ``A negative field width argument is taken as a
230
                                 * - flag followed by a positive field width.''
231
                                 *      -- ANSI X3J11
232
                                 * They don't exclude field widths read from args.
233
                                 */
234
                                if ((width = va_arg(ap, int)) >= 0)
235
                                        goto rflag;
236
                                width = -width;
237
                        }
238
                        flags |= LADJUST;
239
                        flags &= ~ZEROPAD; /* '-' disables '0' */
240
                        goto rflag;
241
                } else if (ch=='+') {
242
                        sign = '+';
243
                        goto rflag;
244
                } else if (ch=='.') {
245
                        if ((ch = PRG_RDB(fmt++)) == '*') {
246
                                n = va_arg(ap, int);
247
                                prec = n < 0 ? -1 : n;
248
                                goto rflag;
249
                        }
250
                        n = 0;
251
                        while (is_digit(ch)) {
252
                                n = n*10 + to_digit(ch);
253
                                ch = PRG_RDB(fmt++);
254
                        }
255
                        prec = n < 0 ? -1 : n;
256
                        goto reswitch;
257
                } else
258
#endif /* LIGHTPRINTF */
259
                if (ch=='0') {
260
                        /*
261
                         * ``Note that 0 is taken as a flag, not as the
262
                         * beginning of a field width.''
263
                         *      -- ANSI X3J11
264
                         */
265
                        if (!(flags & LADJUST))
266
                            flags |= ZEROPAD; /* '-' disables '0' */
267
                        goto rflag;
268
                } else if (ch>='1' && ch<='9') {
269
                        n = 0;
270
                        do {
271
                                n = 10 * n + to_digit(ch);
272
                                ch = PRG_RDB(fmt++);
273
                        } while (is_digit(ch));
274
                        width = n;
275
                        goto reswitch;
276
                } else if (ch=='h') {
277
                        flags |= SHORTINT;
278
                        goto rflag;
279
                } else if (ch=='l') {
280
                        flags |= LONGINT;
281
                        goto rflag;
282
                } else if (ch=='c') {
283
                        *(cp = buf) = va_arg(ap, int);
284
                        size = 1;
285
                        sign = '\0';
286
                } else if (ch=='D'||ch=='d'||ch=='i') {
287
                        if(ch=='D')
288
                                flags |= LONGINT;
289
                        if (flags&LONGINT) {
290
                                _ulong=va_arg(ap, long);
291
                        } else {
292
                                register int _d;
293
                                _d=va_arg(ap, int);
294
                                _ulong = flags&SHORTINT ? (long)(short)_d : (long)_d;
295
                        }
296
 
297
                        if ((long)_ulong < 0) {
298
                                _ulong = -_ulong;
299
                                sign = '-';
300
                        }
301
                        base = DEC;
302
                        goto number;
303
                } else
304
/*
305
                if (ch=='n') {
306
                        if (flags & LONGINT)
307
                                *va_arg(ap, long *) = ret;
308
                        else if (flags & SHORTINT)
309
                                *va_arg(ap, short *) = ret;
310
                        else
311
                                *va_arg(ap, int *) = ret;
312
                        continue;       // no output
313
                } else
314
*/
315
#ifndef LIGHTPRINTF
316
                if (ch=='O'||ch=='o') {
317
                        if (ch=='O')
318
                                flags |= LONGINT;
319
                        base = OCT;
320
                        goto nosign;
321
                } else if (ch=='p') {
322
                        /*
323
                         * ``The argument shall be a pointer to void.  The
324
                         * value of the pointer is converted to a sequence
325
                         * of printable characters, in an implementation-
326
                         * defined manner.''
327
                         *      -- ANSI X3J11
328
                         */
329
                        /* NOSTRICT */
330
                        _ulong = (unsigned int)va_arg(ap, void *);
331
                        base = HEX;
332
                        flags |= HEXPREFIX;
333
                        ch = 'x';
334
                        goto nosign;
335
                } else if (ch=='s') {  // print a string from RAM
336
                        if ((cp = va_arg(ap, char *)) == NULL) {
337
                                cp=buf;
338
                                cp[0] = '(';
339
                                cp[1] = 'n';
340
                                cp[2] = 'u';
341
                                cp[4] = cp[3] = 'l';
342
                                cp[5] = ')';
343
                                cp[6] = '\0';
344
                        }
345
                        if (prec >= 0) {
346
                                /*
347
                                 * can't use strlen; can only look for the
348
                                 * NUL in the first `prec' characters, and
349
                                 * strlen() will go further.
350
                                 */
351
                                char *p = (char*)memchr(cp, 0, prec);
352
 
353
                                if (p != NULL) {
354
                                        size = p - cp;
355
                                        if (size > prec)
356
                                                size = prec;
357
                                } else
358
                                        size = prec;
359
                        } else
360
                                size = strlen(cp);
361
                        sign = '\0';
362
                } else
363
#endif /* LIGHTPRINTF */
364
                if(ch=='U'||ch=='u') {
365
                        if (ch=='U')
366
                                flags |= LONGINT;
367
                        base = DEC;
368
                        goto nosign;
369
                } else if (ch=='X'||ch=='x') {
370
                        base = HEX;
371
                        /* leading 0x/X only if non-zero */
372
                        if (flags & ALT && _ulong != 0)
373
                                flags |= HEXPREFIX;
374
 
375
                        /* unsigned conversions */
376
nosign:                 sign = '\0';
377
                        /*
378
                         * ``... diouXx conversions ... if a precision is
379
                         * specified, the 0 flag will be ignored.''
380
                         *      -- ANSI X3J11
381
                         */
382
number: if ((dprec = prec) >= 0)
383
                                flags &= ~ZEROPAD;
384
 
385
                        /*
386
                         * ``The result of converting a zero value with an
387
                         * explicit precision of zero is no characters.''
388
                         *      -- ANSI X3J11
389
                         */
390
                        cp = buf + BUF;
391
                        if (_ulong != 0 || prec != 0) {
392
                                register unsigned char _d,notlastdigit;
393
                                do {
394
                                        notlastdigit=(_ulong>=base);
395
                                        _d = _ulong % base;
396
 
397
                                        if (_d<10) {
398
                                                _d+='0';
399
                                        } else {
400
                                                _d+='a'-10;
401
                                                if (ch=='X') _d&=~0x20;
402
                                        }
403
                                        *--cp=_d;
404
                                        _ulong /= base;
405
                                } while (notlastdigit);
406
#ifndef LIGHTPRINTF
407
                                // handle octal leading 0
408
                                if (base==OCT && flags & ALT && *cp != '0')
409
                                        *--cp = '0';
410
#endif
411
                        }
412
 
413
                        size = buf + BUF - cp;
414
        } else {  //default
415
                /* "%?" prints ?, unless ? is NUL */
416
                        if (ch == '\0')
417
                                goto done;
418
                        /* pretend it was %c with argument ch */
419
                        cp = buf;
420
                        *cp = ch;
421
                        size = 1;
422
                        sign = '\0';
423
                }
424
 
425
                /*
426
                 * All reasonable formats wind up here.  At this point,
427
                 * `cp' points to a string which (if not flags&LADJUST)
428
                 * should be padded out to `width' places.  If
429
                 * flags&ZEROPAD, it should first be prefixed by any
430
                 * sign or other prefix; otherwise, it should be blank
431
                 * padded before the prefix is emitted.  After any
432
                 * left-hand padding and prefixing, emit zeroes
433
                 * required by a decimal [diouxX] precision, then print
434
                 * the string proper, then emit zeroes required by any
435
                 * leftover floating precision; finally, if LADJUST,
436
                 * pad with blanks.
437
                 */
438
 
439
                /*
440
                 * compute actual size, so we know how much to pad.
441
                 */
442
                fieldsz = size;
443
 
444
                dpad = dprec - size;
445
                if (dpad < 0)
446
                    dpad = 0;
447
 
448
                if (sign)
449
                        fieldsz++;
450
                else if (flags & HEXPREFIX)
451
                        fieldsz += 2;
452
                fieldsz += dpad;
453
 
454
                /* right-adjusting blank padding */
455
                if ((flags & (LADJUST|ZEROPAD)) == 0)
456
                        PAD_SP(width - fieldsz);
457
 
458
                /* prefix */
459
                if (sign) {
460
                        PRINT(&sign, 1);
461
                } else if (flags & HEXPREFIX) {
462
                        ox[0] = '0';
463
                        ox[1] = ch;
464
                        PRINT(ox, 2);
465
                }
466
 
467
                /* right-adjusting zero padding */
468
                if ((flags & (LADJUST|ZEROPAD)) == ZEROPAD)
469
                        PAD_0(width - fieldsz);
470
 
471
                /* leading zeroes from decimal precision */
472
                PAD_0(dpad);
473
 
474
                /* the string or number proper */
475
                PRINT(cp, size);
476
 
477
                /* left-adjusting padding (always blank) */
478
                if (flags & LADJUST)
479
                        PAD_SP(width - fieldsz);
480
        }
481
done:
482
        va_end(ap);
483
}