mtk平台计步器移植

        LSM6DS33是一款加速度计和陀螺仪,还支持计步功能。刚好手上有这款传感器,便移植下。
kernel-3.18/arch/arm64/configs/len6737t_66_n_defconfig
CONFIG_CUSTOM_KERNEL_STEP_COUNTER=y

这是因为加了这个才编译step_counter这个文件夹。

kernel-3.18/drivers/misc/mediatek/Makefile
obj-$(CONFIG_CUSTOM_KERNEL_STEP_COUNTER)        += step_counter/

源码路径kernel-3.18/drivers/misc/mediatek/accelerometer/lsm6ds3,相关计步代码摘录如下

#define LSM6DS3_CONFIG_PEDO_THS_MIN			0x0F
#define LSM6DS3_SUCCESS					0
#define LSM6DS3_ERR_I2C					-1
#define LSM6DS3_STEP_COUNTER_L	0x4B
#define LSM6DS3_STEP_COUNTER_H	0x4C
#define LSM6DS3_RAM_ACCESS	0X01
#define LSM6DS3_RAM_PAGE_MASK	0x80
#define LSM6DS3_CTRL10_C	0x19
#define LSM6DS3_PEDO_RST_STEP_MASK	0x02
#define LSM6DS3_ERR_STATUS			  -3
#define LSM6DS3_CTRL1_XL 0x10
#define LSM6DS3_ACC_ODR_MASK	0xF0
#define LSM6DS3_ACC_ODR_POWER_DOWN		0x00
#define LSM6DS3_TAP_CFG	0x58
#define LSM6DS3_PEDO_EN_MASK 0x40
#define	LSM6DS3_ACC_GYRO_FUNC_EN_MASK	0x04

typedef enum {
	LSM6DS3_ACC_GYRO_PEDO_RST_STEP_DISABLED	= 0x00,
	LSM6DS3_ACC_GYRO_PEDO_RST_STEP_ENABLED	= 0x02,
} LSM6DS3_ACC_GYRO_PEDO_RST_STEP_t;
typedef enum {
	LSM6DS3_ACC_GYRO_FUNC_EN_DISABLED		= 0x00,
	LSM6DS3_ACC_GYRO_FUNC_EN_ENABLED		= 0x04,
} LSM6DS3_ACC_GYRO_FUNC_EN_t;
typedef enum {
	LSM6DS3_ACC = 1,
	LSM6DS3_STEP_C = 2,
	LSM6DS3_TILT = 3,
} LSM6DS3_INIT_TYPE;
typedef enum {
	LSM6DS3_ACC_GYRO_RAM_PAGE_DISABLED		= 0x00,
	LSM6DS3_ACC_GYRO_RAM_PAGE_ENABLED		= 0x80,
} LSM6DS3_ACC_GYRO_RAM_PAGE_t;
typedef enum {
	LSM6DS3_ACC_GYRO_PEDO_EN_DISABLED		= 0x00,
	LSM6DS3_ACC_GYRO_PEDO_EN_ENABLED		= 0x40,
} LSM6DS3_ACC_GYRO_PEDO_EN_t;
static DEFINE_MUTEX(lsm6ds3_init_mutex);
static int lsm6ds3_acc_init_flag;			/*initial in module_init     = -1;*/
static bool tilt_enable_status;			/*initial in module_init      = false;*/
static unsigned long lsm6ds3_init_flag_test;
static bool pedo_enable_status;			/*initial in module_init      = false;*/

static int LSM6DS3_acc_SetPowerMode(struct i2c_client *client, bool enable)
{
	u8 databuf[2] = {0};
	int res = 0;
	struct lsm6ds3h_i2c_data *obj = i2c_get_clientdata(client);

	if (enable == sensor_power) {

		GSE_LOG("Sensor power status is newest!\n");
		return LSM6DS3_SUCCESS;
	}

	if (hwmsen_read_byte(client, LSM6DS3_CTRL1_XL, databuf)) {

		GSE_ERR("read lsm6ds3 power ctl register err!\n");
		return LSM6DS3_ERR_I2C;
	}
	GSE_LOG("LSM6DS3_CTRL1_XL:databuf[0] =  %x!\n", databuf[0]);


	if (true == enable) {

		databuf[0] &= ~LSM6DS3_ACC_ODR_MASK;/*clear lsm6ds3 gyro ODR bits*/
		databuf[0] |= obj->sample_rate;/*LSM6DS3_ACC_ODR_104HZ; //default set 100HZ for LSM6DS3 acc*/
	} else {

		databuf[0] &= ~LSM6DS3_ACC_ODR_MASK;/*clear lsm6ds3 acc ODR bits*/
		databuf[0] |= LSM6DS3_ACC_ODR_POWER_DOWN;
	}
	databuf[1] = databuf[0];
	databuf[0] = LSM6DS3_CTRL1_XL;
	res = i2c_master_send(client, databuf, 0x2);
	if (res <= 0) {

		GSE_LOG("LSM6DS3 set power mode: ODR 100hz failed!\n");
		return LSM6DS3_ERR_I2C;
	} else {

		GSE_LOG("set LSM6DS3 gyro power mode:ODR 100HZ ok %d!\n", enable);
	}

	sensor_power = enable;

	return LSM6DS3_SUCCESS;
}

static int lsm6ds3_step_c_enable_significant(int en)
{
	int res = 0;
	return res;
}

static int LSM6DS3_acc_Enable_Func(struct i2c_client *client, LSM6DS3_ACC_GYRO_FUNC_EN_t newValue)
{
	u8 databuf[2] = {0};
	int res = 0;
	GSE_FUN();

	if (hwmsen_read_byte(client, LSM6DS3_CTRL10_C, databuf)) {

		GSE_ERR("%s read LSM6DS3_CTRL10_C register err!\n", __func__);
		return LSM6DS3_ERR_I2C;
	} else {

		GSE_LOG("%s read acc data format register: 0x%x\n", __func__, databuf[0]);
	}
	databuf[0] &= ~LSM6DS3_ACC_GYRO_FUNC_EN_MASK;/*clear */
	databuf[0] |= newValue;

	databuf[1] = databuf[0];
	databuf[0] = LSM6DS3_CTRL10_C;
	res = i2c_master_send(client, databuf, 0x2);
	if (res <= 0) {

		GSE_ERR("%s write LSM6DS3_CTRL10_C register err!\n", __func__);
		return LSM6DS3_ERR_I2C;
	}

	return LSM6DS3_SUCCESS;
}

static int LSM6DS3_acc_SetSampleRate(struct i2c_client *client, u8 sample_rate)
{
	u8 databuf[2] = {0};
	int res = 0;
	GSE_FUN();

	res = LSM6DS3_acc_SetPowerMode(client, true);	/*set Sample Rate will enable power and should changed power status*/
	if (res != LSM6DS3_SUCCESS) {

		return res;
	}

	if (hwmsen_read_byte(client, LSM6DS3_CTRL1_XL, databuf)) {

		GSE_ERR("read acc data format register err!\n");
		return LSM6DS3_ERR_I2C;
	} else {

		GSE_LOG("read  acc data format register: 0x%x\n", databuf[0]);
	}

	databuf[0] &= ~LSM6DS3_ACC_ODR_MASK;	/*clear*/
	databuf[0] |= sample_rate;

	databuf[1] = databuf[0];
	databuf[0] = LSM6DS3_CTRL1_XL;

	res = i2c_master_send(client, databuf, 0x2);
	if (res <= 0) {

		GSE_ERR("write sample rate register err!\n");
		return LSM6DS3_ERR_I2C;
	}

	return LSM6DS3_SUCCESS;
}

static int LSM6DS3_acc_Enable_Pedometer_Func(struct i2c_client *client, bool enable)
{
	u8 databuf[2] = {0};
	int res = 0;
	GSE_FUN();

	if (hwmsen_read_byte(client, LSM6DS3_TAP_CFG, databuf)) {

		GSE_ERR("read acc data format register err!\n");
		return LSM6DS3_ERR_I2C;
	} else {

		GSE_LOG("read  acc data format register: 0x%x\n", databuf[0]);
	}

	if (enable) {

		databuf[0] &= ~LSM6DS3_PEDO_EN_MASK;	/*clear */
		databuf[0] |= LSM6DS3_ACC_GYRO_PEDO_EN_ENABLED;
	} else {

		databuf[0] &= ~LSM6DS3_PEDO_EN_MASK;	/*clear */
		databuf[0] |= LSM6DS3_ACC_GYRO_PEDO_EN_DISABLED;
	}

	databuf[1] = databuf[0];
	databuf[0] = LSM6DS3_TAP_CFG;
	res = i2c_master_send(client, databuf, 0x2);
	if (res < 0) {

		GSE_ERR("write enable pedometer func register err!\n");
		return LSM6DS3_ERR_I2C;
	}

	return LSM6DS3_SUCCESS;
}


static int LSM6DS3_W_Open_RAM_Page(struct i2c_client *client, LSM6DS3_ACC_GYRO_RAM_PAGE_t newValue)
{
	u8 databuf[2] = {0};
	int res = 0;
	GSE_FUN();

	if (hwmsen_read_byte(client, LSM6DS3_RAM_ACCESS, databuf)) {

		GSE_ERR("%s read LSM6DS3_RAM_ACCESS register err!\n", __func__);
		return LSM6DS3_ERR_I2C;
	} else {

		GSE_LOG("%s read acc data format register: 0x%x\n", __func__, databuf[0]);
	}
	databuf[0] &= ~LSM6DS3_RAM_PAGE_MASK;/*clear */
	databuf[0] |= newValue;

	databuf[1] = databuf[0];
	databuf[0] = LSM6DS3_RAM_ACCESS;
	res = i2c_master_send(client, databuf, 0x2);
	if (res <= 0) {

		GSE_ERR("%s write LSM6DS3_RAM_ACCESS register err!\n", __func__);
		return LSM6DS3_ERR_I2C;
	}

	return LSM6DS3_SUCCESS;
}

static int LSM6DS3_Reset_Pedo_Data(struct i2c_client *client, LSM6DS3_ACC_GYRO_PEDO_RST_STEP_t newValue)
{
	u8 databuf[2] = {0};
	int res = 0;
	GSE_FUN();

	if (hwmsen_read_byte(client, LSM6DS3_CTRL10_C, databuf)) {

		GSE_ERR("%s read LSM6DS3_CTRL10_C register err!\n", __func__);
		return LSM6DS3_ERR_I2C;
	} else {

		GSE_LOG("%s read acc LSM6DS3_CTRL10_C data format register: 0x%x\n", __func__, databuf[0]);
	}
	databuf[0] &= ~LSM6DS3_PEDO_RST_STEP_MASK;/*clear */
	databuf[0] |= newValue;

	databuf[1] = databuf[0];
	databuf[0] = LSM6DS3_CTRL10_C;
	res = i2c_master_send(client, databuf, 0x2);
	if (res <= 0) {

		GSE_ERR("%s write LSM6DS3_CTRL10_C register err!\n", __func__);
		return LSM6DS3_ERR_I2C;
	}

	return LSM6DS3_SUCCESS;
}

static int LSM6DS3_Write_PedoThreshold(struct i2c_client *client, u8 newValue)
{
	u8 databuf[2] = {0};
	int res = 0;
	GSE_FUN();

	res = LSM6DS3_W_Open_RAM_Page(client, LSM6DS3_ACC_GYRO_RAM_PAGE_ENABLED);
	if (LSM6DS3_SUCCESS != res) {

		return res;
	}
	if (hwmsen_read_byte(client, LSM6DS3_CONFIG_PEDO_THS_MIN, databuf)) {

		GSE_ERR("%s read LSM6DS3_CTRL10_C register err!\n", __func__);
		return LSM6DS3_ERR_I2C;
	} else {

		GSE_LOG("%s read acc data format register: 0x%x\n", __func__, databuf[0]);
	}

	databuf[0] &= ~0x1F;
	databuf[0] |= (newValue & 0x1F);

	databuf[1] = databuf[0];
	databuf[0] = LSM6DS3_CONFIG_PEDO_THS_MIN;
	res = i2c_master_send(client, databuf, 0x2);
	if (res <= 0) {

		GSE_ERR("%s write LSM6DS3_CTRL10_C register err!\n", __func__);
		return LSM6DS3_ERR_I2C;
	}

	databuf[0] = 0x14;
	databuf[1] = 0x6e;
	res = i2c_master_send(client, databuf, 0x2);
	if (res <= 0) {

		GSE_ERR("%s write LSM6DS3_CTRL10_C register err!\n", __func__);
		return LSM6DS3_ERR_I2C;
	}
	res = LSM6DS3_W_Open_RAM_Page(client, LSM6DS3_ACC_GYRO_RAM_PAGE_DISABLED);
	if (LSM6DS3_SUCCESS != res) {

		GSE_ERR("%s write LSM6DS3_W_Open_RAM_Page failed!\n", __func__);
		return res;
	}

	return LSM6DS3_SUCCESS;
}

static int LSM6DS3_enable_pedo(struct i2c_client *client, bool enable)
{

	int res = 0;
	struct lsm6ds3h_i2c_data *obj = i2c_get_clientdata(client);

	if (true == enable) {

		/*/software reset
		//set ODR to 26 hz
		//res = LSM6DS3_acc_SetSampleRate(client, LSM6DS3_ACC_ODR_26HZ);*/
		res = LSM6DS3_acc_SetSampleRate(client, obj->sample_rate);
		if (LSM6DS3_SUCCESS == res) {

			GSE_LOG(" %s set 26hz odr to acc\n", __func__);
		}
		/*enable tilt feature and pedometer feature*/
		res = LSM6DS3_acc_Enable_Pedometer_Func(client, enable);
		if (res != LSM6DS3_SUCCESS) {

			GSE_LOG(" LSM6DS3_acc_Enable_Pedometer_Func failed!\n");
			return LSM6DS3_ERR_STATUS;
		}

		res = LSM6DS3_acc_Enable_Func(client, LSM6DS3_ACC_GYRO_FUNC_EN_ENABLED);
		if (res != LSM6DS3_SUCCESS) {

			GSE_LOG(" LSM6DS3_acc_Enable_Func failed!\n");
			return LSM6DS3_ERR_STATUS;
		}
		res = LSM6DS3_Write_PedoThreshold(client, 0x11);/* set threshold to a certain value here*/
		if (res != LSM6DS3_SUCCESS) {

			GSE_LOG(" LSM6DS3_Write_PedoThreshold failed!\n");
			return LSM6DS3_ERR_STATUS;
		}
		res = LSM6DS3_Reset_Pedo_Data(client, LSM6DS3_ACC_GYRO_PEDO_RST_STEP_ENABLED);

		if (res != LSM6DS3_SUCCESS) {

			GSE_LOG(" LSM6DS3_Reset_Pedo_Data failed!\n");
			return LSM6DS3_ERR_STATUS;
		}
	} else {

		res = LSM6DS3_acc_Enable_Pedometer_Func(client, enable);
		if (res != LSM6DS3_SUCCESS) {

			GSE_LOG(" LSM6DS3_acc_Enable_Func failed at disable pedo!\n");
			return LSM6DS3_ERR_STATUS;
		}
		/*do not turn off the func*/
		if (!enable_status && !tilt_enable_status) {

			res = LSM6DS3_acc_SetPowerMode(client, false);
			if (res != LSM6DS3_SUCCESS) {

				GSE_LOG(" LSM6DS3_acc_SetPowerMode failed at disable pedo!\n");
				return LSM6DS3_ERR_STATUS;
			}
		}
	}
	return LSM6DS3_SUCCESS;
}

static int LSM6DS3_Get_Pedo_DataReg(struct i2c_client *client, u16 *Value)
{
	u8 databuf[2] = {0};
	GSE_FUN();

	if (hwmsen_read_block(client, LSM6DS3_STEP_COUNTER_L, databuf, 2)) {

		GSE_ERR("LSM6DS3 read acc data  error\n");
		return -2;
	}

	*Value = (databuf[1]<<8)|databuf[0];

	return LSM6DS3_SUCCESS;
}

static int lsm6ds3_step_c_open_report_data(int open)
{

	return LSM6DS3_SUCCESS;
}
static int lsm6ds3_step_c_enable_nodata(int en)
{
	int res = 0;
	int value = en;
	int err = 0;
	struct lsm6ds3h_i2c_data *priv = obj_i2c_data;

	if (priv == NULL) {

		GSE_ERR("%s obj_i2c_data is NULL!\n", __func__);
		return -1;
	}

	if (value == 1) {

		pedo_enable_status = true;
		res = LSM6DS3_enable_pedo(priv->client, true);
		if (LSM6DS3_SUCCESS != res) {

			GSE_LOG("LSM6DS3_enable_pedo failed at open action!\n");
			return res;
		}
	} else {

		pedo_enable_status = false;
		res = LSM6DS3_enable_pedo(priv->client, false);
		if (LSM6DS3_SUCCESS != res) {

			GSE_LOG("LSM6DS3_enable_pedo failed at close action!\n");
			return res;
		}

	}

	GSE_LOG("lsm6ds3_step_c_enable_nodata OK!\n");
		return err;
}
static int lsm6ds3_step_c_enable_step_detect(int en)
{
	return lsm6ds3_step_c_enable_nodata(en);
}

static int lsm6ds3_step_c_set_delay(u64 delay)
{

	return 0;
}

static int lsm6ds3_step_c_get_data(uint32_t *value, int *status)
{
	int err = 0;
	u16 pedo_data = 0;

	struct lsm6ds3h_i2c_data *priv = obj_i2c_data;
	err = LSM6DS3_Get_Pedo_DataReg(priv->client, &pedo_data);
	*value = (u32)pedo_data;
	*status = SENSOR_STATUS_ACCURACY_MEDIUM;
	return err;
}
static int lsm6ds3_step_c_get_data_step_d(uint32_t *value, int *status)
{
	return 0;
}
static int lsm6ds3_step_c_get_data_significant(uint32_t *value, int *status)
{
	return 0;
}

static int lsm6ds3_step_c_local_init(void)
{
	int res = 0;

	struct step_c_control_path step_ctl = {0};
	struct step_c_data_path step_data = {0};

	mutex_lock(&lsm6ds3_init_mutex);

	set_bit(LSM6DS3_STEP_C, &lsm6ds3_init_flag_test);

	if ((0 == test_bit(LSM6DS3_ACC, &lsm6ds3_init_flag_test)) \
		&& (0 == test_bit(LSM6DS3_TILT, &lsm6ds3_init_flag_test))) {

		//res = lsm6ds3_local_init_common();
		if (res < 0) {

			goto lsm6ds3_step_c_local_init_failed;
		}

	}

	if (lsm6ds3_acc_init_flag == -1) {

		mutex_unlock(&lsm6ds3_init_mutex);
		GSE_ERR("%s init failed!\n", __FUNCTION__);
		return -1;
	} else {

		step_ctl.open_report_data = lsm6ds3_step_c_open_report_data;
		step_ctl.enable_nodata = lsm6ds3_step_c_enable_nodata;
		step_ctl.enable_step_detect  = lsm6ds3_step_c_enable_step_detect;
		step_ctl.step_c_set_delay = lsm6ds3_step_c_set_delay;
		step_ctl.step_d_set_delay = lsm6ds3_step_c_set_delay;
		step_ctl.is_report_input_direct = false;
		step_ctl.is_support_batch = false;
//#ifdef LSM6DS3_SIGNIFICANT_MOTION
		step_ctl.enable_significant = lsm6ds3_step_c_enable_significant;
//#endif

		res = step_c_register_control_path(&step_ctl);
		if (res) {

			 GSE_ERR("register step counter control path err\n");
			goto lsm6ds3_step_c_local_init_failed;
		}

		step_data.get_data = lsm6ds3_step_c_get_data;
		step_data.get_data_step_d = lsm6ds3_step_c_get_data_step_d;
		step_data.get_data_significant = lsm6ds3_step_c_get_data_significant;

		step_data.vender_div = 1;
		res = step_c_register_data_path(&step_data);
		if (res) {

			GSE_ERR("register step counter data path err= %d\n", res);
			goto lsm6ds3_step_c_local_init_failed;
		}
	}
	mutex_unlock(&lsm6ds3_init_mutex);
	return 0;

lsm6ds3_step_c_local_init_failed:
	mutex_unlock(&lsm6ds3_init_mutex);
	GSE_ERR("%s init failed!\n", __FUNCTION__);
	return res;

}
static int lsm6ds3_step_c_local_uninit(void)
{
	clear_bit(LSM6DS3_STEP_C, &lsm6ds3_init_flag_test);
		return 0;
}

/*----------------------------------------------------------------------------*/
static int lsm6ds3_step_c_local_init(void);
static int lsm6ds3_step_c_local_uninit(void);
static struct step_c_init_info  lsm6ds3_step_c_init_info = {
	.name   = "LSM6DS3_STEP_C",
	.init   = lsm6ds3_step_c_local_init,
	.uninit = lsm6ds3_step_c_local_uninit,
};

step_c_driver_add(&lsm6ds3_step_c_init_info); /*step counter*/

先让上层知道底层支持计步功能,修改如下

device/lentek/len6737t_66_n/ProjectConfig.mk
CUSTOM_KERNEL_STEP_COUNTER= yes

这是因为定义CUSTOM_KERNEL_STEP_COUNTER,数组sSensorList才保存计步传感器信息。

vendor/mediatek/proprietary/hardware/sensor/sensors.c
struct sensor_t sSensorList[] ={

#ifdef CUSTOM_KERNEL_STEP_COUNTER
    {
        .name       = STEP_COUNTER,
        .vendor     = STEP_COUNTER_VENDER,
        .version    = 1,
        .handle     = ID_STEP_COUNTER+ID_OFFSET,
        .type       = SENSOR_TYPE_STEP_COUNTER,
        .maxRange   = STEP_COUNTER_RANGE,//600.0f,
        .resolution = STEP_COUNTER_RESOLUTION,//0.0016667f,
        .power      = STEP_COUNTER_POWER,//0.25f,
        .minDelay   = STEP_COUNTER_MINDELAY,
        .flags      = SENSOR_FLAG_ON_CHANGE_MODE,
        .reserved   = {}
    },
    {
        .name       = STEP_DETECTOR,
        .vendor     = STEP_DETECTOR_VENDER,
        .version    = 1,
        .handle     = ID_STEP_DETECTOR+ID_OFFSET,
        .type       = SENSOR_TYPE_STEP_DETECTOR,
        .maxRange   = STEP_DETECTOR_RANGE,//600.0f,
        .resolution = STEP_DETECTOR_RESOLUTION,//0.0016667f,
        .power      = STEP_DETECTOR_POWER,//0.25f,
        .minDelay   = STEP_DETECTOR_MINDELAY,
        .flags      = SENSOR_FLAG_SPECIAL_REPORTING_MODE,
        .reserved   = {}
    },
#endif
}
mmm vendor/mediatek/proprietary/hardware/sensor编译生成相应的hal库,并push

到机器了,如sensors.mt6737t.so,重启后用安兔兔检测能看到手机支持了计步功能。

这时发现驱动并没有使能,找到如下代码,让ALOGD输入调试信息
vendor/mediatek/proprietary/hardware/sensor/StepCounter.cpp
#undef NDEBUG

adb logcat -s STEP_COUNTER能找到出错原因,原来是权限不够导致不能使能,在device/mediatek/mt6735/init.mt6735.rc加入

    chmod 0660 /sys/class/misc/m_step_c_misc/step_cactive
    chmod 0660 /sys/class/misc/m_step_c_misc/step_cbatch
    chmod 0660 /sys/class/misc/m_step_c_misc/step_cdelay
    chmod 0660 /sys/class/misc/m_step_c_misc/step_cdevnum
    chmod 0660 /sys/class/misc/m_step_c_misc/step_cenablenodata
    chmod 0660 /sys/class/misc/m_step_c_misc/step_cflush
    chown system system /sys/class/misc/m_step_c_misc/step_cactive
    chown system system /sys/class/misc/m_step_c_misc/step_cbatch
    chown system system /sys/class/misc/m_step_c_misc/step_cdelay
    chown system system /sys/class/misc/m_step_c_misc/step_cdevnum
    chown system system /sys/class/misc/m_step_c_misc/step_cenablenodata
    chown system system /sys/class/misc/m_step_c_misc/step_cflush
这时,流程已基本没有问题的,晃动手机,上层也读到了数据,至此移植功能完成。

猜你喜欢

转载自blog.csdn.net/mike8825/article/details/79628668