vg_msg.c

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