#include "project.h"

static volatile unsigned us_ptp_idx;
static volatile uint64_t us_ptp_freq[2];
static volatile uint64_t us_ptp_offset[2];
static volatile uint64_t us_ptp_phase[2];


static int64_t ptp_phase;
static int64_t ptp_freq = PTP_CLOCK_HZ;
static int64_t ptp_offset;

int ptp_offset_known;



static void us_put (void)
{
  unsigned i = us_ptp_idx;

  i ^= 1;
  us_ptp_freq[i] = ptp_freq;
  us_ptp_offset[i] = ptp_offset;
  us_ptp_phase[i] = ptp_phase;

  compiler_mb();
  us_ptp_idx = i;
}

static void us_get (uint64_t *f, uint64_t *o, uint64_t *p)
{
  unsigned i;

  do {
    i = us_ptp_idx;
    *f = us_ptp_freq[i];
    *o = us_ptp_offset[i];
    *p = us_ptp_phase[i];
    compiler_mb();
  } while (i != us_ptp_idx);
}

static void modify_ptp_freq (uint64_t now, uint64_t new)
{
  int64_t pd1, pd2, te;

  pd1 = now - ptp_phase;
  te = pd1 / ptp_freq;
  pd1 %= ptp_freq;

  if (pd1 > (ptp_freq >> 1)) {
    te++;
    pd1 = pd1 - ptp_freq;
  }

  ptp_freq = new;

  pd2 = pd1 + (te * ptp_freq);

  ptp_phase = now - pd2;

}





uint64_t ptp_to_u64 (uint32_t l, uint32_t h)
{
  uint64_t ret;

  ret = h;
  ret <<= 31;
  ret |= l;

  return ret;
}

uint64_t ptp_get (void)
{
  uint32_t pl, ph;


  do {
    ph = ETH_PTPTSHR;
    compiler_mb();
    pl = ETH_PTPTSLR;
    compiler_mb();
  } while (ph != ETH_PTPTSHR);

  return ptp_to_u64 (pl, ph);
}



#define FF_B 16
#define FF_A ((FF_B)-1)
#define FF_C 16

static uint64_t fff;

static void fll_init (uint64_t start_freq)
{
  fff = start_freq * FF_C;
}

static uint64_t fll (uint64_t obs_freq)
{
  uint64_t new_freq;



  fff *= FF_A;
  fff += obs_freq * FF_C;
  fff += FF_B / 2;
  fff /= FF_B;

  new_freq = fff;
  new_freq += FF_C / 2;
  new_freq /= FF_C;


  return new_freq;
}


static int64_t edge_to_phase (uint64_t edge)
{
  int64_t obs_phase;
  obs_phase = edge - ptp_phase;
  obs_phase %= ptp_freq;

  if (obs_phase > ptp_freq / 2)
    obs_phase -= ptp_freq;

  return obs_phase;
}

#define PF_A 16
static uint64_t pll (int64_t obs_phase)
{
  int64_t pd ;


  pd = obs_phase / PF_A;

  if (!pd) {
    if (obs_phase < 0) pd--;

    if (obs_phase > 0) pd++;
  }


  return pd;
}


char ptp_info[128];



void ptp_dispatch (uint64_t edge, const char *src)
{
  static uint64_t last_edge;
  uint64_t obs_freq, obs_phase, new_freq;
  static int jump_start = 1;

  obs_freq = edge - last_edge;
  last_edge = edge;
  obs_phase = edge_to_phase (edge);

  snprintf (ptp_info, sizeof (ptp_info) - 1, "PTP PLL: obs_f=%9d delta_phi=%5d f=%9d %s\n",
            (int) obs_freq,
            (int) obs_phase,
            (int) ptp_freq, src);

#ifdef CHATTY_PLLS
  printf ("PTP PLL: obs_f=%9d delta_phi=%5d f=%9d %s\r\n",
          (int) obs_freq,
          (int) obs_phase,
          (int) ptp_freq, src);
#endif

  /*Ignore bogus observations*/
  if (obs_freq > (PTP_CLOCK_HZ + (PTP_CLOCK_HZ / 2)))
    return;

  if (obs_freq < (PTP_CLOCK_HZ - (PTP_CLOCK_HZ / 2)))
    return;

  if (jump_start) {
    new_freq = obs_freq;
    fll_init (new_freq);
    modify_ptp_freq (edge, new_freq);
    ptp_phase += obs_phase;
    jump_start = 0;
  } else {
    new_freq = fll (obs_freq);
    modify_ptp_freq (edge, new_freq);
    ptp_phase += pll (obs_phase);
  }

  us_put();

}

void ptp_set_offset (EPOCH epoch, uint64_t abs)
{
  int64_t new_offset;
  int diff;


  /* Find nearest second to abs*/
  abs += ptp_freq >> 2;
  abs -= ptp_phase;
  abs /= ptp_freq;

  new_offset = epoch.s - abs;

  if (new_offset != ptp_offset) {
    diff = (int) (new_offset - ptp_offset);


    printf ("PTP wallclock offset moved by %d\r\n", diff);
    ptp_offset = new_offset;
    us_put();
  }

  ptp_offset_known = 1;
}

static EPOCH _ptp_decompose (uint64_t abs, uint64_t f, uint64_t o)
{
  EPOCH ret;


  ret.s = abs / f;
  abs -= f * ret.s;

  ret.s += o;

  abs *= (uint64_t) 1000000000;
  abs = abs / f;

  ret.ns = abs;

  return ret;
}

EPOCH ptp_decompose_diff (int64_t diff)
{
  EPOCH ret;
  uint64_t f, o, p;

  us_get (&f, &o, &p);

  if (diff >= 0)
    return _ptp_decompose (diff, f, o);

  ret = _ptp_decompose (-diff, f, o);
  ret.s = -ret.s;
  ret.ns = -ret.ns;

  return ret;
}


EPOCH ptp_decompose (uint64_t abs)
{
  uint64_t f, o, p;
  us_get (&f, &o, &p);

  abs -= p;

  return _ptp_decompose (abs, f, o);
}

void ptp_slow_tick (void)
{
}