/* 	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");
/* static char *cfg = 0; */
/* config string
RTAPI_MP_STRING(cfg, "config string"); */


/***********************************************************************
*                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_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[5];
	hal_float_t *outscale[5];
	hal_float_t *dcontrol[5];
	hal_float_t *digital_out_step;

} to_pci_t;

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


typedef struct {
	__u8 *CH_num;
	float *vesimp;
	float *cord_tek;
	float *l_move;
	__u8 *timp;
	__u8 *tz;

} str_step_t;

str_step_t *sd_data1;

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

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

#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_index 15
#define control_reg 30
#define reg_WD 31

#define reg_stepdir0 16

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

#define driver_NAME "to_pci"

#define SERV 1000000

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

	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<=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 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;
		}
				/* 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; 
		}     

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


	for ( i=0; i<=4;i++) {
		retval = hal_pin_float_newf(HAL_IN, &(device_data-> dcontrol[i]), comp_id, "to_pci.%d.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.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;
		}
	}

//////////////////////////////
/////////////////////////////
/////////////////////////////
	/* export control drive STEP_DIR*/

	retval = hal_pin_float_newf(HAL_IN, &(device_data->digital_out_step),comp_id, "to_pci.%d.outstep", 1);
	if (retval < 0) {
		rtapi_print_msg(RTAPI_MSG_ERR,"to_pci: ERROR:  err out_step=%i\n", 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");
}


//###########################################################################################################
//
//			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;
}

///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
///    STEP DIR FUNCTIONS
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////

__s32 DDcontr(void *farg,float ccor)
{

	str_step_t *data_sd;
	data_sd = farg;

	float ftemp;
	int Nsch;
	__u8 codimp,ttimp;
	__s32 temp1,pkor;
		
//текущая дистанция для задания 	dist = dist + (actual_coordinat - last_coordinat)
	ftemp = *(data_sd->l_move) + ccor - *(data_sd->cord_tek);	

// Обновление actual_coordinate
	*(data_sd->cord_tek) = ccor;

	Nsch = (int) (ftemp/ (*(data_sd->vesimp))); //количество импульсов
	ftemp -= *(data_sd->vesimp)*Nsch;

// сохранение оставшейся дистанции
	*(data_sd->l_move) = ftemp;
	
	pkor = 0;		//установка dir
	if(Nsch<0) 
	{
    		pkor=1;	      //установка dir
    		Nsch = -Nsch; //модуль Nsch
  	}
	
	ttimp = *(data_sd->timp);
	
	if(ttimp == 8)
		codimp = 0x03;
	else
		if(ttimp == 4)
			codimp = 0x02;
		else
			if(ttimp == 2)
				codimp = 0x01;
			else
				codimp = 0x00;

	pkor <<= 2;
	pkor = pkor | codimp;

	temp1 = *(data_sd->tz) & 0x0f;
	pkor <<= 4;
	pkor = pkor | temp1;

	temp1 = Nsch & 0x1ff;
	pkor <<= 9;
	pkor = pkor | temp1;


	if(Nsch)
		temp1 = (int) (50*(SERV*0.001 - *(data_sd->tz) - 2*Nsch*ttimp)/Nsch);
	else
		temp1 = 50000;
                                     
	temp1 = temp1 & 0xffff;
	
	pkor <<= 16;
	pkor = pkor | temp1;

	return pkor;
}
/**************************************************************
* 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;

	tmask = 0x60000000;
	tanmask = 0xEFFFFFFF;


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


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


// 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/(*port->encscale[0]);

// 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/(*port->encscale[1]);

// 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/(*port->encscale[2]);

// 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/(*port->encscale[3]);

// 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/(*port->encscale[4]);
	
////////////////////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 = 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[0])*((*(port-> dcontrol[0]))/10)*0xffff;	
	ikor |= tmask;
	ikor &= tanmask;
	writel( ikor,(port->mem_base)+(reg_dcont0*4));


	ikor = (*port->outscale[1])*((*(port-> dcontrol[1]))/10)*0xffff;	
	ikor |= tmask;
	ikor &= tanmask;
	writel( ikor,(port->mem_base)+(reg_dcont1*4));


	ikor = (*port->outscale[2])*((*(port-> dcontrol[2]))/10)*0xffff;	
	ikor |= tmask;
	ikor &= tanmask;
	writel( ikor,(port->mem_base)+(reg_dcont2*4));


	ikor = (*port->outscale[3])*((*(port-> dcontrol[3]))/10)*0xffff;	
	ikor |= tmask;
	ikor &= tanmask;
	writel( ikor,(port->mem_base)+(reg_dcont3*4));


	ikor = (*port->outscale[4])*((*(port-> dcontrol[4]))/10)*0xffff;	
	ikor |= tmask;
	ikor &= tanmask;
	writel( ikor,(port->mem_base)+(reg_dcont4*4));

// WD 
// Step dir 

	ikor = DDcontr( sd_data1,*(port-> digital_out_step));
	writel( ikor,(port->mem_base)+(reg_stepdir0*4));

}