2009-11-25 10:48:29

by Amit Kucheria

[permalink] [raw]
Subject: [PATCH] mfd: twl4030: Driver for twl4030 madc module

From: Mikko Ylinen <[email protected]>

This ADC allows monitoring of analog signals such as battery levels,
temperatures, etc.

Several people have contributed to this driver on the linux-omap list.

Signed-off-by: Amit Kucheria <[email protected]>
---
drivers/mfd/Kconfig | 21 ++
drivers/mfd/Makefile | 3 +-
drivers/mfd/twl4030-madc.c | 548 ++++++++++++++++++++++++++++++++++++++
include/linux/i2c/twl4030-madc.h | 126 +++++++++
4 files changed, 697 insertions(+), 1 deletions(-)
create mode 100644 drivers/mfd/twl4030-madc.c
create mode 100644 include/linux/i2c/twl4030-madc.h

diff --git a/drivers/mfd/Kconfig b/drivers/mfd/Kconfig
index 08f2d07..fb1ba3a 100644
--- a/drivers/mfd/Kconfig
+++ b/drivers/mfd/Kconfig
@@ -127,6 +127,27 @@ config TWL4030_CODEC
select MFD_CORE
default n

+config TWL4030_MADC
+ tristate "TWL4030 MADC Driver"
+ depends on TWL4030_CORE
+ help
+ The TWL4030 Monitoring ADC driver enables the host
+ processor to monitor analog signals using analog-to-digital
+ conversions on the input source. TWL4030 MADC provides the
+ following features:
+ - Single 10-bit ADC with successive approximation register (SAR) conversion;
+ - Analog multiplexer for 16 inputs;
+ - Seven (of the 16) inputs are freely available;
+ - Battery voltage monitoring;
+ - Concurrent conversion request management;
+ - Interrupt signal to Primary Interrupt Handler;
+ - Averaging feature;
+ - Selective enable/disable of the averaging feature.
+
+ Say 'y' here to statically link this module into the kernel or 'm'
+ to build it as a dinamically loadable module. The module will be
+ called twl4030-madc.ko
+
config MFD_TMIO
bool
default n
diff --git a/drivers/mfd/Makefile b/drivers/mfd/Makefile
index af0fc90..df1897b 100644
--- a/drivers/mfd/Makefile
+++ b/drivers/mfd/Makefile
@@ -25,8 +25,9 @@ obj-$(CONFIG_TPS65010) += tps65010.o
obj-$(CONFIG_MENELAUS) += menelaus.o

obj-$(CONFIG_TWL4030_CORE) += twl4030-core.o twl4030-irq.o
-obj-$(CONFIG_TWL4030_POWER) += twl4030-power.o
+obj-$(CONFIG_TWL4030_POWER) += twl4030-power.o
obj-$(CONFIG_TWL4030_CODEC) += twl4030-codec.o
+obj-$(CONFIG_TWL4030_MADC) += twl4030-madc.o

obj-$(CONFIG_MFD_MC13783) += mc13783-core.o

diff --git a/drivers/mfd/twl4030-madc.c b/drivers/mfd/twl4030-madc.c
new file mode 100644
index 0000000..90ce99a
--- /dev/null
+++ b/drivers/mfd/twl4030-madc.c
@@ -0,0 +1,548 @@
+/*
+ * drivers/i2c/chips/twl4030-madc.c
+ *
+ * TWL4030 MADC module driver
+ *
+ * Copyright (C) 2008 Nokia Corporation
+ * Mikko Ylinen <[email protected]>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/platform_device.h>
+#include <linux/miscdevice.h>
+#include <linux/i2c/twl4030.h>
+#include <linux/i2c/twl4030-madc.h>
+
+#include <asm/uaccess.h>
+
+#define TWL4030_MADC_PFX "twl4030-madc: "
+
+struct twl4030_madc_data {
+ struct device *dev;
+ struct mutex lock;
+ struct work_struct ws;
+ struct twl4030_madc_request requests[TWL4030_MADC_NUM_METHODS];
+ int imr;
+ int isr;
+};
+
+static struct twl4030_madc_data *the_madc;
+static int twl4030_madc_set_current_generator(struct twl4030_madc_data *madc, int chan, int on);
+
+static
+const struct twl4030_madc_conversion_method twl4030_conversion_methods[] = {
+ [TWL4030_MADC_RT] = {
+ .sel = TWL4030_MADC_RTSELECT_LSB,
+ .avg = TWL4030_MADC_RTAVERAGE_LSB,
+ .rbase = TWL4030_MADC_RTCH0_LSB,
+ },
+ [TWL4030_MADC_SW1] = {
+ .sel = TWL4030_MADC_SW1SELECT_LSB,
+ .avg = TWL4030_MADC_SW1AVERAGE_LSB,
+ .rbase = TWL4030_MADC_GPCH0_LSB,
+ .ctrl = TWL4030_MADC_CTRL_SW1,
+ },
+ [TWL4030_MADC_SW2] = {
+ .sel = TWL4030_MADC_SW2SELECT_LSB,
+ .avg = TWL4030_MADC_SW2AVERAGE_LSB,
+ .rbase = TWL4030_MADC_GPCH0_LSB,
+ .ctrl = TWL4030_MADC_CTRL_SW2,
+ },
+};
+
+static int twl4030_madc_read(struct twl4030_madc_data *madc, u8 reg)
+{
+ int ret;
+ u8 val;
+
+ ret = twl4030_i2c_read_u8(TWL4030_MODULE_MADC, &val, reg);
+ if (ret) {
+ dev_dbg(madc->dev, "unable to read register 0x%X\n", reg);
+ return ret;
+ }
+
+ return val;
+}
+
+static void twl4030_madc_write(struct twl4030_madc_data *madc, u8 reg, u8 val)
+{
+ int ret;
+
+ ret = twl4030_i2c_write_u8(TWL4030_MODULE_MADC, val, reg);
+ if (ret)
+ dev_err(madc->dev, "unable to write register 0x%X\n", reg);
+}
+
+static int twl4030_madc_channel_raw_read(struct twl4030_madc_data *madc, u8 reg)
+{
+ u8 msb, lsb;
+
+ /* For each ADC channel, we have MSB and LSB register pair. MSB address
+ * is always LSB address+1. reg parameter is the addr of LSB register */
+ msb = twl4030_madc_read(madc, reg + 1);
+ lsb = twl4030_madc_read(madc, reg);
+
+ return (int)(((msb << 8) | lsb) >> 6);
+}
+
+static int twl4030_madc_read_channels(struct twl4030_madc_data *madc,
+ u8 reg_base, u16 channels, int *buf)
+{
+ int count = 0;
+ u8 reg, i;
+
+ if (unlikely(!buf))
+ return 0;
+
+ for (i = 0; i < TWL4030_MADC_MAX_CHANNELS; i++) {
+ if (channels & (1<<i)) {
+ reg = reg_base + 2*i;
+ buf[i] = twl4030_madc_channel_raw_read(madc, reg);
+ count++;
+ }
+ }
+ return count;
+}
+
+static void twl4030_madc_enable_irq(struct twl4030_madc_data *madc, int id)
+{
+ u8 val;
+
+ val = twl4030_madc_read(madc, madc->imr);
+ val &= ~(1 << id);
+ twl4030_madc_write(madc, madc->imr, val);
+}
+
+static void twl4030_madc_disable_irq(struct twl4030_madc_data *madc, int id)
+{
+ u8 val;
+
+ val = twl4030_madc_read(madc, madc->imr);
+ val |= (1 << id);
+ twl4030_madc_write(madc, madc->imr, val);
+}
+
+static irqreturn_t twl4030_madc_irq_handler(int irq, void *_madc)
+{
+ struct twl4030_madc_data *madc = _madc;
+ u8 isr_val, imr_val;
+ int i;
+
+#ifdef CONFIG_LOCKDEP
+ /* WORKAROUND for lockdep forcing IRQF_DISABLED on us, which
+ * we don't want and can't tolerate. Although it might be
+ * friendlier not to borrow this thread context...
+ */
+ local_irq_enable();
+#endif
+
+ /* Use COR to ack interrupts since we have no shared IRQs in ISRx */
+ isr_val = twl4030_madc_read(madc, madc->isr);
+ imr_val = twl4030_madc_read(madc, madc->imr);
+
+ isr_val &= ~imr_val;
+
+ for (i = 0; i < TWL4030_MADC_NUM_METHODS; i++) {
+
+ if (!(isr_val & (1<<i)))
+ continue;
+
+ twl4030_madc_disable_irq(madc, i);
+ madc->requests[i].result_pending = 1;
+ }
+
+ schedule_work(&madc->ws);
+
+ return IRQ_HANDLED;
+}
+
+static void twl4030_madc_work(struct work_struct *ws)
+{
+ const struct twl4030_madc_conversion_method *method;
+ struct twl4030_madc_data *madc;
+ struct twl4030_madc_request *r;
+ int len, i;
+
+ madc = container_of(ws, struct twl4030_madc_data, ws);
+ mutex_lock(&madc->lock);
+
+ for (i = 0; i < TWL4030_MADC_NUM_METHODS; i++) {
+
+ r = &madc->requests[i];
+
+ /* No pending results for this method, move to next one */
+ if (!r->result_pending)
+ continue;
+
+ method = &twl4030_conversion_methods[r->method];
+
+ /* Read results */
+ len = twl4030_madc_read_channels(madc, method->rbase,
+ r->channels, r->rbuf);
+
+ /* Return results to caller */
+ if (r->func_cb != NULL) {
+ r->func_cb(len, r->channels, r->rbuf);
+ r->func_cb = NULL;
+ }
+
+ /* Free request */
+ r->result_pending = 0;
+ r->active = 0;
+ }
+
+ mutex_unlock(&madc->lock);
+}
+
+static int twl4030_madc_set_irq(struct twl4030_madc_data *madc,
+ struct twl4030_madc_request *req)
+{
+ struct twl4030_madc_request *p;
+
+ p = &madc->requests[req->method];
+
+ memcpy(p, req, sizeof *req);
+
+ twl4030_madc_enable_irq(madc, req->method);
+
+ return 0;
+}
+
+static inline void twl4030_madc_start_conversion(struct twl4030_madc_data *madc,
+ int conv_method)
+{
+ const struct twl4030_madc_conversion_method *method;
+
+ method = &twl4030_conversion_methods[conv_method];
+
+ switch (conv_method) {
+ case TWL4030_MADC_SW1:
+ case TWL4030_MADC_SW2:
+ twl4030_madc_write(madc, method->ctrl, TWL4030_MADC_SW_START);
+ break;
+ case TWL4030_MADC_RT:
+ default:
+ break;
+ }
+}
+
+static int twl4030_madc_wait_conversion_ready(
+ struct twl4030_madc_data *madc,
+ unsigned int timeout_ms, u8 status_reg)
+{
+ unsigned long timeout;
+
+ timeout = jiffies + msecs_to_jiffies(timeout_ms);
+ do {
+ int reg;
+
+ reg = twl4030_madc_read(madc, status_reg);
+ if (unlikely(reg < 0))
+ return reg;
+ if (!(reg & TWL4030_MADC_BUSY) && (reg & TWL4030_MADC_EOC_SW))
+ return 0;
+ } while (!time_after(jiffies, timeout));
+
+ return -EAGAIN;
+}
+
+static int twl4030_madc_set_power(struct twl4030_madc_data *madc, int on);
+int twl4030_madc_conversion(struct twl4030_madc_request *req)
+{
+ const struct twl4030_madc_conversion_method *method;
+ u8 ch_msb, ch_lsb;
+ int ret;
+
+ if (unlikely(!req))
+ return -EINVAL;
+
+ mutex_lock(&the_madc->lock);
+
+ twl4030_madc_set_power(the_madc, 1);
+
+ /* Do we have a conversion request ongoing */
+ if (the_madc->requests[req->method].active) {
+ ret = -EBUSY;
+ goto out;
+ }
+
+ ch_msb = (req->channels >> 8) & 0xff;
+ ch_lsb = req->channels & 0xff;
+
+ method = &twl4030_conversion_methods[req->method];
+
+ /* Select channels to be converted */
+ twl4030_madc_write(the_madc, method->sel + 1, ch_msb);
+ twl4030_madc_write(the_madc, method->sel, ch_lsb);
+
+ /* Select averaging for all channels if do_avg is set */
+ if (req->do_avg) {
+ twl4030_madc_write(the_madc, method->avg + 1, ch_msb);
+ twl4030_madc_write(the_madc, method->avg, ch_lsb);
+ }
+
+ if ((req->type == TWL4030_MADC_IRQ_ONESHOT) && (req->func_cb != NULL)) {
+ twl4030_madc_set_irq(the_madc, req);
+ twl4030_madc_start_conversion(the_madc, req->method);
+ the_madc->requests[req->method].active = 1;
+ ret = 0;
+ goto out;
+ }
+
+ /* With RT method we should not be here anymore */
+ if (req->method == TWL4030_MADC_RT) {
+ ret = -EINVAL;
+ goto out;
+ }
+
+ twl4030_madc_start_conversion(the_madc, req->method);
+ the_madc->requests[req->method].active = 1;
+
+ /* Wait until conversion is ready (ctrl register returns EOC) */
+ ret = twl4030_madc_wait_conversion_ready(the_madc, 5, method->ctrl);
+ if (ret) {
+ dev_dbg(the_madc->dev, "conversion timeout!\n");
+ the_madc->requests[req->method].active = 0;
+ goto out;
+ }
+
+ ret = twl4030_madc_read_channels(the_madc, method->rbase, req->channels,
+ req->rbuf);
+
+ the_madc->requests[req->method].active = 0;
+
+ twl4030_madc_set_power(the_madc, 0);
+
+out:
+ mutex_unlock(&the_madc->lock);
+
+ return ret;
+}
+EXPORT_SYMBOL(twl4030_madc_conversion);
+
+static int twl4030_madc_set_current_generator(struct twl4030_madc_data *madc,
+ int chan, int on)
+{
+ int ret;
+ u8 regval;
+
+ /* Current generator is only available for ADCIN0 and ADCIN1. NB:
+ * ADCIN1 current generator only works when AC or VBUS is present */
+ if (chan > 1)
+ return EINVAL;
+
+ ret = twl4030_i2c_read_u8(TWL4030_MODULE_MAIN_CHARGE,
+ &regval, TWL4030_BCI_BCICTL1);
+ if (ret) {
+ dev_dbg(madc->dev, "unable to read register 0x%X\n", TWL4030_BCI_BCICTL1);
+ return ret;
+ }
+
+ if (on) {
+ regval |= (chan) ? TWL4030_BCI_ITHEN : TWL4030_BCI_TYPEN;
+ regval |= TWL4030_BCI_MESBAT;
+ }
+ else {
+ regval &= (chan) ? ~TWL4030_BCI_ITHEN : ~TWL4030_BCI_TYPEN;
+ regval &= ~TWL4030_BCI_MESBAT;
+ }
+
+ ret = twl4030_i2c_write_u8(TWL4030_MODULE_MAIN_CHARGE,
+ regval, TWL4030_BCI_BCICTL1);
+ if (ret) {
+ dev_dbg(madc->dev, "unable to write register 0x%X\n", TWL4030_BCI_BCICTL1);
+ }
+ return ret;
+}
+
+static int twl4030_madc_set_power(struct twl4030_madc_data *madc, int on)
+{
+ int ret = 0;
+ u8 regval;
+
+ if (on) {
+ regval = twl4030_madc_read(madc, TWL4030_MADC_CTRL1);
+ regval |= TWL4030_MADC_MADCON;
+ twl4030_madc_write(madc, TWL4030_MADC_CTRL1, regval);
+
+ ret |= twl4030_madc_set_current_generator(madc, 0, 1);
+
+ } else {
+ ret |= twl4030_madc_set_current_generator(madc, 0, 0);
+
+ regval = twl4030_madc_read(madc, TWL4030_MADC_CTRL1);
+ regval &= ~TWL4030_MADC_MADCON;
+ twl4030_madc_write(madc, TWL4030_MADC_CTRL1, regval);
+ }
+ return ret;
+}
+
+static long twl4030_madc_ioctl(struct file *filp, unsigned int cmd,
+ unsigned long arg)
+{
+ struct twl4030_madc_user_parms par;
+ int val, ret;
+
+ ret = copy_from_user(&par, (void __user *) arg, sizeof(par));
+ if (ret) {
+ dev_dbg(the_madc->dev, "copy_from_user: %d\n", ret);
+ return -EACCES;
+ }
+
+ switch (cmd) {
+ case TWL4030_MADC_IOCX_ADC_RAW_READ: {
+ struct twl4030_madc_request req;
+ if (par.channel >= TWL4030_MADC_MAX_CHANNELS)
+ return -EINVAL;
+
+ req.channels = (1 << par.channel);
+ req.do_avg = par.average;
+ req.method = TWL4030_MADC_SW1;
+ req.func_cb = NULL;
+ req.type = TWL4030_MADC_WAIT;
+
+ val = twl4030_madc_conversion(&req);
+ if (likely(val > 0)) {
+ par.status = 0;
+ par.result = (u16)req.rbuf[par.channel];
+ } else if (val == 0) {
+ par.status = -ENODATA;
+ } else {
+ par.status = val;
+ }
+ break;
+ }
+ default:
+ return -EINVAL;
+ }
+
+ ret = copy_to_user((void __user *) arg, &par, sizeof(par));
+ if (ret) {
+ dev_dbg(the_madc->dev, "copy_to_user: %d\n", ret);
+ return -EACCES;
+ }
+
+ return 0;
+}
+
+static struct file_operations twl4030_madc_fileops = {
+ .owner = THIS_MODULE,
+ .unlocked_ioctl = twl4030_madc_ioctl
+};
+
+static struct miscdevice twl4030_madc_device = {
+ .minor = MISC_DYNAMIC_MINOR,
+ .name = "twl4030-adc",
+ .fops = &twl4030_madc_fileops
+};
+
+static int __init twl4030_madc_probe(struct platform_device *pdev)
+{
+ struct twl4030_madc_data *madc;
+ struct twl4030_madc_platform_data *pdata = pdev->dev.platform_data;
+
+ int ret;
+ u8 regval;
+
+ madc = kzalloc(sizeof *madc, GFP_KERNEL);
+ if (!madc)
+ return -ENOMEM;
+
+ if (!pdata) {
+ dev_dbg(&pdev->dev, "platform_data not available\n");
+ ret = -EINVAL;
+ goto err_pdata;
+ }
+
+ madc->imr = (pdata->irq_line == 1) ? TWL4030_MADC_IMR1 : TWL4030_MADC_IMR2;
+ madc->isr = (pdata->irq_line == 1) ? TWL4030_MADC_ISR1 : TWL4030_MADC_ISR2;
+
+ ret = misc_register(&twl4030_madc_device);
+ if (ret) {
+ dev_dbg(&pdev->dev, "could not register misc_device\n");
+ goto err_misc;
+ }
+
+ ret = request_irq(platform_get_irq(pdev, 0), twl4030_madc_irq_handler,
+ 0, "twl4030_madc", madc);
+ if (ret) {
+ dev_dbg(&pdev->dev, "could not request irq\n");
+ goto err_irq;
+ }
+
+ platform_set_drvdata(pdev, madc);
+ mutex_init(&madc->lock);
+ INIT_WORK(&madc->ws, twl4030_madc_work);
+
+ the_madc = madc;
+
+ return 0;
+
+err_irq:
+ misc_deregister(&twl4030_madc_device);
+
+err_misc:
+err_pdata:
+ kfree(madc);
+
+ return ret;
+}
+
+static int __exit twl4030_madc_remove(struct platform_device *pdev)
+{
+ struct twl4030_madc_data *madc = platform_get_drvdata(pdev);
+
+ free_irq(platform_get_irq(pdev, 0), madc);
+ cancel_work_sync(&madc->ws);
+ misc_deregister(&twl4030_madc_device);
+
+ return 0;
+}
+
+static struct platform_driver twl4030_madc_driver = {
+ .probe = twl4030_madc_probe,
+ .remove = __exit_p(twl4030_madc_remove),
+ .driver = {
+ .name = "twl4030_madc",
+ .owner = THIS_MODULE,
+ },
+};
+
+static int __init twl4030_madc_init(void)
+{
+ return platform_driver_register(&twl4030_madc_driver);
+}
+module_init(twl4030_madc_init);
+
+static void __exit twl4030_madc_exit(void)
+{
+ platform_driver_unregister(&twl4030_madc_driver);
+}
+module_exit(twl4030_madc_exit);
+
+MODULE_ALIAS("platform:twl4030-madc");
+MODULE_AUTHOR("Nokia Corporation");
+MODULE_DESCRIPTION("twl4030 ADC driver");
+MODULE_LICENSE("GPL");
+
diff --git a/include/linux/i2c/twl4030-madc.h b/include/linux/i2c/twl4030-madc.h
new file mode 100644
index 0000000..24523b5
--- /dev/null
+++ b/include/linux/i2c/twl4030-madc.h
@@ -0,0 +1,126 @@
+/*
+ * include/linux/i2c/twl4030-madc.h
+ *
+ * TWL4030 MADC module driver header
+ *
+ * Copyright (C) 2008 Nokia Corporation
+ * Mikko Ylinen <[email protected]>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#ifndef _TWL4030_MADC_H
+#define _TWL4030_MADC_H
+
+struct twl4030_madc_conversion_method {
+ u8 sel;
+ u8 avg;
+ u8 rbase;
+ u8 ctrl;
+};
+
+#define TWL4030_MADC_MAX_CHANNELS 16
+
+struct twl4030_madc_request {
+ u16 channels;
+ u16 do_avg;
+ u16 method;
+ u16 type;
+ int active;
+ int result_pending;
+ int rbuf[TWL4030_MADC_MAX_CHANNELS];
+ void (*func_cb)(int len, int channels, int *buf);
+};
+
+enum conversion_methods {
+ TWL4030_MADC_RT,
+ TWL4030_MADC_SW1,
+ TWL4030_MADC_SW2,
+ TWL4030_MADC_NUM_METHODS
+};
+
+enum sample_type {
+ TWL4030_MADC_WAIT,
+ TWL4030_MADC_IRQ_ONESHOT,
+ TWL4030_MADC_IRQ_REARM
+};
+
+#define TWL4030_MADC_CTRL1 0x00
+#define TWL4030_MADC_CTRL2 0x01
+
+#define TWL4030_MADC_RTSELECT_LSB 0x02
+#define TWL4030_MADC_SW1SELECT_LSB 0x06
+#define TWL4030_MADC_SW2SELECT_LSB 0x0A
+
+#define TWL4030_MADC_RTAVERAGE_LSB 0x04
+#define TWL4030_MADC_SW1AVERAGE_LSB 0x08
+#define TWL4030_MADC_SW2AVERAGE_LSB 0x0C
+
+#define TWL4030_MADC_CTRL_SW1 0x12
+#define TWL4030_MADC_CTRL_SW2 0x13
+
+#define TWL4030_MADC_RTCH0_LSB 0x17
+#define TWL4030_MADC_GPCH0_LSB 0x37
+
+#define TWL4030_MADC_MADCON (1<<0) /* MADC power on */
+#define TWL4030_MADC_BUSY (1<<0) /* MADC busy */
+#define TWL4030_MADC_EOC_SW (1<<1) /* MADC conversion completion */
+#define TWL4030_MADC_SW_START (1<<5) /* MADC SWx start conversion */
+
+#define TWL4030_MADC_ADCIN0 (1<<0)
+#define TWL4030_MADC_ADCIN1 (1<<1)
+#define TWL4030_MADC_ADCIN2 (1<<2)
+#define TWL4030_MADC_ADCIN3 (1<<3)
+#define TWL4030_MADC_ADCIN4 (1<<4)
+#define TWL4030_MADC_ADCIN5 (1<<5)
+#define TWL4030_MADC_ADCIN6 (1<<6)
+#define TWL4030_MADC_ADCIN7 (1<<7)
+#define TWL4030_MADC_ADCIN8 (1<<8)
+#define TWL4030_MADC_ADCIN9 (1<<9)
+#define TWL4030_MADC_ADCIN10 (1<<10)
+#define TWL4030_MADC_ADCIN11 (1<<11)
+#define TWL4030_MADC_ADCIN12 (1<<12)
+#define TWL4030_MADC_ADCIN13 (1<<13)
+#define TWL4030_MADC_ADCIN14 (1<<14)
+#define TWL4030_MADC_ADCIN15 (1<<15)
+
+/* Fixed channels */
+#define TWL4030_MADC_BTEMP TWL4030_MADC_ADCIN1
+#define TWL4030_MADC_VBUS TWL4030_MADC_ADCIN8
+#define TWL4030_MADC_VBKB TWL4030_MADC_ADCIN9
+#define TWL4030_MADC_ICHG TWL4030_MADC_ADCIN10
+#define TWL4030_MADC_VCHG TWL4030_MADC_ADCIN11
+#define TWL4030_MADC_VBAT TWL4030_MADC_ADCIN12
+
+/* BCI related - XXX To be moved elsewhere */
+#define TWL4030_BCI_BCICTL1 0x23
+#define TWL4030_BCI_MESBAT (1<<1)
+#define TWL4030_BCI_TYPEN (1<<4)
+#define TWL4030_BCI_ITHEN (1<<3)
+
+#define TWL4030_MADC_IOC_MAGIC '`'
+#define TWL4030_MADC_IOCX_ADC_RAW_READ _IO(TWL4030_MADC_IOC_MAGIC, 0)
+
+struct twl4030_madc_user_parms {
+ int channel;
+ int average;
+ int status;
+ u16 result;
+};
+
+int twl4030_madc_conversion(struct twl4030_madc_request *conv);
+
+#endif
--
1.6.3.3


2009-11-27 19:34:57

by Samuel Ortiz

[permalink] [raw]
Subject: Re: [PATCH] mfd: twl4030: Driver for twl4030 madc module

Hi Amit,

On Wed, Nov 25, 2009 at 12:47:51PM +0200, Amit Kucheria wrote:
> diff --git a/drivers/mfd/Makefile b/drivers/mfd/Makefile
> index af0fc90..df1897b 100644
> --- a/drivers/mfd/Makefile
> +++ b/drivers/mfd/Makefile
> @@ -25,8 +25,9 @@ obj-$(CONFIG_TPS65010) += tps65010.o
> obj-$(CONFIG_MENELAUS) += menelaus.o
>
> obj-$(CONFIG_TWL4030_CORE) += twl4030-core.o twl4030-irq.o
> -obj-$(CONFIG_TWL4030_POWER) += twl4030-power.o
> +obj-$(CONFIG_TWL4030_POWER) += twl4030-power.o
> obj-$(CONFIG_TWL4030_CODEC) += twl4030-codec.o
> +obj-$(CONFIG_TWL4030_MADC) += twl4030-madc.o
>
> obj-$(CONFIG_MFD_MC13783) += mc13783-core.o
>
> diff --git a/drivers/mfd/twl4030-madc.c b/drivers/mfd/twl4030-madc.c
> new file mode 100644
> index 0000000..90ce99a
> --- /dev/null
> +++ b/drivers/mfd/twl4030-madc.c
> @@ -0,0 +1,548 @@
> +/*
> + * drivers/i2c/chips/twl4030-madc.c
drivers/mfd/twl4030-madc.c
By the way, have you considered moving this one to drivers/hwmon ?


> + * TWL4030 MADC module driver
> + *
> + * Copyright (C) 2008 Nokia Corporation
> + * Mikko Ylinen <[email protected]>
> + *
> + * This program is free software; you can redistribute it and/or
> + * modify it under the terms of the GNU General Public License
> + * version 2 as published by the Free Software Foundation.
> + *
> + * This program is distributed in the hope that it will be useful, but
> + * WITHOUT ANY WARRANTY; without even the implied warranty of
> + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
> + * General Public License for more details.
> + *
> + * You should have received a copy of the GNU General Public License
> + * along with this program; if not, write to the Free Software
> + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
> + * 02110-1301 USA
> + *
> + */
> +
> +#include <linux/init.h>
> +#include <linux/interrupt.h>
> +#include <linux/kernel.h>
> +#include <linux/types.h>
> +#include <linux/module.h>
> +#include <linux/delay.h>
> +#include <linux/fs.h>
> +#include <linux/platform_device.h>
> +#include <linux/miscdevice.h>
> +#include <linux/i2c/twl4030.h>
> +#include <linux/i2c/twl4030-madc.h>
> +
> +#include <asm/uaccess.h>
> +
> +#define TWL4030_MADC_PFX "twl4030-madc: "
> +
> +struct twl4030_madc_data {
> + struct device *dev;
> + struct mutex lock;
> + struct work_struct ws;
> + struct twl4030_madc_request requests[TWL4030_MADC_NUM_METHODS];
Typically, I'd expect to have a list (limited in length) of requests here, but
I guess you're happy with that array .



> + int imr;
> + int isr;
> +};
> +
> +static struct twl4030_madc_data *the_madc;
I dont particularly enjoy that. All of the twl4030 drivers have this bad habit
of assuming they will be alone on a platform. Although it's certainly very
likely, I still think having this kind of design is not so nice.


> +static int twl4030_madc_set_current_generator(struct twl4030_madc_data *madc, int chan, int on);
> +
> +static
> +const struct twl4030_madc_conversion_method twl4030_conversion_methods[] = {
> + [TWL4030_MADC_RT] = {
> + .sel = TWL4030_MADC_RTSELECT_LSB,
> + .avg = TWL4030_MADC_RTAVERAGE_LSB,
> + .rbase = TWL4030_MADC_RTCH0_LSB,
> + },
> + [TWL4030_MADC_SW1] = {
> + .sel = TWL4030_MADC_SW1SELECT_LSB,
> + .avg = TWL4030_MADC_SW1AVERAGE_LSB,
> + .rbase = TWL4030_MADC_GPCH0_LSB,
> + .ctrl = TWL4030_MADC_CTRL_SW1,
> + },
> + [TWL4030_MADC_SW2] = {
> + .sel = TWL4030_MADC_SW2SELECT_LSB,
> + .avg = TWL4030_MADC_SW2AVERAGE_LSB,
> + .rbase = TWL4030_MADC_GPCH0_LSB,
> + .ctrl = TWL4030_MADC_CTRL_SW2,
> + },
> +};
> +
> +static int twl4030_madc_read(struct twl4030_madc_data *madc, u8 reg)
> +{
> + int ret;
> + u8 val;
> +
> + ret = twl4030_i2c_read_u8(TWL4030_MODULE_MADC, &val, reg);
> + if (ret) {
> + dev_dbg(madc->dev, "unable to read register 0x%X\n", reg);
> + return ret;
> + }
> +
> + return val;
> +}
FWIW, you're not checking the return value on any of the madc_read calls
below.


> +static void twl4030_madc_write(struct twl4030_madc_data *madc, u8 reg, u8 val)
> +{
> + int ret;
> +
> + ret = twl4030_i2c_write_u8(TWL4030_MODULE_MADC, val, reg);
> + if (ret)
> + dev_err(madc->dev, "unable to write register 0x%X\n", reg);
> +}
> +
> +static int twl4030_madc_channel_raw_read(struct twl4030_madc_data *madc, u8 reg)
> +{
> + u8 msb, lsb;
> +
> + /* For each ADC channel, we have MSB and LSB register pair. MSB address
> + * is always LSB address+1. reg parameter is the addr of LSB register */
> + msb = twl4030_madc_read(madc, reg + 1);
> + lsb = twl4030_madc_read(madc, reg);
> +
> + return (int)(((msb << 8) | lsb) >> 6);
> +}
> +
> +static int twl4030_madc_read_channels(struct twl4030_madc_data *madc,
> + u8 reg_base, u16 channels, int *buf)
> +{
> + int count = 0;
> + u8 reg, i;
> +
> + if (unlikely(!buf))
> + return 0;
> +
> + for (i = 0; i < TWL4030_MADC_MAX_CHANNELS; i++) {
> + if (channels & (1<<i)) {
> + reg = reg_base + 2*i;
> + buf[i] = twl4030_madc_channel_raw_read(madc, reg);
> + count++;
> + }
> + }
> + return count;
> +}
> +
> +static void twl4030_madc_enable_irq(struct twl4030_madc_data *madc, int id)
> +{
> + u8 val;
> +
> + val = twl4030_madc_read(madc, madc->imr);
> + val &= ~(1 << id);
> + twl4030_madc_write(madc, madc->imr, val);
> +}
> +
> +static void twl4030_madc_disable_irq(struct twl4030_madc_data *madc, int id)
> +{
> + u8 val;
> +
> + val = twl4030_madc_read(madc, madc->imr);
> + val |= (1 << id);
> + twl4030_madc_write(madc, madc->imr, val);
> +}
> +
> +static irqreturn_t twl4030_madc_irq_handler(int irq, void *_madc)
> +{
> + struct twl4030_madc_data *madc = _madc;
> + u8 isr_val, imr_val;
> + int i;
> +
> +#ifdef CONFIG_LOCKDEP
> + /* WORKAROUND for lockdep forcing IRQF_DISABLED on us, which
> + * we don't want and can't tolerate. Although it might be
> + * friendlier not to borrow this thread context...
> + */
> + local_irq_enable();
> +#endif
I'm curious, what's special about madc so that it can't tolerate that ?


> + /* Use COR to ack interrupts since we have no shared IRQs in ISRx */
> + isr_val = twl4030_madc_read(madc, madc->isr);
> + imr_val = twl4030_madc_read(madc, madc->imr);
> +
> + isr_val &= ~imr_val;
> +
> + for (i = 0; i < TWL4030_MADC_NUM_METHODS; i++) {
> +
> + if (!(isr_val & (1<<i)))
> + continue;
> +
> + twl4030_madc_disable_irq(madc, i);
> + madc->requests[i].result_pending = 1;
> + }
> +
> + schedule_work(&madc->ws);
> +
> + return IRQ_HANDLED;
> +}
> +
> +static void twl4030_madc_work(struct work_struct *ws)
> +{
> + const struct twl4030_madc_conversion_method *method;
> + struct twl4030_madc_data *madc;
> + struct twl4030_madc_request *r;
> + int len, i;
> +
> + madc = container_of(ws, struct twl4030_madc_data, ws);
> + mutex_lock(&madc->lock);
> +
> + for (i = 0; i < TWL4030_MADC_NUM_METHODS; i++) {
> +
> + r = &madc->requests[i];
> +
> + /* No pending results for this method, move to next one */
> + if (!r->result_pending)
> + continue;
> +
> + method = &twl4030_conversion_methods[r->method];
> +
> + /* Read results */
> + len = twl4030_madc_read_channels(madc, method->rbase,
> + r->channels, r->rbuf);
> +
> + /* Return results to caller */
> + if (r->func_cb != NULL) {
> + r->func_cb(len, r->channels, r->rbuf);
> + r->func_cb = NULL;
> + }
> +
> + /* Free request */
> + r->result_pending = 0;
> + r->active = 0;
> + }
> +
> + mutex_unlock(&madc->lock);
> +}
I think you may want to consider using a threaded irq here, see
request_threaded_irq().


> +static int twl4030_madc_set_irq(struct twl4030_madc_data *madc,
> + struct twl4030_madc_request *req)
> +{
> + struct twl4030_madc_request *p;
> +
> + p = &madc->requests[req->method];
> +
> + memcpy(p, req, sizeof *req);
> +
> + twl4030_madc_enable_irq(madc, req->method);
> +
> + return 0;
> +}
> +
> +static inline void twl4030_madc_start_conversion(struct twl4030_madc_data *madc,
> + int conv_method)
> +{
> + const struct twl4030_madc_conversion_method *method;
> +
> + method = &twl4030_conversion_methods[conv_method];
> +
> + switch (conv_method) {
> + case TWL4030_MADC_SW1:
> + case TWL4030_MADC_SW2:
> + twl4030_madc_write(madc, method->ctrl, TWL4030_MADC_SW_START);
> + break;
> + case TWL4030_MADC_RT:
> + default:
> + break;
> + }
> +}
> +
> +static int twl4030_madc_wait_conversion_ready(
> + struct twl4030_madc_data *madc,
> + unsigned int timeout_ms, u8 status_reg)
> +{
> + unsigned long timeout;
> +
> + timeout = jiffies + msecs_to_jiffies(timeout_ms);
> + do {
> + int reg;
> +
> + reg = twl4030_madc_read(madc, status_reg);
> + if (unlikely(reg < 0))
> + return reg;
> + if (!(reg & TWL4030_MADC_BUSY) && (reg & TWL4030_MADC_EOC_SW))
> + return 0;
> + } while (!time_after(jiffies, timeout));
> +
> + return -EAGAIN;
> +}
> +
> +static int twl4030_madc_set_power(struct twl4030_madc_data *madc, int on);
> +int twl4030_madc_conversion(struct twl4030_madc_request *req)
> +{
> + const struct twl4030_madc_conversion_method *method;
> + u8 ch_msb, ch_lsb;
> + int ret;
> +
> + if (unlikely(!req))
> + return -EINVAL;
> +
> + mutex_lock(&the_madc->lock);
> +
> + twl4030_madc_set_power(the_madc, 1);
> +
> + /* Do we have a conversion request ongoing */
> + if (the_madc->requests[req->method].active) {
> + ret = -EBUSY;
> + goto out;
> + }
> +
> + ch_msb = (req->channels >> 8) & 0xff;
> + ch_lsb = req->channels & 0xff;
> +
> + method = &twl4030_conversion_methods[req->method];
> +
> + /* Select channels to be converted */
> + twl4030_madc_write(the_madc, method->sel + 1, ch_msb);
> + twl4030_madc_write(the_madc, method->sel, ch_lsb);
> +
> + /* Select averaging for all channels if do_avg is set */
> + if (req->do_avg) {
> + twl4030_madc_write(the_madc, method->avg + 1, ch_msb);
> + twl4030_madc_write(the_madc, method->avg, ch_lsb);
> + }
> +
> + if ((req->type == TWL4030_MADC_IRQ_ONESHOT) && (req->func_cb != NULL)) {
> + twl4030_madc_set_irq(the_madc, req);
> + twl4030_madc_start_conversion(the_madc, req->method);
> + the_madc->requests[req->method].active = 1;
> + ret = 0;
> + goto out;
> + }
> +
> + /* With RT method we should not be here anymore */
> + if (req->method == TWL4030_MADC_RT) {
> + ret = -EINVAL;
> + goto out;
> + }
> +
> + twl4030_madc_start_conversion(the_madc, req->method);
> + the_madc->requests[req->method].active = 1;
> +
> + /* Wait until conversion is ready (ctrl register returns EOC) */
> + ret = twl4030_madc_wait_conversion_ready(the_madc, 5, method->ctrl);
So, you're busy looping with all conversions ? I have no idea how often this
is called, but putting the caller to sleep on e.g. a waitqueue would be more
elegant.


> + if (ret) {
> + dev_dbg(the_madc->dev, "conversion timeout!\n");
> + the_madc->requests[req->method].active = 0;
> + goto out;
> + }
> +
> + ret = twl4030_madc_read_channels(the_madc, method->rbase, req->channels,
> + req->rbuf);
> +
> + the_madc->requests[req->method].active = 0;
> +
> + twl4030_madc_set_power(the_madc, 0);
> +
> +out:
> + mutex_unlock(&the_madc->lock);
> +
> + return ret;
> +}
> +EXPORT_SYMBOL(twl4030_madc_conversion);
Who's supposed to use this one ?


> +static int twl4030_madc_set_current_generator(struct twl4030_madc_data *madc,
> + int chan, int on)
> +{
> + int ret;
> + u8 regval;
> +
> + /* Current generator is only available for ADCIN0 and ADCIN1. NB:
> + * ADCIN1 current generator only works when AC or VBUS is present */
> + if (chan > 1)
> + return EINVAL;
> +
> + ret = twl4030_i2c_read_u8(TWL4030_MODULE_MAIN_CHARGE,
> + &regval, TWL4030_BCI_BCICTL1);
> + if (ret) {
> + dev_dbg(madc->dev, "unable to read register 0x%X\n", TWL4030_BCI_BCICTL1);
> + return ret;
> + }
> +
> + if (on) {
> + regval |= (chan) ? TWL4030_BCI_ITHEN : TWL4030_BCI_TYPEN;
> + regval |= TWL4030_BCI_MESBAT;
> + }
> + else {
> + regval &= (chan) ? ~TWL4030_BCI_ITHEN : ~TWL4030_BCI_TYPEN;
> + regval &= ~TWL4030_BCI_MESBAT;
> + }
> +
> + ret = twl4030_i2c_write_u8(TWL4030_MODULE_MAIN_CHARGE,
> + regval, TWL4030_BCI_BCICTL1);
> + if (ret) {
> + dev_dbg(madc->dev, "unable to write register 0x%X\n", TWL4030_BCI_BCICTL1);
> + }
> + return ret;
> +}
> +
> +static int twl4030_madc_set_power(struct twl4030_madc_data *madc, int on)
> +{
> + int ret = 0;
> + u8 regval;
> +
> + if (on) {
> + regval = twl4030_madc_read(madc, TWL4030_MADC_CTRL1);
> + regval |= TWL4030_MADC_MADCON;
> + twl4030_madc_write(madc, TWL4030_MADC_CTRL1, regval);
> +
> + ret |= twl4030_madc_set_current_generator(madc, 0, 1);
> +
> + } else {
> + ret |= twl4030_madc_set_current_generator(madc, 0, 0);
> +
> + regval = twl4030_madc_read(madc, TWL4030_MADC_CTRL1);
> + regval &= ~TWL4030_MADC_MADCON;
> + twl4030_madc_write(madc, TWL4030_MADC_CTRL1, regval);
> + }
> + return ret;
> +}
> +
> +static long twl4030_madc_ioctl(struct file *filp, unsigned int cmd,
> + unsigned long arg)
> +{
> + struct twl4030_madc_user_parms par;
> + int val, ret;
> +
> + ret = copy_from_user(&par, (void __user *) arg, sizeof(par));
> + if (ret) {
> + dev_dbg(the_madc->dev, "copy_from_user: %d\n", ret);
> + return -EACCES;
> + }
> +
> + switch (cmd) {
> + case TWL4030_MADC_IOCX_ADC_RAW_READ: {
> + struct twl4030_madc_request req;
> + if (par.channel >= TWL4030_MADC_MAX_CHANNELS)
> + return -EINVAL;
> +
> + req.channels = (1 << par.channel);
> + req.do_avg = par.average;
> + req.method = TWL4030_MADC_SW1;
> + req.func_cb = NULL;
> + req.type = TWL4030_MADC_WAIT;
> +
> + val = twl4030_madc_conversion(&req);
> + if (likely(val > 0)) {
> + par.status = 0;
> + par.result = (u16)req.rbuf[par.channel];
> + } else if (val == 0) {
> + par.status = -ENODATA;
> + } else {
> + par.status = val;
> + }
> + break;
> + }
> + default:
> + return -EINVAL;
> + }
> +
> + ret = copy_to_user((void __user *) arg, &par, sizeof(par));
> + if (ret) {
> + dev_dbg(the_madc->dev, "copy_to_user: %d\n", ret);
> + return -EACCES;
> + }
> +
> + return 0;
> +}
> +
> +static struct file_operations twl4030_madc_fileops = {
> + .owner = THIS_MODULE,
> + .unlocked_ioctl = twl4030_madc_ioctl
> +};
> +
> +static struct miscdevice twl4030_madc_device = {
> + .minor = MISC_DYNAMIC_MINOR,
> + .name = "twl4030-adc",
> + .fops = &twl4030_madc_fileops
> +};
> +
> +static int __init twl4030_madc_probe(struct platform_device *pdev)
> +{
> + struct twl4030_madc_data *madc;
> + struct twl4030_madc_platform_data *pdata = pdev->dev.platform_data;
> +
> + int ret;
> + u8 regval;
> +
> + madc = kzalloc(sizeof *madc, GFP_KERNEL);
> + if (!madc)
> + return -ENOMEM;
> +
> + if (!pdata) {
> + dev_dbg(&pdev->dev, "platform_data not available\n");
> + ret = -EINVAL;
> + goto err_pdata;
> + }
> +
> + madc->imr = (pdata->irq_line == 1) ? TWL4030_MADC_IMR1 : TWL4030_MADC_IMR2;
> + madc->isr = (pdata->irq_line == 1) ? TWL4030_MADC_ISR1 : TWL4030_MADC_ISR2;
> +
> + ret = misc_register(&twl4030_madc_device);
> + if (ret) {
> + dev_dbg(&pdev->dev, "could not register misc_device\n");
> + goto err_misc;
> + }
> +
> + ret = request_irq(platform_get_irq(pdev, 0), twl4030_madc_irq_handler,
> + 0, "twl4030_madc", madc);
> + if (ret) {
> + dev_dbg(&pdev->dev, "could not request irq\n");
> + goto err_irq;
> + }
> +
> + platform_set_drvdata(pdev, madc);
> + mutex_init(&madc->lock);
> + INIT_WORK(&madc->ws, twl4030_madc_work);
> +
> + the_madc = madc;
> +
> + return 0;
> +
> +err_irq:
> + misc_deregister(&twl4030_madc_device);
> +
> +err_misc:
> +err_pdata:
> + kfree(madc);
> +
> + return ret;
> +}
> +
> +static int __exit twl4030_madc_remove(struct platform_device *pdev)
> +{
> + struct twl4030_madc_data *madc = platform_get_drvdata(pdev);
> +
> + free_irq(platform_get_irq(pdev, 0), madc);
> + cancel_work_sync(&madc->ws);
> + misc_deregister(&twl4030_madc_device);
> +
> + return 0;
> +}
> +
> +static struct platform_driver twl4030_madc_driver = {
> + .probe = twl4030_madc_probe,
> + .remove = __exit_p(twl4030_madc_remove),
> + .driver = {
> + .name = "twl4030_madc",
> + .owner = THIS_MODULE,
> + },
> +};
> +
> +static int __init twl4030_madc_init(void)
> +{
> + return platform_driver_register(&twl4030_madc_driver);
> +}
> +module_init(twl4030_madc_init);
> +
> +static void __exit twl4030_madc_exit(void)
> +{
> + platform_driver_unregister(&twl4030_madc_driver);
> +}
> +module_exit(twl4030_madc_exit);
> +
> +MODULE_ALIAS("platform:twl4030-madc");
> +MODULE_AUTHOR("Nokia Corporation");
> +MODULE_DESCRIPTION("twl4030 ADC driver");
> +MODULE_LICENSE("GPL");
> +
> diff --git a/include/linux/i2c/twl4030-madc.h b/include/linux/i2c/twl4030-madc.h
> new file mode 100644
> index 0000000..24523b5
> --- /dev/null
> +++ b/include/linux/i2c/twl4030-madc.h
> @@ -0,0 +1,126 @@
> +/*
> + * include/linux/i2c/twl4030-madc.h
> + *
> + * TWL4030 MADC module driver header
> + *
> + * Copyright (C) 2008 Nokia Corporation
> + * Mikko Ylinen <[email protected]>
> + *
> + * This program is free software; you can redistribute it and/or
> + * modify it under the terms of the GNU General Public License
> + * version 2 as published by the Free Software Foundation.
> + *
> + * This program is distributed in the hope that it will be useful, but
> + * WITHOUT ANY WARRANTY; without even the implied warranty of
> + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
> + * General Public License for more details.
> + *
> + * You should have received a copy of the GNU General Public License
> + * along with this program; if not, write to the Free Software
> + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
> + * 02110-1301 USA
> + *
> + */
> +
> +#ifndef _TWL4030_MADC_H
> +#define _TWL4030_MADC_H
> +
> +struct twl4030_madc_conversion_method {
> + u8 sel;
> + u8 avg;
> + u8 rbase;
> + u8 ctrl;
> +};
> +
> +#define TWL4030_MADC_MAX_CHANNELS 16
> +
> +struct twl4030_madc_request {
> + u16 channels;
> + u16 do_avg;
> + u16 method;
> + u16 type;
> + int active;
> + int result_pending;
active and result_pending can be bool.

Cheers,
Samuel.

--
Intel Open Source Technology Centre
http://oss.intel.com/

2009-11-30 13:58:38

by Amit Kucheria

[permalink] [raw]
Subject: Re: [PATCH] mfd: twl4030: Driver for twl4030 madc module

Hi Samuel,

On Fri, Nov 27, 2009 at 9:36 PM, Samuel Ortiz <[email protected]> wrote:
> Hi Amit,
>
> On Wed, Nov 25, 2009 at 12:47:51PM +0200, Amit Kucheria wrote:
>> diff --git a/drivers/mfd/Makefile b/drivers/mfd/Makefile
>> index af0fc90..df1897b 100644
>> --- a/drivers/mfd/Makefile
>> +++ b/drivers/mfd/Makefile
>> @@ -25,8 +25,9 @@ obj-$(CONFIG_TPS65010) ? ? ? ? ? ? ?+= tps65010.o
>> ?obj-$(CONFIG_MENELAUS) ? ? ? ? ? ? ? += menelaus.o
>>
>> ?obj-$(CONFIG_TWL4030_CORE) ? += twl4030-core.o twl4030-irq.o
>> -obj-$(CONFIG_TWL4030_POWER) ? ?+= twl4030-power.o
>> +obj-$(CONFIG_TWL4030_POWER) ?+= twl4030-power.o
>> ?obj-$(CONFIG_TWL4030_CODEC) ?+= twl4030-codec.o
>> +obj-$(CONFIG_TWL4030_MADC) ? += twl4030-madc.o
>>
>> ?obj-$(CONFIG_MFD_MC13783) ? ?+= mc13783-core.o
>>
>> diff --git a/drivers/mfd/twl4030-madc.c b/drivers/mfd/twl4030-madc.c
>> new file mode 100644
>> index 0000000..90ce99a
>> --- /dev/null
>> +++ b/drivers/mfd/twl4030-madc.c
>> @@ -0,0 +1,548 @@
>> +/*
>> + * drivers/i2c/chips/twl4030-madc.c
> drivers/mfd/twl4030-madc.c
> By the way, have you considered moving this one to drivers/hwmon ?

I haven't. I moved it from i2c/chips/ to the most obvious place I
could think of - drivers/mfd. But wasn't this the point of mfd - that
various subcomponents drivers could live there instead of being
scattered across the driver tree?

Adding Jean to CC, if he would consider taking this driver in drivers/hwmon.

>> + * TWL4030 MADC module driver
>> + *
>> + * Copyright (C) 2008 Nokia Corporation
>> + * Mikko Ylinen <[email protected]>
>> + *
>> + * This program is free software; you can redistribute it and/or
>> + * modify it under the terms of the GNU General Public License
>> + * version 2 as published by the Free Software Foundation.
>> + *
>> + * This program is distributed in the hope that it will be useful, but
>> + * WITHOUT ANY WARRANTY; without even the implied warranty of
>> + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. ?See the GNU
>> + * General Public License for more details.
>> + *
>> + * You should have received a copy of the GNU General Public License
>> + * along with this program; if not, write to the Free Software
>> + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
>> + * 02110-1301 USA
>> + *
>> + */
>> +
>> +#include <linux/init.h>
>> +#include <linux/interrupt.h>
>> +#include <linux/kernel.h>
>> +#include <linux/types.h>
>> +#include <linux/module.h>
>> +#include <linux/delay.h>
>> +#include <linux/fs.h>
>> +#include <linux/platform_device.h>
>> +#include <linux/miscdevice.h>
>> +#include <linux/i2c/twl4030.h>
>> +#include <linux/i2c/twl4030-madc.h>
>> +
>> +#include <asm/uaccess.h>
>> +
>> +#define TWL4030_MADC_PFX ? ? "twl4030-madc: "
>> +
>> +struct twl4030_madc_data {
>> + ? ? struct device ? ? ? ? ? *dev;
>> + ? ? struct mutex ? ? ? ? ? ?lock;
>> + ? ? struct work_struct ? ? ?ws;
>> + ? ? struct twl4030_madc_request ? ? requests[TWL4030_MADC_NUM_METHODS];
> Typically, I'd expect to have a list (limited in length) of requests here, but
> I guess you're happy with that array .
>
>
>
>> + ? ? int imr;
>> + ? ? int isr;
>> +};
>> +
>> +static struct twl4030_madc_data *the_madc;
> I dont particularly enjoy that. All of the twl4030 drivers have this bad habit
> of assuming they will be alone on a platform. Although it's certainly very
> likely, I still think having this kind of design is not so nice.

I agree. Unfortunately, twl4030-core.c is unable to handle multiple
devices currently. See note from line 779 in twl4030-core below:

/* NOTE: this driver only handles a single twl4030/tps659x0 chip */
static int
twl4030_probe(struct i2c_client *client, const struct i2c_device_id *id)

>> +static int twl4030_madc_set_current_generator(struct twl4030_madc_data *madc, int chan, int on);
>> +
>> +static
>> +const struct twl4030_madc_conversion_method twl4030_conversion_methods[] = {
>> + ? ? [TWL4030_MADC_RT] = {
>> + ? ? ? ? ? ? .sel ? ?= TWL4030_MADC_RTSELECT_LSB,
>> + ? ? ? ? ? ? .avg ? ?= TWL4030_MADC_RTAVERAGE_LSB,
>> + ? ? ? ? ? ? .rbase ?= TWL4030_MADC_RTCH0_LSB,
>> + ? ? },
>> + ? ? [TWL4030_MADC_SW1] = {
>> + ? ? ? ? ? ? .sel ? ?= TWL4030_MADC_SW1SELECT_LSB,
>> + ? ? ? ? ? ? .avg ? ?= TWL4030_MADC_SW1AVERAGE_LSB,
>> + ? ? ? ? ? ? .rbase ?= TWL4030_MADC_GPCH0_LSB,
>> + ? ? ? ? ? ? .ctrl ? = TWL4030_MADC_CTRL_SW1,
>> + ? ? },
>> + ? ? [TWL4030_MADC_SW2] = {
>> + ? ? ? ? ? ? .sel ? ?= TWL4030_MADC_SW2SELECT_LSB,
>> + ? ? ? ? ? ? .avg ? ?= TWL4030_MADC_SW2AVERAGE_LSB,
>> + ? ? ? ? ? ? .rbase ?= TWL4030_MADC_GPCH0_LSB,
>> + ? ? ? ? ? ? .ctrl ? = TWL4030_MADC_CTRL_SW2,
>> + ? ? },
>> +};
>> +
>> +static int twl4030_madc_read(struct twl4030_madc_data *madc, u8 reg)
>> +{
>> + ? ? int ret;
>> + ? ? u8 val;
>> +
>> + ? ? ret = twl4030_i2c_read_u8(TWL4030_MODULE_MADC, &val, reg);
>> + ? ? if (ret) {
>> + ? ? ? ? ? ? dev_dbg(madc->dev, "unable to read register 0x%X\n", reg);
>> + ? ? ? ? ? ? return ret;
>> + ? ? }
>> +
>> + ? ? return val;
>> +}
> FWIW, you're not checking the return value on any of the madc_read calls
> below.

I've changed the dev_dbg above to dev_err now. If we see those error
messages, then anything that follows from the higher level functions
is overhead IMHO.
The higher level functions in this case aren't adding any more useful
information to the error. E.g. I could check the return value again in
twl4030_madc_channel_raw_read() below. But if would simply repeat the
same error message we show in twl4030_madc_read().

Hmm, perhaps twl4030_madc_read should return void?

>> +static void twl4030_madc_write(struct twl4030_madc_data *madc, u8 reg, u8 val)
>> +{
>> + ? ? int ret;
>> +
>> + ? ? ret = twl4030_i2c_write_u8(TWL4030_MODULE_MADC, val, reg);
>> + ? ? if (ret)
>> + ? ? ? ? ? ? dev_err(madc->dev, "unable to write register 0x%X\n", reg);
>> +}
>> +
>> +static int twl4030_madc_channel_raw_read(struct twl4030_madc_data *madc, u8 reg)
>> +{
>> + ? ? u8 msb, lsb;
>> +
>> + ? ? /* For each ADC channel, we have MSB and LSB register pair. MSB address
>> + ? ? ?* is always LSB address+1. reg parameter is the addr of LSB register */
>> + ? ? msb = twl4030_madc_read(madc, reg + 1);
>> + ? ? lsb = twl4030_madc_read(madc, reg);
>> +
>> + ? ? return (int)(((msb << 8) | lsb) >> 6);
>> +}
>> +
>> +static int twl4030_madc_read_channels(struct twl4030_madc_data *madc,
>> + ? ? ? ? ? ? u8 reg_base, u16 channels, int *buf)
>> +{
>> + ? ? int count = 0;
>> + ? ? u8 reg, i;
>> +
>> + ? ? if (unlikely(!buf))
>> + ? ? ? ? ? ? return 0;
>> +
>> + ? ? for (i = 0; i < TWL4030_MADC_MAX_CHANNELS; i++) {
>> + ? ? ? ? ? ? if (channels & (1<<i)) {
>> + ? ? ? ? ? ? ? ? ? ? reg = reg_base + 2*i;
>> + ? ? ? ? ? ? ? ? ? ? buf[i] = twl4030_madc_channel_raw_read(madc, reg);
>> + ? ? ? ? ? ? ? ? ? ? count++;
>> + ? ? ? ? ? ? }
>> + ? ? }
>> + ? ? return count;
>> +}
>> +
>> +static void twl4030_madc_enable_irq(struct twl4030_madc_data *madc, int id)
>> +{
>> + ? ? u8 val;
>> +
>> + ? ? val = twl4030_madc_read(madc, madc->imr);
>> + ? ? val &= ~(1 << id);
>> + ? ? twl4030_madc_write(madc, madc->imr, val);
>> +}
>> +
>> +static void twl4030_madc_disable_irq(struct twl4030_madc_data *madc, int id)
>> +{
>> + ? ? u8 val;
>> +
>> + ? ? val = twl4030_madc_read(madc, madc->imr);
>> + ? ? val |= (1 << id);
>> + ? ? twl4030_madc_write(madc, madc->imr, val);
>> +}
>> +
>> +static irqreturn_t twl4030_madc_irq_handler(int irq, void *_madc)
>> +{
>> + ? ? struct twl4030_madc_data *madc = _madc;
>> + ? ? u8 isr_val, imr_val;
>> + ? ? int i;
>> +
>> +#ifdef CONFIG_LOCKDEP
>> + ? ? /* WORKAROUND for lockdep forcing IRQF_DISABLED on us, which
>> + ? ? ?* we don't want and can't tolerate. ?Although it might be
>> + ? ? ?* friendlier not to borrow this thread context...
>> + ? ? ?*/
>> + ? ? local_irq_enable();
>> +#endif
> I'm curious, what's special about madc so that it can't tolerate that ?

This was part of some bugfixes by David Brownell:
http://markmail.org/message/zlisiurni3ei6gst
TBH, I am not upto speed on the working of LOCKDEP.

>> + ? ? /* Use COR to ack interrupts since we have no shared IRQs in ISRx */
>> + ? ? isr_val = twl4030_madc_read(madc, madc->isr);
>> + ? ? imr_val = twl4030_madc_read(madc, madc->imr);
>> +
>> + ? ? isr_val &= ~imr_val;
>> +
>> + ? ? for (i = 0; i < TWL4030_MADC_NUM_METHODS; i++) {
>> +
>> + ? ? ? ? ? ? if (!(isr_val & (1<<i)))
>> + ? ? ? ? ? ? ? ? ? ? continue;
>> +
>> + ? ? ? ? ? ? twl4030_madc_disable_irq(madc, i);
>> + ? ? ? ? ? ? madc->requests[i].result_pending = 1;
>> + ? ? }
>> +
>> + ? ? schedule_work(&madc->ws);
>> +
>> + ? ? return IRQ_HANDLED;
>> +}
>> +
>> +static void twl4030_madc_work(struct work_struct *ws)
>> +{
>> + ? ? const struct twl4030_madc_conversion_method *method;
>> + ? ? struct twl4030_madc_data *madc;
>> + ? ? struct twl4030_madc_request *r;
>> + ? ? int len, i;
>> +
>> + ? ? madc = container_of(ws, struct twl4030_madc_data, ws);
>> + ? ? mutex_lock(&madc->lock);
>> +
>> + ? ? for (i = 0; i < TWL4030_MADC_NUM_METHODS; i++) {
>> +
>> + ? ? ? ? ? ? r = &madc->requests[i];
>> +
>> + ? ? ? ? ? ? /* No pending results for this method, move to next one */
>> + ? ? ? ? ? ? if (!r->result_pending)
>> + ? ? ? ? ? ? ? ? ? ? continue;
>> +
>> + ? ? ? ? ? ? method = &twl4030_conversion_methods[r->method];
>> +
>> + ? ? ? ? ? ? /* Read results */
>> + ? ? ? ? ? ? len = twl4030_madc_read_channels(madc, method->rbase,
>> + ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?r->channels, r->rbuf);
>> +
>> + ? ? ? ? ? ? /* Return results to caller */
>> + ? ? ? ? ? ? if (r->func_cb != NULL) {
>> + ? ? ? ? ? ? ? ? ? ? r->func_cb(len, r->channels, r->rbuf);
>> + ? ? ? ? ? ? ? ? ? ? r->func_cb = NULL;
>> + ? ? ? ? ? ? }
>> +
>> + ? ? ? ? ? ? /* Free request */
>> + ? ? ? ? ? ? r->result_pending = 0;
>> + ? ? ? ? ? ? r->active ? ? ? ? = 0;
>> + ? ? }
>> +
>> + ? ? mutex_unlock(&madc->lock);
>> +}
> I think you may want to consider using a threaded irq here, see
> request_threaded_irq().

I am working on moving the entire twl* driver set to use threaded irqs
on the side. Will you consider merging this code with the work_struct
since it is known to work while I work on the conversion?

>> +static int twl4030_madc_set_irq(struct twl4030_madc_data *madc,
>> + ? ? ? ? ? ? struct twl4030_madc_request *req)
>> +{
>> + ? ? struct twl4030_madc_request *p;
>> +
>> + ? ? p = &madc->requests[req->method];
>> +
>> + ? ? memcpy(p, req, sizeof *req);
>> +
>> + ? ? twl4030_madc_enable_irq(madc, req->method);
>> +
>> + ? ? return 0;
>> +}
>> +
>> +static inline void twl4030_madc_start_conversion(struct twl4030_madc_data *madc,
>> + ? ? ? ? ? ? int conv_method)
>> +{
>> + ? ? const struct twl4030_madc_conversion_method *method;
>> +
>> + ? ? method = &twl4030_conversion_methods[conv_method];
>> +
>> + ? ? switch (conv_method) {
>> + ? ? case TWL4030_MADC_SW1:
>> + ? ? case TWL4030_MADC_SW2:
>> + ? ? ? ? ? ? twl4030_madc_write(madc, method->ctrl, TWL4030_MADC_SW_START);
>> + ? ? ? ? ? ? break;
>> + ? ? case TWL4030_MADC_RT:
>> + ? ? default:
>> + ? ? ? ? ? ? break;
>> + ? ? }
>> +}
>> +
>> +static int twl4030_madc_wait_conversion_ready(
>> + ? ? ? ? ? ? struct twl4030_madc_data *madc,
>> + ? ? ? ? ? ? unsigned int timeout_ms, u8 status_reg)
>> +{
>> + ? ? unsigned long timeout;
>> +
>> + ? ? timeout = jiffies + msecs_to_jiffies(timeout_ms);
>> + ? ? do {
>> + ? ? ? ? ? ? int reg;
>> +
>> + ? ? ? ? ? ? reg = twl4030_madc_read(madc, status_reg);
>> + ? ? ? ? ? ? if (unlikely(reg < 0))
>> + ? ? ? ? ? ? ? ? ? ? return reg;
>> + ? ? ? ? ? ? if (!(reg & TWL4030_MADC_BUSY) && (reg & TWL4030_MADC_EOC_SW))
>> + ? ? ? ? ? ? ? ? ? ? return 0;
>> + ? ? } while (!time_after(jiffies, timeout));
>> +
>> + ? ? return -EAGAIN;
>> +}
>> +
>> +static int twl4030_madc_set_power(struct twl4030_madc_data *madc, int on);
>> +int twl4030_madc_conversion(struct twl4030_madc_request *req)
>> +{
>> + ? ? const struct twl4030_madc_conversion_method *method;
>> + ? ? u8 ch_msb, ch_lsb;
>> + ? ? int ret;
>> +
>> + ? ? if (unlikely(!req))
>> + ? ? ? ? ? ? return -EINVAL;
>> +
>> + ? ? mutex_lock(&the_madc->lock);
>> +
>> + ? ? twl4030_madc_set_power(the_madc, 1);
>> +
>> + ? ? /* Do we have a conversion request ongoing */
>> + ? ? if (the_madc->requests[req->method].active) {
>> + ? ? ? ? ? ? ret = -EBUSY;
>> + ? ? ? ? ? ? goto out;
>> + ? ? }
>> +
>> + ? ? ch_msb = (req->channels >> 8) & 0xff;
>> + ? ? ch_lsb = req->channels & 0xff;
>> +
>> + ? ? method = &twl4030_conversion_methods[req->method];
>> +
>> + ? ? /* Select channels to be converted */
>> + ? ? twl4030_madc_write(the_madc, method->sel + 1, ch_msb);
>> + ? ? twl4030_madc_write(the_madc, method->sel, ch_lsb);
>> +
>> + ? ? /* Select averaging for all channels if do_avg is set */
>> + ? ? if (req->do_avg) {
>> + ? ? ? ? ? ? twl4030_madc_write(the_madc, method->avg + 1, ch_msb);
>> + ? ? ? ? ? ? twl4030_madc_write(the_madc, method->avg, ch_lsb);
>> + ? ? }
>> +
>> + ? ? if ((req->type == TWL4030_MADC_IRQ_ONESHOT) && (req->func_cb != NULL)) {
>> + ? ? ? ? ? ? twl4030_madc_set_irq(the_madc, req);
>> + ? ? ? ? ? ? twl4030_madc_start_conversion(the_madc, req->method);
>> + ? ? ? ? ? ? the_madc->requests[req->method].active = 1;
>> + ? ? ? ? ? ? ret = 0;
>> + ? ? ? ? ? ? goto out;
>> + ? ? }
>> +
>> + ? ? /* With RT method we should not be here anymore */
>> + ? ? if (req->method == TWL4030_MADC_RT) {
>> + ? ? ? ? ? ? ret = -EINVAL;
>> + ? ? ? ? ? ? goto out;
>> + ? ? }
>> +
>> + ? ? twl4030_madc_start_conversion(the_madc, req->method);
>> + ? ? the_madc->requests[req->method].active = 1;
>> +
>> + ? ? /* Wait until conversion is ready (ctrl register returns EOC) */
>> + ? ? ret = twl4030_madc_wait_conversion_ready(the_madc, 5, method->ctrl);
> So, you're busy looping with all conversions ? I have no idea how often this
> is called, but putting the caller to sleep on e.g. a waitqueue would be more
> elegant.

I suspect this was done for latency reasons since this is used for
battery charging software.
Mikko can you comment on this?

>> + ? ? if (ret) {
>> + ? ? ? ? ? ? dev_dbg(the_madc->dev, "conversion timeout!\n");
>> + ? ? ? ? ? ? the_madc->requests[req->method].active = 0;
>> + ? ? ? ? ? ? goto out;
>> + ? ? }
>> +
>> + ? ? ret = twl4030_madc_read_channels(the_madc, method->rbase, req->channels,
>> + ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?req->rbuf);
>> +
>> + ? ? the_madc->requests[req->method].active = 0;
>> +
>> + ? ? twl4030_madc_set_power(the_madc, 0);
>> +
>> +out:
>> + ? ? mutex_unlock(&the_madc->lock);
>> +
>> + ? ? return ret;
>> +}
>> +EXPORT_SYMBOL(twl4030_madc_conversion);
> Who's supposed to use this one ?

Nokia AV accessory detection code uses this. So does the BCI battery
driver from TI. The BCI driver has been removed from the omap tree
pending the merge of the madc driver upstream.

>> +static int twl4030_madc_set_current_generator(struct twl4030_madc_data *madc,
>> + ? ? ? ? ? ? int chan, int on)
>> +{
>> + ? ? int ret;
>> + ? ? u8 regval;
>> +
>> + ? ? /* Current generator is only available for ADCIN0 and ADCIN1. NB:
>> + ? ? ?* ADCIN1 current generator only works when AC or VBUS is present */
>> + ? ? if (chan > 1)
>> + ? ? ? ? ? ? return EINVAL;
>> +
>> + ? ? ret = twl4030_i2c_read_u8(TWL4030_MODULE_MAIN_CHARGE,
>> + ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? &regval, TWL4030_BCI_BCICTL1);
>> + ? ? if (ret) {
>> + ? ? ? ? ? ? dev_dbg(madc->dev, "unable to read register 0x%X\n", TWL4030_BCI_BCICTL1);
>> + ? ? ? ? ? ? return ret;
>> + ? ? }
>> +
>> + ? ? if (on) {
>> + ? ? ? ? ? ? regval |= (chan) ? TWL4030_BCI_ITHEN : TWL4030_BCI_TYPEN;
>> + ? ? ? ? ? ? regval |= TWL4030_BCI_MESBAT;
>> + ? ? }
>> + ? ? else {
>> + ? ? ? ? ? ? regval &= (chan) ? ~TWL4030_BCI_ITHEN : ~TWL4030_BCI_TYPEN;
>> + ? ? ? ? ? ? regval &= ~TWL4030_BCI_MESBAT;
>> + ? ? }
>> +
>> + ? ? ret = twl4030_i2c_write_u8(TWL4030_MODULE_MAIN_CHARGE,
>> + ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?regval, TWL4030_BCI_BCICTL1);
>> + ? ? if (ret) {
>> + ? ? ? ? ? ? dev_dbg(madc->dev, "unable to write register 0x%X\n", TWL4030_BCI_BCICTL1);
>> + ? ? }
>> + ? ? return ret;
>> +}
>> +
>> +static int twl4030_madc_set_power(struct twl4030_madc_data *madc, int on)
>> +{
>> + ? ? int ret = 0;
>> + ? ? u8 regval;
>> +
>> + ? ? if (on) {
>> + ? ? ? ? ? ? regval = twl4030_madc_read(madc, TWL4030_MADC_CTRL1);
>> + ? ? ? ? ? ? regval |= TWL4030_MADC_MADCON;
>> + ? ? ? ? ? ? twl4030_madc_write(madc, TWL4030_MADC_CTRL1, regval);
>> +
>> + ? ? ? ? ? ? ret |= twl4030_madc_set_current_generator(madc, 0, 1);
>> +
>> + ? ? } else {
>> + ? ? ? ? ? ? ret |= twl4030_madc_set_current_generator(madc, 0, 0);
>> +
>> + ? ? ? ? ? ? regval = twl4030_madc_read(madc, TWL4030_MADC_CTRL1);
>> + ? ? ? ? ? ? regval &= ~TWL4030_MADC_MADCON;
>> + ? ? ? ? ? ? twl4030_madc_write(madc, TWL4030_MADC_CTRL1, regval);
>> + ? ? }
>> + ? ? return ret;
>> +}
>> +
>> +static long twl4030_madc_ioctl(struct file *filp, unsigned int cmd,
>> + ? ? ? ? ? ? ? ? ? ? ? ? ? ?unsigned long arg)
>> +{
>> + ? ? struct twl4030_madc_user_parms par;
>> + ? ? int val, ret;
>> +
>> + ? ? ret = copy_from_user(&par, (void __user *) arg, sizeof(par));
>> + ? ? if (ret) {
>> + ? ? ? ? ? ? dev_dbg(the_madc->dev, "copy_from_user: %d\n", ret);
>> + ? ? ? ? ? ? return -EACCES;
>> + ? ? }
>> +
>> + ? ? switch (cmd) {
>> + ? ? case TWL4030_MADC_IOCX_ADC_RAW_READ: {
>> + ? ? ? ? ? ? struct twl4030_madc_request req;
>> + ? ? ? ? ? ? if (par.channel >= TWL4030_MADC_MAX_CHANNELS)
>> + ? ? ? ? ? ? ? ? ? ? return -EINVAL;
>> +
>> + ? ? ? ? ? ? req.channels = (1 << par.channel);
>> + ? ? ? ? ? ? req.do_avg ? ? ?= par.average;
>> + ? ? ? ? ? ? req.method ? ? ?= TWL4030_MADC_SW1;
>> + ? ? ? ? ? ? req.func_cb ? ? = NULL;
>> + ? ? ? ? ? ? req.type ? ? ? ?= TWL4030_MADC_WAIT;
>> +
>> + ? ? ? ? ? ? val = twl4030_madc_conversion(&req);
>> + ? ? ? ? ? ? if (likely(val > 0)) {
>> + ? ? ? ? ? ? ? ? ? ? par.status = 0;
>> + ? ? ? ? ? ? ? ? ? ? par.result = (u16)req.rbuf[par.channel];
>> + ? ? ? ? ? ? } else if (val == 0) {
>> + ? ? ? ? ? ? ? ? ? ? par.status = -ENODATA;
>> + ? ? ? ? ? ? } else {
>> + ? ? ? ? ? ? ? ? ? ? par.status = val;
>> + ? ? ? ? ? ? }
>> + ? ? ? ? ? ? break;
>> + ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?}
>> + ? ? default:
>> + ? ? ? ? ? ? return -EINVAL;
>> + ? ? }
>> +
>> + ? ? ret = copy_to_user((void __user *) arg, &par, sizeof(par));
>> + ? ? if (ret) {
>> + ? ? ? ? ? ? dev_dbg(the_madc->dev, "copy_to_user: %d\n", ret);
>> + ? ? ? ? ? ? return -EACCES;
>> + ? ? }
>> +
>> + ? ? return 0;
>> +}
>> +
>> +static struct file_operations twl4030_madc_fileops = {
>> + ? ? .owner = THIS_MODULE,
>> + ? ? .unlocked_ioctl = twl4030_madc_ioctl
>> +};
>> +
>> +static struct miscdevice twl4030_madc_device = {
>> + ? ? .minor = MISC_DYNAMIC_MINOR,
>> + ? ? .name = "twl4030-adc",
>> + ? ? .fops = &twl4030_madc_fileops
>> +};
>> +
>> +static int __init twl4030_madc_probe(struct platform_device *pdev)
>> +{
>> + ? ? struct twl4030_madc_data *madc;
>> + ? ? struct twl4030_madc_platform_data *pdata = pdev->dev.platform_data;
>> +
>> + ? ? int ret;
>> + ? ? u8 regval;
>> +
>> + ? ? madc = kzalloc(sizeof *madc, GFP_KERNEL);
>> + ? ? if (!madc)
>> + ? ? ? ? ? ? return -ENOMEM;
>> +
>> + ? ? if (!pdata) {
>> + ? ? ? ? ? ? dev_dbg(&pdev->dev, "platform_data not available\n");
>> + ? ? ? ? ? ? ret = -EINVAL;
>> + ? ? ? ? ? ? goto err_pdata;
>> + ? ? }
>> +
>> + ? ? madc->imr = (pdata->irq_line == 1) ? TWL4030_MADC_IMR1 : TWL4030_MADC_IMR2;
>> + ? ? madc->isr = (pdata->irq_line == 1) ? TWL4030_MADC_ISR1 : TWL4030_MADC_ISR2;
>> +
>> + ? ? ret = misc_register(&twl4030_madc_device);
>> + ? ? if (ret) {
>> + ? ? ? ? ? ? dev_dbg(&pdev->dev, "could not register misc_device\n");
>> + ? ? ? ? ? ? goto err_misc;
>> + ? ? }
>> +
>> + ? ? ret = request_irq(platform_get_irq(pdev, 0), twl4030_madc_irq_handler,
>> + ? ? ? ? ? ? ? ? ? ? ? 0, "twl4030_madc", madc);
>> + ? ? if (ret) {
>> + ? ? ? ? ? ? dev_dbg(&pdev->dev, "could not request irq\n");
>> + ? ? ? ? ? ? goto err_irq;
>> + ? ? }
>> +
>> + ? ? platform_set_drvdata(pdev, madc);
>> + ? ? mutex_init(&madc->lock);
>> + ? ? INIT_WORK(&madc->ws, twl4030_madc_work);
>> +
>> + ? ? the_madc = madc;
>> +
>> + ? ? return 0;
>> +
>> +err_irq:
>> + ? ? misc_deregister(&twl4030_madc_device);
>> +
>> +err_misc:
>> +err_pdata:
>> + ? ? kfree(madc);
>> +
>> + ? ? return ret;
>> +}
>> +
>> +static int __exit twl4030_madc_remove(struct platform_device *pdev)
>> +{
>> + ? ? struct twl4030_madc_data *madc = platform_get_drvdata(pdev);
>> +
>> + ? ? free_irq(platform_get_irq(pdev, 0), madc);
>> + ? ? cancel_work_sync(&madc->ws);
>> + ? ? misc_deregister(&twl4030_madc_device);
>> +
>> + ? ? return 0;
>> +}
>> +
>> +static struct platform_driver twl4030_madc_driver = {
>> + ? ? .probe ? ? ? ? ?= twl4030_madc_probe,
>> + ? ? .remove ? ? ? ? = __exit_p(twl4030_madc_remove),
>> + ? ? .driver ? ? ? ? = {
>> + ? ? ? ? ? ? .name ? = "twl4030_madc",
>> + ? ? ? ? ? ? .owner ?= THIS_MODULE,
>> + ? ? },
>> +};
>> +
>> +static int __init twl4030_madc_init(void)
>> +{
>> + ? ? return platform_driver_register(&twl4030_madc_driver);
>> +}
>> +module_init(twl4030_madc_init);
>> +
>> +static void __exit twl4030_madc_exit(void)
>> +{
>> + ? ? platform_driver_unregister(&twl4030_madc_driver);
>> +}
>> +module_exit(twl4030_madc_exit);
>> +
>> +MODULE_ALIAS("platform:twl4030-madc");
>> +MODULE_AUTHOR("Nokia Corporation");
>> +MODULE_DESCRIPTION("twl4030 ADC driver");
>> +MODULE_LICENSE("GPL");
>> +
>> diff --git a/include/linux/i2c/twl4030-madc.h b/include/linux/i2c/twl4030-madc.h
>> new file mode 100644
>> index 0000000..24523b5
>> --- /dev/null
>> +++ b/include/linux/i2c/twl4030-madc.h
>> @@ -0,0 +1,126 @@
>> +/*
>> + * include/linux/i2c/twl4030-madc.h
>> + *
>> + * TWL4030 MADC module driver header
>> + *
>> + * Copyright (C) 2008 Nokia Corporation
>> + * Mikko Ylinen <[email protected]>
>> + *
>> + * This program is free software; you can redistribute it and/or
>> + * modify it under the terms of the GNU General Public License
>> + * version 2 as published by the Free Software Foundation.
>> + *
>> + * This program is distributed in the hope that it will be useful, but
>> + * WITHOUT ANY WARRANTY; without even the implied warranty of
>> + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. ?See the GNU
>> + * General Public License for more details.
>> + *
>> + * You should have received a copy of the GNU General Public License
>> + * along with this program; if not, write to the Free Software
>> + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
>> + * 02110-1301 USA
>> + *
>> + */
>> +
>> +#ifndef _TWL4030_MADC_H
>> +#define _TWL4030_MADC_H
>> +
>> +struct twl4030_madc_conversion_method {
>> + ? ? u8 sel;
>> + ? ? u8 avg;
>> + ? ? u8 rbase;
>> + ? ? u8 ctrl;
>> +};
>> +
>> +#define TWL4030_MADC_MAX_CHANNELS 16
>> +
>> +struct twl4030_madc_request {
>> + ? ? u16 channels;
>> + ? ? u16 do_avg;
>> + ? ? u16 method;
>> + ? ? u16 type;
>> + ? ? int active;
>> + ? ? int result_pending;
> active and result_pending can be bool.
>
> Cheers,
> Samuel.
>
> --
> Intel Open Source Technology Centre
> http://oss.intel.com/
>

2009-11-30 17:52:19

by Jean Delvare

[permalink] [raw]
Subject: Re: [PATCH] mfd: twl4030: Driver for twl4030 madc module

On Mon, 30 Nov 2009 15:58:39 +0200, Amit Kucheria wrote:
> On Fri, Nov 27, 2009 at 9:36 PM, Samuel Ortiz <[email protected]> wrote:
> > By the way, have you considered moving this one to drivers/hwmon ?
>
> I haven't. I moved it from i2c/chips/ to the most obvious place I
> could think of - drivers/mfd. But wasn't this the point of mfd - that
> various subcomponents drivers could live there instead of being
> scattered across the driver tree?
>
> Adding Jean to CC, if he would consider taking this driver in drivers/hwmon..

In its current form, no. This driver doesn't implement the sysfs
interface described in Documentation/hwmon/sysfs-interface. It doesn't
even register a hwmon class device. So it doesn't belong in
drivers/hwmon.

--
Jean Delvare

2009-12-01 10:18:07

by Samuel Ortiz

[permalink] [raw]
Subject: Re: [PATCH] mfd: twl4030: Driver for twl4030 madc module

Hi Amit,

On Mon, Nov 30, 2009 at 03:58:39PM +0200, Amit Kucheria wrote:
> >> + * drivers/i2c/chips/twl4030-madc.c
> > drivers/mfd/twl4030-madc.c
> > By the way, have you considered moving this one to drivers/hwmon ?
>
> I haven't. I moved it from i2c/chips/ to the most obvious place I
> could think of - drivers/mfd. But wasn't this the point of mfd - that
> various subcomponents drivers could live there instead of being
> scattered across the driver tree?
Not really. Most of the drivers in mfd/ are for the core parts of the
corresponding chip (chip init and setup, subdevices definitions and
declarations, API export, IRQ setups, etc...).
I can take this driver for now, but converting it to a proper hwmon driver
would make sense because that's what it is after all.


> >> +static struct twl4030_madc_data *the_madc;
> > I dont particularly enjoy that. All of the twl4030 drivers have this bad habit
> > of assuming they will be alone on a platform. Although it's certainly very
> > likely, I still think having this kind of design is not so nice.
>
> I agree. Unfortunately, twl4030-core.c is unable to handle multiple
> devices currently. See note from line 779 in twl4030-core below:
Oh, I know about that. That's also something the code maintainers (Nokia I
assume) of that driver should start looking at.


> >> +static int twl4030_madc_read(struct twl4030_madc_data *madc, u8 reg)
> >> +{
> >> + ? ? int ret;
> >> + ? ? u8 val;
> >> +
> >> + ? ? ret = twl4030_i2c_read_u8(TWL4030_MODULE_MADC, &val, reg);
> >> + ? ? if (ret) {
> >> + ? ? ? ? ? ? dev_dbg(madc->dev, "unable to read register 0x%X\n", reg);
> >> + ? ? ? ? ? ? return ret;
> >> + ? ? }
> >> +
> >> + ? ? return val;
> >> +}
> > FWIW, you're not checking the return value on any of the madc_read calls
> > below.
>
> I've changed the dev_dbg above to dev_err now. If we see those error
> messages, then anything that follows from the higher level functions
> is overhead IMHO.
I usually expect code to check for function return values :) And also exit if
a IO fails.


> The higher level functions in this case aren't adding any more useful
> information to the error. E.g. I could check the return value again in
> twl4030_madc_channel_raw_read() below. But if would simply repeat the
> same error message we show in twl4030_madc_read().
The error message matter less than the code flow itself. For example if
twl4030_madc_start_conversion() fails because of your i2c failing, you will
still busy loop (Yes, only for 5ms, but still) waiting for a register bit to
toggle.

> Hmm, perhaps twl4030_madc_read should return void?
That would be weird, imho. In fact, your write routine returning void is
already weird.


> >> +static void twl4030_madc_work(struct work_struct *ws)
> >> +{
> >> + ? ? const struct twl4030_madc_conversion_method *method;
> >> + ? ? struct twl4030_madc_data *madc;
> >> + ? ? struct twl4030_madc_request *r;
> >> + ? ? int len, i;
> >> +
> >> + ? ? madc = container_of(ws, struct twl4030_madc_data, ws);
> >> + ? ? mutex_lock(&madc->lock);
> >> +
> >> + ? ? for (i = 0; i < TWL4030_MADC_NUM_METHODS; i++) {
> >> +
> >> + ? ? ? ? ? ? r = &madc->requests[i];
> >> +
> >> + ? ? ? ? ? ? /* No pending results for this method, move to next one */
> >> + ? ? ? ? ? ? if (!r->result_pending)
> >> + ? ? ? ? ? ? ? ? ? ? continue;
> >> +
> >> + ? ? ? ? ? ? method = &twl4030_conversion_methods[r->method];
> >> +
> >> + ? ? ? ? ? ? /* Read results */
> >> + ? ? ? ? ? ? len = twl4030_madc_read_channels(madc, method->rbase,
> >> + ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?r->channels, r->rbuf);
> >> +
> >> + ? ? ? ? ? ? /* Return results to caller */
> >> + ? ? ? ? ? ? if (r->func_cb != NULL) {
> >> + ? ? ? ? ? ? ? ? ? ? r->func_cb(len, r->channels, r->rbuf);
> >> + ? ? ? ? ? ? ? ? ? ? r->func_cb = NULL;
> >> + ? ? ? ? ? ? }
> >> +
> >> + ? ? ? ? ? ? /* Free request */
> >> + ? ? ? ? ? ? r->result_pending = 0;
> >> + ? ? ? ? ? ? r->active ? ? ? ? = 0;
> >> + ? ? }
> >> +
> >> + ? ? mutex_unlock(&madc->lock);
> >> +}
> > I think you may want to consider using a threaded irq here, see
> > request_threaded_irq().
>
> I am working on moving the entire twl* driver set to use threaded irqs
> on the side. Will you consider merging this code with the work_struct
> since it is known to work while I work on the conversion?
That's fine, yes. Thanks in advance for the conversion.


> >> +static int twl4030_madc_set_power(struct twl4030_madc_data *madc, int on);
> >> +int twl4030_madc_conversion(struct twl4030_madc_request *req)
> >> +{
> >> + ? ? const struct twl4030_madc_conversion_method *method;
> >> + ? ? u8 ch_msb, ch_lsb;
> >> + ? ? int ret;
> >> +
> >> + ? ? if (unlikely(!req))
> >> + ? ? ? ? ? ? return -EINVAL;
> >> +
> >> + ? ? mutex_lock(&the_madc->lock);
> >> +
> >> + ? ? twl4030_madc_set_power(the_madc, 1);
> >> +
> >> + ? ? /* Do we have a conversion request ongoing */
> >> + ? ? if (the_madc->requests[req->method].active) {
> >> + ? ? ? ? ? ? ret = -EBUSY;
> >> + ? ? ? ? ? ? goto out;
> >> + ? ? }
> >> +
> >> + ? ? ch_msb = (req->channels >> 8) & 0xff;
> >> + ? ? ch_lsb = req->channels & 0xff;
> >> +
> >> + ? ? method = &twl4030_conversion_methods[req->method];
> >> +
> >> + ? ? /* Select channels to be converted */
> >> + ? ? twl4030_madc_write(the_madc, method->sel + 1, ch_msb);
> >> + ? ? twl4030_madc_write(the_madc, method->sel, ch_lsb);
> >> +
> >> + ? ? /* Select averaging for all channels if do_avg is set */
> >> + ? ? if (req->do_avg) {
> >> + ? ? ? ? ? ? twl4030_madc_write(the_madc, method->avg + 1, ch_msb);
> >> + ? ? ? ? ? ? twl4030_madc_write(the_madc, method->avg, ch_lsb);
> >> + ? ? }
> >> +
> >> + ? ? if ((req->type == TWL4030_MADC_IRQ_ONESHOT) && (req->func_cb != NULL)) {
> >> + ? ? ? ? ? ? twl4030_madc_set_irq(the_madc, req);
> >> + ? ? ? ? ? ? twl4030_madc_start_conversion(the_madc, req->method);
> >> + ? ? ? ? ? ? the_madc->requests[req->method].active = 1;
> >> + ? ? ? ? ? ? ret = 0;
> >> + ? ? ? ? ? ? goto out;
> >> + ? ? }
> >> +
> >> + ? ? /* With RT method we should not be here anymore */
> >> + ? ? if (req->method == TWL4030_MADC_RT) {
> >> + ? ? ? ? ? ? ret = -EINVAL;
> >> + ? ? ? ? ? ? goto out;
> >> + ? ? }
> >> +
> >> + ? ? twl4030_madc_start_conversion(the_madc, req->method);
> >> + ? ? the_madc->requests[req->method].active = 1;
> >> +
> >> + ? ? /* Wait until conversion is ready (ctrl register returns EOC) */
> >> + ? ? ret = twl4030_madc_wait_conversion_ready(the_madc, 5, method->ctrl);
> > So, you're busy looping with all conversions ? I have no idea how often this
> > is called, but putting the caller to sleep on e.g. a waitqueue would be more
> > elegant.
>
> I suspect this was done for latency reasons since this is used for
> battery charging software.
> Mikko can you comment on this?
I'd be curious to know, yes :)


> >> +EXPORT_SYMBOL(twl4030_madc_conversion);
> > Who's supposed to use this one ?
>
> Nokia AV accessory detection code uses this. So does the BCI battery
> driver from TI. The BCI driver has been removed from the omap tree
> pending the merge of the madc driver upstream.
Ok, cool then.

> >> +struct twl4030_madc_request {
> >> + ? ? u16 channels;
> >> + ? ? u16 do_avg;
> >> + ? ? u16 method;
> >> + ? ? u16 type;
> >> + ? ? int active;
> >> + ? ? int result_pending;
> > active and result_pending can be bool.
Could you please fix this one as well ?

Cheers,
Samuel.

--
Intel Open Source Technology Centre
http://oss.intel.com/

2010-01-20 12:12:36

by Sergey Lapin

[permalink] [raw]
Subject: Re: [PATCH] mfd: twl4030: Driver for twl4030 madc module

Hi, all!

On Wed, Nov 25, 2009 at 1:47 PM, Amit Kucheria
<[email protected]> wrote:
> From: Mikko Ylinen <[email protected]>
>
> This ADC allows monitoring of analog signals such as battery levels,
> temperatures, etc.
>
> Several people have contributed to this driver on the linux-omap list.
>
> Signed-off-by: Amit Kucheria <[email protected]>
> ---
> ?drivers/mfd/Kconfig ? ? ? ? ? ? ?| ? 21 ++
> ?drivers/mfd/Makefile ? ? ? ? ? ? | ? ?3 +-
> ?drivers/mfd/twl4030-madc.c ? ? ? | ?548 ++++++++++++++++++++++++++++++++++++++
> ?include/linux/i2c/twl4030-madc.h | ?126 +++++++++
> ?4 files changed, 697 insertions(+), 1 deletions(-)
> ?create mode 100644 drivers/mfd/twl4030-madc.c
> ?create mode 100644 include/linux/i2c/twl4030-madc.h
>

We have just tried to adopt this driver to our custom OMAP3 board, but
were unable to get any interrupts.
Any ideas on what is missing? We've checked whole TRM and found
nothing wrong yet :(

Thanks a lot.
S.

2010-01-25 09:34:12

by Janakiram Sistla

[permalink] [raw]
Subject: Re: [PATCH] mfd: twl4030: Driver for twl4030 madc module

> > drivers/mfd/Kconfig | 21 ++
> > drivers/mfd/Makefile | 3 +-
> > drivers/mfd/twl4030-madc.c | 548 ++++++++++++++++++++++++++++++++++++++
> > include/linux/i2c/twl4030-madc.h | 126 +++++++++
> > 4 files changed, 697 insertions(+), 1 deletions(-)
> > create mode 100644 drivers/mfd/twl4030-madc.c
> > create mode 100644 include/linux/i2c/twl4030-madc.h
> >
>
> We have just tried to adopt this driver to our custom OMAP3 board, but
> were unable to get any interrupts.
> Any ideas on what is missing? We've checked whole TRM and found
> nothing wrong yet :(
>
we tried the same driver for Omap zoom platform ,I too dont see any
interrupts from this driver.I also observe that duing bluetooth file
transfer i see the below crash from twl4030-madc driver.

Once i disable the madc driver in kernel configuration my Bluetooth
works fine,while Bluetooth has nothing to do with i2c or madc driver.A
similar crash is also observed during GFX operation

Regards,
Ram.

here is the log:
i2c_omap i2c_omap.1: controller timed out waiting for start condition to finish
twl: i2c_write failed to transfer all messages
Unable to handle kernel NULL pointer dereference at virtual address 00000044
pgd = c0004000
[00000044] *pgd=00000000
Internal error: Oops: 17 [#1] PREEMPT
last sysfs file: /sys/devices/platform/kim/firmware/kim/loading
Modules linked in: bt_drv st_drv
CPU: 0 Not tainted (2.6.32-14922-g86eec44 #1)
PC is at dev_driver_string+0x0/0x38
LR is at twl4030_madc_write+0x2c/0x4c
pc : [<c01f2bc0>] lr : [<c01fede0>] psr: a0000013
sp : cf02bf08 ip : 0000738f fp : 00000000
r10: cf002cc8 r9 : c038a839 r8 : 00000000
r7 : cf02bf3c r6 : c04edff8 r5 : 00000007 r4 : cf227a00
r3 : 00000007 r2 : 02070002 r1 : 00000007 r0 : 00000000
Flags: NzCv IRQs on FIQs on Mode SVC_32 ISA ARM Segment kernel
Control: 10c5387d Table: 8d23c019 DAC: 00000017
Process events/0 (pid: 5, stack limit = 0xcf02a2e8)
Stack: (0xcf02bf08 to 0xcf02c000)
bf00: 00000006 00000002 c04edff8 c01fef6c cf179680 cf179680
bf20: cf02a000 cf002cc0 c028a5a8 00000000 cf002cc8 c028a5dc 00000000 00000200
bf40: cf020001 00000000 cf020d40 00000017 cf0213c0 c007f1dc 00000000 cd277e40
bf60: cf02bf94 c036e58c cd0bac9c cf020ed4 60000013 cf002cc0 cf002cbc cf02a000
bf80: cf002cc0 c01dcc00 cf002cc0 00000000 cf179684 c0076ff4 cf02bfcc 00000000
bfa0: cf020d40 c007a660 cf02bfa8 cf02bfa8 cf023f60 cf02bfd4 cf023f60 cf002cc0
bfc0: c0076e8c 00000000 00000000 c007a334 00000000 00000000 cf02bfd8 cf02bfd8
bfe0: 00000000 00000000 00000000 00000000 00000000 c0035f80 00000000 00000000
[<c01f2bc0>] (dev_driver_string+0x0/0x38) from [<c01fede0>] (twl4030_madc_write+
0x2c/0x4c)
[<c01fede0>] (twl4030_madc_write+0x2c/0x4c) from [<c01fef6c>] (twl4030_madc_conv
ersion+0x74/0x288)
[<c01fef6c>] (twl4030_madc_conversion+0x74/0x288) from [<c028a5dc>] (twl4030_bk_
bci_battery_work+0x34/0x60)
[<c028a5dc>] (twl4030_bk_bci_battery_work+0x34/0x60) from [<c0076ff4>] (worker_t
hread+0x168/0x214)
[<c0076ff4>] (worker_thread+0x168/0x214) from [<c007a334>] (kthread+0x7c/0x84)
[<c007a334>] (kthread+0x7c/0x84) from [<c0035f80>] (kernel_thread_exit+0x0/0x8)
Code: c04edd3c c04eddcc c042bef5 c040d709 (e5903044)

2010-07-19 18:24:45

by Michael Trimarchi

[permalink] [raw]
Subject: Re: [PATCH] mfd: twl4030: Driver for twl4030 madc module

Hi all

Janakiram Sistla wrote:
> On Mon, Jul 19, 2010 at 10:46 AM, Michael Trimarchi <
> [email protected]> wrote:
>> Janakiram Sistla wrote:
>>>>> drivers/mfd/Kconfig | 21 ++
>>>>> drivers/mfd/Makefile | 3 +-
>>>>> drivers/mfd/twl4030-madc.c | 548
>>>>> ++++++++++++++++++++++++++++++++++++++
>>>>> include/linux/i2c/twl4030-madc.h | 126 +++++++++
>>>>> 4 files changed, 697 insertions(+), 1 deletions(-)
>>>>> create mode 100644 drivers/mfd/twl4030-madc.c
>>>>> create mode 100644 include/linux/i2c/twl4030-madc.h
>>>>>
>>>> We have just tried to adopt this driver to our custom OMAP3 board, but
>>>> were unable to get any interrupts.
>>>> Any ideas on what is missing? We've checked whole TRM and found
>>>> nothing wrong yet :(
>>>>
>>> we tried the same driver for Omap zoom platform ,I too dont see any
>>> interrupts from this driver.I also observe that duing bluetooth file
>>> transfer i see the below crash from twl4030-madc driver.
>>>
>>> Once i disable the madc driver in kernel configuration my Bluetooth
>>> works fine,while Bluetooth has nothing to do with i2c or madc driver.A
>>> similar crash is also observed during GFX operation
>>>
>>> Regards,
>>> Ram.
>>>
>>> here is the log:
>>> i2c_omap i2c_omap.1: controller timed out waiting for start condition to
>>> finish
>>> twl: i2c_write failed to transfer all messages
>>> Unable to handle kernel NULL pointer dereference at virtual address
>>> 00000044
>>> pgd = c0004000
>>> [00000044] *pgd=00000000
>>> Internal error: Oops: 17 [#1] PREEMPT
>>> last sysfs file: /sys/devices/platform/kim/firmware/kim/loading
>>> Modules linked in: bt_drv st_drv
>>> CPU: 0 Not tainted (2.6.32-14922-g86eec44 #1)
>>> PC is at dev_driver_string+0x0/0x38
>>> LR is at twl4030_madc_write+0x2c/0x4c
>>> pc : [<c01f2bc0>] lr : [<c01fede0>] psr: a0000013
>>> sp : cf02bf08 ip : 0000738f fp : 00000000
>>> r10: cf002cc8 r9 : c038a839 r8 : 00000000
>>> r7 : cf02bf3c r6 : c04edff8 r5 : 00000007 r4 : cf227a00
>>> r3 : 00000007 r2 : 02070002 r1 : 00000007 r0 : 00000000
>>> Flags: NzCv IRQs on FIQs on Mode SVC_32 ISA ARM Segment kernel
>>> Control: 10c5387d Table: 8d23c019 DAC: 00000017
>>> Process events/0 (pid: 5, stack limit = 0xcf02a2e8)
>>> Stack: (0xcf02bf08 to 0xcf02c000)
>>> bf00: 00000006 00000002 c04edff8 c01fef6c cf179680
>>> cf179680
>>> bf20: cf02a000 cf002cc0 c028a5a8 00000000 cf002cc8 c028a5dc 00000000
>>> 00000200
>>> bf40: cf020001 00000000 cf020d40 00000017 cf0213c0 c007f1dc 00000000
>>> cd277e40
>>> bf60: cf02bf94 c036e58c cd0bac9c cf020ed4 60000013 cf002cc0 cf002cbc
>>> cf02a000
>>> bf80: cf002cc0 c01dcc00 cf002cc0 00000000 cf179684 c0076ff4 cf02bfcc
>>> 00000000
>>> bfa0: cf020d40 c007a660 cf02bfa8 cf02bfa8 cf023f60 cf02bfd4 cf023f60
>>> cf002cc0
>>> bfc0: c0076e8c 00000000 00000000 c007a334 00000000 00000000 cf02bfd8
>>> cf02bfd8
>>> bfe0: 00000000 00000000 00000000 00000000 00000000 c0035f80 00000000
>>> 00000000
>>> [<c01f2bc0>] (dev_driver_string+0x0/0x38) from [<c01fede0>]
>>> (twl4030_madc_write+
>>> 0x2c/0x4c)
>>> [<c01fede0>] (twl4030_madc_write+0x2c/0x4c) from [<c01fef6c>]
>>> (twl4030_madc_conv
>>> ersion+0x74/0x288)
>>> [<c01fef6c>] (twl4030_madc_conversion+0x74/0x288) from [<c028a5dc>]
>>> (twl4030_bk_
>>> bci_battery_work+0x34/0x60)
>>> [<c028a5dc>] (twl4030_bk_bci_battery_work+0x34/0x60) from [<c0076ff4>]
>>> (worker_t
>>> hread+0x168/0x214)
>>> [<c0076ff4>] (worker_thread+0x168/0x214) from [<c007a334>]
>>> (kthread+0x7c/0x84)
>>> [<c007a334>] (kthread+0x7c/0x84) from [<c0035f80>]
>>> (kernel_thread_exit+0x0/0x8)
>>> Code: c04edd3c c04eddcc c042bef5 c040d709 (e5903044)
>>> --
>>> To unsubscribe from this list: send the line "unsubscribe linux-omap" in
>>> the body of a message to [email protected]
>>> More majordomo info at http://vger.kernel.org/majordomo-info.html
>> diff --git a/drivers/mfd/twl4030-madc.c b/drivers/mfd/twl4030-madc.c
>> index 2d2aa87..e12ac1a 100644
>> --- a/drivers/mfd/twl4030-madc.c
>> +++ b/drivers/mfd/twl4030-madc.c
>> @@ -478,6 +478,7 @@ static int __init twl4030_madc_probe(struct
>> platform_device
>> madc->imr = (pdata->irq_line == 1) ? TWL4030_MADC_IMR1 :
>> TWL4030_MADC_IM
>> madc->isr = (pdata->irq_line == 1) ? TWL4030_MADC_ISR1 :
>> TWL4030_MADC_IS
>> + madc->dev = &pdev->dev;
>>
>> ret = misc_register(&twl4030_madc_device);
>> if (ret) {
>>
>> This is the reason of the crash
>>
>> Michael Trimarchi
>>
>>
>>
>
>
> I think its fixed already.. ..
>

I'm testing in overo and I have notice varius issue:

+#if 0
if (dev->rev <= OMAP_I2C_REV_ON_3430) {
while (!(stat & OMAP_I2C_STAT_XU
if (stat & (OMAP_I2C_STA
@@ -826,7 +826,7 @@ complete:
stat = omap_i2c_read_reg
}
}
-
+#endif

I need to comment out this code, because it loop and it doesn't have an exit condition. This couse
the kernel to stuck, but after comment out this I have every 5sec/10 seconds some error on i2c bus
during write

Michael

2010-07-19 18:25:21

by Michael Trimarchi

[permalink] [raw]
Subject: Re: [PATCH] mfd: twl4030: Driver for twl4030 madc module

Janakiram Sistla wrote:
>>> drivers/mfd/Kconfig | 21 ++
>>> drivers/mfd/Makefile | 3 +-
>>> drivers/mfd/twl4030-madc.c | 548 ++++++++++++++++++++++++++++++++++++++
>>> include/linux/i2c/twl4030-madc.h | 126 +++++++++
>>> 4 files changed, 697 insertions(+), 1 deletions(-)
>>> create mode 100644 drivers/mfd/twl4030-madc.c
>>> create mode 100644 include/linux/i2c/twl4030-madc.h
>>>
>> We have just tried to adopt this driver to our custom OMAP3 board, but
>> were unable to get any interrupts.
>> Any ideas on what is missing? We've checked whole TRM and found
>> nothing wrong yet :(
>>
> we tried the same driver for Omap zoom platform ,I too dont see any
> interrupts from this driver.I also observe that duing bluetooth file
> transfer i see the below crash from twl4030-madc driver.
>
> Once i disable the madc driver in kernel configuration my Bluetooth
> works fine,while Bluetooth has nothing to do with i2c or madc driver.A
> similar crash is also observed during GFX operation
>
> Regards,
> Ram.
>
> here is the log:
> i2c_omap i2c_omap.1: controller timed out waiting for start condition to finish
> twl: i2c_write failed to transfer all messages
> Unable to handle kernel NULL pointer dereference at virtual address 00000044
> pgd = c0004000
> [00000044] *pgd=00000000
> Internal error: Oops: 17 [#1] PREEMPT
> last sysfs file: /sys/devices/platform/kim/firmware/kim/loading
> Modules linked in: bt_drv st_drv
> CPU: 0 Not tainted (2.6.32-14922-g86eec44 #1)
> PC is at dev_driver_string+0x0/0x38
> LR is at twl4030_madc_write+0x2c/0x4c
> pc : [<c01f2bc0>] lr : [<c01fede0>] psr: a0000013
> sp : cf02bf08 ip : 0000738f fp : 00000000
> r10: cf002cc8 r9 : c038a839 r8 : 00000000
> r7 : cf02bf3c r6 : c04edff8 r5 : 00000007 r4 : cf227a00
> r3 : 00000007 r2 : 02070002 r1 : 00000007 r0 : 00000000
> Flags: NzCv IRQs on FIQs on Mode SVC_32 ISA ARM Segment kernel
> Control: 10c5387d Table: 8d23c019 DAC: 00000017
> Process events/0 (pid: 5, stack limit = 0xcf02a2e8)
> Stack: (0xcf02bf08 to 0xcf02c000)
> bf00: 00000006 00000002 c04edff8 c01fef6c cf179680 cf179680
> bf20: cf02a000 cf002cc0 c028a5a8 00000000 cf002cc8 c028a5dc 00000000 00000200
> bf40: cf020001 00000000 cf020d40 00000017 cf0213c0 c007f1dc 00000000 cd277e40
> bf60: cf02bf94 c036e58c cd0bac9c cf020ed4 60000013 cf002cc0 cf002cbc cf02a000
> bf80: cf002cc0 c01dcc00 cf002cc0 00000000 cf179684 c0076ff4 cf02bfcc 00000000
> bfa0: cf020d40 c007a660 cf02bfa8 cf02bfa8 cf023f60 cf02bfd4 cf023f60 cf002cc0
> bfc0: c0076e8c 00000000 00000000 c007a334 00000000 00000000 cf02bfd8 cf02bfd8
> bfe0: 00000000 00000000 00000000 00000000 00000000 c0035f80 00000000 00000000
> [<c01f2bc0>] (dev_driver_string+0x0/0x38) from [<c01fede0>] (twl4030_madc_write+
> 0x2c/0x4c)
> [<c01fede0>] (twl4030_madc_write+0x2c/0x4c) from [<c01fef6c>] (twl4030_madc_conv
> ersion+0x74/0x288)
> [<c01fef6c>] (twl4030_madc_conversion+0x74/0x288) from [<c028a5dc>] (twl4030_bk_
> bci_battery_work+0x34/0x60)
> [<c028a5dc>] (twl4030_bk_bci_battery_work+0x34/0x60) from [<c0076ff4>] (worker_t
> hread+0x168/0x214)
> [<c0076ff4>] (worker_thread+0x168/0x214) from [<c007a334>] (kthread+0x7c/0x84)
> [<c007a334>] (kthread+0x7c/0x84) from [<c0035f80>] (kernel_thread_exit+0x0/0x8)
> Code: c04edd3c c04eddcc c042bef5 c040d709 (e5903044)
> --
> To unsubscribe from this list: send the line "unsubscribe linux-omap" in
> the body of a message to [email protected]
> More majordomo info at http://vger.kernel.org/majordomo-info.html
diff --git a/drivers/mfd/twl4030-madc.c b/drivers/mfd/twl4030-madc.c
index 2d2aa87..e12ac1a 100644
--- a/drivers/mfd/twl4030-madc.c
+++ b/drivers/mfd/twl4030-madc.c
@@ -478,6 +478,7 @@ static int __init twl4030_madc_probe(struct platform_device

madc->imr = (pdata->irq_line == 1) ? TWL4030_MADC_IMR1 : TWL4030_MADC_IM
madc->isr = (pdata->irq_line == 1) ? TWL4030_MADC_ISR1 : TWL4030_MADC_IS
+ madc->dev = &pdev->dev;

ret = misc_register(&twl4030_madc_device);
if (ret) {

This is the reason of the crash

Michael Trimarchi


>