/* * Copyright (C) 2015 Meizu. * * This software is licensed under the terms of the GNU General Public * License version 2, as published by the Free Software Foundation, and * may be copied, distributed, and modified under those terms. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #define MZ_MAG_DEV_NAME "mz_mag" #define MZ_MAG_I2C_NAME "mz_mag_i2c" #define MZ_MAG_DEFAULT_MIN_POLLRATE 10 #define MZ_MAG_DEFAULT_POLLRATE 100 #define MZ_MAG_DEFAULT_RANGE 2 #define ABSMIN_MAG -32767 #define ABSMAX_MAG 32767 #define I2C_RETRY_DELAY 5 /* Waiting for signals [ms] */ #define I2C_RETRIES 5 /* Number of retries */ #define I2C_AUTO_INCREMENT 0x80 /* Autoincrement i2c address */ //#define MZ_MAG_DEBUG 1 #ifdef MZ_MAG_DEBUG #define LOG_TAG_MZ_MAG "[mz_mag]" #define pr_info(format, arg...) printk(KERN_EMERG LOG_TAG_MZ_MAG format , ## arg) #define dev_err(dev, format, arg...) printk(KERN_EMERG LOG_TAG_MZ_MAG format , ## arg) #define dev_info(dev, format, arg...) printk(KERN_EMERG LOG_TAG_MZ_MAG format , ## arg) #define dev_dbg(dev, format, arg...) printk(KERN_EMERG LOG_TAG_MZ_MAG format , ## arg) #define dev_warn(dev, format, arg...) printk(KERN_EMERG LOG_TAG_MZ_MAG format , ## arg) #define dev_notice(dev, format, arg...) printk(KERN_EMERG LOG_TAG_MZ_MAG format , ## arg) #endif /* axis_map_x, axis_map_y, axis_map_z can be used to swap x, y and z axes. If axis_map_x is 0 then it will be mapped to x-axis in android, if it is 1 then it will be mapped to y axis in Android and so on. To change the axes direction we can set negate_x, negate_y and negate_z value to 1. If negate_x, negate_y and negate_z values are set to 1 then driver will negate accelerometer axes readings before passing to user space (Android). */ struct mz_mag_rotation { u8 axis_map_x; u8 axis_map_y; u8 axis_map_z; u8 negate_x; u8 negate_y; u8 negate_z; }; struct mz_mag_data; struct mz_mag_ops { int (*enable_device)(struct mz_mag_data *mz_mag); int (*disable_device)(struct mz_mag_data *mz_mag); int (*set_range)(struct mz_mag_data *mz_mag, int range); int (*get_range)(struct mz_mag_data *mz_mag); int (*set_pollrate)(struct mz_mag_data *mz_mag, int pollrate); int (*get_pollrate)(struct mz_mag_data *mz_mag); int (*get_mag_data)(struct mz_mag_data *mz_mag, int16_t *mag_buf); int (*self_test)(struct mz_mag_data *mz_mag); }; struct mz_mag_data { struct i2c_client *client; struct input_dev *input_dev; struct delayed_work input_work; struct iio_dev *indio_dev; struct iio_trigger *trig; u8 *iio_buffer_data; struct workqueue_struct *iio_workq; struct delayed_work iio_work; struct mutex lock; struct mz_mag_rotation *rota; struct mz_mag_ops *ops; uint16_t device_id; char *device_name; uint8_t sens[3]; atomic_t enabled; int range; int pollrate; int odr; int min_pollrate; }; /* ==================================================== st480 Driver ==================================================== */ #define ST480_DEVICE_ID 0x7C #define ST480_DEVICE_NAME "st480" /* * register shift */ #define ST480_REG_DRR_SHIFT 2 /* * BURST MODE(INT) */ #define ST480_BURST_MODE 0 #define BURST_MODE_CMD 0x1F #define BURST_MODE_DATA_LOW 0x01 /* * SINGLE MODE */ #define ST480_SINGLE_MODE 1 #define SINGLE_MEASUREMENT_MODE_CMD 0x3F /* * register */ #define READ_MEASUREMENT_CMD 0x4F #define WRITE_REGISTER_CMD 0x60 #define READ_REGISTER_CMD 0x50 #define EXIT_REGISTER_CMD 0x80 #define MEMORY_RECALL_CMD 0xD0 #define MEMORY_STORE_CMD 0xE0 #define RESET_CMD 0xF0 #define CALIBRATION_REG (0x02 << ST480_REG_DRR_SHIFT) #define CALIBRATION_DATA_LOW 0x1C #define CALIBRATION_DATA_HIGH 0x00 #define ONE_INIT_DATA_LOW 0x7C #define ONE_INIT_DATA_HIGH 0x00 #define ONE_INIT_REG (0x00 << ST480_REG_DRR_SHIFT) #define TWO_INIT_DATA_LOW 0x00 #define TWO_INIT_DATA_HIGH 0x00 #define TWO_INIT_REG (0x02 << ST480_REG_DRR_SHIFT) #define TEMP_DATA_LOW 0x00 #define TEMP_DATA_HIGH 0x00 #define TEMP_REG (0x01 << ST480_REG_DRR_SHIFT) static int st480_i2c_transfer_data(struct i2c_client *client, int len, char *buf, int length) { int ret; int tries = 0; struct i2c_msg msgs[] = { { .addr = client->addr, .flags = 0, .len = len, .buf = buf, }, { .addr = client->addr, .flags = I2C_M_RD, .len = length, .buf = buf, }, }; do { ret = i2c_transfer(client->adapter, msgs, 2); if (ret != 2) msleep_interruptible(I2C_RETRY_DELAY); } while ((ret != 2) && (++tries < I2C_RETRIES)); if (ret != 2) { dev_err(&client->dev, "read transfer error\n"); ret = -EIO; } else { ret = 0; } return ret; } /* the MTK platform can only use dma to transfer more tha 8 bytes data */ static u8 *gpDMABuf_va; static dma_addr_t gpDMABuf_pa; static char * st480_read_measurement(struct i2c_client *client) { int ret = 0; char cmd[1] = {READ_MEASUREMENT_CMD}; struct i2c_msg msgs[] = { { .addr = client->addr, .flags = 0, .len = 1, .buf = cmd, //.scl_rate = 400 * 1000, }, { .addr = client->addr, /* the ext_flag is mtk specific */ .ext_flag = (client->ext_flag | I2C_ENEXT_FLAG | I2C_DMA_FLAG), .flags = I2C_M_RD, .len = 9, .buf = (u8 *)gpDMABuf_pa, //.scl_rate = 400 * 1000, }, }; ret = i2c_transfer(client->adapter, msgs, 2); if(ret < 0){ printk(KERN_EMERG "st480_i2c_transfer_data i2c_transfer error, ret: %d\n", ret); return NULL; } return gpDMABuf_va; } static int st480_self_test(struct mz_mag_data *mz_mag) { int ret; char * buffer; char * xyz_data; s16 hw_data_z0; s16 hw_data_z1; s16 z_bist; unsigned char buf[5]; buf[0] = SINGLE_MEASUREMENT_MODE_CMD; st480_i2c_transfer_data(mz_mag->client, 1, buf, 1); msleep(500); /* read the normal z data */ buffer = st480_read_measurement(mz_mag->client); if (NULL == buffer) { dev_err(&mz_mag->client->dev, "Failed to read measurement\n"); ret = -1; goto err1; } xyz_data = buffer + 3; hw_data_z0 = (xyz_data[4]<<8)|xyz_data[5]; /* enter self test mode */ buf[0] = WRITE_REGISTER_CMD; buf[1] = 0x01; buf[2] = 0x7C; buf[3] = ONE_INIT_REG; st480_i2c_transfer_data(mz_mag->client, 4, buf, 1); msleep(250); buf[0] = SINGLE_MEASUREMENT_MODE_CMD; st480_i2c_transfer_data(mz_mag->client, 1, buf, 1); msleep(500); /* read the self test z data */ buffer = st480_read_measurement(mz_mag->client); if (NULL == buffer) { dev_err(&mz_mag->client->dev, "Failed to read measurement\n"); ret = -1; goto err2; } xyz_data = buffer + 3; hw_data_z1 = (xyz_data[4]<<8)|xyz_data[5]; /* calculate z bist */ z_bist = hw_data_z1 - hw_data_z0; dev_dbg(&mz_mag->client->dev, "z1: %d, z0: %d, z_bist: %d\n", hw_data_z1, hw_data_z0, z_bist); if (z_bist >= -180 && z_bist <= -90) { dev_dbg(&mz_mag->client->dev, "st480 self test success!\n"); ret = 0; } else { dev_err(&mz_mag->client->dev, "self test failed\n"); ret = -1; } err2: buf[0] = WRITE_REGISTER_CMD; buf[1] = 0x00; buf[2] = 0x7C; buf[3] = ONE_INIT_REG; st480_i2c_transfer_data(mz_mag->client, 4, buf, 1); err1: return ret; } #define READ_MEASUREMENT_XYZ_CMD (0x4E) #define READ_MEASUREMENT_T_CMD (0x41) static int st480_get_mag_data(struct mz_mag_data *mz_mag, int16_t * mag_data) { int ret; char cmd[1]; char * buffer; char * temp_data; char * xyz_data; s16 hw_data[3]; int16_t mag_x, mag_y, mag_z; buffer = st480_read_measurement(mz_mag->client); if (NULL == buffer) { dev_err(&mz_mag->client->dev, "Failed to read measurement\n"); ret = -1; goto out; } temp_data = buffer + 1; xyz_data = buffer + 3; if(!((buffer[0]>>4) & 0X01)) { hw_data[0] = (xyz_data[0]<<8)|xyz_data[1]; hw_data[1] = (xyz_data[2]<<8)|xyz_data[3]; hw_data[2] = (xyz_data[4]<<8)|xyz_data[5]; mag_x = ((mz_mag->rota->negate_x)?(-hw_data[mz_mag->rota->axis_map_x]) : (hw_data[mz_mag->rota->axis_map_x])); mag_y = ((mz_mag->rota->negate_y)?(-hw_data[mz_mag->rota->axis_map_y]) : (hw_data[mz_mag->rota->axis_map_y])); mag_z = ((mz_mag->rota->negate_z)?(-hw_data[mz_mag->rota->axis_map_z]) : (hw_data[mz_mag->rota->axis_map_z])); if( ((temp_data[0]<<8)|(temp_data[1])) > 46244) { mag_x = mag_x * (1 + (70/128/4096) * (((temp_data[0]<<8)|(temp_data[1])) - 46244)); mag_y = mag_y * (1 + (70/128/4096) * (((temp_data[0]<<8)|(temp_data[1])) - 46244)); mag_z = mag_z * (1 + (70/128/4096) * (((temp_data[0]<<8)|(temp_data[1])) - 46244)); } else if( ((temp_data[0]<<8)|(temp_data[1])) < 46244) { mag_x = mag_x * (1 + (60/128/4096) * (((temp_data[0]<<8)|(temp_data[1])) - 46244)); mag_y = mag_y * (1 + (60/128/4096) * (((temp_data[0]<<8)|(temp_data[1])) - 46244)); mag_z = mag_z * (1 + (60/128/4096) * (((temp_data[0]<<8)|(temp_data[1])) - 46244)); } dev_dbg(&mz_mag->client->dev, "st480 raw data: x = %d, y = %d, z = %d \n", mag_x,mag_y,mag_z); mag_data[0] = mag_x; mag_data[1] = mag_y; mag_data[2] = mag_z; ret = 0; } else { dev_dbg(&mz_mag->client->dev, "get data state: 0x%X \n", buffer[0]); dev_dbg(&mz_mag->client->dev, "temp data: 0x%02X 0x%02X\n", temp_data[0], temp_data[1]); dev_dbg(&mz_mag->client->dev, "xyz data: 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X \n", xyz_data[0], xyz_data[1], xyz_data[2], xyz_data[3], xyz_data[4], xyz_data[5]); ret = -1; } out: cmd[0] = SINGLE_MEASUREMENT_MODE_CMD; st480_i2c_transfer_data(mz_mag->client, 1, cmd, 1); return ret; } struct mz_mag_ops st480_ops = { .get_mag_data = st480_get_mag_data, .self_test = st480_self_test, }; struct mz_mag_rotation st480_rota = { .axis_map_x = 0, .axis_map_y = 1, .axis_map_z = 2, .negate_x = 1, .negate_y = 1, .negate_z = 0, }; static int st480_hw_init(struct mz_mag_data *mz_mag) { unsigned char buf[5]; #define ST480_DMA_MAX_TRANSACTION_LENGTH 255 gpDMABuf_va = (u8 *)dma_alloc_coherent(&mz_mag->client->dev, ST480_DMA_MAX_TRANSACTION_LENGTH, &gpDMABuf_pa, GFP_KERNEL); if(!gpDMABuf_va){ printk(KERN_EMERG "Allocate DMA I2C Buffer failed!\n"); return -1; } memset(buf, 0, 5); buf[0] = READ_REGISTER_CMD; buf[1] = 0x00; st480_i2c_transfer_data(mz_mag->client, 2, buf, 3); if(buf[2] != ST480_DEVICE_ID) { dev_dbg(&mz_mag->client->dev, "mag device is not st480\n"); return -1; } dev_dbg(&mz_mag->client->dev, "mag device is st480\n"); dev_dbg(&mz_mag->client->dev, "device id: 0x00%02X\n", buf[2]); mz_mag->device_id = ST480_DEVICE_ID; mz_mag->device_name = ST480_DEVICE_NAME; mz_mag->rota = &st480_rota; mz_mag->ops = &st480_ops; mz_mag->min_pollrate = 10; //init register step 1 buf[0] = WRITE_REGISTER_CMD; buf[1] = ONE_INIT_DATA_HIGH; buf[2] = ONE_INIT_DATA_LOW; buf[3] = ONE_INIT_REG; st480_i2c_transfer_data(mz_mag->client, 4, buf, 1); //init register step 2 buf[0] = WRITE_REGISTER_CMD; buf[1] = TWO_INIT_DATA_HIGH; buf[2] = TWO_INIT_DATA_LOW; buf[3] = TWO_INIT_REG; st480_i2c_transfer_data(mz_mag->client, 4, buf, 1); //disable temperature compensation register buf[0] = WRITE_REGISTER_CMD; buf[1] = TEMP_DATA_HIGH; buf[2] = TEMP_DATA_LOW; buf[3] = TEMP_REG; st480_i2c_transfer_data(mz_mag->client, 4, buf, 1); //set calibration register buf[0] = WRITE_REGISTER_CMD; buf[1] = CALIBRATION_DATA_HIGH; buf[2] = CALIBRATION_DATA_LOW; buf[3] = CALIBRATION_REG; st480_i2c_transfer_data(mz_mag->client, 4, buf, 1); //set mode config buf[0] = SINGLE_MEASUREMENT_MODE_CMD; st480_i2c_transfer_data(mz_mag->client, 1, buf, 1); return 0; } /* ==================================================== akm09911 Driver ==================================================== */ #define AKM09911_DEVICE_ID 0x0548 #define AKM09911_DEVICE_NAME "ak09911" #define AKM09911_REG_WIA1 0x00 #define AKM09911_REG_WIA2 0x01 #define AKM09911_REG_INFO1 0x02 #define AKM09911_REG_INFO2 0x03 #define AKM09911_REG_ST1 0x10 #define AKM09911_REG_DATA AKM09911_REG_HXL #define AKM09911_REG_HXL 0x11 #define AKM09911_REG_HXH 0x12 #define AKM09911_REG_HYL 0x13 #define AKM09911_REG_HYH 0x14 #define AKM09911_REG_HZL 0x15 #define AKM09911_REG_HZH 0x16 #define AKM09911_REG_TMPS 0x17 #define AKM09911_REG_ST2 0x18 #define AKM09911_REG_CNTL1 0x30 #define AKM09911_REG_CNTL2 0x31 #define AKM09911_REG_CNTL3 0x32 #define AKM09911_FUSE_ASAX 0x60 #define AKM09911_FUSE_ASAY 0x61 #define AKM09911_FUSE_ASAZ 0x62 #define AKM09911_MODE_SNG_MEASURE 0x01 #define AKM09911_MODE_SELF_TEST 0x10 #define AKM09911_MODE_FUSE_ACCESS 0x1F #define AKM09911_MODE_POWERDOWN 0x00 #define AKM09911_RESET_DATA 0x01 static int akm09911_i2c_read(struct i2c_client *client, u8 * buf, int len) { int ret; int tries = 0; struct i2c_msg msgs[] = { { .addr = client->addr, .flags = client->flags & I2C_M_TEN, .len = 1, .buf = buf, }, { .addr = client->addr, .flags = (client->flags & I2C_M_TEN) | I2C_M_RD, .len = len, .buf = buf, }, }; do { ret = i2c_transfer(client->adapter, msgs, 2); if (ret != 2) msleep_interruptible(I2C_RETRY_DELAY); } while ((ret != 2) && (++tries < I2C_RETRIES)); if (ret != 2) { dev_err(&client->dev, "read transfer error\n"); ret = -EIO; } else { ret = 0; } return ret; } static int akm09911_i2c_write(struct i2c_client *client, u8 * buf, int len) { int ret; int tries = 0; struct i2c_msg msgs[] = { { .addr = client->addr, .flags = client->flags & I2C_M_TEN, .len = len + 1, .buf = buf, }, }; do { ret = i2c_transfer(client->adapter, msgs, 1); if (ret != 1) msleep_interruptible(I2C_RETRY_DELAY); } while ((ret != 1) && (++tries < I2C_RETRIES)); if (ret != 1) { dev_err(&client->dev, "write transfer error\n"); ret = -EIO; } else { ret = 0; } return ret; } static int akm09911_get_mag_data(struct mz_mag_data *mz_mag, int16_t * mag_data) { int ret; u8 buf[9]; int hw_data[3]; u8 *sens; int16_t mag_x, mag_y, mag_z; sens = mz_mag->sens; buf[0] = AKM09911_REG_ST1; ret = akm09911_i2c_read(mz_mag->client, buf, 1); if (ret < 0) { goto out; } if (!(buf[0] & 0x01)) { dev_dbg(&mz_mag->client->dev, "data not ready\n"); ret = -1; goto out; } buf[0] = AKM09911_REG_DATA; ret = akm09911_i2c_read(mz_mag->client, buf, 8); if (ret < 0) { ret = -1; goto out; } if (buf[7] & 0x08) { dev_dbg(&mz_mag->client->dev, "magnetic sensor overflow\n"); ret = -1; goto out; } hw_data[0] = (int)((int16_t)(buf[1]<<8)+((int16_t)buf[0])); hw_data[1] = (int)((int16_t)(buf[3]<<8)+((int16_t)buf[2])); hw_data[2] = (int)((int16_t)(buf[5]<<8)+((int16_t)buf[4])); hw_data[0] = ((hw_data[0] * (sens[0] + 128)) >> 7); hw_data[1] = ((hw_data[1] * (sens[1] + 128)) >> 7); hw_data[2] = ((hw_data[2] * (sens[2] + 128)) >> 7); #if 0 mag_x = -hw_data[0]; mag_y = hw_data[1]; mag_z = -hw_data[2]; #else mag_x = ((mz_mag->rota->negate_x) ? (-hw_data[mz_mag->rota->axis_map_x]) : (hw_data[mz_mag->rota->axis_map_x])); mag_y = ((mz_mag->rota->negate_y) ? (-hw_data[mz_mag->rota->axis_map_y]) : (hw_data[mz_mag->rota->axis_map_y])); mag_z = ((mz_mag->rota->negate_z) ? (-hw_data[mz_mag->rota->axis_map_z]) : (hw_data[mz_mag->rota->axis_map_z])); #endif #if 0 dev_dbg(&mz_mag->client->dev, "mag data: %d %d %d\n", mag_x, mag_y, mag_z); #endif mag_data[0] = mag_x; mag_data[1] = mag_y; mag_data[2] = mag_z; ret = 0; out: buf[0] = 0x31; buf[1] = 0x01; akm09911_i2c_write(mz_mag->client, buf, 1); return ret; } static int akm09911_self_test(struct mz_mag_data *mz_mag) { int i; int ret; int retry; int hw_data[3]; unsigned char buf[8]; int ak09911_st_lower[3] = {-50, -50, -400}; int ak09911_st_upper[3] = {50, 50, -100}; /* power down mode */ buf[0] = AKM09911_REG_CNTL2; buf[1] = AKM09911_MODE_POWERDOWN; akm09911_i2c_write(mz_mag->client, buf, 1); /* self test mode */ buf[0] = AKM09911_REG_CNTL2; buf[1] = AKM09911_MODE_SELF_TEST; akm09911_i2c_write(mz_mag->client, buf, 1); /* wait for data ready */ retry = 10; while (retry--) { msleep(100); buf[0] = AKM09911_REG_ST1; ret = akm09911_i2c_read(mz_mag->client, buf, 1); if (ret < 0) { goto err1; } if (!(buf[0] & 0x01)) { dev_dbg(&mz_mag->client->dev, "data not ready\n"); continue; } else { dev_dbg(&mz_mag->client->dev, "data is ready\n"); break; } } if (!(buf[0] & 0x01)) { dev_err(&mz_mag->client->dev, "failed to wait data ready\n"); ret = -1; goto err2; } /* read sensor data */ buf[0] = AKM09911_REG_DATA; ret = akm09911_i2c_read(mz_mag->client, buf, 8); if (ret < 0) { ret = -1; goto err2; } if (buf[7] & 0x08) { dev_dbg(&mz_mag->client->dev, "magnetic sensor overflow\n"); ret = -1; goto err2; } hw_data[0] = (int)((int16_t)(buf[1]<<8)+((int16_t)buf[0])); hw_data[1] = (int)((int16_t)(buf[3]<<8)+((int16_t)buf[2])); hw_data[2] = (int)((int16_t)(buf[5]<<8)+((int16_t)buf[4])); hw_data[0] = ((hw_data[0] * (mz_mag->sens[0] + 128)) >> 7); hw_data[1] = ((hw_data[1] * (mz_mag->sens[1] + 128)) >> 7); hw_data[2] = ((hw_data[2] * (mz_mag->sens[2] + 128)) >> 7); dev_dbg(&mz_mag->client->dev, "data: %d %d %d\n", hw_data[0], hw_data[1], hw_data[2]); dev_dbg(&mz_mag->client->dev, "upper: %d %d %d\n", ak09911_st_upper[0], ak09911_st_upper[1], ak09911_st_upper[2]); dev_dbg(&mz_mag->client->dev, "lower: %d %d %d\n", ak09911_st_lower[0], ak09911_st_lower[1], ak09911_st_lower[2]); ret = 0; for (i=0; i<3; i++) { if (hw_data[i]>ak09911_st_upper[i] || hw_data[i]client->dev, "self test failed\n"); ret = -1; break; } } if (ret == 0) { dev_dbg(&mz_mag->client->dev, "self test success\n"); } err2: err1: /* revert to power down mode */ buf[0] = AKM09911_REG_CNTL2; buf[1] = AKM09911_MODE_POWERDOWN; akm09911_i2c_write(mz_mag->client, buf, 1); return ret; } struct mz_mag_ops akm09911_ops = { .get_mag_data = akm09911_get_mag_data, .self_test = akm09911_self_test, }; struct mz_mag_rotation akm09911_rota = { .axis_map_x = 1, .axis_map_y = 0, .axis_map_z = 2, .negate_x = 0, .negate_y = 0, .negate_z = 1, }; static int akm09911_hw_init(struct mz_mag_data *mz_mag) { unsigned char buf[4]; memset(buf, 0, 4); /* get device id of magnetometer */ buf[0] = AKM09911_REG_WIA1; akm09911_i2c_read(mz_mag->client, buf, 2); if (AKM09911_DEVICE_ID != (buf[1]<<8|buf[0])) { dev_dbg(&mz_mag->client->dev, "mag device is not akm09911\n"); return -1; } dev_dbg(&mz_mag->client->dev, "mag device is akm09911\n"); dev_dbg(&mz_mag->client->dev, "device id: 0x%02X%02X\n", buf[1], buf[0]); mz_mag->device_id = AKM09911_DEVICE_ID; mz_mag->device_name = AKM09911_DEVICE_NAME; mz_mag->rota = &akm09911_rota; mz_mag->ops = &akm09911_ops; /* set AKM to Fuse ROM access mode */ buf[0] = AKM09911_REG_CNTL2; buf[1] = AKM09911_MODE_FUSE_ACCESS; akm09911_i2c_write(mz_mag->client, buf, 1); /* read sens */ buf[0] = AKM09911_FUSE_ASAX; akm09911_i2c_read(mz_mag->client, buf, 3); dev_dbg(&mz_mag->client->dev, "mag sens: 0x%X 0x%X 0x%X\n", buf[0], buf[1], buf[2]); mz_mag->sens[0] = buf[0]; mz_mag->sens[1] = buf[1]; mz_mag->sens[2] = buf[2]; /* revert to power down mode */ buf[0] = AKM09911_REG_CNTL2; buf[1] = AKM09911_MODE_POWERDOWN; akm09911_i2c_write(mz_mag->client, buf, 1); return 0; } /* ==================================================== af8133 Driver ==================================================== */ /*AF8133 register address*/ #define AF8133_REG_PCODE 0x00 #define AF8133_REG_STATUS1 0x02 #define AF8133_REG_DATA 0x03 #define AF8133_REG_STATUS 0x0A #define AF8133_REG_RANGE1 0x0B #define AF8133_REG_SWR 0x11 #define AF8133_DEVICE_ID 0x50 #define FIND_SW_OFFSET_LOOP 5 #define FIND_SW_OFFSET_INDEX 2 static int mag_pos[3][FIND_SW_OFFSET_LOOP]; static int mag_neg[3][FIND_SW_OFFSET_LOOP]; static int mag_offset[3]; static int mag_cnt=0; static int af8133_i2c_read(struct i2c_client *client, u8 * buf, int len) { int ret; int tries = 0; struct i2c_msg msgs[] = { { .addr = client->addr, .flags = client->flags & I2C_M_TEN, .len = 1, .buf = buf, }, { .addr = client->addr, .flags = (client->flags & I2C_M_TEN) | I2C_M_RD, .len = len, .buf = buf, }, }; do { ret = i2c_transfer(client->adapter, msgs, 2); if (ret != 2) msleep_interruptible(I2C_RETRY_DELAY); } while ((ret != 2) && (++tries < I2C_RETRIES)); if (ret != 2) { dev_err(&client->dev, "read transfer error\n"); ret = -EIO; } else { ret = 0; } return ret; } static int af8133_i2c_write(struct i2c_client *client, u8 * buf, int len) { int ret; int tries = 0; struct i2c_msg msgs[] = { { .addr = client->addr, .flags = client->flags & I2C_M_TEN, .len = len + 1, .buf = buf, }, }; do { ret = i2c_transfer(client->adapter, msgs, 1); if (ret != 1) msleep_interruptible(I2C_RETRY_DELAY); } while ((ret != 1) && (++tries < I2C_RETRIES)); if (ret != 1) { dev_err(&client->dev, "write transfer error\n"); ret = -EIO; } else { ret = 0; } return ret; } static int af8133_get_mag_data(struct mz_mag_data *mz_mag, int16_t * mag_data) { int ret; u8 buf[9]; int hw_data[3]; u8 *sens; int i, j, k; int16_t mag_x, mag_y, mag_z; sens = mz_mag->sens; buf[0] = AF8133_REG_STATUS1; ret = af8133_i2c_read(mz_mag->client, buf, 1); if (ret < 0) { dev_dbg(&mz_mag->client->dev, "read status1 failed\n"); goto out; } if (!(buf[0] & 0x01)) { dev_dbg(&mz_mag->client->dev, "data not ready\n"); ret = -1; goto out; } buf[0] = AF8133_REG_DATA; ret = af8133_i2c_read(mz_mag->client, buf, 6); if (ret < 0) { dev_dbg(&mz_mag->client->dev, "read data failed\n"); ret = -1; goto out; } hw_data[0] = (int)((int16_t)(buf[1]<<8)+((int16_t)buf[0])); hw_data[1] = (int)((int16_t)(buf[3]<<8)+((int16_t)buf[2])); hw_data[2] = (int)((int16_t)(buf[5]<<8)+((int16_t)buf[4])); for(i = 0; i < 3; i++) hw_data[i] = (hw_data[i] > 32767) ? (hw_data[i] - 65536) : hw_data[i]; if(mag_cnt >= 0 && mag_cnt <= (FIND_SW_OFFSET_LOOP*2)) { /* Using the set and reset function to calculate the offset. offset = (value_set + value_reset) / 2 */ if(mag_cnt >= 1 && mag_cnt <= FIND_SW_OFFSET_LOOP) { for(i = 0; i < 3; i++) mag_neg[i][mag_cnt-1] = hw_data[i]; } else if (mag_cnt > FIND_SW_OFFSET_LOOP && mag_cnt <= (FIND_SW_OFFSET_LOOP*2)) { for(i = 0; i < 3; i++) mag_pos[i][mag_cnt-FIND_SW_OFFSET_LOOP-1] = hw_data[i]; } mag_cnt++; if(mag_cnt >= 1 && mag_cnt <= FIND_SW_OFFSET_LOOP) { buf[0] = 0x14; buf[1] = 0x34; ret = af8133_i2c_write(mz_mag->client, buf, 1); } else if(mag_cnt > FIND_SW_OFFSET_LOOP && mag_cnt <= (FIND_SW_OFFSET_LOOP*2)) { buf[0] = 0x14; buf[1] = 0x38; ret = af8133_i2c_write(mz_mag->client, buf, 1); } if(mag_cnt > (FIND_SW_OFFSET_LOOP*2)) { for(i=0;i<3;i++) { for(j=0;j<(FIND_SW_OFFSET_LOOP-1);j++) { for(k=0;k<(FIND_SW_OFFSET_LOOP-1);k++){ if(mag_neg[i][k] < mag_neg[i][k+1]) { int tmp = mag_neg[i][k]; mag_neg[i][k] = mag_neg[i][k+1]; mag_neg[i][k+1] = tmp; } if(mag_pos[i][k] < mag_pos[i][k+1]) { int tmp = mag_pos[i][k]; mag_pos[i][k] = mag_pos[i][k+1]; mag_pos[i][k+1] = tmp; } } } mag_offset[i] = (mag_pos[i][(FIND_SW_OFFSET_INDEX)] + mag_neg[i][FIND_SW_OFFSET_INDEX]) / 2; } } } else { buf[0] = 0x14; buf[1] = 0x38; ret = af8133_i2c_write(mz_mag->client, buf, 1); if(ret < 0) return ret; } for(i = 0; i < 3; i++) hw_data[i] -= mag_offset[i]; //dev_dbg(&mz_mag->client->dev, "hw_data: %d %d %d\n", hw_data[0], hw_data[1], hw_data[2]); #if 1 /* the rotation is in the hal layer */ mag_x = hw_data[0]; mag_y = hw_data[1]; mag_z = hw_data[2]; #else mag_x = ((mz_mag->rota->negate_x) ? (-hw_data[mz_mag->rota->axis_map_x]) : (hw_data[mz_mag->rota->axis_map_x])); mag_y = ((mz_mag->rota->negate_y) ? (-hw_data[mz_mag->rota->axis_map_y]) : (hw_data[mz_mag->rota->axis_map_y])); mag_z = ((mz_mag->rota->negate_z) ? (-hw_data[mz_mag->rota->axis_map_z]) : (hw_data[mz_mag->rota->axis_map_z])); #endif #if 0 dev_dbg(&mz_mag->client->dev, "mag data: %d %d %d\n", mag_x, mag_y, mag_z); #endif mag_data[0] = mag_x; mag_data[1] = mag_y; mag_data[2] = mag_z; ret = 0; out: buf[0] = AF8133_REG_STATUS; buf[1] = 0x01; af8133_i2c_write(mz_mag->client, buf, 1); return ret; } struct mz_mag_ops af8133_ops = { .get_mag_data = af8133_get_mag_data, }; struct mz_mag_rotation af8133_rota = { .axis_map_x = 0, .axis_map_y = 1, .axis_map_z = 2, .negate_x = 0, .negate_y = 0, .negate_z = 0, }; static int af8133_hw_init(struct mz_mag_data *mz_mag) { unsigned char buf[4]; memset(buf, 0, 4); /* get device id of magnetometer */ buf[0] = AF8133_REG_PCODE; af8133_i2c_read(mz_mag->client, buf, 1); if (AF8133_DEVICE_ID != buf[0]) { dev_dbg(&mz_mag->client->dev, "mag device is not af8133, id:0x%x\n", buf[0]); return -1; } dev_dbg(&mz_mag->client->dev, "mag device is af8133\n"); dev_dbg(&mz_mag->client->dev, "device id: 0x%02X\n", buf[0]); mz_mag->device_id = AF8133_DEVICE_ID; mz_mag->rota = &af8133_rota; mz_mag->ops = &af8133_ops; mz_mag->sens[0] = 0; mz_mag->sens[1] = 0; mz_mag->sens[2] = 0; buf[0] = 0x10; buf[1] = 0x55; af8133_i2c_write(mz_mag->client, buf, 1); buf[0] = 0x14; buf[1] = 0x34; af8133_i2c_write(mz_mag->client, buf, 1); buf[0] = 0x33; buf[1] = 0x16; af8133_i2c_write(mz_mag->client, buf, 1); buf[0] = 0x0B; buf[1] = 0x3C; af8133_i2c_write(mz_mag->client, buf, 1); buf[0] = 0x13; buf[1] = 0x00; af8133_i2c_write(mz_mag->client, buf, 1); buf[0] = 0x0A; buf[1] = 0x01; af8133_i2c_write(mz_mag->client, buf, 1); mag_cnt = 0; mag_offset[0] = 0; mag_offset[1] = 0; mag_offset[2] = 0; return 0; } /* ==================================================== Meizu Magnetometer Driver ==================================================== */ /* sysfs interfaces */ static inline int mz_mag_enable(struct mz_mag_data *mz_mag) { int ret = 0; if (!atomic_cmpxchg(&mz_mag->enabled, 0, 1)) { if (mz_mag->ops->enable_device) ret = mz_mag->ops->enable_device(mz_mag); if (ret < 0) { atomic_set(&mz_mag->enabled, 0); return ret; } dev_dbg(&mz_mag->client->dev, "mag input work start\n"); schedule_delayed_work(&mz_mag->input_work, msecs_to_jiffies(mz_mag->pollrate)); } return 0; } static inline int mz_mag_disable(struct mz_mag_data *mz_mag) { if (atomic_cmpxchg(&mz_mag->enabled, 1, 0)) { dev_dbg(&mz_mag->client->dev, "mag input work stop\n"); cancel_delayed_work_sync(&mz_mag->input_work); if (mz_mag->ops->disable_device) mz_mag->ops->disable_device(mz_mag); } return 0; } static inline int mz_mag_iio_enable(struct mz_mag_data *mz_mag) { int ret = 0; if (!atomic_cmpxchg(&mz_mag->enabled, 0, 1)) { if (mz_mag->ops->enable_device) ret = mz_mag->ops->enable_device(mz_mag); if (ret < 0) { atomic_set(&mz_mag->enabled, 0); return ret; } dev_dbg(&mz_mag->client->dev, "mag iio work start\n"); queue_delayed_work(mz_mag->iio_workq, &mz_mag->iio_work, msecs_to_jiffies(mz_mag->pollrate)); } return 0; } static inline int mz_mag_iio_disable(struct mz_mag_data *mz_mag) { if (atomic_cmpxchg(&mz_mag->enabled, 1, 0)) { dev_dbg(&mz_mag->client->dev, "mag iio work stop\n"); cancel_delayed_work_sync(&mz_mag->iio_work); if (mz_mag->ops->disable_device) mz_mag->ops->disable_device(mz_mag); } return 0; } static ssize_t attr_pollrate_ms_show(struct device *dev, struct device_attribute *attr, char *buf) { int val; struct mz_mag_data *mz_mag = dev_get_drvdata(dev); mutex_lock(&mz_mag->lock); val = mz_mag->pollrate; mutex_unlock(&mz_mag->lock); dev_dbg(&mz_mag->client->dev, "get mag pollrate: %d\n", val); return sprintf(buf, "%d\n", val); } static ssize_t attr_pollrate_ms_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t size) { int ret = 0; struct mz_mag_data *mz_mag = dev_get_drvdata(dev); unsigned long rate; dev_dbg(&mz_mag->client->dev, "set mag pollrate: %s\n", buf); if (strict_strtoul(buf, 10, &rate)) return -EINVAL; if (!rate) return -EINVAL; rate = rate > mz_mag->min_pollrate ? rate : mz_mag->min_pollrate; mutex_lock(&mz_mag->lock); if (mz_mag->ops->set_pollrate) ret = mz_mag->ops->set_pollrate(mz_mag, rate); if(ret >= 0) { mz_mag->pollrate = rate; } mutex_unlock(&mz_mag->lock); return size; } static ssize_t attr_full_scale_show(struct device *dev, struct device_attribute *attr, char *buf) { struct mz_mag_data *mz_mag = dev_get_drvdata(dev); int range = 2; mutex_lock(&mz_mag->lock); range = mz_mag->range; mutex_unlock(&mz_mag->lock); dev_dbg(&mz_mag->client->dev, "get mag range: %d\n", range); return sprintf(buf, "%d\n", range); } static ssize_t attr_full_scale_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t size) { struct mz_mag_data *mz_mag = dev_get_drvdata(dev); unsigned long range; dev_dbg(&mz_mag->client->dev, "set mag range: %s\n", buf); if (strict_strtoul(buf, 10, &range)) return -EINVAL; mutex_lock(&mz_mag->lock); if (mz_mag->ops->set_range) mz_mag->ops->set_range(mz_mag, range); mz_mag->range = range; mutex_unlock(&mz_mag->lock); return size; } static ssize_t attr_enable_device_show(struct device *dev, struct device_attribute *attr, char *buf) { struct mz_mag_data *mz_mag = dev_get_drvdata(dev); int val = atomic_read(&mz_mag->enabled); return sprintf(buf, "%d\n", val); } static ssize_t attr_enable_device_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t size) { struct mz_mag_data *mz_mag = dev_get_drvdata(dev); unsigned long val; if (strict_strtoul(buf, 10, &val)) return -EINVAL; if (val) mz_mag_enable(mz_mag); else mz_mag_disable(mz_mag); return size; } static ssize_t attr_self_test_show(struct device *dev, struct device_attribute *attr, char *buf) { int ret = 1; struct mz_mag_data *mz_mag = dev_get_drvdata(dev); mutex_lock(&mz_mag->lock); if (mz_mag->ops->self_test) ret = mz_mag->ops->self_test(mz_mag); mutex_unlock(&mz_mag->lock); return sprintf(buf, "%d\n", ret); } static struct device_attribute attributes[] = { __ATTR(pollrate_ms, 0664, attr_pollrate_ms_show, attr_pollrate_ms_store), __ATTR(full_scale, 0664, attr_full_scale_show, attr_full_scale_store), __ATTR(enable_device, 0664, attr_enable_device_show, attr_enable_device_store), __ATTR(compass_self_test, S_IRUGO, attr_self_test_show, NULL), }; static int create_sysfs_interfaces(struct device *dev) { int i; for (i = 0; i < ARRAY_SIZE(attributes); i++) if (device_create_file(dev, attributes + i)) goto error; return 0; error: for ( ; i >= 0; i--) device_remove_file(dev, attributes + i); dev_err(dev, "%s:Unable to create interface\n", __func__); return -1; } #if 0 static int remove_sysfs_interfaces(struct device *dev) { int i; for (i = 0; i < ARRAY_SIZE(attributes); i++) device_remove_file(dev, attributes + i); return 0; } #endif /* input device interfaces */ static void mz_mag_input_func(struct work_struct *work) { int ret; struct mz_mag_data *mz_mag; int16_t mag_data[3]; mz_mag = container_of((struct delayed_work *)work, struct mz_mag_data, input_work); if (mz_mag->ops->get_mag_data) ret = mz_mag->ops->get_mag_data(mz_mag, mag_data); else ret = -1; if (!ret) { input_report_abs(mz_mag->input_dev, ABS_X, mag_data[0]); input_report_abs(mz_mag->input_dev, ABS_Y, mag_data[1]); input_report_abs(mz_mag->input_dev, ABS_Z, mag_data[2]); input_event(mz_mag->input_dev, EV_SYN, SYN_REPORT, mz_mag->device_id); } schedule_delayed_work(&mz_mag->input_work, msecs_to_jiffies(mz_mag->pollrate)); } static int mz_mag_input_init(struct mz_mag_data *mz_mag) { int ret; INIT_DELAYED_WORK(&mz_mag->input_work, mz_mag_input_func); mz_mag->input_dev = input_allocate_device(); if (!mz_mag->input_dev) { ret = -ENOMEM; dev_err(&mz_mag->client->dev, "Failed to allocate input device\n"); goto err0; } /* Setup input device */ set_bit(EV_ABS, mz_mag->input_dev->evbit); input_set_capability(mz_mag->input_dev, EV_ABS, ABS_X); input_set_capability(mz_mag->input_dev, EV_ABS, ABS_Y); input_set_capability(mz_mag->input_dev, EV_ABS, ABS_Z); /* x-axis of raw magnetic vector (-32768, 32767) */ input_set_abs_params(mz_mag->input_dev, ABS_X, ABSMIN_MAG, ABSMAX_MAG, 0, 0); /* y-axis of raw magnetic vector (-32768, 32767) */ input_set_abs_params(mz_mag->input_dev, ABS_Y, ABSMIN_MAG, ABSMAX_MAG, 0, 0); /* z-axis of raw magnetic vector (-32768, 32767) */ input_set_abs_params(mz_mag->input_dev, ABS_Z, ABSMIN_MAG, ABSMAX_MAG, 0, 0); /* Set name */ mz_mag->input_dev->name = MZ_MAG_DEV_NAME; mz_mag->input_dev->dev.parent = &mz_mag->client->dev; /* Register */ ret = input_register_device(mz_mag->input_dev); if (ret) { dev_err(&mz_mag->client->dev, "Failed to register input device\n"); goto err1; } input_set_drvdata(mz_mag->input_dev, mz_mag); return 0; err1: input_free_device(mz_mag->input_dev); mz_mag->input_dev = NULL; err0: return ret; } /* iio device interfaces */ #define MZ_MAG_IIO_NUMBER_DATA_CHANNELS 3 #define MZ_MAG_IIO_BYTE_FOR_CHANNEL 2 static void mz_mag_iio_func(struct work_struct *work) { int ret; int i, n = 0; struct mz_mag_data *mz_mag; int16_t mag_data[3]; int64_t timestamp; struct timespec ts; mz_mag = container_of((struct delayed_work *)work, struct mz_mag_data, iio_work); //dev_dbg(&mz_mag->client->dev, "%s: enter\n", __func__); if (mz_mag->ops->get_mag_data) ret = mz_mag->ops->get_mag_data(mz_mag, mag_data); else ret = -1; if (!ret) { //dev_dbg(&mz_mag->client->dev, "iio_buffer_data: ================\n"); for (i = 0; i < MZ_MAG_IIO_NUMBER_DATA_CHANNELS; i++) { if (mz_mag->indio_dev->active_scan_mask == NULL) { printk(KERN_EMERG "mag bug: active_scan_mask: %p\n", mz_mag->indio_dev->active_scan_mask); return; } if (test_bit(i, mz_mag->indio_dev->active_scan_mask)) { memcpy(&mz_mag->iio_buffer_data[n * MZ_MAG_IIO_BYTE_FOR_CHANNEL], &mag_data[i], MZ_MAG_IIO_BYTE_FOR_CHANNEL); #if 0 dev_dbg(&mz_mag->client->dev, "%d ", *(int16_t *)&mz_mag->iio_buffer_data[n * MZ_MAG_IIO_BYTE_FOR_CHANNEL]); #endif n++; } } //timestamp = iio_get_time_ns(); get_monotonic_boottime(&ts); timestamp = timespec_to_ns(&ts); if (mz_mag->indio_dev->scan_timestamp) { *(s64 *)((u8 *)mz_mag->iio_buffer_data + ALIGN(n * MZ_MAG_IIO_BYTE_FOR_CHANNEL, sizeof(s64))) = timestamp; #if 0 dev_dbg(&mz_mag->client->dev, "%lld ", *(s64 *)((u8 *)mz_mag->iio_buffer_data + ALIGN(n * MZ_MAG_IIO_BYTE_FOR_CHANNEL, sizeof(s64)))); #endif } //dev_dbg(&mz_mag->client->dev, "\niio_buffer_data ============= \n"); //dev_dbg(&mz_mag->client->dev, "%s: push buffer\n", __func__); iio_push_to_buffers(mz_mag->indio_dev, mz_mag->iio_buffer_data); } queue_delayed_work(mz_mag->iio_workq, &mz_mag->iio_work, msecs_to_jiffies(mz_mag->pollrate)); //dev_dbg(&mz_mag->client->dev, "%s: leave\n", __func__); } int mz_mag_trig_set_state(struct iio_trigger *trig, bool state) { int ret; struct mz_mag_data *mz_mag = iio_device_get_drvdata( iio_trigger_get_drvdata(trig)); dev_dbg(&mz_mag->client->dev, "%s: enter\n", __func__); if (state) { ret = mz_mag_iio_enable(mz_mag); } else { ret = mz_mag_iio_disable(mz_mag); } return ret; } static const struct iio_trigger_ops mz_mag_trigger_ops = { .owner = THIS_MODULE, .set_trigger_state = &mz_mag_trig_set_state, }; static inline irqreturn_t mz_mag_iio_handler_empty(int irq, void *p) { return IRQ_HANDLED; } static int mz_mag_iio_buffer_preenable(struct iio_dev *indio_dev) { return iio_sw_buffer_preenable(indio_dev); } static int mz_mag_iio_buffer_postenable(struct iio_dev *indio_dev) { int err; struct mz_mag_data *mz_mag = iio_device_get_drvdata(indio_dev); dev_dbg(&mz_mag->client->dev, "%s: enter\n", __func__); err = iio_triggered_buffer_postenable(indio_dev); if (err < 0) { dev_err(&mz_mag->client->dev, "failed to call iio_triggered_buffer_postenable\n"); return err; } dev_dbg(&mz_mag->client->dev, "%s: leave\n", __func__); return 0; } static int mz_mag_iio_buffer_predisable(struct iio_dev *indio_dev) { int err; struct mz_mag_data *mz_mag = iio_device_get_drvdata(indio_dev); dev_dbg(&mz_mag->client->dev, "%s: enter\n", __func__); err = iio_triggered_buffer_predisable(indio_dev); if (err < 0) { dev_err(&mz_mag->client->dev, "failed to call iio_triggered_buffer_predisable\n"); return err; } dev_dbg(&mz_mag->client->dev, "%s: leave\n", __func__); return 0; } static int mz_mag_iio_buffer_postdisable(struct iio_dev *indio_dev) { int err; struct mz_mag_data *mz_mag = iio_device_get_drvdata(indio_dev); dev_dbg(&mz_mag->client->dev, "%s: enter\n", __func__); mz_mag_iio_disable(mz_mag); dev_dbg(&mz_mag->client->dev, "%s: leave\n", __func__); return 0; } static const struct iio_buffer_setup_ops mz_mag_iio_buffer_setup_ops = { .preenable = &mz_mag_iio_buffer_preenable, .postenable = &mz_mag_iio_buffer_postenable, .predisable = &mz_mag_iio_buffer_predisable, .postdisable = &mz_mag_iio_buffer_postdisable, }; static int mz_mag_iio_read_raw(struct iio_dev *indio_dev, struct iio_chan_spec const *ch, int *val, int *val2, long mask) { u8 outdata[2]; struct mz_mag_data *mz_mag = iio_device_get_drvdata(indio_dev); dev_dbg(&mz_mag->client->dev, "%s: enter\n", __func__); switch (mask) { case IIO_CHAN_INFO_RAW: mutex_lock(&indio_dev->mlock); if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { mutex_unlock(&indio_dev->mlock); dev_dbg(&mz_mag->client->dev, "%s: leave\n", __func__); return -EBUSY; } /* get data from device */ /* outdata = xxx */ *val = (s16)get_unaligned_le16(outdata); *val = *val >> ch->scan_type.shift; mutex_unlock(&indio_dev->mlock); dev_dbg(&mz_mag->client->dev, "%s: leave\n", __func__); return IIO_VAL_INT; case IIO_CHAN_INFO_SCALE: /* the scale is always 1 */ *val = 1; *val2 = 0; return IIO_VAL_INT_PLUS_MICRO; default: return -EINVAL; } dev_dbg(&mz_mag->client->dev, "%s: leave\n", __func__); return 0; } #define MZ_MAG_IIO_CHANNELS(device_type, index, mod, endian, bits, addr) \ { \ .type = device_type, \ .modified = 1, \ .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 = bits, \ .shift = 16 - bits, \ .storagebits = 16, \ .endianness = endian, \ }, \ } #define MZ_MAG_X_L_ADDR 0x28 #define MZ_MAG_Y_L_ADDR 0x2a #define MZ_MAG_Z_L_ADDR 0x2c static const struct iio_chan_spec mz_mag_iio_ch[] = { MZ_MAG_IIO_CHANNELS(IIO_MAGN, 0, IIO_MOD_X, IIO_LE, 16, MZ_MAG_X_L_ADDR), MZ_MAG_IIO_CHANNELS(IIO_MAGN, 1, IIO_MOD_Y, IIO_LE, 16, MZ_MAG_Y_L_ADDR), MZ_MAG_IIO_CHANNELS(IIO_MAGN, 2, IIO_MOD_Z, IIO_LE, 16, MZ_MAG_Z_L_ADDR), IIO_CHAN_SOFT_TIMESTAMP(3) }; /* we use the hw_fifo_len to identify which ic we are using */ #define ST480_HW_FIFO_LEN (1) #define AK09911_HW_FIFO_LEN (2) #define AF8133_HW_FIFO_LEN (3) ssize_t mz_mag_iio_sysfs_get_hw_fifo_lenght(struct device *dev, struct device_attribute *attr, char *buf) { int hw_fifo_lenght; struct mz_mag_data *mz_mag = dev_get_drvdata(dev); if (mz_mag->device_id == ST480_DEVICE_ID) { hw_fifo_lenght = ST480_HW_FIFO_LEN; } else if (mz_mag->device_id == AKM09911_DEVICE_ID) { hw_fifo_lenght = AK09911_HW_FIFO_LEN; } else if (mz_mag->device_id == AF8133_DEVICE_ID) { hw_fifo_lenght = AF8133_HW_FIFO_LEN; } else { hw_fifo_lenght = 0; } return sprintf(buf, "%d\n", hw_fifo_lenght); } ssize_t mz_mag_iio_sysfs_flush_fifo(struct device *dev, struct device_attribute *attr, const char *buf, size_t size) { return size; } static ssize_t mz_mag_iio_sysfs_sampling_frequency_avail( struct device *dev, struct device_attribute *attr, char *buf) { return scnprintf(buf, PAGE_SIZE, "%d %d %d %d %d\n", 26, 52, 104, 208, 416); } static ssize_t mz_mag_iio_sysfs_get_sampling_frequency( struct device *dev, struct device_attribute *attr, char *buf) { struct mz_mag_data *mz_mag = dev_get_drvdata(dev); dev_dbg(&mz_mag->client->dev, "%s: enter\n", __func__); return sprintf(buf, "%d\n", mz_mag->odr); } static ssize_t mz_mag_iio_sysfs_set_sampling_frequency( struct device *dev, struct device_attribute *attr, const char *buf, size_t size) { int err, err2 = -EINVAL; int ret = 0; unsigned int odr; int rate; struct mz_mag_data *mz_mag = dev_get_drvdata(dev); dev_dbg(&mz_mag->client->dev, "%s: enter\n", __func__); err = kstrtoint(buf, 10, &odr); if (err < 0) { dev_err(&mz_mag->client->dev, "kstrtoint error\n"); return err; } dev_dbg(&mz_mag->client->dev, "set up odr: %d\n", odr); #if 1 mutex_lock(&mz_mag->indio_dev->mlock); switch (odr) { case 26: case 52: case 104: case 208: case 416: rate = 1000 / odr; rate = rate > mz_mag->min_pollrate ? rate : mz_mag->min_pollrate; if (mz_mag->ops->set_pollrate) ret = mz_mag->ops->set_pollrate(mz_mag, rate); if(ret >= 0) { mz_mag->pollrate = rate; mz_mag->odr = odr; err2 = 0; } break; default: err2 = -EINVAL; break; } mutex_unlock(&mz_mag->indio_dev->mlock); #endif dev_dbg(&mz_mag->client->dev, "%s: leave\n", __func__); return err2 < 0 ? err2 : size; } static ssize_t mz_mag_iio_sysfs_get_selftest_status(struct device *dev, struct device_attribute *attr, char *buf) { int ret = 1; struct mz_mag_data *mz_mag = dev_get_drvdata(dev); mutex_lock(&mz_mag->indio_dev->mlock); if (mz_mag->ops->self_test) ret = mz_mag->ops->self_test(mz_mag); mutex_unlock(&mz_mag->indio_dev->mlock); return sprintf(buf, "%d\n", ret); } static ssize_t mz_mag_iio_sysfs_set_selftest_status(struct device *dev, struct device_attribute *attr, const char *buf, size_t size) { /* we do nothing when writing to the self_test */ return size; } static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO, mz_mag_iio_sysfs_get_sampling_frequency, mz_mag_iio_sysfs_set_sampling_frequency); static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(mz_mag_iio_sysfs_sampling_frequency_avail); static IIO_DEVICE_ATTR(hw_fifo_lenght, S_IRUGO, mz_mag_iio_sysfs_get_hw_fifo_lenght, NULL, 0); static IIO_DEVICE_ATTR(flush, S_IWUSR, NULL, mz_mag_iio_sysfs_flush_fifo, 0); static IIO_DEVICE_ATTR(compass_self_test, S_IWUSR | S_IRUGO, mz_mag_iio_sysfs_get_selftest_status, mz_mag_iio_sysfs_set_selftest_status, 0); static struct attribute *mz_mag_iio_attributes[] = { &iio_dev_attr_sampling_frequency_available.dev_attr.attr, &iio_dev_attr_sampling_frequency.dev_attr.attr, &iio_dev_attr_hw_fifo_lenght.dev_attr.attr, &iio_dev_attr_compass_self_test.dev_attr.attr, &iio_dev_attr_flush.dev_attr.attr, NULL, }; static const struct attribute_group mz_mag_iio_attribute_group = { .attrs = mz_mag_iio_attributes, }; static const struct iio_info mz_mag_iio_info = { .driver_module = THIS_MODULE, .attrs = &mz_mag_iio_attribute_group, .read_raw = &mz_mag_iio_read_raw, }; static int mz_sensors_mag_get_name(struct device *dev, char **name) { struct mz_mag_data *mz_mag = dev_get_drvdata(dev); *name = mz_mag->device_name; dev_info(dev, "ltr559_als_get_name succeeded\n"); return 0; } struct meizu_sensors_ops meizu_mag_ops = { .get_name = &mz_sensors_mag_get_name, }; static int mz_mag_iio_init(struct mz_mag_data *mz_mag) { int ret = -1; mz_mag->iio_buffer_data = kmalloc(32, GFP_KERNEL); if (mz_mag->iio_buffer_data == NULL) { dev_err(&mz_mag->client->dev, "failed to call kmalloc\n"); return -ENOMEM; } INIT_DELAYED_WORK(&mz_mag->iio_work, mz_mag_iio_func); mz_mag->iio_workq = create_singlethread_workqueue("mz_mag_iio"); if (!mz_mag->iio_workq) { dev_err(&mz_mag->client->dev, "failed to call create_singlethread_workqueue\n"); goto free_iio_buffer_data; } mz_mag->indio_dev = iio_device_alloc(0); if (!mz_mag->indio_dev) { dev_err(&mz_mag->client->dev, "failed to call iio_device_alloc\n"); goto wq_destory; } mz_mag->indio_dev->name = kasprintf(GFP_KERNEL, "%s_%s", "lsm6ds3", "magn"); mz_mag->indio_dev->info = &mz_mag_iio_info; mz_mag->indio_dev->channels = (struct iio_chan_spec const *)&mz_mag_iio_ch; mz_mag->indio_dev->num_channels = ARRAY_SIZE(mz_mag_iio_ch); mz_mag->indio_dev->modes = INDIO_DIRECT_MODE; iio_device_set_drvdata(mz_mag->indio_dev, mz_mag); ret = iio_triggered_buffer_setup(mz_mag->indio_dev, &mz_mag_iio_handler_empty, NULL, &mz_mag_iio_buffer_setup_ops); if (ret < 0) { dev_err(&mz_mag->client->dev, "failed to setup triggered buffer\n"); goto iio_device_free; } mz_mag->trig = iio_trigger_alloc("%s-trigger", "magn"); if (!mz_mag->trig) { dev_err(&mz_mag->client->dev, "failed to call iio_trigger_alloc\n"); goto iio_deallocate_buffer; } mz_mag->trig->ops = &mz_mag_trigger_ops; mz_mag->trig->dev.parent = &mz_mag->client->dev; iio_trigger_set_drvdata(mz_mag->trig, mz_mag->indio_dev); ret = iio_trigger_register(mz_mag->trig); if (ret < 0) { dev_err(&mz_mag->client->dev, "failed to register iio trigger.\n"); goto iio_deallocate_trigger; } mz_mag->indio_dev->trig = mz_mag->trig; ret = iio_device_register(mz_mag->indio_dev); if (ret < 0) { dev_err(&mz_mag->client->dev, "failed to register iio device.\n"); goto iio_deallocate_trigger; } #if 0 ret = meizu_sysfslink_register_name(&mz_mag->indio_dev->dev, "magnetometer"); if (ret < 0) { dev_err(&mz_mag->client->dev, "failed to meizu_sysfslink_register_name.\n"); goto iio_deallocate_trigger; } #endif ret = meizu_sensor_register(MEIZU_SENSOR_ID_COMPASS, &mz_mag->indio_dev->dev, &meizu_mag_ops); if (ret < 0) { dev_err(&mz_mag->client->dev, "failed to meizu_sensor_register.\n"); goto iio_deallocate_trigger; } return 0; iio_deallocate_trigger: iio_trigger_free(mz_mag->trig); iio_deallocate_buffer: iio_triggered_buffer_cleanup(mz_mag->indio_dev); iio_device_free: iio_device_free(mz_mag->indio_dev); wq_destory: destroy_workqueue(mz_mag->iio_workq); free_iio_buffer_data: kfree(mz_mag->iio_buffer_data); return ret; } static int mz_mag_setup(struct mz_mag_data *mz_mag) { int ret; dev_dbg(&mz_mag->client->dev, "setup magnetometer sensor, mz_mag: %llu!!\n", (uint64_t)mz_mag); ret = akm09911_hw_init(mz_mag); if (ret == 0) return 0; ret = st480_hw_init(mz_mag); if (ret == 0) return 0; ret = af8133_hw_init(mz_mag); if (ret == 0) return 0; dev_err(&mz_mag->client->dev, "Failed to setup magnetometer sensor\n"); return ret; } int mz_mag_probe(struct i2c_client *client, const struct i2c_device_id *id) { int ret = 0; struct mz_mag_data *mz_mag; pr_info("%s mz_mag_probe\n", MZ_MAG_DEV_NAME); mz_mag = kzalloc(sizeof(struct mz_mag_data), GFP_KERNEL); if (!mz_mag) { pr_err("%s mz_mag_probe: memory allocation failed.\n", MZ_MAG_DEV_NAME); ret = -ENOMEM; goto err0; } mz_mag->client = client; mz_mag->range = MZ_MAG_DEFAULT_RANGE; mz_mag->pollrate = MZ_MAG_DEFAULT_POLLRATE; mz_mag->min_pollrate = MZ_MAG_DEFAULT_MIN_POLLRATE; atomic_set(&mz_mag->enabled, 0); mutex_init(&mz_mag->lock); i2c_set_clientdata(client, mz_mag); ret = mz_mag_setup(mz_mag); if(ret < 0) goto err1; #if 0 ret = mz_mag_input_init(mz_mag); if (ret < 0) goto err2; ret = create_sysfs_interfaces(&client->dev); if (ret < 0) goto err3; #endif ret = mz_mag_iio_init(mz_mag); if (ret < 0) goto err3; dev_dbg(&mz_mag->client->dev, "mz mag probe done\n"); return 0; err3: err2: err1: kfree(mz_mag); err0: pr_err("%s mz_mag_probe failed.\n", MZ_MAG_DEV_NAME); return ret; } static const struct i2c_device_id mz_mag_id[] = { {MZ_MAG_DEV_NAME, 0 }, { } }; static struct i2c_driver mz_mag_driver = { .probe = mz_mag_probe, .id_table = mz_mag_id, .driver = { .name = MZ_MAG_DEV_NAME, .owner = THIS_MODULE, }, }; static struct i2c_board_info mz_mag_i2c[] = { { //I2C_BOARD_INFO(MZ_MAG_DEV_NAME, 0x1e), /* af8133 */ I2C_BOARD_INFO(MZ_MAG_DEV_NAME, 0x0c), /* st480 & akm09911 */ }, }; static int __init mz_mag_init(void) { int ret; pr_info("%s mz_mag_init\n", MZ_MAG_DEV_NAME); #if 1 ret = i2c_register_board_info(2, mz_mag_i2c, 1); if (ret < 0) { pr_err("%s i2c_register_board_info failed\n", MZ_MAG_DEV_NAME); } pr_info("%s i2c_register_board_info success\n", MZ_MAG_DEV_NAME); #endif return 0; } static void __exit mz_mag_exit(void) { i2c_del_driver(&mz_mag_driver); } //module_init(mz_mag_init); //module_exit(mz_mag_exit); postcore_initcall(mz_mag_init); module_i2c_driver(mz_mag_driver); MODULE_AUTHOR("ZhangJiajing "); MODULE_DESCRIPTION("Meizu magnetometer linux driver"); MODULE_LICENSE("GPL"); MODULE_VERSION("1.0.1");