Skip to content

Commit

Permalink
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
d4xx: Update d4xx.c driver to match RealSense latest development.
Browse files Browse the repository at this point in the history
RealSense PRQ d4xx version is 1.0.1.13.
ADL-P - support for sensor vc.
      - support for sensor state.
      - sensor own v4l2 controls.
Bump version for ADL-P to 1.0.2.13.

Signed-off-by: Dmitry Perchanov <[email protected]>
dmipx committed Jan 2, 2023
1 parent a285b51 commit b0aa357
Showing 1 changed file with 1,076 additions and 313 deletions.
1,389 changes: 1,076 additions & 313 deletions drivers/media/i2c/d4xx.c
Original file line number Diff line number Diff line change
@@ -115,6 +115,7 @@
#define DS5_EXPOSURE_ROI_BOTTOM 0x0018
#define DS5_EXPOSURE_ROI_RIGHT 0x001C
#define DS5_MANUAL_LASER_POWER 0x0024
#define DS5_PWM_FREQUENCY 0x0028

#define DS5_DEPTH_CONFIG_STATUS 0x4800
#define DS5_RGB_CONFIG_STATUS 0x4802
@@ -128,9 +129,9 @@

#define MIPI_LANE_RATE 1000

#define MAX_DEPTH_EXP 2000
#define MAX_DEPTH_EXP 200000
#define MAX_RGB_EXP 10000
#define DEF_DEPTH_EXP 330
#define DEF_DEPTH_EXP 33000
#define DEF_RGB_EXP 1660

/* Currently both depth and IR use VC 0 */
@@ -166,40 +167,41 @@ enum ds5_mux_pad {
/* DFU definition section */
#define DFU_MAGIC_NUMBER "/0x01/0x02/0x03/0x04"
#define DFU_BLOCK_SIZE 1024

#define DFU_I2C_STANDARD_MODE 100000
#define DFU_I2C_FAST_MODE 400000
#define DFU_I2C_BUS_CLK_RATE DFU_I2C_FAST_MODE

#define ds5_read_with_check(state, addr, val) {\
if (ds5_read(state, addr, val)) \
return -EINVAL; \
}
return -EINVAL;}
#define ds5_raw_read_with_check(state, addr, buf, size) {\
if (ds5_raw_read(state, addr, buf, size)) \
return -EINVAL; \
}
return -EINVAL;}
#define ds5_write_with_check(state, addr, val) {\
if (ds5_write(state, addr, val)) \
return -EINVAL; \
}
if (ds5_write(state, addr, val)) \
return -EINVAL;}
#define ds5_raw_write_with_check(state, addr, buf, size) {\
if (ds5_raw_write(state, addr, buf, size)) \
return -EINVAL; \
}
if (ds5_raw_write(state, addr, buf, size)) \
return -EINVAL;}
#define D4XX_LINK_FREQ_360MHZ 360000000ULL
#define D4XX_LINK_FREQ_300MHZ 300000000ULL
#define D4XX_LINK_FREQ_288MHZ 288000000ULL
#define D4XX_LINK_FREQ_240MHZ 240000000ULL
#define D4XX_LINK_FREQ_225MHZ 22500000ULL

enum dfu_fw_state {
appIDLE = 0x0000,
appDETACH = 0x0001,
dfuIDLE = 0x0002,
dfuDNLOAD_SYNC = 0x0003,
dfuDNBUSY = 0x0004,
dfuDNLOAD_IDLE = 0x0005,
dfuMANIFEST_SYNC = 0x0006,
dfuMANIFEST = 0x0007,
dfuMANIFEST_WAIT_RESET = 0x0008,
dfuUPLOAD_IDLE = 0x0009,
dfuERROR = 0x000a
appIDLE = 0x0000,
appDETACH = 0x0001,
dfuIDLE = 0x0002,
dfuDNLOAD_SYNC = 0x0003,
dfuDNBUSY = 0x0004,
dfuDNLOAD_IDLE = 0x0005,
dfuMANIFEST_SYNC = 0x0006,
dfuMANIFEST = 0x0007,
dfuMANIFEST_WAIT_RESET = 0x0008,
dfuUPLOAD_IDLE = 0x0009,
dfuERROR = 0x000a
};

enum dfu_state {
@@ -319,6 +321,9 @@ struct __fw_status {

struct ds5_ctrls {
struct v4l2_ctrl_handler handler;
struct v4l2_ctrl_handler handler_depth;
struct v4l2_ctrl_handler handler_rgb;
struct v4l2_ctrl_handler handler_y8;
struct {
struct v4l2_ctrl *log;
struct v4l2_ctrl *fw_version;
@@ -405,6 +410,7 @@ struct ds5_dfu_dev {
unsigned char *dfu_msg;
u16 msg_write_once;
unsigned char init_v4l_f;
u32 bus_clk_rate;
};

enum {
@@ -496,42 +502,43 @@ static int ds5_write(struct ds5 *state, u16 reg, u16 val)
value[1] = val >> 8;
value[0] = val & 0x00FF;

dev_info(&state->client->dev, "%s(): writing to register: 0x%04x, value1: 0x%x, value2:0x%x\n",
__func__, reg, value[1], value[0]);
dev_info(&state->client->dev,
"%s(): writing to register: 0x%04x, value1: 0x%x, value2:0x%x\n",
__func__, reg, value[1], value[0]);

ret = regmap_raw_write(state->regmap, reg, value, sizeof(value));
if (ret < 0)
dev_err(&state->client->dev, "%s(): i2c write failed %d, 0x%04x = 0x%x\n",
__func__, ret, reg, val);
dev_err(&state->client->dev,
"%s(): i2c write failed %d, 0x%04x = 0x%x\n",
__func__, ret, reg, val);
else
if (state->dfu_dev.dfu_state_flag == DS5_DFU_IDLE)
dev_info(&state->client->dev, "%s(): i2c write 0x%04x: 0x%x\n",
__func__, reg, val);
__func__, reg, val);

return ret;
}

static int ds5_raw_write(struct ds5 *state, u16 reg, const void *val, size_t val_len)
static int ds5_raw_write(struct ds5 *state, u16 reg,
const void *val, size_t val_len)
{
int ret;

ret = regmap_raw_write(state->regmap, reg, val, val_len);
int ret = regmap_raw_write(state->regmap, reg, val, val_len);
if (ret < 0)
dev_err(&state->client->dev, "%s(): i2c raw write failed %d, %04x size(%d) bytes\n",
__func__, ret, reg, (int)val_len);
dev_err(&state->client->dev,
"%s(): i2c raw write failed %d, %04x size(%d) bytes\n",
__func__, ret, reg, (int)val_len);
else
if (state->dfu_dev.dfu_state_flag == DS5_DFU_IDLE)
dev_info(&state->client->dev, "%s(): i2c raw write 0x%04x: %d bytes\n",
__func__, reg, (int)val_len);
dev_info(&state->client->dev,
"%s(): i2c raw write 0x%04x: %d bytes\n",
__func__, reg, (int)val_len);

return ret;
}

static int ds5_read(struct ds5 *state, u16 reg, u16 *val)
{
int ret;

ret = regmap_raw_read(state->regmap, reg, val, 2);
int ret = regmap_raw_read(state->regmap, reg, val, 2);
if (ret < 0)
dev_err(&state->client->dev, "%s(): i2c read failed %d, 0x%04x\n",
__func__, ret, reg);
@@ -546,9 +553,7 @@ static int ds5_read(struct ds5 *state, u16 reg, u16 *val)

static int ds5_raw_read(struct ds5 *state, u16 reg, void *val, size_t val_len)
{
int ret;

ret = regmap_raw_read(state->regmap, reg, val, val_len);
int ret = regmap_raw_read(state->regmap, reg, val, val_len);
if (ret < 0)
dev_err(&state->client->dev, "%s(): i2c read failed %d, 0x%04x\n",
__func__, ret, reg);
@@ -600,6 +605,14 @@ static void set_sub_stream_vc_id(int index, u32 vc_id)
d4xx_query_sub_stream[index] |= val << 56;
}

static int get_sub_stream_vc_id(int index)
{
s64 val = 0;
val = d4xx_query_sub_stream[index] >> 56;
val &= 0xFF;
return (int)val;
}

static u8 d4xx_set_sub_stream[] = {
0, 0, 0, 0, 0, 0
};
@@ -936,7 +949,20 @@ static const struct v4l2_mbus_framefmt ds5_mbus_framefmt_template = {
.xfer_func = V4L2_XFER_FUNC_DEFAULT,
};

/* This is needed for .get_fmt() and if streaming is started without .set_fmt() */
/* Get readable sensor name */
static const char *ds5_get_sensor_name(struct ds5 *state)
{
static const char *sensor_name[] = {"unknown", "RGB", "DEPTH", "Y8", "IMU"};
int sensor_id = state->is_rgb * 1 + state->is_depth * 2 + \
state->is_y8 * 3 + state->is_imu * 4;
if (sensor_id > (sizeof(sensor_name)/sizeof(*sensor_name)))
sensor_id = 0;

return sensor_name[sensor_id];
}

/* This is needed for .get_fmt()
* and if streaming is started without .set_fmt() */
static void ds5_sensor_format_init(struct ds5_sensor *sensor)
{
const struct ds5_format *fmt;
@@ -1005,11 +1031,8 @@ static int ds5_sensor_enum_frame_size(struct v4l2_subdev *sd,
const struct ds5_format *fmt;
unsigned int i;

dev_info(sensor->sd.dev, "%s(): sensor %s \n", __func__, sensor->sd.name);
dev_info(sensor->sd.dev, "%s(): state->is_rgb %d\n", __func__, state->is_rgb);
dev_info(sensor->sd.dev, "%s(): state->is_depth %d\n", __func__, state->is_depth);
dev_info(sensor->sd.dev, "%s(): state->is_y8 %d\n", __func__, state->is_y8);
dev_info(sensor->sd.dev, "%s(): state->is_imu %d\n", __func__, state->is_imu);
dev_info(sensor->sd.dev, "%s(): sensor %s is %s\n",
__func__, sensor->sd.name, ds5_get_sensor_name(state));

for (i = 0, fmt = sensor->formats; i < sensor->n_formats; i++, fmt++)
if (fse->code == fmt->mbus_code)
@@ -1095,16 +1118,18 @@ static int ds5_sensor_get_fmt(struct v4l2_subdev *sd,

mutex_unlock(&state->lock);

dev_info(sd->dev, "%s(): pad %x, code %x, res %ux%u\n", __func__, fmt->pad, fmt->format.code,
fmt->format.width, fmt->format.height);
dev_info(sd->dev, "%s(): pad %x, code %x, res %ux%u\n",
__func__, fmt->pad, fmt->format.code,
fmt->format.width, fmt->format.height);

return 0;
}

/* Called with lock held */
static const struct ds5_format *ds5_sensor_find_format(struct ds5_sensor *sensor,
struct v4l2_mbus_framefmt *ffmt,
const struct ds5_resolution **best)
static const struct ds5_format *ds5_sensor_find_format(
struct ds5_sensor *sensor,
struct v4l2_mbus_framefmt *ffmt,
const struct ds5_resolution **best)
{
const struct ds5_resolution *res;
const struct ds5_format *fmt;
@@ -1115,16 +1140,16 @@ static const struct ds5_format *ds5_sensor_find_format(struct ds5_sensor *sensor
if (fmt->mbus_code == ffmt->code)
break;
}
dev_info(sensor->sd.dev, "%s(): mbus_code = %x, code = %x \n", __func__,
fmt->mbus_code, ffmt->code);
dev_info(sensor->sd.dev, "%s(): mbus_code = %x, code = %x \n",
__func__, fmt->mbus_code, ffmt->code);

if (i == sensor->n_formats)
/* Not found, use default */
fmt = sensor->formats;

for (i = 0, res = fmt->resolutions; i < fmt->n_resolutions; i++, res++) {
unsigned long delta = abs(ffmt->width * ffmt->height -
res->width * res->height);
res->width * res->height);
if (delta < best_delta) {
best_delta = delta;
*best = res;
@@ -1208,10 +1233,9 @@ static int __ds5_sensor_set_fmt(struct ds5 *state, struct ds5_sensor *sensor,
struct v4l2_mbus_framefmt *mf;// = &fmt->format;
//unsigned r;

dev_info(sensor->sd.dev, "%s(): state %p\n", __func__, state);
dev_info(sensor->sd.dev, "%s(): sensor %p\n", __func__, sensor);
dev_info(sensor->sd.dev, "%s(): fmt %p\n", __func__, fmt);
dev_info(sensor->sd.dev, "%s(): fmt->format %p\n", __func__, &fmt->format);
dev_dbg(sensor->sd.dev, "%s(): state %p, "
"sensor %p, fmt %p, fmt->format %p\n",
__func__, state, sensor, fmt, &fmt->format);

mf = &fmt->format;

@@ -1628,6 +1652,43 @@ static const struct v4l2_subdev_pad_ops ds5_depth_pad_ops = {
.set_fmt = ds5_sensor_set_fmt,
};

static int ds5_sensor_g_frame_interval(struct v4l2_subdev *sd,
struct v4l2_subdev_frame_interval *fi)
{
struct ds5_sensor *sensor = container_of(sd, struct ds5_sensor, sd);

if (NULL == sd || NULL == fi)
return -EINVAL;

fi->interval.numerator = 1;
fi->interval.denominator = sensor->config.framerate;

dev_info(sd->dev, "%s(): %s %u\n", __func__, sd->name,
fi->interval.denominator);

return 0;
}
static u16 __ds5_probe_framerate(const struct ds5_resolution *res, u16 target);

static int ds5_sensor_s_frame_interval(struct v4l2_subdev *sd,
struct v4l2_subdev_frame_interval *fi)
{
struct ds5_sensor *sensor = container_of(sd, struct ds5_sensor, sd);
u16 framerate = 1;
if (NULL == sd || NULL == fi || fi->interval.numerator == 0)
return -EINVAL;

framerate = fi->interval.denominator / fi->interval.numerator;
framerate = __ds5_probe_framerate(sensor->config.resolution, framerate);
sensor->config.framerate = framerate;
fi->interval.numerator = 1;
fi->interval.denominator = framerate;

dev_info(sd->dev, "%s(): %s %u\n", __func__, sd->name, framerate);

return 0;
}

static int ds5_sensor_s_stream(struct v4l2_subdev *sd, int on)
{
struct ds5_sensor *sensor = container_of(sd, struct ds5_sensor, sd);
@@ -1640,6 +1701,8 @@ static int ds5_sensor_s_stream(struct v4l2_subdev *sd, int on)
}

static const struct v4l2_subdev_video_ops ds5_sensor_video_ops = {
.g_frame_interval = ds5_sensor_g_frame_interval,
.s_frame_interval = ds5_sensor_s_frame_interval,
.s_stream = ds5_sensor_s_stream,
};

@@ -1691,10 +1754,10 @@ static const struct v4l2_subdev_ops ds5_imu_subdev_ops = {
.video = &ds5_sensor_video_ops,
};

static int ds5_hw_set_auto_exposure(struct ds5 *state, u32 base, u32 val)
static int ds5_hw_set_auto_exposure(struct ds5 *state, u32 base, s32 val)
{
if (val != V4L2_EXPOSURE_APERTURE_PRIORITY &&
val != V4L2_EXPOSURE_MANUAL)
val != V4L2_EXPOSURE_MANUAL)
return -EINVAL;

/*
@@ -1714,15 +1777,15 @@ static int ds5_hw_set_auto_exposure(struct ds5 *state, u32 base, u32 val)
val = 0;
}

return ds5_write(state, base | DS5_AUTO_EXPOSURE_MODE, val);
return ds5_write(state, base | DS5_AUTO_EXPOSURE_MODE, (u16)val);
}

/*
* Manual exposure in us
* Depth/Y8: between 100 and 200000 (200ms)
* Color: between 100 and 1000000 (1s)
*/
static int ds5_hw_set_exposure(struct ds5 *state, u32 base, u32 val)
static int ds5_hw_set_exposure(struct ds5 *state, u32 base, s32 val)
{
int ret;

@@ -1738,13 +1801,13 @@ static int ds5_hw_set_exposure(struct ds5 *state, u32 base, u32 val)
* Color: 1 is 100 us
* Depth: 1 is 1 us
*/
if (!state->is_rgb)
val *= 100;
// if (!state->is_rgb)
// val *= 100;

ret = ds5_write(state, base | DS5_MANUAL_EXPOSURE_MSB, val >> 16);
ret = ds5_write(state, base | DS5_MANUAL_EXPOSURE_MSB, (u16)(val >> 16));
if (!ret)
ret = ds5_write(state, base | DS5_MANUAL_EXPOSURE_LSB,
val & 0xffff);
(u16)(val & 0xffff));

return ret;
}
@@ -1773,32 +1836,41 @@ static int ds5_hw_set_exposure(struct ds5 *state, u32 base, u32 val)
#define DS5_CAMERA_CID_EWB (DS5_CAMERA_CID_BASE+14)
#define DS5_CAMERA_CID_HWMC (DS5_CAMERA_CID_BASE+15)

#define DS5_CAMERA_CID_PWM (DS5_CAMERA_CID_BASE+22)

/* the HWMC will remain for legacy tools compatibility,
* HWMC_RW used for UVC compatibility*/
#define DS5_CAMERA_CID_HWMC_RW (DS5_CAMERA_CID_BASE+32)

static int ds5_send_hwmc(struct ds5 *state, u16 cmdLen, struct hwm_cmd *cmd,
bool isRead, u16 *dataLen)
{
int ret = 0;
u16 status = 2;
int retries = 20;
int retries = 100;
int errorCode;
int iter = retries;

dev_info(&state->client->dev,
"%s(): HWMC header: 0x%x, magic: 0x%x, opcode: 0x%x, param1: %d, param2: %d, param3: %d, param4: %d\n",
__func__,
cmd->header, cmd->magic_word, cmd->opcode, cmd->param1, cmd->param2, cmd->param3, cmd->param4);
dev_dbg(&state->client->dev,
"%s(): HWMC header: 0x%x, magic: 0x%x, opcode: 0x%x, "
"param1: %d, param2: %d, param3: %d, param4: %d\n",
__func__, cmd->header, cmd->magic_word, cmd->opcode,
cmd->param1, cmd->param2, cmd->param3, cmd->param4);

ds5_raw_write_with_check(state, 0x4900, cmd, cmdLen);

ds5_write_with_check(state, 0x490C, 0x01); /* execute cmd */
do {
if (retries != 5)
msleep_range(50);
if (iter != retries)
msleep_range(10);
ret = ds5_read(state, 0x4904, &status);
} while (retries-- && status != 0);
} while (iter-- && status != 0);

if (ret || status != 0) {
ds5_raw_read(state, 0x4900, &errorCode, 4);
dev_err(&state->client->dev, "%s(): HWMC failed, ret: %d, status: %x, error code: %d\n",
__func__, ret, status, errorCode);
dev_err(&state->client->dev,
"%s(): HWMC failed, ret: %d, status: %x, error code: %d\n",
__func__, ret, status, errorCode);
ret = -EAGAIN;
}

@@ -1809,8 +1881,8 @@ static int ds5_send_hwmc(struct ds5 *state, u16 cmdLen, struct hwm_cmd *cmd,
return -EAGAIN;
}

dev_err(&state->client->dev, "%s(): HWMC read len: %d\n",
__func__, *dataLen);
dev_dbg(&state->client->dev, "%s(): HWMC read len: %d\n",
__func__, *dataLen);
// First 4 bytes of cmd->Data after read will include opcode
ds5_raw_read_with_check(state, 0x4900, cmd->Data, *dataLen);

@@ -1822,7 +1894,74 @@ static int ds5_send_hwmc(struct ds5 *state, u16 cmdLen, struct hwm_cmd *cmd,
return 0;
}

static int ds5_set_calibration_data(struct ds5 *state, struct hwm_cmd *cmd, u16 length)
#define DS5_HWMC_DATA 0x4900
#define DS5_HWMC_STATUS 0x4904
#define DS5_HWMC_RESP_LEN 0x4908
#define DS5_HWMC_EXEC 0x490C

#define DS5_HWMC_STATUS_OK 0
#define DS5_HWMC_STATUS_ERR 1
#define DS5_HWMC_STATUS_WIP 2
#define DS5_HWMC_BUFFER_SIZE 1024

static int ds5_get_hwmc(struct ds5 *state, unsigned char *data)
{
int ret = 0;
u16 status = DS5_HWMC_STATUS_WIP;
int retries = 100;
int errorCode;
u16 tmp_len = 0;
const int SIZE_OF_HW_MONITOR_HEADER = 4;

memset(data, 0, DS5_HWMC_BUFFER_SIZE);

do {
if (retries != 100)
msleep_range(1);
ret = ds5_read(state, DS5_HWMC_STATUS, &status);
} while (!ret && retries-- && status != DS5_HWMC_STATUS_OK);

if (ret || status != DS5_HWMC_STATUS_OK) {
if (status == DS5_HWMC_STATUS_ERR) {
ds5_raw_read(state, DS5_HWMC_DATA, &errorCode, sizeof(errorCode));
dev_err(&state->client->dev,
"%s(): HWMC failed, ret: %d, error code: %d\n",
__func__, ret, errorCode);
} else {
dev_err(&state->client->dev,
"%s(): HWMC failed because of timeout, ret: %d\n",
__func__, ret);
}
return -EAGAIN;
}

ret = regmap_raw_read(state->regmap, DS5_HWMC_RESP_LEN,
&tmp_len, sizeof(tmp_len));
if (ret)
return -EAGAIN;

if (tmp_len > DS5_HWMC_BUFFER_SIZE)
return -ENOBUFS;

dev_dbg(&state->client->dev,
"%s(): HWMC read len: %d, lrs_len: %d\n",
__func__, tmp_len, tmp_len - SIZE_OF_HW_MONITOR_HEADER);

ds5_raw_read_with_check(state, DS5_HWMC_DATA, data, tmp_len);

/* This is needed for librealsense, to align there code with UVC,
* last word is length - 4 bytes header length */
tmp_len -= SIZE_OF_HW_MONITOR_HEADER;
data[DS5_HWMC_BUFFER_SIZE - 4] = (unsigned char)(tmp_len & 0x00FF);
data[DS5_HWMC_BUFFER_SIZE - 3] = (unsigned char)((tmp_len & 0xFF00) >> 8);
data[DS5_HWMC_BUFFER_SIZE - 2] = 0;
data[DS5_HWMC_BUFFER_SIZE - 1] = 0;

return 0;
}

static int ds5_set_calibration_data(struct ds5 *state,
struct hwm_cmd *cmd, u16 length)
{
int ret;
int retries = 10;
@@ -1838,30 +1977,107 @@ static int ds5_set_calibration_data(struct ds5 *state, struct hwm_cmd *cmd, u16
} while (retries-- && status != 0);

if (ret || status != 0) {
dev_err(&state->client->dev, "%s(): Failed to set calibration table %d, ret: %d, fw error: %x\n",
dev_err(&state->client->dev,
"%s(): Failed to set calibration table %d,"
"ret: %d, fw error: %x\n",
__func__, cmd->param1, ret, status);
}

return -EINVAL;
}

static int ds5_s_state(struct ds5 *state, int vc)
{
int ret = 0;
dev_dbg(&state->client->dev, "%s(): set state for vc: %d\n", __func__, vc);

switch (vc)
{
case 0:
state->is_depth = 1;
state->is_rgb = 0;
state->is_y8 = 0;
state->is_imu = 0;
break;
case 1:
state->is_depth = 0;
state->is_rgb = 1;
state->is_y8 = 0;
state->is_imu = 0;
break;
case 2:
state->is_depth = 0;
state->is_rgb = 0;
state->is_y8 = 1;
state->is_imu = 0;
break;
case 3:
state->is_depth = 0;
state->is_rgb = 0;
state->is_y8 = 0;
state->is_imu = 1;
break;
default:
dev_warn(&state->client->dev, "%s(): unknown vc: %d\n", __func__, vc);
ret = -EINVAL;
break;
}
return ret;
}

static int ds5_s_ctrl(struct v4l2_ctrl *ctrl)
{
struct ds5 *state = container_of(ctrl->handler, struct ds5,
ctrls.handler);
struct v4l2_subdev *sd = &state->mux.sd.subdev;
struct ds5_sensor *sensor = (struct ds5_sensor *)ctrl->priv;
int ret = -EINVAL;
u16 base = DS5_DEPTH_CONTROL_BASE;
u32 val;
u16 on;
u16 vc_id;

if (sensor) {
switch (sensor->mux_pad) {
case DS5_MUX_PAD_DEPTH_A:
state = container_of(ctrl->handler, struct ds5, ctrls.handler_depth);
state->is_rgb = 0;
state->is_depth = 1;
state->is_y8 = 0;
state->is_imu = 0;
break;
case DS5_MUX_PAD_RGB_A:
state = container_of(ctrl->handler, struct ds5, ctrls.handler_rgb);
state->is_rgb = 1;
state->is_depth = 0;
state->is_y8 = 0;
state->is_imu = 0;
break;
case DS5_MUX_PAD_MOTION_T_A:
state = container_of(ctrl->handler, struct ds5, ctrls.handler_y8);
state->is_rgb = 0;
state->is_depth = 0;
state->is_y8 = 1;
state->is_imu = 0;
break;
default:
state->is_rgb = 0;
state->is_depth = 0;
state->is_y8 = 0;
state->is_imu = 1;
break;

}
}

if (state->is_rgb)
base = DS5_RGB_CONTROL_BASE;
else if (state->is_imu)
return ret;

v4l2_dbg(1, 1, sd, "ctrl: %s, value: %d\n", ctrl->name, ctrl->val);
dev_dbg(&state->client->dev, "%s(): %s - ctrl: %s, value: %d\n",
__func__, ds5_get_sensor_name(state), ctrl->name, ctrl->val);

mutex_lock(&state->lock);

@@ -1890,56 +2106,93 @@ static int ds5_s_ctrl(struct v4l2_ctrl *ctrl)
case DS5_CAMERA_DEPTH_CALIBRATION_TABLE_SET: {
struct hwm_cmd *calib_cmd;

dev_info(&state->client->dev, "%s(): DS5_CAMERA_DEPTH_CALIBRATION_TABLE_SET \n", __func__);
dev_info(&state->client->dev, "%s(): table id: 0x%x\n", __func__, *((u8 *)ctrl->p_new.p + 2));
if (ctrl->p_new.p && DEPTH_CALIBRATION_ID == *((u8 *)ctrl->p_new.p + 2)) {
calib_cmd = devm_kzalloc(&state->client->dev, sizeof(struct hwm_cmd) + 256, GFP_KERNEL);
memcpy(calib_cmd, &set_calib_data, sizeof(set_calib_data));
dev_dbg(&state->client->dev,
"%s(): DS5_CAMERA_DEPTH_CALIBRATION_TABLE_SET \n",
__func__);
dev_dbg(&state->client->dev,
"%s(): table id: 0x%x\n",
__func__, *((u8*)ctrl->p_new.p + 2));
if (ctrl->p_new.p && DEPTH_CALIBRATION_ID ==
*((u8*)ctrl->p_new.p + 2)) {
calib_cmd = devm_kzalloc(&state->client->dev,
sizeof(struct hwm_cmd) + 256, GFP_KERNEL);
if (!calib_cmd) {
dev_err(&state->client->dev,
"%s(): Can't allocate memory for 0x%x\n",
__func__, ctrl->id);
ret = -ENOMEM;
break;
}
memcpy(calib_cmd, &set_calib_data, sizeof (set_calib_data));
calib_cmd->header = 276;
calib_cmd->param1 = DEPTH_CALIBRATION_ID;
memcpy(calib_cmd->Data, (u8 *)ctrl->p_new.p, 256);
ret = ds5_set_calibration_data(state, calib_cmd, sizeof(struct hwm_cmd) + 256);
memcpy(calib_cmd->Data, (u8*)ctrl->p_new.p , 256);
ret = ds5_set_calibration_data(state, calib_cmd,
sizeof(struct hwm_cmd) + 256);
devm_kfree(&state->client->dev, calib_cmd);
}
break;
}
case DS5_CAMERA_COEFF_CALIBRATION_TABLE_SET: {
struct hwm_cmd *calib_cmd;

dev_info(&state->client->dev, "%s(): DS5_CAMERA_COEFF_CALIBRATION_TABLE_SET \n", __func__);
dev_info(&state->client->dev, "%s(): table id %d\n", __func__, *((u8 *)ctrl->p_new.p + 2));
if (ctrl->p_new.p && COEF_CALIBRATION_ID == *((u8 *)ctrl->p_new.p + 2)) {
calib_cmd = devm_kzalloc(&state->client->dev, sizeof(struct hwm_cmd) + 512, GFP_KERNEL);
memcpy(calib_cmd, &set_calib_data, sizeof(set_calib_data));
dev_dbg(&state->client->dev,
"%s(): DS5_CAMERA_COEFF_CALIBRATION_TABLE_SET \n",
__func__);
dev_dbg(&state->client->dev,
"%s(): table id %d\n",
__func__, *((u8*)ctrl->p_new.p + 2));
if (ctrl->p_new.p && COEF_CALIBRATION_ID ==
*((u8*)ctrl->p_new.p + 2)) {
calib_cmd = devm_kzalloc(&state->client->dev,
sizeof(struct hwm_cmd) + 512, GFP_KERNEL);
if (!calib_cmd) {
dev_err(&state->client->dev,
"%s(): Can't allocate memory for 0x%x\n",
__func__, ctrl->id);
ret = -ENOMEM;
break;
}
memcpy(calib_cmd, &set_calib_data, sizeof (set_calib_data));
calib_cmd->header = 532;
calib_cmd->param1 = COEF_CALIBRATION_ID;
memcpy(calib_cmd->Data, (u8 *)ctrl->p_new.p, 512);
ret = ds5_set_calibration_data(state, calib_cmd, sizeof(struct hwm_cmd) + 512);
memcpy(calib_cmd->Data, (u8*)ctrl->p_new.p , 512);
ret = ds5_set_calibration_data(state, calib_cmd,
sizeof(struct hwm_cmd) + 512);
devm_kfree(&state->client->dev, calib_cmd);
}

}
break;
case DS5_CAMERA_CID_AE_ROI_SET: {
struct hwm_cmd ae_roi_cmd;

memcpy(&ae_roi_cmd, &set_ae_roi, sizeof(ae_roi_cmd));
ae_roi_cmd.param1 = *((u16 *)ctrl->p_new.p_u16);
ae_roi_cmd.param2 = *((u16 *)ctrl->p_new.p_u16 + 1);
ae_roi_cmd.param3 = *((u16 *)ctrl->p_new.p_u16 + 2);
ae_roi_cmd.param4 = *((u16 *)ctrl->p_new.p_u16 + 3);
ret = ds5_send_hwmc(state, sizeof(struct hwm_cmd), &ae_roi_cmd, false, NULL);
ae_roi_cmd.param1 = *((u16*)ctrl->p_new.p_u16);
ae_roi_cmd.param2 = *((u16*)ctrl->p_new.p_u16 + 1);
ae_roi_cmd.param3 = *((u16*)ctrl->p_new.p_u16 + 2);
ae_roi_cmd.param4 = *((u16*)ctrl->p_new.p_u16 + 3);
ret = ds5_send_hwmc(state, sizeof(struct hwm_cmd),
&ae_roi_cmd, false, NULL);
break;
}
case DS5_CAMERA_CID_AE_SETPOINT_SET: {
struct hwm_cmd *ae_setpoint_cmd;

if (ctrl->p_new.p_s32) {
dev_err(&state->client->dev, "%s():0x%x \n", __func__,
*(ctrl->p_new.p_s32));
ae_setpoint_cmd = devm_kzalloc(&state->client->dev, sizeof(struct hwm_cmd) + 4, GFP_KERNEL);
memcpy(ae_setpoint_cmd, &set_ae_setpoint, sizeof(set_ae_setpoint));
memcpy(ae_setpoint_cmd->Data, (u8 *)ctrl->p_new.p_s32, 4);
ret = ds5_send_hwmc(state, sizeof(struct hwm_cmd) + 4, ae_setpoint_cmd, false, NULL);
dev_dbg(&state->client->dev, "%s():0x%x \n",
__func__, *(ctrl->p_new.p_s32));
ae_setpoint_cmd = devm_kzalloc(&state->client->dev,
sizeof(struct hwm_cmd) + 4, GFP_KERNEL);
if (!ae_setpoint_cmd) {
dev_err(&state->client->dev,
"%s(): Can't allocate memory for 0x%x\n",
__func__, ctrl->id);
ret = -ENOMEM;
break;
}
memcpy(ae_setpoint_cmd, &set_ae_setpoint, sizeof (set_ae_setpoint));
memcpy(ae_setpoint_cmd->Data, (u8*)ctrl->p_new.p_s32 , 4);
ret = ds5_send_hwmc(state, sizeof(struct hwm_cmd) + 4,
ae_setpoint_cmd, false, NULL);
devm_kfree(&state->client->dev, ae_setpoint_cmd);
}
break;
@@ -1955,18 +2208,29 @@ static int ds5_s_ctrl(struct v4l2_ctrl *ctrl)
size = *(ctrl->p_new.p_u8 + 2) << 8;
size |= *(ctrl->p_new.p_u8 + 3);

dev_err(&state->client->dev, "%s(): offset %x, size: %x\n",
dev_dbg(&state->client->dev, "%s(): offset %x, size: %x\n",
__func__, offset, size);

erb_cmd = devm_kzalloc(&state->client->dev, sizeof(struct hwm_cmd) + size, GFP_KERNEL);
erb_cmd = devm_kzalloc(&state->client->dev,
sizeof(struct hwm_cmd) + size, GFP_KERNEL);
if (!erb_cmd) {
dev_err(&state->client->dev,
"%s(): Can't allocate memory for 0x%x\n",
__func__, ctrl->id);
ret = -ENOMEM;
break;
}
memcpy(erb_cmd, &erb, sizeof(struct hwm_cmd));
erb_cmd->param1 = offset;
erb_cmd->param2 = size;
ret = ds5_send_hwmc(state, sizeof(struct hwm_cmd), erb_cmd, true, &size);
ret = ds5_send_hwmc(state, sizeof(struct hwm_cmd),
erb_cmd, true, &size);

if (ret) {
dev_err(&state->client->dev, "%s(): ERB cmd failed, ret: %d, requested size: %d, actual size: %d\n",
__func__, ret, erb_cmd->param2, size);
dev_err(&state->client->dev,
"%s(): ERB cmd failed, ret: %d,"
"requested size: %d, actual size: %d\n",
__func__, ret, erb_cmd->param2, size);
devm_kfree(&state->client->dev, erb_cmd);
return -EAGAIN;
}
@@ -1976,7 +2240,7 @@ static int ds5_s_ctrl(struct v4l2_ctrl *ctrl)
*(ctrl->p_new.p_u8 + 3) = (size & 0x00FF);

memcpy(ctrl->p_new.p_u8 + 4, erb_cmd->Data + 4, size - 4);
dev_info(&state->client->dev, "%s(): 0x%x 0x%x 0x%x 0x%x \n",
dev_dbg(&state->client->dev, "%s(): 0x%x 0x%x 0x%x 0x%x \n",
__func__,
*(ctrl->p_new.p_u8),
*(ctrl->p_new.p_u8+1),
@@ -1991,28 +2255,41 @@ static int ds5_s_ctrl(struct v4l2_ctrl *ctrl)
u16 size = 0;
struct hwm_cmd *ewb_cmd;

offset = *((u8 *)ctrl->p_new.p_u8) << 8;
offset |= *((u8 *)ctrl->p_new.p_u8 + 1);
size = *((u8 *)ctrl->p_new.p_u8 + 2) << 8;
size |= *((u8 *)ctrl->p_new.p_u8 + 3);
offset = *((u8*)ctrl->p_new.p_u8) << 8;
offset |= *((u8*)ctrl->p_new.p_u8 + 1);
size = *((u8*)ctrl->p_new.p_u8 + 2) << 8;
size |= *((u8*)ctrl->p_new.p_u8 + 3);

dev_err(&state->client->dev, "%s():0x%x 0x%x 0x%x 0x%x\n", __func__,
*((u8 *)ctrl->p_new.p_u8),
*((u8 *)ctrl->p_new.p_u8 + 1),
*((u8 *)ctrl->p_new.p_u8 + 2),
*((u8 *)ctrl->p_new.p_u8 + 3));

ewb_cmd = devm_kzalloc(&state->client->dev, sizeof(struct hwm_cmd) + size, GFP_KERNEL);
memcpy(ewb_cmd, &ewb, sizeof(ewb));
dev_dbg(&state->client->dev, "%s():0x%x 0x%x 0x%x 0x%x\n",
__func__,
*((u8*)ctrl->p_new.p_u8),
*((u8*)ctrl->p_new.p_u8 + 1),
*((u8*)ctrl->p_new.p_u8 + 2),
*((u8*)ctrl->p_new.p_u8 + 3));

ewb_cmd = devm_kzalloc(&state->client->dev,
sizeof(struct hwm_cmd) + size,
GFP_KERNEL);
if (!ewb_cmd) {
dev_err(&state->client->dev,
"%s(): Can't allocate memory for 0x%x\n",
__func__, ctrl->id);
ret = -ENOMEM;
break;
}
memcpy(ewb_cmd, &ewb, sizeof (ewb));
ewb_cmd->header = 0x14 + size;
ewb_cmd->param1 = offset; // start index
ewb_cmd->param2 = size; // size
memcpy(ewb_cmd->Data, (u8 *)ctrl->p_new.p_u8 + 4, size);
ret = ds5_send_hwmc(state, sizeof(struct hwm_cmd) + size, ewb_cmd, false, NULL);
memcpy(ewb_cmd->Data, (u8*)ctrl->p_new.p_u8 + 4, size);
ret = ds5_send_hwmc(state, sizeof(struct hwm_cmd) + size,
ewb_cmd, false, NULL);

if (ret) {
dev_err(&state->client->dev, "%s(): EWB cmd failed, ret: %d, requested size: %d, actual size: %d\n",
__func__, ret, ewb_cmd->param2, size);
dev_err(&state->client->dev,
"%s(): EWB cmd failed, ret: %d,"
"requested size: %d, actual size: %d\n",
__func__, ret, ewb_cmd->param2, size);
devm_kfree(&state->client->dev, ewb_cmd);
return -EAGAIN;
}
@@ -2025,17 +2302,36 @@ static int ds5_s_ctrl(struct v4l2_ctrl *ctrl)
u16 dataLen = 0;
u16 size = 0;

size = *((u8 *)ctrl->p_new.p_u8 + 1) << 8;
size |= *((u8 *)ctrl->p_new.p_u8 + 0);
dev_err(&state->client->dev, "%s(): HWMC size %d\n", __func__, size);
ret = ds5_send_hwmc(state, size + 4, (struct hwm_cmd *)ctrl->p_new.p_u8, true, &dataLen);
size = *((u8*)ctrl->p_new.p_u8 + 1) << 8;
size |= *((u8*)ctrl->p_new.p_u8 + 0);
dev_dbg(&state->client->dev,
"%s(): HWMC size %d\n",
__func__, size);
ret = ds5_send_hwmc(state, size + 4,
(struct hwm_cmd *)ctrl->p_new.p_u8, true, &dataLen);
}
break;
case DS5_CAMERA_CID_HWMC_RW:
if (ctrl->p_new.p_u8) {
u16 size = *((u8*)ctrl->p_new.p_u8 + 1) << 8;
size |= *((u8*)ctrl->p_new.p_u8 + 0);
ret = ds5_send_hwmc(state, size + 4,
(struct hwm_cmd *)ctrl->p_new.p_u8, false, NULL);
}
break;
case DS5_CAMERA_CID_PWM:
if (state->is_depth)
ret = ds5_write(state, base | DS5_PWM_FREQUENCY, ctrl->val);
break;
case V4L2_CID_IPU_SET_SUB_STREAM:
val = (*ctrl->p_new.p_s64 & 0xFFFF);
dev_info(&state->client->dev, "V4L2_CID_IPU_SET_SUB_STREAM %x\n", val);
vc_id = (val >> 8) & 0x00FF;
on = val & 0x00FF;
if (on == 0xff) {
ret = ds5_s_state(state, vc_id);
break;
}
if (vc_id > NR_OF_DS5_STREAMS - 1)
dev_err(&state->client->dev, "invalid vc %d\n", vc_id);
else
@@ -2051,15 +2347,22 @@ static int ds5_s_ctrl(struct v4l2_ctrl *ctrl)
return ret;
}

static int ds5_get_calibration_data(struct ds5 *state, enum table_id id, unsigned char *table, unsigned int length)
static int ds5_get_calibration_data(struct ds5 *state, enum table_id id,
unsigned char *table, unsigned int length)
{
struct hwm_cmd *cmd;
int ret;
int retries = 3;
u16 status = 2;
u16 table_length;

cmd = devm_kzalloc(&state->client->dev, sizeof(struct hwm_cmd) + length + 4, GFP_KERNEL);
cmd = devm_kzalloc(&state->client->dev,
sizeof(struct hwm_cmd) + length + 4, GFP_KERNEL);
if (!cmd) {
dev_err(&state->client->dev, "%s(): Can't allocate memory\n", __func__);
return -ENOMEM;
}

memcpy(cmd, &get_calib_data, sizeof(get_calib_data));
cmd->param1 = id;
ds5_raw_write_with_check(state, 0x4900, cmd, sizeof(struct hwm_cmd));
@@ -2072,13 +2375,15 @@ static int ds5_get_calibration_data(struct ds5 *state, enum table_id id, unsigne

if (ret || status != 0) {
dev_err(&state->client->dev,
"%s(): Failed to get calibration table %d, fw error: %x\n", __func__, id, status);
"%s(): Failed to get calibration table %d, fw error: %x\n",
__func__, id, status);
devm_kfree(&state->client->dev, cmd);
return status;
}

// get table length from fw
ret = regmap_raw_read(state->regmap, 0x4908, &table_length, sizeof(table_length));
ret = regmap_raw_read(state->regmap, 0x4908,
&table_length, sizeof(table_length));

// read table
ds5_raw_read_with_check(state, 0x4900, cmd->Data, table_length);
@@ -2108,7 +2413,9 @@ static int ds5_gvd(struct ds5 *state, unsigned char *data)
} while (ret && retries-- && status != 0);

if (ret || status != 0) {
dev_err(&state->client->dev, "%s(): Failed to read GVD, HWM cmd status: %x\n", __func__, status);
dev_err(&state->client->dev,
"%s(): Failed to read GVD, HWM cmd status: %x\n",
__func__, status);
return status;
}

@@ -2118,26 +2425,113 @@ static int ds5_gvd(struct ds5 *state, unsigned char *data)
return ret;
}


static int ds5_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
{
struct ds5 *state = container_of(ctrl->handler, struct ds5,
ctrls.handler);
ctrls.handler);
u16 log_prepare[] = {0x0014, 0xcdab, 0x000f, 0x0000, 0x0400, 0x0000,
0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000};
0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000};
u16 execute_cmd = 0x0001;
unsigned int i;
u32 data;
int ret = 0;
struct ds5_sensor *sensor = (struct ds5_sensor *)ctrl->priv;
u16 base = (state->is_rgb) ? DS5_RGB_CONTROL_BASE : DS5_DEPTH_CONTROL_BASE;
u16 reg;

if (sensor) {
switch (sensor->mux_pad) {
case DS5_MUX_PAD_DEPTH_A:
state = container_of(ctrl->handler, struct ds5, ctrls.handler_depth);
state->is_rgb = 0;
state->is_depth = 1;
state->is_y8 = 0;
state->is_imu = 0;
break;
case DS5_MUX_PAD_RGB_A:
state = container_of(ctrl->handler, struct ds5, ctrls.handler_rgb);
state->is_rgb = 1;
state->is_depth = 0;
state->is_y8 = 0;
state->is_imu = 0;
break;
case DS5_MUX_PAD_MOTION_T_A:
state = container_of(ctrl->handler, struct ds5, ctrls.handler_y8);
state->is_rgb = 0;
state->is_depth = 0;
state->is_y8 = 1;
state->is_imu = 0;
break;
default:
state->is_rgb = 0;
state->is_depth = 0;
state->is_y8 = 0;
state->is_imu = 1;
break;

}
}
base = (state->is_rgb) ? DS5_RGB_CONTROL_BASE : DS5_DEPTH_CONTROL_BASE;

dev_dbg(&state->client->dev, "%s(): %s - ctrl: %s \n",
__func__, ds5_get_sensor_name(state), ctrl->name);

switch (ctrl->id) {

case V4L2_CID_ANALOGUE_GAIN:
if (state->is_imu)
return -EINVAL;
ret = ds5_read(state, base | DS5_MANUAL_GAIN, ctrl->p_new.p_u16);
break;

case V4L2_CID_EXPOSURE_AUTO:
if (state->is_imu)
return -EINVAL;
ds5_read(state, base | DS5_AUTO_EXPOSURE_MODE, &reg);
*ctrl->p_new.p_u16 = reg;
/* see ds5_hw_set_auto_exposure */
if (!state->is_rgb) {
if (reg == 1)
*ctrl->p_new.p_u16 = V4L2_EXPOSURE_APERTURE_PRIORITY;
else if (reg == 0)
*ctrl->p_new.p_u16 = V4L2_EXPOSURE_MANUAL;
}

if (state->is_rgb && reg == 8)
*ctrl->p_new.p_u16 = V4L2_EXPOSURE_APERTURE_PRIORITY;

break;

case V4L2_CID_EXPOSURE_ABSOLUTE:
if (state->is_imu)
return -EINVAL;
/* see ds5_hw_set_exposure */
ds5_read(state, base | DS5_MANUAL_EXPOSURE_MSB, &reg);
data = ((u32)reg << 16) & 0xffff0000;
ds5_read(state, base | DS5_MANUAL_EXPOSURE_LSB, &reg);
data |= reg;
*ctrl->p_new.p_u32 = data;
break;

case DS5_CAMERA_CID_LASER_POWER:
if (!state->is_rgb)
ds5_read(state, base | DS5_LASER_POWER, ctrl->p_new.p_u16);
break;

case DS5_CAMERA_CID_MANUAL_LASER_POWER:
if (!state->is_rgb)
ds5_read(state, base | DS5_MANUAL_LASER_POWER, ctrl->p_new.p_u16);
break;

case DS5_CAMERA_CID_LOG:
// TODO: wrap HWMonitor command
// 1. prepare and send command
// 2. send command
// 3. execute command
// 4. wait for ccompletion
ret = regmap_raw_write(state->regmap, 0x4900,
log_prepare, sizeof(log_prepare));
log_prepare, sizeof(log_prepare));
if (ret < 0)
return ret;

@@ -2148,8 +2542,8 @@ static int ds5_g_volatile_ctrl(struct v4l2_ctrl *ctrl)

for (i = 0; i < DS5_MAX_LOG_POLL; i++) {
ret = regmap_raw_read(state->regmap, 0x4904,
&data, sizeof(data));
dev_info(&state->client->dev, "%s(): log ready 0x%x\n",
&data, sizeof(data));
dev_dbg(&state->client->dev, "%s(): log ready 0x%x\n",
__func__, data);
if (ret < 0)
return ret;
@@ -2162,8 +2556,8 @@ static int ds5_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
// return -ETIMEDOUT;

ret = regmap_raw_read(state->regmap, 0x4908,
&data, sizeof(data));
dev_info(&state->client->dev, "%s(): log size 0x%x\n",
&data, sizeof(data));
dev_dbg(&state->client->dev, "%s(): log size 0x%x\n",
__func__, data);
if (ret < 0)
return ret;
@@ -2175,12 +2569,16 @@ static int ds5_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
ctrl->p_new.p_u8, data);
break;
case DS5_CAMERA_DEPTH_CALIBRATION_TABLE_GET:
ret = ds5_get_calibration_data(state, DEPTH_CALIBRATION_ID, ctrl->p_new.p_u8, 256);
ret = ds5_get_calibration_data(state, DEPTH_CALIBRATION_ID,
ctrl->p_new.p_u8, 256);
break;
case DS5_CAMERA_COEFF_CALIBRATION_TABLE_GET:
ret = ds5_get_calibration_data(state, COEF_CALIBRATION_ID, ctrl->p_new.p_u8, 512);
ret = ds5_get_calibration_data(state, COEF_CALIBRATION_ID,
ctrl->p_new.p_u8, 512);
break;
case DS5_CAMERA_CID_FW_VERSION:
ret = ds5_read(state, DS5_FW_VERSION, &state->fw_version);
ret = ds5_read(state, DS5_FW_BUILD, &state->fw_build);
*ctrl->p_new.p_u32 = state->fw_version << 16;
*ctrl->p_new.p_u32 |= state->fw_build;
break;
@@ -2190,28 +2588,63 @@ static int ds5_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
case DS5_CAMERA_CID_AE_ROI_GET: {
u16 len = 0;
struct hwm_cmd *ae_roi_cmd;

ae_roi_cmd = devm_kzalloc(&state->client->dev, sizeof(struct hwm_cmd) + 12, GFP_KERNEL);
ae_roi_cmd = devm_kzalloc(&state->client->dev,
sizeof(struct hwm_cmd) + 12, GFP_KERNEL);
if (!ae_roi_cmd) {
dev_err(&state->client->dev,
"%s(): Can't allocate memory for 0x%x\n",
__func__, ctrl->id);
ret = -ENOMEM;
break;
}
memcpy(ae_roi_cmd, &get_ae_roi, sizeof(struct hwm_cmd));
ret = ds5_send_hwmc(state, sizeof(struct hwm_cmd), ae_roi_cmd, true, &len);
ret = ds5_send_hwmc(state, sizeof(struct hwm_cmd),
ae_roi_cmd, true, &len);
memcpy(ctrl->p_new.p_u16, ae_roi_cmd->Data + 4, 8);
devm_kfree(&state->client->dev, ae_roi_cmd);
}
break;
case DS5_CAMERA_CID_AE_SETPOINT_GET: {
u16 len = 0;
struct hwm_cmd *ae_setpoint_cmd;

ae_setpoint_cmd = devm_kzalloc(&state->client->dev, sizeof(struct hwm_cmd) + 8, GFP_KERNEL);
ae_setpoint_cmd = devm_kzalloc(&state->client->dev,
sizeof(struct hwm_cmd) + 8, GFP_KERNEL);
if (!ae_setpoint_cmd) {
dev_err(&state->client->dev,
"%s(): Can't allocate memory for 0x%x\n",
__func__, ctrl->id);
ret = -ENOMEM;
break;
}
memcpy(ae_setpoint_cmd, &get_ae_setpoint, sizeof(struct hwm_cmd));
ret = ds5_send_hwmc(state, sizeof(struct hwm_cmd), ae_setpoint_cmd, true, &len);
ret = ds5_send_hwmc(state, sizeof(struct hwm_cmd),
ae_setpoint_cmd, true, &len);
memcpy(ctrl->p_new.p_s32, ae_setpoint_cmd->Data + 4, 4);
dev_info(&state->client->dev, "%s(): 0x%x \n",
dev_dbg(&state->client->dev, "%s(): 0x%x \n",
__func__,
*(ctrl->p_new.p_s32));
devm_kfree(&state->client->dev, ae_setpoint_cmd);
}
break;
case DS5_CAMERA_CID_HWMC_RW:
ds5_get_hwmc(state, ctrl->p_new.p_u8);
break;
case DS5_CAMERA_CID_PWM:
if (state->is_depth)
ds5_read(state, base | DS5_PWM_FREQUENCY, ctrl->p_new.p_u16);
break;
case V4L2_CID_IPU_QUERY_SUB_STREAM: {
if (sensor) {
int vc_id = get_sub_stream_vc_id(pad_to_substream[sensor->mux_pad]);
dev_info(sensor->sd.dev, "%s(): V4L2_CID_IPU_QUERY_SUB_STREAM sensor->mux_pad:%d vc:[%d]\n", __func__, sensor->mux_pad, vc_id);
*ctrl->p_new.p_s32 = pad_to_substream[sensor->mux_pad];
}
else {
/* we are in DS5 MUX case */
*ctrl->p_new.p_s32 = -1;
}
}
break;
}

return ret;
@@ -2242,6 +2675,7 @@ static const struct v4l2_ctrl_config ds5_ctrl_laser_power = {
.max = 1,
.step = 1,
.def = 1,
.flags = V4L2_CTRL_FLAG_VOLATILE | V4L2_CTRL_FLAG_EXECUTE_ON_WRITE,
};

static const struct v4l2_ctrl_config ds5_ctrl_manual_laser_power = {
@@ -2252,7 +2686,8 @@ static const struct v4l2_ctrl_config ds5_ctrl_manual_laser_power = {
.min = 0,
.max = 360,
.step = 30,
.def = 240,
.def = 150,
.flags = V4L2_CTRL_FLAG_VOLATILE | V4L2_CTRL_FLAG_EXECUTE_ON_WRITE,
};

static const struct v4l2_ctrl_config ds5_ctrl_fw_version = {
@@ -2402,13 +2837,39 @@ static const struct v4l2_ctrl_config ds5_ctrl_hwmc = {
.id = DS5_CAMERA_CID_HWMC,
.name = "HWMC",
.type = V4L2_CTRL_TYPE_U8,
.dims = {1028},
.dims = {DS5_HWMC_BUFFER_SIZE + 4},
.elem_size = sizeof(u8),
.min = 0,
.max = 0xFFFFFFFF,
.def = 240,
.step = 1,
.step = 1,
};

static const struct v4l2_ctrl_config ds5_ctrl_hwmc_rw = {
.ops = &ds5_ctrl_ops,
.id = DS5_CAMERA_CID_HWMC_RW,
.name = "HWMC_RW",
.type = V4L2_CTRL_TYPE_U8,
.dims = {DS5_HWMC_BUFFER_SIZE},
.elem_size = sizeof(u8),
.min = 0,
.max = 0xFFFFFFFF,
.def = 240,
.step = 1,
.flags = V4L2_CTRL_FLAG_VOLATILE | V4L2_CTRL_FLAG_EXECUTE_ON_WRITE,
};

static const struct v4l2_ctrl_config ds5_ctrl_pwm = {
.ops = &ds5_ctrl_ops,
.id = DS5_CAMERA_CID_PWM,
.name = "PWM Frequency Selector",
.type = V4L2_CTRL_TYPE_INTEGER,
.min = 0,
.max = 1,
.step = 1,
.def = 1,
.flags = V4L2_CTRL_FLAG_VOLATILE | V4L2_CTRL_FLAG_EXECUTE_ON_WRITE,
};

static const struct v4l2_ctrl_config d4xx_controls_link_freq = {
@@ -2453,7 +2914,6 @@ static int ds5_mux_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
dev_info(sd->dev, "%s(): %s (%p)\n", __func__, sd->name, fh);
if (state->dfu_dev.dfu_state_flag)
return -EBUSY;
try_module_get(THIS_MODULE);
state->dfu_dev.device_open_count++;

return 0;
@@ -2465,7 +2925,6 @@ static int ds5_mux_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)

dev_info(sd->dev, "%s(): %s (%p)\n", __func__, sd->name, fh);
state->dfu_dev.device_open_count--;
module_put(THIS_MODULE);
return 0;
};

@@ -2474,74 +2933,172 @@ static const struct v4l2_subdev_internal_ops ds5_sensor_internal_ops = {
.close = ds5_mux_close,
};

static int ds5_ctrl_init(struct ds5 *state)
static int ds5_ctrl_init(struct ds5 *state, int sid)
{
const struct v4l2_ctrl_ops *ops = &ds5_ctrl_ops;
struct ds5_ctrls *ctrls = &state->ctrls;
struct v4l2_ctrl_handler *hdl = &ctrls->handler;
struct v4l2_subdev *sd = &state->mux.sd.subdev;
int ret;
struct ds5_sensor *sensor = NULL;

switch( sid ) {
case 0:
hdl = &ctrls->handler_depth;
sensor = &state->depth.sensor;
break;
case 1:
hdl = &ctrls->handler_rgb;
sensor = &state->rgb.sensor;
break;
case 2:
hdl = &ctrls->handler_y8;
sensor = &state->motion_t.sensor;
break;
default:
hdl = &ctrls->handler;
sensor = NULL;
break;
}

dev_info(NULL, "%s(), line %d sid: %d\n", __func__, __LINE__, sid);
ret = v4l2_ctrl_handler_init(hdl, DS5_N_CONTROLS);
if (ret < 0) {
v4l2_err(sd, "cannot init ctrl handler (%d)\n", ret);
return ret;
}

ctrls->log = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_log, NULL);
ctrls->fw_version = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_fw_version, NULL);
ctrls->gvd = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_gvd, NULL);
ctrls->get_depth_calib = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_get_depth_calib, NULL);
ctrls->set_depth_calib = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_set_depth_calib, NULL);
ctrls->get_coeff_calib = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_get_coeff_calib, NULL);
ctrls->set_coeff_calib = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_set_coeff_calib, NULL);
ctrls->ae_roi_get = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_ae_roi_get, NULL);
ctrls->ae_roi_set = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_ae_roi_set, NULL);
ctrls->ae_setpoint_get = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_ae_setpoint_get, NULL);
ctrls->ae_setpoint_set = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_ae_setpoint_set, NULL);
ctrls->erb = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_erb, NULL);
ctrls->ewb = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_ewb, NULL);
ctrls->hwmc = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_hwmc, NULL);

// TODO: wait for decision from FW if to replace with one control
// should report as cluster?
ctrls->laser_power = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_laser_power, NULL);
ctrls->manual_laser_power = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_manual_laser_power, NULL);
if (sid == 0 || sid == 2) {
ctrls->laser_power = v4l2_ctrl_new_custom(hdl,
&ds5_ctrl_laser_power,
sensor);
ctrls->manual_laser_power = v4l2_ctrl_new_custom(hdl,
&ds5_ctrl_manual_laser_power,
sensor);
}

/* Total gain */
if (sid == 0 || sid == 2) {
ctrls->gain = v4l2_ctrl_new_std(hdl, ops,
V4L2_CID_ANALOGUE_GAIN,
16, 248, 1, 16);
} else if (sid == 1) {
ctrls->gain = v4l2_ctrl_new_std(hdl, ops,
V4L2_CID_ANALOGUE_GAIN,
0, 128, 1, 64);
}

ctrls->link_freq = v4l2_ctrl_new_custom(hdl, &d4xx_controls_link_freq, NULL);
dev_info(sd->dev, "%s(): %p\n", __func__, ctrls->link_freq);
if (ctrls->link_freq)
ctrls->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
if (ctrls->gain) {
ctrls->gain->priv = sensor;
ctrls->gain->flags =
V4L2_CTRL_FLAG_VOLATILE | V4L2_CTRL_FLAG_EXECUTE_ON_WRITE;
}
if (sid >= 0) {

ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
V4L2_CID_EXPOSURE_AUTO,
V4L2_EXPOSURE_APERTURE_PRIORITY,
~((1 << V4L2_EXPOSURE_MANUAL) |
(1 << V4L2_EXPOSURE_APERTURE_PRIORITY)),
V4L2_EXPOSURE_APERTURE_PRIORITY);

/* Exposure time: V4L2_CID_EXPOSURE_ABSOLUTE unit: 100 us. */
V4L2_CID_EXPOSURE_AUTO,
V4L2_EXPOSURE_APERTURE_PRIORITY,
~((1 << V4L2_EXPOSURE_MANUAL) |
(1 << V4L2_EXPOSURE_APERTURE_PRIORITY)),
V4L2_EXPOSURE_APERTURE_PRIORITY);

if (ctrls->auto_exp) {
ctrls->auto_exp->flags |=
V4L2_CTRL_FLAG_VOLATILE | V4L2_CTRL_FLAG_EXECUTE_ON_WRITE;
ctrls->auto_exp->priv = sensor;
}
}
/* Exposure time: V4L2_CID_EXPOSURE_ABSOLUTE default unit: 100 us. */
if (sid == 0 || sid == 2) {
ctrls->exposure = v4l2_ctrl_new_std(hdl, ops,
V4L2_CID_EXPOSURE_ABSOLUTE,
1, MAX_DEPTH_EXP, 1, DEF_DEPTH_EXP);
} else if (sid == 1) {
ctrls->exposure = v4l2_ctrl_new_std(hdl, ops,
V4L2_CID_EXPOSURE_ABSOLUTE,
1, MAX_RGB_EXP, 1, DEF_RGB_EXP);
}

ctrls->query_sub_stream = v4l2_ctrl_new_custom(hdl, &d4xx_controls_q_sub_stream, NULL);
ctrls->set_sub_stream = v4l2_ctrl_new_custom(hdl, &d4xx_controls_s_sub_stream, NULL);
if (ctrls->exposure) {
ctrls->exposure->priv = sensor;
ctrls->exposure->flags |=
V4L2_CTRL_FLAG_VOLATILE | V4L2_CTRL_FLAG_EXECUTE_ON_WRITE;
/* override default int type to u32 to match SKU & UVC */
ctrls->exposure->type = V4L2_CTRL_TYPE_U32;
}

ctrls->link_freq = v4l2_ctrl_new_custom(hdl, &d4xx_controls_link_freq, sensor);
dev_info(sd->dev, "%s(): %p\n", __func__, ctrls->link_freq);
if (ctrls->link_freq)
ctrls->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;

ctrls->query_sub_stream = v4l2_ctrl_new_custom(hdl, &d4xx_controls_q_sub_stream, sensor);
ctrls->query_sub_stream->flags |=
V4L2_CTRL_FLAG_VOLATILE | V4L2_CTRL_FLAG_EXECUTE_ON_WRITE;

ctrls->set_sub_stream = v4l2_ctrl_new_custom(hdl, &d4xx_controls_s_sub_stream, sensor);
dev_info(NULL, "%s(), line %d\n", __func__, __LINE__);
if (hdl->error) {
v4l2_err(sd, "error creating controls (%d)\n", hdl->error);
ret = hdl->error;
v4l2_ctrl_handler_free(hdl);
return ret;
}

// TODO: consider invoking v4l2_ctrl_handler_setup(hdl);
// ret = v4l2_ctrl_handler_setup(hdl);
// if (ret < 0) {
// dev_err(&state->client->dev,
// "failed to set default values for controls\n");
// v4l2_ctrl_handler_free(hdl);
// return ret;
// }

// Add these after v4l2_ctrl_handler_setup so they won't be set up
if (sid >= 0) {
ctrls->log = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_log, sensor);
ctrls->fw_version = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_fw_version, sensor);
ctrls->gvd = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_gvd, sensor);
ctrls->get_depth_calib =
v4l2_ctrl_new_custom(hdl, &ds5_ctrl_get_depth_calib, sensor);
ctrls->set_depth_calib =
v4l2_ctrl_new_custom(hdl, &ds5_ctrl_set_depth_calib, sensor);
ctrls->get_coeff_calib =
v4l2_ctrl_new_custom(hdl, &ds5_ctrl_get_coeff_calib, sensor);
ctrls->set_coeff_calib =
v4l2_ctrl_new_custom(hdl, &ds5_ctrl_set_coeff_calib, sensor);
ctrls->ae_roi_get = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_ae_roi_get, sensor);
ctrls->ae_roi_set = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_ae_roi_set, sensor);
ctrls->ae_setpoint_get =
v4l2_ctrl_new_custom(hdl, &ds5_ctrl_ae_setpoint_get, sensor);
ctrls->ae_setpoint_set =
v4l2_ctrl_new_custom(hdl, &ds5_ctrl_ae_setpoint_set, sensor);
ctrls->erb = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_erb, sensor);
ctrls->ewb = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_ewb, sensor);
ctrls->hwmc = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_hwmc, sensor);
v4l2_ctrl_new_custom(hdl, &ds5_ctrl_hwmc_rw, sensor);
}
if (sid == 0)
v4l2_ctrl_new_custom(hdl, &ds5_ctrl_pwm, sensor);

switch( sid ) {
case 0:
state->depth.sensor.sd.ctrl_handler = hdl;
dev_info(state->depth.sensor.sd.dev, "%s():%d set ctrl_handler pad:%d\n", __func__, __LINE__, state->depth.sensor.mux_pad);
break;
case 1:
state->rgb.sensor.sd.ctrl_handler = hdl;
dev_info(state->rgb.sensor.sd.dev, "%s():%d set ctrl_handler pad:%d\n", __func__, __LINE__, state->rgb.sensor.mux_pad);
break;
case 2:
state->motion_t.sensor.sd.ctrl_handler = hdl;
dev_info(state->motion_t.sensor.sd.dev, "%s():%d set ctrl_handler pad:%d\n", __func__, __LINE__, state->motion_t.sensor.mux_pad);
break;
default:
state->mux.sd.subdev.ctrl_handler = hdl;
dev_info(state->mux.sd.subdev.dev, "%s():%d set ctrl_handler for MUX\n", __func__, __LINE__);
break;
}

state->mux.sd.subdev.ctrl_handler = hdl;

return 0;
}
@@ -2554,10 +3111,12 @@ static int ds5_sensor_init(struct i2c_client *c, struct ds5 *state,
struct media_entity *entity = &sensor->sd.entity;
struct media_pad *pad = &sensor->pad;
dev_t *dev_num = &state->client->dev.devt;
struct d4xx_pdata *dpdata = c->dev.platform_data;

dev_info(sd->dev, "%s(): %p %s %p %p", __func__, c, c->name, state, state->client);

struct d4xx_pdata *dpdata = c->dev.platform_data;
v4l2_i2c_subdev_init(sd, c, ops);
sd->owner = THIS_MODULE;
sd->owner = NULL;
sd->internal_ops = &ds5_sensor_internal_ops;
sd->grp_id = *dev_num;
v4l2_set_subdevdata(sd, state);
@@ -2580,12 +3139,15 @@ static int ds5_sensor_register(struct ds5 *state, struct ds5_sensor *sensor)
// FIXME: is async needed?
ret = v4l2_device_register_subdev(state->mux.sd.subdev.v4l2_dev, sd);
if (ret < 0) {
dev_err(sd->dev, "%s(): %d: %d\n", __func__, __LINE__, ret);
return ret;
}

ret = media_create_pad_link(entity, 0, &state->mux.sd.subdev.entity, sensor->mux_pad,
MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
ret = media_create_pad_link(entity, 0,
&state->mux.sd.subdev.entity, sensor->mux_pad,
MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
if (ret < 0) {
dev_err(sd->dev, "%s(): %d: %d\n", __func__, __LINE__, ret);
goto e_sd;
}

@@ -2602,7 +3164,7 @@ static int ds5_sensor_register(struct ds5 *state, struct ds5_sensor *sensor)
static void ds5_sensor_remove(struct ds5_sensor *sensor)
{
v4l2_device_unregister_subdev(&sensor->sd);
// FIXME: test rmmod, unbind, init failures to make sure the entity is always freed

media_entity_cleanup(&sensor->sd.entity);
}

@@ -2611,28 +3173,28 @@ static int ds5_depth_init(struct i2c_client *c, struct ds5 *state)
/* Which mux pad we're connecting to */
state->depth.sensor.mux_pad = DS5_MUX_PAD_DEPTH_A;
return ds5_sensor_init(c, state, &state->depth.sensor,
&ds5_depth_subdev_ops, "depth");
&ds5_depth_subdev_ops, "depth");
}

static int ds5_motion_t_init(struct i2c_client *c, struct ds5 *state)
{
state->motion_t.sensor.mux_pad = DS5_MUX_PAD_MOTION_T_A;
return ds5_sensor_init(c, state, &state->motion_t.sensor,
&ds5_motion_t_subdev_ops, "motion detection");
&ds5_motion_t_subdev_ops, "motion detection");
}

static int ds5_rgb_init(struct i2c_client *c, struct ds5 *state)
{
state->rgb.sensor.mux_pad = DS5_MUX_PAD_RGB_A;
return ds5_sensor_init(c, state, &state->rgb.sensor,
&ds5_rgb_subdev_ops, "rgb");
&ds5_rgb_subdev_ops, "rgb");
}

static int ds5_imu_init(struct i2c_client *c, struct ds5 *state)
{
state->imu.sensor.mux_pad = DS5_MUX_PAD_IMU;
return ds5_sensor_init(c, state, &state->imu.sensor,
&ds5_imu_subdev_ops, "imu");
&ds5_imu_subdev_ops, "imu");
}

/* No locking needed */
@@ -2665,7 +3227,7 @@ static int ds5_mux_enum_mbus_code(struct v4l2_subdev *sd,
break;
case DS5_MUX_PAD_EXTERNAL:
if (mce->index >= state->motion_t.sensor.n_formats +
state->depth.sensor.n_formats)
state->depth.sensor.n_formats)
return -EINVAL;

/*
@@ -2793,8 +3355,9 @@ static int ds5_mux_enum_frame_interval(struct v4l2_subdev *sd,

tmp.pad = 0;

dev_err(state->depth.sensor.sd.dev, "%s(): pad %d code %x width %d height %d\n", __func__,
pad, tmp.code, tmp.width, tmp.height);
dev_err(state->depth.sensor.sd.dev,
"%s(): pad %d code %x width %d height %d\n",
__func__, pad, tmp.code, tmp.width, tmp.height);

if (state->is_depth)
pad = DS5_MUX_PAD_DEPTH_A;
@@ -2856,6 +3419,7 @@ static int ds5_mux_set_fmt(struct v4l2_subdev *sd,
#endif
struct v4l2_subdev_format *fmt)
{
struct ds5 *state = container_of(sd, struct ds5, mux.sd.subdev);
struct v4l2_mbus_framefmt *ffmt;
u32 pad = fmt->pad;
int ret = 0;
@@ -2929,7 +3493,7 @@ static int ds5_mux_get_fmt(struct v4l2_subdev *sd,

/* Video ops */
static int ds5_mux_g_frame_interval(struct v4l2_subdev *sd,
struct v4l2_subdev_frame_interval *fi)
struct v4l2_subdev_frame_interval *fi)
{
struct ds5 *state = container_of(sd, struct ds5, mux.sd.subdev);
struct ds5_sensor *sensor = NULL;
@@ -2943,7 +3507,7 @@ static int ds5_mux_g_frame_interval(struct v4l2_subdev *sd,
fi->interval.denominator = sensor->config.framerate;

dev_info(sd->dev, "%s(): %s %u\n", __func__, sd->name,
fi->interval.denominator);
fi->interval.denominator);

return 0;
}
@@ -2963,7 +3527,7 @@ static u16 __ds5_probe_framerate(const struct ds5_resolution *res, u16 target)
}

static int ds5_mux_s_frame_interval(struct v4l2_subdev *sd,
struct v4l2_subdev_frame_interval *fi)
struct v4l2_subdev_frame_interval *fi)
{
struct ds5 *state = container_of(sd, struct ds5, mux.sd.subdev);
struct ds5_sensor *sensor = NULL;
@@ -3452,9 +4016,7 @@ static int ds5_mux_register(struct i2c_client *c, struct ds5 *state)
static int ds5_hw_init(struct i2c_client *c, struct ds5 *state)
{
u16 mipi_status, n_lanes, phy, drate_min, drate_max;
int ret;

ret = ds5_read(state, DS5_MIPI_SUPPORT_LINES, &n_lanes);
int ret = ds5_read(state, DS5_MIPI_SUPPORT_LINES, &n_lanes);
if (!ret)
ret = ds5_read(state, DS5_MIPI_SUPPORT_PHY, &phy);

@@ -3514,12 +4076,15 @@ static int ds5_mux_init(struct i2c_client *c, struct ds5 *state)

struct d4xx_pdata *dpdata = c->dev.platform_data;
v4l2_i2c_subdev_init(sd, c, &ds5_mux_subdev_ops);
sd->owner = THIS_MODULE;

// Set owner to NULL so we can unload the driver module
sd->owner = NULL;
sd->internal_ops = &ds5_mux_internal_ops;
v4l2_set_subdevdata(sd, state);
snprintf(sd->name, sizeof(sd->name), "DS5 mux %c", dpdata->suffix);

sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;

entity->obj_type = MEDIA_ENTITY_TYPE_V4L2_SUBDEV;
entity->function = MEDIA_ENT_F_CAM_SENSOR;

@@ -3531,12 +4096,31 @@ static int ds5_mux_init(struct i2c_client *c, struct ds5 *state)
if (ret < 0)
return ret;

// FIXME: this is most likely different for depth and motion detection
ret = ds5_ctrl_init(state);
/*set for mux*/
ret = ds5_ctrl_init(state, -1);
if (ret < 0)
return ret;
/*set for depth*/
ret = ds5_ctrl_init(state, 0);
if (ret < 0)
return ret;
/*set for rgb*/
ret = ds5_ctrl_init(state, 1);
if (ret < 0)
return ret;
/*set for y8*/
ret = ds5_ctrl_init(state, 2);
if (ret < 0)
return ret;

state->mux.last_set = &state->motion_t.sensor;
if (state->is_depth)
state->mux.last_set = &state->depth.sensor;
else if (state->is_rgb)
state->mux.last_set = &state->rgb.sensor;
else if (state->is_y8)
state->mux.last_set = &state->motion_t.sensor;
else
state->mux.last_set = &state->imu.sensor;

#ifdef CONFIG_TEGRA_CAMERA_PLATFORM
state->mux.sd.dev = &c->dev;
@@ -3563,7 +4147,8 @@ static int ds5_mux_init(struct i2c_client *c, struct ds5 *state)
static int ds5_fixed_configuration(struct i2c_client *client, struct ds5 *state)
{
struct ds5_sensor *sensor;
u16 cfg0, cfg0_md, cfg1, cfg1_md, dw, dh, yw, yh, dev_type;
u16 cfg0 = 0, cfg0_md = 0, cfg1 = 0, cfg1_md = 0;
u16 dw = 0, dh = 0, yw = 0, yh = 0, dev_type = 0;
int ret;

ret = ds5_read(state, DS5_DEPTH_STREAM_DT, &cfg0);
@@ -3727,17 +4312,21 @@ static int ds5_dfu_wait_for_status(struct ds5 *state)
int i, ret = 0;
u16 status;

for (i = 0; i < DS5_START_MAX_COUNT; i++) {
for (i = 0; i < DS5_START_MAX_COUNT; i++)
{
ds5_read(state, 0x5000, &status);
if (status == 0x0001 || status == 0x0002) {
dev_err(&state->client->dev, "%s(): dfu failed status(0x%4x)\n", __func__, status);
if (status == 0x0001 || status == 0x0002)
{
dev_err(&state->client->dev,
"%s(): dfu failed status(0x%4x)\n",
__func__, status);
ret = -EREMOTEIO;
break;
}
if (!status)
break;
msleep_range(DS5_START_POLL_TIME);
}
}

return ret;
};
@@ -3748,67 +4337,70 @@ static int ds5_dfu_switch_to_dfu(struct ds5 *state)
int i = DS5_START_MAX_COUNT;
u16 status;

ds5_raw_write_with_check(state, 0x4900, &cmd_switch_to_dfu, sizeof(cmd_switch_to_dfu));
ds5_raw_write_with_check(state, 0x4900,
&cmd_switch_to_dfu, sizeof(cmd_switch_to_dfu));
ds5_write_with_check(state, 0x490c, 0x01); /* execute cmd */
/*Wait for DFU fw to boot*/
do {
msleep_range(DS5_START_POLL_TIME*10);
ret = ds5_read(state, 0x5000, &status);
} while (ret && i--);
} while (ret && i-- );
return ret;
};

static int ds5_dfu_wait_for_get_dfu_status(struct ds5 *state, enum dfu_fw_state exp_state)
static int ds5_dfu_wait_for_get_dfu_status(struct ds5 *state,
enum dfu_fw_state exp_state)
{
int ret = 0;
u16 status, dfu_state_len = 0x0000;
u16 status,dfu_state_len = 0x0000;
unsigned char dfu_asw_buf[DFU_WAIT_RET_LEN];
unsigned int dfu_wr_wait_msec = 0;

do {
ds5_write_with_check(state, 0x5008, 0x0003); // Get Write state
do {
ds5_read_with_check(state, 0x5000, &status);
if (status == 0x0001) {
dev_err(&state->client->dev,
"%s(): Write status error I2C_STATUS_ERROR(1)\n", __func__);
"%s(): Write status error I2C_STATUS_ERROR(1)\n",
__func__);
return -EINVAL;
} else
if (status == 0x0002 && dfu_wr_wait_msec)
msleep_range(dfu_wr_wait_msec);
/*
*dev_notice(&state->client->dev,
* "%s():waiting (%x)\n", __func__, dfu_wr_wait_msec);
*/

} while (status);

ds5_read_with_check(state, 0x5004, &dfu_state_len);
if (dfu_state_len != DFU_WAIT_RET_LEN) {
dev_err(&state->client->dev, "%s(): Wrong answer len (%d)\n", __func__, dfu_state_len);
dev_err(&state->client->dev,
"%s(): Wrong answer len (%d)\n", __func__, dfu_state_len);
return -EINVAL;
}
ds5_raw_read_with_check(state, 0x4e00, &dfu_asw_buf, DFU_WAIT_RET_LEN);
if (dfu_asw_buf[0]) {
dev_err(&state->client->dev, "%s(): Wrong dfu_status (%d)\n", __func__, dfu_asw_buf[0]);
dev_err(&state->client->dev,
"%s(): Wrong dfu_status (%d)\n", __func__, dfu_asw_buf[0]);
return -EINVAL;
}
dfu_wr_wait_msec = (((unsigned int)dfu_asw_buf[3]) << 16)
| (((unsigned int)dfu_asw_buf[2]) << 8) | dfu_asw_buf[1];
| (((unsigned int)dfu_asw_buf[2]) << 8)
| dfu_asw_buf[1];
} while (dfu_asw_buf[4] == dfuDNBUSY && exp_state == dfuDNLOAD_IDLE);

if (dfu_asw_buf[4] != exp_state) {

if (dfu_asw_buf[4] != exp_state) {
dev_notice(&state->client->dev,
"%s(): Wrong dfu_state (%d) while expected(%d)\n", __func__, dfu_asw_buf[4], exp_state);
ret = -EINVAL;
"%s(): Wrong dfu_state (%d) while expected(%d)\n",
__func__, dfu_asw_buf[4], exp_state);
ret = -EINVAL;
}
return ret;
};

static int ds5_dfu_get_dev_info(struct ds5 *state, struct __fw_status *buf)
{
int ret;
u16 len;
u16 len = 0;

ret = ds5_write(state, 0x5008, 0x0002); //Upload DFU cmd
if (!ret)
@@ -3819,7 +4411,9 @@ static int ds5_dfu_get_dev_info(struct ds5 *state, struct __fw_status *buf)
if (len == sizeof(struct __fw_status)) {
ds5_raw_read_with_check(state, 0x4e00, buf, len);
} else {
dev_err(&state->client->dev, "%s(): Wrong state size (%d)\n", __func__, len);
dev_err(&state->client->dev,
"%s(): Wrong state size (%d)\n",
__func__, len);
ret = -EINVAL;
}
return ret;
@@ -3828,20 +4422,24 @@ static int ds5_dfu_get_dev_info(struct ds5 *state, struct __fw_status *buf)
static int ds5_dfu_detach(struct ds5 *state)
{
int ret;
struct __fw_status buf;
struct __fw_status buf = {0};

ds5_write_with_check(state, 0x500c, 0x00);
ret = ds5_dfu_wait_for_get_dfu_status(state, dfuIDLE);
if (!ret)
ret = ds5_dfu_get_dev_info(state, &buf);
dev_notice(&state->client->dev, "%s():DFU ver (0x%x) received\n", __func__, buf.DFU_version);
dev_notice(&state->client->dev, "%s():FW last version (0x%x) received\n", __func__, buf.FW_lastVersion);
dev_notice(&state->client->dev, "%s():FW status (%s)\n", __func__, buf.DFU_isLocked ? "locked" : "unlocked");
dev_notice(&state->client->dev, "%s():DFU ver (0x%x) recieved\n",
__func__, buf.DFU_version);
dev_notice(&state->client->dev, "%s():FW last version (0x%x) recieved\n",
__func__, buf.FW_lastVersion);
dev_notice(&state->client->dev, "%s():FW status (%s)\n",
__func__, buf.DFU_isLocked ? "locked" : "unlocked");
return ret;
};

/* When a process reads from our device, this gets called. */
static ssize_t device_read(struct file *flip, char __user *buffer, size_t len, loff_t *offset)
static ssize_t ds5_dfu_device_read(struct file *flip,
char __user *buffer, size_t len, loff_t *offset)
{
struct ds5 *state = flip->private_data;
u16 fw_ver;
@@ -3853,7 +4451,7 @@ static ssize_t device_read(struct file *flip, char __user *buffer, size_t len, l
ret = ds5_read(state, DS5_FW_VERSION, &fw_ver);
if (ret < 0)
goto e_dfu_read_failed;
sprintf(msg, "DFU info: \tver: (0x%x)\n", fw_ver);
snprintf(msg, sizeof(msg) ,"DFU info: \tver: (0x%x)\n", fw_ver);
if (copy_to_user(buffer, msg, strlen(msg)))
ret = -EFAULT;
else {
@@ -3866,72 +4464,82 @@ static ssize_t device_read(struct file *flip, char __user *buffer, size_t len, l
return ret;
};

static ssize_t device_write(struct file *flip, const char __user *buffer, size_t len, loff_t *offset)
static ssize_t ds5_dfu_device_write(struct file *flip,
const char __user *buffer, size_t len, loff_t *offset)
{
struct ds5 *state = flip->private_data;
int ret = 0;

if (mutex_lock_interruptible(&state->lock))
return -ERESTARTSYS;
switch (state->dfu_dev.dfu_state_flag) {

if (state->dfu_dev.dfu_state_flag == DS5_DFU_OPEN) {
case DS5_DFU_OPEN:
ret = ds5_dfu_switch_to_dfu(state);
if (ret < 0) {
dev_err(&state->client->dev, "%s(): Switch to dfu failed (%d)\n", __func__, ret);
dev_err(&state->client->dev, "%s(): Switch to dfu failed (%d)\n",
__func__, ret);
goto dfu_write_error;
}
state->dfu_dev.dfu_state_flag = DS5_DFU_RECOVERY;
}

if (state->dfu_dev.dfu_state_flag == DS5_DFU_RECOVERY) {
/*no break - procceed to recovery*/
case DS5_DFU_RECOVERY:
ret = ds5_dfu_detach(state);
if (ret < 0) {
dev_err(&state->client->dev, "%s(): Detach failed (%d)\n", __func__, ret);
dev_err(&state->client->dev, "%s(): Detach failed (%d)\n",
__func__, ret);
goto dfu_write_error;
}
state->dfu_dev.dfu_state_flag = DS5_DFU_IN_PROGRESS;
state->dfu_dev.init_v4l_f = 1;
}

if (state->dfu_dev.dfu_state_flag == DS5_DFU_IN_PROGRESS) {
/*no break - procceed to download*/
case DS5_DFU_IN_PROGRESS: {
unsigned int dfu_full_blocks = len / DFU_BLOCK_SIZE;
unsigned int dfu_part_blocks = len % DFU_BLOCK_SIZE;

while (dfu_full_blocks--) {
if (copy_from_user(state->dfu_dev.dfu_msg, buffer, DFU_BLOCK_SIZE)) {
if(copy_from_user(state->dfu_dev.dfu_msg, buffer, DFU_BLOCK_SIZE)) {
ret = -EFAULT;
goto dfu_write_error;
}
ret = ds5_raw_write(state, 0x4a00, state->dfu_dev.dfu_msg, DFU_BLOCK_SIZE);
ret = ds5_raw_write(state, 0x4a00,
state->dfu_dev.dfu_msg, DFU_BLOCK_SIZE);
if (ret < 0)
goto dfu_write_error;
ret = ds5_dfu_wait_for_get_dfu_status(state, dfuDNLOAD_IDLE);
ret = ds5_dfu_wait_for_get_dfu_status (state, dfuDNLOAD_IDLE);
if (ret < 0)
goto dfu_write_error;
buffer += DFU_BLOCK_SIZE;
}
if (copy_from_user(state->dfu_dev.dfu_msg, buffer, dfu_part_blocks)) {
ret = -EFAULT;
goto dfu_write_error;
if(copy_from_user(state->dfu_dev.dfu_msg , buffer, dfu_part_blocks)) {
ret = -EFAULT;
goto dfu_write_error;
}
if (dfu_part_blocks) {
ret = ds5_raw_write(state, 0x4a00, state->dfu_dev.dfu_msg, dfu_part_blocks);
if (!ret)
ret = ds5_dfu_wait_for_get_dfu_status(state, dfuDNLOAD_IDLE);
ret = ds5_raw_write(state, 0x4a00,
state->dfu_dev.dfu_msg, dfu_part_blocks);
if (!ret) {
ret = ds5_dfu_wait_for_get_dfu_status (state, dfuDNLOAD_IDLE);
}
if (!ret)
ret = ds5_write(state, 0x4a04, 0x00); /*Download complete */
if (!ret)
ret = ds5_dfu_wait_for_get_dfu_status(state, dfuMANIFEST);
ret = ds5_dfu_wait_for_get_dfu_status (state, dfuMANIFEST);
if (ret < 0)
goto dfu_write_error;
state->dfu_dev.dfu_state_flag = DS5_DFU_DONE;
}
dev_notice(&state->client->dev, "%s(): DFU block (%d) bytes written\n", __func__, (int)len);
} else {
dev_err(&state->client->dev, "%s(): Wrong state (%d)\n", __func__, state->dfu_dev.dfu_state_flag);
ret = -EINVAL;
goto dfu_write_error;
dev_notice(&state->client->dev, "%s(): DFU block (%d) bytes written\n",
__func__, (int)len);
break;
}
default:
dev_err(&state->client->dev, "%s(): Wrong state (%d)\n",
__func__, state->dfu_dev.dfu_state_flag);
ret = -EINVAL;
goto dfu_write_error;

};
mutex_unlock(&state->lock);
return len;

@@ -3942,36 +4550,35 @@ static ssize_t device_write(struct file *flip, const char __user *buffer, size_t
return ret;
};

static int device_open(struct inode *inode, struct file *file)
static int ds5_dfu_device_open(struct inode *inode, struct file *file)
{
struct ds5 *state = container_of(inode->i_cdev, struct ds5, dfu_dev.ds5_cdev);
struct ds5 *state = container_of(inode->i_cdev, struct ds5,
dfu_dev.ds5_cdev);
struct i2c_adapter *parent = i2c_parent_is_i2c_adapter(
state->client->adapter);

try_module_get(THIS_MODULE);
if (state->dfu_dev.device_open_count)
return -EBUSY;
state->dfu_dev.device_open_count++;
if (state->dfu_dev.dfu_state_flag != DS5_DFU_RECOVERY)
if (state->dfu_dev.dfu_state_flag != DS5_DFU_RECOVERY)
state->dfu_dev.dfu_state_flag = DS5_DFU_OPEN;
state->dfu_dev.dfu_msg = devm_kzalloc(&state->client->dev, DFU_BLOCK_SIZE, GFP_KERNEL);
state->dfu_dev.dfu_msg = devm_kzalloc(&state->client->dev,
DFU_BLOCK_SIZE, GFP_KERNEL);
if (!state->dfu_dev.dfu_msg)
return -ENOMEM;

file->private_data = state;
return 0;
};

static int ds5_v4l_init(struct i2c_client *c, struct ds5 *state)
{
u16 fw_ver;
u16 fw_build;
int ret;

ret = ds5_parse_cam(c, state);
if (ret < 0)
return ret;

ds5_read_with_check(state, DS5_FW_VERSION, &fw_ver);
ds5_read_with_check(state, DS5_FW_BUILD, &fw_build);
state->fw_version = fw_ver;
state->fw_build = fw_build;

ret = ds5_depth_init(c, state);
if (ret < 0)
return ret;
@@ -4014,33 +4621,34 @@ static int ds5_v4l_init(struct i2c_client *c, struct ds5 *state)
return ret;
}

static int device_release(struct inode *inode, struct file *file)
static int ds5_dfu_device_release(struct inode *inode, struct file *file)
{
struct ds5 *state = container_of(inode->i_cdev, struct ds5, dfu_dev.ds5_cdev);

state->dfu_dev.device_open_count--;
if (state->dfu_dev.dfu_state_flag != DS5_DFU_RECOVERY)
if (state->dfu_dev.dfu_state_flag != DS5_DFU_RECOVERY)
state->dfu_dev.dfu_state_flag = DS5_DFU_IDLE;
if (state->dfu_dev.dfu_state_flag == DS5_DFU_DONE && state->dfu_dev.init_v4l_f)
if (state->dfu_dev.dfu_state_flag == DS5_DFU_DONE
&& state->dfu_dev.init_v4l_f)
ds5_v4l_init(state->client, state);
state->dfu_dev.init_v4l_f = 0;
if (state->dfu_dev.dfu_msg)
devm_kfree(&state->client->dev, state->dfu_dev.dfu_msg);
state->dfu_dev.dfu_msg = NULL;
module_put(THIS_MODULE);

return 0;
};

static const struct file_operations ds5_device_file_ops = {
.owner = THIS_MODULE,
.read = &device_read,
.write = &device_write,
.open = &device_open,
.release = &device_release
.owner = THIS_MODULE,
.read = &ds5_dfu_device_read,
.write = &ds5_dfu_device_write,
.open = &ds5_dfu_device_open,
.release = &ds5_dfu_device_release
};

struct class *g_ds5_class;
atomic_t primary_chardev = ATOMIC_INIT(0);
struct class* g_ds5_class;
atomic_t primary_chardev=ATOMIC_INIT(0);

static int ds5_chrdev_init(struct i2c_client *c, struct ds5 *state)
{
@@ -4058,7 +4666,8 @@ static int ds5_chrdev_init(struct i2c_client *c, struct ds5 *state)
return ret;

if (!atomic_cmpxchg(&primary_chardev, 0, MAJOR(*dev_num))) {
dev_info(&c->dev, "%s(): <Major, Minor>: <%d, %d>\n", __func__, MAJOR(*dev_num), MINOR(*dev_num));
dev_dbg(&c->dev, "%s(): <Major, Minor>: <%d, %d>\n",
__func__, MAJOR(*dev_num), MINOR(*dev_num));
/* Create a class : appears at /sys/class */
*ds5_class = class_create(THIS_MODULE, DS5_DRIVER_NAME_CLASS);
if (IS_ERR(*ds5_class)) {
@@ -4074,32 +4683,27 @@ static int ds5_chrdev_init(struct i2c_client *c, struct ds5 *state)
cdev_init(ds5_cdev, &ds5_device_file_ops);
/* Build up the current device number. To be used further */
*dev_num = MKDEV(MAJOR(*dev_num), MINOR(*dev_num));
/* Create a device node for this device. Look, the class is
* being used here. The same class is associated with N_MINOR
* devices. Once the function returns, device nodes will be
* created as /dev/my_dev0, /dev/my_dev1,... You can also view
* the devices under /sys/class/my_driver_class.
*/
sprintf(dev_name, "%s%d", DS5_DRIVER_NAME_DFU, MAJOR(*dev_num));
/* Create a device node for this device. */
snprintf (dev_name, sizeof(dev_name), "%s%d",
DS5_DRIVER_NAME_DFU, MAJOR(*dev_num));
chr_dev = device_create(*ds5_class, NULL, *dev_num, NULL, dev_name);
if (IS_ERR(chr_dev)) {
ret = PTR_ERR(chr_dev);
dev_err(&c->dev, "Could not create device\n");
dev_err(&c->dev,"Could not create device\n");
class_destroy(*ds5_class);
unregister_chrdev_region(0, 1);
return ret;
}
/* Now make the device live for the users to access */
cdev_add(ds5_cdev, *dev_num, 1);
return 0;
};

static int ds5_chrdev_remove(struct ds5 *state)
{
struct class **ds5_class = &state->dfu_dev.ds5_class;
struct class* *ds5_class = &state->dfu_dev.ds5_class;
dev_t *dev_num = &state->client->dev.devt;

dev_info(&state->client->dev, "%s()\n", __func__);
dev_dbg(&state->client->dev, "%s()\n", __func__);
unregister_chrdev_region(*dev_num, 1);
device_destroy(*ds5_class, *dev_num);
if (atomic_cmpxchg(&primary_chardev, MAJOR(*dev_num), 0) == MAJOR(*dev_num))
@@ -4156,6 +4760,127 @@ static void ds5_substream_init(void)
pad_to_substream[DS5_MUX_PAD_MOTION_T_A] = 4;
}

/* SYSFS attributes */
#ifdef CONFIG_SYSFS
static ssize_t ds5_fw_ver_show(struct device *dev,
struct device_attribute *attr, char *buf)
{
struct i2c_client *c = to_i2c_client(dev);
struct ds5 *state = container_of(i2c_get_clientdata(c),
struct ds5, mux.sd.subdev);

ds5_read(state, DS5_FW_VERSION, &state->fw_version);
ds5_read(state, DS5_FW_BUILD, &state->fw_build);

return snprintf(buf, PAGE_SIZE, "D4XX Sensor: %s, Version: %d.%d.%d.%d\n",
ds5_get_sensor_name(state),
(state->fw_version >> 8) & 0xff, state->fw_version & 0xff,
(state->fw_build >> 8) & 0xff, state->fw_build & 0xff);
}

static DEVICE_ATTR_RO(ds5_fw_ver);

/* Derive 'device_attribute' structure for a read register's attribute */
struct dev_ds5_reg_attribute {
struct device_attribute attr;
u16 reg; // register
u8 valid; // validity of above data
};

/** Read DS5 register.
* ds5_read_reg_show will actually read register from ds5 while
* ds5_read_reg_store will store register to read
* Example:
* echo -n "0xc03c" >ds5_read_reg
* Read register result:
* cat ds5_read_reg
* Expected:
* reg:0xc93c, result:0x11
*/
static ssize_t ds5_read_reg_show(struct device *dev,
struct device_attribute *attr, char *buf)
{
u16 rbuf;
int n;
struct i2c_client *c = to_i2c_client(dev);
struct ds5 *state = container_of(i2c_get_clientdata(c),
struct ds5, mux.sd.subdev);
struct dev_ds5_reg_attribute *ds5_rw_attr = container_of(attr,
struct dev_ds5_reg_attribute, attr);
if (ds5_rw_attr->valid != 1)
return -EINVAL;
ds5_read(state, ds5_rw_attr->reg, &rbuf);

n = snprintf(buf, PAGE_SIZE, "register:0x%4x, value:0x%02x\n",
ds5_rw_attr->reg, rbuf);

return n;
}

/** Read DS5 register - Store reg to attr struct pointer
* ds5_read_reg_show will actually read register from ds5 while
* ds5_read_reg_store will store module, offset and length
*/
static ssize_t ds5_read_reg_store(struct device *dev,
struct device_attribute *attr, const char *buf, size_t count)
{
struct dev_ds5_reg_attribute *ds5_rw_attr = container_of(attr,
struct dev_ds5_reg_attribute, attr);
int rc = -1;
u32 reg;
ds5_rw_attr->valid = 0;
/* Decode input */
rc = sscanf(buf, "0x%04x", &reg);
if (rc != 1)
return -EINVAL;
ds5_rw_attr->reg = reg;
ds5_rw_attr->valid = 1;
return count;
}

#define DS5_RW_REG_ATTR(_name) \
struct dev_ds5_reg_attribute dev_attr_##_name = { \
__ATTR(_name, S_IRUGO | S_IWUSR, \
ds5_read_reg_show, ds5_read_reg_store), \
0, 0 }

static DS5_RW_REG_ATTR(ds5_read_reg);

static ssize_t ds5_write_reg_store(struct device *dev,
struct device_attribute *attr, const char *buf, size_t count)
{
struct i2c_client *c = to_i2c_client(dev);
struct ds5 *state = container_of(i2c_get_clientdata(c),
struct ds5, mux.sd.subdev);

int rc = -1;
u32 reg, w_val = 0;
u16 val = -1;
/* Decode input */
rc = sscanf(buf, "0x%04x 0x%04x", &reg, &w_val);
if (rc != 2)
return -EINVAL;
val = w_val & 0xffff;
mutex_lock(&state->lock);
ds5_write(state, reg, val);
mutex_unlock(&state->lock);
return count;
}

static DEVICE_ATTR_WO(ds5_write_reg);

static struct attribute *ds5_attributes[] = {
&dev_attr_ds5_fw_ver.attr,
&dev_attr_ds5_read_reg.attr.attr,
&dev_attr_ds5_write_reg.attr,
NULL
};

static const struct attribute_group ds5_attr_group = {
.attrs = ds5_attributes,
};
#endif

static int ds5_probe(struct i2c_client *c, const struct i2c_device_id *id)
{
struct ds5 *state = devm_kzalloc(&c->dev, sizeof(*state), GFP_KERNEL);
@@ -4230,7 +4955,8 @@ static int ds5_probe(struct i2c_client *c, const struct i2c_device_id *id)
}
ret = ds5_read(state, 0x5020, &rec_state);
if (ret < 0) {
dev_err(&c->dev, "%s(): cannot communicate with D4XX: %d\n", __func__, ret);
dev_err(&c->dev, "%s(): cannot communicate with D4XX: %d\n",
__func__, ret);
goto e_chardev;
}

@@ -4240,10 +4966,29 @@ static int ds5_probe(struct i2c_client *c, const struct i2c_device_id *id)
return 0;
}

state->is_depth = 0;
state->is_y8 = 0;
state->is_rgb = 0;
state->is_imu = 0;

err = of_property_read_string(c->dev.of_node, "cam-type",
&str);
if (!err && !strncmp(str, "Depth", strlen("Depth")))
state->is_depth = 1;
if (!err && !strncmp(str, "Y8", strlen("Y8")))
state->is_y8 = 1;
if (!err && !strncmp(str, "RGB", strlen("RGB")))
state->is_rgb = 1;
if (!err && !strncmp(str, "IMU", strlen("IMU")))
state->is_imu = 1;

ds5_read_with_check(state, DS5_FW_VERSION, &state->fw_version);
ds5_read_with_check(state, DS5_FW_BUILD, &state->fw_build);

dev_info(&c->dev, "D4XX Sensor: %s, firmware build: %d.%d.%d.%d\n",
ds5_get_sensor_name(state),
(state->fw_version >> 8) & 0xff, state->fw_version & 0xff,
(state->fw_build >> 8) & 0xff, state->fw_build & 0xff);

ret = ds5_v4l_init(c, state);
if (ret < 0)
@@ -4259,6 +5004,11 @@ static int ds5_probe(struct i2c_client *c, const struct i2c_device_id *id)
* goto err;
* }
*/
#ifdef CONFIG_SYSFS
/* Custom sysfs attributes */
/* create the sysfs file group */
err = sysfs_create_group(&state->client->dev.kobj, &ds5_attr_group);
#endif
ds5_substream_init();
return 0;

@@ -4274,13 +5024,18 @@ static int ds5_remove(struct i2c_client *c)
{
struct ds5 *state = container_of(i2c_get_clientdata(c), struct ds5, mux.sd.subdev);

dev_info(&c->dev, "%s()\n", __func__);
dev_info(&c->dev, "D4XX remove %s\n",
ds5_get_sensor_name(state));
if (state->vcc)
regulator_disable(state->vcc);
/* gpio_free(state->pwdn_gpio); */
// gpio_free(state->pwdn_gpio);
ds5_chrdev_remove(state);
if (state->dfu_dev.dfu_state_flag != DS5_DFU_RECOVERY)
if (state->dfu_dev.dfu_state_flag != DS5_DFU_RECOVERY) {
#ifdef CONFIG_SYSFS
sysfs_remove_group(&c->dev.kobj, &ds5_attr_group);
#endif
ds5_mux_remove(state);
}
return 0;
}

@@ -4304,7 +5059,15 @@ static struct i2c_driver ds5_i2c_driver = {

module_i2c_driver(ds5_i2c_driver);

MODULE_DESCRIPTION("Intel D4XX camera driver");
MODULE_AUTHOR("Guennadi Liakhovetski (guennadi.liakhovetski@intel.com)");
MODULE_DESCRIPTION("Intel RealSense D4XX Camera Driver");
MODULE_AUTHOR( "Guennadi Liakhovetski <guennadi.liakhovetski@intel.com>,\n\
Nael Masalha <nael.masalha@intel.com>,\n\
Alexander Gantman <alexander.gantman@intel.com>,\n\
Emil Jahshan <emil.jahshan@intel.com>,\n\
Xin Zhang <xin.x.zhang@intel.com>,\n\
Qingwu Zhang <qingwu.zhang@intel.com>,\n\
Evgeni Raikhel <evgeni.raikhel@intel.com>,\n\
Shikun Ding <shikun.ding@intel.com>");
MODULE_AUTHOR("Dmitry Perchanov <dmitry.perchanov@intel.com>");
MODULE_LICENSE("GPL v2");
MODULE_VERSION("0.16.1.0");
MODULE_VERSION("1.0.2.13");

0 comments on commit b0aa357

Please sign in to comment.