hal_parport.c

CNC 的开放码,EMC2 V2.2.8版

	/* export write function */
	retval = hal_export_funct(name, reset_port, &(port_data_array[n]),
	    0, 0, comp_id);
	if (retval != 0) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
		"PARPORT: ERROR: port %d reset funct export failed\n", n);
	    hal_exit(comp_id);
	    return -1;
	}
    }
    /* export functions that read and write all ports */
    retval = hal_export_funct("parport.read-all", read_all,
	port_data_array, 0, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "PARPORT: ERROR: read all funct export failed\n");
	hal_exit(comp_id);
	return -1;
    }
    retval = hal_export_funct("parport.write-all", write_all,
	port_data_array, 0, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "PARPORT: ERROR: write all funct export failed\n");
	hal_exit(comp_id);
	return -1;
    }
    for (n = 0; n < num_ports; n++) {
        void *region = rtapi_request_region(port_data_array[n].base_addr, 4, "hal_parport");
        if(!region) {
            int m;
            for(m = 0; m < n; m++) {
                rtapi_release_region(port_data_array[m].base_addr, 4);
            }
            rtapi_print_msg(RTAPI_MSG_ERR,
                 "PARPORT: ERROR: request_region(%x) failed\n"
                 , port_data_array[n].base_addr);
            rtapi_print_msg(RTAPI_MSG_ERR,
                 "(make sure the kernel module 'parport' is unloaded)\n");
            hal_exit(comp_id);
	    return -EBUSY;
        }
    }
    rtapi_print_msg(RTAPI_MSG_INFO,
	"PARPORT: installed driver for %d ports\n", num_ports);
    hal_ready(comp_id);
    return 0;
}

void rtapi_app_exit(void)
{
    int n;
    for (n = 0; n < num_ports; n++) {
        rtapi_release_region(port_data_array[n].base_addr, 4);
    }
    hal_exit(comp_id);
}

#else /* user space */

static int done = 0;
static void quit(int sig)
{
    done = 1;
}

int main(int argc, char *argv[])
{
    char name[HAL_NAME_LEN + 2];
    int n, retval;
    hal_s32_t *read_funct_flags;
    hal_s32_t *write_funct_flags;
    struct timeval tv;

    /* ask linux for permission to use the I/O ports */
    retval = iopl(3);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "PARPORT: ERROR: could not get I/O permission\n");
	return -1;
    }
    /* parse command line, set up pins and parameters */
    retval = pins_and_params(&(argv[1]));
    if (retval != 0) {
	return retval;
    }
    /* allocate space for function run/stop parameters */
    read_funct_flags = hal_malloc((num_ports + 1) * sizeof(hal_s32_t) * 2);
    if (read_funct_flags == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "PARPORT: ERROR: hal_malloc() failed\n");
	hal_exit(comp_id);
	return -1;
    }
    write_funct_flags = read_funct_flags + (num_ports + 1);
    /* export function run/stop parameters for each port */
    for (n = 0; n < num_ports; n++) {
	/* export read function parameter */
	retval =
	    hal_param_s32_newf(HAL_RW, &read_funct_flags[n + 1],
                    comp_id, "parport.%d.read", n);
	if (retval != 0) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
		"PARPORT: ERROR: port %d read funct param failed\n", n);
	    hal_exit(comp_id);
	    return -1;
	}
	/* make write function name */
	/* export read function parameter */
	retval =
	    hal_param_s32_newf(name, HAL_RW, &write_funct_flags[n + 1],
                    comp_id, "parport.%d.write", n);
	if (retval != 0) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
		"PARPORT: ERROR: port %d write funct param failed\n", n);
	    hal_exit(comp_id);
	    return -1;
	}
    }
    /* export parameters for read/write all port functuons */
    retval =
	hal_param_s32_new("parport.read_all", HAL_RW, &read_funct_flags[0],
	comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "PARPORT: ERROR: read all funct param failed\n");
	hal_exit(comp_id);
	return -1;
    }
    retval =
	hal_param_s32_new("parport.write_all", HAL_RW,
	&write_funct_flags[0], comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "PARPORT: ERROR: write all funct param failed\n");
	hal_exit(comp_id);
	return -1;
    }
    rtapi_print_msg(RTAPI_MSG_INFO,
	"PARPORT: installed driver for %d ports\n", num_ports);
    /* capture INT (ctrl-C) and TERM signals */
    signal(SIGINT, quit);
    signal(SIGTERM, quit);

    /*************************************/
    /* main loop - loops forever until */
    /* SIGINT (ctrl-C) or SIGTERM (kill) */
    /*************************************/
    while (!done) {
	if (read_funct_flags[0]) {
	    /* run the function */
	    read_all(port_data_array, 0);
	    /* if flag is positive, reset it */
	    if (read_funct_flags[0] > 0) {
		read_funct_flags[0] = 0;
	    }
	}
	for (n = 0; n < num_ports; n++) {
	    if (read_funct_flags[n + 1]) {
		/* run the function */
		read_port(&(port_data_array[n]), 0);
		/* if flag is positive, reset it */
		if (read_funct_flags[n + 1] > 0) {
		    read_funct_flags[n + 1] = 0;
		}
	    }
	}
	if (write_funct_flags[0]) {
	    /* run the function */
	    write_all(port_data_array, 0);
	    /* if flag is positive, reset it */
	    if (write_funct_flags[0] > 0) {
		write_funct_flags[0] = 0;
	    }
	}
	for (n = 0; n < num_ports; n++) {
	    if (write_funct_flags[n + 1]) {
		/* run the function */
		write_port(&(port_data_array[n]), 0);
		/* if flag is positive, reset it */
		if (write_funct_flags[n + 1] > 0) {
		    write_funct_flags[n + 1] = 0;
		}
	    }
	}
	/* set timeout to 0.05 seconds (20 Hz update rate if nothing else is
	   running) */
	tv.tv_sec = 0;
	tv.tv_usec = 50000;
	/* call select() with no file descriptors and a timeout to yield the
	   CPU for (at least) 0,05 seconds - see NOTES section of man 2
	   select for details */
	select(0, 0, 0, 0, &tv);
    }
    hal_exit(comp_id);
    return 0;
}

#endif

/***********************************************************************
*                  REALTIME PORT READ AND WRITE FUNCTIONS              *
************************************************************************/

static void read_port(void *arg, long period)
{
    parport_t *port;
    int b;
    unsigned char indata, mask;

    port = arg;
    /* read the status port */
    indata = rtapi_inb(port->base_addr + 1);
    /* invert bit 7 (pin 11) to compensate for hardware inverter */
    indata ^= 0x80;
    /* split the bits into 10 variables (5 regular, 5 inverted) */
    mask = 0x08;
    for (b = 0; b < 10; b += 2) {
	*(port->status_in[b]) = indata & mask;
	*(port->status_in[b + 1]) = !(indata & mask);
	mask <<= 1;
    }
    /* are we using the data port for input? */
    if (port->data_dir != 0) {
	/* yes, read the data port */
	indata = rtapi_inb(port->base_addr);
	/* split the bits into 16 variables (8 regular, 8 inverted) */
	mask = 0x01;
	for (b = 0; b < 16; b += 2) {
	    *(port->data_in[b]) = indata & mask;
	    *(port->data_in[b + 1]) = !(indata & mask);
	    mask <<= 1;
	}
    }
    /* are we using the control port for input? */
    if(port->use_control_in) {
        mask = 0x01;
        /* correct for hardware inverters on pins 1, 14, & 17 */
        indata = rtapi_inb(port->base_addr + 2) ^ 0x0B;
        for (b = 0; b < 8; b += 2) {
            *(port->control_in[b]) = indata & mask;
            *(port->control_in[b + 1]) = !(indata & mask);
	    mask <<= 1;
        }
    }
}

static void reset_port(void *arg, long period) {
    parport_t *port = arg;
    long long deadline, reset_time_tsc;
    unsigned char outdata = (port->outdata&~port->reset_mask) ^ port->reset_val;
   
    if(port->reset_time > period/4) port->reset_time = period/4;
    reset_time_tsc = ns2tsc(port->reset_time);

    if(outdata != port->outdata) {
        deadline = port->write_time + reset_time_tsc;
        while(rtapi_get_clocks() < deadline) {}
        rtapi_outb(outdata, port->base_addr);
    }

    outdata = (port->outdata_ctrl&~port->reset_mask_ctrl)^port->reset_val_ctrl;

    if(outdata != port->outdata_ctrl) {
	/* correct for hardware inverters on pins 1, 14, & 17 */
	outdata ^= 0x0B;
        deadline = port->write_time_ctrl + reset_time_tsc;
        while(rtapi_get_clocks() < deadline) {}
        rtapi_outb(outdata, port->base_addr + 2);
    }
}

static void write_port(void *arg, long period)
{
    parport_t *port;
    int b;
    unsigned char outdata, mask;

    port = arg;
    /* are we using the data port for output? */
    if (port->data_dir == 0) {
	int reset_mask=0, reset_val=0;
	/* yes */
	outdata = 0x00;
	mask = 0x01;
	/* assemble output byte for data port from 8 source variables */
	for (b = 0; b < 8; b++) {
	    /* get the data, add to output byte */
	    if ((*(port->data_out[b])) && (!port->data_inv[b])) {
		outdata |= mask;
	    }
	    if ((!*(port->data_out[b])) && (port->data_inv[b])) {
		outdata |= mask;
	    }
	    if (port->data_reset[b]) {
		reset_mask |= mask;
		if(port->data_inv[b]) reset_val |= mask;
	    }
	    mask <<= 1;
	}
	/* write it to the hardware */
	rtapi_outb(outdata, port->base_addr);
	port->write_time = rtapi_get_clocks();
	port->reset_val = reset_val;
	port->reset_mask = reset_mask;
	port->outdata = outdata;