| /* 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 = ®, |
| .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 = ®, |
| .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, ®_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, ®_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, ®_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, ®_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); |