245 lines
8.6 KiB
C
245 lines
8.6 KiB
C
#include "transport.h"
|
|
|
|
|
|
#include "geometry.h"
|
|
#include "traces.h"
|
|
|
|
static void parse_buf( struct Transport *this );
|
|
static gboolean check_checksum( struct Transport *this, guint msg_len );
|
|
static void remove_n_from_buf( struct Transport *this, guint len );
|
|
static void remove_until_stx( struct Transport *this );
|
|
|
|
struct Transport *transport_new( gboolean fixed_size,
|
|
gboolean two_bytes_checksum, guint nb_msg,
|
|
guint *size_msg, guint max_msg_size,
|
|
guchar stx, guchar etx,
|
|
void( *err_callback )( gpointer callback_data, GError *error ),
|
|
void( *msg_callback )( gpointer callback_data, struct TransportMsg *msg ),
|
|
gpointer callback_data )
|
|
{
|
|
struct Transport *this = g_new( struct Transport, 1 );
|
|
if ( this ) {
|
|
this->stx = stx;
|
|
this->etx = etx;
|
|
this->err_callback = err_callback;
|
|
this->msg_callback = msg_callback;
|
|
this->callback_data = callback_data;
|
|
this->buf_len = 0;
|
|
this->fixed_size = fixed_size;
|
|
this->two_bytes_checksum = two_bytes_checksum;
|
|
this->quark = g_quark_from_string( "Transport" );
|
|
g_get_current_time( &this->start_date );
|
|
if ( fixed_size ) {
|
|
this->nb_msg_type = nb_msg;
|
|
this->size_msg = size_msg;
|
|
//this->max_msg_size = FIXME find max size in array;;
|
|
} else
|
|
this->max_msg_size = max_msg_size;
|
|
this->nb_byte_last_status = 0;
|
|
this->nb_msg_last_status = 0;
|
|
this->status.nb_byte = 0;
|
|
this->status.nb_msg = 0;
|
|
this->status.nb_err = 0;
|
|
}
|
|
return this;
|
|
}
|
|
|
|
#define GDOUBLE_FROM_DIF_TV(a,b) ((gdouble)(a.tv_sec-b.tv_sec) + 1e-6 * (gdouble)(a.tv_usec-b.tv_usec))
|
|
|
|
struct TransportStatus *transport_get_status( struct Transport *this )
|
|
{
|
|
GTimeVal now;
|
|
gdouble duration;
|
|
|
|
g_get_current_time( &now );
|
|
this->status.run_time = DELAY_SEC_OF_TIMEVAL( now, this->start_date );
|
|
|
|
duration = GDOUBLE_FROM_DIF_TV( now, this->last_status_date );
|
|
this->status.byte_rate = this->nb_byte_last_status / duration;
|
|
this->status.msg_rate = this->nb_msg_last_status / duration;
|
|
|
|
this->nb_byte_last_status = 0;
|
|
this->nb_msg_last_status = 0;
|
|
this->last_status_date = now;
|
|
|
|
return &this->status;
|
|
}
|
|
|
|
|
|
|
|
void transport_free( struct Transport *this )
|
|
{
|
|
g_free( this );
|
|
}
|
|
|
|
void transport_feed_data( struct Transport *this, const guchar *buf,
|
|
guint len )
|
|
{
|
|
if ( len >= TRANSPORT_BUF_LEN - this->buf_len ) {
|
|
GError *err = g_error_new( this->quark,
|
|
TRANSPORT_BUF_OVFW, /* guint code */
|
|
"buffer overflow" /* const gchar *format */
|
|
);
|
|
this->err_callback( this->callback_data, err );
|
|
g_error_free( err );
|
|
} else {
|
|
this->status.nb_byte += len;
|
|
this->nb_byte_last_status += len;
|
|
memcpy( this->buf + this->buf_len, buf, len );
|
|
this->buf_len += len;
|
|
parse_buf( this );
|
|
}
|
|
}
|
|
|
|
static gboolean check_checksum( struct Transport *this, guint msg_len )
|
|
{
|
|
guint checksum_idx = msg_len - TRANSPORT_TAIL_LEN;
|
|
guint i;
|
|
if ( this->two_bytes_checksum ) {
|
|
guchar cka = 0, ckb = 0;
|
|
for ( i = TRANSPORT_PAYLOAD_OFFSET; i < checksum_idx; i++ ) {
|
|
cka += this->buf[ i ];
|
|
ckb += cka;
|
|
}
|
|
return ( cka == this->buf[ checksum_idx ] &&
|
|
ckb == this->buf[ checksum_idx + 1 ] );
|
|
} else {
|
|
guchar checksum = 0;
|
|
for ( i = TRANSPORT_PAYLOAD_OFFSET; i < checksum_idx; i++ )
|
|
checksum ^= this->buf[ i ];
|
|
if ( checksum != this->buf[ checksum_idx ] )
|
|
TRACE( TRACE_ERROR, "transport checksum error (found : 0x%02X read : 0x%02X)\n",
|
|
checksum, this->buf[ checksum_idx ] );
|
|
return checksum == this->buf[ checksum_idx ];
|
|
}
|
|
}
|
|
|
|
static void remove_n_from_buf( struct Transport *this, guint len )
|
|
{
|
|
memmove( this->buf, this->buf + len, this->buf_len - len );
|
|
this->buf_len -= len;
|
|
}
|
|
|
|
static void remove_until_stx( struct Transport *this )
|
|
{
|
|
guchar *stx = memchr( this->buf, this->stx, this->buf_len );
|
|
if ( stx ) {
|
|
if ( stx != this->buf ) {
|
|
memmove( this->buf, stx, this->buf_len - ( stx - this->buf ) );
|
|
this->buf_len -= ( stx - this->buf );
|
|
}
|
|
} else
|
|
this->buf_len = 0;
|
|
}
|
|
|
|
static gboolean get_msg_len( struct Transport *this, guint *len )
|
|
{
|
|
if ( this->fixed_size ) {
|
|
guint type = this->buf[ TRANSPORT_PAYLOAD_OFFSET ];
|
|
if ( type >= this->nb_msg_type ) { /* BAP ID */
|
|
TRACE( TRACE_TRANSPORT, "%d bad type\n", this->buf[ TRANSPORT_PAYLOAD_OFFSET ] );
|
|
TRACE( TRACE_TRANSPORT, "parse_buf %s\n", print_hex( this->buf,
|
|
this->buf_len ) );
|
|
this->status.nb_err++;
|
|
{
|
|
GError *err = g_error_new( this->quark,
|
|
TRANSPORT_INVALID_MSG_ID, /* guint code */
|
|
"invalid msg id" /* const gchar *format */
|
|
);
|
|
this->err_callback( this->callback_data, err );
|
|
g_error_free( err );
|
|
}
|
|
return FALSE;
|
|
} else *len = this->size_msg[ type ] + TRANSPORT_HEAD_LEN + TRANSPORT_TAIL_LEN;
|
|
} else {
|
|
*len = this->buf[ TRANSPORT_PAYLOAD_OFFSET ] + TRANSPORT_HEAD_LEN +
|
|
TRANSPORT_TAIL_LEN;
|
|
if ( *len >= this->max_msg_size ) { /* BAD LEN */
|
|
TRACE( TRACE_TRANSPORT, "%d bad len\n", this->buf[ TRANSPORT_PAYLOAD_OFFSET ] );
|
|
this->status.nb_err++;
|
|
{
|
|
GError *err = g_error_new( this->quark,
|
|
TRANSPORT_INVALID_MSG_LEN, /* guint code */
|
|
"invalid message len" /* const gchar *format */
|
|
);
|
|
this->err_callback( this->callback_data, err );
|
|
g_error_free( err );
|
|
}
|
|
return FALSE;
|
|
}
|
|
}
|
|
return TRUE;
|
|
}
|
|
|
|
|
|
static void parse_buf( struct Transport *this )
|
|
{
|
|
/* make sure first byte in buffer is STX */
|
|
remove_until_stx( this );
|
|
// if (this->fixed_size)
|
|
// TRACE(TRACE_TRANSPORT, "parse_buf %s\n", print_hex(this->buf, this->buf_len));
|
|
if ( this->buf_len < TRANSPORT_PAYLOAD_OFFSET ) /* NO PAYLOAD */
|
|
TRACE( TRACE_TRANSPORT_VERB, "no payload\n" );
|
|
else {
|
|
guint msg_len;
|
|
if ( !get_msg_len( this,
|
|
&msg_len ) ) { /* BAD LEN ( or TYPE) */
|
|
remove_n_from_buf( this, 1 );
|
|
parse_buf( this );
|
|
} else {
|
|
if ( this->buf_len < msg_len ) /* NOT ENOUGTH DATA */
|
|
TRACE( TRACE_TRANSPORT_VERB, "not enough data\n" );
|
|
else {
|
|
if ( !check_checksum( this, msg_len ) ) {
|
|
TRACE( TRACE_TRANSPORT, "parse_buf %s\n", print_hex( this->buf,
|
|
this->buf_len ) );
|
|
this->status.nb_err++;
|
|
{
|
|
GError *err = g_error_new( this->quark,
|
|
TRANSPORT_CHECKSUM_ERROR, /* guint code */
|
|
"checksum error" /* const gchar *format */
|
|
);
|
|
this->err_callback( this->callback_data, err );
|
|
g_error_free( err );
|
|
}
|
|
|
|
remove_n_from_buf( this, 1 );
|
|
parse_buf( this );
|
|
} else {
|
|
if ( !this->two_bytes_checksum && this->buf[ msg_len - 1 ] != this->etx ) {
|
|
TRACE( TRACE_TRANSPORT, "parse_buf %s\n", print_hex( this->buf,
|
|
this->buf_len ) );
|
|
TRACE( TRACE_TRANSPORT_VERB, "%d is not ETX\n", this->buf[ msg_len - 1 ] );
|
|
this->status.nb_err++;
|
|
{
|
|
GError *err = g_error_new( this->quark,
|
|
TRANSPORT_NO_ETX, /* guint code */
|
|
"NO ETX" /* const gchar *format */
|
|
);
|
|
this->err_callback( this->callback_data, err );
|
|
g_error_free( err );
|
|
}
|
|
remove_n_from_buf( this, 1 );
|
|
parse_buf( this );
|
|
} else {
|
|
struct TransportMsg msg;
|
|
guint payload_len = msg_len - TRANSPORT_HEAD_LEN - TRANSPORT_TAIL_LEN;
|
|
g_get_current_time( &msg.date );
|
|
msg.len = payload_len;
|
|
msg.data = this->buf + TRANSPORT_PAYLOAD_OFFSET;
|
|
this->status.nb_msg++;
|
|
this->nb_msg_last_status++;
|
|
|
|
TRACE( TRACE_TRANSPORT_VERB,
|
|
"transport msg date : %ld%06ld len : %d data : %s\n", msg.date.tv_sec,
|
|
msg.date.tv_usec, msg.len, print_hex( msg.data, msg.len ) );
|
|
this->msg_callback( this->callback_data, &msg );
|
|
remove_n_from_buf( this, msg_len );
|
|
parse_buf( this );
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|