J1939-vcan

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

#include <stdint.h>

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

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

#define FILTER_BC 0x00F00000                //83C
#define MASK_BC   0x00FF0000                //3C

#define FILTER_CA 0x00002800
#define MASK_CA   0x3C3FC00
#define NODE_ID   0x0A

#define J_MASK_PGN 0x03FFFF00


void print_pdu_specifics(uint8_t j_pdu_format, uint8_t pdu_specific);
void print_edp_dp(uint8_t edp_dp);

int main(int argc, char **argv)
{
    int s, i;
    int nbytes;
    struct sockaddr_can addr;

    // ifreq structure for configuring network device
    struct ifreq ifr;
    struct can_frame frame_receive;
    struct can_frame frame_send;


    if ((s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
        perror("Socket");
        return 1;
    }

    // Interface name
    strcpy(ifr.ifr_name, "vcan0" );
    // Get interface index
    int ioctl_result = ioctl(s, SIOCGIFINDEX, &ifr);
    if(ioctl_result < 0) {
        perror("error with ioctl");
        return 1;
    } else {
        printf("Interface index: [%d]\n", ifr.ifr_ifindex);
    }

    memset(&addr, 0, sizeof(addr));
    addr.can_family = AF_CAN;
    addr.can_ifindex = ifr.ifr_ifindex;


    if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
        perror("Bind");
        return 1;
    }

//     struct can_filter sid_filter[2];
//     sid_filter[0].can_id = 0x123;
//     sid_filter[0].can_mask = (CAN_EFF_MASK | CAN_RTR_FLAG | CAN_SFF_MASK);
//     sid_filter[1].can_id   = 0x12345678 | CAN_EFF_FLAG;
//     sid_filter[1].can_mask = (CAN_EFF_FLAG | CAN_RTR_FLAG | CAN_EFF_MASK);
//
//     setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, &sid_filter, sizeof(sid_filter));


    int id, j_1939_pgn;
    uint8_t j_edp_dp, j_pdu_format, j_pdu_specific;
    while(1) {

        nbytes = read(s, &frame_receive, sizeof(struct can_frame));

        if (nbytes < 0) {
            perror("Read");
            return 1;
        }


        if(frame_receive.can_id & CAN_EFF_FLAG)
        {
            id = frame_receive.can_id & CAN_EFF_MASK;
            printf("Extended ID: %08X DLC: %d\n", id, frame_receive.can_dlc);

            j_1939_pgn = id & J_MASK_PGN;
            j_edp_dp = (j_1939_pgn & 0x03000000) >> 24;
            j_pdu_format = (j_1939_pgn & 0x00FF0000) >> 16;
            j_pdu_specific = (j_1939_pgn & 0x0000FF00) >> 8;

            printf("Message format J1939\n");
            printf("PGN: %X\n", j_1939_pgn);
            print_edp_dp(j_edp_dp);
            printf("PDU Format: %X\n", j_pdu_format);
            print_pdu_specifics(j_pdu_format, j_pdu_specific);

        } else {
           id = frame_receive.can_id & CAN_SFF_MASK;
            printf("Standard ID: %03X      DLC: %d", id, frame_receive.can_dlc);
        }
        if(frame_receive.can_id & CAN_RTR_FLAG)
        {
            printf("Received RTR frame! ");
        }
        if(frame_receive.can_id & CAN_ERR_FLAG)
        {
            printf("Received ERROR message frame! ");
        }



        for (i = 0; i < frame_receive.can_dlc; i++) {
            printf("%02X ",frame_receive.data[i]);
        }
        printf("\n----------------------------------");
        printf("\r\n");
    }
    if (close(s) < 0) {
        perror("Close");
        return 1;
    }

    return 0;
}

void print_pdu_specifics(uint8_t j_pdu_format, uint8_t pdu_specific)
{
    if(j_pdu_format > 240){
        printf("Broadcast message with group extension: %X\n", pdu_specific);
    } else {
        printf("Message with destination address: %X\n", pdu_specific);
    }
}

void print_edp_dp(uint8_t edp_dp)
{
    switch(edp_dp){
        case 0: printf("SAE J1939 page 0 PGN\n"); break;
        case 1: printf("SAE J1939 page 1 PGN\n"); break;
        case 2: printf("SAE J1939 reserved\n"); break;
        case 3: printf("ISO 15765-3 defined\n"); break;
        default: break;
    }
}