/* Reads the next packet. */
static gboolean observer_read(wtap *wth, int *err, gchar **err_info,
gint64 *data_offset)
{
int header_bytes_consumed;
int data_bytes_consumed;
packet_entry_header packet_header;
/* skip records other than data records */
for (;;) {
*data_offset = file_tell(wth->fh);
/* process the packet header, including TLVs */
header_bytes_consumed = read_packet_header(wth->fh, &wth->phdr.pseudo_header, &packet_header, err,
err_info);
if (header_bytes_consumed <= 0)
return FALSE; /* EOF or error */
if (packet_header.packet_type == PACKET_TYPE_DATA_PACKET)
break;
/* skip to next packet */
if (!skip_to_next_packet(wth, packet_header.offset_to_next_packet,
header_bytes_consumed, err, err_info)) {
return FALSE; /* EOF or error */
}
}
if (!process_packet_header(wth, &packet_header, &wth->phdr, err, err_info))
return FALSE;
/* read the frame data */
data_bytes_consumed = read_packet_data(wth->fh, packet_header.offset_to_frame,
header_bytes_consumed, wth->frame_buffer,
wth->phdr.caplen, err, err_info);
if (data_bytes_consumed < 0) {
return FALSE;
}
/* skip over any extra bytes following the frame data */
if (!skip_to_next_packet(wth, packet_header.offset_to_next_packet,
header_bytes_consumed + data_bytes_consumed, err, err_info)) {
return FALSE;
}
return TRUE;
}
static bool start_next_frame (rtp_plugin_data_t *pifptr,
uint8_t **buffer,
uint32_t *buflen,
frame_timestamp_t *ts,
void **userdata)
{
isma_enc_rtp_data_t *iptr = (isma_enc_rtp_data_t *)pifptr;
uint64_t timetick;
if (iptr->m_frame_data_on != NULL) {
uint32_t next_ts;
#ifdef DEBUG_ISMA_AAC
isma_message(LOG_DEBUG, ismaencrtp, "Advancing to next pak data - old ts %x",
iptr->m_frame_data_on->rtp_timestamp);
#endif
if (iptr->m_frame_data_on->last_in_pak != 0) {
// We're done with all the data in this packet - get rid
// of the rtp packet.
if (iptr->m_frame_data_on->is_fragment == 1) {
// if fragmented, need to get rid of all paks pointed to
isma_frag_data_t *q = iptr->m_frame_data_on->frag_data;
while (q != NULL) {
rtp_packet *pak = q->pak;
q->pak = NULL;
if (pak != NULL) {
iptr->m_vft->free_pak(pak);
}
q = q->frag_data_next;
#ifdef DEBUG_ISMA_AAC
isma_message(LOG_DEBUG, ismaencrtp, "removing pak - frag %d", pak->rtp_pak_seq);
#endif
}
} else {
rtp_packet *pak = iptr->m_frame_data_on->pak;
iptr->m_frame_data_on->pak = NULL;
iptr->m_vft->free_pak(pak);
#ifdef DEBUG_ISMA_AAC
isma_message(LOG_DEBUG, ismaencrtp, "removing pak %d", pak->rtp_pak_seq);
#endif
}
}
/*
* Remove the frame data head pointer, and put it on the free list
*/
isma_frame_data_t *p = NULL;
SDL_LockMutex(iptr->m_rtp_packet_mutex);
p = iptr->m_frame_data_on;
iptr->m_frame_data_on = NULL;
next_ts = p->rtp_timestamp;
p->frame_data_next = iptr->m_frame_data_free;
iptr->m_frame_data_free = p;
// free all frag_data for this frame
if (p->is_fragment == 1) {
isma_frag_data_t * q = p->frag_data;
while (q != NULL) {
p->frag_data = q->frag_data_next;
CHECK_AND_FREE(q);
q = p->frag_data;
}
}
SDL_UnlockMutex(iptr->m_rtp_packet_mutex);
/*
* Now, look for the next timestamp - process a bunch of new
* rtp packets, if we have to...
*/
next_ts += iptr->m_rtp_ts_add;
if (iptr->m_frame_data_head == NULL ||
iptr->m_frame_data_head->rtp_timestamp != next_ts) {
// process next pak in list. Process until next timestamp is found,
// or 500 msec worth of data is found (in which case, go with first)
do {
process_packet_header(iptr);
} while (iptr->m_vft->get_next_pak(iptr->m_ifptr, NULL, 0) != NULL &&
((iptr->m_frame_data_head == NULL) ||
(iptr->m_frame_data_head->rtp_timestamp != next_ts)) &&
(iptr->m_frame_data_free != NULL));
}
#ifdef DEBUG_ISMA_AAC
else {
// iptr->m_frame_data_head is correct
isma_message(LOG_DEBUG, ismaencrtp, "frame_data_head is correct");
}
#endif
} else {
// first time. Process a bunch of packets, go with first one...
// asdf - will want to eventually add code to drop the first couple
// of packets if they're not consecutive.
do {
process_packet_header(iptr);
} while (iptr->m_frame_data_free != NULL);
}
/*
* Init up the offsets
*/
if (iptr->m_frame_data_head != NULL) {
SDL_LockMutex(iptr->m_rtp_packet_mutex);
iptr->m_frame_data_on = iptr->m_frame_data_head;
iptr->m_frame_data_head = iptr->m_frame_data_head->frame_data_next;
SDL_UnlockMutex(iptr->m_rtp_packet_mutex);
if (iptr->m_frame_data_on->is_fragment == 1) {
iptr->m_frag_reass_size = 0;
isma_frag_data_t *ptr;
ptr = iptr->m_frame_data_on->frag_data;
while (ptr != NULL) {
if (iptr->m_frag_reass_size + ptr->frag_len > iptr->m_frag_reass_size_max) {
iptr->m_frag_reass_size_max += MAX(4096, ptr->frag_len);
iptr->m_frag_reass_buffer =
(uint8_t *)realloc(iptr->m_frag_reass_buffer,
iptr->m_frag_reass_size_max);
}
memmove(iptr->m_frag_reass_buffer + iptr->m_frag_reass_size,
ptr->frag_ptr,
ptr->frag_len);
iptr->m_frag_reass_size += ptr->frag_len;
ptr = ptr->frag_data_next;
}
*buffer = iptr->m_frag_reass_buffer;
*buflen = iptr->m_frag_reass_size;
} else {
*buffer = iptr->m_frame_data_on->frame_ptr;
*buflen = iptr->m_frame_data_on->frame_len;
}
} else {
*buffer = NULL;
}
#ifdef ISMA_RTP_DUMP_OUTPUT_TO_FILE
if (*buffer != NULL) {
fwrite(*buffer, *buflen, 1, iptr->m_outfile);
}
#endif
timetick =
iptr->m_vft->rtp_ts_to_msec(iptr->m_ifptr,
iptr->m_frame_data_on != NULL ?
iptr->m_frame_data_on->rtp_timestamp :
iptr->m_ts,
iptr->m_frame_data_on ?
iptr->m_frame_data_on->pak->pd.rtp_pd_timestamp : 0,
0);
if (iptr->m_frame_data_on != NULL)
iptr->m_ts = iptr->m_frame_data_on->rtp_timestamp;
// We're going to have to handle wrap better...
#ifdef DEBUG_ISMA_AAC
isma_message(LOG_DEBUG, ismaencrtp, "start next frame %p %d ts "LLX" "LLU,
*buffer, *buflen, iptr->m_ts, timetick);
#endif
ismacrypDecryptSampleRandomAccess(iptr->myEncSID, iptr->m_frame_data_on->IV, *buflen, *buffer);
ts->audio_freq_timestamp = iptr->m_ts;
ts->msec_timestamp = timetick;
ts->timestamp_is_pts = true;
return (true);
}
PROCESS_THREAD(decryptProcess, ev, data) {
PROCESS_BEGIN();
// AES
static uint8_t myMic[8] = {0x0};
static uint8_t myNonce[13] = {0};
static uint8_t aes_key[] = AES_KEY;
aes_load_keys(aes_key, AES_KEY_STORE_SIZE_KEY_SIZE_128, 1, 0);
static uint8_t srcExtAddr[8];
static uint8_t packetPayload[128];
static uint8_t packetDuplicated[123];
static uint8_t* cData = packetDuplicated;
static uint8_t* aData = srcExtAddr;
uint8_t *packet_hdr;
uint8_t *packet_ptr;
uint16_t packet_length;
uint8_t src_addr_len;
uint8_t auth_res;
uint8_t myPDATA_LEN;
uint16_t i, j, k;
uint8_t tmp;
packet_header_t rx_pkt_header;
while(1){
PROCESS_YIELD();
#ifndef LED_DEBUG
leds_on(LEDS_RED);
#else
leds_on(LEDS_RED);
leds_on(LEDS_GREEN);
leds_on(LEDS_BLUE);
#endif
// Get data from radio buffer and parse it
packet_hdr = packetbuf_hdrptr();
packet_ptr = packetbuf_dataptr();
packet_length = packetbuf_datalen();
process_packet_header(&rx_pkt_header, packet_hdr);
memcpy(packetPayload, packet_ptr, packet_length);
src_addr_len = rx_pkt_header.pkt_src_addr_len;
// data format:
// 1 bytes ID,
// 8 bytes source addr,
// 4 bytes power,
// 1 bytes status reg
// 2 bytes panel/circuit ID (optional)
// 4 bytes pf (optional)
// 4 bytes VRMS (optional)
// 4 bytes IRMS (optional)
// RX buffer is not full
if (triumviRXBufFull==0){
triumviRXPackets[triumviAvailIDX].length = 1;
if (src_addr_len>0){
for (i=0; i<src_addr_len; i++){
srcExtAddr[i] = rx_pkt_header.pkt_src_addr[src_addr_len - 1 - i];
triumviRXPackets[triumviAvailIDX].payload[1+i] = srcExtAddr[i];
}
triumviRXPackets[triumviAvailIDX].length += src_addr_len;
}
// Decrypt packet
if (packet_ptr[0]==TRIUMVI_PKT_IDENTIFIER){
memcpy(myNonce, srcExtAddr, 8);
memcpy(&myNonce[9], &packetPayload[1], 4); // Nonce[8] should be 0
for (i=0; i<POSSIBLE_PKT_LEN_COMBINATION; i++){
myPDATA_LEN = possible_packet_length[i];
memcpy(packetDuplicated, &packetPayload[5], packet_length-5);
ccm_auth_decrypt_start(LEN_LEN, 0, myNonce, aData, ADATA_LEN,
cData, (myPDATA_LEN+MIC_LEN), MIC_LEN, NULL);
while (ccm_auth_decrypt_check_status()!=AES_CTRL_INT_STAT_RESULT_AV){}
auth_res = ccm_auth_decrypt_get_result(cData, myPDATA_LEN+MIC_LEN, myMic, MIC_LEN);
if (auth_res==CRYPTO_SUCCESS)
break;
}
// succefully decoded the packet, pack the data into buffer
if (auth_res==CRYPTO_SUCCESS){
triumviRXPackets[triumviAvailIDX].payload[0] = packet_ptr[0];
tmp = triumviRXPackets[triumviAvailIDX].length;
for (i=0; i<4; i++){
triumviRXPackets[triumviAvailIDX].payload[tmp+i] = cData[i];
}
triumviRXPackets[triumviAvailIDX].payload[tmp+4] = cData[4]; // Status register
j = 5;
k = 5;
if (cData[4]&BATTERYPACK_STATUSREG){
triumviRXPackets[triumviAvailIDX].payload[tmp+j] = cData[k]; // Panel ID
triumviRXPackets[triumviAvailIDX].payload[tmp+j+1] = cData[k+1]; // Circuit ID
triumviRXPackets[triumviAvailIDX].length += 2;
j += 2;
k += 2;
}
// PF, VRMS, IRMS
if (cData[4]&POWERFACTOR_STATUSREG){
for (i=0; i<6; i++){
triumviRXPackets[triumviAvailIDX].payload[tmp+j+i] = cData[k+i];
}
triumviRXPackets[triumviAvailIDX].length += 6;
}
// base length
triumviRXPackets[triumviAvailIDX].length += 5;
// check if fifo is full
if (((triumviAvailIDX == TRIUMVI_PACKET_BUF_LEN-1) && (triumviFullIDX == 0)) ||
((triumviAvailIDX != TRIUMVI_PACKET_BUF_LEN-1) && (triumviFullIDX == triumviAvailIDX+1))){
triumviRXBufFull = 1;
#ifndef LED_DEBUG
leds_on(LEDS_GREEN);
#endif
}
// advance pointer
if (triumviAvailIDX == TRIUMVI_PACKET_BUF_LEN-1)
triumviAvailIDX = 0;
else
triumviAvailIDX += 1;
}
}
}
#ifndef LED_DEBUG
leds_off(LEDS_RED);
#endif
process_poll(&mainProcess);
}
PROCESS_END();
}
