/* 
    V_4  Version (13.06.19)
*/
// ADD:
// v2 - invert input
// v3 - encoder velocity
// v4 - reset encoders v3.42a! 6pwm 6enc initscale
//		001- (486) FFFFFFDF

/* 	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

*/


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



/* module information */
//MODULE_AUTHOR("");
//MODULE_DESCRIPTION("");
//MODULE_LICENSE("GPL");

/***********************************************************************
*              				 MACROS               				        *
************************************************************************/

#define reg_enc0 0
#define reg_enc1 1
#define reg_enc2 2
#define reg_enc3 3
#define reg_enc4 4
#define reg_enc5 5

#define reg_zero 6

#define reg_in 7
#define reg_out 14

#define reg_dcont0 8
#define reg_dcont1 9
#define reg_dcont2 10
#define reg_dcont3 11
#define reg_dcont4 12
#define reg_dcont5 13

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

#define wd_lock_ad 29

#define bit_en 4
#define Nenc 6
#define Nout 6


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

#define driver_NAME "to_pci"

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

struct pci_dev *to_io;

/* 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_in_n[32];
  	hal_bit_t *digital_out[32];    /* ptrs for digital output pins 0 - 31 */
	hal_bit_t *enable_dr; 
	hal_bit_t *wd_lock;
	hal_bit_t *index_en[Nenc];
	hal_float_t *enccounts[Nenc];
	hal_float_t *encscale[Nenc];
	hal_float_t *encvel[Nenc];

	hal_float_t *outscale[Nout];
	hal_float_t *dcontrol[Nout];

	__s32 enc_lv[Nenc];

} to_pci_t;


/* pointer to array of 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;

__u8 rdctr[] = {reg_dcont0,reg_dcont1,reg_dcont2,reg_dcont3,reg_dcont4,reg_dcont5};
__u8 renci[] = {reg_enc0,reg_enc1,reg_enc2,reg_enc3,reg_enc4,reg_enc5};

/***********************************************************************
*                  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                             *
************************************************************************/

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 = -2;
		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 -3;
    }
    
    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);
	rtapi_print_msg(RTAPI_MSG_INFO,"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 = -4;
                goto fail0;
	}

    //writel(0x6000000A,(device_data->mem_base)+(reg_WD*4));
	writel(0x600000A0,(device_data->mem_base)+(control_reg*4));
	writel(0x60000000,(device_data->mem_base)+(control_reg*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;
		}
      
      	retval = hal_pin_bit_newf(HAL_OUT, &(device_data->digital_in_n[i]),comp_id, "to_pci.%d.pins.pin-%02d-in-n", 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 */

	for ( i=0; i<Nenc;i++) {
		/* encoder_count */
		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 encoder=%i\n", retval);
			r = -1;
			goto fail1;
		}
				/* encoder_scale */
		retval = hal_pin_float_newf(HAL_IN, &(device_data-> encscale[i]), comp_id, "to_pci.%d.feedback.enc_scale%d", 1, i);
		if (retval < 0) {
			rtapi_print_msg(RTAPI_MSG_ERR,"to_pci: ERROR:  err enc_scale=%i\n", retval);
			r = -1;
			goto fail1;
		}
		*device_data-> encscale[i] = 1;

				/* index */
		retval = hal_pin_bit_newf(HAL_IO, &(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; 
		}  
			/* encoder_velocity */
		retval = hal_pin_float_newf(HAL_IN, &(device_data-> encvel[i]), comp_id, "to_pci.%d.feedback.enc_vel%d", 1, i);
		if (retval < 0) {
			rtapi_print_msg(RTAPI_MSG_ERR,"to_pci: ERROR:  err enc_velocity=%i\n", retval);
			r = -1;
			goto fail1;
		}   
	}
	

//////////////////////////////
/////////////////////////////
/////////////////////////////
	/* export control drive PWM*/


	for ( i=0; i<Nout;i++) {
		retval = hal_pin_float_newf(HAL_IN, &(device_data-> dcontrol[i]), comp_id, "to_pci.%d.PWM.dcontrol%d", 1, i);
		if (retval < 0) {
			rtapi_print_msg(RTAPI_MSG_ERR,"to_pci: ERROR:  err dcontrol=%i\n", retval);
			r = -1;
			goto fail1;
		}
	

		retval = hal_pin_float_newf(HAL_IN, &(device_data-> outscale[i]), comp_id, "to_pci.%d.PWM.out_scale%d", 1, i);
		if (retval < 0) {
			rtapi_print_msg(RTAPI_MSG_ERR,"to_pci: ERROR:  err out_scale=%i\n", retval);
			r = -1;
			goto fail1;
		}
		*device_data-> outscale[i] = 1;
	}

//////////////////////////////
/////////////////////////////
/////////////////////////////
	retval = hal_pin_bit_newf(HAL_IO, &(device_data->wd_lock),comp_id, "to_pci.%d.wd_lock", 1);
		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; 
		}  


/////////////////////////
////UPDATE
////////////////////////
    /* 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");
	rtapi_print_msg(RTAPI_MSG_INFO,"to_pci: exit\n"); 
}
//###########################################################################################################
//
//			FUNCTIONS
//
//###########################################################################################################
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,flagok,k;

	__s32 temp1,temp2;

	port = arg;

	tmask = 0x60000000;
	tanmask = 0xEFFFFFFF;

	if(!WD_start){
		writel(0x60000080,(device_data->mem_base)+(control_reg*4));
		writel(0x60000000,(device_data->mem_base)+(control_reg*4));
		WD_start = 1;
	}


// read digital inputs 	    
	tmp = readl((port->mem_base)+(reg_in*4)); 
      	mask = 0x00000001;
	for (pin=0 ; pin < 32 ; pin++) {
		*(port->digital_in[pin]) = (tmp & mask) ? 1:0 ;
		*(port->digital_in_n[pin]) = (tmp & mask) ? 0:1 ;
		mask <<= 1;
	}

// Index....
	tmp = readl((port->mem_base)+(reg_zero*4));
	mask = 0x01;
	for (pin=0; pin < 5; pin++) {
		if(*port->index_en[pin] && (tmp & mask)){
			*port->index_en[pin] = 0;
		}
		mask <<= 1;
     	}
///////////// read encoders /////////////////////////////////////////////

	for ( k=0; k<Nenc;k++) {
		flagok =1;
		temp1=(__s32)readl((port->mem_base)+(renci[k]*4));

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

		*port->enccounts[k] = temp1/(*port->encscale[k]);

		temp2 = temp1 - (port->enc_lv[k]);
		port->enc_lv[k] = temp1;
		temp2 = temp2*1000;

		*port->encvel[k] = temp2/(*port->encscale[k]);
	}

///////////// wd_lock /////////////////////////////////////////////

	if(readl((port->mem_base)+(wd_lock_ad*4)) & 0x00000001)
		*port->wd_lock = 1;
	else
		*port->wd_lock = 0;



////////////////////ALL OUTPUTS///////////////////////////////////////////////

// write index_enable outputs 
     	tmp = 0x0;
     	mask = 0x01;
     	for (pin=0; pin < 5; 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 = 0;
	if (*port->enable_dr) {
		tmp |= 0x00000010;//(1 << bit_en)
        }

	tmp |= tmask;
	tmp &= 0xFFFFFFDF;  //encoders not reset
     	writel(tmp,(port->mem_base)+(control_reg*4)); 
  
// out to drive

	for ( k=0; k<Nout;k++) {
		ikor = (*port->outscale[k])*((*(port-> dcontrol[k]))/10)*0xffff;	
		ikor |= tmask;
		ikor &= tanmask;
		writel( ikor,(port->mem_base)+(rdctr[k]*4));
	} 


// WD 
	writel(0x6000000A,(device_data->mem_base)+(reg_WD*4));

}