diff options
Diffstat (limited to 'daemons/gptp/windows/daemon_cl/windows_hal.cpp')
| -rw-r--r-- | daemons/gptp/windows/daemon_cl/windows_hal.cpp | 180 |
1 files changed, 178 insertions, 2 deletions
diff --git a/daemons/gptp/windows/daemon_cl/windows_hal.cpp b/daemons/gptp/windows/daemon_cl/windows_hal.cpp index a05d473c..3d034e39 100644 --- a/daemons/gptp/windows/daemon_cl/windows_hal.cpp +++ b/daemons/gptp/windows/daemon_cl/windows_hal.cpp @@ -67,8 +67,184 @@ VOID CALLBACK WindowsTimerQueueHandler( PVOID arg_in, BOOLEAN ignore ) { ReleaseSRWLockExclusive( &arg->queue->retiredTimersLock ); } +inline uint64_t scale64(uint64_t i, uint32_t m, uint32_t n) +{ + uint64_t tmp, res, rem; + + rem = i % n; + i /= n; + + res = i * m; + tmp = rem * m; + + tmp /= n; + + return res + tmp; +} + +void WirelessTimestamperCallback( LPVOID arg ) +{ + WirelessTimestamperCallbackArg *larg = + (WirelessTimestamperCallbackArg *)arg; + WindowsWirelessTimestamper *timestamper = + dynamic_cast<WindowsWirelessTimestamper *> (larg->timestamper); + WirelessDialog tmp1, tmp2; + LinkLayerAddress *peer_addr = NULL; + + if (timestamper == NULL) + { + GPTP_LOG_ERROR( "Wrong timestamper type: %p", + larg->timestamper ); + return; + } + + switch( larg->iEvent_type ) + { + default: + case TIMINGMSMT_CONFIRM_EVENT: + tmp1.action_devclk = larg->event_data.confirm.T1; + tmp1.ack_devclk = larg->event_data.confirm.T4; + tmp1.dialog_token = (BYTE)larg->event_data.confirm.DialogToken; + GPTP_LOG_VERBOSE + ( "Got confirm, %hhu(%llu,%llu)", tmp1.dialog_token, + tmp1.action_devclk, tmp1.ack_devclk ); + peer_addr = new LinkLayerAddress + ( larg->event_data.confirm.PeerMACAddress ); + larg->timestamper-> + timingMeasurementConfirmCB( *peer_addr, &tmp1 ); + + break; + + case TIMINGMSMT_EVENT: + tmp1/*prev*/.action_devclk = larg->event_data.indication. + indication.T1; + tmp1/*prev*/.ack_devclk = larg->event_data.indication. + indication.T4; + tmp1/*prev*/.dialog_token = (BYTE)larg->event_data.indication. + indication.FollowUpDialogToken; + tmp2/*curr*/.action_devclk = larg->event_data.indication. + indication.T2; + tmp2/*curr*/.ack_devclk = larg->event_data.indication. + indication.T3; + tmp2/*curr*/.dialog_token = (BYTE)larg->event_data.indication. + indication.DialogToken; + GPTP_LOG_VERBOSE + ("Got indication, %hhu(%llu,%llu) %hhu(%llu,%llu)", + tmp1.dialog_token, tmp1.action_devclk, + tmp1.ack_devclk, tmp2.dialog_token, + tmp2.action_devclk, tmp2.ack_devclk); + peer_addr = new LinkLayerAddress(larg->event_data.indication. + indication.PeerMACAddress); + + larg->timestamper->timeMeasurementIndicationCB + ( *peer_addr, &tmp2, &tmp1, + larg->event_data.indication. + indication.PtpSpec.fwup_data, + larg->event_data.indication. + indication.WiFiVSpecHdr.Length - (sizeof(PTP_SPEC) - + 1)); + + break; + + case TIMINGMSMT_CORRELATEDTIME_EVENT: + timestamper->system_counter = + scale64( larg->event_data.ptm_wa.TSC, NS_PER_SECOND, + (uint32_t)timestamper->tsc_hz.QuadPart ); + timestamper->system_time.set64(timestamper->system_counter); + // Scale from TM timescale to nanoseconds + larg->event_data.ptm_wa.LocalClk *= 10; + timestamper->device_time.set64 + (larg->event_data.ptm_wa.LocalClk*10); + + break; + } + + delete peer_addr; +} + +net_result WindowsWirelessTimestamper::_requestTimingMeasurement +(TIMINGMSMT_REQUEST *timingmsmt_req) +{ + net_result ret = net_succeed; + + if (!adapter->initiateTimingRequest(timingmsmt_req)) { + GPTP_LOG_ERROR("Failed to send timing measurement request\n"); + ret = net_fatal; + } + + return ret; +} + +bool WindowsWirelessTimestamper::HWTimestamper_gettime +( Timestamp *system_time, + Timestamp * device_time, + uint32_t * local_clock, + uint32_t * nominal_clock_rate ) const +{ + bool refreshed = adapter->refreshCrossTimestamp(); + if (refreshed) + { + // We have a fresh cross-timestamp just use it + *system_time = this->system_time; + *device_time = this->device_time; + } else + { + // We weren't able to get a fresh timestamp, + // extrapolate from the last + LARGE_INTEGER tsc_now; + QueryPerformanceCounter(&tsc_now); + unsigned device_delta = (unsigned) + (((long double) (tsc_now.QuadPart - system_counter)) / + (((long double)tsc_hz.QuadPart) / 1000000000)); + device_delta = (unsigned)(device_delta*getPort()-> + getLocalSystemFreqOffset()); + system_time->set64((uint64_t) + (((long double)tsc_now.QuadPart) / + ((long double)tsc_hz.QuadPart / + 1000000000))); + device_time->set64(device_delta); + *device_time = *device_time + this->device_time; + } + + return true; +} + +bool WindowsWirelessTimestamper::HWTimestamper_init +(InterfaceLabel *iface_label, OSNetworkInterface *iface) +{ + uint8_t mac_addr_local[ETHER_ADDR_OCTETS]; + + if (!initialized) { + if (!adapter->initialize()) return false; + if (getPort()->getLocalAddr() == NULL) + return false; + + getPort()->getLocalAddr()->toOctetArray(mac_addr_local); + if (!adapter->attachAdapter(mac_addr_local)) { + return false; + } + + tsc_hz.QuadPart = getTSCFrequency(false); + if (tsc_hz.QuadPart == 0) { + return false; + } + + if (!adapter->registerTimestamper(this)) + return false; + } + + initialized = true; + return true; +} + +WindowsWirelessTimestamper::~WindowsWirelessTimestamper() { + if (adapter->deregisterTimestamper(this)) + adapter->shutdown(); + else + GPTP_LOG_INFO("Failed to shutdown time sync on adapter"); +} -bool WindowsTimestamper::HWTimestamper_init( InterfaceLabel *iface_label, OSNetworkInterface *net_iface ) { +bool WindowsEtherTimestamper::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; @@ -112,7 +288,7 @@ bool WindowsTimestamper::HWTimestamper_init( InterfaceLabel *iface_label, OSNetw NULL, OPEN_EXISTING, 0, NULL ); if( miniport == INVALID_HANDLE_VALUE ) return false; - tsc_hz.QuadPart = getTSCFrequency( 1000 ); + tsc_hz.QuadPart = getTSCFrequency( true ); if( tsc_hz.QuadPart == 0 ) { return false; } |