mirror of https://github.com/Qortal/Brooklyn
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1475 lines
35 KiB
1475 lines
35 KiB
// SPDX-License-Identifier: GPL-2.0 |
|
/* |
|
* A V4L2 driver for Arducam Pivariety Cameras |
|
* Copyright (C) 2022 Arducam Technology co., Ltd. |
|
* |
|
* Based on Sony IMX219 camera driver |
|
* Copyright (C) 2019, Raspberry Pi (Trading) Ltd |
|
* |
|
* I2C read and write method is taken from the OV9281 driver |
|
* Copyright (C) 2017 Fuzhou Rockchip Electronics Co., Ltd. |
|
*/ |
|
|
|
#include <linux/clk.h> |
|
#include <linux/delay.h> |
|
#include <linux/gpio/consumer.h> |
|
#include <linux/i2c.h> |
|
#include <linux/module.h> |
|
#include <linux/pm_runtime.h> |
|
#include <linux/regulator/consumer.h> |
|
#include <media/v4l2-ctrls.h> |
|
#include <media/v4l2-device.h> |
|
#include <media/v4l2-event.h> |
|
#include <media/v4l2-fwnode.h> |
|
#include "arducam-pivariety.h" |
|
|
|
static int debug; |
|
module_param(debug, int, 0644); |
|
|
|
/* regulator supplies */ |
|
static const char * const pivariety_supply_name[] = { |
|
/* Supplies can be enabled in any order */ |
|
"VANA", /* Analog (2.8V) supply */ |
|
"VDIG", /* Digital Core (1.8V) supply */ |
|
"VDDL", /* IF (1.2V) supply */ |
|
}; |
|
|
|
/* The supported raw formats. */ |
|
static const u32 codes[] = { |
|
MEDIA_BUS_FMT_SBGGR8_1X8, |
|
MEDIA_BUS_FMT_SGBRG8_1X8, |
|
MEDIA_BUS_FMT_SGRBG8_1X8, |
|
MEDIA_BUS_FMT_SRGGB8_1X8, |
|
MEDIA_BUS_FMT_Y8_1X8, |
|
|
|
MEDIA_BUS_FMT_SBGGR10_1X10, |
|
MEDIA_BUS_FMT_SGBRG10_1X10, |
|
MEDIA_BUS_FMT_SGRBG10_1X10, |
|
MEDIA_BUS_FMT_SRGGB10_1X10, |
|
MEDIA_BUS_FMT_Y10_1X10, |
|
|
|
MEDIA_BUS_FMT_SBGGR12_1X12, |
|
MEDIA_BUS_FMT_SGBRG12_1X12, |
|
MEDIA_BUS_FMT_SGRBG12_1X12, |
|
MEDIA_BUS_FMT_SRGGB12_1X12, |
|
MEDIA_BUS_FMT_Y12_1X12, |
|
}; |
|
|
|
#define ARDUCAM_NUM_SUPPLIES ARRAY_SIZE(pivariety_supply_name) |
|
|
|
#define ARDUCAM_XCLR_MIN_DELAY_US 10000 |
|
#define ARDUCAM_XCLR_DELAY_RANGE_US 1000 |
|
|
|
#define MAX_CTRLS 32 |
|
|
|
struct pivariety { |
|
struct v4l2_subdev sd; |
|
struct media_pad pad; |
|
|
|
struct v4l2_mbus_config_mipi_csi2 bus; |
|
struct clk *xclk; |
|
u32 xclk_freq; |
|
|
|
struct gpio_desc *reset_gpio; |
|
struct regulator_bulk_data supplies[ARDUCAM_NUM_SUPPLIES]; |
|
|
|
struct arducam_format *supported_formats; |
|
int num_supported_formats; |
|
int current_format_idx; |
|
int current_resolution_idx; |
|
int lanes; |
|
int bayer_order_volatile; |
|
bool wait_until_free; |
|
|
|
struct v4l2_ctrl_handler ctrl_handler; |
|
struct v4l2_ctrl *ctrls[MAX_CTRLS]; |
|
/* V4L2 Controls */ |
|
struct v4l2_ctrl *vflip; |
|
struct v4l2_ctrl *hflip; |
|
|
|
struct v4l2_rect crop; |
|
/* |
|
* Mutex for serialized access: |
|
* Protect sensor module set pad format and start/stop streaming safely. |
|
*/ |
|
struct mutex mutex; |
|
|
|
/* Streaming on/off */ |
|
bool streaming; |
|
}; |
|
|
|
static inline struct pivariety *to_pivariety(struct v4l2_subdev *_sd) |
|
{ |
|
return container_of(_sd, struct pivariety, sd); |
|
} |
|
|
|
/* Write registers up to 4 at a time */ |
|
static int pivariety_write_reg(struct i2c_client *client, u16 reg, u32 val) |
|
{ |
|
unsigned int len = sizeof(u32); |
|
u32 buf_i, val_i = 0; |
|
u8 buf[6]; |
|
u8 *val_p; |
|
__be32 val_be; |
|
|
|
buf[0] = reg >> 8; |
|
buf[1] = reg & 0xff; |
|
|
|
val_be = cpu_to_be32(val); |
|
val_p = (u8 *)&val_be; |
|
buf_i = 2; |
|
|
|
while (val_i < 4) |
|
buf[buf_i++] = val_p[val_i++]; |
|
|
|
if (i2c_master_send(client, buf, len + 2) != len + 2) |
|
return -EIO; |
|
|
|
return 0; |
|
} |
|
|
|
/* Read registers up to 4 at a time */ |
|
static int pivariety_read_reg(struct i2c_client *client, u16 reg, u32 *val) |
|
{ |
|
struct i2c_msg msgs[2]; |
|
unsigned int len = sizeof(u32); |
|
u8 *data_be_p; |
|
__be32 data_be = 0; |
|
__be16 reg_addr_be = cpu_to_be16(reg); |
|
int ret; |
|
|
|
data_be_p = (u8 *)&data_be; |
|
/* Write register address */ |
|
msgs[0].addr = client->addr; |
|
msgs[0].flags = 0; |
|
msgs[0].len = 2; |
|
msgs[0].buf = (u8 *)®_addr_be; |
|
|
|
/* Read data from register */ |
|
msgs[1].addr = client->addr; |
|
msgs[1].flags = I2C_M_RD; |
|
msgs[1].len = len; |
|
msgs[1].buf = data_be_p; |
|
|
|
ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs)); |
|
if (ret != ARRAY_SIZE(msgs)) |
|
return -EIO; |
|
|
|
*val = be32_to_cpu(data_be); |
|
|
|
return 0; |
|
} |
|
|
|
static int |
|
pivariety_read(struct pivariety *pivariety, u16 addr, u32 *value) |
|
{ |
|
struct v4l2_subdev *sd = &pivariety->sd; |
|
struct i2c_client *client = v4l2_get_subdevdata(sd); |
|
int ret, count = 0; |
|
|
|
while (count++ < I2C_READ_RETRY_COUNT) { |
|
ret = pivariety_read_reg(client, addr, value); |
|
if (!ret) { |
|
v4l2_dbg(2, debug, sd, "%s: 0x%02x 0x%04x\n", |
|
__func__, addr, *value); |
|
return ret; |
|
} |
|
} |
|
|
|
v4l2_err(sd, "%s: Reading register 0x%02x failed\n", |
|
__func__, addr); |
|
|
|
return ret; |
|
} |
|
|
|
static int pivariety_write(struct pivariety *pivariety, u16 addr, u32 value) |
|
{ |
|
struct v4l2_subdev *sd = &pivariety->sd; |
|
struct i2c_client *client = v4l2_get_subdevdata(sd); |
|
int ret, count = 0; |
|
|
|
while (count++ < I2C_WRITE_RETRY_COUNT) { |
|
ret = pivariety_write_reg(client, addr, value); |
|
if (!ret) |
|
return ret; |
|
} |
|
|
|
v4l2_err(sd, "%s: Write 0x%04x to register 0x%02x failed\n", |
|
__func__, value, addr); |
|
|
|
return ret; |
|
} |
|
|
|
static int wait_for_free(struct pivariety *pivariety, int interval) |
|
{ |
|
u32 value; |
|
u32 count = 0; |
|
|
|
while (count++ < (1000 / interval)) { |
|
int ret = pivariety_read(pivariety, SYSTEM_IDLE_REG, &value); |
|
|
|
if (!ret && !value) |
|
break; |
|
msleep(interval); |
|
} |
|
|
|
v4l2_dbg(2, debug, &pivariety->sd, "%s: End wait, Count: %d.\n", |
|
__func__, count); |
|
|
|
return 0; |
|
} |
|
|
|
static int is_raw(int pixformat) |
|
{ |
|
return pixformat >= 0x28 && pixformat <= 0x2D; |
|
} |
|
|
|
static u32 bayer_to_mbus_code(int data_type, int bayer_order) |
|
{ |
|
const u32 depth8[] = { |
|
MEDIA_BUS_FMT_SBGGR8_1X8, |
|
MEDIA_BUS_FMT_SGBRG8_1X8, |
|
MEDIA_BUS_FMT_SGRBG8_1X8, |
|
MEDIA_BUS_FMT_SRGGB8_1X8, |
|
MEDIA_BUS_FMT_Y8_1X8, |
|
}; |
|
|
|
const u32 depth10[] = { |
|
MEDIA_BUS_FMT_SBGGR10_1X10, |
|
MEDIA_BUS_FMT_SGBRG10_1X10, |
|
MEDIA_BUS_FMT_SGRBG10_1X10, |
|
MEDIA_BUS_FMT_SRGGB10_1X10, |
|
MEDIA_BUS_FMT_Y10_1X10, |
|
}; |
|
|
|
const u32 depth12[] = { |
|
MEDIA_BUS_FMT_SBGGR12_1X12, |
|
MEDIA_BUS_FMT_SGBRG12_1X12, |
|
MEDIA_BUS_FMT_SGRBG12_1X12, |
|
MEDIA_BUS_FMT_SRGGB12_1X12, |
|
MEDIA_BUS_FMT_Y12_1X12, |
|
}; |
|
|
|
if (bayer_order < 0 || bayer_order > 4) |
|
return 0; |
|
|
|
switch (data_type) { |
|
case IMAGE_DT_RAW8: |
|
return depth8[bayer_order]; |
|
case IMAGE_DT_RAW10: |
|
return depth10[bayer_order]; |
|
case IMAGE_DT_RAW12: |
|
return depth12[bayer_order]; |
|
} |
|
|
|
return 0; |
|
} |
|
|
|
static u32 yuv422_to_mbus_code(int data_type, int order) |
|
{ |
|
const u32 depth8[] = { |
|
MEDIA_BUS_FMT_YUYV8_1X16, |
|
MEDIA_BUS_FMT_YVYU8_1X16, |
|
MEDIA_BUS_FMT_UYVY8_1X16, |
|
MEDIA_BUS_FMT_VYUY8_1X16, |
|
}; |
|
|
|
const u32 depth10[] = { |
|
MEDIA_BUS_FMT_YUYV10_1X20, |
|
MEDIA_BUS_FMT_YVYU10_1X20, |
|
MEDIA_BUS_FMT_UYVY10_1X20, |
|
MEDIA_BUS_FMT_VYUY10_1X20, |
|
}; |
|
|
|
if (order < 0 || order > 3) |
|
return 0; |
|
|
|
switch (data_type) { |
|
case IMAGE_DT_YUV422_8: |
|
return depth8[order]; |
|
case IMAGE_DT_YUV422_10: |
|
return depth10[order]; |
|
} |
|
|
|
return 0; |
|
} |
|
|
|
static u32 data_type_to_mbus_code(int data_type, int bayer_order) |
|
{ |
|
if (is_raw(data_type)) |
|
return bayer_to_mbus_code(data_type, bayer_order); |
|
|
|
switch (data_type) { |
|
case IMAGE_DT_YUV422_8: |
|
case IMAGE_DT_YUV422_10: |
|
return yuv422_to_mbus_code(data_type, bayer_order); |
|
case IMAGE_DT_RGB565: |
|
return MEDIA_BUS_FMT_RGB565_2X8_LE; |
|
case IMAGE_DT_RGB888: |
|
return MEDIA_BUS_FMT_RGB888_1X24; |
|
} |
|
|
|
return 0; |
|
} |
|
|
|
/* Get bayer order based on flip setting. */ |
|
static u32 pivariety_get_format_code(struct pivariety *pivariety, |
|
struct arducam_format *format) |
|
{ |
|
unsigned int order, origin_order; |
|
|
|
lockdep_assert_held(&pivariety->mutex); |
|
|
|
/* |
|
* Only the bayer format needs to transform the format. |
|
*/ |
|
if (!is_raw(format->data_type) || |
|
!pivariety->bayer_order_volatile || |
|
format->bayer_order == BAYER_ORDER_GRAY) |
|
return data_type_to_mbus_code(format->data_type, |
|
format->bayer_order); |
|
|
|
order = format->bayer_order; |
|
|
|
origin_order = order; |
|
|
|
order = (pivariety->hflip && pivariety->hflip->val ? order ^ 1 : order); |
|
order = (pivariety->vflip && pivariety->vflip->val ? order ^ 2 : order); |
|
|
|
v4l2_dbg(1, debug, &pivariety->sd, "%s: before: %d, after: %d.\n", |
|
__func__, origin_order, order); |
|
|
|
return data_type_to_mbus_code(format->data_type, order); |
|
} |
|
|
|
/* Power/clock management functions */ |
|
static int pivariety_power_on(struct device *dev) |
|
{ |
|
struct i2c_client *client = to_i2c_client(dev); |
|
struct v4l2_subdev *sd = i2c_get_clientdata(client); |
|
struct pivariety *pivariety = to_pivariety(sd); |
|
int ret; |
|
|
|
ret = regulator_bulk_enable(ARDUCAM_NUM_SUPPLIES, |
|
pivariety->supplies); |
|
if (ret) { |
|
dev_err(dev, "%s: failed to enable regulators\n", |
|
__func__); |
|
return ret; |
|
} |
|
|
|
ret = clk_prepare_enable(pivariety->xclk); |
|
if (ret) { |
|
dev_err(dev, "%s: failed to enable clock\n", |
|
__func__); |
|
goto reg_off; |
|
} |
|
|
|
gpiod_set_value_cansleep(pivariety->reset_gpio, 1); |
|
usleep_range(ARDUCAM_XCLR_MIN_DELAY_US, |
|
ARDUCAM_XCLR_MIN_DELAY_US + ARDUCAM_XCLR_DELAY_RANGE_US); |
|
|
|
return 0; |
|
|
|
reg_off: |
|
regulator_bulk_disable(ARDUCAM_NUM_SUPPLIES, pivariety->supplies); |
|
|
|
return ret; |
|
} |
|
|
|
static int pivariety_power_off(struct device *dev) |
|
{ |
|
struct i2c_client *client = to_i2c_client(dev); |
|
struct v4l2_subdev *sd = i2c_get_clientdata(client); |
|
struct pivariety *pivariety = to_pivariety(sd); |
|
|
|
gpiod_set_value_cansleep(pivariety->reset_gpio, 0); |
|
regulator_bulk_disable(ARDUCAM_NUM_SUPPLIES, pivariety->supplies); |
|
clk_disable_unprepare(pivariety->xclk); |
|
|
|
return 0; |
|
} |
|
|
|
static int pivariety_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh) |
|
{ |
|
struct pivariety *pivariety = to_pivariety(sd); |
|
struct v4l2_mbus_framefmt *try_fmt = |
|
v4l2_subdev_get_try_format(sd, fh->state, 0); |
|
struct arducam_format *def_fmt = &pivariety->supported_formats[0]; |
|
|
|
/* Initialize try_fmt */ |
|
try_fmt->width = def_fmt->resolution_set->width; |
|
try_fmt->height = def_fmt->resolution_set->height; |
|
try_fmt->code = def_fmt->mbus_code; |
|
try_fmt->field = V4L2_FIELD_NONE; |
|
|
|
return 0; |
|
} |
|
|
|
static int pivariety_s_ctrl(struct v4l2_ctrl *ctrl) |
|
{ |
|
int ret, i; |
|
struct pivariety *pivariety = |
|
container_of(ctrl->handler, struct pivariety, |
|
ctrl_handler); |
|
struct arducam_format *supported_fmts = pivariety->supported_formats; |
|
int num_supported_formats = pivariety->num_supported_formats; |
|
|
|
v4l2_dbg(3, debug, &pivariety->sd, "%s: cid = (0x%X), value = (%d).\n", |
|
__func__, ctrl->id, ctrl->val); |
|
|
|
ret = pivariety_write(pivariety, CTRL_ID_REG, ctrl->id); |
|
ret += pivariety_write(pivariety, CTRL_VALUE_REG, ctrl->val); |
|
if (ret < 0) |
|
return -EINVAL; |
|
|
|
/* When flip is set, modify all bayer formats */ |
|
if (ctrl->id == V4L2_CID_VFLIP || ctrl->id == V4L2_CID_HFLIP) { |
|
for (i = 0; i < num_supported_formats; i++) { |
|
supported_fmts[i].mbus_code = |
|
pivariety_get_format_code(pivariety, |
|
&supported_fmts[i]); |
|
} |
|
} |
|
|
|
/* |
|
* When starting streaming, controls are set in batches, |
|
* and the short interval will cause some controls to be unsuccessfully |
|
* set. |
|
*/ |
|
if (pivariety->wait_until_free) |
|
wait_for_free(pivariety, 1); |
|
else |
|
usleep_range(200, 210); |
|
|
|
return 0; |
|
} |
|
|
|
static const struct v4l2_ctrl_ops pivariety_ctrl_ops = { |
|
.s_ctrl = pivariety_s_ctrl, |
|
}; |
|
|
|
static int pivariety_enum_mbus_code(struct v4l2_subdev *sd, |
|
struct v4l2_subdev_state *sd_state, |
|
struct v4l2_subdev_mbus_code_enum *code) |
|
{ |
|
struct pivariety *pivariety = to_pivariety(sd); |
|
struct arducam_format *supported_formats = pivariety->supported_formats; |
|
int num_supported_formats = pivariety->num_supported_formats; |
|
|
|
v4l2_dbg(1, debug, sd, "%s: index = (%d)\n", __func__, code->index); |
|
|
|
if (code->index >= num_supported_formats) |
|
return -EINVAL; |
|
|
|
code->code = supported_formats[code->index].mbus_code; |
|
|
|
return 0; |
|
} |
|
|
|
static int pivariety_enum_framesizes(struct v4l2_subdev *sd, |
|
struct v4l2_subdev_state *sd_state, |
|
struct v4l2_subdev_frame_size_enum *fse) |
|
{ |
|
int i; |
|
struct pivariety *pivariety = to_pivariety(sd); |
|
struct arducam_format *supported_formats = pivariety->supported_formats; |
|
int num_supported_formats = pivariety->num_supported_formats; |
|
struct arducam_format *format; |
|
struct arducam_resolution *resolution; |
|
|
|
v4l2_dbg(1, debug, sd, "%s: code = (0x%X), index = (%d)\n", |
|
__func__, fse->code, fse->index); |
|
|
|
for (i = 0; i < num_supported_formats; i++) { |
|
format = &supported_formats[i]; |
|
if (fse->code == format->mbus_code) { |
|
if (fse->index >= format->num_resolution_set) |
|
return -EINVAL; |
|
|
|
resolution = &format->resolution_set[fse->index]; |
|
fse->min_width = resolution->width; |
|
fse->max_width = resolution->width; |
|
fse->min_height = resolution->height; |
|
fse->max_height = resolution->height; |
|
|
|
return 0; |
|
} |
|
} |
|
|
|
return -EINVAL; |
|
} |
|
|
|
static int pivariety_get_fmt(struct v4l2_subdev *sd, |
|
struct v4l2_subdev_state *sd_state, |
|
struct v4l2_subdev_format *format) |
|
{ |
|
struct pivariety *pivariety = to_pivariety(sd); |
|
struct arducam_format *current_format; |
|
struct v4l2_mbus_framefmt *fmt = &format->format; |
|
int cur_res_idx; |
|
|
|
if (format->pad != 0) |
|
return -EINVAL; |
|
|
|
mutex_lock(&pivariety->mutex); |
|
|
|
current_format = |
|
&pivariety->supported_formats[pivariety->current_format_idx]; |
|
cur_res_idx = pivariety->current_resolution_idx; |
|
format->format.width = |
|
current_format->resolution_set[cur_res_idx].width; |
|
format->format.height = |
|
current_format->resolution_set[cur_res_idx].height; |
|
format->format.code = current_format->mbus_code; |
|
format->format.field = V4L2_FIELD_NONE; |
|
fmt->colorspace = V4L2_COLORSPACE_RAW; |
|
fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace); |
|
fmt->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true, |
|
fmt->colorspace, |
|
fmt->ycbcr_enc); |
|
fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace); |
|
|
|
v4l2_dbg(1, debug, sd, "%s: width: (%d) height: (%d) code: (0x%X)\n", |
|
__func__, format->format.width, format->format.height, |
|
format->format.code); |
|
|
|
mutex_unlock(&pivariety->mutex); |
|
return 0; |
|
} |
|
|
|
static int pivariety_get_fmt_idx_by_code(struct pivariety *pivariety, |
|
u32 mbus_code) |
|
{ |
|
int i; |
|
u32 data_type; |
|
struct arducam_format *formats = pivariety->supported_formats; |
|
|
|
for (i = 0; i < pivariety->num_supported_formats; i++) { |
|
if (formats[i].mbus_code == mbus_code) |
|
return i; |
|
} |
|
|
|
/* |
|
* If the specified format is not found in the list of supported |
|
* formats, try to find a format of the same data type. |
|
*/ |
|
for (i = 0; i < ARRAY_SIZE(codes); i++) |
|
if (codes[i] == mbus_code) |
|
break; |
|
|
|
if (i >= ARRAY_SIZE(codes)) |
|
return -EINVAL; |
|
|
|
data_type = i / 5 + IMAGE_DT_RAW8; |
|
|
|
for (i = 0; i < pivariety->num_supported_formats; i++) { |
|
if (formats[i].data_type == data_type) |
|
return i; |
|
} |
|
|
|
return -EINVAL; |
|
} |
|
|
|
static struct v4l2_ctrl *get_control(struct pivariety *pivariety, |
|
u32 id) |
|
{ |
|
int index = 0; |
|
|
|
while (index < MAX_CTRLS && pivariety->ctrls[index]) { |
|
if (pivariety->ctrls[index]->id == id) |
|
return pivariety->ctrls[index]; |
|
index++; |
|
} |
|
|
|
return NULL; |
|
} |
|
|
|
static int update_control(struct pivariety *pivariety, u32 id) |
|
{ |
|
struct v4l2_subdev *sd = &pivariety->sd; |
|
struct v4l2_ctrl *ctrl; |
|
u32 min, max, step, def, id2; |
|
int ret = 0; |
|
|
|
pivariety_write(pivariety, CTRL_ID_REG, id); |
|
pivariety_read(pivariety, CTRL_ID_REG, &id2); |
|
|
|
v4l2_dbg(1, debug, sd, "%s: Write ID: 0x%08X Read ID: 0x%08X\n", |
|
__func__, id, id2); |
|
|
|
pivariety_write(pivariety, CTRL_VALUE_REG, 0); |
|
wait_for_free(pivariety, 1); |
|
|
|
ret += pivariety_read(pivariety, CTRL_MAX_REG, &max); |
|
ret += pivariety_read(pivariety, CTRL_MIN_REG, &min); |
|
ret += pivariety_read(pivariety, CTRL_DEF_REG, &def); |
|
ret += pivariety_read(pivariety, CTRL_STEP_REG, &step); |
|
|
|
if (ret < 0) |
|
goto err; |
|
|
|
if (id == NO_DATA_AVAILABLE || max == NO_DATA_AVAILABLE || |
|
min == NO_DATA_AVAILABLE || def == NO_DATA_AVAILABLE || |
|
step == NO_DATA_AVAILABLE) |
|
goto err; |
|
|
|
v4l2_dbg(1, debug, sd, "%s: min: %d, max: %d, step: %d, def: %d\n", |
|
__func__, min, max, step, def); |
|
|
|
ctrl = get_control(pivariety, id); |
|
return __v4l2_ctrl_modify_range(ctrl, min, max, step, def); |
|
|
|
err: |
|
return -EINVAL; |
|
} |
|
|
|
static int update_controls(struct pivariety *pivariety) |
|
{ |
|
int ret = 0; |
|
int index = 0; |
|
|
|
wait_for_free(pivariety, 5); |
|
|
|
while (index < MAX_CTRLS && pivariety->ctrls[index]) { |
|
ret += update_control(pivariety, pivariety->ctrls[index]->id); |
|
index++; |
|
} |
|
|
|
return ret; |
|
} |
|
|
|
static int pivariety_set_fmt(struct v4l2_subdev *sd, |
|
struct v4l2_subdev_state *sd_state, |
|
struct v4l2_subdev_format *format) |
|
{ |
|
int i, j; |
|
struct pivariety *pivariety = to_pivariety(sd); |
|
struct arducam_format *supported_formats = pivariety->supported_formats; |
|
|
|
if (format->pad != 0) |
|
return -EINVAL; |
|
|
|
mutex_lock(&pivariety->mutex); |
|
|
|
format->format.colorspace = V4L2_COLORSPACE_RAW; |
|
format->format.field = V4L2_FIELD_NONE; |
|
|
|
v4l2_dbg(1, debug, sd, "%s: code: 0x%X, width: %d, height: %d\n", |
|
__func__, format->format.code, format->format.width, |
|
format->format.height); |
|
|
|
i = pivariety_get_fmt_idx_by_code(pivariety, format->format.code); |
|
if (i < 0) |
|
i = 0; |
|
|
|
format->format.code = supported_formats[i].mbus_code; |
|
|
|
for (j = 0; j < supported_formats[i].num_resolution_set; j++) { |
|
if (supported_formats[i].resolution_set[j].width == |
|
format->format.width && |
|
supported_formats[i].resolution_set[j].height == |
|
format->format.height) { |
|
v4l2_dbg(1, debug, sd, |
|
"%s: format match.\n", __func__); |
|
v4l2_dbg(1, debug, sd, |
|
"%s: set format to device: %d %d.\n", |
|
__func__, supported_formats[i].index, j); |
|
|
|
pivariety_write(pivariety, PIXFORMAT_INDEX_REG, |
|
supported_formats[i].index); |
|
pivariety_write(pivariety, RESOLUTION_INDEX_REG, j); |
|
|
|
pivariety->current_format_idx = i; |
|
pivariety->current_resolution_idx = j; |
|
|
|
update_controls(pivariety); |
|
|
|
goto unlock; |
|
} |
|
} |
|
|
|
format->format.width = supported_formats[i].resolution_set[0].width; |
|
format->format.height = supported_formats[i].resolution_set[0].height; |
|
|
|
pivariety_write(pivariety, PIXFORMAT_INDEX_REG, |
|
supported_formats[i].index); |
|
pivariety_write(pivariety, RESOLUTION_INDEX_REG, 0); |
|
|
|
pivariety->current_format_idx = i; |
|
pivariety->current_resolution_idx = 0; |
|
update_controls(pivariety); |
|
|
|
unlock: |
|
|
|
mutex_unlock(&pivariety->mutex); |
|
|
|
return 0; |
|
} |
|
|
|
/* Start streaming */ |
|
static int pivariety_start_streaming(struct pivariety *pivariety) |
|
{ |
|
int ret; |
|
|
|
/* set stream on register */ |
|
ret = pivariety_write(pivariety, MODE_SELECT_REG, |
|
ARDUCAM_MODE_STREAMING); |
|
|
|
if (ret) |
|
return ret; |
|
|
|
wait_for_free(pivariety, 2); |
|
|
|
/* |
|
* When starting streaming, controls are set in batches, |
|
* and the short interval will cause some controls to be unsuccessfully |
|
* set. |
|
*/ |
|
pivariety->wait_until_free = true; |
|
/* Apply customized values from user */ |
|
ret = __v4l2_ctrl_handler_setup(pivariety->sd.ctrl_handler); |
|
|
|
pivariety->wait_until_free = false; |
|
if (ret) |
|
return ret; |
|
|
|
wait_for_free(pivariety, 2); |
|
|
|
return ret; |
|
} |
|
|
|
static int pivariety_read_sel(struct pivariety *pivariety, |
|
struct v4l2_rect *rect) |
|
{ |
|
int ret = 0; |
|
|
|
ret += pivariety_read(pivariety, IPC_SEL_TOP_REG, &rect->top); |
|
ret += pivariety_read(pivariety, IPC_SEL_LEFT_REG, &rect->left); |
|
ret += pivariety_read(pivariety, IPC_SEL_WIDTH_REG, &rect->width); |
|
ret += pivariety_read(pivariety, IPC_SEL_HEIGHT_REG, &rect->height); |
|
|
|
if (ret || rect->top == NO_DATA_AVAILABLE || |
|
rect->left == NO_DATA_AVAILABLE || |
|
rect->width == NO_DATA_AVAILABLE || |
|
rect->height == NO_DATA_AVAILABLE) { |
|
v4l2_err(&pivariety->sd, "%s: Failed to read selection.\n", |
|
__func__); |
|
return -EINVAL; |
|
} |
|
|
|
return 0; |
|
} |
|
|
|
static const struct v4l2_rect * |
|
__pivariety_get_pad_crop(struct pivariety *pivariety, |
|
struct v4l2_subdev_state *sd_state, |
|
unsigned int pad, |
|
enum v4l2_subdev_format_whence which) |
|
{ |
|
int ret; |
|
|
|
switch (which) { |
|
case V4L2_SUBDEV_FORMAT_TRY: |
|
return v4l2_subdev_get_try_crop(&pivariety->sd, sd_state, pad); |
|
case V4L2_SUBDEV_FORMAT_ACTIVE: |
|
ret = pivariety_read_sel(pivariety, &pivariety->crop); |
|
if (ret) |
|
return NULL; |
|
return &pivariety->crop; |
|
} |
|
|
|
return NULL; |
|
} |
|
|
|
static int pivariety_get_selection(struct v4l2_subdev *sd, |
|
struct v4l2_subdev_state *sd_state, |
|
struct v4l2_subdev_selection *sel) |
|
{ |
|
int ret = 0; |
|
struct v4l2_rect rect; |
|
struct pivariety *pivariety = to_pivariety(sd); |
|
|
|
ret = pivariety_write(pivariety, IPC_SEL_TARGET_REG, sel->target); |
|
if (ret) { |
|
v4l2_err(sd, "%s: Write register 0x%02x failed\n", |
|
__func__, IPC_SEL_TARGET_REG); |
|
return -EINVAL; |
|
} |
|
|
|
wait_for_free(pivariety, 2); |
|
|
|
switch (sel->target) { |
|
case V4L2_SEL_TGT_CROP: { |
|
mutex_lock(&pivariety->mutex); |
|
sel->r = *__pivariety_get_pad_crop(pivariety, sd_state, |
|
sel->pad, |
|
sel->which); |
|
mutex_unlock(&pivariety->mutex); |
|
|
|
return 0; |
|
} |
|
|
|
case V4L2_SEL_TGT_NATIVE_SIZE: |
|
case V4L2_SEL_TGT_CROP_DEFAULT: |
|
case V4L2_SEL_TGT_CROP_BOUNDS: |
|
ret = pivariety_read_sel(pivariety, &rect); |
|
if (ret) |
|
return -EINVAL; |
|
|
|
sel->r = rect; |
|
return 0; |
|
} |
|
|
|
return -EINVAL; |
|
} |
|
|
|
/* Stop streaming */ |
|
static int pivariety_stop_streaming(struct pivariety *pivariety) |
|
{ |
|
int ret; |
|
|
|
/* set stream off register */ |
|
ret = pivariety_write(pivariety, MODE_SELECT_REG, ARDUCAM_MODE_STANDBY); |
|
if (ret) |
|
v4l2_err(&pivariety->sd, "%s failed to set stream\n", __func__); |
|
|
|
/* |
|
* Return success even if it was an error, as there is nothing the |
|
* caller can do about it. |
|
*/ |
|
return 0; |
|
} |
|
|
|
static int pivariety_set_stream(struct v4l2_subdev *sd, int enable) |
|
{ |
|
struct pivariety *pivariety = to_pivariety(sd); |
|
struct i2c_client *client = v4l2_get_subdevdata(sd); |
|
int ret = 0; |
|
|
|
mutex_lock(&pivariety->mutex); |
|
if (pivariety->streaming == enable) { |
|
mutex_unlock(&pivariety->mutex); |
|
return 0; |
|
} |
|
|
|
if (enable) { |
|
ret = pm_runtime_get_sync(&client->dev); |
|
if (ret < 0) { |
|
pm_runtime_put_noidle(&client->dev); |
|
goto err_unlock; |
|
} |
|
|
|
/* |
|
* Apply default & customized values |
|
* and then start streaming. |
|
*/ |
|
ret = pivariety_start_streaming(pivariety); |
|
if (ret) |
|
goto err_rpm_put; |
|
} else { |
|
pivariety_stop_streaming(pivariety); |
|
pm_runtime_put(&client->dev); |
|
} |
|
|
|
pivariety->streaming = enable; |
|
|
|
/* |
|
* vflip and hflip cannot change during streaming |
|
* Pivariety may not implement flip control. |
|
*/ |
|
if (pivariety->vflip) |
|
__v4l2_ctrl_grab(pivariety->vflip, enable); |
|
|
|
if (pivariety->hflip) |
|
__v4l2_ctrl_grab(pivariety->hflip, enable); |
|
|
|
mutex_unlock(&pivariety->mutex); |
|
|
|
return ret; |
|
|
|
err_rpm_put: |
|
pm_runtime_put(&client->dev); |
|
err_unlock: |
|
mutex_unlock(&pivariety->mutex); |
|
|
|
return ret; |
|
} |
|
|
|
static int __maybe_unused pivariety_suspend(struct device *dev) |
|
{ |
|
struct i2c_client *client = to_i2c_client(dev); |
|
struct v4l2_subdev *sd = i2c_get_clientdata(client); |
|
struct pivariety *pivariety = to_pivariety(sd); |
|
|
|
if (pivariety->streaming) |
|
pivariety_stop_streaming(pivariety); |
|
|
|
return 0; |
|
} |
|
|
|
static int __maybe_unused pivariety_resume(struct device *dev) |
|
{ |
|
struct i2c_client *client = to_i2c_client(dev); |
|
struct v4l2_subdev *sd = i2c_get_clientdata(client); |
|
struct pivariety *pivariety = to_pivariety(sd); |
|
int ret; |
|
|
|
if (pivariety->streaming) { |
|
ret = pivariety_start_streaming(pivariety); |
|
if (ret) |
|
goto error; |
|
} |
|
|
|
return 0; |
|
|
|
error: |
|
pivariety_stop_streaming(pivariety); |
|
pivariety->streaming = 0; |
|
return ret; |
|
} |
|
|
|
static int pivariety_get_regulators(struct pivariety *pivariety) |
|
{ |
|
struct i2c_client *client = v4l2_get_subdevdata(&pivariety->sd); |
|
int i; |
|
|
|
for (i = 0; i < ARDUCAM_NUM_SUPPLIES; i++) |
|
pivariety->supplies[i].supply = pivariety_supply_name[i]; |
|
|
|
return devm_regulator_bulk_get(&client->dev, |
|
ARDUCAM_NUM_SUPPLIES, |
|
pivariety->supplies); |
|
} |
|
|
|
static int pivariety_get_mbus_config(struct v4l2_subdev *sd, unsigned int pad, |
|
struct v4l2_mbus_config *cfg) |
|
{ |
|
struct pivariety *pivariety = to_pivariety(sd); |
|
|
|
if (pivariety->lanes > pivariety->bus.num_data_lanes) |
|
return -EINVAL; |
|
|
|
cfg->type = V4L2_MBUS_CSI2_DPHY; |
|
cfg->bus.mipi_csi2.flags = pivariety->bus.flags; |
|
cfg->bus.mipi_csi2.num_data_lanes = pivariety->lanes; |
|
|
|
return 0; |
|
} |
|
|
|
static const struct v4l2_subdev_core_ops pivariety_core_ops = { |
|
.subscribe_event = v4l2_ctrl_subdev_subscribe_event, |
|
.unsubscribe_event = v4l2_event_subdev_unsubscribe, |
|
}; |
|
|
|
static const struct v4l2_subdev_video_ops pivariety_video_ops = { |
|
.s_stream = pivariety_set_stream, |
|
}; |
|
|
|
static const struct v4l2_subdev_pad_ops pivariety_pad_ops = { |
|
.enum_mbus_code = pivariety_enum_mbus_code, |
|
.get_fmt = pivariety_get_fmt, |
|
.set_fmt = pivariety_set_fmt, |
|
.enum_frame_size = pivariety_enum_framesizes, |
|
.get_selection = pivariety_get_selection, |
|
.get_mbus_config = pivariety_get_mbus_config, |
|
}; |
|
|
|
static const struct v4l2_subdev_ops pivariety_subdev_ops = { |
|
.core = &pivariety_core_ops, |
|
.video = &pivariety_video_ops, |
|
.pad = &pivariety_pad_ops, |
|
}; |
|
|
|
static const struct v4l2_subdev_internal_ops pivariety_internal_ops = { |
|
.open = pivariety_open, |
|
}; |
|
|
|
static void pivariety_free_controls(struct pivariety *pivariety) |
|
{ |
|
v4l2_ctrl_handler_free(pivariety->sd.ctrl_handler); |
|
mutex_destroy(&pivariety->mutex); |
|
} |
|
|
|
static int pivariety_get_length_of_set(struct pivariety *pivariety, |
|
u16 idx_reg, u16 val_reg) |
|
{ |
|
int ret; |
|
int index = 0; |
|
u32 val; |
|
|
|
while (1) { |
|
ret = pivariety_write(pivariety, idx_reg, index); |
|
ret += pivariety_read(pivariety, val_reg, &val); |
|
|
|
if (ret < 0) |
|
return -1; |
|
|
|
if (val == NO_DATA_AVAILABLE) |
|
break; |
|
index++; |
|
} |
|
pivariety_write(pivariety, idx_reg, 0); |
|
return index; |
|
} |
|
|
|
static int pivariety_enum_resolution(struct pivariety *pivariety, |
|
struct arducam_format *format) |
|
{ |
|
struct i2c_client *client = v4l2_get_subdevdata(&pivariety->sd); |
|
int index = 0; |
|
u32 width, height; |
|
int num_resolution = 0; |
|
int ret; |
|
|
|
num_resolution = pivariety_get_length_of_set(pivariety, |
|
RESOLUTION_INDEX_REG, |
|
FORMAT_WIDTH_REG); |
|
if (num_resolution < 0) |
|
goto err; |
|
|
|
format->resolution_set = devm_kzalloc(&client->dev, |
|
sizeof(*format->resolution_set) * |
|
num_resolution, |
|
GFP_KERNEL); |
|
while (1) { |
|
ret = pivariety_write(pivariety, RESOLUTION_INDEX_REG, index); |
|
ret += pivariety_read(pivariety, FORMAT_WIDTH_REG, &width); |
|
ret += pivariety_read(pivariety, FORMAT_HEIGHT_REG, &height); |
|
|
|
if (ret < 0) |
|
goto err; |
|
|
|
if (width == NO_DATA_AVAILABLE || height == NO_DATA_AVAILABLE) |
|
break; |
|
|
|
format->resolution_set[index].width = width; |
|
format->resolution_set[index].height = height; |
|
|
|
index++; |
|
} |
|
|
|
format->num_resolution_set = index; |
|
pivariety_write(pivariety, RESOLUTION_INDEX_REG, 0); |
|
return 0; |
|
err: |
|
return -ENODEV; |
|
} |
|
|
|
static int pivariety_enum_pixformat(struct pivariety *pivariety) |
|
{ |
|
int ret = 0; |
|
u32 mbus_code = 0; |
|
int pixfmt_type; |
|
int bayer_order; |
|
int bayer_order_not_volatile; |
|
int lanes; |
|
int index = 0; |
|
int num_pixformat = 0; |
|
struct arducam_format *arducam_fmt; |
|
struct i2c_client *client = v4l2_get_subdevdata(&pivariety->sd); |
|
|
|
num_pixformat = pivariety_get_length_of_set(pivariety, |
|
PIXFORMAT_INDEX_REG, |
|
PIXFORMAT_TYPE_REG); |
|
|
|
if (num_pixformat < 0) |
|
goto err; |
|
|
|
ret = pivariety_read(pivariety, FLIPS_DONT_CHANGE_ORDER_REG, |
|
&bayer_order_not_volatile); |
|
if (bayer_order_not_volatile == NO_DATA_AVAILABLE) |
|
pivariety->bayer_order_volatile = 1; |
|
else |
|
pivariety->bayer_order_volatile = !bayer_order_not_volatile; |
|
|
|
if (ret < 0) |
|
goto err; |
|
|
|
pivariety->supported_formats = |
|
devm_kzalloc(&client->dev, |
|
sizeof(*pivariety->supported_formats) * |
|
num_pixformat, |
|
GFP_KERNEL); |
|
|
|
while (1) { |
|
ret = pivariety_write(pivariety, PIXFORMAT_INDEX_REG, index); |
|
ret += pivariety_read(pivariety, PIXFORMAT_TYPE_REG, |
|
&pixfmt_type); |
|
|
|
if (pixfmt_type == NO_DATA_AVAILABLE) |
|
break; |
|
|
|
ret += pivariety_read(pivariety, MIPI_LANES_REG, &lanes); |
|
if (lanes == NO_DATA_AVAILABLE) |
|
break; |
|
|
|
ret += pivariety_read(pivariety, PIXFORMAT_ORDER_REG, |
|
&bayer_order); |
|
if (ret < 0) |
|
goto err; |
|
|
|
mbus_code = data_type_to_mbus_code(pixfmt_type, bayer_order); |
|
arducam_fmt = &pivariety->supported_formats[index]; |
|
arducam_fmt->index = index; |
|
arducam_fmt->mbus_code = mbus_code; |
|
arducam_fmt->bayer_order = bayer_order; |
|
arducam_fmt->data_type = pixfmt_type; |
|
if (pivariety_enum_resolution(pivariety, arducam_fmt)) |
|
goto err; |
|
|
|
index++; |
|
} |
|
|
|
pivariety_write(pivariety, PIXFORMAT_INDEX_REG, 0); |
|
pivariety->num_supported_formats = index; |
|
pivariety->current_format_idx = 0; |
|
pivariety->current_resolution_idx = 0; |
|
pivariety->lanes = lanes; |
|
|
|
return 0; |
|
|
|
err: |
|
return -ENODEV; |
|
} |
|
|
|
static const char *pivariety_ctrl_get_name(u32 id) |
|
{ |
|
switch (id) { |
|
case V4L2_CID_ARDUCAM_EXT_TRI: |
|
return "trigger_mode"; |
|
case V4L2_CID_ARDUCAM_IRCUT: |
|
return "ircut"; |
|
case V4L2_CID_ARDUCAM_STROBE_SHIFT: |
|
return "strobe_shift"; |
|
case V4L2_CID_ARDUCAM_STROBE_WIDTH: |
|
return "strobe_width"; |
|
case V4L2_CID_ARDUCAM_MODE: |
|
return "mode"; |
|
default: |
|
return NULL; |
|
} |
|
} |
|
|
|
enum v4l2_ctrl_type pivariety_get_v4l2_ctrl_type(u32 id) |
|
{ |
|
switch (id) { |
|
case V4L2_CID_ARDUCAM_EXT_TRI: |
|
return V4L2_CTRL_TYPE_BOOLEAN; |
|
case V4L2_CID_ARDUCAM_IRCUT: |
|
return V4L2_CTRL_TYPE_BOOLEAN; |
|
default: |
|
return V4L2_CTRL_TYPE_INTEGER; |
|
} |
|
} |
|
|
|
static struct v4l2_ctrl *v4l2_ctrl_new_arducam(struct v4l2_ctrl_handler *hdl, |
|
const struct v4l2_ctrl_ops *ops, |
|
u32 id, s64 min, s64 max, |
|
u64 step, s64 def) |
|
{ |
|
struct v4l2_ctrl_config ctrl_cfg = { |
|
.ops = ops, |
|
.id = id, |
|
.name = NULL, |
|
.type = V4L2_CTRL_TYPE_INTEGER, |
|
.flags = 0, |
|
.min = min, |
|
.max = max, |
|
.def = def, |
|
.step = step, |
|
}; |
|
|
|
ctrl_cfg.name = pivariety_ctrl_get_name(id); |
|
ctrl_cfg.type = pivariety_get_v4l2_ctrl_type(id); |
|
|
|
return v4l2_ctrl_new_custom(hdl, &ctrl_cfg, NULL); |
|
} |
|
|
|
static int pivariety_enum_controls(struct pivariety *pivariety) |
|
{ |
|
struct v4l2_subdev *sd = &pivariety->sd; |
|
struct i2c_client *client = v4l2_get_subdevdata(sd); |
|
struct v4l2_ctrl_handler *ctrl_hdlr = &pivariety->ctrl_handler; |
|
struct v4l2_fwnode_device_properties props; |
|
struct v4l2_ctrl **ctrl = pivariety->ctrls; |
|
int ret, index, num_ctrls; |
|
u32 id, min, max, def, step; |
|
|
|
num_ctrls = pivariety_get_length_of_set(pivariety, CTRL_INDEX_REG, |
|
CTRL_ID_REG); |
|
if (num_ctrls < 0) |
|
goto err; |
|
|
|
v4l2_dbg(1, debug, sd, "%s: num_ctrls = %d\n", |
|
__func__, num_ctrls); |
|
|
|
ret = v4l2_ctrl_handler_init(ctrl_hdlr, num_ctrls); |
|
if (ret) |
|
return ret; |
|
|
|
index = 0; |
|
while (1) { |
|
ret = pivariety_write(pivariety, CTRL_INDEX_REG, index); |
|
pivariety_write(pivariety, CTRL_VALUE_REG, 0); |
|
wait_for_free(pivariety, 1); |
|
|
|
ret += pivariety_read(pivariety, CTRL_ID_REG, &id); |
|
ret += pivariety_read(pivariety, CTRL_MAX_REG, &max); |
|
ret += pivariety_read(pivariety, CTRL_MIN_REG, &min); |
|
ret += pivariety_read(pivariety, CTRL_DEF_REG, &def); |
|
ret += pivariety_read(pivariety, CTRL_STEP_REG, &step); |
|
if (ret < 0) |
|
goto err; |
|
|
|
if (id == NO_DATA_AVAILABLE || max == NO_DATA_AVAILABLE || |
|
min == NO_DATA_AVAILABLE || def == NO_DATA_AVAILABLE || |
|
step == NO_DATA_AVAILABLE) |
|
break; |
|
|
|
v4l2_dbg(1, debug, sd, |
|
"%s: index = %d, id = 0x%x, max = %d, min = %d, def = %d, step = %d\n", |
|
__func__, index, id, max, min, def, step); |
|
|
|
if (v4l2_ctrl_get_name(id)) { |
|
*ctrl = v4l2_ctrl_new_std(ctrl_hdlr, |
|
&pivariety_ctrl_ops, |
|
id, min, |
|
max, step, |
|
def); |
|
v4l2_dbg(1, debug, sd, "%s: ctrl: 0x%p\n", |
|
__func__, *ctrl); |
|
} else if (pivariety_ctrl_get_name(id)) { |
|
*ctrl = v4l2_ctrl_new_arducam(ctrl_hdlr, |
|
&pivariety_ctrl_ops, |
|
id, min, max, step, def); |
|
|
|
v4l2_dbg(1, debug, sd, |
|
"%s: new custom ctrl, ctrl: 0x%p.\n", |
|
__func__, *ctrl); |
|
} else { |
|
index++; |
|
continue; |
|
} |
|
|
|
if (!*ctrl) |
|
goto err; |
|
|
|
switch (id) { |
|
case V4L2_CID_HFLIP: |
|
pivariety->hflip = *ctrl; |
|
if (pivariety->bayer_order_volatile) |
|
pivariety->hflip->flags |= |
|
V4L2_CTRL_FLAG_MODIFY_LAYOUT; |
|
break; |
|
|
|
case V4L2_CID_VFLIP: |
|
pivariety->vflip = *ctrl; |
|
if (pivariety->bayer_order_volatile) |
|
pivariety->vflip->flags |= |
|
V4L2_CTRL_FLAG_MODIFY_LAYOUT; |
|
break; |
|
|
|
case V4L2_CID_HBLANK: |
|
(*ctrl)->flags |= V4L2_CTRL_FLAG_READ_ONLY; |
|
break; |
|
} |
|
|
|
ctrl++; |
|
index++; |
|
} |
|
|
|
pivariety_write(pivariety, CTRL_INDEX_REG, 0); |
|
|
|
ret = v4l2_fwnode_device_parse(&client->dev, &props); |
|
if (ret) |
|
goto err; |
|
|
|
ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, |
|
&pivariety_ctrl_ops, |
|
&props); |
|
if (ret) |
|
goto err; |
|
|
|
pivariety->sd.ctrl_handler = ctrl_hdlr; |
|
v4l2_ctrl_handler_setup(ctrl_hdlr); |
|
return 0; |
|
err: |
|
return -ENODEV; |
|
} |
|
|
|
static int pivariety_parse_dt(struct pivariety *pivariety, struct device *dev) |
|
{ |
|
struct fwnode_handle *endpoint; |
|
struct v4l2_fwnode_endpoint ep_cfg = { |
|
.bus_type = V4L2_MBUS_CSI2_DPHY |
|
}; |
|
int ret = -EINVAL; |
|
|
|
/* 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; |
|
} |
|
|
|
if (v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep_cfg)) { |
|
dev_err(dev, "could not parse endpoint\n"); |
|
goto error_out; |
|
} |
|
|
|
pivariety->bus = ep_cfg.bus.mipi_csi2; |
|
|
|
ret = 0; |
|
|
|
error_out: |
|
v4l2_fwnode_endpoint_free(&ep_cfg); |
|
fwnode_handle_put(endpoint); |
|
|
|
return ret; |
|
} |
|
|
|
static int pivariety_probe(struct i2c_client *client, |
|
const struct i2c_device_id *id) |
|
{ |
|
struct device *dev = &client->dev; |
|
struct pivariety *pivariety; |
|
u32 device_id, firmware_version; |
|
int ret; |
|
|
|
pivariety = devm_kzalloc(&client->dev, sizeof(*pivariety), GFP_KERNEL); |
|
if (!pivariety) |
|
return -ENOMEM; |
|
|
|
/* Initialize subdev */ |
|
v4l2_i2c_subdev_init(&pivariety->sd, client, |
|
&pivariety_subdev_ops); |
|
|
|
if (pivariety_parse_dt(pivariety, dev)) |
|
return -EINVAL; |
|
|
|
/* Get system clock (xclk) */ |
|
pivariety->xclk = devm_clk_get(dev, "xclk"); |
|
if (IS_ERR(pivariety->xclk)) { |
|
dev_err(dev, "failed to get xclk\n"); |
|
return PTR_ERR(pivariety->xclk); |
|
} |
|
|
|
pivariety->xclk_freq = clk_get_rate(pivariety->xclk); |
|
if (pivariety->xclk_freq != 24000000) { |
|
dev_err(dev, "xclk frequency not supported: %d Hz\n", |
|
pivariety->xclk_freq); |
|
return -EINVAL; |
|
} |
|
|
|
ret = pivariety_get_regulators(pivariety); |
|
if (ret) |
|
return ret; |
|
|
|
/* Request optional enable pin */ |
|
pivariety->reset_gpio = devm_gpiod_get_optional(dev, "reset", |
|
GPIOD_OUT_HIGH); |
|
|
|
ret = pivariety_power_on(dev); |
|
if (ret) |
|
return ret; |
|
|
|
ret = pivariety_read(pivariety, DEVICE_ID_REG, &device_id); |
|
if (ret || device_id != DEVICE_ID) { |
|
dev_err(dev, "probe failed\n"); |
|
ret = -ENODEV; |
|
goto error_power_off; |
|
} |
|
|
|
ret = pivariety_read(pivariety, DEVICE_VERSION_REG, &firmware_version); |
|
if (ret) |
|
dev_err(dev, "read firmware version failed\n"); |
|
|
|
dev_info(dev, "firmware version: 0x%04X\n", firmware_version); |
|
|
|
if (pivariety_enum_pixformat(pivariety)) { |
|
dev_err(dev, "enum pixformat failed.\n"); |
|
ret = -ENODEV; |
|
goto error_power_off; |
|
} |
|
|
|
if (pivariety_enum_controls(pivariety)) { |
|
dev_err(dev, "enum controls failed.\n"); |
|
ret = -ENODEV; |
|
goto error_power_off; |
|
} |
|
|
|
/* Initialize subdev */ |
|
pivariety->sd.internal_ops = &pivariety_internal_ops; |
|
pivariety->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; |
|
pivariety->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR; |
|
/* Initialize source pad */ |
|
pivariety->pad.flags = MEDIA_PAD_FL_SOURCE; |
|
|
|
ret = media_entity_pads_init(&pivariety->sd.entity, 1, &pivariety->pad); |
|
if (ret) |
|
goto error_handler_free; |
|
|
|
ret = v4l2_async_register_subdev_sensor(&pivariety->sd); |
|
if (ret < 0) |
|
goto error_media_entity; |
|
|
|
pm_runtime_set_active(dev); |
|
pm_runtime_enable(dev); |
|
pm_runtime_idle(dev); |
|
|
|
return 0; |
|
|
|
error_media_entity: |
|
media_entity_cleanup(&pivariety->sd.entity); |
|
|
|
error_handler_free: |
|
pivariety_free_controls(pivariety); |
|
|
|
error_power_off: |
|
pivariety_power_off(dev); |
|
|
|
return ret; |
|
} |
|
|
|
static int pivariety_remove(struct i2c_client *client) |
|
{ |
|
struct v4l2_subdev *sd = i2c_get_clientdata(client); |
|
struct pivariety *pivariety = to_pivariety(sd); |
|
|
|
v4l2_async_unregister_subdev(sd); |
|
media_entity_cleanup(&sd->entity); |
|
pivariety_free_controls(pivariety); |
|
|
|
pm_runtime_disable(&client->dev); |
|
pm_runtime_set_suspended(&client->dev); |
|
|
|
return 0; |
|
} |
|
|
|
static const struct dev_pm_ops pivariety_pm_ops = { |
|
SET_SYSTEM_SLEEP_PM_OPS(pivariety_suspend, pivariety_resume) |
|
SET_RUNTIME_PM_OPS(pivariety_power_off, pivariety_power_on, NULL) |
|
}; |
|
|
|
static const struct of_device_id arducam_pivariety_dt_ids[] = { |
|
{ .compatible = "arducam,arducam-pivariety" }, |
|
{ /* sentinel */ } |
|
}; |
|
MODULE_DEVICE_TABLE(of, arducam_pivariety_dt_ids); |
|
|
|
static struct i2c_driver arducam_pivariety_i2c_driver = { |
|
.driver = { |
|
.name = "arducam-pivariety", |
|
.of_match_table = arducam_pivariety_dt_ids, |
|
.pm = &pivariety_pm_ops, |
|
}, |
|
.probe = pivariety_probe, |
|
.remove = pivariety_remove, |
|
}; |
|
|
|
module_i2c_driver(arducam_pivariety_i2c_driver); |
|
|
|
MODULE_AUTHOR("Lee Jackson <[email protected]>"); |
|
MODULE_DESCRIPTION("Arducam Pivariety v4l2 driver"); |
|
MODULE_LICENSE("GPL v2");
|
|
|