X-Git-Url: https://harrygodden.com/git/?p=vg.git;a=blobdiff_plain;f=vg_msg.c;fp=vg_msg.c;h=db7380a91bfacd0f4292a043a1868e3b4fce89b7;hp=0000000000000000000000000000000000000000;hb=3b14f3dcd5bf9dd3c85144f2123d667bfa4bb63f;hpb=fce86711735b15bff37de0f70716808410fcf269

diff --git a/vg_msg.c b/vg_msg.c
new file mode 100644
index 0000000..db7380a
--- /dev/null
+++ b/vg_msg.c
@@ -0,0 +1,494 @@
+#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" );
+         }
+      }
+   }
+}