From 127301cb5dc094ea449173a5f24c8326b2369ead Mon Sep 17 00:00:00 2001 From: H Hartley Sweeten Date: Fri, 7 Mar 2014 13:41:01 -0700 Subject: [PATCH] staging: comedi: jr3_pci: tidy up jr3_download_firmware() This callback function for comedi_load_firmware() first validates that the firmware data is the correct format then it writes the data to each subdevice. Split the two operations out as separate functions to clarify the code. Tidy up the new functions. Signed-off-by: H Hartley Sweeten Cc: Ian Abbott Signed-off-by: Greg Kroah-Hartman --- drivers/staging/comedi/drivers/jr3_pci.c | 157 ++++++++++++----------- drivers/staging/comedi/drivers/jr3_pci.h | 6 +- 2 files changed, 85 insertions(+), 78 deletions(-) diff --git a/drivers/staging/comedi/drivers/jr3_pci.c b/drivers/staging/comedi/drivers/jr3_pci.c index af0633920bce..ceaf417d3e23 100644 --- a/drivers/staging/comedi/drivers/jr3_pci.c +++ b/drivers/staging/comedi/drivers/jr3_pci.c @@ -326,105 +326,112 @@ static int read_idm_word(const u8 *data, size_t size, int *pos, return result; } -static int jr3_download_firmware(struct comedi_device *dev, - const u8 *data, size_t size, - unsigned long context) +static int jr3_check_firmware(struct comedi_device *dev, + const u8 *data, size_t size) { + int more = 1; + int pos = 0; + /* * IDM file format is: * { count, address, data } * * ffff */ - int result, more, pos, OK; - - result = 0; - more = 1; - pos = 0; - OK = 0; while (more) { - unsigned int count, addr; + unsigned int count = 0; + unsigned int addr = 0; more = more && read_idm_word(data, size, &pos, &count); - if (more && count == 0xffff) { - OK = 1; - break; - } + if (more && count == 0xffff) + return 0; + more = more && read_idm_word(data, size, &pos, &addr); while (more && count > 0) { - unsigned int dummy; + unsigned int dummy = 0; + more = more && read_idm_word(data, size, &pos, &dummy); count--; } } - if (!OK) { - result = -ENODATA; - } else { - int i; - struct jr3_pci_dev_private *p = dev->private; + return -ENODATA; +} + +static void jr3_write_firmware(struct comedi_device *dev, + int subdev, const u8 *data, size_t size) +{ + struct jr3_pci_dev_private *devpriv = dev->private; + struct jr3_t __iomem *iobase = devpriv->iobase; + u32 __iomem *lo; + u32 __iomem *hi; + int more = 1; + int pos = 0; - for (i = 0; i < p->n_channels; i++) { - struct jr3_pci_subdev_private *sp; + while (more) { + unsigned int count = 0; + unsigned int addr = 0; + + more = more && read_idm_word(data, size, &pos, &count); + if (more && count == 0xffff) + return; + + more = more && read_idm_word(data, size, &pos, &addr); + + dev_dbg(dev->class_dev, "Loading#%d %4.4x bytes at %4.4x\n", + subdev, count, addr); + + while (more && count > 0) { + if (addr & 0x4000) { + /* 16 bit data, never seen in real life!! */ + unsigned int data1 = 0; + + more = more && + read_idm_word(data, size, &pos, &data1); + count--; + /* jr3[addr + 0x20000 * pnum] = data1; */ + } else { + /* Download 24 bit program */ + unsigned int data1 = 0; + unsigned int data2 = 0; + + lo = &iobase->channel[subdev].program_lo[addr]; + hi = &iobase->channel[subdev].program_hi[addr]; - sp = dev->subdevices[i].private; - more = 1; - pos = 0; - while (more) { - unsigned int count, addr; more = more && - read_idm_word(data, size, &pos, &count); - if (more && count == 0xffff) - break; + read_idm_word(data, size, &pos, &data1); more = more && - read_idm_word(data, size, &pos, &addr); - dev_dbg(dev->class_dev, - "Loading#%d %4.4x bytes at %4.4x\n", - i, count, addr); - while (more && count > 0) { - if (addr & 0x4000) { - /* 16 bit data, never seen - * in real life!! */ - unsigned int data1; - - more = more && - read_idm_word(data, - size, &pos, - &data1); - count--; - /* jr3[addr + 0x20000 * pnum] = - data1; */ - } else { - /* Download 24 bit program */ - unsigned int data1, data2; - - more = more && - read_idm_word(data, - size, &pos, - &data1); - more = more && - read_idm_word(data, size, - &pos, - &data2); - count -= 2; - if (more) { - set_u16(&p-> - iobase->channel - [i].program_low - [addr], data1); - udelay(1); - set_u16(&p-> - iobase->channel - [i].program_high - [addr], data2); - udelay(1); - } - } - addr++; + read_idm_word(data, size, &pos, &data2); + count -= 2; + if (more) { + set_u16(lo, data1); + udelay(1); + set_u16(hi, data2); + udelay(1); } } + addr++; } } - return result; +} + +static int jr3_download_firmware(struct comedi_device *dev, + const u8 *data, size_t size, + unsigned long context) +{ + struct jr3_pci_dev_private *devpriv = dev->private; + int subdev; + int ret; + + /* verify IDM file format */ + ret = jr3_check_firmware(dev, data, size); + if (ret) + return ret; + + /* write firmware to each subdevice */ + for (subdev = 0; subdev < devpriv->n_channels; subdev++) + jr3_write_firmware(dev, subdev, data, size); + + return 0; } static struct poll_delay_t jr3_pci_poll_subdevice(struct comedi_subdevice *s) diff --git a/drivers/staging/comedi/drivers/jr3_pci.h b/drivers/staging/comedi/drivers/jr3_pci.h index 3317f7a04c48..20478ae8fad6 100644 --- a/drivers/staging/comedi/drivers/jr3_pci.h +++ b/drivers/staging/comedi/drivers/jr3_pci.h @@ -671,11 +671,11 @@ struct jr3_channel { struct jr3_t { struct { - u32 program_low[0x4000]; /* 0x00000 - 0x10000 */ + u32 program_lo[0x4000]; /* 0x00000 - 0x10000 */ struct jr3_channel data; /* 0x10000 - 0x10c00 */ char pad2[0x30000 - 0x00c00]; /* 0x10c00 - 0x40000 */ - u32 program_high[0x8000]; /* 0x40000 - 0x60000 */ - u32 reset; /* 0x60000 - 0x60004 */ + u32 program_hi[0x8000]; /* 0x40000 - 0x60000 */ + u32 reset; /* 0x60000 - 0x60004 */ char pad3[0x20000 - 0x00004]; /* 0x60004 - 0x80000 */ } channel[4]; }; -- 2.30.2