From 623a73926c7012e3bb132e225621890207f5c611 Mon Sep 17 00:00:00 2001 From: H Hartley Sweeten Date: Fri, 5 Apr 2013 16:09:15 -0700 Subject: [PATCH] staging: comedi: serial2002: split up serial_2002_open() Split out the code that sets up the comedi subdevices that are attached to the serial port. There are actually two steps: 1) Read the configuration of the attached subdevices. 2) Use the configuration data to setup the comedi subdevices. Step 1 is split out as serial2002_setup_subdevs(). Step 2 is split out as serial2002_setup_subdevice(). Cleanup the split out code to remove all the extra '{ }' and indents. Signed-off-by: H Hartley Sweeten Cc: Ian Abbott Signed-off-by: Greg Kroah-Hartman --- drivers/staging/comedi/drivers/serial2002.c | 516 +++++++++----------- 1 file changed, 234 insertions(+), 282 deletions(-) diff --git a/drivers/staging/comedi/drivers/serial2002.c b/drivers/staging/comedi/drivers/serial2002.c index 74a0bc32b0c..b5c464e4321 100644 --- a/drivers/staging/comedi/drivers/serial2002.c +++ b/drivers/staging/comedi/drivers/serial2002.c @@ -328,311 +328,263 @@ static void serial_write(struct file *f, struct serial_data data) } } -static int serial_2002_open(struct comedi_device *dev) +struct config_t { + short int kind; + short int bits; + int min; + int max; +}; + +static int serial2002_setup_subdevice(struct comedi_subdevice *s, + struct config_t *cfg, + struct serial2002_range_table_t *range, + unsigned char *mapping, + int kind) +{ + const struct comedi_lrange **range_table_list = NULL; + unsigned int *maxdata_list; + int j, chan; + + for (chan = 0, j = 0; j < 32; j++) { + if (cfg[j].kind == kind) + chan++; + } + s->n_chan = chan; + s->maxdata = 0; + kfree(s->maxdata_list); + maxdata_list = kmalloc(sizeof(unsigned int) * s->n_chan, GFP_KERNEL); + if (!maxdata_list) + return -ENOMEM; + s->maxdata_list = maxdata_list; + kfree(s->range_table_list); + s->range_table = NULL; + s->range_table_list = NULL; + if (kind == 1 || kind == 2) { + s->range_table = &range_digital; + } else if (range) { + range_table_list = + kmalloc(sizeof(struct serial2002_range_table_t) * + s->n_chan, GFP_KERNEL); + if (!range_table_list) + return -ENOMEM; + s->range_table_list = range_table_list; + } + for (chan = 0, j = 0; j < 32; j++) { + if (cfg[j].kind == kind) { + if (mapping) + mapping[chan] = j; + if (range) { + range[j].length = 1; + range[j].range.min = cfg[j].min; + range[j].range.max = cfg[j].max; + range_table_list[chan] = + (const struct comedi_lrange *)&range[j]; + } + maxdata_list[chan] = ((long long)1 << cfg[j].bits) - 1; + chan++; + } + } + return 0; +} + +static int serial2002_setup_subdevs(struct comedi_device *dev) { struct serial2002_private *devpriv = dev->private; + struct config_t *di_cfg; + struct config_t *do_cfg; + struct config_t *ai_cfg; + struct config_t *ao_cfg; + struct config_t *cfg; + struct comedi_subdevice *s; int result; - char port[20]; + int i; - sprintf(port, "/dev/ttyS%d", devpriv->port); - devpriv->tty = filp_open(port, O_RDWR, 0); - if (IS_ERR(devpriv->tty)) { - result = (int)PTR_ERR(devpriv->tty); - dev_err(dev->class_dev, "file open error = %d\n", result); - } else { - struct config_t { - - short int kind; - short int bits; - int min; - int max; - }; - - struct config_t *dig_in_config; - struct config_t *dig_out_config; - struct config_t *chan_in_config; - struct config_t *chan_out_config; - int i; - - result = 0; - dig_in_config = kcalloc(32, sizeof(struct config_t), - GFP_KERNEL); - dig_out_config = kcalloc(32, sizeof(struct config_t), - GFP_KERNEL); - chan_in_config = kcalloc(32, sizeof(struct config_t), - GFP_KERNEL); - chan_out_config = kcalloc(32, sizeof(struct config_t), - GFP_KERNEL); - if (!dig_in_config || !dig_out_config - || !chan_in_config || !chan_out_config) { - result = -ENOMEM; - goto err_alloc_configs; - } + result = 0; + di_cfg = kcalloc(32, sizeof(*cfg), GFP_KERNEL); + do_cfg = kcalloc(32, sizeof(*cfg), GFP_KERNEL); + ai_cfg = kcalloc(32, sizeof(*cfg), GFP_KERNEL); + ao_cfg = kcalloc(32, sizeof(*cfg), GFP_KERNEL); + if (!di_cfg || !do_cfg || !ai_cfg || !ao_cfg) { + result = -ENOMEM; + goto err_alloc_configs; + } - tty_setspeed(devpriv->tty, devpriv->speed); - poll_channel(devpriv->tty, 31); /* Start reading configuration */ - while (1) { - struct serial_data data; + tty_setspeed(devpriv->tty, devpriv->speed); + poll_channel(devpriv->tty, 31); /* Start reading configuration */ + while (1) { + struct serial_data data; - data = serial_read(devpriv->tty, 1000); - if (data.kind != is_channel || data.index != 31 - || !(data.value & 0xe0)) { - break; - } else { - int command, channel, kind; - struct config_t *cur_config = NULL; - - channel = data.value & 0x1f; - kind = (data.value >> 5) & 0x7; - command = (data.value >> 8) & 0x3; - switch (kind) { - case 1:{ - cur_config = dig_in_config; - } - break; - case 2:{ - cur_config = dig_out_config; - } - break; - case 3:{ - cur_config = chan_in_config; - } - break; - case 4:{ - cur_config = chan_out_config; - } - break; - case 5:{ - cur_config = chan_in_config; - } - break; - } + data = serial_read(devpriv->tty, 1000); + if (data.kind != is_channel || data.index != 31 || + !(data.value & 0xe0)) { + break; + } else { + int channel = data.value & 0x1f; + int kind = (data.value >> 5) & 0x7; + int command = (data.value >> 8) & 0x3; - if (cur_config) { - cur_config[channel].kind = kind; - switch (command) { - case 0:{ - cur_config[channel].bits - = - (data.value >> 10) & - 0x3f; - } - break; - case 1:{ - int unit, sign, min; - unit = - (data.value >> 10) & - 0x7; - sign = - (data.value >> 13) & - 0x1; - min = - (data.value >> 14) & - 0xfffff; - - switch (unit) { - case 0:{ - min = - min - * - 1000000; - } - break; - case 1:{ - min = - min - * - 1000; - } - break; - case 2:{ - min = - min - * 1; - } - break; - } - if (sign) - min = -min; - cur_config[channel].min - = min; - } - break; - case 2:{ - int unit, sign, max; - unit = - (data.value >> 10) & - 0x7; - sign = - (data.value >> 13) & - 0x1; - max = - (data.value >> 14) & - 0xfffff; - - switch (unit) { - case 0:{ - max = - max - * - 1000000; - } - break; - case 1:{ - max = - max - * - 1000; - } - break; - case 2:{ - max = - max - * 1; - } - break; - } - if (sign) - max = -max; - cur_config[channel].max - = max; - } - break; - } - } - } - } - for (i = 0; i <= 4; i++) { - /* Fill in subdev data */ - struct config_t *c; - unsigned char *mapping = NULL; - struct serial2002_range_table_t *range = NULL; - int kind = 0; - - switch (i) { - case 0:{ - c = dig_in_config; - mapping = devpriv->digital_in_mapping; - kind = 1; - } + switch (kind) { + case 1: + cfg = di_cfg; break; - case 1:{ - c = dig_out_config; - mapping = devpriv->digital_out_mapping; - kind = 2; - } + case 2: + cfg = do_cfg; break; - case 2:{ - c = chan_in_config; - mapping = devpriv->analog_in_mapping; - range = devpriv->in_range; - kind = 3; - } + case 3: + cfg = ai_cfg; break; - case 3:{ - c = chan_out_config; - mapping = devpriv->analog_out_mapping; - range = devpriv->out_range; - kind = 4; - } + case 4: + cfg = ao_cfg; break; - case 4:{ - c = chan_in_config; - mapping = devpriv->encoder_in_mapping; - range = devpriv->in_range; - kind = 5; - } + case 5: + cfg = ai_cfg; break; - default:{ - c = NULL; - } + default: + cfg = NULL; break; } - if (c) { - struct comedi_subdevice *s; - const struct comedi_lrange **range_table_list = - NULL; - unsigned int *maxdata_list; - int j, chan; - - for (chan = 0, j = 0; j < 32; j++) { - if (c[j].kind == kind) - chan++; - } - s = &dev->subdevices[i]; - s->n_chan = chan; - s->maxdata = 0; - kfree(s->maxdata_list); - s->maxdata_list = maxdata_list = - kmalloc(sizeof(unsigned int) * s->n_chan, - GFP_KERNEL); - if (!s->maxdata_list) - break; /* error handled below */ - kfree(s->range_table_list); - s->range_table = NULL; - s->range_table_list = NULL; - if (kind == 1 || kind == 2) { - s->range_table = &range_digital; - } else if (range) { - s->range_table_list = range_table_list = - kmalloc(sizeof - (struct - serial2002_range_table_t) * - s->n_chan, GFP_KERNEL); - if (!s->range_table_list) - break; /* err handled below */ - } - for (chan = 0, j = 0; j < 32; j++) { - if (c[j].kind == kind) { - if (mapping) - mapping[chan] = j; - if (range) { - range[j].length = 1; - range[j].range.min = - c[j].min; - range[j].range.max = - c[j].max; - range_table_list[chan] = - (const struct - comedi_lrange *) - &range[j]; - } - maxdata_list[chan] = - ((long long)1 << c[j].bits) - - 1; - chan++; + + if (cfg) { + short int bits = (data.value >> 10) & 0x3f; + int unit = (data.value >> 10) & 0x7; + int sign = (data.value >> 13) & 0x1; + int min = (data.value >> 14) & 0xfffff; + int max = min; + + cfg[channel].kind = kind; + switch (command) { + case 0: + cfg[channel].bits = bits; + break; + case 1: + switch (unit) { + case 0: + min *= 1000000; + break; + case 1: + min *= 1000; + break; + case 2: + min *= 1; + break; } + if (sign) + min = -min; + cfg[channel].min = min; + break; + case 2: + switch (unit) { + case 0: + max *= 1000000; + break; + case 1: + max *= 1000; + break; + case 2: + max *= 1; + break; + } + if (sign) + max = -max; + cfg[channel].max = max; + break; } } } - if (i <= 4) { - /* Failed to allocate maxdata_list or range_table_list - * for a subdevice that needed it. */ - result = -ENOMEM; - for (i = 0; i <= 4; i++) { - struct comedi_subdevice *s; - - s = &dev->subdevices[i]; - kfree(s->maxdata_list); - s->maxdata_list = NULL; - kfree(s->range_table_list); - s->range_table_list = NULL; - } + } + + /* Fill in subdevice data */ + for (i = 0; i <= 4; i++) { + unsigned char *mapping = NULL; + struct serial2002_range_table_t *range = NULL; + int kind = 0; + + switch (i) { + case 0: + cfg = di_cfg; + mapping = devpriv->digital_in_mapping; + kind = 1; + break; + case 1: + cfg = do_cfg; + mapping = devpriv->digital_out_mapping; + kind = 2; + break; + case 2: + cfg = ai_cfg; + mapping = devpriv->analog_in_mapping; + range = devpriv->in_range; + kind = 3; + break; + case 3: + cfg = ao_cfg; + mapping = devpriv->analog_out_mapping; + range = devpriv->out_range; + kind = 4; + break; + case 4: + cfg = ai_cfg; + mapping = devpriv->encoder_in_mapping; + range = devpriv->in_range; + kind = 5; + break; + default: + cfg = NULL; + break; } + if (cfg) { + s = &dev->subdevices[i]; + if (serial2002_setup_subdevice(s, cfg, range, mapping, + kind)) + break; /* err handled below */ + } + } + if (i <= 4) { + /* + * Failed to allocate maxdata_list or range_table_list + * for a subdevice that needed it. + */ + result = -ENOMEM; + for (i = 0; i <= 4; i++) { + s = &dev->subdevices[i]; + kfree(s->maxdata_list); + s->maxdata_list = NULL; + kfree(s->range_table_list); + s->range_table_list = NULL; + } + } + err_alloc_configs: - kfree(dig_in_config); - kfree(dig_out_config); - kfree(chan_in_config); - kfree(chan_out_config); - - if (result) { - if (devpriv->tty) { - filp_close(devpriv->tty, NULL); - devpriv->tty = NULL; - } + kfree(di_cfg); + kfree(do_cfg); + kfree(ai_cfg); + kfree(ao_cfg); + + if (result) { + if (devpriv->tty) { + filp_close(devpriv->tty, NULL); + devpriv->tty = NULL; } } + + return result; +} + +static int serial_2002_open(struct comedi_device *dev) +{ + struct serial2002_private *devpriv = dev->private; + int result; + char port[20]; + + sprintf(port, "/dev/ttyS%d", devpriv->port); + devpriv->tty = filp_open(port, O_RDWR, 0); + if (IS_ERR(devpriv->tty)) { + result = (int)PTR_ERR(devpriv->tty); + dev_err(dev->class_dev, "file open error = %d\n", result); + } else { + result = serial2002_setup_subdevs(dev); + } return result; } -- 2.46.0