// Send all packets that are ready (i.e. tell kernel to send them)
int ringRawsockSend(void *pvRawsock)
{
AVB_TRACE_ENTRY(AVB_TRACE_RAWSOCK_DETAIL);
ring_rawsock_t *rawsock = (ring_rawsock_t*)pvRawsock;
if (!VALID_TX_RAWSOCK(rawsock)) {
AVB_LOG_ERROR("Send; invalid argument");
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK_DETAIL);
return -1;
}
// Linux does something dumb to wait for frames to be sent.
// Without MSG_DONTWAIT, CPU usage is bad.
int flags = MSG_DONTWAIT;
int sent = send(rawsock->sock, NULL, 0, flags);
if (errno == EINTR) {
// ignore
}
else if (sent < 0) {
AVB_LOGF_ERROR("Send failed: %s", strerror(errno));
assert(0);
}
else {
AVB_LOGF_VERBOSE("Sent %d bytes, %d frames", sent, rawsock->buffersReady);
rawsock->buffersOut -= rawsock->buffersReady;
rawsock->buffersReady = 0;
}
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK_DETAIL);
return sent;
}
// 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;
}
// Pre-set the ethernet header information that will be used on TX frames
bool baseRawsockTxSetHdr(void *pvRawsock, hdr_info_t *pHdr)
{
AVB_TRACE_ENTRY(AVB_TRACE_RAWSOCK_DETAIL);
base_rawsock_t *rawsock = (base_rawsock_t*)pvRawsock;
if (!VALID_TX_RAWSOCK(rawsock)) {
AVB_LOG_ERROR("Setting TX header; invalid argument");
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK_DETAIL);
return FALSE;
}
// source address
if (pHdr->shost) {
memcpy(&(rawsock->ethHdr.notag.shost), pHdr->shost, ETH_ALEN);
}
// destination address
if (pHdr->dhost) {
memcpy(&(rawsock->ethHdr.notag.dhost), pHdr->dhost, ETH_ALEN);
}
// VLAN tag?
if (!pHdr->vlan) {
// No, set ethertype in normal location
rawsock->ethHdr.notag.ethertype = htons(rawsock->ethertype);
// and set ethernet header length
rawsock->ethHdrLen = sizeof(eth_hdr_t);
}
else {
// Add VLAN tag
AVB_LOGF_DEBUG("VLAN=%d pcp=%d vid=%d", pHdr->vlan_vid, pHdr->vlan_pcp, pHdr->vlan_vid);
// Build bitfield with vlan_pcp and vlan_vid.
// I think CFI bit is alway 0
u_int16_t bits = 0;
bits |= (pHdr->vlan_pcp << 13) & 0xE000;
bits |= pHdr->vlan_vid & 0x0FFF;
// Create VLAN tag
rawsock->ethHdr.tagged.vlan.tpip = htons(ETHERTYPE_VLAN);
rawsock->ethHdr.tagged.vlan.bits = htons(bits);
rawsock->ethHdr.tagged.ethertype = htons(rawsock->ethertype);
// and set ethernet header length
rawsock->ethHdrLen = sizeof(eth_vlan_hdr_t);
}
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK_DETAIL);
return TRUE;
}
// Release a TX frame, without marking it as ready to send
bool ringRawsockRelTxFrame(void *pvRawsock, U8 *pBuffer)
{
AVB_TRACE_ENTRY(AVB_TRACE_RAWSOCK_DETAIL);
ring_rawsock_t *rawsock = (ring_rawsock_t*)pvRawsock;
if (!VALID_TX_RAWSOCK(rawsock) || pBuffer == NULL) {
AVB_LOG_ERROR("Releasing TX frame; invalid argument");
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK_DETAIL);
return FALSE;
}
volatile struct tpacket2_hdr *pHdr = (struct tpacket2_hdr*)(pBuffer - rawsock->bufHdrSize);
AVB_LOGF_VERBOSE("pBuffer=%p, pHdr=%p", pBuffer, pHdr);
pHdr->tp_len = 0;
pHdr->tp_status = TP_STATUS_KERNEL;
rawsock->buffersOut -= 1;
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK_DETAIL);
return TRUE;
}
// Get a buffer from the ring to use for TX
U8* ringRawsockGetTxFrame(void *pvRawsock, bool blocking, unsigned int *len)
{
AVB_TRACE_ENTRY(AVB_TRACE_RAWSOCK_DETAIL);
ring_rawsock_t *rawsock = (ring_rawsock_t*)pvRawsock;
// Displays only warning when buffer busy after second try
int bBufferBusyReported = 0;
if (!VALID_TX_RAWSOCK(rawsock) || len == NULL) {
AVB_LOG_ERROR("Getting TX frame; bad arguments");
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK_DETAIL);
return NULL;
}
if (rawsock->buffersOut >= rawsock->frameCount) {
AVB_LOG_ERROR("Getting TX frame; too many TX buffers in use");
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK_DETAIL);
return NULL;
}
// Get pointer to next framebuf.
volatile struct tpacket2_hdr *pHdr =
(struct tpacket2_hdr*)(rawsock->pMem
+ (rawsock->blockIndex * rawsock->blockSize)
+ (rawsock->bufferIndex * rawsock->bufferSize));
// And pointer to portion of buffer to be filled with frame
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 send mode, we want to see TP_STATUS_AVAILABLE
while (pHdr->tp_status != TP_STATUS_AVAILABLE)
{
switch (pHdr->tp_status) {
case TP_STATUS_SEND_REQUEST:
case TP_STATUS_SENDING:
if (blocking) {
#if 0
// We should be able to poll on the socket to wait for the buffer to
// be ready, but it doesn't work (at least on 2.6.37).
// Keep this code, because it may work on newer kernels
// poll until tx buffer is ready
struct pollfd pfd;
pfd.fd = rawsock->sock;
pfd.events = POLLWRNORM;
pfd.revents = 0;
int ret = poll(&pfd, 1, -1);
if (ret < 0 && errno != EINTR) {
AVB_LOGF_DEBUG("getting TX frame; poll failed: %s", strerror(errno));
}
#else
// Can't poll, so sleep instead to avoid tight loop
if(0 == bBufferBusyReported) {
if(!rawsock->txOutOfBuffer) {
// Display this info only once just to let know that something like this happened
AVB_LOGF_INFO("Getting TX frame (sock=%d): TX buffer busy", rawsock->sock);
}
++rawsock->txOutOfBuffer;
++rawsock->txOutOfBufferCyclic;
} else if(1 == bBufferBusyReported) {
//Display this warning if buffer was busy more than once because it might influence late/lost
AVB_LOGF_WARNING("Getting TX frame (sock=%d): TX buffer busy after usleep(50) verify if there are any lost/late frames", rawsock->sock);
}
++bBufferBusyReported;
usleep(50);
#endif
}
else {
AVB_LOG_DEBUG("Non-blocking, return NULL");
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK_DETAIL);
return NULL;
}
break;
case TP_STATUS_WRONG_FORMAT:
default:
pHdr->tp_status = TP_STATUS_AVAILABLE;
break;
}
}
// Remind client how big the frame buffer is
if (len)
*len = rawsock->base.frameSize;
// increment indexes to point to next buffer
if (++(rawsock->bufferIndex) >= (rawsock->frameCount/rawsock->blockCount)) {
rawsock->bufferIndex = 0;
if (++(rawsock->blockIndex) >= rawsock->blockCount) {
rawsock->blockIndex = 0;
}
}
// increment the count of buffers held by client
rawsock->buffersOut += 1;
// warn if too many are out
if (rawsock->buffersOut >= (rawsock->frameCount * 4)/5) {
AVB_LOGF_WARNING("Getting TX frame; consider increasing buffers: count=%d, out=%d",
rawsock->frameCount, rawsock->buffersOut);
}
AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK_DETAIL);
return (U8*)pBuffer;
}
