clock.c

Go to the documentation of this file.

/*
 * clock.c
 *
 *  Date: 19/06/2014
 *  Author: Fernando Biazi Nascimento
 *  Copyright © 2014 Fernando Biazi Nascimento. All rights reserved.
 *
 *  License of use and copy on file license.txt
 *
 */

/*
 * ======== Standard MSP430 includes ========
 */
#include <msp430f2274.h>

/*
 * ======== Includes ========
 */

#include "clock.h"
#include "im.h"

/*
 * ======== Constants ========
 */

#define CLOCK_DEFAULT_TICK_SIZE 250000

#define CLOCK_TICK_SIZE_MAX 500000

#define CLOCK_NS_IN_ONE_S 1000000000

/*
 * ======== Macros ========
 */

#define CLOCK_FLAGADJUSTING  1

#define CLOCK_FLAGCALLTICK   2

#define CLOCK_GETADJUSTING clock_runningFlags & CLOCK_FLAGADJUSTING

#define CLOCK_SETADJUSTING clock_runningFlags |= CLOCK_FLAGADJUSTING

#define CLOCK_CLEARADJUSTING clock_runningFlags &= ~CLOCK_FLAGADJUSTING

#define CLOCK_GETCALLTICK clock_runningFlags & CLOCK_FLAGCALLTICK

#define CLOCK_SETCALLTICK clock_runningFlags |= CLOCK_FLAGCALLTICK

#define CLOCK_CLEARCALLTICK clock_runningFlags &= ~CLOCK_FLAGCALLTICK

/*
 * ======== Types ========
 */

/*
 * ======== Global Variables ========
 */

UInt32 clock_tickSize;

/*
 * ======== Local Variables ========
 */

clock_timeStamp clock_time;

UInt8 clock_runningFlags;

UInt16 clock_ticksSinceSint;

int64_t clock_adjSumSinceSint;

/*
 * ======== Local Functions ========
 */

/*
 * ================
 */

void clock_init() {
  im_disable_interrupt();

  P2OUT &= ~BIT2;

  clock_ticksSinceSint = 0xFFFF;
  clock_adjSumSinceSint = 0;
  clock_time.seconds.secondsLow = 0;
  clock_time.seconds.secondsHigh = 0;
  clock_time.nanoSeconds = 0;
  clock_tickSize = CLOCK_DEFAULT_TICK_SIZE;
  CLOCK_CLEARADJUSTING;
  CLOCK_CLEARCALLTICK;

  im_enable_interrupt();
}

void clock_tick() {
  if (CLOCK_GETADJUSTING) { // If clock is adjusting, do not count tick and set to call again later
    CLOCK_SETCALLTICK;
    return;
  }
  clock_time.nanoSeconds += clock_tickSize;
  if (clock_time.nanoSeconds >= CLOCK_NS_IN_ONE_S) {
    clock_time.nanoSeconds -= CLOCK_NS_IN_ONE_S;
    clock_time.seconds.secondsLow += 1;
    if (!clock_time.seconds.secondsLow) {
      clock_time.seconds.secondsHigh += 1;
    }

    // Square wave for measurements, 0 for even s, 1 for odd s.
    if (clock_time.seconds.secondsLow & 0x01) {
      P2OUT |= BIT2;
    } else {
      P2OUT &= ~BIT2;
    }

  }
  if (clock_ticksSinceSint != 0xFFFF)
    clock_ticksSinceSint++;
}

void clock_checkDelayedTick() {
  if (CLOCK_GETCALLTICK) {
    CLOCK_CLEARCALLTICK;
    clock_tick();
  }
}

void clock_getTime(clock_timeStamp *destTime) {
  CLOCK_SETADJUSTING;
  destTime->nanoSeconds = clock_time.nanoSeconds;
  destTime->seconds.secondsLow = clock_time.seconds.secondsLow;
  destTime->seconds.secondsHigh = clock_time.seconds.secondsHigh;
  CLOCK_CLEARADJUSTING;
  clock_checkDelayedTick();
}

void clock_setTime(clock_timeStamp *sourceTime) {
  CLOCK_SETADJUSTING;
  clock_time.seconds.secondsLow = sourceTime->seconds.secondsLow;
  clock_time.seconds.secondsHigh = sourceTime->seconds.secondsHigh;
  clock_time.nanoSeconds = sourceTime->nanoSeconds;
  CLOCK_CLEARADJUSTING;
  clock_checkDelayedTick();
}

void clock_adjTime(int64_t *adjValue) {
  Int32 tickAdj;

  if (clock_ticksSinceSint == 0xFFFF) {
    clock_ticksSinceSint = 0; // begin adjust cycle
    clock_adjSumSinceSint = 0;
  } else {
    if ((((*adjValue) < 3000000000) // if adjust is not too big
    && (-(*adjValue) < 3000000000))) {
      clock_adjSumSinceSint += *adjValue; // add this adjust to the sum

      if ((clock_ticksSinceSint & 0x8000)
          || ((!(clock_adjSumSinceSint & 0x8000000000000000)) // positive
          && (clock_adjSumSinceSint & 0x0000000200000000)) // more than half
          || ((clock_adjSumSinceSint & 0x8000000000000000) // negative
          && !(clock_adjSumSinceSint & 0xfffffffe00000000))) {
        // after more than half capacity of the counter or the adjust, for better precision
        if (clock_adjSumSinceSint) { // But not if the current adjust is 0
          // The adjust is the error divided by the number of ticks since last syntonization.
          // the >> 2 and << 2 makes possible a 32bits arithmetics and serves as a tolerance,
          // no adjust is done if precision is (2^2)ns or better per tick.
          // the limitation is that the adjust can not be larger than ~17s.
          tickAdj = clock_adjSumSinceSint >> 2;
          tickAdj /= clock_ticksSinceSint;
          tickAdj <<= 2;
          clock_adjTick(tickAdj);
        }
        clock_ticksSinceSint = 0xFFFF; // reset tick adjust
      }
    } else { // adjust is too big
      clock_ticksSinceSint = 0xFFFF; // reset tick adjust
    }
  }

  // adjust the time
  clock_adjTimeValues(adjValue, &clock_time);
}

void clock_adjTimeValues(int64_t *adjValue, clock_timeStamp *time) {
  int64_t adjSeconds = 0;
  Int32 adjNanoseconds;
  int64_t tmpInt64;

  // range check - Value cannot be larger than (+ or -) (2^31) seconds, that is ~ 68 years
  if ((*adjValue >= 2147483647000000000)
      || (*adjValue < -2147483648000000000))
    return;

  if (*adjValue) {
    // If adjValueis less than 32s, avoid division and multiplication, use while inside else
    // Thats because the while loop will use about 96 c lines on the worst
    // case, and the division by itself will use 88 c lines all the times.
    if (((*adjValue > 0) && (((*adjValue) >> 5) >= CLOCK_NS_IN_ONE_S))
        || ((*adjValue < 0)
            && (((-*adjValue) >> 5) >= CLOCK_NS_IN_ONE_S))) {
      // adjSeconds = *adjValue / CLOCK_NS_IN_ONE_S;
      // Horner's Method, slaa329
      // adjSeconds = *adjValue * 1 / CLOCK_NS_IN_ONE_S;
      // 1/CLOCK_NS_IN_ONE_S =
      // .00000000000000000000000000000100010010111000001011111010000010011011010110100101001011001011100110001011010000000101010001000111b ->
      // .0000000000000000000000000000010001001100-100000110000-1010000010011011010110100101001011001100-10011000101101000000010101000100100-1csd
      // csd would cause complications to subtraction/shift right, it could be done with an arithmetic shift right
      adjSeconds = *adjValue;
      adjSeconds >>= 1;
      adjSeconds += *adjValue;
      adjSeconds >>= 1;
      adjSeconds += *adjValue;
      adjSeconds >>= 4;
      adjSeconds += *adjValue;
      adjSeconds >>= 4;
      adjSeconds += *adjValue;
      adjSeconds >>= 2;
      adjSeconds += *adjValue;
      adjSeconds >>= 2;
      adjSeconds += *adjValue;
      adjSeconds >>= 8;
      adjSeconds += *adjValue;
      adjSeconds >>= 2;
      adjSeconds += *adjValue;
      adjSeconds >>= 1;
      adjSeconds += *adjValue;
      adjSeconds >>= 2;
      adjSeconds += *adjValue;
      adjSeconds >>= 4;
      adjSeconds += *adjValue;
      adjSeconds >>= 1;
      adjSeconds += *adjValue;
      adjSeconds >>= 3;
      adjSeconds += *adjValue;
      adjSeconds >>= 1;
      adjSeconds += *adjValue;
      adjSeconds >>= 1;
      adjSeconds += *adjValue;
      adjSeconds >>= 2;
      adjSeconds += *adjValue;
      adjSeconds >>= 3;
      adjSeconds += *adjValue;
      adjSeconds >>= 1;
      adjSeconds += *adjValue;
      adjSeconds >>= 2;
      adjSeconds += *adjValue;
      adjSeconds >>= 3;
      adjSeconds += *adjValue;
      adjSeconds >>= 2;
      adjSeconds += *adjValue;
      adjSeconds >>= 3;
      adjSeconds += *adjValue;
      adjSeconds >>= 2;
      adjSeconds += *adjValue;
      adjSeconds >>= 1;
      adjSeconds += *adjValue;
      adjSeconds >>= 2;
      adjSeconds += *adjValue;
      adjSeconds >>= 2;
      adjSeconds += *adjValue;
      adjSeconds >>= 1;
      adjSeconds += *adjValue;
      adjSeconds >>= 2;
      adjSeconds += *adjValue;
      adjSeconds >>= 1;
      adjSeconds += *adjValue;
      adjSeconds >>= 3;
      adjSeconds += *adjValue;
      adjSeconds >>= 6;
      adjSeconds += *adjValue;
      adjSeconds >>= 2;
      adjSeconds += *adjValue;
      adjSeconds >>= 1;
      adjSeconds += *adjValue;
      adjSeconds >>= 1;
      adjSeconds += *adjValue;
      adjSeconds >>= 1;
      adjSeconds += *adjValue;
      adjSeconds >>= 1;
      adjSeconds += *adjValue;
      adjSeconds >>= 2;
      adjSeconds += *adjValue;
      adjSeconds >>= 6;
      adjSeconds += *adjValue;
      adjSeconds >>= 1;
      adjSeconds += *adjValue;
      adjSeconds >>= 1;
      adjSeconds += *adjValue;
      adjSeconds >>= 2;
      adjSeconds += *adjValue;
      adjSeconds >>= 3;
      adjSeconds += *adjValue;
      adjSeconds >>= 4;
      adjSeconds += *adjValue;
      adjSeconds >>= 30;

      // get the seconds part in ns. (ns = s * 1e9)
      tmpInt64 = adjSeconds;
      tmpInt64 <<= 4;
      tmpInt64 -= adjSeconds;
      tmpInt64 <<= 3;
      tmpInt64 -= adjSeconds;
      tmpInt64 <<= 3;
      tmpInt64 += adjSeconds;
      tmpInt64 <<= 1;
      tmpInt64 += adjSeconds;
      tmpInt64 <<= 2;
      tmpInt64 += adjSeconds;
      tmpInt64 <<= 2;
      tmpInt64 += adjSeconds;
      tmpInt64 <<= 1;
      tmpInt64 += adjSeconds;
      tmpInt64 <<= 3;
      tmpInt64 += adjSeconds;
      tmpInt64 <<= 2;
      tmpInt64 += adjSeconds;
      tmpInt64 <<= 9;

      adjNanoseconds = *adjValue - tmpInt64;
      // The result of the division above has been 1 less than it should in several
      // tests, the loops bellow must fix it.
      // Number of seconds, calculated by loops
      while (adjNanoseconds >= CLOCK_NS_IN_ONE_S) {
        adjNanoseconds -= CLOCK_NS_IN_ONE_S;
        adjSeconds += 1;
      }
      while (adjNanoseconds <= -CLOCK_NS_IN_ONE_S) {
        adjNanoseconds += CLOCK_NS_IN_ONE_S;
        adjSeconds -= 1;
      }
    } else {
      adjSeconds = 0;
      tmpInt64 = *adjValue;
      while (tmpInt64 >= CLOCK_NS_IN_ONE_S) {
        tmpInt64 -= CLOCK_NS_IN_ONE_S;
        adjSeconds += 1;
      }
      while (tmpInt64 <= -CLOCK_NS_IN_ONE_S) {
        tmpInt64 += CLOCK_NS_IN_ONE_S;
        adjSeconds -= 1;
      }
      adjNanoseconds = tmpInt64;
    }
    // If adjusting the clock, begin adjusting phase
    if (time == &clock_time) {
      CLOCK_SETADJUSTING;
    }
    // Adjust seconds if != 0
    if (adjSeconds) {
      if (adjSeconds < 0) {
        if (time->seconds.secondsLow < (UInt32) -adjSeconds)
          time->seconds.secondsHigh -= 1;
        time->seconds.secondsLow += adjSeconds;
      } else {
        time->seconds.secondsLow += adjSeconds;
        if (time->seconds.secondsLow < (UInt32) adjSeconds)
          time->seconds.secondsHigh += 1;
      }
    }
    if ((adjNanoseconds < 0)
        && (time->nanoSeconds < (UInt32) -adjNanoseconds)) {
      if (!time->seconds.secondsLow)
        time->seconds.secondsHigh -= 1;
      time->seconds.secondsLow -= 1;
      time->nanoSeconds += CLOCK_NS_IN_ONE_S + adjNanoseconds;
    } else {
      time->nanoSeconds += adjNanoseconds;
      while (time->nanoSeconds >= CLOCK_NS_IN_ONE_S) {
        time->seconds.secondsLow += 1;
        if (!time->seconds.secondsLow)
          time->seconds.secondsHigh += 1;
        time->nanoSeconds -= CLOCK_NS_IN_ONE_S;
      }
    }
    // if adjusting the clock, end adjusting phase
    if (time == &clock_time) {
      CLOCK_CLEARADJUSTING;
      clock_checkDelayedTick();
    }
  }
}

void clock_adjTick(Int32 adjValue) {
  if ((adjValue < 0) && (-adjValue >= clock_tickSize)) {
    clock_tickSize = 1;
  } else {
    clock_tickSize += adjValue;
    if (clock_tickSize > CLOCK_TICK_SIZE_MAX) {
      clock_tickSize = CLOCK_TICK_SIZE_MAX;
    }
  }
}

int64_t clock_timeDiff(clock_timeStamp *t1, clock_timeStamp *t2) {
  int64_t r;
  int64_t x;
  if (t2->seconds.secondsHigh != t1->seconds.secondsHigh) {
    r = -t1->seconds.secondsHigh;
    r += t2->seconds.secondsHigh;
    r <<= 32;
  } else {
    r = 0;
  }
  r -= t1->seconds.secondsLow;
  r += t2->seconds.secondsLow; // r unit is s.
  // On values within less than 1s delay, on transition from one second to another the
  // subtraction may result in 1 or -1, avoid multiplication if able.
  if (r == 1)
    r = 1000000000;
  else if (r == -1)
    r = -1000000000;
  else if (r) { // if not zero
    //  r *= 1e9;
    // Horner's Method, slaa329
    // r * 1000000000; 1000000000 = 111011100110101100101000000000b ->
    // -> 1000-100-100110101100101000000000csd
    x = r;
    r <<= 4;
    r -= x;
    r <<= 3;
    r -= x;
    r <<= 3;
    r += x;
    r <<= 1;
    r += x;
    r <<= 2;
    r += x;
    r <<= 2;
    r += x;
    r <<= 1;
    r += x;
    r <<= 3;
    r += x;
    r <<= 2;
    r += x;
    r <<= 9;
  }
  // now r unit is ns.
  r -= t1->nanoSeconds;
  r += t2->nanoSeconds; // add ns fields to r.
  return r;
}