pds.c

   1 /* Copyright (C) 2009, Martin Johansson <martin@fatbob.nu>
   2    Copyright (C) 2005-2009, 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 static inline uint64_t next(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 pds_t *Pds_create(uint8_t *buf, int size)
  97 {
  98         pds_t *pds = malloc(sizeof(pds_t));
  99         if (pds == NULL)
 100                 Log_fatal("Out of memory");
 101         pds->data = buf;
 102         pds->offset = pds->overshoot = 0;
 103         pds->maxsize = size;
 104         pds->bOk = true;
 105         return pds;
 106 }
 107
 108 void Pds_free(pds_t *pds)
 109 {
 110         free(pds);
 111 }
 112
 113 void Pds_add_double(pds_t *pds, double value)
 114 {
 115         double64u_t u;
 116
 117         u.dval = value;
 118
 119         Pds_add_numval(pds, u.u64);
 120 }
 121
 122 double Pds_get_double(pds_t *pds)
 123 {
 124         double64u_t u;
 125         u.u64 = Pds_get_numval(pds);
 126         return u.dval;
 127 }
 128 void Pds_add_numval(pds_t *pds, const uint64_t value)
 129 {
 130         uint64_t i = value;
 131
 132         if ((i & 0x8000000000000000LL) && (~i < 0x100000000LL)) {
 133                 // Signed number.
 134                 i = ~i;
 135                 if (i <= 0x3) {
 136                         // Shortcase for -1 to -4
 137                         append_val(pds, 0xFC | i);
 138                         return;
 139                 } else {
 140                         append_val(pds, 0xF8);
 141                 }
 142         }
 143         if (i < 0x80) {
 144                 // Need top bit clear
 145                 append_val(pds, i);
 146         } else if (i < 0x4000) {
 147                 // Need top two bits clear
 148                 append_val(pds, (i >> 8) | 0x80);
 149                 append_val(pds, i & 0xFF);
 150         } else if (i < 0x200000) {
 151                 // Need top three bits clear
 152                 append_val(pds, (i >> 16) | 0xC0);
 153                 append_val(pds, (i >> 8) & 0xFF);
 154                 append_val(pds, i & 0xFF);
 155         } else if (i < 0x10000000) {
 156                 // Need top four bits clear
 157                 append_val(pds, (i >> 24) | 0xE0);
 158                 append_val(pds, (i >> 16) & 0xFF);
 159                 append_val(pds, (i >> 8) & 0xFF);
 160                 append_val(pds, i & 0xFF);
 161         } else if (i < 0x100000000LL) {
 162                 // It's a full 32-bit integer.
 163                 append_val(pds, 0xF0);
 164                 append_val(pds, (i >> 24) & 0xFF);
 165                 append_val(pds, (i >> 16) & 0xFF);
 166                 append_val(pds, (i >> 8) & 0xFF);
 167                 append_val(pds, i & 0xFF);
 168         } else {
 169                 // It's a 64-bit value.
 170                 append_val(pds, 0xF4);
 171                 append_val(pds, (i >> 56) & 0xFF);
 172                 append_val(pds, (i >> 48) & 0xFF);
 173                 append_val(pds, (i >> 40) & 0xFF);
 174                 append_val(pds, (i >> 32) & 0xFF);
 175                 append_val(pds, (i >> 24) & 0xFF);
 176                 append_val(pds, (i >> 16) & 0xFF);
 177                 append_val(pds, (i >> 8) & 0xFF);
 178                 append_val(pds, i & 0xFF);
 179         }
 180 }
 181
 182 uint64_t Pds_get_numval(pds_t *pds)
 183 {
 184         uint64_t i = 0;
 185         uint64_t v = next(pds);
 186
 187         if ((v & 0x80) == 0x00) {
 188                 i=(v & 0x7F);
 189         } else if ((v & 0xC0) == 0x80) {
 190                 i=(v & 0x3F) << 8 | next(pds);
 191         } else if ((v & 0xF0) == 0xF0) {
 192                 switch (v & 0xFC) {
 193                 case 0xF0:
 194                         i=next(pds) << 24 | next(pds) << 16 | next(pds) << 8 | next(pds);
 195                         break;
 196                 case 0xF4:
 197                         i=next(pds) << 56 | next(pds) << 48 | next(pds) << 40 | next(pds) << 32 | next(pds) << 24 | next(pds) << 16 | next(pds) << 8 | next(pds);
 198                         break;
 199                 case 0xF8:
 200                         i = Pds_get_numval(pds);
 201                         i = ~i;
 202                         break;
 203                 case 0xFC:
 204                         i=v & 0x03;
 205                         i = ~i;
 206                         break;
 207                 default:
 208                         //ok = false;
 209                         i = 0;
 210                         break;
 211                 }
 212         } else if ((v & 0xF0) == 0xE0) {
 213                 i=(v & 0x0F) << 24 | next(pds) << 16 | next(pds) << 8 | next(pds);
 214         } else if ((v & 0xE0) == 0xC0) {
 215                 i=(v & 0x1F) << 16 | next(pds) << 8 | next(pds);
 216         }
 217         return i;
 218 }
 219
 220 void Pds_add_string(pds_t *pds, const char *str)
 221 {
 222         Pds_append_data(pds, (uint8_t *)str, strlen(str));
 223 }
 224
 225 void Pds_get_string(pds_t *pds, char *str, int maxlen)
 226 {
 227         int len = Pds_get_numval(pds);
 228         if (len < maxlen) {
 229                 memcpy(str, &pds->data[pds->offset], len);
 230                 str[len] = '\0';
 231                 pds->offset += len;
 232         } else {
 233                 Log_warn("Too long string from network");
 234                 strcpy(str, "N/A");
 235         }
 236 }
 237
 238 int Pds_get_data(pds_t *pds, uint8_t *data, int maxlen)
 239 {
 240         int len = Pds_get_numval(pds);
 241         if (len < maxlen) {
 242                 memcpy(data, &pds->data[pds->offset], len);
 243                 pds->offset += len;
 244                 return len;
 245         } else {
 246                 Log_warn("Pds_get_data: Too much data from network");
 247                 return -1;
 248         }
 249 }