SmartLockerTools/Peripheral/DeviceDiscovery.cpp

#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
}