owner-nrf52sdk/nRF5_SDK_17.0.2/external/fprintf/nrf_fprintf_format.c

/*********************************************************************
*                SEGGER Microcontroller GmbH & Co. KG                *
*                        The Embedded Experts                        *
**********************************************************************
*                                                                    *
*       (c) 2014 - 2017  SEGGER Microcontroller GmbH & Co. KG        *
*                                                                    *
*       www.segger.com     Support: support@segger.com               *
*                                                                    *
**********************************************************************
*                                                                    *
*       SEGGER RTT * Real Time Transfer for embedded targets         *
*                                                                    *
**********************************************************************
*                                                                    *
* All rights reserved.                                               *
*                                                                    *
* SEGGER strongly recommends to not make any changes                 *
* to or modify the source code of this software in order to stay     *
* compatible with the RTT protocol and J-Link.                       *
*                                                                    *
* Redistribution and use in source and binary forms, with or         *
* without modification, are permitted provided that the following    *
* conditions are met:                                                *
*                                                                    *
* o Redistributions of source code must retain the above copyright   *
*   notice, this list of conditions and the following disclaimer.    *
*                                                                    *
* o Redistributions in binary form must reproduce the above          *
*   copyright notice, this list of conditions and the following      *
*   disclaimer in the documentation and/or other materials provided  *
*   with the distribution.                                           *
*                                                                    *
* o Neither the name of SEGGER Microcontroller GmbH & Co. KG         *
*   nor the names of its contributors may be used to endorse or      *
*   promote products derived from this software without specific     *
*   prior written permission.                                        *
*                                                                    *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND             *
* CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,        *
* INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF           *
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE           *
* DISCLAIMED. IN NO EVENT SHALL SEGGER Microcontroller BE LIABLE FOR *
* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR           *
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT  *
* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;    *
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF      *
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT          *
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE  *
* USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH   *
* DAMAGE.                                                            *
*                                                                    *
**********************************************************************
*                                                                    *
*       RTT version: 6.14d                                           *
*                                                                    *
*********************************************************************/

#include "sdk_common.h"
#if NRF_MODULE_ENABLED(NRF_FPRINTF)

#include <stdarg.h>

#include "nrf_assert.h"
#include "nrf_fprintf.h"
#include "nrf_fprintf_format.h"

#define NRF_CLI_FORMAT_FLAG_LEFT_JUSTIFY        (1u << 0)
#define NRF_CLI_FORMAT_FLAG_PAD_ZERO            (1u << 1)
#define NRF_CLI_FORMAT_FLAG_PRINT_SIGN          (1u << 2)

#define NRF_CLI_FORMAT_DOUBLE_DEF_PRECISION     6

#define NRF_CLI_FORMAT_DOUBLE_SIGN_POSITION     63U
#define NRF_CLI_FORMAT_DOUBLE_SIGN_MASK         1ULL
#define NRF_CLI_FORMAT_DOUBLE_SIGN              (NRF_CLI_FORMAT_DOUBLE_SIGN_MASK << NRF_CLI_FORMAT_DOUBLE_SIGN_POSITION)
#define NRF_CLI_FORMAT_DOUBLE_EXP_POSITION      52U
#define NRF_CLI_FORMAT_DOUBLE_EXP_MASK          0x7FFULL
#define NRF_CLI_FORMAT_DOUBLE_EXP               (NRF_CLI_FORMAT_DOUBLE_EXP_MASK << NRF_CLI_FORMAT_DOUBLE_EXP_POSITION)
#define NRF_CLI_FORMAT_DOUBLE_MANT_POSITION     0U
#define NRF_CLI_FORMAT_DOUBLE_MANT_MASK         0xFFFFFFFFFFFFF
#define NRF_CLI_FORMAT_DOUBLE_MANT              (NRF_CLI_FORMAT_DOUBLE_MANT_MASK << NRF_CLI_FORMAT_DOUBLE_MANT_POSITION)

#define NRF_CLI_FORMAT_DOUBLE_SIGN_GET(v)       (!!((v) & NRF_CLI_FORMAT_DOUBLE_SIGN))
#define NRF_CLI_FORMAT_DOUBLE_EXP_GET(v)        (((v) & NRF_CLI_FORMAT_DOUBLE_EXP) >> NRF_CLI_FORMAT_DOUBLE_EXP_POSITION)
#define NRF_CLI_FORMAT_DOUBLE_MANT_GET(v)       (((v) & NRF_CLI_FORMAT_DOUBLE_MANT) >> NRF_CLI_FORMAT_DOUBLE_MANT_POSITION)
#define NRF_CLI_FORMAT_REQ_SIGN_SPACE(s, f)     ((s) | (!!((f) & NRF_CLI_FORMAT_FLAG_PRINT_SIGN)))

#define HIGH_32(v)                              ((v) >> 32)
#define LOW_32(v)                               (((1ULL << 32) - 1) & v)


static void buffer_add(nrf_fprintf_ctx_t * const p_ctx, char c)
{
#if NRF_MODULE_ENABLED(NRF_FPRINTF_FLAG_AUTOMATIC_CR_ON_LF)
    if (c == '\n')
    {
        buffer_add(p_ctx, '\r');
    }
#endif
    p_ctx->p_io_buffer[p_ctx->io_buffer_cnt++] = c;

    if (p_ctx->io_buffer_cnt >= p_ctx->io_buffer_size)
    {
        nrf_fprintf_buffer_flush(p_ctx);
    }
}

static void string_print(nrf_fprintf_ctx_t * const p_ctx,
                         char const *              p_str,
                         uint32_t                  FieldWidth,
                         uint32_t                  FormatFlags)
{
    uint32_t Width = 0;
    char c;

    if ((FormatFlags & NRF_CLI_FORMAT_FLAG_LEFT_JUSTIFY) == NRF_CLI_FORMAT_FLAG_LEFT_JUSTIFY)
    {
        while ((c = *p_str) != '\0')
        {
            p_str++;
            Width++;
            buffer_add(p_ctx, c);
        }

        while ((FieldWidth > Width) && (FieldWidth > 0))
        {
            FieldWidth--;
            buffer_add(p_ctx, ' ');
        }
    }
    else
    {
        if (p_str != 0)
        {
            Width = strlen(p_str);
        }

        while ((FieldWidth > Width) && (FieldWidth > 0))
        {
            FieldWidth--;
            buffer_add(p_ctx, ' ');
        }

        while ((c = *p_str) != '\0')
        {
            p_str++;
            Width++;
            buffer_add(p_ctx, c);
        }
    }
}

static void unsigned_print(nrf_fprintf_ctx_t * const p_ctx,
                           uint32_t                  v,
                           uint32_t                  Base,
                           uint32_t                  NumDigits,
                           uint32_t                  FieldWidth,
                           uint32_t                  FormatFlags)
{
    static const char _aV2C[16] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9',
                                   'A', 'B', 'C', 'D', 'E', 'F' };
    uint32_t Div;
    uint32_t Value;
    uint32_t Width;
    char c;

    Value = v;
    //
    // Get actual field width
    //
    Width = 1u;
    while (Value >= Base)
    {
        Value = (Value / Base);
        Width++;
    }
    if (NumDigits > Width)
    {
        Width = NumDigits;
    }
    //
    // Print leading chars if necessary
    //
    if ((FormatFlags & NRF_CLI_FORMAT_FLAG_LEFT_JUSTIFY) == 0u)
    {
        if (FieldWidth != 0u)
        {
            if (((FormatFlags & NRF_CLI_FORMAT_FLAG_PAD_ZERO) == NRF_CLI_FORMAT_FLAG_PAD_ZERO) &&
                (NumDigits == 0u))
            {
                c = '0';
            }
            else
            {
                c = ' ';
            }
            while ((FieldWidth != 0u) && (Width < FieldWidth))
            {
                FieldWidth--;
                buffer_add(p_ctx, c);
            }
        }
    }

    Value = 1;
    /*
     * Compute Digit.
     * Loop until Digit has the value of the highest digit required.
     * Example: If the output is 345 (Base 10), loop 2 times until Digit is 100.
     */
    while (1)
    {
        /* User specified a min number of digits to print? => Make sure we loop at least that
         * often, before checking anything else (> 1 check avoids problems with NumDigits
         * being signed / unsigned)
         */
        if (NumDigits > 1u)
        {
            NumDigits--;
        }
        else
        {
            Div = v / Value;
            // Is our divider big enough to extract the highest digit from value? => Done
            if (Div < Base)
            {
                break;
            }
        }
        Value *= Base;
    }
    //
    // Output digits
    //
    do
    {
        Div = v / Value;
        v -= Div * Value;
        buffer_add(p_ctx, _aV2C[Div]);
        Value /= Base;
    } while (Value);
    //
    // Print trailing spaces if necessary
    //
    if ((FormatFlags & NRF_CLI_FORMAT_FLAG_LEFT_JUSTIFY) == NRF_CLI_FORMAT_FLAG_LEFT_JUSTIFY)
    {
        if (FieldWidth != 0u)
        {
            while ((FieldWidth != 0u) && (Width < FieldWidth))
            {
                FieldWidth--;
                buffer_add(p_ctx, ' ');
            }
        }
    }
}

static void int_print(nrf_fprintf_ctx_t * const p_ctx,
                      int32_t                   v,
                      uint32_t                  Base,
                      uint32_t                  NumDigits,
                      uint32_t                  FieldWidth,
                      uint32_t                  FormatFlags)
{
    uint32_t Width;
    int32_t Number;

    Number = (v < 0) ? -v : v;

    //
    // Get actual field width
    //
    Width = 1u;
    while (Number >= (int32_t)Base)
    {
        Number = (Number / (int32_t)Base);
        Width++;
    }
    if (NumDigits > Width)
    {
        Width = NumDigits;
    }
    if ((FieldWidth > 0u) && ((v < 0) ||
        ((FormatFlags & NRF_CLI_FORMAT_FLAG_PRINT_SIGN) == NRF_CLI_FORMAT_FLAG_PRINT_SIGN)))
    {
        FieldWidth--;
    }
    //
    // Print leading spaces if necessary
    //
    if ((((FormatFlags & NRF_CLI_FORMAT_FLAG_PAD_ZERO) == 0u) || (NumDigits != 0u)) &&
        ((FormatFlags & NRF_CLI_FORMAT_FLAG_LEFT_JUSTIFY) == 0u))
    {
        if (FieldWidth != 0u)
        {
            while ((FieldWidth != 0u) && (Width < FieldWidth))
            {
                FieldWidth--;
                buffer_add(p_ctx, ' ');
            }
        }
    }
    //
    // Print sign if necessary
    //
    if (v < 0)
    {
        v = -v;
        buffer_add(p_ctx, '-');
    }
    else if ((FormatFlags & NRF_CLI_FORMAT_FLAG_PRINT_SIGN) == NRF_CLI_FORMAT_FLAG_PRINT_SIGN)
    {
        buffer_add(p_ctx, '+');
    }
    else
    {
        /* do nothing */
    }
    //
    // Print leading zeros if necessary
    //
    if (((FormatFlags & NRF_CLI_FORMAT_FLAG_PAD_ZERO) == NRF_CLI_FORMAT_FLAG_PAD_ZERO) &&
        ((FormatFlags & NRF_CLI_FORMAT_FLAG_LEFT_JUSTIFY) == 0u) && (NumDigits == 0u))
    {
        if (FieldWidth != 0u)
        {
            while ((FieldWidth != 0u) && (Width < FieldWidth))
            {
                FieldWidth--;
                buffer_add(p_ctx, '0');
            }
        }
    }
    //
    // Print number without sign
    //
    unsigned_print(p_ctx, (uint32_t)v, Base, NumDigits, FieldWidth, FormatFlags);
}

#if NRF_MODULE_ENABLED(NRF_FPRINTF_DOUBLE)

static void fill_space(nrf_fprintf_ctx_t * const p_ctx,
                       uint8_t len,
                       bool zeros)
{
    for (; len > 0; len--)
    {
        if (zeros)
        {
            buffer_add(p_ctx, '0');
        }
        else
        {
            buffer_add(p_ctx, ' ');
        }
    }
}

static void float_print(nrf_fprintf_ctx_t * const p_ctx,
                        double                    v,
                        uint32_t                  digits,
                        uint32_t                  width,
                        uint32_t                  format,
                        bool                      uppercase)
{
    bool sign, transform = false;
    uint64_t num, mant, lead, low, base, res, carry, x, s0, s1, s2, s3, fr;
    int32_t exp;
    uint8_t highest, offset, lead_len = 0, skipped = 0;
    uint8_t precision = digits ? digits + 1 : NRF_CLI_FORMAT_DOUBLE_DEF_PRECISION + 1;
    /* Default digits should be -1, because 0 could be a requirement, not the default.
     * This should be changed for the whole library.
     */

    if ((v > 0.0) && (v < 1.0))
    {
        v += 1.0;
        transform = true;
    }
    else if ((v > -1.0) && (v < 0.0))
    {
        v -= 1.0;
        transform = true;
    }

    memcpy(&num, &v, sizeof(num));
    sign = NRF_CLI_FORMAT_DOUBLE_SIGN_GET(num);
    exp = NRF_CLI_FORMAT_DOUBLE_EXP_GET(num);
    mant = NRF_CLI_FORMAT_DOUBLE_MANT_GET(num);

    /* Special cases */
    if (exp == NRF_CLI_FORMAT_DOUBLE_EXP_MASK)
    {
        if (width && (!(format & NRF_CLI_FORMAT_FLAG_LEFT_JUSTIFY)))
        {
            fill_space(p_ctx, width - 3 - NRF_CLI_FORMAT_REQ_SIGN_SPACE(sign, format), false);
        }

        if (sign)
        {
            buffer_add(p_ctx, '-');
        }
        else if (format & NRF_CLI_FORMAT_FLAG_PRINT_SIGN)
        {
            buffer_add(p_ctx, '+');
        }

        if (mant != 0)
        {
            if(uppercase)
            {
                buffer_add(p_ctx, 'N');
                buffer_add(p_ctx, 'A');
                buffer_add(p_ctx, 'N');
            }
            else
            {
                buffer_add(p_ctx, 'n');
                buffer_add(p_ctx, 'a');
                buffer_add(p_ctx, 'n');
            }
        }
        else
        {
            if(uppercase)
            {
                buffer_add(p_ctx, 'I');
                buffer_add(p_ctx, 'N');
                buffer_add(p_ctx, 'F');
            }
            else
            {
                buffer_add(p_ctx, 'i');
                buffer_add(p_ctx, 'n');
                buffer_add(p_ctx, 'f');
            }
        }
        return;
    }

    /* Add leading 1 to mantissa (except 0.0) */
    if ((mant != 0) || (exp != 0))
    {
        mant |= (1ULL << 52);
    }

    /* Convert the exponent */
    exp = exp - 1023;

    /* Whole numbers */
    offset  = 52 - exp;

    if (offset > 64)
    {
        /* Float fraction offset overflow */
        return;
    }

    lead = (mant >> (offset));

    /* Fraction */
    low = mant & (~(lead << offset));

    while (((low & 0x1) == 0) && low > 0)
    {
        low = low >> 1U;
        skipped++;
    }

    highest = (offset - skipped);
    base = 1;

    for(uint8_t i = 0; i < precision; i++)
    {
        base *= 10;
    }

    /* Handle multiplication with possible overflow */
    x = LOW_32(low) * LOW_32(base);
    s0 = LOW_32(x);

    x = HIGH_32(low) * LOW_32(base) + HIGH_32(x);
    s1 = LOW_32(x);
    s2 = HIGH_32(x);

    x = s1 + LOW_32(low) * HIGH_32(base);
    s1 = LOW_32(x);

    x = s2 + HIGH_32(low) * HIGH_32(base) + HIGH_32(x);
    s2 = LOW_32(x);
    s3 = HIGH_32(x);

    res = s1 << 32 | s0;
    carry = s3 << 32 | s2;

    /* Divide and combine */
    carry = carry << (64 - highest);
    res = res >> highest;
    fr = res | carry;

    /* Roundup */
    if (fr%10 >= 5)
    {
        fr /= 10;
        fr++;
    }
    else
    {
        fr /= 10;
    }
    precision--;

    if (transform && (lead == 1))
    {
        lead = 0;
    }

    /* Maximum precision handled by int_print() is 10 */
    if (precision > 10)
    {
        for (uint8_t delta = precision - 10; delta > 0; delta--)
        {
            fr /= 10;
        }
        precision = 10;
    }

    res = lead;
    while (res > 0)
    {
        res /= 10;
        lead_len++;
    }

    if ((lead == 0) && (fr == 0))
    {
        lead_len = 1;
    }

    if(lead_len == 0)
    {
        lead_len = 1;
    }

    if (width && (!(format & NRF_CLI_FORMAT_FLAG_LEFT_JUSTIFY)))
    {
        int32_t space = width - lead_len - precision - NRF_CLI_FORMAT_REQ_SIGN_SPACE(sign, format) - 1;
        if (space > 0)
        {
            fill_space(p_ctx, space, format & NRF_CLI_FORMAT_FLAG_PAD_ZERO);
        }
    }

    if (sign)
    {
        buffer_add(p_ctx, '-');
    }
    else if (format & NRF_CLI_FORMAT_FLAG_PRINT_SIGN)
    {
        buffer_add(p_ctx, '+');
    }

    int_print(p_ctx,
              lead,
              10u,
              0,
              0,
              0);
    buffer_add(p_ctx, '.');
    int_print(p_ctx,
              fr,
              10u,
              precision,
              0,
              0);

    if (width && (format & NRF_CLI_FORMAT_FLAG_LEFT_JUSTIFY))
    {
        int32_t space = width - lead_len - precision - NRF_CLI_FORMAT_REQ_SIGN_SPACE(sign, format) - 1;
        if (space > 0)
        {
            fill_space(p_ctx, space, false);
        }
    }
}

#endif

void nrf_fprintf_fmt(nrf_fprintf_ctx_t * const p_ctx,
                    char const *               p_fmt,
                    va_list *                  p_args)
{
    ASSERT(p_ctx != NULL);

    ASSERT(p_ctx->fwrite != NULL);
    ASSERT(p_ctx->p_io_buffer != NULL);
    ASSERT(p_ctx->io_buffer_size > 0);

    if (p_fmt == NULL)
    {
        return;
    }

    char c;
    int32_t v;
    uint32_t NumDigits;
    uint32_t FormatFlags;
    uint32_t FieldWidth;

    do
    {
        c = *p_fmt;
        p_fmt++;

        if (c == 0u)
        {
            break;
        }
        if (c == '%')
        {
            //
            // Filter out flags
            //
            FormatFlags = 0u;
            v = 1;

            do
            {
                c = *p_fmt;
                switch (c)
                {
                    case '-':
                        FormatFlags |= NRF_CLI_FORMAT_FLAG_LEFT_JUSTIFY;
                        p_fmt++;
                        break;
                    case '0':
                        FormatFlags |= NRF_CLI_FORMAT_FLAG_PAD_ZERO;
                        p_fmt++;
                        break;
                    case '+':
                        FormatFlags |= NRF_CLI_FORMAT_FLAG_PRINT_SIGN;
                        p_fmt++;
                        break;
                    default:
                        v = 0;
                        break;
                }
            } while (v);

            //
            // filter out field width
            //
            FieldWidth = 0u;
            do
            {
                if (c == '*')
                {
                    /*lint -save -e64 -e56*/
                    FieldWidth += va_arg(*p_args, unsigned);
                    /*lint -restore*/
                    p_fmt++;
                    break;
                }
                c = *p_fmt;
                if ((c < '0') || (c > '9'))
                {
                    break;
                }
                p_fmt++;
                FieldWidth = (FieldWidth * 10u) + (c - '0');
            } while (1);

            //
            // Filter out precision (number of digits to display)
            //
            NumDigits = 0u;
            c = *p_fmt;
            if (c == '.')
            {
                p_fmt++;
                do
                {
                    c = *p_fmt;
                    if ((c < '0') || (c > '9'))
                    {
                        break;
                    }
                    p_fmt++;
                    NumDigits = NumDigits * 10u + (c - '0');
                } while (1);
            }
            //
            // Filter out length modifier
            //
            c = *p_fmt;
            do
            {
                if ((c == 'l') || (c == 'h'))
                {
                    p_fmt++;
                    c = *p_fmt;
                }
                else
                {
                    break;
                }
            } while (1);
            //
            // Handle specifiers
            //
            /*lint -save -e64*/
            switch (c)
            {
                case 'c':
                {
                    char c0;
                    v = va_arg(*p_args, int32_t);
                    c0 = (char)v;
                    buffer_add(p_ctx, c0);
                    break;
                }
                case 'd':
                case 'i':
                    v = va_arg(*p_args, int32_t);
                    int_print(p_ctx,
                              v,
                              10u,
                              NumDigits,
                              FieldWidth,
                              FormatFlags);
                    break;
                case 'u':
                    v = va_arg(*p_args, int32_t);
                    unsigned_print(p_ctx,
                                   (uint32_t)v,
                                   10u,
                                   NumDigits,
                                   FieldWidth,
                                   FormatFlags);
                    break;
                case 'x':
                case 'X':
                    v = va_arg(*p_args, int32_t);
                    unsigned_print(p_ctx,
                                   (uint32_t)v,
                                   16u,
                                   NumDigits,
                                   FieldWidth,
                                   FormatFlags);
                    break;
                case 's':
                {
                    char const * p_s = va_arg(*p_args, const char *);
                    string_print(p_ctx, p_s, FieldWidth, FormatFlags);
                    break;
                }
                case 'p':
                    v = va_arg(*p_args, int32_t);
                    buffer_add(p_ctx, '0');
                    buffer_add(p_ctx, 'x');
                    unsigned_print(p_ctx, (uint32_t)v, 16u, 8u, 8u, 0);
                    break;
                case '%':
                    buffer_add(p_ctx, '%');
                    break;
#if NRF_MODULE_ENABLED(NRF_FPRINTF_DOUBLE)
                case 'f':
                {
                    double dbl = va_arg(*p_args, double);
                    float_print(p_ctx,
                                dbl,
                                NumDigits,
                                FieldWidth,
                                FormatFlags,
                                false);
                    break;
                }
                case 'F':
                {
                    double dbl = va_arg(*p_args, double);
                    float_print(p_ctx,
                                dbl,
                                NumDigits,
                                FieldWidth,
                                FormatFlags,
                                true);
                    break;
                }
#endif
                default:
                    break;
            }
            /*lint -restore*/
            p_fmt++;
        }
        else
        {
            buffer_add(p_ctx, c);
        }
    } while (*p_fmt != '\0');

    if (p_ctx->auto_flush)
    {
        nrf_fprintf_buffer_flush(p_ctx);
    }
}

#endif // NRF_MODULE_ENABLED(NRF_FPRINTF)