Diff: SEGGER_RTT_printf.c

Revision:

0:7dcd871d726b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SEGGER_RTT_printf.c	Mon Dec 18 10:17:08 2017 +0000
@@ -0,0 +1,443 @@
+/*********************************************************************
+*              SEGGER MICROCONTROLLER GmbH & Co. KG                  *
+*        Solutions for real time microcontroller applications        *
+**********************************************************************
+*                                                                    *
+*        (c) 2014-2014 SEGGER Microcontroller GmbH & Co. KG          *
+*                                                                    *
+*       Internet: www.segger.com Support: support@segger.com         *
+*                                                                    *
+**********************************************************************
+----------------------------------------------------------------------
+File    : SEGGER_RTT_printf.c
+Date    : 17 Dec 2014
+Purpose : Replacement for printf to write formatted data via RTT
+---------------------------END-OF-HEADER------------------------------
+*/
+#include "SEGGER_RTT.h"
+#include "SEGGER_RTT_Conf.h"
+
+/*********************************************************************
+*
+*       Defines, configurable
+*
+**********************************************************************
+*/
+
+#ifndef SEGGER_RTT_PRINTF_BUFFER_SIZE
+  #define SEGGER_RTT_PRINTF_BUFFER_SIZE (64)
+#endif
+
+#include <stdlib.h>
+#include <stdarg.h>
+
+
+#define FORMAT_FLAG_LEFT_JUSTIFY   (1 << 0)
+#define FORMAT_FLAG_PAD_ZERO       (1 << 1)
+#define FORMAT_FLAG_PRINT_SIGN     (1 << 2)
+#define FORMAT_FLAG_ALTERNATE      (1 << 3)
+
+/*********************************************************************
+*
+*       Types
+*
+**********************************************************************
+*/
+
+typedef struct {
+  char* pBuffer;
+  int   BufferSize;
+  int   Cnt;
+
+  int   ReturnValue;
+
+  unsigned RTTBufferIndex;
+} SEGGER_RTT_PRINTF_DESC;
+
+/*********************************************************************
+*
+*       Function prototypes
+*
+**********************************************************************
+*/
+int SEGGER_RTT_vprintf(unsigned BufferIndex, const char * sFormat, va_list * pParamList);
+
+/*********************************************************************
+*
+*       Static code
+*
+**********************************************************************
+*/
+/*********************************************************************
+*
+*       _StoreChar
+*/
+static void _StoreChar(SEGGER_RTT_PRINTF_DESC * p, char c) {
+  int Cnt;
+
+  Cnt = p->Cnt;
+  if ((Cnt + 1) <= p->BufferSize) {
+    *(p->pBuffer + Cnt) = c;
+    p->Cnt = Cnt + 1;
+    p->ReturnValue++;
+  }
+  //
+  // Write part of string, when the buffer is full
+  //
+  if (p->Cnt == p->BufferSize) {
+    if (SEGGER_RTT_Write(p->RTTBufferIndex, p->pBuffer, p->Cnt) != p->Cnt) {
+      p->ReturnValue = -1;
+    } else {
+      p->Cnt = 0;
+    }
+  }
+}
+
+/*********************************************************************
+*
+*       _PrintUnsigned
+*/
+static void _PrintUnsigned(SEGGER_RTT_PRINTF_DESC * pBufferDesc, unsigned v, unsigned Base, int NumDigits, unsigned FieldWidth, unsigned FormatFlags) {
+  static const char _aV2C[16] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' };
+  unsigned Div;
+  unsigned Digit = 1;
+  unsigned Number;
+  unsigned Width;
+  char c;
+
+  Number = v;
+
+  //
+  // Get actual field width
+  //
+  Width = 1;
+  while (Number >= Base) {
+    Number = (Number / Base);
+    Width++;
+  }
+  if ((unsigned)NumDigits > Width) {
+    Width = NumDigits;
+  }
+  //
+  // Print leading chars if necessary
+  //
+  if ((FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY) == 0) {
+    if (FieldWidth != 0) {
+      if (((FormatFlags & FORMAT_FLAG_PAD_ZERO) == FORMAT_FLAG_PAD_ZERO) && (NumDigits == 0)) {
+        c = '0';
+      } else {
+        c = ' ';
+      }
+      while ((FieldWidth != 0) && (Width < FieldWidth--)) {
+        _StoreChar(pBufferDesc, c);
+        if (pBufferDesc->ReturnValue < 0) {
+          return;
+        }
+      }
+    }
+  }
+  //
+  // Count how many digits are required by precision
+  //
+  while (((v / Digit) >= Base) | (NumDigits-- > 1)) {
+    Digit *= Base;
+  }
+  //
+  // Output digits
+  //
+  do {
+    Div = v / Digit;
+    v -= Div * Digit;
+    _StoreChar(pBufferDesc, _aV2C[Div]);
+    if (pBufferDesc->ReturnValue < 0) {
+      break;
+    }
+    Digit /= Base;
+  } while (Digit);
+  //
+  // Print trailing spaces if necessary
+  //
+  if ((FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY) == FORMAT_FLAG_LEFT_JUSTIFY) {
+    if (FieldWidth != 0) {
+      while ((FieldWidth != 0) && (Width < FieldWidth--)) {
+        _StoreChar(pBufferDesc, ' ');
+        if (pBufferDesc->ReturnValue < 0) {
+          return;
+        }
+      }
+    }
+  }
+}
+
+/*********************************************************************
+*
+*       _PrintInt
+*/
+static void _PrintInt(SEGGER_RTT_PRINTF_DESC * pBufferDesc, int v, unsigned Base, unsigned NumDigits, unsigned FieldWidth, unsigned FormatFlags) {
+  unsigned Width;
+  unsigned Number;
+
+  Number = (v < 0) ? -v : v;
+
+  //
+  // Get actual field width
+  //
+  Width = 1;
+  while (Number >= Base) {
+    Number = (Number / Base);
+    Width++;
+  }
+  if (NumDigits > Width) {
+    Width = NumDigits;
+  }
+  if ((FieldWidth > 0) && ((v < 0) || ((FormatFlags & FORMAT_FLAG_PRINT_SIGN) == FORMAT_FLAG_PRINT_SIGN))) {
+    FieldWidth--;
+  }
+
+  //
+  // Print leading spaces if necessary
+  //
+  if ((((FormatFlags & FORMAT_FLAG_PAD_ZERO) == 0) || (NumDigits != 0)) && ((FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY) == 0)) {
+    if (FieldWidth != 0) {
+      while ((FieldWidth != 0) && (Width < FieldWidth--)) {
+        _StoreChar(pBufferDesc, ' ');
+        if (pBufferDesc->ReturnValue < 0) {
+          return;
+        }
+      }
+    }
+  }
+  //
+  // Print sign if necessary
+  //
+  if (v < 0) {
+    v = -v;
+    _StoreChar(pBufferDesc, '-');
+    if (pBufferDesc->ReturnValue < 0) {
+      return;
+    }
+  } else if ((FormatFlags & FORMAT_FLAG_PRINT_SIGN) == FORMAT_FLAG_PRINT_SIGN) {
+    _StoreChar(pBufferDesc, '+');
+    if (pBufferDesc->ReturnValue < 0) {
+      return;
+    }
+  }
+  //
+  // Print leading zeros if necessary
+  //
+  if (((FormatFlags & FORMAT_FLAG_PAD_ZERO) == FORMAT_FLAG_PAD_ZERO) && ((FormatFlags & FORMAT_FLAG_LEFT_JUSTIFY) == 0) && (NumDigits == 0)) {
+    if (FieldWidth != 0) {
+      while ((FieldWidth != 0) && (Width < FieldWidth--)) {
+        _StoreChar(pBufferDesc, '0');
+        if (pBufferDesc->ReturnValue < 0) {
+          return;
+        }
+      }
+    }
+  }
+
+  //
+  // Print number without sign
+  //
+  _PrintUnsigned(pBufferDesc, v, Base, NumDigits, FieldWidth, FormatFlags);
+}
+
+/*********************************************************************
+*
+*       Public code
+*
+**********************************************************************
+*/
+/*********************************************************************
+*
+*       SEGGER_RTT_vprintf
+*
+*  Function description
+*    Stores a formatted string in SEGGER RTT control block.
+*    This data is read by the host.
+*
+*  Parameters
+*    BufferIndex  Index of "Up"-buffer to be used. (e.g. 0 for "Terminal")
+*    sFormat      Pointer to format string
+*    pParamList   Pointer to the list of arguments for the format string
+*
+*  Return values
+*    >= 0:  Number of bytes which have been stored in the "Up"-buffer.
+*     < 0:  Error
+*/
+int SEGGER_RTT_vprintf(unsigned BufferIndex, const char * sFormat, va_list * pParamList) {
+  char c;
+  SEGGER_RTT_PRINTF_DESC BufferDesc;
+  int v;
+  unsigned NumDigits;
+  unsigned FormatFlags;
+  unsigned FieldWidth;
+  char acBuffer[SEGGER_RTT_PRINTF_BUFFER_SIZE];
+
+  BufferDesc.pBuffer        = acBuffer;
+  BufferDesc.BufferSize     = SEGGER_RTT_PRINTF_BUFFER_SIZE;
+  BufferDesc.Cnt            = 0;
+  BufferDesc.RTTBufferIndex = BufferIndex;
+  BufferDesc.ReturnValue    = 0;
+
+  do {
+    c = *sFormat++;
+    if (c == 0) {
+      break;
+    }
+    if (c == '%') {
+      //
+      // Filter out flags
+      //
+      FormatFlags = 0;
+      do {
+        c = *sFormat;
+        switch (c) {
+        case '-': FormatFlags |= FORMAT_FLAG_LEFT_JUSTIFY; sFormat++; break;
+        case '0': FormatFlags |= FORMAT_FLAG_PAD_ZERO;     sFormat++; break;
+        case '+': FormatFlags |= FORMAT_FLAG_PRINT_SIGN;   sFormat++; break;
+        case '#': FormatFlags |= FORMAT_FLAG_ALTERNATE;    sFormat++; break;
+        default:  goto FilterFieldWidth;                   break;
+        }
+      } while (1);
+      //
+      // filter out field with
+      //
+FilterFieldWidth:
+      FieldWidth = 0;
+      do {
+        c = *sFormat;
+        if (c < '0' || c > '9') {
+          break;
+        }
+        sFormat++;
+        FieldWidth = FieldWidth * 10 + (c - '0');
+      } while (1);
+
+      //
+      // Filter out precision (number of digits to display)
+      //
+      NumDigits = 0;
+      c = *sFormat;
+      if (c == '.') {
+        sFormat++;
+        do {
+          c = *sFormat;
+          if (c < '0' || c > '9') {
+            break;
+          }
+          sFormat++;
+          NumDigits = NumDigits * 10 + (c - '0');
+        } while (1);
+      }
+      //
+      // Filter out length modifier
+      //
+      c = *sFormat;
+      do {
+        if (c == 'l' || c == 'h') {
+          c = *sFormat++;
+          continue;
+        }
+        break;
+      } while (1);
+      //
+      // Handle specifiers
+      //
+      switch (c) {
+      case 'c': {
+        char c0;
+        v = va_arg(*pParamList, int);
+        c0 = (char)v;
+        _StoreChar(&BufferDesc, c0);
+        break;
+      }
+      case 'd':
+        v = va_arg(*pParamList, int);
+        _PrintInt(&BufferDesc, v, 10, NumDigits, FieldWidth, FormatFlags);
+        break;
+      case 'u':
+        v = va_arg(*pParamList, int);
+        _PrintUnsigned(&BufferDesc, v, 10, NumDigits, FieldWidth, FormatFlags);
+        break;
+      case 'x':
+      case 'X':
+        v = va_arg(*pParamList, int);
+        _PrintUnsigned(&BufferDesc, v, 16, NumDigits, FieldWidth, FormatFlags);
+        break;
+      case 's':
+        {
+          const char * s = va_arg(*pParamList, const char *);
+          do {
+            c = *s++;
+            if (c == 0) {
+              break;
+            }
+           _StoreChar(&BufferDesc, c);
+          } while (BufferDesc.ReturnValue >= 0);
+        }
+        break;
+      case 'p':
+        v = va_arg(*pParamList, int);
+        _PrintUnsigned(&BufferDesc, v, 16, 8, 8, 0);
+        break;
+      case '%':
+        _StoreChar(&BufferDesc, '%');
+        break;
+      }
+      sFormat++;
+    } else {
+      _StoreChar(&BufferDesc, c);
+    }
+  } while (BufferDesc.ReturnValue >= 0);
+
+  if (BufferDesc.ReturnValue > 0) {
+    //
+    // Write remaining data, if any
+    //
+    if (BufferDesc.Cnt != 0) {
+      SEGGER_RTT_Write(BufferIndex, acBuffer, BufferDesc.Cnt);
+    }
+    BufferDesc.ReturnValue += BufferDesc.Cnt;
+  }
+  return BufferDesc.ReturnValue;
+}
+
+/*********************************************************************
+*
+*       SEGGER_RTT_printf
+*
+*  Function description
+*    Stores a formatted string in SEGGER RTT control block.
+*    This data is read by the host.
+*
+*  Parameters
+*    BufferIndex  Index of "Up"-buffer to be used. (e.g. 0 for "Terminal")
+*    sFormat      Pointer to format string, followed by the arguments for conversion
+*
+*  Return values
+*    >= 0:  Number of bytes which have been stored in the "Up"-buffer.
+*     < 0:  Error
+*
+*  Notes
+*    (1) Conversion specifications have following syntax:
+*          %[flags][FieldWidth][.Precision]ConversionSpecifier
+*    (2) Supported flags:
+*          -: Left justify within the field width
+*          +: Always print sign extension for signed conversions
+*          0: Pad with 0 instead of spaces. Ignored when using '-'-flag or precision
+*        Supported conversion specifiers:
+*          c: Print the argument as one char
+*          d: Print the argument as a signed integer
+*          u: Print the argument as an unsigned integer
+*          x: Print the argument as an hexadecimal integer
+*          s: Print the string pointed to by the argument
+*          p: Print the argument as an 8-digit hexadecimal integer. (Argument shall be a pointer to void.)
+*/
+int SEGGER_RTT_printf(unsigned BufferIndex, const char * sFormat, ...) {
+  va_list ParamList;
+
+  va_start(ParamList, sFormat);
+  return SEGGER_RTT_vprintf(BufferIndex, sFormat, &ParamList);
+}
+/*************************** End of file ****************************/