26 #include <sys/types.h>
34 inline void debugCtrlReq(
struct usb_ctrlrequest
const & ctrl)
38 LDEBUG(std::showbase << std::hex <<
"bRequestType " << ctrl.bRequestType <<
" bRequest " << ctrl.bRequest
39 <<
" wValue " << ctrl.wValue <<
" wIndex " << ctrl.wIndex <<
" wLength " << ctrl.wLength);
42 inline void debugStreamingCtrl(std::string
const & msg,
struct uvc_streaming_control
const & ctrl)
46 LDEBUG(msg <<
": " << std::showbase << std::hex <<
"bmHint=" << ctrl.bmHint <<
", bFormatIndex=" <<
47 ctrl.bFormatIndex <<
", bFrameIndex=" << ctrl.bFrameIndex <<
", dwFrameInterval=" << ctrl.dwFrameInterval <<
48 ", wKeyFrameRate=" << ctrl.wKeyFrameRate <<
", wPFrameRate=" << ctrl.wPFrameRate <<
49 ", wCompQuality=" << ctrl.wCompQuality <<
", wCompWindowSize=" << ctrl.wCompWindowSize <<
50 ", wDelay=" << ctrl.wDelay <<
", dwMaxVideoFrameSize=" << ctrl.dwMaxVideoFrameSize <<
51 ", dwMaxPayloadTransferSize=" << ctrl.dwMaxPayloadTransferSize <<
", dwClockFrequency=" <<
52 ctrl.dwClockFrequency <<
", bmFramingInfo=" << ctrl.bmFramingInfo <<
", bPreferedVersion=" <<
53 ctrl.bPreferedVersion <<
", bMinVersion=" << ctrl.bMinVersion <<
", bMaxVersion=" << ctrl.bMaxVersion);
56 unsigned int uvcToV4Lcontrol(
unsigned int entity,
unsigned int cs)
85 case UVC_CT_AE_MODE_CONTROL:
return V4L2_CID_EXPOSURE_AUTO;
86 case UVC_CT_AE_PRIORITY_CONTROL:
return V4L2_CID_EXPOSURE_AUTO_PRIORITY;
87 case UVC_CT_EXPOSURE_TIME_ABSOLUTE_CONTROL:
return V4L2_CID_EXPOSURE_ABSOLUTE;
118 case UVC_PU_BACKLIGHT_COMPENSATION_CONTROL:
return V4L2_CID_BACKLIGHT_COMPENSATION;
119 case UVC_PU_BRIGHTNESS_CONTROL:
return V4L2_CID_BRIGHTNESS;
120 case UVC_PU_CONTRAST_CONTROL:
return V4L2_CID_CONTRAST;
121 case UVC_PU_GAIN_CONTROL:
return V4L2_CID_GAIN;
122 case UVC_PU_POWER_LINE_FREQUENCY_CONTROL:
return V4L2_CID_POWER_LINE_FREQUENCY;
123 case UVC_PU_HUE_CONTROL:
return V4L2_CID_HUE;
124 case UVC_PU_SATURATION_CONTROL:
return V4L2_CID_SATURATION;
125 case UVC_PU_SHARPNESS_CONTROL:
return V4L2_CID_SHARPNESS;
126 case UVC_PU_WHITE_BALANCE_COMPONENT_CONTROL:
return V4L2_CID_RED_BALANCE;
127 case UVC_PU_WHITE_BALANCE_COMPONENT_AUTO_CONTROL:
return V4L2_CID_AUTO_WHITE_BALANCE;
133 LFATAL(
"Request to access unsupported control " << cs <<
" on entity " << entity);
140 size_t const nbufs,
bool multicam) :
141 itsFd(-1), itsMulticam(multicam), itsNbufs(nbufs), itsBuffers(nullptr), itsCamera(camera), itsEngine(engine),
142 itsRunning(false), itsFormat(), itsFps(0.0
F), itsStreaming(false), itsErrorCode(0), itsControl(0), itsEntity(0)
146 if (itsCamera ==
nullptr)
LFATAL(
"Gadget requires a valid camera to work");
149 fillStreamingControl(&itsProbe, m);
150 fillStreamingControl(&itsCommit, m);
154 while (itsRunning.load() ==
false) std::this_thread::sleep_for(std::chrono::milliseconds(5));
157 itsFd = open(devname.c_str(), O_RDWR | O_NONBLOCK);
158 if (itsFd == -1)
PLFATAL(
"Gadget device open failed for " << devname);
161 struct v4l2_event_subscription sub = { };
164 XIOCTL(itsFd, VIDIOC_SUBSCRIBE_EVENT, &sub);
167 XIOCTL(itsFd, VIDIOC_SUBSCRIBE_EVENT, &sub);
170 XIOCTL(itsFd, VIDIOC_SUBSCRIBE_EVENT, &sub);
173 XIOCTL(itsFd, VIDIOC_SUBSCRIBE_EVENT, &sub);
176 struct v4l2_capability cap = { };
177 XIOCTL(itsFd, VIDIOC_QUERYCAP, &cap);
179 LINFO(
'[' << itsFd <<
"] UVC gadget " << devname <<
" card " << cap.card <<
" bus " << cap.bus_info);
180 if ((cap.capabilities & V4L2_CAP_VIDEO_OUTPUT) == 0)
LFATAL(devname <<
" is not a video output device");
181 if ((cap.capabilities & V4L2_CAP_STREAMING) == 0)
LFATAL(devname <<
" does not support streaming");
192 itsRunning.store(
false);
197 if (close(itsFd) == -1)
PLERROR(
"Error closing UVC gadget -- IGNORED");
208 memset(&itsFormat, 0,
sizeof(
struct v4l2_format));
210 itsFormat.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
211 itsFormat.fmt.pix.width = m.ow;
212 itsFormat.fmt.pix.height = m.oh;
213 itsFormat.fmt.pix.pixelformat = m.ofmt;
214 itsFormat.fmt.pix.field = V4L2_FIELD_NONE;
215 itsFormat.fmt.pix.sizeimage = m.osize();
219 if (m.ofmt == 0) {
LINFO(
"USB Gadget set video format to NONE");
return; }
222 XIOCTL(itsFd, VIDIOC_S_FMT, &itsFormat);
228 struct v4l2_streamparm sparm = { };
229 sparm.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
230 sparm.parm.output.outputmode = 2;
231 sparm.parm.output.timeperframe = jevois::VideoMapping::fpsToV4l2(m.ofps);
232 XIOCTL_QUIET(itsFd, VIDIOC_S_PARM, &sparm);
235 LINFO(
"USB Gadget set video format to " << itsFormat.fmt.pix.width <<
'x' << itsFormat.fmt.pix.height <<
' ' <<
240 void jevois::Gadget::run()
249 itsRunning.store(
true);
252 while (itsFd == -1) std::this_thread::sleep_for(std::chrono::milliseconds(1));
255 while (itsRunning.load())
258 FD_ZERO(&wfds); FD_ZERO(&efds); FD_SET(itsFd, &wfds); FD_SET(itsFd, &efds);
259 tv.tv_sec = 0; tv.tv_usec = 10000;
261 int ret = select(itsFd + 1,
nullptr, &wfds, &efds, &tv);
263 if (ret == -1) {
PLERROR(
"Select error");
if (errno == EINTR)
continue;
else break; }
267 if (FD_ISSET(itsFd, &efds))
273 while (
true)
try { processEvents(); }
catch (...) {
break; }
283 while (
true)
try { processEvents(); }
catch (...) {
break; }
289 if (itsDoneImgs.size())
291 LDEBUG(
"Queuing image " << itsDoneImgs.front() <<
" for sending over USB");
294 struct v4l2_buffer buf = { };
296 buf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
297 buf.memory = V4L2_MEMORY_MMAP;
298 buf.index = itsDoneImgs.front();
299 buf.length = itsBuffers->get(buf.index)->length();
301 if (itsFormat.fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG)
302 buf.bytesused = itsBuffers->get(buf.index)->bytesUsed();
304 buf.bytesused = buf.length;
306 buf.field = V4L2_FIELD_NONE;
308 gettimeofday(&buf.timestamp,
nullptr);
311 itsBuffers->qbuf(buf);
314 itsDoneImgs.pop_front();
320 itsRunning.store(
false);
324 void jevois::Gadget::processEvents()
329 struct v4l2_event v4l2ev = { };
330 XIOCTL_QUIET(itsFd, VIDIOC_DQEVENT, &v4l2ev);
353 void jevois::Gadget::processVideo()
361 if (itsStreaming.load() ==
false)
LFATAL(
"Aborted while not streaming");
365 struct v4l2_buffer buf;
366 itsBuffers->dqbuf(buf);
369 img.
width = itsFormat.fmt.pix.width;
370 img.
height = itsFormat.fmt.pix.height;
371 img.
fmt = itsFormat.fmt.pix.pixelformat;
373 img.
buf = itsBuffers->get(buf.index);
377 itsImageQueue.push_back(img);
378 LDEBUG(
"Empty image " << img.
bufindex <<
" ready for filling in by application code");
382 void jevois::Gadget::processEventSetup(
struct usb_ctrlrequest
const & ctrl,
struct uvc_request_data & resp)
386 itsControl = 0; itsEntity = 0;
390 switch (ctrl.bRequestType & USB_TYPE_MASK)
392 case USB_TYPE_STANDARD: processEventStandard(ctrl, resp);
break;
393 case USB_TYPE_CLASS: processEventClass(ctrl, resp);
break;
394 default:
LERROR(
"Unsupported setup event type " << std::showbase << std::hex <<
395 (ctrl.bRequestType & USB_TYPE_MASK) <<
" -- IGNORED");
401 void jevois::Gadget::processEventStandard(
struct usb_ctrlrequest
const & ctrl,
406 LDEBUG(
"UVC standard setup event ignored:");
411 void jevois::Gadget::processEventClass(
struct usb_ctrlrequest
const & ctrl,
struct uvc_request_data & resp)
415 if ((ctrl.bRequestType & USB_RECIP_MASK) != USB_RECIP_INTERFACE)
return;
417 switch (ctrl.wIndex & 0xff)
420 processEventControl(ctrl.bRequest, ctrl.wValue >> 8, ctrl.wIndex >> 8, ctrl.wLength, resp);
424 processEventStreaming(ctrl.bRequest, ctrl.wValue >> 8, resp);
428 LERROR(
"Unsupported setup event class " << std::showbase << std::hex << (ctrl.wIndex & 0xff) <<
" -- IGNORED");
433 void jevois::Gadget::processEventControl(uint8_t req, uint8_t cs, uint8_t entity_id, uint8_t len,
439 #define success() { itsErrorCode = 0; }
442 #define failure(code) { resp.length = -EL2HLT; itsErrorCode = code; }
445 #define byteresponse(val) { resp.data[0] = val; resp.length = 1; itsErrorCode = 0; }
448 #define wordresponse(val) { resp.data[0] = val & 0xff; resp.data[1] = (val >> 8) & 0xff; \
449 resp.length = 2; itsErrorCode = 0; }
452 #define intresponse(val) { resp.data[0] = val & 0xff; resp.data[1] = (val >> 8) & 0xff; \
453 resp.data[2] = (val >> 16) & 0xff; resp.data[3] = (val >> 24) & 0xff; \
454 resp.length = 4; itsErrorCode = 0; }
457 #define arrayblankresponse(len) { memset(resp.data, 0, len); resp.length = len; itsErrorCode = 0; }
467 case UVC_VC_REQUEST_ERROR_CODE_CONTROL:
byteresponse(itsErrorCode);
return;
468 default:
failure(0x06);
return;
473 if (req == UVC_SET_CUR)
476 itsEntity = entity_id; itsControl = cs;
478 LDEBUG(
"SET_CUR ent " << itsEntity <<
" ctrl "<< itsControl <<
" len "<< len);
480 else if (req == UVC_GET_INFO)
483 byteresponse(UVC_CONTROL_CAP_GET | UVC_CONTROL_CAP_SET);
485 else if (req == UVC_GET_CUR)
490 struct v4l2_control ctrl = { };
491 try { ctrl.id = uvcToV4Lcontrol(entity_id, cs); itsCamera->getControl(ctrl); }
catch (...) { ctrl.id = 0; }
494 if (ctrl.id == V4L2_CID_RED_BALANCE)
496 unsigned int redval = (ctrl.value & 0xffff) << 16;
499 ctrl.id = V4L2_CID_BLUE_BALANCE;
500 itsCamera->getControl(ctrl);
503 ctrl.value = (ctrl.value & 0xffff) | redval;
506 else if (ctrl.id == V4L2_CID_EXPOSURE_AUTO)
508 if (ctrl.value == V4L2_EXPOSURE_MANUAL) ctrl.value = 0x01;
509 else if (ctrl.value == V4L2_EXPOSURE_AUTO) ctrl.value = 0x02;
510 else ctrl.value = 0x03;
514 else if (ctrl.id == 0) ctrl.value = 0;
521 default:
LERROR(
"Unsupported control with length " << len <<
" -- SENDING BLANK RESPONSE");
530 struct v4l2_queryctrl qc = { };
531 try { qc.id = uvcToV4Lcontrol(entity_id, cs); itsCamera->queryControl(qc); }
catch (...) { qc.id = 0; }
534 if (qc.id == V4L2_CID_RED_BALANCE)
537 struct v4l2_queryctrl qc2 = { };
538 qc2.id = V4L2_CID_BLUE_BALANCE;
539 itsCamera->queryControl(qc2);
542 qc.default_value = (qc.default_value << 16) | qc2.default_value;
543 qc.step = (qc.step << 16) | qc2.step;
544 qc.minimum = (qc.minimum << 16) | qc2.minimum;
545 qc.maximum = (qc.maximum << 16) | qc2.maximum;
548 else if (qc.id == V4L2_CID_EXPOSURE_AUTO)
553 qc.minimum = 0; qc.step = 3; qc.maximum = 3; qc.default_value = 1;
557 { qc.minimum = 0; qc.step = 1; qc.maximum = 1; qc.default_value = 0; }
562 case UVC_GET_DEF: val = qc.default_value;
break;
563 case UVC_GET_RES: val = qc.step;
break;
564 case UVC_GET_MIN: val = qc.minimum;
break;
565 case UVC_GET_MAX: val = qc.maximum;
break;
566 default:
failure(0x07);
return;
574 default:
LERROR(
"Unsupported control with length " << len <<
" -- SENDING BLANK RESPONSE");
581 LERROR(
"FAILED entity " << entity_id <<
" cs " << cs <<
" len " << len);
587 void jevois::Gadget::fillStreamingControl(
struct uvc_streaming_control * ctrl, jevois::VideoMapping
const & m)
591 memset(ctrl, 0,
sizeof(
struct uvc_streaming_control));
593 ctrl->bFormatIndex = m.uvcformat;
594 ctrl->bFrameIndex = m.uvcframe;
595 ctrl->dwFrameInterval = jevois::VideoMapping::fpsToUvc(m.ofps);
596 ctrl->dwMaxVideoFrameSize = m.osize();
597 ctrl->dwMaxPayloadTransferSize = itsMulticam ? 1024 : 3072;
598 ctrl->bmFramingInfo = 3;
599 ctrl->bPreferedVersion = 1;
600 ctrl->bMaxVersion = 1;
604 void jevois::Gadget::processEventStreaming(uint8_t req, uint8_t cs,
struct uvc_request_data & resp)
608 int const datalen = 26;
609 if (cs != UVC_VS_PROBE_CONTROL && cs != UVC_VS_COMMIT_CONTROL)
return;
611 struct uvc_streaming_control * ctrl =
reinterpret_cast<struct uvc_streaming_control *
>(&resp.
data);
612 struct uvc_streaming_control * target = (cs == UVC_VS_PROBE_CONTROL) ? &itsProbe : &itsCommit;
617 case UVC_SET_CUR: itsControl = cs; resp.
length = datalen;
break;
622 memcpy(ctrl, target, datalen);
629 size_t idx = itsEngine->getDefaultVideoMappingIdx();
630 try { idx = itsEngine->getVideoMappingIdx(ctrl->bFormatIndex, ctrl->bFrameIndex, ctrl->dwFrameInterval); }
632 fillStreamingControl(target, itsEngine->getVideoMapping(idx));
633 memcpy(ctrl, target, datalen);
637 case UVC_GET_RES: memset(ctrl, 0, datalen);
break;
639 case UVC_GET_LEN: resp.
data[0] = 0x00; resp.
data[1] = datalen; resp.
length = 2;
break;
641 case UVC_GET_INFO: resp.
data[0] = 0x03; resp.
length = 1;
break;
650 struct uvc_streaming_control * target;
653 if (itsEntity == 2 || itsEntity == 1) { processEventControlData(data);
return; }
657 case UVC_VS_PROBE_CONTROL: target = &itsProbe;
break;
658 case UVC_VS_COMMIT_CONTROL: target = &itsCommit;
break;
659 default: processEventControlData(data);
return;
663 struct uvc_streaming_control * ctrl =
reinterpret_cast<struct uvc_streaming_control *
>(&data.
data);
665 size_t idx = itsEngine->getVideoMappingIdx(ctrl->bFormatIndex, ctrl->bFrameIndex, ctrl->dwFrameInterval);
667 fillStreamingControl(target, itsEngine->getVideoMapping(idx));
669 LDEBUG(
"Host requested " << ctrl->bFormatIndex <<
'/' << ctrl->bFrameIndex <<
'/' << ctrl->dwFrameInterval <<
670 ", " << ((itsControl == UVC_VS_COMMIT_CONTROL) ?
"setting " :
"returning ") <<
671 itsEngine->getVideoMapping(idx).str());
674 if (itsControl == UVC_VS_COMMIT_CONTROL) itsEngine->setFormat(idx);
678 void jevois::Gadget::processEventControlData(
struct uvc_request_data & data)
682 struct v4l2_control ctrl;
685 ctrl.id = uvcToV4Lcontrol(itsEntity, itsControl);
690 case 1: ctrl.value =
static_cast<int>(data.
data[0]);
break;
691 case 2: ctrl.value =
static_cast<int>(__s16(data.
data[0] | (
static_cast<short>(data.
data[1]) << 8)));
break;
692 case 4: ctrl.value = data.
data[0] | (data.
data[1] << 8) | (data.
data[2] << 16) | (data.
data[3] << 24);
break;
701 case V4L2_CID_RED_BALANCE:
704 int blue = ctrl.value & 0xffff;
705 ctrl.value >>= 16; itsCamera->setControl(ctrl);
706 ctrl.id = V4L2_CID_BLUE_BALANCE; ctrl.value = blue; itsCamera->setControl(ctrl);
710 case V4L2_CID_EXPOSURE_AUTO:
711 if (ctrl.value & 0x01) ctrl.value = V4L2_EXPOSURE_MANUAL;
712 else if (ctrl.value & 0x02) ctrl.value = V4L2_EXPOSURE_AUTO;
714 itsCamera->setControl(ctrl);
717 default: itsCamera->setControl(ctrl);
726 LDEBUG(
"Turning on UVC stream");
730 if (itsStreaming.load() || itsBuffers) {
LERROR(
"Stream is already on -- IGNORED");
return; }
731 if (itsFormat.fmt.pix.pixelformat == 0) {
LINFO(
"Gadget output format is NONE");
return; }
734 unsigned int nbuf = itsNbufs;
737 unsigned int framesize =
jevois::v4l2ImageSize(itsFormat.fmt.pix.pixelformat, itsFormat.fmt.pix.width,
738 itsFormat.fmt.pix.height);
741 nbuf = (4U * 1024U * 1024U) / framesize;
742 if (nbuf > 4) nbuf = 4;
746 if (nbuf < 3) nbuf = 3;
else if (nbuf > 16) nbuf = 16;
750 LINFO(itsBuffers->size() <<
" buffers of " << itsBuffers->get(0)->length() <<
" bytes allocated");
753 for (
size_t i = 0; i < nbuf; ++i)
756 img.
width = itsFormat.fmt.pix.width;
757 img.
height = itsFormat.fmt.pix.height;
758 img.
fmt = itsFormat.fmt.pix.pixelformat;
760 img.
buf = itsBuffers->get(i);
764 itsImageQueue.push_back(img);
765 LDEBUG(
"Empty image " << img.
bufindex <<
" ready for filling in by application code");
769 int type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
770 XIOCTL(itsFd, VIDIOC_STREAMON, &type);
771 LDEBUG(
"Device stream on");
773 itsStreaming.store(
true);
782 itsStreaming.store(
false);
793 LDEBUG(
"Turning off gadget stream");
802 int type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
803 try { XIOCTL_QUIET(itsFd, VIDIOC_STREAMOFF, &type); }
catch (...) { }
806 if (itsBuffers) {
delete itsBuffers; itsBuffers =
nullptr; }
807 itsImageQueue.clear();
810 LDEBUG(
"Gadget stream is off");
821 if (itsStreaming.load() ==
false)
822 {
LDEBUG(
"Not streaming");
throw std::runtime_error(
"Gadget get() rejected while not streaming"); }
824 if (itsMtx.try_lock_for(std::chrono::milliseconds(100)))
826 if (itsStreaming.load() ==
false)
830 throw std::runtime_error(
"Gadget get() rejected while not streaming");
833 if (itsImageQueue.size())
835 img = itsImageQueue.front();
836 itsImageQueue.pop_front();
838 LDEBUG(
"Empty image " << img.
bufindex <<
" handed over to application code for filling");
844 LDEBUG(
"Waiting for blank UVC image...");
845 std::this_thread::sleep_for(std::chrono::milliseconds(5));
849 LDEBUG(
"Waiting for lock");
850 std::this_thread::sleep_for(std::chrono::milliseconds(5));
853 LFATAL(
"Giving up waiting for blank UVC image");
864 if (itsStreaming.load() ==
false)
865 {
LDEBUG(
"Not streaming");
throw std::runtime_error(
"Gadget send() rejected while not streaming"); }
867 if (itsMtx.try_lock_for(std::chrono::milliseconds(100)))
869 if (itsStreaming.load() ==
false)
873 throw std::runtime_error(
"Gadget send() rejected while not streaming");
878 if (img.
width != itsFormat.fmt.pix.width ||
879 img.
height != itsFormat.fmt.pix.height ||
880 img.
fmt != itsFormat.fmt.pix.pixelformat)
882 LDEBUG(
"Dropping image to send out as format just changed");
889 itsDoneImgs.push_back(img.
bufindex);
891 LDEBUG(
"Filled image " << img.
bufindex <<
" received from application code");
896 LDEBUG(
"Waiting for lock");
897 std::this_thread::sleep_for(std::chrono::milliseconds(5));
900 LFATAL(
"Giving up waiting for lock");