diff --git a/drivers/iio/magnetometer/Kconfig b/drivers/iio/magnetometer/Kconfig
index 1697a8c03506..bfd2866faa99 100644
--- a/drivers/iio/magnetometer/Kconfig
+++ b/drivers/iio/magnetometer/Kconfig
@@ -205,4 +205,27 @@ config SENSORS_RM3100_SPI
 	  To compile this driver as a module, choose M here: the module
 	  will be called rm3100-spi.
 
+config ST_MAG40_IIO
+	tristate "STMicroelectronics LIS2MDL/LSM303AH/LSM303AGR/ISM303DAC/IIS2MDC sensor"
+	depends on (I2C || SPI) && SYSFS
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	select ST_MAG40_I2C_IIO if (I2C)
+	select ST_MAG40_SPI_IIO if (SPI)
+	help
+	  Say yes here to build support for STMicroelectronics magnetometers:
+	  LIS2MDL, LSM303AH, LSM303AGR, ISM303DAC, IIS2MDC.
+
+	  To compile this driver as a module, choose M here. The module
+	  will be called st_mag40.
+
+config ST_MAG40_I2C_IIO
+	tristate
+	depends on ST_MAG40_IIO
+	depends on I2C
+
+config ST_MAG40_SPI_IIO
+	tristate
+	depends on ST_MAG40_IIO
+	depends on SPI
 endmenu
diff --git a/drivers/iio/magnetometer/Makefile b/drivers/iio/magnetometer/Makefile
index ba1bc34b82fa..b6b427cfc284 100644
--- a/drivers/iio/magnetometer/Makefile
+++ b/drivers/iio/magnetometer/Makefile
@@ -25,6 +25,10 @@ obj-$(CONFIG_SENSORS_HMC5843)		+= hmc5843_core.o
 obj-$(CONFIG_SENSORS_HMC5843_I2C)	+= hmc5843_i2c.o
 obj-$(CONFIG_SENSORS_HMC5843_SPI)	+= hmc5843_spi.o
 
+st_mag40-y += st_mag40_buffer.o st_mag40_core.o
+obj-$(CONFIG_ST_MAG40_IIO) += st_mag40.o
+obj-$(CONFIG_ST_MAG40_I2C_IIO) += st_mag40_i2c.o
+obj-$(CONFIG_ST_MAG40_SPI_IIO) += st_mag40_spi.o
 obj-$(CONFIG_SENSORS_RM3100)		+= rm3100-core.o
 obj-$(CONFIG_SENSORS_RM3100_I2C)	+= rm3100-i2c.o
 obj-$(CONFIG_SENSORS_RM3100_SPI)	+= rm3100-spi.o
diff --git a/drivers/iio/magnetometer/st_mag40_buffer.c b/drivers/iio/magnetometer/st_mag40_buffer.c
new file mode 100644
index 000000000000..d2a67c9dae5e
--- /dev/null
+++ b/drivers/iio/magnetometer/st_mag40_buffer.c
@@ -0,0 +1,191 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * STMicroelectronics st_mag40 driver
+ *
+ * Copyright 2016 STMicroelectronics Inc.
+ *
+ * Matteo Dameno <matteo.dameno@st.com>
+ * Armando Visconti <armando.visconti@st.com>
+ * Lorenzo Bianconi <lorenzo.bianconi@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/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 "st_mag40_core.h"
+
+#define ST_MAG40_EWMA_DIV			128
+static inline s64 st_mag40_ewma(s64 old, s64 new, int weight)
+{
+	s64 diff, incr;
+
+	diff = new - old;
+	incr = div_s64((ST_MAG40_EWMA_DIV - weight) * diff,
+			ST_MAG40_EWMA_DIV);
+
+	return old + incr;
+}
+
+static irqreturn_t st_mag40_trigger_irq_handler(int irq, void *private)
+{
+	struct st_mag40_data *cdata = private;
+	s64 ts;
+	u8 weight = (cdata->odr >= 50) ? 96 : 0;
+
+	ts = st_mag40_get_timestamp();
+	cdata->delta_ts = st_mag40_ewma(cdata->delta_ts, ts - cdata->ts_irq, weight);
+	cdata->ts_irq = ts;
+
+	return IRQ_WAKE_THREAD;
+}
+
+static irqreturn_t st_mag40_trigger_thread_handler(int irq, void *private)
+{
+	struct st_mag40_data *cdata = private;
+	u8 status;
+	int err;
+
+	err = cdata->tf->read(cdata, ST_MAG40_STATUS_ADDR,
+			      sizeof(status), &status);
+	if (err < 0)
+		return IRQ_HANDLED;
+
+	if (!(status & ST_MAG40_AVL_DATA_MASK))
+		return IRQ_NONE;
+
+	iio_trigger_poll_chained(cdata->iio_trig);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t st_mag40_buffer_thread_handler(int irq, void *p)
+{
+	u8 buffer[ALIGN(ST_MAG40_OUT_LEN, sizeof(s64)) + sizeof(s64)];
+	struct iio_poll_func *pf = p;
+	struct iio_dev *iio_dev = pf->indio_dev;
+	struct st_mag40_data *cdata = iio_priv(iio_dev);
+	int err;
+
+	err = cdata->tf->read(cdata, ST_MAG40_OUTX_L_ADDR,
+			      ST_MAG40_OUT_LEN, buffer);
+	if (err < 0)
+		goto out;
+
+	/* discard samples generated during the turn-on time */
+	if (cdata->samples_to_discard > 0) {
+		cdata->samples_to_discard--;
+		goto out;
+	}
+
+	iio_push_to_buffers_with_timestamp(iio_dev, buffer, cdata->ts);
+	cdata->ts += cdata->delta_ts;
+
+out:
+	iio_trigger_notify_done(cdata->iio_trig);
+
+	return IRQ_HANDLED;
+}
+
+static int st_mag40_buffer_preenable(struct iio_dev *indio_dev)
+{
+	struct st_mag40_data *cdata = iio_priv(indio_dev);
+
+	return st_mag40_set_enable(cdata, true);
+}
+
+static int st_mag40_buffer_postdisable(struct iio_dev *indio_dev)
+{
+	struct st_mag40_data *cdata = iio_priv(indio_dev);
+	int err;
+
+	err = st_mag40_set_enable(cdata, false);
+
+	return err < 0 ? err : 0;
+}
+
+static const struct iio_buffer_setup_ops st_mag40_buffer_setup_ops = {
+	.preenable = st_mag40_buffer_preenable,
+	.postenable = iio_triggered_buffer_postenable,
+	.predisable = iio_triggered_buffer_predisable,
+	.postdisable = st_mag40_buffer_postdisable,
+};
+
+int st_mag40_trig_set_state(struct iio_trigger *trig, bool state)
+{
+	struct st_mag40_data *cdata = iio_priv(iio_trigger_get_drvdata(trig));
+	int err;
+
+	err = st_mag40_write_register(cdata, ST_MAG40_INT_DRDY_ADDR,
+				      ST_MAG40_INT_DRDY_MASK, state);
+
+	return err < 0 ? err : 0;
+}
+
+int st_mag40_allocate_ring(struct iio_dev *iio_dev)
+{
+	return  iio_triggered_buffer_setup(iio_dev, NULL,
+					   st_mag40_buffer_thread_handler,
+					   &st_mag40_buffer_setup_ops);
+}
+
+void st_mag40_deallocate_ring(struct iio_dev *iio_dev)
+{
+	iio_triggered_buffer_cleanup(iio_dev);
+}
+
+static const struct iio_trigger_ops st_mag40_trigger_ops = {
+	.set_trigger_state = st_mag40_trig_set_state,
+};
+
+int st_mag40_allocate_trigger(struct iio_dev *iio_dev)
+{
+	struct st_mag40_data *cdata = iio_priv(iio_dev);
+	int err;
+
+	err = devm_request_threaded_irq(cdata->dev, cdata->irq,
+					st_mag40_trigger_irq_handler,
+					st_mag40_trigger_thread_handler,
+					IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
+					cdata->name, cdata);
+	if (err)
+		return err;
+
+	cdata->iio_trig = devm_iio_trigger_alloc(cdata->dev, "%s-trigger",
+						 iio_dev->name);
+	if (!cdata->iio_trig) {
+		dev_err(cdata->dev, "failed to allocate iio trigger.\n");
+		return -ENOMEM;
+	}
+	iio_trigger_set_drvdata(cdata->iio_trig, iio_dev);
+	cdata->iio_trig->ops = &st_mag40_trigger_ops;
+	cdata->iio_trig->dev.parent = cdata->dev;
+
+	err = iio_trigger_register(cdata->iio_trig);
+	if (err < 0) {
+		dev_err(cdata->dev, "failed to register iio trigger.\n");
+		return err;
+	}
+	iio_dev->trig = cdata->iio_trig;
+
+	return 0;
+}
+
+void st_mag40_deallocate_trigger(struct st_mag40_data *cdata)
+{
+	iio_trigger_unregister(cdata->iio_trig);
+}
+
+MODULE_DESCRIPTION("STMicroelectronics st_mag40 driver");
+MODULE_AUTHOR("Armando Visconti <armando.visconti@st.com>");
+MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@st.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/magnetometer/st_mag40_core.c b/drivers/iio/magnetometer/st_mag40_core.c
new file mode 100644
index 000000000000..3c5f2d91897b
--- /dev/null
+++ b/drivers/iio/magnetometer/st_mag40_core.c
@@ -0,0 +1,371 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * STMicroelectronics st_mag40 driver
+ *
+ * Copyright 2016 STMicroelectronics Inc.
+ *
+ * Matteo Dameno <matteo.dameno@st.com>
+ * Armando Visconti <armando.visconti@st.com>
+ * Lorenzo Bianconi <lorenzo.bianconi@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/gpio.h>
+#include <linux/irq.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/trigger.h>
+#include <linux/delay.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/events.h>
+#include <asm/unaligned.h>
+
+#include "st_mag40_core.h"
+
+struct st_mag40_odr_reg {
+	u32 hz;
+	u8 value;
+};
+
+#define ST_MAG40_ODR_TABLE_SIZE		4
+static const struct st_mag40_odr_table_t {
+	u8 addr;
+	u8 mask;
+	struct st_mag40_odr_reg odr_avl[ST_MAG40_ODR_TABLE_SIZE];
+} st_mag40_odr_table = {
+	.addr = ST_MAG40_ODR_ADDR,
+	.mask = ST_MAG40_ODR_MASK,
+	.odr_avl[0] = { .hz = 10, .value = ST_MAG40_CFG_REG_A_ODR_10Hz, },
+	.odr_avl[1] = { .hz = 20, .value = ST_MAG40_CFG_REG_A_ODR_20Hz, },
+	.odr_avl[2] = { .hz = 50, .value = ST_MAG40_CFG_REG_A_ODR_50Hz, },
+	.odr_avl[3] = { .hz = 100, .value = ST_MAG40_CFG_REG_A_ODR_100Hz, },
+};
+
+#define ST_MAG40_ADD_CHANNEL(device_type, modif, index, mod,	\
+				endian, sbits, rbits, addr, s)	\
+{								\
+	.type = device_type,					\
+	.modified = modif,					\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |		\
+				BIT(IIO_CHAN_INFO_SCALE),	\
+	.scan_index = index,					\
+	.channel2 = mod,					\
+	.address = addr,					\
+	.scan_type = {						\
+		.sign = s,					\
+		.realbits = rbits,				\
+		.shift = sbits - rbits,				\
+		.storagebits = sbits,				\
+		.endianness = endian,				\
+	},							\
+}
+
+static const struct iio_chan_spec st_mag40_channels[] = {
+	ST_MAG40_ADD_CHANNEL(IIO_MAGN, 1, 0, IIO_MOD_X, IIO_LE, 16, 16,
+				ST_MAG40_OUTX_L_ADDR, 's'),
+	ST_MAG40_ADD_CHANNEL(IIO_MAGN, 1, 1, IIO_MOD_Y, IIO_LE, 16, 16,
+				ST_MAG40_OUTY_L_ADDR, 's'),
+	ST_MAG40_ADD_CHANNEL(IIO_MAGN, 1, 2, IIO_MOD_Z, IIO_LE, 16, 16,
+				ST_MAG40_OUTZ_L_ADDR, 's'),
+	IIO_CHAN_SOFT_TIMESTAMP(3),
+};
+
+int st_mag40_write_register(struct st_mag40_data *cdata, u8 reg_addr,
+			    u8 mask, u8 data)
+{
+	int err;
+	u8 val;
+
+	mutex_lock(&cdata->lock);
+
+	err = cdata->tf->read(cdata, reg_addr, sizeof(val), &val);
+	if (err < 0)
+		goto unlock;
+
+	val = ((val & ~mask) | ((data << __ffs(mask)) & mask));
+
+	err = cdata->tf->write(cdata, reg_addr, sizeof(val), &val);
+
+unlock:
+	mutex_unlock(&cdata->lock);
+
+	return err < 0 ? err : 0;
+}
+
+static int st_mag40_write_odr(struct st_mag40_data *cdata, uint32_t odr)
+{
+	int err, i;
+
+	for (i = 0; i < ST_MAG40_ODR_TABLE_SIZE; i++)
+		if (st_mag40_odr_table.odr_avl[i].hz >= odr)
+			break;
+
+	if (i == ST_MAG40_ODR_TABLE_SIZE)
+		return -EINVAL;
+
+	err = st_mag40_write_register(cdata, st_mag40_odr_table.addr,
+				      st_mag40_odr_table.mask,
+				      st_mag40_odr_table.odr_avl[i].value);
+	if (err < 0)
+		return err;
+
+	cdata->odr = odr;
+	cdata->samples_to_discard = ST_MAG40_TURNON_TIME_SAMPLES_NUM;
+
+	return 0;
+}
+
+int st_mag40_set_enable(struct st_mag40_data *cdata, bool state)
+{
+	u8 mode;
+
+	mode = state ? ST_MAG40_CFG_REG_A_MD_CONT : ST_MAG40_CFG_REG_A_MD_IDLE;
+
+	if (state) {
+		cdata->ts = cdata->ts_irq = st_mag40_get_timestamp();
+		cdata->delta_ts = div_s64(1000000000LL, cdata->odr);
+	}
+
+	return st_mag40_write_register(cdata, ST_MAG40_EN_ADDR,
+				       ST_MAG40_EN_MASK, mode);
+}
+
+int st_mag40_init_sensors(struct st_mag40_data *cdata)
+{
+	int err;
+
+	/*
+	 * Enable block data update feature.
+	 */
+	err = st_mag40_write_register(cdata, ST_MAG40_CFG_REG_C_ADDR,
+				      ST_MAG40_CFG_REG_C_BDU_MASK, 1);
+	if (err < 0)
+		return err;
+
+	/*
+	 * Enable the temperature compensation feature
+	 */
+	err = st_mag40_write_register(cdata, ST_MAG40_CFG_REG_A_ADDR,
+				      ST_MAG40_TEMP_COMP_EN, 1);
+	if (err < 0)
+		return err;
+
+	err = st_mag40_write_register(cdata, ST_MAG40_CFG_REG_B_ADDR,
+				      ST_MAG40_CFG_REG_B_OFF_CANC_MASK, 1);
+
+	return err < 0 ? err : 0;
+}
+
+static ssize_t st_mag40_get_sampling_frequency(struct device *dev,
+						struct device_attribute *attr,
+						char *buf)
+{
+	struct st_mag40_data *cdata = iio_priv(dev_get_drvdata(dev));
+
+	return sprintf(buf, "%d\n", cdata->odr);
+}
+
+static ssize_t st_mag40_set_sampling_frequency(struct device *dev,
+						struct device_attribute *attr,
+						const char *buf, size_t count)
+{
+	struct iio_dev *iio_dev = dev_get_drvdata(dev);
+	struct st_mag40_data *cdata = iio_priv(iio_dev);
+	unsigned int odr;
+	int err;
+
+	err = kstrtoint(buf, 10, &odr);
+	if (err < 0)
+		return err;
+
+	err = st_mag40_write_odr(cdata, odr);
+
+	return err < 0 ? err : count;
+}
+
+static ssize_t
+st_mag40_get_sampling_frequency_avail(struct device *dev,
+				      struct device_attribute *attr,
+				      char *buf)
+{
+	int i, len = 0;
+
+	for (i = 0; i < ST_MAG40_ODR_TABLE_SIZE; i++)
+		len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
+				 st_mag40_odr_table.odr_avl[i].hz);
+	buf[len - 1] = '\n';
+
+	return len;
+}
+
+static int st_mag40_read_oneshot(struct st_mag40_data *cdata,
+				 u8 addr, int *val)
+{
+	u8 data[2];
+	int err;
+
+	err = st_mag40_set_enable(cdata, true);
+	if (err < 0)
+		return err;
+
+	msleep(40);
+
+	err = cdata->tf->read(cdata, addr, sizeof(data), data);
+	if (err < 0)
+		return err;
+
+	*val = (s16)get_unaligned_le16(data);
+
+	err = st_mag40_set_enable(cdata, false);
+
+	return err < 0 ? err : IIO_VAL_INT;
+}
+
+static int st_mag40_read_raw(struct iio_dev *iio_dev,
+			     struct iio_chan_spec const *ch,
+			     int *val, int *val2, long mask)
+{
+	struct st_mag40_data *cdata = iio_priv(iio_dev);
+	int ret;
+
+	mutex_lock(&iio_dev->mlock);
+
+	if (iio_buffer_enabled(iio_dev)) {
+		mutex_unlock(&iio_dev->mlock);
+		return -EBUSY;
+	}
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		ret = st_mag40_read_oneshot(cdata, ch->address, val);
+		break;
+	case IIO_CHAN_INFO_SCALE:
+		*val = 0;
+		*val2 = 1500;
+		ret = IIO_VAL_INT_PLUS_MICRO;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	mutex_unlock(&iio_dev->mlock);
+
+	return ret;
+}
+
+static IIO_DEV_ATTR_SAMP_FREQ(0444,
+			      st_mag40_get_sampling_frequency,
+			      st_mag40_set_sampling_frequency);
+static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_mag40_get_sampling_frequency_avail);
+
+static struct attribute *st_mag40_attributes[] = {
+	&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
+	&iio_dev_attr_sampling_frequency.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group st_mag40_attribute_group = {
+	.attrs = st_mag40_attributes,
+};
+
+static const struct iio_info st_mag40_info = {
+	.attrs = &st_mag40_attribute_group,
+	.read_raw = &st_mag40_read_raw,
+};
+
+int st_mag40_common_probe(struct iio_dev *iio_dev)
+{
+	struct st_mag40_data *cdata = iio_priv(iio_dev);
+	int32_t err;
+	u8 wai;
+
+	mutex_init(&cdata->lock);
+
+	err = cdata->tf->read(cdata, ST_MAG40_WHO_AM_I_ADDR,
+			      sizeof(wai), &wai);
+	if (err < 0) {
+		dev_err(cdata->dev, "failed to read Who-Am-I register.\n");
+
+		return err;
+	}
+
+	if (wai != ST_MAG40_WHO_AM_I_DEF) {
+		dev_err(cdata->dev, "Who-Am-I value not valid. (%02x)\n", wai);
+		return -ENODEV;
+	}
+
+	cdata->odr = st_mag40_odr_table.odr_avl[0].hz;
+
+	iio_dev->channels = st_mag40_channels;
+	iio_dev->num_channels = ARRAY_SIZE(st_mag40_channels);
+	iio_dev->info = &st_mag40_info;
+	iio_dev->modes = INDIO_DIRECT_MODE;
+
+	err = st_mag40_init_sensors(cdata);
+	if (err < 0)
+		return err;
+
+	if (cdata->irq > 0) {
+		err = st_mag40_allocate_ring(iio_dev);
+		if (err < 0)
+			return err;
+
+		err = st_mag40_allocate_trigger(iio_dev);
+		if (err < 0)
+			goto deallocate_ring;
+	}
+
+	err = devm_iio_device_register(cdata->dev, iio_dev);
+	if (err)
+		goto iio_trigger_deallocate;
+
+	return 0;
+
+iio_trigger_deallocate:
+	st_mag40_deallocate_trigger(cdata);
+
+deallocate_ring:
+	st_mag40_deallocate_ring(iio_dev);
+
+	return err;
+}
+EXPORT_SYMBOL(st_mag40_common_probe);
+
+void st_mag40_common_remove(struct iio_dev *iio_dev)
+{
+	struct st_mag40_data *cdata = iio_priv(iio_dev);
+
+	if (cdata->irq > 0) {
+		st_mag40_deallocate_trigger(cdata);
+		st_mag40_deallocate_ring(iio_dev);
+	}
+}
+EXPORT_SYMBOL(st_mag40_common_remove);
+
+#ifdef CONFIG_PM
+int st_mag40_common_suspend(struct st_mag40_data *cdata)
+{
+	return 0;
+}
+EXPORT_SYMBOL(st_mag40_common_suspend);
+
+int st_mag40_common_resume(struct st_mag40_data *cdata)
+{
+	return 0;
+}
+EXPORT_SYMBOL(st_mag40_common_resume);
+#endif /* CONFIG_PM */
+
+MODULE_DESCRIPTION("STMicroelectronics st_mag40 driver");
+MODULE_AUTHOR("Armando Visconti <armando.visconti@st.com>");
+MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@st.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/magnetometer/st_mag40_core.h b/drivers/iio/magnetometer/st_mag40_core.h
new file mode 100644
index 000000000000..cc8e9cbf00ce
--- /dev/null
+++ b/drivers/iio/magnetometer/st_mag40_core.h
@@ -0,0 +1,136 @@
+/* SPDX-License-Identifier: GPL-2.0
+ *
+ * STMicroelectronics st_mag40 driver
+ *
+ * Copyright 2016 STMicroelectronics Inc.
+ *
+ * Matteo Dameno <matteo.dameno@st.com>
+ * Armando Visconti <armando.visconti@st.com>
+ * Lorenzo Bianconi <lorenzo.bianconi@st.com>
+ *
+ */
+
+#ifndef __ST_MAG40_H
+#define __ST_MAG40_H
+
+#include <linux/types.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/trigger.h>
+
+#define ST_MAG40_DEV_NAME			"st_mag40"
+#define LIS2MDL_DEV_NAME			"lis2mdl_magn"
+#define LSM303AH_DEV_NAME			"lsm303ah_magn"
+#define LSM303AGR_DEV_NAME			"lsm303agr_magn"
+#define ISM303DAC_DEV_NAME			"ism303dac_magn"
+#define IIS2MDC_DEV_NAME			"iis2mdc_magn"
+
+/* Power Modes */
+enum {
+	ST_MAG40_LWC_MODE = 0,
+	ST_MAG40_NORMAL_MODE,
+	ST_MAG40_MODE_COUNT,
+};
+
+#define ST_MAG40_WHO_AM_I_ADDR				0x4f
+#define ST_MAG40_WHO_AM_I_DEF				0x40
+
+/* Magnetometer control registers */
+#define ST_MAG40_CFG_REG_A_ADDR				0x60
+#define ST_MAG40_TEMP_COMP_EN				0x80
+#define ST_MAG40_CFG_REG_A_ODR_MASK			0x0c
+#define ST_MAG40_CFG_REG_A_ODR_10Hz			0x00
+#define ST_MAG40_CFG_REG_A_ODR_20Hz			0x01
+#define ST_MAG40_CFG_REG_A_ODR_50Hz			0x02
+#define ST_MAG40_CFG_REG_A_ODR_100Hz			0x03
+#define ST_MAG40_CFG_REG_A_ODR_COUNT			4
+#define ST_MAG40_CFG_REG_A_MD_MASK			0x03
+#define ST_MAG40_CFG_REG_A_MD_CONT			0x00
+#define ST_MAG40_CFG_REG_A_MD_IDLE			0x03
+
+#define ST_MAG40_ODR_ADDR				ST_MAG40_CFG_REG_A_ADDR
+#define ST_MAG40_ODR_MASK				ST_MAG40_CFG_REG_A_ODR_MASK
+
+#define ST_MAG40_EN_ADDR				ST_MAG40_CFG_REG_A_ADDR
+#define ST_MAG40_EN_MASK				ST_MAG40_CFG_REG_A_MD_MASK
+
+#define ST_MAG40_CFG_REG_B_ADDR				0x61
+#define ST_MAG40_CFG_REG_B_OFF_CANC_MASK		0x02
+
+#define ST_MAG40_CFG_REG_C_ADDR				0x62
+#define ST_MAG40_CFG_REG_C_BDU_MASK			0x10
+#define ST_MAG40_CFG_REG_C_INT_MASK			0x01
+
+#define ST_MAG40_INT_DRDY_ADDR				ST_MAG40_CFG_REG_C_ADDR
+#define ST_MAG40_INT_DRDY_MASK				ST_MAG40_CFG_REG_C_INT_MASK
+
+#define ST_MAG40_STATUS_ADDR				0x67
+#define ST_MAG40_AVL_DATA_MASK				0x7
+
+/* Magnetometer output registers */
+#define ST_MAG40_OUTX_L_ADDR				0x68
+#define ST_MAG40_OUTY_L_ADDR				0x6A
+#define ST_MAG40_OUTZ_L_ADDR				0x6C
+
+#define ST_MAG40_BDU_ADDR				ST_MAG40_CTRL1_ADDR
+#define ST_MAG40_BDU_MASK				0x02
+
+#define ST_MAG40_TURNON_TIME_SAMPLES_NUM	2
+
+/* 3 axis of 16 bit each */
+#define ST_MAG40_OUT_LEN				6
+
+#define ST_MAG40_TX_MAX_LENGTH				16
+#define ST_MAG40_RX_MAX_LENGTH				16
+
+struct st_mag40_transfer_buffer {
+	u8 rx_buf[ST_MAG40_RX_MAX_LENGTH];
+	u8 tx_buf[ST_MAG40_TX_MAX_LENGTH] ____cacheline_aligned;
+};
+
+struct st_mag40_data;
+
+struct st_mag40_transfer_function {
+	int (*write)(struct st_mag40_data *cdata, u8 reg_addr, int len, u8 *data);
+	int (*read)(struct st_mag40_data *cdata, u8 reg_addr, int len, u8 *data);
+};
+
+struct st_mag40_data {
+	const char *name;
+	struct mutex lock;
+	u8 drdy_int_pin;
+	int irq;
+	s64 ts;
+	s64 ts_irq;
+	s64 delta_ts;
+
+	u16 odr;
+	u8 samples_to_discard;
+
+	struct device *dev;
+	struct iio_trigger *iio_trig;
+	const struct st_mag40_transfer_function *tf;
+	struct st_mag40_transfer_buffer tb;
+};
+
+static inline s64 st_mag40_get_timestamp(void)
+{
+	return ktime_get_boottime_ns();
+}
+
+int st_mag40_common_probe(struct iio_dev *iio_dev);
+void st_mag40_common_remove(struct iio_dev *iio_dev);
+
+#ifdef CONFIG_PM
+int st_mag40_common_suspend(struct st_mag40_data *cdata);
+int st_mag40_common_resume(struct st_mag40_data *cdata);
+#endif /* CONFIG_PM */
+
+int st_mag40_allocate_ring(struct iio_dev *iio_dev);
+int st_mag40_allocate_trigger(struct iio_dev *iio_dev);
+int st_mag40_trig_set_state(struct iio_trigger *trig, bool state);
+int st_mag40_set_enable(struct st_mag40_data *cdata, bool enable);
+void st_mag40_deallocate_ring(struct iio_dev *iio_dev);
+void st_mag40_deallocate_trigger(struct st_mag40_data *cdata);
+int st_mag40_write_register(struct st_mag40_data *cdata, u8 reg_addr, u8 mask, u8 data);
+
+#endif /* __ST_MAG40_H */
diff --git a/drivers/iio/magnetometer/st_mag40_i2c.c b/drivers/iio/magnetometer/st_mag40_i2c.c
new file mode 100644
index 000000000000..8980972ad65e
--- /dev/null
+++ b/drivers/iio/magnetometer/st_mag40_i2c.c
@@ -0,0 +1,180 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * STMicroelectronics st_mag40 driver
+ *
+ * Copyright 2016 STMicroelectronics Inc.
+ *
+ * Armando Visconti <armando.visconti@st.com>
+ * Lorenzo Bianconi <lorenzo.bianconi@st.com>
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/types.h>
+
+#include "st_mag40_core.h"
+
+#define I2C_AUTO_INCREMENT	0x80
+
+static int st_mag40_i2c_read(struct st_mag40_data *cdata, u8 reg_addr,
+			     int len, u8 *data)
+{
+	struct i2c_client *client = to_i2c_client(cdata->dev);
+	struct i2c_msg msg[2];
+
+	if (len > 1)
+		reg_addr |= I2C_AUTO_INCREMENT;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = client->flags;
+	msg[0].len = 1;
+	msg[0].buf = &reg_addr;
+
+	msg[1].addr = client->addr;
+	msg[1].flags = client->flags | I2C_M_RD;
+	msg[1].len = len;
+	msg[1].buf = data;
+
+	return i2c_transfer(client->adapter, msg, 2);
+}
+
+static int st_mag40_i2c_write(struct st_mag40_data *cdata, u8 reg_addr,
+			      int len, u8 *data)
+{
+	struct i2c_client *client = to_i2c_client(cdata->dev);
+	struct i2c_msg msg;
+	u8 send[len + 1];
+
+	send[0] = reg_addr;
+	memcpy(&send[1], data, len * sizeof(u8));
+	len++;
+
+	msg.addr = client->addr;
+	msg.flags = client->flags;
+	msg.len = len;
+	msg.buf = send;
+
+	return i2c_transfer(client->adapter, &msg, 1);
+}
+
+static const struct st_mag40_transfer_function st_mag40_tf_i2c = {
+	.write = st_mag40_i2c_write,
+	.read = st_mag40_i2c_read,
+};
+
+static int st_mag40_i2c_probe(struct i2c_client *client,
+			      const struct i2c_device_id *id)
+{
+	struct st_mag40_data *cdata;
+	struct iio_dev *iio_dev;
+
+	iio_dev = devm_iio_device_alloc(&client->dev, sizeof(*cdata));
+	if (!iio_dev)
+		return -ENOMEM;
+
+	i2c_set_clientdata(client, iio_dev);
+	iio_dev->dev.parent = &client->dev;
+	iio_dev->name = client->name;
+
+	cdata = iio_priv(iio_dev);
+	cdata->dev = &client->dev;
+	cdata->name = client->name;
+	cdata->tf = &st_mag40_tf_i2c;
+	cdata->irq = client->irq;
+
+	return st_mag40_common_probe(iio_dev);
+}
+
+static int st_mag40_i2c_remove(struct i2c_client *client)
+{
+	struct iio_dev *iio_dev = i2c_get_clientdata(client);
+
+	st_mag40_common_remove(iio_dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int st_mag40_i2c_suspend(struct device *dev)
+{
+	struct iio_dev *iio_dev = dev_get_drvdata(dev);
+	struct st_mag40_data *cdata = iio_priv(iio_dev);
+
+	return st_mag40_common_suspend(cdata);
+}
+
+static int st_mag40_i2c_resume(struct device *dev)
+{
+	struct iio_dev *iio_dev = dev_get_drvdata(dev);
+	struct st_mag40_data *cdata = iio_priv(iio_dev);
+
+	return st_mag40_common_resume(cdata);
+}
+
+static const struct dev_pm_ops st_mag40_i2c_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(st_mag40_i2c_suspend, st_mag40_i2c_resume)
+};
+#endif /* CONFIG_PM */
+
+static const struct i2c_device_id st_mag40_ids[] = {
+	{ LSM303AH_DEV_NAME, 0 },
+	{ LSM303AGR_DEV_NAME, 0 },
+	{ LIS2MDL_DEV_NAME, 0 },
+	{ ISM303DAC_DEV_NAME, 0 },
+	{ IIS2MDC_DEV_NAME, 0 },
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, st_mag40_ids);
+
+#ifdef CONFIG_OF
+static const struct of_device_id st_mag40_id_table[] = {
+	{
+		.compatible = "st,lsm303ah_magn",
+		.data = LSM303AH_DEV_NAME,
+	},
+	{
+		.compatible = "st,lsm303agr_magn",
+		.data = LSM303AGR_DEV_NAME,
+	},
+	{
+		.compatible = "st,lis2mdl_magn",
+		.data = LSM303AGR_DEV_NAME,
+	},
+	{
+		.compatible = "st,ism303dac_magn",
+		.data = ISM303DAC_DEV_NAME,
+	},
+	{
+		.compatible = "st,iis2mdc_magn",
+		.data = IIS2MDC_DEV_NAME,
+	},
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, st_mag40_id_table);
+#endif /* CONFIG_OF */
+
+static struct i2c_driver st_mag40_i2c_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = ST_MAG40_DEV_NAME,
+#ifdef CONFIG_PM
+		   .pm = &st_mag40_i2c_pm_ops,
+#endif
+#ifdef CONFIG_OF
+		   .of_match_table = st_mag40_id_table,
+#endif /* CONFIG_OF */
+		   },
+	.probe = st_mag40_i2c_probe,
+	.remove = st_mag40_i2c_remove,
+	.id_table = st_mag40_ids,
+};
+module_i2c_driver(st_mag40_i2c_driver);
+
+MODULE_DESCRIPTION("STMicroelectronics st_mag40 i2c driver");
+MODULE_AUTHOR("Armando Visconti <armando.visconti@st.com>");
+MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@st.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/magnetometer/st_mag40_spi.c b/drivers/iio/magnetometer/st_mag40_spi.c
new file mode 100644
index 000000000000..7412dfbf7fa6
--- /dev/null
+++ b/drivers/iio/magnetometer/st_mag40_spi.c
@@ -0,0 +1,188 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * STMicroelectronics st_mag40 driver
+ *
+ * Copyright 2016 STMicroelectronics Inc.
+ *
+ * Armando Visconti <armando.visconti@st.com>
+ * Lorenzo Bianconi <lorenzo.bianconi@st.com>
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/spi/spi.h>
+#include <linux/types.h>
+
+#include "st_mag40_core.h"
+
+#define ST_SENSORS_SPI_READ	0x80
+
+static int st_mag40_spi_read(struct st_mag40_data *cdata,
+			     u8 reg_addr, int len, u8 *data)
+{
+	int err;
+
+	struct spi_transfer xfers[] = {
+		{
+			.tx_buf = cdata->tb.tx_buf,
+			.bits_per_word = 8,
+			.len = 1,
+		},
+		{
+			.rx_buf = cdata->tb.rx_buf,
+			.bits_per_word = 8,
+			.len = len,
+		}
+	};
+
+	cdata->tb.tx_buf[0] = reg_addr | ST_SENSORS_SPI_READ;
+
+	err = spi_sync_transfer(to_spi_device(cdata->dev),
+						xfers, ARRAY_SIZE(xfers));
+	if (err)
+		return err;
+
+	memcpy(data, cdata->tb.rx_buf, len*sizeof(u8));
+
+	return len;
+}
+
+static int st_mag40_spi_write(struct st_mag40_data *cdata,
+			      u8 reg_addr, int len, u8 *data)
+{
+	struct spi_transfer xfers = {
+		.tx_buf = cdata->tb.tx_buf,
+		.bits_per_word = 8,
+		.len = len + 1,
+	};
+
+	if (len >= ST_MAG40_RX_MAX_LENGTH)
+		return -ENOMEM;
+
+	cdata->tb.tx_buf[0] = reg_addr;
+
+	memcpy(&cdata->tb.tx_buf[1], data, len);
+
+	return spi_sync_transfer(to_spi_device(cdata->dev), &xfers, 1);
+}
+
+static const struct st_mag40_transfer_function st_mag40_tf_spi = {
+	.write = st_mag40_spi_write,
+	.read = st_mag40_spi_read,
+};
+
+static int st_mag40_spi_probe(struct spi_device *spi)
+{
+	struct st_mag40_data *cdata;
+	struct iio_dev *iio_dev;
+
+	iio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*cdata));
+	if (!iio_dev)
+		return -ENOMEM;
+
+	spi_set_drvdata(spi, iio_dev);
+	iio_dev->dev.parent = &spi->dev;
+	iio_dev->name = spi->modalias;
+
+	cdata = iio_priv(iio_dev);
+	cdata->dev = &spi->dev;
+	cdata->name = spi->modalias;
+	cdata->tf = &st_mag40_tf_spi;
+	cdata->irq = spi->irq;
+
+	return st_mag40_common_probe(iio_dev);
+}
+
+static int st_mag40_spi_remove(struct spi_device *spi)
+{
+	struct iio_dev *iio_dev = spi_get_drvdata(spi);
+
+	st_mag40_common_remove(iio_dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int st_mag40_spi_suspend(struct device *dev)
+{
+	struct iio_dev *iio_dev = dev_get_drvdata(dev);
+	struct st_mag40_data *cdata = iio_priv(iio_dev);
+
+	return st_mag40_common_suspend(cdata);
+}
+
+static int st_mag40_spi_resume(struct device *dev)
+{
+	struct iio_dev *iio_dev = dev_get_drvdata(dev);
+	struct st_mag40_data *cdata = iio_priv(iio_dev);
+
+	return st_mag40_common_resume(cdata);
+}
+
+static const struct dev_pm_ops st_mag40_spi_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(st_mag40_spi_suspend, st_mag40_spi_resume)
+};
+#endif /* CONFIG_PM */
+
+static const struct spi_device_id st_mag40_ids[] = {
+	{ LSM303AH_DEV_NAME, 0 },
+	{ LSM303AGR_DEV_NAME, 0 },
+	{ LIS2MDL_DEV_NAME, 0 },
+	{ ISM303DAC_DEV_NAME, 0 },
+	{ IIS2MDC_DEV_NAME, 0 },
+	{}
+};
+
+MODULE_DEVICE_TABLE(spi, st_mag40_ids);
+
+#ifdef CONFIG_OF
+static const struct of_device_id st_mag40_id_table[] = {
+	{
+		.compatible = "st,lsm303ah_magn",
+		.data = LSM303AH_DEV_NAME,
+	},
+	{
+		.compatible = "st,lsm303agr_magn",
+		.data = LSM303AGR_DEV_NAME,
+	},
+	{
+		.compatible = "st,lis2mdl_magn",
+		.data = LSM303AGR_DEV_NAME,
+	},
+	{
+		.compatible = "st,ism303dac_magn",
+		.data = ISM303DAC_DEV_NAME,
+	},
+	{
+		.compatible = "st,iis2mdc_magn",
+		.data = IIS2MDC_DEV_NAME,
+	},
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, st_mag40_id_table);
+#endif /* CONFIG_OF */
+
+static struct spi_driver st_mag40_spi_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = ST_MAG40_DEV_NAME,
+#ifdef CONFIG_PM
+		   .pm = &st_mag40_spi_pm_ops,
+#endif /* CONFIG_PM */
+#ifdef CONFIG_OF
+		   .of_match_table = st_mag40_id_table,
+#endif /* CONFIG_OF */
+		   },
+	.probe = st_mag40_spi_probe,
+	.remove = st_mag40_spi_remove,
+	.id_table = st_mag40_ids,
+};
+module_spi_driver(st_mag40_spi_driver);
+
+MODULE_DESCRIPTION("STMicroelectronics st_mag40 spi driver");
+MODULE_AUTHOR("Armando Visconti <armando.visconti@st.com>");
+MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@st.com>");
+MODULE_LICENSE("GPL v2");
