https://github.com/j123b567/scpi-parser

Dependents:   scpi_sx127x scpi_sx127x_firstTest MLX90418_I2C_master

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers utils.c Source File

utils.c

00001 /*-
00002  * Copyright (c) 2013 Jan Breuer
00003  *                    Richard.hmm
00004  * Copyright (c) 2012 Jan Breuer
00005  *
00006  * All Rights Reserved
00007  * 
00008  * Redistribution and use in source and binary forms, with or without
00009  * modification, are permitted provided that the following conditions are
00010  * met:
00011  * 1. Redistributions of source code must retain the above copyright notice,
00012  *    this list of conditions and the following disclaimer.
00013  * 2. Redistributions in binary form must reproduce the above copyright
00014  *    notice, this list of conditions and the following disclaimer in the
00015  *    documentation and/or other materials provided with the distribution.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE AUTHORS ``AS IS'' AND ANY EXPRESS OR
00018  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020  * DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE
00021  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
00024  * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
00025  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00026  * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
00027  * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /**
00031  * @file   scpi_utils.c
00032  * @date   Thu Nov 15 10:58:45 UTC 2012
00033  * 
00034  * @brief  Conversion routines and string manipulation routines
00035  * 
00036  * 
00037  */
00038 
00039 #include <stdio.h>
00040 #include <stdlib.h>
00041 #include <string.h>
00042 #include <ctype.h>
00043 
00044 #include "utils_private.h"
00045 #include "scpi/utils.h"
00046 
00047 static size_t patternSeparatorShortPos(const char * pattern, size_t len);
00048 static size_t patternSeparatorPos(const char * pattern, size_t len);
00049 static size_t cmdSeparatorPos(const char * cmd, size_t len);
00050 
00051 /**
00052  * Find the first occurrence in str of a character in set.
00053  * @param str
00054  * @param size
00055  * @param set
00056  * @return 
00057  */
00058 char * strnpbrk(const char *str, size_t size, const char *set) {
00059     const char *scanp;
00060     long c, sc;
00061     const char * strend = str + size;
00062 
00063     while ((strend != str) && ((c = *str++) != 0)) {
00064         for (scanp = set; (sc = *scanp++) != '\0';)
00065             if (sc == c)
00066                 return ((char *) (str - 1));
00067     }
00068     return (NULL);
00069 }
00070 
00071 /**
00072  * Converts signed 32b integer value to string
00073  * @param val   integer value
00074  * @param str   converted textual representation
00075  * @param len   string buffer length
00076  * @param base  output base
00077  * @return number of bytes written to str (without '\0')
00078  */
00079 size_t SCPI_LongToStr(int32_t val, char * str, size_t len, int8_t base) {
00080     const char digits[] = "0123456789ABCDEF";
00081 
00082 #define ADD_CHAR(c) if (pos < len) str[pos++] = (c)
00083     uint32_t x = 0;
00084     int_fast8_t digit;
00085     size_t pos = 0;
00086     uint32_t uval = val;
00087 
00088     if (uval == 0) {
00089         ADD_CHAR('0');
00090     } else {
00091 
00092         switch (base) {
00093             case 2: 
00094                 x = 0x80000000L;
00095                 break;
00096             case 8:
00097                 x = 0x40000000L;
00098                 break;
00099             case 10:
00100                 x = 1000000000L;
00101                 break;
00102             case 0x10:
00103                 x = 0x10000000L;
00104                 break;
00105             default:
00106                 x = 1000000000L;
00107                 base = 10;
00108                 break;
00109         }
00110 
00111         // add sign for numbers in base 10
00112         if ((val < 0) && (base == 10)) {
00113             uval = -val;
00114             ADD_CHAR('-');
00115         }
00116 
00117         // remove leading zeros
00118         while ((uval / x) == 0) {
00119             x /= base;
00120         }
00121 
00122         do {
00123             digit = (uint8_t) (uval / x);
00124             ADD_CHAR(digits[digit]);
00125             uval -= digit * x;
00126             x /= base;
00127         } while (x && (pos < len));
00128     }
00129 
00130     if (pos < len) str[pos] = 0;
00131     return pos;
00132 #undef ADD_CHAR
00133 }
00134 
00135 /**
00136  * Converts double value to string
00137  * @param val   double value
00138  * @param str   converted textual representation
00139  * @param len   string buffer length
00140  * @return number of bytes written to str (without '\0')
00141  */
00142 size_t SCPI_DoubleToStr(double val, char * str, size_t len) {
00143     return SCPIDEFINE_doubleToStr(val, str, len);
00144 }
00145 
00146 /**
00147  * Converts string to signed 32bit integer representation
00148  * @param str   string value
00149  * @param val   32bit integer result
00150  * @return      number of bytes used in string
00151  */
00152 size_t strToLong(const char * str, int32_t * val, int8_t base) {
00153     char * endptr;
00154     *val = strtol(str, &endptr, base);
00155     return endptr - str;
00156 }
00157 
00158 /**
00159  * Converts string to double representation
00160  * @param str   string value
00161  * @param val   double result
00162  * @return      number of bytes used in string
00163  */
00164 size_t strToDouble(const char * str, double * val) {
00165     char * endptr;
00166     *val = strtod(str, &endptr);
00167     return endptr - str;
00168 }
00169 
00170 /**
00171  * Compare two strings with exact length
00172  * @param str1
00173  * @param len1
00174  * @param str2
00175  * @param len2
00176  * @return TRUE if len1==len2 and "len" characters of both strings are equal
00177  */
00178 scpi_bool_t compareStr(const char * str1, size_t len1, const char * str2, size_t len2) {
00179     if (len1 != len2) {
00180         return FALSE;
00181     }
00182 
00183     if (SCPIDEFINE_strncasecmp(str1, str2, len2) == 0) {
00184         return TRUE;
00185     }
00186 
00187     return FALSE;
00188 }
00189 
00190 /**
00191  * Compare two strings, one be longer but may contains only numbers in that section
00192  * @param str1
00193  * @param len1
00194  * @param str2
00195  * @param len2
00196  * @return TRUE if strings match
00197  */
00198 scpi_bool_t compareStrAndNum(const char * str1, size_t len1, const char * str2, size_t len2, int32_t * num) {
00199     scpi_bool_t result = FALSE;
00200     size_t i;
00201 
00202     if (len2 < len1) {
00203         return FALSE;
00204     }
00205 
00206     if (SCPIDEFINE_strncasecmp(str1, str2, len1) == 0) {
00207         result = TRUE;
00208 
00209         if (num) {
00210             if (len1 == len2) {
00211                 *num = 1;
00212             } else {
00213                 int32_t tmpNum;
00214                 i = len1 + strToLong(str2 + len1, &tmpNum, 10);
00215                 if (i != len2) {
00216                     result = FALSE;
00217                 } else {
00218                     *num = tmpNum;
00219                 }
00220             }
00221         } else {
00222             for (i = len1; i<len2; i++) {
00223                 if (!isdigit((int) str2[i])) {
00224                     result = FALSE;
00225                     break;
00226                 }
00227             }
00228         }
00229     }
00230 
00231     return result;
00232 }
00233 
00234 /**
00235  * Count white spaces from the beggining
00236  * @param cmd - command
00237  * @param len - max search length
00238  * @return number of white spaces
00239  */
00240 size_t skipWhitespace(const char * cmd, size_t len) {
00241     size_t i;
00242     for (i = 0; i < len; i++) {
00243         if (!isspace((unsigned char) cmd[i])) {
00244             return i;
00245         }
00246     }
00247     return len;
00248 }
00249 
00250 /**
00251  * Pattern is composed from upper case an lower case letters. This function
00252  * search the first lowercase letter
00253  * @param pattern
00254  * @param len - max search length
00255  * @return position of separator or len
00256  */
00257 static size_t patternSeparatorShortPos(const char * pattern, size_t len) {
00258     size_t i;
00259     for (i = 0; (i < len) && pattern[i]; i++) {
00260         if (islower((unsigned char) pattern[i])) {
00261             return i;
00262         }
00263     }
00264     return i;
00265 }
00266 
00267 /**
00268  * Find pattern separator position
00269  * @param pattern
00270  * @param len - max search length
00271  * @return position of separator or len
00272  */
00273 static size_t patternSeparatorPos(const char * pattern, size_t len) {
00274 
00275     char * separator = strnpbrk(pattern, len, "?:[]");
00276     if (separator == NULL) {
00277         return len;
00278     } else {
00279         return separator - pattern;
00280     }
00281 }
00282 
00283 /**
00284  * Find command separator position
00285  * @param cmd - input command
00286  * @param len - max search length
00287  * @return position of separator or len
00288  */
00289 static size_t cmdSeparatorPos(const char * cmd, size_t len) {
00290     char * separator = strnpbrk(cmd, len, ":?");
00291     size_t result;
00292     if (separator == NULL) {
00293         result = len;
00294     } else {
00295         result = separator - cmd;
00296     }
00297 
00298     return result;
00299 }
00300 
00301 /**
00302  * Match pattern and str. Pattern is in format UPPERCASElowercase
00303  * @param pattern
00304  * @param pattern_len
00305  * @param str
00306  * @param str_len
00307  * @return 
00308  */
00309 scpi_bool_t matchPattern(const char * pattern, size_t pattern_len, const char * str, size_t str_len, int32_t * num) {
00310     int pattern_sep_pos_short;
00311 
00312     if (pattern[pattern_len - 1] == '#') {
00313         size_t new_pattern_len = pattern_len - 1;
00314 
00315         pattern_sep_pos_short = patternSeparatorShortPos(pattern, new_pattern_len);
00316 
00317         return compareStrAndNum(pattern, new_pattern_len, str, str_len, num) ||
00318                 compareStrAndNum(pattern, pattern_sep_pos_short, str, str_len, num);
00319     } else {
00320 
00321         pattern_sep_pos_short = patternSeparatorShortPos(pattern, pattern_len);
00322 
00323         return compareStr(pattern, pattern_len, str, str_len) ||
00324                 compareStr(pattern, pattern_sep_pos_short, str, str_len);
00325     }
00326 }
00327 
00328 /**
00329  * Compare pattern and command
00330  * @param pattern eg. [:MEASure]:VOLTage:DC?
00331  * @param cmd - command
00332  * @param len - max search length
00333  * @return TRUE if pattern matches, FALSE otherwise
00334  */
00335 scpi_bool_t matchCommand(const char * pattern, const char * cmd, size_t len, int32_t *numbers, size_t numbers_len) {
00336     scpi_bool_t result = FALSE;
00337     int leftFlag = 0; // flag for '[' on left
00338     int rightFlag = 0; // flag for ']' on right
00339     int cmd_sep_pos = 0;
00340 
00341     size_t numbers_idx = 0;
00342     int32_t *number_ptr = NULL;
00343 
00344     const char * pattern_ptr = pattern;
00345     int pattern_len = strlen(pattern);
00346     const char * pattern_end = pattern + pattern_len;
00347 
00348     const char * cmd_ptr = cmd;
00349     size_t cmd_len = SCPIDEFINE_strnlen(cmd, len);
00350     const char * cmd_end = cmd + cmd_len;
00351 
00352     /* now support optional keywords in pattern style, e.g. [:MEASure]:VOLTage:DC? */
00353     if (pattern_ptr[0] == '[') { // skip first '['
00354         pattern_len--;
00355         pattern_ptr++;
00356         leftFlag++;
00357     }
00358     if (pattern_ptr[0] == ':') { // skip first ':'
00359         pattern_len--;
00360         pattern_ptr++;
00361     }
00362 
00363     if (cmd_ptr[0] == ':') {
00364         /* handle errornouse ":*IDN?" */
00365         if ((cmd_len >= 2) && (cmd_ptr[1] != '*')) {
00366             cmd_len--;
00367             cmd_ptr++;
00368         }
00369     }
00370 
00371     while (1) {
00372         int pattern_sep_pos = patternSeparatorPos(pattern_ptr, pattern_end - pattern_ptr);
00373 
00374         if ((leftFlag > 0) && (rightFlag > 0)) {
00375             leftFlag--;
00376             rightFlag--;
00377         } else {
00378             cmd_sep_pos = cmdSeparatorPos(cmd_ptr, cmd_end - cmd_ptr);
00379         }
00380 
00381         if (pattern_ptr[pattern_sep_pos - 1] == '#') {
00382             if (numbers && (numbers_idx < numbers_len)) {
00383                 number_ptr = numbers + numbers_idx;
00384                 *number_ptr = 1; // default value
00385             } else {
00386                 number_ptr = NULL;
00387             }
00388             numbers_idx++;
00389         } else {
00390             number_ptr = NULL;
00391         }
00392 
00393         if (matchPattern(pattern_ptr, pattern_sep_pos, cmd_ptr, cmd_sep_pos, number_ptr)) {
00394             pattern_ptr = pattern_ptr + pattern_sep_pos;
00395             cmd_ptr = cmd_ptr + cmd_sep_pos;
00396             result = TRUE;
00397 
00398             /* command is complete */
00399             if ((pattern_ptr == pattern_end) && (cmd_ptr >= cmd_end)) {
00400                 break;
00401             }
00402 
00403             /* pattern complete, but command not */
00404             if ((pattern_ptr == pattern_end) && (cmd_ptr < cmd_end)) {
00405                 result = FALSE;
00406                 break;
00407             }
00408 
00409             /* command complete, but pattern not */
00410             if (cmd_ptr >= cmd_end) {
00411                 if (cmd_end == cmd_ptr) {
00412                     if (cmd_ptr[0] == pattern_ptr[pattern_end - pattern_ptr - 1]) {
00413                         break; /* exist optional keyword, command is complete */
00414                     }
00415                     if (']' == pattern_ptr[pattern_end - pattern_ptr - 1]) {
00416                         break; /* exist optional keyword, command is complete */
00417                     }
00418                 }
00419                 result = FALSE;
00420                 break;
00421             }
00422 
00423             /* both command and patter contains command separator at this position */
00424             if ((pattern_ptr[0] == cmd_ptr[0]) && ((pattern_ptr[0] == ':') || (pattern_ptr[0] == '?'))) {
00425                 pattern_ptr = pattern_ptr + 1;
00426                 cmd_ptr = cmd_ptr + 1;
00427             } else if ((pattern_ptr[1] == cmd_ptr[0])
00428                     && (pattern_ptr[0] == '[')
00429                     && (pattern_ptr[1] == ':')) {
00430                 pattern_ptr = pattern_ptr + 2; // for skip '[' in "[:"
00431                 cmd_ptr = cmd_ptr + 1;
00432                 leftFlag++;
00433             } else if ((pattern_ptr[1] == cmd_ptr[0])
00434                     && (pattern_ptr[0] == ']')
00435                     && (pattern_ptr[1] == ':')) {
00436                 pattern_ptr = pattern_ptr + 2; // for skip ']' in "]:"
00437                 cmd_ptr = cmd_ptr + 1;
00438             } else if ((pattern_ptr[2] == cmd_ptr[0])
00439                     && (pattern_ptr[0] == ']')
00440                     && (pattern_ptr[1] == '[')
00441                     && (pattern_ptr[2] == ':')) {
00442                 pattern_ptr = pattern_ptr + 3; // for skip '][' in "][:"
00443                 cmd_ptr = cmd_ptr + 1;
00444                 leftFlag++;
00445             } else if (((pattern_ptr[0] == ']')
00446                     || (pattern_ptr[0] == '['))
00447                     && (*(pattern_end - 1) == '?') // last is '?'
00448                     && (cmd_ptr[0] == '?')) {
00449                 result = TRUE; // exist optional keyword, and they are end with '?'
00450                 break; // command is complete  OK
00451             } else {
00452                 result = FALSE;
00453                 break;
00454             }
00455         } else {
00456             pattern_ptr = pattern_ptr + pattern_sep_pos;
00457             if ((pattern_ptr[0] == ']') && (pattern_ptr[1] == ':')) {
00458                 pattern_ptr = pattern_ptr + 2; // for skip ']' in "]:" , pattern_ptr continue, while cmd_ptr remain unchanged
00459                 rightFlag++;
00460             } else if ((pattern_ptr[0] == ']')
00461                     && (pattern_ptr[1] == '[')
00462                     && (pattern_ptr[2] == ':')) {
00463                 pattern_ptr = pattern_ptr + 3; // for skip ']' in "][:" , pattern_ptr continue, while cmd_ptr remain unchanged
00464                 rightFlag++;
00465             } else {
00466                 result = FALSE;
00467                 break;
00468             }
00469         }
00470     }
00471 
00472     return result;
00473 }
00474 
00475 /**
00476  * Compose command from previsou command anc current command
00477  *
00478  * @param prev pointer to previous command
00479  * @param current pointer of current command
00480  *
00481  * prev and current should be in the same memory buffer
00482  */
00483 scpi_bool_t composeCompoundCommand(const scpi_token_t * prev, scpi_token_t * current) {
00484     size_t i;
00485 
00486     /* Invalid input */
00487     if (current == NULL || current->ptr == NULL || current->len == 0)
00488         return FALSE;
00489 
00490     /* no previous command - nothing to do*/
00491     if (prev->ptr == NULL || prev->len == 0)
00492         return TRUE;
00493 
00494     /* Common command or command root - nothing to do */
00495     if (current->ptr[0] == '*' || current->ptr[0] == ':')
00496         return TRUE;
00497 
00498     /* Previsou command was common command - nothing to do */
00499     if (prev->ptr[0] == '*')
00500         return TRUE;
00501 
00502     /* Find last occurence of ':' */
00503     for (i = prev->len; i > 0; i--) {
00504         if (prev->ptr[i - 1] == ':') {
00505             break;
00506         }
00507     }
00508 
00509     /* Previous command was simple command - nothing to do*/
00510     if (i == 0)
00511         return TRUE;
00512 
00513     current->ptr -= i;
00514     current->len += i;
00515     memmove(current->ptr, prev->ptr, i);
00516     return TRUE;
00517 }
00518 
00519 
00520 
00521 #if !HAVE_STRNLEN
00522 /* use FreeBSD strnlen */
00523 
00524 /*-
00525  * Copyright (c) 2009 David Schultz <das@FreeBSD.org>
00526  * All rights reserved.
00527  */
00528 size_t
00529 BSD_strnlen(const char *s, size_t maxlen) {
00530     size_t len;
00531 
00532     for (len = 0; len < maxlen; len++, s++) {
00533         if (!*s)
00534             break;
00535     }
00536     return (len);
00537 }
00538 #endif
00539 
00540 #if !HAVE_STRNCASECMP && !HAVE_STRNICMP
00541 
00542 int OUR_strncasecmp(const char *s1, const char *s2, size_t n) {
00543     unsigned char c1, c2;
00544 
00545     for (; n != 0; n--) {
00546         c1 = tolower((unsigned char) *s1++);
00547         c2 = tolower((unsigned char) *s2++);
00548         if (c1 != c2) {
00549             return c1 - c2;
00550         }
00551         if (c1 == '\0') {
00552             return 0;
00553         }
00554     }
00555     return 0;
00556 }
00557 #endif