b886b72daa04b65b4f7e7c63615affb3481acecb
[media-ctl.git] / src / v4l2subdev.c
1 /*
2  * V4L2 subdev interface library
3  *
4  * Copyright (C) 2010-2011 Ideas on board SPRL
5  *
6  * Contact: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
7  *
8  * This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU Lesser General Public License as published
10  * by the Free Software Foundation; either version 2.1 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16  * GNU Lesser General Public License for more details.
17  *
18  * You should have received a copy of the GNU Lesser General Public License
19  * along with this program. If not, see <http://www.gnu.org/licenses/>.
20  */
21
22 #include <sys/ioctl.h>
23 #include <sys/stat.h>
24 #include <sys/types.h>
25
26 #include <errno.h>
27 #include <fcntl.h>
28 #include <stdio.h>
29 #include <string.h>
30 #include <unistd.h>
31
32 #include <linux/v4l2-subdev.h>
33
34 #include "mediactl.h"
35 #include "v4l2subdev.h"
36 #include "tools.h"
37
38 int v4l2_subdev_open(struct media_entity *entity)
39 {
40         if (entity->fd != -1)
41                 return 0;
42
43         entity->fd = open(entity->devname, O_RDWR);
44         if (entity->fd == -1) {
45                 media_dbg(entity->media,
46                           "%s: Failed to open subdev device node %s\n", __func__,
47                           entity->devname);
48                 return -errno;
49         }
50
51         return 0;
52 }
53
54 void v4l2_subdev_close(struct media_entity *entity)
55 {
56         close(entity->fd);
57         entity->fd = -1;
58 }
59
60 int v4l2_subdev_get_format(struct media_entity *entity,
61         struct v4l2_mbus_framefmt *format, unsigned int pad,
62         enum v4l2_subdev_format_whence which)
63 {
64         struct v4l2_subdev_format fmt;
65         int ret;
66
67         ret = v4l2_subdev_open(entity);
68         if (ret < 0)
69                 return ret;
70
71         memset(&fmt, 0, sizeof(fmt));
72         fmt.pad = pad;
73         fmt.which = which;
74
75         ret = ioctl(entity->fd, VIDIOC_SUBDEV_G_FMT, &fmt);
76         if (ret < 0)
77                 return -errno;
78
79         *format = fmt.format;
80         return 0;
81 }
82
83 int v4l2_subdev_set_format(struct media_entity *entity,
84         struct v4l2_mbus_framefmt *format, unsigned int pad,
85         enum v4l2_subdev_format_whence which)
86 {
87         struct v4l2_subdev_format fmt;
88         int ret;
89
90         ret = v4l2_subdev_open(entity);
91         if (ret < 0)
92                 return ret;
93
94         memset(&fmt, 0, sizeof(fmt));
95         fmt.pad = pad;
96         fmt.which = which;
97         fmt.format = *format;
98
99         ret = ioctl(entity->fd, VIDIOC_SUBDEV_S_FMT, &fmt);
100         if (ret < 0)
101                 return -errno;
102
103         *format = fmt.format;
104         return 0;
105 }
106
107 int v4l2_subdev_get_crop(struct media_entity *entity, struct v4l2_rect *rect,
108                          unsigned int pad, enum v4l2_subdev_format_whence which)
109 {
110         struct v4l2_subdev_crop crop;
111         int ret;
112
113         ret = v4l2_subdev_open(entity);
114         if (ret < 0)
115                 return ret;
116
117         memset(&crop, 0, sizeof(crop));
118         crop.pad = pad;
119         crop.which = which;
120
121         ret = ioctl(entity->fd, VIDIOC_SUBDEV_G_CROP, &crop);
122         if (ret < 0)
123                 return -errno;
124
125         *rect = crop.rect;
126         return 0;
127 }
128
129 int v4l2_subdev_set_crop(struct media_entity *entity, struct v4l2_rect *rect,
130                          unsigned int pad, enum v4l2_subdev_format_whence which)
131 {
132         struct v4l2_subdev_crop crop;
133         int ret;
134
135         ret = v4l2_subdev_open(entity);
136         if (ret < 0)
137                 return ret;
138
139         memset(&crop, 0, sizeof(crop));
140         crop.pad = pad;
141         crop.which = which;
142         crop.rect = *rect;
143
144         ret = ioctl(entity->fd, VIDIOC_SUBDEV_S_CROP, &crop);
145         if (ret < 0)
146                 return -errno;
147
148         *rect = crop.rect;
149         return 0;
150 }
151
152 int v4l2_subdev_get_frame_interval(struct media_entity *entity,
153                                    struct v4l2_fract *interval)
154 {
155         struct v4l2_subdev_frame_interval ival;
156         int ret;
157
158         ret = v4l2_subdev_open(entity);
159         if (ret < 0)
160                 return ret;
161
162         memset(&ival, 0, sizeof(ival));
163
164         ret = ioctl(entity->fd, VIDIOC_SUBDEV_G_FRAME_INTERVAL, &ival);
165         if (ret < 0)
166                 return -errno;
167
168         *interval = ival.interval;
169         return 0;
170 }
171
172 int v4l2_subdev_set_frame_interval(struct media_entity *entity,
173                                    struct v4l2_fract *interval)
174 {
175         struct v4l2_subdev_frame_interval ival;
176         int ret;
177
178         ret = v4l2_subdev_open(entity);
179         if (ret < 0)
180                 return ret;
181
182         memset(&ival, 0, sizeof(ival));
183         ival.interval = *interval;
184
185         ret = ioctl(entity->fd, VIDIOC_SUBDEV_S_FRAME_INTERVAL, &ival);
186         if (ret < 0)
187                 return -errno;
188
189         *interval = ival.interval;
190         return 0;
191 }
192
193 static int v4l2_subdev_parse_format(struct v4l2_mbus_framefmt *format,
194                                     const char *p, char **endp)
195 {
196         enum v4l2_mbus_pixelcode code;
197         unsigned int width, height;
198         char *end;
199
200         for (; isspace(*p); ++p);
201         for (end = (char *)p; !isspace(*end) && *end != '\0'; ++end);
202
203         code = v4l2_subdev_string_to_pixelcode(p, end - p);
204         if (code == (enum v4l2_mbus_pixelcode)-1)
205                 return -EINVAL;
206
207         for (p = end; isspace(*p); ++p);
208         width = strtoul(p, &end, 10);
209         if (*end != 'x')
210                 return -EINVAL;
211
212         p = end + 1;
213         height = strtoul(p, &end, 10);
214         *endp = end;
215
216         memset(format, 0, sizeof(*format));
217         format->width = width;
218         format->height = height;
219         format->code = code;
220
221         return 0;
222 }
223
224 static int v4l2_subdev_parse_crop(
225         struct v4l2_rect *crop, const char *p, char **endp)
226 {
227         char *end;
228
229         if (*p++ != '(')
230                 return -EINVAL;
231
232         crop->left = strtoul(p, &end, 10);
233         if (*end != ',')
234                 return -EINVAL;
235
236         p = end + 1;
237         crop->top = strtoul(p, &end, 10);
238         if (*end++ != ')')
239                 return -EINVAL;
240         if (*end != '/')
241                 return -EINVAL;
242
243         p = end + 1;
244         crop->width = strtoul(p, &end, 10);
245         if (*end != 'x')
246                 return -EINVAL;
247
248         p = end + 1;
249         crop->height = strtoul(p, &end, 10);
250         *endp = end;
251
252         return 0;
253 }
254
255 static int v4l2_subdev_parse_frame_interval(struct v4l2_fract *interval,
256                                             const char *p, char **endp)
257 {
258         char *end;
259
260         for (; isspace(*p); ++p);
261
262         interval->numerator = strtoul(p, &end, 10);
263
264         for (p = end; isspace(*p); ++p);
265         if (*p++ != '/')
266                 return -EINVAL;
267
268         for (; isspace(*p); ++p);
269         interval->denominator = strtoul(p, &end, 10);
270
271         *endp = end;
272         return 0;
273 }
274
275 static struct media_pad *v4l2_subdev_parse_pad_format(
276         struct media_device *media, struct v4l2_mbus_framefmt *format,
277         struct v4l2_rect *crop, struct v4l2_fract *interval, const char *p,
278         char **endp)
279 {
280         struct media_pad *pad;
281         char *end;
282         int ret;
283
284         for (; isspace(*p); ++p);
285
286         pad = media_parse_pad(media, p, &end);
287         if (pad == NULL)
288                 return NULL;
289
290         for (p = end; isspace(*p); ++p);
291         if (*p++ != '[')
292                 return NULL;
293
294         for (; isspace(*p); ++p);
295
296         if (isalnum(*p)) {
297                 ret = v4l2_subdev_parse_format(format, p, &end);
298                 if (ret < 0)
299                         return NULL;
300
301                 for (p = end; isspace(*p); p++);
302         }
303
304         if (*p == '(') {
305                 ret = v4l2_subdev_parse_crop(crop, p, &end);
306                 if (ret < 0)
307                         return NULL;
308
309                 for (p = end; isspace(*p); p++);
310         }
311
312         if (*p == '@') {
313                 ret = v4l2_subdev_parse_frame_interval(interval, ++p, &end);
314                 if (ret < 0)
315                         return NULL;
316
317                 for (p = end; isspace(*p); p++);
318         }
319
320         if (*p != ']')
321                 return NULL;
322
323         *endp = (char *)p + 1;
324         return pad;
325 }
326
327 static int set_format(struct media_pad *pad,
328                       struct v4l2_mbus_framefmt *format)
329 {
330         int ret;
331
332         if (format->width == 0 || format->height == 0)
333                 return 0;
334
335         media_dbg(pad->entity->media,
336                   "Setting up format %s %ux%u on pad %s/%u\n",
337                   v4l2_subdev_pixelcode_to_string(format->code),
338                   format->width, format->height,
339                   pad->entity->info.name, pad->index);
340
341         ret = v4l2_subdev_set_format(pad->entity, format, pad->index,
342                                      V4L2_SUBDEV_FORMAT_ACTIVE);
343         if (ret < 0) {
344                 media_dbg(pad->entity->media,
345                           "Unable to set format: %s (%d)\n",
346                           strerror(-ret), ret);
347                 return ret;
348         }
349
350         media_dbg(pad->entity->media,
351                   "Format set: %s %ux%u\n",
352                   v4l2_subdev_pixelcode_to_string(format->code),
353                   format->width, format->height);
354
355         return 0;
356 }
357
358 static int set_crop(struct media_pad *pad, struct v4l2_rect *crop)
359 {
360         int ret;
361
362         if (crop->left == -1 || crop->top == -1)
363                 return 0;
364
365         media_dbg(pad->entity->media,
366                   "Setting up crop rectangle (%u,%u)/%ux%u on pad %s/%u\n",
367                   crop->left, crop->top, crop->width, crop->height,
368                   pad->entity->info.name, pad->index);
369
370         ret = v4l2_subdev_set_crop(pad->entity, crop, pad->index,
371                                    V4L2_SUBDEV_FORMAT_ACTIVE);
372         if (ret < 0) {
373                 media_dbg(pad->entity->media,
374                           "Unable to set crop rectangle: %s (%d)\n",
375                           strerror(-ret), ret);
376                 return ret;
377         }
378
379         media_dbg(pad->entity->media,
380                   "Crop rectangle set: (%u,%u)/%ux%u\n",
381                   crop->left, crop->top, crop->width, crop->height);
382
383         return 0;
384 }
385
386 static int set_frame_interval(struct media_entity *entity,
387                               struct v4l2_fract *interval)
388 {
389         int ret;
390
391         if (interval->numerator == 0)
392                 return 0;
393
394         media_dbg(entity->media,
395                   "Setting up frame interval %u/%u on entity %s\n",
396                   interval->numerator, interval->denominator,
397                   entity->info.name);
398
399         ret = v4l2_subdev_set_frame_interval(entity, interval);
400         if (ret < 0) {
401                 media_dbg(entity->media,
402                           "Unable to set frame interval: %s (%d)",
403                           strerror(-ret), ret);
404                 return ret;
405         }
406
407         media_dbg(entity->media, "Frame interval set: %u/%u\n",
408                   interval->numerator, interval->denominator);
409
410         return 0;
411 }
412
413
414 static int v4l2_subdev_parse_setup_format(struct media_device *media,
415                                           const char *p, char **endp)
416 {
417         struct v4l2_mbus_framefmt format = { 0, 0, 0 };
418         struct media_pad *pad;
419         struct v4l2_rect crop = { -1, -1, -1, -1 };
420         struct v4l2_fract interval = { 0, 0 };
421         unsigned int i;
422         char *end;
423         int ret;
424
425         pad = v4l2_subdev_parse_pad_format(media, &format, &crop, &interval,
426                                            p, &end);
427         if (pad == NULL) {
428                 media_dbg(media, "Unable to parse format\n");
429                 return -EINVAL;
430         }
431
432         if (pad->flags & MEDIA_PAD_FL_SOURCE) {
433                 ret = set_crop(pad, &crop);
434                 if (ret < 0)
435                         return ret;
436         }
437
438         ret = set_format(pad, &format);
439         if (ret < 0)
440                 return ret;
441
442         if (pad->flags & MEDIA_PAD_FL_SINK) {
443                 ret = set_crop(pad, &crop);
444                 if (ret < 0)
445                         return ret;
446         }
447
448         ret = set_frame_interval(pad->entity, &interval);
449         if (ret < 0)
450                 return ret;
451
452
453         /* If the pad is an output pad, automatically set the same format on
454          * the remote subdev input pads, if any.
455          */
456         if (pad->flags & MEDIA_PAD_FL_SOURCE) {
457                 for (i = 0; i < pad->entity->num_links; ++i) {
458                         struct media_link *link = &pad->entity->links[i];
459                         struct v4l2_mbus_framefmt remote_format;
460
461                         if (!(link->flags & MEDIA_LNK_FL_ENABLED))
462                                 continue;
463
464                         if (link->source == pad &&
465                             link->sink->entity->info.type == MEDIA_ENT_T_V4L2_SUBDEV) {
466                                 remote_format = format;
467                                 set_format(link->sink, &remote_format);
468                         }
469                 }
470         }
471
472         *endp = end;
473         return 0;
474 }
475
476 int v4l2_subdev_parse_setup_formats(struct media_device *media, const char *p)
477 {
478         char *end;
479         int ret;
480
481         do {
482                 ret = v4l2_subdev_parse_setup_format(media, p, &end);
483                 if (ret < 0)
484                         return ret;
485
486                 p = end + 1;
487         } while (*end == ',');
488
489         return *end ? -EINVAL : 0;
490 }
491
492 static struct {
493         const char *name;
494         enum v4l2_mbus_pixelcode code;
495 } mbus_formats[] = {
496         { "Y8", V4L2_MBUS_FMT_Y8_1X8},
497         { "Y10", V4L2_MBUS_FMT_Y10_1X10 },
498         { "Y12", V4L2_MBUS_FMT_Y12_1X12 },
499         { "YUYV", V4L2_MBUS_FMT_YUYV8_1X16 },
500         { "YUYV2X8", V4L2_MBUS_FMT_YUYV8_2X8 },
501         { "UYVY", V4L2_MBUS_FMT_UYVY8_1X16 },
502         { "UYVY2X8", V4L2_MBUS_FMT_UYVY8_2X8 },
503         { "SBGGR8", V4L2_MBUS_FMT_SBGGR8_1X8 },
504         { "SGBRG8", V4L2_MBUS_FMT_SGBRG8_1X8 },
505         { "SGRBG8", V4L2_MBUS_FMT_SGRBG8_1X8 },
506         { "SRGGB8", V4L2_MBUS_FMT_SRGGB8_1X8 },
507         { "SBGGR10", V4L2_MBUS_FMT_SBGGR10_1X10 },
508         { "SGBRG10", V4L2_MBUS_FMT_SGBRG10_1X10 },
509         { "SGRBG10", V4L2_MBUS_FMT_SGRBG10_1X10 },
510         { "SRGGB10", V4L2_MBUS_FMT_SRGGB10_1X10 },
511         { "SBGGR10_DPCM8", V4L2_MBUS_FMT_SBGGR10_DPCM8_1X8 },
512         { "SGBRG10_DPCM8", V4L2_MBUS_FMT_SGBRG10_DPCM8_1X8 },
513         { "SGRBG10_DPCM8", V4L2_MBUS_FMT_SGRBG10_DPCM8_1X8 },
514         { "SRGGB10_DPCM8", V4L2_MBUS_FMT_SRGGB10_DPCM8_1X8 },
515         { "SBGGR12", V4L2_MBUS_FMT_SBGGR12_1X12 },
516         { "SGBRG12", V4L2_MBUS_FMT_SGBRG12_1X12 },
517         { "SGRBG12", V4L2_MBUS_FMT_SGRBG12_1X12 },
518         { "SRGGB12", V4L2_MBUS_FMT_SRGGB12_1X12 },
519 };
520
521 const char *v4l2_subdev_pixelcode_to_string(enum v4l2_mbus_pixelcode code)
522 {
523         unsigned int i;
524
525         for (i = 0; i < ARRAY_SIZE(mbus_formats); ++i) {
526                 if (mbus_formats[i].code == code)
527                         return mbus_formats[i].name;
528         }
529
530         return "unknown";
531 }
532
533 enum v4l2_mbus_pixelcode v4l2_subdev_string_to_pixelcode(const char *string,
534                                                          unsigned int length)
535 {
536         unsigned int i;
537
538         for (i = 0; i < ARRAY_SIZE(mbus_formats); ++i) {
539                 if (strncmp(mbus_formats[i].name, string, length) == 0)
540                         break;
541         }
542
543         if (i == ARRAY_SIZE(mbus_formats))
544                 return (enum v4l2_mbus_pixelcode)-1;
545
546         return mbus_formats[i].code;
547 }