JeVois  1.0
JeVois Smart Embedded Machine Vision Toolkit
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  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 
212  // First try to set our own format, will throw if phony:
213  XIOCTL(itsFd, VIDIOC_S_FMT, &itsFormat);
214 
215  // Note that the format does not include fps, this is done with VIDIOC_S_PARM:
216  try
217  {
218  // The gadget driver may not support this ioctl...
219  struct v4l2_streamparm sparm = { };
220  sparm.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
221  sparm.parm.output.outputmode = 2; // V4L2_MODE_VIDEO not defined in our headers? its value is 2.
222  sparm.parm.output.timeperframe = jevois::VideoMapping::fpsToV4l2(m.ofps);
223  XIOCTL_QUIET(itsFd, VIDIOC_S_PARM, &sparm);
224  } catch (...) { }
225 }
226 
227 // ##############################################################################################################
228 void jevois::Gadget::run()
229 {
230  JEVOIS_TRACE(1);
231 
232  fd_set wfds; // For UVC video streaming
233  fd_set efds; // For UVC events
234  struct timeval tv;
235 
236  // Switch to running state:
237  itsRunning.store(true);
238 
239  // We may have to wait until the device is opened:
240  while (itsFd == -1) std::this_thread::sleep_for(std::chrono::milliseconds(1));
241 
242  // Wait for event from the gadget kernel driver and process them:
243  while (itsRunning.load())
244  {
245  // Wait until we either receive an event or we are ready to send the next buffer over:
246  FD_ZERO(&wfds); FD_ZERO(&efds); FD_SET(itsFd, &wfds); FD_SET(itsFd, &efds);
247  tv.tv_sec = 0; tv.tv_usec = 10000;
248 
249  int ret = select(itsFd + 1, nullptr, &wfds, &efds, &tv);
250 
251  if (ret == -1) { PLERROR("Select error"); if (errno == EINTR) continue; else break; }
252  else if (ret > 0) // We have some events, handle them right away:
253  {
254  // Note: we may have more than one event, so here we try processEvents() several times to be sure:
255  if (FD_ISSET(itsFd, &efds))
256  {
257  // First event, we will report error if any:
258  try { processEvents(); } catch (...) { jevois::warnAndIgnoreException(); }
259 
260  // Let's try to dequeue one more, in most cases it should throw:
261  while (true) try { processEvents(); } catch (...) { break; }
262  }
263 
264  if (FD_ISSET(itsFd, &wfds)) try { processVideo(); } catch (...) { jevois::warnAndIgnoreException(); }
265  }
266 
267  // We timed out
268 
269  // Sometimes we miss events in the main loop, likely because more events come while we are unlocked in the USB UDC
270  // driver and processing here. So let's try to dequeue one more, in most cases it should throw:
271  while (true) try { processEvents(); } catch (...) { break; }
272 
273  // While the driver is not busy in select(), queue at most one buffer that is ready to send off:
274  try
275  {
276  JEVOIS_TIMED_LOCK(itsMtx);
277  if (itsDoneImgs.size())
278  {
279  LDEBUG("Queuing image " << itsDoneImgs.front() << " for sending over USB");
280 
281  // We need to prepare a legit v4l2_buffer, including bytesused:
282  struct v4l2_buffer buf = { };
283 
284  buf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
285  buf.memory = V4L2_MEMORY_MMAP;
286  buf.index = itsDoneImgs.front();
287  buf.length = itsBuffers->get(buf.index)->length();
288 
289  if (itsFormat.fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG)
290  buf.bytesused = itsBuffers->get(buf.index)->bytesUsed();
291  else
292  buf.bytesused = buf.length;
293 
294  buf.field = V4L2_FIELD_NONE;
295  buf.flags = 0;
296  gettimeofday(&buf.timestamp, nullptr);
297 
298  // Queue it up so it can be sent to the host:
299  itsBuffers->qbuf(buf);
300 
301  // This one is done:
302  itsDoneImgs.pop_front();
303  }
304  } catch (...) { jevois::warnAndIgnoreException(); std::this_thread::sleep_for(std::chrono::milliseconds(10)); }
305  }
306 
307  // Switch out of running state in case we did interrupt the loop here by a break statement:
308  itsRunning.store(false);
309 }
310 
311 // ##############################################################################################################
312 void jevois::Gadget::processEvents()
313 {
314  JEVOIS_TRACE(3);
315 
316  // Get the event from the driver:
317  struct v4l2_event v4l2ev = { };
318  XIOCTL_QUIET(itsFd, VIDIOC_DQEVENT, &v4l2ev);
319  struct uvc_event * uvcev = reinterpret_cast<struct uvc_event *>(&v4l2ev.u.data);
320 
321  // Prepare our response, if any will be sent:
322  struct uvc_request_data resp = { };
323  resp.length = -EL2HLT;
324 
325  // Act according to the event type:
326  try
327  {
328  switch (v4l2ev.type)
329  {
330  case UVC_EVENT_CONNECT: return;
331  case UVC_EVENT_DISCONNECT: LDEBUG("EVENT DISCONNECT"); itsEngine->streamOff(); return;
332  case UVC_EVENT_SETUP: LDEBUG("EVENT SETUP"); processEventSetup(uvcev->req, resp); return;
333  case UVC_EVENT_DATA: LDEBUG("EVENT DATA"); processEventData(uvcev->data); return;
334  case UVC_EVENT_STREAMON: LDEBUG("EVENT STREAMON"); itsEngine->streamOn(); return;
335  case UVC_EVENT_STREAMOFF: LDEBUG("EVENT STREAMOFF"); itsEngine->streamOff(); return;
336  }
337  } catch (...) { }
338 }
339 
340 // ##############################################################################################################
341 void jevois::Gadget::processVideo()
342 {
343  JEVOIS_TRACE(3);
344 
345  jevois::RawImage img;
346  JEVOIS_TIMED_LOCK(itsMtx);
347 
348  // If we are not streaming anymore, abort:
349  if (itsStreaming.load() == false) LFATAL("Aborted while not streaming");
350 
351  // Dequeue a buffer from the gadget driver, this is an image that has been sent to the host and hence the buffer is
352  // now available to be filled up with image data and later queued again to the gadget driver:
353  struct v4l2_buffer buf;
354  itsBuffers->dqbuf(buf);
355 
356  // Create a RawImage from that buffer:
357  img.width = itsFormat.fmt.pix.width;
358  img.height = itsFormat.fmt.pix.height;
359  img.fmt = itsFormat.fmt.pix.pixelformat;
360  img.buf = itsBuffers->get(buf.index);
361  img.bufindex = buf.index;
362 
363  // Push the RawImage to outside consumers:
364  itsImageQueue.push_back(img);
365  LDEBUG("Empty image " << img.bufindex << " ready for filling in by application code");
366 }
367 
368 // ##############################################################################################################
369 void jevois::Gadget::processEventSetup(struct usb_ctrlrequest const & ctrl, struct uvc_request_data & resp)
370 {
371  JEVOIS_TRACE(3);
372 
373  itsControl = 0; itsEntity = 0;
374 
375  debugCtrlReq(ctrl);
376 
377  switch (ctrl.bRequestType & USB_TYPE_MASK)
378  {
379  case USB_TYPE_STANDARD: processEventStandard(ctrl, resp); break;
380  case USB_TYPE_CLASS: processEventClass(ctrl, resp); break;
381  default: LERROR("Unsupported setup event type " << std::showbase << std::hex <<
382  (ctrl.bRequestType & USB_TYPE_MASK) << " -- IGNORED");
383  }
384  if (ctrl.bRequestType != 0x21) XIOCTL(itsFd, UVCIOC_SEND_RESPONSE, &resp);
385 }
386 
387 // ##############################################################################################################
388 void jevois::Gadget::processEventStandard(struct usb_ctrlrequest const & ctrl,
389  struct uvc_request_data & JEVOIS_UNUSED_PARAM(resp))
390 {
391  JEVOIS_TRACE(3);
392 
393  LDEBUG("UVC standard setup event ignored:");
394  debugCtrlReq(ctrl);
395 }
396 
397 // ##############################################################################################################
398 void jevois::Gadget::processEventClass(struct usb_ctrlrequest const & ctrl, struct uvc_request_data & resp)
399 {
400  JEVOIS_TRACE(3);
401 
402  if ((ctrl.bRequestType & USB_RECIP_MASK) != USB_RECIP_INTERFACE) return;
403 
404  switch (ctrl.wIndex & 0xff)
405  {
406  case UVC_INTF_CONTROL:
407  processEventControl(ctrl.bRequest, ctrl.wValue >> 8, ctrl.wIndex >> 8, ctrl.wLength, resp);
408  break;
409 
410  case UVC_INTF_STREAMING:
411  processEventStreaming(ctrl.bRequest, ctrl.wValue >> 8, resp);
412  break;
413 
414  default:
415  LERROR("Unsupported setup event class " << std::showbase << std::hex << (ctrl.wIndex & 0xff) << " -- IGNORED");
416  }
417 }
418 
419 // ##############################################################################################################
420 void jevois::Gadget::processEventControl(uint8_t req, uint8_t cs, uint8_t entity_id, uint8_t len,
421  struct uvc_request_data & resp)
422 {
423  JEVOIS_TRACE(3);
424 
425  // Local function we run on successful processing of an event: we just reset our internal error code
426 #define success() { itsErrorCode = 0; }
427 
428  // Local function we run on failed processing of an event: stall the request, set our internal error code
429 #define failure(code) { resp.length = -EL2HLT; itsErrorCode = code; }
430 
431  // Shortcurt to successfully send a 1-byte response:
432 #define byteresponse(val) { resp.data[0] = val; resp.length = 1; itsErrorCode = 0; }
433 
434  // Shortcurt to successfully send a 2-byte response:
435 #define wordresponse(val) { resp.data[0] = val & 0xff; resp.data[1] = (val >> 8) & 0xff; \
436  resp.length = 2; itsErrorCode = 0; }
437 
438  // Shortcurt to successfully send a 4-byte response:
439 #define intresponse(val) { resp.data[0] = val & 0xff; resp.data[1] = (val >> 8) & 0xff; \
440  resp.data[2] = (val >> 16) & 0xff; resp.data[3] = (val >> 24) & 0xff; \
441  resp.length = 4; itsErrorCode = 0; }
442 
443  // If anything throws here, we will return failure:
444  try
445  {
446  // First handle any request that is directed to entity 0:
447  if (entity_id == 0)
448  {
449  switch (cs)
450  {
451  case UVC_VC_REQUEST_ERROR_CODE_CONTROL: byteresponse(itsErrorCode); return; // Send error code last prepared
452  default: failure(0x06); return;
453  }
454  }
455 
456  // Process according to the entity that this event is directed to and the control that is requested:
457  if (req == UVC_SET_CUR)
458  {
459  // We need to wait for the data phase, so for now just remember the control and return success:
460  itsEntity = entity_id; itsControl = cs;
461  resp.data[0] = 0x0; resp.length = len; success();
462  LDEBUG("SET_CUR ent " << itsEntity <<" ctrl "<< itsControl <<" len "<< len);
463  }
464  else if (req == UVC_GET_INFO)
465  {
466  // FIXME: controls could also be disabled, autoupdate, or asynchronous:
467  byteresponse(UVC_CONTROL_CAP_GET | UVC_CONTROL_CAP_SET);
468  }
469  else if (req == UVC_GET_CUR)
470  {
471  // Fetch the current value from the camera. Note: both Windows and Android insist on querying some controls, like
472  // IRIS and GAMMA, which we did not declare as supported in the kernel driver. We need to handle those requests
473  // and to send some phony data:
474  struct v4l2_control ctrl = { };
475  try { ctrl.id = uvcToV4Lcontrol(entity_id, cs); itsCamera->getControl(ctrl); } catch (...) { ctrl.id = 0; }
476 
477  // We need a special handling of white balance here:
478  if (ctrl.id == V4L2_CID_RED_BALANCE)
479  {
480  unsigned int redval = (ctrl.value & 0xffff) << 16; // red is at offset 2 in PU_WHITE_BALANCE_COMPONENT_CONTROL
481 
482  // Also get the blue balance value:
483  ctrl.id = V4L2_CID_BLUE_BALANCE;
484  itsCamera->getControl(ctrl);
485 
486  // Combine both red and blue values:
487  ctrl.value = (ctrl.value & 0xffff) | redval;
488  }
489  // We also need to remap auto exposure values:
490  else if (ctrl.id == V4L2_CID_EXPOSURE_AUTO)
491  {
492  if (ctrl.value == V4L2_EXPOSURE_MANUAL) ctrl.value = 0x01; // manual mode, set UVC bit D0
493  else if (ctrl.value == V4L2_EXPOSURE_AUTO) ctrl.value = 0x02; // auto mode, set UVC bit D1
494  else ctrl.value = 0x03;
495  // Note, there are 2 more bits under CT_AE_MODE_CONTROL
496  }
497  // Handle the unknown controls:
498  else if (ctrl.id == 0) ctrl.value = 0;
499 
500  switch (len)
501  {
502  case 1: byteresponse(ctrl.value); break;
503  case 2: wordresponse(ctrl.value); break;
504  case 4: intresponse(ctrl.value); break;
505  default: LFATAL("Unsupported control with length " << len);
506  }
507  }
508  else
509  {
510  // It's a GET_DEF/RES/MIN/MAX let's first get the data from the camera: Note: both Windows and Android insist on
511  // querying some controls, like IRIS and GAMMA, which we did not declare as supported in the kernel driver. We
512  // need to handle those requests and to send some phony data:
513  struct v4l2_queryctrl qc = { };
514  try { qc.id = uvcToV4Lcontrol(entity_id, cs); itsCamera->queryControl(qc); } catch (...) { qc.id = 0; }
515 
516  // We need a special handling of white balance here:
517  if (qc.id == V4L2_CID_RED_BALANCE)
518  {
519  // Also get the blue balance values:
520  struct v4l2_queryctrl qc2 = { };
521  qc2.id = V4L2_CID_BLUE_BALANCE;
522  itsCamera->queryControl(qc2);
523 
524  // Combine red and blue values into qc:
525  qc.default_value = (qc.default_value << 16) | qc2.default_value;
526  qc.step = (qc.step << 16) | qc2.step;
527  qc.minimum = (qc.minimum << 16) | qc2.minimum;
528  qc.maximum = (qc.maximum << 16) | qc2.maximum;
529  }
530  // We also need to remap auto exposure values:
531  else if (qc.id == V4L2_CID_EXPOSURE_AUTO)
532  {
533  // Tricky: in the 'step' field, we are supposed to provide a bitmap of the modes that are supported, see UVC
534  // specs. D0=manual, D1=auto, D2=shutter priority, D3=aperture priority. Min and max are ignored for this
535  // control, default is handled.
536  qc.minimum = 0; qc.step = 3; qc.maximum = 3; qc.default_value = 1;
537  }
538  // Also handle the unknown controls here:
539  else if (qc.id == 0)
540  { qc.minimum = 0; qc.step = 1; qc.maximum = 1; qc.default_value = 0; }
541 
542  int val = 0;
543  switch (req)
544  {
545  case UVC_GET_DEF: val = qc.default_value; break;
546  case UVC_GET_RES: val = qc.step; break;
547  case UVC_GET_MIN: val = qc.minimum; break;
548  case UVC_GET_MAX: val = qc.maximum; break;
549  default: failure(0x07); return;
550  }
551 
552  switch (len)
553  {
554  case 1: byteresponse(val); break;
555  case 2: wordresponse(val); break;
556  case 4: intresponse(val); break;
557  default: LFATAL("Unsupported control with length " << len);
558  }
559  }
560  }
561  catch (...)
562  {
563  LERROR("FAILED entity " << entity_id << " cs " << cs << " len " << len);
564  failure(0x06);
565  }
566 }
567 
568 // ##############################################################################################################
569 void jevois::Gadget::fillStreamingControl(struct uvc_streaming_control * ctrl, jevois::VideoMapping const & m)
570 {
571  JEVOIS_TRACE(3);
572 
573  memset(ctrl, 0, sizeof(struct uvc_streaming_control));
574 
575  ctrl->bFormatIndex = m.uvcformat;
576  ctrl->bFrameIndex = m.uvcframe;
577  ctrl->dwFrameInterval = jevois::VideoMapping::fpsToUvc(m.ofps);
578  ctrl->dwMaxVideoFrameSize = m.osize();
579  ctrl->dwMaxPayloadTransferSize = 3072;
580  ctrl->bmFramingInfo = 3;
581  ctrl->bPreferedVersion = 1;
582  ctrl->bMaxVersion = 1;
583 }
584 
585 // ##############################################################################################################
586 void jevois::Gadget::processEventStreaming(uint8_t req, uint8_t cs, struct uvc_request_data & resp)
587 {
588  JEVOIS_TRACE(3);
589 
590  int const datalen = 26; // uvc 1.0 as reported by our kernel driver
591  if (cs != UVC_VS_PROBE_CONTROL && cs != UVC_VS_COMMIT_CONTROL) return;
592 
593  struct uvc_streaming_control * ctrl = reinterpret_cast<struct uvc_streaming_control *>(&resp.data);
594  struct uvc_streaming_control * target = (cs == UVC_VS_PROBE_CONTROL) ? &itsProbe : &itsCommit;
595  resp.length = datalen;
596 
597  switch (req)
598  {
599  case UVC_SET_CUR: itsControl = cs; resp.length = datalen; break; // will finish up in data stage
600 
601  case UVC_GET_CUR:
602  case UVC_GET_MIN: // we have nothing to negotiate
603  case UVC_GET_MAX: // we have nothing to negotiate
604  memcpy(ctrl, target, datalen);
605  break;
606 
607  case UVC_GET_DEF:
608  {
609  // If requested format index, frame index, or interval is bogus (including zero), initialize to our default mapping,
610  // otherwise pass down the selected mapping:
611  size_t idx = itsEngine->getDefaultVideoMappingIdx();
612  try { idx = itsEngine->getVideoMappingIdx(ctrl->bFormatIndex, ctrl->bFrameIndex, ctrl->dwFrameInterval); }
613  catch (...) { }
614  fillStreamingControl(target, itsEngine->getVideoMapping(idx));
615  memcpy(ctrl, target, datalen);
616  }
617  break;
618 
619  case UVC_GET_RES: memset(ctrl, 0, datalen); break;
620 
621  case UVC_GET_LEN: resp.data[0] = 0x00; resp.data[1] = datalen; resp.length = 2; break;
622 
623  case UVC_GET_INFO: resp.data[0] = 0x03; resp.length = 1; break;
624  }
625 }
626 
627 // ##############################################################################################################
628 void jevois::Gadget::processEventData(struct uvc_request_data & data)
629 {
630  JEVOIS_TRACE(3);
631 
632  struct uvc_streaming_control * target;
633 
634  // If entity is 1 or 2, this is to set a control:
635  if (itsEntity == 2 || itsEntity == 1) { processEventControlData(data); return; }
636 
637  switch (itsControl)
638  {
639  case UVC_VS_PROBE_CONTROL: target = &itsProbe; break;
640  case UVC_VS_COMMIT_CONTROL: target = &itsCommit; break;
641  default: processEventControlData(data); return;
642  }
643 
644  // Find the selected format and frame info and fill-in the control data:
645  struct uvc_streaming_control * ctrl = reinterpret_cast<struct uvc_streaming_control *>(&data.data);
646 
647  size_t idx = itsEngine->getVideoMappingIdx(ctrl->bFormatIndex, ctrl->bFrameIndex, ctrl->dwFrameInterval);
648 
649  fillStreamingControl(target, itsEngine->getVideoMapping(idx));
650 
651  LDEBUG("Host requested " << ctrl->bFormatIndex << '/' << ctrl->bFrameIndex << '/' << ctrl->dwFrameInterval <<
652  ", " << ((itsControl == UVC_VS_COMMIT_CONTROL) ? "setting " : "returning ") <<
653  itsEngine->getVideoMapping(idx).str());
654 
655  // Set the format if we are doing a commit control:
656  if (itsControl == UVC_VS_COMMIT_CONTROL) itsEngine->setFormat(idx);
657 }
658 
659 // ##############################################################################################################
660 void jevois::Gadget::processEventControlData(struct uvc_request_data & data)
661 {
662  JEVOIS_TRACE(3);
663 
664  struct v4l2_control ctrl;
665 
666  // Get the control ID for V4L or throw if unsupported:
667  ctrl.id = uvcToV4Lcontrol(itsEntity, itsControl);
668 
669  // Copy the data we received into the control's value:
670  switch (data.length)
671  {
672  case 1: ctrl.value = static_cast<int>(data.data[0]); break;
673  case 2: ctrl.value = static_cast<int>(__s16(data.data[0] | (static_cast<short>(data.data[1]) << 8))); break;
674  case 4: ctrl.value = data.data[0] | (data.data[1] << 8) | (data.data[2] << 16) | (data.data[3] << 24); break;
675  default: LFATAL("Ooops data len is " << data.length);
676  }
677 
678  // Tell the camera to set the control. We do not have enough time here to do it as otherwise our USB transaction will
679  // time out while we transfer a bunch of bytes to the camera over the 400kHz serial control link, so we just push it
680  // to a queue and our run() thread will do the work. First, handle special cases:
681  switch (ctrl.id)
682  {
683  case V4L2_CID_RED_BALANCE:
684  {
685  // We need to set both the red and the blue:
686  int blue = ctrl.value & 0xffff;
687  ctrl.value >>= 16; itsCamera->setControl(ctrl);
688  ctrl.id = V4L2_CID_BLUE_BALANCE; ctrl.value = blue; itsCamera->setControl(ctrl);
689  }
690  break;
691 
692  case V4L2_CID_EXPOSURE_AUTO:
693  if (ctrl.value & 0x01) ctrl.value = V4L2_EXPOSURE_MANUAL; // UVC bit D0 set for manual mode
694  else if (ctrl.value & 0x02) ctrl.value = V4L2_EXPOSURE_AUTO; // auto mode
695  // Note, there are 2 more bits under CT_AE_MODE_CONTROL
696  itsCamera->setControl(ctrl);
697  break;
698 
699  default: itsCamera->setControl(ctrl);
700  }
701 }
702 
703 // ##############################################################################################################
705 {
706  JEVOIS_TRACE(2);
707 
708  LDEBUG("Turning on UVC stream");
709 
710  JEVOIS_TIMED_LOCK(itsMtx);
711 
712  if (itsStreaming.load() || itsBuffers) { LERROR("Stream is already on -- IGNORED"); return; }
713 
714  // If number of buffers is zero, adjust it depending on frame size:
715  unsigned int nbuf = itsNbufs;
716  if (nbuf == 0)
717  {
718  unsigned int framesize = jevois::v4l2ImageSize(itsFormat.fmt.pix.pixelformat, itsFormat.fmt.pix.width,
719  itsFormat.fmt.pix.height);
720 
721  // Aim for about 4 mbyte when using small images:
722  nbuf = (4U * 1024U * 1024U) / framesize;
723  }
724 
725  // Force number of buffers to a sane value:
726  if (nbuf < 3) nbuf = 3; else if (nbuf > 16) nbuf = 16;
727 
728  // Allocate our buffers for the currently selected resolution, format, etc:
729  itsBuffers = new jevois::VideoBuffers("gadget", itsFd, V4L2_BUF_TYPE_VIDEO_OUTPUT, nbuf);
730  LINFO(itsBuffers->size() << " buffers of " << itsBuffers->get(0)->length() << " bytes allocated");
731 
732  // Fill itsImageQueue with blank frames that can be given off to application code:
733  for (size_t i = 0; i < nbuf; ++i)
734  {
735  jevois::RawImage img;
736  img.width = itsFormat.fmt.pix.width;
737  img.height = itsFormat.fmt.pix.height;
738  img.fmt = itsFormat.fmt.pix.pixelformat;
739  img.buf = itsBuffers->get(i);
740  img.bufindex = i;
741 
742  // Push the RawImage to outside consumers:
743  itsImageQueue.push_back(img);
744  LDEBUG("Empty image " << img.bufindex << " ready for filling in by application code");
745  }
746 
747  // Start streaming over the USB link:
748  int type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
749  XIOCTL(itsFd, VIDIOC_STREAMON, &type);
750  LDEBUG("Device stream on");
751 
752  itsStreaming.store(true);
753  LDEBUG("Stream is on");
754 }
755 
756 // ##############################################################################################################
758 {
759  JEVOIS_TRACE(2);
760 
761  itsStreaming.store(false);
762 }
763 
764 // ##############################################################################################################
766 {
767  JEVOIS_TRACE(2);
768 
769  // Note: we allow for several streamOff() without complaining, this happens, e.g., when destroying a Gadget that is
770  // not currently streaming.
771 
772  LDEBUG("Turning off gadget stream");
773 
774  // Abort stream in case it was not already done, which will introduce some sleeping in our run() thread, thereby
775  // helping us acquire our needed double lock:
776  abortStream();
777 
778  JEVOIS_TIMED_LOCK(itsMtx);
779 
780  // Stop streaming over the USB link:
781  int type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
782  try { XIOCTL_QUIET(itsFd, VIDIOC_STREAMOFF, &type); } catch (...) { }
783 
784  // Nuke all our buffers:
785  if (itsBuffers) { delete itsBuffers; itsBuffers = nullptr; }
786  itsImageQueue.clear();
787  itsDoneImgs.clear();
788 
789  LDEBUG("Gadget stream is off");
790 }
791 
792 // ##############################################################################################################
794 {
795  JEVOIS_TRACE(4);
796  int retry = 2000;
797 
798  while (--retry >= 0)
799  {
800  if (itsStreaming.load() == false)
801  { LDEBUG("Not streaming"); throw std::runtime_error("Gadget get() rejected while not streaming"); }
802 
803  if (itsMtx.try_lock_for(std::chrono::milliseconds(100)))
804  {
805  if (itsStreaming.load() == false)
806  {
807  LDEBUG("Not streaming");
808  itsMtx.unlock();
809  throw std::runtime_error("Gadget get() rejected while not streaming");
810  }
811 
812  if (itsImageQueue.size())
813  {
814  img = itsImageQueue.front();
815  itsImageQueue.pop_front();
816  itsMtx.unlock();
817  LDEBUG("Empty image " << img.bufindex << " handed over to application code for filling");
818  return;
819  }
820 
821  // No image in the queue, unlock and wait for one:
822  itsMtx.unlock();
823  LDEBUG("Waiting for blank UVC image...");
824  std::this_thread::sleep_for(std::chrono::milliseconds(5));
825  }
826  else
827  {
828  LDEBUG("Waiting for lock");
829  std::this_thread::sleep_for(std::chrono::milliseconds(5));
830  }
831  }
832  LFATAL("Giving up waiting for blank UVC image");
833 }
834 
835 // ##############################################################################################################
837 {
838  JEVOIS_TRACE(4);
839  int retry = 2000;
840 
841  while (--retry >= 0)
842  {
843  if (itsStreaming.load() == false)
844  { LDEBUG("Not streaming"); throw std::runtime_error("Gadget send() rejected while not streaming"); }
845 
846  if (itsMtx.try_lock_for(std::chrono::milliseconds(100)))
847  {
848  if (itsStreaming.load() == false)
849  {
850  LDEBUG("Not streaming");
851  itsMtx.unlock();
852  throw std::runtime_error("Gadget send() rejected while not streaming");
853  }
854 
855  // Check that the format matches, this may not be the case if we changed format while the buffer was out for
856  // processing. IF so, we just drop this image since it cannot be sent to the host anymore:
857  if (img.width != itsFormat.fmt.pix.width ||
858  img.height != itsFormat.fmt.pix.height ||
859  img.fmt != itsFormat.fmt.pix.pixelformat)
860  {
861  LDEBUG("Dropping image to send out as format just changed");
862  itsMtx.unlock();
863  return;
864  }
865 
866  // We cannot just qbuf() here as our run() thread is likely in select() and the driver will bomb the qbuf as
867  // resource unavailable. So we just enqueue the buffer index and the run() thread will handle the qbuf later:
868  itsDoneImgs.push_back(img.bufindex);
869  itsMtx.unlock();
870  LDEBUG("Filled image " << img.bufindex << " received from application code");
871  return;
872  }
873  else
874  {
875  LDEBUG("Waiting for lock");
876  std::this_thread::sleep_for(std::chrono::milliseconds(5));
877  }
878  }
879  LFATAL("Giving up waiting for lock");
880 }
881 
#define LDEBUG(msg)
Convenience macro for users to print out console or syslog messages, DEBUG level. ...
Definition: Log.H:148
#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:200
void abortStream() override
Abort streaming.
Definition: Gadget.C:757
float ofps
output frame rate in frames/sec
Definition: VideoMapping.H:46
#define UVC_INTF_CONTROL
Definition: uvc.h:53
unsigned int uvcformat
USB-UVC format number (1-based)
Definition: VideoMapping.H:53
unsigned int osize() const
Return the size in bytes of an output image.
Definition: VideoMapping.C:51
#define UVC_EVENT_STREAMOFF
Definition: uvc.h:29
unsigned int height
Image height in pixels.
Definition: RawImage.H:140
static struct v4l2_fract fpsToV4l2(float fps)
Convert from fps to V4L2 interval.
Definition: VideoMapping.C:79
void send(RawImage const &img) override
Send an image out over USB to the host computer.
Definition: Gadget.C:836
virtual ~Gadget()
Close the device and free all resources.
Definition: Gadget.C:180
void streamOn() override
Start streaming.
Definition: Gadget.C:704
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:141
#define UVC_EVENT_DATA
Definition: uvc.h:31
__u8 data[UVC_MAX_REQUEST_SIZE-sizeof(__s32)]
Definition: uvc.h:39
#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:142
#define JEVOIS_TIMED_LOCK(mtx)
Helper macro to create a timed_lock_guard object.
Definition: Log.H:292
#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:260
#define PLFATAL(msg)
Like LDEBUG but appends errno and strerror(errno), to be used when some system call fails...
Definition: Log.H:214
#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:793
#define LERROR(msg)
Convenience macro for users to print out console or syslog messages, ERROR level. ...
Definition: Log.H:186
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
VideoMapping const & getDefaultVideoMapping() const
Allow access to the default video mapping.
Definition: Engine.C:725
Collection of buffers for V4L2 video frames (Camera or Gadget) with hooks to the MMAP'd areas...
Definition: VideoBuffers.H:41
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...
__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
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:205
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:61
#define failure(code)
JeVois processing engine - gets images from camera sensor, processes them, and sends results over USB...
Definition: Engine.H:223
Definition: uvc.h:42
static unsigned int fpsToUvc(float fps)
Convert from fps to USB/UVC interval.
Definition: VideoMapping.C:66
#define PLERROR(msg)
Like LERROR but appends errno and strerror(errno), to be used when some system call fails...
Definition: Log.H:194
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:169
size_t bufindex
The index of the data buffer in the kernel driver.
Definition: RawImage.H:143
unsigned int ow
output width
Definition: VideoMapping.H:44
unsigned int width
Image width in pixels.
Definition: RawImage.H:139
void streamOff() override
Stop streaming.
Definition: Gadget.C:765
#define intresponse(val)
#define UVC_EVENT_STREAMON
Definition: uvc.h:28
#define UVC_EVENT_CONNECT
Definition: uvc.h:26
#define UVC_EVENT_SETUP
Definition: uvc.h:30