JeVois  1.5
JeVois Smart Embedded Machine Vision Toolkit
Share this page:
Gadget.C
Go to the documentation of this file.
1 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
2 //
3 // JeVois Smart Embedded Machine Vision Toolkit - Copyright (C) 2016 by Laurent Itti, the University of Southern
4 // California (USC), and iLab at USC. See http://iLab.usc.edu and http://jevois.org for information about this project.
5 //
6 // This file is part of the JeVois Smart Embedded Machine Vision Toolkit. This program is free software; you can
7 // redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software
8 // Foundation, version 2. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
9 // without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public
10 // License for more details. You should have received a copy of the GNU General Public License along with this program;
11 // if not, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
12 //
13 // Contact information: Laurent Itti - 3641 Watt Way, HNB-07A - Los Angeles, CA 90089-2520 - USA.
14 // Tel: +1 213 740 3527 - itti@pollux.usc.edu - http://iLab.usc.edu - http://jevois.org
15 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
16 /*! \file */
17 
18 #include <jevois/Core/Gadget.H>
19 #include <jevois/Debug/Log.H>
20 #include <jevois/Core/VideoInput.H>
21 #include <jevois/Util/Utils.H>
23 #include <jevois/Core/Engine.H>
24 
25 #include <sys/types.h>
26 #include <sys/stat.h>
27 #include <fcntl.h>
28 #include <unistd.h>
29 #include <sys/time.h> // for gettimeofday()
30 
31 namespace
32 {
33  inline void debugCtrlReq(struct usb_ctrlrequest const & ctrl)
34  {
35  (void)ctrl; // avoid compiler warning about unused param if LDEBUG is turned off
36 
37  LDEBUG(std::showbase << std::hex << "bRequestType " << ctrl.bRequestType << " bRequest " << ctrl.bRequest
38  << " wValue " << ctrl.wValue << " wIndex " << ctrl.wIndex << " wLength " << ctrl.wLength);
39  }
40 
41  inline void debugStreamingCtrl(std::string const & msg, struct uvc_streaming_control const & ctrl)
42  {
43  (void)ctrl; // avoid compiler warning about unused param if LDEBUG is turned off
44  (void)msg; // avoid compiler warning about unused param if LDEBUG is turned off
45  LDEBUG(msg << ": " << std::showbase << std::hex << "bmHint=" << ctrl.bmHint << ", bFormatIndex=" <<
46  ctrl.bFormatIndex << ", bFrameIndex=" << ctrl.bFrameIndex << ", dwFrameInterval=" << ctrl.dwFrameInterval <<
47  ", wKeyFrameRate=" << ctrl.wKeyFrameRate << ", wPFrameRate=" << ctrl.wPFrameRate <<
48  ", wCompQuality=" << ctrl.wCompQuality << ", wCompWindowSize=" << ctrl.wCompWindowSize <<
49  ", wDelay=" << ctrl.wDelay << ", dwMaxVideoFrameSize=" << ctrl.dwMaxVideoFrameSize <<
50  ", dwMaxPayloadTransferSize=" << ctrl.dwMaxPayloadTransferSize << ", dwClockFrequency=" <<
51  ctrl.dwClockFrequency << ", bmFramingInfo=" << ctrl.bmFramingInfo << ", bPreferedVersion=" <<
52  ctrl.bPreferedVersion << ", bMinVersion=" << ctrl.bMinVersion << ", bMaxVersion=" << ctrl.bMaxVersion);
53  }
54 
55  unsigned int uvcToV4Lcontrol(unsigned int entity, unsigned int cs)
56  {
57  switch (entity)
58  {
59  case 1: // Our camera unit
60  // -- #define UVC_CT_SCANNING_MODE_CONTROL 0x01
61  // OK #define UVC_CT_AE_MODE_CONTROL 0x02
62  // OK #define UVC_CT_AE_PRIORITY_CONTROL 0x03
63  // OK #define UVC_CT_EXPOSURE_TIME_ABSOLUTE_CONTROL 0x04
64  // -- #define UVC_CT_EXPOSURE_TIME_RELATIVE_CONTROL 0x05
65  // -- #define UVC_CT_FOCUS_ABSOLUTE_CONTROL 0x06
66  // -- #define UVC_CT_FOCUS_RELATIVE_CONTROL 0x07
67  // -- #define UVC_CT_FOCUS_AUTO_CONTROL 0x08
68  // -- #define UVC_CT_IRIS_ABSOLUTE_CONTROL 0x09
69  // -- #define UVC_CT_IRIS_RELATIVE_CONTROL 0x0a
70  // -- #define UVC_CT_ZOOM_ABSOLUTE_CONTROL 0x0b
71  // -- #define UVC_CT_ZOOM_RELATIVE_CONTROL 0x0c
72  // -- #define UVC_CT_PANTILT_ABSOLUTE_CONTROL 0x0d
73  // -- #define UVC_CT_PANTILT_RELATIVE_CONTROL 0x0e
74  // -- #define UVC_CT_ROLL_ABSOLUTE_CONTROL 0x0f
75  // -- #define UVC_CT_ROLL_RELATIVE_CONTROL 0x10
76  // -- #define UVC_CT_PRIVACY_CONTROL 0x11
77  // note: UVC 1.5 has a few more...
78  switch (cs)
79  {
80  case UVC_CT_AE_MODE_CONTROL: return V4L2_CID_EXPOSURE_AUTO;
81  case UVC_CT_AE_PRIORITY_CONTROL: return V4L2_CID_EXPOSURE_AUTO_PRIORITY;
82  case UVC_CT_EXPOSURE_TIME_ABSOLUTE_CONTROL: return V4L2_CID_EXPOSURE_ABSOLUTE;
83  }
84  break;
85 
86  case 2: // Our processing unit
87  // From uvcvideo.h and the UVC specs, here are the available processing unit controls:
88  // A.9.5. Processing Unit Control Selectors
89 
90  // Note one trick here: UVC_PU_WHITE_BALANCE_COMPONENT_CONTROL contains both the V4L2_CID_RED_BALANCE and
91  // V4L2_CID_BLUE_BALANCE; we handle that in the ioctl processing section, here we just return V4L2_CID_RED_BALANCE
92 
93  // OK #define UVC_PU_BACKLIGHT_COMPENSATION_CONTROL 0x01
94  // OK #define UVC_PU_BRIGHTNESS_CONTROL 0x02
95  // OK #define UVC_PU_CONTRAST_CONTROL 0x03
96  // OK #define UVC_PU_GAIN_CONTROL 0x04
97  // OK #define UVC_PU_POWER_LINE_FREQUENCY_CONTROL 0x05
98  // OK #define UVC_PU_HUE_CONTROL 0x06
99  // OK #define UVC_PU_SATURATION_CONTROL 0x07
100  // OK #define UVC_PU_SHARPNESS_CONTROL 0x08
101  // -- #define UVC_PU_GAMMA_CONTROL 0x09
102  // -- #define UVC_PU_WHITE_BALANCE_TEMPERATURE_CONTROL 0x0a
103  // -- #define UVC_PU_WHITE_BALANCE_TEMPERATURE_AUTO_CONTROL 0x0b
104  // OK #define UVC_PU_WHITE_BALANCE_COMPONENT_CONTROL 0x0c
105  // OK #define UVC_PU_WHITE_BALANCE_COMPONENT_AUTO_CONTROL 0x0d
106  // TODO #define UVC_PU_DIGITAL_MULTIPLIER_CONTROL 0x0e
107  // TODO #define UVC_PU_DIGITAL_MULTIPLIER_LIMIT_CONTROL 0x0f
108  // TODO #define UVC_PU_HUE_AUTO_CONTROL 0x10
109  // TODO #define UVC_PU_ANALOG_VIDEO_STANDARD_CONTROL 0x11
110  // TODO #define UVC_PU_ANALOG_LOCK_STATUS_CONTROL 0x12
111  switch (cs)
112  {
113  case UVC_PU_BACKLIGHT_COMPENSATION_CONTROL: return V4L2_CID_BACKLIGHT_COMPENSATION;
114  case UVC_PU_BRIGHTNESS_CONTROL: return V4L2_CID_BRIGHTNESS;
115  case UVC_PU_CONTRAST_CONTROL: return V4L2_CID_CONTRAST;
116  case UVC_PU_GAIN_CONTROL: return V4L2_CID_GAIN;
117  case UVC_PU_POWER_LINE_FREQUENCY_CONTROL: return V4L2_CID_POWER_LINE_FREQUENCY;
118  case UVC_PU_HUE_CONTROL: return V4L2_CID_HUE;
119  case UVC_PU_SATURATION_CONTROL: return V4L2_CID_SATURATION;
120  case UVC_PU_SHARPNESS_CONTROL: return V4L2_CID_SHARPNESS;
121  case UVC_PU_WHITE_BALANCE_COMPONENT_CONTROL: return V4L2_CID_RED_BALANCE;
122  case UVC_PU_WHITE_BALANCE_COMPONENT_AUTO_CONTROL: return V4L2_CID_AUTO_WHITE_BALANCE;
123  //case UVC_PU_GAMMA_CONTROL: return V4L2_CID_GAMMA;
124  }
125  break;
126  }
127 
128  LFATAL("Request to access unsupported control " << cs << " on entity " << entity);
129  }
130 
131 } // namespace
132 
133 // ##############################################################################################################
134 jevois::Gadget::Gadget(std::string const & devname, jevois::VideoInput * camera, jevois::Engine * engine,
135  size_t const nbufs) :
136  itsFd(-1), itsNbufs(nbufs), itsBuffers(nullptr), itsCamera(camera), itsEngine(engine), itsRunning(false),
137  itsFormat(), itsFps(0.0F), itsStreaming(false), itsErrorCode(0), itsControl(0), itsEntity(0)
138 {
139  JEVOIS_TRACE(1);
140 
141  if (itsCamera == nullptr) LFATAL("Gadget requires a valid camera to work");
142 
143  jevois::VideoMapping const & m = itsEngine->getDefaultVideoMapping();
144  fillStreamingControl(&itsProbe, m);
145  fillStreamingControl(&itsCommit, m);
146 
147  // Get our run() thread going and wait until it is cranking, it will flip itsRunning to true as it starts:
148  itsRunFuture = std::async(std::launch::async, &jevois::Gadget::run, this);
149  while (itsRunning.load() == false) std::this_thread::sleep_for(std::chrono::milliseconds(5));
150 
151  // Open the device:
152  itsFd = open(devname.c_str(), O_RDWR | O_NONBLOCK);
153  if (itsFd == -1) PLFATAL("Gadget device open failed for " << devname);
154 
155  // Get ready to handle UVC events:
156  struct v4l2_event_subscription sub = { };
157 
158  sub.type = UVC_EVENT_SETUP;
159  XIOCTL(itsFd, VIDIOC_SUBSCRIBE_EVENT, &sub);
160 
161  sub.type = UVC_EVENT_DATA;
162  XIOCTL(itsFd, VIDIOC_SUBSCRIBE_EVENT, &sub);
163 
164  sub.type = UVC_EVENT_STREAMON;
165  XIOCTL(itsFd, VIDIOC_SUBSCRIBE_EVENT, &sub);
166 
167  sub.type = UVC_EVENT_STREAMOFF;
168  XIOCTL(itsFd, VIDIOC_SUBSCRIBE_EVENT, &sub);
169 
170  // Find out what the driver can do:
171  struct v4l2_capability cap = { };
172  XIOCTL(itsFd, VIDIOC_QUERYCAP, &cap);
173 
174  LINFO('[' << itsFd << "] UVC gadget " << devname << " card " << cap.card << " bus " << cap.bus_info);
175  if ((cap.capabilities & V4L2_CAP_VIDEO_OUTPUT) == 0) LFATAL(devname << " is not a video output device");
176  if ((cap.capabilities & V4L2_CAP_STREAMING) == 0) LFATAL(devname << " does not support streaming");
177 }
178 
179 // ##############################################################################################################
181 {
182  JEVOIS_TRACE(1);
183 
184  streamOff();
185 
186  // Tell run() thread to finish up:
187  itsRunning.store(false);
188 
189  // Will block until the run() thread completes:
190  if (itsRunFuture.valid()) try { itsRunFuture.get(); } catch (...) { jevois::warnAndIgnoreException(); }
191 
192  if (close(itsFd) == -1) PLERROR("Error closing UVC gadget -- IGNORED");
193 }
194 
195 // ##############################################################################################################
197 {
198  JEVOIS_TRACE(2);
199 
200  JEVOIS_TIMED_LOCK(itsMtx);
201 
202  // Set the format:
203  memset(&itsFormat, 0, sizeof(struct v4l2_format));
204 
205  itsFormat.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
206  itsFormat.fmt.pix.width = m.ow;
207  itsFormat.fmt.pix.height = m.oh;
208  itsFormat.fmt.pix.pixelformat = m.ofmt;
209  itsFormat.fmt.pix.field = V4L2_FIELD_NONE;
210  itsFormat.fmt.pix.sizeimage = m.osize();
211  itsFps = m.ofps;
212 
213  // First try to set our own format, will throw if phony:
214  XIOCTL(itsFd, VIDIOC_S_FMT, &itsFormat);
215 
216  // Note that the format does not include fps, this is done with VIDIOC_S_PARM:
217  try
218  {
219  // The gadget driver may not support this ioctl...
220  struct v4l2_streamparm sparm = { };
221  sparm.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
222  sparm.parm.output.outputmode = 2; // V4L2_MODE_VIDEO not defined in our headers? its value is 2.
223  sparm.parm.output.timeperframe = jevois::VideoMapping::fpsToV4l2(m.ofps);
224  XIOCTL_QUIET(itsFd, VIDIOC_S_PARM, &sparm);
225  } catch (...) { }
226 }
227 
228 // ##############################################################################################################
229 void jevois::Gadget::run()
230 {
231  JEVOIS_TRACE(1);
232 
233  fd_set wfds; // For UVC video streaming
234  fd_set efds; // For UVC events
235  struct timeval tv;
236 
237  // Switch to running state:
238  itsRunning.store(true);
239 
240  // We may have to wait until the device is opened:
241  while (itsFd == -1) std::this_thread::sleep_for(std::chrono::milliseconds(1));
242 
243  // Wait for event from the gadget kernel driver and process them:
244  while (itsRunning.load())
245  {
246  // Wait until we either receive an event or we are ready to send the next buffer over:
247  FD_ZERO(&wfds); FD_ZERO(&efds); FD_SET(itsFd, &wfds); FD_SET(itsFd, &efds);
248  tv.tv_sec = 0; tv.tv_usec = 10000;
249 
250  int ret = select(itsFd + 1, nullptr, &wfds, &efds, &tv);
251 
252  if (ret == -1) { PLERROR("Select error"); if (errno == EINTR) continue; else break; }
253  else if (ret > 0) // We have some events, handle them right away:
254  {
255  // Note: we may have more than one event, so here we try processEvents() several times to be sure:
256  if (FD_ISSET(itsFd, &efds))
257  {
258  // First event, we will report error if any:
259  try { processEvents(); } catch (...) { jevois::warnAndIgnoreException(); }
260 
261  // Let's try to dequeue one more, in most cases it should throw:
262  while (true) try { processEvents(); } catch (...) { break; }
263  }
264 
265  if (FD_ISSET(itsFd, &wfds)) try { processVideo(); } catch (...) { jevois::warnAndIgnoreException(); }
266  }
267 
268  // We timed out
269 
270  // Sometimes we miss events in the main loop, likely because more events come while we are unlocked in the USB UDC
271  // driver and processing here. So let's try to dequeue one more, in most cases it should throw:
272  while (true) try { processEvents(); } catch (...) { break; }
273 
274  // While the driver is not busy in select(), queue at most one buffer that is ready to send off:
275  try
276  {
277  JEVOIS_TIMED_LOCK(itsMtx);
278  if (itsDoneImgs.size())
279  {
280  LDEBUG("Queuing image " << itsDoneImgs.front() << " for sending over USB");
281 
282  // We need to prepare a legit v4l2_buffer, including bytesused:
283  struct v4l2_buffer buf = { };
284 
285  buf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
286  buf.memory = V4L2_MEMORY_MMAP;
287  buf.index = itsDoneImgs.front();
288  buf.length = itsBuffers->get(buf.index)->length();
289 
290  if (itsFormat.fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG)
291  buf.bytesused = itsBuffers->get(buf.index)->bytesUsed();
292  else
293  buf.bytesused = buf.length;
294 
295  buf.field = V4L2_FIELD_NONE;
296  buf.flags = 0;
297  gettimeofday(&buf.timestamp, nullptr);
298 
299  // Queue it up so it can be sent to the host:
300  itsBuffers->qbuf(buf);
301 
302  // This one is done:
303  itsDoneImgs.pop_front();
304  }
305  } catch (...) { jevois::warnAndIgnoreException(); std::this_thread::sleep_for(std::chrono::milliseconds(10)); }
306  }
307 
308  // Switch out of running state in case we did interrupt the loop here by a break statement:
309  itsRunning.store(false);
310 }
311 
312 // ##############################################################################################################
313 void jevois::Gadget::processEvents()
314 {
315  JEVOIS_TRACE(3);
316 
317  // Get the event from the driver:
318  struct v4l2_event v4l2ev = { };
319  XIOCTL_QUIET(itsFd, VIDIOC_DQEVENT, &v4l2ev);
320  struct uvc_event * uvcev = reinterpret_cast<struct uvc_event *>(&v4l2ev.u.data);
321 
322  // Prepare our response, if any will be sent:
323  struct uvc_request_data resp = { };
324  resp.length = -EL2HLT;
325 
326  // Act according to the event type:
327  try
328  {
329  switch (v4l2ev.type)
330  {
331  case UVC_EVENT_CONNECT: return;
332  case UVC_EVENT_DISCONNECT: LDEBUG("EVENT DISCONNECT"); itsEngine->streamOff(); return;
333  case UVC_EVENT_SETUP: LDEBUG("EVENT SETUP"); processEventSetup(uvcev->req, resp); return;
334  case UVC_EVENT_DATA: LDEBUG("EVENT DATA"); processEventData(uvcev->data); return;
335  case UVC_EVENT_STREAMON: LDEBUG("EVENT STREAMON"); itsEngine->streamOn(); return;
336  case UVC_EVENT_STREAMOFF: LDEBUG("EVENT STREAMOFF"); itsEngine->streamOff(); return;
337  }
338  } catch (...) { }
339 }
340 
341 // ##############################################################################################################
342 void jevois::Gadget::processVideo()
343 {
344  JEVOIS_TRACE(3);
345 
346  jevois::RawImage img;
347  JEVOIS_TIMED_LOCK(itsMtx);
348 
349  // If we are not streaming anymore, abort:
350  if (itsStreaming.load() == false) LFATAL("Aborted while not streaming");
351 
352  // Dequeue a buffer from the gadget driver, this is an image that has been sent to the host and hence the buffer is
353  // now available to be filled up with image data and later queued again to the gadget driver:
354  struct v4l2_buffer buf;
355  itsBuffers->dqbuf(buf);
356 
357  // Create a RawImage from that buffer:
358  img.width = itsFormat.fmt.pix.width;
359  img.height = itsFormat.fmt.pix.height;
360  img.fmt = itsFormat.fmt.pix.pixelformat;
361  img.fps = itsFps;
362  img.buf = itsBuffers->get(buf.index);
363  img.bufindex = buf.index;
364 
365  // Push the RawImage to outside consumers:
366  itsImageQueue.push_back(img);
367  LDEBUG("Empty image " << img.bufindex << " ready for filling in by application code");
368 }
369 
370 // ##############################################################################################################
371 void jevois::Gadget::processEventSetup(struct usb_ctrlrequest const & ctrl, struct uvc_request_data & resp)
372 {
373  JEVOIS_TRACE(3);
374 
375  itsControl = 0; itsEntity = 0;
376 
377  debugCtrlReq(ctrl);
378 
379  switch (ctrl.bRequestType & USB_TYPE_MASK)
380  {
381  case USB_TYPE_STANDARD: processEventStandard(ctrl, resp); break;
382  case USB_TYPE_CLASS: processEventClass(ctrl, resp); break;
383  default: LERROR("Unsupported setup event type " << std::showbase << std::hex <<
384  (ctrl.bRequestType & USB_TYPE_MASK) << " -- IGNORED");
385  }
386  if (ctrl.bRequestType != 0x21) XIOCTL(itsFd, UVCIOC_SEND_RESPONSE, &resp);
387 }
388 
389 // ##############################################################################################################
390 void jevois::Gadget::processEventStandard(struct usb_ctrlrequest const & ctrl,
391  struct uvc_request_data & JEVOIS_UNUSED_PARAM(resp))
392 {
393  JEVOIS_TRACE(3);
394 
395  LDEBUG("UVC standard setup event ignored:");
396  debugCtrlReq(ctrl);
397 }
398 
399 // ##############################################################################################################
400 void jevois::Gadget::processEventClass(struct usb_ctrlrequest const & ctrl, struct uvc_request_data & resp)
401 {
402  JEVOIS_TRACE(3);
403 
404  if ((ctrl.bRequestType & USB_RECIP_MASK) != USB_RECIP_INTERFACE) return;
405 
406  switch (ctrl.wIndex & 0xff)
407  {
408  case UVC_INTF_CONTROL:
409  processEventControl(ctrl.bRequest, ctrl.wValue >> 8, ctrl.wIndex >> 8, ctrl.wLength, resp);
410  break;
411 
412  case UVC_INTF_STREAMING:
413  processEventStreaming(ctrl.bRequest, ctrl.wValue >> 8, resp);
414  break;
415 
416  default:
417  LERROR("Unsupported setup event class " << std::showbase << std::hex << (ctrl.wIndex & 0xff) << " -- IGNORED");
418  }
419 }
420 
421 // ##############################################################################################################
422 void jevois::Gadget::processEventControl(uint8_t req, uint8_t cs, uint8_t entity_id, uint8_t len,
423  struct uvc_request_data & resp)
424 {
425  JEVOIS_TRACE(3);
426 
427  // Local function we run on successful processing of an event: we just reset our internal error code
428 #define success() { itsErrorCode = 0; }
429 
430  // Local function we run on failed processing of an event: stall the request, set our internal error code
431 #define failure(code) { resp.length = -EL2HLT; itsErrorCode = code; }
432 
433  // Shortcurt to successfully send a 1-byte response:
434 #define byteresponse(val) { resp.data[0] = val; resp.length = 1; itsErrorCode = 0; }
435 
436  // Shortcurt to successfully send a 2-byte response:
437 #define wordresponse(val) { resp.data[0] = val & 0xff; resp.data[1] = (val >> 8) & 0xff; \
438  resp.length = 2; itsErrorCode = 0; }
439 
440  // Shortcurt to successfully send a 4-byte response:
441 #define intresponse(val) { resp.data[0] = val & 0xff; resp.data[1] = (val >> 8) & 0xff; \
442  resp.data[2] = (val >> 16) & 0xff; resp.data[3] = (val >> 24) & 0xff; \
443  resp.length = 4; itsErrorCode = 0; }
444 
445  // If anything throws here, we will return failure:
446  try
447  {
448  // First handle any request that is directed to entity 0:
449  if (entity_id == 0)
450  {
451  switch (cs)
452  {
453  case UVC_VC_REQUEST_ERROR_CODE_CONTROL: byteresponse(itsErrorCode); return; // Send error code last prepared
454  default: failure(0x06); return;
455  }
456  }
457 
458  // Process according to the entity that this event is directed to and the control that is requested:
459  if (req == UVC_SET_CUR)
460  {
461  // We need to wait for the data phase, so for now just remember the control and return success:
462  itsEntity = entity_id; itsControl = cs;
463  resp.data[0] = 0x0; resp.length = len; success();
464  LDEBUG("SET_CUR ent " << itsEntity <<" ctrl "<< itsControl <<" len "<< len);
465  }
466  else if (req == UVC_GET_INFO)
467  {
468  // FIXME: controls could also be disabled, autoupdate, or asynchronous:
469  byteresponse(UVC_CONTROL_CAP_GET | UVC_CONTROL_CAP_SET);
470  }
471  else if (req == UVC_GET_CUR)
472  {
473  // Fetch the current value from the camera. Note: both Windows and Android insist on querying some controls, like
474  // IRIS and GAMMA, which we did not declare as supported in the kernel driver. We need to handle those requests
475  // and to send some phony data:
476  struct v4l2_control ctrl = { };
477  try { ctrl.id = uvcToV4Lcontrol(entity_id, cs); itsCamera->getControl(ctrl); } catch (...) { ctrl.id = 0; }
478 
479  // We need a special handling of white balance here:
480  if (ctrl.id == V4L2_CID_RED_BALANCE)
481  {
482  unsigned int redval = (ctrl.value & 0xffff) << 16; // red is at offset 2 in PU_WHITE_BALANCE_COMPONENT_CONTROL
483 
484  // Also get the blue balance value:
485  ctrl.id = V4L2_CID_BLUE_BALANCE;
486  itsCamera->getControl(ctrl);
487 
488  // Combine both red and blue values:
489  ctrl.value = (ctrl.value & 0xffff) | redval;
490  }
491  // We also need to remap auto exposure values:
492  else if (ctrl.id == V4L2_CID_EXPOSURE_AUTO)
493  {
494  if (ctrl.value == V4L2_EXPOSURE_MANUAL) ctrl.value = 0x01; // manual mode, set UVC bit D0
495  else if (ctrl.value == V4L2_EXPOSURE_AUTO) ctrl.value = 0x02; // auto mode, set UVC bit D1
496  else ctrl.value = 0x03;
497  // Note, there are 2 more bits under CT_AE_MODE_CONTROL
498  }
499  // Handle the unknown controls:
500  else if (ctrl.id == 0) ctrl.value = 0;
501 
502  switch (len)
503  {
504  case 1: byteresponse(ctrl.value); break;
505  case 2: wordresponse(ctrl.value); break;
506  case 4: intresponse(ctrl.value); break;
507  default: LFATAL("Unsupported control with length " << len);
508  }
509  }
510  else
511  {
512  // It's a GET_DEF/RES/MIN/MAX let's first get the data from the camera: Note: both Windows and Android insist on
513  // querying some controls, like IRIS and GAMMA, which we did not declare as supported in the kernel driver. We
514  // need to handle those requests and to send some phony data:
515  struct v4l2_queryctrl qc = { };
516  try { qc.id = uvcToV4Lcontrol(entity_id, cs); itsCamera->queryControl(qc); } catch (...) { qc.id = 0; }
517 
518  // We need a special handling of white balance here:
519  if (qc.id == V4L2_CID_RED_BALANCE)
520  {
521  // Also get the blue balance values:
522  struct v4l2_queryctrl qc2 = { };
523  qc2.id = V4L2_CID_BLUE_BALANCE;
524  itsCamera->queryControl(qc2);
525 
526  // Combine red and blue values into qc:
527  qc.default_value = (qc.default_value << 16) | qc2.default_value;
528  qc.step = (qc.step << 16) | qc2.step;
529  qc.minimum = (qc.minimum << 16) | qc2.minimum;
530  qc.maximum = (qc.maximum << 16) | qc2.maximum;
531  }
532  // We also need to remap auto exposure values:
533  else if (qc.id == V4L2_CID_EXPOSURE_AUTO)
534  {
535  // Tricky: in the 'step' field, we are supposed to provide a bitmap of the modes that are supported, see UVC
536  // specs. D0=manual, D1=auto, D2=shutter priority, D3=aperture priority. Min and max are ignored for this
537  // control, default is handled.
538  qc.minimum = 0; qc.step = 3; qc.maximum = 3; qc.default_value = 1;
539  }
540  // Also handle the unknown controls here:
541  else if (qc.id == 0)
542  { qc.minimum = 0; qc.step = 1; qc.maximum = 1; qc.default_value = 0; }
543 
544  int val = 0;
545  switch (req)
546  {
547  case UVC_GET_DEF: val = qc.default_value; break;
548  case UVC_GET_RES: val = qc.step; break;
549  case UVC_GET_MIN: val = qc.minimum; break;
550  case UVC_GET_MAX: val = qc.maximum; break;
551  default: failure(0x07); return;
552  }
553 
554  switch (len)
555  {
556  case 1: byteresponse(val); break;
557  case 2: wordresponse(val); break;
558  case 4: intresponse(val); break;
559  default: LFATAL("Unsupported control with length " << len);
560  }
561  }
562  }
563  catch (...)
564  {
565  LERROR("FAILED entity " << entity_id << " cs " << cs << " len " << len);
566  failure(0x06);
567  }
568 }
569 
570 // ##############################################################################################################
571 void jevois::Gadget::fillStreamingControl(struct uvc_streaming_control * ctrl, jevois::VideoMapping const & m)
572 {
573  JEVOIS_TRACE(3);
574 
575  memset(ctrl, 0, sizeof(struct uvc_streaming_control));
576 
577  ctrl->bFormatIndex = m.uvcformat;
578  ctrl->bFrameIndex = m.uvcframe;
579  ctrl->dwFrameInterval = jevois::VideoMapping::fpsToUvc(m.ofps);
580  ctrl->dwMaxVideoFrameSize = m.osize();
581  ctrl->dwMaxPayloadTransferSize = 3072;
582  ctrl->bmFramingInfo = 3;
583  ctrl->bPreferedVersion = 1;
584  ctrl->bMaxVersion = 1;
585 }
586 
587 // ##############################################################################################################
588 void jevois::Gadget::processEventStreaming(uint8_t req, uint8_t cs, struct uvc_request_data & resp)
589 {
590  JEVOIS_TRACE(3);
591 
592  int const datalen = 26; // uvc 1.0 as reported by our kernel driver
593  if (cs != UVC_VS_PROBE_CONTROL && cs != UVC_VS_COMMIT_CONTROL) return;
594 
595  struct uvc_streaming_control * ctrl = reinterpret_cast<struct uvc_streaming_control *>(&resp.data);
596  struct uvc_streaming_control * target = (cs == UVC_VS_PROBE_CONTROL) ? &itsProbe : &itsCommit;
597  resp.length = datalen;
598 
599  switch (req)
600  {
601  case UVC_SET_CUR: itsControl = cs; resp.length = datalen; break; // will finish up in data stage
602 
603  case UVC_GET_CUR:
604  case UVC_GET_MIN: // we have nothing to negotiate
605  case UVC_GET_MAX: // we have nothing to negotiate
606  memcpy(ctrl, target, datalen);
607  break;
608 
609  case UVC_GET_DEF:
610  {
611  // If requested format index, frame index, or interval is bogus (including zero), initialize to our default mapping,
612  // otherwise pass down the selected mapping:
613  size_t idx = itsEngine->getDefaultVideoMappingIdx();
614  try { idx = itsEngine->getVideoMappingIdx(ctrl->bFormatIndex, ctrl->bFrameIndex, ctrl->dwFrameInterval); }
615  catch (...) { }
616  fillStreamingControl(target, itsEngine->getVideoMapping(idx));
617  memcpy(ctrl, target, datalen);
618  }
619  break;
620 
621  case UVC_GET_RES: memset(ctrl, 0, datalen); break;
622 
623  case UVC_GET_LEN: resp.data[0] = 0x00; resp.data[1] = datalen; resp.length = 2; break;
624 
625  case UVC_GET_INFO: resp.data[0] = 0x03; resp.length = 1; break;
626  }
627 }
628 
629 // ##############################################################################################################
630 void jevois::Gadget::processEventData(struct uvc_request_data & data)
631 {
632  JEVOIS_TRACE(3);
633 
634  struct uvc_streaming_control * target;
635 
636  // If entity is 1 or 2, this is to set a control:
637  if (itsEntity == 2 || itsEntity == 1) { processEventControlData(data); return; }
638 
639  switch (itsControl)
640  {
641  case UVC_VS_PROBE_CONTROL: target = &itsProbe; break;
642  case UVC_VS_COMMIT_CONTROL: target = &itsCommit; break;
643  default: processEventControlData(data); return;
644  }
645 
646  // Find the selected format and frame info and fill-in the control data:
647  struct uvc_streaming_control * ctrl = reinterpret_cast<struct uvc_streaming_control *>(&data.data);
648 
649  size_t idx = itsEngine->getVideoMappingIdx(ctrl->bFormatIndex, ctrl->bFrameIndex, ctrl->dwFrameInterval);
650 
651  fillStreamingControl(target, itsEngine->getVideoMapping(idx));
652 
653  LDEBUG("Host requested " << ctrl->bFormatIndex << '/' << ctrl->bFrameIndex << '/' << ctrl->dwFrameInterval <<
654  ", " << ((itsControl == UVC_VS_COMMIT_CONTROL) ? "setting " : "returning ") <<
655  itsEngine->getVideoMapping(idx).str());
656 
657  // Set the format if we are doing a commit control:
658  if (itsControl == UVC_VS_COMMIT_CONTROL) itsEngine->setFormat(idx);
659 }
660 
661 // ##############################################################################################################
662 void jevois::Gadget::processEventControlData(struct uvc_request_data & data)
663 {
664  JEVOIS_TRACE(3);
665 
666  struct v4l2_control ctrl;
667 
668  // Get the control ID for V4L or throw if unsupported:
669  ctrl.id = uvcToV4Lcontrol(itsEntity, itsControl);
670 
671  // Copy the data we received into the control's value:
672  switch (data.length)
673  {
674  case 1: ctrl.value = static_cast<int>(data.data[0]); break;
675  case 2: ctrl.value = static_cast<int>(__s16(data.data[0] | (static_cast<short>(data.data[1]) << 8))); break;
676  case 4: ctrl.value = data.data[0] | (data.data[1] << 8) | (data.data[2] << 16) | (data.data[3] << 24); break;
677  default: LFATAL("Ooops data len is " << data.length);
678  }
679 
680  // Tell the camera to set the control. We do not have enough time here to do it as otherwise our USB transaction will
681  // time out while we transfer a bunch of bytes to the camera over the 400kHz serial control link, so we just push it
682  // to a queue and our run() thread will do the work. First, handle special cases:
683  switch (ctrl.id)
684  {
685  case V4L2_CID_RED_BALANCE:
686  {
687  // We need to set both the red and the blue:
688  int blue = ctrl.value & 0xffff;
689  ctrl.value >>= 16; itsCamera->setControl(ctrl);
690  ctrl.id = V4L2_CID_BLUE_BALANCE; ctrl.value = blue; itsCamera->setControl(ctrl);
691  }
692  break;
693 
694  case V4L2_CID_EXPOSURE_AUTO:
695  if (ctrl.value & 0x01) ctrl.value = V4L2_EXPOSURE_MANUAL; // UVC bit D0 set for manual mode
696  else if (ctrl.value & 0x02) ctrl.value = V4L2_EXPOSURE_AUTO; // auto mode
697  // Note, there are 2 more bits under CT_AE_MODE_CONTROL
698  itsCamera->setControl(ctrl);
699  break;
700 
701  default: itsCamera->setControl(ctrl);
702  }
703 }
704 
705 // ##############################################################################################################
707 {
708  JEVOIS_TRACE(2);
709 
710  LDEBUG("Turning on UVC stream");
711 
712  JEVOIS_TIMED_LOCK(itsMtx);
713 
714  if (itsStreaming.load() || itsBuffers) { LERROR("Stream is already on -- IGNORED"); return; }
715 
716  // If number of buffers is zero, adjust it depending on frame size:
717  unsigned int nbuf = itsNbufs;
718  if (nbuf == 0)
719  {
720  unsigned int framesize = jevois::v4l2ImageSize(itsFormat.fmt.pix.pixelformat, itsFormat.fmt.pix.width,
721  itsFormat.fmt.pix.height);
722 
723  // Aim for about 4 mbyte when using small images:
724  nbuf = (4U * 1024U * 1024U) / framesize;
725  }
726 
727  // Force number of buffers to a sane value:
728  if (nbuf < 3) nbuf = 3; else if (nbuf > 16) nbuf = 16;
729 
730  // Allocate our buffers for the currently selected resolution, format, etc:
731  itsBuffers = new jevois::VideoBuffers("gadget", itsFd, V4L2_BUF_TYPE_VIDEO_OUTPUT, nbuf);
732  LINFO(itsBuffers->size() << " buffers of " << itsBuffers->get(0)->length() << " bytes allocated");
733 
734  // Fill itsImageQueue with blank frames that can be given off to application code:
735  for (size_t i = 0; i < nbuf; ++i)
736  {
737  jevois::RawImage img;
738  img.width = itsFormat.fmt.pix.width;
739  img.height = itsFormat.fmt.pix.height;
740  img.fmt = itsFormat.fmt.pix.pixelformat;
741  img.fps = itsFps;
742  img.buf = itsBuffers->get(i);
743  img.bufindex = i;
744 
745  // Push the RawImage to outside consumers:
746  itsImageQueue.push_back(img);
747  LDEBUG("Empty image " << img.bufindex << " ready for filling in by application code");
748  }
749 
750  // Start streaming over the USB link:
751  int type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
752  XIOCTL(itsFd, VIDIOC_STREAMON, &type);
753  LDEBUG("Device stream on");
754 
755  itsStreaming.store(true);
756  LDEBUG("Stream is on");
757 }
758 
759 // ##############################################################################################################
761 {
762  JEVOIS_TRACE(2);
763 
764  itsStreaming.store(false);
765 }
766 
767 // ##############################################################################################################
769 {
770  JEVOIS_TRACE(2);
771 
772  // Note: we allow for several streamOff() without complaining, this happens, e.g., when destroying a Gadget that is
773  // not currently streaming.
774 
775  LDEBUG("Turning off gadget stream");
776 
777  // Abort stream in case it was not already done, which will introduce some sleeping in our run() thread, thereby
778  // helping us acquire our needed double lock:
779  abortStream();
780 
781  JEVOIS_TIMED_LOCK(itsMtx);
782 
783  // Stop streaming over the USB link:
784  int type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
785  try { XIOCTL_QUIET(itsFd, VIDIOC_STREAMOFF, &type); } catch (...) { }
786 
787  // Nuke all our buffers:
788  if (itsBuffers) { delete itsBuffers; itsBuffers = nullptr; }
789  itsImageQueue.clear();
790  itsDoneImgs.clear();
791 
792  LDEBUG("Gadget stream is off");
793 }
794 
795 // ##############################################################################################################
797 {
798  JEVOIS_TRACE(4);
799  int retry = 2000;
800 
801  while (--retry >= 0)
802  {
803  if (itsStreaming.load() == false)
804  { LDEBUG("Not streaming"); throw std::runtime_error("Gadget get() rejected while not streaming"); }
805 
806  if (itsMtx.try_lock_for(std::chrono::milliseconds(100)))
807  {
808  if (itsStreaming.load() == false)
809  {
810  LDEBUG("Not streaming");
811  itsMtx.unlock();
812  throw std::runtime_error("Gadget get() rejected while not streaming");
813  }
814 
815  if (itsImageQueue.size())
816  {
817  img = itsImageQueue.front();
818  itsImageQueue.pop_front();
819  itsMtx.unlock();
820  LDEBUG("Empty image " << img.bufindex << " handed over to application code for filling");
821  return;
822  }
823 
824  // No image in the queue, unlock and wait for one:
825  itsMtx.unlock();
826  LDEBUG("Waiting for blank UVC image...");
827  std::this_thread::sleep_for(std::chrono::milliseconds(5));
828  }
829  else
830  {
831  LDEBUG("Waiting for lock");
832  std::this_thread::sleep_for(std::chrono::milliseconds(5));
833  }
834  }
835  LFATAL("Giving up waiting for blank UVC image");
836 }
837 
838 // ##############################################################################################################
840 {
841  JEVOIS_TRACE(4);
842  int retry = 2000;
843 
844  while (--retry >= 0)
845  {
846  if (itsStreaming.load() == false)
847  { LDEBUG("Not streaming"); throw std::runtime_error("Gadget send() rejected while not streaming"); }
848 
849  if (itsMtx.try_lock_for(std::chrono::milliseconds(100)))
850  {
851  if (itsStreaming.load() == false)
852  {
853  LDEBUG("Not streaming");
854  itsMtx.unlock();
855  throw std::runtime_error("Gadget send() rejected while not streaming");
856  }
857 
858  // Check that the format matches, this may not be the case if we changed format while the buffer was out for
859  // processing. IF so, we just drop this image since it cannot be sent to the host anymore:
860  if (img.width != itsFormat.fmt.pix.width ||
861  img.height != itsFormat.fmt.pix.height ||
862  img.fmt != itsFormat.fmt.pix.pixelformat)
863  {
864  LDEBUG("Dropping image to send out as format just changed");
865  itsMtx.unlock();
866  return;
867  }
868 
869  // We cannot just qbuf() here as our run() thread is likely in select() and the driver will bomb the qbuf as
870  // resource unavailable. So we just enqueue the buffer index and the run() thread will handle the qbuf later:
871  itsDoneImgs.push_back(img.bufindex);
872  itsMtx.unlock();
873  LDEBUG("Filled image " << img.bufindex << " received from application code");
874  return;
875  }
876  else
877  {
878  LDEBUG("Waiting for lock");
879  std::this_thread::sleep_for(std::chrono::milliseconds(5));
880  }
881  }
882  LFATAL("Giving up waiting for lock");
883 }
884 
#define LDEBUG(msg)
Convenience macro for users to print out console or syslog messages, DEBUG level. ...
Definition: Log.H:155
#define UVC_EVENT_DISCONNECT
Definition: uvc.h:27
std::string warnAndIgnoreException()
Convenience function to catch an exception, issue some LERROR (depending on type), and ignore it.
Definition: Log.C:211
void abortStream() override
Abort streaming.
Definition: Gadget.C:760
float ofps
output frame rate in frames/sec
Definition: VideoMapping.H:46
#define UVC_INTF_CONTROL
Definition: uvc.h:53
size_t getVideoMappingIdx(unsigned int iformat, unsigned int iframe, unsigned int interval) const
Get the video mapping index for a given UVC iformat, iframe and interval.
Definition: Engine.C:876
unsigned int uvcformat
USB-UVC format number (1-based)
Definition: VideoMapping.H:53
#define UVC_EVENT_STREAMOFF
Definition: uvc.h:29
void dqbuf(struct v4l2_buffer &buf)
Dequeue the next captured/displayed buffer, blocks until one is available.
Definition: VideoBuffers.C:133
unsigned int height
Image height in pixels.
Definition: RawImage.H:146
static struct v4l2_fract fpsToV4l2(float fps)
Convert from fps to V4L2 interval.
Definition: VideoMapping.C:65
__u8 data[UVC_MAX_REQUEST_SIZE - sizeof(__s32)]
Definition: uvc.h:39
void streamOff()
Stop streaming on video from camera, processing, and USB.
Definition: Engine.C:546
virtual void setControl(struct v4l2_control const &ctrl)=0
Set a control, throw if the hardware rejects the value.
void send(RawImage const &img) override
Send an image out over USB to the host computer.
Definition: Gadget.C:839
virtual ~Gadget()
Close the device and free all resources.
Definition: Gadget.C:180
void streamOn() override
Start streaming.
Definition: Gadget.C:706
void streamOn()
Start streaming on video from camera, processing, and USB.
Definition: Engine.C:535
unsigned int ofmt
output pixel format, or 0 for no output over USB
Definition: VideoMapping.H:43
unsigned int fmt
Pixel format as a V4L2_PIX_FMT_XXX.
Definition: RawImage.H:147
#define UVC_EVENT_DATA
Definition: uvc.h:31
#define XIOCTL_QUIET(dev, req, mem)
Helper macro to execute an ioctl, ignore interruptions, and, if error throw quietly.
std::shared_ptr< VideoBuf > buf
The pixel data buffer.
Definition: RawImage.H:149
#define JEVOIS_TIMED_LOCK(mtx)
Helper macro to create a timed_lock_guard object.
Definition: Log.H:299
#define success()
Gadget(std::string const &devname, VideoInput *camera, Engine *engine, size_t const nbufs=0)
Construct and open the device.
Definition: Gadget.C:134
#define wordresponse(val)
struct usb_ctrlrequest req
Definition: uvc.h:46
#define JEVOIS_TRACE(level)
Trace object.
Definition: Log.H:267
#define PLFATAL(msg)
Like LDEBUG but appends errno and strerror(errno), to be used when some system call fails...
Definition: Log.H:221
#define UVCIOC_SEND_RESPONSE
Definition: uvc.h:51
void get(RawImage &img) override
Get a pre-allocated image so that we can fill the pixel data and later send out over USB using send()...
Definition: Gadget.C:796
std::string str() const
Convenience function to print out the whole mapping in a human-friendly way.
Definition: VideoMapping.C:87
#define LERROR(msg)
Convenience macro for users to print out console or syslog messages, ERROR level. ...
Definition: Log.H:193
A raw image as coming from a V4L2 Camera and/or being sent out to a USB Gadget.
Definition: RawImage.H:110
struct uvc_request_data data
Definition: uvc.h:47
Collection of buffers for V4L2 video frames (Camera or Gadget) with hooks to the MMAP&#39;d areas...
Definition: VideoBuffers.H:41
VideoMapping const & getVideoMapping(size_t idx) const
Allow access to our video mappings which are parsed from file at construction.
Definition: Engine.C:867
size_t size() const
Get the number of buffers allocated.
Definition: VideoBuffers.C:80
Simple struct to hold video mapping definitions for the processing Engine.
Definition: VideoMapping.H:41
#define UVC_INTF_STREAMING
Definition: uvc.h:54
#define XIOCTL(dev, req, mem)
Helper macro to execute an ioctl, ignore interruptions, and, if error, issue a fatal message and thro...
std::shared_ptr< VideoBuf > get(size_t const index) const
Get one buffer, by index [0 .. size()[.
Definition: VideoBuffers.C:92
__s32 length
Definition: uvc.h:38
unsigned int uvcframe
USB UVC frame number (1-based)
Definition: VideoMapping.H:54
unsigned int oh
output height
Definition: VideoMapping.H:45
virtual void queryControl(struct v4l2_queryctrl &qc) const =0
Get information about a control, throw if unsupported by hardware.
Base class for video input, which will get derived into Camera and MovieInput.
Definition: VideoInput.H:31
#define byteresponse(val)
#define LFATAL(msg)
Convenience macro for users to print out console or syslog messages, FATAL level. ...
Definition: Log.H:212
size_t getDefaultVideoMappingIdx() const
Allow access to the default video mapping index.
Definition: Engine.C:911
unsigned int v4l2ImageSize(unsigned int fcc, unsigned int width, unsigned int height)
Return the image size in bytes for a given V4L2_PIX_FMT_..., width, height.
Definition: Utils.C:79
void qbuf(size_t const index)
Queue one buffer to V4L2, by index [0 .. size()[.
Definition: VideoBuffers.C:100
#define failure(code)
JeVois processing engine - gets images from camera sensor, processes them, and sends results over USB...
Definition: Engine.H:229
unsigned int osize() const
Return the size in bytes of an output image.
Definition: VideoMapping.C:37
VideoMapping const & getDefaultVideoMapping() const
Allow access to the default video mapping.
Definition: Engine.C:907
Definition: uvc.h:42
float fps
Programmed frames/s as given by current video mapping, may not be actual.
Definition: RawImage.H:148
static unsigned int fpsToUvc(float fps)
Convert from fps to USB/UVC interval.
Definition: VideoMapping.C:52
#define PLERROR(msg)
Like LERROR but appends errno and strerror(errno), to be used when some system call fails...
Definition: Log.H:201
void setFormat(size_t idx)
Callback for when the user selects a new output video format.
Definition: Engine.C:567
void setFormat(jevois::VideoMapping const &m) override
Set the video format and frame rate.
Definition: Gadget.C:196
#define LINFO(msg)
Convenience macro for users to print out console or syslog messages, INFO level.
Definition: Log.H:176
size_t bufindex
The index of the data buffer in the kernel driver.
Definition: RawImage.H:150
unsigned int ow
output width
Definition: VideoMapping.H:44
unsigned int width
Image width in pixels.
Definition: RawImage.H:145
void streamOff() override
Stop streaming.
Definition: Gadget.C:768
#define intresponse(val)
#define UVC_EVENT_STREAMON
Definition: uvc.h:28
#define UVC_EVENT_CONNECT
Definition: uvc.h:26
virtual void getControl(struct v4l2_control &ctrl) const =0
Get a control&#39;s current value, throw if unsupported by hardware.
#define UVC_EVENT_SETUP
Definition: uvc.h:30