Demo entry 6850839

Qifeng Wang

   

Submitted by anonymous on Jul 11, 2019 at 22:44
Language: ABAP. Code size: 41.8 kB.

/**********************************************************************************************************************/
/**
 * @file            trdp_can_gw.c
 *
 * @brief           Demo application for CAN to TRDP Gateway
 *
 *
 * @author          qifeng wang
 *

 *                  2019.04.23
 */

/***********************************************************************************************************************
 * INCLUDES
 */
/***** realtime *****/
 #include <sched.h>    //linux scheduler
 #include <sys/mman.h> // memory lock


/***** TRDP INCLUDES *****/
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <sys/time.h>
#include "trdp_if_light.h"

#include "vos_thread.h"
#include "vos_utils.h"

#if defined (POSIX)
#include <unistd.h>
#include <sys/select.h>
#elif (defined (WIN32) || defined (WIN64))
#include "getopt.h"
#endif

/***** SOCKETCAN INCLUDES  *****/
#include <signal.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <pthread.h>
#include <sys/ipc.h>
#include <sys/msg.h>

/***********************************************************************************************************************
 * DEFINITIONS
 */
 #define DATA_MAX            1432u
 #define RESERVED_MEMORY     160000u   //dynamic memory for TRDP
 #define MAX_SAFE_STACK (8*1024)
 #define MY_PRIO  99

/***********************************************************************************************************************
 * GLOBAL VARIABLES
 */



/***** TRDP GLOBAL VARIABLES *****/

/*data Buffer*/
UINT8 TRDPSendBuffer[DATA_MAX];
CHAR8 TRDPRecvBuffer[DATA_MAX];

TRDP_APP_SESSION_T appHandle;  /*    Our identifier to the library instance    */
TRDP_PUB_T pubHandle;          /*    Our identifier to the publication         */
TRDP_SUB_T subHandle;          /*    Our identifier to the subscription    */
TRDP_ERR_T err;
TRDP_PD_CONFIG_T pdConfiguration =
{NULL, NULL, {0u, 64u, 0u}, TRDP_FLAGS_NONE, 1000000u, TRDP_TO_SET_TO_ZERO, 0};
TRDP_MEM_CONFIG_T dynamicConfig   = {NULL, RESERVED_MEMORY, {0}};
TRDP_PROCESS_CONFIG_T processConfig   = {"Me", "", 0, 0, TRDP_OPTION_BLOCK};
UINT32 ownIP           = 0u;
UINT32 destIP_Subscribe   = 0u;
int rv = 0;
TRDP_PD_INFO_T myPDInfo;

/* structure for Parame_For_TRDP */
struct Parame_For_TRDP
{
        UINT32 destIP_publish;
        UINT32 PD_COMID_SEND;
        UINT32 PD_COMID_REC;
        UINT32 PD_COMID_CYCLE;
        int framesAmountInPD_Threshold;  //frames amount in one PD paket
        UINT32 time_Threshold;  //ms  wait time for pd send
};

/***** SOCKETCAN GLOBAL VARIABLES *****/
char CAN_IFIN[10];
int s_in;             // raw CAN input
int nbytes_in;
struct sockaddr_can addr_in;
struct can_frame frame;
struct iovec iov;
struct msghdr msg;

/* recev CAN frames buffer */
struct can_frame *CANRecvBuffer;  // buffer for recev CAN frames
int CANRecvBuffer_length=100;
int CANRecvBuffer_load=0;
int CANRecvBuffer_begin_idx=0,CANRecvBuffer_end_idx=0;

pthread_mutex_t mutex_buffer;  //thread mutual exclusion
pthread_t CAN_Recv_Thread, TRDP_Thread, CAN_Send_Thread;  //threads

char CAN_IFOUT[10];   // raw CAN output
int s_out;
int nbytes_out;
struct sockaddr_can addr_out;

/* send CAN frames buffer */
struct can_frame *CANSendBuffer;  // buffer for CAN frames
int CANSendBuffer_length=100;
int CANSendBuffer_load=0;
int CANSendBuffer_begin_idx=0,CANSendBuffer_end_idx=0;


/***********************************************************************************************************************
 * FUNCTIONS PROTOTYPES
 */

void TRDP_DbgOut (
        void        *pRefCon,
        TRDP_LOG_T category,
        const CHAR8 *pTime,
        const CHAR8 *pFile,
        UINT16 LineNumber,
        const CHAR8 *pMsgStr);

void setPrio();
void MemLock();
void stack_prefault();

/* functiong for data*/
void addProcessMessage(int framesAmount_inPD);
UINT8 addFrameIn_CANSendBuffer(int pisiton_CANSendBuffer,UINT8 *uint8_dataArrayTemp,UINT8 position_uint8_dataArrayTemp);
void readProcessMessage();
void printTRDPReceivedMessage();

void usage (const char *appName);
void intHandler(int dummy);
void dummyIntHandler(int dummy);

/* initial functions */
void initInputSocketCAN(char *CAN_IFIN,int can_id,int can_mask);
void initOutputSocketCAN(char *CAN_IFOUT);
void initTRDP();

/* Functions: SocketCAN threads cleanup */
void cleanup_CAN_recv(void *dummy);
void cleanup_CAN_send(void *dummy);
void cleanup_TRDP(void *dummy);

/* Thread Functions*/
void *sendFramestoCAN(void *dummy);
void *recvFramesFromCAN(void *dummy);
void *recvANDsendFramesInTRDP(void *dummy);


/***********************************************************************************************************************
 * Main FUNCTION
 */

int main (int argc, char *argv[])
{

        /* settings for RT conformity */
        // setPrio();
        // MemLock();
        // stack_prefault();

        int can_id = 0x000;
        int can_mask=0x000;
        UINT32 destIP_publish           = 0u;
        UINT32 comId_send       = 0u;
        UINT32 comId_rec       = 0u;
        UINT32 PD_cycleTime   = 1000000u;
        int framesAmountInPD_Threshold =3;
        UINT32 time_Threshold  = 1000;      //ms
        int ch;
        unsigned int ip[4];

        if (argc <= 1)
        {
                usage(argv[0]);
                return 1;
        }
        while ((ch = getopt(argc, argv, "t:c:r:s:f:w:b:d:i:m:")) != -1)
        {
                switch (ch)
                {
                case 't':
                { /*  read destIP_publish    */
                        if (sscanf(optarg, "%u.%u.%u.%u",
                                   &ip[3], &ip[2], &ip[1], &ip[0]) < 4)
                        {
                                usage(argv[0]);
                                exit(1);
                        }
                        destIP_publish = (ip[3] << 24) | (ip[2] << 16) | (ip[1] << 8) | ip[0];
                        break;
                }
                case 'c':
                { /*  read comId_send    */
                        if (sscanf(optarg, "%u",
                                   &comId_send) < 1)
                        {
                                usage(argv[0]);
                                exit(1);
                        }
                        break;
                }
                case 'r':
                { /*  read comId_rec    */
                        if (sscanf(optarg, "%u",
                                   &comId_rec) < 1)
                        {
                                usage(argv[0]);
                                exit(1);
                        }
                        break;
                }
                case 's':
                { /*  read cycle time    */
                        if (sscanf(optarg, "%u",
                                   &PD_cycleTime) < 1)
                        {
                                usage(argv[0]);
                                exit(1);
                        }
                        break;
                }
                case 'f':
                { /*  read frames amount in one PD Paket    */
                        if (sscanf(optarg, "%u",
                                   &framesAmountInPD_Threshold) < 1)
                        {
                                usage(argv[0]);
                                exit(1);
                        }
                        break;
                }
                case 'w':
                { /*  read max wait time for send PD Paket    */
                        if (sscanf(optarg, "%u",
                                   &time_Threshold) < 1)
                        {
                                usage(argv[0]);
                                exit(1);
                        }
                        break;
                }
                case 'b':
                { /*  data    */
                        char c;
                        UINT32 dataSize = 0u;
                        do
                        {
                                c = optarg[dataSize];
                                dataSize++;
                        }
                        while (c != '\0');


                        memcpy(CAN_IFIN, optarg, dataSize);
                        break;
                }
                case 'd':
                { /*  data    */
                        char c;
                        UINT32 dataSize = 0u;
                        do
                        {
                                c = optarg[dataSize];
                                dataSize++;
                        }
                        while (c != '\0');


                        memcpy(CAN_IFOUT, optarg, dataSize);
                        break;
                }
                case 'i':
                { /*  read max wait time for send PD Paket    */
                        if (sscanf(optarg, "%X",
                                   &can_id) < 1)
                        {
                                usage(argv[0]);
                                exit(1);
                        }
                        break;
                }
                case 'm':
                { /*  read max wait time for send PD Paket    */
                        if (sscanf(optarg, "%X",
                                   &can_mask) < 1)
                        {
                                usage(argv[0]);
                                exit(1);
                        }
                        break;
                }
                default:
                        usage(argv[0]);
                        return 1;
                }
        }


        struct Parame_For_TRDP parameterStructForTRDP;
        parameterStructForTRDP.destIP_publish=destIP_publish;
        parameterStructForTRDP.PD_COMID_SEND=comId_send;
        parameterStructForTRDP.PD_COMID_REC=comId_rec;
        parameterStructForTRDP.PD_COMID_CYCLE=PD_cycleTime; /* in us (1000000 = 1 sec) */
        parameterStructForTRDP.framesAmountInPD_Threshold=framesAmountInPD_Threshold; //how many frames in one PD paket
        parameterStructForTRDP.time_Threshold=  time_Threshold;                                   //1000ms


        /* set up interrupt for Ctrl-C */
        signal(SIGINT, intHandler);


        /* initialize sockets */
        initInputSocketCAN(CAN_IFIN,can_id,can_mask);
        initOutputSocketCAN(CAN_IFOUT);
        initTRDP();


        /* initialize buffer and mutex */
        CANRecvBuffer = malloc(CANRecvBuffer_length * sizeof(*CANRecvBuffer));
        CANSendBuffer= malloc(CANSendBuffer_length * sizeof(*CANSendBuffer));

        /**/
        pthread_mutex_init(&mutex_buffer, NULL);


        /* start threads for forwarding of Recevie CAN frames and send CAN frames to TRDP */
        pthread_create(&CAN_Recv_Thread, NULL, recvFramesFromCAN,NULL);
        pthread_create(&CAN_Send_Thread, NULL, sendFramestoCAN,NULL);
        pthread_create(&TRDP_Thread, NULL, recvANDsendFramesInTRDP, &parameterStructForTRDP);


        /* wait for the user to cancel the transmission by ctrl-C */
        pthread_join(CAN_Recv_Thread,NULL);
        pthread_join(CAN_Send_Thread,NULL);
        pthread_join(TRDP_Thread,NULL);


        /* clean up */
        free(CANRecvBuffer);
        free(CANSendBuffer);


        return 0;
}



/***********************************************************************************************************************
 * FUNCTIONS
 */





/***************************************************/
/** callback routine for TRDP logging/error output
 *
 *  @param[in]      pRefCon            user supplied context pointer
 *  @param[in]        category        Log category (Error, Warning, Info etc.)
 *  @param[in]        pTime            pointer to NULL-terminated string of time stamp
 *  @param[in]        pFile            pointer to NULL-terminated string of source module
 *  @param[in]        LineNumber        line
 *  @param[in]        pMsgStr         pointer to NULL-terminated string
 *  @retval         none
 */

void TRDP_DbgOut (
        void        *pRefCon,
        TRDP_LOG_T category,
        const CHAR8 *pTime,
        const CHAR8 *pFile,
        UINT16 LineNumber,
        const CHAR8 *pMsgStr)
{
        const char *catStr[] = {"**Error(TRDP):", "Warning(TRDP):", "   Info(TRDP):", "  Debug(TRDP):", "   User(TRDP):"};
        printf("%s %s %s:%d %s",
               strrchr(pTime, '-') + 1,
               catStr[category],
               strrchr(pFile, VOS_DIR_SEP) + 1,
               LineNumber,
               pMsgStr);
}


/********** functions for real time compatibility **********/

/***** *****/
void setPrio() {
        printf("Trying to set thread realtime prio = %i\n",MY_PRIO);
        struct sched_param param;
        param.sched_priority = MY_PRIO;
        if(sched_setscheduler(0, SCHED_FIFO, &param) == -1) {
                perror("sched_setscheduler failed");
                exit(-1);
        }
        printf("Priority is now %i (SCHED_FIFO)\n",MY_PRIO);
}

/***** *****/
void MemLock() {
        if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1)
        {
                perror("mlockall failed");
                exit(-2);
        }
        printf("Memory has been locked successfully\n");
}

/***** *****/
void stack_prefault() {
        unsigned char dummy[MAX_SAFE_STACK];
        memset(dummy, 0, MAX_SAFE_STACK);
        printf("Stack has been pre-faulted successfully\n");
}


/***** Init TRDP *****/
void initTRDP(){

        if (tlc_init(&TRDP_DbgOut,                    /* no logging    */
                     NULL,
                     &dynamicConfig) != TRDP_NO_ERR) /* Use application supplied memory    */
        {
                printf("Initialization error\n");
        }

        /* Open a session  */
        if (tlc_openSession(&appHandle,
                            ownIP, 0,     /* use default IP address           */
                            NULL,         /* no Marshalling                   */
                            &pdConfiguration, NULL, /* system defaults for PD and MD    */
                            &processConfig) != TRDP_NO_ERR)
        {
                vos_printLogStr(VOS_LOG_USR, "Initialization error\n");
        }
}


/***** add process message *****/
void addProcessMessage(int framesAmount_inPD){

        UINT8 uint8_FrameArray[DATA_MAX];
        UINT32 uint8_FrameArray_length=(UINT32)strlen((char*)uint8_FrameArray);
        char char_FrameArray[DATA_MAX];
        UINT32 char_FrameArray_length;
        UINT8 position_uint8_FrameArray=0;

        /* initial Array*/
        memset(uint8_FrameArray,0,uint8_FrameArray_length);


        /** write frames from CANRecvBuffer to uint8_FrameArray **/




        /** write frames in uint8_FrameArray **/
        UINT8 i_framesAmount_inPD=framesAmount_inPD;
        for(; i_framesAmount_inPD>0; i_framesAmount_inPD--)
        {
                /* wirte can_dlc */
                uint8_FrameArray[position_uint8_FrameArray]=CANRecvBuffer[CANRecvBuffer_begin_idx].can_dlc;
                position_uint8_FrameArray++;

                /* wirte can_data */
                int i_can_data;
                for(i_can_data=0; i_can_data<8; i_can_data++) {
                        uint8_FrameArray[position_uint8_FrameArray]=CANRecvBuffer[CANRecvBuffer_begin_idx].data[i_can_data];
                        position_uint8_FrameArray++;
                }

                /* wirte can_id(UINT32 to UINT8) */
                UINT8 unit8array[4];
                memcpy(unit8array, &CANRecvBuffer[CANRecvBuffer_begin_idx].can_id, sizeof(CANRecvBuffer[CANRecvBuffer_begin_idx].can_id));
                int i_can_id;
                for(i_can_id=0; i_can_id<4; i_can_id++) {
                        uint8_FrameArray[position_uint8_FrameArray]=unit8array[i_can_id];
                        position_uint8_FrameArray++;
                }

                /*update CANRecvBuffer index*/
                if(CANRecvBuffer_begin_idx<CANRecvBuffer_length) {
                        CANRecvBuffer_begin_idx++;
                }else{CANRecvBuffer_begin_idx=0; }
                CANRecvBuffer_load--;
        }

        /** translate uint8_FrameArray to char_FrameArray(UINT8 to char) **/
        uint8_FrameArray_length=1+(13*framesAmount_inPD);
        char_FrameArray_length=(UINT32)strlen((char*)char_FrameArray);
        memset(char_FrameArray,'\0',char_FrameArray_length);

        int i;
        for(i=0; i<uint8_FrameArray_length; i++) {

                char strTemp1[10];
                sprintf(strTemp1, "%x", uint8_FrameArray[i]);
                UINT8 strTemp1_length = strlen(strTemp1);
                if(strTemp1_length==1) {
                        char strTemp2[10] ="0";
                        strcat(strTemp2,strTemp1);
                        strcat(char_FrameArray,strTemp2);
                }else{
                        strcat(char_FrameArray,strTemp1);
                }
        }



        /**copy in TRDPSendBuffer **/
        memset(TRDPSendBuffer,0,sizeof(TRDPSendBuffer));
        char_FrameArray_length=(UINT32)strlen((char*)char_FrameArray);
        memcpy(TRDPSendBuffer, char_FrameArray, char_FrameArray_length);
}



/***** fucntiong: add frame to CANSendBuffer *****/
UINT8 addFrameIn_CANSendBuffer(int pisiton_CANSendBuffer,UINT8 *uint8_dataArrayTemp,UINT8 position_uint8_dataArrayTemp){

/* get can_dlc */
        CANSendBuffer[pisiton_CANSendBuffer].can_dlc=uint8_dataArrayTemp[position_uint8_dataArrayTemp];
        position_uint8_dataArrayTemp++;

/* get can_data */
        int i_can_data;
        for(i_can_data=0; i_can_data<8; i_can_data++) {
                CANSendBuffer[pisiton_CANSendBuffer].data[i_can_data]=uint8_dataArrayTemp[position_uint8_dataArrayTemp];
                position_uint8_dataArrayTemp++;
        }

/* get can_id (UINT8 to UINT32) */
        UINT8 UINT32_to_UINT8_Temp[4];
        int i_can_id;
        for(i_can_id=0; i_can_id<4; i_can_id++) {
                UINT32_to_UINT8_Temp[i_can_id]=uint8_dataArrayTemp[position_uint8_dataArrayTemp];
                position_uint8_dataArrayTemp++;
        }
        CANSendBuffer[pisiton_CANSendBuffer].can_id=*(UINT32*)&UINT32_to_UINT8_Temp;
        return position_uint8_dataArrayTemp;

}



/***** Function: decoder process messages from TRDPRecvBuffer to CANSendBuffer frames *****/
void readProcessMessage(){

        UINT8 position_uint8_dataArrayTemp=0;
        UINT32 TRDPRecvBuffer_length=(UINT32)strlen((char*)TRDPRecvBuffer);
        UINT32 framesAmount_TRDPRecvBuffer=TRDPRecvBuffer_length/26; //  1 frame:1 dlc+8 data+1 CAN_ID(UINT32) =13 UINT8 = 26 char
        UINT8 uint8_dataArrayTemp[DATA_MAX];

        /** from TRDPRecvBuffer to uint8_dataArrayTemp(char array to uint8 array) **/
        char strTemp1[2];
        char strTemp2[2];
        long longTemp1;
        long longTemp2;
        char *ptr;
        int i;
        int dataPosition=0;
        for(i=0; i<TRDPRecvBuffer_length; i=i+2) {
                strTemp1[0]=TRDPRecvBuffer[i];
                strTemp2[0]=TRDPRecvBuffer[i+1];
                longTemp1 = strtol(strTemp1, &ptr, 16);
                longTemp2 = strtol(strTemp2, &ptr, 16);
                longTemp1=longTemp1*0x10+longTemp2;
                uint8_dataArrayTemp[dataPosition]=longTemp1;
                dataPosition++;
        }

        /** add frames from uint8_dataArrayTemp to CANSendBuffer**/
        int n;
        for(n=0; n<framesAmount_TRDPRecvBuffer; n++) {

                /* If CANSendBuffer is full, the new CAN frame will replace the oldest  */
                if(CANSendBuffer_load == CANSendBuffer_length) { // check load of buffer

                        /*add frame in CANSendBuffer at begin index*/
                        position_uint8_dataArrayTemp=addFrameIn_CANSendBuffer(CANSendBuffer_begin_idx,uint8_dataArrayTemp,position_uint8_dataArrayTemp);

                }

                if(CANSendBuffer_load<CANSendBuffer_length) {

                        /* update the end index,add new frame at end */
                        CANSendBuffer_end_idx = CANSendBuffer_begin_idx + CANSendBuffer_load;
                        if(CANSendBuffer_end_idx < CANSendBuffer_length ) {
                                CANSendBuffer_end_idx=CANSendBuffer_end_idx;
                        }else{
                                CANSendBuffer_end_idx=CANSendBuffer_end_idx-CANSendBuffer_length;
                        }

                        /*add frame in CANSendBuffer at end index*/
                        position_uint8_dataArrayTemp=addFrameIn_CANSendBuffer(CANSendBuffer_end_idx,uint8_dataArrayTemp,position_uint8_dataArrayTemp);

                        /* update load*/
                        CANSendBuffer_load++;
                }
        }




}





/***** Function: print received message *****/
void printTRDPReceivedMessage(){
        vos_printLogStr(VOS_LOG_USR, "\nMessage received:\n");
        vos_printLog(VOS_LOG_USR, "Type = %c%c, ", myPDInfo.msgType >> 8, myPDInfo.msgType & 0xFF);
        vos_printLog(VOS_LOG_USR, "Seq  = %u, ", myPDInfo.seqCount);
        vos_printLog(VOS_LOG_USR, "with %d Bytes:\n", sizeof(TRDPRecvBuffer));
        vos_printLog(VOS_LOG_USR, "   %02hhx %02hhx %02hhx %02hhx %02hhx %02hhx %02hhx %02hhx\n",
                     TRDPRecvBuffer[0], TRDPRecvBuffer[1], TRDPRecvBuffer[2], TRDPRecvBuffer[3],
                     TRDPRecvBuffer[4], TRDPRecvBuffer[5], TRDPRecvBuffer[6], TRDPRecvBuffer[7]);
        vos_printLog(VOS_LOG_USR, "   %02hhx %02hhx %02hhx %02hhx %02hhx %02hhx %02hhx %02hhx\n",
                     TRDPRecvBuffer[8], TRDPRecvBuffer[9], TRDPRecvBuffer[10], TRDPRecvBuffer[11],
                     TRDPRecvBuffer[12], TRDPRecvBuffer[13], TRDPRecvBuffer[14], TRDPRecvBuffer[15]);
        vos_printLog(VOS_LOG_USR, "   %02hhx %02hhx %02hhx %02hhx %02hhx %02hhx %02hhx %02hhx\n",
                     TRDPRecvBuffer[16], TRDPRecvBuffer[17], TRDPRecvBuffer[18], TRDPRecvBuffer[19],
                     TRDPRecvBuffer[20], TRDPRecvBuffer[21], TRDPRecvBuffer[22], TRDPRecvBuffer[23]);
        vos_printLog(VOS_LOG_USR, "   %02hhx %02hhx %02hhx %02hhx %02hhx %02hhx %02hhx %02hhx\n",
                     TRDPRecvBuffer[24], TRDPRecvBuffer[25], TRDPRecvBuffer[26], TRDPRecvBuffer[27],
                     TRDPRecvBuffer[28], TRDPRecvBuffer[29], TRDPRecvBuffer[30], TRDPRecvBuffer[31]);
}


/***** Function: Print a sensible usage message *****/
void usage (const char *appName)
{
        printf("Usage of %s\n", appName);
        printf("This is CAN and TRDP gateway .\n"
               "Arguments are:\n"
               "-t <target IP address>\n"
               "-c <PD_comId_Send> (default 0)\n"
               "-r <PD_comId_REC> (default 0)\n"
               "-s <cycle time> (default 1000000 [us])\n"
               "-f frames amount in one PD Paket (default 3)\n"
               "-w max wait time for send PD Paket (default 1000ms)\n"
               "-b   can bus to rece \n"
               "-d   can bus to send \n"
               "-i  can frames filter id (default 0x000)\n"
               "-m  can fremes filter mask (default 0x000)\n"
               );
}




/***** interrupt handler function for the catched SIGINT *****/

void intHandler(int dummy)
{
        signal(SIGINT, dummyIntHandler);
        printf(" - exiting\n");
        pthread_cancel(CAN_Recv_Thread);
        pthread_cancel(CAN_Send_Thread);
        pthread_cancel(TRDP_Thread);

        exit(0);
}

/***** dummy interrupt handler function for the all following SIGINTs *****/
void dummyIntHandler(int dummy) {
        // cancelation is already in progress, so just wait until pthread_send has
        // sent all frames that were in the buffer.
        signal(SIGINT, dummyIntHandler);
        //fflush(stdout);
}


/***** Function: open a socket for CAN input *****/
void initInputSocketCAN(char *CAN_IFIN,int can_id,int can_mask) {
        s_in = socket(PF_CAN, SOCK_RAW, CAN_RAW);  //open a socket for CAN input
        if(s_in < 0) {
                perror("socket");
                exit(1);
        }

        //setting CAN filter
        struct can_filter rfilter_in[1]; // pass the  filter, when <received_can_id> & mask == can_id & mask
        rfilter_in[0].can_id   = can_id;
        rfilter_in[0].can_mask = can_mask;
        setsockopt(s_in, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter_in, sizeof(rfilter_in));

        /* enable loopback and message*/
        int loopback = 1; //0 = disabled, 1 = enabled (default)
        int recv_own_msgs = 1; // 0 = disabled (default), 1 = enabled
        setsockopt(s_in, SOL_CAN_RAW, CAN_RAW_LOOPBACK, &loopback, sizeof(loopback));
        setsockopt(s_in, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS,
                   &recv_own_msgs, sizeof(recv_own_msgs));




        // find out interface index of CAN device
        struct ifreq ifr;
        strcpy(ifr.ifr_name, CAN_IFIN);
        if (ioctl(s_in, SIOCGIFINDEX, &ifr) < 0) {
                perror("SIOCGIFINDEX");
                exit(1);
        }
        addr_in.can_family = AF_CAN;
        addr_in.can_ifindex = ifr.ifr_ifindex;

        // assign address of CAN device to socket
        if (bind(s_in, (struct sockaddr *)&addr_in, sizeof(addr_in)) < 0) {
                perror("bind");
                exit(1);
        }

        // configure structs for input
        iov.iov_base = &frame;
        iov.iov_len = sizeof(frame);
        msg.msg_iov = &iov;
        msg.msg_iovlen = 1;
        msg.msg_name = &addr_in;
        msg.msg_namelen = sizeof(addr_in);
}

/***** Function: open a socket for CAN output *****/
void initOutputSocketCAN(char *CAN_IFOUT) {
        s_out = socket(PF_CAN, SOCK_RAW, CAN_RAW);  // open socket for raw CAN
        if(s_out < 0) {
                perror("socket");
                exit(1);
        }


        // find out interface index of CAN device
        struct ifreq ifr;
        strcpy(ifr.ifr_name, CAN_IFOUT);
        if (ioctl(s_out, SIOCGIFINDEX, &ifr) < 0) {
                perror("SIOCGIFINDEX");
                exit(1);
        }
        addr_out.can_family = AF_CAN;
        addr_out.can_ifindex = ifr.ifr_ifindex;

        // assign address of CAN device to socket
        if (bind(s_out, (struct sockaddr *)&addr_out, sizeof(addr_out)) < 0) {
                perror("bind");
                exit(1);
        }
}

/********** Functions: SocketCAN threads cleanup **********/
void cleanup_CAN_recv(void *dummy) {
        close(s_in);
}

void cleanup_CAN_send(void *dummy) {
        close(s_out);
}

void cleanup_TRDP(void *dummy) {
        tlc_terminate();
}




/***********************************************************************************************************************
 * Thread Function: SEND FRAMES TO CANBUS
 */
void *sendFramestoCAN(void *dummy) {
        /* thread should be cancelable at specific points */
        pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL);
        pthread_cleanup_push(&cleanup_CAN_send, NULL);
        while(1)
        {
                /* configure thread so that it cannot be interrupted */
                pthread_setcancelstate(PTHREAD_CANCEL_DISABLE,NULL);



                if(CANSendBuffer_load>0) {

                        /* lock mutex */
                        pthread_mutex_lock(&mutex_buffer);


                        /* extract + send frame */
                        nbytes_out = write(s_out, &CANSendBuffer[CANSendBuffer_begin_idx], sizeof(CANSendBuffer[CANSendBuffer_begin_idx]));
                        CANSendBuffer_load--;

                        /* round robin buffer */
                        if( CANSendBuffer_begin_idx<CANSendBuffer_length) {
                                CANSendBuffer_begin_idx++;
                        }else{
                                CANSendBuffer_begin_idx=0;
                        }

                        /* unlock mutex */
                        pthread_mutex_unlock(&mutex_buffer);

                }

                /* check if having been canceled */
                pthread_setcancelstate(PTHREAD_CANCEL_ENABLE,NULL);
                pthread_testcancel();
        }
        pthread_cleanup_pop(0); // never reached but still needed to complement
                                // pthread_cleanup_push(...)
        return NULL;
}


/**********************************************
 * Thread Function: RECEIVE FRAMES FROM CANBUS
 */
void *recvFramesFromCAN(void *dummy) {

        pthread_cleanup_push(&cleanup_CAN_recv, NULL); // ensure that the socket will
        // be closed eventually
        while(1) {
                /* configure thread so that it can be interrupted at any time */
                pthread_setcancelstate(PTHREAD_CANCEL_ENABLE,NULL);
                pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS,NULL);

                /* wait for frames */
                nbytes_in = recvmsg(s_in, &msg, 0); // wait for frame
                if (nbytes_in < 0) {
                        perror("read");
                        //}else {
                        //}else if(msg.msg_flags==MSG_DONTROUTE){ //MSG_DONTROUTE: set when the received frame was created on the local host.

                } else{
                        /* no interruption now */
                        pthread_setcancelstate(PTHREAD_CANCEL_DISABLE,NULL);
                        pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL);

                        /* lock mutex */
                        pthread_mutex_lock(&mutex_buffer);

                        /* copy frame to buffer */
                        if(CANRecvBuffer_load == CANRecvBuffer_length) {  // check load of buffer

                                /* If CANRecvBuffer is full, the new frame will replace the oldest */
                                CANRecvBuffer[CANRecvBuffer_begin_idx]=frame;
                        } else {
                                /* If CANRecvBuffer is not full,add new frame at end */
                                CANRecvBuffer_end_idx = CANRecvBuffer_begin_idx + CANRecvBuffer_load;
                                if(CANRecvBuffer_end_idx < CANRecvBuffer_length) {
                                        CANRecvBuffer[CANRecvBuffer_end_idx]=frame;
                                }else{
                                        CANRecvBuffer_end_idx=CANRecvBuffer_end_idx-CANRecvBuffer_length;
                                        CANRecvBuffer[CANRecvBuffer_end_idx]=frame;
                                }
                                CANRecvBuffer_load++;
                        }

                        /* unlock mutex */
                        pthread_mutex_unlock(&mutex_buffer);

                }



                /* check if having been canceled */
                pthread_setcancelstate(PTHREAD_CANCEL_ENABLE,NULL);
                pthread_testcancel();

        }
        pthread_cleanup_pop(0); // never reached but still needed to complement
                                // pthread_cleanup_push(...)
        return NULL;
}





/***************************************************
 * Thread Function: recevie and send Frames in TRDP
 */

void *recvANDsendFramesInTRDP(void *dummy) {

        /* thread should be cancelable at specific points */
        pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL);
        pthread_cleanup_push(&cleanup_TRDP, NULL);



        /* timer variables */
        int timeflag=0;
        double timeuse=0;
        struct timeval starttime;
        struct timeval endtime;

        UINT8 framesAmount_inPD=0;
        int publishFlag=0;


        /* get setting parameters */
        int framesAmountInPD_Threshold=((struct Parame_For_TRDP *) dummy)->framesAmountInPD_Threshold;
        UINT32 time_Threshold=((struct Parame_For_TRDP *) dummy)->time_Threshold; //1ms
        UINT32 destIP_publish=((struct Parame_For_TRDP *) dummy)->destIP_publish;
        UINT32 comIdSend       = ((struct Parame_For_TRDP *) dummy)->PD_COMID_SEND;
        UINT32 comIdRec       =((struct Parame_For_TRDP *) dummy)->PD_COMID_REC;
        UINT32 cycleTime   = ((struct Parame_For_TRDP *) dummy)->PD_COMID_CYCLE;



        /***** Subscribe PD *****/
        err = tlp_subscribe( appHandle,             /*    our application identifier            */
                             &subHandle,            /*    our subscription identifier           */
                             NULL,                  /*    user reference                        */
                             NULL,                  /*    callback functiom                     */
                             comIdRec,              /*    ComID                                 */
                             0,                     /*    etbTopoCnt: local consist only        */
                             0,                     /*    opTopoCnt                             */
                             VOS_INADDR_ANY, VOS_INADDR_ANY, /*    Source IP filter             */
                             destIP_Subscribe,    /*    Default destination    (or MC Group)  */
                             TRDP_FLAGS_DEFAULT,    /*    TRDP flags                            */
                             cycleTime * 1,         /*    Time out in us                        */
                             TRDP_TO_SET_TO_ZERO    /*    delete invalid data on timeout        */
                             );

        if (err != TRDP_NO_ERR)
        {
                vos_printLogStr(VOS_LOG_ERROR, "prep pd receive error\n");
                tlc_terminate();
        }


        /***** start TRDP loop*****/
        while(1)
        {
                /* configure thread so that it cannot be interrupted */
                pthread_setcancelstate(PTHREAD_CANCEL_DISABLE,NULL);

                /* Timer */
                if(timeflag==0) {
                        gettimeofday(&starttime,NULL);
                        timeflag=1;
                }
                if(timeflag==1) {
                        gettimeofday(&endtime,NULL);
                        timeuse =1000*(endtime.tv_sec - starttime.tv_sec)+(endtime.tv_usec - starttime.tv_usec)/1000; //ms
                }

                /* publish multiple frames into process  message ,when time or amount of CAN frames conditions are met */
                if(((CANRecvBuffer_load>=framesAmountInPD_Threshold)||(timeuse>=time_Threshold))&&(CANRecvBuffer_load>0)) {
                         //if(CANRecvBuffer_load>0) {
                        timeflag=0;

                        /* Determine the amount of CAN frames in a PD Packet */
                        if(CANRecvBuffer_load>=framesAmountInPD_Threshold) {
                                framesAmount_inPD=framesAmountInPD_Threshold;
                        }
                        else{framesAmount_inPD=CANRecvBuffer_load; }

                        /* lock mutex */
                        pthread_mutex_lock(&mutex_buffer);

                        /* add the frames from CANRecvBuffer to TRDPSendBuffer as process message */
                        addProcessMessage(framesAmount_inPD);

                        /* unlock mutex */
                        pthread_mutex_unlock(&mutex_buffer);



                        err = tlp_publish(  appHandle, /*    our application identifier    */
                                            &pubHandle, /*    our pulication identifier     */
                                            NULL, NULL,
                                            comIdSend,  /*    ComID to send                 */
                                            0,      /*    etbTopoCnt = 0 for local consist only     */
                                            0,      /*    opTopoCnt = 0 for non-directinal data     */
                                            ownIP,  /*    default source IP             */
                                            destIP_publish, /*    where to send to              */
                                            cycleTime, /*    Cycle time in us              */
                                            0,      /*    not redundant                 */
                                            TRDP_FLAGS_NONE, /*    Use callback for errors       */
                                            NULL,   /*    default qos and ttl           */
                                            (UINT8 *)TRDPSendBuffer, /*    initial data                  */
                                            (UINT32) strlen((char *)TRDPSendBuffer) /*    data size                     */
                                            );
                        if (err != TRDP_NO_ERR)
                        {
                                vos_printLogStr(VOS_LOG_ERROR, "prep pd send error\n");
                                tlc_terminate();
                        }
                        publishFlag=1;
                }



                /***** trdp process *****/
                TRDP_FDS_T rfds;
                INT32 noDesc;
                TRDP_TIME_T tv;

                /* too large max_tv can cause high latency*/
                const TRDP_TIME_T max_tv  = {0, 12000};
                const TRDP_TIME_T min_tv  = {0, 10000};

                /*
                   Prepare the file descriptor set for the select call.
                   Additional descriptors can be added here.
                 */
                FD_ZERO(&rfds);
                /* FD_SET(pd_fd, &rfds); */

                /*
                   Compute the min. timeout value for select.
                   This way we can guarantee that PDs are sent in time
                   with minimum CPU load and minimum jitter.
                 */
                tlc_getInterval(appHandle, &tv, &rfds, &noDesc);

                /*
                   The wait time for select must consider cycle times and timeouts of
                   the PD packets received or sent.
                   If we need to poll something faster than the lowest PD cycle,
                   we need to set the maximum time out our self.
                 */
                if (vos_cmpTime(&tv, &max_tv) > 0)
                {
                        tv = max_tv;
                }
                else if (vos_cmpTime(&tv, &min_tv) < 0)
                {
                        tv = min_tv;
                }

                /*
                   Select() will wait for ready descriptors or time out,
                   what ever comes first.
                 */
                rv = vos_select(noDesc + 1, &rfds, NULL, NULL, &tv);

                /*
                   Check for overdue PDs (sending and receiving)
                   Send any pending PDs if it's time...
                   Detect missing PDs...
                   'rv' will be updated to show the handled events, if there are
                   more than one...
                   The callback function will be called from within the tlc_process
                   function (in it's context and thread)!
                 */

                (void) tlc_process(appHandle, &rfds, &rv);

                if(publishFlag==1) {
                        tlp_unpublish(appHandle, pubHandle); //after send 1 process data,stop send process data
                        publishFlag=0;
                }

                /* Handle other ready descriptors... */
                if (rv > 0)
                {
                        vos_printLogStr(VOS_LOG_USR, "other descriptors were ready\n");
                }
                else
                {
                        fprintf(stdout, "rv!>0 .");
                        fflush(stdout);
                }




                /***** updata TRDPRecvBuffer *****/
                memset(TRDPRecvBuffer, 0, sizeof(TRDPRecvBuffer));
                UINT32 receivedSize = sizeof(TRDPRecvBuffer);
                err = tlp_get(appHandle,
                              subHandle,
                              &myPDInfo,
                              (UINT8 *) TRDPRecvBuffer,
                              &receivedSize);

                /* handle the returned result of tlp_get */
                if ((TRDP_NO_ERR == err)
                    && ((sizeof(TRDPRecvBuffer)) > 0))
                {
                        /* decoder the frames from PD paket to CAN frames */
                        readProcessMessage();

                        /*print the message for debug*/
                        printTRDPReceivedMessage();
                }
                else if (TRDP_NO_ERR == err)
                {
                        vos_printLogStr(VOS_LOG_USR, "\nMessage reveived:\n");
                        vos_printLog(VOS_LOG_USR, "Type = %c%c - ", myPDInfo.msgType >> 8, myPDInfo.msgType & 0xFF);
                        vos_printLog(VOS_LOG_USR, "Seq  = %u\n", myPDInfo.seqCount);
                }
                else if (TRDP_TIMEOUT_ERR == err)
                {
                        vos_printLogStr(VOS_LOG_INFO, "Packet timed out\n");
                }
                else if (TRDP_NODATA_ERR == err)
                {
                        vos_printLogStr(VOS_LOG_INFO, "No data yet\n");
                }
                else
                {
                        vos_printLog(VOS_LOG_ERROR, "PD GET ERROR: %d\n", err);
                }

                /* check if having been canceled */
                pthread_setcancelstate(PTHREAD_CANCEL_ENABLE,NULL);
                pthread_testcancel();
        }

        tlp_unsubscribe(appHandle, subHandle);
        tlp_unpublish(appHandle, pubHandle);
        tlc_closeSession(appHandle);
        tlc_terminate();
        pthread_cleanup_pop(0); // never reached but still needed to complement
                                // pthread_cleanup_push(...)
}

This snippet took 0.05 seconds to highlight.

Back to the Entry List or Home.

Delete this entry (admin only).