/* 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(); }