A PLL designed to manage time on an ESP8266

pll.c

#include <stdint.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>

// We model a local clock with a small but consistent inaccuracy and a
// large initial error.  i.e., the local clock is
//
//   t_l = t_w * (1+b) + t_{l0}, |b| < 1e-2, t_{l0} = -100_000
//
// We can fetch a world clock with some (positive) perturbation
//
// t_wp < 1e-1, σ(t_wp) < 1e-2
//
// We wish to reconstruct the world clock as closely as possible.  As
// the resulting reconstructor needs to work on a microcontroller, we
// measure time in ms, thus all of the above numbers are multiplied by
// 1000

int t_world = 0;
float b = -0.1;
const int t_l0 = -10000000;

// Random generation
//
// We use RC4 because it's simple, but skip the first 1024 bytes to
// avoid the well-known early bias

struct {
  uint8_t buf[256];
  uint8_t i,j;
} rc4_state;

void rc4_init(uint8_t* key, int len) {
  size_t i=0, j=0;
  for (i = 0; i < 256; i++) {
    rc4_state.buf[i] = (uint8_t)i;
  }

  for (i=0; i < 256; i++) {
    j = (j + rc4_state.buf[i] + key[i % len]) % 256;
    uint8_t tmp = rc4_state.buf[i];
    rc4_state.buf[i] = rc4_state.buf[j];
    rc4_state.buf[j] = tmp;
  }

  rc4_state.i = rc4_state.j = 0;
}

uint8_t rand_u8(void) {
  rc4_state.i += 1;
  rc4_state.j += rc4_state.buf[rc4_state.i];

  uint8_t tmp = rc4_state.buf[rc4_state.i];
  rc4_state.buf[rc4_state.i] = rc4_state.buf[rc4_state.j];
  rc4_state.buf[rc4_state.j] = tmp;

  return rc4_state.buf[(rc4_state.buf[rc4_state.i] + rc4_state.buf[rc4_state.j]) % 256];
}

void rand_b(void* buf, size_t len) {
  uint8_t *obuf = buf;
  while(len-->0) {
    *obuf++ = rand_u8();
  }
}

// Same as rand_uniform_d, but between (-1,1).
double rand_uniform_d_wide() {
  union {
    double dval;
    uint64_t ival;
  } pun;

  // We construct a double with exponent 0 and random mantissa, then
  // subtract one
  rand_b(&pun.ival, sizeof(pun.ival));
  pun.ival &= ((1L << 52) - 1) | (1L << 63);
  pun.ival |= 0x3ffL << 52;
  return pun.dval /  2.0;
}

// Same as rand_uniform_d, but between (-1,1).
double rand_uniform_d() {
  union {
    double dval;
    uint64_t ival;
  } pun;

  pun.dval = rand_uniform_d_wide();
  pun.ival &= (~0UL) >> 1;
  return pun.dval;
}

double rand_normal_d() {
  // The box-muller transform generates two random normals at once.
  // We cache the last one here
  static double last_rand = NAN;
  if (!isnan(last_rand)) {
    double ret = last_rand;
    last_rand = NAN;
    return ret;
  }

  double u, v, s;
  do {
    u = 2 * rand_uniform_d() - 1;
    v = 2 * rand_uniform_d() - 1;
    s = u * u + v * v;
  } while(s > 1);
  // u,v is guaranteed to be in a circle of unit radius; s = R^2

  double mag = sqrt(-2 * log(s) / s);
  last_rand = u * mag;
  return v * mag;
}

// Model of the uc's view of world time
int uc_ms(void) {
  return t_world + (int)(t_world * b) + t_l0;
}

int sample_world_ms(void) {
  return t_world + (int)(rand_normal_d() * 10);
}


// Implementation of PLL
float pll_b = 1;
int pll_o = 0;
int pll_slew_end = t_l0 - 1000;
float pll_slew_b;
int pll_slew_o;
int last_sync = t_l0 - 100;
int pll_ms() {
  int ms = uc_ms();
  if (ms < pll_slew_end) {
    return ms * pll_slew_b + pll_slew_o;
  } else {
    return ms * pll_b + pll_o;
  }
}

float pll_sync(void) {
  // returns change in pll_b; should converge to 1.  NaN means that it
  // is currently slewing, INF means that the clock stepped
  int ms = uc_ms();
  const float SMOOTH_FACTOR = 0.0f/8.0f;
  const int SLEW_TIME = 60000;
  const int SLEW_MAX = 50000;
  const float SLEW_MIN_RATIO = 0.125f;

  if (ms < pll_slew_end) {
  // If pll is currently slewing, do nothing
    fprintf(stderr, "not syncing; too soon\n");
    return -1;
  }

  int model_ms = pll_ms();
  int world_ms = sample_world_ms();
  int error = model_ms - world_ms;
  fprintf(stderr, "error: %d ( %d %d )\n", error, model_ms, world_ms);
  if (error < -SLEW_MAX || error > SLEW_MAX) {
    fprintf(stderr, "Stepping!\n");
    // error too big, step
    pll_slew_b = pll_b = 1;
    pll_o = pll_slew_o = world_ms - ms;
    pll_slew_end = ms - 100;
    last_sync = ms;
    return INFINITY;
  } else {
    // Assume that time was last synced at ms, and that the
    // non-slewing model was accurate then. Compute the "correct" b
    int last_sync_world = last_sync * pll_b + pll_o;
    float ideal_b = (world_ms - last_sync_world) * 1.0f / (ms - last_sync);

    // Drift b towards ideal_b to average out noise in the upstream
    // clock
    float last_pll_b = pll_b;
    pll_b = ideal_b * (1-SMOOTH_FACTOR) + pll_b * SMOOTH_FACTOR;
    // compute new pll_o so that the model gives the right result
    // *now*
    pll_o = world_ms - pll_b * ms;

    // Compute slew factors
    pll_slew_end = ms + SLEW_TIME;
    pll_slew_b = (world_ms + pll_b * SLEW_TIME - model_ms) / SLEW_TIME;

    if (pll_slew_b < SLEW_MIN_RATIO * pll_b) {
      // We can't have time run backwards; slew at minimum
      // SLEW_MIN_RATIO Note that this will fail if the system clock
      // runs backwards. Such is life.
      fprintf(stderr, "Not going back in time\n");
      pll_slew_b = pll_b * SLEW_MIN_RATIO;
      pll_slew_o = model_ms - pll_slew_b * ms;

      // t*sb+so = t*b+o
      // t*(sb-b) = o-so
      // t = (o-so)/ (b-sb)

      pll_slew_end = (pll_o - pll_slew_o) / (pll_b - pll_slew_b);
    } else {
      pll_slew_o = model_ms - pll_slew_b * ms;
    }
    return last_pll_b / pll_b;
  }
}


// Test driver

int main(int argc, char** argv) {
  // Columns:
  // real_world, time_err, pll_err

  (void)argc;
  (void)argv;
  
  // Bootstrap the RNG
  if (getenv("ENTROPY") != NULL) {
    char* entropy = getenv("ENTROPY");
    rc4_init((uint8_t*)entropy, strlen(entropy));
  } else {
    uint8_t entropy[128];
    getentropy(entropy, 128);
    rc4_init(entropy, 128);
  }

#if MODE == 1
  for (int i = 0; i < 20; i++) {
    printf("%lf\n", rand_uniform_d_wide());
  }

#else
  int next_step = -1;
  float last_sync_res = 1;
  for (t_world = 0; t_world < 86400000; t_world += 10000) {
    if (t_world > next_step) {
      printf("# sync\n");
      last_sync_res = pll_sync();
      if (isinf(last_sync_res) || last_sync_res > 2.0) {
	next_step = t_world + 1;
      } else {
	next_step = t_world + 3600000;
      }
    }
    // print a result line
    printf("%d %d %f %f\n", t_world, pll_ms() - t_world, pll_b * (b+1.0f), last_sync_res-1);
  }
#endif
  return 0;
  
}