bool openavbAdpOpenSocket(const char* ifname, U16 vlanID, U8 vlanPCP)
{
AVB_TRACE_ENTRY(AVB_TRACE_ADP);
hdr_info_t hdr;
#ifndef UBUNTU
// This is the normal case for most of our supported platforms
rxSock = openavbRawsockOpen(ifname, TRUE, FALSE, ETHERTYPE_8021Q, ADP_FRAME_LEN, ADP_NUM_BUFFERS);
#else
rxSock = openavbRawsockOpen(ifname, TRUE, FALSE, ETHERTYPE_AVTP, ADP_FRAME_LEN, ADP_NUM_BUFFERS);
#endif
txSock = openavbRawsockOpen(ifname, FALSE, TRUE, ETHERTYPE_AVTP, ADP_FRAME_LEN, ADP_NUM_BUFFERS);
if (txSock && rxSock
&& openavbRawsockGetAddr(txSock, ADDR_PTR(&intfAddr))
&& ether_aton_r(ADP_PROTOCOL_ADDR, &adpAddr)
&& openavbRawsockRxMulticast(rxSock, TRUE, ADDR_PTR(&adpAddr)))
{
if (!openavbRawsockRxAVTPSubtype(rxSock, OPENAVB_ADP_AVTP_SUBTYPE | 0x80)) {
AVB_LOG_DEBUG("RX AVTP Subtype not supported");
}
memset(&hdr, 0, sizeof(hdr_info_t));
hdr.shost = ADDR_PTR(&intfAddr);
hdr.dhost = ADDR_PTR(&adpAddr);
hdr.ethertype = ETHERTYPE_AVTP;
if (vlanID != 0 || vlanPCP != 0) {
hdr.vlan = TRUE;
hdr.vlan_pcp = vlanPCP;
hdr.vlan_vid = vlanID;
AVB_LOGF_DEBUG("VLAN pcp=%d vid=%d", hdr.vlan_pcp, hdr.vlan_vid);
}
if (!openavbRawsockTxSetHdr(txSock, &hdr)) {
AVB_LOG_ERROR("TX socket Header Failure");
openavbAdpCloseSocket();
AVB_TRACE_EXIT(AVB_TRACE_ADP);
return false;
}
AVB_TRACE_EXIT(AVB_TRACE_ADP);
return true;
}
AVB_LOG_ERROR("Invalid socket");
openavbAdpCloseSocket();
AVB_TRACE_EXIT(AVB_TRACE_ADP);
return false;
}
Res SACFill(Addr *p_o, SAC sac, Size size, Bool hasReservoirPermit)
{
Index i;
Count blockCount, j;
Size blockSize;
Addr p, fl;
Res res = ResOK; /* stop compiler complaining */
mps_sac_t esac;
AVER(p_o != NULL);
AVERT(SAC, sac);
AVER(size != 0);
AVERT(Bool, hasReservoirPermit);
esac = ExternalSACOfSAC(sac);
sacFind(&i, &blockSize, sac, size);
/* Check it's empty (in the future, there will be other cases). */
AVER(esac->_freelists[i]._count == 0);
/* Fill 1/3 of the cache for this class. */
blockCount = esac->_freelists[i]._count_max / 3;
/* Adjust size for the overlarge class. */
if (blockSize == SizeMAX)
/* .align: align 'cause some classes don't accept unaligned. */
blockSize = SizeAlignUp(size, PoolAlignment(sac->pool));
for (j = 0, fl = esac->_freelists[i]._blocks;
j <= blockCount; ++j) {
res = PoolAlloc(&p, sac->pool, blockSize, hasReservoirPermit);
if (res != ResOK)
break;
/* @@@@ ignoring shields for now */
*ADDR_PTR(Addr, p) = fl; fl = p;
}
/* If didn't get any, just return. */
if (j == 0) {
AVER(res != ResOK);
return res;
}
/* Take the last one off, and return it. */
esac->_freelists[i]._count = j - 1;
*p_o = fl;
/* @@@@ ignoring shields for now */
esac->_freelists[i]._blocks = *ADDR_PTR(Addr, fl);
return ResOK;
}
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);
}
static Bool sacFreeListBlockCheck(SACFreeListBlock fb)
{
Count j;
Addr cb;
/* nothing to check about size */
CHECKL(fb->_count <= fb->_count_max);
/* check the freelist has the right number of blocks */
for (j = 0, cb = fb->_blocks; j < fb->_count; ++j) {
CHECKL(cb != NULL);
/* @@@@ ignoring shields for now */
cb = *ADDR_PTR(Addr, cb);
}
CHECKL(cb == NULL);
return TRUE;
}
static void sacClassFlush(SAC sac, Index i, Size blockSize,
Count blockCount)
{
Addr cb, fl;
Count j;
mps_sac_t esac;
esac = ExternalSACOfSAC(sac);
for (j = 0, fl = esac->_freelists[i]._blocks;
j < blockCount; ++j) {
/* @@@@ ignoring shields for now */
cb = fl; fl = *ADDR_PTR(Addr, cb);
PoolFree(sac->pool, cb, blockSize);
}
esac->_freelists[i]._count -= blockCount;
esac->_freelists[i]._blocks = fl;
}
void SACEmpty(SAC sac, Addr p, Size size)
{
Index i;
Size blockSize;
mps_sac_t esac;
AVERT(SAC, sac);
AVER(p != NULL);
AVER(PoolHasAddr(sac->pool, p));
AVER(size > 0);
esac = ExternalSACOfSAC(sac);
sacFind(&i, &blockSize, sac, size);
/* Check it's full (in the future, there will be other cases). */
AVER(esac->_freelists[i]._count
== esac->_freelists[i]._count_max);
/* Adjust size for the overlarge class. */
if (blockSize == SizeMAX)
/* see .align */
blockSize = SizeAlignUp(size, PoolAlignment(sac->pool));
if (esac->_freelists[i]._count_max > 0) {
Count blockCount;
/* Flush 2/3 of the cache for this class. */
/* Computed as count - count/3, so that the rounding works out right. */
blockCount = esac->_freelists[i]._count;
blockCount -= esac->_freelists[i]._count / 3;
sacClassFlush(sac, i, blockSize, (blockCount > 0) ? blockCount : 1);
/* Leave the current one in the cache. */
esac->_freelists[i]._count += 1;
/* @@@@ ignoring shields for now */
*ADDR_PTR(Addr, p) = esac->_freelists[i]._blocks;
esac->_freelists[i]._blocks = p;
} else {
/* Free even the current one. */
PoolFree(sac->pool, p, blockSize);
}
}