drivers/sensor/sensor_bmc150_magn.c

/* sensor_bmc150_magn.c - Driver for Bosch BMC150 magnetometer sensor */

/*
 * Copyright (c) 2016 Intel Corporation
 *
 * This code is based on bmm050.c from https://github.com/BoschSensortec/BMM050_driver
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <sensor.h>
#include <nanokernel.h>
#include <device.h>
#include <init.h>
#include <i2c.h>
#include <misc/byteorder.h>

#include <gpio.h>

#include "sensor_bmc150_magn.h"

static struct bmc150_magn_data bmc150_magn_data;

#ifdef CONFIG_SENSOR_DEBUG
#include <misc/printk.h>
#define sensor_dbg(fmt, ...) printk("bmc150_magn: " fmt, ##__VA_ARGS__)
#else
#define sensor_dbg(fmt, ...) do { } while (0)
#endif /* CONFIG_SENSOR_DEBUG */

static const struct {
	int freq;
	uint8_t reg_val;
} bmc150_magn_samp_freq_table[] = { {2, 0x01},
				    {6, 0x02},
				    {8, 0x03},
				    {10, 0x00},
				    {15, 0x04},
				    {20, 0x05},
				    {25, 0x06},
				    {30, 0x07} };

static const struct bmc150_magn_preset {
	uint8_t rep_xy;
	uint8_t rep_z;
	uint8_t odr;
} bmc150_magn_presets_table[] = {
	[LOW_POWER_PRESET] = {3, 3, 10},
	[REGULAR_PRESET] = {9, 15, 10},
	[ENHANCED_REGULAR_PRESET] = {15, 27, 10},
	[HIGH_ACCURACY_PRESET] = {47, 83, 20}
};

static int bmc150_magn_reg_read(struct device *dev, uint8_t reg, uint8_t *val)
{
	struct bmc150_magn_data *data = (struct bmc150_magn_data *) dev->driver_data;
	struct bmc150_magn_config *config =
		(struct bmc150_magn_config *) dev->config->config_info;

	struct i2c_msg msgs[2] = {
		{
		       .buf = &reg,
		       .len = 1,
		       .flags = I2C_MSG_WRITE | I2C_MSG_RESTART,
		},
		{
		       .buf = val,
		       .len = 1,
		       .flags = I2C_MSG_READ | I2C_MSG_STOP,
		},
	};

	return i2c_transfer(data->i2c_master, msgs, 2, config->i2c_slave_addr);
}

static int bmc150_magn_reg_bulk_read(struct device *dev, uint8_t reg,
				     uint8_t *buf, uint32_t len)
{
	struct bmc150_magn_data *data = (struct bmc150_magn_data *) dev->driver_data;
	struct bmc150_magn_config *config =
		(struct bmc150_magn_config *) dev->config->config_info;

	struct i2c_msg msgs[2] = {
		{
			.buf = &reg,
			.len = 1,
			.flags = I2C_MSG_WRITE | I2C_MSG_RESTART,
		},
		{
			.buf = buf,
			.len = len,
			.flags = I2C_MSG_READ | I2C_MSG_STOP,
		},
	};

	return i2c_transfer(data->i2c_master, msgs, 2, config->i2c_slave_addr);
}

static int bmc150_magn_reg_write(struct device *dev, uint8_t reg, uint8_t val)
{
	struct bmc150_magn_data *data = (struct bmc150_magn_data *) dev->driver_data;
	struct bmc150_magn_config *config =
		(struct bmc150_magn_config *) dev->config->config_info;

	uint8_t buf[2] = {reg, val};

	return i2c_write(data->i2c_master, buf, 2, config->i2c_slave_addr);
}

static int bmc150_magn_update_bits(struct device *dev, uint8_t reg,
				   uint8_t mask, uint8_t val)
{
	uint8_t old_val, new_val;

	if (bmc150_magn_reg_read(dev, reg, &old_val) != 0) {
		return -EIO;
	}

	new_val = (old_val & ~mask) | (val & mask);

	if (new_val == old_val) {
		return 0;
	}

	return bmc150_magn_reg_write(dev, reg, new_val);
}

#if defined(CONFIG_BMC150_MAGN_TRIGGER_DRDY)
static int bmc150_magn_set_drdy_polarity(struct device *dev, int state)
{
	if (state) {
		state = 1;
	}

	return bmc150_magn_update_bits(dev, BMC150_MAGN_REG_INT_DRDY,
				   BMC150_MAGN_MASK_DRDY_DR_POLARITY,
				   state << BMC150_MAGN_SHIFT_DRDY_DR_POLARITY);
}
#endif

static int bmc150_magn_set_power_mode(struct device *dev,
				      enum bmc150_magn_power_modes mode,
				      int state)
{
	switch (mode) {
	case BMC150_MAGN_POWER_MODE_SUSPEND:
		if (bmc150_magn_update_bits(dev, BMC150_MAGN_REG_POWER,
					    BMC150_MAGN_MASK_POWER_CTL, !state)
					    != 0) {
			return -EIO;
		}
		sys_thread_busy_wait(5 * USEC_PER_MSEC);

		return 0;
	case BMC150_MAGN_POWER_MODE_SLEEP:
		return bmc150_magn_update_bits(dev,
					       BMC150_MAGN_REG_OPMODE_ODR,
					       BMC150_MAGN_MASK_OPMODE,
					       BMC150_MAGN_MODE_SLEEP <<
					       BMC150_MAGN_SHIFT_OPMODE);
		break;
	case BMC150_MAGN_POWER_MODE_NORMAL:
		return bmc150_magn_update_bits(dev,
					       BMC150_MAGN_REG_OPMODE_ODR,
					       BMC150_MAGN_MASK_OPMODE,
					       BMC150_MAGN_MODE_NORMAL <<
					       BMC150_MAGN_SHIFT_OPMODE);
		break;
	}

	return -ENOTSUP;
}

static int bmc150_magn_set_odr(struct device *dev, uint8_t val)
{
	uint8_t i;

	for (i = 0; i < ARRAY_SIZE(bmc150_magn_samp_freq_table); ++i) {
		if (bmc150_magn_samp_freq_table[i].freq == val) {
			return bmc150_magn_update_bits(dev,
						       BMC150_MAGN_REG_OPMODE_ODR,
						       BMC150_MAGN_MASK_ODR,
						       bmc150_magn_samp_freq_table[i].
						       reg_val <<
						       BMC150_MAGN_SHIFT_ODR);
		}
	}

	return -ENOTSUP;
}

#if defined(BMC150_MAGN_SET_ATTR)
static int bmc150_magn_read_rep_xy(struct device *dev)
{
	struct bmc150_magn_data *data = (struct bmc150_magn_data *) dev->driver_data;
	uint8_t reg_val;

	if (bmc150_magn_reg_read(dev, BMC150_MAGN_REG_REP_XY, &reg_val) != 0) {
		return -EIO;
	}

	data->rep_xy = BMC150_MAGN_REGVAL_TO_REPXY((int)(reg_val));

	return 0;
}

static int bmc150_magn_read_rep_z(struct device *dev)
{
	struct bmc150_magn_data *data = (struct bmc150_magn_data *) dev->driver_data;
	uint8_t reg_val;

	if (bmc150_magn_reg_read(dev, BMC150_MAGN_REG_REP_Z, &reg_val) != 0) {
		return -EIO;
	}

	data->rep_z = BMC150_MAGN_REGVAL_TO_REPZ((int)(reg_val));

	return 0;
}

static int bmc150_magn_compute_max_odr(struct device *dev, int rep_xy, int rep_z, int *max_odr)
{
	struct bmc150_magn_data *data = (struct bmc150_magn_data *) dev->driver_data;

	if (rep_xy == 0) {
		if (data->rep_xy <= 0) {
			if (bmc150_magn_read_rep_xy(dev) != 0) {
				return -EIO;
			}
		}
		rep_xy = data->rep_xy;
	}

	if (rep_z == 0) {
		if (data->rep_z <= 0) {
			if (bmc150_magn_read_rep_z(dev) != 0) {
				return -EIO;
			}
		}
		rep_z = data->rep_z;
	}

	*max_odr = 1000000 / (145 * rep_xy + 500 * rep_z + 980);

	return 0;
}
#endif

#if defined(BMC150_MAGN_SET_ATTR_REP)
static int bmc150_magn_read_odr(struct device *dev)
{
	struct bmc150_magn_data *data = (struct bmc150_magn_data *) dev->driver_data;
	uint8_t i, odr_val, reg_val;

	if (bmc150_magn_reg_read(dev, BMC150_MAGN_REG_OPMODE_ODR, &reg_val) != 0) {
		return -EIO;
	}

	odr_val = (reg_val & BMC150_MAGN_MASK_ODR) >> BMC150_MAGN_SHIFT_ODR;

	for (i = 0; i < ARRAY_SIZE(bmc150_magn_samp_freq_table); ++i) {
		if (bmc150_magn_samp_freq_table[i].reg_val == odr_val) {
			data->odr = bmc150_magn_samp_freq_table[i].freq;
			return 0;
		}
	}

	return -ENOTSUP;
}
#endif

#if defined(CONFIG_BMC150_MAGN_SAMPLING_REP_XY)
static int bmc150_magn_write_rep_xy(struct device *dev, int val)
{
	struct bmc150_magn_data *data = (struct bmc150_magn_data *) dev->driver_data;

	if (bmc150_magn_update_bits(dev,
				    BMC150_MAGN_REG_REP_XY,
				    BMC150_MAGN_REG_REP_DATAMASK,
				    BMC150_MAGN_REPXY_TO_REGVAL(val)) != 0) {
		return -EIO;
	}

	data->rep_xy = val;

	return 0;
}
#endif

#if defined(CONFIG_BMC150_MAGN_SAMPLING_REP_Z)
static int bmc150_magn_write_rep_z(struct device *dev, int val)
{
	struct bmc150_magn_data *data = (struct bmc150_magn_data *) dev->driver_data;

	if (bmc150_magn_update_bits(dev,
				      BMC150_MAGN_REG_REP_Z,
				      BMC150_MAGN_REG_REP_DATAMASK,
				      BMC150_MAGN_REPZ_TO_REGVAL(val)) != 0) {
		return -EIO;
	}

	data->rep_z = val;

	return 0;
}
#endif

/*
 * Datasheet part 4.3.4, provided by Bosch here:
 * https://github.com/BoschSensortec/BMM050_driver
 */
static int32_t bmc150_magn_compensate_xy(struct bmc150_magn_trim_regs *tregs, int16_t xy,
					uint16_t rhall, bool is_x)
{
	int8_t txy1, txy2;
	int16_t val;

	if (xy == BMC150_MAGN_XY_OVERFLOW_VAL) {
		return INT32_MIN;
	}

	if (!rhall) {
		rhall = tregs->xyz1;
	}

	if (is_x) {
		txy1 = tregs->x1;
		txy2 = tregs->x2;
	} else {
		txy1 = tregs->y1;
		txy2 = tregs->y2;
	}

	val = ((int16_t)(((uint16_t)((((int32_t)tregs->xyz1) << 14) / rhall)) -
	      ((uint16_t)0x4000)));
	val = ((int16_t)((((int32_t)xy) * ((((((((int32_t)tregs->xy2) * ((((int32_t)val) *
	      ((int32_t)val)) >> 7)) + (((int32_t)val) *
	      ((int32_t)(((int16_t)tregs->xy1) << 7)))) >> 9) + ((int32_t)0x100000)) *
	      ((int32_t)(((int16_t)txy2) + ((int16_t)0xA0)))) >> 12)) >> 13)) +
	      (((int16_t)txy1) << 3);

	return (int32_t)val;
}

static int32_t bmc150_magn_compensate_z(struct bmc150_magn_trim_regs *tregs, int16_t z,
					uint16_t rhall)
{
	int32_t val;

	if (z == BMC150_MAGN_Z_OVERFLOW_VAL) {
		return INT32_MIN;
	}

	val = (((((int32_t)(z - tregs->z4)) << 15) - ((((int32_t)tregs->z3) *
	      ((int32_t)(((int16_t)rhall) - ((int16_t)tregs->xyz1)))) >> 2)) /
	      (tregs->z2 + ((int16_t)(((((int32_t)tregs->z1) *
	      ((((int16_t)rhall) << 1))) + (1 << 15)) >> 16))));

	return val;
}

static int bmc150_magn_sample_fetch(struct device *dev)
{
	struct bmc150_magn_data *data = (struct bmc150_magn_data *) dev->driver_data;
	uint16_t values[BMC150_MAGN_AXIS_XYZR_MAX];
	int16_t raw_x, raw_y, raw_z;
	uint16_t rhall;

	if (bmc150_magn_reg_bulk_read(dev, BMC150_MAGN_REG_X_L,
					(uint8_t *)values, sizeof(values)) != 0) {
		sensor_dbg("failed to read sample\n");
		return -EIO;
	}

	raw_x = (int16_t)sys_le16_to_cpu(values[BMC150_MAGN_AXIS_X]) >> BMC150_MAGN_SHIFT_XY_L;
	raw_y = (int16_t)sys_le16_to_cpu(values[BMC150_MAGN_AXIS_Y]) >> BMC150_MAGN_SHIFT_XY_L;
	raw_z = (int16_t)sys_le16_to_cpu(values[BMC150_MAGN_AXIS_Z]) >> BMC150_MAGN_SHIFT_Z_L;
	rhall = sys_le16_to_cpu(values[BMC150_MAGN_RHALL]) >> BMC150_MAGN_SHIFT_RHALL_L;

	data->sample_x = bmc150_magn_compensate_xy(&data->tregs, raw_x, rhall, true);
	data->sample_y = bmc150_magn_compensate_xy(&data->tregs, raw_y, rhall, false);
	data->sample_z = bmc150_magn_compensate_z(&data->tregs, raw_z, rhall);

	return 0;
}

static int bmc150_magn_channel_get(struct device *dev,
				   enum sensor_channel chan,
				   struct sensor_value *val)
{
	struct bmc150_magn_data *data = (struct bmc150_magn_data *) dev->driver_data;

	val->type = SENSOR_TYPE_DOUBLE;
	switch (chan) {
	case SENSOR_CHAN_MAGN_X:
		val->dval = (double)(data->sample_x) * (1.0/1600.0);
		break;
	case SENSOR_CHAN_MAGN_Y:
		val->dval = (double)(data->sample_y) * (1.0/1600.0);
		break;
	case SENSOR_CHAN_MAGN_Z:
		val->dval = (double)(data->sample_z) * (1.0/1600.0);
		break;
	default:
		return -EINVAL;
	}

	return 0;
}

#if defined(BMC150_MAGN_SET_ATTR_REP)
static inline int bmc150_magn_attr_set_oversampling(struct device *dev,
						    enum sensor_channel chan,
						    const struct sensor_value *val)
{
	struct bmc150_magn_data *data = (struct bmc150_magn_data *) dev->driver_data;
	int max_odr;

	switch (chan) {
#if defined(CONFIG_BMC150_MAGN_SAMPLING_REP_XY)
	case SENSOR_CHAN_MAGN_X:
	case SENSOR_CHAN_MAGN_Y:
		if (val->val1 < 1 || val->val1 > 511) {
			return -EINVAL;
		}

		if (bmc150_magn_compute_max_odr(dev, val->val1, 0, &max_odr) != 0) {
			return -EIO;
		}

		if (data->odr <= 0) {
			if (bmc150_magn_read_odr(dev) != 0) {
				return -EIO;
			}
		}

		if (data->odr > max_odr) {
			return -EINVAL;
		}

		if (bmc150_magn_write_rep_xy(dev, val->val1) != 0) {
			return -EIO;
		}
		break;
#endif
#if defined(CONFIG_BMC150_MAGN_SAMPLING_REP_Z)
	case SENSOR_CHAN_MAGN_Z:
		if (val->val1 < 1 || val->val1 > 256) {
			return -EINVAL;
		}

		if (bmc150_magn_compute_max_odr(dev, 0, val->val1, &max_odr) != 0) {
			return -EIO;
		}

		if (data->odr <= 0) {
			if (bmc150_magn_read_odr(dev) != 0) {
				return -EIO;
			}
		}

		if (data->odr > max_odr) {
			return -EINVAL;
		}

		if (bmc150_magn_write_rep_z(dev, val->val1) != 0) {
			return -EIO;
		}
		break;
#endif
	default:
		return -EINVAL;
	}
}
#endif

#if defined(BMC150_MAGN_SET_ATTR)
static int bmc150_magn_attr_set(struct device *dev,
				enum sensor_channel chan,
				enum sensor_attribute attr,
				const struct sensor_value *val)
{
	struct bmc150_magn_data *data = (struct bmc150_magn_data *) dev->driver_data;

	switch (attr) {
#if defined(CONFIG_BMC150_MAGN_SAMPLING_RATE_RUNTIME)
	case SENSOR_ATTR_SAMPLING_FREQUENCY:
		if (val->type != SENSOR_TYPE_INT) {
			sensor_dbg("invalid parameter type for SENSOR_ATTR_SAMPLING_FREQUENCY\n");
			return -ENOTSUP;
		}

		if (data->max_odr <= 0) {
			if (bmc150_magn_compute_max_odr(dev, 0, 0, &data->max_odr) != 0) {
				return -EIO;
			}
		}

		if (data->max_odr < val->val1) {
			sensor_dbg("sampling rate not supported with current oversampling factor\n");
			return -ENOTSUP;
		}

		if (bmc150_magn_set_odr(dev, (uint8_t)(val->val1)) != 0) {
			return -EIO;
		}
		break;
#endif
#if defined(BMC150_MAGN_SET_ATTR_REP)
	case SENSOR_ATTR_OVERSAMPLING:
		if (val->type != SENSOR_TYPE_INT) {
			sensor_dbg("invalid parameter type for SENSOR_ATTR_OVERSAMPLING\n");
			return -ENOTSUP;
		}

		bmc150_magn_attr_set_oversampling(dev, chan, val);

		break;
#endif
	default:
		return -EINVAL;
	}

	return 0;
}
#endif

#if defined(CONFIG_BMC150_MAGN_TRIGGERS)
static int bmc150_magn_trigger_set(struct device *dev,
				   const struct sensor_trigger *trig,
				   sensor_trigger_handler_t handler)
{
#if defined(CONFIG_BMC150_MAGN_TRIGGER_DRDY)
	struct bmc150_magn_data *data = (struct bmc150_magn_data *)dev->driver_data;
	const struct bmc150_magn_config * const config = dev->config->config_info;
	uint8_t state;
#endif

#if defined(CONFIG_BMC150_MAGN_TRIGGER_DRDY)
	if (trig->type == SENSOR_TRIG_DATA_READY) {
		gpio_pin_disable_callback(data->gpio_drdy, config->gpio_drdy_int_pin);

		state = 0;
		if (handler) {
			state = 1;
		}

		data->handler_drdy = handler;
		data->trigger_drdy = *trig;

		if (!bmc150_magn_update_bits(dev, BMC150_MAGN_REG_INT_DRDY,
					     BMC150_MAGN_MASK_DRDY_EN,
					     state << BMC150_MAGN_SHIFT_DRDY_EN)
					     != 0) {
			sensor_dbg("failed to set DRDY interrupt\n");
			return -EIO;
		}

		gpio_pin_enable_callback(data->gpio_drdy, config->gpio_drdy_int_pin);
	}
#endif

	return 0;
}
#endif

#if defined(CONFIG_BMC150_MAGN_TRIGGER_DRDY)
static void bmc150_magn_gpio_drdy_callback(struct device *dev, uint32_t pin)
{
	gpio_pin_disable_callback(dev, pin);

	nano_isr_sem_give(&bmc150_magn_data.sem);
}

static void bmc150_magn_fiber_main(int arg1, int gpio_pin)
{
	struct device *dev = (struct device *) arg1;
	struct bmc150_magn_data *data = (struct bmc150_magn_data *)dev->driver_data;
	uint8_t reg_val;

	while (1) {
		nano_fiber_sem_take(&data->sem, TICKS_UNLIMITED);

		while (bmc150_magn_reg_read(dev, BMC150_MAGN_REG_INT_STATUS, &reg_val) != 0) {
			sensor_dbg("failed to clear data ready interrupt\n");
		}

		if (data->handler_drdy) {
			data->handler_drdy(dev, &data->trigger_drdy);
		}

		gpio_pin_enable_callback(data->gpio_drdy, gpio_pin);
	}
}
#endif

static struct sensor_driver_api bmc150_magn_api_funcs = {
#if defined(BMC150_MAGN_SET_ATTR)
	.attr_set = bmc150_magn_attr_set,
#endif
	.sample_fetch = bmc150_magn_sample_fetch,
	.channel_get = bmc150_magn_channel_get,
#if defined(CONFIG_BMC150_MAGN_TRIGGERS)
	.trigger_set = bmc150_magn_trigger_set,
#endif
};

static int bmc150_magn_init_chip(struct device *dev)
{
	struct bmc150_magn_data *data = (struct bmc150_magn_data *) dev->driver_data;
	uint8_t chip_id;
	struct bmc150_magn_preset preset;

	bmc150_magn_set_power_mode(dev, BMC150_MAGN_POWER_MODE_NORMAL, 0);
	bmc150_magn_set_power_mode(dev, BMC150_MAGN_POWER_MODE_SUSPEND, 1);

	if (bmc150_magn_set_power_mode(dev, BMC150_MAGN_POWER_MODE_SUSPEND, 0) != 0) {
		sensor_dbg("failed to bring up device from suspend mode\n");
		return -EIO;
	}

	if (bmc150_magn_reg_read(dev, BMC150_MAGN_REG_CHIP_ID, &chip_id) != 0) {
		sensor_dbg("failed reading chip id\n");
		goto err_poweroff;
	}
	if (chip_id != BMC150_MAGN_CHIP_ID_VAL) {
		sensor_dbg("invalid chip id 0x%x\n", chip_id);
		goto err_poweroff;
	}
	sensor_dbg("chip id 0x%x\n", chip_id);

	preset = bmc150_magn_presets_table[BMC150_MAGN_DEFAULT_PRESET];
	if (bmc150_magn_set_odr(dev, preset.odr) != 0) {
		sensor_dbg("failed to set ODR to %d\n",
			    preset.odr);
		goto err_poweroff;
	}

	if (bmc150_magn_reg_write(dev, BMC150_MAGN_REG_REP_XY,
				  BMC150_MAGN_REPXY_TO_REGVAL(preset.rep_xy)) != 0) {
		sensor_dbg("failed to set REP XY to %d\n",
			   preset.rep_xy);
		goto err_poweroff;
	}

	if (bmc150_magn_reg_write(dev, BMC150_MAGN_REG_REP_Z,
				  BMC150_MAGN_REPZ_TO_REGVAL(preset.rep_z)) != 0) {
		sensor_dbg("failed to set REP Z to %d\n",
			    preset.rep_z);
		goto err_poweroff;
	}

#if defined(CONFIG_BMC150_MAGN_TRIGGER_DRDY)
	if (bmc150_magn_set_drdy_polarity(dev, 0) != 0) {
		sensor_dbg("failed to set DR polarity\n");
		goto err_poweroff;
	}

	if (bmc150_magn_update_bits(dev, BMC150_MAGN_REG_INT_DRDY,
				      BMC150_MAGN_MASK_DRDY_EN,
				      0 << BMC150_MAGN_SHIFT_DRDY_EN) != 0) {
		sensor_dbg("failed to update data ready interrupt enabled bit\n");
		goto err_poweroff;
	}
#endif

	if (bmc150_magn_set_power_mode(dev, BMC150_MAGN_POWER_MODE_NORMAL, 1) != 0) {
		sensor_dbg("failed to power on device\n");
		goto err_poweroff;
	}

	if (bmc150_magn_reg_bulk_read(dev, BMC150_MAGN_REG_TRIM_START,
				      (uint8_t *)&data->tregs, sizeof(data->tregs))
				      != 0) {
		sensor_dbg("failed to read trim regs\n");
		goto err_poweroff;
	}

	data->rep_xy = 0;
	data->rep_z = 0;
	data->odr = 0;
	data->max_odr = 0;
	data->sample_x = 0;
	data->sample_y = 0;
	data->sample_z = 0;

#if defined(CONFIG_BMC150_MAGN_TRIGGER_DRDY)
	data->handler_drdy = NULL;
#endif

	data->tregs.xyz1 = sys_le16_to_cpu(data->tregs.xyz1);
	data->tregs.z1 = sys_le16_to_cpu(data->tregs.z1);
	data->tregs.z2 = sys_le16_to_cpu(data->tregs.z2);
	data->tregs.z3 = sys_le16_to_cpu(data->tregs.z3);
	data->tregs.z4 = sys_le16_to_cpu(data->tregs.z4);

	return 0;

err_poweroff:
	bmc150_magn_set_power_mode(dev, BMC150_MAGN_POWER_MODE_NORMAL, 0);
	bmc150_magn_set_power_mode(dev, BMC150_MAGN_POWER_MODE_SUSPEND, 1);
	return -EIO;
}

int bmc150_magn_init(struct device *dev)
{
	const struct bmc150_magn_config * const config = dev->config->config_info;
	struct bmc150_magn_data *data = dev->driver_data;

	dev->driver_api = &bmc150_magn_api_funcs;

	data->i2c_master = device_get_binding((char *)config->i2c_master_dev_name);
	if (!data->i2c_master) {
		sensor_dbg("i2c master not found: %s\n",
			   config->i2c_master_dev_name);
		return -EINVAL;
	}

	if (bmc150_magn_init_chip(dev) != 0) {
		sensor_dbg("failed to initialize chip\n");
		return -EIO;
	}

#if defined(CONFIG_BMC150_MAGN_TRIGGER_DRDY)
	nano_sem_init(&data->sem);

	task_fiber_start(data->bmc150_magn_fiber_stack, CONFIG_BMC150_MAGN_TRIGGER_FIBER_STACK,
			 bmc150_magn_fiber_main, (int) dev, config->gpio_drdy_int_pin, 10, 0);

	data->gpio_drdy = device_get_binding(config->gpio_drdy_dev_name);
	if (!data->gpio_drdy) {
		sensor_dbg("gpio controller %s not found\n",
			   config->gpio_drdy_dev_name);
		return -EINVAL;
	}

	gpio_pin_configure(data->gpio_drdy, config->gpio_drdy_int_pin,
			   GPIO_DIR_IN | GPIO_INT | GPIO_INT_EDGE |
			   GPIO_INT_ACTIVE_LOW | GPIO_INT_DEBOUNCE);
	if (gpio_set_callback(data->gpio_drdy, bmc150_magn_gpio_drdy_callback) != 0) {
		sensor_dbg("failed to set gpio callback\n");
		return -EIO;
	}
#endif

	return 0;
}

static struct bmc150_magn_config bmc150_magn_config = {
	.i2c_master_dev_name = CONFIG_BMC150_MAGN_I2C_MASTER_DEV_NAME,
	.i2c_slave_addr = BMC150_MAGN_I2C_ADDR,
#if defined(CONFIG_BMC150_MAGN_TRIGGER_DRDY)
	.gpio_drdy_dev_name = CONFIG_BMC150_MAGN_GPIO_DRDY_DEV_NAME,
	.gpio_drdy_int_pin = CONFIG_BMC150_MAGN_GPIO_DRDY_INT_PIN,
#endif
};

DEVICE_INIT(bmc150_magn, CONFIG_BMC150_MAGN_DEV_NAME, bmc150_magn_init, &bmc150_magn_data,
	    &bmc150_magn_config, NANOKERNEL, CONFIG_BMC150_MAGN_INIT_PRIORITY);