v4l2_device.cpp revision 2f9c6c5fdfd5c87046295b37cd3bd99b6d4e7b09 
1/*
2 * v4l2_device.cpp - v4l2 device
3 *
4 *  Copyright (c) 2014-2015 Intel Corporation
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
9 *
10 *      http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 *
18 * Author: Wind Yuan <feng.yuan@intel.com>
19 * Author: John Ye <john.ye@intel.com>
20 */
21
22#include "v4l2_device.h"
23#include <sys/ioctl.h>
24#include <stdlib.h>
25#include <unistd.h>
26#include <fcntl.h>
27#include <poll.h>
28#include <errno.h>
29#include <sys/mman.h>
30
31#include "v4l2_buffer_proxy.h"
32
33namespace XCam {
34
35#define XCAM_V4L2_DEFAULT_BUFFER_COUNT  6
36
37V4l2Device::V4l2Device (const char *name)
38    : _name (NULL)
39    , _fd (-1)
40    , _sensor_id (0)
41    , _capture_mode (0)
42    , _capture_buf_type (V4L2_BUF_TYPE_VIDEO_CAPTURE)
43    , _memory_type (V4L2_MEMORY_MMAP)
44    , _fps_n (0)
45    , _fps_d (0)
46    , _active (false)
47    , _buf_count (XCAM_V4L2_DEFAULT_BUFFER_COUNT)
48{
49    if (name)
50        _name = strdup (name);
51    xcam_mem_clear (_format);
52}
53
54V4l2Device::~V4l2Device ()
55{
56    close();
57    if (_name)
58        xcam_free (_name);
59}
60
61bool
62V4l2Device::set_device_name (const char *name)
63{
64    XCAM_ASSERT (name);
65
66    if (is_opened()) {
67        XCAM_LOG_WARNING ("can't set device name since device opened");
68        return false;
69    }
70    if (_name)
71        xcam_free (_name);
72    _name = strdup (name);
73    return true;
74}
75
76bool
77V4l2Device::set_sensor_id (int id)
78{
79    if (is_opened()) {
80        XCAM_LOG_WARNING ("can't set sensor id since device opened");
81        return false;
82    }
83    _sensor_id = id;
84    return true;
85}
86
87bool
88V4l2Device::set_capture_mode (uint32_t capture_mode)
89{
90    if (is_opened()) {
91        XCAM_LOG_WARNING ("can't set sensor id since device opened");
92        return false;
93    }
94    _capture_mode = capture_mode;
95    return true;
96}
97
98bool
99V4l2Device::set_framerate (uint32_t n, uint32_t d)
100{
101    if (_format.fmt.pix.pixelformat) {
102        XCAM_LOG_WARNING ("device(%s) set framerate failed since formated was already set.", XCAM_STR(_name));
103        return false;
104    }
105
106    _fps_n = n;
107    _fps_d = d;
108
109    return true;
110}
111
112void
113V4l2Device::get_framerate (uint32_t &n, uint32_t &d)
114{
115    n = _fps_n;
116    d = _fps_d;
117}
118
119bool
120V4l2Device::set_mem_type (enum v4l2_memory type) {
121    if (is_activated ()) {
122        XCAM_LOG_WARNING ("device(%s) set mem type failed", XCAM_STR (_name));
123        return false;
124    }
125    _memory_type = type;
126    return true;
127}
128
129bool
130V4l2Device::set_buffer_count (uint32_t buf_count)
131{
132    if (is_activated ()) {
133        XCAM_LOG_WARNING ("device(%s) set buffer count failed", XCAM_STR (_name));
134        return false;
135    }
136    _buf_count = buf_count;
137    return true;
138}
139
140
141XCamReturn
142V4l2Device::open ()
143{
144    struct v4l2_streamparm param;
145
146    if (is_opened()) {
147        XCAM_LOG_DEBUG ("device(%s) was already opened", XCAM_STR(_name));
148        return XCAM_RETURN_NO_ERROR;
149    }
150
151    if (!_name) {
152        XCAM_LOG_DEBUG ("v4l2 device open failed, there's no device name");
153        return XCAM_RETURN_ERROR_PARAM;
154    }
155    _fd = ::open (_name, O_RDWR);
156    if (_fd == -1) {
157        XCAM_LOG_DEBUG ("open device(%s) failed", _name);
158        return XCAM_RETURN_ERROR_IOCTL;
159    }
160
161    // set sensor id
162    if (io_control (VIDIOC_S_INPUT, &_sensor_id) < 0) {
163        XCAM_LOG_WARNING ("set sensor id(%d) failed but continue", _sensor_id);
164    }
165
166    // set capture mode
167    xcam_mem_clear (param);
168    param.type = _capture_buf_type;
169    param.parm.capture.capturemode = _capture_mode;
170    if (io_control (VIDIOC_S_PARM, &param) < 0) {
171        XCAM_LOG_WARNING ("set capture mode(0x%08x) failed but continue", _capture_mode);
172    }
173
174    return XCAM_RETURN_NO_ERROR;
175}
176
177XCamReturn
178V4l2Device::close ()
179{
180    if (!is_opened())
181        return XCAM_RETURN_NO_ERROR;
182    ::close (_fd);
183    _fd = -1;
184    return XCAM_RETURN_NO_ERROR;
185}
186
187int
188V4l2Device::io_control (int cmd, void *arg)
189
190{
191    if (_fd <= 0)
192        return -1;
193
194    return xcam_device_ioctl (_fd, cmd, arg);
195}
196
197int
198V4l2Device::poll_event (int timeout_msec)
199{
200    struct pollfd poll_fd;
201    int ret = 0;
202
203    XCAM_ASSERT (_fd > 0);
204
205    xcam_mem_clear (poll_fd);
206    poll_fd.fd = _fd;
207    poll_fd.events = (POLLPRI | POLLIN | POLLERR | POLLNVAL | POLLHUP);
208
209    ret = poll (&poll_fd, 1, timeout_msec);
210    if (ret > 0 && (poll_fd.revents & (POLLERR | POLLNVAL | POLLHUP))) {
211        XCAM_LOG_DEBUG ("v4l2 subdev(%s) polled error", XCAM_STR(_name));
212        return -1;
213    }
214    return ret;
215
216}
217
218XCamReturn
219V4l2Device::set_format (struct v4l2_format &format)
220{
221    XCamReturn ret = XCAM_RETURN_NO_ERROR;
222
223    XCAM_FAIL_RETURN (ERROR, !is_activated (), XCAM_RETURN_ERROR_PARAM,
224                      "Cannot set format to v4l2 device while it is active.");
225
226    XCAM_FAIL_RETURN (ERROR, is_opened (), XCAM_RETURN_ERROR_FILE,
227                      "Cannot set format to v4l2 device while it is closed.");
228
229    ret = pre_set_format (format);
230    if (ret != XCAM_RETURN_NO_ERROR) {
231        XCAM_LOG_WARNING ("device(%s) pre_set_format failed", XCAM_STR (_name));
232        return ret;
233    }
234
235    if (io_control (VIDIOC_S_FMT, &format) < 0) {
236        if (errno == EBUSY) {
237            // TODO log device name
238            XCAM_LOG_ERROR("Video device is busy, fail to set format.");
239        } else {
240            // TODO log format details and errno
241            XCAM_LOG_ERROR("Fail to set format: %s", strerror(errno));
242        }
243
244        return XCAM_RETURN_ERROR_IOCTL;
245    }
246
247    while (_fps_n && _fps_d) {
248        struct v4l2_streamparm param;
249        xcam_mem_clear (param);
250        param.type = _capture_buf_type;
251        if (io_control (VIDIOC_G_PARM, &param) < 0) {
252            XCAM_LOG_WARNING ("device(%s) set framerate failed on VIDIOC_G_PARM but continue", XCAM_STR (_name));
253            break;
254        }
255
256        if (!(param.parm.capture.capability & V4L2_CAP_TIMEPERFRAME))
257            break;
258
259        param.parm.capture.timeperframe.numerator = _fps_d;
260        param.parm.capture.timeperframe.denominator = _fps_n;
261
262        if (io_control (VIDIOC_S_PARM, &param) < 0) {
263            XCAM_LOG_WARNING ("device(%s) set framerate failed on VIDIOC_S_PARM but continue", XCAM_STR (_name));
264            break;
265        }
266        _fps_n = param.parm.capture.timeperframe.denominator;
267        _fps_d = param.parm.capture.timeperframe.numerator;
268        XCAM_LOG_INFO ("device(%s) set framerate(%d/%d)", XCAM_STR (_name), _fps_n, _fps_d);
269    }
270
271    ret = post_set_format (format);
272    if (ret != XCAM_RETURN_NO_ERROR) {
273        XCAM_LOG_WARNING ("device(%s) post_set_format failed", XCAM_STR (_name));
274        return ret;
275    }
276
277    _format = format;
278    XCAM_LOG_INFO (
279        "device(%s) set format(w:%d, h:%d, pixelformat:%s, bytesperline:%d,image_size:%d)",
280        XCAM_STR (_name),
281        format.fmt.pix.width, format.fmt.pix.height,
282        xcam_fourcc_to_string (format.fmt.pix.pixelformat),
283        format.fmt.pix.bytesperline,
284        format.fmt.pix.sizeimage);
285
286    return XCAM_RETURN_NO_ERROR;
287}
288
289/*! \brief v4l2 set format
290 *
291 * \param[in]    width            format width
292 * \param[in]    height           format height
293 * \param[in]    pixelformat      fourcc
294 * \param[in]    field            V4L2_FIELD_INTERLACED or V4L2_FIELD_NONE
295 */
296XCamReturn
297V4l2Device::set_format (
298    uint32_t width,  uint32_t height,
299    uint32_t pixelformat, enum v4l2_field field, uint32_t bytes_perline)
300{
301
302    struct v4l2_format format;
303    xcam_mem_clear (format);
304
305    format.type = _capture_buf_type;
306    format.fmt.pix.width = width;
307    format.fmt.pix.height = height;
308    format.fmt.pix.pixelformat = pixelformat;
309    format.fmt.pix.field = field;
310
311    if (bytes_perline != 0)
312        format.fmt.pix.bytesperline = bytes_perline;
313    return set_format (format);
314}
315
316XCamReturn
317V4l2Device::pre_set_format (struct v4l2_format &format)
318{
319    XCAM_UNUSED (format);
320    return XCAM_RETURN_NO_ERROR;
321}
322
323XCamReturn
324V4l2Device::post_set_format (struct v4l2_format &format)
325{
326    XCAM_UNUSED (format);
327    return XCAM_RETURN_NO_ERROR;
328}
329
330std::list<struct v4l2_fmtdesc>
331V4l2Device::enum_formats ()
332{
333    std::list<struct v4l2_fmtdesc> formats;
334    struct v4l2_fmtdesc format;
335    uint32_t i = 0;
336
337    while (1) {
338        xcam_mem_clear (format);
339        format.index = i++;
340        format.type = _capture_buf_type;
341        if (this->io_control (VIDIOC_ENUM_FMT, &format) < 0) {
342            if (errno == EINVAL)
343                break;
344            else { // error
345                XCAM_LOG_DEBUG ("enum formats failed");
346                return formats;
347            }
348        }
349        formats.push_back (format);
350    }
351
352    return formats;
353}
354
355XCamReturn
356V4l2Device::get_format (struct v4l2_format &format)
357{
358    if (is_activated ()) {
359        format = _format;
360        return XCAM_RETURN_NO_ERROR;
361    }
362
363    if (!is_opened ())
364        return XCAM_RETURN_ERROR_IOCTL;
365
366    xcam_mem_clear (format);
367    format.type = _capture_buf_type;
368
369    if (this->io_control (VIDIOC_G_FMT, &format) < 0) {
370        // FIXME: also log the device name?
371        XCAM_LOG_ERROR("Fail to get format via ioctl VIDVIO_G_FMT.");
372        return XCAM_RETURN_ERROR_IOCTL;
373    }
374
375    return XCAM_RETURN_NO_ERROR;
376}
377
378XCamReturn
379V4l2Device::start ()
380{
381    XCamReturn ret = XCAM_RETURN_NO_ERROR;
382    // request buffer first
383    ret = request_buffer ();
384    XCAM_FAIL_RETURN (
385        ERROR, ret == XCAM_RETURN_NO_ERROR, ret,
386        "device(%s) start failed", XCAM_STR (_name));
387
388    //alloc buffers
389    ret = init_buffer_pool ();
390    XCAM_FAIL_RETURN (
391        ERROR, ret == XCAM_RETURN_NO_ERROR, ret,
392        "device(%s) start failed", XCAM_STR (_name));
393
394    //queue all buffers
395    for (uint32_t i = 0; i < _buf_count; ++i) {
396        SmartPtr<V4l2Buffer> &buf = _buf_pool [i];
397        XCAM_ASSERT (buf.ptr());
398        XCAM_ASSERT (buf->get_buf().index == i);
399        ret = queue_buffer (buf);
400        if (ret != XCAM_RETURN_NO_ERROR) {
401            XCAM_LOG_ERROR (
402                "device(%s) start failed on queue index:%d",
403                XCAM_STR (_name), i);
404            stop ();
405            return ret;
406        }
407    }
408
409    // stream on
410    if (io_control (VIDIOC_STREAMON, &_capture_buf_type) < 0) {
411        XCAM_LOG_ERROR (
412            "device(%s) start failed on VIDIOC_STREAMON",
413            XCAM_STR (_name));
414        stop ();
415        return XCAM_RETURN_ERROR_IOCTL;
416    }
417    _active = true;
418    XCAM_LOG_INFO ("device(%s) started successfully", XCAM_STR (_name));
419    return XCAM_RETURN_NO_ERROR;
420}
421
422XCamReturn
423V4l2Device::stop ()
424{
425    // stream off
426    if (_active) {
427        if (io_control (VIDIOC_STREAMOFF, &_capture_buf_type) < 0) {
428            XCAM_LOG_WARNING ("device(%s) steamoff failed", XCAM_STR (_name));
429        }
430        _active = false;
431    }
432
433    fini_buffer_pool ();
434
435    XCAM_LOG_INFO ("device(%s) stopped", XCAM_STR (_name));
436    return XCAM_RETURN_NO_ERROR;
437}
438
439XCamReturn
440V4l2Device::request_buffer ()
441{
442    struct v4l2_requestbuffers request_buf;
443
444    XCAM_ASSERT (!is_activated());
445
446    xcam_mem_clear (request_buf);
447    request_buf.type = _capture_buf_type;
448    request_buf.count = _buf_count;
449    request_buf.memory = _memory_type;
450
451    if (io_control (VIDIOC_REQBUFS, &request_buf) < 0) {
452        XCAM_LOG_INFO ("device(%s) starts failed on VIDIOC_REQBUFS", XCAM_STR (_name));
453        return XCAM_RETURN_ERROR_IOCTL;
454    }
455
456    if (request_buf.count != _buf_count) {
457        XCAM_LOG_DEBUG (
458            "device(%s) request buffer count doesn't match user settings, reset buffer count to %d",
459            XCAM_STR (_name), request_buf.count);
460        _buf_count = request_buf.count;
461    }
462    return XCAM_RETURN_NO_ERROR;
463}
464
465XCamReturn
466V4l2Device::allocate_buffer (
467    SmartPtr<V4l2Buffer> &buf,
468    const struct v4l2_format &format,
469    const uint32_t index)
470{
471    struct v4l2_buffer v4l2_buf;
472
473    xcam_mem_clear (v4l2_buf);
474    v4l2_buf.index = index;
475    v4l2_buf.type = _capture_buf_type;
476    v4l2_buf.memory = _memory_type;
477
478    switch (_memory_type) {
479    case V4L2_MEMORY_DMABUF:
480    {
481        struct v4l2_exportbuffer expbuf;
482        xcam_mem_clear (expbuf);
483        expbuf.type = _capture_buf_type;
484        expbuf.index = index;
485        expbuf.flags = O_CLOEXEC;
486        if (io_control (VIDIOC_EXPBUF, &expbuf) < 0) {
487            XCAM_LOG_WARNING ("device(%s) get dma buf(%d) failed", XCAM_STR (_name), index);
488            return XCAM_RETURN_ERROR_MEM;
489        }
490        v4l2_buf.m.fd = expbuf.fd;
491        v4l2_buf.length = format.fmt.pix.sizeimage;
492    }
493    break;
494    case V4L2_MEMORY_MMAP:
495    {
496        void *pointer;
497        if (io_control (VIDIOC_QUERYBUF, &v4l2_buf) < 0) {
498            XCAM_LOG_WARNING("device(%s) query MMAP buf(%d) failed", XCAM_STR(_name), index);
499            return XCAM_RETURN_ERROR_MEM;
500        }
501        pointer = mmap (0, v4l2_buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, _fd, v4l2_buf.m.offset);
502        if (pointer == MAP_FAILED) {
503            XCAM_LOG_WARNING("device(%s) mmap buf(%d) failed", XCAM_STR(_name), index);
504            return XCAM_RETURN_ERROR_MEM;
505        }
506        v4l2_buf.m.userptr = (uintptr_t) pointer;
507    }
508    break;
509    case V4L2_MEMORY_USERPTR:
510    default:
511        XCAM_ASSERT (false);
512        XCAM_LOG_WARNING (
513            "device(%s) allocated buffer mem_type(%d) doesn't support",
514            XCAM_STR (_name), _memory_type);
515        return XCAM_RETURN_ERROR_MEM;
516    }
517
518    buf = new V4l2Buffer (v4l2_buf, _format);
519
520    return XCAM_RETURN_NO_ERROR;
521}
522
523XCamReturn
524V4l2Device::init_buffer_pool ()
525{
526    XCamReturn ret = XCAM_RETURN_NO_ERROR;
527    uint32_t i = 0;
528
529    _buf_pool.clear ();
530    _buf_pool.reserve (_buf_count);
531
532    for (; i < _buf_count; i++) {
533        SmartPtr<V4l2Buffer> new_buf;
534        ret = allocate_buffer (new_buf, _format, i);
535        if (ret != XCAM_RETURN_NO_ERROR) {
536            break;
537        }
538        _buf_pool.push_back (new_buf);
539    }
540
541    if (_buf_pool.empty()) {
542        XCAM_LOG_ERROR ("No bufer allocated in device(%s)", XCAM_STR (_name));
543        return XCAM_RETURN_ERROR_MEM;
544    }
545
546    if (i != _buf_count) {
547        XCAM_LOG_WARNING (
548            "device(%s) allocate buffer count:%d failback to %d",
549            XCAM_STR (_name), _buf_count, i);
550        _buf_count = i;
551    }
552
553    return XCAM_RETURN_NO_ERROR;
554}
555
556XCamReturn
557V4l2Device::fini_buffer_pool()
558{
559    _buf_pool.clear ();
560    return XCAM_RETURN_NO_ERROR;
561}
562
563XCamReturn
564V4l2Device::dequeue_buffer(SmartPtr<V4l2Buffer> &buf)
565{
566    struct v4l2_buffer v4l2_buf;
567
568    if (!is_activated()) {
569        XCAM_LOG_DEBUG (
570            "device(%s) dequeue buffer failed since not activated", XCAM_STR (_name));
571        return XCAM_RETURN_ERROR_PARAM;
572    }
573
574    xcam_mem_clear (v4l2_buf);
575    v4l2_buf.type = _capture_buf_type;
576    v4l2_buf.memory = _memory_type;
577
578    if (this->io_control (VIDIOC_DQBUF, &v4l2_buf) < 0) {
579        XCAM_LOG_ERROR ("device(%s) fail to dequeue buffer.", XCAM_STR (_name));
580        return XCAM_RETURN_ERROR_IOCTL;
581    }
582
583    XCAM_LOG_DEBUG ("device(%s) dequeue buffer index:%d", XCAM_STR (_name), v4l2_buf.index);
584
585    if (v4l2_buf.index > _buf_count) {
586        XCAM_LOG_ERROR (
587            "device(%s) dequeue wrong buffer index:%d",
588            XCAM_STR (_name), v4l2_buf.index);
589        return XCAM_RETURN_ERROR_ISP;
590    }
591    buf = _buf_pool [v4l2_buf.index];
592    buf->set_timestamp (v4l2_buf.timestamp);
593    buf->set_timecode (v4l2_buf.timecode);
594    buf->set_sequence (v4l2_buf.sequence);
595    //buf.set_length (v4l2_buf.length); // not necessary to set length
596    return XCAM_RETURN_NO_ERROR;
597}
598
599XCamReturn
600V4l2Device::queue_buffer (SmartPtr<V4l2Buffer> &buf)
601{
602    XCAM_ASSERT (buf.ptr());
603    buf->reset ();
604
605    struct v4l2_buffer v4l2_buf = buf->get_buf ();
606    XCAM_ASSERT (v4l2_buf.index < _buf_count);
607
608    XCAM_LOG_DEBUG ("device(%s) queue buffer index:%d", XCAM_STR (_name), v4l2_buf.index);
609    if (io_control (VIDIOC_QBUF, &v4l2_buf) < 0) {
610        XCAM_LOG_ERROR("fail to enqueue buffer index:%d.", v4l2_buf.index);
611        return XCAM_RETURN_ERROR_IOCTL;
612    }
613    return XCAM_RETURN_NO_ERROR;
614}
615
616V4l2SubDevice::V4l2SubDevice (const char *name)
617    : V4l2Device (name)
618{
619}
620
621XCamReturn
622V4l2SubDevice::subscribe_event (int event)
623{
624    struct v4l2_event_subscription sub;
625    int ret = 0;
626
627    XCAM_ASSERT (is_opened());
628
629    xcam_mem_clear (sub);
630    sub.type = event;
631
632    ret = this->io_control (VIDIOC_SUBSCRIBE_EVENT, &sub);
633    if (ret < 0) {
634        XCAM_LOG_DEBUG ("subdev(%s) subscribe event(%d) failed", XCAM_STR(_name), event);
635        return XCAM_RETURN_ERROR_IOCTL;
636    }
637    return XCAM_RETURN_NO_ERROR;
638}
639
640XCamReturn
641V4l2SubDevice::unsubscribe_event (int event)
642{
643    struct v4l2_event_subscription sub;
644    int ret = 0;
645
646    XCAM_ASSERT (is_opened());
647
648    xcam_mem_clear (sub);
649    sub.type = event;
650
651    ret = this->io_control (VIDIOC_UNSUBSCRIBE_EVENT, &sub);
652    if (ret < 0) {
653        XCAM_LOG_DEBUG ("subdev(%s) unsubscribe event(%d) failed", XCAM_STR(_name), event);
654        return XCAM_RETURN_ERROR_IOCTL;
655    }
656    return XCAM_RETURN_NO_ERROR;
657}
658
659XCamReturn
660V4l2SubDevice::dequeue_event (struct v4l2_event &event)
661{
662    int ret = 0;
663    XCAM_ASSERT (is_opened());
664
665    ret = this->io_control (VIDIOC_DQEVENT, &event);
666    if (ret < 0) {
667        XCAM_LOG_DEBUG ("subdev(%s) dequeue event failed", XCAM_STR(_name));
668        return XCAM_RETURN_ERROR_IOCTL;
669    }
670
671    return XCAM_RETURN_NO_ERROR;
672}
673
674XCamReturn V4l2SubDevice::start ()
675{
676    if (!is_opened())
677        return XCAM_RETURN_ERROR_PARAM;
678
679    _active = true;
680    return XCAM_RETURN_NO_ERROR;
681}
682
683XCamReturn V4l2SubDevice::stop ()
684{
685    if (_active)
686        _active = false;
687
688    return XCAM_RETURN_NO_ERROR;
689}
690
691};
692