G_MODULE_EXPORT gchar * request_interface_version(gint *len)
{
OutputData *output = NULL;
GAsyncQueue *queue = NULL;
FreeEMS_Packet *packet = NULL;
gchar *version = NULL;
GTimeVal tval;
Serial_Params *serial_params = NULL;
guint8 *buf = NULL;
/* Raw packet */
guint8 pkt[INTVER_REQ_PKT_LEN];
gint res = 0;
gint i = 0;
guint8 sum = 0;
gint tmit_len = 0;
serial_params = DATA_GET(global_data,"serial_params");
g_return_val_if_fail(serial_params,NULL);
if (DATA_GET(global_data,"offline"))
return g_strdup("Offline");
pkt[HEADER_IDX] = 0;
pkt[H_PAYLOAD_IDX] = (REQUEST_INTERFACE_VERSION & 0xff00 ) >> 8;
pkt[L_PAYLOAD_IDX] = (REQUEST_INTERFACE_VERSION & 0x00ff );
for (i=0;i<INTVER_REQ_PKT_LEN-1;i++)
sum += pkt[i];
pkt[INTVER_REQ_PKT_LEN-1] = sum;
buf = finalize_packet((guint8 *)&pkt,INTVER_REQ_PKT_LEN,&tmit_len);
queue = g_async_queue_new();
register_packet_queue(PAYLOAD_ID,queue,RESPONSE_INTERFACE_VERSION);
if (!write_wrapper_f(serial_params->fd,buf, tmit_len, NULL))
{
deregister_packet_queue(PAYLOAD_ID,queue,RESPONSE_INTERFACE_VERSION);
g_free(buf);
g_async_queue_unref(queue);
return NULL;
}
g_free(buf);
g_get_current_time(&tval);
g_time_val_add(&tval,500000);
packet = g_async_queue_timed_pop(queue,&tval);
deregister_packet_queue(PAYLOAD_ID,queue,RESPONSE_INTERFACE_VERSION);
g_async_queue_unref(queue);
/*
if (packet)
printf("Firmware version PACKET ARRIVED!\n");
else
printf("TIMEOUT\n");
*/
if (packet)
{
version = g_strndup((const gchar *)(packet->data+packet->payload_base_offset),packet->payload_length);
if (len)
*len = packet->payload_length;
freeems_packet_cleanup(packet);
}
return version;
}
/*
\brief This dispatches out packets to awaiting subscribers (if any)
If the payload or sequnce number matches what's in the packet, this packet
is copied and pushed down the supplied queue to the lucky winner. This allows
multiple subscribers per payloadID or sequence number, which offers some
interesting flexibility
\param packet is a pointer to the new packet
\see FreeEMS_Packet
*/
G_MODULE_EXPORT void dispatch_packet_queues(FreeEMS_Packet *packet)
{
static GHashTable *payloads = NULL;
static GHashTable *sequences = NULL;
static GMutex *mutex = NULL;
GAsyncQueue *queue = NULL;
guint8 header = packet->data[0];
guint i = 0;
GList *list = NULL;
if (!mutex)
mutex = (GMutex *)DATA_GET(global_data,"queue_mutex");
if (!payloads)
payloads = (GHashTable *)DATA_GET(global_data,"payload_id_queue_hash");
if (!sequences)
sequences = (GHashTable *)DATA_GET(global_data,"sequence_num_queue_hash");
g_return_if_fail(mutex);
g_mutex_lock(mutex);
/* If sequence set, look for it and dispatch if found */
if ((sequences) && ((packet->header_bits & HAS_SEQUENCE_MASK) > 0))
{
list = (GList *)g_hash_table_lookup(sequences,GINT_TO_POINTER((GINT)packet->seq_num));
if (list)
{
for (i=0;i<g_list_length(list);i++)
{
queue = (GAsyncQueue *)g_list_nth_data(list,i);
if (queue)
{
g_async_queue_ref(queue);
g_async_queue_push(queue,(gpointer)packet_deep_copy(packet));
g_async_queue_unref(queue);
}
}
}
}
if (payloads)
{
/* If payload ID matches, dispatch if found */
list = (GList *)g_hash_table_lookup(payloads,GINT_TO_POINTER((GINT)packet->payload_id));
if (list)
{
for (i=0;i<g_list_length(list);i++)
{
queue = (GAsyncQueue *)g_list_nth_data(list,i);
if (queue)
{
g_async_queue_ref(queue);
g_async_queue_push(queue,(gpointer)packet_deep_copy(packet));
g_async_queue_unref(queue);
}
}
}
}
g_mutex_unlock(mutex);
freeems_packet_cleanup(packet);
}
/*
\brief Queries the ECU for a location ID list
*/
G_MODULE_EXPORT Location_Details *request_location_id_details(guint16 loc_id)
{
OutputData *output = NULL;
GAsyncQueue *queue = NULL;
FreeEMS_Packet *packet = NULL;
GTimeVal tval;
GList *list = NULL;
Serial_Params *serial_params = NULL;
guint8 *buf = NULL;
Location_Details *details = NULL;
/* Raw packet */
guint8 pkt[LOC_ID_DETAILS_REQ_PKT_LEN];
gint res = 0;
gint i = 0;
gint h = 0;
gint l = 0;
gint tmpi = 0;
guint8 sum = 0;
gint tmit_len = 0;
serial_params = DATA_GET(global_data,"serial_params");
g_return_val_if_fail(serial_params,NULL);
pkt[HEADER_IDX] = 0;
pkt[H_PAYLOAD_IDX] = (REQUEST_RETRIEVE_LOCATION_ID_DETAILS & 0xff00 ) >> 8;
pkt[L_PAYLOAD_IDX] = (REQUEST_RETRIEVE_LOCATION_ID_DETAILS & 0x00ff );
pkt[L_PAYLOAD_IDX+1] = (loc_id & 0xff00) >> 8; /* H location bits */
pkt[L_PAYLOAD_IDX+2] = (loc_id & 0x00ff); /* L location bits */
for (i=0;i<LOC_ID_DETAILS_REQ_PKT_LEN-1;i++)
sum += pkt[i];
pkt[LOC_ID_DETAILS_REQ_PKT_LEN-1] = sum;
buf = finalize_packet((guint8 *)&pkt,LOC_ID_DETAILS_REQ_PKT_LEN,&tmit_len);
queue = g_async_queue_new();
register_packet_queue(PAYLOAD_ID,queue,RESPONSE_RETRIEVE_LOCATION_ID_DETAILS);
if (!write_wrapper_f(serial_params->fd,buf, tmit_len, NULL))
{
deregister_packet_queue(PAYLOAD_ID,queue,RESPONSE_RETRIEVE_LOCATION_ID_DETAILS);
g_free(buf);
g_async_queue_unref(queue);
return NULL;
}
g_free(buf);
g_get_current_time(&tval);
g_time_val_add(&tval,500000);
packet = g_async_queue_timed_pop(queue,&tval);
deregister_packet_queue(PAYLOAD_ID,queue,RESPONSE_RETRIEVE_LOCATION_ID_DETAILS);
g_async_queue_unref(queue);
if (packet)
{
/*printf("packet payload length %i\n",packet->payload_length);*/
if (packet->payload_length != 12)
printf("ERROR in locationID details response!\n");
details = g_new0(Location_Details, 1);
tmpi = 0;
h = packet->data[packet->payload_base_offset];
l = packet->data[packet->payload_base_offset+1];
details->flags = (h << 8) + l;
/*printf("loc id details flags %i\n",details->flags);*/
h = packet->data[packet->payload_base_offset+2];
l = packet->data[packet->payload_base_offset+3];
details->parent = (h << 8) + l;
/*printf("loc id details parent %i\n",details->parent);*/
details->ram_page = packet->data[packet->payload_base_offset+4];
details->flash_page = packet->data[packet->payload_base_offset+5];
/*printf("loc id details ram_page %i\n",details->ram_page);*/
/*printf("loc id details flash_page %i\n",details->flash_page);*/
h = packet->data[packet->payload_base_offset+6];
l = packet->data[packet->payload_base_offset+7];
details->ram_address = (h << 8) + l;
/*printf("loc id details ram_address %0x\n",details->ram_address);*/
h = packet->data[packet->payload_base_offset+8];
l = packet->data[packet->payload_base_offset+9];
details->flash_address = (h << 8) + l;
/*printf("loc id details flash_address %0x\n",details->flash_address);*/
h = packet->data[packet->payload_base_offset+10];
l = packet->data[packet->payload_base_offset+11];
details->length = (h << 8) + l;
/*printf("loc id details length %i\n",details->length);*/
freeems_packet_cleanup(packet);
}
return details;
}
/*
\brief Queries the ECU for a location ID list
*/
G_MODULE_EXPORT GList *request_location_ids(gint * len)
{
OutputData *output = NULL;
GAsyncQueue *queue = NULL;
FreeEMS_Packet *packet = NULL;
GTimeVal tval;
GList *list = NULL;
Serial_Params *serial_params = NULL;
guint8 *buf = NULL;
/* Raw packet */
guint8 pkt[LOC_ID_LIST_REQ_PKT_LEN];
gint res = 0;
gint i = 0;
gint h = 0;
gint l = 0;
gint tmpi = 0;
guint8 sum = 0;
gint tmit_len = 0;
guint8 flag = BLOCK_BITS_AND;
guint16 bits = 0;
serial_params = DATA_GET(global_data,"serial_params");
g_return_val_if_fail(serial_params,NULL);
pkt[HEADER_IDX] = 0;
pkt[H_PAYLOAD_IDX] = (REQUEST_RETRIEVE_LIST_OF_LOCATION_IDS & 0xff00 ) >> 8;
pkt[L_PAYLOAD_IDX] = (REQUEST_RETRIEVE_LIST_OF_LOCATION_IDS & 0x00ff );
pkt[L_PAYLOAD_IDX+1] = flag; /* AND/OR */
bits |= BLOCK_IS_INDEXABLE | BLOCK_IN_RAM;
pkt[L_PAYLOAD_IDX+2] = (bits & 0xff00) >> 8; /* H bits */
pkt[L_PAYLOAD_IDX+3] = (bits & 0x00ff); /* L bits */
for (i=0;i<LOC_ID_LIST_REQ_PKT_LEN-1;i++)
sum += pkt[i];
pkt[LOC_ID_LIST_REQ_PKT_LEN-1] = sum;
buf = finalize_packet((guint8 *)&pkt,LOC_ID_LIST_REQ_PKT_LEN,&tmit_len);
queue = g_async_queue_new();
register_packet_queue(PAYLOAD_ID,queue,RESPONSE_RETRIEVE_LIST_OF_LOCATION_IDS);
if (!write_wrapper_f(serial_params->fd,buf, tmit_len, NULL))
{
deregister_packet_queue(PAYLOAD_ID,queue,RESPONSE_RETRIEVE_LIST_OF_LOCATION_IDS);
g_free(buf);
g_async_queue_unref(queue);
return NULL;
}
g_free(buf);
g_get_current_time(&tval);
g_time_val_add(&tval,500000);
packet = g_async_queue_timed_pop(queue,&tval);
deregister_packet_queue(PAYLOAD_ID,queue,RESPONSE_RETRIEVE_LIST_OF_LOCATION_IDS);
g_async_queue_unref(queue);
if (packet)
{
for (i=0;i<packet->payload_length;i++)
{
tmpi = 0;
h = packet->data[packet->payload_base_offset+i];
i++;
l = packet->data[packet->payload_base_offset+i];
tmpi = (h << 8) + l;
list = g_list_append(list,GINT_TO_POINTER(tmpi));
}
if (len)
*len = packet->payload_length;
freeems_packet_cleanup(packet);
}
return list;
}
/*!
\brief This functions handles all incoing data from the ECU and validates
its content for proper START/STOP/ESCAPING and allocates a FreeEMS_Packet
structure for VALID packets and populates the required fields as needed
\param buf is a pointer to the incoming data buffer
\param len is the numbe of bytes to pull from the incoming buffer
*/
G_MODULE_EXPORT void handle_data(guchar *buf, gint len)
{
static GAsyncQueue *queue = NULL;
/* Statistic collection variables */
static guchar packetBuffer[3000];
static unsigned int packets = 0;
static unsigned int charsDropped = 0;
static unsigned int badChecksums = 0;
static unsigned int badPackets = 0;
static unsigned int goodChecksums = 0;
static unsigned int startsInsidePacket = 0;
static unsigned int totalFalseStartLost = 0;
static unsigned int doubleStartByteOccurances = 0;
static unsigned int strayDataBytesOccurances = 0;
static unsigned int escapeBytesFound = 0;
static unsigned int escapedStopBytesFound = 0;
static unsigned int escapedStartBytesFound = 0;
static unsigned int escapedEscapeBytesFound = 0;
static unsigned int escapePairMismatches = 0;
static unsigned long sumOfGoodPacketLengths = 0;
/* Loop and state variables */
static gboolean insidePacket = FALSE;
static gboolean unescapeNext = FALSE;
static unsigned int processed = 0;
static unsigned char checksum = 0;
static unsigned char lastChar = 0;
static unsigned int currentPacketLength = 0;
guchar character;
gint i = 0;
FreeEMS_Packet *packet = NULL;
if (!queue)
queue = (GAsyncQueue *)DATA_GET(global_data,"packet_queue");
log_inbound_data_f(buf,len);
for (i=0;i<len;i++)
{
character = buf[i];
if (character == START_BYTE)
{
if (insidePacket)
{
startsInsidePacket++;
if (currentPacketLength == 0)
{
doubleStartByteOccurances++;
}
else
{
totalFalseStartLost += currentPacketLength;
strayDataBytesOccurances++;
}
}
insidePacket = TRUE;
checksum = 0;
currentPacketLength = 0;
}
else if (insidePacket)
{
if (unescapeNext)
{ /* Clear escaped byte next flag */
unescapeNext = FALSE;
if (character == ESCAPED_ESCAPE_BYTE)
{
checksum += ESCAPE_BYTE;
lastChar = ESCAPE_BYTE;
escapedEscapeBytesFound++;
packetBuffer[currentPacketLength] = ESCAPE_BYTE;
currentPacketLength++;
}
else if (character == ESCAPED_START_BYTE)
{
/* Store and checksum start byte */
checksum += START_BYTE;
lastChar = START_BYTE;
escapedStartBytesFound++;
packetBuffer[currentPacketLength] = START_BYTE;
currentPacketLength++;
}
else if(character == ESCAPED_STOP_BYTE)
{
/* Store and checksum stop byte */
checksum += STOP_BYTE;
lastChar = STOP_BYTE;
escapedStopBytesFound++;
packetBuffer[currentPacketLength] = STOP_BYTE;
currentPacketLength++;
}else
{
/* Otherwise reset and record as data is bad */
insidePacket = FALSE;
checksum = 0;
currentPacketLength = 0;
escapePairMismatches++;
}
}
else if (character == ESCAPE_BYTE)
{
/* Set flag to indicate that the next byte should be un-escaped. */
unescapeNext = TRUE;
escapeBytesFound++;
}
else if (character == STOP_BYTE)
{
packets++;
/* Bring the checksum back to where it should be */
checksum -= lastChar;
/* Check that the checksum matches */
if(checksum != lastChar)
{
badChecksums++;
printf("Packet number %u ending of length %u at char number %u failed checksum! Received %u Calculated %u\n", packets, currentPacketLength, processed, lastChar, checksum);
}
else
{
goodChecksums++;
/* Add the length to the SUM */
sumOfGoodPacketLengths += currentPacketLength;
/* Clear the state */
packet = g_new0(FreeEMS_Packet, 1);
packet->data = (guchar *)g_memdup(packetBuffer,currentPacketLength);
packet->raw_length = currentPacketLength;
mtxlog_packet(packet->data,packet->raw_length,FALSE);
if (!packet_decode(packet))
{
printf("Packet fields don't make sense!\n");
freeems_packet_cleanup(packet);
badPackets++;
}
else if (queue)
{
g_async_queue_ref(queue);
g_async_queue_push(queue,(gpointer)packet);
g_async_queue_unref(queue);
}
else
printf("packet queue not found!?!!\n");
}
insidePacket = FALSE;
currentPacketLength= 0;
checksum = 0;
}
else
{
/* If it isn't special checksum it! */
checksum += character;
lastChar = character;
packetBuffer[currentPacketLength] = character;
currentPacketLength++;
}
}
else
charsDropped++;
}
}
