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** V4l2MmapDevice.cpp
7**
8** V4L2 source using mmap API
9**
10** -------------------------------------------------------------------------*/
11
12#include <string.h>
13#include <fcntl.h>
14#include <stdio.h>
15#include <errno.h>
16#include <sys/mman.h>
17#include <sys/ioctl.h>
18
19// libv4l2
20#include <linux/videodev2.h>
21
22// project
23#include "logger.h"
24#include "V4l2MmapDevice.h"
25
26V4l2MmapDevice::V4l2MmapDevice(const V4L2DeviceParameters & params, v4l2_buf_type deviceType) : V4l2Device(params, deviceType), n_buffers(0)
27{
28 memset(&m_buffer, 0, sizeof(m_buffer));
29}
30
31bool V4l2MmapDevice::init(unsigned int mandatoryCapabilities)
32{
33 bool ret = V4l2Device::init(mandatoryCapabilities);
34 if (ret)
35 {
36 ret = this->start();
37 }
38 return ret;
39}
40
41V4l2MmapDevice::~V4l2MmapDevice()
42{
43 this->stop();
44}
45
46
47bool V4l2MmapDevice::start()
48{
49 LOG(NOTICE) << "Device " << m_params.m_devName;
50
51 bool success = true;
52 struct v4l2_requestbuffers req;
53 memset (&req, 0, sizeof(req));
54 req.count = V4L2MMAP_NBBUFFER;
55 req.type = m_deviceType;
56 req.memory = V4L2_MEMORY_MMAP;
57
58 if (-1 == ioctl(m_fd, VIDIOC_REQBUFS, &req))
59 {
60 if (EINVAL == errno)
61 {
62 LOG(ERROR) << "Device " << m_params.m_devName << " does not support memory mapping";
63 success = false;
64 }
65 else
66 {
67 perror("VIDIOC_REQBUFS");
68 success = false;
69 }
70 }
71 else
72 {
73 LOG(NOTICE) << "Device " << m_params.m_devName << " nb buffer:" << req.count;
74
75 // allocate buffers
76 memset(&m_buffer,0, sizeof(m_buffer));
77 for (n_buffers = 0; n_buffers < req.count; ++n_buffers)
78 {
79 struct v4l2_buffer buf;
80 memset (&buf, 0, sizeof(buf));
81 buf.type = m_deviceType;
82 buf.memory = V4L2_MEMORY_MMAP;
83 buf.index = n_buffers;
84
85 if (-1 == ioctl(m_fd, VIDIOC_QUERYBUF, &buf))
86 {
87 perror("VIDIOC_QUERYBUF");
88 success = false;
89 }
90 else
91 {
92 LOG(INFO) << "Device " << m_params.m_devName << " buffer idx:" << n_buffers << " size:" << buf.length << " offset:" << buf.m.offset;
93 m_buffer[n_buffers].length = buf.length;
94 if (!m_buffer[n_buffers].length) {
95 m_buffer[n_buffers].length = buf.bytesused;
96 }
97 m_buffer[n_buffers].start = mmap ( NULL /* start anywhere */,
98 m_buffer[n_buffers].length,
99 PROT_READ | PROT_WRITE /* required */,
100 MAP_SHARED /* recommended */,
101 m_fd,
102 buf.m.offset);
103
104 if (MAP_FAILED == m_buffer[n_buffers].start)
105 {
106 perror("mmap");
107 success = false;
108 }
109 }
110 }
111
112 // queue buffers
113 for (unsigned int i = 0; i < n_buffers; ++i)
114 {
115 struct v4l2_buffer buf;
116 memset (&buf, 0, sizeof(buf));
117 buf.type = m_deviceType;
118 buf.memory = V4L2_MEMORY_MMAP;
119 buf.index = i;
120
121 if (-1 == ioctl(m_fd, VIDIOC_QBUF, &buf))
122 {
123 perror("VIDIOC_QBUF");
124 success = false;
125 }
126 }
127
128 // start stream
129 int type = m_deviceType;
130 if (-1 == ioctl(m_fd, VIDIOC_STREAMON, &type))
131 {
132 perror("VIDIOC_STREAMON");
133 success = false;
134 }
135 }
136 return success;
137}
138
139bool V4l2MmapDevice::stop()
140{
141 LOG(NOTICE) << "Device " << m_params.m_devName;
142
143 bool success = true;
144
145 int type = m_deviceType;
146 if (-1 == ioctl(m_fd, VIDIOC_STREAMOFF, &type))
147 {
148 perror("VIDIOC_STREAMOFF");
149 success = false;
150 }
151
152 for (unsigned int i = 0; i < n_buffers; ++i)
153 {
154 if (-1 == munmap (m_buffer[i].start, m_buffer[i].length))
155 {
156 perror("munmap");
157 success = false;
158 }
159 }
160
161 // free buffers
162 struct v4l2_requestbuffers req;
163 memset (&req, 0, sizeof(req));
164 req.count = 0;
165 req.type = m_deviceType;
166 req.memory = V4L2_MEMORY_MMAP;
167 if (-1 == ioctl(m_fd, VIDIOC_REQBUFS, &req))
168 {
169 perror("VIDIOC_REQBUFS");
170 success = false;
171 }
172
173 n_buffers = 0;
174 return success;
175}
176
177size_t V4l2MmapDevice::readInternal(char* buffer, size_t bufferSize)
178{
179 size_t size = 0;
180 if (n_buffers > 0)
181 {
182 struct v4l2_buffer buf;
183 memset (&buf, 0, sizeof(buf));
184 buf.type = m_deviceType;
185 buf.memory = V4L2_MEMORY_MMAP;
186
187 if (-1 == ioctl(m_fd, VIDIOC_DQBUF, &buf))
188 {
189 perror("VIDIOC_DQBUF");
190 size = -1;
191 }
192 else if (buf.index < n_buffers)
193 {
194 size = buf.bytesused;
195 if (size > bufferSize)
196 {
197 size = bufferSize;
198 LOG(WARN) << "Device " << m_params.m_devName << " buffer truncated available:" << bufferSize << " needed:" << buf.bytesused;
199 }
200 memcpy(buffer, m_buffer[buf.index].start, size);
201
202 if (-1 == ioctl(m_fd, VIDIOC_QBUF, &buf))
203 {
204 perror("VIDIOC_QBUF");
205 size = -1;
206 }
207 }
208 }
209 return size;
210}
211
212size_t V4l2MmapDevice::writeInternal(char* buffer, size_t bufferSize)
213{
214 size_t size = 0;
215 if (n_buffers > 0)
216 {
217 struct v4l2_buffer buf;
218 memset (&buf, 0, sizeof(buf));
219 buf.type = m_deviceType;
220 buf.memory = V4L2_MEMORY_MMAP;
221
222 if (-1 == ioctl(m_fd, VIDIOC_DQBUF, &buf))
223 {
224 perror("VIDIOC_DQBUF");
225 size = -1;
226 }
227 else if (buf.index < n_buffers)
228 {
229 size = bufferSize;
230 if (size > buf.length)
231 {
232 LOG(WARN) << "Device " << m_params.m_devName << " buffer truncated available:" << buf.length << " needed:" << size;
233 size = buf.length;
234 }
235 memcpy(m_buffer[buf.index].start, buffer, size);
236 buf.bytesused = size;
237
238 if (-1 == ioctl(m_fd, VIDIOC_QBUF, &buf))
239 {
240 perror("VIDIOC_QBUF");
241 size = -1;
242 }
243 }
244 }
245 return size;
246}
247
248bool V4l2MmapDevice::startPartialWrite(void)
249{
250 if (n_buffers <= 0)
251 return false;
252 if (m_partialWriteInProgress)
253 return false;
254 memset(&m_partialWriteBuf, 0, sizeof(m_partialWriteBuf));
255 m_partialWriteBuf.type = m_deviceType;
256 m_partialWriteBuf.memory = V4L2_MEMORY_MMAP;
257 if (-1 == ioctl(m_fd, VIDIOC_DQBUF, &m_partialWriteBuf))
258 {
259 perror("VIDIOC_DQBUF");
260 return false;
261 }
262 m_partialWriteBuf.bytesused = 0;
263 m_partialWriteInProgress = true;
264 return true;
265}
266
267size_t V4l2MmapDevice::writePartialInternal(char* buffer, size_t bufferSize)
268{
269 size_t new_size = 0;
270 size_t size = 0;
271 if ((n_buffers > 0) && m_partialWriteInProgress)
272 {
273 if (m_partialWriteBuf.index < n_buffers)
274 {
275 new_size = m_partialWriteBuf.bytesused + bufferSize;
276 if (new_size > m_partialWriteBuf.length)
277 {
278 LOG(WARN) << "Device " << m_params.m_devName << " buffer truncated available:" << m_partialWriteBuf.length << " needed:" << new_size;
279 new_size = m_partialWriteBuf.length;
280 }
281 size = new_size - m_partialWriteBuf.bytesused;
282 memcpy(&((char *)m_buffer[m_partialWriteBuf.index].start)[m_partialWriteBuf.bytesused], buffer, size);
283
284 m_partialWriteBuf.bytesused += size;
285 }
286 }
287 return size;
288}
289
290bool V4l2MmapDevice::endPartialWrite(void)
291{
292 if (!m_partialWriteInProgress)
293 return false;
294 if (n_buffers <= 0)
295 {
296 m_partialWriteInProgress = false; // abort partial write
297 return true;
298 }
299 if (-1 == ioctl(m_fd, VIDIOC_QBUF, &m_partialWriteBuf))
300 {
301 perror("VIDIOC_QBUF");
302 m_partialWriteInProgress = false; // abort partial write
303 return true;
304 }
305 m_partialWriteInProgress = false;
306 return true;
307}
308