diff options
Diffstat (limited to 'drivers/staging/comedi/drivers/ssv_dnp.c')
| -rw-r--r-- | drivers/staging/comedi/drivers/ssv_dnp.c | 79 |
1 files changed, 30 insertions, 49 deletions
diff --git a/drivers/staging/comedi/drivers/ssv_dnp.c b/drivers/staging/comedi/drivers/ssv_dnp.c index 45c661cbdbb..11758a515c1 100644 --- a/drivers/staging/comedi/drivers/ssv_dnp.c +++ b/drivers/staging/comedi/drivers/ssv_dnp.c @@ -26,6 +26,7 @@ Status: unknown /* include files ----------------------------------------------------------- */ +#include <linux/module.h> #include "../comedidev.h" /* Some global definitions: the registers of the DNP ----------------------- */ @@ -92,68 +93,48 @@ static int dnp_dio_insn_bits(struct comedi_device *dev, } -/* ------------------------------------------------------------------------- */ -/* Configure the direction of the bidirectional digital i/o pins. chanspec */ -/* contains the channel to be changed and data[0] contains either */ -/* COMEDI_INPUT or COMEDI_OUTPUT. */ -/* ------------------------------------------------------------------------- */ - static int dnp_dio_insn_config(struct comedi_device *dev, struct comedi_subdevice *s, - struct comedi_insn *insn, unsigned int *data) + struct comedi_insn *insn, + unsigned int *data) { + unsigned int chan = CR_CHAN(insn->chanspec); + unsigned int mask; + unsigned int val; + int ret; - u8 register_buffer; - - /* reduces chanspec to lower 16 bits */ - int chan = CR_CHAN(insn->chanspec); - - switch (data[0]) { - case INSN_CONFIG_DIO_OUTPUT: - case INSN_CONFIG_DIO_INPUT: - break; - case INSN_CONFIG_DIO_QUERY: - data[1] = - (inb(CSCDR) & (1 << chan)) ? COMEDI_OUTPUT : COMEDI_INPUT; - return insn->n; - break; - default: - return -EINVAL; - break; - } - /* Test: which port does the channel belong to? */ - - /* We have to pay attention with port C: this is the meaning of PCMR: */ - /* Bit in PCMR: 7 6 5 4 3 2 1 0 */ - /* Corresponding port C pin: d 3 d 2 d 1 d 0 d= don't touch */ + ret = comedi_dio_insn_config(dev, s, insn, data, 0); + if (ret) + return ret; - if ((chan >= 0) && (chan <= 7)) { - /* this is port A */ + if (chan < 8) { /* Port A */ + mask = 1 << chan; outb(PAMR, CSCIR); - } else if ((chan >= 8) && (chan <= 15)) { - /* this is port B */ - chan -= 8; + } else if (chan < 16) { /* Port B */ + mask = 1 << (chan - 8); outb(PBMR, CSCIR); - } else if ((chan >= 16) && (chan <= 19)) { - /* this is port C; multiplication with 2 brings bits into */ - /* correct position for PCMR! */ - chan -= 16; - chan *= 2; + } else { /* Port C */ + /* + * We have to pay attention with port C. + * This is the meaning of PCMR: + * Bit in PCMR: 7 6 5 4 3 2 1 0 + * Corresponding port C pin: d 3 d 2 d 1 d 0 d= don't touch + * + * Multiplication by 2 brings bits into correct position + * for PCMR! + */ + mask = 1 << ((chan - 16) * 2); outb(PCMR, CSCIR); - } else { - return -EINVAL; } - /* read 'old' direction of the port and set bits (out=1, in=0) */ - register_buffer = inb(CSCDR); + val = inb(CSCDR); if (data[0] == COMEDI_OUTPUT) - register_buffer |= (1 << chan); + val |= mask; else - register_buffer &= ~(1 << chan); - - outb(register_buffer, CSCDR); + val &= ~mask; + outb(val, CSCDR); - return 1; + return insn->n; } |