modularization/src/driver/driver_can_socket/nvcan.cpp

#include "nvcan.h"

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <unistd.h>
#include <string.h>
#include <signal.h>
#include <ctype.h>
#include <libgen.h>
#include <time.h>
#include <errno.h>

#include <sys/time.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <sys/uio.h>
#include <net/if.h>


#include <linux/can.h>
#include <linux/can/raw.h>

#include <QDateTime>

#include <iostream>
#include  <thread>

/* for hardware timestamps - since Linux 2.6.30 */
#ifndef SO_TIMESTAMPING
#define SO_TIMESTAMPING 37
#endif

/* from #include <linux/net_tstamp.h> - since Linux 2.6.30 */
#define SOF_TIMESTAMPING_SOFTWARE (1<<4)
#define SOF_TIMESTAMPING_RX_SOFTWARE (1<<3)
#define SOF_TIMESTAMPING_RAW_HARDWARE (1<<6)

#define MAXSOCK 16    /* max. number of CAN interfaces given on the cmdline */
#define MAXIFNAMES 30 /* size of receive name index to omit ioctls */
#define MAXCOL 6      /* number of different colors for colorized output */
#define ANYDEV "any"  /* name of interface to receive from any CAN interface */
#define ANL "\r\n"    /* newline in ASC mode */

#define SILENT_INI 42 /* detect user setting on commandline */
#define SILENT_OFF 0  /* no silent mode */
#define SILENT_ANI 1  /* silent mode with animation */
#define SILENT_ON  2  /* silent mode (completely silent) */

#include <QTime>

#define BUF_SIZE 1000

std::string CANNAME[] = {"can0","can1"};

nvcan::nvcan(const char * strcanname)
{
//    qDebug("nvcan");
//    connect(this,SIGNAL(SIG_CANOPENSTATE(bool,int,const char*)),this,SLOT(onMsg(bool,int,const char*)));

    CANNAME[0] = strcanname;
    mfault = new iv::Ivfault("can_socket");
    mivlog = new iv::Ivlog("can_socket");


    mfault->SetFaultState(0,0,"Prepare Initialize.");

    mpsendthread = new std::thread(&nvcan::threadsend,this);


}

void nvcan::ExecRecv(int s)
{

}

void nvcan::run()
{

    int currmax = 1;
    fd_set rdfs;
    int s[MAXSOCK];
    int ret;
    
    struct sockaddr_can addr;
    char ctrlmsg[CMSG_SPACE(sizeof(struct timeval) + 3*sizeof(struct timespec) + sizeof(__u32))];
    struct iovec iov;
    struct msghdr msg;
    struct canfd_frame frame;
    int nbytes, i, maxdlen;
    struct ifreq ifr;
    struct timeval tv, last_tv;
    struct timeval timeout_config = { 0, 0 }, *timeout_current = 0;

    mfault->SetFaultState(0,0,"Initializing.");
    
    for(i=0;i<currmax;i++)
    {
        s[i] = socket(PF_CAN, SOCK_RAW, CAN_RAW);
        if (s[i] < 0) {
            mfault->SetFaultState(2,1,"Create Socket Error.");
            emit SIG_CANOPENSTATE(false,-1,"Create Socket Error");
            return;
        }

        addr.can_family = AF_CAN;

        memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
        strncpy(ifr.ifr_name, CANNAME[i].data(), 5);

        if (ioctl(s[i], SIOCGIFINDEX, &ifr) < 0) {
            mfault->SetFaultState(2,2,"SIOCGIFINDEX.");
            emit SIG_CANOPENSTATE(false,-2,"SIOCGIFINDEX");
            return;
        }
        addr.can_ifindex = ifr.ifr_ifindex;

        if (bind(s[i], (struct sockaddr *)&addr, sizeof(addr)) < 0) {
            mfault->SetFaultState(2,3,"bind error.");
            emit SIG_CANOPENSTATE(false,-3,"bind error");
            return;
        }
    }

    mps = &s[0];
    mbCANOpen = true;
    mivlog->verbose("open can succesfully.");
    mfault->SetFaultState(0,0,"CAN OK.");
    emit SIG_CANOPENSTATE(true,0,"open can card successfully");

    std::cout<<"can open succesfully."<<std::endl;

    iov.iov_base = &frame;
    msg.msg_name = &addr;
    msg.msg_iov = &iov;
    msg.msg_iovlen = 1;
    msg.msg_control = &ctrlmsg;

    qint64 nLastRecv = QDateTime::currentMSecsSinceEpoch();
    int nRecvState = 0; // 0 Have Data  1 No Data;

    mbRunning = true;

    int nrecvcount = 0;

    qint64 nlastsecond = 0;
    int nsecondrecvcount = 0;

    qint64 nLastSecond = 0;
    int nsecondsend = 0;
    int nretry = 0;
#ifdef SEND_STAT
    std::vector<qint64> xvectorlat;
#endif
    int secondretrycount = 0;

    while((!QThread::isInterruptionRequested())&&(mbCANOpen))
    {
        FD_ZERO(&rdfs);
        for (i=0; i<currmax; i++)
            FD_SET(s[i], &rdfs);

        if (timeout_current)
            *timeout_current = timeout_config;

        timeout_config.tv_sec= 0;
        timeout_config.tv_usec = 0;;
        timeout_current = &timeout_config;
        ret = select(s[currmax-1]+1, &rdfs, NULL, NULL, timeout_current);
        if (ret < 0) {
            emit SIG_CANOPENSTATE(false,-4,"select error");
            mfault->SetFaultState(2,4,"select error.");
            std::cout<<"select error."<<std::endl;
            mbCANOpen = false;
            continue;
        }

 //       std::cout<<"time: "<<QDateTime::currentMSecsSinceEpoch()<<" ret : "<<ret<<std::endl;

        bool bRecv = false;
        for (i=0; i<currmax; i++) {  /* check all CAN RAW sockets */

            if (FD_ISSET(s[i], &rdfs)) {

                nLastRecv = QDateTime::currentMSecsSinceEpoch();
                /* these settings may be modified by recvmsg() */
                iov.iov_len = sizeof(frame);
                msg.msg_namelen = sizeof(addr);
                msg.msg_controllen = sizeof(ctrlmsg);
                msg.msg_flags = 0;

                mMutexRW.lock();
                nbytes = recvmsg(s[i], &msg, 0);
                mMutexRW.unlock();

                if (nbytes < 0) {
 //                   if ((errno == ENETDOWN) && !down_causes_exit) {
                    if ((errno == ENETDOWN)) {
                        mivlog->error("%s interface down", CANNAME[i].data());
                        mfault->SetFaultState(1, 0, "interface down");
                        emit SIG_CANOPENSTATE(false,-5,"can card down");
                        fprintf(stderr, "%s: interface down\n", CANNAME[i].data());
                        return;
                    }
                    continue;
//                    perror("read");
//                    return 1;
                }

                if ((size_t)nbytes == CAN_MTU)
                    maxdlen = CAN_MAX_DLEN;
                else if ((size_t)nbytes == CANFD_MTU)
                    maxdlen = CANFD_MAX_DLEN;
                else {
                    mivlog->warn("read incomplete message");
                    continue;
                }

                bRecv = true;
                nrecvcount++;
 //               qDebug("receive msg.");
                mMutex.lock();

                basecan_msg msg;
                msg.id = frame.can_id&0x1fffffff;
                if((frame.can_id&0x80000000)!= 0)msg.isExtern = true;
                else msg.isExtern = false;
                if((frame.can_id&0x40000000)!= 0)msg.isRemote = true;
                else msg.isRemote = false;
                msg.nLen = frame.len;
                nsecondrecvcount++;
                if((frame.len<9)&&(frame.len>0))memcpy(msg.data,frame.data,frame.len);
                if(mMsgRecvBuf[i].size()<BUF_SIZE)
                {
                    mMsgRecvBuf[i].push_back(msg);
                }

                mMutex.unlock();



            }
        }

        qint64 nsecondnow = QDateTime::currentSecsSinceEpoch();
        if(nlastsecond != nsecondnow)
        {
            nlastsecond = nsecondnow;
            std::cout<<"second recv count:"<<nsecondrecvcount<<std::endl;
            nsecondrecvcount = 0;
        }

        if((QDateTime::currentMSecsSinceEpoch() - nLastRecv)> 1000)
        {
            if(nRecvState == 0)
            {
                nRecvState = -1;
                mfault->SetFaultState(0,1,"More than 1 second not receive data.");
            }
        }
        else
        {
            if(nRecvState == -1)
            {
                nRecvState = 0;
                mfault->SetFaultState(0,0,"CAN OK.");
            }
        }

        if(bRecv)continue;


        mWaitMutex.lock();
        mwc.wait(&mWaitMutex,1);
        mWaitMutex.unlock();

#ifdef TEST_PROG
 //       qDebug("send time : %lld",QDateTime::currentMSecsSinceEpoch());
#endif

        struct canfd_frame framesend[2500];
#ifdef SEND_STAT
        qint64 sendsettime[2500];
#endif

        for(int nch =0;nch<currmax;nch++)
        {
            int nsend = 0;
            mMutex.lock();
            int nbufsize = mMsgSendBuf[nch].size();
            if(nbufsize>2500)nbufsize = 2500;
            for(i=0;i<nbufsize;i++)
            {
                if(i>=2500)break;
                memcpy(framesend[i].data,mMsgSendBuf[nch].at(i).data,8);
                framesend[i].can_id = mMsgSendBuf[nch].at(i).id;
                if(mMsgSendBuf[nch].at(i).isExtern)
                {
                    framesend[i].can_id = framesend[i].can_id|0x80000000;
                }
                else
                {
                    framesend[i].can_id = framesend[i].can_id&0x7ff;
                }
                if(mMsgSendBuf[nch].at(i).isRemote)
                {
                    framesend[i].can_id= framesend[i].can_id|0x40000000;
                }

                framesend[i].len = mMsgSendBuf[nch].at(i).nLen;
#ifdef SEND_STAT
                sendsettime[i] = mMsgSendBuf[nch].at(i).mSetTime;
#endif

                nsend++;
            }
            mMsgSendBuf[nch].clear();
            mMutex.unlock();

            if(nsend > 0)
            {
                for(i=0;i<nsend;i++)
                {
                    mMutexRW.lock();

                    if (write(mps[nch], &framesend[i],16) != 16) {
                        mMutexRW.unlock();
                        mivlog->error("write error 1");
 //                       perror("write error 1.");

                        nretry++;
                        secondretrycount++;
                        if(nretry > 5)
                        {
 //                           std::cout<<"retry fail,retry:"<<nretry<<std::endl;
                        }
                        else
                        {


                        }
                        if(nretry < 5)
                        {
                            i--;
                        }
                        else
                        {
                            nretry = 0;
//                            std::cout<<"retry more than 100. drop this message."<<std::endl;
                        }
                         std::this_thread::sleep_for(std::chrono::microseconds(100));
 //                       std::cout<<"retry send."<<std::endl;
                        continue;
                    }
                    else
                    {
                        mMutexRW.unlock();
#ifdef SEND_STAT
                        qint64 nnowms = QDateTime::currentMSecsSinceEpoch();
                        qint64 nlat = nnowms - sendsettime[i];
                        xvectorlat.push_back(nlat);
                        nretry = 0;
                        nsecondsend++;
#endif
                    }
                }
            }

                        qint64 nnowsecond = QDateTime::currentSecsSinceEpoch();
                        if( nnowsecond != nLastSecond)
                        {
                            nLastSecond = nnowsecond;
                            std::cout<<" second send count: "<<nsecondsend<<std::endl;
                            nsecondsend = 0;
#ifdef SEND_STAT
                            int j;
                            int nsendcount = xvectorlat.size();
                            if(nsendcount > 0)
                            {
                                qint64 xlatmax = 0;
                                qint64 xlatavg = 0;
                                for(j=0;j<nsendcount;j++)
                                {
                                    if(xvectorlat[j]> xlatmax)xlatmax = xvectorlat[j];
                                    xlatavg = xlatavg + xvectorlat[j];
                                }
                                xlatavg = xlatavg/nsendcount;
                                std::cout<<" max latency: "<<xlatmax<<" avg latency: "<<xlatavg
                                        <<" second retry count:"<<secondretrycount<<std::endl;


                                xvectorlat.clear();
                            }
#endif
                            secondretrycount = 0;
                        }


        }



    }

    for (i=0; i<currmax; i++)
    {
        close(s[i]);
    }
    qDebug("nvcan thread close.");
    mbRunning = false;
}

void nvcan::threadsend()
{

    return;
    int currmax = 1;
    int s[MAXSOCK];
    int i;

    struct sockaddr_can addr;
    struct ifreq ifr;

    for(i=0;i<currmax;i++)
    {
        s[i] = socket(PF_CAN, SOCK_RAW, CAN_RAW);
        if (s[i] < 0) {
            return;
        }

        addr.can_family = AF_CAN;

        memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
        strncpy(ifr.ifr_name, CANNAME[i].data(), 5);

        if (ioctl(s[i], SIOCGIFINDEX, &ifr) < 0) {
            return;
        }
        addr.can_ifindex = ifr.ifr_ifindex;

        if (bind(s[i], (struct sockaddr *)&addr, sizeof(addr)) < 0) {
            return;
        }
    }

    std::cout<<"threadsend open can success."<<std::endl;

    mps = &s[0];

 //   int currmax = 1;
 //   int i;
    while(mbCANOpen == false)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }

    qint64 nLastSecond = 0;
    int nsecondsend = 0;
    int nretry = 0;
#ifdef SEND_STAT
    std::vector<qint64> xvectorlat;
#endif
    int secondretrycount = 0;
    while(mbSendRun)
    {
        mWaitMutex.lock();
        mwc.wait(&mWaitMutex,100);
        mWaitMutex.unlock();

#ifdef TEST_PROG
 //       qDebug("send time : %lld",QDateTime::currentMSecsSinceEpoch());
#endif

        struct canfd_frame framesend[2500];
#ifdef SEND_STAT
        qint64 sendsettime[2500];
#endif

        for(int nch =0;nch<currmax;nch++)
        {
            int nsend = 0;
            mMutex.lock();
            int nbufsize = mMsgSendBuf[nch].size();
            if(nbufsize>2500)nbufsize = 2500;
            for(i=0;i<nbufsize;i++)
            {
                if(i>=2500)break;
                memcpy(framesend[i].data,mMsgSendBuf[nch].at(i).data,8);
                framesend[i].can_id = mMsgSendBuf[nch].at(i).id;
                if(mMsgSendBuf[nch].at(i).isExtern)
                {
                    framesend[i].can_id = framesend[i].can_id|0x80000000;
                }
                else
                {
                    framesend[i].can_id = framesend[i].can_id&0x7ff;
                }
                if(mMsgSendBuf[nch].at(i).isRemote)
                {
                    framesend[i].can_id= framesend[i].can_id|0x40000000;
                }

                framesend[i].len = mMsgSendBuf[nch].at(i).nLen;
#ifdef SEND_STAT
                sendsettime[i] = mMsgSendBuf[nch].at(i).mSetTime;
#endif

                nsend++;
            }
            mMsgSendBuf[nch].clear();
            mMutex.unlock();

            if(nsend > 0)
            {
                for(i=0;i<nsend;i++)
                {
                    mMutexRW.lock();

                    if (write(mps[nch], &framesend[i],16) != 16) {
                        mMutexRW.unlock();
                        mivlog->error("write error 1");
 //                       perror("write error 1.");

                        nretry++;
                        secondretrycount++;
                        if(nretry > 30)
                        {
 //                           std::cout<<"retry fail,retry:"<<nretry<<std::endl;
                        }
                        else
                        {


                        }
                        if(nretry < 100)
                        {
                            i--;
                        }
                        else
                        {
                            nretry = 0;
//                            std::cout<<"retry more than 100. drop this message."<<std::endl;
                        }
                         std::this_thread::sleep_for(std::chrono::microseconds(100));
 //                       std::cout<<"retry send."<<std::endl;
                        continue;
                    }
                    else
                    {
                        mMutexRW.unlock();
#ifdef SEND_STAT
                        qint64 nnowms = QDateTime::currentMSecsSinceEpoch();
                        qint64 nlat = nnowms - sendsettime[i];
                        xvectorlat.push_back(nlat);
                        nretry = 0;
                        nsecondsend++;
#endif
                    }
                }
            }

                        qint64 nnowsecond = QDateTime::currentSecsSinceEpoch();
                        if( nnowsecond != nLastSecond)
                        {
                            nLastSecond = nnowsecond;
                            std::cout<<" second send count: "<<nsecondsend<<std::endl;
                            nsecondsend = 0;
#ifdef SEND_STAT
                            int j;
                            int nsendcount = xvectorlat.size();
                            if(nsendcount > 0)
                            {
                                qint64 xlatmax = 0;
                                qint64 xlatavg = 0;
                                for(j=0;j<nsendcount;j++)
                                {
                                    if(xvectorlat[j]> xlatmax)xlatmax = xvectorlat[j];
                                    xlatavg = xlatavg + xvectorlat[j];
                                }
                                xlatavg = xlatavg/nsendcount;
                                std::cout<<" max latency: "<<xlatmax<<" avg latency: "<<xlatavg
                                        <<" second retry count:"<<secondretrycount<<std::endl;


                                xvectorlat.clear();
                            }
#endif
                            secondretrycount = 0;
                        }


        }
    }

    std::cout<<"nvcan::threadsend exit."<<std::endl;

}

void nvcan::startdev()
{
    start();
}

void nvcan::stopdev()
{
    requestInterruption();
    QTime xTime;
    xTime.start();
    while(xTime.elapsed()<100)
    {
        if(mbRunning == false)
        {
            mfault->SetFaultState(1, 0, "can closed");
            mivlog->error("can is closed at %d",xTime.elapsed());
            qDebug("can is closed.");
            break;
        }
    }
}

int nvcan::GetMessage(const int nch,basecan_msg *pMsg, const int nCap)
{
    if((nch>1)||(nch < 0))return -1;
    if(mMsgRecvBuf[nch].size() == 0)return 0;

    int nRtn;
    nRtn = nCap;
    mMutex.lock();
    if(nRtn > mMsgRecvBuf[nch].size())nRtn = mMsgRecvBuf[nch].size();
    int i;
    for(i=0;i<nRtn;i++)
    {
        memcpy(&pMsg[i],&(mMsgRecvBuf[nch].at(i)),sizeof(basecan_msg));
    }

    std::vector<basecan_msg>::iterator iter;
    iter = mMsgRecvBuf[nch].begin();
    for(i=0;i<nRtn;i++)
    {
        iter = mMsgRecvBuf[nch].erase(iter);
    }

    mMutex.unlock();

    return nRtn;

}

int nvcan::SetMessage(const int nch, basecan_msg *pMsg)
{
    if((nch>1)||(nch < 0))return -1;

    mMutex.lock();
    if(mMsgSendBuf[nch].size() > BUF_SIZE)
    {
        std::cout<<"buffer full."<<std::endl;
        mMutex.unlock();
        return -2;
    }

    if(mMsgRecvBuf[nch].size() > 100)
    {
        std::cout<<"buffer data more 100"<<std::endl;
    }


    mMsgSendBuf[nch].push_back(*pMsg);
    mMutex.unlock();
    return 0;
}

void nvcan::CmdSend()
{
    mwc.wakeAll();
}

void nvcan::onMsg(bool bCAN, int nR, const char *strres)
{
    mivlog->verbose("msg is %s ",strres);
}