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); }
int main(int argc, char* argv[]) { GError *error = NULL; GOptionContext *context; context = g_option_context_new("- rawsock listenr"); g_option_context_add_main_entries(context, entries, NULL); if (!g_option_context_parse(context, &argc, &argv, &error)) { printf("error: %s\n", error->message); exit(1); } if (interface == NULL) { printf("error: must specify network interface\n"); exit(2); } void* rs = openavbRawsockOpen(interface, TRUE, FALSE, ethertype, 0, MAX_NUM_FRAMES); if (!rs) { printf("error: failed to open raw socket (are you root?)\n"); exit(3); } hdr_info_t hdr; U8 *pBuf, *pFrame, tmp8; U32 offset, len; U16 uid, i; long nTotal = 0, nRecv[NUM_STREAMS]; for (i = 0; i < NUM_STREAMS; i++) nRecv[i] = 0; struct timespec now, report; clock_gettime(CLOCK_MONOTONIC, &report); report.tv_sec += REPORT_SECONDS; while (bRunning) { pBuf = openavbRawsockGetRxFrame(rs, 1000, &offset, &len); if (pBuf) { pFrame = pBuf + offset; offset = openavbRawsockRxParseHdr(rs, pBuf, &hdr); if ((int)offset < 0) { printf("error parsing frame header"); } else { #ifndef UBUNTU if (hdr.ethertype == ETHERTYPE_8021Q) { // Oh! Need to look past the VLAN tag U16 vlan_bits = ntohs(*(U16 *)(pFrame + offset)); hdr.vlan = TRUE; hdr.vlan_vid = vlan_bits & 0x0FFF; hdr.vlan_pcp = (vlan_bits >> 13) & 0x0007; offset += 2; hdr.ethertype = ntohs(*(U16 *)(pFrame + offset)); offset += 2; } #endif if (hdr.ethertype == ETHERTYPE_AVTP) { //dumpFrame(pFrame + offset, len - offset, &hdr); // Look for stream data frames // (ignore control frames, including MAAP) tmp8 = *(pFrame + offset); if ((tmp8 & 0x80) == 0) { // Find the unique ID in the streamID uid = htons(*(U16*)(pFrame + offset + 10)); if (uid < NUM_STREAMS) nRecv[uid]++; nTotal++; } } } openavbRawsockRelRxFrame(rs, pBuf); } clock_gettime(CLOCK_MONOTONIC, &now); if (timespec_cmp(&now, &report) >= 0) { printf("total=%ld\t", nTotal); nTotal = 0; for (i = 0; i < NUM_STREAMS-1; i++) { if (nRecv[i] > 0) { printf("%d=%ld\t", i, nRecv[i]); nRecv[i] = 0; } } printf("\n"); report.tv_sec += REPORT_SECONDS; } }