vg_msg.c

   1 #include "vg_msg.h"
   2 #include "vg_platform.h"
   3 #include "vg_string.h"
   4 #include <string.h>
   5 #include <stdio.h>
   6
   7 /* write a buffer from msg, rang checked. */
   8 void vg_msg_wbuf( vg_msg *msg, u8 *buf, u32 len )
   9 {
  10    if( msg->error != k_vg_msg_error_OK ) return;
  11    if( msg->cur.co+len > msg->max ){
  12       msg->error = k_vg_msg_error_overflow;
  13       return;
  14    }
  15
  16    for( u32 i=0; i<len; i++ ){
  17       msg->buf[ msg->cur.co ++ ] = buf[i];
  18    }
  19 }
  20
  21 /* read a buffer from msg, rang checked. */
  22 void vg_msg_rbuf( vg_msg *msg, u8 *buf, u32 len )
  23 {
  24    if( msg->error != k_vg_msg_error_OK ) return;
  25    if( msg->cur.co+len > msg->max ){
  26       msg->error = k_vg_msg_error_overflow;
  27       return;
  28    }
  29
  30    for( u32 i=0; i<len; i++ ){
  31       buf[i] = msg->buf[ msg->cur.co ++ ];
  32    }
  33 }
  34
  35 /* write null terminated string to stream */
  36 void vg_msg_wstr( vg_msg *msg, const char *str )
  37 {
  38    if( msg->error != k_vg_msg_error_OK ) return;
  39    for( u32 i=0;; i++ ){
  40       vg_msg_wbuf( msg, (u8[]){ str[i] }, 1 );
  41       if( !str[i] ) break;
  42    }
  43 }
  44
  45 /* read null terminated string, range check and generate hash (djb2) */
  46 const char *vg_msg_rstr( vg_msg *msg, u32 *djb2 )
  47 {
  48    if( msg->error != k_vg_msg_error_OK ) return 0;
  49
  50    u32 hash = 5381, c;
  51    const char *str = (void *)(&msg->buf[ msg->cur.co ]);
  52
  53    while( (c = msg->buf[ msg->cur.co ++ ]) ){
  54       if( msg->cur.co >= msg->max ){
  55          msg->error = k_vg_msg_error_overflow;
  56          return 0;
  57       }
  58       hash = ((hash << 5) + hash) + c; /* hash * 33 + c */
  59    }
  60
  61    *djb2 = hash;
  62    return str;
  63 }
  64
  65 /* begin a new frame in message stream */
  66 void vg_msg_frame( vg_msg *msg, const char *name )
  67 {
  68    if( msg->error != k_vg_msg_error_OK ) return;
  69
  70    msg->cur.depth ++;
  71    vg_msg_wbuf( msg, (u8[]){ k_vg_msg_frame }, 1 );
  72    vg_msg_wstr( msg, name );
  73 }
  74
  75 /* end frame in message stream */
  76 void vg_msg_end_frame( vg_msg *msg )
  77 {
  78    if( msg->error != k_vg_msg_error_OK ) return;
  79    if( !msg->cur.depth ){
  80       msg->error = k_vg_msg_error_unbalanced;
  81       return;
  82    }
  83    msg->cur.depth --;
  84    vg_msg_wbuf( msg, (u8[]){ k_vg_msg_endframe }, 1 );
  85 }
  86
  87 /* write a KV string to stream */
  88 void vg_msg_wkvstr( vg_msg *msg, const char *key, const char *value )
  89 {
  90    vg_msg_wbuf( msg, (u8[]){ k_vg_msg_kvstring }, 1 );
  91    vg_msg_wstr( msg, key );
  92    vg_msg_wstr( msg, value );
  93 }
  94
  95 /* write a binary block to stream */
  96 void vg_msg_wkvbin( vg_msg *msg, const char *key, u8 *bin, u32 len )
  97 {
  98    vg_msg_wbuf( msg, (u8[]){ k_vg_msg_kvbin }, 1 );
  99    vg_msg_wstr( msg, key );
 100    vg_msg_wbuf( msg, (u8 *)(&len), 4 );
 101    vg_msg_wbuf( msg, bin, len );
 102 }
 103
 104 u32 vg_msg_cmd_array_count( u8 code )
 105 {
 106    return ((code & k_vg_msg_array_count_bits)>>2) + 1;
 107 }
 108
 109 u32 vg_msg_cmd_type_size( u8 code )
 110 {
 111    return 0x1 << (code & k_vg_msg_type_size_bits);
 112 }
 113
 114 /* get the byte count of a sized code */
 115 u32 vg_msg_cmd_bytecount( u8 code )
 116 {
 117    return vg_msg_cmd_array_count( code ) * vg_msg_cmd_type_size( code );
 118 }
 119
 120 u8 vg_msg_count_bits( u32 count )
 121 {
 122    if( count > 16 ) vg_fatal_error( "Too large\n" );
 123    return ((count-1)<<2);
 124 }
 125
 126 /* write a sized type */
 127 void vg_msg_wkvnum( vg_msg *msg, const char *key,
 128                     u8 type, u8 count, void *data )
 129 {
 130    u8 code = type | vg_msg_count_bits(count);
 131
 132    vg_msg_wbuf( msg, &code, 1 );
 133    vg_msg_wstr( msg, key );
 134    vg_msg_wbuf( msg, data, vg_msg_cmd_bytecount(code) );
 135 }
 136
 137 void vg_msg_init( vg_msg *msg, u8 *buffer, u32 len )
 138 {
 139    msg->buf = buffer;
 140    msg->cur.co = 0;
 141    msg->cur.depth = 0;
 142    msg->error = k_vg_msg_error_OK;
 143    msg->max = len;
 144 }
 145
 146 /*
 147  * The stream reading interface
 148  * -----------------------------------------------------------------------------
 149  */
 150
 151 /* move the cursor through the next message. it will always read in the value or
 152  * create an error if it runs of the end of the stream. every possible command
 153  * must be handled in this function */
 154 int vg_msg_next( vg_msg *msg, vg_msg_cmd *cmd )
 155 {
 156    vg_msg_rbuf( msg, &cmd->code, 1 );
 157    if( msg->error != k_vg_msg_error_OK ) return 0;
 158
 159 #ifdef VG_MSG_V1_SUPPORT
 160   /* |sized|  |count-1| |shift|
 161    * 0     1  0  0  0 1 0     0  0x44 (1 byte, float[2]. So, never used anyway)
 162    * converts to
 163    * 1     0  0  0  0 0 1     0  0x82
 164    */
 165    if( cmd->code == 0x44 ) cmd->code = 0x82;
 166 #endif
 167    cmd->key_djb2 = 0;
 168    if( msg->error != k_vg_msg_error_OK ) return 0;
 169
 170    if( cmd->code == k_vg_msg_frame ){
 171       cmd->key = vg_msg_rstr( msg, &cmd->key_djb2 );
 172       msg->cur.depth ++;
 173    }
 174    else if( cmd->code == k_vg_msg_endframe ){
 175       if( !msg->cur.depth ){
 176          msg->error = k_vg_msg_error_unbalanced;
 177          return 0;
 178       }
 179       msg->cur.depth --;
 180    }
 181    else if( cmd->code >= k_vg_msg_kv ){
 182       cmd->key = vg_msg_rstr( msg, &cmd->key_djb2 );
 183       cmd->value_djb2 = 0;
 184
 185       if( cmd->code & k_vg_msg_type_base_bits ){
 186          u32 bytes = vg_msg_cmd_bytecount( cmd->code );
 187          cmd->value = &msg->buf[ msg->cur.co ];
 188          msg->cur.co += bytes;
 189       }
 190       else if( cmd->code == k_vg_msg_kvstring ){
 191          cmd->value = vg_msg_rstr( msg, &cmd->value_djb2 );
 192       }
 193       else if( cmd->code == k_vg_msg_kvbin ){
 194          vg_msg_rbuf( msg, (u8 *)(&cmd->len), 4 );
 195          if( msg->error != k_vg_msg_error_OK )
 196             return 0;
 197          cmd->value = &msg->buf[ msg->cur.co ];
 198          msg->cur.co += cmd->len;
 199       }
 200       else
 201          msg->error = k_vg_msg_error_unhandled_cmd;
 202
 203       if( msg->cur.co > msg->max )
 204          msg->error = k_vg_msg_error_overflow;
 205    }
 206    else
 207       msg->error = k_vg_msg_error_unhandled_cmd;
 208
 209    if( msg->error != k_vg_msg_error_OK )
 210       return 0;
 211    else
 212       return 1;
 213 }
 214
 215 /* move through the frame(and subframes) until we fall out of it */
 216 int vg_msg_skip_frame( vg_msg *msg )
 217 {
 218    vg_msg_cmd cmd;
 219
 220    u32 start_depth = msg->cur.depth;
 221    while( vg_msg_next( msg, &cmd ) )
 222       if( msg->cur.depth < start_depth )
 223          return 1;
 224    return 0;
 225 }
 226
 227 /*
 228  * A more friendly but slower interface
 229  * -----------------------------------------------------------------------------
 230  */
 231
 232 /* moves to a frame,
 233  * returns 0 if drops out of scope or ends.
 234  */
 235 int vg_msg_seekframe( vg_msg *msg, const char *name )
 236 {
 237    vg_msg_cursor orig = msg->cur;
 238    vg_msg_cmd cmd;
 239    while( vg_msg_next( msg, &cmd ) ){
 240       if( msg->cur.depth < orig.depth ){
 241          msg->cur = orig;
 242          return 0;
 243       }
 244       if( msg->cur.depth != orig.depth+1 )
 245          continue;
 246       if( cmd.code == k_vg_msg_frame )
 247          if( !name || VG_STRDJB2_EQ( name, cmd.key, cmd.key_djb2 ) )
 248             return 1;
 249    }
 250
 251    msg->cur = orig;
 252    return 0;
 253 }
 254
 255 /*
 256  * Convert any type integral type to u64
 257  */
 258 u64 vg_msg_cast_to_u64( const void *src, u8 src_base, u8 src_size )
 259 {
 260    if( src_base == k_vg_msg_float ){
 261       if( src_size == 4 )      return (u64)(*((f32*)src));
 262       else if( src_size == 8 ) return (u64)(*((f64*)src));
 263       else                     return 0;
 264    }
 265    else {
 266       u64 a = 0;
 267       memcpy( &a, src, src_size );
 268       return a;
 269    }
 270 }
 271
 272 /*
 273  * Convert any integral type to i64
 274  */
 275 i64 vg_msg_cast_to_i64( const void *src, u8 src_base, u8 src_size )
 276 {
 277    if( src_base == k_vg_msg_float ){
 278       if( src_size == 4 )      return (i64)(*((f32*)src));
 279       else if( src_size == 8 ) return (i64)(*((f64*)src));
 280       else                     return 0;
 281    }
 282    else {
 283       u64 a = 0;
 284       memcpy( &a, src, src_size );
 285
 286       if( (src_base == k_vg_msg_signed) && (src_size != 8) ){
 287          /* extend sign bit */
 288          u64 sign_bit = 0x1llu << ((src_size*8)-1);
 289          if( a & sign_bit )
 290             a |= (0xffffffffffffffffllu << (64-__builtin_clzll( a )));
 291       }
 292
 293       return *((i64*)&a);
 294    }
 295 }
 296
 297 /*
 298  * Convert any integral type to f64
 299  */
 300 f64 vg_msg_cast_to_f64( const void *src, u8 src_base, u8 src_size )
 301 {
 302    if( src_base == k_vg_msg_float ){
 303       if( src_size == 4 )      return (f64)(*((f32*)src));
 304       else if( src_size == 8 ) return *((f64*)src);
 305       else                     return 0.0;
 306    }
 307    else
 308       return (f64)vg_msg_cast_to_i64( src, src_base, src_size );
 309 }
 310
 311 /*
 312  * Convert any full integral type code to another
 313  * Passing in non-integral codes is undefined
 314  */
 315 void vg_msg_cast( const void *src, u8 src_code, void *dst, u8 dst_code )
 316 {
 317    if( src_code == dst_code ){
 318       memcpy( dst, src, vg_msg_cmd_bytecount( src_code ) );
 319    }
 320    else {
 321       u32 src_n = vg_msg_cmd_array_count( src_code ),
 322           dst_n = vg_msg_cmd_array_count( dst_code ),
 323           src_s = vg_msg_cmd_type_size( src_code ),
 324           dst_s = vg_msg_cmd_type_size( dst_code ),
 325           src_b = src_code & k_vg_msg_type_base_bits,
 326           dst_b = dst_code & k_vg_msg_type_base_bits;
 327
 328       memset( dst, 0, dst_s * dst_n );
 329
 330       for( u32 i=0; i<VG_MIN(src_n,dst_n); i ++ ){
 331          const void *ptr_s = src + i*src_s;
 332          void *ptr_d = dst + i*dst_s;
 333
 334          if( dst_b == k_vg_msg_unsigned ){
 335             u64 a = vg_msg_cast_to_u64( ptr_s, src_b, src_s );
 336             if     ( dst_s == 1 ) *((u8 *)ptr_d) = (u8 )a;
 337             else if( dst_s == 2 ) *((u16*)ptr_d) = (u16)a;
 338             else if( dst_s == 4 ) *((u32*)ptr_d) = (u32)a;
 339             else if( dst_s == 8 ) *((u64*)ptr_d) = a;
 340          }
 341          else if( dst_b == k_vg_msg_signed ){
 342             i64 a = vg_msg_cast_to_i64( ptr_s, src_b, src_s );
 343             if     ( dst_s == 1 ) *((i8 *)ptr_d) = (i8 )a;
 344             else if( dst_s == 2 ) *((i16*)ptr_d) = (i16)a;
 345             else if( dst_s == 4 ) *((i32*)ptr_d) = (i32)a;
 346             else if( dst_s == 8 ) *((i64*)ptr_d) = a;
 347          }
 348          else {
 349             f64 a = vg_msg_cast_to_f64( ptr_s, src_b, src_s );
 350             if     ( dst_s == 4 ) *((f32*)ptr_d) = (f32)a;
 351             else if( dst_s == 8 ) *((f64*)ptr_d) = a;
 352          }
 353       }
 354    }
 355 }
 356
 357 /*
 358  * search in current level from cursor, to find kv cmd
 359  * returns 0 if not found
 360  * Cursor does not move
 361  * If found, the kv command is written to cmd
 362  */
 363 int vg_msg_getkvcmd( vg_msg *msg, const char *key, vg_msg_cmd *cmd )
 364 {
 365    vg_msg_cursor orig = msg->cur;
 366    while( vg_msg_next( msg, cmd ) ){
 367       if( msg->cur.depth < orig.depth ){
 368          msg->cur = orig;
 369          return 0;
 370       }
 371       if( msg->cur.depth > orig.depth )
 372          continue;
 373       if( cmd->code > k_vg_msg_kv ){
 374          if( VG_STRDJB2_EQ( key, cmd->key, cmd->key_djb2 ) ){
 375             msg->cur = orig;
 376             return 1;
 377          }
 378       }
 379    }
 380    msg->error = k_vg_msg_error_OK;
 381    msg->cur = orig;
 382    return 0;
 383 }
 384
 385 /*
 386  * Read a integral KV out to dst, and perform conversion if needed
 387  * dst is always defined, if its not found its set to 0
 388  */
 389 int vg_msg_getkvintg( vg_msg *msg, const char *key, u8 type, void *dst,
 390                       void *default_value )
 391 {
 392    vg_msg_cmd cmd;
 393    if( vg_msg_getkvcmd( msg, key, &cmd ) )
 394    {
 395       vg_msg_cast( cmd.value, cmd.code, dst, type );
 396       return 1;
 397    }
 398    else
 399    {
 400       if( default_value )
 401          memcpy( dst, default_value, vg_msg_cmd_bytecount(type) );
 402       else
 403          memset( dst, 0, vg_msg_cmd_bytecount(type) );
 404
 405       return 0;
 406    }
 407 }
 408
 409 /* helper for reading string kvs. returns NULL if not found */
 410 const char *vg_msg_getkvstr( vg_msg *msg, const char *key )
 411 {
 412    vg_msg_cmd cmd;
 413    if( vg_msg_getkvcmd( msg, key, &cmd ) )
 414       return cmd.value;
 415    else
 416       return NULL;
 417 }
 418
 419 int vg_msg_getkvvecf( vg_msg *msg, const char *key, u8 type,
 420                       void *v, void *default_value )
 421 {
 422    vg_msg_cmd cmd;
 423    if( vg_msg_getkvcmd( msg, key, &cmd ) )
 424    {
 425       vg_msg_cast( cmd.value, cmd.code, v, type );
 426       return 1;
 427    }
 428    else if( default_value )
 429       vg_msg_cast( default_value, type, v, type );
 430
 431    return 0;
 432 }
 433
 434
 435 /* debug the thing */
 436 void vg_msg_print( vg_msg *msg, u32 len )
 437 {
 438    if( msg->error != k_vg_msg_error_OK ){
 439       printf( "Message contains errors\n" );
 440       return;
 441    }
 442
 443    vg_msg b;
 444    vg_msg_init( &b, msg->buf, len );
 445
 446    vg_msg_cmd cmd;
 447    while( vg_msg_next( &b, &cmd ) ){
 448       if( cmd.code == k_vg_msg_frame ){
 449          for( u32 i=0; i<b.cur.depth-1; i++ ) printf( "  " );
 450          printf( "'%s'{\n", cmd.key );
 451       }
 452       else {
 453          for( u32 i=0; i<b.cur.depth; i++ ) printf( "  " );
 454
 455          if( cmd.code == k_vg_msg_endframe )
 456             printf( "}\n" );
 457          else if( cmd.code == k_vg_msg_kvstring )
 458             printf( "'%s': '%s'\n", cmd.key, (char *)cmd.value );
 459          else if( cmd.code == k_vg_msg_kvbin )
 460             printf( "'%s': <binary data> (%u bytes)\n", cmd.key, cmd.len );
 461          else {
 462             u32 base = cmd.code & k_vg_msg_type_base_bits,
 463                 count = vg_msg_cmd_array_count( cmd.code ),
 464                 size = vg_msg_cmd_type_size( cmd.code );
 465
 466             printf( "'%s': ", cmd.key );
 467
 468             if( count > 1 ) printf( "{ " );
 469
 470             for( u32 i=0; i<count; i++ ){
 471                const void *p = cmd.value + size*i;
 472
 473                if( base == k_vg_msg_unsigned ){
 474                   printf(
 475 #ifdef _WIN32
 476                   "%llu"
 477 #else
 478                   "%lu"
 479 #endif
 480                   , vg_msg_cast_to_u64( p, base, size ) );
 481                }
 482                else if( base == k_vg_msg_signed ){
 483                   printf(
 484 #ifdef _WIN32
 485                   "%lld"
 486 #else
 487                   "%ld"
 488 #endif
 489                   , vg_msg_cast_to_i64( p, base, size ) );
 490                }
 491                else
 492                   printf( "%f", vg_msg_cast_to_f64( p, base, size ));
 493
 494                if( i+1<count ) printf(", ");
 495             }
 496
 497             if( count > 1 ) printf( " }" );
 498             printf( "\n" );
 499          }
 500       }
 501    }
 502 }