1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3  * vimc-lens.c Virtual Media Controller Driver
4  * Copyright (C) 2022 Google, Inc
5  * Author: yunkec@google.com (Yunke Cao)
6  */
7 
8 #include <media/v4l2-ctrls.h>
9 #include <media/v4l2-event.h>
10 #include <media/v4l2-subdev.h>
11 
12 #include "vimc-common.h"
13 
14 #define VIMC_LENS_MAX_FOCUS_POS	1023
15 #define VIMC_LENS_MAX_FOCUS_STEP	1
16 
17 struct vimc_lens_device {
18 	struct vimc_ent_device ved;
19 	struct v4l2_subdev sd;
20 	struct v4l2_ctrl_handler hdl;
21 	u32 focus_absolute;
22 };
23 
24 static const struct v4l2_subdev_core_ops vimc_lens_core_ops = {
25 	.log_status = v4l2_ctrl_subdev_log_status,
26 	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
27 	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
28 };
29 
30 static const struct v4l2_subdev_ops vimc_lens_ops = {
31 	.core = &vimc_lens_core_ops
32 };
33 
34 static int vimc_lens_s_ctrl(struct v4l2_ctrl *ctrl)
35 {
36 	struct vimc_lens_device *vlens =
37 		container_of(ctrl->handler, struct vimc_lens_device, hdl);
38 	if (ctrl->id == V4L2_CID_FOCUS_ABSOLUTE) {
39 		vlens->focus_absolute = ctrl->val;
40 		return 0;
41 	}
42 	return -EINVAL;
43 }
44 
45 static const struct v4l2_ctrl_ops vimc_lens_ctrl_ops = {
46 	.s_ctrl = vimc_lens_s_ctrl,
47 };
48 
49 static struct vimc_ent_device *vimc_lens_add(struct vimc_device *vimc,
50 					     const char *vcfg_name)
51 {
52 	struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
53 	struct vimc_lens_device *vlens;
54 	int ret;
55 
56 	/* Allocate the vlens struct */
57 	vlens = kzalloc(sizeof(*vlens), GFP_KERNEL);
58 	if (!vlens)
59 		return ERR_PTR(-ENOMEM);
60 
61 	v4l2_ctrl_handler_init(&vlens->hdl, 1);
62 
63 	v4l2_ctrl_new_std(&vlens->hdl, &vimc_lens_ctrl_ops,
64 			  V4L2_CID_FOCUS_ABSOLUTE, 0,
65 			  VIMC_LENS_MAX_FOCUS_POS, VIMC_LENS_MAX_FOCUS_STEP, 0);
66 	vlens->sd.ctrl_handler = &vlens->hdl;
67 	if (vlens->hdl.error) {
68 		ret = vlens->hdl.error;
69 		goto err_free_vlens;
70 	}
71 	vlens->ved.dev = vimc->mdev.dev;
72 
73 	ret = vimc_ent_sd_register(&vlens->ved, &vlens->sd, v4l2_dev,
74 				   vcfg_name, MEDIA_ENT_F_LENS, 0,
75 				   NULL, &vimc_lens_ops);
76 	if (ret)
77 		goto err_free_hdl;
78 
79 	return &vlens->ved;
80 
81 err_free_hdl:
82 	v4l2_ctrl_handler_free(&vlens->hdl);
83 err_free_vlens:
84 	kfree(vlens);
85 
86 	return ERR_PTR(ret);
87 }
88 
89 static void vimc_lens_release(struct vimc_ent_device *ved)
90 {
91 	struct vimc_lens_device *vlens =
92 		container_of(ved, struct vimc_lens_device, ved);
93 
94 	v4l2_ctrl_handler_free(&vlens->hdl);
95 	media_entity_cleanup(vlens->ved.ent);
96 	kfree(vlens);
97 }
98 
99 struct vimc_ent_type vimc_lens_type = {
100 	.add = vimc_lens_add,
101 	.release = vimc_lens_release
102 };
103