From 3ce14a4680f4d4529b52d0567ae385a31b57066e Mon Sep 17 00:00:00 2001 From: Kaio Karam Galvao Date: Fri, 24 Aug 2018 16:18:39 -0300 Subject: [PATCH] Apply mot changes Mot changes ported from 8998 Android O: (CR): 8998: ToF sensor VL53L1 bring up (CR): change netlink protocol ID (CR): moto change for TOF_INT (CR): update pinctl and regulator for power (CR): 8998: To make laser sensor work on Nash DVT1B (CR): moto changes on 7.2.1 (CR): implement sar function (CR): turn off l22 when sensor is off (CR): Apply moto changes based on new ST driver 8.0.0_b4.1.0_1268 (CR): adjust xtalk value per ST's request (CR): fix laser autonomous mode issue KSWN-43010: fix black target at 0mm range issue (CR): [CTS] Nash ATT: 4 failures on Module CtsSensorTestCases (CR): unlock ipp_mutex in ipp_in_process to avoid deadlock. (CR): Set timing budget for SAR mode (CR): Add default calibration Change-Id: Ieebb105bf64706d3ef6df9141a0ba8d1989290ac Reviewed-on: https://gerrit.mot.com/1233052 SLTApproved: Slta Waiver SME-Granted: SME Approvals Granted Tested-by: Jira Key Reviewed-by: Sumit Jamadar Reviewed-by: Rav(CR) Vembu Submit-Approved: Jira Key --- .../bindings/input/misc/stmvl53l1.txt | 28 + drivers/input/misc/vl53L1/stmvl53l1.h | 18 +- drivers/input/misc/vl53L1/stmvl53l1_if.h | 43 +- drivers/input/misc/vl53L1/stmvl53l1_ipp_nl.c | 2 + .../input/misc/vl53L1/stmvl53l1_module-i2c.c | 77 ++- drivers/input/misc/vl53L1/stmvl53l1_module.c | 621 +++++++++++++++++- 6 files changed, 748 insertions(+), 41 deletions(-) create mode 100644 Documentation/devicetree/bindings/input/misc/stmvl53l1.txt diff --git a/Documentation/devicetree/bindings/input/misc/stmvl53l1.txt b/Documentation/devicetree/bindings/input/misc/stmvl53l1.txt new file mode 100644 index 000000000000..6c6b7a34a923 --- /dev/null +++ b/Documentation/devicetree/bindings/input/misc/stmvl53l1.txt @@ -0,0 +1,28 @@ +st,stmvl53l1 laser driver + +Required properties: + + - compatible : Should be "st,stmvl53l1". + +Optional properites: + + + Example: + stmvl53l1@29 { + status = "ok"; + compatible = "st,stmvl53l1"; + reg = <0x29>; + /* L22 is always on for now */ + /* vdd-supply = <&pm8998_l22>; */ + gpios = <&pm8998_gpios 15 0>, <&tlmm 79 0>; + qcom,gpio-reset = <0>; + qcom,gpio-req-tbl-num = <0 1>; + qcom,gpio-req-tbl-flags = <0 1>; + qcom,gpio-req-tbl-label ="TOF_XSHUTDOWN","TOF_INT"; + pinctrl-names = "laser_default", "laser_suspend"; + pinctrl-0 = <&laser_active >; + pinctrl-1 = <&laser_suspend >; + st,sensorthreshold = <60 65>; + st,xtalkval = <19>; + }; + diff --git a/drivers/input/misc/vl53L1/stmvl53l1.h b/drivers/input/misc/vl53L1/stmvl53l1.h index 3a64bebf2c28..83ff6460ef3e 100644 --- a/drivers/input/misc/vl53L1/stmvl53l1.h +++ b/drivers/input/misc/vl53L1/stmvl53l1.h @@ -57,7 +57,7 @@ /** * Configure the Netlink-id use */ -#define STMVL531_CFG_NETLINK_USER 31 +#define STMVL531_CFG_NETLINK_USER 23 #define STMVL53L1_MAX_CCI_XFER_SZ 256 #define STMVL53L1_DRV_NAME "stmvl53l1" @@ -120,7 +120,7 @@ extern int stmvl53l1_enable_debug; #endif #define vl53l1_info(str, args...) \ - pr_info("%s: " str "\n", __func__, ##args) + pr_info("%s: " str, __func__, ##args) #define vl53l1_errmsg(str, args...) \ pr_err("%s: " str, __func__, ##args) @@ -183,6 +183,7 @@ struct stmvl53l1_waiters { struct stmvl53l1_data { int id; /*!< multiple device id 0 based*/ char name[64]; /*!< misc device name */ + int32_t hw_rev; /*!< hardware revision number*/ VL53L1_DevData_t stdev; /*!0 + * + * c example userland : + @code +int smtvl53l1_stop(int fd){ + int rc; + rc= ioctl(fd, VL53L1_IOCTL_SUSPEND,NULL); + if( rc ){ + if( errno == EBUSY ){ + ioctl_warn("already suspended"); + return errno; + } + ioctl_error("%d %s", rc,strerror(errno)); + } + return rc; +} +@endcode + */ +#define VL53L1_IOCTL_SUSPEND _IO('p', 0x15) + /** @} */ /* ioctl group */ #endif /* STMVL53L1_IF_H */ diff --git a/drivers/input/misc/vl53L1/stmvl53l1_ipp_nl.c b/drivers/input/misc/vl53L1/stmvl53l1_ipp_nl.c index f426218dd56d..ccf1ec687552 100644 --- a/drivers/input/misc/vl53L1/stmvl53l1_ipp_nl.c +++ b/drivers/input/misc/vl53L1/stmvl53l1_ipp_nl.c @@ -153,6 +153,7 @@ int ipp_in_process(struct ipp_work_t *pwork) /* work id check already done */ data = stmvl53l1_dev_table[pwork->dev_id]; ipp_dbg("to lock "); + mutex_unlock(&ipp_mutex); mutex_lock(&data->work_mutex); if (data->ipp.buzy == IPP_STATE_PENDING) { /* if it was already handled ignore it */ @@ -172,6 +173,7 @@ int ipp_in_process(struct ipp_work_t *pwork) pwork->xfer_id); done_lock: mutex_unlock(&data->work_mutex); + mutex_lock(&ipp_mutex); return 0; } diff --git a/drivers/input/misc/vl53L1/stmvl53l1_module-i2c.c b/drivers/input/misc/vl53L1/stmvl53l1_module-i2c.c index 0105c6fdb10e..161b4e646f81 100644 --- a/drivers/input/misc/vl53L1/stmvl53l1_module-i2c.c +++ b/drivers/input/misc/vl53L1/stmvl53l1_module-i2c.c @@ -157,6 +157,12 @@ MODULE_PARM_DESC(intr_gpio_nb, "select gpio numer to use for vl53l1 interrupt"); # define modi2c_dbg(...) (void)0 #endif +struct vl53l1_pinctrl_info { + struct pinctrl *pinctrl; + struct pinctrl_state *gpio_state_active; + struct pinctrl_state *gpio_state_suspend; +} g_pinctrl_info; + static int insert_device(void) { int ret = 0; @@ -329,6 +335,35 @@ static void put_intr(struct i2c_data *i2c_data) i2c_data->intr_gpio = -1; } +static int get_dt_xtalk_data(struct device_node *of_node, + struct stmvl53l1_data *vl53l1_data) +{ + int rc = 0; + uint32_t x_array[3] = {0}; + + if(of_node == NULL || vl53l1_data == NULL) + return -EINVAL; + + rc = of_property_read_u32_array(of_node, "st,xtalkval", + x_array, 3); + if (rc) { + vl53l1_errmsg("failed to read xtalk-offset\n"); + vl53l1_data->xtalk_offset = 0; + vl53l1_data->xtalk_x = 0; + vl53l1_data->xtalk_y = 0; + } else { + vl53l1_data->xtalk_offset = (uint16_t)x_array[0] & 0xFFFF; + vl53l1_data->xtalk_x = (int16_t)(x_array[1] & 0xFFFF); + vl53l1_data->xtalk_y = (int16_t)(x_array[2] & 0xFFFF); + } + + vl53l1_info("xtalk info: %u %d %d\n", + vl53l1_data->xtalk_offset, + vl53l1_data->xtalk_x, + vl53l1_data->xtalk_y); + return rc; +} + /** * parse dev tree for all platform specific input */ @@ -350,7 +385,7 @@ static int stmvl53l1_parse_tree(struct device *dev, struct i2c_data *i2c_data) i2c_data->intr_gpio = intr_gpio_nb; } else if (dev->of_node) { /* power : either vdd or pwren_gpio. try reulator first */ - i2c_data->vdd = regulator_get(dev, "vdd"); + i2c_data->vdd = regulator_get(dev, "vdd-vl53l1"); if (IS_ERR(i2c_data->vdd) || i2c_data->vdd == NULL) { i2c_data->vdd = NULL; /* try gpio */ @@ -362,19 +397,40 @@ static int stmvl53l1_parse_tree(struct device *dev, struct i2c_data *i2c_data) "no regulator, nor power gpio => power ctrl disabled"); } } + + g_pinctrl_info.pinctrl = devm_pinctrl_get(dev); + if (!IS_ERR_OR_NULL(g_pinctrl_info.pinctrl)) { + g_pinctrl_info.gpio_state_active = + pinctrl_lookup_state(g_pinctrl_info.pinctrl, + "laser_default"); + + g_pinctrl_info.gpio_state_suspend = + pinctrl_lookup_state(g_pinctrl_info.pinctrl, + "laser_suspend"); + } else + vl53l1_errmsg("Getting pinctrl handle failed\n"); + rc = of_property_read_u32_array(dev->of_node, "xsdn-gpio", &i2c_data->xsdn_gpio, 1); if (rc) { vl53l1_wanrmsg("Unable to find xsdn-gpio %d %d", rc, i2c_data->xsdn_gpio); - i2c_data->xsdn_gpio = -1; + if (of_gpio_count(dev->of_node) >= 2) + i2c_data->xsdn_gpio = of_get_gpio(dev->of_node, + 0); + else + i2c_data->xsdn_gpio = -1; } rc = of_property_read_u32_array(dev->of_node, "intr-gpio", &i2c_data->intr_gpio, 1); if (rc) { vl53l1_wanrmsg("Unable to find intr-gpio %d %d", rc, i2c_data->intr_gpio); - i2c_data->intr_gpio = -1; + if (of_gpio_count(dev->of_node) >= 2) + i2c_data->intr_gpio = of_get_gpio(dev->of_node, + 1); + else + i2c_data->intr_gpio = -1; } rc = of_property_read_u32_array(dev->of_node, "boot-reg", &i2c_data->boot_reg, 1); @@ -383,6 +439,8 @@ static int stmvl53l1_parse_tree(struct device *dev, struct i2c_data *i2c_data) rc, i2c_data->boot_reg); i2c_data->boot_reg = STMVL53L1_SLAVE_ADDR; } + + get_dt_xtalk_data(dev->of_node, i2c_data->vl53l1_data); } /* configure gpios */ @@ -464,6 +522,11 @@ static int stmvl53l1_probe(struct i2c_client *client, /* end up by core driver setup */ rc = stmvl53l1_setup(vl53l1_data); + if (rc) + goto release_gpios; + + vl53l1_data->sysfs_base = i2c_data->client->addr; + rc = stmvl53l1_sysfs_laser(vl53l1_data, true); if (rc) goto release_gpios; vl53l1_dbgmsg("End\n"); @@ -490,6 +553,9 @@ static int stmvl53l1_remove(struct i2c_client *client) vl53l1_dbgmsg("Enter\n"); mutex_lock(&data->work_mutex); + + stmvl53l1_sysfs_laser(data, false); + /* main driver cleanup */ stmvl53l1_cleanup(data); @@ -659,6 +725,8 @@ int stmvl53l1_reset_release_i2c(void *i2c_object) vl53l1_dbgmsg("Enter\n"); rc = release_reset(data); + pinctrl_select_state(g_pinctrl_info.pinctrl, + g_pinctrl_info.gpio_state_active); if (rc) goto error; @@ -693,6 +761,9 @@ int stmvl53l1_reset_hold_i2c(void *i2c_object) gpio_set_value(data->xsdn_gpio, 0); + pinctrl_select_state(g_pinctrl_info.pinctrl, + g_pinctrl_info.gpio_state_suspend); + vl53l1_dbgmsg("End\n"); return 0; diff --git a/drivers/input/misc/vl53L1/stmvl53l1_module.c b/drivers/input/misc/vl53L1/stmvl53l1_module.c index 7de395abaab5..e1e020ef333a 100644 --- a/drivers/input/misc/vl53L1/stmvl53l1_module.c +++ b/drivers/input/misc/vl53L1/stmvl53l1_module.c @@ -88,6 +88,7 @@ * Can be change at run time via @ref vl53l1_ioctl or @ref sysfs_attrib */ #define STMVL53L1_CFG_TIMING_BUDGET_US 16000 +#define STMVL53L1_CFG_SAR_MODE_TIMING_BUDGET_US 76000 /** default preset ranging mode */ #define STMVL53L1_CFG_DEFAULT_MODE VL53L1_PRESETMODE_RANGING @@ -96,7 +97,13 @@ #define STMVL53L1_CFG_DEFAULT_DISTANCE_MODE VL53L1_DISTANCEMODE_LONG /** default crosstalk enable */ -#define STMVL53L1_CFG_DEFAULT_CROSSTALK_ENABLE 0 +#define STMVL53L1_CFG_DEFAULT_CROSSTALK_ENABLE 1 + +/** default crosstalk for autonomous mode */ +#define STMVL53L1_CFG_DEFAULT_CROSSTALK_AUTONOMOUS 1000 + +/** max distance value mm */ +#define STMVL53L1_MAX_DISTANCE 0x1FFF /** default output mode */ #define STMVL53L1_CFG_DEFAULT_OUTPUT_MODE VL53L1_OUTPUTMODE_NEAREST @@ -145,6 +152,12 @@ static int stmvl53l1_open(struct inode *inode, struct file *file); static int stmvl53l1_release(struct inode *inode, struct file *file); static int ctrl_start(struct stmvl53l1_data *data); static int ctrl_stop(struct stmvl53l1_data *data); +static int ctrl_suspend(struct stmvl53l1_data *data); + +#ifdef CONFIG_COMPAT +static long stmvl53l1_compat_ioctl(struct file *file, + unsigned int cmd, unsigned long arg); +#endif static bool force_device_on_en_default = true; @@ -304,6 +317,9 @@ struct stmvl53l1_data *stmvl53l1_dev_table[STMVL53L1_CFG_MAX_DEV]; static const struct file_operations stmvl53l1_ranging_fops = { .owner = THIS_MODULE, .unlocked_ioctl = stmvl53l1_ioctl, +#ifdef CONFIG_COMPAT + .compat_ioctl = stmvl53l1_compat_ioctl, +#endif .open = stmvl53l1_open, .release = stmvl53l1_release, /* .flush = stmvl53l0_flush, */ @@ -404,6 +420,13 @@ static int reset_release(struct stmvl53l1_data *data) if (!data->reset_state) return 0; + vl53l1_dbgmsg("turn on vdd\n"); + rc = stmvl53l1_module_func_tbl.power_up(data->client_object); + if (rc) { + vl53l1_errmsg("%d,error rc %d\n", __LINE__, rc); + return rc; + } + rc = stmvl53l1_module_func_tbl.reset_release(data->client_object); if (rc) vl53l1_errmsg("reset release fail rc=%d\n", rc); @@ -427,6 +450,13 @@ static int reset_hold(struct stmvl53l1_data *data) if (!rc) data->reset_state = 1; + vl53l1_dbgmsg("turn off vdd\n"); + rc = stmvl53l1_module_func_tbl.power_down(data->client_object); + if (rc) { + vl53l1_errmsg("%d,error rc %d\n", __LINE__, rc); + return rc; + } + return rc; } @@ -560,6 +590,7 @@ static int stmvl53l1_check_calibration_id(struct stmvl53l1_data *data) static int stmvl53l1_start(struct stmvl53l1_data *data) { int rc; + VL53L1_CalibrationData_t cali; data->is_first_irq = true; data->is_data_valid = false; @@ -587,6 +618,20 @@ static int stmvl53l1_start(struct stmvl53l1_data *data) goto done; } + if (data->preset_mode == VL53L1_PRESETMODE_AUTONOMOUS) { + /* disable xtalk and set short distance mode + * for autonomous mode */ + data->crosstalk_enable = 0; + data->distance_mode = VL53L1_DISTANCEMODE_SHORT; + } else { + /* restore xtalk and distance mode to default + * values for non-autonomous mode */ + data->crosstalk_enable = + STMVL53L1_CFG_DEFAULT_CROSSTALK_ENABLE; + data->distance_mode = + STMVL53L1_CFG_DEFAULT_DISTANCE_MODE; + } + rc = VL53L1_SetXTalkCompensationEnable(&data->stdev, data->crosstalk_enable); if (rc) { @@ -687,23 +732,67 @@ static int stmvl53l1_start(struct stmvl53l1_data *data) memset(&data->meas.single_range_data, 0, 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; + } + /* set autonomous mode configuration */ if ((data->preset_mode == VL53L1_PRESETMODE_AUTONOMOUS) || (data->preset_mode == VL53L1_PRESETMODE_LOWPOWER_AUTONOMOUS)) { - rc = VL53L1_SetInterMeasurementPeriodMilliSeconds(&data->stdev, - data->auto_pollingTimeInMs); - if (rc) { - vl53l1_errmsg("Fail to set auto period %d\n", rc); - rc = store_last_error(data, rc); - goto done; - } - rc = VL53L1_SetThresholdConfig(&data->stdev, - &data->auto_config); - if (rc) { - vl53l1_errmsg("Fail to set auto config %d\n", rc); - rc = store_last_error(data, rc); - goto done; + if (data->cam_mode == 0) { + rc = VL53L1_SetInterMeasurementPeriodMilliSeconds( + &data->stdev, + data->auto_pollingTimeInMs); + if (rc) { + vl53l1_errmsg("Fail to set auto period %d\n", + rc); + rc = store_last_error(data, rc); + goto done; + } + rc = VL53L1_SetThresholdConfig(&data->stdev, + &data->auto_config); + if (rc) { + vl53l1_errmsg("Fail to set auto config %d\n", + rc); + rc = store_last_error(data, rc); + goto done; + } + } else { + /* set camera autonomous configration */ + rc = VL53L1_SetInterMeasurementPeriodMilliSeconds( + &data->stdev, + data->auto_pollingTimeInMs_cam); + if (rc) { + vl53l1_errmsg("Fail to set auto period %d\n", + rc); + rc = store_last_error(data, rc); + goto done; + } + rc = VL53L1_SetThresholdConfig(&data->stdev, + &data->auto_config_cam); + if (rc) { + vl53l1_errmsg("Fail to set auto config %d\n", + rc); + rc = store_last_error(data, rc); + goto done; + } } + cali.customer.algo__crosstalk_compensation_plane_offset_kcps + = STMVL53L1_CFG_DEFAULT_CROSSTALK_AUTONOMOUS; + } else { + cali.customer.algo__crosstalk_compensation_plane_offset_kcps + = data->xtalk_offset; + } + + rc = VL53L1_SetCalibrationData(&data->stdev, &cali); + if (rc) { + vl53l1_errmsg("VL53L1_SetCalibrationData fail\n"); + rc = store_last_error(data, rc); + goto done; } data->allow_hidden_start_stop = false; @@ -768,6 +857,54 @@ static int stmvl53l1_stop(struct stmvl53l1_data *data) return rc; } +static int activate_sar_mode(struct stmvl53l1_data *data) +{ + int rc = VL53L1_ERROR_NONE; + + if (data == NULL) + return -EFAULT; + + if (data->enable_sensor == 0 && + !data->is_calibrating && + data->sar_mode == 1 && + data->cam_mode != 1) { + data->preset_mode = VL53L1_PRESETMODE_AUTONOMOUS; + data->timing_budget = STMVL53L1_CFG_SAR_MODE_TIMING_BUDGET_US; + rc = stmvl53l1_start(data); + if (rc == VL53L1_ERROR_NONE) + vl53l1_info("sensor in sar mode!\n"); + else + vl53l1_errmsg("fail to active sar err %d\n", rc); + } else + vl53l1_info("skip sar activation\n"); + + return rc; +} + +static int deactivate_sar_mode(struct stmvl53l1_data *data) +{ + int rc = VL53L1_ERROR_NONE; + + if (data == NULL) + return -EFAULT; + + if (data->enable_sensor == 1 && + !data->is_calibrating && + data->preset_mode == VL53L1_PRESETMODE_AUTONOMOUS && + data->cam_mode != 1) { + rc = stmvl53l1_stop(data); + if (rc == VL53L1_ERROR_NONE) { + vl53l1_info("sensor is out of sar mode!\n"); + /* set preset mode back to default ranging mode */ + data->preset_mode = VL53L1_PRESETMODE_RANGING; + } else + vl53l1_errmsg("fail to deactivate sar err %d\n", rc); + } else + vl53l1_info("sar is not actived, skip deactivation\n"); + + return rc; +} + /* * SysFS support */ @@ -1264,8 +1401,7 @@ static ssize_t stmvl53l1_do_flush(struct device *dev, mutex_lock(&data->work_mutex); data->flush_todo_counter++; - if (data->enable_sensor == 0) - stmvl53l1_insert_flush_events_lock(data); + stmvl53l1_insert_flush_events_lock(data); mutex_unlock(&data->work_mutex); @@ -1438,9 +1574,6 @@ static ssize_t stmvl53l1_store_autonomous_config(struct device *dev, mutex_lock(&data->work_mutex); - if (data->enable_sensor) - goto busy; - buf = parse_integer(buf, &pollingTimeInMs); if (!buf) goto invalid; @@ -1483,11 +1616,6 @@ static ssize_t stmvl53l1_store_autonomous_config(struct device *dev, return count; -busy: - vl53l1_errmsg("can't change config while ranging"); - rc = -EBUSY; - goto error; - invalid: vl53l1_errmsg("invalid syntax in %s", buf_ori); rc = -EINVAL; @@ -1872,6 +2000,142 @@ static DEVICE_ATTR(is_xtalk_value_changed, 0440/*S_IRUGO*/, stmvl53l1_show_is_xtalk_value_changed_config, NULL); +static ssize_t stmvl53l1_show_enable_sar(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct stmvl53l1_data *data = dev_get_drvdata(dev); + + return scnprintf(buf, PAGE_SIZE, "%d\n", data->sar_mode); +} + +static ssize_t stmvl53l1_store_enable_sar(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct stmvl53l1_data *data = dev_get_drvdata(dev); + int rc = 0; + int mode; + + mutex_lock(&data->work_mutex); + + if (kstrtoint(buf, 0, &mode)) { + vl53l1_errmsg("invalid syntax in %s\n", buf); + rc = -EINVAL; + } else { + data->sar_mode = mode?1:0; + vl53l1_dbgmsg("sar mode is set to %d\n", data->sar_mode); + + if (data->sar_mode == 1) { + if (mode == 1) { + /* report far distance as initial value */ + struct input_dev *input = data->input_dev_ps; + + input_report_abs(input, ABS_HAT1X, 100); + input_report_abs(input, ABS_HAT1X, 101); + input_report_abs(input, ABS_HAT1X, 100); + input_sync(input); + } + activate_sar_mode(data); + } else + deactivate_sar_mode(data); + } + + mutex_unlock(&data->work_mutex); + + return rc ? rc : count; +} + +/** + * sysfs attribute "enable_sar" [rd/wr] + * + * To enable/disable the sar mode + * @li 0 disable sar mode + * @li 1 enable sar mode + * + * @ingroup sysfs_attrib + */ +static DEVICE_ATTR(enable_sar, 0660/*S_IWUGO | S_IRUGO*/, + stmvl53l1_show_enable_sar, + stmvl53l1_store_enable_sar); + +static ssize_t stmvl53l1_show_offset(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct stmvl53l1_data *data = dev_get_drvdata(dev); + + return scnprintf(buf, PAGE_SIZE, "%d,%d\n", + data->inner_offset, data->outer_offset); +} + +static ssize_t stmvl53l1_store_offset(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct stmvl53l1_data *data = dev_get_drvdata(dev); + VL53L1_CalibrationData_t cali_data; + int rc = 0; + int n = 0; + int inner = 0; + int outer = 0; + + mutex_lock(&data->work_mutex); + + if (data->enable_sensor) { + vl53l1_errmsg("can't set offset while ranging\n"); + rc = -EBUSY; + goto finish; + } + + n = sscanf(buf, "%d,%d", &inner, &outer); + if(n != 2) { + vl53l1_errmsg("wrong offset syntax around %s\n", buf); + rc = -EINVAL; + goto finish; + } + + memset(&cali_data, 0, sizeof(cali_data)); + rc = VL53L1_GetCalibrationData(&data->stdev, &cali_data); + if (rc) { + vl53l1_errmsg("VL53L1_GetCalibrationData fail\n"); + rc = -EINVAL; + goto finish; + } + + /* update offset values */ + vl53l1_info("previous offset is %d,%d\n", + cali_data.customer.mm_config__inner_offset_mm, + cali_data.customer.mm_config__outer_offset_mm); + cali_data.customer.mm_config__inner_offset_mm = inner; + cali_data.customer.mm_config__outer_offset_mm = outer; + + rc = VL53L1_SetCalibrationData(&data->stdev, &cali_data); + if (rc) { + vl53l1_errmsg("VL53L1_SetCalibrationData fail\n"); + rc = -EINVAL; + goto finish; + } + + data->inner_offset = (int16_t)inner; + data->outer_offset = (int16_t)outer; + vl53l1_info("offset is set to %d,%d\n", inner, outer); +finish: + mutex_unlock(&data->work_mutex); + return rc ? rc : count; +} + +/** + * sysfs attribute "enable_sar" [rd/wr] + * + * To enable/disable the sar mode + * @li 0 disable sar mode + * @li 1 enable sar mode + * + * @ingroup sysfs_attrib + */ +static DEVICE_ATTR(offset, 0660/*S_IWUGO | S_IRUGO*/, + stmvl53l1_show_offset, + stmvl53l1_store_offset); + static struct attribute *stmvl53l1_attributes[] = { &dev_attr_enable_ps_sensor.attr, &dev_attr_set_delay_ms.attr, @@ -1894,6 +2158,8 @@ static struct attribute *stmvl53l1_attributes[] = { &dev_attr_tuning_status.attr, &dev_attr_smudge_correction_mode.attr, &dev_attr_is_xtalk_value_changed.attr, + &dev_attr_enable_sar.attr, + &dev_attr_offset.attr, NULL }; @@ -2246,6 +2512,44 @@ static int ctrl_stop(struct stmvl53l1_data *data) mutex_lock(&data->work_mutex); + if (data->is_device_remove) { + rc = -ENODEV; + goto done; + } + if (data->enable_sensor) { + rc = _ctrl_stop(data); + + if (data->cam_mode == 1) { + /* only restore sar mode when + it quits from camera mode */ + data->cam_mode = 0; + + if (data->sar_mode == 1) { + vl53l1_info("restore sar mode\n"); + activate_sar_mode(data); + } + } + } else + rc = -EBUSY; + +done: + mutex_unlock(&data->work_mutex); + + return rc; +} + +/** + * get work lock and suspend sensor without checking sar mode + * + * @param data device + * @return 0 on success EBUSY if arleady off + */ +static int ctrl_suspend(struct stmvl53l1_data *data) +{ + int rc; + + mutex_lock(&data->work_mutex); + vl53l1_info("enter ctrl_suspend\n"); if (data->is_device_remove) { rc = -ENODEV; goto done; @@ -2408,8 +2712,9 @@ static int ctrl_mz_data_blocking_common(struct stmvl53l1_data *data, goto done; } /* sleep if data already read */ - if (!is_new_data_for_me(data, pid, &data->mz_data_reader_list)) + if (!is_new_data_for_me(data, pid, &data->mz_data_reader_list)) { rc = sleep_for_data(data, pid, &data->mz_data_reader_list); + } if (rc) goto done; @@ -2546,6 +2851,38 @@ static int ctrl_param_is_xtalk_value_changed(struct stmvl53l1_data *data, return 0; } +static int ctrl_param_camera_mode(struct stmvl53l1_data *data, + struct stmvl53l1_parameter *param) +{ + if (param->is_read) { + param->value = data->cam_mode; + param->status = 0; + vl53l1_dbgmsg("get camera mode %d", param->value); + } else { + data->cam_mode = param->value; + vl53l1_dbgmsg("set camera mode %d", param->value); + } + + return 0; +} + +static int ctrl_param_hw_rev(struct stmvl53l1_data *data, + struct stmvl53l1_parameter *param) +{ + int rc; + + if (param->is_read) { + param->value = data->hw_rev; + param->status = 0; + vl53l1_dbgmsg("get hw rev %d", param->value); + rc = 0; + } else { + rc = -EINVAL; + } + + return rc; +} + /** * handle ioctl set param mode * @@ -2616,6 +2953,12 @@ static int ctrl_params(struct stmvl53l1_data *data, void __user *p) case VL53L1_ISXTALKVALUECHANGED_PAR: rc = ctrl_param_is_xtalk_value_changed(data, ¶m); break; + case VL53L1_CAMERAMODE_PAR: + rc = ctrl_param_camera_mode(data, ¶m); + break; + case VL53L1_HWREV_PAR: + rc = ctrl_param_hw_rev(data, ¶m); + break; default: vl53l1_errmsg("unknown or unsupported %d\n", param.name); rc = -EINVAL; @@ -2748,6 +3091,46 @@ done: return rc; } +static int ctrl_autonomous_config_cam(struct stmvl53l1_data *data, + void __user *p) +{ + int rc = 0; + struct stmvl53l1_autonomous_config_t full; + + mutex_lock(&data->work_mutex); + if (data->is_device_remove) { + rc = -ENODEV; + goto done; + } + /* first copy all data */ + rc = copy_from_user(&full, p, sizeof(full)); + if (rc) { + rc = -EFAULT; + goto done; + } + + if (full.is_read) { + full.pollingTimeInMs = data->auto_pollingTimeInMs_cam; + full.config = data->auto_config_cam; + rc = copy_to_user(p, &full, sizeof(full)); + if (rc) + rc = -EFAULT; + } else { + if (data->enable_sensor) { + rc = -EBUSY; + vl53l1_errmsg("can't change config while ranging\n"); + goto done; + } + data->auto_pollingTimeInMs_cam = full.pollingTimeInMs; + data->auto_config_cam = full.config; + } + +done: + mutex_unlock(&data->work_mutex); + + return rc; +} + static int ctrl_calibration_data(struct stmvl53l1_data *data, void __user *p) { int rc; @@ -3107,6 +3490,11 @@ static int stmvl53l1_ioctl_handler( rc = ctrl_stop(data); break; + case VL53L1_IOCTL_SUSPEND: + vl53l1_dbgmsg("VL53L1_IOCTL_SUSPEND\n"); + rc = ctrl_suspend(data); + break; + case VL53L1_IOCTL_GETDATAS: /* vl53l1_dbgmsg("VL53L1_IOCTL_GETDATAS\n"); */ rc = ctrl_getdata(data, p); @@ -3150,7 +3538,10 @@ static int stmvl53l1_ioctl_handler( break; case VL53L1_IOCTL_AUTONOMOUS_CONFIG: vl53l1_dbgmsg("VL53L1_IOCTL_AUTONOMOUS_CONFIG\n"); - rc = ctrl_autonomous_config(data, p); + if (data->cam_mode == 0) + rc = ctrl_autonomous_config(data, p); + else + rc = ctrl_autonomous_config_cam(data, p); break; case VL53L1_IOCTL_ZONE_CALIBRATION_DATA: vl53l1_dbgmsg("VL53L1_IOCTL_ZONE_CALIBRATION_DATA\n"); @@ -3449,6 +3840,12 @@ static void stmvl53l1_input_push_data_singleobject(struct stmvl53l1_data *data) FixPoint1616_t LimitCheckCurrent; VL53L1_Error st = VL53L1_ERROR_NONE; + /* set the distance value to max if range + * status is invalid in autonomous mode */ + if (data->preset_mode == VL53L1_PRESETMODE_AUTONOMOUS && + meas->RangeStatus != VL53L1_RANGESTATUS_RANGE_VALID) + meas->RangeMilliMeter = STMVL53L1_MAX_DISTANCE; + input_report_abs(input, ABS_DISTANCE, (meas->RangeMilliMeter + 5) / 10); input_report_abs(input, ABS_HAT0X, meas->TimeStamp / 1000); input_report_abs(input, ABS_HAT0Y, (meas->TimeStamp % 1000) * 1000); @@ -3461,7 +3858,7 @@ static void stmvl53l1_input_push_data_singleobject(struct stmvl53l1_data *data) VL53L1_CHECKENABLE_SIGMA_FINAL_RANGE, &LimitCheckCurrent); if (st == VL53L1_ERROR_NONE) input_report_abs(input, ABS_WHEEL, LimitCheckCurrent); - input_report_abs(input, ABS_PRESSURE, meas->EffectiveSpadRtnCount); + input_report_abs(input, ABS_MISC, meas->EffectiveSpadRtnCount); input_report_abs(input, ABS_TOOL_WIDTH, meas->RangeQualityLevel); input_sync(input); @@ -3499,7 +3896,7 @@ static void stmvl53l1_input_push_data_multiobject(struct stmvl53l1_data *data) &LimitCheckCurrent); if (st == VL53L1_ERROR_NONE) input_report_abs(input, ABS_WHEEL, LimitCheckCurrent); - input_report_abs(input, ABS_PRESSURE, + input_report_abs(input, ABS_MISC, mmeas->EffectiveSpadRtnCount); input_report_abs(input, ABS_BRAKE, (mmeas->RoiStatus << 24) | (roi << 16) | (obj_number << 8) | i); @@ -3564,7 +3961,7 @@ static int stmvl53l1_input_setup(struct stmvl53l1_data *data) /* LimitCheckCurrent*/ input_set_abs_params(idev, ABS_WHEEL, 0, 0xffffffff, 0, 0); /* EffectiveSpadRtnCount */ - input_set_abs_params(idev, ABS_PRESSURE, 0, 0xffffffff, 0, 0); + input_set_abs_params(idev, ABS_MISC, 0, 0xffffffff, 0, 0); /* roi number in 16 msb bits with object number in 16 lsb bits */ input_set_abs_params(idev, ABS_BRAKE, 0, 0xffffffff, 0, 0); /* RangeMaxMilliMeter in 16 msb bits with RangeMinMilliMeter in lsb */ @@ -3643,10 +4040,13 @@ int stmvl53l1_intr_handler(struct stmvl53l1_data *data) int stmvl53l1_setup(struct stmvl53l1_data *data) { int rc = 0; + VL53L1_CalibrationData_t cali_data; VL53L1_DeviceInfo_t dev_info; vl53l1_dbgmsg("Enter\n"); + data->hw_rev = 0; + /* acquire an id */ data->id = allocate_dev_id(); if (data->id < 0) { @@ -3672,11 +4072,7 @@ int stmvl53l1_setup(struct stmvl53l1_data *data) data->last_error = VL53L1_ERROR_NONE; data->is_device_remove = false; - rc = stmvl53l1_module_func_tbl.power_up(data->client_object); - if (rc) { - vl53l1_errmsg("%d,error rc %d\n", __LINE__, rc); - goto exit_ipp_cleanup; - } + /* vdd will be controlled in reset_release() */ rc = reset_release(data); if (rc) goto exit_ipp_cleanup; @@ -3724,6 +4120,8 @@ int stmvl53l1_setup(struct stmvl53l1_data *data) data->output_mode = STMVL53L1_CFG_DEFAULT_OUTPUT_MODE; data->offset_correction_mode = STMVL53L1_CFG_DEFAULT_OFFSET_CORRECTION_MODE; + data->cam_mode = 0; + data->sar_mode = 0; stmvl53l1_setup_auto_config(data); data->dmax_mode = STMVL53L1_CFG_DEFAULT_DMAX_MODE; data->smudge_correction_mode = @@ -3768,6 +4166,57 @@ int stmvl53l1_setup(struct stmvl53l1_data *data) goto exit_unregister_dev_ps; } + data->hw_rev = dev_info.ProductRevisionMinor; + + memset(&cali_data, 0, sizeof(cali_data)); + rc = VL53L1_GetCalibrationData(&data->stdev, &cali_data); + if (rc) + vl53l1_errmsg("VL53L1_GetCalibrationData fail\n"); + else { + data->inner_offset = cali_data.customer.mm_config__inner_offset_mm; + data->outer_offset = cali_data.customer.mm_config__outer_offset_mm; + + /* update xtalk values */ + cali_data.customer.algo__crosstalk_compensation_plane_offset_kcps + = data->xtalk_offset; + cali_data.customer.algo__crosstalk_compensation_x_plane_gradient_kcps + = data->xtalk_x; + cali_data.customer.algo__crosstalk_compensation_y_plane_gradient_kcps + = data->xtalk_y; + + cali_data.xtalkhisto.xtalk_shape.VL53L1_PRM_00019 = 0 ; + cali_data.xtalkhisto.xtalk_shape.VL53L1_PRM_00020 = 24; + cali_data.xtalkhisto.xtalk_shape.VL53L1_PRM_00021 = 12; + cali_data.xtalkhisto.xtalk_shape.bin_data[0] = 353; + cali_data.xtalkhisto.xtalk_shape.bin_data[1] = 472; + cali_data.xtalkhisto.xtalk_shape.bin_data[2] = 472; + cali_data.xtalkhisto.xtalk_shape.bin_data[3] = 353; + cali_data.xtalkhisto.xtalk_shape.bin_data[4] = 0; + cali_data.xtalkhisto.xtalk_shape.bin_data[5] = 0; + cali_data.xtalkhisto.xtalk_shape.bin_data[6] = 0; + cali_data.xtalkhisto.xtalk_shape.bin_data[7] = 0; + cali_data.xtalkhisto.xtalk_shape.bin_data[8] = 0; + cali_data.xtalkhisto.xtalk_shape.bin_data[9] = 0; + cali_data.xtalkhisto.xtalk_shape.bin_data[10] = 0; + cali_data.xtalkhisto.xtalk_shape.bin_data[11] = 0; + cali_data.xtalkhisto.xtalk_shape.phasecal_result__reference_phase = 10240; + cali_data.xtalkhisto.xtalk_shape.phasecal_result__vcsel_start = 6; + cali_data.xtalkhisto.xtalk_shape.cal_config__vcsel_start = 9; + cali_data.xtalkhisto.xtalk_shape.vcsel_width = 40; + cali_data.xtalkhisto.xtalk_shape.VL53L1_PRM_00022 = 48332; + cali_data.xtalkhisto.xtalk_shape.zero_distance_phase = 4096; + cali_data.gain_cal.standard_ranging_gain_factor = 0x07D7; + cali_data.gain_cal.histogram_ranging_gain_factor = 0x07CC; + + rc = VL53L1_SetCalibrationData(&data->stdev, &cali_data); + if (rc) + vl53l1_errmsg("VL53L1_SetCalibrationData fail\n"); + else { + vl53l1_info("device crosstalk data updated: %u %d %d\n", + data->xtalk_offset, data->xtalk_x, data->xtalk_y); + } + } + /* if working in interrupt ask intr to enable and hook the handler */ data->poll_mode = 0; rc = stmvl53l1_module_func_tbl.start_intr(data->client_object, @@ -3816,6 +4265,96 @@ exit_ipp_cleanup: return rc; } +/* Attribute: name (RO) */ +static ssize_t name_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return scnprintf(buf, PAGE_SIZE, VL53L1_INPUT_DEVICE_NAME); +} + +static struct device_attribute laser_name_attributes = __ATTR_RO(name); + +#define LASERDEV_MINOR_BASE 256 +#define LASERDEV_MINOR_MAX 32 + +int stmvl53l1_sysfs_laser(struct stmvl53l1_data *data, bool create) +{ + int i, error = 0; + static struct class *laser_class; + static struct device *laser_class_dev; + static int minor; + + if (data == NULL) { + vl53l1_errmsg("stmvl53l1 dev data is null!\n"); + return -ENODEV; + } + + if (create) { + minor = input_get_new_minor(data->sysfs_base, 1, false); + if (minor < 0) + minor = input_get_new_minor(LASERDEV_MINOR_BASE, + LASERDEV_MINOR_MAX, true); + vl53l1_info("assigned minor %d\n", minor); + + laser_class = class_create(THIS_MODULE, "laser"); + if (IS_ERR(laser_class)) { + error = PTR_ERR(laser_class); + laser_class = NULL; + return error; + } + + laser_class_dev = device_create(laser_class, NULL, + MKDEV(INPUT_MAJOR, minor), + data, STMVL53L1_DRV_NAME); + if (IS_ERR(laser_class_dev)) { + error = PTR_ERR(laser_class_dev); + laser_class_dev = NULL; + return error; + } + + for (i = 0; stmvl53l1_attributes[i] != NULL; ++i) { + error = sysfs_create_file(&laser_class_dev->kobj, + stmvl53l1_attributes[i]); + if (error) + break; + } + + if (error) + goto device_destroy; + + error = device_create_file(laser_class_dev, + &laser_name_attributes); + if (error) + goto device_destroy; + } else { + if (!laser_class || !laser_class_dev) + return -ENODEV; + + device_remove_file(laser_class_dev, + &laser_name_attributes); + for (i = 0; stmvl53l1_attributes[i] != NULL; ++i) + sysfs_remove_file(&laser_class_dev->kobj, + stmvl53l1_attributes[i]); + + device_unregister(laser_class_dev); + class_unregister(laser_class); + } + + vl53l1_info("laser sysfs %s(0x%x) created\n", + STMVL53L1_DRV_NAME, data->sysfs_base); + return 0; + +device_destroy: + for (--i; i >= 0; --i) + sysfs_remove_file(&laser_class_dev->kobj, + stmvl53l1_attributes[i]); + device_destroy(laser_class, MKDEV(INPUT_MAJOR, minor)); + laser_class_dev = NULL; + class_unregister(laser_class); + vl53l1_errmsg("error creating laser class\n"); + + return -ENODEV; +} void stmvl53l1_cleanup(struct stmvl53l1_data *data) { @@ -3848,12 +4387,24 @@ void stmvl53l1_cleanup(struct stmvl53l1_data *data) /* be sure device is put under reset */ data->force_device_on_en = false; reset_hold(data); - stmvl53l1_module_func_tbl.power_down(data->client_object); vl53l1_dbgmsg("done\n"); deallocate_dev_id(data->id); data->is_device_remove = true; } +#ifdef CONFIG_COMPAT +static long stmvl53l1_compat_ioctl(struct file *file, + unsigned int cmd, unsigned long arg) +{ + int ret; + struct stmvl53l1_data *data = + container_of(file->private_data, + struct stmvl53l1_data, miscdev); + ret = stmvl53l1_ioctl_handler(data, cmd, arg, compat_ptr(arg)); + return ret; +} +#endif + static long stmvl53l1_ioctl(struct file *file, unsigned int cmd, unsigned long arg){ long ret;