1/* ---------------------------------------------------------------------------
2** This software is in the public domain, furnished "as is", without technical
3** support, and with no warranty, express or implied, as to its usefulness for
4** any purpose.
5**
6** V4l2Device.cpp
7**
8** -------------------------------------------------------------------------*/
9
10#include <unistd.h>
11#include <errno.h>
12#include <string.h>
13#include <sys/ioctl.h>
14#include <sys/stat.h>
15
16// libv4l2
17#include <linux/videodev2.h>
18
19#include "logger.h"
20
21#include "V4l2Device.h"
22
23std::string fourcc(unsigned int format)
24{
25 char formatArray[] = { (char)(format&0xff), (char)((format>>8)&0xff), (char)((format>>16)&0xff), (char)((format>>24)&0xff), 0 };
26 return std::string(formatArray, strlen(formatArray));
27}
28
29// -----------------------------------------
30// V4L2Device
31// -----------------------------------------
32V4l2Device::V4l2Device(const V4L2DeviceParameters& params, v4l2_buf_type deviceType) : m_params(params), m_fd(-1), m_deviceType(deviceType), m_bufferSize(0), m_format(0)
33{
34}
35
36V4l2Device::~V4l2Device()
37{
38 this->close();
39}
40
41void V4l2Device::close()
42{
43 if (m_fd != -1)
44 ::close(m_fd);
45
46 m_fd = -1;
47}
48
49// query current format
50void V4l2Device::queryFormat()
51{
52 struct v4l2_format fmt;
53 memset(&fmt,0,sizeof(fmt));
54 fmt.type = m_deviceType;
55 if (0 == ioctl(m_fd,VIDIOC_G_FMT,&fmt))
56 {
57 m_format = fmt.fmt.pix.pixelformat;
58 m_width = fmt.fmt.pix.width;
59 m_height = fmt.fmt.pix.height;
60 m_bufferSize = fmt.fmt.pix.sizeimage;
61
62 LOG(NOTICE) << m_params.m_devName << ":" << fourcc(m_format) << " size:" << m_width << "x" << m_height << " bufferSize:" << m_bufferSize;
63 }
64}
65
66// intialize the V4L2 connection
67bool V4l2Device::init(unsigned int mandatoryCapabilities)
68{
69 struct stat sb;
70 if ( (stat(m_params.m_devName.c_str(), &sb)==0) && ((sb.st_mode & S_IFMT) == S_IFCHR) )
71 {
72 if (initdevice(m_params.m_devName.c_str(), mandatoryCapabilities) == -1)
73 {
74 LOG(ERROR) << "Cannot init device:" << m_params.m_devName;
75 }
76 }
77 else
78 {
79 // open a normal file
80 m_fd = open(m_params.m_devName.c_str(), O_WRONLY | O_CREAT | O_TRUNC, S_IRWXU);
81 }
82 return (m_fd!=-1);
83}
84
85// intialize the V4L2 device
86int V4l2Device::initdevice(const char *dev_name, unsigned int mandatoryCapabilities)
87{
88 m_fd = open(dev_name, m_params.m_openFlags);
89 if (m_fd < 0)
90 {
91 LOG(ERROR) << "Cannot open device:" << m_params.m_devName << " " << strerror(errno);
92 this->close();
93 return -1;
94 }
95 if (checkCapabilities(m_fd,mandatoryCapabilities) !=0)
96 {
97 this->close();
98 return -1;
99 }
100 if (configureFormat(m_fd) !=0)
101 {
102 this->close();
103 return -1;
104 }
105 if (configureParam(m_fd) !=0)
106 {
107 this->close();
108 return -1;
109 }
110
111 return m_fd;
112}
113
114// check needed V4L2 capabilities
115int V4l2Device::checkCapabilities(int fd, unsigned int mandatoryCapabilities)
116{
117 struct v4l2_capability cap;
118 memset(&(cap), 0, sizeof(cap));
119 if (-1 == ioctl(fd, VIDIOC_QUERYCAP, &cap))
120 {
121 LOG(ERROR) << "Cannot get capabilities for device:" << m_params.m_devName << " " << strerror(errno);
122 return -1;
123 }
124 LOG(NOTICE) << "driver:" << cap.driver << " capabilities:" << std::hex << cap.capabilities << " mandatory:" << mandatoryCapabilities << std::dec;
125
126 if ((cap.capabilities & V4L2_CAP_VIDEO_OUTPUT)) LOG(NOTICE) << m_params.m_devName << " support output";
127 if ((cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) LOG(NOTICE) << m_params.m_devName << " support capture";
128
129 if ((cap.capabilities & V4L2_CAP_READWRITE)) LOG(NOTICE) << m_params.m_devName << " support read/write";
130 if ((cap.capabilities & V4L2_CAP_STREAMING)) LOG(NOTICE) << m_params.m_devName << " support streaming";
131
132 if ((cap.capabilities & V4L2_CAP_TIMEPERFRAME)) LOG(NOTICE) << m_params.m_devName << " support timeperframe";
133
134 if ( (cap.capabilities & mandatoryCapabilities) != mandatoryCapabilities )
135 {
136 LOG(ERROR) << "Mandatory capability not available for device:" << m_params.m_devName;
137 return -1;
138 }
139
140 return 0;
141}
142
143// configure capture format
144int V4l2Device::configureFormat(int fd)
145{
146 // get current configuration
147 this->queryFormat();
148
149 unsigned int width = m_width;
150 unsigned int height = m_height;
151 if (m_params.m_width != 0) {
152 width= m_params.m_width;
153 }
154 if (m_params.m_height != 0) {
155 height= m_params.m_height;
156 }
157 if ( (m_params.m_formatList.size()==0) && (m_format != 0) ) {
158 m_params.m_formatList.push_back(m_format);
159 }
160
161 // try to set format, widht, height
162 std::list<unsigned int>::iterator it;
163 for (it = m_params.m_formatList.begin(); it != m_params.m_formatList.end(); ++it) {
164 unsigned int format = *it;
165 if (this->configureFormat(fd, format, width, height)==0) {
166 // format has been set
167 // get the format again because calling SET-FMT return a bad buffersize using v4l2loopback
168 this->queryFormat();
169 return 0;
170 }
171 }
172 return -1;
173}
174
175// configure capture format
176int V4l2Device::configureFormat(int fd, unsigned int format, unsigned int width, unsigned int height)
177{
178 struct v4l2_format fmt;
179 memset(&(fmt), 0, sizeof(fmt));
180 fmt.type = m_deviceType;
181 fmt.fmt.pix.width = width;
182 fmt.fmt.pix.height = height;
183 fmt.fmt.pix.pixelformat = format;
184 fmt.fmt.pix.field = V4L2_FIELD_ANY;
185
186 if (ioctl(fd, VIDIOC_S_FMT, &fmt) == -1)
187 {
188 LOG(ERROR) << "Cannot set format:" << fourcc(format) << " for device:" << m_params.m_devName << " " << strerror(errno);
189 return -1;
190 }
191 if (fmt.fmt.pix.pixelformat != format)
192 {
193 LOG(ERROR) << "Cannot set pixelformat to:" << fourcc(format) << " format is:" << fourcc(fmt.fmt.pix.pixelformat);
194 return -1;
195 }
196 if ((fmt.fmt.pix.width != width) || (fmt.fmt.pix.height != height))
197 {
198 LOG(WARN) << "Cannot set size to:" << width << "x" << height << " size is:" << fmt.fmt.pix.width << "x" << fmt.fmt.pix.height;
199 }
200
201 m_format = fmt.fmt.pix.pixelformat;
202 m_width = fmt.fmt.pix.width;
203 m_height = fmt.fmt.pix.height;
204 m_bufferSize = fmt.fmt.pix.sizeimage;
205
206 LOG(NOTICE) << m_params.m_devName << ":" << fourcc(m_format) << " size:" << m_width << "x" << m_height << " bufferSize:" << m_bufferSize;
207
208 return 0;
209}
210
211// configure capture FPS
212int V4l2Device::configureParam(int fd)
213{
214 if (m_params.m_fps!=0)
215 {
216 struct v4l2_streamparm param;
217 memset(&(param), 0, sizeof(param));
218 param.type = m_deviceType;
219 param.parm.capture.timeperframe.numerator = 1;
220 param.parm.capture.timeperframe.denominator = m_params.m_fps;
221
222 if (ioctl(fd, VIDIOC_S_PARM, &param) == -1)
223 {
224 LOG(WARN) << "Cannot set param for device:" << m_params.m_devName << " " << strerror(errno);
225 }
226
227 LOG(NOTICE) << "fps:" << param.parm.capture.timeperframe.numerator << "/" << param.parm.capture.timeperframe.denominator;
228 LOG(NOTICE) << "nbBuffer:" << param.parm.capture.readbuffers;
229 }
230
231 return 0;
232}
233
234
235