src/cdr.c

   1 /*
   2  *  $Id: cdr.c,v 0.0.0.1                2004/11/26
   3  *
   4  *                      CDR codeing
   5  *
   6  *  -------------------------------------------------------------------
   7  *                                ORTE
   8  *                      Open Real-Time Ethernet
   9  *
  10  *                      Copyright (C) 2001-2006
  11  *  Department of Control Engineering FEE CTU Prague, Czech Republic
  12  *                      http://dce.felk.cvut.cz
  13  *                      http://www.ocera.org
  14  *
  15  *  Author:              Petr Smolik    petr.smolik@wo.cz
  16  *  Advisor:             Pavel Pisa
  17  *  Project Responsible: Zdenek Hanzalek
  18  *
  19  *  Modified for FORB by Michal Sojka <sojkam1@fel.cvut.cz>, 2008
  20  *  --------------------------------------------------------------------
  21  *
  22  *  This program is free software; you can redistribute it and/or modify
  23  *  it under the terms of the GNU General Public License as published by
  24  *  the Free Software Foundation; either version 2 of the License, or
  25  *  (at your option) any later version.
  26  *
  27  *  This program is distributed in the hope that it will be useful,
  28  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  29  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  30  *  GNU General Public License for more details.
  31  *
  32  *  Original of source was from ORBit: A CORBA v2.2 ORB
  33  *
  34  */
  35
  36 #include <forb/cdr.h>
  37 #include "forb.h"
  38 #include "forb_endian.h"
  39 #include <string.h>
  40
  41 #define ALIGN_ADDRESS(this, boundary) \
  42   ((void*)((( ((unsigned long)(this)) + (((unsigned long)(boundary)) -1)) & (~(((unsigned long)(boundary))-1)))))
  43
  44 #define CDR_GROW_AMOUNT 128
  45
  46 CORBA_boolean
  47 CDR_buffer_init(CDR_Codec *codec, const unsigned int size, const unsigned int first)
  48 {
  49
  50         if(codec->release_buffer) {
  51                 forb_free(codec->buffer);
  52         }
  53
  54         if (first > size) {
  55                 return CORBA_FALSE;
  56         }
  57         codec->buffer=(CORBA_octet *)forb_malloc(size);
  58         if (!codec->buffer) {
  59                 return CORBA_FALSE;
  60         }
  61         codec->buf_len=size;
  62         codec->wptr_max=size;
  63         CDR_buffer_reset(codec, first);
  64         codec->release_buffer=CORBA_TRUE;
  65
  66         return CORBA_TRUE;
  67 }
  68
  69 CORBA_boolean
  70 CDR_buffer_reset(CDR_Codec *codec, const unsigned int first)
  71 {
  72         if (first > codec->buf_len) {
  73                 return CORBA_FALSE;
  74         }
  75         codec->rptr = codec->wptr = codec->wptr_last = first;
  76         return CORBA_TRUE;
  77 }
  78
  79 CORBA_boolean
  80 CDR_buffer_prepend(CDR_Codec *codec, const unsigned int len)
  81 {
  82         if (codec->rptr < len) {
  83                 return CORBA_FALSE;
  84         }
  85         codec->rptr -= len;
  86
  87         codec->wptr = codec->rptr;
  88
  89         return CORBA_TRUE;
  90 }
  91
  92 /*
  93 CORBA_boolean
  94 CDR_buffer_grow(CDR_Codec *codec, const unsigned int growth)
  95 {
  96         unsigned int real_growth,quot;
  97
  98         if(codec->release_buffer) {
  99                 quot = growth / CDR_GROW_AMOUNT;
 100                 real_growth=CDR_GROW_AMOUNT * (quot+1);
 101
 102                 codec->buffer=(CORBA_octet *)REALLOC(codec->buffer,
 103                                                      codec->buf_len
 104                                                      +real_growth);
 105         }
 106
 107         return CORBA_TRUE;
 108 }
 109 */
 110 CORBA_boolean
 111 CDR_buffer_puts(CDR_Codec *codec, const void *data, const unsigned int len)
 112 {
 113         if(codec->wptr+len > codec->wptr_max) {
 114                 return CORBA_FALSE;
 115         }
 116
 117         memcpy(&codec->buffer[codec->wptr], data, len);
 118         codec->wptr+=len;
 119         if (codec->wptr > codec->wptr_last) codec->wptr_last = codec->wptr;
 120
 121         return CORBA_TRUE;
 122 }
 123
 124 CORBA_boolean
 125 CDR_buffer_gets(CDR_Codec *codec, void *dest, const unsigned int len)
 126 {
 127         if(codec->rptr+len > codec->buf_len) {
 128                 return CORBA_FALSE;
 129         }
 130
 131         memcpy(dest, &codec->buffer[codec->rptr], len);
 132         codec->rptr+=len;
 133
 134         return CORBA_TRUE;
 135 }
 136
 137 CORBA_boolean
 138 CDR_buffer_put(CDR_Codec *codec, void *datum)
 139 {
 140         if(codec->wptr+1 > codec->wptr_max) {
 141                 return(CORBA_FALSE);
 142         }
 143
 144         codec->buffer[codec->wptr++]=*(unsigned char *)datum;
 145         if (codec->wptr > codec->wptr_last) codec->wptr_last = codec->wptr;
 146         return CORBA_TRUE;
 147 }
 148
 149 CORBA_boolean
 150 CDR_buffer_get(CDR_Codec *codec, void *dest)
 151 {
 152         if(codec->rptr+1 > codec->buf_len) {
 153                 return(CORBA_FALSE);
 154         }
 155
 156         *(CORBA_octet *)dest=codec->buffer[codec->rptr++];
 157         return CORBA_TRUE;
 158 }
 159
 160 #define CDR_buffer_put2(codec, datum) CDR_buffer_putn(codec, datum, 2)
 161 #define CDR_buffer_put4(codec, datum) CDR_buffer_putn(codec, datum, 4)
 162 #define CDR_buffer_put8(codec, datum) CDR_buffer_putn(codec, datum, 8)
 163 #define CDR_buffer_put16(codec, datum) CDR_buffer_putn(codec, datum, 16)
 164 #define CDR_buffer_get2(codec, dest) CDR_buffer_getn(codec, dest, 2)
 165 #define CDR_buffer_get4(codec, dest) CDR_buffer_getn(codec, dest, 4)
 166 #define CDR_buffer_get8(codec, dest) CDR_buffer_getn(codec, dest, 8)
 167 #define CDR_buffer_get16(codec, dest) CDR_buffer_getn(codec, dest, 16)
 168
 169 static CORBA_boolean
 170 CDR_buffer_getn(CDR_Codec *codec, void *dest, int bsize)
 171 {
 172         codec->rptr = (unsigned long)ALIGN_ADDRESS(codec->rptr, bsize);
 173         if(codec->host_endian==codec->data_endian)
 174                 memcpy(dest, codec->buffer + codec->rptr, bsize);
 175         else
 176                 forb_byteswap(dest, codec->buffer + codec->rptr, bsize);
 177         codec->rptr += bsize;
 178
 179         return CORBA_TRUE;
 180 }
 181
 182 static CORBA_boolean
 183 CDR_buffer_putn(CDR_Codec *codec, void *datum, int bsize)
 184 {
 185         unsigned long forward,i;
 186
 187         forward = (unsigned long)ALIGN_ADDRESS(codec->wptr, bsize);
 188         if (forward+bsize > codec->wptr_max) {
 189                 return CORBA_FALSE;
 190         }
 191
 192         i = codec->wptr;
 193         while(forward > i)
 194                 codec->buffer[i++] = '\0';
 195
 196         codec->wptr = forward;
 197         if(codec->host_endian==codec->data_endian)
 198                 memcpy(codec->buffer + codec->wptr, datum, bsize);
 199         else
 200                 forb_byteswap(codec->buffer + codec->wptr, datum, bsize);
 201         codec->wptr += bsize;
 202         if (codec->wptr > codec->wptr_last) codec->wptr_last = codec->wptr;
 203
 204         return CORBA_TRUE;
 205 }
 206
 207 CORBA_boolean CDR_put_align(CDR_Codec *codec, unsigned bsize)
 208 {
 209         unsigned long forward,i;
 210
 211         forward = (unsigned long)ALIGN_ADDRESS(codec->wptr, bsize);
 212         if (forward+bsize > codec->wptr_max) {
 213                 return CORBA_FALSE;
 214         }
 215
 216         i = codec->wptr;
 217         while(forward > i)
 218                 codec->buffer[i++] = '\0';
 219
 220         codec->wptr = forward;
 221         if (codec->wptr > codec->wptr_last) codec->wptr_last = codec->wptr;
 222         return CORBA_TRUE;
 223 }
 224
 225 CORBA_boolean CDR_get_align(CDR_Codec *codec, unsigned bsize)
 226 {
 227         codec->rptr = (unsigned long)ALIGN_ADDRESS(codec->rptr, bsize);
 228         return codec->rptr <= codec->wptr_max;
 229 }
 230
 231 #define CDR_swap2(d,s) forb_byteswap((d), (s), 2)
 232 #define CDR_swap4(d,s) forb_byteswap((d), (s), 4)
 233 #define CDR_swap8(d,s) forb_byteswap((d), (s), 8)
 234 #define CDR_swap16(d,s) forb_byteswap((d), (s), 16)
 235
 236 inline CORBA_boolean
 237 CDR_put_short(CDR_Codec *codec, CORBA_short s)
 238 {
 239         return CDR_buffer_put2(codec, &s);
 240 }
 241
 242 inline CORBA_boolean
 243 CDR_get_short(CDR_Codec *codec, CORBA_short *s)
 244 {
 245         return CDR_buffer_get2(codec, s);
 246 }
 247
 248 inline CORBA_boolean
 249 CDR_put_ushort(CDR_Codec *codec, CORBA_unsigned_short us)
 250 {
 251         return CDR_buffer_put2(codec, &us);
 252 }
 253
 254 inline CORBA_boolean
 255 CDR_get_ushort(CDR_Codec *codec, CORBA_unsigned_short *us)
 256 {
 257         return CDR_buffer_get2(codec, us);
 258 }
 259
 260 inline CORBA_boolean
 261 CDR_put_long(CDR_Codec *codec, CORBA_long l)
 262 {
 263         return CDR_buffer_put4(codec, &l);
 264 }
 265
 266 inline CORBA_boolean
 267 CDR_get_long(CDR_Codec *codec, CORBA_long *l)
 268 {
 269         return CDR_buffer_get4(codec, l);
 270 }
 271
 272 inline CORBA_boolean
 273 CDR_put_ulong(CDR_Codec *codec, CORBA_unsigned_long ul)
 274 {
 275         return CDR_buffer_put4(codec, &ul);
 276 }
 277
 278 inline CORBA_boolean
 279 CDR_get_ulong(CDR_Codec *codec, CORBA_unsigned_long *ul)
 280 {
 281         return CDR_buffer_get4(codec, ul);
 282 }
 283
 284 //#ifdef HAVE_CORBA_LONG_LONG
 285 inline CORBA_boolean
 286 CDR_get_long_long(CDR_Codec *codec, CORBA_long_long *ul)
 287 {
 288         return CDR_buffer_get8(codec, ul);
 289 }
 290
 291 inline CORBA_boolean
 292 CDR_put_long_long(CDR_Codec *codec, CORBA_long_long ll)
 293 {
 294         return CDR_buffer_put8(codec, &ll);
 295 }
 296
 297 inline CORBA_boolean
 298 CDR_put_ulong_long(CDR_Codec *codec, CORBA_unsigned_long_long ll)
 299 {
 300         return CDR_buffer_put8(codec, &ll);
 301 }
 302
 303 inline CORBA_boolean
 304 CDR_get_ulong_long(CDR_Codec *codec, CORBA_unsigned_long_long *ull)
 305 {
 306         return CDR_buffer_get8(codec, ull);
 307 }
 308 //#endif /* HAVE_CORBA_LONG_LONG */
 309
 310 inline CORBA_boolean
 311 CDR_put_float(CDR_Codec *codec, CORBA_float f)
 312 {
 313         return CDR_buffer_put4(codec, &f);
 314 }
 315
 316 inline CORBA_boolean
 317 CDR_get_float(CDR_Codec *codec, CORBA_float *f)
 318 {
 319         return CDR_buffer_get4(codec, f);
 320 }
 321
 322 inline CORBA_boolean
 323 CDR_put_double(CDR_Codec *codec, CORBA_double d)
 324 {
 325         return CDR_buffer_put8(codec, &d);
 326 }
 327
 328 inline CORBA_boolean
 329 CDR_get_double(CDR_Codec *codec, CORBA_double *d)
 330 {
 331         return CDR_buffer_get8(codec, d);
 332 }
 333
 334 inline CORBA_boolean
 335 CDR_put_long_double(CDR_Codec *codec, CORBA_long_double ld)
 336 {
 337         return CDR_buffer_put16(codec, &ld);
 338 }
 339
 340 inline CORBA_boolean
 341 CDR_put_octet(CDR_Codec *codec, CORBA_octet datum)
 342 {
 343         return CDR_buffer_put(codec, &datum);
 344 }
 345
 346 inline CORBA_boolean
 347 CDR_get_octet(CDR_Codec *codec, CORBA_octet *datum)
 348 {
 349         return(CDR_buffer_get(codec, datum));
 350 }
 351
 352 inline CORBA_boolean
 353 CDR_put_octets(CDR_Codec *codec, void *data, unsigned long len)
 354 {
 355         return CDR_buffer_puts(codec, data, len);
 356 }
 357
 358 inline CORBA_boolean
 359 CDR_put_char(CDR_Codec *codec, CORBA_char c)
 360 {
 361         return CDR_buffer_put(codec, &c);
 362 }
 363
 364 inline CORBA_boolean
 365 CDR_get_char(CDR_Codec *codec, CORBA_char *c)
 366 {
 367         return CDR_buffer_get(codec, c);
 368 }
 369
 370 inline CORBA_boolean
 371 CDR_put_boolean(CDR_Codec *codec, CORBA_boolean datum)
 372 {
 373         datum = datum&&1;
 374         return CDR_buffer_put(codec, &datum);
 375 }
 376
 377 inline CORBA_boolean
 378 CDR_get_boolean(CDR_Codec *codec, CORBA_boolean *b)
 379 {
 380         return CDR_buffer_get(codec, b);
 381 }
 382
 383 CORBA_boolean
 384 CDR_put_string(CDR_Codec *codec, const CORBA_char *str)
 385 {
 386         unsigned int len;
 387
 388         len=strlen(str)+1;
 389
 390         if (CDR_put_ulong(codec, len)==CORBA_FALSE) return -1;
 391         return CDR_buffer_puts(codec, str, len);
 392 }
 393
 394 CORBA_boolean
 395 CDR_get_string_static(CDR_Codec *codec,CORBA_char **str)
 396 {
 397         CORBA_unsigned_long len;
 398
 399         if(CDR_get_ulong(codec, (CORBA_unsigned_long *)&len)==CORBA_FALSE)
 400                 return CORBA_FALSE;
 401
 402         if((codec->rptr + len) > codec->buf_len)
 403                 return CORBA_FALSE;
 404
 405         *str = ((CORBA_char *)codec->buffer) + codec->rptr;
 406
 407         codec->rptr += len;
 408
 409         return CORBA_TRUE;
 410 }
 411
 412 CORBA_boolean
 413 CDR_get_string(CDR_Codec *codec, CORBA_char **str)
 414 {
 415         CORBA_unsigned_long len;
 416
 417         if(CDR_get_ulong(codec, (CORBA_unsigned_long *)&len)==CORBA_FALSE)
 418                 return(CORBA_FALSE);
 419
 420         if(len==0)
 421                 return(CORBA_FALSE);
 422
 423         *str=forb_malloc(len);
 424
 425         if(CDR_buffer_gets(codec, *str, len)==CORBA_FALSE) {
 426                 forb_free(*str);
 427                 return(CORBA_FALSE);
 428         }
 429
 430         if((*str)[len-1]!='\0') {
 431                 (*str)[len-1]='\0';
 432         }
 433
 434         return(CORBA_TRUE);
 435 }
 436
 437 CORBA_boolean
 438 CDR_get_string_buff(CDR_Codec *codec, CORBA_char *str)
 439 {
 440         CORBA_unsigned_long len;
 441
 442         if(CDR_get_ulong(codec, (CORBA_unsigned_long *)&len)==CORBA_FALSE)
 443                 return(CORBA_FALSE);
 444
 445         if(len==0)
 446                 return(CORBA_FALSE);
 447
 448         if(CDR_buffer_gets(codec, str, len)==CORBA_FALSE) {
 449                 return(CORBA_FALSE);
 450         }
 451
 452         if(str[len-1]!='\0') {
 453                 str[len-1]='\0';
 454         }
 455
 456         return(CORBA_TRUE);
 457 }
 458
 459 CORBA_boolean
 460 CDR_get_seq_begin(CDR_Codec *codec, CORBA_unsigned_long *ul)
 461 {
 462         return(CDR_get_ulong(codec, (CORBA_unsigned_long *)ul));
 463 }
 464
 465 CDR_Codec *
 466 CDR_codec_init_static(CDR_Codec *codec, forb_orb orb)
 467 {
 468         memset(codec, 0, sizeof(CDR_Codec));
 469
 470         codec->host_endian = FLAG_ENDIANNESS;
 471         codec->data_endian = FLAG_ENDIANNESS;
 472         codec->orb = orb;
 473
 474         return codec;
 475 }
 476
 477 CDR_Codec *
 478 CDR_codec_init(forb_orb orb)
 479 {
 480         CDR_Codec *c;
 481
 482         c=forb_malloc(sizeof(CDR_Codec));
 483         if (c) {
 484                 CDR_codec_init_static(c, orb);
 485         }
 486
 487         return(c);
 488 }
 489
 490 void
 491 CDR_codec_release_buffer(CDR_Codec *codec)
 492 {
 493         if(codec->release_buffer) {
 494                 forb_free(codec->buffer);
 495                 codec->release_buffer = CORBA_FALSE;
 496         }
 497 }
 498
 499
 500 void
 501 CDR_codec_free(CDR_Codec *codec)
 502 {
 503         CDR_codec_release_buffer(codec);
 504         forb_free(codec);
 505 }