From af226ab03cf17c5bff7d381474346026d14582cc Mon Sep 17 00:00:00 2001 From: Erik Lilliebjerg Date: Mon, 26 Mar 2012 09:40:58 -0700 Subject: media: video: tegra: sh532u focus driver - Abort initialization if an I2C error to avoid excessive load on the I2C bus since it is heavily used during initialization. - Updated to the latest NVC framework. - Added feature that allows for the key focus points to be set at runtime and the relative positions recalculated. Bug 929133 Bug 938934 Change-Id: Ida4ab885bf35057ae6df131e3ec3587a891a7dc9 Signed-off-by: Erik Lilliebjerg Reviewed-on: http://git-master/r/93944 Reviewed-by: Automatic_Commit_Validation_User Reviewed-by: Bharat Nihalani --- drivers/media/video/tegra/sh532u.c | 870 +++++++++++++++++++++++-------------- 1 file changed, 553 insertions(+), 317 deletions(-) (limited to 'drivers') diff --git a/drivers/media/video/tegra/sh532u.c b/drivers/media/video/tegra/sh532u.c index 7db14fcf2573..014b4bd30ca9 100644 --- a/drivers/media/video/tegra/sh532u.c +++ b/drivers/media/video/tegra/sh532u.c @@ -1,7 +1,7 @@ /* * SH532U focuser driver. * - * Copyright (C) 2011 NVIDIA Corporation. + * Copyright (C) 2011-2012 NVIDIA Corporation. * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License version 2 as @@ -21,7 +21,7 @@ /* Implementation * -------------- * The board level details about the device need to be provided in the board - * file with the sh532u_platform_data structure. + * file with the _platform_data structure. * Standard among NVC kernel drivers in this structure is: * .cfg = Use the NVC_CFG_ defines that are in nvc.h. * Descriptions of the configuration options are with the defines. @@ -40,11 +40,26 @@ * .num = 2, * .sync = 1, * The above example sync's device 1 and 2. + * To disable sync, set .sync = 0. Note that the .num = 0 device is + * not allowed to be synced to. * This is typically used for stereo applications. * .dev_name = The MISC driver name the device registers as. If not used, * then the part number of the device is used for the driver name. * If using the NVC user driver then use the name found in this * driver under _default_pdata. + * .gpio_count = The ARRAY_SIZE of the nvc_gpio_pdata table. + * .gpio = A pointer to the nvc_gpio_pdata structure's platform GPIO data. + * The GPIO mechanism works by cross referencing the .gpio_type key + * among the nvc_gpio_pdata GPIO data and the driver's nvc_gpio_init + * GPIO data to build a GPIO table the driver can use. The GPIO's + * defined in the device header file's _gpio_type enum are the + * gpio_type keys for the nvc_gpio_pdata and nvc_gpio_init structures. + * These need to be present in the board file's nvc_gpio_pdata + * structure for the GPIO's that are used. + * The driver's GPIO logic uses assert/deassert throughout until the + * low level _gpio_wr/rd calls where the .assert_high is used to + * convert the value to the correct signal level. + * See the GPIO notes in nvc.h for additional information. * * The following is specific to NVC kernel focus drivers: * .nvc = Pointer to the nvc_focus_nvc structure. This structure needs to @@ -52,27 +67,21 @@ * .cap = Pointer to the nvc_focus_cap structure. This structure needs to * be defined and populated if overriding the driver defaults. * - * The following is specific to only this NVC kernel focus driver: + * The following is specific to this NVC kernel focus driver: * .info = Pointer to the sh532u_pdata_info structure. This structure does * not need to be defined and populated unless overriding ROM data. .* .i2c_addr_rom = The I2C address of the onboard ROM. - * .gpio_reset = The GPIO connected to the devices reset. If not used then - * leave blank. - * .gpio_en = Due to a Linux limitation, a GPIO is defined to "enable" the - * device. This workaround is for when the device's power GPIO's - * are behind an I2C expander. The Linux limitation doesn't allow - * the I2C GPIO expander to be ready for use when this device is - * probed. When this problem is solved, this driver needs to - * remove the gpio_en WAR. * - * Power Requirements - * The board power file must contain the following labels for the power - * regulator(s) of this device: - * "vdd" = the power regulator for the device's power. - * "vdd_i2c" = the power regulator for the I2C power. - * - * The above values should be all that is needed to use the device with this - * driver. Modifications of this driver should not be needed. + * Power Requirements: + * The device's header file defines the voltage regulators needed with the + * enumeration _vreg. The order these are enumerated is the order + * the regulators will be enabled when powering on the device. When the + * device is powered off the regulators are disabled in descending order. + * The _vregs table in this driver uses the nvc_regulator_init + * structure to define the regulator ID strings that go with the regulators + * defined with _vreg. These regulator ID strings (or supply names) + * will be used in the regulator_get function in the _vreg_init function. + * The board power file and _vregs regulator ID strings must match. */ @@ -85,10 +94,10 @@ #include #include #include -#include #include -#define SH532U_ID 0xF0 +#define SH532U_ID 0x0532 +#define SH532U_STARTUP_DELAY_MS 10 /* defaults if no ROM data */ #define SH532U_HYPERFOCAL_RATIO 1836 /* 41.2f/224.4f Ratio source: SEMCO */ /* _HYPERFOCAL_RATIO is multiplied and _HYPERFOCAL_DIV divides for float */ @@ -96,10 +105,6 @@ #define SH532U_FOCAL_LENGTH 0x408D70A4 #define SH532U_FNUMBER 0x40333333 #define SH532U_MAX_APERATURE 0x3FCA0EA1 -/* SH532U_CAPS_VER = 0: invalid value */ -/* SH532U_CAPS_VER = 1: added NVC_PARAM_STS */ -/* SH532U_CAPS_VER = 2: expanded nvc_focus_cap */ -#define SH532U_CAPS_VER 2 #define SH532U_ACTUATOR_RANGE 1000 #define SH532U_SETTLETIME 30 #define SH532U_FOCUS_MACRO 950 @@ -110,24 +115,42 @@ #define SH532U_POS_HIGH_DEFAULT 0x6000 +static u8 sh532u_ids[] = { + 0xF0, +}; + +static struct nvc_gpio_init sh532u_gpios[] = { + { SH532U_GPIO_RESET, GPIOF_OUT_INIT_LOW, "reset", false, true, }, + { SH532U_GPIO_I2CMUX, 0, "i2c_mux", 0, false, }, + { SH532U_GPIO_GP1, 0, "gp1", 0, false, }, + { SH532U_GPIO_GP2, 0, "gp2", 0, false, }, + { SH532U_GPIO_GP3, 0, "gp3", 0, false, }, +}; + +static struct nvc_regulator_init sh532u_vregs[] = { + { SH532U_VREG_AVDD, "avdd", }, + { SH532U_VREG_DVDD, "dvdd", }, +}; + struct sh532u_info { atomic_t in_use; struct i2c_client *i2c_client; struct sh532u_platform_data *pdata; struct miscdevice miscdev; struct list_head list; + struct nvc_gpio gpio[ARRAY_SIZE(sh532u_gpios)]; + struct nvc_regulator vreg[ARRAY_SIZE(sh532u_vregs)]; int pwr_api; int pwr_dev; - struct nvc_regulator vreg_vdd; - struct nvc_regulator vreg_i2c; u8 s_mode; struct sh532u_info *s_info; + u8 id_minor; unsigned i2c_addr_rom; struct nvc_focus_nvc nvc; struct nvc_focus_cap cap; enum nvc_focus_sts sts; struct sh532u_pdata_info cfg; - bool gpio_flag_reset; + bool reset_flag; bool init_cal_flag; s16 abs_base; u32 abs_range; @@ -143,7 +166,7 @@ static struct sh532u_pdata_info sh532u_default_info = { }; static struct nvc_focus_cap sh532u_default_cap = { - .version = SH532U_CAPS_VER, + .version = NVC_FOCUS_CAP_VER2, .actuator_range = SH532U_ACTUATOR_RANGE, .settle_time = SH532U_SETTLETIME, .focus_macro = SH532U_FOCUS_MACRO, @@ -299,91 +322,227 @@ static int sh532u_i2c_rd32(struct sh532u_info *info, u8 addr, u8 reg, u32 *val) return 0; } -static void sh532u_gpio_en(struct sh532u_info *info, int val) +static int sh532u_gpio_wr(struct sh532u_info *info, + enum sh532u_gpio i, + int val) /* val: 0=deassert, 1=assert */ { - if (info->pdata->gpio_en) - gpio_set_value_cansleep(info->pdata->gpio_en, val); + int err = -EINVAL; + + if (info->gpio[i].flag) { + if (val) + val = 1; + if (!info->gpio[i].active_high) + val = !val; + val &= 1; + err = val; + gpio_set_value_cansleep(info->gpio[i].gpio, val); + dev_dbg(&info->i2c_client->dev, "%s %u %d\n", + __func__, info->gpio[i].gpio, val); + } + return err; /* return value written or error */ } -static void sh532u_gpio_reset(struct sh532u_info *info, int val) +static int sh532u_gpio_reset(struct sh532u_info *info, int val) { + int err = 0; + if (val) { - if (!info->gpio_flag_reset && info->pdata->gpio_reset) { - gpio_set_value_cansleep(info->pdata->gpio_reset, 0); + if (!info->reset_flag) { + info->reset_flag = true; + err = sh532u_gpio_wr(info, SH532U_GPIO_RESET, 1); + if (err < 0) + return 0; /* flag no reset */ + mdelay(1); - gpio_set_value_cansleep(info->pdata->gpio_reset, 1); - mdelay(10); /* delay for device startup */ - info->gpio_flag_reset = 1; + sh532u_gpio_wr(info, SH532U_GPIO_RESET, 0); + mdelay(SH532U_STARTUP_DELAY_MS); /* startup delay */ + err = 1; /* flag that a reset was done */ } } else { - info->gpio_flag_reset = 0; + info->reset_flag = false; } + return err; } -static void sh532u_pm_regulator_put(struct nvc_regulator *sreg) +static void sh532u_gpio_able(struct sh532u_info *info, int val) { - regulator_put(sreg->vreg); - sreg->vreg = NULL; + if (val) { + sh532u_gpio_wr(info, SH532U_GPIO_GP1, val); + sh532u_gpio_wr(info, SH532U_GPIO_GP2, val); + sh532u_gpio_wr(info, SH532U_GPIO_GP3, val); + } else { + sh532u_gpio_wr(info, SH532U_GPIO_GP3, val); + sh532u_gpio_wr(info, SH532U_GPIO_GP2, val); + sh532u_gpio_wr(info, SH532U_GPIO_GP1, val); + } } -static int sh532u_pm_regulator_get(struct sh532u_info *info, - struct nvc_regulator *sreg, - char vreg_name[]) +static void sh532u_gpio_exit(struct sh532u_info *info) +{ + unsigned i; + + for (i = 0; i <= ARRAY_SIZE(sh532u_gpios); i++) { + if (info->gpio[i].flag && info->gpio[i].own) { + gpio_free(info->gpio[i].gpio); + info->gpio[i].own = false; + } + } +} + +static void sh532u_gpio_init(struct sh532u_info *info) +{ + char label[32]; + unsigned long flags; + unsigned type; + unsigned i; + unsigned j; + int err; + + for (i = 0; i < ARRAY_SIZE(sh532u_gpios); i++) + info->gpio[i].flag = false; + if (!info->pdata->gpio_count || !info->pdata->gpio) + return; + + for (i = 0; i < ARRAY_SIZE(sh532u_gpios); i++) { + type = sh532u_gpios[i].gpio_type; + for (j = 0; j < info->pdata->gpio_count; j++) { + if (type == info->pdata->gpio[j].gpio_type) + break; + } + if (j == info->pdata->gpio_count) + continue; + + info->gpio[type].gpio = info->pdata->gpio[j].gpio; + info->gpio[type].flag = true; + if (sh532u_gpios[i].use_flags) { + flags = sh532u_gpios[i].flags; + info->gpio[type].active_high = + sh532u_gpios[i].active_high; + } else { + info->gpio[type].active_high = + info->pdata->gpio[j].active_high; + if (info->gpio[type].active_high) + flags = GPIOF_OUT_INIT_LOW; + else + flags = GPIOF_OUT_INIT_HIGH; + } + if (!info->pdata->gpio[j].init_en) + continue; + + snprintf(label, sizeof(label), "sh532u_%u_%s", + info->pdata->num, sh532u_gpios[i].label); + err = gpio_request_one(info->gpio[type].gpio, flags, label); + if (err) { + dev_err(&info->i2c_client->dev, "%s ERR %s %u\n", + __func__, label, info->gpio[type].gpio); + } else { + info->gpio[type].own = true; + dev_dbg(&info->i2c_client->dev, "%s %s %u\n", + __func__, label, info->gpio[type].gpio); + } + } +} + +static int sh532u_vreg_dis(struct sh532u_info *info, + enum sh532u_vreg i) { int err = 0; - sreg->vreg_flag = 0; - sreg->vreg = regulator_get(&info->i2c_client->dev, vreg_name); - if (IS_ERR_OR_NULL(sreg->vreg)) { - dev_err(&info->i2c_client->dev, - "%s err for regulator: %s err: %d\n", - __func__, vreg_name, (int)sreg->vreg); - err = PTR_ERR(sreg->vreg); - sreg->vreg = NULL; - } else { - sreg->vreg_name = vreg_name; - dev_dbg(&info->i2c_client->dev, - "%s vreg_name: %s\n", - __func__, sreg->vreg_name); + if (info->vreg[i].vreg_flag && (info->vreg[i].vreg != NULL)) { + err = regulator_disable(info->vreg[i].vreg); + if (!err) + dev_dbg(&info->i2c_client->dev, "%s: %s\n", + __func__, info->vreg[i].vreg_name); + else + dev_err(&info->i2c_client->dev, "%s %s ERR\n", + __func__, info->vreg[i].vreg_name); } + info->vreg[i].vreg_flag = false; return err; } -static int sh532u_pm_regulator_en(struct sh532u_info *info, - struct nvc_regulator *sreg) +static int sh532u_vreg_dis_all(struct sh532u_info *info) { + unsigned i; int err = 0; - if (!sreg->vreg_flag && (sreg->vreg != NULL)) { - err = regulator_enable(sreg->vreg); + for (i = ARRAY_SIZE(sh532u_vregs); i > 0; i--) + err |= sh532u_vreg_dis(info, (i - 1)); + return err; +} + +static int sh532u_vreg_en(struct sh532u_info *info, + enum sh532u_vreg i) +{ + int err = 0; + + if (!info->vreg[i].vreg_flag && (info->vreg[i].vreg != NULL)) { + err = regulator_enable(info->vreg[i].vreg); if (!err) { - dev_dbg(&info->i2c_client->dev, - "%s vreg_name: %s\n", - __func__, sreg->vreg_name); - sreg->vreg_flag = 1; + dev_dbg(&info->i2c_client->dev, "%s: %s\n", + __func__, info->vreg[i].vreg_name); + info->vreg[i].vreg_flag = true; err = 1; /* flag regulator state change */ } else { - dev_err(&info->i2c_client->dev, - "%s err, regulator: %s\n", - __func__, sreg->vreg_name); + dev_err(&info->i2c_client->dev, "%s %s ERR\n", + __func__, info->vreg[i].vreg_name); } } return err; } -static int sh532u_pm_regulator_dis(struct sh532u_info *info, - struct nvc_regulator *sreg) +static int sh532u_vreg_en_all(struct sh532u_info *info) { + unsigned i; int err = 0; - if (sreg->vreg_flag && (sreg->vreg != NULL)) { - err = regulator_disable(sreg->vreg); - if (err) - dev_err(&info->i2c_client->dev, - "%s err, regulator: %s\n", - __func__, sreg->vreg_name); + for (i = 0; i < ARRAY_SIZE(sh532u_vregs); i++) + err |= sh532u_vreg_en(info, i); + return err; +} + +static void sh532u_vreg_exit(struct sh532u_info *info) +{ + unsigned i; + + for (i = 0; i < ARRAY_SIZE(sh532u_vregs); i++) { + regulator_put(info->vreg[i].vreg); + info->vreg[i].vreg = NULL; + } +} + +static int sh532u_vreg_init(struct sh532u_info *info) +{ + unsigned i; + unsigned j; + int err = 0; + + for (i = 0; i < ARRAY_SIZE(sh532u_vregs); i++) { + j = sh532u_vregs[i].vreg_num; + info->vreg[j].vreg_name = sh532u_vregs[i].vreg_name; + info->vreg[j].vreg_flag = false; + info->vreg[j].vreg = regulator_get(&info->i2c_client->dev, + info->vreg[j].vreg_name); + if (IS_ERR_OR_NULL(info->vreg[j].vreg)) { + if (PTR_ERR(info->vreg[j].vreg) != -ENODEV) + dev_dbg(&info->i2c_client->dev, + "%s %s ERR: %d\n", + __func__, info->vreg[j].vreg_name, + (int)info->vreg[j].vreg); + else + dev_info(&info->i2c_client->dev, + "%s no regulator found for %s. " + "This board may not have an" + "independent %s regulator.\n", + __func__, info->vreg[j].vreg_name, + info->vreg[j].vreg_name); + err |= PTR_ERR(info->vreg[j].vreg); + info->vreg[j].vreg = NULL; + } else { + dev_dbg(&info->i2c_client->dev, "%s: %s\n", + __func__, info->vreg[j].vreg_name); + } } - sreg->vreg_flag = 0; return err; } @@ -402,17 +561,15 @@ static int sh532u_pm_wr(struct sh532u_info *info, int pwr) switch (pwr) { case NVC_PWR_OFF_FORCE: case NVC_PWR_OFF: - sh532u_gpio_en(info, 0); - err = sh532u_pm_regulator_dis(info, &info->vreg_vdd); - err |= sh532u_pm_regulator_dis(info, &info->vreg_i2c); + err = sh532u_vreg_dis_all(info); + sh532u_gpio_able(info, 0); sh532u_gpio_reset(info, 0); break; case NVC_PWR_STDBY_OFF: case NVC_PWR_STDBY: - err = sh532u_pm_regulator_en(info, &info->vreg_vdd); - err |= sh532u_pm_regulator_en(info, &info->vreg_i2c); - sh532u_gpio_en(info, 1); + err = sh532u_vreg_en_all(info); + sh532u_gpio_able(info, 1); sh532u_gpio_reset(info, 1); err |= sh532u_i2c_wr8(info, STBY_211, 0x80); err |= sh532u_i2c_wr8(info, CLKSEL_211, 0x38); @@ -421,9 +578,8 @@ static int sh532u_pm_wr(struct sh532u_info *info, int pwr) case NVC_PWR_COMM: case NVC_PWR_ON: - err = sh532u_pm_regulator_en(info, &info->vreg_vdd); - err |= sh532u_pm_regulator_en(info, &info->vreg_i2c); - sh532u_gpio_en(info, 1); + err = sh532u_vreg_en_all(info); + sh532u_gpio_able(info, 1); sh532u_gpio_reset(info, 1); err |= sh532u_i2c_wr8(info, CLKSEL_211, 0x38); err |= sh532u_i2c_wr8(info, CLKSEL_211, 0x34); @@ -436,11 +592,12 @@ static int sh532u_pm_wr(struct sh532u_info *info, int pwr) } if (err < 0) { - dev_err(&info->i2c_client->dev, "%s pwr err: %d\n", - __func__, pwr); + dev_err(&info->i2c_client->dev, "%s err %d\n", __func__, err); pwr = NVC_PWR_ERR; } info->pwr_dev = pwr; + dev_dbg(&info->i2c_client->dev, "%s pwr_dev=%d\n", + __func__, info->pwr_dev); if (err > 0) return 0; @@ -493,31 +650,58 @@ static int sh532u_pm_dev_wr(struct sh532u_info *info, int pwr) static void sh532u_pm_exit(struct sh532u_info *info) { sh532u_pm_wr(info, NVC_PWR_OFF_FORCE); - sh532u_pm_regulator_put(&info->vreg_vdd); - sh532u_pm_regulator_put(&info->vreg_i2c); - if (info->s_info != NULL) { - sh532u_pm_wr(info->s_info, NVC_PWR_OFF_FORCE); - sh532u_pm_regulator_put(&info->s_info->vreg_vdd); - sh532u_pm_regulator_put(&info->s_info->vreg_i2c); - } + sh532u_vreg_exit(info); + sh532u_gpio_exit(info); } static void sh532u_pm_init(struct sh532u_info *info) { - sh532u_pm_regulator_get(info, &info->vreg_vdd, "vdd"); - sh532u_pm_regulator_get(info, &info->vreg_i2c, "vdd_i2c"); + sh532u_gpio_init(info); + sh532u_vreg_init(info); +} + +static int sh532u_reset(struct sh532u_info *info, u32 level) +{ + int err; + + if (level == NVC_RESET_SOFT) { + err = sh532u_pm_wr(info, NVC_PWR_COMM); + err |= sh532u_i2c_wr8(info, SFTRST_211, 0xFF); /* SW reset */ + mdelay(1); + err |= sh532u_i2c_wr8(info, SFTRST_211, 0); + } else { + err = sh532u_pm_wr(info, NVC_PWR_OFF_FORCE); + } + err |= sh532u_pm_wr(info, info->pwr_api); + return err; } static int sh532u_dev_id(struct sh532u_info *info) { - u8 val; + u8 val = 0; + unsigned i; int err; + sh532u_pm_dev_wr(info, NVC_PWR_COMM); err = sh532u_i2c_rd8(info, 0, HVCA_DEVICE_ID, &val); - if (!err && (val == SH532U_ID)) - return 0; - - return -ENODEV; + if (!err) { + dev_dbg(&info->i2c_client->dev, "%s found devId: %x\n", + __func__, val); + info->id_minor = 0; + for (i = 0; i < ARRAY_SIZE(sh532u_ids); i++) { + if (val == sh532u_ids[i]) { + info->id_minor = val; + break; + } + } + if (!info->id_minor) { + err = -ENODEV; + dev_dbg(&info->i2c_client->dev, "%s No devId match\n", + __func__); + } + } + sh532u_pm_dev_wr(info, NVC_PWR_OFF); + return err; } static void sh532u_sts_rd(struct sh532u_info *info) @@ -633,9 +817,8 @@ static int sh532u_rel_pos_rd(struct sh532u_info *info, u32 *position) return 0; } -static int sh532u_calibration(struct sh532u_info *info, bool use_defaults) +static void sh532u_calibration_caps(struct sh532u_info *info) { - u8 reg; s16 abs_top; u32 rel_range; u32 rel_lo; @@ -643,45 +826,75 @@ static int sh532u_calibration(struct sh532u_info *info, bool use_defaults) u32 step; u32 loop_limit; u32 i; + + /* + * calculate relative and absolute positions + * Note that relative values, what upper SW uses, are the + * abstraction of HW (absolute) values. + * |<--limit_low limit_high-->| + * | |<-------------------_ACTUATOR_RANGE------------------->| | + * -focus_inf -focus_mac + * |<---RI--->| |<---RM--->| + * -abs_base -pos_low -pos_high -abs_top + * + * The pos_low and pos_high are fixed absolute positions and correspond + * to the relative focus_infinity and focus_macro, respectively. We'd + * like to have "wiggle" room (RI and RM) around these relative + * positions so the loop below finds the best fit for RI and RM without + * passing the absolute limits. + * We want our _ACTUATOR_RANGE to be infinity on the 0 end and macro + * on the max end. However, the focuser HW is opposite this. + * Therefore we use the rel(ative)_lo/hi variables in the calculation + * loop and assign them the focus_infinity and focus_macro values. + */ + rel_lo = (info->cap.actuator_range - info->cap.focus_macro); + rel_hi = info->cap.focus_infinity; + info->abs_range = (u32)(info->cfg.pos_high - info->cfg.pos_low); + loop_limit = (rel_lo > rel_hi) ? rel_lo : rel_hi; + for (i = 0; i <= loop_limit; i++) { + rel_range = info->cap.actuator_range - (rel_lo + rel_hi); + step = info->abs_range / rel_range; + info->abs_base = info->cfg.pos_low - (step * rel_lo); + abs_top = info->cfg.pos_high + (step * rel_hi); + if (info->abs_base < info->cfg.limit_low) { + if (rel_lo > 0) + rel_lo--; + } + if (abs_top > info->cfg.limit_high) { + if (rel_hi > 0) + rel_hi--; + } + if (info->abs_base >= info->cfg.limit_low && + abs_top <= info->cfg.limit_high) + break; + } + info->cap.focus_hyper = info->abs_range; + info->abs_range = (u32)(abs_top - info->abs_base); + /* calculate absolute hyperfocus position */ + info->cap.focus_hyper *= info->cfg.focus_hyper_ratio; + info->cap.focus_hyper /= info->cfg.focus_hyper_div; + abs_top = (s16)(info->cfg.pos_high - info->cap.focus_hyper); + /* update actual relative positions */ + info->cap.focus_hyper = sh532u_abs2rel(info, abs_top); + info->cap.focus_infinity = sh532u_abs2rel(info, info->cfg.pos_high); + info->cap.focus_macro = sh532u_abs2rel(info, info->cfg.pos_low); + dev_dbg(&info->i2c_client->dev, "%s focus_macro=%u\n", + __func__, info->cap.focus_macro); + dev_dbg(&info->i2c_client->dev, "%s focus_infinity=%u\n", + __func__, info->cap.focus_infinity); + dev_dbg(&info->i2c_client->dev, "%s focus_hyper=%u\n", + __func__, info->cap.focus_hyper); +} + +static int sh532u_calibration(struct sh532u_info *info, bool use_defaults) +{ + u8 reg; int err; int ret = 0; if (info->init_cal_flag) return 0; - /* set defaults */ - memcpy(&info->cfg, &sh532u_default_info, sizeof(info->cfg)); - memcpy(&info->nvc, &sh532u_default_nvc, sizeof(info->nvc)); - memcpy(&info->cap, &sh532u_default_cap, sizeof(info->cap)); - if (info->pdata->i2c_addr_rom) - info->i2c_addr_rom = info->pdata->i2c_addr_rom; - else - info->i2c_addr_rom = sh532u_default_pdata.i2c_addr_rom; - /* set overrides if any */ - if (info->pdata->nvc) { - if (info->pdata->nvc->fnumber) - info->nvc.fnumber = info->pdata->nvc->fnumber; - if (info->pdata->nvc->focal_length) - info->nvc.focal_length = - info->pdata->nvc->focal_length; - if (info->pdata->nvc->max_aperature) - info->nvc.max_aperature = - info->pdata->nvc->max_aperature; - } - if (info->pdata->cap) { - if (info->pdata->cap->actuator_range) - info->cap.actuator_range = - info->pdata->cap->actuator_range; - if (info->pdata->cap->settle_time) - info->cap.settle_time = info->pdata->cap->settle_time; - if (info->pdata->cap->focus_macro) - info->cap.focus_macro = info->pdata->cap->focus_macro; - if (info->pdata->cap->focus_hyper) - info->cap.focus_hyper = info->pdata->cap->focus_hyper; - if (info->pdata->cap->focus_infinity) - info->cap.focus_infinity = - info->pdata->cap->focus_infinity; - } /* * Get Inf1, Mac1 * Inf1 and Mac1 are the mechanical limit position. @@ -770,78 +983,22 @@ static int sh532u_calibration(struct sh532u_info *info, bool use_defaults) info->cfg.limit_low = SH532U_POS_LOW_DEFAULT; info->cfg.limit_high = SH532U_POS_HIGH_DEFAULT; dev_err(&info->i2c_client->dev, "%s ERR: ERPOM data is void! " - "Focuser will use defaults that will cause " - "reduced functionality!\n", __func__); + "Focuser will use defaults that will cause " + "reduced functionality!\n", __func__); } if (info->cfg.pos_low < info->cfg.limit_low) info->cfg.pos_low = info->cfg.limit_low; if (info->cfg.pos_high > info->cfg.limit_high) info->cfg.pos_high = info->cfg.limit_high; - dev_dbg(&info->i2c_client->dev, "%s pos_low=%d\n", __func__, - (int)info->cfg.pos_low); - dev_dbg(&info->i2c_client->dev, "%s pos_high=%d\n", __func__, - (int)info->cfg.pos_high); - dev_dbg(&info->i2c_client->dev, "%s limit_low=%d\n", __func__, - (int)info->cfg.limit_low); - dev_dbg(&info->i2c_client->dev, "%s limit_high=%d\n", __func__, - (int)info->cfg.limit_high); - /* - * calculate relative and absolute positions - * Note that relative values, what upper SW uses, are the - * abstraction of HW (absolute) values. - * |<--limit_low limit_high-->| - * | |<-------------------_ACTUATOR_RANGE------------------->| | - * -focus_inf -focus_mac - * |<---RI--->| |<---RM--->| - * -abs_base -pos_low -pos_high -abs_top - * - * The pos_low and pos_high are fixed absolute positions and correspond - * to the relative focus_infinity and focus_macro, respectively. We'd - * like to have "wiggle" room (RI and RM) around these relative - * positions so the loop below finds the best fit for RI and RM without - * passing the absolute limits. - * We want our _ACTUATOR_RANGE to be infinity on the 0 end and macro - * on the max end. However, the focuser HW is opposite this. - * Therefore we use the rel(ative)_lo/hi variables in the calculation - * loop and assign them the focus_infinity and focus_macro values. - */ - rel_lo = (info->cap.actuator_range - info->cap.focus_macro); - rel_hi = info->cap.focus_infinity; - info->abs_range = (u32)(info->cfg.pos_high - info->cfg.pos_low); - loop_limit = (rel_lo > rel_hi) ? rel_lo : rel_hi; - for (i = 0; i <= loop_limit; i++) { - rel_range = info->cap.actuator_range - (rel_lo + rel_hi); - step = info->abs_range / rel_range; - info->abs_base = info->cfg.pos_low - (step * rel_lo); - abs_top = info->cfg.pos_high + (step * rel_hi); - if (info->abs_base < info->cfg.limit_low) { - if (rel_lo > 0) - rel_lo--; - } - if (abs_top > info->cfg.limit_high) { - if (rel_hi > 0) - rel_hi--; - } - if (info->abs_base >= info->cfg.limit_low && - abs_top <= info->cfg.limit_high) - break; - } - info->cap.focus_hyper = info->abs_range; - info->abs_range = (u32)(abs_top - info->abs_base); - /* calculate absolute hyperfocus position */ - info->cap.focus_hyper *= info->cfg.focus_hyper_ratio; - info->cap.focus_hyper /= info->cfg.focus_hyper_div; - abs_top = (s16)(info->cfg.pos_high - info->cap.focus_hyper); - /* update actual relative positions */ - info->cap.focus_hyper = sh532u_abs2rel(info, abs_top); - info->cap.focus_infinity = sh532u_abs2rel(info, info->cfg.pos_high); - info->cap.focus_macro = sh532u_abs2rel(info, info->cfg.pos_low); - dev_dbg(&info->i2c_client->dev, "%s focus_macro=%u\n", __func__, - info->cap.focus_macro); - dev_dbg(&info->i2c_client->dev, "%s focus_infinity=%u\n", __func__, - info->cap.focus_infinity); - dev_dbg(&info->i2c_client->dev, "%s focus_hyper=%u\n", __func__, - info->cap.focus_hyper); + dev_dbg(&info->i2c_client->dev, "%s pos_low=%d\n", + __func__, (int)info->cfg.pos_low); + dev_dbg(&info->i2c_client->dev, "%s pos_high=%d\n", + __func__, (int)info->cfg.pos_high); + dev_dbg(&info->i2c_client->dev, "%s limit_low=%d\n", + __func__, (int)info->cfg.limit_low); + dev_dbg(&info->i2c_client->dev, "%s limit_high=%d\n", + __func__, (int)info->cfg.limit_high); + sh532u_calibration_caps(info); info->init_cal_flag = 1; dev_dbg(&info->i2c_client->dev, "%s complete\n", __func__); return 0; @@ -955,6 +1112,9 @@ static int sh532u_dev_init(struct sh532u_info *info) int ret = 0; err = sh532u_i2c_rd8(info, 0, SWTCH_211, &ep_data1); + if (err) + return err; /* exit if unable to communicate with device */ + ep_data2 = ep_data1; err |= sh532u_i2c_rd8(info, 0, ANA1_211, &ep_data1); ep_data2 |= ep_data1; @@ -1007,7 +1167,7 @@ static int sh532u_dev_init(struct sh532u_info *info) err = ret; if (err) dev_err(&info->i2c_client->dev, "%s programming err=%d\n", - __func__, err); + __func__, err); err |= sh532u_calibration(info, false); info->sts = NVC_FOCUS_STS_LENS_SETTLED; return err; @@ -1031,7 +1191,7 @@ static int sh532u_pos_abs_wr(struct sh532u_info *info, s16 tar_pos) return err; dev_dbg(&info->i2c_client->dev, "%s cur_pos=%d tar_pos=%d\n", - __func__, (int)cur_pos, (int)tar_pos); + __func__, (int)cur_pos, (int)tar_pos); info->sts = NVC_FOCUS_STS_WAIT_FOR_MOVE_END; /* Check move distance to Target Position */ move_distance = abs((int)cur_pos - (int)tar_pos); @@ -1135,7 +1295,7 @@ static int sh532u_pos_rel_wr(struct sh532u_info *info, u32 position) if (position > info->cap.actuator_range) { dev_err(&info->i2c_client->dev, "%s invalid position %u\n", - __func__, position); + __func__, position); return -EINVAL; } @@ -1159,7 +1319,7 @@ static int sh532u_param_rd(struct sh532u_info *info, unsigned long arg) (const void __user *)arg, sizeof(struct nvc_param))) { dev_err(&info->i2c_client->dev, "%s %d copy_from_user err\n", - __func__, __LINE__); + __func__, __LINE__); return -EFAULT; } @@ -1176,28 +1336,28 @@ static int sh532u_param_rd(struct sh532u_info *info, unsigned long arg) data_size = sizeof(position); sh532u_pm_dev_wr(info, NVC_PWR_STDBY); dev_dbg(&info->i2c_client->dev, "%s LOCUS: %d\n", - __func__, position); + __func__, position); break; case NVC_PARAM_FOCAL_LEN: data_ptr = &info->nvc.focal_length; data_size = sizeof(info->nvc.focal_length); dev_dbg(&info->i2c_client->dev, "%s FOCAL_LEN: %x\n", - __func__, info->nvc.focal_length); + __func__, info->nvc.focal_length); break; case NVC_PARAM_MAX_APERTURE: data_ptr = &info->nvc.max_aperature; data_size = sizeof(info->nvc.max_aperature); dev_dbg(&info->i2c_client->dev, "%s MAX_APERTURE: %x\n", - __func__, info->nvc.max_aperature); + __func__, info->nvc.max_aperature); break; case NVC_PARAM_FNUMBER: data_ptr = &info->nvc.fnumber; data_size = sizeof(info->nvc.fnumber); - dev_dbg(&info->i2c_client->dev, "%s FNUMBER: %x\n", - __func__, info->nvc.fnumber); + dev_dbg(&info->i2c_client->dev, "%s FNUMBER: %u\n", + __func__, info->nvc.fnumber); break; case NVC_PARAM_CAPS: @@ -1215,41 +1375,42 @@ static int sh532u_param_rd(struct sh532u_info *info, unsigned long arg) else data_size = sizeof(info->cap); dev_dbg(&info->i2c_client->dev, "%s CAPS\n", - __func__); + __func__); break; case NVC_PARAM_STS: data_ptr = &info->sts; data_size = sizeof(info->sts); dev_dbg(&info->i2c_client->dev, "%s STS: %d\n", - __func__, info->sts); + __func__, info->sts); break; case NVC_PARAM_STEREO: data_ptr = &info->s_mode; data_size = sizeof(info->s_mode); dev_dbg(&info->i2c_client->dev, "%s STEREO: %d\n", - __func__, info->s_mode); + __func__, info->s_mode); break; default: - dev_err(&info->i2c_client->dev, - "%s unsupported parameter: %d\n", - __func__, params.param); + dev_dbg(&info->i2c_client->dev, + "%s unsupported parameter: %d\n", + __func__, params.param); return -EINVAL; } if (params.sizeofvalue < data_size) { - dev_err(&info->i2c_client->dev, "%s %d data size err\n", - __func__, __LINE__); + dev_err(&info->i2c_client->dev, + "%s data size mismatch %d != %d Param: %d\n", + __func__, params.sizeofvalue, data_size, params.param); return -EINVAL; } if (copy_to_user((void __user *)params.p_value, data_ptr, data_size)) { - dev_err(&info->i2c_client->dev, "%s %d copy_to_user err\n", - __func__, __LINE__); + dev_err(&info->i2c_client->dev, + "%s copy_to_user err line %d\n", __func__, __LINE__); return -EFAULT; } @@ -1258,35 +1419,59 @@ static int sh532u_param_rd(struct sh532u_info *info, unsigned long arg) static int sh532u_param_wr_s(struct sh532u_info *info, struct nvc_param *params, - u32 u32_val) + u32 u32val) { + struct nvc_focus_cap cap; + u8 u8val; int err; + u8val = (u8)u32val; switch (params->param) { case NVC_PARAM_LOCUS: dev_dbg(&info->i2c_client->dev, "%s LOCUS: %u\n", - __func__, u32_val); - err = sh532u_pos_rel_wr(info, u32_val); + __func__, u32val); + err = sh532u_pos_rel_wr(info, u32val); return err; case NVC_PARAM_RESET: - err = sh532u_pm_wr(info, NVC_PWR_OFF); - err |= sh532u_pm_wr(info, NVC_PWR_ON); - err |= sh532u_pm_wr(info, info->pwr_api); + err = sh532u_reset(info, u32val); dev_dbg(&info->i2c_client->dev, "%s RESET: %d\n", - __func__, err); + __func__, err); return err; case NVC_PARAM_SELF_TEST: err = sh532u_hvca_pos_init(info); dev_dbg(&info->i2c_client->dev, "%s SELF_TEST: %d\n", - __func__, err); + __func__, err); return err; + case NVC_PARAM_CAPS: + dev_dbg(&info->i2c_client->dev, "%s CAPS\n", + __func__); + if (copy_from_user(&cap, (const void __user *)params->p_value, + sizeof(params->sizeofvalue))) { + dev_err(&info->i2c_client->dev, "%s %d copy_from_user err\n", + __func__, __LINE__); + return -EFAULT; + } + + if (!cap.version) + return -EINVAL; + + if (cap.version >= NVC_FOCUS_CAP_VER1) + info->cap.actuator_range = cap.actuator_range; + if (cap.version >= NVC_FOCUS_CAP_VER2) { + info->cap.focus_macro = cap.focus_macro; + info->cap.focus_hyper = cap.focus_hyper; + info->cap.focus_infinity = cap.focus_infinity; + } + sh532u_calibration_caps(info); + return 0; + default: - dev_err(&info->i2c_client->dev, - "%s unsupported parameter: %d\n", - __func__, params->param); + dev_dbg(&info->i2c_client->dev, + "%s unsupported parameter: %d\n", + __func__, params->param); return -EINVAL; } } @@ -1294,47 +1479,48 @@ static int sh532u_param_wr_s(struct sh532u_info *info, static int sh532u_param_wr(struct sh532u_info *info, unsigned long arg) { struct nvc_param params; - u8 val; - u32 u32_val; + u8 u8val; + u32 u32val; int err = 0; - if (copy_from_user(¶ms, - (const void __user *)arg, - sizeof(struct nvc_param))) { - dev_err(&info->i2c_client->dev, "%s %d copy_from_user err\n", - __func__, __LINE__); + if (copy_from_user(¶ms, (const void __user *)arg, + sizeof(struct nvc_param))) { + dev_err(&info->i2c_client->dev, + "%s copy_from_user err line %d\n", __func__, __LINE__); return -EFAULT; } - if (copy_from_user(&u32_val, (const void __user *)params.p_value, - sizeof(u32_val))) { + if (copy_from_user(&u32val, (const void __user *)params.p_value, + sizeof(u32val))) { dev_err(&info->i2c_client->dev, "%s %d copy_from_user err\n", - __func__, __LINE__); + __func__, __LINE__); return -EFAULT; } + u8val = (u8)u32val; /* parameters independent of sync mode */ switch (params.param) { case NVC_PARAM_STEREO: - dev_dbg(&info->i2c_client->dev, "%s STEREO: %u\n", - __func__, u32_val); - val = (u8)u32_val; - if (val == info->s_mode) + dev_dbg(&info->i2c_client->dev, "%s STEREO: %d\n", + __func__, u8val); + if (u8val == info->s_mode) return 0; - switch (val) { + switch (u8val) { case NVC_SYNC_OFF: - info->s_mode = val; + info->s_mode = u8val; + sh532u_gpio_wr(info, SH532U_GPIO_I2CMUX, 0); if (info->s_info != NULL) { - info->s_info->s_mode = val; + info->s_info->s_mode = u8val; sh532u_pm_wr(info->s_info, NVC_PWR_OFF); } break; case NVC_SYNC_MASTER: - info->s_mode = val; + info->s_mode = u8val; + sh532u_gpio_wr(info, SH532U_GPIO_I2CMUX, 0); if (info->s_info != NULL) - info->s_info->s_mode = val; + info->s_info->s_mode = u8val; break; case NVC_SYNC_SLAVE: @@ -1343,8 +1529,10 @@ static int sh532u_param_wr(struct sh532u_info *info, unsigned long arg) err = sh532u_pos_rel_wr(info->s_info, info->s_info->cap.focus_infinity); if (!err) { - info->s_mode = val; - info->s_info->s_mode = val; + info->s_mode = u8val; + info->s_info->s_mode = u8val; + sh532u_gpio_wr(info, + SH532U_GPIO_I2CMUX, 0); } else { if (info->s_mode != NVC_SYNC_STEREO) sh532u_pm_wr(info->s_info, @@ -1364,8 +1552,10 @@ static int sh532u_param_wr(struct sh532u_info *info, unsigned long arg) err = sh532u_pos_rel_wr(info->s_info, info->pos_rel); if (!err) { - info->s_mode = val; - info->s_info->s_mode = val; + info->s_mode = u8val; + info->s_info->s_mode = u8val; + sh532u_gpio_wr(info, + SH532U_GPIO_I2CMUX, 1); } else { if (info->s_mode != NVC_SYNC_SLAVE) sh532u_pm_wr(info->s_info, @@ -1390,49 +1580,52 @@ static int sh532u_param_wr(struct sh532u_info *info, unsigned long arg) switch (info->s_mode) { case NVC_SYNC_OFF: case NVC_SYNC_MASTER: - return sh532u_param_wr_s(info, ¶ms, u32_val); + return sh532u_param_wr_s(info, ¶ms, u32val); case NVC_SYNC_SLAVE: - return sh532u_param_wr_s(info->s_info, - ¶ms, - u32_val); + return sh532u_param_wr_s(info->s_info, ¶ms, + u32val); case NVC_SYNC_STEREO: - err = sh532u_param_wr_s(info, ¶ms, u32_val); + err = sh532u_param_wr_s(info, ¶ms, u32val); if (!(info->pdata->cfg & NVC_CFG_SYNC_I2C_MUX)) err |= sh532u_param_wr_s(info->s_info, ¶ms, - u32_val); + u32val); return err; default: dev_err(&info->i2c_client->dev, "%s %d internal err\n", - __func__, __LINE__); + __func__, __LINE__); return -EINVAL; } } } static long sh532u_ioctl(struct file *file, - unsigned int cmd, - unsigned long arg) + unsigned int cmd, + unsigned long arg) { struct sh532u_info *info = file->private_data; int pwr; + int err; switch (cmd) { case NVC_IOCTL_PARAM_WR: - return sh532u_param_wr(info, arg); + err = sh532u_param_wr(info, arg); + return err; case NVC_IOCTL_PARAM_RD: - return sh532u_param_rd(info, arg); + err = sh532u_param_rd(info, arg); + return err; case NVC_IOCTL_PWR_WR: /* This is a Guaranteed Level of Service (GLOS) call */ pwr = (int)arg * 2; - dev_dbg(&info->i2c_client->dev, "%s PWR: %d\n", - __func__, pwr); - return sh532u_pm_api_wr(info, pwr); + dev_dbg(&info->i2c_client->dev, "%s PWR_WR: %d\n", + __func__, pwr); + err = sh532u_pm_api_wr(info, pwr); + return err; case NVC_IOCTL_PWR_RD: if (info->s_mode == NVC_SYNC_SLAVE) @@ -1440,7 +1633,7 @@ static long sh532u_ioctl(struct file *file, else pwr = info->pwr_api / 2; dev_dbg(&info->i2c_client->dev, "%s PWR_RD: %d\n", - __func__, pwr); + __func__, pwr); if (copy_to_user((void __user *)arg, (const void *)&pwr, sizeof(pwr))) { dev_err(&info->i2c_client->dev, @@ -1452,46 +1645,84 @@ static long sh532u_ioctl(struct file *file, return 0; default: - dev_err(&info->i2c_client->dev, "%s unsupported ioctl: %x\n", - __func__, cmd); - return -EINVAL; + dev_dbg(&info->i2c_client->dev, "%s unsupported ioctl: %x\n", + __func__, cmd); } + + return -EINVAL; } -static int sh532u_sync_en(int dev1, int dev2) +static void sh532u_sdata_init(struct sh532u_info *info) { - struct sh532u_info *sync1 = NULL; - struct sh532u_info *sync2 = NULL; + /* set defaults */ + memcpy(&info->cfg, &sh532u_default_info, sizeof(info->cfg)); + memcpy(&info->nvc, &sh532u_default_nvc, sizeof(info->nvc)); + memcpy(&info->cap, &sh532u_default_cap, sizeof(info->cap)); + if (info->pdata->i2c_addr_rom) + info->i2c_addr_rom = info->pdata->i2c_addr_rom; + else + info->i2c_addr_rom = sh532u_default_pdata.i2c_addr_rom; + /* set overrides if any */ + if (info->pdata->nvc) { + if (info->pdata->nvc->fnumber) + info->nvc.fnumber = info->pdata->nvc->fnumber; + if (info->pdata->nvc->focal_length) + info->nvc.focal_length = + info->pdata->nvc->focal_length; + if (info->pdata->nvc->max_aperature) + info->nvc.max_aperature = + info->pdata->nvc->max_aperature; + } + if (info->pdata->cap) { + if (info->pdata->cap->actuator_range) + info->cap.actuator_range = + info->pdata->cap->actuator_range; + if (info->pdata->cap->settle_time) + info->cap.settle_time = info->pdata->cap->settle_time; + if (info->pdata->cap->focus_macro) + info->cap.focus_macro = info->pdata->cap->focus_macro; + if (info->pdata->cap->focus_hyper) + info->cap.focus_hyper = info->pdata->cap->focus_hyper; + if (info->pdata->cap->focus_infinity) + info->cap.focus_infinity = + info->pdata->cap->focus_infinity; + } +} + +static int sh532u_sync_en(unsigned num, unsigned sync) +{ + struct sh532u_info *master = NULL; + struct sh532u_info *slave = NULL; struct sh532u_info *pos = NULL; rcu_read_lock(); list_for_each_entry_rcu(pos, &sh532u_info_list, list) { - if (pos->pdata->num == dev1) { - sync1 = pos; + if (pos->pdata->num == num) { + master = pos; break; } } pos = NULL; list_for_each_entry_rcu(pos, &sh532u_info_list, list) { - if (pos->pdata->num == dev2) { - sync2 = pos; + if (pos->pdata->num == sync) { + slave = pos; break; } } rcu_read_unlock(); - if (sync1 != NULL) - sync1->s_info = NULL; - if (sync2 != NULL) - sync2->s_info = NULL; - if (!dev1 && !dev2) - return 0; /* no err if default instance 0's used */ - - if (dev1 == dev2) + if (master != NULL) + master->s_info = NULL; + if (slave != NULL) + slave->s_info = NULL; + if (!sync) + return 0; /* no err if sync disabled */ + + if (num == sync) return -EINVAL; /* err if sync instance is itself */ - if ((sync1 != NULL) && (sync2 != NULL)) { - sync1->s_info = sync2; - sync2->s_info = sync1; + if ((master != NULL) && (slave != NULL)) { + master->s_info = slave; + slave->s_info = master; } return 0; } @@ -1529,8 +1760,8 @@ static int sh532u_open(struct inode *inode, struct file *file) err = sh532u_sync_en(info->pdata->num, info->pdata->sync); if (err == -EINVAL) dev_err(&info->i2c_client->dev, - "%s err: invalid num (%u) and sync (%u) instance\n", - __func__, info->pdata->num, info->pdata->sync); + "%s err: invalid num (%u) and sync (%u) instance\n", + __func__, info->pdata->num, info->pdata->sync); if (atomic_xchg(&info->in_use, 1)) return -EBUSY; @@ -1545,7 +1776,7 @@ static int sh532u_open(struct inode *inode, struct file *file) return 0; } -int sh532u_release(struct inode *inode, struct file *file) +static int sh532u_release(struct inode *inode, struct file *file) { struct sh532u_info *info = file->private_data; @@ -1569,6 +1800,9 @@ static const struct file_operations sh532u_fileops = { static void sh532u_del(struct sh532u_info *info) { sh532u_pm_exit(info); + if ((info->s_mode == NVC_SYNC_SLAVE) || + (info->s_mode == NVC_SYNC_STEREO)) + sh532u_pm_exit(info->s_info); sh532u_sync_dis(info); spin_lock(&sh532u_spinlock); list_del_rcu(&info->list); @@ -1590,7 +1824,7 @@ static int sh532u_probe( struct i2c_client *client, const struct i2c_device_id *id) { - struct sh532u_info *info = NULL; + struct sh532u_info *info; char dname[16]; int err; @@ -1607,8 +1841,7 @@ static int sh532u_probe( } else { info->pdata = &sh532u_default_pdata; dev_dbg(&client->dev, - "%s No platform data. Using defaults.\n", - __func__); + "%s No platform data. Using defaults.\n", __func__); } i2c_set_clientdata(client, info); INIT_LIST_HEAD(&info->list); @@ -1616,24 +1849,27 @@ static int sh532u_probe( list_add_rcu(&info->list, &sh532u_info_list); spin_unlock(&sh532u_spinlock); sh532u_pm_init(info); - sh532u_pm_dev_wr(info, NVC_PWR_COMM); - err = sh532u_dev_id(info); - if (err < 0) { - dev_err(&client->dev, "%s device not found\n", __func__); - sh532u_pm_wr(info, NVC_PWR_OFF); - if (info->pdata->cfg & NVC_CFG_NODEV) { - sh532u_del(info); - return -ENODEV; - } - } else { - dev_dbg(&client->dev, "%s device found\n", __func__); - sh532u_calibration(info, false); - if (info->pdata->cfg & NVC_CFG_BOOT_INIT) { - /* initial move causes full initialization */ - sh532u_pos_rel_wr(info, info->cap.focus_infinity); + sh532u_sdata_init(info); + if (info->pdata->cfg & (NVC_CFG_NODEV | NVC_CFG_BOOT_INIT)) { + err = sh532u_dev_id(info); + if (err < 0) { + if (info->pdata->cfg & NVC_CFG_NODEV) { + sh532u_del(info); + return -ENODEV; + } else { + dev_err(&client->dev, "%s dev %x not found\n", + __func__, SH532U_ID); + } } else { - sh532u_pm_wr(info, NVC_PWR_OFF); + dev_dbg(&client->dev, "%s device found\n", __func__); + sh532u_pm_dev_wr(info, NVC_PWR_ON); + sh532u_calibration(info, false); + if (info->pdata->cfg & NVC_CFG_BOOT_INIT) + /* initial move causes full initialization */ + sh532u_pos_rel_wr(info, + info->cap.focus_infinity); } + sh532u_pm_dev_wr(info, NVC_PWR_OFF); } if (info->pdata->dev_name != 0) @@ -1648,7 +1884,7 @@ static int sh532u_probe( info->miscdev.minor = MISC_DYNAMIC_MINOR; if (misc_register(&info->miscdev)) { dev_err(&client->dev, "%s unable to register misc device %s\n", - __func__, dname); + __func__, dname); sh532u_del(info); return -ENODEV; } -- cgit v1.2.3