From 9b1cd7fac64f53d070154fd6440ace60f6cfeab0 Mon Sep 17 00:00:00 2001 From: Markus Proeller Date: Thu, 10 Oct 2019 19:12:36 +0200 Subject: [PATCH] media: i2c: Add a driver for the Infineon IRS1125 depth sensor The Infineon IRS1125 is a time of flight depth sensor that has a CSI-2 interface. Add a V4L2 subdevice driver for this device. Signed-off-by: Markus Proeller --- drivers/media/i2c/Kconfig | 12 + drivers/media/i2c/Makefile | 1 + drivers/media/i2c/irs1125.c | 1112 +++++++++++++++++++++++++++++++++++ drivers/media/i2c/irs1125.h | 61 ++ 4 files changed, 1186 insertions(+) create mode 100644 drivers/media/i2c/irs1125.c create mode 100644 drivers/media/i2c/irs1125.h --- a/drivers/media/i2c/Kconfig +++ b/drivers/media/i2c/Kconfig @@ -1366,6 +1366,18 @@ config VIDEO_TW9910 To compile this driver as a module, choose M here: the module will be called tw9910. +config VIDEO_IRS1125 + tristate "Infineon IRS1125 sensor support" + depends on I2C && VIDEO_DEV && VIDEO_V4L2_SUBDEV_API + depends on MEDIA_CAMERA_SUPPORT + select V4L2_FWNODE + help + This is a Video4Linux2 sensor-level driver for the Infineon + IRS1125 camera. + + To compile this driver as a module, choose M here: the + module will be called irs1125. + config VIDEO_VPX3220 tristate "vpx3220a, vpx3216b & vpx3214c video decoders" depends on VIDEO_DEV && I2C --- a/drivers/media/i2c/Makefile +++ b/drivers/media/i2c/Makefile @@ -51,6 +51,7 @@ obj-$(CONFIG_VIDEO_IMX412) += imx412.o obj-$(CONFIG_VIDEO_IMX477) += imx477.o obj-$(CONFIG_VIDEO_IMX519) += imx519.o obj-$(CONFIG_VIDEO_IR_I2C) += ir-kbd-i2c.o +obj-$(CONFIG_VIDEO_IRS1125) += irs1125.o obj-$(CONFIG_VIDEO_ISL7998X) += isl7998x.o obj-$(CONFIG_VIDEO_KS0127) += ks0127.o obj-$(CONFIG_VIDEO_LM3560) += lm3560.o --- /dev/null +++ b/drivers/media/i2c/irs1125.c @@ -0,0 +1,1112 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * A V4L2 driver for Infineon IRS1125 TOF cameras. + * Copyright (C) 2018, pieye GmbH + * + * Based on V4L2 OmniVision OV5647 Image Sensor driver + * Copyright (C) 2016 Ramiro Oliveira + * + * DT / fwnode changes, and GPIO control taken from ov5640.c + * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved. + * Copyright (C) 2014-2017 Mentor Graphics Inc. + * + */ + +#include "irs1125.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define CHECK_BIT(val, pos) ((val) & BIT(pos)) + +#define SENSOR_NAME "irs1125" + +#define RESET_ACTIVE_DELAY_MS 20 + +#define IRS1125_ALTERNATE_FW "irs1125_af.bin" + +#define IRS1125_REG_CSICFG 0xA882 +#define IRS1125_REG_DESIGN_STEP 0xB0AD +#define IRS1125_REG_EFUSEVAL2 0xB09F +#define IRS1125_REG_EFUSEVAL3 0xB0A0 +#define IRS1125_REG_EFUSEVAL4 0xB0A1 +#define IRS1125_REG_DMEM_SHADOW 0xC320 + +#define IRS1125_DESIGN_STEP_EXPECTED 0x0a12 + +#define IRS1125_ROW_START_DEF 0 +#define IRS1125_COLUMN_START_DEF 0 +#define IRS1125_WINDOW_HEIGHT_DEF 288 +#define IRS1125_WINDOW_WIDTH_DEF 352 + +struct regval_list { + u16 addr; + u16 data; +}; + +struct irs1125 { + struct v4l2_subdev sd; + struct media_pad pad; + /* the parsed DT endpoint info */ + struct v4l2_fwnode_endpoint ep; + + struct clk *xclk; + struct v4l2_ctrl_handler ctrl_handler; + + /* To serialize asynchronus callbacks */ + struct mutex lock; + + /* image data layout */ + unsigned int num_seq; + + /* reset pin */ + struct gpio_desc *reset; + + /* V4l2 Controls to grab */ + struct v4l2_ctrl *ctrl_modplls; + struct v4l2_ctrl *ctrl_numseq; + + int power_count; +}; + +static inline struct irs1125 *to_state(struct v4l2_subdev *sd) +{ + return container_of(sd, struct irs1125, sd); +} + +static struct regval_list irs1125_26MHz[] = { + {0xB017, 0x0413}, + {0xB086, 0x3535}, + {0xB0AE, 0xEF02}, + {0xA000, 0x0004}, + {0xFFFF, 100}, + + {0xB062, 0x6383}, + {0xB063, 0x55A8}, + {0xB068, 0x7628}, + {0xB069, 0x03E2}, + + {0xFFFF, 100}, + {0xB05A, 0x01C5}, + {0xB05C, 0x0206}, + {0xB05D, 0x01C5}, + {0xB05F, 0x0206}, + {0xB016, 0x1335}, + {0xFFFF, 100}, + {0xA893, 0x8261}, + {0xA894, 0x89d8}, + {0xA895, 0x131d}, + {0xA896, 0x4251}, + {0xA897, 0x9D8A}, + {0xA898, 0x0BD8}, + {0xA899, 0x2245}, + {0xA89A, 0xAB9B}, + {0xA89B, 0x03B9}, + {0xA89C, 0x8041}, + {0xA89D, 0xE07E}, + {0xA89E, 0x0307}, + {0xFFFF, 100}, + {0xA88D, 0x0004}, + {0xA800, 0x0E68}, + {0xA801, 0x0000}, + {0xA802, 0x000C}, + {0xA803, 0x0000}, + {0xA804, 0x0E68}, + {0xA805, 0x0000}, + {0xA806, 0x0440}, + {0xA807, 0x0000}, + {0xA808, 0x0E68}, + {0xA809, 0x0000}, + {0xA80A, 0x0884}, + {0xA80B, 0x0000}, + {0xA80C, 0x0E68}, + {0xA80D, 0x0000}, + {0xA80E, 0x0CC8}, + {0xA80F, 0x0000}, + {0xA810, 0x0E68}, + {0xA811, 0x0000}, + {0xA812, 0x2000}, + {0xA813, 0x0000}, + {0xA882, 0x0081}, + {0xA88C, 0x403A}, + {0xA88F, 0x031E}, + {0xA892, 0x0351}, + {0x9813, 0x13FF}, + {0x981B, 0x7608}, + + {0xB008, 0x0000}, + {0xB015, 0x1513}, + + {0xFFFF, 100} +}; + +static struct regval_list irs1125_seq_cfg[] = { + {0xC3A0, 0x823D}, + {0xC3A1, 0xB13B}, + {0xC3A2, 0x0313}, + {0xC3A3, 0x4659}, + {0xC3A4, 0xC4EC}, + {0xC3A5, 0x03CE}, + {0xC3A6, 0x4259}, + {0xC3A7, 0xC4EC}, + {0xC3A8, 0x03CE}, + {0xC3A9, 0x8839}, + {0xC3AA, 0x89D8}, + {0xC3AB, 0x031D}, + + {0xC24C, 0x5529}, + {0xC24D, 0x0000}, + {0xC24E, 0x1200}, + {0xC24F, 0x6CB2}, + {0xC250, 0x0000}, + {0xC251, 0x5529}, + {0xC252, 0x42F4}, + {0xC253, 0xD1AF}, + {0xC254, 0x8A18}, + {0xC255, 0x0002}, + {0xC256, 0x5529}, + {0xC257, 0x6276}, + {0xC258, 0x11A7}, + {0xC259, 0xD907}, + {0xC25A, 0x0000}, + {0xC25B, 0x5529}, + {0xC25C, 0x07E0}, + {0xC25D, 0x7BFE}, + {0xC25E, 0x6402}, + {0xC25F, 0x0019}, + + {0xC3AC, 0x0007}, + {0xC3AD, 0xED88}, + {0xC320, 0x003E}, + {0xC321, 0x0000}, + {0xC322, 0x2000}, + {0xC323, 0x0000}, + {0xC324, 0x0271}, + {0xC325, 0x0000}, + {0xC326, 0x000C}, + {0xC327, 0x0000}, + {0xC328, 0x0271}, + {0xC329, 0x0000}, + {0xC32A, 0x0440}, + {0xC32B, 0x0000}, + {0xC32C, 0x0271}, + {0xC32D, 0x0000}, + {0xC32E, 0x0884}, + {0xC32F, 0x0000}, + {0xC330, 0x0271}, + {0xC331, 0x0000}, + {0xC332, 0x0CC8}, + {0xC333, 0x0000}, + {0xA88D, 0x0004}, + + {0xA890, 0x0000}, + {0xC219, 0x0002}, + {0xC21A, 0x0000}, + {0xC21B, 0x0000}, + {0xC21C, 0x00CD}, + {0xC21D, 0x0009}, + {0xC21E, 0x00CD}, + {0xC21F, 0x0009}, + + {0xA87C, 0x0000}, + {0xC032, 0x0001}, + {0xC034, 0x0000}, + {0xC035, 0x0001}, + {0xC039, 0x0000}, + {0xC401, 0x0002}, + + {0xFFFF, 1}, + {0xA87C, 0x0001} +}; + +static int irs1125_write(struct v4l2_subdev *sd, u16 reg, u16 val) +{ + int ret; + unsigned char data[4] = { reg >> 8, reg & 0xff, val >> 8, val & 0xff}; + struct i2c_client *client = v4l2_get_subdevdata(sd); + + ret = i2c_master_send(client, data, 4); + if (ret < 0) + dev_err(&client->dev, "%s: i2c write error, reg: %x\n", + __func__, reg); + + return ret; +} + +static int irs1125_read(struct v4l2_subdev *sd, u16 reg, u16 *val) +{ + int ret; + unsigned char data_w[2] = { reg >> 8, reg & 0xff }; + char rdval[2]; + + struct i2c_client *client = v4l2_get_subdevdata(sd); + + ret = i2c_master_send(client, data_w, 2); + if (ret < 0) { + dev_dbg(&client->dev, "%s: i2c write error, reg: %x\n", + __func__, reg); + return ret; + } + + ret = i2c_master_recv(client, rdval, 2); + if (ret < 0) + dev_err(&client->dev, "%s: i2c read error, reg: %x\n", + __func__, reg); + + *val = rdval[1] | (rdval[0] << 8); + + return ret; +} + +static int irs1125_write_array(struct v4l2_subdev *sd, + struct regval_list *regs, int array_size) +{ + int i, ret; + + for (i = 0; i < array_size; i++) { + if (regs[i].addr == 0xFFFF) { + msleep(regs[i].data); + } else { + ret = irs1125_write(sd, regs[i].addr, regs[i].data); + if (ret < 0) + return ret; + } + } + + return 0; +} + +static int irs1125_stream_on(struct v4l2_subdev *sd) +{ + int ret; + struct irs1125 *irs1125 = to_state(sd); + struct i2c_client *client = v4l2_get_subdevdata(sd); + + v4l2_ctrl_grab(irs1125->ctrl_numseq, 1); + v4l2_ctrl_grab(irs1125->ctrl_modplls, 1); + + ret = irs1125_write(sd, 0xC400, 0x0001); + if (ret < 0) { + dev_err(&client->dev, "error enabling firmware: %d", ret); + return ret; + } + + msleep(100); + + return irs1125_write(sd, 0xA87C, 0x0001); +} + +static int irs1125_stream_off(struct v4l2_subdev *sd) +{ + int ret; + struct irs1125 *irs1125 = to_state(sd); + struct i2c_client *client = v4l2_get_subdevdata(sd); + + v4l2_ctrl_grab(irs1125->ctrl_numseq, 0); + v4l2_ctrl_grab(irs1125->ctrl_modplls, 0); + + ret = irs1125_write(sd, 0xA87C, 0x0000); + if (ret < 0) { + dev_err(&client->dev, "error disabling trigger: %d", ret); + return ret; + } + + msleep(100); + + return irs1125_write(sd, 0xC400, 0x0002); +} + +static int __sensor_init(struct v4l2_subdev *sd) +{ + unsigned int cnt, idx; + int ret; + u16 val; + struct i2c_client *client = v4l2_get_subdevdata(sd); + struct irs1125 *irs1125 = to_state(sd); + const struct firmware *fw; + struct regval_list *reg_data; + + cnt = 0; + while (1) { + ret = irs1125_read(sd, 0xC40F, &val); + if (ret < 0) { + dev_err(&client->dev, "read register 0xC40F failed\n"); + return ret; + } + if (CHECK_BIT(val, 14) == 0) + break; + + if (cnt >= 5) { + dev_err(&client->dev, "timeout waiting for 0xC40F\n"); + return -EAGAIN; + } + + cnt++; + } + + ret = irs1125_write_array(sd, irs1125_26MHz, + ARRAY_SIZE(irs1125_26MHz)); + if (ret < 0) { + dev_err(&client->dev, "write sensor default regs error\n"); + return ret; + } + + /* set CSI-2 number of data lanes */ + if (irs1125->ep.bus.mipi_csi2.num_data_lanes == 1) { + val = 0x0001; + } else if (irs1125->ep.bus.mipi_csi2.num_data_lanes == 2) { + val = 0x0081; + } else { + dev_err(&client->dev, "invalid number of data lanes %d\n", + irs1125->ep.bus.mipi_csi2.num_data_lanes); + return -EINVAL; + } + + ret = irs1125_write(sd, IRS1125_REG_CSICFG, val); + if (ret < 0) { + dev_err(&client->dev, "write sensor csi2 config error\n"); + return ret; + } + + /* request the firmware, this will block and timeout */ + ret = request_firmware(&fw, IRS1125_ALTERNATE_FW, &client->dev); + if (ret) { + dev_err(&client->dev, + "did not find the firmware file '%s' (status %d)\n", + IRS1125_ALTERNATE_FW, ret); + return ret; + } + + if (fw->size % 4) { + dev_err(&client->dev, "firmware file '%s' invalid\n", + IRS1125_ALTERNATE_FW); + release_firmware(fw); + return -EINVAL; + } + + for (idx = 0; idx < fw->size; idx += 4) { + reg_data = (struct regval_list *)&fw->data[idx]; + ret = irs1125_write(sd, reg_data->addr, reg_data->data); + if (ret < 0) { + dev_err(&client->dev, "firmware write error\n"); + release_firmware(fw); + return ret; + } + } + release_firmware(fw); + + ret = irs1125_write_array(sd, irs1125_seq_cfg, + ARRAY_SIZE(irs1125_seq_cfg)); + if (ret < 0) { + dev_err(&client->dev, "write default sequence failed\n"); + return ret; + } + + return 0; +} + +static int irs1125_sensor_power(struct v4l2_subdev *sd, int on) +{ + int ret = 0; + struct irs1125 *irs1125 = to_state(sd); + struct i2c_client *client = v4l2_get_subdevdata(sd); + + mutex_lock(&irs1125->lock); + + if (on && !irs1125->power_count) { + gpiod_set_value_cansleep(irs1125->reset, 1); + msleep(RESET_ACTIVE_DELAY_MS); + + ret = clk_prepare_enable(irs1125->xclk); + if (ret < 0) { + dev_err(&client->dev, "clk prepare enable failed\n"); + goto out; + } + + ret = __sensor_init(sd); + if (ret < 0) { + clk_disable_unprepare(irs1125->xclk); + dev_err(&client->dev, + "Camera not available, check Power\n"); + goto out; + } + } else if (!on && irs1125->power_count == 1) { + gpiod_set_value_cansleep(irs1125->reset, 0); + } + + /* Update the power count. */ + irs1125->power_count += on ? 1 : -1; + WARN_ON(irs1125->power_count < 0); + +out: + mutex_unlock(&irs1125->lock); + + return ret; +} + +#ifdef CONFIG_VIDEO_ADV_DEBUG +static int irs1125_sensor_get_register(struct v4l2_subdev *sd, + struct v4l2_dbg_register *reg) +{ + u16 val; + int ret; + + ret = irs1125_read(sd, reg->reg & 0xffff, &val); + if (ret < 0) + return ret; + + reg->val = val; + reg->size = 1; + + return 0; +} + +static int irs1125_sensor_set_register(struct v4l2_subdev *sd, + const struct v4l2_dbg_register *reg) +{ + return irs1125_write(sd, reg->reg & 0xffff, reg->val & 0xffff); +} +#endif + +static const struct v4l2_subdev_core_ops irs1125_subdev_core_ops = { + .s_power = irs1125_sensor_power, +#ifdef CONFIG_VIDEO_ADV_DEBUG + .g_register = irs1125_sensor_get_register, + .s_register = irs1125_sensor_set_register, +#endif +}; + +static int irs1125_s_stream(struct v4l2_subdev *sd, int enable) +{ + if (enable) + return irs1125_stream_on(sd); + else + return irs1125_stream_off(sd); +} + +static const struct v4l2_subdev_video_ops irs1125_subdev_video_ops = { + .s_stream = irs1125_s_stream, +}; + +static int irs1125_enum_mbus_code(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_mbus_code_enum *code) +{ + if (code->index > 0) + return -EINVAL; + + code->code = MEDIA_BUS_FMT_Y12_1X12; + + return 0; +} + +static int irs1125_set_get_fmt(struct v4l2_subdev *sd, + struct v4l2_subdev_pad_config *cfg, + struct v4l2_subdev_format *format) +{ + struct v4l2_mbus_framefmt *fmt = &format->format; + struct irs1125 *irs1125 = to_state(sd); + + if (format->pad != 0) + return -EINVAL; + + /* Only one format is supported, so return that */ + memset(fmt, 0, sizeof(*fmt)); + fmt->code = MEDIA_BUS_FMT_Y12_1X12; + fmt->colorspace = V4L2_COLORSPACE_RAW; + fmt->field = V4L2_FIELD_NONE; + fmt->width = IRS1125_WINDOW_WIDTH_DEF; + fmt->height = IRS1125_WINDOW_HEIGHT_DEF * irs1125->num_seq; + + return 0; +} + +static const struct v4l2_subdev_pad_ops irs1125_subdev_pad_ops = { + .enum_mbus_code = irs1125_enum_mbus_code, + .set_fmt = irs1125_set_get_fmt, + .get_fmt = irs1125_set_get_fmt, +}; + +static const struct v4l2_subdev_ops irs1125_subdev_ops = { + .core = &irs1125_subdev_core_ops, + .video = &irs1125_subdev_video_ops, + .pad = &irs1125_subdev_pad_ops, +}; + +static int irs1125_s_ctrl(struct v4l2_ctrl *ctrl) +{ + struct irs1125 *dev = container_of(ctrl->handler, + struct irs1125, ctrl_handler); + struct i2c_client *client = v4l2_get_subdevdata(&dev->sd); + int err, i; + struct irs1125_mod_pll *mod_cur, *mod_new; + struct irs1125_seq_cfg *cfg_cur, *cfg_new; + u16 addr, val; + + err = 0; + + switch (ctrl->id) { + case IRS1125_CID_SAFE_RECONFIG: + { + struct irs1125_illu *illu_cur, *illu_new; + + illu_new = (struct irs1125_illu *)ctrl->p_new.p; + illu_cur = (struct irs1125_illu *)ctrl->p_cur.p; + for (i = 0; i < IRS1125_NUM_SEQ_ENTRIES; i++) { + if (illu_cur[i].exposure != illu_new[i].exposure) { + addr = 0xA850 + i * 2; + val = illu_new[i].exposure; + err = irs1125_write(&dev->sd, addr, val); + if (err < 0) + break; + } + if (illu_cur[i].framerate != illu_new[i].framerate) { + addr = 0xA851 + i * 2; + val = illu_new[i].framerate; + err = irs1125_write(&dev->sd, addr, val); + if (err < 0) + break; + } + } + break; + } + case IRS1125_CID_MOD_PLL: + mod_new = (struct irs1125_mod_pll *)ctrl->p_new.p; + mod_cur = (struct irs1125_mod_pll *)ctrl->p_cur.p; + for (i = 0; i < IRS1125_NUM_MOD_PLLS; i++) { + if (mod_cur[i].pllcfg1 != mod_new[i].pllcfg1) { + addr = 0xC3A0 + i * 3; + val = mod_new[i].pllcfg1; + err = irs1125_write(&dev->sd, addr, val); + if (err < 0) + break; + } + if (mod_cur[i].pllcfg2 != mod_new[i].pllcfg2) { + addr = 0xC3A1 + i * 3; + val = mod_new[i].pllcfg2; + err = irs1125_write(&dev->sd, addr, val); + if (err < 0) + break; + } + if (mod_cur[i].pllcfg3 != mod_new[i].pllcfg3) { + addr = 0xC3A2 + i * 3; + val = mod_new[i].pllcfg3; + err = irs1125_write(&dev->sd, addr, val); + if (err < 0) + break; + } + if (mod_cur[i].pllcfg4 != mod_new[i].pllcfg4) { + addr = 0xC24C + i * 5; + val = mod_new[i].pllcfg4; + err = irs1125_write(&dev->sd, addr, val); + if (err < 0) + break; + } + if (mod_cur[i].pllcfg5 != mod_new[i].pllcfg5) { + addr = 0xC24D + i * 5; + val = mod_new[i].pllcfg5; + err = irs1125_write(&dev->sd, addr, val); + if (err < 0) + break; + } + if (mod_cur[i].pllcfg6 != mod_new[i].pllcfg6) { + addr = 0xC24E + i * 5; + val = mod_new[i].pllcfg6; + err = irs1125_write(&dev->sd, addr, val); + if (err < 0) + break; + } + if (mod_cur[i].pllcfg7 != mod_new[i].pllcfg7) { + addr = 0xC24F + i * 5; + val = mod_new[i].pllcfg7; + err = irs1125_write(&dev->sd, addr, val); + if (err < 0) + break; + } + if (mod_cur[i].pllcfg8 != mod_new[i].pllcfg8) { + addr = 0xC250 + i * 5; + val = mod_new[i].pllcfg8; + err = irs1125_write(&dev->sd, addr, val); + if (err < 0) + break; + } + } + break; + case IRS1125_CID_SEQ_CONFIG: + cfg_new = (struct irs1125_seq_cfg *)ctrl->p_new.p; + cfg_cur = (struct irs1125_seq_cfg *)ctrl->p_cur.p; + for (i = 0; i < IRS1125_NUM_SEQ_ENTRIES; i++) { + if (cfg_cur[i].exposure != cfg_new[i].exposure) { + addr = IRS1125_REG_DMEM_SHADOW + i * 4; + val = cfg_new[i].exposure; + err = irs1125_write(&dev->sd, addr, val); + if (err < 0) + break; + } + if (cfg_cur[i].framerate != cfg_new[i].framerate) { + addr = IRS1125_REG_DMEM_SHADOW + 1 + i * 4; + val = cfg_new[i].framerate; + err = irs1125_write(&dev->sd, addr, val); + if (err < 0) + break; + } + if (cfg_cur[i].ps != cfg_new[i].ps) { + addr = IRS1125_REG_DMEM_SHADOW + 2 + i * 4; + val = cfg_new[i].ps; + err = irs1125_write(&dev->sd, addr, val); + if (err < 0) + break; + } + if (cfg_cur[i].pll != cfg_new[i].pll) { + addr = IRS1125_REG_DMEM_SHADOW + 3 + i * 4; + val = cfg_new[i].pll; + err = irs1125_write(&dev->sd, addr, val); + if (err < 0) + break; + } + } + break; + case IRS1125_CID_NUM_SEQS: + err = irs1125_write(&dev->sd, 0xA88D, ctrl->val - 1); + if (err >= 0) + dev->num_seq = ctrl->val; + break; + case IRS1125_CID_CONTINUOUS_TRIG: + if (ctrl->val == 0) + err = irs1125_write(&dev->sd, 0xA87C, 0); + else + err = irs1125_write(&dev->sd, 0xA87C, 1); + break; + case IRS1125_CID_TRIGGER: + if (ctrl->val != 0) { + err = irs1125_write(&dev->sd, 0xA87C, 1); + if (err >= 0) + err = irs1125_write(&dev->sd, 0xA87C, 0); + } + break; + case IRS1125_CID_RECONFIG: + if (ctrl->val != 0) + err = irs1125_write(&dev->sd, 0xA87A, 1); + break; + case IRS1125_CID_ILLU_ON: + if (ctrl->val == 0) + err = irs1125_write(&dev->sd, 0xA892, 0x377); + else + err = irs1125_write(&dev->sd, 0xA892, 0x355); + break; + default: + break; + } + + if (err < 0) + dev_err(&client->dev, "Error executing control ID: %d, val %d, err %d", + ctrl->id, ctrl->val, err); + else + err = 0; + + return err; +} + +static const struct v4l2_ctrl_ops irs1125_ctrl_ops = { + .s_ctrl = irs1125_s_ctrl, +}; + +static const struct v4l2_ctrl_config irs1125_custom_ctrls[] = { + { + .ops = &irs1125_ctrl_ops, + .id = IRS1125_CID_NUM_SEQS, + .name = "Change number of sequences", + .type = V4L2_CTRL_TYPE_INTEGER, + .flags = V4L2_CTRL_FLAG_MODIFY_LAYOUT, + .min = 1, + .max = 20, + .step = 1, + .def = 5, + }, { + .ops = &irs1125_ctrl_ops, + .id = IRS1125_CID_MOD_PLL, + .name = "Reconfigure modulation PLLs", + .type = V4L2_CTRL_TYPE_U16, + .flags = V4L2_CTRL_FLAG_HAS_PAYLOAD, + .min = 0, + .max = U16_MAX, + .step = 1, + .def = 0, + .elem_size = sizeof(u16), + .dims = {sizeof(struct irs1125_mod_pll) / sizeof(u16), + IRS1125_NUM_MOD_PLLS} + }, { + .ops = &irs1125_ctrl_ops, + .id = IRS1125_CID_SAFE_RECONFIG, + .name = "Change exposure and pause of single seq", + .type = V4L2_CTRL_TYPE_U16, + .flags = V4L2_CTRL_FLAG_HAS_PAYLOAD, + .min = 0, + .max = U16_MAX, + .step = 1, + .def = 0, + .elem_size = sizeof(u16), + .dims = {sizeof(struct irs1125_illu) / sizeof(u16), + IRS1125_NUM_SEQ_ENTRIES} + }, { + .ops = &irs1125_ctrl_ops, + .id = IRS1125_CID_SEQ_CONFIG, + .name = "Change sequence settings", + .type = V4L2_CTRL_TYPE_U16, + .flags = V4L2_CTRL_FLAG_HAS_PAYLOAD, + .min = 0, + .max = U16_MAX, + .step = 1, + .def = 0, + .elem_size = sizeof(u16), + .dims = {sizeof(struct irs1125_seq_cfg) / sizeof(u16), + IRS1125_NUM_SEQ_ENTRIES} + }, { + .ops = &irs1125_ctrl_ops, + .id = IRS1125_CID_CONTINUOUS_TRIG, + .name = "Enable/disable continuous trigger", + .type = V4L2_CTRL_TYPE_BOOLEAN, + .flags = V4L2_CTRL_FLAG_EXECUTE_ON_WRITE, + .min = 0, + .max = 1, + .step = 1, + .def = 0 + }, { + .ops = &irs1125_ctrl_ops, + .id = IRS1125_CID_TRIGGER, + .name = "Capture a single sequence", + .type = V4L2_CTRL_TYPE_BOOLEAN, + .flags = V4L2_CTRL_FLAG_EXECUTE_ON_WRITE, + .min = 0, + .max = 1, + .step = 1, + .def = 0 + }, { + .ops = &irs1125_ctrl_ops, + .id = IRS1125_CID_RECONFIG, + .name = "Trigger imager reconfiguration", + .type = V4L2_CTRL_TYPE_BOOLEAN, + .flags = V4L2_CTRL_FLAG_EXECUTE_ON_WRITE, + .min = 0, + .max = 1, + .step = 1, + .def = 0 + }, { + .ops = &irs1125_ctrl_ops, + .id = IRS1125_CID_ILLU_ON, + .name = "Turn illu on or off", + .type = V4L2_CTRL_TYPE_BOOLEAN, + .flags = V4L2_CTRL_FLAG_EXECUTE_ON_WRITE, + .min = 0, + .max = 1, + .step = 1, + .def = 1 + }, { + .ops = &irs1125_ctrl_ops, + .id = IRS1125_CID_IDENT0, + .name = "Get ident 0 information", + .type = V4L2_CTRL_TYPE_INTEGER, + .flags = V4L2_CTRL_FLAG_READ_ONLY, + .min = S32_MIN, + .max = S32_MAX, + .step = 1, + .def = 0 + }, { + .ops = &irs1125_ctrl_ops, + .id = IRS1125_CID_IDENT1, + .name = "Get ident 1 information", + .type = V4L2_CTRL_TYPE_INTEGER, + .flags = V4L2_CTRL_FLAG_READ_ONLY, + .min = S32_MIN, + .max = S32_MAX, + .step = 1, + .def = 0 + }, { + .ops = &irs1125_ctrl_ops, + .id = IRS1125_CID_IDENT2, + .name = "Get ident 2 information", + .type = V4L2_CTRL_TYPE_INTEGER, + .flags = V4L2_CTRL_FLAG_READ_ONLY, + .min = S32_MIN, + .max = S32_MAX, + .step = 1, + .def = 0 + } +}; + +static int irs1125_detect(struct v4l2_subdev *sd) +{ + u16 read; + int ret; + struct i2c_client *client = v4l2_get_subdevdata(sd); + + ret = irs1125_read(sd, IRS1125_REG_DESIGN_STEP, &read); + if (ret < 0) { + dev_err(&client->dev, "error reading from i2c\n"); + return ret; + } + + if (read != IRS1125_DESIGN_STEP_EXPECTED) { + dev_err(&client->dev, "Design step expected 0x%x got 0x%x", + IRS1125_DESIGN_STEP_EXPECTED, read); + return -ENODEV; + } + + return 0; +} + +static int irs1125_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh) +{ + struct v4l2_mbus_framefmt *format = + v4l2_subdev_get_try_format(sd, fh->pad, 0); + + format->code = MEDIA_BUS_FMT_Y12_1X12; + format->width = IRS1125_WINDOW_WIDTH_DEF; + format->height = IRS1125_WINDOW_HEIGHT_DEF; + format->field = V4L2_FIELD_NONE; + format->colorspace = V4L2_COLORSPACE_RAW; + + return 0; +} + +static const struct v4l2_subdev_internal_ops irs1125_subdev_internal_ops = { + .open = irs1125_open, +}; + +static int irs1125_ctrls_init(struct irs1125 *sensor, struct device *dev) +{ + struct v4l2_ctrl *ctrl; + int err, i; + struct v4l2_ctrl_handler *hdl; + + hdl = &sensor->ctrl_handler; + v4l2_ctrl_handler_init(hdl, ARRAY_SIZE(irs1125_custom_ctrls)); + + for (i = 0; i < ARRAY_SIZE(irs1125_custom_ctrls); i++) { + ctrl = v4l2_ctrl_new_custom(hdl, &irs1125_custom_ctrls[i], + NULL); + if (!ctrl) + dev_err(dev, "Failed to init custom control %s\n", + irs1125_custom_ctrls[i].name); + else if (irs1125_custom_ctrls[i].id == IRS1125_CID_NUM_SEQS) + sensor->ctrl_numseq = ctrl; + else if (irs1125_custom_ctrls[i].id == IRS1125_CID_MOD_PLL) + sensor->ctrl_modplls = ctrl; + } + + if (hdl->error) { + dev_err(dev, "Error %d adding controls\n", hdl->error); + err = hdl->error; + goto error_ctrls; + } + + sensor->sd.ctrl_handler = hdl; + return 0; + +error_ctrls: + v4l2_ctrl_handler_free(&sensor->ctrl_handler); + return -err; +} + +static int irs1125_ident_setup(struct irs1125 *sensor, struct device *dev) +{ + int ret; + struct v4l2_ctrl *ctrl; + struct v4l2_subdev *sd; + u16 read; + + sd = &sensor->sd; + + ctrl = v4l2_ctrl_find(&sensor->ctrl_handler, IRS1125_CID_IDENT0); + if (!ctrl) { + dev_err(dev, "could not find device ctrl.\n"); + return -EINVAL; + } + + ret = irs1125_read(sd, IRS1125_REG_EFUSEVAL2, &read); + if (ret < 0) { + dev_err(dev, "error reading from i2c\n"); + return -EIO; + } + + v4l2_ctrl_s_ctrl(ctrl, read); + + ctrl = v4l2_ctrl_find(&sensor->ctrl_handler, IRS1125_CID_IDENT1); + if (!ctrl) { + dev_err(dev, "could not find device ctrl.\n"); + return -EINVAL; + } + + ret = irs1125_read(sd, IRS1125_REG_EFUSEVAL3, &read); + if (ret < 0) { + dev_err(dev, "error reading from i2c\n"); + return -EIO; + } + + v4l2_ctrl_s_ctrl(ctrl, read); + + ctrl = v4l2_ctrl_find(&sensor->ctrl_handler, IRS1125_CID_IDENT2); + if (!ctrl) { + dev_err(dev, "could not find device ctrl.\n"); + return -EINVAL; + } + + ret = irs1125_read(sd, IRS1125_REG_EFUSEVAL4, &read); + if (ret < 0) { + dev_err(dev, "error reading from i2c\n"); + return -EIO; + } + v4l2_ctrl_s_ctrl(ctrl, read & 0xFFFC); + + return 0; +} + +static int irs1125_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + struct device *dev = &client->dev; + struct irs1125 *sensor; + int ret; + struct fwnode_handle *endpoint; + u32 xclk_freq; + int gpio_num; + + sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL); + if (!sensor) + return -ENOMEM; + + v4l2_i2c_subdev_init(&sensor->sd, client, &irs1125_subdev_ops); + + /* Get CSI2 bus config */ + endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(dev), + NULL); + if (!endpoint) { + dev_err(dev, "endpoint node not found\n"); + return -EINVAL; + } + + ret = v4l2_fwnode_endpoint_parse(endpoint, &sensor->ep); + fwnode_handle_put(endpoint); + if (ret) { + dev_err(dev, "Could not parse endpoint\n"); + return ret; + } + + /* get system clock (xclk) */ + sensor->xclk = devm_clk_get(dev, NULL); + if (IS_ERR(sensor->xclk)) { + dev_err(dev, "could not get xclk"); + return PTR_ERR(sensor->xclk); + } + + xclk_freq = clk_get_rate(sensor->xclk); + if (xclk_freq != 26000000) { + dev_err(dev, "Unsupported clock frequency: %u\n", xclk_freq); + return -EINVAL; + } + + sensor->num_seq = 5; + + /* Request the power down GPIO */ + sensor->reset = devm_gpiod_get(&client->dev, "pwdn", + GPIOD_OUT_LOW); + + if (IS_ERR(sensor->reset)) { + dev_err(dev, "could not get reset"); + return PTR_ERR(sensor->reset); + } + + gpio_num = desc_to_gpio(sensor->reset); + + mutex_init(&sensor->lock); + + ret = irs1125_ctrls_init(sensor, dev); + if (ret < 0) + goto mutex_remove; + + sensor->sd.internal_ops = &irs1125_subdev_internal_ops; + sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; + sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR; + sensor->pad.flags = MEDIA_PAD_FL_SOURCE; + ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad); + if (ret < 0) + goto mutex_remove; + + gpiod_set_value_cansleep(sensor->reset, 1); + msleep(RESET_ACTIVE_DELAY_MS); + + ret = irs1125_detect(&sensor->sd); + if (ret < 0) + goto error; + + ret = irs1125_ident_setup(sensor, dev); + if (ret < 0) + goto error; + + gpiod_set_value_cansleep(sensor->reset, 0); + + ret = v4l2_async_register_subdev(&sensor->sd); + if (ret < 0) + goto error; + + dev_dbg(dev, "Infineon IRS1125 camera driver probed\n"); + + return 0; + +error: + media_entity_cleanup(&sensor->sd.entity); +mutex_remove: + mutex_destroy(&sensor->lock); + return ret; +} + +static int irs1125_remove(struct i2c_client *client) +{ + struct v4l2_subdev *sd = i2c_get_clientdata(client); + struct irs1125 *irs1125 = to_state(sd); + + v4l2_async_unregister_subdev(&irs1125->sd); + media_entity_cleanup(&irs1125->sd.entity); + v4l2_device_unregister_subdev(sd); + mutex_destroy(&irs1125->lock); + v4l2_ctrl_handler_free(&irs1125->ctrl_handler); + + return 0; +} + +#if IS_ENABLED(CONFIG_OF) +static const struct of_device_id irs1125_of_match[] = { + { .compatible = "infineon,irs1125" }, + { /* sentinel */ }, +}; +MODULE_DEVICE_TABLE(of, irs1125_of_match); +#endif + +static struct i2c_driver irs1125_driver = { + .driver = { + .of_match_table = of_match_ptr(irs1125_of_match), + .name = SENSOR_NAME, + }, + .probe = irs1125_probe, + .remove = irs1125_remove, +}; + +module_i2c_driver(irs1125_driver); + +MODULE_AUTHOR("Markus Proeller "); +MODULE_DESCRIPTION("Infineon irs1125 sensor driver"); +MODULE_LICENSE("GPL v2"); + --- /dev/null +++ b/drivers/media/i2c/irs1125.h @@ -0,0 +1,61 @@ +/* SPDX-License-Identifier: GPL-2.0 */ +/* + * A V4L2 driver for Infineon IRS1125 TOF cameras. + * Copyright (C) 2018, pieye GmbH + * + * Based on V4L2 OmniVision OV5647 Image Sensor driver + * Copyright (C) 2016 Ramiro Oliveira + * + * DT / fwnode changes, and GPIO control taken from ov5640.c + * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved. + * Copyright (C) 2014-2017 Mentor Graphics Inc. + * + */ + +#ifndef IRS1125_H +#define IRS1125_H + +#include +#include + +#define IRS1125_NUM_SEQ_ENTRIES 20 +#define IRS1125_NUM_MOD_PLLS 4 + +#define IRS1125_CID_CUSTOM_BASE (V4L2_CID_USER_BASE | 0xf000) +#define IRS1125_CID_SAFE_RECONFIG (IRS1125_CID_CUSTOM_BASE + 0) +#define IRS1125_CID_CONTINUOUS_TRIG (IRS1125_CID_CUSTOM_BASE + 1) +#define IRS1125_CID_TRIGGER (IRS1125_CID_CUSTOM_BASE + 2) +#define IRS1125_CID_RECONFIG (IRS1125_CID_CUSTOM_BASE + 3) +#define IRS1125_CID_ILLU_ON (IRS1125_CID_CUSTOM_BASE + 4) +#define IRS1125_CID_NUM_SEQS (IRS1125_CID_CUSTOM_BASE + 5) +#define IRS1125_CID_MOD_PLL (IRS1125_CID_CUSTOM_BASE + 6) +#define IRS1125_CID_SEQ_CONFIG (IRS1125_CID_CUSTOM_BASE + 7) +#define IRS1125_CID_IDENT0 (IRS1125_CID_CUSTOM_BASE + 8) +#define IRS1125_CID_IDENT1 (IRS1125_CID_CUSTOM_BASE + 9) +#define IRS1125_CID_IDENT2 (IRS1125_CID_CUSTOM_BASE + 10) + +struct irs1125_seq_cfg { + __u16 exposure; + __u16 framerate; + __u16 ps; + __u16 pll; +}; + +struct irs1125_illu { + __u16 exposure; + __u16 framerate; +}; + +struct irs1125_mod_pll { + __u16 pllcfg1; + __u16 pllcfg2; + __u16 pllcfg3; + __u16 pllcfg4; + __u16 pllcfg5; + __u16 pllcfg6; + __u16 pllcfg7; + __u16 pllcfg8; +}; + +#endif /* IRS1125 */ +