392 lines
12 KiB
C++
392 lines
12 KiB
C++
#include "DeviceDiscovery.h"
|
|
#include "BoostLog.h"
|
|
#include "StringUtility.h"
|
|
#include <boost/scope/scope_exit.hpp>
|
|
#include <cstdlib>
|
|
#include <filesystem>
|
|
#include <fstream>
|
|
#ifdef WIN32
|
|
#include <mfapi.h>
|
|
#include <mfcaptureengine.h>
|
|
#else
|
|
#include <fcntl.h>
|
|
#include <linux/videodev2.h>
|
|
#include <sys/ioctl.h>
|
|
#include <sys/mman.h>
|
|
#endif
|
|
|
|
template <class T>
|
|
void SafeRelease(T **ppT) {
|
|
if (*ppT) {
|
|
(*ppT)->Release();
|
|
*ppT = NULL;
|
|
}
|
|
}
|
|
|
|
DeviceDiscovery::DeviceDiscovery() {
|
|
}
|
|
|
|
#ifdef WIN32
|
|
static std::string deviceName(IMFActivate *device) {
|
|
std::string ret;
|
|
WCHAR *friendlyName = nullptr;
|
|
uint32_t nameLength = 0;
|
|
auto result = device->GetAllocatedString(MF_DEVSOURCE_ATTRIBUTE_FRIENDLY_NAME, &friendlyName, &nameLength);
|
|
|
|
if (SUCCEEDED(result)) {
|
|
ret = Amass::StringUtility::wstringToString(std::wstring(friendlyName, nameLength));
|
|
}
|
|
if (friendlyName != nullptr) {
|
|
CoTaskMemFree(friendlyName);
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
std::shared_ptr<DeviceDiscovery::Device> DeviceDiscovery::find(const std::string &deviceName, std::error_code &error) {
|
|
std::shared_ptr<Device> ret;
|
|
|
|
IMFAttributes *attributes = nullptr;
|
|
boost::scope::scope_exit guard([&attributes] { SafeRelease(&attributes); });
|
|
auto result = MFCreateAttributes(&attributes, 1);
|
|
if (FAILED(result)) {
|
|
return ret;
|
|
}
|
|
|
|
result = attributes->SetGUID(MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE, MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE_VIDCAP_GUID);
|
|
if (FAILED(result)) {
|
|
return ret;
|
|
}
|
|
|
|
UINT32 count;
|
|
IMFActivate **devices = nullptr;
|
|
result = MFEnumDeviceSources(attributes, &devices, &count);
|
|
if (FAILED(result)) {
|
|
return ret;
|
|
}
|
|
|
|
if (count == 0) {
|
|
return ret;
|
|
}
|
|
|
|
int index = -1;
|
|
for (int i = 0; i < count; i++) {
|
|
auto name = ::deviceName(devices[i]);
|
|
LOG(info) << "device[" << i << "]: " << name;
|
|
if (name == deviceName) {
|
|
index = i;
|
|
break;
|
|
}
|
|
}
|
|
if (index >= 0) {
|
|
IMFMediaSource *source = nullptr;
|
|
result = devices[index]->ActivateObject(IID_PPV_ARGS(&source));
|
|
if (FAILED(result)) {
|
|
} else {
|
|
ret = std::make_shared<Device>(source);
|
|
}
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
void DeviceDiscovery::enterOtaMode(const std::shared_ptr<Device> &device, std::error_code &error) {
|
|
auto resolutions = deviceResolutions(device);
|
|
LOG(info) << "device resolutions:";
|
|
for (auto &[w, h] : resolutions) {
|
|
LOG(info) << "\t" << w << "*" << h;
|
|
}
|
|
|
|
int32_t otaSpecificHeight = -1;
|
|
for (auto &[width, height] : resolutions) {
|
|
if (width == OtaSpecificWidth) {
|
|
otaSpecificHeight = height;
|
|
break;
|
|
}
|
|
}
|
|
if (otaSpecificHeight <= 0) {
|
|
LOG(error) << "cannot find ota specific resolution.";
|
|
return;
|
|
} else {
|
|
LOG(info) << "found ota specific resolution: " << OtaSpecificWidth << "x" << otaSpecificHeight;
|
|
}
|
|
|
|
IMFMediaType *type = nullptr;
|
|
auto result = MFCreateMediaType(&type);
|
|
if (SUCCEEDED(result)) {
|
|
result = type->SetGUID(MF_MT_MAJOR_TYPE, MFMediaType_Video);
|
|
result = type->SetGUID(MF_MT_SUBTYPE, MFVideoFormat_MJPG);
|
|
result = MFSetAttributeSize(type, MF_MT_FRAME_SIZE, OtaSpecificWidth, otaSpecificHeight);
|
|
result = type->SetUINT32(MF_MT_ALL_SAMPLES_INDEPENDENT, TRUE);
|
|
if (SUCCEEDED(result)) {
|
|
result = device->reader->SetCurrentMediaType((DWORD)MF_SOURCE_READER_FIRST_VIDEO_STREAM, nullptr, type);
|
|
}
|
|
type->Release();
|
|
}
|
|
DWORD streamIndex, flags;
|
|
LONGLONG llTimeStamp;
|
|
IMFSample *pSample = NULL;
|
|
result = device->reader->ReadSample((DWORD)MF_SOURCE_READER_FIRST_VIDEO_STREAM, 0, &streamIndex, &flags,
|
|
&llTimeStamp, &pSample);
|
|
}
|
|
|
|
std::vector<std::string> DeviceDiscovery::devices() {
|
|
std::vector<std::string> ret;
|
|
IMFAttributes *attributes = nullptr;
|
|
boost::scope::scope_exit guard([&attributes] { SafeRelease(&attributes); });
|
|
auto result = MFCreateAttributes(&attributes, 1);
|
|
if (FAILED(result)) {
|
|
return ret;
|
|
}
|
|
|
|
result = attributes->SetGUID(MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE, MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE_VIDCAP_GUID);
|
|
if (FAILED(result)) {
|
|
return ret;
|
|
}
|
|
|
|
UINT32 count;
|
|
IMFActivate **devices = nullptr;
|
|
result = MFEnumDeviceSources(attributes, &devices, &count);
|
|
if (FAILED(result)) {
|
|
return ret;
|
|
}
|
|
|
|
if (count == 0) {
|
|
return ret;
|
|
}
|
|
|
|
for (int i = 0; i < count; i++) {
|
|
auto name = ::deviceName(devices[i]);
|
|
ret.push_back(name);
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
DeviceDiscovery::Resolutions DeviceDiscovery::deviceResolutions(const std::shared_ptr<Device> &source) {
|
|
DeviceDiscovery::Resolutions ret;
|
|
HRESULT result = S_OK;
|
|
DWORD index = 0;
|
|
while (SUCCEEDED(result)) {
|
|
IMFMediaType *mediaType = nullptr;
|
|
result = source->reader->GetNativeMediaType(MF_SOURCE_READER_FIRST_VIDEO_STREAM, index++, &mediaType);
|
|
if (SUCCEEDED(result)) {
|
|
UINT32 width, height;
|
|
MFGetAttributeSize(mediaType, MF_MT_FRAME_SIZE, &width, &height);
|
|
mediaType->Release();
|
|
auto iterator = std::find_if(ret.begin(), ret.end(), [&width, &height](const Resolution &resolution) {
|
|
return (resolution.first == width) && (resolution.second == height);
|
|
});
|
|
if (iterator == ret.end()) {
|
|
ret.push_back({width, height});
|
|
}
|
|
}
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
DeviceDiscovery::Device::Device(IMFMediaSource *source) : source(source) {
|
|
source->AddRef();
|
|
auto result = MFCreateSourceReaderFromMediaSource(source, nullptr, &reader);
|
|
if (FAILED(result)) {
|
|
LOG(error) << "MFCreateSourceReaderFromMediaSource() failed, result: " << result;
|
|
}
|
|
}
|
|
#else
|
|
std::vector<std::string> DeviceDiscovery::devices() {
|
|
std::vector<std::string> ret;
|
|
for (const auto &entry : std::filesystem::directory_iterator("/dev")) {
|
|
if (entry.is_character_file() && entry.path().string().find("video") != std::string::npos) {
|
|
int fd = open(entry.path().c_str(), O_RDWR);
|
|
if (fd < 0) {
|
|
continue;
|
|
}
|
|
|
|
struct v4l2_capability cap;
|
|
if (ioctl(fd, VIDIOC_QUERYCAP, &cap) == 0) {
|
|
if ((strstr(reinterpret_cast<const char *>(cap.card), DeviceName) != nullptr) &&
|
|
(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
|
|
ret.push_back(entry.path().string());
|
|
}
|
|
}
|
|
close(fd);
|
|
}
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
static std::string find_video_device_by_name(const std::string &targetName) {
|
|
std::string ret;
|
|
const std::string base_path = "/sys/class/video4linux";
|
|
for (const auto &entry : std::filesystem::directory_iterator(base_path)) {
|
|
if (!std::filesystem::is_directory(entry.path())) continue;
|
|
|
|
std::string interface;
|
|
std::ifstream ifs(entry.path().string() + "/device/interface");
|
|
if (ifs.is_open()) {
|
|
std::getline(ifs, interface);
|
|
}
|
|
if (interface != targetName) continue;
|
|
|
|
ifs = std::ifstream(entry.path().string() + "/index");
|
|
std::string index;
|
|
if (ifs.is_open()) {
|
|
std::getline(ifs, index);
|
|
}
|
|
if (index != "0") continue;
|
|
|
|
ret = "/dev/" + entry.path().filename().string();
|
|
break;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
std::shared_ptr<DeviceDiscovery::Device> DeviceDiscovery::find(const std::string &deviceName, std::error_code &error) {
|
|
auto ret = std::make_shared<Device>();
|
|
ret->name = find_video_device_by_name(deviceName);
|
|
return ret;
|
|
}
|
|
|
|
void DeviceDiscovery::enterOtaMode(const std::shared_ptr<Device> &device, std::error_code &error) {
|
|
auto resolutions = deviceResolutions(device);
|
|
LOG(info) << "device resolutions:";
|
|
for (auto &[w, h] : resolutions) {
|
|
LOG(info) << "\t" << w << "*" << h;
|
|
}
|
|
int32_t otaSpecificHeight = -1;
|
|
for (auto &[width, height] : resolutions) {
|
|
if (width == OtaSpecificWidth) {
|
|
otaSpecificHeight = height;
|
|
break;
|
|
}
|
|
}
|
|
if (otaSpecificHeight <= 0) {
|
|
LOG(error) << "cannot find ota specific resolution.";
|
|
return;
|
|
} else {
|
|
LOG(info) << "found ota specific resolution: " << OtaSpecificWidth << "x" << otaSpecificHeight;
|
|
}
|
|
int fd = open(device->name.c_str(), O_RDWR);
|
|
if (fd <= 0) {
|
|
LOG(error) << "Failed to open device " << device->name;
|
|
} else {
|
|
struct v4l2_format format;
|
|
memset(&format, 0, sizeof(format));
|
|
format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
format.fmt.pix.width = OtaSpecificWidth;
|
|
format.fmt.pix.height = otaSpecificHeight;
|
|
format.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
|
|
format.fmt.pix.field = V4L2_FIELD_INTERLACED;
|
|
|
|
if (ioctl(fd, VIDIOC_S_FMT, &format) == -1) {
|
|
perror("Setting Pixel Format");
|
|
return;
|
|
}
|
|
#define CLEAR(x) memset(&(x), 0, sizeof(x))
|
|
struct v4l2_requestbuffers req;
|
|
CLEAR(req);
|
|
req.count = 1;
|
|
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
req.memory = V4L2_MEMORY_MMAP;
|
|
|
|
if (ioctl(fd, VIDIOC_REQBUFS, &req) == -1) {
|
|
perror("Requesting Buffer");
|
|
return;
|
|
}
|
|
|
|
struct v4l2_buffer buf;
|
|
CLEAR(buf);
|
|
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
buf.memory = V4L2_MEMORY_MMAP;
|
|
buf.index = 0;
|
|
|
|
if (ioctl(fd, VIDIOC_QUERYBUF, &buf) == -1) {
|
|
perror("Querying Buffer");
|
|
return;
|
|
}
|
|
|
|
struct buffer {
|
|
void *start;
|
|
size_t length;
|
|
};
|
|
|
|
struct buffer buffer;
|
|
buffer.length = buf.length;
|
|
buffer.start = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, buf.m.offset);
|
|
|
|
if (buffer.start == MAP_FAILED) {
|
|
perror("Mapping Buffer");
|
|
return;
|
|
}
|
|
|
|
if (ioctl(fd, VIDIOC_QBUF, &buf) == -1) {
|
|
perror("Queue Buffer");
|
|
return;
|
|
}
|
|
|
|
enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
if (ioctl(fd, VIDIOC_STREAMON, &type) == -1) {
|
|
perror("Start Capture");
|
|
return;
|
|
}
|
|
|
|
if (ioctl(fd, VIDIOC_STREAMOFF, &type) == -1) {
|
|
perror("Stop Capture");
|
|
return;
|
|
}
|
|
|
|
if (munmap(buffer.start, buffer.length) == -1) {
|
|
perror("Unmapping Buffer");
|
|
return;
|
|
}
|
|
|
|
close(fd);
|
|
}
|
|
}
|
|
|
|
DeviceDiscovery::Resolutions DeviceDiscovery::deviceResolutions(const std::shared_ptr<Device> &source) {
|
|
Resolutions ret;
|
|
int fd = open(source->name.c_str(), O_RDWR);
|
|
if (fd <= 0) {
|
|
LOG(error) << "Failed to open device " << source->name;
|
|
} else {
|
|
struct v4l2_fmtdesc fmt;
|
|
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
fmt.index = 0;
|
|
|
|
while (ioctl(fd, VIDIOC_ENUM_FMT, &fmt) == 0) {
|
|
std::cout << "Pixel Format: " << fmt.description << std::endl;
|
|
struct v4l2_frmsizeenum frmsize;
|
|
frmsize.pixel_format = fmt.pixelformat;
|
|
frmsize.index = 0;
|
|
|
|
while (ioctl(fd, VIDIOC_ENUM_FRAMESIZES, &frmsize) == 0) {
|
|
if (frmsize.type == V4L2_FRMSIZE_TYPE_DISCRETE) {
|
|
ret.emplace_back(frmsize.discrete.width, frmsize.discrete.height);
|
|
} else if (frmsize.type == V4L2_FRMSIZE_TYPE_STEPWISE) {
|
|
for (int width = frmsize.stepwise.min_width; width <= frmsize.stepwise.max_width;
|
|
width += frmsize.stepwise.step_width) {
|
|
for (int height = frmsize.stepwise.min_height; height <= frmsize.stepwise.max_height;
|
|
height += frmsize.stepwise.step_height) {
|
|
ret.emplace_back(width, height);
|
|
}
|
|
}
|
|
}
|
|
frmsize.index++;
|
|
}
|
|
fmt.index++;
|
|
}
|
|
close(fd);
|
|
}
|
|
return ret;
|
|
}
|
|
#endif
|
|
|
|
DeviceDiscovery::Device::~Device() {
|
|
#ifdef Q_OS_WIN
|
|
if (source != nullptr) {
|
|
source->Release();
|
|
}
|
|
if (reader != nullptr) {
|
|
reader->Release();
|
|
}
|
|
#endif
|
|
}
|