Commit 872e79ad authored by Denis Ciocca's avatar Denis Ciocca Committed by Jonathan Cameron
Browse files

iio:magnetometer: Add STMicroelectronics magnetometers driver 



This patch adds a generic magnetometer driver for STMicroelectronics
magnetometers, currently it supports:
LSM303DLHC, LSM303DLM, LIS3MDL.

Signed-off-by: default avatarDenis Ciocca <denis.ciocca@st.com>
Signed-off-by: default avatarJonathan Cameron <jic23@kernel.org>
parent 7be56a8f

drivers/iio/magnetometer/Kconfig

+30 −0
Original line number Diff line number Diff line
@@ -14,4 +14,34 @@ config HID_SENSOR_MAGNETOMETER_3D 
	  Say yes here to build support for the HID SENSOR

	  Magnetometer 3D.



config IIO_ST_MAGN_3AXIS

	tristate "STMicroelectronics magnetometers 3-Axis Driver"

	depends on (I2C || SPI_MASTER) && SYSFS

	select IIO_ST_SENSORS_CORE

	select IIO_ST_MAGN_I2C_3AXIS if (I2C)

	select IIO_ST_MAGN_SPI_3AXIS if (SPI_MASTER)

	select IIO_TRIGGERED_BUFFER if (IIO_BUFFER)

	select IIO_ST_MAGN_BUFFER if (IIO_TRIGGERED_BUFFER)

	help

	  Say yes here to build support for STMicroelectronics magnetometers:

	  LSM303DLHC, LSM303DLM, LIS3MDL.



	  This driver can also be built as a module. If so, will be created

	  these modules:

	  - st_magn (core functions for the driver [it is mandatory]);

	  - st_magn_i2c (necessary for the I2C devices [optional*]);

	  - st_magn_spi (necessary for the SPI devices [optional*]);



	  (*) one of these is necessary to do something.



config IIO_ST_MAGN_I2C_3AXIS

	tristate

	depends on IIO_ST_MAGN_3AXIS

	depends on IIO_ST_SENSORS_I2C



config IIO_ST_MAGN_SPI_3AXIS

	tristate

	depends on IIO_ST_MAGN_3AXIS

	depends on IIO_ST_SENSORS_SPI



endmenu

drivers/iio/magnetometer/Makefile

+7 −0
Original line number Diff line number Diff line
@@ -3,3 +3,10 @@ 
#



obj-$(CONFIG_HID_SENSOR_MAGNETOMETER_3D) += hid-sensor-magn-3d.o



obj-$(CONFIG_IIO_ST_MAGN_3AXIS) += st_magn.o

st_magn-y := st_magn_core.o

st_magn-$(CONFIG_IIO_BUFFER) += st_magn_buffer.o



obj-$(CONFIG_IIO_ST_MAGN_I2C_3AXIS) += st_magn_i2c.o

obj-$(CONFIG_IIO_ST_MAGN_SPI_3AXIS) += st_magn_spi.o

drivers/iio/magnetometer/st_magn.h

0 → 100644
+45 −0
Original line number Diff line number Diff line 
/*

 * STMicroelectronics magnetometers driver

 *

 * Copyright 2012-2013 STMicroelectronics Inc.

 *

 * Denis Ciocca <denis.ciocca@st.com>

 * v. 1.0.0

 * Licensed under the GPL-2.

 */



#ifndef ST_MAGN_H

#define ST_MAGN_H



#include <linux/types.h>

#include <linux/iio/common/st_sensors.h>



#define LSM303DLHC_MAGN_DEV_NAME	"lsm303dlhc_magn"

#define LSM303DLM_MAGN_DEV_NAME		"lsm303dlm_magn"

#define LIS3MDL_MAGN_DEV_NAME		"lis3mdl"



int st_magn_common_probe(struct iio_dev *indio_dev);

void st_magn_common_remove(struct iio_dev *indio_dev);



#ifdef CONFIG_IIO_BUFFER

int st_magn_allocate_ring(struct iio_dev *indio_dev);

void st_magn_deallocate_ring(struct iio_dev *indio_dev);

#else /* CONFIG_IIO_BUFFER */

static inline int st_magn_probe_trigger(struct iio_dev *indio_dev, int irq)

{

	return 0;

}

static inline void st_magn_remove_trigger(struct iio_dev *indio_dev, int irq)

{

	return;

}

static inline int st_magn_allocate_ring(struct iio_dev *indio_dev)

{

	return 0;

}

static inline void st_magn_deallocate_ring(struct iio_dev *indio_dev)

{

}

#endif /* CONFIG_IIO_BUFFER */



#endif /* ST_MAGN_H */

drivers/iio/magnetometer/st_magn_buffer.c

0 → 100644
+98 −0
Original line number Diff line number Diff line 
/*

 * STMicroelectronics magnetometers driver

 *

 * Copyright 2012-2013 STMicroelectronics Inc.

 *

 * Denis Ciocca <denis.ciocca@st.com>

 *

 * Licensed under the GPL-2.

 */



#include <linux/module.h>

#include <linux/kernel.h>

#include <linux/slab.h>

#include <linux/stat.h>

#include <linux/interrupt.h>

#include <linux/i2c.h>

#include <linux/delay.h>

#include <linux/iio/iio.h>

#include <linux/iio/buffer.h>

#include <linux/iio/trigger_consumer.h>

#include <linux/iio/triggered_buffer.h>



#include <linux/iio/common/st_sensors.h>

#include "st_magn.h"



static int st_magn_buffer_preenable(struct iio_dev *indio_dev)

{

	int err;



	err = st_sensors_set_enable(indio_dev, true);

	if (err < 0)

		goto st_magn_set_enable_error;



	err = iio_sw_buffer_preenable(indio_dev);



st_magn_set_enable_error:

	return err;

}



static int st_magn_buffer_postenable(struct iio_dev *indio_dev)

{

	int err;

	struct st_sensor_data *mdata = iio_priv(indio_dev);



	mdata->buffer_data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);

	if (mdata->buffer_data == NULL) {

		err = -ENOMEM;

		goto allocate_memory_error;

	}



	err = iio_triggered_buffer_postenable(indio_dev);

	if (err < 0)

		goto st_magn_buffer_postenable_error;



	return err;



st_magn_buffer_postenable_error:

	kfree(mdata->buffer_data);

allocate_memory_error:

	return err;

}



static int st_magn_buffer_predisable(struct iio_dev *indio_dev)

{

	int err;

	struct st_sensor_data *mdata = iio_priv(indio_dev);



	err = iio_triggered_buffer_predisable(indio_dev);

	if (err < 0)

		goto st_magn_buffer_predisable_error;



	err = st_sensors_set_enable(indio_dev, false);



st_magn_buffer_predisable_error:

	kfree(mdata->buffer_data);

	return err;

}



static const struct iio_buffer_setup_ops st_magn_buffer_setup_ops = {

	.preenable = &st_magn_buffer_preenable,

	.postenable = &st_magn_buffer_postenable,

	.predisable = &st_magn_buffer_predisable,

};



int st_magn_allocate_ring(struct iio_dev *indio_dev)

{

	return iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,

		&st_sensors_trigger_handler, &st_magn_buffer_setup_ops);

}



void st_magn_deallocate_ring(struct iio_dev *indio_dev)

{

	iio_triggered_buffer_cleanup(indio_dev);

}



MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");

MODULE_DESCRIPTION("STMicroelectronics magnetometers buffer");

MODULE_LICENSE("GPL v2");

drivers/iio/magnetometer/st_magn_core.c

0 → 100644
+401 −0
Original line number Diff line number Diff line 
/*

 * STMicroelectronics magnetometers driver

 *

 * Copyright 2012-2013 STMicroelectronics Inc.

 *

 * Denis Ciocca <denis.ciocca@st.com>

 *

 * Licensed under the GPL-2.

 */



#include <linux/kernel.h>

#include <linux/module.h>

#include <linux/slab.h>

#include <linux/errno.h>

#include <linux/types.h>

#include <linux/mutex.h>

#include <linux/interrupt.h>

#include <linux/i2c.h>

#include <linux/gpio.h>

#include <linux/irq.h>

#include <linux/delay.h>

#include <linux/iio/iio.h>

#include <linux/iio/sysfs.h>

#include <linux/iio/trigger_consumer.h>

#include <linux/iio/buffer.h>



#include <linux/iio/common/st_sensors.h>

#include "st_magn.h"



/* DEFAULT VALUE FOR SENSORS */

#define ST_MAGN_DEFAULT_OUT_X_L_ADDR		0X04

#define ST_MAGN_DEFAULT_OUT_Y_L_ADDR		0X08

#define ST_MAGN_DEFAULT_OUT_Z_L_ADDR		0X06



/* FULLSCALE */

#define ST_MAGN_FS_AVL_1300MG			1300

#define ST_MAGN_FS_AVL_1900MG			1900

#define ST_MAGN_FS_AVL_2500MG			2500

#define ST_MAGN_FS_AVL_4000MG			4000

#define ST_MAGN_FS_AVL_4700MG			4700

#define ST_MAGN_FS_AVL_5600MG			5600

#define ST_MAGN_FS_AVL_8000MG			8000

#define ST_MAGN_FS_AVL_8100MG			8100

#define ST_MAGN_FS_AVL_10000MG			10000



/* CUSTOM VALUES FOR SENSOR 1 */

#define ST_MAGN_1_WAI_EXP			0x3c

#define ST_MAGN_1_ODR_ADDR			0x00

#define ST_MAGN_1_ODR_MASK			0x1c

#define ST_MAGN_1_ODR_AVL_1HZ_VAL		0x00

#define ST_MAGN_1_ODR_AVL_2HZ_VAL		0x01

#define ST_MAGN_1_ODR_AVL_3HZ_VAL		0x02

#define ST_MAGN_1_ODR_AVL_8HZ_VAL		0x03

#define ST_MAGN_1_ODR_AVL_15HZ_VAL		0x04

#define ST_MAGN_1_ODR_AVL_30HZ_VAL		0x05

#define ST_MAGN_1_ODR_AVL_75HZ_VAL		0x06

#define ST_MAGN_1_ODR_AVL_220HZ_VAL		0x07

#define ST_MAGN_1_PW_ADDR			0x02

#define ST_MAGN_1_PW_MASK			0x03

#define ST_MAGN_1_PW_ON				0x00

#define ST_MAGN_1_PW_OFF			0x03

#define ST_MAGN_1_FS_ADDR			0x01

#define ST_MAGN_1_FS_MASK			0xe0

#define ST_MAGN_1_FS_AVL_1300_VAL		0x01

#define ST_MAGN_1_FS_AVL_1900_VAL		0x02

#define ST_MAGN_1_FS_AVL_2500_VAL		0x03

#define ST_MAGN_1_FS_AVL_4000_VAL		0x04

#define ST_MAGN_1_FS_AVL_4700_VAL		0x05

#define ST_MAGN_1_FS_AVL_5600_VAL		0x06

#define ST_MAGN_1_FS_AVL_8100_VAL		0x07

#define ST_MAGN_1_FS_AVL_1300_GAIN_XY		1100

#define ST_MAGN_1_FS_AVL_1900_GAIN_XY		855

#define ST_MAGN_1_FS_AVL_2500_GAIN_XY		670

#define ST_MAGN_1_FS_AVL_4000_GAIN_XY		450

#define ST_MAGN_1_FS_AVL_4700_GAIN_XY		400

#define ST_MAGN_1_FS_AVL_5600_GAIN_XY		330

#define ST_MAGN_1_FS_AVL_8100_GAIN_XY		230

#define ST_MAGN_1_FS_AVL_1300_GAIN_Z		980

#define ST_MAGN_1_FS_AVL_1900_GAIN_Z		760

#define ST_MAGN_1_FS_AVL_2500_GAIN_Z		600

#define ST_MAGN_1_FS_AVL_4000_GAIN_Z		400

#define ST_MAGN_1_FS_AVL_4700_GAIN_Z		355

#define ST_MAGN_1_FS_AVL_5600_GAIN_Z		295

#define ST_MAGN_1_FS_AVL_8100_GAIN_Z		205

#define ST_MAGN_1_MULTIREAD_BIT			false



/* CUSTOM VALUES FOR SENSOR 2 */

#define ST_MAGN_2_WAI_EXP			0x3d

#define ST_MAGN_2_ODR_ADDR			0x20

#define ST_MAGN_2_ODR_MASK			0x1c

#define ST_MAGN_2_ODR_AVL_1HZ_VAL		0x00

#define ST_MAGN_2_ODR_AVL_2HZ_VAL		0x01

#define ST_MAGN_2_ODR_AVL_3HZ_VAL		0x02

#define ST_MAGN_2_ODR_AVL_5HZ_VAL		0x03

#define ST_MAGN_2_ODR_AVL_10HZ_VAL		0x04

#define ST_MAGN_2_ODR_AVL_20HZ_VAL		0x05

#define ST_MAGN_2_ODR_AVL_40HZ_VAL		0x06

#define ST_MAGN_2_ODR_AVL_80HZ_VAL		0x07

#define ST_MAGN_2_PW_ADDR			0x22

#define ST_MAGN_2_PW_MASK			0x03

#define ST_MAGN_2_PW_ON				0x00

#define ST_MAGN_2_PW_OFF			0x03

#define ST_MAGN_2_FS_ADDR			0x21

#define ST_MAGN_2_FS_MASK			0x60

#define ST_MAGN_2_FS_AVL_4000_VAL		0x00

#define ST_MAGN_2_FS_AVL_8000_VAL		0x01

#define ST_MAGN_2_FS_AVL_10000_VAL		0x02

#define ST_MAGN_2_FS_AVL_4000_GAIN		430

#define ST_MAGN_2_FS_AVL_8000_GAIN		230

#define ST_MAGN_2_FS_AVL_10000_GAIN		230

#define ST_MAGN_2_MULTIREAD_BIT			false

#define ST_MAGN_2_OUT_X_L_ADDR			0x28

#define ST_MAGN_2_OUT_Y_L_ADDR			0x2a

#define ST_MAGN_2_OUT_Z_L_ADDR			0x2c



static const struct iio_chan_spec st_magn_16bit_channels[] = {

	ST_SENSORS_LSM_CHANNELS(IIO_MAGN, ST_SENSORS_SCAN_X, IIO_MOD_X, IIO_LE,

		ST_SENSORS_DEFAULT_16_REALBITS, ST_MAGN_DEFAULT_OUT_X_L_ADDR),

	ST_SENSORS_LSM_CHANNELS(IIO_MAGN, ST_SENSORS_SCAN_Y, IIO_MOD_Y, IIO_LE,

		ST_SENSORS_DEFAULT_16_REALBITS, ST_MAGN_DEFAULT_OUT_Y_L_ADDR),

	ST_SENSORS_LSM_CHANNELS(IIO_MAGN, ST_SENSORS_SCAN_Z, IIO_MOD_Z, IIO_LE,

		ST_SENSORS_DEFAULT_16_REALBITS, ST_MAGN_DEFAULT_OUT_Z_L_ADDR),

	IIO_CHAN_SOFT_TIMESTAMP(3)

};



static const struct iio_chan_spec st_magn_2_16bit_channels[] = {

	ST_SENSORS_LSM_CHANNELS(IIO_MAGN, ST_SENSORS_SCAN_X, IIO_MOD_X, IIO_LE,

		ST_SENSORS_DEFAULT_16_REALBITS, ST_MAGN_2_OUT_X_L_ADDR),

	ST_SENSORS_LSM_CHANNELS(IIO_MAGN, ST_SENSORS_SCAN_Y, IIO_MOD_Y, IIO_LE,

		ST_SENSORS_DEFAULT_16_REALBITS, ST_MAGN_2_OUT_Y_L_ADDR),

	ST_SENSORS_LSM_CHANNELS(IIO_MAGN, ST_SENSORS_SCAN_Z, IIO_MOD_Z, IIO_LE,

		ST_SENSORS_DEFAULT_16_REALBITS, ST_MAGN_2_OUT_Z_L_ADDR),

	IIO_CHAN_SOFT_TIMESTAMP(3)

};



static const struct st_sensors st_magn_sensors[] = {

	{

		.wai = ST_MAGN_1_WAI_EXP,

		.sensors_supported = {

			[0] = LSM303DLHC_MAGN_DEV_NAME,

			[1] = LSM303DLM_MAGN_DEV_NAME,

		},

		.ch = (struct iio_chan_spec *)st_magn_16bit_channels,

		.odr = {

			.addr = ST_MAGN_1_ODR_ADDR,

			.mask = ST_MAGN_1_ODR_MASK,

			.odr_avl = {

				{ 1, ST_MAGN_1_ODR_AVL_1HZ_VAL, },

				{ 2, ST_MAGN_1_ODR_AVL_2HZ_VAL, },

				{ 3, ST_MAGN_1_ODR_AVL_3HZ_VAL, },

				{ 8, ST_MAGN_1_ODR_AVL_8HZ_VAL, },

				{ 15, ST_MAGN_1_ODR_AVL_15HZ_VAL, },

				{ 30, ST_MAGN_1_ODR_AVL_30HZ_VAL, },

				{ 75, ST_MAGN_1_ODR_AVL_75HZ_VAL, },

				{ 220, ST_MAGN_1_ODR_AVL_220HZ_VAL, },

			},

		},

		.pw = {

			.addr = ST_MAGN_1_PW_ADDR,

			.mask = ST_MAGN_1_PW_MASK,

			.value_on = ST_MAGN_1_PW_ON,

			.value_off = ST_MAGN_1_PW_OFF,

		},

		.fs = {

			.addr = ST_MAGN_1_FS_ADDR,

			.mask = ST_MAGN_1_FS_MASK,

			.fs_avl = {

				[0] = {

					.num = ST_MAGN_FS_AVL_1300MG,

					.value = ST_MAGN_1_FS_AVL_1300_VAL,

					.gain = ST_MAGN_1_FS_AVL_1300_GAIN_XY,

					.gain2 = ST_MAGN_1_FS_AVL_1300_GAIN_Z,

				},

				[1] = {

					.num = ST_MAGN_FS_AVL_1900MG,

					.value = ST_MAGN_1_FS_AVL_1900_VAL,

					.gain = ST_MAGN_1_FS_AVL_1900_GAIN_XY,

					.gain2 = ST_MAGN_1_FS_AVL_1900_GAIN_Z,

				},

				[2] = {

					.num = ST_MAGN_FS_AVL_2500MG,

					.value = ST_MAGN_1_FS_AVL_2500_VAL,

					.gain = ST_MAGN_1_FS_AVL_2500_GAIN_XY,

					.gain2 = ST_MAGN_1_FS_AVL_2500_GAIN_Z,

				},

				[3] = {

					.num = ST_MAGN_FS_AVL_4000MG,

					.value = ST_MAGN_1_FS_AVL_4000_VAL,

					.gain = ST_MAGN_1_FS_AVL_4000_GAIN_XY,

					.gain2 = ST_MAGN_1_FS_AVL_4000_GAIN_Z,

				},

				[4] = {

					.num = ST_MAGN_FS_AVL_4700MG,

					.value = ST_MAGN_1_FS_AVL_4700_VAL,

					.gain = ST_MAGN_1_FS_AVL_4700_GAIN_XY,

					.gain2 = ST_MAGN_1_FS_AVL_4700_GAIN_Z,

				},

				[5] = {

					.num = ST_MAGN_FS_AVL_5600MG,

					.value = ST_MAGN_1_FS_AVL_5600_VAL,

					.gain = ST_MAGN_1_FS_AVL_5600_GAIN_XY,

					.gain2 = ST_MAGN_1_FS_AVL_5600_GAIN_Z,

				},

				[6] = {

					.num = ST_MAGN_FS_AVL_8100MG,

					.value = ST_MAGN_1_FS_AVL_8100_VAL,

					.gain = ST_MAGN_1_FS_AVL_8100_GAIN_XY,

					.gain2 = ST_MAGN_1_FS_AVL_8100_GAIN_Z,

				},

			},

		},

		.multi_read_bit = ST_MAGN_1_MULTIREAD_BIT,

		.bootime = 2,

	},

	{

		.wai = ST_MAGN_2_WAI_EXP,

		.sensors_supported = {

			[0] = LIS3MDL_MAGN_DEV_NAME,

		},

		.ch = (struct iio_chan_spec *)st_magn_2_16bit_channels,

		.odr = {

			.addr = ST_MAGN_2_ODR_ADDR,

			.mask = ST_MAGN_2_ODR_MASK,

			.odr_avl = {

				{ 1, ST_MAGN_2_ODR_AVL_1HZ_VAL, },

				{ 2, ST_MAGN_2_ODR_AVL_2HZ_VAL, },

				{ 3, ST_MAGN_2_ODR_AVL_3HZ_VAL, },

				{ 5, ST_MAGN_2_ODR_AVL_5HZ_VAL, },

				{ 10, ST_MAGN_2_ODR_AVL_10HZ_VAL, },

				{ 20, ST_MAGN_2_ODR_AVL_20HZ_VAL, },

				{ 40, ST_MAGN_2_ODR_AVL_40HZ_VAL, },

				{ 80, ST_MAGN_2_ODR_AVL_80HZ_VAL, },

			},

		},

		.pw = {

			.addr = ST_MAGN_2_PW_ADDR,

			.mask = ST_MAGN_2_PW_MASK,

			.value_on = ST_MAGN_2_PW_ON,

			.value_off = ST_MAGN_2_PW_OFF,

		},

		.fs = {

			.addr = ST_MAGN_2_FS_ADDR,

			.mask = ST_MAGN_2_FS_MASK,

			.fs_avl = {

				[0] = {

					.num = ST_MAGN_FS_AVL_4000MG,

					.value = ST_MAGN_2_FS_AVL_4000_VAL,

					.gain = ST_MAGN_2_FS_AVL_4000_GAIN,

				},

				[1] = {

					.num = ST_MAGN_FS_AVL_8000MG,

					.value = ST_MAGN_2_FS_AVL_8000_VAL,

					.gain = ST_MAGN_2_FS_AVL_8000_GAIN,

				},

				[2] = {

					.num = ST_MAGN_FS_AVL_10000MG,

					.value = ST_MAGN_2_FS_AVL_10000_VAL,

					.gain = ST_MAGN_2_FS_AVL_10000_GAIN,

				},

			},

		},

		.multi_read_bit = ST_MAGN_2_MULTIREAD_BIT,

		.bootime = 2,

	},

};



static int st_magn_read_raw(struct iio_dev *indio_dev,

			struct iio_chan_spec const *ch, int *val,

							int *val2, long mask)

{

	int err;

	struct st_sensor_data *mdata = iio_priv(indio_dev);



	switch (mask) {

	case IIO_CHAN_INFO_RAW:

		err = st_sensors_read_info_raw(indio_dev, ch, val);

		if (err < 0)

			goto read_error;



		return IIO_VAL_INT;

	case IIO_CHAN_INFO_SCALE:

		*val = 0;

		if ((ch->scan_index == ST_SENSORS_SCAN_Z) &&

					(mdata->current_fullscale->gain2 != 0))

			*val2 = mdata->current_fullscale->gain2;

		else

			*val2 = mdata->current_fullscale->gain;

		return IIO_VAL_INT_PLUS_MICRO;

	default:

		return -EINVAL;

	}



read_error:

	return err;

}



static int st_magn_write_raw(struct iio_dev *indio_dev,

		struct iio_chan_spec const *chan, int val, int val2, long mask)

{

	int err;



	switch (mask) {

	case IIO_CHAN_INFO_SCALE:

		err = st_sensors_set_fullscale_by_gain(indio_dev, val2);

		break;

	default:

		err = -EINVAL;

	}



	return err;

}



static ST_SENSOR_DEV_ATTR_SAMP_FREQ();

static ST_SENSORS_DEV_ATTR_SAMP_FREQ_AVAIL();

static ST_SENSORS_DEV_ATTR_SCALE_AVAIL(in_magn_scale_available);



static struct attribute *st_magn_attributes[] = {

	&iio_dev_attr_sampling_frequency_available.dev_attr.attr,

	&iio_dev_attr_in_magn_scale_available.dev_attr.attr,

	&iio_dev_attr_sampling_frequency.dev_attr.attr,

	NULL,

};



static const struct attribute_group st_magn_attribute_group = {

	.attrs = st_magn_attributes,

};



static const struct iio_info magn_info = {

	.driver_module = THIS_MODULE,

	.attrs = &st_magn_attribute_group,

	.read_raw = &st_magn_read_raw,

	.write_raw = &st_magn_write_raw,

};



int st_magn_common_probe(struct iio_dev *indio_dev)

{

	int err;

	struct st_sensor_data *mdata = iio_priv(indio_dev);



	indio_dev->modes = INDIO_DIRECT_MODE;

	indio_dev->info = &magn_info;



	err = st_sensors_check_device_support(indio_dev,

				ARRAY_SIZE(st_magn_sensors), st_magn_sensors);

	if (err < 0)

		goto st_magn_common_probe_error;



	mdata->multiread_bit = mdata->sensor->multi_read_bit;

	indio_dev->channels = mdata->sensor->ch;

	indio_dev->num_channels = ST_SENSORS_NUMBER_ALL_CHANNELS;



	mdata->current_fullscale = (struct st_sensor_fullscale_avl *)

						&mdata->sensor->fs.fs_avl[0];

	mdata->odr = mdata->sensor->odr.odr_avl[0].hz;



	err = st_sensors_init_sensor(indio_dev);

	if (err < 0)

		goto st_magn_common_probe_error;



	if (mdata->get_irq_data_ready(indio_dev) > 0) {

		err = st_magn_allocate_ring(indio_dev);

		if (err < 0)

			goto st_magn_common_probe_error;

		err = st_sensors_allocate_trigger(indio_dev, NULL);

		if (err < 0)

			goto st_magn_probe_trigger_error;

	}



	err = iio_device_register(indio_dev);

	if (err)

		goto st_magn_device_register_error;



	return err;



st_magn_device_register_error:

	if (mdata->get_irq_data_ready(indio_dev) > 0)

		st_sensors_deallocate_trigger(indio_dev);

st_magn_probe_trigger_error:

	if (mdata->get_irq_data_ready(indio_dev) > 0)

		st_magn_deallocate_ring(indio_dev);

st_magn_common_probe_error:

	return err;

}

EXPORT_SYMBOL(st_magn_common_probe);



void st_magn_common_remove(struct iio_dev *indio_dev)

{

	struct st_sensor_data *mdata = iio_priv(indio_dev);



	iio_device_unregister(indio_dev);

	if (mdata->get_irq_data_ready(indio_dev) > 0) {

		st_sensors_deallocate_trigger(indio_dev);

		st_magn_deallocate_ring(indio_dev);

	}

	iio_device_free(indio_dev);

}

EXPORT_SYMBOL(st_magn_common_remove);



MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");

MODULE_DESCRIPTION("STMicroelectronics magnetometers driver");

MODULE_LICENSE("GPL v2");

CRA Git | Maintained and supported by SUSTech CRA and CCSE