#ifndef CLIENT_ONLY
#include <assert.h>
#include <sys/mman.h>
#include <unistd.h>
#include <stdio.h>
#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/sem.h>
#include <sched.h>
#include <stdlib.h>
#include <limits.h>
#include "peekpoke.h"
#include "cavium.h"

extern int BusCyclesLog;
void (*buslock)(void);
void (*busunlock)(void);
void (*buspreempt)(void);

void sbuslock(void);
void sbusunlock(void);
void sbuspreempt(void);

static volatile unsigned int *cvspiregs, *cvgpioregs;
static int last_gpio_adr = 0;
static int initted = 0;

int cavium_detect_model() {
  int model;

  sbuslock();
  model = cavium_peek16(0x60);
  sbusunlock();
  return model;
}

void _init_cavium() {
  int i;

  cvspiregs[0x64 / 4] = 0x0; /* RX IRQ threahold 0 */
  cvspiregs[0x40 / 4] = 0x80000c02; /* 24-bit mode, no byte swap */
  cvspiregs[0x60 / 4] = 0x0; /* 0 clock inter-transfer delay */
  cvspiregs[0x6c / 4] = 0x0; /* disable interrupts */
  cvspiregs[0x4c / 4] = 0x4; /* deassert CS# */
  for (i = 0; i < 8; i++) cvspiregs[0x58 / 4];
  last_gpio_adr = 3;
  cvgpioregs[0] = (cvgpioregs[0] & ~(0x3<<15))|(last_gpio_adr<<15);
  cvgpioregs[0] = (2<<15|1<<17|1<<3);
}

void init_cavium() {
  int devmem=-1;

  if (initted) return;
  cvspiregs  = map_phys(0x71000000,&devmem);
  assert(devmem != -1);
  cvgpioregs = map_phys(0x7c000000,&devmem);
  sbuslock();
  _init_cavium();
  sbusunlock();

  buslock = sbuslock;
  busunlock = sbusunlock;
  buspreempt = sbuspreempt;
}

void reservemem(void) {
	char dummy[32768];
	int i, pgsize;
	FILE *maps;

	pgsize = getpagesize();
	mlockall(MCL_CURRENT|MCL_FUTURE);
	for (i = 0; i < sizeof(dummy); i += 4096) {
		dummy[i] = 0;
	}

	maps = fopen("/proc/self/maps", "r"); 
	if (maps == NULL) {
		perror("/proc/self/maps");
		exit(1);
	}
	while (!feof(maps)) {
		unsigned int s, e, i;
		char m[PATH_MAX + 1];
		char perm[16];
		int r = fscanf(maps, "%x-%x %s %*x %*x:%*x %*d",
		  &s, &e, perm);
		if (r == EOF) break;
		assert (r == 3);

		i = 0;
		while ((r = fgetc(maps)) != '\n') {
			if (r == EOF) break;
			m[i++] = r;
		}
		m[i] = '\0';
		assert(s <= e && (s & 0xfff) == 0);
		if (perm[0] == 'r') while (s < e) {
			unsigned char *ptr = (unsigned char *)s;
			*ptr;
			s += pgsize;
		}
	}
}

static int semid = -1;
static int sbuslocked = 0;
extern int gotHUP;
void ts4500_DioDataReInit();
void sbuslock(void) {
	int r;
	struct sembuf sop; 

	if (semid == -1) {
		key_t semkey;
		reservemem();
		semkey = 0x75000000;
		semid = semget(semkey, 1, IPC_CREAT|IPC_EXCL|0777);
		if (semid != -1) {
			sop.sem_num = 0;
			sop.sem_op = 1;
			sop.sem_flg = 0;
			r = semop(semid, &sop, 1);
			assert (r != -1);
		} else semid = semget(semkey, 1, 0777);
		assert (semid != -1);
	}
	sop.sem_num = 0;
	sop.sem_op = -1;
	sop.sem_flg = SEM_UNDO;
	r = semop(semid, &sop, 1);
	assert (r == 0);
	cvgpioregs[0] = (2<<15|1<<17|1<<3);
	sbuslocked = 1;
	if (gotHUP) {
	  _init_cavium();
	  //ts4500_DioDataReInit();
	  //gotHUP = 0;
	}
	cvgpioregs[0] = (cvgpioregs[0] & ~(0x3<<15))|(last_gpio_adr<<15);
	if (gotHUP) {
	  //_init_cavium();
	  ts4500_DioDataReInit();
	  gotHUP = 0;
	}
}

void sbusunlock(void) {
	struct sembuf sop = { 0, 1, SEM_UNDO};
	int r;
	if (!sbuslocked) return;
	r = semop(semid, &sop, 1);
	assert (r == 0);
	sbuslocked = 0;
}


void sbuspreempt(void) {
	int r;
	r = semctl(semid, 0, GETNCNT);
	assert (r != -1);
	if (r) {
		sbusunlock();
		sched_yield();
		sbuslock();
	}
}

unsigned short cavium_peek16(unsigned int adr) {
  unsigned short ret;
  unsigned int adr0 = adr;
  volatile unsigned int *spi = cvspiregs;

  if (last_gpio_adr != adr >> 5) {
    last_gpio_adr = adr >> 5;
    cvgpioregs[0] = ((adr>>5)<<15|1<<3|1<<17);
  }
  adr &= 0x1f;

  asm volatile (
		"mov %0, %1, lsl #18\n"
		"2: str %0, [%2, #0x50]\n"
		"1: ldr r1, [%2, #0x64]\n"
		"cmp r1, #0x0\n"
		"beq 1b\n"
		"ldr %0, [%2, #0x58]\n"
		"ands r1, %0, #0x10000\n"
		"bicne %0, %0, #0xff0000\n"
		"moveq %0, #0x0\n"
		"beq 2b\n" 
		: "+r"(ret) : "r"(adr), "r"(cvspiregs) : "r1", "cc"
		);
  if (BusCyclesLog & 1) {
    fprintf(stderr,"%4X <- cavium_peek16(%4X)\n",ret,adr0);
  }
  return ret;
}

void cavium_poke16(unsigned int adr, unsigned short dat) {
  unsigned int dummy;
  unsigned int d = dat;
  volatile unsigned int *spi = cvspiregs;

  if (BusCyclesLog & 1) {
    fprintf(stderr,"%4X -> cavium_poke16(%4X)\n",dat,adr);
  }

  if (last_gpio_adr != adr >> 5) {
    last_gpio_adr = adr >> 5;
    cvgpioregs[0] = (cvgpioregs[0] & ~(0x3<<15))|((adr>>5)<<15);
  }
  adr &= 0x1f;

  asm volatile (
		"mov %0, %1, lsl #18\n"
		"orr %0, %0, #0x800000\n"
		"orr %0, %0, %2, lsl #3\n"
		"3: ldr r1, [%3, #0x64]\n"
		"cmp r1, #0x0\n"
		"bne 3b\n"
		"2: str %0, [%3, #0x50]\n"
		"1: ldr r1, [%3, #0x64]\n"
		"cmp r1, #0x0\n"
		"beq 1b\n"
		"ldr %0, [%3, #0x58]\n"
		"ands r1, %0, #0x1\n"
		"moveq %0, #0x0\n"
		"beq 3b\n"
		: "+r"(dummy) : "r"(adr), "r"(d), "r"(cvspiregs) : "r1","cc"
		);
}
#endif

/*
  Author: Michael Schmidt (michael@embeddedARM.com)
 */