tmk_core/protocol/arm_atsam/usb/spfssf.c

   1 #include "samd51j18a.h"
   2 #include "stdarg.h"
   3 #include "spfssf.h"
   4 #include "usb_util.h"
   5
   6 int vspf(char *_Dest, const char *_Format, va_list va)
   7 {
   8     //va_list va;                                                   //Variable argument list variable
   9     char *d = _Dest;                                               //Pointer to dest
  10
  11     //va_start(va,_Format);                                         //Initialize the variable argument list
  12     while (*_Format)                                                //While not end of format string
  13     {
  14         if (*_Format == SPF_SPEC_START)                             //If current format string character is the specifier start character
  15         {
  16             _Format++;                                              //Skip over the character
  17             while (*_Format && *_Format <= 64) _Format++;           //Forward past any options
  18             if (*_Format == SPF_SPEC_START) *d++ = *_Format;        //If the character is the specifier start character, output the character and advance dest
  19             else if (*_Format == SPF_SPEC_LONG)                     //If the character is the long type
  20             {
  21                 _Format++;                                          //Skip over the character
  22                 if (*_Format == SPF_SPEC_DECIMAL)                   //If the character is the decimal type
  23                 {
  24                     int64_t buf = va_arg(va,int64_t);               //Get the next value from the va list
  25                     //if (buf < 0) { *d++ = '-'; buf = -buf; }      //If the given number is negative, add a negative sign to the dest and invert number
  26                     //spf_uint2str_32_3t(&d,buf,32);                //Perform the conversion
  27                     d += UTIL_ltoa_radix(buf, d, 10);
  28                 }
  29                 else if (*_Format == SPF_SPEC_UNSIGNED)             //If the character is the unsigned type
  30                 {
  31                     uint64_t num = va_arg(va,uint64_t);             //Get the next value from the va list
  32                     //spf_uint2str_32_3t(&d,num,32);                //Perform the conversion
  33                     d += UTIL_ltoa_radix(num, d, 10);
  34                 }
  35                 else if (*_Format == SPF_SPEC_UHINT || *_Format == SPF_SPEC_UHINT_UP)   //If the character is the unsigned type
  36                 {
  37                     uint64_t buf = va_arg(va,uint64_t);             //Get the next value from the va list
  38                     //spf_uint2hex_32(&d,(unsigned long) buf);
  39                     d += UTIL_ltoa_radix(buf, d, 16);
  40                 }
  41                 else                                                //If the character was not a known type
  42                 {
  43                     *d++ = SPF_SPEC_START;                          //Output the start specifier
  44                     *d++ = SPF_SPEC_LONG;                           //Output the long type
  45                     *d++ = *_Format;                                //Output the unknown type
  46                 }
  47             }
  48             else if (*_Format == SPF_SPEC_DECIMAL)                  //If the character is the decimal type
  49             {
  50                 int buf = va_arg(va,int);                           //Get the next value from the va list
  51                 //if (buf < 0) { *d++ = '-'; buf = -buf; }          //If the given number is negative, add a negative sign to the dest and invert number
  52                 //spf_uint2str_32_3t(&d,buf,16);                    //Perform the conversion
  53                 d += UTIL_itoa(buf, d);
  54             }
  55             else if (*_Format == SPF_SPEC_INT)                      //If the character is the integer type
  56             {
  57                 int buf = va_arg(va,int);                           //Get the next value from the va list
  58                 //if (buf < 0) { *d++ = '-'; buf = -buf; }          //If the given number is negative, add a negative sign to the dest and inverted number
  59                 //spf_uint2str_32_3t(&d,buf,16);                    //Perform the conversion
  60                 d += UTIL_itoa(buf, d);
  61             }
  62             else if (*_Format == SPF_SPEC_UINT)                     //If the character is the unsigned integer type
  63             {
  64                 int buf = va_arg(va,int);                           //Get the next value from the va list
  65                 //spf_uint2str_32_3t(&d,buf,16);                    //Perform the conversion
  66                 d += UTIL_utoa(buf, d);
  67             }
  68             else if (*_Format == SPF_SPEC_STRING)                   //If the character is the string type
  69             {
  70                 char *buf = va_arg(va,char*);                       //Get the next value from the va list
  71                 while (*buf) *d++ = *buf++;                         //Perform the conversion (simply output characters and adcance pointers)
  72             }
  73             else if (*_Format == SPF_SPEC_UHINT || *_Format == SPF_SPEC_UHINT_UP)   //If the character is the short type
  74             {
  75                 int buf = va_arg(va,unsigned int);                  //Get the next value from the va list
  76                 //spf_uint2hex_32(&d,(unsigned long) buf);          //Perform the conversion
  77                 d += UTIL_utoa(buf, d);
  78             }
  79             else                                                    //If the character type is unknown
  80             {
  81                 *d++ = SPF_SPEC_START;                              //Output the start specifier
  82                 *d++ = *_Format;                                    //Output the unknown type
  83             }
  84         }
  85         else *d++ = *_Format;                                       //If the character is unknown, output it to dest and advance dest
  86         _Format++;                                                  //Advance the format buffer pointer to next character
  87     }
  88     //va_end(va);                                                   //End the variable argument list
  89
  90     *d = '\0';                                                      //Cap off the destination string with a zero
  91
  92     return d - _Dest;                                               //Return the length of the destintion buffer
  93 }
  94
  95 int spf(char *_Dest, const char *_Format, ...)
  96 {
  97     va_list va;                                                     //Variable argument list variable
  98     int result;
  99
 100     va_start(va,_Format);                                           //Initialize the variable argument list
 101     result = vspf(_Dest, _Format, va);
 102     va_end(va);
 103     return result;
 104 }
 105
 106 //sscanf string to number (integer types)
 107 int64_t ssf_ston(const char **_Src, uint32_t count, uint32_t *conv_count)
 108 {
 109     int64_t value = 0;                                  //Return value accumulator
 110     uint32_t counter=count;                             //Counter to keep track of numbers converted
 111     const char* p;                                      //Pointer to first non space character
 112
 113     while (*(*_Src) == SSF_SKIP_SPACE) (*_Src)++;       //Forward through the whitespace to next non whitespace
 114
 115     p = (*_Src);                                        //Set pointer to first non space character
 116     if (*p == '+' || *p == '-') (*_Src)++;              //Skip over sign if any
 117     while (*(*_Src) >= ASCII_NUM_START &&
 118         *(*_Src) <= ASCII_NUM_END &&
 119         counter)                                        //While the source character is a digit and counter is not zero
 120     {
 121         value *= 10;                                    //Multiply result by 10 to make room for next 1's place number
 122         value += *(*_Src)++ - ASCII_NUM_START;          //Add source number to value
 123         counter--;                                      //Decrement counter
 124     }
 125     if (counter - count == 0) return 0;                 //If no number conversion were performed, return 0
 126     if (*p == '-') value = -value;                      //If the number given was negative, make the result negative
 127
 128     if (conv_count) (*conv_count)++;                    //Increment the converted count
 129     return value;                                       //Return the value
 130 }
 131
 132 uint64_t ssf_hton(const char **_Src, uint32_t count,uint32_t *conv_count)
 133 {
 134     int64_t value=0;                                            //Return value accumulator
 135     uint32_t counter=count;                                     //Counter to keep track of numbers converted
 136     //const char* p;                                            //Pointer to first non space character
 137     char c;
 138
 139     while (*(*_Src) == SSF_SKIP_SPACE) (*_Src)++;               //Forward through the whitespace to next non whitespace
 140
 141     //p = (*_Src);                                              //Set pointer to first non space character
 142
 143     while (counter)
 144     {
 145         c = *(*_Src)++;
 146         if (c >= 'a' && c <= 'f') c -= ('a'-'A');               //toupper
 147         if (c < '0' || (c > '9' && c < 'A') || c > 'F') break;
 148         value *= 16;                                            //Multiply result by 10 to make room for next 1's place number
 149         c = c - '0';
 150         if (c > 9) c -= 7;
 151         value += c;                                             //Add source number to value
 152         counter--;                                              //Decrement counter
 153     }
 154
 155     if (counter - count == 0) return 0;                         //If no number conversion were performed, return 0
 156     //if (*p == '-') value = -value;                            //If the number given was negative, make the result negative
 157
 158     if (conv_count) (*conv_count)++;                            //Increment the converted count
 159     return value;
 160 }
 161
 162 //sscanf
 163 int ssf(const char *_Src, const char *_Format, ...)
 164 {
 165     va_list va;                                                                         //Variable argument list variable
 166     unsigned char looking_for=0;                                                        //Static char specified in format to be found in source
 167     uint32_t conv_count=0;                                                              //Count of conversions made
 168
 169     va_start(va,_Format);                                                               //Initialize the variable argument list
 170     while (*_Format)                                                                    //While the format string has not been fully read
 171     {
 172         if (looking_for != 0)                                                           //If we are looking for a matching character in the source string
 173         {
 174             while (*_Src != looking_for && *_Src) _Src++;                               //While the character is not found in the source string and not the end of the source
 175                                                                                         //  string, increment the pointer position
 176             if (*_Src == looking_for) _Src++;                                           //If the character was found, step over it
 177             else break;                                                                 //Else the end was reached and the scan is now invalid (Could not find static character)
 178             looking_for = 0;                                                            //Clear the looking for character
 179         }
 180         if (*_Format == SSF_SPEC_START)                                                 //If the current format character is the specifier start character
 181         {
 182             _Format++;                                                                  //Step over the specifier start character
 183             if (*_Format == SSF_SPEC_DECIMAL)                                           //If the decimal specifier type is found
 184             {
 185                 int *value=va_arg(va,int*);                                             //User given destination address
 186                 //*value = (int)ssf_ston(&_Src,5,&conv_count);                          //Run conversion
 187                 *value = (int)ssf_ston(&_Src,10,&conv_count);                           //Run conversion
 188             }
 189             else if (*_Format == SSF_SPEC_LONG)                                         //If the long specifier type is found
 190             {
 191                 _Format++;                                                              //Skip over the specifier type
 192                 if (*_Format == SSF_SPEC_DECIMAL)                                       //If the decimal specifier type is found
 193                 {
 194                     int64_t *value=va_arg(va,int64_t*);                                 //User given destination address
 195                     //*value = (int64_t)ssf_ston(&_Src,10,&conv_count);                 //Run conversion
 196                     *value = (int64_t)ssf_ston(&_Src,19,&conv_count);                   //Run conversion
 197                 }
 198                 else if (*_Format == SSF_SPEC_UHINT)                                    //If the decimal specifier type is found
 199                 {
 200                     uint64_t *value=va_arg(va,uint64_t *);                              //User given destination address
 201                     //*value = (uint64_t int)ssf_hton(&_Src,12,&conv_count);            //Run conversion
 202                     *value = (uint64_t)ssf_hton(&_Src,16,&conv_count);                  //Run conversion
 203                 }
 204             }
 205             else if (*_Format == SSF_SPEC_SHORTINT)                                     //If the short int specifier type is found
 206             {
 207                 _Format++;                                                              //Skip over the specifier type
 208                 if (*_Format == SSF_SPEC_SHORTINT)                                      //If the short int specifier type is found
 209                 {
 210                     _Format++;                                                          //Skip over the specifier type
 211                     if (*_Format == SSF_SPEC_DECIMAL)                                   //If the decimal specifier type is found
 212                     {
 213                         unsigned char *value=va_arg(va,unsigned char*);                 //User given destination address
 214                         //*value = (unsigned char)ssf_ston(&_Src,3,&conv_count);        //Run conversion
 215                         *value = (unsigned char)ssf_ston(&_Src,5,&conv_count);          //Run conversion
 216                     }
 217                 }
 218             }
 219             else if (*_Format == SSF_SPEC_STRING)                                       //If the specifier type is string
 220             {
 221                 char *value=va_arg(va,char*);                                           //User given destination address, max chars read pointer
 222                 while (*_Src == SSF_SKIP_SPACE) _Src++;                                 //Forward through the whitespace to next non whitespace
 223                 while (*_Src != SSF_SKIP_SPACE && *_Src) *value++ = *_Src++;            //While any character but space and not end of string and not end location, copy char to dest
 224                 *value = 0;                                                             //Cap off the string pointer with zero
 225                 conv_count++;                                                           //Increment the converted count
 226             }
 227             else if (*_Format == SSF_SPEC_VERSION)                                      //If the specifier type is string
 228             {
 229                 char *value=va_arg(va,char*);                                           //User given destination address, max chars read pointer
 230                 while (*_Src == SSF_SKIP_SPACE) _Src++;                                 //Forward through the whitespace to next non whitespace
 231                 while (*_Src != SSF_DELIM_COMMA && *_Src) *value++ = *_Src++;           //While any character but space and not end of string and not end location, copy char to dest
 232                 *value = 0;                                                             //Cap off the string pointer with zero
 233                 conv_count++;                                                           //Increment the converted count
 234             }
 235             else if (*_Format >= ASCII_NUM_START && *_Format <= ASCII_NUM_END)
 236             {
 237                 uint32_t len = (uint32_t)ssf_ston(&_Format,3,NULL);                     //Convert the given length
 238                 if (*_Format == SSF_SPEC_STRING)                                        //If the specifier type is string
 239                 {
 240                     char *value=va_arg(va,char*),*e;                                    //User given destination address, max chars read pointer
 241                     while (*_Src == SSF_SKIP_SPACE) _Src++;                             //Forward through the whitespace to next non whitespace
 242                     e = (char*)_Src+len;                                                //Set a maximum length pointer location
 243                     while (*_Src != SSF_SKIP_SPACE && *_Src && _Src != e) *value++ = *_Src++;   //While any character but space and not end of string and not end location, copy char to dest
 244                     *value = 0;                                                         //Cap off the string pointer with zero
 245                     conv_count++;                                                       //Increment the converted count
 246                 }
 247                 else if (*_Format == SSF_SPEC_VERSION)                                  //If the specifier type is string
 248                 {
 249                     char *value=va_arg(va,char*),*e;                                    //User given destination address, max chars read pointer
 250                     while (*_Src == SSF_SKIP_SPACE) _Src++;                             //Forward through the whitespace to next non whitespace
 251                     e = (char*)_Src+len;                                                //Set a maximum length pointer location
 252                     while (*_Src != SSF_DELIM_COMMA && *_Src && _Src != e) *value++ = *_Src++;  //While any character but space and not end of string and not end location, copy char to dest
 253                     *value = 0;                                                         //Cap off the string pointer with zero
 254                     conv_count++;                                                       //Increment the converted count
 255                 }
 256             }
 257             else if (*_Format == SSF_SPEC_START) looking_for = *_Format;                //If another start specifier character is found, output a specifier character
 258             else break;                                                                 //Scan is now invalid (Uknown type specified)
 259         }
 260         else if (*_Format == SSF_SKIP_SPACE) { }                                        //If a space is found, ignore it
 261         else looking_for = *_Format;                                                    //If any other character is found, it is static and should be found in src as well
 262         _Format++;                                                                      //Skip over current format character
 263     }
 264
 265     va_end(va);                                                                         //End the variable argument list
 266     return conv_count;                                                                  //Return the number of conversions made
 267 }
 268