309 lines
6.7 KiB
C
309 lines
6.7 KiB
C
/*
|
|
* Copyright (c) 2022 Semidrive Semiconductor.
|
|
* All rights reserved.
|
|
*/
|
|
|
|
#include "ex_x3d.h"
|
|
|
|
#define X3D_DEVICE_ID 0x58
|
|
#define X3D_SERSOR_ADDR 0x2C
|
|
|
|
#define CONFIG_OV_X3D_FPS 54
|
|
|
|
#ifdef CONFIG_OV_X3D_FPS
|
|
static uint32_t s_x3d_fps = CONFIG_OV_X3D_FPS;
|
|
#else
|
|
static uint32_t s_x3d_fps = 30;
|
|
#endif
|
|
|
|
#ifdef CONFIG_OV_X3D_W
|
|
static uint32_t s_x3d_width = CONFIG_OV_X3D_W;
|
|
#else
|
|
static uint32_t s_x3d_width = 1280;
|
|
#endif
|
|
|
|
#ifdef CONFIG_OV_X3D_H
|
|
static uint32_t s_x3d_height = CONFIG_OV_X3D_H;
|
|
#else
|
|
static uint32_t s_x3d_height = 720;
|
|
#endif
|
|
|
|
enum x3d_status {
|
|
SERSOR_IDLE,
|
|
SERSOR_STREAM_ON,
|
|
};
|
|
|
|
struct x3d_dev {
|
|
struct vdev_device vdev;
|
|
void *i2c_handle;
|
|
uint8_t sensor_addr;
|
|
|
|
uint32_t gpio_pwn;
|
|
uint32_t gpio_intn;
|
|
|
|
osMutexId_t lock;
|
|
uint32_t status;
|
|
};
|
|
|
|
__UNUSED static int x3d_write_reg(struct x3d_dev *sensor, uint8_t reg, uint8_t val)
|
|
{
|
|
uint8_t buf[2];
|
|
int ret = 0;
|
|
|
|
sensor_info("%s: addr=0x%x, reg=0x%x, val=0x%x\n", __func__,
|
|
sensor->sensor_addr, reg, val);
|
|
buf[0] = reg;
|
|
buf[1] = val;
|
|
ret = i2c_write(sensor->i2c_handle, sensor->sensor_addr, buf, 2);
|
|
if (ret < 0) {
|
|
sensor_err("%s: error: reg=0x%x, val=%x\n", __func__, reg, val);
|
|
return ret;
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
__UNUSED static int x3d_read_reg(struct x3d_dev *sensor, uint8_t reg, uint8_t *val)
|
|
{
|
|
uint8_t buf[1];
|
|
int ret = 0;
|
|
|
|
memset(buf, 0, sizeof(buf));
|
|
ret = i2c_write_read(sensor->i2c_handle, sensor->sensor_addr, ®, 1, buf, 1);
|
|
if (ret < 0) {
|
|
sensor_err("%s: error: read reg=0x%02x\n", __func__, reg);
|
|
return ret;
|
|
}
|
|
|
|
*val = buf[0];
|
|
return 0;
|
|
}
|
|
|
|
static int x3d_check_chip_id(struct x3d_dev *sensor)
|
|
{
|
|
sensor_info("%s\n", __func__);
|
|
return 0;
|
|
}
|
|
|
|
static int x3d_init_setup(struct x3d_dev *sensor)
|
|
{
|
|
sensor_info("x3d init setup done\n");
|
|
return 0;
|
|
}
|
|
|
|
//extern gpio_dev_t g_gpio_safety;
|
|
|
|
static int x3d_s_power(struct vdev_device *vdev, int enable)
|
|
{
|
|
struct x3d_dev *sensor;
|
|
uint32_t pin;
|
|
//gpio_dev_t *gpio_dev = &g_gpio_safety;
|
|
|
|
if (!vdev) {
|
|
return -1;
|
|
}
|
|
sensor = (struct x3d_dev *)vdev->priv_data;
|
|
pin = sensor->gpio_pwn;
|
|
|
|
sensor_info("%s: pin %d, enable %d\n", __func__, pin, enable);
|
|
//gpio_set(gpio_dev, pin, enable ? true : false);
|
|
//usleep_range(10000, 11000);
|
|
return 0;
|
|
}
|
|
|
|
static int x3d_enum_mbus_fmt(struct vdev_device *vdev,
|
|
struct vdev_mbus_framefmt *fme)
|
|
{
|
|
struct x3d_dev *sensor;
|
|
|
|
if ((vdev == NULL) || (fme == NULL)) {
|
|
return -1;
|
|
}
|
|
|
|
sensor = (struct x3d_dev *)vdev->priv_data;
|
|
|
|
if (fme->index > 0) {
|
|
fme->index = 0;
|
|
return -1;
|
|
}
|
|
|
|
fme->code = sensor->vdev.fmt.code;
|
|
fme->field = sensor->vdev.fmt.field;
|
|
fme->colorspace = sensor->vdev.fmt.colorspace;
|
|
return 0;
|
|
}
|
|
|
|
static int x3d_enum_frame_size(struct vdev_device *vdev,
|
|
struct vdev_frame_size_enum *fse)
|
|
{
|
|
struct x3d_dev *sensor;
|
|
|
|
if (!vdev || !fse) {
|
|
return -1;
|
|
}
|
|
|
|
sensor = (struct x3d_dev *)vdev->priv_data;
|
|
|
|
if (fse->index > 0) {
|
|
fse->index = 0;
|
|
return -1;
|
|
}
|
|
|
|
fse->min_width = sensor->vdev.fmt.width;
|
|
fse->max_width = sensor->vdev.fmt.width;
|
|
fse->min_height = sensor->vdev.fmt.height;
|
|
fse->max_height = sensor->vdev.fmt.height;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int x3d_enum_frame_interval(struct vdev_device *vdev,
|
|
struct vdev_frame_interval_enum *fie)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
static int x3d_get_frame_interval(struct vdev_device *vdev,
|
|
struct vdev_fract *fi)
|
|
{
|
|
if (!vdev || !fi) {
|
|
return -1;
|
|
}
|
|
|
|
*fi = vdev->frame_interval;
|
|
return 0;
|
|
}
|
|
|
|
static int x3d_set_frame_interval(struct vdev_device *vdev,
|
|
struct vdev_fract fi)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
static int x3d_get_fmt(struct vdev_device *vdev,
|
|
struct vdev_mbus_framefmt *fmt)
|
|
{
|
|
if (!vdev) {
|
|
return -1;
|
|
}
|
|
|
|
*fmt = vdev->fmt;
|
|
return 0;
|
|
}
|
|
|
|
static int x3d_set_fmt(struct vdev_device *vdev,
|
|
struct vdev_mbus_framefmt fmt)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
static int x3d_s_stream(struct vdev_device *vdev, int enable)
|
|
{
|
|
int ret = 0;
|
|
struct x3d_dev *sensor;
|
|
|
|
printf("%s \r\n",__func__);
|
|
if (!vdev) {
|
|
return -1;
|
|
}
|
|
|
|
sensor = (struct x3d_dev *)vdev->priv_data;
|
|
osMutexAcquire(sensor->lock, 0);
|
|
|
|
if (enable == 1) {
|
|
if (sensor->status == SERSOR_STREAM_ON) {
|
|
sensor_info("%s: already stream on\n", __func__);
|
|
goto exit;
|
|
}
|
|
|
|
ret = x3d_init_setup(sensor);
|
|
if (ret < 0)
|
|
goto exit;
|
|
|
|
sensor->status = SERSOR_STREAM_ON;
|
|
|
|
} else {
|
|
sensor->status = SERSOR_IDLE;
|
|
}
|
|
|
|
exit:
|
|
osMutexRelease(sensor->lock);
|
|
return ret;
|
|
}
|
|
|
|
static int x3d_deinit(struct vdev_device *vdev)
|
|
{
|
|
struct x3d_dev *sensor;
|
|
|
|
if (!vdev) {
|
|
return -1;
|
|
}
|
|
|
|
sensor = (struct x3d_dev *)vdev->priv_data;
|
|
osMutexDelete(sensor->lock);
|
|
free(sensor);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static const struct vdev_dev_ops x3d_vdev_ops = {
|
|
.s_power = x3d_s_power,
|
|
.enum_mbus_fmt = x3d_enum_mbus_fmt,
|
|
.enum_frame_size = x3d_enum_frame_size,
|
|
.enum_frame_interval = x3d_enum_frame_interval,
|
|
.g_frame_interval = x3d_get_frame_interval,
|
|
.s_frame_interval = x3d_set_frame_interval,
|
|
.get_fmt = x3d_get_fmt,
|
|
.set_fmt = x3d_set_fmt,
|
|
.s_stream = x3d_s_stream,
|
|
.close = x3d_deinit,
|
|
};
|
|
|
|
struct x3d_dev *g_x3d = NULL;
|
|
|
|
struct vdev_device *x3d_init(void *init_data)
|
|
{
|
|
int ret;
|
|
struct x3d_dev *sensor;
|
|
struct camera_res_cfg *cfg = (struct camera_res_cfg *)init_data;
|
|
|
|
sensor = malloc(sizeof(struct x3d_dev));
|
|
if (!sensor || !cfg)
|
|
return NULL;
|
|
|
|
memset(sensor, 0, sizeof(struct x3d_dev));
|
|
sensor->i2c_handle = cfg->i2c_handle;
|
|
sensor->sensor_addr = X3D_SERSOR_ADDR;
|
|
|
|
sensor->gpio_pwn = cfg->gpio[0];
|
|
sensor->gpio_intn = cfg->gpio[1];
|
|
sensor->vdev.priv_data = (void *)sensor;
|
|
|
|
sensor->lock = osMutexNew(NULL);
|
|
|
|
x3d_s_power(&sensor->vdev, 1);
|
|
ret = x3d_check_chip_id(sensor);
|
|
if (ret == 0)
|
|
x3d_s_stream(&sensor->vdev, 0);
|
|
x3d_s_power(&sensor->vdev, 0);
|
|
if (ret < 0)
|
|
goto err;
|
|
|
|
sensor->vdev.ep.bus_type = VDEV_MBUS_PARALLEL2;
|
|
sensor->vdev.frame_interval.numerator = 1;
|
|
sensor->vdev.frame_interval.denominator = s_x3d_fps;
|
|
sensor->vdev.fmt.width = s_x3d_width;
|
|
sensor->vdev.fmt.height = s_x3d_height;
|
|
sensor->vdev.fmt.code = VDEV_MBUS_FMT_YUYV;
|
|
sensor->vdev.fmt.field = VDEV_FIELD_NONE;
|
|
sensor->vdev.fmt.colorspace = VDEV_COLORSPACE_SRGB;
|
|
sensor->vdev.ops = x3d_vdev_ops;
|
|
|
|
g_x3d = sensor;
|
|
return &sensor->vdev;
|
|
|
|
err:
|
|
free(sensor);
|
|
return NULL;
|
|
}
|