SmartLockerTools/Peripheral/DeviceDiscovery.cpp

392 lines
12 KiB
C++
Raw Normal View History

2024-05-17 10:36:23 +08:00
#include "DeviceDiscovery.h"
#include "BoostLog.h"
#include "StringUtility.h"
#include <boost/scope/scope_exit.hpp>
#include <cstdlib>
2024-10-01 23:33:22 +08:00
#include <filesystem>
2024-10-02 03:01:37 +08:00
#include <fstream>
2024-10-02 03:13:15 +08:00
#ifdef WIN32
2024-05-17 10:36:23 +08:00
#include <mfapi.h>
#include <mfcaptureengine.h>
2024-10-01 23:33:22 +08:00
#else
#include <fcntl.h>
#include <linux/videodev2.h>
#include <sys/ioctl.h>
2024-10-02 03:01:37 +08:00
#include <sys/mman.h>
2024-10-01 23:33:22 +08:00
#endif
2024-05-17 10:36:23 +08:00
template <class T>
void SafeRelease(T **ppT) {
if (*ppT) {
(*ppT)->Release();
*ppT = NULL;
}
}
DeviceDiscovery::DeviceDiscovery() {
}
2024-10-02 03:13:15 +08:00
#ifdef WIN32
2024-10-01 23:01:52 +08:00
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;
}
2024-05-17 10:36:23 +08:00
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++) {
2024-10-01 23:01:52 +08:00
auto name = ::deviceName(devices[i]);
2024-05-17 10:36:23 +08:00
LOG(info) << "device[" << i << "]: " << name;
if (name == deviceName) {
index = i;
break;
}
}
if (index >= 0) {
IMFMediaSource *source = nullptr;
2024-06-21 18:56:48 +08:00
result = devices[index]->ActivateObject(IID_PPV_ARGS(&source));
2024-05-17 10:36:23 +08:00
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);
2024-06-21 18:56:48 +08:00
LOG(info) << "device resolutions:";
for (auto &[w, h] : resolutions) {
LOG(info) << "\t" << w << "*" << h;
}
2024-05-17 10:36:23 +08:00
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;
2024-06-21 18:56:48 +08:00
} else {
LOG(info) << "found ota specific resolution: " << OtaSpecificWidth << "x" << otaSpecificHeight;
2024-05-17 10:36:23 +08:00
}
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);
}
2024-06-13 15:41:40 +08:00
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++) {
2024-10-01 23:01:52 +08:00
auto name = ::deviceName(devices[i]);
2024-06-13 15:41:40 +08:00
ret.push_back(name);
}
return ret;
}
2024-05-17 10:36:23 +08:00
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;
}
}
2024-10-01 23:33:22 +08:00
#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) {
2024-10-02 03:01:37 +08:00
if ((strstr(reinterpret_cast<const char *>(cap.card), DeviceName) != nullptr) &&
(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
2024-10-01 23:33:22 +08:00
ret.push_back(entry.path().string());
}
}
close(fd);
}
}
return ret;
}
2024-10-02 03:01:37 +08:00
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;
}
2024-10-01 23:33:22 +08:00
#endif
2024-05-17 10:36:23 +08:00
DeviceDiscovery::Device::~Device() {
2024-10-01 23:33:22 +08:00
#ifdef Q_OS_WIN
2024-05-17 10:36:23 +08:00
if (source != nullptr) {
source->Release();
}
if (reader != nullptr) {
reader->Release();
}
2024-10-01 23:33:22 +08:00
#endif
2024-05-17 10:36:23 +08:00
}