254 lines
6.5 KiB
C++
Executable file
254 lines
6.5 KiB
C++
Executable file
#include "Device.h"
|
|
#include "Logger.h"
|
|
|
|
#include <unistd.h>
|
|
#include <errno.h>
|
|
#include <string.h>
|
|
#include <sys/ioctl.h>
|
|
#include <sys/stat.h>
|
|
|
|
#include <linux/videodev2.h>
|
|
|
|
namespace Video
|
|
{
|
|
namespace V4L2
|
|
{
|
|
std::string Device::fourcc(unsigned int format)
|
|
{
|
|
char formatArray[] = {(char)(format & 0xff), (char)((format >> 8) & 0xff), (char)((format >> 16) & 0xff),
|
|
(char)((format >> 24) & 0xff), 0};
|
|
return std::string(formatArray, strlen(formatArray));
|
|
}
|
|
|
|
unsigned int Device::fourcc(const char* format)
|
|
{
|
|
char fourcc[4];
|
|
memset(&fourcc, 0, sizeof(fourcc));
|
|
if (format != NULL)
|
|
{
|
|
strncpy(fourcc, format, 4);
|
|
}
|
|
return v4l2_fourcc(fourcc[0], fourcc[1], fourcc[2], fourcc[3]);
|
|
}
|
|
|
|
Device::Device(const DeviceParameters& params, v4l2_buf_type deviceType)
|
|
: m_params(params), m_fd(-1), m_deviceType(deviceType), m_bufferSize(0), m_format(0)
|
|
{
|
|
}
|
|
|
|
Device::~Device()
|
|
{
|
|
this->close();
|
|
}
|
|
|
|
void Device::close()
|
|
{
|
|
if (m_fd != -1)
|
|
::close(m_fd);
|
|
|
|
m_fd = -1;
|
|
}
|
|
|
|
void Device::queryFormat()
|
|
{
|
|
struct v4l2_format fmt;
|
|
memset(&fmt, 0, sizeof(fmt));
|
|
fmt.type = m_deviceType;
|
|
if (0 == ioctl(m_fd, VIDIOC_G_FMT, &fmt))
|
|
{
|
|
m_format = fmt.fmt.pix.pixelformat;
|
|
m_width = fmt.fmt.pix.width;
|
|
m_height = fmt.fmt.pix.height;
|
|
m_bufferSize = fmt.fmt.pix.sizeimage;
|
|
|
|
logger().debug("%s: %s size: %ux%u bufferSize: %u", m_params.m_devName, fourcc(m_format), (uint32_t)m_width,
|
|
(uint32_t)m_height, (uint32_t)m_bufferSize);
|
|
}
|
|
}
|
|
|
|
bool Device::init(unsigned int mandatoryCapabilities)
|
|
{
|
|
struct stat sb;
|
|
if ((stat(m_params.m_devName.c_str(), &sb) == 0) && ((sb.st_mode & S_IFMT) == S_IFCHR))
|
|
{
|
|
if (initdevice(m_params.m_devName.c_str(), mandatoryCapabilities) == -1)
|
|
{
|
|
logger().error("%s: Cannot init device", m_params.m_devName);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
m_fd = open(m_params.m_devName.c_str(), O_WRONLY | O_CREAT | O_TRUNC, S_IRWXU);
|
|
}
|
|
return (m_fd != -1);
|
|
}
|
|
|
|
int Device::initdevice(const char* dev_name, unsigned int mandatoryCapabilities)
|
|
{
|
|
m_fd = open(dev_name, m_params.m_openFlags);
|
|
if (m_fd < 0)
|
|
{
|
|
logger().error("%s: Cannot open device", m_params.m_devName);
|
|
this->close();
|
|
return -1;
|
|
}
|
|
if (checkCapabilities(m_fd, mandatoryCapabilities) != 0)
|
|
{
|
|
this->close();
|
|
return -1;
|
|
}
|
|
if (configureFormat(m_fd) != 0)
|
|
{
|
|
this->close();
|
|
return -1;
|
|
}
|
|
if (configureParam(m_fd, m_params.m_fps) != 0)
|
|
{
|
|
this->close();
|
|
return -1;
|
|
}
|
|
|
|
return m_fd;
|
|
}
|
|
|
|
int Device::checkCapabilities(int fd, unsigned int mandatoryCapabilities)
|
|
{
|
|
struct v4l2_capability cap;
|
|
memset(&(cap), 0, sizeof(cap));
|
|
if (-1 == ioctl(fd, VIDIOC_QUERYCAP, &cap))
|
|
{
|
|
logger().error("%s: Cannot get capabilities: %d(%s)", m_params.m_devName, errno, std::string(strerror(errno)));
|
|
return -1;
|
|
}
|
|
|
|
std::string str = "";
|
|
if ((cap.capabilities & V4L2_CAP_VIDEO_OUTPUT))
|
|
str += "output,";
|
|
|
|
if ((cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
|
|
str += "capture,";
|
|
|
|
if ((cap.capabilities & V4L2_CAP_READWRITE))
|
|
str += "read/write,";
|
|
|
|
if ((cap.capabilities & V4L2_CAP_STREAMING))
|
|
str += "streaming,";
|
|
|
|
if ((cap.capabilities & V4L2_CAP_TIMEPERFRAME))
|
|
str += "timeperframe";
|
|
|
|
m_name = (char*)cap.card;
|
|
logger().debug("%s(%s): support: %s", m_params.m_devName, m_name, str);
|
|
|
|
if ((cap.capabilities & mandatoryCapabilities) != mandatoryCapabilities)
|
|
{
|
|
logger().error("%s: Mandatory capability not available for device", m_params.m_devName);
|
|
return -1;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int Device::configureFormat(int fd)
|
|
{
|
|
this->queryFormat();
|
|
|
|
unsigned int width = m_width;
|
|
unsigned int height = m_height;
|
|
if (m_params.m_width != 0)
|
|
{
|
|
width = m_params.m_width;
|
|
}
|
|
if (m_params.m_height != 0)
|
|
{
|
|
height = m_params.m_height;
|
|
}
|
|
if ((m_params.m_formatList.size() == 0) && (m_format != 0))
|
|
{
|
|
m_params.m_formatList.push_back(m_format);
|
|
}
|
|
|
|
std::list<unsigned int>::iterator it;
|
|
for (it = m_params.m_formatList.begin(); it != m_params.m_formatList.end(); ++it)
|
|
{
|
|
unsigned int format = *it;
|
|
if (this->configureFormat(fd, format, width, height) == 0)
|
|
{
|
|
this->queryFormat();
|
|
return 0;
|
|
}
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
int Device::configureFormat(int fd, unsigned int format, unsigned int width, unsigned int height)
|
|
{
|
|
struct v4l2_format fmt;
|
|
memset(&(fmt), 0, sizeof(fmt));
|
|
fmt.type = m_deviceType;
|
|
if (ioctl(m_fd, VIDIOC_G_FMT, &fmt) == -1)
|
|
{
|
|
logger().error("%s: Cannot get format: %d(%s)", m_params.m_devName, errno, std::string(strerror(errno)));
|
|
return -1;
|
|
}
|
|
if (width != 0)
|
|
{
|
|
fmt.fmt.pix.width = width;
|
|
}
|
|
if (height != 0)
|
|
{
|
|
fmt.fmt.pix.height = height;
|
|
}
|
|
if (format != 0)
|
|
{
|
|
fmt.fmt.pix.pixelformat = format;
|
|
}
|
|
|
|
if (ioctl(fd, VIDIOC_S_FMT, &fmt) == -1)
|
|
{
|
|
logger().error("%s: Cannot set format: %d(%s)", m_params.m_devName, errno, std::string(strerror(errno)));
|
|
return -1;
|
|
}
|
|
|
|
if (fmt.fmt.pix.pixelformat != format)
|
|
{
|
|
logger().error("%s: Cannot set pixelformat to: %s format is ", m_params.m_devName, fourcc(format),
|
|
fourcc(fmt.fmt.pix.pixelformat));
|
|
return -1;
|
|
}
|
|
|
|
if ((fmt.fmt.pix.width != width) || (fmt.fmt.pix.height != height))
|
|
{
|
|
logger().warning("%s: Cannot set size to: %ux%u size is: %ux%u", m_params.m_devName, (uint32_t)width,
|
|
(uint32_t)height, (uint32_t)fmt.fmt.pix.width, (uint32_t)fmt.fmt.pix.height);
|
|
}
|
|
|
|
m_format = fmt.fmt.pix.pixelformat;
|
|
m_width = fmt.fmt.pix.width;
|
|
m_height = fmt.fmt.pix.height;
|
|
m_bufferSize = fmt.fmt.pix.sizeimage;
|
|
|
|
return 0;
|
|
}
|
|
|
|
int Device::configureParam(int fd, int fps)
|
|
{
|
|
if (fps != 0)
|
|
{
|
|
struct v4l2_streamparm param;
|
|
memset(&(param), 0, sizeof(param));
|
|
param.type = m_deviceType;
|
|
param.parm.capture.timeperframe.numerator = 1;
|
|
param.parm.capture.timeperframe.denominator = fps;
|
|
|
|
if (ioctl(fd, VIDIOC_S_PARM, ¶m) == -1)
|
|
{
|
|
logger().warning("%s: Cannot set param for device: %d(%s)", m_params.m_devName, errno,
|
|
std::string(strerror(errno)));
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
}
|
|
}
|