1
2
3
4
5
6
7
8#include <linux/module.h>
9#include <linux/mutex.h>
10
11#include <media/v4l2-ctrls.h>
12#include <media/v4l2-device.h>
13#include <media/v4l2-ioctl.h>
14
15#include "adv748x.h"
16
17static int adv748x_csi2_set_virtual_channel(struct adv748x_csi2 *tx,
18 unsigned int vc)
19{
20 return tx_write(tx, ADV748X_CSI_VC_REF, vc << ADV748X_CSI_VC_REF_SHIFT);
21}
22
23
24
25
26
27
28
29
30
31
32
33
34
35static int adv748x_csi2_register_link(struct adv748x_csi2 *tx,
36 struct v4l2_device *v4l2_dev,
37 struct v4l2_subdev *src,
38 unsigned int src_pad,
39 bool enable)
40{
41 int ret;
42
43 if (!src->v4l2_dev) {
44 ret = v4l2_device_register_subdev(v4l2_dev, src);
45 if (ret)
46 return ret;
47 }
48
49 ret = media_create_pad_link(&src->entity, src_pad,
50 &tx->sd.entity, ADV748X_CSI2_SINK,
51 enable ? MEDIA_LNK_FL_ENABLED : 0);
52 if (ret)
53 return ret;
54
55 if (enable)
56 tx->src = src;
57
58 return 0;
59}
60
61
62
63
64
65
66
67
68
69static int adv748x_csi2_registered(struct v4l2_subdev *sd)
70{
71 struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
72 struct adv748x_state *state = tx->state;
73 int ret;
74
75 adv_dbg(state, "Registered %s (%s)", is_txa(tx) ? "TXA":"TXB",
76 sd->name);
77
78
79
80
81
82
83
84 if (is_afe_enabled(state)) {
85 ret = adv748x_csi2_register_link(tx, sd->v4l2_dev,
86 &state->afe.sd,
87 ADV748X_AFE_SOURCE,
88 is_txb(tx));
89 if (ret)
90 return ret;
91
92
93 if (is_txb(tx))
94 state->afe.tx = tx;
95 }
96
97
98 if (is_txb(tx) || !is_hdmi_enabled(state))
99 return 0;
100
101 ret = adv748x_csi2_register_link(tx, sd->v4l2_dev, &state->hdmi.sd,
102 ADV748X_HDMI_SOURCE, true);
103 if (ret)
104 return ret;
105
106
107 state->hdmi.tx = tx;
108
109 return 0;
110}
111
112static const struct v4l2_subdev_internal_ops adv748x_csi2_internal_ops = {
113 .registered = adv748x_csi2_registered,
114};
115
116
117
118
119
120static int adv748x_csi2_s_stream(struct v4l2_subdev *sd, int enable)
121{
122 struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
123 struct v4l2_subdev *src;
124
125 src = adv748x_get_remote_sd(&tx->pads[ADV748X_CSI2_SINK]);
126 if (!src)
127 return -EPIPE;
128
129 return v4l2_subdev_call(src, video, s_stream, enable);
130}
131
132static const struct v4l2_subdev_video_ops adv748x_csi2_video_ops = {
133 .s_stream = adv748x_csi2_s_stream,
134};
135
136
137
138
139
140
141
142
143static struct v4l2_mbus_framefmt *
144adv748x_csi2_get_pad_format(struct v4l2_subdev *sd,
145 struct v4l2_subdev_pad_config *cfg,
146 unsigned int pad, u32 which)
147{
148 struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
149
150 if (which == V4L2_SUBDEV_FORMAT_TRY)
151 return v4l2_subdev_get_try_format(sd, cfg, pad);
152
153 return &tx->format;
154}
155
156static int adv748x_csi2_get_format(struct v4l2_subdev *sd,
157 struct v4l2_subdev_pad_config *cfg,
158 struct v4l2_subdev_format *sdformat)
159{
160 struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
161 struct adv748x_state *state = tx->state;
162 struct v4l2_mbus_framefmt *mbusformat;
163
164 mbusformat = adv748x_csi2_get_pad_format(sd, cfg, sdformat->pad,
165 sdformat->which);
166 if (!mbusformat)
167 return -EINVAL;
168
169 mutex_lock(&state->mutex);
170
171 sdformat->format = *mbusformat;
172
173 mutex_unlock(&state->mutex);
174
175 return 0;
176}
177
178static int adv748x_csi2_set_format(struct v4l2_subdev *sd,
179 struct v4l2_subdev_pad_config *cfg,
180 struct v4l2_subdev_format *sdformat)
181{
182 struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
183 struct adv748x_state *state = tx->state;
184 struct v4l2_mbus_framefmt *mbusformat;
185 int ret = 0;
186
187 mbusformat = adv748x_csi2_get_pad_format(sd, cfg, sdformat->pad,
188 sdformat->which);
189 if (!mbusformat)
190 return -EINVAL;
191
192 mutex_lock(&state->mutex);
193
194 if (sdformat->pad == ADV748X_CSI2_SOURCE) {
195 const struct v4l2_mbus_framefmt *sink_fmt;
196
197 sink_fmt = adv748x_csi2_get_pad_format(sd, cfg,
198 ADV748X_CSI2_SINK,
199 sdformat->which);
200
201 if (!sink_fmt) {
202 ret = -EINVAL;
203 goto unlock;
204 }
205
206 sdformat->format = *sink_fmt;
207 }
208
209 *mbusformat = sdformat->format;
210
211unlock:
212 mutex_unlock(&state->mutex);
213
214 return ret;
215}
216
217static const struct v4l2_subdev_pad_ops adv748x_csi2_pad_ops = {
218 .get_fmt = adv748x_csi2_get_format,
219 .set_fmt = adv748x_csi2_set_format,
220};
221
222
223
224
225
226static const struct v4l2_subdev_ops adv748x_csi2_ops = {
227 .video = &adv748x_csi2_video_ops,
228 .pad = &adv748x_csi2_pad_ops,
229};
230
231
232
233
234
235int adv748x_csi2_set_pixelrate(struct v4l2_subdev *sd, s64 rate)
236{
237 struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
238
239 if (!tx->pixel_rate)
240 return -EINVAL;
241
242 return v4l2_ctrl_s_ctrl_int64(tx->pixel_rate, rate);
243}
244
245static int adv748x_csi2_s_ctrl(struct v4l2_ctrl *ctrl)
246{
247 switch (ctrl->id) {
248 case V4L2_CID_PIXEL_RATE:
249 return 0;
250 default:
251 return -EINVAL;
252 }
253}
254
255static const struct v4l2_ctrl_ops adv748x_csi2_ctrl_ops = {
256 .s_ctrl = adv748x_csi2_s_ctrl,
257};
258
259static int adv748x_csi2_init_controls(struct adv748x_csi2 *tx)
260{
261
262 v4l2_ctrl_handler_init(&tx->ctrl_hdl, 1);
263
264 tx->pixel_rate = v4l2_ctrl_new_std(&tx->ctrl_hdl,
265 &adv748x_csi2_ctrl_ops,
266 V4L2_CID_PIXEL_RATE, 1, INT_MAX,
267 1, 1);
268
269 tx->sd.ctrl_handler = &tx->ctrl_hdl;
270 if (tx->ctrl_hdl.error) {
271 v4l2_ctrl_handler_free(&tx->ctrl_hdl);
272 return tx->ctrl_hdl.error;
273 }
274
275 return v4l2_ctrl_handler_setup(&tx->ctrl_hdl);
276}
277
278int adv748x_csi2_init(struct adv748x_state *state, struct adv748x_csi2 *tx)
279{
280 int ret;
281
282 if (!is_tx_enabled(tx))
283 return 0;
284
285
286 adv748x_csi2_set_virtual_channel(tx, 0);
287
288 adv748x_subdev_init(&tx->sd, state, &adv748x_csi2_ops,
289 MEDIA_ENT_F_VID_IF_BRIDGE,
290 is_txa(tx) ? "txa" : "txb");
291
292
293 tx->sd.fwnode = of_fwnode_handle(state->endpoints[tx->port]);
294
295
296 tx->sd.internal_ops = &adv748x_csi2_internal_ops;
297
298 tx->pads[ADV748X_CSI2_SINK].flags = MEDIA_PAD_FL_SINK;
299 tx->pads[ADV748X_CSI2_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
300
301 ret = media_entity_pads_init(&tx->sd.entity, ADV748X_CSI2_NR_PADS,
302 tx->pads);
303 if (ret)
304 return ret;
305
306 ret = adv748x_csi2_init_controls(tx);
307 if (ret)
308 goto err_free_media;
309
310 ret = v4l2_async_register_subdev(&tx->sd);
311 if (ret)
312 goto err_free_ctrl;
313
314 return 0;
315
316err_free_ctrl:
317 v4l2_ctrl_handler_free(&tx->ctrl_hdl);
318err_free_media:
319 media_entity_cleanup(&tx->sd.entity);
320
321 return ret;
322}
323
324void adv748x_csi2_cleanup(struct adv748x_csi2 *tx)
325{
326 if (!is_tx_enabled(tx))
327 return;
328
329 v4l2_async_unregister_subdev(&tx->sd);
330 media_entity_cleanup(&tx->sd.entity);
331 v4l2_ctrl_handler_free(&tx->ctrl_hdl);
332}
333