From cf6288f94fff69e2415c8a1a6927288379edf6f9 Mon Sep 17 00:00:00 2001 From: yanyh2 Date: Thu, 22 Aug 2019 16:02:02 +0800 Subject: [PATCH] stmvl53l1: support calibration data dynamic loading In order to support run-time power management with dynamic power on/off, it has to support calibration data loading after power on TOF. if not,xtalk calibration data will change after ranging and override correct xtalk calib data Change-Id: Ia67e3bb806ea8295bb7829888c201f22a0c0b59d Signed-off-by: yanyh2 Reviewed-on: https://gerrit.mot.com/1407816 SLTApproved: Slta Waiver SME-Granted: SME Approvals Granted Tested-by: Jira Key Reviewed-by: Jianqi Yang Submit-Approved: Jira Key (cherry picked from commit 799a22316b6c3b8809a448ff1fb37db9514db9ab) Reviewed-on: https://gerrit.mot.com/1408713 Reviewed-by: Huosheng Liao --- drivers/input/misc/vl53L1/stmvl53l1.h | 1 + drivers/input/misc/vl53L1/stmvl53l1_module.c | 35 +++++++++++++++----- 2 files changed, 28 insertions(+), 8 deletions(-) diff --git a/drivers/input/misc/vl53L1/stmvl53l1.h b/drivers/input/misc/vl53L1/stmvl53l1.h index 467d5e5eac2a..dc760c0eafae 100644 --- a/drivers/input/misc/vl53L1/stmvl53l1.h +++ b/drivers/input/misc/vl53L1/stmvl53l1.h @@ -245,6 +245,7 @@ struct stmvl53l1_data { int16_t xtalk_y; /*!< crosstalk compensation y plane gradient kcps */ int16_t inner_offset; /*!< inner offset mm */ int16_t outer_offset; /*!< outer offset mm */ + VL53L1_CalibrationData_t *cali_data; /*!< full calibration data */ /* Range Data and stat */ struct range_t { diff --git a/drivers/input/misc/vl53L1/stmvl53l1_module.c b/drivers/input/misc/vl53L1/stmvl53l1_module.c index bd79670c3852..1280328ce97d 100644 --- a/drivers/input/misc/vl53L1/stmvl53l1_module.c +++ b/drivers/input/misc/vl53L1/stmvl53l1_module.c @@ -773,11 +773,15 @@ static int stmvl53l1_sendparams(struct stmvl53l1_data *data) sizeof(VL53L1_RangingMeasurementData_t)); memset(&cali, 0, sizeof(cali)); - rc = VL53L1_GetCalibrationData(&data->stdev, &cali); - if (rc) { - vl53l1_errmsg("VL53L1_GetCalibrationData fail\n"); - rc = store_last_error(data, rc); - goto done; + if (!data->cali_data) { + rc = VL53L1_GetCalibrationData(&data->stdev, &cali); + if (rc) { + vl53l1_errmsg("VL53L1_GetCalibrationData fail\n"); + rc = store_last_error(data, rc); + goto done; + } + } else { + memcpy(&cali, data->cali_data, sizeof(VL53L1_CalibrationData_t)); } /* set autonomous mode configuration */ @@ -2491,6 +2495,7 @@ static ssize_t stmvl53l1_calib_data_write(struct file *filp, { struct device *dev = container_of(kobj, struct device, kobj); struct stmvl53l1_data *data = dev_get_drvdata(dev); + VL53L1_CalibrationData_t *cali_data = (VL53L1_CalibrationData_t *)buf; int rc; mutex_lock(&data->work_mutex); @@ -2504,17 +2509,27 @@ static ssize_t stmvl53l1_calib_data_write(struct file *filp, } /* we only support one time write */ - if (off != 0 || count != sizeof(VL53L1_CalibrationData_t)) + if (off != 0 || count != sizeof(VL53L1_CalibrationData_t)) { + vl53l1_errmsg("calib data size not match: count=%lu\n", count); goto invalid; + } - rc = VL53L1_SetCalibrationData(&data->stdev, - (VL53L1_CalibrationData_t *) buf); + rc = VL53L1_SetCalibrationData(&data->stdev, cali_data); if (rc) { vl53l1_errmsg("VL53L1_SetCalibrationData fail %d", rc); rc = store_last_error(data, rc); goto error; } + if (!data->cali_data) + data->cali_data = kmalloc(sizeof(VL53L1_CalibrationData_t), GFP_KERNEL); + if (data->cali_data) + memcpy(data->cali_data, cali_data, sizeof(VL53L1_CalibrationData_t)); + + data->xtalk_offset = cali_data->customer.algo__crosstalk_compensation_plane_offset_kcps; + data->xtalk_x = cali_data->customer.algo__crosstalk_compensation_x_plane_gradient_kcps; + data->xtalk_y = cali_data->customer.algo__crosstalk_compensation_y_plane_gradient_kcps; + mutex_unlock(&data->work_mutex); return count; @@ -4817,6 +4832,10 @@ void stmvl53l1_cleanup(struct stmvl53l1_data *data) vl53l1_dbgmsg("done\n"); deallocate_dev_id(data->id); data->is_device_remove = true; + if (data->cali_data) { + kfree(data->cali_data); + data->cali_data = NULL; + } } #ifdef CONFIG_PM_SLEEP