--- /dev/null
+#include "vg_msg.h"
+#include "vg_platform.h"
+
+/* write a buffer from msg, rang checked. */
+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. */
+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 */
+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) */
+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 */
+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 */
+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 */
+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 */
+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 );
+}
+
+u32 vg_msg_cmd_array_count( u8 code )
+{
+ return ((code & k_vg_msg_array_count_bits)>>2) + 1;
+}
+
+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 */
+u32 vg_msg_cmd_bytecount( u8 code )
+{
+ return vg_msg_cmd_array_count( code ) * vg_msg_cmd_type_size( code );
+}
+
+u8 vg_msg_count_bits( u32 count )
+{
+ if( count > 16 ) vg_fatal_error( "Too large\n" );
+ return ((count-1)<<2);
+}
+
+/* write a sized type */
+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) );
+}
+
+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 */
+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 */
+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.
+ */
+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
+ */
+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
+ */
+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
+ */
+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
+ */
+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
+ */
+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;
+}
+
+/*
+ * Read a integral KV out to dst, and perform conversion if needed
+ * dst is always defined, if its not found its set to 0
+ */
+int vg_msg_getkvintg( vg_msg *msg, const char *key, u8 type, void *dst )
+{
+ vg_msg_cmd cmd;
+ if( vg_msg_getkvcmd( msg, key, &cmd ) )
+ {
+ vg_msg_cast( cmd.value, cmd.code, dst, type );
+ return 1;
+ }
+ else
+ {
+ memset( dst, 0, vg_msg_cmd_bytecount(type) );
+ return 0;
+ }
+}
+
+/* helper for reading string kvs. returns NULL if not found */
+const char *vg_msg_getkvstr( vg_msg *msg, const char *key )
+{
+ vg_msg_cmd cmd;
+ if( vg_msg_getkvcmd( msg, key, &cmd ) )
+ return cmd.value;
+ else
+ return NULL;
+}
+
+int vg_msg_getkvvecf( vg_msg *msg, const char *key, u8 type,
+ void *v, void *default_value )
+{
+ vg_msg_cmd cmd;
+ if( vg_msg_getkvcmd( msg, key, &cmd ) )
+ {
+ vg_msg_cast( cmd.value, cmd.code, v, type );
+ return 1;
+ }
+ else if( default_value )
+ vg_msg_cast( default_value, type, v, type );
+
+ return 0;
+}
+
+
+/* debug the thing */
+void vg_msg_print( vg_msg *msg, u32 len )
+{
+ if( msg->error != k_vg_msg_error_OK ){
+ printf( "Message contains errors\n" );
+ return;
+ }
+
+ vg_msg b;
+ vg_msg_init( &b, msg->buf, len );
+
+ vg_msg_cmd cmd;
+ while( vg_msg_next( &b, &cmd ) ){
+ if( cmd.code == k_vg_msg_frame ){
+ for( u32 i=0; i<b.cur.depth-1; i++ ) printf( " " );
+ printf( "'%s'{\n", cmd.key );
+ }
+ else {
+ for( u32 i=0; i<b.cur.depth; i++ ) printf( " " );
+
+ if( cmd.code == k_vg_msg_endframe )
+ printf( "}\n" );
+ else if( cmd.code == k_vg_msg_kvstring )
+ printf( "'%s': '%s'\n", cmd.key, (char *)cmd.value );
+ else if( cmd.code == k_vg_msg_kvbin )
+ printf( "'%s': <binary data> (%u bytes)\n", cmd.key, cmd.len );
+ else {
+ u32 base = cmd.code & k_vg_msg_type_base_bits,
+ count = vg_msg_cmd_array_count( cmd.code ),
+ size = vg_msg_cmd_type_size( cmd.code );
+
+ printf( "'%s': ", cmd.key );
+
+ if( count > 1 ) printf( "{ " );
+
+ for( u32 i=0; i<count; i++ ){
+ const void *p = cmd.value + size*i;
+
+ if( base == k_vg_msg_unsigned ){
+ printf(
+#ifdef _WIN32
+ "%llu"
+#else
+ "%lu"
+#endif
+ , vg_msg_cast_to_u64( p, base, size ) );
+ }
+ else if( base == k_vg_msg_signed ){
+ printf(
+#ifdef _WIN32
+ "%lld"
+#else
+ "%ld"
+#endif
+ , vg_msg_cast_to_i64( p, base, size ) );
+ }
+ else
+ printf( "%f", vg_msg_cast_to_f64( p, base, size ));
+
+ if( i+1<count ) printf(", ");
+ }
+
+ if( count > 1 ) printf( " }" );
+ printf( "\n" );
+ }
+ }
+ }
+}
projects / vg.git / blobdiff