#include "project.h"


typedef struct {
  union {
    struct {
      uint32_t precision: 8;
      uint32_t poll: 8;
      uint32_t stratum: 8;
      uint32_t mode: 3;
      uint32_t vn: 3;
      uint32_t li: 2;
    };
    uint32_t word_0;
  };

  uint32_t root_delay;
  uint32_t root_dispersion;
  uint32_t reference_id;

  uint64_t reference_ts;
  uint64_t origin_ts;
  uint64_t receive_ts;
  uint64_t transmit_ts;
} ntp_packet_t;




#if 0
packet (uint8_t *_p)
{
  EPOCH reftime = ref_decompose (pll_last_update);

  ntp_packet_t rx = (uint8_t *)_p;
  ntp_packet_t tx;


  tx.li = NTP LEAP_MEH;
  tx.vn = NTP_VERSION;
  tx.mode = NTP_MODE_SERVER;
  tx.poll = ?
            tx.precision = ?
                           tx.rootdelay = ?
                                          tx.rootdisp = ?
                                              tx.refid = FCC ('M', 'S', 'F', 0);
  tx.reftime =;




}
#endif

static uint64_t ntp_ts_ref (uint64_t v)
{
  uint64_t ret;
  EPOCH e = ref_decompose (v);

  ret = e.ns;
  ret <<= 32;
  ret = ret / 1000000000;
  ret = htonl (ret);
  ret <<= 32;
  ret |= htonl (2208988800UL + e.s);

  return ret;
}


static uint64_t ntp_ts_ptp (uint64_t v)
{
  uint64_t ret;
  EPOCH e = ptp_decompose (v);

  ret = e.ns;
  ret <<= 32;
  ret = ret / 1000000000;
  ret = htonl (ret);
  ret <<= 32;
  ret |= htonl (2208988800UL + e.s);

  return ret;
}



static void ntp_rx (void *arg, struct udp_pcb *s, struct pbuf *p,  const ip_addr_t *src_addr, u16_t port)
{
  uint64_t ref = ref_get_irq();
  ip_addr_t dst_addr = *src_addr;
  ntp_packet_t pkt;

  do {
    //    if (p->type_internal != PBUF_POOL) break;

    if (p->len < sizeof (ntp_packet_t)) break;

    if (!ref_valid) break;

    if (!time_known) break;


    memcpy (&pkt, p->payload, sizeof (ntp_packet_t));
    pbuf_realloc (p, sizeof (ntp_packet_t));

    pkt.li = 0;
    pkt.vn = 4;
    pkt.mode = 4;
    pkt.stratum = 1;
    pkt.poll = 3;
    pkt.precision = 0xec;
    pkt.word_0 = htonl (pkt.word_0);

    pkt.root_delay = htonl (0x0);
    pkt.root_dispersion = htonl (0x825);
    pkt.reference_id = htonl (0x47505300);

    pkt.origin_ts = pkt.transmit_ts;
#if 0
    pkt.receive_ts = ntp_ts_ref (ref);
#else
    pkt.receive_ts = ntp_ts_ptp (p->ptp_timestamp);
#endif

    pkt.receive_ts = ntp_ts_ref (ref);
    pkt.reference_ts = ntp_ts_ref (ref_last_update);

    memcpy (p->payload, &pkt, sizeof (ntp_packet_t));
    compiler_mb();
    ref = ref_get_irq();
    pkt.transmit_ts = ntp_ts_ref (ref);

    udp_sendto (s, p, &dst_addr, port);
  } while (0);

  /* Free the p buffer */
  pbuf_free (p);
}


void ntp_init (void)
{
  struct udp_pcb *s;

  s = udp_new();

  udp_bind (s, IP_ADDR_ANY, 123);

  udp_recv (s, ntp_rx, NULL);
}