Skip to content

Commit

Permalink
Merge pull request #4 from COVESA/fix/message-length
Browse files Browse the repository at this point in the history 
can listener - get message length from pdu intead of recv
  • Loading branch information
@nayakned
nayakned authored Jun 16, 2024
2 parents 0fe8fee + 9fd9fc7 commit ea7ff0a
Showing 1 changed file with 55 additions and 63 deletions.
118 changes: 55 additions & 63 deletions examples/acf-can/acf-can-listener.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of COVESA nor the names of its contributors may be
* * Neither the name of COVESA nor the names of its contributors may be
* used to endorse or promote products derived from this software without
* specific prior written permission.
*
Expand Down Expand Up @@ -82,7 +82,7 @@ static error_t parser(int key, char *arg, struct argp_state *state)
{
int res;

switch (key) {
switch (key) {
case 'p':
udp_port = atoi(arg);
break;
Expand All @@ -97,34 +97,34 @@ static error_t parser(int key, char *arg, struct argp_state *state)

if(state->argc < 2){
argp_usage(state);
}
}

if(!use_udp){

strncpy(ifname, arg, sizeof(ifname) - 1);
strncpy(ifname, arg, sizeof(ifname) - 1);

if(state->next < state->argc)
{
{
res = sscanf(state->argv[state->next], "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
&macaddr[0], &macaddr[1], &macaddr[2],
&macaddr[3], &macaddr[4], &macaddr[5]);
if (res != 6) {
fprintf(stderr, "Invalid MAC address\n\n");
argp_usage(state);
}
state->next += 1;
state->next += 1;
}

if(state->next < state->argc)
{
strncpy(can_ifname, state->argv[state->next], sizeof(can_ifname) - 1);
{
strncpy(can_ifname, state->argv[state->next], sizeof(can_ifname) - 1);
state->next = state->argc;
}
}

}else{
strncpy(can_ifname, arg, sizeof(can_ifname) - 1);
strncpy(can_ifname, arg, sizeof(can_ifname) - 1);
state->next = state->argc;
}
}

break;
}
Expand All @@ -143,7 +143,7 @@ static int is_valid_acf_packet(uint8_t* acf_pdu) {
fprintf(stderr, "ACF type mismatch: expected %u, got %lu\n",
AVTP_ACF_TYPE_CAN, val64);
return 0;
}
}

return 1;
}
Expand All @@ -168,95 +168,87 @@ void print_can_acf(uint8_t* acf_pdu)
fprintf(stderr, "Pad: %"PRIu64"\n", pad);
}

static int is_valid_cf_packet(uint8_t* pdu,
ssize_t *proc_bytes) {

uint64_t subtype;
uint64_t pdu_length;
int res;

res = Avtp_CommonHeader_GetField((Avtp_CommonHeader_t*)pdu, AVTP_COMMON_HEADER_FIELD_SUBTYPE, &subtype);
if (res < 0) {
fprintf(stderr, "Failed to get subtype field: %d\n", res);
return 0;
}

if (!((subtype == AVTP_SUBTYPE_NTSCF) ||
(subtype == AVTP_SUBTYPE_TSCF))) {
fprintf(stderr, "Subtype mismatch: expected %u or %u, got %"PRIu64"\n",
AVTP_SUBTYPE_NTSCF, AVTP_SUBTYPE_TSCF, subtype);
return 0;
}

if (subtype == AVTP_SUBTYPE_NTSCF) {
pdu_length = AVTP_NTSCF_HEADER_LEN;
} else if (subtype == AVTP_SUBTYPE_TSCF) {
pdu_length = AVTP_TSCF_HEADER_LEN;
}
*proc_bytes += pdu_length;
return 1;

}

static int new_packet(int sk_fd, int can_socket) {

int res;
ssize_t recd_bytes, proc_bytes = 0;
uint64_t msg_length, proc_bytes = 0, msg_proc_bytes = 0;
uint64_t can_frame_id, udp_seq_num = 0, subtype;
uint16_t payload_length, pdu_length;
uint64_t can_frame_id, udp_seq_num = 0;
uint8_t *can_payload, i;
uint8_t pdu[MAX_PDU_SIZE];
uint8_t* cf_pdu;
uint8_t* acf_pdu;
Avtp_UDP_t *udp_pdu;
char stdout_string[1000] = "\0";
struct can_frame frame;
struct can_frame frame;

recd_bytes = recv(sk_fd, pdu, MAX_PDU_SIZE, 0);
if (recd_bytes < 0 || recd_bytes > MAX_PDU_SIZE) {
res = recv(sk_fd, pdu, MAX_PDU_SIZE, 0);

if (res < 0 || res > MAX_PDU_SIZE) {
perror("Failed to receive data");
return -1;
}

if (use_udp) {
udp_pdu = (Avtp_UDP_t *) pdu;
Avtp_UDP_GetField(udp_pdu, AVTP_UDP_FIELD_ENCAPSULATION_SEQ_NO, &udp_seq_num);
cf_pdu = pdu + sizeof(Avtp_UDP_t);
proc_bytes += sizeof(Avtp_UDP_t);
cf_pdu = pdu + AVTP_UDP_HEADER_LEN;
proc_bytes += AVTP_UDP_HEADER_LEN;
} else {
cf_pdu = pdu;
}

if (!is_valid_cf_packet(cf_pdu, &proc_bytes)) {
fprintf(stderr, "Dropping packet\n");
return 0;
res = Avtp_CommonHeader_GetField((Avtp_CommonHeader_t*)cf_pdu, AVTP_COMMON_HEADER_FIELD_SUBTYPE, &subtype);
if (res < 0) {
fprintf(stderr, "Failed to get subtype field: %d\n", res);
return -1;
}

if (!((subtype == AVTP_SUBTYPE_NTSCF) ||
(subtype == AVTP_SUBTYPE_TSCF))) {
fprintf(stderr, "Subtype mismatch: expected %u or %u, got %"PRIu64". Dropping packet\n",
AVTP_SUBTYPE_NTSCF, AVTP_SUBTYPE_TSCF, subtype);
return -1;
}

if(subtype == AVTP_SUBTYPE_TSCF){
proc_bytes += AVTP_TSCF_HEADER_LEN;
res = Avtp_Tscf_GetField((Avtp_Tscf_t*)cf_pdu, AVTP_TSCF_FIELD_STREAM_DATA_LENGTH, (uint64_t *) &msg_length);
}else{
proc_bytes += AVTP_NTSCF_HEADER_LEN;
res = Avtp_Ntscf_GetField((Avtp_Ntscf_t*)cf_pdu, AVTP_NTSCF_FIELD_NTSCF_DATA_LENGTH, (uint64_t *) &msg_length);
}

while (proc_bytes < recd_bytes) {
if (res < 0) {
fprintf(stderr, "Failed to get message length: %d\n", res);
return -1;
}

while (msg_proc_bytes < msg_length) {

acf_pdu = &pdu[proc_bytes];

if (!is_valid_acf_packet(acf_pdu)) {
fprintf(stderr, "Error: Invalid ACF packet.\n");
return 0;
return -1;
}

res = Avtp_Can_GetField((Avtp_Can_t*)acf_pdu, AVTP_CAN_FIELD_CAN_IDENTIFIER,
res = Avtp_Can_GetField((Avtp_Can_t*)acf_pdu, AVTP_CAN_FIELD_CAN_IDENTIFIER,
&can_frame_id);
if (res < 0) {
fprintf(stderr, "Error: Getting CAN frame ID\n");
return 0;
}
return -1;
}

can_payload = Avtp_Can_GetPayload((Avtp_Can_t*)acf_pdu, &payload_length, &pdu_length);
proc_bytes += pdu_length*4;
msg_proc_bytes += pdu_length*4;

if (can_socket == 0) {
for (i = 0; i < payload_length; i++) {
sprintf(stdout_string+(2*i), "%02x", can_payload[i]);
}

fprintf(stdout, "(000000.000000) elmcan %03lx#%s\n", can_frame_id,
fprintf(stdout, "(000000.000000) elmcan %03lx#%s\n", can_frame_id,
stdout_string);
fflush(stdout);
} else {
Expand All @@ -277,8 +269,8 @@ int main(int argc, char *argv[])
struct pollfd fds;

int can_socket = 0;
struct sockaddr_can can_addr;
struct ifreq ifr;
struct sockaddr_can can_addr;
struct ifreq ifr;

argp_parse(&argp, argc, argv, 0, NULL, NULL);

Expand All @@ -294,7 +286,7 @@ int main(int argc, char *argv[])
memset(&can_addr, 0, sizeof(can_addr));
can_addr.can_family = AF_CAN;
can_addr.can_ifindex = ifr.ifr_ifindex;
if (bind(can_socket, (struct sockaddr *)&can_addr, sizeof(can_addr)) < 0)
if (bind(can_socket, (struct sockaddr *)&can_addr, sizeof(can_addr)) < 0)
return 1;
}

Expand Down

0 comments on commit ea7ff0a

Please sign in to comment.