#include "rtapi_ctype.h"		/* isspace() */
#include "rtapi.h"		/* RTAPI realtime OS API */
#include "rtapi_app.h"		/* RTAPI realtime module decls */
#include "hal.h"			/* HAL public API decls */
#include <linux/types.h>

#include <linux/pci.h>

/* If FASTIO is defined, uses outb() and inb() from <asm.io>,
   instead of rtapi_outb() and rtapi_inb() - the <asm.io> ones
   are inlined, and save a microsecond or two (on my 233MHz box)
*/

/* module information */
MODULE_AUTHOR("");
MODULE_DESCRIPTION("");
MODULE_LICENSE("GPL");
/* static char *cfg = 0; */
/* config string
RTAPI_MP_STRING(cfg, "config string"); */

struct pci_dev *to_io;



/***********************************************************************
*                STRUCTURES AND GLOBAL VARIABLES                       *
************************************************************************/

/* this structure contains the runtime data needed by the
   driver for a single port/channel
*/


typedef struct {
	void *mem_base;
  	__u32 io_base;
	int len;
	hal_bit_t *digital_in[32];    /* ptrs for digital input pins 0 - 31 */
  	hal_bit_t *digital_out[32];    /* ptrs for digital output pins 0 - 31 */
	hal_bit_t *enable_dr; 
	hal_bit_t *index_en[5];
	hal_float_t *enccounts[5];
	hal_float_t *encscale;
	hal_float_t *outscale;
	hal_float_t *dcontrol;
	hal_s32_t *indikator;

} to_pci_t;


/* pointer to array of evoreg_t structs in shared memory, 1 per port */
static to_pci_t *device_data;

/* other globals */
static int comp_id;		/* component ID */
static int num_ports;		/* number of ports configured */

int WD_start =0;

/***********************************************************************
*                  LOCAL FUNCTION DECLARATIONS                         *
************************************************************************/
/* These is the functions that actually do the I/O
   everything else is just init code
*/
static void update_port(void *arg, long period);

/***********************************************************************
*                       INIT AND EXIT CODE                             *
************************************************************************/
/* 	adress 0 - encoder0 counter
	adress 1 - encoder1 counter
	adress 2 - encoder2 counter
	adress 3 - encoder3 counter
	adress 4 - encoder4 counter
	adress 5 - 
	adress 6 - Zero
	adress 7 - port in

	adress 8 - out0 shim
	adress 9 - out1 shim
	adress 10 - out2 shim
	adress 11 - out3 shim
	adress 12 - out4 shim
	adress 13 - 
	adress 14 - out_ pin(16)
	adress 15 - index

	adress 30 - reg_enable(write only)
	adress 31 - wathDog

*/
#define reg_enc0 0
#define reg_enc1 1
#define reg_enc2 2
#define reg_enc3 3
#define reg_enc4 4

#define reg_in 7
#define reg_out 14

#define reg_dcont 8
#define reg_dcont1 9
#define reg_dcont2 10
#define reg_dcont3 11
#define reg_dcont4 12

#define reg_index 15
#define control_reg 30
#define reg_WD 31


#define VENDORID_dev   (0xFFF1)
#define num_dev        (0x5555)

#define driver_NAME "to_pci"


int rtapi_app_main(void)
{
    char name[HAL_NAME_LEN + 1];
    int n,i,r ;
    int retval=0;
    
    /* only one port at the moment */
    num_ports = 1;
    n = 0;

    
    /* STEP 1: initialise the driver */
    comp_id = hal_init(driver_NAME);
    if (comp_id < 0) {
    
    rtapi_print_msg(RTAPI_MSG_ERR,"to_pci: ERROR: hal_init() failed\n");
    
    return -1;
    }


    /* STEP 2: allocate shared memory for to_hal data */
    device_data = hal_malloc(num_ports * sizeof(to_pci_t));
    if (device_data == 0) {
    rtapi_print_msg(RTAPI_MSG_ERR,"to_pci: ERROR: hal_malloc() failed\n");
	r = -1;
	goto fail0;    
    }

////////////PCI INIT...      
   
    to_io = pci_get_device(VENDORID_dev, num_dev, to_io);
    
    if (NULL == to_io) {
    
      rtapi_print_msg(RTAPI_MSG_ERR,"to_io == NULL\n");  
      hal_exit(comp_id);
    return -1;
    }
    
    device_data->io_base = pci_resource_start(to_io, 0);

    device_data->len = pci_resource_len(to_io, 0);

    rtapi_print_msg(RTAPI_MSG_INFO,"to_pci: io_base: %X \n", device_data->io_base); 
   
    device_data->mem_base = ioremap_nocache( device_data->io_base,device_data->len);

	printk(KERN_ALERT "to_pci: io_base: %X, mem_base: %p\n", device_data->io_base, device_data->mem_base);

	if (device_data->mem_base == NULL) {
                rtapi_print_msg(RTAPI_MSG_ERR,"not_remap\n");
                r = -ENODEV;
                goto fail0;
	}
	
//    writel(0x6000000A,(device_data->mem_base)+(reg_WD*4));

//////////////////////


    /* Export IO pin's */

    /* export write only HAL pin's for the input bit */
    for ( i=0; i<=31;i++) {
      retval += hal_pin_bit_newf(HAL_OUT, &(device_data->digital_in[i]),
				 comp_id, "to_pci.%d.pins.pin-%02d-in", 1, i);


      if (retval < 0) {
	  rtapi_print_msg(RTAPI_MSG_ERR, "to_pci: ERROR: port %d var export failed with err=%i\n", n + 1,retval);
  	  r = -1;
	  goto fail1;
      }
    }

    /* export read only HAL pin's for the output bit */
    for ( i=0; i<=15;i++) {
      retval += hal_pin_bit_newf(HAL_IN, &(device_data->digital_out[i]),
				 comp_id, "to_pci.%d.pins.pin-%02d-out", 1, i);

      if (retval < 0) {
	  rtapi_print_msg(RTAPI_MSG_ERR,"to_pci: ERROR: port %d var export failed with err=%i\n", n + 1,
	      retval);
  	  r = -1;
	  goto fail1;
      }
    }





	/* export read only HAL pin's for the control bit */

	retval = hal_pin_bit_newf(HAL_IN, &(device_data->enable_dr),
				 comp_id, "to_pci.%d.enable_drive", 1);

	if (retval < 0) {
	  rtapi_print_msg(RTAPI_MSG_ERR,"to_pci: ERROR: enable_dr var export failed with err=%i\n",retval);
  	  r = -1;
	  goto fail1;
	}

	/* export encoder signal */
	retval = 0;
	for ( i=0; i<=4;i++) {

	retval += hal_pin_float_newf(HAL_OUT, &(device_data-> enccounts[i]), comp_id, "to_pci.%d.feedback.encoder%d", 1, i);

      	if (retval < 0) {
	  rtapi_print_msg(RTAPI_MSG_ERR,"to_pci: ERROR:  err=%i\n", retval);
  	  r = -1;
	  goto fail1;
	}
	}

	/* index */
	retval = 0;
    	for ( i=0; i<=4;i++) {
      	retval += hal_pin_bit_newf(HAL_IN, &(device_data->index_en[i]),
				 comp_id, "to_pci.%d.feedback.index_en%01d", 1, i);

      	if (retval < 0) {
	  rtapi_print_msg(RTAPI_MSG_ERR,"to_pci: ERROR: port %d var export failed with err=%i\n", n + 1,
	      retval);
  	  r = -1;
	  goto fail1;
      		}
    	}



	
	/* export parametr */
	retval = hal_pin_float_newf(HAL_IN, &(device_data-> encscale), comp_id, "to_pci.%d.feedback.enc_scale%d", 1, 0);
	retval += hal_pin_float_newf(HAL_IN, &(device_data-> outscale), comp_id, "to_pci.%d.out_scale%d", 1, 0);
      if (retval < 0) {
	  rtapi_print_msg(RTAPI_MSG_ERR,"to_pci: ERROR:  err=%i\n", retval);
  	  r = -1;
	  goto fail1;
	}

	retval = hal_pin_s32_newf(HAL_IN, &(device_data-> indikator), comp_id, "to_pci.%d.indikator", 1);

      if (retval < 0) {
	  rtapi_print_msg(RTAPI_MSG_ERR,"to_pci: ERROR:  err=%i\n", retval);
  	  r = -1;
	  goto fail1;
	}



	/* export control drive */
	retval = hal_pin_float_newf(HAL_IN, &(device_data-> dcontrol), comp_id, "to_pci.%d.dcontrol%d", 1, 0);

      if (retval < 0) {
	  rtapi_print_msg(RTAPI_MSG_ERR,"to_pci: ERROR:  err=%i\n", retval);
  	  r = -1;
	  goto fail1;
	}

    /* STEP 4: export function */
    rtapi_snprintf(name, sizeof(name), "to_pci.%d.update", n + 1);
    retval = hal_export_funct(name, update_port, device_data, 1, 0,
	comp_id);
    if (retval < 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,"to_pci: ERROR: port %d write funct export failed\n", n + 1);
	r = -1;
	  goto fail1;
    }

    rtapi_print_msg(RTAPI_MSG_INFO,"to_pci: installed driver for %d card(s)\n", num_ports);
	printk(KERN_ALERT "to_pci: installed driver for %d card(s)\n", num_ports);
    hal_ready(comp_id);
    return 0;

fail1:

iounmap((void*)device_data->mem_base);

fail0:

device_data->mem_base = NULL;
hal_exit(comp_id);
return r;

}

void rtapi_app_exit(void)
{	
//	writel(0x60000000,(device_data->mem_base)+(reg_WD*4));
	iounmap((void*)device_data->mem_base);
	hal_exit(comp_id);
	printk(KERN_ALERT "to_pci: exit\n");
}

int ABS_SUB(__s32 lt1, __s32 lt2)
{
	__s32 lt3;
	if(lt1<lt2)
		lt3 = lt2-lt1;
	else
		lt3 = lt1-lt2;

	if(lt3 < 5)
		return 0;
	else	
		return 1;
}
/**************************************************************
* REALTIME PORT WRITE FUNCTION                                *
**************************************************************/

void update_port(void *arg, long period){
	to_pci_t *port;
	__u32 tmp,mask,tmask,tanmask;
	__s32 ikor;
	int pin;
	port = arg;

	int flagok;
	__s32 temp1,temp2;//,temp3;

	tmask = 0x60000000;
	tanmask = 0xEFFFFFFF;

	tmp = readl((port->mem_base)+(reg_in*4));
// WD 
//	if (WD_start = 0)
//		{
			writel(0x6000000A,(device_data->mem_base)+(reg_WD*4));
//			WD_start=1;
//		}
// read digital inputs 	     
      	mask = 0x00000001;
	for (pin=0 ; pin < 32 ; pin++) {
		*(port->digital_in[pin]) = (tmp & mask) ? 1:0 ;
		mask <<= 1;
	}

// read encoder
/*	flagok =1;
	temp1=(__s32)readl((port->mem_base)+(reg_enc0*4));

	while (flagok)
	{
		temp2=(__s32)readl((port->mem_base)+(reg_enc0*4));
		if(temp1<temp2)
		{
			temp3 = temp2-temp1;
		}
		else
		{
			temp3 = temp1-temp2;
		}
		if(temp3 < 5)
		{
			flagok = 0;
		}
		temp1 = temp2;
			
	}

	*port->enccounts[0] = temp1*(*port->encscale);
*/
//	*port->enccounts0 = (__s32)readl((port->mem_base)+(reg_enc0*4))*(*port->encscale);

// read encoder 0
		flagok =1;
		temp1=(__s32)readl((port->mem_base)+(reg_enc0*4));

		while (flagok)
		{
			temp2=(__s32)readl((port->mem_base)+(reg_enc0*4));
			flagok = ABS_SUB(temp1,temp2);
			temp1 = temp2;			
		}
		*port->enccounts[0] = temp1;

// read encoder 1
		flagok =1;
		temp1=(__s32)readl((port->mem_base)+(reg_enc1*4));

		while (flagok)
		{
			temp2=(__s32)readl((port->mem_base)+(reg_enc1*4));
			flagok = ABS_SUB(temp1,temp2);
			temp1 = temp2;			
		}
		*port->enccounts[1] = temp1;

// read encoder 2
		flagok =1;
		temp1=(__s32)readl((port->mem_base)+(reg_enc2*4));

		while (flagok)
		{
			temp2=(__s32)readl((port->mem_base)+(reg_enc2*4));
			flagok = ABS_SUB(temp1,temp2);
			temp1 = temp2;			
		}
		*port->enccounts[2] = temp1;

// read encoder 3
		flagok =1;
		temp1=(__s32)readl((port->mem_base)+(reg_enc3*4));

		while (flagok)
		{
			temp2=(__s32)readl((port->mem_base)+(reg_enc3*4));
			flagok = ABS_SUB(temp1,temp2);
			temp1 = temp2;			
		}
		*port->enccounts[3] = temp1;

// read encoder 4
		flagok =1;
		temp1=(__s32)readl((port->mem_base)+(reg_enc4*4));

		while (flagok)
		{
			temp2=(__s32)readl((port->mem_base)+(reg_enc4*4));
			flagok = ABS_SUB(temp1,temp2);
			temp1 = temp2;			
		}
		*port->enccounts[4] = temp1;
	

// write index_enable outputs 
     	tmp = 0x0;
     	mask = 0x01;
     	for (pin=0; pin < 6; pin++) {
        	if (*port->index_en[pin]) {
        		tmp |= mask;
        	}
		mask <<= 1;
     	}

	tmp |= tmask;	
     	writel(tmp,(port->mem_base)+(reg_index*4)); 



// write digital outputs 
     	tmp = 0x0;
     	mask = 0x01;
     	for (pin=0; pin < 16; pin++) {
        	if (*port->digital_out[pin]) {
        		tmp |= mask;
        	}
		mask <<= 1;
     	}
	tmp |= tmask;
     	writel(tmp,(port->mem_base)+(reg_out*4)); 

// write control reg  control_reg
	tmp = 0x0;
	mask = 0x01;
	for (pin=0; pin < 32; pin++) {
        	if (*port->enable_dr && pin==4) {
       		tmp |= mask;
        	}
		mask <<= 1;
     	}

	tmp |= tmask;
     	writel(tmp,(port->mem_base)+(control_reg*4)); 
  

// out to drive

	ikor = (*port->outscale)*((*(port-> dcontrol))/10)*0xffff;
	
	ikor |= tmask;
	ikor &= tanmask;
	*port->indikator = ikor;
	writel( ikor,(port->mem_base)+(reg_dcont*4));
	writel( ikor,(port->mem_base)+(reg_dcont1*4));
	writel( ikor,(port->mem_base)+(reg_dcont2*4));
	writel( ikor,(port->mem_base)+(reg_dcont3*4));
	writel( ikor,(port->mem_base)+(reg_dcont4*4));
}