From ac2e46aaa79c897cd5e0813839fab1e6bfdecef5 Mon Sep 17 00:00:00 2001 From: Wangqiang Guo Date: Wed, 13 Oct 2021 07:16:01 +0000 Subject: [PATCH] input: sensor: accel: support 3-Axis accelerometer: da215s Signed-off-by: Wangqiang Guo Change-Id: Ia1c00b45fb86043c730835f95543bbd01dd882ee --- drivers/input/sensors/accel/Kconfig | 6 + drivers/input/sensors/accel/Makefile | 1 + drivers/input/sensors/accel/da215s/Makefile | 2 + drivers/input/sensors/accel/da215s/da215s.c | 343 ++++++++++++++++++ .../input/sensors/accel/da215s/da215s_core.c | 178 +++++++++ .../input/sensors/accel/da215s/da215s_core.h | 24 ++ include/linux/sensor-dev.h | 1 + 7 files changed, 555 insertions(+) create mode 100644 drivers/input/sensors/accel/da215s/Makefile create mode 100644 drivers/input/sensors/accel/da215s/da215s.c create mode 100644 drivers/input/sensors/accel/da215s/da215s_core.c create mode 100644 drivers/input/sensors/accel/da215s/da215s_core.h diff --git a/drivers/input/sensors/accel/Kconfig b/drivers/input/sensors/accel/Kconfig index 724cb2ca1e7a..9cde3e630fac 100644 --- a/drivers/input/sensors/accel/Kconfig +++ b/drivers/input/sensors/accel/Kconfig @@ -134,6 +134,12 @@ config BMA2XX_ACC To have support for your specific gsesnor you will have to select the proper drivers which depend on this option. +config GS_DA215S + tristate "gsensor da215s" + help + To have support for your specific gsesnor you will have to + select the proper drivers which depend on this option. + config GS_DA223 tristate "gsensor da223" help diff --git a/drivers/input/sensors/accel/Makefile b/drivers/input/sensors/accel/Makefile index de250ef4a282..108bb258c2ff 100644 --- a/drivers/input/sensors/accel/Makefile +++ b/drivers/input/sensors/accel/Makefile @@ -17,6 +17,7 @@ obj-$(CONFIG_MPU6500_ACC) += mpu6500_acc.o obj-$(CONFIG_LSM330_ACC) += lsm330_acc.o obj-$(CONFIG_BMA2XX_ACC) += bma2xx.o obj-$(CONFIG_STK8BAXX_ACC) += stk8baxx.o +obj-$(CONFIG_GS_DA215S) += da215s/ obj-$(CONFIG_GS_DA223) += da223.o obj-$(CONFIG_ICM2060X_ACC) += icm2060x_acc.o da223-y := da223_cust.o da223_core.o diff --git a/drivers/input/sensors/accel/da215s/Makefile b/drivers/input/sensors/accel/da215s/Makefile new file mode 100644 index 000000000000..1538390d9785 --- /dev/null +++ b/drivers/input/sensors/accel/da215s/Makefile @@ -0,0 +1,2 @@ +obj-$(CONFIG_GS_DA215S) += gs-da215s.o +gs-da215s-y += da215s.o da215s_core.o diff --git a/drivers/input/sensors/accel/da215s/da215s.c b/drivers/input/sensors/accel/da215s/da215s.c new file mode 100644 index 000000000000..b2e6d1710958 --- /dev/null +++ b/drivers/input/sensors/accel/da215s/da215s.c @@ -0,0 +1,343 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Copyright (c) 2021 Rockchip Electronics Co. Ltd. + * + * Author: Kay Guo + */ +#include +#include +#ifdef CONFIG_HAS_EARLYSUSPEND +#include +#endif +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "da215s_core.h" + +/* Linear acceleration register */ +#define DA215S_CONFIG 0X00 +#define DA215S_CHIP_ID 0x01 +#define ACC_X_LSB 0x02 +#define ACC_X_MSB 0x03 +#define ACC_Y_LSB 0x04 +#define ACC_Y_MSB 0x05 +#define ACC_Z_LSB 0x06 +#define ACC_Z_MSB 0x07 +#define MOTION_FLAG 0x09 +#define NEWDATA_FLAG 0x0A +#define ACTIVE_STATUS 0x0B +#define DA215S_RANGE 0x0F +#define ODR_AXIS 0x10 +#define DA215S_MODE_BW 0x11 +#define SWAP_POLARITY 0x12 +#define INT_ACTIVE_SET1 0x16 +#define INT_DATA_SET2 0x17 +#define INT_MAP1 0x19 +#define INT_MAP2 0x1A +#define INT_CONFIG 0x20 +#define INT_LATCH 0x21 +#define ACTIVE_DUR 0x27 +#define ACTIVE_THS 0x28 + +#define DA215S_CHIPID_DATA 0x13 +#define DA215S_CTRL_NORMAL 0x34 +#define DA215S_CTRL_SUSPEND 0x80 +#define INT_ACTIVE_ENABLE 0x87 +#define INT_NEW_DATA_ENABLE 0x10 + +#define DA215S_OFFSET_MAX 200 +#define DA215S_OFFSET_CUS 130 +#define DA215S_OFFSET_SEN 1024 + +#define GSENSOR_MIN 2 +#define DA215S_PRECISION 14 +#define DA215S_DATA_RANGE (16384*4) +#define DA215S_BOUNDARY (0x1 << (DA215S_PRECISION - 1)) +#define DA215S_GRAVITY_STEP (DA215S_DATA_RANGE/DA215S_BOUNDARY) + + +static int sensor_active(struct i2c_client *client, int enable, int rate) +{ + struct sensor_private_data *sensor = + (struct sensor_private_data *)i2c_get_clientdata(client); + int result = 0; + + sensor->ops->ctrl_data = sensor_read_reg(client, sensor->ops->ctrl_reg); + if (enable) + sensor->ops->ctrl_data &= DA215S_CTRL_NORMAL; + else + sensor->ops->ctrl_data |= DA215S_CTRL_SUSPEND; + + result = sensor_write_reg(client, sensor->ops->ctrl_reg, + sensor->ops->ctrl_data); + if (result) + dev_err(&client->dev, "%s:fail to active sensor\n", __func__); + + dev_dbg(&client->dev, "reg = 0x%x, reg_ctrl = 0x%x, enable= %d\n", + sensor->ops->ctrl_reg, sensor->ops->ctrl_data, enable); + + return result; +} + +static int sensor_init(struct i2c_client *client) +{ + struct sensor_private_data *sensor = + (struct sensor_private_data *)i2c_get_clientdata(client); + int status = 0; + int result = 0; + + result = sensor->ops->active(client, 0, 0); + if (result) { + dev_err(&client->dev, + "%s:line=%d,error\n", __func__, __LINE__); + return result; + } + sensor->status_cur = SENSOR_OFF; + + result = sensor_write_reg(client, 0x00, 0x24); + mdelay(25); + /*+/-4G,14bit normal mode ODR = 62.5hz*/ + result |= sensor_write_reg(client, DA215S_RANGE, 0x61); + result |= sensor_write_reg(client, DA215S_MODE_BW, 0x34); + result |= sensor_write_reg(client, ODR_AXIS, 0x06); + if (result) { + dev_err(&client->dev, "%s:fail to config DA215S_accel.\n", + __func__); + return result; + } + + + /* Enable or Disable for active Interrupt */ + status = sensor_read_reg(client, INT_ACTIVE_SET1); + if (sensor->pdata->irq_enable) + status |= INT_ACTIVE_ENABLE; + else + status &= ~INT_ACTIVE_ENABLE; + result = sensor_write_reg(client, INT_ACTIVE_SET1, status); + if (result) { + dev_err(&client->dev, + "%s:fail to set DA215S_INT_ACTIVE.\n", __func__); + return result; + } + + /* Enable or Disable for new data Interrupt */ + status = sensor_read_reg(client, INT_DATA_SET2); + if (sensor->pdata->irq_enable) + status |= INT_NEW_DATA_ENABLE; + else + status &= ~INT_NEW_DATA_ENABLE; + result = sensor_write_reg(client, INT_DATA_SET2, status); + if (result) { + dev_err(&client->dev, + "%s:fail to set DA215S_INT_NEW_DATA.\n", __func__); + return result; + } + + return result; +} + +static int sensor_convert_data(struct i2c_client *client, + unsigned char low_byte4, unsigned char high_byte8) +{ + s64 result; + + result = ((short)((high_byte8 << 8)|low_byte4)) >> 2; + + return (int)result; +} + + +static int gsensor_report_value(struct i2c_client *client, + struct sensor_axis *axis) +{ + struct sensor_private_data *sensor = + (struct sensor_private_data *)i2c_get_clientdata(client); + if ((abs(sensor->axis.x - axis->x) > GSENSOR_MIN) || + (abs(sensor->axis.y - axis->y) > GSENSOR_MIN) || + (abs(sensor->axis.z - axis->z) > GSENSOR_MIN)) { + input_report_abs(sensor->input_dev, ABS_X, axis->x); + input_report_abs(sensor->input_dev, ABS_Y, axis->y); + input_report_abs(sensor->input_dev, ABS_Z, axis->z); + input_sync(sensor->input_dev); + } + + return 0; +} + +static int sensor_report_value(struct i2c_client *client) +{ + struct sensor_axis axis; + struct sensor_private_data *sensor = + (struct sensor_private_data *)i2c_get_clientdata(client); + struct sensor_platform_data *pdata = sensor->pdata; + unsigned char buffer[6] = {0}; + int x = 0, y = 0, z = 0; + int ret = 0; + int tmp_x = 0, tmp_y = 0, tmp_z = 0; + + if (sensor->ops->read_len < 6) { + dev_err(&client->dev, "%s:Read len is error,len= %d\n", + __func__, sensor->ops->read_len); + return -EINVAL; + } + + *buffer = sensor->ops->read_reg; + sensor_rx_data(client, buffer, sensor->ops->read_len); + if (ret < 0) { + dev_err(&client->dev, + "da215s read data failed, ret = %d\n", ret); + return ret; + } + + /* x,y,z axis is the 12-bit acceleration output */ + x = sensor_convert_data(sensor->client, buffer[0], buffer[1]); + y = sensor_convert_data(sensor->client, buffer[2], buffer[3]); + z = sensor_convert_data(sensor->client, buffer[4], buffer[5]); + + dev_dbg(&client->dev, "%s:x=%d, y=%d, z=%d\n", __func__, x, y, z); + da215s_temp_calibrate(&x, &y, &z); + + dev_dbg(&client->dev, "%s:x=%d, y=%d, z=%d\n", __func__, x, y, z); + + tmp_x = x * DA215S_GRAVITY_STEP; + tmp_y = y * DA215S_GRAVITY_STEP; + tmp_z = z * DA215S_GRAVITY_STEP; + dev_dbg(&client->dev, "%s:temp_x=%d, temp_y=%d, temp_z=%d\n", + __func__, tmp_x, tmp_y, tmp_z); + + axis.x = (pdata->orientation[0]) * tmp_x + (pdata->orientation[1]) * tmp_y + + (pdata->orientation[2]) * tmp_z; + axis.y = (pdata->orientation[3]) * tmp_x + (pdata->orientation[4]) * tmp_y + + (pdata->orientation[5]) * tmp_z; + axis.z = (pdata->orientation[6]) * tmp_x + (pdata->orientation[7]) * tmp_y + + (pdata->orientation[8]) * tmp_z; + dev_dbg(&client->dev, "axis = %d, %d, %d\n", axis.x, axis.y, axis.z); + + gsensor_report_value(client, &axis); + + mutex_lock(&(sensor->data_mutex)); + sensor->axis = axis; + mutex_unlock(&(sensor->data_mutex)); + + if (sensor->pdata->irq_enable) { + ret = sensor_write_reg(client, INT_MAP1, 0); + if (ret) { + dev_err(&client->dev, + "%s:fail to clear DA215S_INT_register.\n", + __func__); + return ret; + } + ret = sensor_write_reg(client, INT_MAP2, 0); + if (ret) { + dev_err(&client->dev, + "%s:fail to clear DA215S_INT_register.\n", + __func__); + return ret; + } + } + + return ret; +} + +/******************************************************************************/ +static int sensor_suspend(struct i2c_client *client) +{ + int result = 0; + +// MI_FUN; +// result = mir3da_set_enable(client, false); +// if (result) { +// MI_ERR("sensor_suspend disable fail!!\n"); +// return result; +// } + + return result; +} + +/******************************************************************************/ +static int sensor_resume(struct i2c_client *client) +{ + int result = 0; + +// MI_FUN; + + /* + * result = mir3da_chip_resume(client); + * if(result) { + * MI_ERR("sensor_resume chip resume fail!!\n"); + * return result; + * } + */ +// result = mir3da_set_enable(client, true); +// if (result) { +// MI_ERR("sensor_resume enable fail!!\n"); +// return result; +// } + + return result; +} + +static struct sensor_operate gsensor_da215s_ops = { + .name = "gs_da215s", + .type = SENSOR_TYPE_ACCEL, + .id_i2c = ACCEL_ID_DA215S, + .read_reg = ACC_X_LSB, + .read_len = 6, + .id_reg = DA215S_CHIP_ID, + .id_data = DA215S_CHIPID_DATA, + .precision = DA215S_PRECISION, + .ctrl_reg = DA215S_MODE_BW, + .int_status_reg = INT_MAP1, + .range = {-DA215S_DATA_RANGE, DA215S_DATA_RANGE}, + .trig = IRQF_TRIGGER_LOW | IRQF_ONESHOT, + .active = sensor_active, + .init = sensor_init, + .report = sensor_report_value, + .suspend = sensor_suspend, + .resume = sensor_resume, +}; + +static int gsensor_da215s_probe(struct i2c_client *client, + const struct i2c_device_id *devid) +{ + return sensor_register_device(client, NULL, devid, &gsensor_da215s_ops); +} + +static int gsensor_da215s_remove(struct i2c_client *client) +{ + return sensor_unregister_device(client, NULL, &gsensor_da215s_ops); +} + +static const struct i2c_device_id gsensor_da215s_id[] = { + {"gs_da215s", ACCEL_ID_DA215S}, + {} +}; + +static struct i2c_driver gsensor_da215s_driver = { + .probe = gsensor_da215s_probe, + .remove = gsensor_da215s_remove, + .shutdown = sensor_shutdown, + .id_table = gsensor_da215s_id, + .driver = { + .name = "gsensor_da215s", + #ifdef CONFIG_PM + .pm = &sensor_pm_ops, + #endif + }, +}; + +module_i2c_driver(gsensor_da215s_driver); + +MODULE_AUTHOR("Guo Wangqiang "); +MODULE_DESCRIPTION("da215s 3-Axis accelerometer driver"); +MODULE_LICENSE("GPL"); diff --git a/drivers/input/sensors/accel/da215s/da215s_core.c b/drivers/input/sensors/accel/da215s/da215s_core.c new file mode 100644 index 000000000000..8e675e56c3c9 --- /dev/null +++ b/drivers/input/sensors/accel/da215s/da215s_core.c @@ -0,0 +1,178 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Copyright (c) 2021 Rockchip Electronics Co. Ltd. + * + * Author: Kay Guo + */ + +#include +#include "da215s_core.h" + +#define DA215S_OFFSET_THRESHOLD 20 +#define PEAK_LVL 800 +#define STICK_LSB 2000 +#define AIX_HISTORY_SIZE 20 + + +static int z_offset; + +#define TEMP_CALIBRATE_STATIC_THRESHOLD 60 +#define TEMP_CALIBRATE_STATIC_COUNT 12 + +static int squareRoot(int val) +{ + int r = 0, x; + int shift; + + if (val < 0) + return 0; + + for (shift = 0; shift < 32; shift += 2) { + x = 0x40000000l >> shift; + if ((x+r) <= val) { + val -= x + r; + r = (r >> 1) | x; + } else { + r = r >> 1; + } + } + + return r; +} + +static int da215s_temp_calibrate_detect_static(short x, short y, short z) +{ + static int count_static; + static short temp_x[TEMP_CALIBRATE_STATIC_COUNT]; + static short temp_y[TEMP_CALIBRATE_STATIC_COUNT]; + static short temp_z[TEMP_CALIBRATE_STATIC_COUNT]; + static short max_x, max_y, max_z; + static short min_x, min_y, min_z; + static char is_first = 1; + int i, delta_sum = 0; + + count_static++; + + if (is_first) { + temp_x[0] = x; + temp_y[0] = y; + temp_z[0] = z; + for (i = 1; i < TEMP_CALIBRATE_STATIC_COUNT; i++) { + temp_x[i] = temp_x[0]; + temp_y[i] = temp_y[0]; + temp_z[i] = temp_z[0]; + } + is_first = 0; + } else { + max_x = min_x = temp_x[1]; + max_y = min_y = temp_y[1]; + max_z = min_z = temp_z[1]; + + for (i = 0; i < TEMP_CALIBRATE_STATIC_COUNT; i++) { + if (i == (TEMP_CALIBRATE_STATIC_COUNT-1)) { + temp_x[i] = x; + temp_y[i] = y; + temp_z[i] = z; + } else { + temp_x[i] = temp_x[i+1]; + temp_y[i] = temp_y[i+1]; + temp_z[i] = temp_z[i+1]; + } + max_x = (max_x > temp_x[i]) ? max_x:temp_x[i]; + max_y = (max_y > temp_y[i]) ? max_y:temp_y[i]; + max_z = (max_z > temp_z[i]) ? max_z:temp_z[i]; + + min_x = (min_x < temp_x[i]) ? min_x:temp_x[i]; + min_y = (min_y < temp_y[i]) ? min_y:temp_y[i]; + min_z = (min_z < temp_z[i]) ? min_z:temp_z[i]; + } + } + + if (count_static > TEMP_CALIBRATE_STATIC_COUNT) { + count_static = TEMP_CALIBRATE_STATIC_COUNT; + delta_sum = abs(max_x - min_x) + abs(max_y - min_y) + abs(max_z - min_z); + + if (delta_sum < TEMP_CALIBRATE_STATIC_THRESHOLD) + return 1; + } + + return 0; +} + +int da215s_temp_calibrate(int *x, int *y, int *z) +{ + int tem_z = 0; + int cus = MIR3DA_OFFSET_MAX-MIR3DA_OFFSET_CUS; + int is_static = 0; + short lz_offset; + + *z = *z + z_offset; + lz_offset = (*z) % 10; + + if ((abs(*x) < MIR3DA_OFFSET_MAX) && (abs(*y) < MIR3DA_OFFSET_MAX)) { + is_static = da215s_temp_calibrate_detect_static(*x, *y, *z-z_offset); + tem_z = squareRoot(MIR3DA_OFFSET_SEN*MIR3DA_OFFSET_SEN - + (*x)*(*x) - (*y)*(*y)) + lz_offset; + if (z_offset == 0) { + if (is_static == 1) + z_offset = (*z >= 0) ? (tem_z-*z) : (-tem_z-*z); + *z = ((*z >= 0) ? (1) : (-1))*tem_z; + } else if (is_static) { + if (abs(abs(*z) - MIR3DA_OFFSET_SEN) > MIR3DA_OFFSET_CUS) { + *z = ((*z >= 0) ? (1) : (-1))*tem_z; + z_offset = 0; + } + } + + *x = (*x) * MIR3DA_OFFSET_CUS / MIR3DA_OFFSET_MAX; + *y = (*y) * MIR3DA_OFFSET_CUS / MIR3DA_OFFSET_MAX; + + } else if ((abs((abs(*x)-MIR3DA_OFFSET_SEN)) < MIR3DA_OFFSET_MAX) && + (abs(*y) < MIR3DA_OFFSET_MAX) && (z_offset)) { + if (abs(*x) > MIR3DA_OFFSET_SEN) { + *x = (*x > 0) ? + (*x - (abs(*x) - MIR3DA_OFFSET_SEN) * cus/MIR3DA_OFFSET_MAX) : + (*x + (abs(*x) - MIR3DA_OFFSET_SEN) * cus/MIR3DA_OFFSET_MAX); + } else { + *x = (*x > 0) ? + (*x + (MIR3DA_OFFSET_SEN-abs(*x)) * cus/MIR3DA_OFFSET_MAX) : + (*x - (MIR3DA_OFFSET_SEN-abs(*x)) * cus/MIR3DA_OFFSET_MAX); + } + *y = (*y) * MIR3DA_OFFSET_CUS/MIR3DA_OFFSET_MAX; + } else if ((abs((abs(*y) - MIR3DA_OFFSET_SEN)) < MIR3DA_OFFSET_MAX) && + (abs(*x) < MIR3DA_OFFSET_MAX) && (z_offset)) { + if (abs(*y) > MIR3DA_OFFSET_SEN) { + *y = (*y > 0) ? + (*y - (abs(*y) - MIR3DA_OFFSET_SEN) * cus/MIR3DA_OFFSET_MAX) : + (*y + (abs(*y) - MIR3DA_OFFSET_SEN) * cus/MIR3DA_OFFSET_MAX); + } else { + *y = (*y > 0) ? + (*y + (MIR3DA_OFFSET_SEN-abs(*y)) * cus/MIR3DA_OFFSET_MAX) : + (*y - (MIR3DA_OFFSET_SEN-abs(*y)) * cus/MIR3DA_OFFSET_MAX); + } + *x = (*x) * MIR3DA_OFFSET_CUS/MIR3DA_OFFSET_MAX; + } else if (z_offset == 0) { + if ((abs(*x) < MIR3DA_OFFSET_MAX) && (abs((*y > 0) ? + (MIR3DA_OFFSET_SEN - *y) : (MIR3DA_OFFSET_SEN + *y)) < MIR3DA_OFFSET_MAX)) { + *z = ((*z >= 0) ? + (1) : (-1)) * abs(*x) * MIR3DA_OFFSET_CUS/MIR3DA_OFFSET_MAX; + } else if ((abs(*y) < MIR3DA_OFFSET_MAX) && (abs((*x > 0) ? + (MIR3DA_OFFSET_SEN - *x) : + (MIR3DA_OFFSET_SEN + *x)) < MIR3DA_OFFSET_MAX)) { + *z = ((*z >= 0) ? + (1) : (-1)) * abs(*y) * MIR3DA_OFFSET_CUS/MIR3DA_OFFSET_MAX; + } else { + tem_z = squareRoot(MIR3DA_OFFSET_SEN*MIR3DA_OFFSET_SEN - + (*x) * (*x) - (*y) * (*y)) + lz_offset; + *z = ((*z >= 0) ? (1) : (-1)) * tem_z; + } + } + + if (z_offset) + return 0; + else + return -1; +} + + + diff --git a/drivers/input/sensors/accel/da215s/da215s_core.h b/drivers/input/sensors/accel/da215s/da215s_core.h new file mode 100644 index 000000000000..0c2f43a804f8 --- /dev/null +++ b/drivers/input/sensors/accel/da215s/da215s_core.h @@ -0,0 +1,24 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * Copyright (c) 2021 Rockchip Electronics Co. Ltd. + * + * Author: Kay Guo + */ + + +#ifndef __DA215S_CORE_H__ +#define __DA215S_CORE_H__ + +#define DA215S_OFFSET_TEMP_SOLUTION 0 + +#define MIR3DA_OFFSET_MAX 200 +#define MIR3DA_OFFSET_CUS 130 +#define MIR3DA_OFFSET_SEN 1024 + +int da215s_temp_calibrate(int *x, int *y, int *z); + +#endif /* __DA215S_CORE_H__ */ + + + + diff --git a/include/linux/sensor-dev.h b/include/linux/sensor-dev.h index 6ae9c16e67a3..eeb2dfa5708d 100644 --- a/include/linux/sensor-dev.h +++ b/include/linux/sensor-dev.h @@ -67,6 +67,7 @@ enum sensor_id { ACCEL_ID_STK8BAXX, ACCEL_ID_MIR3DA, ACCEL_ID_ICM2060X, + ACCEL_ID_DA215S, COMPASS_ID_ALL, COMPASS_ID_AK8975, COMPASS_ID_AK8963,