hex.c

   1 /*
   2  * This file is Copyright (c) 2010 by the GPSD project
   3  * BSD terms apply: see the file COPYING in the distribution root for details.
   4  */
   5 #ifndef S_SPLINT_S
   6 #include <unistd.h>
   7 #endif /* S_SPLINT_S */
   8 #include <stdlib.h>
   9 #include <stdio.h>
  10 #include <math.h>
  11 #include <string.h>
  12
  13 #include "gpsd.h"
  14
  15 int gpsd_hexdump_level = -1;
  16 /*
  17  * A wrapper around gpsd_hexdump to prevent wasting cpu time by hexdumping
  18  * buffers and copying strings that will never be printed. only messages at
  19  * level "N" and lower will be printed. By way of example, without any -D
  20  * options, gpsd probably won't ever call the real gpsd_hexdump. At -D2,
  21  * LOG_PROG (and higher) won't get to call the real gpsd_hexdump. For high
  22  * speed, chatty protocols, this can save a lot of CPU.
  23  */
  24 char *gpsd_hexdump_wrapper(const void *binbuf, size_t binbuflen,
  25                            int msg_debug_level)
  26 {
  27 #ifndef SQUELCH_ENABLE
  28     if (msg_debug_level <= gpsd_hexdump_level)
  29         return gpsd_hexdump(binbuf, binbuflen);
  30 #endif /* SQUELCH_ENABLE */
  31     return "";
  32 }
  33
  34 char /*@ observer @*/ *gpsd_hexdump(const void *binbuf, size_t binbuflen)
  35 {
  36     static char hexbuf[MAX_PACKET_LENGTH * 2 + 1];
  37 #ifndef SQUELCH_ENABLE
  38     size_t i, j = 0;
  39     size_t len =
  40         (size_t) ((binbuflen >
  41                    MAX_PACKET_LENGTH) ? MAX_PACKET_LENGTH : binbuflen);
  42     const char *ibuf = (const char *)binbuf;
  43     const char *hexchar = "0123456789abcdef";
  44
  45     if (NULL == binbuf || 0 == binbuflen)
  46         return "";
  47
  48     /*@ -shiftimplementation @*/
  49     for (i = 0; i < len; i++) {
  50         hexbuf[j++] = hexchar[(ibuf[i] & 0xf0) >> 4];
  51         hexbuf[j++] = hexchar[ibuf[i] & 0x0f];
  52     }
  53     /*@ +shiftimplementation @*/
  54     hexbuf[j] = '\0';
  55 #else /* SQUELCH defined */
  56     hexbuf[0] = '\0';
  57 #endif /* SQUELCH_ENABLE */
  58     return hexbuf;
  59 }
  60
  61 int gpsd_hexpack( /*@in@*/ const char *src, /*@out@ */ char *dst, size_t len)
  62 {
  63 /* hex2bin source string to destination - destination can be same as source */
  64     int i, k, l;
  65
  66     /*@ -mustdefine @*/
  67     l = (int)(strlen(src) / 2);
  68     if ((l < 1) || ((size_t) l > len))
  69         return -2;
  70
  71     for (i = 0; i < l; i++)
  72         if ((k = hex2bin(src + i * 2)) != -1)
  73             dst[i] = (char)(k & 0xff);
  74         else
  75             return -1;
  76     (void)memset(dst + i, '\0', (size_t) (len - i));
  77     return l;
  78     /*@ +mustdefine @*/
  79 }
  80
  81 /*@ +charint -shiftimplementation @*/
  82 int hex2bin(const char *s)
  83 {
  84     int a, b;
  85
  86     a = s[0] & 0xff;
  87     b = s[1] & 0xff;
  88
  89     if ((a >= 'a') && (a <= 'f'))
  90         a = a + 10 - 'a';
  91     else if ((a >= 'A') && (a <= 'F'))
  92         a = a + 10 - 'A';
  93     else if ((a >= '0') && (a <= '9'))
  94         a -= '0';
  95     else
  96         return -1;
  97
  98     if ((b >= 'a') && (b <= 'f'))
  99         b = b + 10 - 'a';
 100     else if ((b >= 'A') && (b <= 'F'))
 101         b = b + 10 - 'A';
 102     else if ((b >= '0') && (b <= '9'))
 103         b -= '0';
 104     else
 105         return -1;
 106
 107     return ((a << 4) + b);
 108 }
 109
 110 /*@ -charint +shiftimplementation @*/
 111
 112 ssize_t hex_escapes( /*@out@*/ char *cooked, const char *raw)
 113 /* interpret C-style hex escapes */
 114 {
 115     char c, *cookend;
 116
 117     /*@ +charint -mustdefine -compdef @*/
 118     for (cookend = cooked; *raw != '\0'; raw++)
 119         if (*raw != '\\')
 120             *cookend++ = *raw;
 121         else {
 122             switch (*++raw) {
 123             case 'b':
 124                 *cookend++ = '\b';
 125                 break;
 126             case 'e':
 127                 *cookend++ = '\x1b';
 128                 break;
 129             case 'f':
 130                 *cookend++ = '\f';
 131                 break;
 132             case 'n':
 133                 *cookend++ = '\n';
 134                 break;
 135             case 'r':
 136                 *cookend++ = '\r';
 137                 break;
 138             case 't':
 139                 *cookend++ = '\r';
 140                 break;
 141             case 'v':
 142                 *cookend++ = '\v';
 143                 break;
 144             case 'x':
 145                 switch (*++raw) {
 146                 case '0':
 147                     c = (char)0x00;
 148                     break;
 149                 case '1':
 150                     c = (char)0x10;
 151                     break;
 152                 case '2':
 153                     c = (char)0x20;
 154                     break;
 155                 case '3':
 156                     c = (char)0x30;
 157                     break;
 158                 case '4':
 159                     c = (char)0x40;
 160                     break;
 161                 case '5':
 162                     c = (char)0x50;
 163                     break;
 164                 case '6':
 165                     c = (char)0x60;
 166                     break;
 167                 case '7':
 168                     c = (char)0x70;
 169                     break;
 170                 case '8':
 171                     c = (char)0x80;
 172                     break;
 173                 case '9':
 174                     c = (char)0x90;
 175                     break;
 176                 case 'A':
 177                 case 'a':
 178                     c = (char)0xa0;
 179                     break;
 180                 case 'B':
 181                 case 'b':
 182                     c = (char)0xb0;
 183                     break;
 184                 case 'C':
 185                 case 'c':
 186                     c = (char)0xc0;
 187                     break;
 188                 case 'D':
 189                 case 'd':
 190                     c = (char)0xd0;
 191                     break;
 192                 case 'E':
 193                 case 'e':
 194                     c = (char)0xe0;
 195                     break;
 196                 case 'F':
 197                 case 'f':
 198                     c = (char)0xf0;
 199                     break;
 200                 default:
 201                     return -1;
 202                 }
 203                 switch (*++raw) {
 204                 case '0':
 205                     c += 0x00;
 206                     break;
 207                 case '1':
 208                     c += 0x01;
 209                     break;
 210                 case '2':
 211                     c += 0x02;
 212                     break;
 213                 case '3':
 214                     c += 0x03;
 215                     break;
 216                 case '4':
 217                     c += 0x04;
 218                     break;
 219                 case '5':
 220                     c += 0x05;
 221                     break;
 222                 case '6':
 223                     c += 0x06;
 224                     break;
 225                 case '7':
 226                     c += 0x07;
 227                     break;
 228                 case '8':
 229                     c += 0x08;
 230                     break;
 231                 case '9':
 232                     c += 0x09;
 233                     break;
 234                 case 'A':
 235                 case 'a':
 236                     c += 0x0a;
 237                     break;
 238                 case 'B':
 239                 case 'b':
 240                     c += 0x0b;
 241                     break;
 242                 case 'C':
 243                 case 'c':
 244                     c += 0x0c;
 245                     break;
 246                 case 'D':
 247                 case 'd':
 248                     c += 0x0d;
 249                     break;
 250                 case 'E':
 251                 case 'e':
 252                     c += 0x0e;
 253                     break;
 254                 case 'F':
 255                 case 'f':
 256                     c += 0x0f;
 257                     break;
 258                 default:
 259                     return -2;
 260                 }
 261                 *cookend++ = c;
 262                 break;
 263             case '\\':
 264                 *cookend++ = '\\';
 265                 break;
 266             default:
 267                 return -3;
 268             }
 269         }
 270     return (ssize_t) (cookend - cooked);
 271     /*@ +charint +mustdefine +compdef @*/
 272 }