#include <pkgconf/kernel.h>
#include <cyg/kernel/ktypes.h>         // base kernel types
#include <cyg/kernel/instrmnt.h>

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>

#define CYG_INSTRUMENT_CLOCK_TICK_START (CYG_INSTRUMENT_EVENT_CLOCK_TICK_START|CYG_INSTRUMENT_CLASS_CLOCK)
#define CYG_INSTRUMENT_CLOCK_ISR (CYG_INSTRUMENT_EVENT_CLOCK_ISR|CYG_INSTRUMENT_CLASS_CLOCK)

// -------------------------------------------------------------------------
// Instrumentation record.

struct Instrument_Record
{
    CYG_WORD16  type;                   // record type
    CYG_WORD16  thread;                 // current thread id
    CYG_WORD    timestamp;              // 32 bit timestamp
    CYG_WORD    arg1;                   // first arg
    CYG_WORD    arg2;                   // second arg
};

// -------------------------------------------------------------------------

static long rtc_resolution[] = CYGNUM_KERNEL_COUNTERS_RTC_RESOLUTION;
static CYG_WORD64 ns_per_tick;
static CYG_WORD64 ps_per_hal_clock;
static CYG_WORD64 ticks = -1;

#ifdef CYGDBG_KERNEL_INSTRUMENT_MSGS
#define CYGDBG_KERNEL_INSTRUMENT_MSGS_DEFINE_TABLE
#include <cyg/kernel/instrument_desc.h>
#define NELEM(x) (sizeof(x)/sizeof*(x))


externC char * cyg_instrument_msg(CYG_WORD16 type)
{
  struct instrument_desc_s *record;
  struct instrument_desc_s *end_record;
  CYG_WORD cl, event;

  record = instrument_desc;
  end_record = &instrument_desc[NELEM(instrument_desc)-1];
  cl = type & 0xff00;
  event = type & 0x00ff;

  while ((record != end_record) && (record->num != cl)) {
    record++;
  }

  if (record->num == cl) {
    record++;
    while ((record != end_record) && (record->num != event) &&
           (record->num < 0xff)) {
      record++;
    }

    if (record->num == event) {
      return (record->msg);
    }
  }
  return("Unknown event");
}
#endif // CYGDBG_KERNEL_INSTRUMENT_MSGS

void usage(char *myname) 
{
  fprintf(stderr,"Usage: %s <filename> [start item (in case of wraparound)]\n",myname);
  fprintf(stderr,"where filename is that of the instrumentation data\n");
}

uint16_t swap2bytes(uint16_t input)
{
  uint16_t result = 0;
  result += (input & 0xff00) >> 8;
  result += (input & 0x00ff) << 8;
  return result;
}

uint32_t swap4bytes(uint32_t input)
{
  uint32_t result = 0;
  result += (input & 0xff000000) >> 24;
  result += (input & 0x00ff0000) >> 8;
  result += (input & 0x0000ff00) << 8;
  result += (input & 0x000000ff) << 24;
  return result;
}

/* Return the time in ns */
CYG_WORD64 cvt_time(CYG_WORD timestamp) 
{
  return (  (ticks * ns_per_tick * 1000) + 
            ((CYG_WORD64)timestamp * ps_per_hal_clock)  );
}

void find_first_tick(FILE* file, int firstitem)
{
  struct Instrument_Record record;
  bool wrapped = false;

  fseek(file, sizeof(record)*firstitem, SEEK_SET);
  int cnt = firstitem;

  while (!feof(file)) {
    if (fread(&record,sizeof(record),1,file) == 0) {    // EOF reached, wrap around
      if (!wrapped)
      {
        fseek(file, 0, SEEK_SET);
        cnt = 0;
        wrapped = true;
        continue;
      }
      break;
    }

    if (wrapped && cnt == firstitem)                    // end of buffer reached
    {
      break;
    }

    if (record.type == 0) {
      break;
    }

    record.type = swap2bytes(record.type);
    record.thread = swap2bytes(record.thread);
    record.timestamp = swap4bytes(record.timestamp);
    record.arg1 = swap4bytes(record.arg1);
    record.arg2 = swap4bytes(record.arg2);

    if (record.type == CYG_INSTRUMENT_CLOCK_TICK_START) {
      ticks = ((CYG_WORD64)record.arg2 << 32) + ((CYG_WORD64)record.arg1);
      printf("first tick found on item %d => %d\r\n", cnt, ticks);
      ticks--;
      return;
    }

    cnt++;
  }
  printf("first tick not found\r\n");
}

int main(int argc, char * argv[]) 
{
  FILE * file;
  char * filename;
  struct Instrument_Record record;
  int cnt=0;
  int firstitem = 0;
  bool wrapped = false;

  ns_per_tick = 1000000/rtc_resolution[1];
  ps_per_hal_clock = ns_per_tick * 1000 / CYGNUM_KERNEL_COUNTERS_RTC_PERIOD;

  if (argc != 2 && argc != 3) {
    usage(argv[0]);
    exit(1);
  }

  filename = argv[1];

  if (argc == 3)
  {
    firstitem = atoi(argv[2]);
  }

  file = fopen(filename, "r");
  if (!file) {
    fprintf(stderr,"Error opening file %s: ",filename);
    perror("");
    exit(1);
  }

  find_first_tick(file, firstitem);

  fseek(file, sizeof(record)*firstitem, SEEK_SET);
  cnt = firstitem;

  while (!feof(file)) {
    if (fread(&record,sizeof(record),1,file) == 0) {    // EOF reached, wrap around
      if (!wrapped)
      {
        fseek(file, 0, SEEK_SET);
        cnt = 0;
        wrapped = true;
        continue;
      }
      break;
    }

    if (wrapped && cnt == firstitem)                    // end of buffer reached
    {
      break;
    }

    if (record.type == 0) {
      break;
    }

    record.type = swap2bytes(record.type);
    record.thread = swap2bytes(record.thread);
    record.timestamp = swap4bytes(record.timestamp);
    record.arg1 = swap4bytes(record.arg1);
    record.arg2 = swap4bytes(record.arg2);


    if (record.type == CYG_INSTRUMENT_CLOCK_TICK_START)
    {
      if (ticks != ((CYG_WORD64)record.arg2 << 32) + ((CYG_WORD64)record.arg1))
      {
        printf("tick count error : ticks = %lld, should be %lld\r\n", 
          ticks, ((CYG_WORD64)record.arg2 << 32) + ((CYG_WORD64)record.arg1));
        ticks = ((CYG_WORD64)record.arg2 << 32) + ((CYG_WORD64)record.arg1);
      }
    }


#ifdef CYGDBG_KERNEL_INSTRUMENT_MSGS 
    printf("%4d Record type (0x%04x): %-20s thread %2d, ",
           cnt++,record.type,cyg_instrument_msg(record.type), 
           record.thread);
#else
    printf("%4d Record type 0x%04x, thread %2d, ",
           cnt++,record.type, record.thread);
#endif
    printf("time %10lld, arg1 0x%08x, arg2 0x%08x\n",
           cvt_time(record.timestamp), record.arg1,
           record.arg2);

    if (record.type == CYG_INSTRUMENT_CLOCK_ISR)
    {
      printf("tick increase\r\n");
      ticks++;
    }
  }

  fclose(file);
  printf("end-of-instrumentation\r\n");
  return (0);
}