modularization/src/driver/driver_can_nvidia_agx/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>

/* 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()
{
//    qDebug("nvcan");
//    connect(this,SIGNAL(SIG_CANOPENSTATE(bool,int,const char*)),this,SLOT(onMsg(bool,int,const char*)));

    mfault = new iv::Ivfault("can_agx");
    mivlog = new iv::Ivlog("can_agx");


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


}

void nvcan::run()
{

    int currmax = 2;
    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;
        }
    }

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

    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;

    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 = 100;;
        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.");
            mbCANOpen = false;
            continue;
        }

        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;

                nbytes = recvmsg(s[i], &msg, 0);

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

                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;
                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();

            }
        }

        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.");
            }
        }

        struct canfd_frame framesend[2500];

        for(int nch =0;nch<currmax;nch++)
        {
            int nsend = 0;
            mMutex.lock();
            for(i=0;i<mMsgSendBuf[nch].size();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;

                nsend++;
            }
            mMsgSendBuf[nch].clear();
            mMutex.unlock();
            if(nsend > 0)
            {
                for(i=0;i<nsend;i++)
                if (write(s[nch], &framesend[i],16) != 16) {
                    mivlog->error("write error 1");
                    perror("write error 1.");
                    continue;
                }
            }

        }

    }

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

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;

    if(mMsgSendBuf[nch].size() > BUF_SIZE)return -2;

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

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