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);
}
