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