1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-05 13:24:11 +01:00
LibrePilot/flight/libraries/printf2.c

643 lines
19 KiB
C
Raw Blame History

/*******************************************************************************
Copyright 2001, 2002 Georges Menie (<URL snipped>)
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/*******************************************************************************
putchar is the only external dependency for this file,
if you have a working putchar, just remove the following
define. If the function should be called something else,
replace outbyte(c) by your own function call.
*/
// *******************************************************************************
// Updated by Daniel D Miller. Changes to the original Menie code are
// Copyright 2009-2012 Daniel D Miller
// All such changes are distributed under the same license as the original,
// as described above.
// 11/06/09 - adding floating-point support
// 03/19/10 - pad fractional portion of floating-point number with 0s
// 03/30/10 - document the \% bug
// 07/20/10 - Fix a round-off bug in floating-point conversions
// ( 0.99 with %.1f did not round to 1.0 )
// 10/25/11 - Add support for %+ format (always show + on positive numbers)
// 01/19/12 - fix handling of %f with no decimal; it was defaulting to 0
// decimal places, rather than printf's 6.
// *******************************************************************************
// BUGS
// If '%' is included in a format string, in the form \% with the intent
// of including the percent sign in the output string, this function will
// mis-handle the data entirely!!
// Essentially, it will just discard the character following the percent sign.
// This bug is not easy to fix in the existing code;
// for now, I'll just try to remember to use %% instead of \% ...
// *******************************************************************************
// lint -esym(752, debug_output)
// lint -esym(766, stdio.h)
// #define TEST_PRINTF 1
#include <pios.h>
static uint use_leading_plus = 0;
/* based on a example-code from Keil for CS G++ */
/* for caddr_t (typedef char * caddr_t;) */
#include <sys/types.h>
// * NEWLIB STUBS *//
#include <stdlib.h>
#include <sys/unistd.h>
#include <sys/stat.h>
#include <sys/times.h>
#include <errno.h>
/*==============================================================================
* Environment variables.
* A pointer to a list of environment variables and their values. For a minimal
* environment, this empty list is adequate:
*/
char *__env[1] = { 0 };
char * *environ = __env;
/*==============================================================================
* Close a file.
*/
int _close(__attribute__((unused)) int file)
{
return -1;
}
/*==============================================================================
* Transfer control to a new process.
*/
int _execve(__attribute__((unused)) char *name, __attribute__((unused)) char * *argv, __attribute__((unused)) char * *env)
{
errno = ENOMEM;
return -1;
}
/*==============================================================================
* Exit a program without cleaning up files.
*/
void _exit(__attribute__((unused)) int code)
{
/* Should we force a system reset? */
while (1) {
;
}
}
/*==============================================================================
* Create a new process.
*/
int _fork(void)
{
errno = EAGAIN;
return -1;
}
/*==============================================================================
* Status of an open file.
*/
int _fstat(__attribute__((unused)) int file, struct stat *st)
{
st->st_mode = S_IFCHR;
return 0;
}
/*==============================================================================
* Process-ID
*/
int _getpid(void)
{
return 1;
}
/*==============================================================================
* Query whether output stream is a terminal.
*/
int _isatty(__attribute__((unused)) int file)
{
return 1;
}
/*==============================================================================
* Send a signal.
*/
int _kill(__attribute__((unused)) int pid, __attribute__((unused)) int sig)
{
errno = EINVAL;
return -1;
}
/*==============================================================================
* Establish a new name for an existing file.
*/
int _link(__attribute__((unused)) char *old, __attribute__((unused)) char *new)
{
errno = EMLINK;
return -1;
}
/*==============================================================================
* Set position in a file.
*/
int _lseek(__attribute__((unused)) int file, __attribute__((unused)) int ptr, __attribute__((unused)) int dir)
{
return 0;
}
/*==============================================================================
* Open a file.
*/
int _open(__attribute__((unused)) const char *name, __attribute__((unused)) int flags, __attribute__((unused)) int mode)
{
return -1;
}
/*==============================================================================
* Read from a file.
*/
int _read(__attribute__((unused)) int file, __attribute__((unused)) char *ptr, __attribute__((unused)) int len)
{
return 0;
}
/*==============================================================================
* Write to a file. libc subroutines will use this system routine for output to
* all files, including stdout<75>so if you need to generate any output, for
* example to a serial port for debugging, you should make your minimal write
* capable of doing this.
*/
int _write_r(__attribute__((unused)) void *reent, __attribute__((unused)) int file, __attribute__((unused)) char *ptr, __attribute__((unused)) int len)
{
return 0;
}
/*==============================================================================
* Increase program data space. As malloc and related functions depend on this,
* it is useful to have a working implementation. The following suffices for a
* standalone system; it exploits the symbol _end automatically defined by the
* GNU linker.
*/
caddr_t _sbrk(int incr)
{
extern char _end; /* Defined by the linker */
static char *heap_end;
char *prev_heap_end;
char *stack_ptr;
if (heap_end == 0) {
heap_end = &_end;
}
prev_heap_end = heap_end;
asm volatile ("MRS %0, msp" : "=r" (stack_ptr));
if (heap_end + incr > stack_ptr) {
_write_r((void *)0, 1, "Heap and stack collision\n", 25);
_exit(1);
}
heap_end += incr;
return (caddr_t)prev_heap_end;
}
/*==============================================================================
* Status of a file (by name).
*/
int _stat(__attribute__((unused)) char *file, struct stat *st)
{
st->st_mode = S_IFCHR;
return 0;
}
/*==============================================================================
* Timing information for current process.
*/
int _times(__attribute__((unused)) struct tms *buf)
{
return -1;
}
/*==============================================================================
* Remove a file's directory entry.
*/
int _unlink(__attribute__((unused)) char *name)
{
errno = ENOENT;
return -1;
}
/*==============================================================================
* Wait for a child process.
*/
int _wait(__attribute__((unused)) int *status)
{
errno = ECHILD;
return -1;
}
// * NEWLIB STUBS *//
// ****************************************************************************
static void printchar(char * *str, int c)
{
if (str) {
**str = c;
++(*str);
}
#ifdef TEST_PRINTF
else {
extern int putchar(int c);
(void)putchar(c);
}
#endif
}
// ****************************************************************************
static uint my_strlen(char *str)
{
if (str == 0) {
return 0;
}
uint slen = 0;
while (*str != 0) {
slen++;
str++;
}
return slen;
}
// ****************************************************************************
// This version returns the length of the output string.
// It is more useful when implementing a walking-string function.
// ****************************************************************************
static const double round_nums[8] = {
0.5L,
0.05L,
0.005L,
0.0005L,
0.00005L,
0.000005L,
0.0000005L,
0.00000005L
};
static unsigned dbl2stri(char *outbfr, double dbl, unsigned dec_digits)
{
static char local_bfr[128];
char *output = (outbfr == 0) ? local_bfr : outbfr;
// *******************************************
// extract negative info
// *******************************************
if (dbl < 0.0L) {
*output++ = '-';
dbl *= -1.0L;
} else {
if (use_leading_plus) {
*output++ = '+';
}
}
// handling rounding by adding .5LSB to the floating-point data
if (dec_digits < 8) {
dbl += round_nums[dec_digits];
}
// **************************************************************************
// construct fractional multiplier for specified number of digits.
// **************************************************************************
uint mult = 1;
uint idx;
for (idx = 0; idx < dec_digits; idx++) {
mult *= 10;
}
// printf("mult=%u\n", mult) ;
uint wholeNum = (uint)dbl;
uint decimalNum = (uint)((dbl - wholeNum) * mult);
// *******************************************
// convert integer portion
// *******************************************
char tbfr[40];
idx = 0;
while (wholeNum != 0) {
tbfr[idx++] = '0' + (wholeNum % 10);
wholeNum /= 10;
}
// printf("%.3f: whole=%s, dec=%d\n", dbl, tbfr, decimalNum) ;
if (idx == 0) {
*output++ = '0';
} else {
while (idx > 0) {
*output++ = tbfr[idx - 1]; // lint !e771
idx--;
}
}
if (dec_digits > 0) {
*output++ = '.';
// *******************************************
// convert fractional portion
// *******************************************
idx = 0;
while (decimalNum != 0) {
tbfr[idx++] = '0' + (decimalNum % 10);
decimalNum /= 10;
}
// pad the decimal portion with 0s as necessary;
// We wouldn't want to report 3.093 as 3.93, would we??
while (idx < dec_digits) {
tbfr[idx++] = '0';
}
// printf("decimal=%s\n", tbfr) ;
if (idx == 0) {
*output++ = '0';
} else {
while (idx > 0) {
*output++ = tbfr[idx - 1];
idx--;
}
}
}
*output = 0;
// prepare output
output = (outbfr == 0) ? local_bfr : outbfr;
return my_strlen(output);
}
// ****************************************************************************
#define PAD_RIGHT 1
#define PAD_ZERO 2
static int prints(char * *out, const char *string, int width, int pad)
{
register int pc = 0, padchar = ' ';
if (width > 0) {
int len = 0;
const char *ptr;
for (ptr = string; *ptr; ++ptr) {
++len;
}
if (len >= width) {
width = 0;
} else {
width -= len;
}
if (pad & PAD_ZERO) {
padchar = '0';
}
}
if (!(pad & PAD_RIGHT)) {
for (; width > 0; --width) {
printchar(out, padchar);
++pc;
}
}
for (; *string; ++string) {
printchar(out, *string);
++pc;
}
for (; width > 0; --width) {
printchar(out, padchar);
++pc;
}
return pc;
}
// ****************************************************************************
/* the following should be enough for 32 bit int */
#define PRINT_BUF_LEN 12
static int printi(char * *out, int i, int b, int sg, int width, int pad, int letbase)
{
char print_buf[PRINT_BUF_LEN];
char *s;
int t, neg = 0, pc = 0;
unsigned u = (unsigned)i;
if (i == 0) {
print_buf[0] = '0';
print_buf[1] = '\0';
return prints(out, print_buf, width, pad);
}
if (sg && b == 10 && i < 0) {
neg = 1;
u = (unsigned)-i;
}
// make sure print_buf is NULL-term
s = print_buf + PRINT_BUF_LEN - 1;
*s = '\0';
while (u) {
t = u % b; // lint !e573 Warning 573: Signed-unsigned mix with divide
if (t >= 10) {
t += letbase - '0' - 10;
}
*--s = t + '0';
u /= b; // lint !e573 Warning 573: Signed-unsigned mix with divide
}
if (neg) {
if (width && (pad & PAD_ZERO)) {
printchar(out, '-');
++pc;
--width;
} else {
*--s = '-';
}
} else {
if (use_leading_plus) {
*--s = '+';
}
}
return pc + prints(out, s, width, pad);
}
// ****************************************************************************
static int print(char * *out, int *varg)
{
int post_decimal;
int width, pad;
unsigned dec_width = 6;
int pc = 0;
char *format = (char *)(*varg++);
char scr[2];
use_leading_plus = 0; // start out with this clear
for (; *format != 0; ++format) {
if (*format == '%') {
dec_width = 6;
++format;
width = pad = 0;
if (*format == '\0') {
break;
}
if (*format == '%') {
goto out_lbl;
}
if (*format == '-') {
++format;
pad = PAD_RIGHT;
}
if (*format == '+') {
++format;
use_leading_plus = 1;
}
while (*format == '0') {
++format;
pad |= PAD_ZERO;
}
post_decimal = 0;
if (*format == '.' ||
(*format >= '0' && *format <= '9')) {
while (1) {
if (*format == '.') {
post_decimal = 1;
dec_width = 0;
format++;
} else if ((*format >= '0' && *format <= '9')) {
if (post_decimal) {
dec_width *= 10;
dec_width += *format - '0';
} else {
width *= 10;
width += *format - '0';
}
format++;
} else {
break;
}
}
}
if (*format == 'l') {
++format;
}
switch (*format) {
case 's':
{
// char *s = *((char **) varg++); //lint !e740
char *s = (char *)*varg++; // lint !e740 !e826 convert to double pointer
pc += prints(out, s ? s : "(null)", width, pad);
use_leading_plus = 0; // reset this flag after printing one value
}
break;
case 'd':
pc += printi(out, *varg++, 10, 1, width, pad, 'a');
use_leading_plus = 0; // reset this flag after printing one value
break;
case 'x':
pc += printi(out, *varg++, 16, 0, width, pad, 'a');
use_leading_plus = 0; // reset this flag after printing one value
break;
case 'X':
pc += printi(out, *varg++, 16, 0, width, pad, 'A');
use_leading_plus = 0; // reset this flag after printing one value
break;
case 'u':
pc += printi(out, *varg++, 10, 0, width, pad, 'a');
use_leading_plus = 0; // reset this flag after printing one value
break;
case 'c':
/* char are converted to int then pushed on the stack */
scr[0] = *varg++;
scr[1] = '\0';
pc += prints(out, scr, width, pad);
use_leading_plus = 0; // reset this flag after printing one value
break;
case 'f':
{
// http://wiki.debian.org/ArmEabiPort#Structpackingandalignment
// Stack alignment
//
// The ARM EABI requires 8-byte stack alignment at public function entry points,
// compared to the previous 4-byte alignment.
#ifdef USE_NEWLIB
char *cptr = (char *)varg; // lint !e740 !e826 convert to double pointer
uint caddr = (uint)cptr;
if ((caddr & 0xF) != 0) {
cptr += 4;
}
double *dblptr = (double *)cptr; // lint !e740 !e826 convert to double pointer
#else
double *dblptr = (double *)varg; // lint !e740 !e826 convert to double pointer
#endif
double dbl = *dblptr++; // increment double pointer
varg = (int *)dblptr; // lint !e740 copy updated pointer back to base pointer
char bfr[81];
// unsigned slen =
dbl2stri(bfr, dbl, dec_width);
// stuff_talkf("[%s], width=%u, dec_width=%u\n", bfr, width, dec_width) ;
pc += prints(out, bfr, width, pad);
use_leading_plus = 0; // reset this flag after printing one value
}
break;
default:
printchar(out, '%');
printchar(out, *format);
use_leading_plus = 0; // reset this flag after printing one value
break;
}
} else {
// if (*format == '\\') {
//
// } else
out_lbl:
printchar(out, *format);
++pc;
}
} // for each char in format string
if (out) { // lint !e850
**out = '\0';
}
return pc;
}
// ****************************************************************************
int stringf(char *out, const char *format, ...)
{
// create a pointer into the stack.
// Thematically this should be a void*, since the actual usage of the
// pointer will vary. However, int* works fine too.
// Either way, the called function will need to re-cast the pointer
// for any type which isn't sizeof(int)
int *varg = (int *)(char *)(&format);
return print(&out, varg);
}
int printf(const char *format, ...)
{
int *varg = (int *)(char *)(&format);
return print(0, varg);
}
int sprintf(char *out, const char *format, ...)
{
int *varg = (int *)(char *)(&format);
return print(&out, varg);
}
/**
* @}
*/