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 #include "memory.h"
  36
  37 /*
  38  * Data serialization functions below
  39  */
  40
  41 typedef union double64u {
  42         uint64_t u64;
  43         double dval;
  44 } double64u_t;
  45
  46 static inline void append_val(pds_t *pds, uint64_t val)
  47 {
  48         if (pds->offset < pds->maxsize)
  49                 pds->data[pds->offset++] = ((uint8_t)(val));
  50         else {
  51                 pds->bOk = false;
  52                 pds->overshoot++;
  53         }
  54 }
  55
  56 void Pds_append_data(pds_t *pds, const uint8_t *data, uint32_t len)
  57 {
  58         int left;
  59         Pds_add_numval(pds, len);
  60         left = pds->maxsize - pds->offset;
  61         if (left >= len) {
  62                 memcpy(&pds->data[pds->offset], data, len);
  63                 pds->offset += len;
  64         } else {
  65                 memset(&pds->data[pds->offset], 0, left);
  66                 pds->offset += left;
  67                 pds->overshoot += len - left;
  68                 pds->bOk = false;
  69         }
  70 }
  71
  72 void Pds_append_data_nosize(pds_t *pds, const uint8_t *data, uint32_t len)
  73 {
  74         int left;
  75         left = pds->maxsize - pds->offset;
  76         if (left >= len) {
  77                 memcpy(&pds->data[pds->offset], data, len);
  78                 pds->offset += len;
  79         } else {
  80                 memset(&pds->data[pds->offset], 0, left);
  81                 pds->offset += left;
  82                 pds->overshoot += len - left;
  83                 pds->bOk = false;
  84         }
  85 }
  86
  87 uint8_t Pds_next8(pds_t *pds)
  88 {
  89         if (pds->offset < pds->maxsize)
  90                 return pds->data[pds->offset++];
  91         else {
  92                 pds->bOk = false;
  93                 return 0;
  94         }
  95 }
  96
  97 int Pds_skip(pds_t *pds, int offset)
  98 {
  99         if (pds->offset + offset <= pds->maxsize) {
 100                 pds->offset += offset;
 101                 return offset;
 102         } else {
 103                 pds->bOk = false;
 104                 return 0;
 105         }
 106
 107 }
 108
 109 static inline uint64_t next(pds_t *pds)
 110 {
 111         if (pds->offset < pds->maxsize)
 112                 return pds->data[pds->offset++];
 113         else {
 114                 pds->bOk = false;
 115                 return 0;
 116         }
 117 }
 118
 119 pds_t *Pds_create(uint8_t *buf, int size)
 120 {
 121         pds_t *pds = Memory_safeMalloc(1, sizeof(pds_t));
 122         pds->data = buf;
 123         pds->offset = pds->overshoot = 0;
 124         pds->maxsize = size;
 125         pds->bOk = true;
 126         return pds;
 127 }
 128
 129 void Pds_free(pds_t *pds)
 130 {
 131         free(pds);
 132 }
 133
 134 void Pds_add_double(pds_t *pds, double value)
 135 {
 136         double64u_t u;
 137
 138         u.dval = value;
 139
 140         Pds_add_numval(pds, u.u64);
 141 }
 142
 143 double Pds_get_double(pds_t *pds)
 144 {
 145         double64u_t u;
 146         u.u64 = Pds_get_numval(pds);
 147         return u.dval;
 148 }
 149 void Pds_add_numval(pds_t *pds, const uint64_t value)
 150 {
 151         uint64_t i = value;
 152
 153         if ((i & 0x8000000000000000LL) && (~i < 0x100000000LL)) {
 154                 // Signed number.
 155                 i = ~i;
 156                 if (i <= 0x3) {
 157                         // Shortcase for -1 to -4
 158                         append_val(pds, 0xFC | i);
 159                         return;
 160                 } else {
 161                         append_val(pds, 0xF8);
 162                 }
 163         }
 164         if (i < 0x80) {
 165                 // Need top bit clear
 166                 append_val(pds, i);
 167         } else if (i < 0x4000) {
 168                 // Need top two bits clear
 169                 append_val(pds, (i >> 8) | 0x80);
 170                 append_val(pds, i & 0xFF);
 171         } else if (i < 0x200000) {
 172                 // Need top three bits clear
 173                 append_val(pds, (i >> 16) | 0xC0);
 174                 append_val(pds, (i >> 8) & 0xFF);
 175                 append_val(pds, i & 0xFF);
 176         } else if (i < 0x10000000) {
 177                 // Need top four bits clear
 178                 append_val(pds, (i >> 24) | 0xE0);
 179                 append_val(pds, (i >> 16) & 0xFF);
 180                 append_val(pds, (i >> 8) & 0xFF);
 181                 append_val(pds, i & 0xFF);
 182         } else if (i < 0x100000000LL) {
 183                 // It's a full 32-bit integer.
 184                 append_val(pds, 0xF0);
 185                 append_val(pds, (i >> 24) & 0xFF);
 186                 append_val(pds, (i >> 16) & 0xFF);
 187                 append_val(pds, (i >> 8) & 0xFF);
 188                 append_val(pds, i & 0xFF);
 189         } else {
 190                 // It's a 64-bit value.
 191                 append_val(pds, 0xF4);
 192                 append_val(pds, (i >> 56) & 0xFF);
 193                 append_val(pds, (i >> 48) & 0xFF);
 194                 append_val(pds, (i >> 40) & 0xFF);
 195                 append_val(pds, (i >> 32) & 0xFF);
 196                 append_val(pds, (i >> 24) & 0xFF);
 197                 append_val(pds, (i >> 16) & 0xFF);
 198                 append_val(pds, (i >> 8) & 0xFF);
 199                 append_val(pds, i & 0xFF);
 200         }
 201 }
 202
 203 uint64_t Pds_get_numval(pds_t *pds)
 204 {
 205         uint64_t i = 0;
 206         uint64_t v = next(pds);
 207
 208         if ((v & 0x80) == 0x00) {
 209                 i=(v & 0x7F);
 210         } else if ((v & 0xC0) == 0x80) {
 211                 i=(v & 0x3F) << 8 | next(pds);
 212         } else if ((v & 0xF0) == 0xF0) {
 213                 switch (v & 0xFC) {
 214                 case 0xF0:
 215                         i=next(pds) << 24 | next(pds) << 16 | next(pds) << 8 | next(pds);
 216                         break;
 217                 case 0xF4:
 218                         i=next(pds) << 56 | next(pds) << 48 | next(pds) << 40 | next(pds) << 32 | next(pds) << 24 | next(pds) << 16 | next(pds) << 8 | next(pds);
 219                         break;
 220                 case 0xF8:
 221                         i = Pds_get_numval(pds);
 222                         i = ~i;
 223                         break;
 224                 case 0xFC:
 225                         i=v & 0x03;
 226                         i = ~i;
 227                         break;
 228                 default:
 229                         //ok = false;
 230                         i = 0;
 231                         break;
 232                 }
 233         } else if ((v & 0xF0) == 0xE0) {
 234                 i=(v & 0x0F) << 24 | next(pds) << 16 | next(pds) << 8 | next(pds);
 235         } else if ((v & 0xE0) == 0xC0) {
 236                 i=(v & 0x1F) << 16 | next(pds) << 8 | next(pds);
 237         }
 238         return i;
 239 }
 240
 241 void Pds_add_string(pds_t *pds, const char *str)
 242 {
 243         Pds_append_data(pds, (uint8_t *)str, strlen(str));
 244 }
 245
 246 void Pds_get_string(pds_t *pds, char *str, int maxlen)
 247 {
 248         int len = Pds_get_numval(pds);
 249         if (len < maxlen) {
 250                 memcpy(str, &pds->data[pds->offset], len);
 251                 str[len] = '\0';
 252                 pds->offset += len;
 253         } else {
 254                 Log_warn("Too long string from network");
 255                 strcpy(str, "N/A");
 256         }
 257 }
 258
 259 int Pds_get_data(pds_t *pds, uint8_t *data, int maxlen)
 260 {
 261         int len = Pds_get_numval(pds);
 262         if (len < maxlen) {
 263                 memcpy(data, &pds->data[pds->offset], len);
 264                 pds->offset += len;
 265                 return len;
 266         } else {
 267                 Log_warn("Pds_get_data: Too much data from network");
 268                 return -1;
 269         }
 270 }