Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit 607a568a authored by Denis CIOCCA's avatar Denis CIOCCA Committed by Jonathan Cameron
Browse files

iio:common: Removed stuff macros, added num_data_channels on st_sensors struct... 


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: default avatarDenis Ciocca <denis.ciocca@st.com>
Signed-off-by: default avatarJonathan Cameron <jic23@kernel.org>
parent 762011d6

drivers/iio/accel/st_accel_core.c

+3 −0
Original line number Diff line number Diff line
@@ -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;

drivers/iio/common/st_sensors/st_sensors_buffer.c

+38 −23
Original line number Diff line number Diff line
@@ -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,

				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);

drivers/iio/common/st_sensors/st_sensors_core.c

+26 −8
Original line number Diff line number Diff line
@@ -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 @@ static int st_sensors_match_fs(struct st_sensors *sensor, 
	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 @@ int st_sensors_set_fullscale_by_gain(struct iio_dev *indio_dev, int scale) 
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;



	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;

drivers/iio/gyro/st_gyro_core.c

+3 −0
Original line number Diff line number Diff line
@@ -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;

drivers/iio/magnetometer/st_magn_core.c

+3 −0
Original line number Diff line number Diff line
@@ -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;