src/pds.c

   1 /* Copyright (C) 2009-2014, Martin Johansson <martin@fatbob.nu>
   2    Copyright (C) 2005-2014, Thorvald Natvig <thorvald@natvig.com>
   3
   4    All rights reserved.
   5
   6    Redistribution and use in source and binary forms, with or without
   7    modification, are permitted provided that the following conditions
   8    are met:
   9
  10    - Redistributions of source code must retain the above copyright notice,
  11      this list of conditions and the following disclaimer.
  12    - Redistributions in binary form must reproduce the above copyright notice,
  13      this list of conditions and the following disclaimer in the documentation
  14      and/or other materials provided with the distribution.
  15    - Neither the name of the Developers nor the names of its contributors may
  16      be used to endorse or promote products derived from this software without
  17      specific prior written permission.
  18
  19    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  20    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  21    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  22    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
  23    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  24    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  25    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
  26    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
  27    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
  28    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  29    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  30 */
  31 #include <string.h>
  32 #include <stdlib.h>
  33 #include "pds.h"
  34 #include "log.h"
  35
  36 /*
  37  * Data serialization functions below
  38  */
  39
  40 typedef union double64u {
  41         uint64_t u64;
  42         double dval;
  43 } double64u_t;
  44
  45 static inline void append_val(pds_t *pds, uint64_t val)
  46 {
  47         if (pds->offset < pds->maxsize)
  48                 pds->data[pds->offset++] = ((uint8_t)(val));
  49         else {
  50                 pds->bOk = false;
  51                 pds->overshoot++;
  52         }
  53 }
  54
  55 void Pds_append_data(pds_t *pds, const uint8_t *data, uint32_t len)
  56 {
  57         int left;
  58         Pds_add_numval(pds, len);
  59         left = pds->maxsize - pds->offset;
  60         if (left >= len) {
  61                 memcpy(&pds->data[pds->offset], data, len);
  62                 pds->offset += len;
  63         } else {
  64                 memset(&pds->data[pds->offset], 0, left);
  65                 pds->offset += left;
  66                 pds->overshoot += len - left;
  67                 pds->bOk = false;
  68         }
  69 }
  70
  71 void Pds_append_data_nosize(pds_t *pds, const uint8_t *data, uint32_t len)
  72 {
  73         int left;
  74         left = pds->maxsize - pds->offset;
  75         if (left >= len) {
  76                 memcpy(&pds->data[pds->offset], data, len);
  77                 pds->offset += len;
  78         } else {
  79                 memset(&pds->data[pds->offset], 0, left);
  80                 pds->offset += left;
  81                 pds->overshoot += len - left;
  82                 pds->bOk = false;
  83         }
  84 }
  85
  86 uint8_t Pds_next8(pds_t *pds)
  87 {
  88         if (pds->offset < pds->maxsize)
  89                 return pds->data[pds->offset++];
  90         else {
  91                 pds->bOk = false;
  92                 return 0;
  93         }
  94 }
  95
  96 int Pds_skip(pds_t *pds, int offset)
  97 {
  98         if (pds->offset + offset <= pds->maxsize) {
  99                 pds->offset += offset;
 100                 return offset;
 101         } else {
 102                 pds->bOk = false;
 103                 return 0;
 104         }
 105
 106 }
 107
 108 static inline uint64_t next(pds_t *pds)
 109 {
 110         if (pds->offset < pds->maxsize)
 111                 return pds->data[pds->offset++];
 112         else {
 113                 pds->bOk = false;
 114                 return 0;
 115         }
 116 }
 117
 118 pds_t *Pds_create(uint8_t *buf, int size)
 119 {
 120         pds_t *pds = malloc(sizeof(pds_t));
 121         if (pds == NULL)
 122                 Log_fatal("Out of memory");
 123         pds->data = buf;
 124         pds->offset = pds->overshoot = 0;
 125         pds->maxsize = size;
 126         pds->bOk = true;
 127         return pds;
 128 }
 129
 130 void Pds_free(pds_t *pds)
 131 {
 132         free(pds);
 133 }
 134
 135 void Pds_add_double(pds_t *pds, double value)
 136 {
 137         double64u_t u;
 138
 139         u.dval = value;
 140
 141         Pds_add_numval(pds, u.u64);
 142 }
 143
 144 double Pds_get_double(pds_t *pds)
 145 {
 146         double64u_t u;
 147         u.u64 = Pds_get_numval(pds);
 148         return u.dval;
 149 }
 150 void Pds_add_numval(pds_t *pds, const uint64_t value)
 151 {
 152         uint64_t i = value;
 153
 154         if ((i & 0x8000000000000000LL) && (~i < 0x100000000LL)) {
 155                 // Signed number.
 156                 i = ~i;
 157                 if (i <= 0x3) {
 158                         // Shortcase for -1 to -4
 159                         append_val(pds, 0xFC | i);
 160                         return;
 161                 } else {
 162                         append_val(pds, 0xF8);
 163                 }
 164         }
 165         if (i < 0x80) {
 166                 // Need top bit clear
 167                 append_val(pds, i);
 168         } else if (i < 0x4000) {
 169                 // Need top two bits clear
 170                 append_val(pds, (i >> 8) | 0x80);
 171                 append_val(pds, i & 0xFF);
 172         } else if (i < 0x200000) {
 173                 // Need top three bits clear
 174                 append_val(pds, (i >> 16) | 0xC0);
 175                 append_val(pds, (i >> 8) & 0xFF);
 176                 append_val(pds, i & 0xFF);
 177         } else if (i < 0x10000000) {
 178                 // Need top four bits clear
 179                 append_val(pds, (i >> 24) | 0xE0);
 180                 append_val(pds, (i >> 16) & 0xFF);
 181                 append_val(pds, (i >> 8) & 0xFF);
 182                 append_val(pds, i & 0xFF);
 183         } else if (i < 0x100000000LL) {
 184                 // It's a full 32-bit integer.
 185                 append_val(pds, 0xF0);
 186                 append_val(pds, (i >> 24) & 0xFF);
 187                 append_val(pds, (i >> 16) & 0xFF);
 188                 append_val(pds, (i >> 8) & 0xFF);
 189                 append_val(pds, i & 0xFF);
 190         } else {
 191                 // It's a 64-bit value.
 192                 append_val(pds, 0xF4);
 193                 append_val(pds, (i >> 56) & 0xFF);
 194                 append_val(pds, (i >> 48) & 0xFF);
 195                 append_val(pds, (i >> 40) & 0xFF);
 196                 append_val(pds, (i >> 32) & 0xFF);
 197                 append_val(pds, (i >> 24) & 0xFF);
 198                 append_val(pds, (i >> 16) & 0xFF);
 199                 append_val(pds, (i >> 8) & 0xFF);
 200                 append_val(pds, i & 0xFF);
 201         }
 202 }
 203
 204 uint64_t Pds_get_numval(pds_t *pds)
 205 {
 206         uint64_t i = 0;
 207         uint64_t v = next(pds);
 208
 209         if ((v & 0x80) == 0x00) {
 210                 i=(v & 0x7F);
 211         } else if ((v & 0xC0) == 0x80) {
 212                 i=(v & 0x3F) << 8 | next(pds);
 213         } else if ((v & 0xF0) == 0xF0) {
 214                 switch (v & 0xFC) {
 215                 case 0xF0:
 216                         i=next(pds) << 24 | next(pds) << 16 | next(pds) << 8 | next(pds);
 217                         break;
 218                 case 0xF4:
 219                         i=next(pds) << 56 | next(pds) << 48 | next(pds) << 40 | next(pds) << 32 | next(pds) << 24 | next(pds) << 16 | next(pds) << 8 | next(pds);
 220                         break;
 221                 case 0xF8:
 222                         i = Pds_get_numval(pds);
 223                         i = ~i;
 224                         break;
 225                 case 0xFC:
 226                         i=v & 0x03;
 227                         i = ~i;
 228                         break;
 229                 default:
 230                         //ok = false;
 231                         i = 0;
 232                         break;
 233                 }
 234         } else if ((v & 0xF0) == 0xE0) {
 235                 i=(v & 0x0F) << 24 | next(pds) << 16 | next(pds) << 8 | next(pds);
 236         } else if ((v & 0xE0) == 0xC0) {
 237                 i=(v & 0x1F) << 16 | next(pds) << 8 | next(pds);
 238         }
 239         return i;
 240 }
 241
 242 void Pds_add_string(pds_t *pds, const char *str)
 243 {
 244         Pds_append_data(pds, (uint8_t *)str, strlen(str));
 245 }
 246
 247 void Pds_get_string(pds_t *pds, char *str, int maxlen)
 248 {
 249         int len = Pds_get_numval(pds);
 250         if (len < maxlen) {
 251                 memcpy(str, &pds->data[pds->offset], len);
 252                 str[len] = '\0';
 253                 pds->offset += len;
 254         } else {
 255                 Log_warn("Too long string from network");
 256                 strcpy(str, "N/A");
 257         }
 258 }
 259
 260 int Pds_get_data(pds_t *pds, uint8_t *data, int maxlen)
 261 {
 262         int len = Pds_get_numval(pds);
 263         if (len < maxlen) {
 264                 memcpy(data, &pds->data[pds->offset], len);
 265                 pds->offset += len;
 266                 return len;
 267         } else {
 268                 Log_warn("Pds_get_data: Too much data from network");
 269                 return -1;
 270         }
 271 }