virtual net_result nrecv( LinkLayerAddress *addr, uint8_t *payload, size_t &length ) {
packet_addr_t dest;
packet_error_t pferror = recvFrame( handle, &dest, payload, length );
if( pferror != PACKET_NO_ERROR && pferror != PACKET_RECVTIMEOUT_ERROR ) return net_fatal;
if( pferror == PACKET_RECVTIMEOUT_ERROR ) return net_trfail;
*addr = LinkLayerAddress( dest.addr );
return net_succeed;
}
uint32_t findLinkSpeed( LinkLayerAddress *local_addr )
{
ULONG ret_sz;
char *buffer;
PIP_ADAPTER_ADDRESSES pAddr;
ULONG err;
uint32_t ret;
buffer = (char *) malloc((size_t)15000);
ret_sz = 15000;
pAddr = (PIP_ADAPTER_ADDRESSES)buffer;
err = GetAdaptersAddresses( AF_UNSPEC, 0, NULL, pAddr, &ret_sz );
for (; pAddr != NULL; pAddr = pAddr->Next)
{
//fprintf(stderr, "** here : %p\n", pAddr);
if (pAddr->PhysicalAddressLength == ETHER_ADDR_OCTETS &&
*local_addr == LinkLayerAddress(pAddr->PhysicalAddress))
{
break;
}
}
if (pAddr == NULL)
return INVALID_LINKSPEED;
switch ( pAddr->ReceiveLinkSpeed / WIN_LINKSPEED_MULT )
{
default:
GPTP_LOG_ERROR("Can't find link speed, %llu", pAddr->ReceiveLinkSpeed);
ret = INVALID_LINKSPEED;
break;
case LINKSPEED_1G:
ret = LINKSPEED_1G;
break;
case LINKSPEED_100MB:
ret = LINKSPEED_100MB;
break;
}
delete buffer;
return ret;
}
bool WindowsTimestamper::HWTimestamper_init( InterfaceLabel *iface_label, OSNetworkInterface *net_iface ) {
char network_card_id[64];
LinkLayerAddress *addr = dynamic_cast<LinkLayerAddress *>(iface_label);
if( addr == NULL ) return false;
PIP_ADAPTER_INFO pAdapterInfo;
IP_ADAPTER_INFO AdapterInfo[32]; // Allocate information for up to 32 NICs
DWORD dwBufLen = sizeof(AdapterInfo); // Save memory size of buffer
DWORD dwStatus = GetAdaptersInfo( AdapterInfo, &dwBufLen );
if( dwStatus != ERROR_SUCCESS ) return false;
for( pAdapterInfo = AdapterInfo; pAdapterInfo != NULL; pAdapterInfo = pAdapterInfo->Next ) {
if( pAdapterInfo->AddressLength == ETHER_ADDR_OCTETS && *addr == LinkLayerAddress( pAdapterInfo->Address )) {
break;
}
}
if( pAdapterInfo == NULL ) return false;
if( strstr( pAdapterInfo->Description, NANOSECOND_CLOCK_PART_DESCRIPTION ) != NULL ) {
netclock_hz.QuadPart = NETCLOCK_HZ_NANO;
} else {
netclock_hz.QuadPart = NETCLOCK_HZ_OTHER;
}
fprintf( stderr, "Adapter UID: %s(%s)\n", pAdapterInfo->AdapterName, pAdapterInfo->Description+(strlen(pAdapterInfo->Description)-7));
PLAT_strncpy( network_card_id, NETWORK_CARD_ID_PREFIX, 63 );
PLAT_strncpy( network_card_id+strlen(network_card_id), pAdapterInfo->AdapterName, 63-strlen(network_card_id) );
miniport = CreateFile( network_card_id,
GENERIC_READ | GENERIC_WRITE,
FILE_SHARE_READ | FILE_SHARE_WRITE,
NULL, OPEN_EXISTING, 0, NULL );
if( miniport == INVALID_HANDLE_VALUE ) return false;
tsc_hz.QuadPart = getTSCFrequency( 1000 );
if( tsc_hz.QuadPart == 0 ) {
return false;
}
return true;
}
net_result LinuxNetworkInterface::nrecv
( LinkLayerAddress *addr, uint8_t *payload, size_t &length ) {
fd_set readfds;
int err;
struct msghdr msg;
struct cmsghdr *cmsg;
struct {
struct cmsghdr cm;
char control[256];
} control;
struct sockaddr_ll remote;
struct iovec sgentry;
net_result ret = net_succeed;
bool got_net_lock;
LinuxTimestamperGeneric *gtimestamper;
struct timeval timeout = { 0, 16000 }; // 16 ms
if( !net_lock.lock( &got_net_lock )) {
fprintf( stderr, "A Failed to lock mutex\n" );
return net_fatal;
}
if( !got_net_lock ) {
return net_trfail;
}
FD_ZERO( &readfds );
FD_SET( sd_event, &readfds );
err = select( sd_event+1, &readfds, NULL, NULL, &timeout );
if( err == 0 ) {
ret = net_trfail;
goto done;
} else if( err == -1 ) {
if( err == EINTR ) {
// Caught signal
XPTPD_ERROR( "select() recv signal" );
ret = net_trfail;
goto done;
} else {
XPTPD_ERROR( "select() failed" );
ret = net_fatal;
goto done;
}
} else if( !FD_ISSET( sd_event, &readfds )) {
ret = net_trfail;
goto done;
}
memset( &msg, 0, sizeof( msg ));
msg.msg_iov = &sgentry;
msg.msg_iovlen = 1;
sgentry.iov_base = payload;
sgentry.iov_len = length;
memset( &remote, 0, sizeof(remote));
msg.msg_name = (caddr_t) &remote;
msg.msg_namelen = sizeof( remote );
msg.msg_control = &control;
msg.msg_controllen = sizeof(control);
err = recvmsg( sd_event, &msg, 0 );
if( err < 0 ) {
if( errno == ENOMSG ) {
fprintf( stderr, "Got ENOMSG: %s:%d\n", __FILE__, __LINE__ );
ret = net_trfail;
goto done;
}
XPTPD_ERROR( "recvmsg() failed: %s", strerror(errno) );
ret = net_fatal;
goto done;
}
*addr = LinkLayerAddress( remote.sll_addr );
gtimestamper = dynamic_cast<LinuxTimestamperGeneric *>(timestamper);
if( err > 0 && !(payload[0] & 0x8) && gtimestamper != NULL ) {
/* Retrieve the timestamp */
cmsg = CMSG_FIRSTHDR(&msg);
while( cmsg != NULL ) {
if
( cmsg->cmsg_level == SOL_SOCKET &&
cmsg->cmsg_type == SO_TIMESTAMPING ) {
Timestamp latency( RX_PHY_TIME, 0, 0 );
struct timespec *ts_device, *ts_system;
Timestamp device, system;
ts_system = ((struct timespec *) CMSG_DATA(cmsg)) + 1;
system = tsToTimestamp( ts_system );
ts_device = ts_system + 1; device = tsToTimestamp( ts_device );
device = device - latency;
gtimestamper->pushRXTimestamp( &device );
break;
}
cmsg = CMSG_NXTHDR(&msg,cmsg);
}
}
length = err;
done:
if( !net_lock.unlock()) {
fprintf( stderr, "A Failed to unlock, %d\n", err );
return net_fatal;
}
return ret;
}
