#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"
#include "dioctl.h"
#include "dioname.h"

/*
  Arch Specific DIO functionality
 */
extern int BusCyclesLog;
#ifndef CLIENT_ONLY
void ts4500_SetDioDir(int dionum,int asOutput);
void ts4500_SetDio(int dionum,int asHigh);
int ts4500_GetDioDir(int dionum);
int ts4500_GetDio(int dionum);
void ts4500_CommitDio();

uint16 reg62,reg66,reg68,reg6A,reg6C,reg6E,reg70,reg72,reg78,reg7A,reg7C,reg7E;
int dirty[12],ready[12];

uint8 TotalDioCount;
uint8 DioCapabilities[256];

void ts4500_DioDataReInit() {
  // internal, assumes sbuslock held...
  cavium_poke16(0x62,reg62);
  cavium_poke16(0x66,reg66);
  cavium_poke16(0x68,reg68);
  cavium_poke16(0x6A,reg6A);
  cavium_poke16(0x6C,reg6C);
  cavium_poke16(0x6E,reg6E);
  cavium_poke16(0x70,reg70);
  cavium_poke16(0x72,reg72);
  cavium_poke16(0x78,reg78);
  cavium_poke16(0x7A,reg7A);
  cavium_poke16(0x7C,reg7C);
  cavium_poke16(0x7E,reg7E);
}

void ts4500_DioDataInit() {
  buslock();
  reg62 = cavium_peek16(0x62);
  reg66 = cavium_peek16(0x66);
  reg68 = cavium_peek16(0x68);
  reg6A = cavium_peek16(0x6A);
  reg6C = cavium_peek16(0x6C);
  reg6E = cavium_peek16(0x6E);
  reg70 = cavium_peek16(0x70);
  reg72 = cavium_peek16(0x72);
  reg78 = cavium_peek16(0x78);
  reg7A = cavium_peek16(0x7A);
  reg7C = cavium_peek16(0x7C);
  reg7E = cavium_peek16(0x7E);
  busunlock();
}

void ts4500_DioInit() {
  int i;
  int devmem=-1;
  int spi_ext=0;

  SetDioDir = ts4500_SetDioDir;
  SetDio = ts4500_SetDio;
  GetDioDir = ts4500_GetDioDir;
  GetDio = ts4500_GetDio;
  CommitDio = ts4500_CommitDio;
  ts4500_DioDataInit();
}

static inline void pokebit16(int adrs,int bit,int value) {
	uint16 *reg;
	int n,bval = (1<<bit);
	
	if (BusCyclesLog & 2) {
	  fprintf(stderr,"pokebit16(%4X,%d,%d)\n",adrs,bit,value);
	}
	switch (adrs) {
	case 0x62: reg=&reg62; n = 0; break;
	case 0x66: reg=&reg66; n = 1; break;
	case 0x68: reg=&reg68; n = 2; break;
	case 0x6A: reg=&reg6A; n = 3; break;
	case 0x6C: reg=&reg6C; n = 4; break;
	case 0x6E: reg=&reg6E; n = 5; break;
	case 0x70: reg=&reg70; n = 6; break;
	case 0x72: reg=&reg72; n = 7; break;
	case 0x78: reg=&reg78; n = 8; break;
	case 0x7A: reg=&reg7A; n = 9; break;
	case 0x7C: reg=&reg7C; n = 10; break;
	case 0x7E: reg=&reg7E; n = 11; break;
	default: return;
	}
	if (((*reg & bval)?1:0) != (value?1:0)) {
	  //fprintf(stderr,"n=%d, %X & %X -> %d != %d ",n,*reg, bval,(*reg & bval)?1:0,(value?1:0));
		if (dirty[n] & bval) {
			dirty[n] = 0;
			buslock();
			cavium_poke16(adrs,*reg);
			busunlock();
		}
		*reg = *reg & ~(1 << bit) | (value ? (1<<bit) : 0);
		dirty[n] |= bval;
	}
  //old naive code:
  //value = value ? (1<<bit) : 0;
  //cavium_poke16(adrs,cavium_peek16(adrs) & ~(1 << bit) | value);
}

static inline int peekbit16(int adrs,int bit) {
	uint16 *reg, mask, tmp;
	int n,ret;
	// mask bits high indicate bits that may change under us
	// and so we should not cache them 
	switch (adrs) {
	case 0x62: reg=&reg62; n = 0;  mask=0x30FF; break;
	case 0x66: reg=&reg66; n = 1;  mask=0xF001; break;
	case 0x68: reg=&reg68; n = 2;  mask=0xFFFF; break;
	case 0x6A: reg=&reg6A; n = 3;  mask=0x0000; break;
	case 0x6C: reg=&reg6C; n = 4;  mask=0x0000; break;
	case 0x6E: reg=&reg6E; n = 5;  mask=0xFFFF; break;
	case 0x70: reg=&reg70; n = 6;  mask=0x0000; break;
	case 0x72: reg=&reg72; n = 7;  mask=0x0000; break;
	case 0x78: reg=&reg78; n = 8;  mask=0xFC00; break;
	case 0x7A: reg=&reg7A; n = 9;  mask=0x0000; break;
	case 0x7C: reg=&reg7C; n = 10; mask=0xFFFF; break;
	case 0x7E: reg=&reg7E; n = 11; mask=0xFFC0; break;
	default: return 0;
	}
	if (!ready[n] && ((1 << bit) & mask)) {
	  //fprintf(stderr,"n=%d,ready=%X,bit=%d,mask=%X",n,ready[n],bit,mask);
		ready[n] = 1;
		buslock();
		tmp = cavium_peek16(adrs);
		*reg = (tmp & mask) | (*reg & ~mask);
		busunlock();
	}
	ret = (*reg & (1 << bit)) ? 1 : 0;
	if (BusCyclesLog & 2) {
	  fprintf(stderr,"%d <- peekbit16(%4X,%d)\n",ret,adrs,bit);
	}
	return ret;
  //old naive code:
  //return (cavium_peek16(adrs) & (1 << bit)) ? 1 : 0;
}

void ts4500_SetDioDir(int dionum,int asOutput) {
  if (BusCyclesLog & 4) {
    fprintf(stderr,"ts4500_SetDioDir(%d,%d)\n",dionum,asOutput);
  }
  if (dionum > 52) return;
  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);
}

void ts4500_SetDio(int dionum,int asHigh) {
  if (BusCyclesLog & 4) {
    fprintf(stderr,"ts4500_SetDio(%d,%d)\n",dionum,asHigh);
  }

  if (dionum == 128) { // GREEN LED
	pokebit16(0x62,15,asHigh);
	return;
  } else if (dionum == 129) { // RED LED
	pokebit16(0x62,14,asHigh);
	return;
  }
  if (dionum > 52) return;
  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);
}

int ts4500_GetDioDir(int dionum) {
  int ret;

  if (dionum > 52) {
    ret = 1;
  } else 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);
  }
  if (BusCyclesLog & 4) {
    fprintf(stderr,"%d <- ts4500_GetDioDir(%d)\n",ret,dionum);
  }
  return ret;
}

int ts4500_GetDio(int dionum) {
  int ret;

  if (dionum == 128) { // GREEN LED
	ret = peekbit16(0x62,15);
  } else if (dionum == 129) { // RED LED
	ret = peekbit16(0x62,14);
  } else if (dionum > 52) {
    ret = 1;
  } else 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);
  }
  if (BusCyclesLog & 4) {
    fprintf(stderr,"%d <- ts4500_GetDio(%d)\n",ret,dionum);
  }

  return ret;
}

void ts4500_CommitDio(int write) {
	int i;

	if (BusCyclesLog & 8) {
	  fprintf(stderr,"ts4500_CommitDio(%d)\n",write);
	}

	for (i=0;i<12;i++) {
		ready[i] = 0;
	}
	if (!write) {
	  return;
	}
	for (i=0;i<12;i++) {
		if (dirty[i]) {
			break;
		}
	}
	if (i == 12) {
		return;
	}
	buslock();
	if (dirty[0]) {
	  if (BusCyclesLog & 8) {
	    fprintf(stderr,"dirty[0]=%X\n",dirty[0]);
	  }
	  cavium_poke16(0x62,reg62);
	}
	if (dirty[1]) {
	  if (BusCyclesLog & 8) {
	    fprintf(stderr,"dirty[1]=%X\n",dirty[1]);
	  }
	  cavium_poke16(0x66,reg66);
	}
	if (dirty[2]) {
	  if (BusCyclesLog & 8) {
	    fprintf(stderr,"dirty[2]=%X\n",dirty[2]);
	  }
	  cavium_poke16(0x68,reg68);
	}
	if (dirty[3]) {
	  if (BusCyclesLog & 8) {
	    fprintf(stderr,"dirty[3]=%X\n",dirty[3]);
	  }
	  cavium_poke16(0x6A,reg6A);
	}
	if (dirty[4]) {
	  if (BusCyclesLog & 8) {
	    fprintf(stderr,"dirty[4]=%X\n",dirty[4]);
	  }
	  cavium_poke16(0x6C,reg6C);
	}
	if (dirty[5]) {
	  if (BusCyclesLog & 8) {
	    fprintf(stderr,"dirty[5]=%X\n",dirty[5]);
	  }
	  cavium_poke16(0x6E,reg6E);
	}
	if (dirty[6]) {
	  if (BusCyclesLog & 8) {
	    fprintf(stderr,"dirty[6]=%X\n",dirty[6]);
	  }
	  cavium_poke16(0x70,reg70);
	}
	if (dirty[7]) {
	  if (BusCyclesLog & 8) {
	    fprintf(stderr,"dirty[7]=%X\n",dirty[7]);
	  }
	  cavium_poke16(0x72,reg72);
	}
	if (dirty[8]) {
	  if (BusCyclesLog & 8) {
	    fprintf(stderr,"dirty[8]=%X\n",dirty[8]);
	  }
	  cavium_poke16(0x78,reg78);
	}
	if (dirty[9]) {
	  if (BusCyclesLog & 8) {
	    fprintf(stderr,"dirty[9]=%X\n",dirty[9]);
	  }
	  cavium_poke16(0x7A,reg7A);
	}
	if (dirty[10]) {
	  if (BusCyclesLog & 8) {
	    fprintf(stderr,"dirty[10]=%X\n",dirty[10]);
	  }
	  cavium_poke16(0x7C,reg7C);
	}
	if (dirty[11]) {
	  if (BusCyclesLog & 8) {
	    fprintf(stderr,"dirty[11]=%X\n",dirty[11]);
	  }
	  cavium_poke16(0x7E,reg7E);
	}
	busunlock();
	for (i=0;i<12;i++) {
		dirty[i] = 0;
	}
}

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


#endif