1 /*
2 * mipi_v4l2_adapter.c
3 *
4 * Mipi v4l2 adapter driver.
5 *
6 * Copyright (c) 2021-2023 Huawei Device Co., Ltd.
7 *
8 * This software is licensed under the terms of the GNU General Public
9 * License version 2, as published by the Free Software Foundation, and
10 * may be copied, distributed, and modified under those terms.
11 *
12 * This program is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 */
18
19 #include <asm/unaligned.h>
20 #include <media/v4l2-ctrls.h>
21 #include <media/v4l2-device.h>
22 #include <media/v4l2-event.h>
23 #include <media/v4l2-fwnode.h>
24 #include <linux/acpi.h>
25 #include <linux/i2c.h>
26 #include <linux/slab.h>
27 #include <linux/module.h>
28 #include <linux/version.h>
29 #include "hdf_log.h"
30 #include "mipi_csi_core.h"
31
32 #define HDF_LOG_TAG mipi_v4l2_adapter
33 #define SENSOR_FLL_MAX 0xffff
34 #define PIXEL_RATE_DIVISOR 10
35 #define LINKS_COUNT 2
36 #define LANES_COUNT 4
37 #define CTRLS_COUNT 10
38
39 /* Mode : resolution and related config&values */
40 struct CameraSensorMode {
41 /* V-timing */
42 u32 fll_def;
43 u32 fll_min;
44
45 /* H-timing */
46 u32 llp;
47 };
48
49 struct CameraDrvData {
50 struct v4l2_subdev *sd;
51 struct media_pad *pad;
52
53 /* V4L2 Controls */
54 struct v4l2_ctrl_handler *ctrl_handler;
55 struct v4l2_ctrl *link_freq;
56 struct v4l2_ctrl *pixel_rate;
57 struct v4l2_ctrl *vblank;
58 struct v4l2_ctrl *hblank;
59 struct v4l2_ctrl *exposure;
60 struct v4l2_ctrl *vflip;
61 struct v4l2_ctrl *hflip;
62
63 /* Current mode */
64 const struct CameraSensorMode *cur_mode;
65 s64 link_freqs; /* CSI-2 link frequencies, Application under v4l2 framework */
66
67 /* Streaming on/off */
68 bool streaming;
69 };
70
71 struct AdapterDrvData {
72 struct CameraDrvData *camera;
73 ComboDevAttr *attr;
74
75 struct v4l2_subdev_format fmt;
76 /*
77 * Mutex for serialized access:
78 * Protect sensor set pad format and start/stop streaming safely.
79 * Protect access to sensor v4l2 controls.
80 */
81 struct mutex mutex;
82 };
83
84 static struct AdapterDrvData g_adapterDrvData;
85
86 /**
87 * Get bayer order based on flip setting.
88 * ref. linuxtv.org/downloads/v4l-dvb-apis/userspace-api/v4l/subdev-formats.html?highlight=media_bus_fmt_uv8_1x8
89 */
LinuxGetFormatCode(DataType dataType)90 static u32 LinuxGetFormatCode(DataType dataType)
91 {
92 u32 code = MEDIA_BUS_FMT_SGBRG12_1X12;
93
94 switch (dataType) {
95 case DATA_TYPE_RAW_8BIT:
96 code = MEDIA_BUS_FMT_SBGGR8_1X8;
97 break;
98 case DATA_TYPE_RAW_10BIT:
99 code = MEDIA_BUS_FMT_SBGGR10_1X10;
100 break;
101 case DATA_TYPE_RAW_12BIT:
102 code = MEDIA_BUS_FMT_SBGGR12_1X12;
103 break;
104 case DATA_TYPE_RAW_14BIT:
105 code = MEDIA_BUS_FMT_SBGGR14_1X14;
106 break;
107 case DATA_TYPE_RAW_16BIT:
108 code = MEDIA_BUS_FMT_SBGGR16_1X16;
109 break;
110 case DATA_TYPE_YUV420_8BIT_NORMAL:
111 code = MEDIA_BUS_FMT_Y8_1X8;
112 break;
113 case DATA_TYPE_YUV420_8BIT_LEGACY:
114 code = MEDIA_BUS_FMT_Y8_1X8;
115 break;
116 case DATA_TYPE_YUV422_8BIT:
117 code = MEDIA_BUS_FMT_YUYV8_1X16;
118 break;
119 case DATA_TYPE_YUV422_PACKED:
120 code = MEDIA_BUS_FMT_YVYU8_1X16;
121 break;
122 default:
123 break;
124 }
125
126 return code;
127 }
128
LinuxEnumMbusCode(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_mbus_code_enum * code)129 static int LinuxEnumMbusCode(struct v4l2_subdev *sd,
130 struct v4l2_subdev_pad_config *cfg, struct v4l2_subdev_mbus_code_enum *code)
131 {
132 struct AdapterDrvData *drvData = &g_adapterDrvData;
133 (void)sd;
134 (void)cfg;
135
136 if (code == NULL) {
137 HDF_LOGE("LinuxEnumMbusCode: code is null!");
138 return -EINVAL;
139 }
140 if (code->index > 0) {
141 HDF_LOGE("LinuxEnumMbusCode: code->index is invalid!");
142 return -EINVAL;
143 }
144
145 mutex_lock(&drvData->mutex);
146 code->code = drvData->fmt.format.code;
147 mutex_unlock(&drvData->mutex);
148
149 return 0;
150 }
151
LinuxUpdatePadFormat(const struct AdapterDrvData * drvData,const struct CameraSensorMode * mode,struct v4l2_subdev_format * fmt)152 static void LinuxUpdatePadFormat(const struct AdapterDrvData *drvData,
153 const struct CameraSensorMode *mode, struct v4l2_subdev_format *fmt)
154 {
155 (void)mode;
156
157 if ((drvData == NULL) || (fmt == NULL)) {
158 HDF_LOGE("LinuxUpdatePadFormat: drvData or fmt is null!");
159 return;
160 }
161 fmt->format = drvData->fmt.format;
162 }
163
LinuxSetPadFormat(struct v4l2_subdev * sd,struct v4l2_subdev_pad_config * cfg,struct v4l2_subdev_format * fmt)164 static int LinuxSetPadFormat(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
165 struct v4l2_subdev_format *fmt)
166 {
167 s32 vblank_def;
168 s32 vblank_min;
169 s64 h_blank;
170 u64 pixel_rate;
171 u32 height;
172 struct AdapterDrvData *drvData = &g_adapterDrvData;
173 struct CameraDrvData *camera = drvData->camera;
174 const struct CameraSensorMode *mode = camera->cur_mode;
175 ImgRect *rect = &drvData->attr->imgRect;
176
177 (void)sd;
178 (void)cfg;
179 mutex_lock(&drvData->mutex);
180 /*
181 * Only one bayer order is supported.
182 * It depends on the flip settings.
183 */
184 fmt->format.code = drvData->fmt.format.code;
185 LinuxUpdatePadFormat(drvData, mode, fmt);
186
187 pixel_rate = camera->link_freqs * LINKS_COUNT * LANES_COUNT;
188 do_div(pixel_rate, PIXEL_RATE_DIVISOR);
189 __v4l2_ctrl_s_ctrl_int64(camera->pixel_rate, pixel_rate);
190 /* Update limits and set FPS to default */
191 height = rect->height;
192 vblank_def = camera->cur_mode->fll_def - height;
193 vblank_min = camera->cur_mode->fll_min - height;
194 height = SENSOR_FLL_MAX - height;
195 __v4l2_ctrl_modify_range(camera->vblank, vblank_min, height, 1, vblank_def);
196 __v4l2_ctrl_s_ctrl(camera->vblank, vblank_def);
197 h_blank = mode->llp - rect->width;
198 /*
199 * Currently hblank is not changeable.
200 * So FPS control is done only by vblank.
201 */
202 __v4l2_ctrl_modify_range(camera->hblank, h_blank, h_blank, 1, h_blank);
203 mutex_unlock(&drvData->mutex);
204
205 return 0;
206 }
207
SetStream(int enable)208 static int SetStream(int enable)
209 {
210 struct AdapterDrvData *drvData = &g_adapterDrvData;
211 struct CameraDrvData *camera = drvData->camera;
212
213 mutex_lock(&drvData->mutex);
214 if (camera->streaming == enable) {
215 mutex_unlock(&drvData->mutex);
216 HDF_LOGE("SetStream: streaming-flag is not change!");
217 return 0;
218 }
219
220 camera->streaming = enable;
221
222 /* vflip and hflip cannot change during streaming */
223 #if LINUX_VERSION_CODE > KERNEL_VERSION(4, 19, 249)
224 __v4l2_ctrl_grab(camera->vflip, enable);
225 __v4l2_ctrl_grab(camera->hflip, enable);
226 #else
227 v4l2_ctrl_grab(camera->vflip, enable);
228 v4l2_ctrl_grab(camera->hflip, enable);
229 #endif
230 mutex_unlock(&drvData->mutex);
231
232 return 0;
233 }
234
235 static const struct v4l2_subdev_video_ops g_cameraVideoOps = {
236 .s_stream = NULL,
237 };
238
239 static const struct v4l2_subdev_pad_ops g_cameraPadOps = {
240 .enum_mbus_code = LinuxEnumMbusCode,
241 .set_fmt = LinuxSetPadFormat,
242 };
243
244 static const struct v4l2_subdev_ops g_cameraSubdevOps = {
245 .video = &g_cameraVideoOps,
246 .pad = &g_cameraPadOps,
247 };
248
249 /* Initialize control handlers */
LinuxInitControls(struct AdapterDrvData * drvData)250 static int LinuxInitControls(struct AdapterDrvData *drvData)
251 {
252 s64 vblank_def;
253 s64 vblank_min;
254 s64 hblank;
255 u64 pixel_rate;
256 int ret;
257
258 struct CameraDrvData *camera = drvData->camera;
259 struct v4l2_ctrl_handler *ctrl_hdlr = camera->ctrl_handler;
260 const struct CameraSensorMode *mode = camera->cur_mode;
261 ImgRect *rect = &(drvData->attr->imgRect);
262
263 if (ctrl_hdlr == NULL) {
264 ctrl_hdlr = kzalloc(sizeof(*ctrl_hdlr), GFP_KERNEL);
265 if (!ctrl_hdlr) {
266 HDF_LOGE("LinuxInitControls: [kzalloc] fail!");
267 return -ENOMEM;
268 }
269 ret = v4l2_ctrl_handler_init(ctrl_hdlr, CTRLS_COUNT);
270 if (ret != 0) {
271 kfree(ctrl_hdlr);
272 HDF_LOGE("LinuxInitControls: [v4l2_ctrl_handler_init] fail!");
273 return ret;
274 }
275 camera->ctrl_handler = ctrl_hdlr;
276 }
277 ctrl_hdlr->lock = &drvData->mutex;
278 if (camera->link_freq == NULL) {
279 camera->link_freq = v4l2_ctrl_new_std(ctrl_hdlr, NULL,
280 V4L2_CID_LINK_FREQ, camera->link_freqs, camera->link_freqs, 1, camera->link_freqs);
281 if (camera->link_freq) {
282 camera->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
283 }
284 }
285
286 if (camera->pixel_rate == NULL) {
287 /* pixel_rate = link_freq * 2 * nr_of_lanes / bits_per_sample */
288 pixel_rate = camera->link_freqs * LINKS_COUNT * LANES_COUNT;
289 do_div(pixel_rate, PIXEL_RATE_DIVISOR);
290 /* By default, PIXEL_RATE is read only */
291 camera->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, NULL,
292 V4L2_CID_PIXEL_RATE, pixel_rate, pixel_rate, 1, pixel_rate);
293 }
294
295 /* Initialize vblank/hblank/exposure parameters based on current mode */
296 if (camera->vblank == NULL) {
297 vblank_def = mode->fll_def - rect->height;
298 vblank_min = mode->fll_min - rect->height;
299 camera->vblank = v4l2_ctrl_new_std(ctrl_hdlr, NULL,
300 V4L2_CID_VBLANK, vblank_min, SENSOR_FLL_MAX - rect->height, 1, vblank_def);
301 }
302
303 if (camera->hblank == NULL) {
304 hblank = mode->llp - rect->width;
305 camera->hblank = v4l2_ctrl_new_std(ctrl_hdlr, NULL,
306 V4L2_CID_HBLANK, hblank, hblank, 1, hblank);
307 if (camera->hblank) {
308 camera->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
309 }
310 }
311 if (ctrl_hdlr->error) {
312 ret = ctrl_hdlr->error;
313 HDF_LOGE("LinuxInitControls: control init fail, ret: %d!", ret);
314 v4l2_ctrl_handler_free(ctrl_hdlr);
315 kfree(ctrl_hdlr);
316 return ret;
317 }
318 camera->sd->ctrl_handler = ctrl_hdlr;
319
320 return 0;
321 }
322
MipiCsiAdapterTraceMipiCfg(ComboDevAttr * attr)323 static int32_t MipiCsiAdapterTraceMipiCfg(ComboDevAttr *attr)
324 {
325 unsigned int i;
326 MipiDevAttr *cfg = NULL;
327
328 if (attr == NULL) {
329 HDF_LOGE("MipiCsiAdapterTraceMipiCfg: attr is null!");
330 return HDF_ERR_INVALID_PARAM;
331 }
332
333 cfg = &(attr->mipiAttr);
334 HDF_LOGD("MipiCsiAdapterTraceMipiCfg: inputDataType = %d, wdrMode = %d!", cfg->inputDataType, cfg->wdrMode);
335 for (i = 0; i < MIPI_LANE_NUM; i++) {
336 HDF_LOGD("MipiCsiAdapterTraceMipiCfg: laneId[%d] = %d!", i, cfg->laneId[i]);
337 }
338
339 HDF_LOGD("MipiCsiAdapterTraceMipiCfg: inputMode = %d, dataRate = %d, \
340 imgRect(x = %d, y = %d, width = %d, height = %d)!",
341 attr->inputMode, attr->dataRate, attr->imgRect.x, attr->imgRect.y,
342 attr->imgRect.width, attr->imgRect.height);
343
344 return HDF_SUCCESS;
345 }
346
MipiCsiAdapterTraceCameraCfg(struct CameraDrvData * camera)347 static int32_t MipiCsiAdapterTraceCameraCfg(struct CameraDrvData *camera)
348 {
349 const struct CameraSensorMode *mode = NULL;
350
351 if (camera == NULL) {
352 HDF_LOGE("MipiCsiAdapterTraceCameraCfg: camera is null!");
353 return HDF_ERR_INVALID_PARAM;
354 }
355
356 mode = camera->cur_mode;
357 HDF_LOGD("MipiCsiAdapterTraceCameraCfg: link_freq = %lld!", camera->link_freqs);
358 HDF_LOGD("MipiCsiAdapterTraceCameraCfg: fll_def = %d, fll_min = %d, llp = %d!",
359 mode->fll_def, mode->fll_min, mode->llp);
360
361 return HDF_SUCCESS;
362 }
363
MipiCsiAdapterSetComboDevAttr(struct MipiCsiCntlr * cntlr,ComboDevAttr * pAttr)364 static int32_t MipiCsiAdapterSetComboDevAttr(struct MipiCsiCntlr *cntlr, ComboDevAttr *pAttr)
365 {
366 int32_t ret;
367 struct AdapterDrvData *drvData = (struct AdapterDrvData *)cntlr->priv;
368 struct v4l2_mbus_framefmt *fmt = NULL;
369
370 if ((drvData == NULL) || (pAttr == NULL)) {
371 HDF_LOGE("MipiCsiAdapterSetComboDevAttr: drvData or pAttr is null!");
372 return HDF_ERR_INVALID_PARAM;
373 }
374
375 drvData->attr = pAttr;
376 fmt = &drvData->fmt.format;
377 fmt->width = pAttr->imgRect.width;
378 fmt->height = pAttr->imgRect.height;
379 fmt->code = LinuxGetFormatCode(pAttr->mipiAttr.inputDataType);
380 fmt->field = V4L2_FIELD_NONE;
381
382 ret = MipiCsiAdapterTraceMipiCfg(pAttr);
383
384 return ret;
385 }
386
MipiCsiAdapterResetRx(struct MipiCsiCntlr * cntlr,uint8_t comboDev)387 static int32_t MipiCsiAdapterResetRx(struct MipiCsiCntlr *cntlr, uint8_t comboDev)
388 {
389 int32_t ret;
390
391 (void)cntlr;
392 ret = SetStream(1);
393 return (ret == 0) ? HDF_SUCCESS : HDF_FAILURE;
394 }
395
MipiCsiAdapterUnresetRx(struct MipiCsiCntlr * cntlr,uint8_t comboDev)396 static int32_t MipiCsiAdapterUnresetRx(struct MipiCsiCntlr *cntlr, uint8_t comboDev)
397 {
398 int32_t ret;
399
400 (void)cntlr;
401 ret = SetStream(0);
402 return (ret == 0) ? HDF_SUCCESS : HDF_FAILURE;
403 }
404
MipiCsiAdapterProbeV4l2(struct MipiCsiCntlr * cntlr)405 static int32_t MipiCsiAdapterProbeV4l2(struct MipiCsiCntlr *cntlr)
406 {
407 int32_t ret;
408 struct AdapterDrvData *drvData = (struct AdapterDrvData *)cntlr->priv;
409 struct CameraDrvData *camera = NULL;
410
411 if ((drvData == NULL) || (drvData->camera == NULL)) {
412 HDF_LOGE("MipiCsiAdapterProbeV4l2: drvData or drvData->camera is null!");
413 return HDF_ERR_INVALID_PARAM;
414 }
415
416 camera = drvData->camera;
417 /* Initialize subdev */
418 v4l2_subdev_init(camera->sd, &g_cameraSubdevOps);
419 ret = LinuxInitControls(drvData);
420 if (ret) {
421 HDF_LOGE("MipiCsiAdapterProbeV4l2: fail to init controls, ret: %d!", ret);
422 return HDF_FAILURE;
423 }
424
425 mutex_init(&drvData->mutex);
426 return HDF_SUCCESS;
427 }
428
MipiCsiAdapterSetDrvData(struct MipiCsiCntlr * cntlr,void * cameraData)429 static int32_t MipiCsiAdapterSetDrvData(struct MipiCsiCntlr *cntlr, void *cameraData)
430 {
431 int32_t ret;
432 struct AdapterDrvData *drvData = (struct AdapterDrvData *)cntlr->priv;
433
434 if ((drvData == NULL) || (cameraData == NULL)) {
435 HDF_LOGE("MipiCsiAdapterSetDrvData: drvData or cameraData is null!");
436 return HDF_ERR_INVALID_PARAM;
437 }
438 drvData->camera = (struct CameraDrvData *)cameraData;
439 ret = MipiCsiAdapterTraceCameraCfg(drvData->camera);
440 if (ret != HDF_SUCCESS) {
441 HDF_LOGE("MipiCsiAdapterSetDrvData: trace camera cfg fail!");
442 return HDF_FAILURE;
443 }
444 ret = MipiCsiAdapterProbeV4l2(cntlr);
445
446 return ret;
447 }
448
449 static struct MipiCsiCntlr g_mipiRx = {
450 .devNo = 0
451 };
452
453 static struct MipiCsiCntlrMethod g_method = {
454 .setComboDevAttr = MipiCsiAdapterSetComboDevAttr,
455 .resetRx = MipiCsiAdapterResetRx,
456 .unresetRx = MipiCsiAdapterUnresetRx,
457 .setDrvData = MipiCsiAdapterSetDrvData,
458 };
459
MipiCsiAdapterRemoveV4l2(const struct MipiCsiCntlr * cntlr)460 static void MipiCsiAdapterRemoveV4l2(const struct MipiCsiCntlr *cntlr)
461 {
462 struct AdapterDrvData *drvData = (struct AdapterDrvData *)cntlr->priv;
463 struct CameraDrvData *camera = NULL;
464
465 if ((drvData == NULL) || (drvData->camera == NULL)) {
466 HDF_LOGE("MipiCsiAdapterRemoveV4l2: drvData or drvData->camera is null!");
467 return;
468 }
469
470 camera = drvData->camera;
471 if ((camera->sd != NULL) && (camera->sd->ctrl_handler != NULL)) {
472 v4l2_ctrl_handler_free(camera->sd->ctrl_handler);
473 kfree(camera->sd->ctrl_handler);
474 camera->sd->ctrl_handler = NULL;
475 }
476 mutex_destroy(&drvData->mutex);
477 drvData->attr = NULL;
478 drvData->camera = NULL;
479 }
480
MipiCsiAdapterBind(struct HdfDeviceObject * device)481 static int32_t MipiCsiAdapterBind(struct HdfDeviceObject *device)
482 {
483 int32_t ret;
484
485 HDF_LOGI("MipiCsiAdapterBind: enter!");
486 g_mipiRx.priv = &g_adapterDrvData;
487 g_mipiRx.ops = &g_method;
488 ret = MipiCsiRegisterCntlr(&g_mipiRx, device);
489 if (ret != HDF_SUCCESS) {
490 return ret;
491 }
492 HDF_LOGI("MipiCsiAdapterBind: success!");
493
494 return HDF_SUCCESS;
495 }
496
MipiCsiAdapterInit(struct HdfDeviceObject * device)497 static int32_t MipiCsiAdapterInit(struct HdfDeviceObject *device)
498 {
499 (void)device;
500
501 HDF_LOGI("MipiCsiAdapterInit: success!");
502 return HDF_SUCCESS;
503 }
504
MipiCsiAdapterRelease(struct HdfDeviceObject * device)505 static void MipiCsiAdapterRelease(struct HdfDeviceObject *device)
506 {
507 struct MipiCsiCntlr *cntlr = NULL;
508
509 if (device == NULL) {
510 HDF_LOGE("MipiCsiAdapterRelease: device is null!");
511 return;
512 }
513 cntlr = MipiCsiCntlrFromDevice(device);
514 if (cntlr == NULL) {
515 HDF_LOGE("MipiCsiAdapterRelease: cntlr is null!");
516 return;
517 }
518 MipiCsiAdapterRemoveV4l2(cntlr);
519 MipiCsiUnregisterCntlr(cntlr);
520 cntlr->priv = NULL;
521
522 HDF_LOGI("MipiCsiAdapterRelease: success!");
523 }
524
525 struct HdfDriverEntry g_mipiCsiLinuxDriverEntry = {
526 .moduleVersion = 1,
527 .Bind = MipiCsiAdapterBind,
528 .Init = MipiCsiAdapterInit,
529 .Release = MipiCsiAdapterRelease,
530 .moduleName = "linux_mipi_csi_adapter",
531 };
532 HDF_INIT(g_mipiCsiLinuxDriverEntry);
533
534