#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 "baudtab.h"
#include "canctl.h"
#include "peekpoke.h"

#ifdef REMOTE_ONLY
void init_cavium() {
  printf("No Cavium support in this binary");
  exit(3);
}
#else
void cavium_poke8(unsigned int, unsigned char);
void cavium_poke16(unsigned int, unsigned short);
void cavium_poke32(unsigned int, unsigned int);
unsigned char cavium_peek8(unsigned int);
unsigned short cavium_peek16(unsigned int);
unsigned int cavium_peek32(unsigned int);
void sbuslock(void);
void sbusunlock(void);
void sbuspreempt(void);
unsigned int cavium_can_peek8(int adr);
void cavium_can_poke8(int adr, unsigned int dat);
void cavium_can_peekstream(int adr, unsigned char *buf, int n);
void cavium_can_pokestream(int adr, unsigned char *buf, int n);

extern int gotHUP;
static volatile unsigned int *cvspiregs, *cvgpioregs;

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];
  cvgpioregs[0] = (2<<15|1<<17|1<<3);
}

void init_cavium() {
  int devmem=-1;
  cvspiregs  = map_phys(0x71000000,&devmem);
  assert(devmem != -1);
  cvgpioregs = map_phys(0x7c000000,&devmem);
  
  sbuslock();
  _init_cavium();
  sbusunlock();

  buslock = sbuslock;
  busunlock = sbusunlock;
  buspreempt = sbuspreempt;
  can_peek8 = cavium_can_peek8;
  can_poke8 = cavium_can_poke8;
  can_peekstream = cavium_can_peekstream;
  can_pokestream = cavium_can_pokestream;

  irq_num = 30;
  baudparms = baudparms25;
}

void cavium_poke8(unsigned int adr, unsigned char dat) {
	if (!cvspiregs) *(unsigned char *)adr = dat;
	else {
		if (adr & 0x1) {
			cvgpioregs[0] = (0x2<<15|1<<17);
			cavium_poke16(adr, dat << 8);
		} else {
			cvgpioregs[0] = (0x2<<15|1<<3);
			cavium_poke16(adr, dat);
		}
		cvgpioregs[0] = (0x2<<15|1<<17|1<<3);
	}

}

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

	if (!cvspiregs) {
		*(unsigned short *)adr = dat;
	} else 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"
	);
}

void cavium_poke32(unsigned int adr, unsigned int dat) {
	if (!cvspiregs) *(unsigned int *)adr = dat;
	else {
		cavium_poke16(adr, dat & 0xffff);
		cavium_poke16(adr + 2, dat >> 16);
	}
}

unsigned char cavium_peek8(unsigned int adr) {
	unsigned char ret;
	if (!cvspiregs) ret = *(unsigned char *)adr;
	else {
		unsigned short x;
		x = cavium_peek16(adr);
		if (adr & 0x1) ret = x >> 8;
		else ret = x & 0xff;
	}
	return ret;
}

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

	if (!cvspiregs) ret = *(unsigned short *)adr;
	else 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"
	);

	return ret;

}

unsigned int cavium_peek32(unsigned int adr) {
	unsigned int ret;
	if (!cvspiregs) ret = *(unsigned int *)adr;
	else {
		unsigned short l, h;
		l = cavium_peek16(adr);
		h = cavium_peek16(adr + 2);
		ret = (l|(h<<16));
	}
	return ret;
}

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;
int canadr;
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;
	canadr = -1;
	if (gotHUP) {
	  _init_cavium();
	  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 int cavium_can_peek8(int adr) {
	if (adr != canadr) cavium_poke16(0x10, adr);
	canadr = adr + 1;
	
	return cavium_peek16(0x12) & 0xff;
}


void cavium_can_poke8(int adr, unsigned int dat) {
	dat &= 0xff;
	cavium_poke16(0x16, adr<<8|dat);
	canadr = adr + 1;
}


void cavium_can_peekstream(int adr, unsigned char *buf, int n) {
	assert(n != 0);

	if (adr != canadr) cavium_poke16(0x10, adr);
	canadr = adr + n;

	while (n >= 2) {
		unsigned int s = cavium_peek16(0x14);
		*buf++ = s & 0xff;
		*buf++ = s >> 8;
		n -= 2;
	}

	if (n) *buf = cavium_peek16(0x12) & 0xff;
};


void cavium_can_pokestream(int adr, unsigned char *buf, int n) {
	int na = adr + n + 1;
	assert(n != 0);

	if (adr != canadr) {
		can_poke8(adr, *buf++);
		n--;
	} 
	
	canadr = na;

	while (n >= 2) {
		unsigned int s;
		s = *buf++;
		s |= *buf++ << 8;
		cavium_poke16(0x14, s);
		n -= 2;
	}

	if (n) cavium_poke16(0x12, *buf);
}
#endif