// Release a TX frame, and mark it as ready to send
bool ringRawsockTxFrameReady(void *pvRawsock, U8 *pBuffer, unsigned int len, U64 timeNsec)
{
AVB_TRACE_ENTRY(AVB_TRACE_RAWSOCK_DETAIL);
ring_rawsock_t *rawsock = (ring_rawsock_t*)pvRawsock;
if (!VALID_TX_RAWSOCK(rawsock)) {
AVB_LOG_ERROR("Marking TX frame ready; invalid argument");
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK_DETAIL);
return FALSE;
}
if (timeNsec) {
IF_LOG_INTERVAL(1000) AVB_LOG_WARNING("launch time is unsupported in ring_rawsock");
}
volatile struct tpacket2_hdr *pHdr = (struct tpacket2_hdr*)(pBuffer - rawsock->bufHdrSize);
AVB_LOGF_VERBOSE("pBuffer=%p, pHdr=%p szFrame=%d, len=%d", pBuffer, pHdr, rawsock->base.frameSize, len);
assert(len <= rawsock->bufferSize);
pHdr->tp_len = len;
pHdr->tp_status = TP_STATUS_SEND_REQUEST;
rawsock->buffersReady += 1;
if (rawsock->buffersReady >= rawsock->frameCount) {
AVB_LOG_WARNING("All buffers in ready/unsent state, calling send");
ringRawsockSend(pvRawsock);
}
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK_DETAIL);
return TRUE;
}
/* Talker Listener thread function that talks primarily with the endpoint
*/
void* openavbTLThreadFn(void *pv)
{
AVB_TRACE_ENTRY(AVB_TRACE_TL);
tl_state_t *pTLState = (tl_state_t *)pv;
while (pTLState->bRunning) {
AVB_TRACE_LINE(AVB_TRACE_TL_DETAIL);
int endpointHandle = openavbEptClntOpenSrvrConnection(pTLState);
if (endpointHandle == AVB_ENDPOINT_HANDLE_INVALID) {
// error connecting to endpoint, already logged
}
else {
pTLState->endpointHandle = endpointHandle;
// Validate the AVB version for TL and Endpoint are the same before continuing
pTLState->AVBVerState = OPENAVB_TL_AVB_VER_UNKNOWN;
pTLState->bConnected = openavbEptClntRequestVersionFromServer(pTLState->endpointHandle);
while (pTLState->bRunning && pTLState->bConnected && pTLState->AVBVerState == OPENAVB_TL_AVB_VER_UNKNOWN) {
// Check for endpoint version message. Timeout in 50 msec.
if (!openavbEptClntService(pTLState->endpointHandle, 50)) {
AVB_LOG_WARNING("Lost connection to endpoint, will retry");
pTLState->bConnected = FALSE;
pTLState->endpointHandle = 0;
}
}
if (pTLState->AVBVerState == OPENAVB_TL_AVB_VER_INVALID) {
AVB_LOG_ERROR("AVB core version is different than Endpoint AVB core version. Streams will not be started. Will reconnect to the endpoint and check again.");
}
if (pTLState->bConnected && pTLState->AVBVerState == OPENAVB_TL_AVB_VER_VALID) {
if (pTLState->cfg.role == AVB_ROLE_TALKER) {
openavbTLRunTalker(pTLState);
}
else {
openavbTLRunListener(pTLState);
}
}
// Close the endpoint connection. unless connection already gone in which case the socket could already be reused.
if (pTLState->bConnected) {
openavbEptClntCloseSrvrConnection(endpointHandle);
pTLState->bConnected = FALSE;
pTLState->endpointHandle = 0;
}
}
if (pTLState->bRunning) {
SLEEP(1);
}
}
THREAD_JOINABLE(pTLState->TLThread);
AVB_TRACE_EXIT(AVB_TRACE_TL);
return NULL;
}
static bool checkMapCallbacks(openavb_tl_cfg_t *pCfg)
{
bool validCfg = TRUE;
if (!pCfg->map_cb.map_cfg_cb) {
AVB_LOG_WARNING("INI doesn't specify mapping callback for '_cfg'.");
// validCfg = FALSE;
}
if (!pCfg->map_cb.map_subtype_cb) {
AVB_LOG_WARNING("INI doesn't specify mapping callback for '_subtype'.");
// validCfg = FALSE;
}
if (!pCfg->map_cb.map_avtp_version_cb) {
AVB_LOG_WARNING("INI doesn't specify mapping callback for '_avtp_version'.");
// validCfg = FALSE;
}
if ((pCfg->role == AVB_ROLE_TALKER) && !pCfg->map_cb.map_tx_init_cb) {
AVB_LOG_WARNING("INI doesn't specify mapping callback for '_tx_init'.");
// validCfg = FALSE;
}
if ((pCfg->role == AVB_ROLE_TALKER) && !pCfg->map_cb.map_tx_cb) {
AVB_LOG_ERROR("INI doesn't specify mapping callback for '_tx'.");
validCfg = FALSE;
}
if ((pCfg->role == AVB_ROLE_LISTENER) && !pCfg->map_cb.map_rx_init_cb) {
AVB_LOG_WARNING("INI doesn't specify mapping callback for '_rx_init'.");
// validCfg = FALSE;
}
if ((pCfg->role == AVB_ROLE_LISTENER) && !pCfg->map_cb.map_rx_cb) {
AVB_LOG_ERROR("INI doesn't specify mapping callback for '_rx'.");
validCfg = FALSE;
}
if (!pCfg->map_cb.map_end_cb) {
AVB_LOG_WARNING("INI doesn't specify mapping callback for '_end'.");
// validCfg = FALSE;
}
if (!pCfg->map_cb.map_gen_init_cb) {
AVB_LOG_WARNING("INI doesn't specify mapping callback for '_gen_init'.");
// validCfg = FALSE;
}
if (!pCfg->map_cb.map_gen_end_cb) {
AVB_LOG_WARNING("INI doesn't specify mapping callback for '_gen_end'.");
// validCfg = FALSE;
}
if (!pCfg->map_cb.map_avdecc_init_cb) {
// Optional callback
// CORE_TODO: AVDECC not formally supported yet.
// AVB_LOG_WARNING("INI doesn't specify mapping callback for '_avdecc_init'.");
// validCfg = FALSE;
}
return validCfg;
}
static void openavbAdpMessageRxFrameReceive(U32 timeoutUsec)
{
AVB_TRACE_ENTRY(AVB_TRACE_ADP);
hdr_info_t hdrInfo;
unsigned int offset, len;
U8 *pBuf, *pFrame;
memset(&hdrInfo, 0, sizeof(hdr_info_t));
pBuf = (U8 *)openavbRawsockGetRxFrame(rxSock, timeoutUsec, &offset, &len);
if (pBuf) {
pFrame = pBuf + offset;
offset = openavbRawsockRxParseHdr(rxSock, pBuf, &hdrInfo);
{
#ifndef UBUNTU
if (hdrInfo.ethertype == ETHERTYPE_8021Q) {
// Oh! Need to look past the VLAN tag
U16 vlan_bits = ntohs(*(U16 *)(pFrame + offset));
hdrInfo.vlan = TRUE;
hdrInfo.vlan_vid = vlan_bits & 0x0FFF;
hdrInfo.vlan_pcp = (vlan_bits >> 13) & 0x0007;
offset += 2;
hdrInfo.ethertype = ntohs(*(U16 *)(pFrame + offset));
offset += 2;
}
#endif
// Make sure that this is an AVTP packet
// (Should always be AVTP if it's to our AVTP-specific multicast address)
if (hdrInfo.ethertype == ETHERTYPE_AVTP) {
// parse the PDU only for ADP messages
if (*(pFrame + offset) == (0x80 | OPENAVB_ADP_AVTP_SUBTYPE)) {
if (memcmp(hdrInfo.shost, ADDR_PTR(&intfAddr), 6) != 0) { // Not from us!
openavbAdpMessageRxFrameParse(pFrame + offset, len - offset, &hdrInfo);
}
}
}
else {
AVB_LOG_WARNING("Received non-AVTP frame!");
AVB_LOGF_DEBUG("Unexpected packet data (length %d):", len);
AVB_LOG_BUFFER(AVB_LOG_LEVEL_DEBUG, pFrame, len, 16);
}
}
// Release the frame
openavbRawsockRelRxFrame(rxSock, pBuf);
}
bool openavbEndpointServerOpen(void)
{
AVB_TRACE_ENTRY(AVB_TRACE_ENDPOINT);
int i;
for (i=0; i < POLL_FD_COUNT; i++) {
fds[i].fd = SOCK_INVALID;
fds[i].events = 0;
}
lsock = socket(AF_UNIX, SOCK_STREAM, 0);
if (lsock < 0) {
AVB_LOGF_ERROR("Failed to open socket: %s", strerror(errno));
goto error;
}
// serverAddr is file static
serverAddr.sun_family = AF_UNIX;
snprintf(serverAddr.sun_path, UNIX_PATH_MAX, AVB_ENDPOINT_UNIX_PATH);
int rslt = bind(lsock, (struct sockaddr*)&serverAddr, sizeof(struct sockaddr_un));
if (rslt != 0) {
AVB_LOGF_ERROR("Failed to create %s: %s", serverAddr.sun_path, strerror(errno));
AVB_LOG_WARNING("** If endpoint process crashed, run the cleanup script **");
goto error;
}
rslt = listen(lsock, 5);
if (rslt != 0) {
AVB_LOGF_ERROR("Failed to listen on socket: %s", strerror(errno));
goto error;
}
AVB_LOGF_DEBUG("Listening on socket: %s", serverAddr.sun_path);
fds[AVB_ENDPOINT_LISTEN_FDS].fd = lsock;
fds[AVB_ENDPOINT_LISTEN_FDS].events = POLLIN;
AVB_TRACE_EXIT(AVB_TRACE_ENDPOINT);
return TRUE;
error:
if (lsock >= 0) {
close(lsock);
lsock = -1;
}
AVB_TRACE_EXIT(AVB_TRACE_ENDPOINT);
return FALSE;
}
bool pcapRawsockTxFrameReady(void *pvRawsock, U8 *pBuffer, unsigned int len, U64 timeNsec)
{
pcap_rawsock_t *rawsock = (pcap_rawsock_t*)pvRawsock;
int ret = -1;
if (timeNsec) {
IF_LOG_INTERVAL(1000) AVB_LOG_WARNING("launch time is unsupported in pcap_rawsock");
}
if (rawsock) {
ret = pcap_sendpacket(rawsock->handle, pBuffer, len);
if (ret == -1) {
AVB_LOGF_ERROR("pcap_sendpacket failed: %s", pcap_geterr(rawsock->handle));
}
}
return ret == 0;
}
static bool checkIntfCallbacks(openavb_tl_cfg_t *pCfg)
{
bool validCfg = TRUE;
if (!pCfg->intf_cb.intf_cfg_cb) {
AVB_LOG_WARNING("INI file doesn't specify inferface callback for '_cfg'.");
// validCfg = FALSE;
}
if ((pCfg->role == AVB_ROLE_TALKER) && !pCfg->intf_cb.intf_tx_init_cb) {
AVB_LOG_WARNING("INI file doesn't specify inferface callback for '_tx_init'.");
// validCfg = FALSE;
}
if ((pCfg->role == AVB_ROLE_TALKER) && !pCfg->intf_cb.intf_tx_cb) {
AVB_LOG_ERROR("INI file doesn't specify inferface callback for '_tx'.");
validCfg = FALSE;
}
if ((pCfg->role == AVB_ROLE_LISTENER) && !pCfg->intf_cb.intf_rx_init_cb) {
AVB_LOG_WARNING("INI file doesn't specify inferface callback for '_rx_init'.");
// validCfg = FALSE;
}
if ((pCfg->role == AVB_ROLE_LISTENER) && !pCfg->intf_cb.intf_rx_cb) {
AVB_LOG_ERROR("INI file doesn't specify inferface callback for '_rx'.");
validCfg = FALSE;
}
if (!pCfg->intf_cb.intf_end_cb) {
AVB_LOG_WARNING("INI file doesn't specify inferface callback for '_end'.");
// validCfg = FALSE;
}
if (!pCfg->intf_cb.intf_gen_init_cb) {
AVB_LOG_WARNING("INI file doesn't specify inferface callback for '_gen_init'.");
// validCfg = FALSE;
}
if (!pCfg->intf_cb.intf_gen_end_cb) {
AVB_LOG_WARNING("INI file doesn't specify inferface callback for '_gen_end'.");
// validCfg = FALSE;
}
if (!pCfg->intf_cb.intf_avdecc_init_cb) {
// Optional callback
// CORE_TODO: AVDECC not formally supported yet.
// AVB_LOG_WARNING("INI file doesn't specify inferface callback for '_avdecc_init'.");
// validCfg = FALSE;
}
return validCfg;
}
// Called from openavbTLThreadFn() which is started from openavbTLRun()
void openavbTLRunListener(tl_state_t *pTLState)
{
AVB_TRACE_ENTRY(AVB_TRACE_TL);
if (!pTLState) {
AVB_LOG_ERROR("Invalid TLState");
AVB_TRACE_EXIT(AVB_TRACE_TL);
return;
}
openavb_tl_cfg_t *pCfg = &pTLState->cfg;
pTLState->pPvtListenerData = calloc(1, sizeof(listener_data_t));
if (!pTLState->pPvtListenerData) {
AVB_LOG_WARNING("Failed to allocate listener data.");
return;
}
AVBStreamID_t streamID;
memset(&streamID, 0, sizeof(streamID));
memcpy(streamID.addr, pCfg->stream_addr.mac, ETH_ALEN);
streamID.uniqueID = pCfg->stream_uid;
AVB_LOGF_INFO("Attach "STREAMID_FORMAT, STREAMID_ARGS(&streamID));
// Create Stats Mutex
{
MUTEX_ATTR_HANDLE(mta);
MUTEX_ATTR_INIT(mta);
MUTEX_ATTR_SET_TYPE(mta, MUTEX_ATTR_TYPE_DEFAULT);
MUTEX_ATTR_SET_NAME(mta, "TLStatsMutex");
MUTEX_CREATE_ERR();
MUTEX_CREATE(pTLState->statsMutex, mta);
MUTEX_LOG_ERR("Could not create/initialize 'TLStatsMutex' mutex");
}
// Tell endpoint to listen for our stream.
// If there is a talker, we'll get callback (above.)
pTLState->bConnected = openavbTLRunListenerInit(pTLState->endpointHandle, &streamID);
if (pTLState->bConnected) {
bool bServiceIPC;
// Notify AVDECC Msg of the state change.
openavbAvdeccMsgClntNotifyCurrentState(pTLState);
// Do until we are stopped or lose connection to endpoint
while (pTLState->bRunning && pTLState->bConnected) {
// Listen for an RX frame (or just sleep if not streaming)
bServiceIPC = listenerDoStream(pTLState);
if (bServiceIPC) {
// Look for messages from endpoint. Don't block (timeout=0)
if (!openavbEptClntService(pTLState->endpointHandle, 0)) {
AVB_LOGF_WARNING("Lost connection to endpoint "STREAMID_FORMAT, STREAMID_ARGS(&streamID));
pTLState->bConnected = FALSE;
pTLState->endpointHandle = 0;
}
}
}
// Stop streaming
listenerStopStream(pTLState);
{
MUTEX_CREATE_ERR();
MUTEX_DESTROY(pTLState->statsMutex); // Destroy Stats Mutex
MUTEX_LOG_ERR("Error destroying mutex");
}
// withdraw our listener attach
if (pTLState->bConnected)
openavbEptClntStopStream(pTLState->endpointHandle, &streamID);
// Notify AVDECC Msg of the state change.
openavbAvdeccMsgClntNotifyCurrentState(pTLState);
}
else {
AVB_LOGF_WARNING("Failed to connect to endpoint "STREAMID_FORMAT, STREAMID_ARGS(&streamID));
}
if (pTLState->pPvtListenerData) {
free(pTLState->pPvtListenerData);
pTLState->pPvtListenerData = NULL;
}
AVB_TRACE_EXIT(AVB_TRACE_TL);
}
// Called from openavbTLThreadFn() which is started from openavbTLRun()
void openavbTLRunTalker(tl_state_t *pTLState)
{
AVB_TRACE_ENTRY(AVB_TRACE_TL);
if (!pTLState) {
AVB_LOG_ERROR("Invalid TLState");
AVB_TRACE_EXIT(AVB_TRACE_TL);
return;
}
pTLState->pPvtTalkerData = calloc(1, sizeof(talker_data_t));
if (!pTLState->pPvtTalkerData) {
AVB_LOG_WARNING("Failed to allocate talker data.");
return;
}
// Create Stats Mutex
{
MUTEX_ATTR_HANDLE(mta);
MUTEX_ATTR_INIT(mta);
MUTEX_ATTR_SET_TYPE(mta, MUTEX_ATTR_TYPE_DEFAULT);
MUTEX_ATTR_SET_NAME(mta, "TLStatsMutex");
MUTEX_CREATE_ERR();
MUTEX_CREATE(pTLState->statsMutex, mta);
MUTEX_LOG_ERR("Could not create/initialize 'TLStatsMutex' mutex");
}
/* If using endpoint register talker,
else register with tpsec */
pTLState->bConnected = openavbTLRunTalkerInit(pTLState);
if (pTLState->bConnected) {
bool bServiceIPC;
// Do until we are stopped or loose connection to endpoint
while (pTLState->bRunning && pTLState->bConnected) {
// Talk (or just sleep if not streaming.)
bServiceIPC = talkerDoStream(pTLState);
// TalkerDoStream() returns TRUE once per second,
// so that we can service our IPC at that low rate.
if (bServiceIPC) {
// Look for messages from endpoint. Don't block (timeout=0)
if (!openavbEptClntService(pTLState->endpointHandle, 0)) {
AVB_LOGF_WARNING("Lost connection to endpoint, will retry "STREAMID_FORMAT, STREAMID_ARGS(&(((talker_data_t *)pTLState->pPvtTalkerData)->streamID)));
pTLState->bConnected = FALSE;
pTLState->endpointHandle = 0;
}
}
}
// Stop streaming
talkerStopStream(pTLState);
{
MUTEX_CREATE_ERR();
MUTEX_DESTROY(pTLState->statsMutex); // Destroy Stats Mutex
MUTEX_LOG_ERR("Error destroying mutex");
}
// withdraw our talker registration
if (pTLState->bConnected)
openavbEptClntStopStream(pTLState->endpointHandle, &(((talker_data_t *)pTLState->pPvtTalkerData)->streamID));
openavbTLRunTalkerFinish(pTLState);
}
else {
AVB_LOGF_WARNING("Failed to connect to endpoint"STREAMID_FORMAT, STREAMID_ARGS(&(((talker_data_t *)pTLState->pPvtTalkerData)->streamID)));
}
if (pTLState->pPvtTalkerData) {
free(pTLState->pPvtTalkerData);
pTLState->pPvtTalkerData = NULL;
}
AVB_TRACE_EXIT(AVB_TRACE_TL);
}
// Returns TRUE, to say we're connected and registers tspec with FQTSS tspec should be initialized
bool openavbTLRunTalkerInit(tl_state_t *pTLState)
{
openavb_tl_cfg_t *pCfg = &pTLState->cfg;
talker_data_t *pTalkerData = pTLState->pPvtTalkerData;
//avtp_stream_t *pStream = (avtp_stream_t *)(pTalkerData->avtpHandle);
strncpy(pTalkerData->ifname, pCfg->ifname, IFNAMSIZ);
// CORE_TODO: It would be good to have some parts of endpoint moved into non-endpoint general code to handle some the stream
// configuration values.
// strncpy(pTalkerData->ifname, pCfg->ifname, IFNAMSIZ);
if (pCfg->stream_addr.mac) {
memcpy(pTalkerData->streamID.addr, pCfg->stream_addr.mac, ETH_ALEN);
}else {
AVB_LOG_WARNING("Stream Address Not Set");
}
pTalkerData->streamID.uniqueID = pCfg->stream_uid;
if (pCfg->sr_class == SR_CLASS_A) {
pTalkerData->classRate = 8000;
pTalkerData->vlanID = pCfg->vlan_id == VLAN_NULL ?
SR_CLASS_A_DEFAULT_VID : pCfg->vlan_id;
pTalkerData->vlanPCP = SR_CLASS_A_DEFAULT_PRIORITY;
}
else if (pCfg->sr_class == SR_CLASS_B) {
pTalkerData->classRate = 4000;
pTalkerData->vlanID = pCfg->vlan_id == VLAN_NULL ?
SR_CLASS_B_DEFAULT_VID : pCfg->vlan_id;
pTalkerData->vlanPCP = SR_CLASS_B_DEFAULT_PRIORITY;
}
memcpy(&pTalkerData->destAddr, &pCfg->dest_addr.mac->ether_addr_octet, ETH_ALEN);
unsigned int maxBitrate = 0;
if (pCfg->intf_cb.intf_get_src_bitrate_cb != NULL) {
maxBitrate = pCfg->intf_cb.intf_get_src_bitrate_cb(pTLState->pMediaQ);
}
if (maxBitrate > 0) {
if (pCfg->map_cb.map_set_src_bitrate_cb != NULL) {
pCfg->map_cb.map_set_src_bitrate_cb(pTLState->pMediaQ, maxBitrate);
}
if (pCfg->map_cb.map_get_max_interval_frames_cb != NULL) {
unsigned int map_intv_frames = pCfg->map_cb.map_get_max_interval_frames_cb(pTLState->pMediaQ, pTLState->cfg.sr_class);
pCfg->max_interval_frames = map_intv_frames > 0 ? map_intv_frames : pCfg->max_interval_frames;
}
}
pTalkerData->tSpec.maxIntervalFrames = pCfg->max_interval_frames;
pTalkerData->tSpec.maxFrameSize = pCfg->map_cb.map_max_data_size_cb(pTLState->pMediaQ);
// TODO_COREAVB : This wakeRate should also be set in the endpoint case and removed from the tasker.c start stream
if (!pCfg->map_cb.map_transmit_interval_cb(pTLState->pMediaQ)) {
pTalkerData->wakeRate = pTalkerData->classRate / pCfg->batch_factor;
}
else {
// Override the class observation interval with the one provided by the mapping module.
pTalkerData->wakeRate = pCfg->map_cb.map_transmit_interval_cb(pTLState->pMediaQ) / pCfg->batch_factor;
}
if_info_t ifinfo;
openavbCheckInterface(pTalkerData->ifname, &ifinfo);
pTalkerData->fwmark = openavbQmgrAddStream((SRClassIdx_t)pCfg->sr_class,
pTalkerData->wakeRate,
pTalkerData->tSpec.maxIntervalFrames,
pTalkerData->tSpec.maxFrameSize);
if (pTalkerData->fwmark == INVALID_FWMARK)
return FALSE;
AVB_LOGF_INFO("Dest Addr: "ETH_FORMAT, ETH_OCTETS(pTalkerData->destAddr));
AVB_LOGF_INFO("Starting stream: "STREAMID_FORMAT, STREAMID_ARGS(&pTalkerData->streamID));
talkerStartStream(pTLState);
return TRUE;
}
// Get a RX frame
U8* ringRawsockGetRxFrame(void *pvRawsock, U32 timeout, unsigned int *offset, unsigned int *len)
{
AVB_TRACE_ENTRY(AVB_TRACE_RAWSOCK_DETAIL);
ring_rawsock_t *rawsock = (ring_rawsock_t*)pvRawsock;
if (!VALID_RX_RAWSOCK(rawsock)) {
AVB_LOG_ERROR("Getting RX frame; invalid arguments");
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK_DETAIL);
return NULL;
}
if (rawsock->buffersOut >= rawsock->frameCount) {
AVB_LOG_ERROR("Too many RX buffers in use");
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK_DETAIL);
return NULL;
}
// Get pointer to active buffer in ring
volatile struct tpacket2_hdr *pHdr =
(struct tpacket2_hdr*)(rawsock->pMem
+ (rawsock->blockIndex * rawsock->blockSize)
+ (rawsock->bufferIndex * rawsock->bufferSize));
volatile U8 *pBuffer = (U8*)pHdr + rawsock->bufHdrSize;
AVB_LOGF_VERBOSE("block=%d, buffer=%d, out=%d, pBuffer=%p, pHdr=%p",
rawsock->blockIndex, rawsock->bufferIndex, rawsock->buffersOut,
pBuffer, pHdr);
// Check if buffer ready for user
// In receive mode, we want TP_STATUS_USER flag set
if ((pHdr->tp_status & TP_STATUS_USER) == 0)
{
struct timespec ts, *pts = NULL;
struct pollfd pfd;
// Use poll to wait for "ready to read" condition
// Poll even if our timeout is 0 - to catch the case where
// kernel is writing to the wrong slot (see below.)
if (timeout != OPENAVB_RAWSOCK_BLOCK) {
ts.tv_sec = timeout / MICROSECONDS_PER_SECOND;
ts.tv_nsec = (timeout % MICROSECONDS_PER_SECOND) * NANOSECONDS_PER_USEC;
pts = &ts;
}
pfd.fd = rawsock->sock;
pfd.events = POLLIN;
pfd.revents = 0;
int ret = ppoll(&pfd, 1, pts, NULL);
if (ret < 0) {
if (errno != EINTR) {
AVB_LOGF_ERROR("Getting RX frame; poll failed: %s", strerror(errno));
}
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK_DETAIL);
return NULL;
}
if ((pfd.revents & POLLIN) == 0) {
// timeout
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK_DETAIL);
return NULL;
}
if ((pHdr->tp_status & TP_STATUS_USER) == 0) {
// Hmmm, this is unexpected. poll indicated that the
// socket was ready to read, but the slot in the TX ring
// that we're looking for the kernel to fill isn't filled.
// If there aren't any RX buffers held by the application,
// we can try to fix this sticky situation...
if (rawsock->buffersOut == 0) {
// Scan forward through the RX ring, and look for a
// buffer that's ready for us to read. The kernel has
// a bad habit of not starting at the beginning of the
// ring when the listener process is restarted.
int nSkipped = 0;
while((pHdr->tp_status & TP_STATUS_USER) == 0) {
// Move to next slot in ring.
// (Increment buffer/block indexes)
if (++(rawsock->bufferIndex) >= (rawsock->frameCount/rawsock->blockCount)) {
rawsock->bufferIndex = 0;
if (++(rawsock->blockIndex) >= rawsock->blockCount) {
rawsock->blockIndex = 0;
}
}
// Adjust pHdr, pBuffer to point to the new slot
pHdr = (struct tpacket2_hdr*)(rawsock->pMem
+ (rawsock->blockIndex * rawsock->blockSize)
+ (rawsock->bufferIndex * rawsock->bufferSize));
pBuffer = (U8*)pHdr + rawsock->bufHdrSize;
// If we've scanned all the way around the ring, bail out.
if (++nSkipped > rawsock->frameCount) {
AVB_LOG_WARNING("Getting RX frame; no frame after poll");
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK_DETAIL);
return NULL;
}
}
// We found a slot that's ready. Hopefully, we're good now.
AVB_LOGF_WARNING("Getting RX frame; skipped %d empty slots (rawsock=%p)", nSkipped, rawsock);
}
else {
AVB_LOG_WARNING("Getting RX frame; no frame after poll");
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK_DETAIL);
return NULL;
}
}
}
AVB_LOGF_VERBOSE("Buffer status=0x%4.4lx", (unsigned long)pHdr->tp_status);
if (pHdr->tp_status & TP_STATUS_COPY) {
AVB_LOG_WARNING("Frame too big for receive buffer");
}
// Check the "losing" flag. That indicates that the ring is full,
// and the kernel had to toss some frames. There is no "winning" flag.
if ((pHdr->tp_status & TP_STATUS_LOSING)) {
if (!rawsock->bLosing) {
AVB_LOG_WARNING("Getting RX frame; mmap buffers full");
rawsock->bLosing = TRUE;
}
}
else {
rawsock->bLosing = FALSE;
}
// increment indexes for next time
if (++(rawsock->bufferIndex) >= (rawsock->frameCount/rawsock->blockCount)) {
rawsock->bufferIndex = 0;
if (++(rawsock->blockIndex) >= rawsock->blockCount) {
rawsock->blockIndex = 0;
}
}
// Remember that the client has another buffer
rawsock->buffersOut += 1;
if (pHdr->tp_snaplen < pHdr->tp_len) {
#if (AVB_LOG_LEVEL >= AVB_LOG_LEVEL_VERBOSE)
AVB_LOGF_WARNING("Getting RX frame; partial frame ignored (len %d, snaplen %d)", pHdr->tp_len, pHdr->tp_snaplen);
AVB_LOG_BUFFER(AVB_LOG_LEVEL_VERBOSE, (const U8 *) pBuffer + (pHdr->tp_mac - rawsock->bufHdrSize), pHdr->tp_len, 16);
#else
IF_LOG_INTERVAL(1000) AVB_LOGF_WARNING("Getting RX frame; partial frame ignored (len %d, snaplen %d)", pHdr->tp_len, pHdr->tp_snaplen);
#endif
ringRawsockRelRxFrame(rawsock, (U8*)pBuffer);
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK_DETAIL);
return NULL;
}
// Return pointer to the buffer and length
*offset = pHdr->tp_mac - rawsock->bufHdrSize;
*len = pHdr->tp_snaplen;
AVB_LOGF_VERBOSE("Good RX frame (len %d, snaplen %d)", pHdr->tp_len, pHdr->tp_snaplen);
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK_DETAIL);
return (U8*)pBuffer;
}
/* Talker callback comes from endpoint, to indicate when listeners
* come and go. We may need to start or stop the talker thread.
*/
void openavbEptClntNotifyTlkrOfSrpCb(int endpointHandle,
AVBStreamID_t *streamID,
char *ifname,
U8 destAddr[],
openavbSrpLsnrDeclSubtype_t lsnrDecl,
U8 srClass,
U32 classRate,
U16 vlanID,
U8 priority,
U16 fwmark)
{
AVB_TRACE_ENTRY(AVB_TRACE_TL);
tl_state_t *pTLState = TLHandleListGet(endpointHandle);
talker_data_t *pTalkerData = pTLState->pPvtTalkerData;
if (!pTLState) {
AVB_LOG_WARNING("Unable to get talker from endpoint handle.");
return;
}
// If not a talker, ignore this callback.
if (pTLState->cfg.role != AVB_ROLE_TALKER) {
AVB_LOG_DEBUG("Ignoring Talker callback");
return;
}
AVB_LOGF_DEBUG("%s streaming=%d, lsnrDecl=%d", __FUNCTION__, pTLState->bStreaming, lsnrDecl);
openavb_tl_cfg_t *pCfg = &pTLState->cfg;
if (!pTLState->bStreaming) {
if (lsnrDecl == openavbSrp_LDSt_Ready
|| lsnrDecl == openavbSrp_LDSt_Ready_Failed) {
// Save the data provided by endpoint/SRP
if (!pCfg->ifname[0]) {
strncpy(pTalkerData->ifname, ifname, IFNAMSIZ);
} else {
strncpy(pTalkerData->ifname, pCfg->ifname, IFNAMSIZ);
}
memcpy(&pTalkerData->streamID, streamID, sizeof(AVBStreamID_t));
memcpy(&pTalkerData->destAddr, destAddr, ETH_ALEN);
pTalkerData->srClass = srClass;
pTalkerData->classRate = classRate;
pTalkerData->vlanID = vlanID;
pTalkerData->vlanPCP = priority;
pTalkerData->fwmark = fwmark;
// We should start streaming
AVB_LOGF_INFO("Starting stream: "STREAMID_FORMAT, STREAMID_ARGS(streamID));
talkerStartStream(pTLState);
}
else if (lsnrDecl == openavbSrp_LDSt_Stream_Info) {
// Stream information is available does NOT mean listener is ready. Stream not started yet.
if (!pCfg->ifname[0]) {
strncpy(pTalkerData->ifname, ifname, IFNAMSIZ);
} else {
strncpy(pTalkerData->ifname, pCfg->ifname, IFNAMSIZ);
}
memcpy(&pTalkerData->streamID, streamID, sizeof(AVBStreamID_t));
memcpy(&pTalkerData->destAddr, destAddr, ETH_ALEN);
pTalkerData->srClass = srClass;
pTalkerData->classRate = classRate;
pTalkerData->vlanID = vlanID;
pTalkerData->vlanPCP = priority;
pTalkerData->fwmark = fwmark;
}
}
else {
if (lsnrDecl != openavbSrp_LDSt_Ready
&& lsnrDecl != openavbSrp_LDSt_Ready_Failed) {
// Nobody is listening any more
AVB_LOGF_INFO("Stopping stream: "STREAMID_FORMAT, STREAMID_ARGS(streamID));
talkerStopStream(pTLState);
}
}
// Let the AVDECC Msg server know our current stream ID, in case it was updated by MAAP.
if (pTLState->avdeccMsgHandle != AVB_AVDECC_MSG_HANDLE_INVALID) {
if (!openavbAvdeccMsgClntTalkerStreamID(pTLState->avdeccMsgHandle,
pTalkerData->srClass, pTalkerData->streamID.addr, pTalkerData->streamID.uniqueID,
pTalkerData->destAddr, pTalkerData->vlanID)) {
AVB_LOG_ERROR("openavbAvdeccMsgClntTalkerStreamID() failed");
}
}
AVB_TRACE_EXIT(AVB_TRACE_TL);
}
int avbEndpointLoop(void)
{
AVB_TRACE_ENTRY(AVB_TRACE_ENDPOINT);
int retVal = -1;
openavbRC rc = OPENAVB_SUCCESS;
do {
if (!x_cfg.bypassAsCapableCheck && (startPTP() < 0)) {
// make sure ptp, a separate process, starts and is using the same interface as endpoint
AVB_LOG_ERROR("PTP failed to start - Exiting");
break;
} else if(x_cfg.bypassAsCapableCheck) {
AVB_LOG_WARNING(" ");
AVB_LOG_WARNING("Configuration 'gptp_asCapable_not_required = 1' is set.");
AVB_LOG_WARNING("This configuration bypasses the requirement for gPTP");
AVB_LOG_WARNING("and openavb_gptp is not started automatically.");
AVB_LOG_WARNING("An appropriate ptp MUST be started separately.");
AVB_LOG_WARNING("Any network which does not use ptp to synchronize time");
AVB_LOG_WARNING("on each and every network device is NOT an AVB network.");
AVB_LOG_WARNING("Such a network WILL NOT FUNCTION PROPERLY.");
AVB_LOG_WARNING(" ");
}
x_streamList = NULL;
if (!openavbQmgrInitialize(x_cfg.fqtss_mode, x_cfg.ifindex, x_cfg.ifname, x_cfg.mtu, x_cfg.link_kbit, x_cfg.nsr_kbit)) {
AVB_LOG_ERROR("Failed to initialize QMgr");
break;
}
if (!openavbMaapInitialize(x_cfg.ifname, x_cfg.maapPort, &(x_cfg.maap_preferred), maapRestartCallback)) {
AVB_LOG_ERROR("Failed to initialize MAAP");
openavbQmgrFinalize();
break;
}
if (!openavbShaperInitialize(x_cfg.ifname, x_cfg.shaperPort)) {
AVB_LOG_ERROR("Failed to initialize Shaper");
openavbMaapFinalize();
openavbQmgrFinalize();
break;
}
if(!x_cfg.noSrp) {
// Initialize SRP
rc = openavbSrpInitialize(strmAttachCb, strmRegCb, x_cfg.ifname, x_cfg.link_kbit, x_cfg.bypassAsCapableCheck);
} else {
rc = OPENAVB_SUCCESS;
AVB_LOG_WARNING(" ");
AVB_LOG_WARNING("Configuration 'preconfigured = 1' is set.");
AVB_LOG_WARNING("SRP is disabled. Streams MUST be configured manually");
AVB_LOG_WARNING("on each and every device in the network, without exception.");
AVB_LOG_WARNING("AN AVB NETWORK WILL NOT FUNCTION AS EXPECTED UNLESS ALL");
AVB_LOG_WARNING("STREAMS ARE PROPERLY CONFIGURED ON ALL NETWORK DEVICES.");
AVB_LOG_WARNING(" ");
}
if (!IS_OPENAVB_SUCCESS(rc)) {
AVB_LOG_ERROR("Failed to initialize SRP");
openavbShaperFinalize();
openavbMaapFinalize();
openavbQmgrFinalize();
break;
}
if (openavbEndpointServerOpen()) {
retVal = 0;
while (endpointRunning) {
openavbEptSrvrService();
}
openavbEndpointServerClose();
}
if(!x_cfg.noSrp) {
// Shutdown SRP
openavbSrpShutdown();
}
openavbShaperFinalize();
openavbMaapFinalize();
openavbQmgrFinalize();
} while (0);
if (!x_cfg.bypassAsCapableCheck && (stopPTP() < 0)) {
AVB_LOG_WARNING("Failed to execute PTP stop command: killall -s SIGINT openavb_gptp");
}
AVB_TRACE_EXIT(AVB_TRACE_ENDPOINT);
return retVal;
}
