SVG5/sthreadwatchvideolink.cpp
2025-10-12 13:55:56 +09:00

731 lines
17 KiB
C++

#include "sthreadwatchvideolink.h"
#include <stdlib.h>
#include <assert.h>
#include <fcntl.h> /* low-level i/o */
#include <unistd.h>
#include <errno.h>
#include <string.h> // strerrno
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
//#include <stdexcept>
#include <sys/klog.h>
#include <linux/videodev2.h>
#include "mainwindow.h"
#include "commondata.h"
/*
* The 24-bit IEEE Registration Identifier for the HDMI-LLC Vendor
* Specific Data Block.
*/
#define HDMI_VSDB_EXT_TAG 0x000c03
/*
* The 24-bit IEEE Registration Identifier for the HDMI-Forum Vendor
* Specific Data Block.
*/
#define HF_VSDB_EXT_TAG 0xc45dd8
#define VID_CAP_EXT_TAG 0
#define COLORIMETRY_EXT_TAG 5
#define HDR_MD_EXT_TAG 6
#define VSDB_TAG 3
#define SPEAKER_TAG 4
#define EXTENDED_TAG 7
enum format {
HEX,
RAW,
CARRAY
};
static struct v4l2_edid sedid;
static struct v4l2_edid info_edid;
static enum format gformat;
static enum format sformat;
static unsigned clear_pad;
static long phys_addr = -1;
static __u8 toggle_cta861_hdr_flags;
#define CTA861_HDR_UNDERSCAN (1 << 6)
#define CTA861_HDR_AUDIO (1 << 6)
#define CTA861_HDR_YCBCR444 (1 << 5)
#define CTA861_HDR_YCBCR422 (1 << 4)
static __u8 toggle_speaker1_flags;
#define SPEAKER1_FL_FR (1 << 0)
#define SPEAKER1_LFE (1 << 1)
#define SPEAKER1_FC (1 << 2)
#define SPEAKER1_BL_BR (1 << 3)
#define SPEAKER1_BC (1 << 4)
#define SPEAKER1_FLC_FRC (1 << 5)
#define SPEAKER1_RLC_RRC (1 << 6)
#define SPEAKER1_FLW_FRW (1 << 7)
static __u8 toggle_speaker2_flags;
#define SPEAKER2_TPFL_TPFR (1 << 0)
#define SPEAKER2_TPC (1 << 1)
#define SPEAKER2_TPFC (1 << 2)
#define SPEAKER2_LS_RS (1 << 3)
#define SPEAKER2_LFE2 (1 << 4)
#define SPEAKER2_TPBC (1 << 5)
#define SPEAKER2_SIL_SIR (1 << 6)
#define SPEAKER2_TPSIL_TPSIR (1 << 7)
static __u8 toggle_speaker3_flags;
#define SPEAKER3_TPBL_TPBR (1 << 0)
#define SPEAKER3_BTFC (1 << 1)
#define SPEAKER3_BTFL_BTFR (1 << 2)
#define SPEAKER3_TPLS_TPRS (1 << 3)
static __u8 toggle_hdmi_vsdb_dc_flags;
#define HDMI_VSDB_Y444_BIT (1 << 3)
#define HDMI_VSDB_30_BIT (1 << 4)
#define HDMI_VSDB_36_BIT (1 << 5)
#define HDMI_VSDB_48_BIT (1 << 6)
static __u8 toggle_hdmi_vsdb_cnc_flags;
#define HDMI_VSDB_GRAPHICS (1 << 0)
#define HDMI_VSDB_PHOTO (1 << 1)
#define HDMI_VSDB_CINEMA (1 << 2)
#define HDMI_VSDB_GAME (1 << 3)
#define HDMI_VSDB_I_LATENCY (1 << 6)
#define HDMI_VSDB_LATENCY (1 << 7)
static __u8 toggle_hf_vsdb_flags;
#define HF_VSDB_SCSD_PRESENT (1 << 7)
static int mod_s_pt = -1;
static int mod_s_it = -1;
static int mod_s_ce = -1;
static __u8 toggle_vid_cap_flags;
#define VID_CAP_QS (1 << 6)
#define VID_CAP_QY (1 << 7)
static __u8 toggle_colorimetry_flags1;
#define COLORIMETRY_XVYCC601 (1 << 0)
#define COLORIMETRY_XVYCC709 (1 << 1)
#define COLORIMETRY_SYCC (1 << 2)
#define COLORIMETRY_ADOBEYCC (1 << 3)
#define COLORIMETRY_ADOBERGB (1 << 4)
#define COLORIMETRY_BT2020CYCC (1 << 5)
#define COLORIMETRY_BT2020YCC (1 << 6)
#define COLORIMETRY_BT2020RGB (1 << 7)
static __u8 toggle_colorimetry_flags2;
#define COLORIMETRY_DCIP3 (1 << 0)
static __u8 toggle_hdr_md_flags;
#define HDR_MD_SDR (1 << 0)
#define HDR_MD_HDR (1 << 1)
#define HDR_MD_SMPTE_2084 (1 << 2)
#define HDR_MD_HLG (1 << 3)
static void edid_add_block(struct v4l2_edid *e)
{
e->blocks++;
if (e->blocks > 256) {
fprintf(stderr, "edid file error: too long\n");
free(e->edid);
e->edid = NULL;
exit(1);
}
e->edid = (unsigned char *)realloc(e->edid, e->blocks * 128);
}
static void read_edid_file(FILE *f, struct v4l2_edid *e)
{
char value[3] = { 0 };
unsigned i = 0;
int c;
fseek(f, SEEK_SET, 0);
e->edid = NULL;
e->blocks = 0;
while ((c = fgetc(f)) != EOF) {
if (sformat == RAW) {
if (i % 256 == 0)
edid_add_block(e);
e->edid[i / 2] = c;
i += 2;
continue;
}
/* Handle '0x' prefix */
if ((i & 1) && value[0] == '0' && (c == 'x' || c == 'X'))
i--;
if (!isxdigit(c))
continue;
if (i & 0x01) {
value[1] = c;
if (i % 256 == 1)
edid_add_block(e);
e->edid[i / 2] = strtoul(value, 0, 16);
} else {
value[0] = c;
}
i++;
}
}
static unsigned char crc_calc(const unsigned char *b)
{
unsigned char sum = 0;
int i;
for (i = 0; i < 127; i++)
sum += b[i];
return 256 - sum;
}
static bool crc_ok(const unsigned char *b)
{
return crc_calc(b) == b[127];
}
static void fix_edid(struct v4l2_edid *e)
{
for (unsigned b = 0; b < e->blocks; b++) {
unsigned char *buf = e->edid + 128 * b;
if (!crc_ok(buf))
buf[127] = crc_calc(buf);
}
}
static bool verify_edid(struct v4l2_edid *e)
{
bool valid = true;
for (unsigned b = 0; b < e->blocks; b++) {
const unsigned char *buf = e->edid + 128 * b;
if (!crc_ok(buf)) {
fprintf(stderr, "Block %u has a checksum error (should be 0x%02x)\n",
b, crc_calc(buf));
valid = false;
}
}
return valid;
}
static int get_edid_tag_location(const unsigned char *edid, unsigned size,
unsigned char want_tag, __u32 ext_tag)
{
unsigned char d;
if (size < 256)
return -1;
if (edid[0x7e] != 1 || edid[0x80] != 0x02 || edid[0x81] != 0x03)
return -1;
/* search tag */
d = edid[0x82] & 0x7f;
if (d <= 4)
return -1;
int i = 0x84;
int end = 0x80 + d;
do {
unsigned char tag = edid[i] >> 5;
unsigned char len = edid[i] & 0x1f;
if (tag != want_tag || i + len > end) {
i += len + 1;
continue;
}
/*
* Tag 3 (Vendor-Specific Data Block) has
* a 24 bit IEEE identifier.
*/
if (tag == VSDB_TAG && len >= 3 &&
edid[i + 1] == (ext_tag & 0xff) &&
edid[i + 2] == ((ext_tag >> 8) & 0xff) &&
edid[i + 3] == ((ext_tag >> 16) & 0xff))
return i;
/*
* Tag 7 has an extended tag, others (0-2, 4-6)
* have no identifiers.
*/
if ((tag < EXTENDED_TAG && tag != VSDB_TAG) ||
(tag == EXTENDED_TAG && len >= 1 && edid[i + 1] == ext_tag))
return i;
i += len + 1;
} while (i < end);
return -1;
}
static int get_edid_cta861_hdr_location(const unsigned char *edid, unsigned size)
{
if (size < 256)
return -1;
if (edid[0x7e] != 1 || edid[0x80] != 0x02 || edid[0x81] != 0x03)
return -1;
return 0x83;
}
static int get_edid_spa_location(const unsigned char *edid, unsigned size)
{
int loc = get_edid_tag_location(edid, size, VSDB_TAG, HDMI_VSDB_EXT_TAG);
if (loc < 0)
return loc;
return (edid[loc] & 0x1f) >= 5 ? loc + 4 : -1;
}
static int get_edid_hdmi_vsdb_location(const unsigned char *edid, unsigned size)
{
int loc = get_edid_tag_location(edid, size, VSDB_TAG, HDMI_VSDB_EXT_TAG);
if (loc < 0)
return loc;
return (edid[loc] & 0x1f) >= 5 ? loc : -1;
}
static int get_edid_hf_vsdb_location(const unsigned char *edid, unsigned size)
{
int loc = get_edid_tag_location(edid, size, VSDB_TAG, HF_VSDB_EXT_TAG);
if (loc < 0)
return loc;
return (edid[loc] & 0x1f) >= 6 ? loc + 5 : -1;
}
static int get_edid_speaker_location(const unsigned char *edid, unsigned size)
{
int loc = get_edid_tag_location(edid, size, SPEAKER_TAG, 0);
if (loc < 0)
return loc;
return (edid[loc] & 0x1f) >= 3 ? loc + 1 : -1;
}
static int get_edid_vid_cap_location(const unsigned char *edid, unsigned size)
{
int loc = get_edid_tag_location(edid, size, EXTENDED_TAG, VID_CAP_EXT_TAG);
if (loc < 0)
return loc;
return (edid[loc] & 0x1f) >= 2 ? loc + 2 : -1;
}
static int get_edid_colorimetry_location(const unsigned char *edid, unsigned size)
{
int loc = get_edid_tag_location(edid, size, EXTENDED_TAG, COLORIMETRY_EXT_TAG);
if (loc < 0)
return loc;
return (edid[loc] & 0x1f) >= 3 ? loc + 2 : -1;
}
static int get_edid_hdr_md_location(const unsigned char *edid, unsigned size)
{
int loc = get_edid_tag_location(edid, size, EXTENDED_TAG, HDR_MD_EXT_TAG);
if (loc < 0)
return loc;
return (edid[loc] & 0x1f) >= 3 ? loc + 2 : -1;
}
static void set_edid_phys_addr(unsigned char *edid, unsigned size, unsigned short phys_addr)
{
int loc = get_edid_spa_location(edid, size);
unsigned char sum = 0;
int i;
if (loc < 0)
return;
edid[loc] = phys_addr >> 8;
edid[loc + 1] = phys_addr & 0xff;
loc &= ~0x7f;
for (i = loc; i < loc + 127; i++)
sum += edid[i];
edid[i] = 256 - sum;
}
static unsigned short get_edid_phys_addr(const unsigned char *edid, unsigned size)
{
int loc = get_edid_spa_location(edid, size);
if (loc < 0)
return 0xffff;
return (edid[loc] << 8) | edid[loc + 1];
}
static unsigned short parse_phys_addr(const char *value)
{
unsigned p1, p2, p3, p4;
if (!strchr(value, '.'))
return strtoul(value, NULL, 0);
if (sscanf(value, "%x.%x.%x.%x", &p1, &p2, &p3, &p4) != 4) {
fprintf(stderr, "Expected a physical address of the form x.x.x.x\n");
return 0xffff;
}
if (p1 > 0xf || p2 > 0xf || p3 > 0xf || p4 > 0xf) {
fprintf(stderr, "Physical address components should never be larger than 0xf\n");
return 0xffff;
}
return (p1 << 12) | (p2 << 8) | (p3 << 4) | p4;
}
SThreadWatchVideoLink::SThreadWatchVideoLink():QSThread()
{
m_nCurrentState = 0;
FILE *fin = NULL;
bool must_fix_edid = true;
int loc = 0;
fin = fopen("/home/birdhead/test.edid", "r");
if (fin) {
read_edid_file(fin, &sedid);
if (sedid.blocks == 0) {
//Error!!!!
}
}
if (toggle_cta861_hdr_flags || phys_addr >= 0) {
loc = get_edid_cta861_hdr_location(sedid.edid, sedid.blocks * 128);
if (loc >= 0) {
sedid.edid[loc] ^= toggle_cta861_hdr_flags;
if (phys_addr >= 0)
set_edid_phys_addr(sedid.edid, sedid.blocks * 128, phys_addr);
must_fix_edid = true;
}
}
if (toggle_speaker1_flags || toggle_speaker2_flags || toggle_speaker3_flags) {
loc = get_edid_speaker_location(sedid.edid, sedid.blocks * 128);
if (loc >= 0) {
sedid.edid[loc] ^= toggle_speaker1_flags;
sedid.edid[loc + 1] ^= toggle_speaker2_flags;
sedid.edid[loc + 2] ^= toggle_speaker3_flags;
must_fix_edid = true;
}
}
if (toggle_hdmi_vsdb_dc_flags || toggle_hdmi_vsdb_cnc_flags) {
loc = get_edid_hdmi_vsdb_location(sedid.edid, sedid.blocks * 128);
if (loc >= 0) {
__u8 len = sedid.edid[loc] & 0x1f;
if (len >= 6) {
sedid.edid[loc + 6] ^= toggle_hdmi_vsdb_dc_flags;
must_fix_edid = true;
}
if (len >= 8) {
sedid.edid[loc + 8] ^= toggle_hdmi_vsdb_cnc_flags;
must_fix_edid = true;
}
}
}
if (toggle_hf_vsdb_flags) {
loc = get_edid_hf_vsdb_location(sedid.edid, sedid.blocks * 128);
if (loc >= 0) {
sedid.edid[loc + 1] ^= toggle_hf_vsdb_flags;
must_fix_edid = true;
}
}
if (toggle_vid_cap_flags || mod_s_pt >= 0 ||
mod_s_ce >= 0 || mod_s_it >= 0) {
loc = get_edid_vid_cap_location(sedid.edid, sedid.blocks * 128);
if (loc >= 0) {
sedid.edid[loc] ^= toggle_vid_cap_flags;
if (mod_s_ce >= 0) {
sedid.edid[loc] &= 0xfc;
sedid.edid[loc] |= mod_s_ce << 0;
}
if (mod_s_it >= 0) {
sedid.edid[loc] &= 0xf3;
sedid.edid[loc] |= mod_s_it << 2;
}
if (mod_s_pt >= 0) {
sedid.edid[loc] &= 0xcf;
sedid.edid[loc] |= mod_s_pt << 4;
}
must_fix_edid = true;
}
}
if (toggle_colorimetry_flags1 || toggle_colorimetry_flags2) {
loc = get_edid_colorimetry_location(sedid.edid, sedid.blocks * 128);
if (loc >= 0) {
sedid.edid[loc] ^= toggle_colorimetry_flags1;
sedid.edid[loc + 1] ^= toggle_colorimetry_flags2;
must_fix_edid = true;
}
}
if (toggle_hdr_md_flags) {
loc = get_edid_hdr_md_location(sedid.edid, sedid.blocks * 128);
if (loc >= 0) {
sedid.edid[loc] ^= toggle_hdr_md_flags;
must_fix_edid = true;
}
}
if (must_fix_edid)
fix_edid(&sedid);
//print_edid_mods(&sedid);
if (verify_edid(&sedid))
{
//doioctl(fd, VIDIOC_S_EDID, &sedid);
}
else
{
fprintf(stderr, "EDID not set due to checksum errors\n");
}
if (fin) {
fclose(fin);
}
}
SThreadWatchVideoLink::~SThreadWatchVideoLink()
{
if (sedid.edid) {
free(sedid.edid);
sedid.edid = NULL;
}
ExitThread();
}
void SThreadWatchVideoLink::Init()
{
#ifdef _PC
m_nCurrentState = 0x00;
#else
//m_nCurrentState = 0x00;
m_nCurrentState = 0x01;
#endif
m_TimerEDID.restart();
}
void SThreadWatchVideoLink::InnerRun()
{
if(m_nCurrentState&0x01)
{
WatchVideoConnect();
usleep(100*1000);
}
else
{
//usleep(100*1000);
usleep(200*1000);
}
}
int SThreadWatchVideoLink::WatchVideoConnect()
{
int nCaptureMode = 0;
CommonData* pCommonData = MainWindow::GetCommonData();
if(pCommonData==NULL)
{
return -1;
}
if(m_pThreadRTSP!=NULL)
{
nCaptureMode = m_pThreadRTSP->GetCaptureMode();
}
std::string strDevice = "/dev/video0";
int fd = open_device(strDevice);
int input = 1;
bool bConnectHDMI = false;
if (xioctl(fd, VIDIOC_G_INPUT, &input) == 0) {
struct v4l2_input vin;
//printf("Video input set to %d", input);
vin.index = input;
if (xioctl(fd, VIDIOC_ENUMINPUT, &vin) >= 0)
{
//printf(" (%s)", vin.name);
}
//printf("\n");
close(fd);
bool bPrevIsConnectHDMI = false;
bPrevIsConnectHDMI = pCommonData->IsConnectHDMI();
if(vin.status & V4L2_IN_ST_NO_SIGNAL)
{
bool bExecStop = false;
if(m_nRTSPStatus==1)
{
bExecStop = true;
m_nRTSPStatus = 0; // Stop RTSP
}
if(m_pThreadRTSP!=NULL)
{
if(m_pThreadRTSP->GetState()==0)
{
if(bExecStop==true)
{
//qDebug() << "StopRTSP!!!!!!!!!!!!!!!!!";
//m_pThreadRTSP->StopRTSP();
}
}
}
if(bPrevIsConnectHDMI==true)
{
//HDMICalbeConnect(false);
//pCommonData->SetConnectHDMI(false);
m_TimerEDID.restart();
}
else
{
//HDMICalbeConnect(true);
if(m_TimerEDID.elapsed()>= 360*1000) //360 seconds
{
//QProcess::execute("/home/birdhead/edid.sh");
m_TimerEDID.restart();
}
}
}
else
{
/*
bool bExecStart = false;
if(m_nRTSPStatus==0)
{
bExecStart = true;
m_nRTSPStatus = 1; //Playing
}
if(m_pThreadRTSP!=NULL)
{
if(m_pThreadRTSP->GetState()==-1)
{
m_nCaptureMode = m_pThreadRTSP->GetCaptureMode();
m_pThreadRTSP->ChangeState(0);
}
else if(m_pThreadRTSP->GetState()==11 || bExecStart==true)
{
m_nCaptureMode = m_pThreadRTSP->GetCaptureMode();
m_pThreadRTSP->ChangeState(0);
}
}
*/
if(bPrevIsConnectHDMI==false)
{
//HDMICalbeConnect(true);
//pCommonData->SetConnectHDMI(true);
m_TimerEDID.restart();
}
bConnectHDMI = true;
}
}
else
{
close(fd);
}
m_Lock.lock();
m_bConnectHDMI = bConnectHDMI;
m_Lock.unlock();
return 0;
}
bool SThreadWatchVideoLink::IsConnectHDMI()
{
bool bConnectHDMI = false;
m_Lock.lock();
bConnectHDMI = m_bConnectHDMI;
//bConnectHDMI = true;
m_Lock.unlock();
return bConnectHDMI;
}
int SThreadWatchVideoLink::open_device(std::string strDevice)
{
struct stat st;
int fd = 0;
if (-1 == stat(strDevice.c_str(), &st)) {
return -1;
}
if (!S_ISCHR(st.st_mode)) {
return -2;
}
fd = open(strDevice.c_str(), O_RDWR /* required */ | O_NONBLOCK, 0);
if (-1 == fd) {
return -3;
}
return fd;
}
int SThreadWatchVideoLink::xioctl(int fh, unsigned long int request, void *arg)
{
int r = 0;
int nCount = 0;
do {
r = ioctl(fh, request, arg);
nCount++;
} while (-1 == r && EINTR == errno && nCount<50);
if(nCount>=50)
{
r = -1;
}
return r;
}
void SThreadWatchVideoLink::SetThreadRTSP(QRTSPThread* pThread)
{
m_pThreadRTSP = pThread;
}