#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"

/*
  Arch Specific DIO functionality
 */
#define DIO_NOT_PRESENT 0
#define DIO_CAN_INPUT 1
#define DIO_CAN_DRIVE_HIGH 2
#define DIO_CAN_DRIVE_LOW 4
#define DIO_INPUT_ONLY DIO_CAN_INPUT
#define DIO_OUTPUT_ONLY (DIO_CAN_DRIVE_HIGH | DIO_CAN_DRIVE_LOW)
#define DIO_NORMAL (DIO_CAN_DRIVE_HIGH | DIO_CAN_DRIVE_LOW | DIO_CAN_INPUT)
typedef unsigned char uint8;
typedef unsigned uint32;

void (*buslock)(void);
void (*busunlock)(void);
void (*buspreempt)(void);
void cavium_poke16(unsigned int, unsigned short);
unsigned short cavium_peek16(unsigned int);
void sbuslock(void);
void sbusunlock(void);
void sbuspreempt(void);

extern uint8 TotalDioCount;
extern uint8 DioCapabilities[256];

void DioInit();
void SetDioDir(int dionum,int asOutput);
void SetDio(int dionum,int asHigh);
int GetDioDir(int dionum);
int GetDio(int dionum);

#ifdef CLIENT_ONLY
/*
void DioInit() {
}

void SetDioDir(int dionum,int asOutput) {
}
void SetDio(int dionum,int asHigh) {
}

int GetDioDir(int dionum) {
}

int GetDio(int dionum) {
}
*/
#else
static volatile unsigned int *cvspiregs, *cvgpioregs;
static int last_gpio_adr = 0;

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 DioInit() {
  int i;
  int devmem=-1;
  int spi_ext=0;

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

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

  TotalDioCount = 53;
  for (i=0;i<TotalDioCount;i++) {
    DioCapabilities[i] = DIO_NORMAL;
  }
}

static inline void pokebit16(int adrs,int bit,int value) {
  value = value ? (1<<bit) : 0;
  cavium_poke16(adrs,cavium_peek16(adrs) & ~(1 << bit) | value);
}

static inline int peekbit16(int adrs,int bit) {
  //  unsigned short x= cavium_peek16(adrs);
  //fprintf(stderr,"peekbit16(%X,%d) read %X\n",adrs,bit,x);
  return (cavium_peek16(adrs) & (1 << bit)) ? 1 : 0;
}

void SetDioDir(int dionum,int asOutput) {
  if (dionum > 52) return;
  buslock();
  if (dionum > 48) {
    pokebit16(0x7E,dionum-49,asOutput);
  } else if (dionum > 40) {
    pokebit16(0x7A,dionum-41,asOutput);
  } else if (dionum > 36) {
    pokebit16(0x66,dionum-37+4,asOutput);
  } else if (dionum > 20) {
    pokebit16(0x6C,dionum-21,asOutput);
  } else if (dionum > 4) {
    pokebit16(0x72,dionum-5,asOutput);
  } else if (dionum >= 0) {
    pokebit16(0x78,dionum,asOutput);
  }
  //  fprintf(stderr,"SetDioDir(%d,%d)\n",dionum,asOutput);
  busunlock();
}

void SetDio(int dionum,int asHigh) {
  if (dionum > 52) return;
  buslock();
  if (dionum > 48) {
    pokebit16(0x7E,dionum-49+4,asHigh);
  } else if (dionum > 40) {
    pokebit16(0x7A,dionum-41+8,asHigh);
  } else if (dionum > 36) {
    pokebit16(0x66,dionum-37+8,asHigh);
  } else if (dionum > 20) {
    pokebit16(0x6A,dionum-21,asHigh);
  } else if (dionum > 4) {
    pokebit16(0x70,dionum-5,asHigh);
  } else if (dionum >= 0) {
    pokebit16(0x78,dionum+5,asHigh);
  }
  //fprintf(stderr,"SetDio(%d,%d)\n",dionum,asHigh);
  busunlock();
}

int GetDioDir(int dionum) {
  int ret;

  if (dionum > 52) return;
  buslock();
  if (dionum > 48) {
    ret = peekbit16(0x7E,dionum-49);
  } else if (dionum > 40) {
    ret = peekbit16(0x7A,dionum-41);
  } else if (dionum > 36) {
    ret = peekbit16(0x66,dionum-37+4);
  } else if (dionum > 20) {
    ret = peekbit16(0x6C,dionum-21);
  } else if (dionum > 4) {
    ret = peekbit16(0x72,dionum-5);
  } else if (dionum >= 0) {
    ret = peekbit16(0x78,dionum);
  }
  busunlock();
  return ret;
}

int GetDio(int dionum) {
  int ret;

  if (dionum > 52) return;
  buslock();
  if (dionum > 40) {
    ret = peekbit16(0x7C,dionum-41);
  } else if (dionum > 36) {
    ret = peekbit16(0x66,dionum-37+12);
  } else if (dionum > 20) {
    ret = peekbit16(0x68,dionum-21);
  } else if (dionum > 4) {
    ret = peekbit16(0x6E,dionum-5);
  } else if (dionum >= 0) {
    ret = peekbit16(0x78,dionum+10);
  }
  busunlock();
  return ret;
}
/*
Register Map for DIOs

pin  ibit  obit  dirbit
52   7C:11 7E:7  7E:3
51   7C:10 7E:6  7E:2
50   7C:9  7E:5  7E:1
49   7C:8  7E:4  7E:0
48   7C:7  7A:15 7A:7
47   7C:6  7A:14 7A:6
46   7C:5  7A:13 7A:5
45   7C:4  7A:12 7A:4
44   7C:3  7A:11 7A:3
43   7C:2  7A:10 7A:2
42   7C:1  7A:9  7A:1
41   7C:0  7A:8  7A:0

40   66:15 66:11 66:7
39   66:14 66:10 66:6
38   66:13 66:9  66:5
37   66:12 66:8  66:4

36   68:15 6A:15 6C:15
35   68:14 6A:14 6C:14
34   68:13 6A:13 6C:13
33   68:12 6A:12 6C:12
32   68:11 6A:11 6C:11
31   68:10 6A:10 6C:10
30   68:9  6A:9  6C:9 
29   68:8  6A:8  6C:8 
28   68:7  6A:7  6C:7 
27   68:6  6A:6  6C:6 
26   68:5  6A:5  6C:5 
25   68:4  6A:4  6C:4 
24   68:3  6A:3  6C:3 
23   68:2  6A:2  6C:2 
22   68:1  6A:1  6C:1 
21   68:0  6A:0  6C:0 

20   6E:15 70:15 72:15
19   6E:14 70:14 72:14
18   6E:13 70:13 72:13
17   6E:12 70:12 72:12
16   6E:11 70:11 72:11
15   6E:10 70:10 72:10
14   6E:9  70:9  72:9 
13   6E:8  70:8  72:8 
12   6E:7  70:7  72:7 
11   6E:6  70:6  72:6 
10   6E:5  70:5  72:5 
 9   6E:4  70:4  72:4 
 8   6E:3  70:3  72:3 
 7   6E:2  70:2  72:2 
 6   6E:1  70:1  72:1 
 5   6E:0  70:0  72:0 

 4   78:14 78:9  78:4
 3   78:13 78:8  78:3
 2   78:12 78:7  78:2
 1   78:11 78:6  78:1
 0   78:10 78:5  78:0

Run these commands on the TS-4500 to test:
killall ts7500ctl;ts7500ctl --setrng --getrtc --resetswitchoff --autofeed 2
udhcpc
mount -o nolock,vers=2 192.168.0.11:/u/x/ /mnt/host
chroot /mnt/host
mount -t proc proc /proc
cd /home/michael/dioctl/
./dioctl-cavium-uclibc -s

A couple of ways to load new bitstreams:
export PATH=$PATH:/usr/local/bin
ts7500ctl --loadfpga ts4500B.jed

ts7500ctl --loadfpga /mnt/host/var/ts-production/images/ts4500/ts4500-rev1.vme.gz

known bugs:
1. when setting DIO states we assume the correct number of values are supplied.
   if this is not the case we will get out of sync with the server.

TS-8200 loopback wiring:
0
1 - 23
2 - 3
4 - 5
6 - 35
7
8 - 10
9
11 - 12
13 - 14
15
16
17 - 18
19 - 20
21
22 - 44
24 - 29
25 - 28
26 - 27
30 - 31 - 32
33 - 34
36
37
38
39
40 - 47
41 - 42
43 - 50
45 - 46
48
49
51 - 52

More sample commands:
./dioctl -p 192.168.0.216:4500 -i 1 -1 23 -r 23 -r 1
./dioctl -p 192.168.0.216:4500 -0 23 -r 23 -r 1
./dioctl-x86 -p 192.168.0.216:4500 -G 1:1:2:4:6:8:11:13:17:19:22:24:25:26:30:33 -G 2:23:3:5:35:10:12:14:18:20:44:29:28:27:31:32:34 -i 1001 -1 1002 -r 1001 -0 1002 -r 1001
/u/x/home/michael/dioctl/dioctl-x86 -p 192.168.0.216:4500 --test

To Do later:
Detect TS-4500
Detect TS-8200 base board

 */
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 gotHUP;
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();
	  gotHUP = 0;
	}
	cvgpioregs[0] = (cvgpioregs[0] & ~(0x3<<15))|(last_gpio_adr<<15);
}

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;
  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"
		);
  return ret;
}

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

  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