-/* write a buffer from msg, rang checked. */
-static void vg_msg_wbuf( vg_msg *msg, u8 *buf, u32 len ){
- if( msg->error != k_vg_msg_error_OK ) return;
- if( msg->cur.co+len > msg->max ){
- msg->error = k_vg_msg_error_overflow;
- return;
- }
-
- for( u32 i=0; i<len; i++ ){
- msg->buf[ msg->cur.co ++ ] = buf[i];
- }
-}
-
-/* read a buffer from msg, rang checked. */
-static void vg_msg_rbuf( vg_msg *msg, u8 *buf, u32 len ){
- if( msg->error != k_vg_msg_error_OK ) return;
- if( msg->cur.co+len > msg->max ){
- msg->error = k_vg_msg_error_overflow;
- return;
- }
-
- for( u32 i=0; i<len; i++ ){
- buf[i] = msg->buf[ msg->cur.co ++ ];
- }
-}
-
-/* write null terminated string to stream */
-static void vg_msg_wstr( vg_msg *msg, const char *str ){
- if( msg->error != k_vg_msg_error_OK ) return;
- for( u32 i=0;; i++ ){
- vg_msg_wbuf( msg, (u8[]){ str[i] }, 1 );
- if( !str[i] ) break;
- }
-}
-
-/* read null terminated string, range check and generate hash (djb2) */
-static const char *vg_msg_rstr( vg_msg *msg, u32 *djb2 ){
- if( msg->error != k_vg_msg_error_OK ) return 0;
-
- u32 hash = 5381, c;
- const char *str = (void *)(&msg->buf[ msg->cur.co ]);
-
- while( (c = msg->buf[ msg->cur.co ++ ]) ){
- if( msg->cur.co >= msg->max ){
- msg->error = k_vg_msg_error_overflow;
- return 0;
- }
- hash = ((hash << 5) + hash) + c; /* hash * 33 + c */
- }
-
- *djb2 = hash;
- return str;
-}
-
-/* begin a new frame in message stream */
-static void vg_msg_frame( vg_msg *msg, const char *name ){
- if( msg->error != k_vg_msg_error_OK ) return;
-
- msg->cur.depth ++;
- vg_msg_wbuf( msg, (u8[]){ k_vg_msg_frame }, 1 );
- vg_msg_wstr( msg, name );
-}
-
-/* end frame in message stream */
-static void vg_msg_end_frame( vg_msg *msg ){
- if( msg->error != k_vg_msg_error_OK ) return;
- if( !msg->cur.depth ){
- msg->error = k_vg_msg_error_unbalanced;
- return;
- }
- msg->cur.depth --;
- vg_msg_wbuf( msg, (u8[]){ k_vg_msg_endframe }, 1 );
-}
-
-/* write a KV string to stream */
-static void vg_msg_wkvstr( vg_msg *msg, const char *key, const char *value ){
- vg_msg_wbuf( msg, (u8[]){ k_vg_msg_kvstring }, 1 );
- vg_msg_wstr( msg, key );
- vg_msg_wstr( msg, value );
-}
-
-/* write a binary block to stream */
-static void vg_msg_wkvbin( vg_msg *msg, const char *key, u8 *bin, u32 len ){
- vg_msg_wbuf( msg, (u8[]){ k_vg_msg_kvbin }, 1 );
- vg_msg_wstr( msg, key );
- vg_msg_wbuf( msg, (u8 *)(&len), 4 );
- vg_msg_wbuf( msg, bin, len );
-}
-
-static u32 vg_msg_cmd_array_count( u8 code ){
- return ((code & k_vg_msg_array_count_bits)>>2) + 1;
-}
-
-static u32 vg_msg_cmd_type_size( u8 code ){
- return 0x1 << (code & k_vg_msg_type_size_bits);
-}
-
-/* get the byte count of a sized code */
-static u32 vg_msg_cmd_bytecount( u8 code ){
- return vg_msg_cmd_array_count( code ) * vg_msg_cmd_type_size( code );
-}
-
-static u8 vg_msg_count_bits( u32 count ){
- assert( (count <= 16) && count );
- return ((count-1)<<2);
-}
-
-/* write a sized type */
-static void vg_msg_wkvnum( vg_msg *msg, const char *key,
- u8 type, u8 count, void *data ){
- u8 code = type | vg_msg_count_bits(count);
-
- vg_msg_wbuf( msg, &code, 1 );
- vg_msg_wstr( msg, key );
- vg_msg_wbuf( msg, data, vg_msg_cmd_bytecount(code) );
-}
-
-#define _WRITE_KV_INTG_HELPER( TYPE ) \
-static void vg_msg_wkv##TYPE( vg_msg *msg, const char *key, TYPE v ){ \
- vg_msg_wkvnum( msg, key, k_vg_msg_##TYPE, 1, &v ); \
-}
-
-#define _WRITE_KV_VEC_HELPER( TYPE ) \
-static void vg_msg_wkv##TYPE( vg_msg *msg, const char *key, TYPE v ){ \
- vg_msg_wkvnum( msg, key, k_vg_msg_##TYPE, 1, v ); \
-}
-
-_WRITE_KV_INTG_HELPER( u8 )
-_WRITE_KV_INTG_HELPER( u16 )
-_WRITE_KV_INTG_HELPER( u32 )
-_WRITE_KV_INTG_HELPER( u64 )
-_WRITE_KV_INTG_HELPER( i8 )
-_WRITE_KV_INTG_HELPER( i16 )
-_WRITE_KV_INTG_HELPER( i32 )
-_WRITE_KV_INTG_HELPER( i64 )
-_WRITE_KV_INTG_HELPER( f32 )
-_WRITE_KV_INTG_HELPER( f64 )
-_WRITE_KV_VEC_HELPER( v2f )
-_WRITE_KV_VEC_HELPER( v3f )
-_WRITE_KV_VEC_HELPER( v4f )
-
-static void vg_msg_init( vg_msg *msg, u8 *buffer, u32 len ){
- msg->buf = buffer;
- msg->cur.co = 0;
- msg->cur.depth = 0;
- msg->error = k_vg_msg_error_OK;
- msg->max = len;
-}
-
-/*
- * The stream reading interface
- * -----------------------------------------------------------------------------
- */
-
-/* move the cursor through the next message. it will always read in the value or
- * create an error if it runs of the end of the stream. every possible command
- * must be handled in this function */
-static int vg_msg_next( vg_msg *msg, vg_msg_cmd *cmd ){
- vg_msg_rbuf( msg, &cmd->code, 1 );
- if( msg->error != k_vg_msg_error_OK ) return 0;
-
-#ifdef VG_MSG_V1_SUPPORT
- /* |sized| |count-1| |shift|
- * 0 1 0 0 0 1 0 0 0x44 (1 byte, float[2]. So, never used anyway)
- * converts to
- * 1 0 0 0 0 0 1 0 0x82
- */
- if( cmd->code == 0x44 ) cmd->code = 0x82;
-#endif
- cmd->key_djb2 = 0;
- if( msg->error != k_vg_msg_error_OK ) return 0;
-
- if( cmd->code == k_vg_msg_frame ){
- cmd->key = vg_msg_rstr( msg, &cmd->key_djb2 );
- msg->cur.depth ++;
- }
- else if( cmd->code == k_vg_msg_endframe ){
- if( !msg->cur.depth ){
- msg->error = k_vg_msg_error_unbalanced;
- return 0;
- }
- msg->cur.depth --;
- }
- else if( cmd->code >= k_vg_msg_kv ){
- cmd->key = vg_msg_rstr( msg, &cmd->key_djb2 );
- cmd->value_djb2 = 0;
-
- if( cmd->code & k_vg_msg_type_base_bits ){
- u32 bytes = vg_msg_cmd_bytecount( cmd->code );
- cmd->value = &msg->buf[ msg->cur.co ];
- msg->cur.co += bytes;
- }
- else if( cmd->code == k_vg_msg_kvstring ){
- cmd->value = vg_msg_rstr( msg, &cmd->value_djb2 );
- }
- else if( cmd->code == k_vg_msg_kvbin ){
- vg_msg_rbuf( msg, (u8 *)(&cmd->len), 4 );
- if( msg->error != k_vg_msg_error_OK )
- return 0;
- cmd->value = &msg->buf[ msg->cur.co ];
- msg->cur.co += cmd->len;
- }
- else
- msg->error = k_vg_msg_error_unhandled_cmd;
-
- if( msg->cur.co > msg->max )
- msg->error = k_vg_msg_error_overflow;
- }
- else
- msg->error = k_vg_msg_error_unhandled_cmd;
-
- if( msg->error != k_vg_msg_error_OK )
- return 0;
- else
- return 1;
-}
-
-/* move through the frame(and subframes) until we fall out of it */
-static int vg_msg_skip_frame( vg_msg *msg ){
- vg_msg_cmd cmd;
-
- u32 start_depth = msg->cur.depth;
- while( vg_msg_next( msg, &cmd ) )
- if( msg->cur.depth < start_depth )
- return 1;
- return 0;
-}
-
-/*
- * A more friendly but slower interface
- * -----------------------------------------------------------------------------
- */
-
-/* moves to a frame,
- * returns 0 if drops out of scope or ends.
- */
-static int vg_msg_seekframe( vg_msg *msg, const char *name ){
- vg_msg_cursor orig = msg->cur;
- vg_msg_cmd cmd;
- while( vg_msg_next( msg, &cmd ) ){
- if( msg->cur.depth < orig.depth ){
- msg->cur = orig;
- return 0;
- }
- if( msg->cur.depth != orig.depth+1 )
- continue;
- if( cmd.code == k_vg_msg_frame )
- if( !name || VG_STRDJB2_EQ( name, cmd.key, cmd.key_djb2 ) )
- return 1;
- }
-
- msg->cur = orig;
- return 0;
-}
-
-/*
- * Convert any type integral type to u64
- */
-static u64 vg_msg_cast_to_u64( const void *src, u8 src_base, u8 src_size ){
- if( src_base == k_vg_msg_float ){
- if( src_size == 4 ) return (u64)(*((f32*)src));
- else if( src_size == 8 ) return (u64)(*((f64*)src));
- else return 0;
- }
- else {
- u64 a = 0;
- memcpy( &a, src, src_size );
- return a;
- }
-}
-
-/*
- * Convert any integral type to i64
- */
-static i64 vg_msg_cast_to_i64( const void *src, u8 src_base, u8 src_size ){
- if( src_base == k_vg_msg_float ){
- if( src_size == 4 ) return (i64)(*((f32*)src));
- else if( src_size == 8 ) return (i64)(*((f64*)src));
- else return 0;
- }
- else {
- u64 a = 0;
- memcpy( &a, src, src_size );
-
- if( (src_base == k_vg_msg_signed) && (src_size != 8) ){
- /* extend sign bit */
- u64 sign_bit = 0x1llu << ((src_size*8)-1);
- if( a & sign_bit )
- a |= (0xffffffffffffffffllu << (64-__builtin_clzll( a )));
- }
-
- return *((i64*)&a);
- }
-}
-
-/*
- * Convert any integral type to f64
- */
-static f64 vg_msg_cast_to_f64( const void *src, u8 src_base, u8 src_size ){
- if( src_base == k_vg_msg_float ){
- if( src_size == 4 ) return (f64)(*((f32*)src));
- else if( src_size == 8 ) return *((f64*)src);
- else return 0.0;
- }
- else
- return (f64)vg_msg_cast_to_i64( src, src_base, src_size );
-}
-
-/*
- * Convert any full integral type code to another
- * Passing in non-integral codes is undefined
- */
-static void vg_msg_cast( const void *src, u8 src_code, void *dst, u8 dst_code ){
- if( src_code == dst_code ){
- memcpy( dst, src, vg_msg_cmd_bytecount( src_code ) );
- }
- else {
- u32 src_n = vg_msg_cmd_array_count( src_code ),
- dst_n = vg_msg_cmd_array_count( dst_code ),
- src_s = vg_msg_cmd_type_size( src_code ),
- dst_s = vg_msg_cmd_type_size( dst_code ),
- src_b = src_code & k_vg_msg_type_base_bits,
- dst_b = dst_code & k_vg_msg_type_base_bits;
-
- memset( dst, 0, dst_s * dst_n );
-
- for( u32 i=0; i<VG_MIN(src_n,dst_n); i ++ ){
- const void *ptr_s = src + i*src_s;
- void *ptr_d = dst + i*dst_s;
-
- if( dst_b == k_vg_msg_unsigned ){
- u64 a = vg_msg_cast_to_u64( ptr_s, src_b, src_s );
- if ( dst_s == 1 ) *((u8 *)ptr_d) = (u8 )a;
- else if( dst_s == 2 ) *((u16*)ptr_d) = (u16)a;
- else if( dst_s == 4 ) *((u32*)ptr_d) = (u32)a;
- else if( dst_s == 8 ) *((u64*)ptr_d) = a;
- }
- else if( dst_b == k_vg_msg_signed ){
- i64 a = vg_msg_cast_to_i64( ptr_s, src_b, src_s );
- if ( dst_s == 1 ) *((i8 *)ptr_d) = (i8 )a;
- else if( dst_s == 2 ) *((i16*)ptr_d) = (i16)a;
- else if( dst_s == 4 ) *((i32*)ptr_d) = (i32)a;
- else if( dst_s == 8 ) *((i64*)ptr_d) = a;
- }
- else {
- f64 a = vg_msg_cast_to_f64( ptr_s, src_b, src_s );
- if ( dst_s == 4 ) *((f32*)ptr_d) = (f32)a;
- else if( dst_s == 8 ) *((f64*)ptr_d) = a;
- }
- }
- }
-}
-
-/*
- * search in current level from cursor, to find kv cmd
- * returns 0 if not found
- * Cursor does not move
- * If found, the kv command is written to cmd
- */
-static int vg_msg_getkvcmd( vg_msg *msg, const char *key, vg_msg_cmd *cmd ){
- vg_msg_cursor orig = msg->cur;
- while( vg_msg_next( msg, cmd ) ){
- if( msg->cur.depth < orig.depth ){
- msg->cur = orig;
- return 0;
- }
- if( msg->cur.depth > orig.depth )
- continue;
- if( cmd->code > k_vg_msg_kv ){
- if( VG_STRDJB2_EQ( key, cmd->key, cmd->key_djb2 ) ){
- msg->cur = orig;
- return 1;
- }
- }
- }
- msg->error = k_vg_msg_error_OK;
- msg->cur = orig;
- return 0;
-}