summaryrefslogtreecommitdiff
path: root/drivers/iio
diff options
context:
space:
mode:
authorDenis CIOCCA <denis.ciocca@st.com>2013-06-03 15:58:00 +0100
committerJonathan Cameron <jic23@kernel.org>2013-06-05 18:41:23 +0100
commit607a568ab69c5ac345a286267a27294888f8bb5f (patch)
treef52d27c3f06725c7078d6fe876a3a194e45322b4 /drivers/iio
parent762011d6193f8b9af9b491ded87dde3221d0600a (diff)
iio:common: Removed stuff macros, added num_data_channels on st_sensors struct and added support on one-shot sysfs reads to 3 byte channel
This patch introduce num_data_channels variable on st_sensors struct to manage different type of channels (size or number) in st_sensors_get_buffer_element function. Removed ST_SENSORS_NUMBER_DATA_CHANNELS and ST_SENSORS_BYTE_FOR_CHANNEL and used struct iio_chan_spec const *ch to catch data. Added 3 byte channel data support on one-shot reads. Signed-off-by: Denis Ciocca <denis.ciocca@st.com> Signed-off-by: Jonathan Cameron <jic23@kernel.org>
Diffstat (limited to 'drivers/iio')
-rw-r--r--drivers/iio/accel/st_accel_core.c3
-rw-r--r--drivers/iio/common/st_sensors/st_sensors_buffer.c61
-rw-r--r--drivers/iio/common/st_sensors/st_sensors_core.c34
-rw-r--r--drivers/iio/gyro/st_gyro_core.c3
-rw-r--r--drivers/iio/magnetometer/st_magn_core.c3
5 files changed, 73 insertions, 31 deletions
diff --git a/drivers/iio/accel/st_accel_core.c b/drivers/iio/accel/st_accel_core.c
index 40236d5d8753..4aec121261d7 100644
--- a/drivers/iio/accel/st_accel_core.c
+++ b/drivers/iio/accel/st_accel_core.c
@@ -26,6 +26,8 @@
#include <linux/iio/common/st_sensors.h>
#include "st_accel.h"
+#define ST_ACCEL_NUMBER_DATA_CHANNELS 3
+
/* DEFAULT VALUE FOR SENSORS */
#define ST_ACCEL_DEFAULT_OUT_X_L_ADDR 0x28
#define ST_ACCEL_DEFAULT_OUT_Y_L_ADDR 0x2a
@@ -454,6 +456,7 @@ int st_accel_common_probe(struct iio_dev *indio_dev)
if (err < 0)
goto st_accel_common_probe_error;
+ adata->num_data_channels = ST_ACCEL_NUMBER_DATA_CHANNELS;
adata->multiread_bit = adata->sensor->multi_read_bit;
indio_dev->channels = adata->sensor->ch;
indio_dev->num_channels = ST_SENSORS_NUMBER_ALL_CHANNELS;
diff --git a/drivers/iio/common/st_sensors/st_sensors_buffer.c b/drivers/iio/common/st_sensors/st_sensors_buffer.c
index 09b236d6ee89..71a2c5f63b9c 100644
--- a/drivers/iio/common/st_sensors/st_sensors_buffer.c
+++ b/drivers/iio/common/st_sensors/st_sensors_buffer.c
@@ -24,11 +24,20 @@
int st_sensors_get_buffer_element(struct iio_dev *indio_dev, u8 *buf)
{
+ u8 *addr;
int i, n = 0, len;
- u8 addr[ST_SENSORS_NUMBER_DATA_CHANNELS];
struct st_sensor_data *sdata = iio_priv(indio_dev);
+ unsigned int num_data_channels = sdata->num_data_channels;
+ unsigned int byte_for_channel =
+ indio_dev->channels[0].scan_type.storagebits >> 3;
- for (i = 0; i < ST_SENSORS_NUMBER_DATA_CHANNELS; i++) {
+ addr = kmalloc(num_data_channels, GFP_KERNEL);
+ if (!addr) {
+ len = -ENOMEM;
+ goto st_sensors_get_buffer_element_error;
+ }
+
+ for (i = 0; i < num_data_channels; i++) {
if (test_bit(i, indio_dev->active_scan_mask)) {
addr[n] = indio_dev->channels[i].address;
n++;
@@ -37,52 +46,58 @@ int st_sensors_get_buffer_element(struct iio_dev *indio_dev, u8 *buf)
switch (n) {
case 1:
len = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
- addr[0], ST_SENSORS_BYTE_FOR_CHANNEL, buf,
- sdata->multiread_bit);
+ addr[0], byte_for_channel, buf, sdata->multiread_bit);
break;
case 2:
- if ((addr[1] - addr[0]) == ST_SENSORS_BYTE_FOR_CHANNEL) {
+ if ((addr[1] - addr[0]) == byte_for_channel) {
len = sdata->tf->read_multiple_byte(&sdata->tb,
- sdata->dev, addr[0],
- ST_SENSORS_BYTE_FOR_CHANNEL*n,
- buf, sdata->multiread_bit);
+ sdata->dev, addr[0], byte_for_channel * n,
+ buf, sdata->multiread_bit);
} else {
- u8 rx_array[ST_SENSORS_BYTE_FOR_CHANNEL*
- ST_SENSORS_NUMBER_DATA_CHANNELS];
+ u8 *rx_array;
+ rx_array = kmalloc(byte_for_channel * num_data_channels,
+ GFP_KERNEL);
+ if (!rx_array) {
+ len = -ENOMEM;
+ goto st_sensors_free_memory;
+ }
+
len = sdata->tf->read_multiple_byte(&sdata->tb,
sdata->dev, addr[0],
- ST_SENSORS_BYTE_FOR_CHANNEL*
- ST_SENSORS_NUMBER_DATA_CHANNELS,
+ byte_for_channel * num_data_channels,
rx_array, sdata->multiread_bit);
- if (len < 0)
- goto read_data_channels_error;
+ if (len < 0) {
+ kfree(rx_array);
+ goto st_sensors_free_memory;
+ }
- for (i = 0; i < n * ST_SENSORS_NUMBER_DATA_CHANNELS;
- i++) {
+ for (i = 0; i < n * num_data_channels; i++) {
if (i < n)
buf[i] = rx_array[i];
else
buf[i] = rx_array[n + i];
}
- len = ST_SENSORS_BYTE_FOR_CHANNEL*n;
+ kfree(rx_array);
+ len = byte_for_channel * n;
}
break;
case 3:
len = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
- addr[0], ST_SENSORS_BYTE_FOR_CHANNEL*
- ST_SENSORS_NUMBER_DATA_CHANNELS,
+ addr[0], byte_for_channel * num_data_channels,
buf, sdata->multiread_bit);
break;
default:
len = -EINVAL;
- goto read_data_channels_error;
+ goto st_sensors_free_memory;
}
- if (len != ST_SENSORS_BYTE_FOR_CHANNEL*n) {
+ if (len != byte_for_channel * n) {
len = -EIO;
- goto read_data_channels_error;
+ goto st_sensors_free_memory;
}
-read_data_channels_error:
+st_sensors_free_memory:
+ kfree(addr);
+st_sensors_get_buffer_element_error:
return len;
}
EXPORT_SYMBOL(st_sensors_get_buffer_element);
diff --git a/drivers/iio/common/st_sensors/st_sensors_core.c b/drivers/iio/common/st_sensors/st_sensors_core.c
index ed9bc8ae9330..865b1781df66 100644
--- a/drivers/iio/common/st_sensors/st_sensors_core.c
+++ b/drivers/iio/common/st_sensors/st_sensors_core.c
@@ -20,6 +20,11 @@
#define ST_SENSORS_WAI_ADDRESS 0x0f
+static inline u32 st_sensors_get_unaligned_le24(const u8 *p)
+{
+ return ((s32)((p[0] | p[1] << 8 | p[2] << 16) << 8) >> 8);
+}
+
static int st_sensors_write_data_with_mask(struct iio_dev *indio_dev,
u8 reg_addr, u8 mask, u8 data)
{
@@ -112,7 +117,8 @@ st_sensors_match_odr_error:
return ret;
}
-static int st_sensors_set_fullscale(struct iio_dev *indio_dev, unsigned int fs)
+static int st_sensors_set_fullscale(struct iio_dev *indio_dev,
+ unsigned int fs)
{
int err, i = 0;
struct st_sensor_data *sdata = iio_priv(indio_dev);
@@ -273,21 +279,33 @@ st_sensors_match_scale_error:
EXPORT_SYMBOL(st_sensors_set_fullscale_by_gain);
static int st_sensors_read_axis_data(struct iio_dev *indio_dev,
- u8 ch_addr, int *data)
+ struct iio_chan_spec const *ch, int *data)
{
int err;
- u8 outdata[ST_SENSORS_BYTE_FOR_CHANNEL];
+ u8 *outdata;
struct st_sensor_data *sdata = iio_priv(indio_dev);
+ unsigned int byte_for_channel = ch->scan_type.storagebits >> 3;
+
+ outdata = kmalloc(byte_for_channel, GFP_KERNEL);
+ if (!outdata) {
+ err = -EINVAL;
+ goto st_sensors_read_axis_data_error;
+ }
err = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
- ch_addr, ST_SENSORS_BYTE_FOR_CHANNEL,
+ ch->address, byte_for_channel,
outdata, sdata->multiread_bit);
if (err < 0)
- goto read_error;
+ goto st_sensors_free_memory;
- *data = (s16)get_unaligned_le16(outdata);
+ if (byte_for_channel == 2)
+ *data = (s16)get_unaligned_le16(outdata);
+ else if (byte_for_channel == 3)
+ *data = (s32)st_sensors_get_unaligned_le24(outdata);
-read_error:
+st_sensors_free_memory:
+ kfree(outdata);
+st_sensors_read_axis_data_error:
return err;
}
@@ -307,7 +325,7 @@ int st_sensors_read_info_raw(struct iio_dev *indio_dev,
goto read_error;
msleep((sdata->sensor->bootime * 1000) / sdata->odr);
- err = st_sensors_read_axis_data(indio_dev, ch->address, val);
+ err = st_sensors_read_axis_data(indio_dev, ch, val);
if (err < 0)
goto read_error;
diff --git a/drivers/iio/gyro/st_gyro_core.c b/drivers/iio/gyro/st_gyro_core.c
index 9bae46bcccee..f9ed3488c314 100644
--- a/drivers/iio/gyro/st_gyro_core.c
+++ b/drivers/iio/gyro/st_gyro_core.c
@@ -27,6 +27,8 @@
#include <linux/iio/common/st_sensors.h>
#include "st_gyro.h"
+#define ST_GYRO_NUMBER_DATA_CHANNELS 3
+
/* DEFAULT VALUE FOR SENSORS */
#define ST_GYRO_DEFAULT_OUT_X_L_ADDR 0x28
#define ST_GYRO_DEFAULT_OUT_Y_L_ADDR 0x2a
@@ -313,6 +315,7 @@ int st_gyro_common_probe(struct iio_dev *indio_dev)
if (err < 0)
goto st_gyro_common_probe_error;
+ gdata->num_data_channels = ST_GYRO_NUMBER_DATA_CHANNELS;
gdata->multiread_bit = gdata->sensor->multi_read_bit;
indio_dev->channels = gdata->sensor->ch;
indio_dev->num_channels = ST_SENSORS_NUMBER_ALL_CHANNELS;
diff --git a/drivers/iio/magnetometer/st_magn_core.c b/drivers/iio/magnetometer/st_magn_core.c
index 715d61681df3..ebfe8f11a0c2 100644
--- a/drivers/iio/magnetometer/st_magn_core.c
+++ b/drivers/iio/magnetometer/st_magn_core.c
@@ -26,6 +26,8 @@
#include <linux/iio/common/st_sensors.h>
#include "st_magn.h"
+#define ST_MAGN_NUMBER_DATA_CHANNELS 3
+
/* DEFAULT VALUE FOR SENSORS */
#define ST_MAGN_DEFAULT_OUT_X_L_ADDR 0X04
#define ST_MAGN_DEFAULT_OUT_Y_L_ADDR 0X08
@@ -356,6 +358,7 @@ int st_magn_common_probe(struct iio_dev *indio_dev)
if (err < 0)
goto st_magn_common_probe_error;
+ mdata->num_data_channels = ST_MAGN_NUMBER_DATA_CHANNELS;
mdata->multiread_bit = mdata->sensor->multi_read_bit;
indio_dev->channels = mdata->sensor->ch;
indio_dev->num_channels = ST_SENSORS_NUMBER_ALL_CHANNELS;