2 #include "vg_platform.h"
7 /* write a buffer from msg, rang checked. */
8 void vg_msg_wbuf( vg_msg
*msg
, u8
*buf
, u32 len
)
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
;
16 for( u32 i
=0; i
<len
; i
++ ){
17 msg
->buf
[ msg
->cur
.co
++ ] = buf
[i
];
21 /* read a buffer from msg, rang checked. */
22 void vg_msg_rbuf( vg_msg
*msg
, u8
*buf
, u32 len
)
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
;
30 for( u32 i
=0; i
<len
; i
++ ){
31 buf
[i
] = msg
->buf
[ msg
->cur
.co
++ ];
35 /* write null terminated string to stream */
36 void vg_msg_wstr( vg_msg
*msg
, const char *str
)
38 if( msg
->error
!= k_vg_msg_error_OK
) return;
40 vg_msg_wbuf( msg
, (u8
[]){ str
[i
] }, 1 );
45 /* read null terminated string, range check and generate hash (djb2) */
46 const char *vg_msg_rstr( vg_msg
*msg
, u32
*djb2
)
48 if( msg
->error
!= k_vg_msg_error_OK
) return 0;
51 const char *str
= (void *)(&msg
->buf
[ msg
->cur
.co
]);
53 while( (c
= msg
->buf
[ msg
->cur
.co
++ ]) ){
54 if( msg
->cur
.co
>= msg
->max
){
55 msg
->error
= k_vg_msg_error_overflow
;
58 hash
= ((hash
<< 5) + hash
) + c
; /* hash * 33 + c */
65 /* begin a new frame in message stream */
66 void vg_msg_frame( vg_msg
*msg
, const char *name
)
68 if( msg
->error
!= k_vg_msg_error_OK
) return;
71 vg_msg_wbuf( msg
, (u8
[]){ k_vg_msg_frame
}, 1 );
72 vg_msg_wstr( msg
, name
);
75 /* end frame in message stream */
76 void vg_msg_end_frame( vg_msg
*msg
)
78 if( msg
->error
!= k_vg_msg_error_OK
) return;
79 if( !msg
->cur
.depth
){
80 msg
->error
= k_vg_msg_error_unbalanced
;
84 vg_msg_wbuf( msg
, (u8
[]){ k_vg_msg_endframe
}, 1 );
87 /* write a KV string to stream */
88 void vg_msg_wkvstr( vg_msg
*msg
, const char *key
, const char *value
)
90 vg_msg_wbuf( msg
, (u8
[]){ k_vg_msg_kvstring
}, 1 );
91 vg_msg_wstr( msg
, key
);
92 vg_msg_wstr( msg
, value
);
95 /* write a binary block to stream */
96 void vg_msg_wkvbin( vg_msg
*msg
, const char *key
, u8
*bin
, u32 len
)
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
);
104 u32
vg_msg_cmd_array_count( u8 code
)
106 return ((code
& k_vg_msg_array_count_bits
)>>2) + 1;
109 u32
vg_msg_cmd_type_size( u8 code
)
111 return 0x1 << (code
& k_vg_msg_type_size_bits
);
114 /* get the byte count of a sized code */
115 u32
vg_msg_cmd_bytecount( u8 code
)
117 return vg_msg_cmd_array_count( code
) * vg_msg_cmd_type_size( code
);
120 u8
vg_msg_count_bits( u32 count
)
122 if( count
> 16 ) vg_fatal_error( "Too large\n" );
123 return ((count
-1)<<2);
126 /* write a sized type */
127 void vg_msg_wkvnum( vg_msg
*msg
, const char *key
,
128 u8 type
, u8 count
, void *data
)
130 u8 code
= type
| vg_msg_count_bits(count
);
132 vg_msg_wbuf( msg
, &code
, 1 );
133 vg_msg_wstr( msg
, key
);
134 vg_msg_wbuf( msg
, data
, vg_msg_cmd_bytecount(code
) );
137 void vg_msg_init( vg_msg
*msg
, u8
*buffer
, u32 len
)
142 msg
->error
= k_vg_msg_error_OK
;
147 * The stream reading interface
148 * -----------------------------------------------------------------------------
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
)
156 vg_msg_rbuf( msg
, &cmd
->code
, 1 );
157 if( msg
->error
!= k_vg_msg_error_OK
) return 0;
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)
163 * 1 0 0 0 0 0 1 0 0x82
165 if( cmd
->code
== 0x44 ) cmd
->code
= 0x82;
168 if( msg
->error
!= k_vg_msg_error_OK
) return 0;
170 if( cmd
->code
== k_vg_msg_frame
){
171 cmd
->key
= vg_msg_rstr( msg
, &cmd
->key_djb2
);
174 else if( cmd
->code
== k_vg_msg_endframe
){
175 if( !msg
->cur
.depth
){
176 msg
->error
= k_vg_msg_error_unbalanced
;
181 else if( cmd
->code
>= k_vg_msg_kv
){
182 cmd
->key
= vg_msg_rstr( msg
, &cmd
->key_djb2
);
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
;
190 else if( cmd
->code
== k_vg_msg_kvstring
){
191 cmd
->value
= vg_msg_rstr( msg
, &cmd
->value_djb2
);
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
)
197 cmd
->value
= &msg
->buf
[ msg
->cur
.co
];
198 msg
->cur
.co
+= cmd
->len
;
201 msg
->error
= k_vg_msg_error_unhandled_cmd
;
203 if( msg
->cur
.co
> msg
->max
)
204 msg
->error
= k_vg_msg_error_overflow
;
207 msg
->error
= k_vg_msg_error_unhandled_cmd
;
209 if( msg
->error
!= k_vg_msg_error_OK
)
215 /* move through the frame(and subframes) until we fall out of it */
216 int vg_msg_skip_frame( vg_msg
*msg
)
220 u32 start_depth
= msg
->cur
.depth
;
221 while( vg_msg_next( msg
, &cmd
) )
222 if( msg
->cur
.depth
< start_depth
)
228 * A more friendly but slower interface
229 * -----------------------------------------------------------------------------
233 * returns 0 if drops out of scope or ends.
235 int vg_msg_seekframe( vg_msg
*msg
, const char *name
)
237 vg_msg_cursor orig
= msg
->cur
;
239 while( vg_msg_next( msg
, &cmd
) ){
240 if( msg
->cur
.depth
< orig
.depth
){
244 if( msg
->cur
.depth
!= orig
.depth
+1 )
246 if( cmd
.code
== k_vg_msg_frame
)
247 if( !name
|| VG_STRDJB2_EQ( name
, cmd
.key
, cmd
.key_djb2
) )
256 * Convert any type integral type to u64
258 u64
vg_msg_cast_to_u64( const void *src
, u8 src_base
, u8 src_size
)
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
));
267 memcpy( &a
, src
, src_size
);
273 * Convert any integral type to i64
275 i64
vg_msg_cast_to_i64( const void *src
, u8 src_base
, u8 src_size
)
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
));
284 memcpy( &a
, src
, src_size
);
286 if( (src_base
== k_vg_msg_signed
) && (src_size
!= 8) ){
287 /* extend sign bit */
288 u64 sign_bit
= 0x1llu
<< ((src_size
*8)-1);
290 a
|= (0xffffffffffffffffllu
<< (64-__builtin_clzll( a
)));
298 * Convert any integral type to f64
300 f64
vg_msg_cast_to_f64( const void *src
, u8 src_base
, u8 src_size
)
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
);
308 return (f64
)vg_msg_cast_to_i64( src
, src_base
, src_size
);
312 * Convert any full integral type code to another
313 * Passing in non-integral codes is undefined
315 void vg_msg_cast( const void *src
, u8 src_code
, void *dst
, u8 dst_code
)
317 if( src_code
== dst_code
){
318 memcpy( dst
, src
, vg_msg_cmd_bytecount( src_code
) );
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
;
328 memset( dst
, 0, dst_s
* dst_n
);
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
;
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
;
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
;
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
;
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
363 int vg_msg_getkvcmd( vg_msg
*msg
, const char *key
, vg_msg_cmd
*cmd
)
365 vg_msg_cursor orig
= msg
->cur
;
366 while( vg_msg_next( msg
, cmd
) ){
367 if( msg
->cur
.depth
< orig
.depth
){
371 if( msg
->cur
.depth
> orig
.depth
)
373 if( cmd
->code
> k_vg_msg_kv
){
374 if( VG_STRDJB2_EQ( key
, cmd
->key
, cmd
->key_djb2
) ){
380 msg
->error
= k_vg_msg_error_OK
;
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
389 int vg_msg_getkvintg( vg_msg
*msg
, const char *key
, u8 type
, void *dst
,
390 void *default_value
)
393 if( vg_msg_getkvcmd( msg
, key
, &cmd
) )
395 vg_msg_cast( cmd
.value
, cmd
.code
, dst
, type
);
401 memcpy( dst
, default_value
, vg_msg_cmd_bytecount(type
) );
403 memset( dst
, 0, vg_msg_cmd_bytecount(type
) );
409 /* helper for reading string kvs. returns NULL if not found */
410 const char *vg_msg_getkvstr( vg_msg
*msg
, const char *key
)
413 if( vg_msg_getkvcmd( msg
, key
, &cmd
) )
419 int vg_msg_getkvvecf( vg_msg
*msg
, const char *key
, u8 type
,
420 void *v
, void *default_value
)
423 if( vg_msg_getkvcmd( msg
, key
, &cmd
) )
425 vg_msg_cast( cmd
.value
, cmd
.code
, v
, type
);
428 else if( default_value
)
429 vg_msg_cast( default_value
, type
, v
, type
);
435 /* debug the thing */
436 void vg_msg_print( vg_msg
*msg
, u32 len
)
438 if( msg
->error
!= k_vg_msg_error_OK
){
439 printf( "Message contains errors\n" );
444 vg_msg_init( &b
, msg
->buf
, len
);
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
);
453 for( u32 i
=0; i
<b
.cur
.depth
; i
++ ) printf( " " );
455 if( cmd
.code
== k_vg_msg_endframe
)
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
);
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
);
466 printf( "'%s': ", cmd
.key
);
468 if( count
> 1 ) printf( "{ " );
470 for( u32 i
=0; i
<count
; i
++ ){
471 const void *p
= cmd
.value
+ size
*i
;
473 if( base
== k_vg_msg_unsigned
){
480 , vg_msg_cast_to_u64( p
, base
, size
) );
482 else if( base
== k_vg_msg_signed
){
489 , vg_msg_cast_to_i64( p
, base
, size
) );
492 printf( "%f", vg_msg_cast_to_f64( p
, base
, size
));
494 if( i
+1<count
) printf(", ");
497 if( count
> 1 ) printf( " }" );