clock.hpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #pragma once
00006 
00007 #include <cassert>
00008 #include <ctime>
00009 #include <cstdint>
00010 
00011 #include <unistd.h>
00012 #include <sys/time.h>
00013 #include <sys/types.h>
00014 
00015 #include <uavcan/driver/system_clock.hpp>
00016 #include <uavcan_linux/exception.hpp>
00017 
00018 namespace uavcan_linux
00019 {
00020 /**
00021  * Different adjustment modes can be used for time synchronization
00022  */
00023 enum class ClockAdjustmentMode
00024 {
00025     SystemWide,      ///< Adjust the clock globally for the whole system; requires root privileges
00026     PerDriverPrivate ///< Adjust the clock only for the current driver instance
00027 };
00028 
00029 /**
00030  * Linux system clock driver.
00031  * Requires librt.
00032  */
00033 class SystemClock : public uavcan::ISystemClock
00034 {
00035     uavcan::UtcDuration private_adj_;
00036     uavcan::UtcDuration gradual_adj_limit_;
00037     const ClockAdjustmentMode adj_mode_;
00038     std::uint64_t step_adj_cnt_;
00039     std::uint64_t gradual_adj_cnt_;
00040 
00041     static constexpr std::int64_t Int1e6   = 1000000;
00042     static constexpr std::uint64_t UInt1e6 = 1000000;
00043 
00044     bool performStepAdjustment(const uavcan::UtcDuration adjustment)
00045     {
00046         step_adj_cnt_++;
00047         const std::int64_t usec = adjustment.toUSec();
00048         timeval tv;
00049         if (gettimeofday(&tv, NULL) != 0)
00050         {
00051             return false;
00052         }
00053         tv.tv_sec  += usec / Int1e6;
00054         tv.tv_usec += usec % Int1e6;
00055         return settimeofday(&tv, nullptr) == 0;
00056     }
00057 
00058     bool performGradualAdjustment(const uavcan::UtcDuration adjustment)
00059     {
00060         gradual_adj_cnt_++;
00061         const std::int64_t usec = adjustment.toUSec();
00062         timeval tv;
00063         tv.tv_sec  = usec / Int1e6;
00064         tv.tv_usec = usec % Int1e6;
00065         return adjtime(&tv, nullptr) == 0;
00066     }
00067 
00068 public:
00069     /**
00070      * By default, the clock adjustment mode will be selected automatically - global if root, private otherwise.
00071      */
00072     explicit SystemClock(ClockAdjustmentMode adj_mode = detectPreferredClockAdjustmentMode())
00073         : gradual_adj_limit_(uavcan::UtcDuration::fromMSec(4000))
00074         , adj_mode_(adj_mode)
00075         , step_adj_cnt_(0)
00076         , gradual_adj_cnt_(0)
00077     { }
00078 
00079     /**
00080      * Returns monotonic timestamp from librt.
00081      * @throws uavcan_linux::Exception.
00082      */
00083     uavcan::MonotonicTime getMonotonic() const override
00084     {
00085         timespec ts;
00086         if (clock_gettime(CLOCK_MONOTONIC, &ts) != 0)
00087         {
00088             throw Exception("Failed to get monotonic time");
00089         }
00090         return uavcan::MonotonicTime::fromUSec(std::uint64_t(ts.tv_sec) * UInt1e6 + ts.tv_nsec / 1000);
00091     }
00092 
00093     /**
00094      * Returns wall time from gettimeofday().
00095      * @throws uavcan_linux::Exception.
00096      */
00097     uavcan::UtcTime getUtc() const override
00098     {
00099         timeval tv;
00100         if (gettimeofday(&tv, NULL) != 0)
00101         {
00102             throw Exception("Failed to get UTC time");
00103         }
00104         uavcan::UtcTime utc = uavcan::UtcTime::fromUSec(std::uint64_t(tv.tv_sec) * UInt1e6 + tv.tv_usec);
00105         if (adj_mode_ == ClockAdjustmentMode::PerDriverPrivate)
00106         {
00107             utc += private_adj_;
00108         }
00109         return utc;
00110     }
00111 
00112     /**
00113      * Adjusts the wall clock.
00114      * Behavior depends on the selected clock adjustment mode - @ref ClockAdjustmentMode.
00115      * Clock adjustment mode can be set only once via constructor.
00116      *
00117      * If the system wide adjustment mode is selected, two ways for performing adjustment exist:
00118      *  - Gradual adjustment using adjtime(), if the phase error is less than gradual adjustment limit.
00119      *  - Step adjustment using settimeofday(), if the phase error is above gradual adjustment limit.
00120      * The gradual adjustment limit can be configured at any time via the setter method.
00121      *
00122      * @throws uavcan_linux::Exception.
00123      */
00124     void adjustUtc(const uavcan::UtcDuration adjustment) override
00125     {
00126         if (adj_mode_ == ClockAdjustmentMode::PerDriverPrivate)
00127         {
00128             private_adj_ += adjustment;
00129         }
00130         else
00131         {
00132             assert(private_adj_.isZero());
00133             assert(!gradual_adj_limit_.isNegative());
00134 
00135             bool success = false;
00136             if (adjustment.getAbs() < gradual_adj_limit_)
00137             {
00138                 success = performGradualAdjustment(adjustment);
00139             }
00140             else
00141             {
00142                 success = performStepAdjustment(adjustment);
00143             }
00144             if (!success)
00145             {
00146                 throw Exception("Clock adjustment failed");
00147             }
00148         }
00149     }
00150 
00151     /**
00152      * Sets the maximum phase error to use adjtime().
00153      * If the phase error exceeds this value, settimeofday() will be used instead.
00154      */
00155     void setGradualAdjustmentLimit(uavcan::UtcDuration limit)
00156     {
00157         if (limit.isNegative())
00158         {
00159             limit = uavcan::UtcDuration();
00160         }
00161         gradual_adj_limit_ = limit;
00162     }
00163 
00164     uavcan::UtcDuration getGradualAdjustmentLimit() const { return gradual_adj_limit_; }
00165 
00166     ClockAdjustmentMode getAdjustmentMode() const { return adj_mode_; }
00167 
00168     /**
00169      * This is only applicable if the selected clock adjustment mode is private.
00170      * In system wide mode this method will always return zero duration.
00171      */
00172     uavcan::UtcDuration getPrivateAdjustment() const { return private_adj_; }
00173 
00174     /**
00175      * Statistics that allows to evaluate clock sync preformance.
00176      */
00177     std::uint64_t getStepAdjustmentCount() const { return step_adj_cnt_; }
00178     std::uint64_t getGradualAdjustmentCount() const { return gradual_adj_cnt_; }
00179     std::uint64_t getAdjustmentCount() const
00180     {
00181         return getStepAdjustmentCount() + getGradualAdjustmentCount();
00182     }
00183 
00184     /**
00185      * This static method decides what is the optimal clock sync adjustment mode for the current configuration.
00186      * It selects system wide mode if the application is running as root; otherwise it prefers
00187      * the private adjustment mode because the system wide mode requires root privileges.
00188      */
00189     static ClockAdjustmentMode detectPreferredClockAdjustmentMode()
00190     {
00191         const bool godmode = geteuid() == 0;
00192         return godmode ? ClockAdjustmentMode::SystemWide : ClockAdjustmentMode::PerDriverPrivate;
00193     }
00194 };
00195 
00196 }