/* * This file is part of the COMROGUE Operating System for Raspberry Pi * * Copyright (c) 2013, Eric J. Bowersox / Erbosoft Enterprises * All rights reserved. * * This program is free for commercial and non-commercial use as long as the following conditions are * adhered to. * * Copyright in this file remains Eric J. Bowersox and/or Erbosoft, and as such any copyright notices * in the code are not to be removed. * * Redistribution and use in source and binary forms, with or without modification, are permitted * provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright notice, this list of conditions and * the following disclaimer. * * 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. * * 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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. * * "Raspberry Pi" is a trademark of the Raspberry Pi Foundation. */ #include #include #include #include #include /*------------------------- * Basic string operations *------------------------- */ /* * Returns whether or not the character is a digit (0-9). * * Parameters: * - ch = Character to be tested. * * Returns: * TRUE if the character is a digit, FALSE otherwise. */ SEG_LIB_CODE BOOL StrIsDigit8(CHAR ch) { /* TODO: replace with something nicer later */ return MAKEBOOL((ch >= '0') && (ch <= '9')); } /* * Returns the length of a null-terminated string. * * Parameters: * - psz = The string to get the length of. * * Returns: * The length of the string in characters. */ SEG_LIB_CODE INT32 StrLength8(PCSTR psz) { register PCSTR p = psz; while (*p) p++; return (INT32)(p - psz); } /* * Locates a character within a null-terminated string. * * Parameters: * - psz = String to search through for character. * - ch = Character to look for. * * Returns: * NULL if character was not found, otherwise pointer to character within string. */ SEG_LIB_CODE PCHAR StrChar8(PCSTR psz, INT32 ch) { const CHAR mych = ch; for (; *psz; psz++) if (*psz == mych) return (PCHAR)psz; return NULL; } /*------------------------------- * Numeric-to-string conversions *------------------------------- */ /* * Formats an unsigned numeric value into a buffer. This function formats the value into the END of the * buffer and returns a pointer to the first character of the number. Note that the string-converted number * is NOT null-terminated! * * Parameters: * - value = The value to be converted. * - pchBuf = Pointer to the buffer in which to store the converted value. * - nBytes = Number of characters in the buffer. * - nBase = The numeric base to use to output the number. Valid values are in the interval [2,36]. * - fUppercase = TRUE to use upper-case letters as digits in output, FALSE to use lower-case letters. * * Returns: * A pointer to the first digit of the converted value, which will be somewhere in the interval * [pchBuf, pchBuf + nBytes). */ SEG_LIB_CODE static PCHAR convert_number_engine8(UINT64 value, PCHAR pchBuf, INT32 nBytes, INT32 nBase, BOOL fUppercase) { static DECLARE_LIB_STRING8_CONST(szDigitsUppercase, "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ"); static DECLARE_LIB_STRING8_CONST(szDigitsLowercase, "0123456789abcdefghijklmnopqrstuvwxyz"); PCSTR pszDigits = (fUppercase ? szDigitsUppercase : szDigitsLowercase); /* digit set to use */ PCHAR p = pchBuf + nBytes; /* output pointer */ INT64 accumulator; /* accumulator of quotients */ LDIV ldiv; /* buffer for division */ /* do initial division to get rid of high-order bit & handle 0 */ *--p = pszDigits[value % nBase]; accumulator = (INT64)(value / nBase); while ((p > pchBuf) && (accumulator > 0)) { /* output digits */ IntLDiv(&ldiv, accumulator, nBase); *--p = pszDigits[ldiv.rem]; accumulator = ldiv.quot; } return p; } /*----------------------------------------- * StrFormatV8 and its support definitions *----------------------------------------- */ /* Flag values for conversion characters */ #define FLAG_LEFTJUST 0x01 /* left-justify output field */ #define FLAG_PLUSSIGN 0x02 /* use a '+' for the sign if >= 0 */ #define FLAG_SPACESIGN 0x04 /* use a space for the sign if >= 0 */ #define FLAG_ALTMODE 0x08 /* alternate conversion mode */ #define FLAG_ZEROPAD 0x10 /* zero-pad to width */ /* Macro that calls the output function and either updates total output or exits */ #define do_output(pBuf, nChr) \ do { if ((nChr) > 0) { if (SUCCEEDED((*pfnFormat)(&pArg, (pBuf), (nChr)))) nTotalOutput += (nChr); \ else { errorFlag = SEVERITY_ERROR; goto error; } } } while (0) /* Strings used for padding */ static DECLARE_LIB_STRING8_CONST(szPadSpace, " "); static DECLARE_LIB_STRING8_CONST(szPadZero, "0000000000000000000000000000000000000000000000000000000000"); #define PAD_SIZE (sizeof(szPadSpace) - 1) /* number of characters available in pad string */ /* Macro that outputs characters for padding */ #define do_pad_output(szPad, nChr) \ do { int n, nt = (nChr); while (nt > 0) { n = intMin(nt, PAD_SIZE); do_output(szPad, n); nt -= n; } } while (0) #define MAX_VAL 509 /* maximum field value for conversion */ #define BUFSIZE 64 /* buffer size on stack for conversion */ #define NUM_CONVERT_BYTES 32 /* maximum number of bytes for converting a numeric value */ /* * Format a series of arguments like "printf," calling a function to perform the actual character output. * * Parameters: * - pfnFormat = Pointer to the format function * - pArg = Pointer argument passed to the format function, which may modify it * - pszFormat = Format string, like a printf format string. * - pargs = Pointer to varargs-style function arguments. * * Returns: * A HRESULT as follows: Severity indicates whether the output function failed or succeeded, facility is always * FACILITY_STRFORMAT, code indicates the number of characters output. * * Notes: * Floating-point conversion formats are not supported at this time. */ SEG_LIB_CODE HRESULT StrFormatV8(PFNFORMAT8 pfnFormat, PVOID pArg, PCSTR pszFormat, va_list pargs) { static DECLARE_LIB_STRING8_CONST(szFlagChars, "0+- #"); /* flag characters and values */ static const UINT32 SEG_LIB_RODATA auiFlagValues[] = { FLAG_ZEROPAD, FLAG_LEFTJUST, FLAG_PLUSSIGN, FLAG_SPACESIGN, FLAG_ALTMODE, 0 }; static DECLARE_LIB_STRING8_CONST(szConversionChars, "hl"); /* conversion characters */ PCSTR p; /* pointer used to walk the format string */ PCHAR pFlag; /* pointer to flag character/temp buffer pointer */ PCHAR pchOutput; /* pointer to characters actually output */ INT32 nchOutput; /* number of characters output */ PCHAR pchPrefix; /* pointer to prefix characters */ INT32 nchPrefix; /* number of prefix characters */ INT32 nchZeroPrefix; /* number of zeroes to write as a prefix */ INT32 nTotalOutput = 0; /* total number of characters output */ UINT32 errorFlag = SEVERITY_SUCCESS; /* error flag to return */ UINT32 flags; /* conversion flags seen */ INT32 ncWidth; /* width for field */ INT32 ncPrecision; /* precision for field */ INT32 nBase; /* numeric base for field */ INT64 input; /* input integer value */ CHAR chConvert; /* conversion character for field */ CHAR achBuffer[BUFSIZE]; /* buffer into which output is formatted */ while (*pszFormat) { /* locate all plain characters up to next % or end of string and output them */ p = pszFormat; while (*p && (*p != '%')) p++; do_output(pszFormat, p - pszFormat); if (*p == '%') { flags = 0; /* convert flags first */ for (p++; (pFlag = StrChar8(szFlagChars, *p)) != NULL; p++) flags |= auiFlagValues[pFlag - szFlagChars]; ncWidth = 0; /* now convert width */ if (*p == '*') { ncWidth = va_arg(pargs, INT32); /* specified by arg */ p++; if (ncWidth < 0) { flags |= FLAG_LEFTJUST; /* negative width is taken as use of - flag */ ncWidth = -ncWidth; } } else { while (StrIsDigit8(*p)) ncWidth = ncWidth * 10 + (*p++ - '0'); } if (ncWidth > MAX_VAL) ncWidth = MAX_VAL; ncPrecision = -1; /* now convert precision */ if (*p == '.') { p++; if (*p == '*') { ncPrecision = va_arg(pargs, INT32); /* specified by arg */ p++; /* N.B. ncPrecision < 0 = "omitted precision" */ } else { ncPrecision = 0; while (StrIsDigit8(*p)) ncPrecision = ncPrecision * 10 + (*p++ - '0'); } if (ncPrecision > MAX_VAL) ncPrecision = MAX_VAL; } chConvert = StrChar8(szConversionChars, *p) ? *p++ : '\0'; /* get conversion character */ /* based on field character, convert the field */ pchOutput = achBuffer; nchOutput = 0; pchPrefix = NULL; nchPrefix = 0; nchZeroPrefix = 0; switch (*p) { case 'c': /* output a character */ achBuffer[nchOutput++] = (CHAR)va_arg(pargs, INT32); break; case 'd': case 'i': /* output a decimal number */ if (ncPrecision < 0) ncPrecision = 1; /* default precision is 1 */ if (chConvert == 'h') input = va_arg(pargs, INT32); else if (chConvert == '\0') input = va_arg(pargs, INT32); else input = va_arg(pargs, INT64); if ((input == 0) && (ncPrecision == 0)) break; /* no output */ if (input < 0) { achBuffer[nchPrefix++] = '-'; /* minus sign */ input = -input; } else { /* plus or space for sign if flags dictate */ if (flags & FLAG_PLUSSIGN) achBuffer[nchPrefix++] = '+'; else if (flags & FLAG_SPACESIGN) achBuffer[nchPrefix++] = ' '; } if (nchPrefix > 0) { pchPrefix = achBuffer; /* adjust output pointer and set prefix */ pchOutput = achBuffer + nchPrefix; } /* clamp input value and run it through number conversion engine */ if (chConvert == 'h') input &= UINT16_MAX; else if (chConvert == '\0') input &= UINT32_MAX; pFlag = pchOutput; pchOutput = convert_number_engine8((UINT64)input, pFlag, NUM_CONVERT_BYTES, 10, FALSE); nchOutput = (pFlag + NUM_CONVERT_BYTES) - pchOutput; /* calculate zero prefix based on precision */ if ((flags & (FLAG_ZEROPAD|FLAG_LEFTJUST)) == FLAG_ZEROPAD) nchZeroPrefix = intMax(0, ncWidth - (nchPrefix + nchOutput)); else if (ncPrecision > 0) nchZeroPrefix = intMax(0, ncPrecision - nchOutput); break; case 'o': case 'u': case 'x': case 'X': /* output an unsigned number */ if (ncPrecision < 0) ncPrecision = 1; /* defualt precision is 1 */ if (chConvert == 'h') input = va_arg(pargs, UINT32); else if (chConvert == '\0') input = va_arg(pargs, UINT32); else input = (INT64)va_arg(pargs, UINT64); if ((input == 0) && (ncPrecision == 0)) break; /* no output */ /* select numeric base of output */ if (*p == 'o') nBase = 8; else if (*p == 'u') nBase = 10; else nBase = 16; if (flags & FLAG_ALTMODE) { if (*p == 'o') { /* forced octal notation */ achBuffer[nchPrefix++] = '0'; ncPrecision--; } else if (*p != 'u') { /* forced hexadecimal notation */ achBuffer[nchPrefix++] = '0'; achBuffer[nchPrefix++] = *p; } } if (nchPrefix > 0) { pchPrefix = achBuffer; /* adjust output pointer and set prefix */ pchOutput = achBuffer + nchPrefix; } /* clamp input value and run it through number conversion engine */ if (chConvert == 'h') input &= UINT16_MAX; else if (chConvert == '\0') input &= UINT32_MAX; pFlag = pchOutput; pchOutput = convert_number_engine8((UINT64)input, pFlag, NUM_CONVERT_BYTES, nBase, MAKEBOOL(*p == 'X')); nchOutput = (pFlag + NUM_CONVERT_BYTES) - pchOutput; /* calculate zero prefix based on precision */ if ((flags & (FLAG_ZEROPAD|FLAG_LEFTJUST)) == FLAG_ZEROPAD) nchZeroPrefix = intMax(0, ncWidth - (nchPrefix + nchOutput)); else if (ncPrecision > 0) nchZeroPrefix = intMax(0, ncPrecision - nchOutput); break; case 'p': /* output a pointer value - treated as unsigned hex with precision of 8 */ input = (INT_PTR)va_arg(pargs, PVOID); input &= UINT_PTR_MAX; pFlag = pchOutput; pchOutput = convert_number_engine8((UINT64)input, pFlag, NUM_CONVERT_BYTES, 16, FALSE); nchOutput = (pFlag + NUM_CONVERT_BYTES) - pchOutput; nchZeroPrefix = intMax(0, (sizeof(UINT_PTR) / sizeof(CHAR) * 2) - nchOutput); break; case 's': /* output a string */ pchOutput = (PCHAR)va_arg(pargs, PCSTR); nchOutput = StrLength8(pchOutput); if ((ncPrecision >= 0) && (ncPrecision < nchOutput)) nchOutput = ncPrecision; break; case 'n': /* output nothing, just store number of characters output so far */ *(va_arg(pargs, PINT32)) = nTotalOutput; goto no_output; case '%': /* output a single % sign */ default: /* unrecognized format character, output it and continue */ achBuffer[nchOutput++] = *p; break; } ncWidth -= (nchPrefix + nchZeroPrefix + nchOutput); if (!(flags & FLAG_LEFTJUST)) do_pad_output(szPadSpace, ncWidth); do_output(pchPrefix, nchPrefix); do_pad_output(szPadZero, nchZeroPrefix); do_output(pchOutput, nchOutput); if (flags & FLAG_LEFTJUST) do_pad_output(szPadSpace, ncWidth); no_output: p++; } pszFormat = p; } error: return MAKE_SCODE(errorFlag, FACILITY_STRFORMAT, nTotalOutput); }