bintools.c

Open DMT Client C Source code

    return binFmtVPrintf(fb, fmt, ap);
}

int binFmtVPrintf(FmtBuffer_t *dest, const char *fmt, va_list ap)
{
    UInt16 startingDataLen = BUFFER_DATA_LENGTH(dest);
    const char *v = fmt? fmt : ""; // 'v' must not be null
    
    /* valid data pointer? */
    if (!dest || !BUFFER_DATA(dest)) {
        logERROR(LOGSRC,"Buffer is null\n");
        return -1;
    }
    
    /* loop through formats */
    int f = 0;
    for (f = 0; ;f++) {
        
        /* find start of next format */
        while (*v && (*v != '%')) { v++; }
        if (!*v) {
            break; 
        }
        v++;
        
        /* 1+ digits must follow */
        int len = 0;
        if (*v == '*') {
            // pull the field length off the stack
            len = va_arg(ap, int);
            v++;
        } else
        if (isdigit(*v)) {
            // the field length is explicitly specified in the format
            const char *d = v;
            while (*d && isdigit(*d)) { d++; }
            len = atoi(v);
            v = d;
        } else {
            // invalid format (not a digit)
            logERROR(LOGSRC,"Invalid format at #%d (not a digit)\n", f);
            return -1;
        }

        /* check length */
        if (len > BUFFER_DATA_SIZE(dest)) {
            // overflow
            logERROR(LOGSRC,"Overflow at %d [%d > %d]\n", f, len, BUFFER_DATA_SIZE(dest));
            return -1;
        }

        /* make sure the field is cleared */
        memset(BUFFER_DATA(dest), 0, len);

        /* next char must be one of 'iuxsbgz' */
        UInt32 i;
        const char *s;
        utBool padString = utFalse;
        const UInt8 *b;
        const GPSPoint_t *gp;
        switch (*v) {
            case 'x': case 'X': // unsigned
            case 'u': case 'U': // unsigned
            case 'i': case 'I': // signed
                i = va_arg(ap, UInt32);
                if (len < sizeof(UInt32)) {
                    i &= (1L << (len*8)) - 1L;
                }
                binEncodeInt32(BUFFER_DATA(dest), len, i, (((*v == 'i')||(*v == 'I'))? utTrue : utFalse));
                binAppendFmtField(dest, len, *v);
                binAdvanceFmtBuffer(dest, len);
                break;
            case 'p': case 'P': // padded strings
            case 's': case 'S': // strings
                s = va_arg(ap, const char*);
                padString = ((*v == 'p') || (*v == 'P'))? utTrue : utFalse;
                if (s) {
                    char *d = (char*)BUFFER_DATA(dest);
                    int actualLen = strLength(s, len); // MIN(strlen(s),len)
                    memcpy(d, s, actualLen);
                    if (actualLen < len) {
                        if (padString) {
                            // pad short strings with spaces
                            for (;actualLen < len; actualLen++) {
                                d[actualLen] = ' '; // pad with space
                            }
                        } else {
                            // include a string terminator
                            d[actualLen++] = 0;
                            len = actualLen;
                        }
                    }
                } else {
                    if (padString) {
                        // fill the field with spaces
                        char *d = (char*)BUFFER_DATA(dest);
                        int actualLen = 0;
                        for (;actualLen < len; actualLen++) {
                            d[actualLen] = ' '; // pad with space
                        }
                    } else {
                        // no string
                        len = 0;
                    }
                }
                binAppendFmtField(dest, len, *v);
                binAdvanceFmtBuffer(dest, len);
                break;
            case 'b': case 'B': // binary
                b = va_arg(ap, const UInt8*);
                if (b) {
                    memcpy(BUFFER_DATA(dest), b, len); // copy
                } else {
                    memset(BUFFER_DATA(dest), 0, len); // clear
                }
                binAppendFmtField(dest, len, *v);
                binAdvanceFmtBuffer(dest, len);
                break;
            case 'g': case 'G':
                gp = va_arg(ap, const GPSPoint_t*);
                if (gp) {
                    if ((len >= 6) && (len < 8)) {
                        gpsPointEncode6(BUFFER_DATA(dest), gp);
                    } else
                    if (len >= 8) {
                        gpsPointEncode8(BUFFER_DATA(dest), gp);
                    } else {
                        // invalid format
                    }
                }
                binAppendFmtField(dest, len, *v);
                binAdvanceFmtBuffer(dest, len);
                break;
            case 'z': case 'Z': // zero filler
                binAppendFmtField(dest, len, *v);
                binAdvanceFmtBuffer(dest, len);
                break;
            default:
                // invalid format (unrecognized type)
                logERROR(LOGSRC,"Invalid format (unrecognized type)\n");
                return -1;
        }
        
    }
    
    /* return number of bytes written to buffer in this function */
    return BUFFER_DATA_LENGTH(dest) - startingDataLen;
    
}

// ----------------------------------------------------------------------------

int binScanf(const UInt8 *buf, UInt16 bufLen, const char *fmt, ...)
{
    va_list ap;
    va_start(ap, fmt);
    int rtn = binVScanf(buf, bufLen, fmt, ap);
    va_end(ap);
    return rtn;
}

int binVScanf(const UInt8 *buf, UInt16 bufLen, const char *fmt, va_list ap)
{
    Buffer_t bb, *src = binBuffer(&bb, (UInt8*)buf, bufLen, BUFFER_SOURCE);
    return binBufVScanf(src, fmt, ap);
}

int binBufScanf(Buffer_t *src, const char *fmt, ...)
{
    va_list ap;
    va_start(ap, fmt);
    int rtn = binBufVScanf(src, fmt, ap);
    va_end(ap);
    return rtn;
}

int binBufVScanf(Buffer_t *src, const char *fmt, va_list ap)
{
    const char *v = fmt;
    int fldCnt;
    for (fldCnt = 0; (BUFFER_DATA_LENGTH(src) > 0); fldCnt++) {
        
        /* find start of next format */
        while (*v && (*v != '%')) { v++; }
        if (!*v) {
            // no more fields
            break; 
        }
        v++;
        
        /* 1+ digits must follow */
        int len = 0;
        if (*v == '*') {
            // pull the field length off the stack
            len = va_arg(ap, int);
            v++;
        } else
        if (isdigit(*v)) {
            // the field length is explicitly specified in the format
            const char *d = v;
            while (*d && isdigit(*d)) { d++; }
            len = atoi(v);
            v = d;
        } else {
            // invalid format (not a digit)
            logERROR(LOGSRC,"Invalid format at #%d (not a digit)\n", fldCnt);
            return -1;
        }

        /* check length */
        if (len > BUFFER_DATA_LENGTH(src)) {
            // overflow
            //return -1;
            len = BUFFER_DATA_LENGTH(src); // use whatever we have left
        }
        
        /* next char must be one of 'iuxsbgz' */
        UInt32 *i;
        char *s;
        UInt8 *b;
        GPSPoint_t *gp;
        int actualLen = 0;
        switch (*v) {
            case 'x': case 'X': // unsigned
            case 'u': case 'U': // unsigned
            case 'i': case 'I': // signed
                i = va_arg(ap, UInt32*);
                if (i) {
                    *i = binDecodeInt32(BUFFER_DATA(src), len, (((*v == 'i')||(*v == 'I'))? utTrue : utFalse));
                }
                binAdvanceBuffer(src, len);
                break;
            case 's': case 'S': // strings
            case 'p': case 'P': // padded strings
                s = va_arg(ap, char*);
                actualLen = strLength((char*)BUFFER_DATA(src), len); // may be less than 'len'
                if (s) {
                    sprintf(s, "%.*s", actualLen, BUFFER_DATA(src)); // copy and terminate
                    // ??? trim trailing space?
                }
                binAdvanceBuffer(src, ((actualLen<len)?actualLen+1:len)); // incl terminator
                break;
            case 'b': case 'B':
                b = va_arg(ap, UInt8*);
                if (b) { 
                    memcpy(b, BUFFER_DATA(src), len); 
                }
                binAdvanceBuffer(src, len);
                break;
            case 'g': case 'G':
                gp = va_arg(ap, GPSPoint_t*);
                if (gp) {
                    if ((len >= 6) && (len < 8)) {
                        gpsPointDecode6(gp, BUFFER_DATA(src));
                    } else
                    if (len >= 8) {
                        gpsPointDecode8(gp, BUFFER_DATA(src));
                    } else {
                        // invalid format
                    }
                }
                binAdvanceBuffer(src, len);
                break;
            case 'z': case 'Z': // zero filled
                binAdvanceBuffer(src, len);
                break;
            default:
                // invalid format (unrecognized type)
                logERROR(LOGSRC,"Invalid format at #%d (unrecognized type)\n", fldCnt);
                return -1;
        }

    }
    return fldCnt;
}

// ----------------------------------------------------------------------------