Fix for bug 6691452

    common_time_server_packets.cpp \

    clock_recovery.cpp \

    common_clock.cpp \

    main.cpp

    main.cpp \

    utils.cpp

# Uncomment to enable vesbose logging and debug service.

#TIME_SERVICE_DEBUG=true

    local_clock_can_slew_ = local_clock_->initCheck() &&

                           (local_clock_->setLocalSlew(0) == OK);

    tgt_correction_ = 0;

    cur_correction_ = 0;

    // Precompute the max rate at which we are allowed to change the VCXO

    // control.

    uint64_t N = 0x10000ull * 1000ull;

    uint64_t D = local_clock_->getLocalFreq() * kMinFullRangeSlewChange_mSec;

    LinearTransform::reduce(&N, &D);

    while ((N > INT32_MAX) || (D > UINT32_MAX)) {

        N >>= 1;

        D >>= 1;

        LinearTransform::reduce(&N, &D);

    }

    time_to_cur_slew_.a_to_b_numer = static_cast<int32_t>(N);

    time_to_cur_slew_.a_to_b_denom = static_cast<uint32_t>(D);

    reset(true, true);

const int64_t ClockRecoveryLoop::control_thresh_ = 10000;

const float ClockRecoveryLoop::COmin = -100.0f;

const float ClockRecoveryLoop::COmax = 100.0f;

const uint32_t ClockRecoveryLoop::kMinFullRangeSlewChange_mSec = 300;

const int ClockRecoveryLoop::kSlewChangeStepPeriod_mSec = 10;

void ClockRecoveryLoop::reset(bool position, bool frequency) {

    Mutex::Autolock lock(&lock_);

    int64_t observed_common;

    int64_t delta;

    float delta_f, dCO;

    int32_t correction_cur;

    int32_t tgt_correction;

    if (OK != common_clock_->localToCommon(local_time, &observed_common)) {

        // Since we just checked to make certain that this conversion was valid,

    // Convert PPM to 16-bit int range. Add some guard band (-0.01) so we

    // don't get fp weirdness.

    correction_cur = CO * 327.66;

    tgt_correction = CO * 327.66;

    // If there was a change in the amt of correction to use, update the

    // system.

    if (correction_cur_ != correction_cur) {

        correction_cur_ = correction_cur;

        applySlew();

    }

    setTargetCorrection_l(tgt_correction);

    LOG_TS("clock_loop %lld %f %f %f %d\n", raw_delta, delta_f, CO, CObias, correction_cur);

    LOG_TS("clock_loop %lld %f %f %f %d\n", raw_delta, delta_f, CO, CObias, tgt_correction);

#ifdef TIME_SERVICE_DEBUG

    diag_thread_->pushDisciplineEvent(

            local_time,

            observed_common,

            nominal_common_time,

            correction_cur,

            tgt_correction,

            rtt);

#endif

        last_delta_valid_ = false;

        last_delta_ = 0;

        last_delta_f_ = 0.0;

        correction_cur_ = 0x0;

        CO = 0.0f;

        lastCObias = CObias = 0.0f;

        applySlew();

        setTargetCorrection_l(0);

        applySlew_l();

    }

    filter_wr_   = 0;

    filter_full_ = false;

}

void ClockRecoveryLoop::applySlew() {

void ClockRecoveryLoop::setTargetCorrection_l(int32_t tgt) {

    // When we make a change to the slew rate, we need to be careful to not

    // change it too quickly as it can anger some HDMI sinks out there, notably

    // some Sony panels from the 2010-2011 timeframe.  From experimenting with

    // some of these sinks, it seems like swinging from one end of the range to

    // another in less that 190mSec or so can start to cause trouble.  Adding in

    // a hefty margin, we limit the system to a full range sweep in no less than

    // 300mSec.

    if (tgt_correction_ != tgt) {

        int64_t now = local_clock_->getLocalTime();

        status_t res;

        tgt_correction_ = tgt;

        // Set up the transformation to figure out what the slew should be at

        // any given point in time in the future.

        time_to_cur_slew_.a_zero = now;

        time_to_cur_slew_.b_zero = cur_correction_;

        // Make sure the sign of the slope is headed in the proper direction.

        bool needs_increase = (cur_correction_ < tgt_correction_);

        bool is_increasing  = (time_to_cur_slew_.a_to_b_numer > 0);

        if (( needs_increase && !is_increasing) ||

            (!needs_increase &&  is_increasing)) {

            time_to_cur_slew_.a_to_b_numer = -time_to_cur_slew_.a_to_b_numer;

        }

        // Finally, figure out when the change will be finished and start the

        // slew operation.

        time_to_cur_slew_.doReverseTransform(tgt_correction_,

                                             &slew_change_end_time_);

        applySlew_l();

    }

}

bool ClockRecoveryLoop::applySlew_l() {

    bool ret = true;

    // If cur == tgt, there is no ongoing sleq rate change and we are already

    // finished.

    if (cur_correction_ == tgt_correction_)

        goto bailout;

    if (local_clock_can_slew_) {

        local_clock_->setLocalSlew(correction_cur_);

        int64_t now = local_clock_->getLocalTime();

        int64_t tmp;

        if (now >= slew_change_end_time_) {

            cur_correction_ = tgt_correction_;

            next_slew_change_timeout_.setTimeout(-1);

        } else {

            time_to_cur_slew_.doForwardTransform(now, &tmp);

            if (tmp > INT16_MAX)

                cur_correction_ = INT16_MAX;

            else if (tmp < INT16_MIN)

                cur_correction_ = INT16_MIN;

            else

                cur_correction_ = static_cast<int16_t>(tmp);

            next_slew_change_timeout_.setTimeout(kSlewChangeStepPeriod_mSec);

            ret = false;

        }

        local_clock_->setLocalSlew(cur_correction_);

    } else {

        // Since we are not actually changing the rate of a HW clock, we don't

        // need to worry to much about changing the slew rate so fast that we

        // anger any downstream HDMI devices.

        cur_correction_ = tgt_correction_;

        next_slew_change_timeout_.setTimeout(-1);

        // The SW clock recovery implemented by the common clock class expects

        // values expressed in PPM. CO is in ppm.

        common_clock_->setSlew(local_clock_->getLocalTime(), CO);

    }

bailout:

    return ret;

}

int ClockRecoveryLoop::applyRateLimitedSlew() {

    Mutex::Autolock lock(&lock_);

    int ret = next_slew_change_timeout_.msecTillTimeout();

    if (!ret) {

        if (applySlew_l())

            next_slew_change_timeout_.setTimeout(-1);

        ret = next_slew_change_timeout_.msecTillTimeout();

    }

    return ret;

}

}  // namespace android

#include "diag_thread.h"

#endif

#include "utils.h"

namespace android {

class CommonClock;

                             int64_t data_point_rtt);

    int32_t getLastErrorEstimate();

    // Applies the next step in any ongoing slew change operation.  Returns a

    // timeout suitable for use with poll/select indicating the number of mSec

    // until the next change should be applied.

    int applyRateLimitedSlew();

  private:

    // Tuned using the "Good Gain" method.

    static uint32_t findMinRTTNdx(DisciplineDataPoint* data, uint32_t count);

    void reset_l(bool position, bool frequency);

    void applySlew();

    void setTargetCorrection_l(int32_t tgt);

    bool applySlew_l();

    // The local clock HW abstraction we use as the basis for common time.

    LocalClock* local_clock_;

    int32_t last_delta_;

    float last_delta_f_;

    int32_t integrated_error_;

    int32_t correction_cur_;

    int32_t tgt_correction_;

    int32_t cur_correction_;

    LinearTransform time_to_cur_slew_;

    int64_t slew_change_end_time_;

    Timeout next_slew_change_timeout_;

    // Contoller Output.

    float CO;

    DisciplineDataPoint startup_filter_data_[kStartupFilterSize];

    uint32_t startup_filter_wr_;

    // Minimum number of milliseconds over which we allow a full range change

    // (from rail to rail) of the VCXO control signal.  This is the rate

    // limiting factor which keeps us from changing the clock rate so fast that

    // we get in trouble with certain HDMI sinks.

    static const uint32_t kMinFullRangeSlewChange_mSec;

    // How much time (in msec) to wait 

    static const int kSlewChangeStepPeriod_mSec;

#ifdef TIME_SERVICE_DEBUG

    sp<DiagThread> diag_thread_;

#endif

    // run the state machine

    while (!exitPending()) {

        struct pollfd pfds[2];

        int rc;

        int rc, timeout;

        int eventCnt = 0;

        int64_t wakeupTime;

        uint32_t t1, t2;

        bool needHandleTimeout = false;

        // We are always interested in our wakeup FD.

        pfds[eventCnt].fd      = mWakeupThreadFD;

            eventCnt++;

        }

        t1 = static_cast<uint32_t>(mCurTimeout.msecTillTimeout());

        t2 = static_cast<uint32_t>(mClockRecovery.applyRateLimitedSlew());

        timeout = static_cast<int>(t1 < t2 ? t1 : t2);

        // Note, we were holding mLock when this function was called.  We

        // release it only while we are blocking and hold it at all other times.

        mLock.unlock();

        rc          = poll(pfds, eventCnt, mCurTimeout.msecTillTimeout());

        rc          = poll(pfds, eventCnt, timeout);

        wakeupTime  = mLocalClock.getLocalTime();

        mLock.lock();

            return false;

        }

        if (rc == 0)

        if (rc == 0) {

            needHandleTimeout = !mCurTimeout.msecTillTimeout();

            if (needHandleTimeout)

                mCurTimeout.setTimeout(kInfiniteTimeout);

        }

        // Were we woken up on purpose?  If so, clear the eventfd with a read.

        if (pfds[0].revents)

            continue;

        }

        // Did we wakeup with no signalled events across all of our FDs?  If so,

        // we must have hit our timeout.

        if (rc == 0) {

        // Time to handle the timeouts?

        if (needHandleTimeout) {

            if (!handleTimeout())

                ALOGE("handleTimeout failed");

            continue;

    }

}

void CommonTimeServer::TimeoutHelper::setTimeout(int msec) {

    mTimeoutValid = (msec >= 0);

    if (mTimeoutValid)

        mEndTime = systemTime() +

                   (static_cast<nsecs_t>(msec) * 1000000);

}

int CommonTimeServer::TimeoutHelper::msecTillTimeout() {

    if (!mTimeoutValid)

        return kInfiniteTimeout;

    nsecs_t now = systemTime();

    if (now >= mEndTime)

        return 0;

    uint64_t deltaMsec = (((mEndTime - now) + 999999) / 1000000);

    if (deltaMsec > static_cast<uint64_t>(MAX_INT))

        return MAX_INT;

    return static_cast<int>(deltaMsec);

}

bool CommonTimeServer::shouldPanicNotGettingGoodData() {

    if (mClient_FirstSyncTX) {

        int64_t now = mLocalClock.getLocalTime();

#include "clock_recovery.h"

#include "common_clock.h"

#include "common_time_server_packets.h"

#include "utils.h"

#define RTT_LOG_SIZE 30

        int64_t rxTimes[RTT_LOG_SIZE];

    };

    class TimeoutHelper {

      public:

        TimeoutHelper() : mTimeoutValid(false) { }

        void setTimeout(int msec);

        int msecTillTimeout();

      private:

        bool        mTimeoutValid;

        nsecs_t     mEndTime;

    };

    bool threadLoop();

    bool runStateMachine_l();

    bool shouldPanicNotGettingGoodData();

    // Helper to keep track of the state machine's current timeout

    TimeoutHelper mCurTimeout;

    Timeout mCurTimeout;

    // common clock, local clock abstraction, and clock recovery loop

    CommonClock mCommonClock;