static int avb_1722_1_entity_database_add(avb_1722_1_adp_packet_t* pkt) { guid_t guid; int found_slot_index = AVB_1722_1_MAX_ENTITIES; int i; int entity_update = 0; GET_LONG_WORD(guid, pkt->entity_guid); for (i=0; i < AVB_1722_1_MAX_ENTITIES; ++i) { if (entities[i].guid.l == 0) found_slot_index = i; // Found an empty entry in the database if (entities[i].guid.l == guid.l) { // Entity is already in the database - break from loop early and update it found_slot_index = i; entity_update = 1; break; } } if (found_slot_index != AVB_1722_1_MAX_ENTITIES) { entities[found_slot_index].guid.l = guid.l; entities[found_slot_index].vendor_id = NTOH_U32(pkt->vendor_id); entities[found_slot_index].model_id = NTOH_U32(pkt->model_id); entities[found_slot_index].capabilities = NTOH_U32(pkt->entity_capabilities); entities[found_slot_index].talker_stream_sources = NTOH_U16(pkt->talker_stream_sources); entities[found_slot_index].talker_capabilities = NTOH_U16(pkt->talker_capabilities); entities[found_slot_index].listener_stream_sinks = NTOH_U16(pkt->listener_stream_sinks); entities[found_slot_index].listener_capabilites = NTOH_U16(pkt->listener_capabilites); entities[found_slot_index].controller_capabilities = NTOH_U32(pkt->controller_capabilities); entities[found_slot_index].available_index = NTOH_U32(pkt->available_index); GET_LONG_WORD(entities[found_slot_index].as_grandmaster_id, pkt->as_grandmaster_id) entities[found_slot_index].default_audio_format = NTOH_U32(pkt->default_audio_format); entities[found_slot_index].default_video_format = NTOH_U32(pkt->default_video_format); entities[found_slot_index].association_id = NTOH_U32(pkt->association_id); entities[found_slot_index].type = NTOH_U32(pkt->entity_type); entities[found_slot_index].timeout = GET_1722_1_VALID_TIME(&pkt->header) + adp_two_second_counter; if (entity_update) { return 0; } else { adp_latest_entity_added_index = found_slot_index; return 1; } } return 0; }
int avb_1722_listener_process_packet(chanend buf_ctl, unsigned char Buf0[], int numBytes, avb_1722_stream_info_t *stream_info, ptp_time_info_mod64* timeInfo, int index, int *notified_buf_ctl) { int pktDataLength, dbc_value; AVB_DataHeader_t *pAVBHdr; AVB_AVB1722_CIP_Header_t *pAVB1722Hdr; unsigned char *Buf = &Buf0[2]; int avb_ethernet_hdr_size = (Buf[12]==0x81) ? 18 : 14; int num_samples_in_payload, num_channels_in_payload; pAVBHdr = (AVB_DataHeader_t *) &(Buf[avb_ethernet_hdr_size]); pAVB1722Hdr = (AVB_AVB1722_CIP_Header_t *) &(Buf[avb_ethernet_hdr_size + AVB_TP_HDR_SIZE]); unsigned char *sample_ptr; int i; int num_channels = stream_info->num_channels; media_output_fifo_t *map = &stream_info->map[0]; int stride; #if !AVB_1722_FORMAT_SAF int dbc_diff; #endif // sanity check on number bytes in payload #if AVB_1722_FORMAT_SAF if (numBytes <= avb_ethernet_hdr_size + AVB_TP_HDR_SIZE) #else if (numBytes <= avb_ethernet_hdr_size + AVB_TP_HDR_SIZE + AVB_CIP_HDR_SIZE) #endif { return (0); } if (AVBTP_CD(pAVBHdr) != AVBTP_CD_DATA) { return (0); } if (AVBTP_SV(pAVBHdr) == 0) { return (0); } #if !AVB_1722_FORMAT_SAF dbc_value = (int) pAVB1722Hdr->DBC; dbc_diff = dbc_value - stream_info->dbc; stream_info->dbc = dbc_value; if (dbc_diff < 0) dbc_diff += 0x100; #endif pktDataLength = NTOH_U16(pAVBHdr->packet_data_length); #if AVB_1722_FORMAT_SAF num_samples_in_payload = pktDataLength>>2; #else num_samples_in_payload = (pktDataLength-8)>>2; #endif int prev_num_samples = stream_info->prev_num_samples; stream_info->prev_num_samples = num_samples_in_payload; if (stream_info->chan_lock < 16) { int num_channels; #if !AVB_1722_FORMAT_SAF if (!prev_num_samples || dbc_diff == 0) return 0; #endif #if AVB_1722_FORMAT_SAF num_channels = AVBTP_PROTOCOL_SPECIFIC(pAVBHdr); #else num_channels = prev_num_samples / dbc_diff; #endif if (!stream_info->num_channels_in_payload || stream_info->num_channels_in_payload != num_channels) { stream_info->num_channels_in_payload = num_channels; stream_info->chan_lock = 0; stream_info->rate = 0; } stream_info->rate += num_samples_in_payload; stream_info->chan_lock++; #if !AVB_1722_FORMAT_SAF if (stream_info->chan_lock == 16) { stream_info->rate = (stream_info->rate / stream_info->num_channels_in_payload / 16); switch (stream_info->rate) { case 1: stream_info->rate = 8000; break; case 2: stream_info->rate = 16000; break; case 4: stream_info->rate = 32000; break; case 5: stream_info->rate = 44100; break; case 6: stream_info->rate = 48000; break; case 11: stream_info->rate = 88200; break; case 12: stream_info->rate = 96000; break; default: stream_info->rate = 0; break; } } #endif return 0; } #ifdef AVB_1722_RECORD_ERRORS else if (dbc_diff != num_samples_in_payload) { avb_1722_listener_dbc_discontinuity++; } #endif #if AVB_1722_FORMAT_SAF if ((AVBTP_TV(pAVBHdr)==1)) { unsigned sample_num = 0; #else if ((AVBTP_TV(pAVBHdr)==1)) { // See 61883-6 section 6.2 which explains that the receiver can calculate // which data block (sample) the timestamp refers to using the formula: // index = (SYT_INTERVAL - dbc % SYT_INTERVAL) % SYT_INTERVAL unsigned syt_interval, sample_num; switch (stream_info->rate) { case 8000: syt_interval = 1; break; case 16000: syt_interval = 2; break; case 32000: syt_interval = 8; break; case 44100: syt_interval = 8; break; case 48000: syt_interval = 8; break; case 88200: syt_interval = 16; break; case 96000: syt_interval = 16; break; case 176400: syt_interval = 32; break; case 192000: syt_interval = 32; break; default: break; } sample_num = (syt_interval - (dbc_value & (syt_interval-1))) & (syt_interval-1); #endif // register timestamp for (int i=0; i<num_channels; i++) { if (map[i]) { media_output_fifo_set_ptp_timestamp(map[i], AVBTP_TIMESTAMP(pAVBHdr), sample_num); } } } for (i=0; i<num_channels; i++) { if (map[i]) { media_output_fifo_maintain(map[i], buf_ctl, notified_buf_ctl); } } // now send the samples #if AVB_1722_FORMAT_SAF sample_ptr = (unsigned char *) &Buf[(avb_ethernet_hdr_size + AVB_TP_HDR_SIZE)]; #else sample_ptr = (unsigned char *) &Buf[(avb_ethernet_hdr_size + AVB_TP_HDR_SIZE + AVB_CIP_HDR_SIZE)]; #endif num_channels_in_payload = stream_info->num_channels_in_payload; stride = num_channels_in_payload; num_channels = num_channels < num_channels_in_payload ? num_channels : num_channels_in_payload; for(i=0; i<num_channels; i++) { if (map[i]) { media_output_fifo_strided_push(map[i], (unsigned int *) sample_ptr, stride, num_samples_in_payload); } sample_ptr += 4; } return(1); }