SUBDEV驱动的set_fmt和get_fmt实现

开发环境：正点原子RV1126开发板
内核版本：4.19.111

用户API

int ioctl(int fd, VIDIOC_SUBDEV_S_FMT, struct v4l2_subdev_foramt *argp);
int ioctl(int fd, VIDIOC_SUBDEV_G_FMT, struct v4l2_subdev_foramt *argp);

内核API

/* 
1. 仅当format->which == V4L2_SUBDEV_FORMAT_TRY时，cfg有效，否则为NULL 
2. cfg有效时是一个数组，数组下标即pad_id
*/  
int set_fmt(struct v4l2_subdev *sd,  
    struct v4l2_subdev_pad_config* cfg,  
    struct v4l2_subdev_foramt *format)；
int get_fmt(struct v4l2_subdev *sd,
    struct v4l2_subdev_pad_config *cfg,
    struct v4l2_subdev_format *fmt);

/*
1. set_fmt实现只使用try_fmt成员
*/
struct v4l2_subdev_pad_config {  
    struct v4l2_mbus_framefmt try_fmt;
    struct v4l2_rect try_crop;  
    struct v4l2_rect try_compose;  
};

/ *frame format on the media bus* /  
struct v4l2_mbus_framefmt {  
    __u32 width; //image width  
    __u32 height; //image height  
    __u32 code; //data format code (from enum v4l2_mbus_pixelcode)  
    __u32 field; //used interlacing type (from enum v4l2_field)  
    __u32 colorspace; //colorspace of the data (from enum v4l2_colorspace)  
    __u32 ycbcrenc; //YCbCr encoding of the data (from enum v4l2_ycbcr_encoding)  
    __u32 quantization; //quantization of the data (from enum v4l2_quantization)  
    __u32 xfer_func; //transfer function of the data (from enum v4l2_xfer_func)  
    __u16 reserved[11];  
};

/ *Pad-level media bus format* /  
struct v4l2_subdev_format {  
    __u32 which; //format type (from enum v4l2_subdev_format_whence)  
    __u32 pad; //pad number, as reported by the media API  
    struct v4l2_mbus_framefmt format; //media bus format (format code and frame size)  
    __u32 reserved[8];  
};

驱动实现

set_fmt实现

set_fmt的基本逻辑：
1.寻找与申请一致的mbus_code，否则使用supported_mbus_code[0]；寻找与申请差距最小的frame_size。
2.TRY模式向fmt->format和cfg更新协商后配置；ACTIVE模式向fmt->format更新协商后配置，以及硬件操作。
set_fmt的注意事项：
1.辅助函数__v4l2_find_nearest_size()前需检查cfg非NULL，如果是NULL不要报错，因为要兼容旧模式。 2.除了code、width、height成员，其他成员赋默认值，不要向应用层提供不确定值。

struct imx415 {
    struct i2c_client *client;
    struct v4l2_subdev subdev;
    struct mutex mutex;
    struct v4l2_mbus_framefmt active_fmt;
};

static const u32 supported_mbus_code[] = {
    MEDIA_BUS_FMT_SGBRG10_1X10,
    MEDIA_BUS_FMT_SGBRG12_1X12,
};
static const struct v4l2_frmsize_discrete supported_frmsize[] = {
    {.width = 3864, .height = 2192,},
    {.width = 1944, .height = 1097,}
};
static const s64 supported_link_freq[] = {
    891000000,
    446000000,
    743000000,
    297000000,
};

static int imx415_set_fmt(struct v4l2_subdev *sd,
                struct v4l2_subdev_pad_config *cfg,
                struct v4l2_subdev_format *fmt)
{
    struct imx415 *imx415 = to_imx415(sd);
    u32 i = 0;
    u32 mbus_code_id = 0;
    u32 frmsize_id = 0;
    const struct v4l2_frmsize_discrete *best_frmsize;

    /* Pad validity check: sensor only has pad0 */
    if (fmt->pad != 0) {
        dev_err(sd->dev, "Invalid pad: %d\n", fmt->pad);
        return -EINVAL;
    }

    /* Check fmt->which validity: only support TRY / ACTIVE modes */
    if (fmt->which != V4L2_SUBDEV_FORMAT_TRY && fmt->which != V4L2_SUBDEV_FORMAT_ACTIVE) {
        dev_err(sd->dev, "Invalid format mode: %d\n", fmt->which);
        return -EINVAL;
    }

    /* Find matching mbus_code, use the first if not matched */
    for (i = 0; i < ARRAY_SIZE(supported_mbus_code); i++) {
        if (fmt->format.code == supported_mbus_code[i]) {
            mbus_code_id = i;
            break;
        }
    }
    if (i == ARRAY_SIZE(supported_mbus_code)) {
        mbus_code_id = 0;
        dev_warn(sd->dev, "Unsupported mbus format: 0x%x, use default 0x%x\n", fmt->format.code, supported_mbus_code[0]);
    }

    /* 寻找与申请差距最小的frame_size */
    best_frmsize = __v4l2_find_nearest_size(supported_frmsize, ARRAY_SIZE(supported_frmsize),
			 sizeof(struct v4l2_frmsize_discrete), offsetof(struct v4l2_frmsize_discrete, width),
			 offsetof(struct v4l2_frmsize_discrete, height), fmt->format.width, fmt->format.height);
    frmsize_id = best_frmsize - supported_frmsize;

    /* fmt->foramt赋值，除了width、height、code外赋默认值 */
    fmt->format.height = supported_frmsize[frmsize_id].height;
    fmt->format.width = supported_frmsize[frmsize_id].width;
    fmt->format.code = supported_mbus_code[mbus_code_id];
    fmt->format.field = V4L2_FIELD_NONE;
    fmt->format.colorspace = V4L2_COLORSPACE_SRGB;
    fmt->format.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
    fmt->format.quantization = V4L2_QUANTIZATION_DEFAULT;
    fmt->format.xfer_func = V4L2_XFER_FUNC_DEFAULT;

    if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
    {
        if (cfg) {
            *v4l2_subdev_get_try_format(sd, cfg, fmt->pad) = fmt->format;
        }
    } else {
        mutex_lock(&imx415->mutex);
        
        /* 这里可以添加对硬件寄存器的配置代码，设置传感器的输出格式和分辨率 */

        imx415->active_fmt = fmt->format;
        mutex_unlock(&imx415->mutex);
    }

    return 0;
}

get_fmt实现

static int imx415_get_fmt(struct v4l2_subdev *sd,
		       struct v4l2_subdev_pad_config *cfg,
		       struct v4l2_subdev_format *fmt)
{
    struct imx415 *imx415 = to_imx415(sd);

    /* Pad validity check: sensor only has pad0 */
    if (fmt->pad != 0) {
        dev_err(sd->dev, "Invalid pad: %d\n", fmt->pad);
        return -EINVAL;
    }

    /* Check fmt->which validity: only support TRY / ACTIVE modes */
    if (fmt->which != V4L2_SUBDEV_FORMAT_TRY && fmt->which != V4L2_SUBDEV_FORMAT_ACTIVE) {
        dev_err(sd->dev, "Invalid format mode: %d\n", fmt->which);
        return -EINVAL;
    }

    if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
        if (cfg) {
            fmt->format = *v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
        }
    } else {
        mutex_lock(&imx415->mutex);
        fmt->format = imx415->active_fmt;
        mutex_unlock(&imx415->mutex);
    }
    
    return 0;
}