From 47dbff6c6fd0860b14f8e42a94c82d3fde61e292 Mon Sep 17 00:00:00 2001 From: Vasili-Sk <1153192+Vasili-Sk@users.noreply.github.com> Date: Mon, 25 Oct 2021 20:08:44 +0300 Subject: [PATCH 1/5] Added Remote Transfer Request messages --- socketcand.c | 2 +- socketcandcl.c | 253 +++++++++++++++++++++----------------------- state_bcm.c | 280 ++++++++++++++++++++----------------------------- state_isotp.c | 76 ++++++-------- state_raw.c | 110 ++++++++++--------- 5 files changed, 325 insertions(+), 396 deletions(-) diff --git a/socketcand.c b/socketcand.c index 6125be9..b64ead1 100644 --- a/socketcand.c +++ b/socketcand.c @@ -120,7 +120,7 @@ int state_changed(char *buf, int current_state) return (current_state != state); } -char *element_start(char *buf, int element) +char* element_start(char *buf, int element) { int len = strlen(buf); int elem, i; diff --git a/socketcandcl.c b/socketcandcl.c index c78e976..3bcc29f 100644 --- a/socketcandcl.c +++ b/socketcandcl.c @@ -86,24 +86,22 @@ void state_connected(); int server_socket; int raw_socket; int port; -int verbose_flag=0; -int cmd_index=0; -int more_elements=0; +int verbose_flag = 0; +int cmd_index = 0; +int more_elements = 0; int state, previous_state; char ldev[IFNAMSIZ]; char rdev[IFNAMSIZ]; char buf[MAXLEN]; char cmd_buffer[MAXLEN]; - -int main(int argc, char **argv) -{ +int main(int argc, char **argv) { int i; struct sockaddr_in serveraddr; struct hostent *server_ent; struct sigaction sigint_action; char buf[MAXLEN]; - char* server_string; + char *server_string; /* set default config settings */ port = PORT; @@ -111,29 +109,22 @@ int main(int argc, char **argv) strcpy(rdev, "can0"); server_string = malloc(strlen("localhost")); - /* Parse commandline arguments */ - for(;;) { + for (;;) { /* getopt_long stores the option index here. */ int c, option_index = 0; - static struct option long_options[] = { - {"verbose", no_argument, 0, 'v'}, - {"interfaces", required_argument, 0, 'i'}, - {"server", required_argument, 0, 's'}, - {"port", required_argument, 0, 'p'}, - {"version", no_argument, 0, 'z'}, - {0, 0, 0, 0} - }; + static struct option long_options[] = { { "verbose", no_argument, 0, 'v' }, { "interfaces", required_argument, 0, 'i' }, { "server", required_argument, 0, + 's' }, { "port", required_argument, 0, 'p' }, { "version", no_argument, 0, 'z' }, { 0, 0, 0, 0 } }; c = getopt_long(argc, argv, "vhi:p:l:s:", long_options, &option_index); - if(c == -1) + if (c == -1) break; - switch(c) { + switch (c) { case 0: /* If this option set a flag, do nothing else now. */ - if(long_options[option_index].flag != 0) + if (long_options[option_index].flag != 0) break; break; @@ -147,7 +138,7 @@ int main(int argc, char **argv) break; case 's': - server_string = realloc(server_string, strlen(optarg)+1); + server_string = realloc(server_string, strlen(optarg) + 1); strcpy(server_string, optarg); break; @@ -174,14 +165,13 @@ int main(int argc, char **argv) } } - sigint_action.sa_handler = &sigint; sigemptyset(&sigint_action.sa_mask); sigint_action.sa_flags = 0; sigaction(SIGINT, &sigint_action, NULL); server_socket = socket(AF_INET, SOCK_STREAM, 0); - if(server_socket < 0) { + if (server_socket < 0) { perror("socket"); exit(1); } @@ -191,35 +181,32 @@ int main(int argc, char **argv) serveraddr.sin_port = htons(port); server_ent = gethostbyname(server_string); - if(server_ent == 0) { + if (server_ent == 0) { perror(server_string); exit(1); } memcpy(&(serveraddr.sin_addr.s_addr), server_ent->h_addr, - server_ent->h_length); - + server_ent->h_length); - if(connect(server_socket, (struct sockaddr*)&serveraddr, - sizeof(serveraddr)) != 0) { + if (connect(server_socket, (struct sockaddr*) &serveraddr, sizeof(serveraddr)) != 0) { perror("connect"); exit(1); } - - for(;;) { - switch(state) { + for (;;) { + switch (state) { case STATE_INIT: /* has to start with a command */ - i = receive_command(server_socket, (char *) &buf); - if(i != 0) { + i = receive_command(server_socket, (char*) &buf); + if (i != 0) { PRINT_ERROR("Connection terminated while waiting for command.\n"); state = STATE_SHUTDOWN; previous_state = STATE_INIT; break; } - if(!strncmp("< hi", buf, 4)) { + if (!strncmp("< hi", buf, 4)) { /* send open and rawmode command */ sprintf(buf, "< open %s >", rdev); send(server_socket, buf, strlen(buf), 0); @@ -235,7 +222,8 @@ int main(int argc, char **argv) state_connected(); break; case STATE_SHUTDOWN: - PRINT_VERBOSE("Closing client connection.\n"); + PRINT_VERBOSE("Closing client connection.\n") + ; close(server_socket); return 0; } @@ -243,9 +231,7 @@ int main(int argc, char **argv) return 0; } - -inline void state_connected() -{ +inline void state_connected() { int ret; static struct can_frame frame; @@ -253,16 +239,16 @@ inline void state_connected() static struct sockaddr_can addr; fd_set readfds; - if(previous_state != STATE_CONNECTED) { + if (previous_state != STATE_CONNECTED) { - if((raw_socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { + if ((raw_socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { PRINT_ERROR("Error while creating RAW socket %s\n", strerror(errno)); state = STATE_SHUTDOWN; return; } strcpy(ifr.ifr_name, ldev); - if(ioctl(raw_socket, SIOCGIFINDEX, &ifr) < 0) { + if (ioctl(raw_socket, SIOCGIFINDEX, &ifr) < 0) { PRINT_ERROR("Error while searching for bus %s\n", strerror(errno)); state = STATE_SHUTDOWN; return; @@ -273,14 +259,13 @@ inline void state_connected() /* turn on timestamp */ const int timestamp_on = 0; - if(setsockopt(raw_socket, SOL_SOCKET, SO_TIMESTAMP, - ×tamp_on, sizeof(timestamp_on)) < 0) { + if (setsockopt(raw_socket, SOL_SOCKET, SO_TIMESTAMP, ×tamp_on, sizeof(timestamp_on)) < 0) { PRINT_ERROR("Could not enable CAN timestamps\n"); state = STATE_SHUTDOWN; return; } /* bind socket */ - if(bind(raw_socket, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + if (bind(raw_socket, (struct sockaddr*) &addr, sizeof(addr)) < 0) { PRINT_ERROR("Error while binding RAW socket %s\n", strerror(errno)); state = STATE_SHUTDOWN; return; @@ -288,10 +273,9 @@ inline void state_connected() previous_state = STATE_CONNECTED; } + if (fork()) { - if(fork()) { - - for(;;) { + for (;;) { FD_ZERO(&readfds); FD_SET(server_socket, &readfds); @@ -300,51 +284,55 @@ inline void state_connected() * Check if there are more elements in the element buffer before * calling select() and blocking for new packets. */ - if(!more_elements) { - ret = select(server_socket+1, &readfds, NULL, NULL, NULL); + if (!more_elements) { + ret = select(server_socket + 1, &readfds, NULL, NULL, NULL); - if(ret < 0) { + if (ret < 0) { PRINT_ERROR("Error in select()\n") - state = STATE_SHUTDOWN; + state = STATE_SHUTDOWN; return; } } - if(FD_ISSET(server_socket, &readfds) || more_elements) { - ret = receive_command(server_socket, (char *) &buf); - if(ret == 0) { - if(!strncmp("< frame", buf, 7)) { - char data_str[2*8]; + if (FD_ISSET(server_socket, &readfds) || more_elements) { + ret = receive_command(server_socket, (char*) &buf); + if (ret == 0) { + if (!strncmp("< frame", buf, 7)) { + char data_str[2 * 8]; - sscanf(buf, "< frame %x %*d.%*d %s >", &frame.can_id, - data_str); + sscanf(buf, "< frame %x %*d.%*d %s >", &frame.can_id, data_str); - char* s = buf + 7; - for(; ++s;) { - if(*s== ' ') { - break; - } - } - if((s - buf - 7) > 4) + if (element_length(buf, 2) == 8) frame.can_id |= CAN_EFF_FLAG; frame.can_dlc = strlen(data_str) / 2; - sscanf(data_str, "%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx", - &frame.data[0], &frame.data[1], - &frame.data[2], &frame.data[3], - &frame.data[4], &frame.data[5], - &frame.data[6], &frame.data[7]); + sscanf(data_str, "%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx", &frame.data[0], &frame.data[1], &frame.data[2], &frame.data[3], + &frame.data[4], &frame.data[5], &frame.data[6], &frame.data[7]); ret = write(raw_socket, &frame, sizeof(struct can_frame)); - if(ret", &frame.can_id); + //force DLC 0 since it is undocumented feature + frame.can_dlc = 0; + frame.can_id |= CAN_RTR_FLAG; + if (element_length(buf, 2) == 8) + frame.can_id |= CAN_EFF_FLAG; //extended + + ret = write(raw_socket, &frame, sizeof(struct can_frame)); + if (ret < sizeof(struct can_frame)) { + perror("Writing CAN frame to can socket\n"); + } + } } else { ret = read(server_socket, &buf, 0); - if(ret == -1) { + if (ret == -1) { state = STATE_SHUTDOWN; return; } @@ -352,45 +340,50 @@ inline void state_connected() } } else { - for(;;) { + for (;;) { FD_ZERO(&readfds); FD_SET(raw_socket, &readfds); - ret = select(raw_socket+1, &readfds, NULL, NULL, NULL); - if(ret < 0) { + ret = select(raw_socket + 1, &readfds, NULL, NULL, NULL); + if (ret < 0) { PRINT_ERROR("Error in select()\n") - state = STATE_SHUTDOWN; + state = STATE_SHUTDOWN; return; } - if(FD_ISSET(raw_socket, &readfds)) { + if (FD_ISSET(raw_socket, &readfds)) { ret = recv(raw_socket, &frame, sizeof(struct can_frame), MSG_WAITALL); - if(ret < sizeof(struct can_frame)) { + if (ret < sizeof(struct can_frame)) { PRINT_ERROR("Error reading frame from RAW socket\n") - perror("Reading CAN socket\n"); + perror("Reading CAN socket\n"); } else { - if(frame.can_id & CAN_ERR_FLAG) { - /* TODO implement */ - } else if(frame.can_id & CAN_RTR_FLAG) { + if (frame.can_id & CAN_ERR_FLAG) { /* TODO implement */ } else { - int i; - if(frame.can_id & CAN_EFF_FLAG) { - ret = sprintf(buf, "< send %08X %d ", - frame.can_id & CAN_EFF_MASK, frame.can_dlc); + if (frame.can_id & CAN_RTR_FLAG) { + if (frame.can_id & CAN_EFF_FLAG) { + ret = sprintf(buf, "< sendrtr %08X >", frame.can_id & CAN_EFF_MASK); + } else { + ret = sprintf(buf, "< sendrtr %03X >", frame.can_id & CAN_SFF_MASK); + } } else { - ret = sprintf(buf, "< send %03X %d ", - frame.can_id & CAN_SFF_MASK, frame.can_dlc); - } - for(i=0; i"); + } - sprintf(buf+ret, " >"); const size_t len = strlen(buf); ret = send(server_socket, buf, len, 0); - if(ret < sizeof(len)) { + if (ret < sizeof(len)) { perror("Error sending TCP frame\n"); } } @@ -404,23 +397,22 @@ inline void state_connected() /* reads all available data from the socket into the command buffer. * returns '-1' if no command could be received. */ -int receive_command(int socket, char *buffer) -{ +int receive_command(int socket, char *buffer) { int i, start, stop; /* if there are no more elements in the buffer read more data from the * socket. */ - if(!more_elements) { - cmd_index += read(socket, cmd_buffer+cmd_index, MAXLEN-cmd_index); + if (!more_elements) { + cmd_index += read(socket, cmd_buffer + cmd_index, MAXLEN - cmd_index); } more_elements = 0; /* find first '<' in string */ start = -1; - for(i=0; i') { + for (i = 1; i < cmd_index; i++) { + if (cmd_buffer[i] == '>') { stop = i; break; } } /* if no '>' is in the string we have to wait for more data */ - if(stop == -1) { + if (stop == -1) { return -1; } /* copy string to new destination and correct cmd_buffer */ - for(i=start; i<=stop; i++) { - buffer[i-start] = cmd_buffer[i]; + for (i = start; i <= stop; i++) { + buffer[i - start] = cmd_buffer[i]; } - buffer[i-start] = '\0'; - + buffer[i - start] = '\0'; /* if only this message was in the buffer we're done */ - if(stop == cmd_index-1) { + if (stop == cmd_index - 1) { cmd_index = 0; } else { /* check if there is a '<' after the stop */ start = -1; - for(i=stop; i') { + for (i = 1; i < cmd_index; i++) { + if (cmd_buffer[i] == '>') { stop = i; break; } } - if(stop != -1) { + if (stop != -1) { more_elements = 1; } } @@ -497,9 +488,7 @@ int receive_command(int socket, char *buffer) return 0; } - -void print_usage(void) -{ +void print_usage(void) { printf("Usage: socketcandcl [-v | --verbose] [-i interfaces | --interfaces interfaces]\n\t\t[-s server | --server server ]\n\t\t[-p port | --port port]\n"); printf("Options:\n"); printf("\t-v activates verbose output to STDOUT\n"); @@ -509,28 +498,24 @@ void print_usage(void) printf("\t-h prints this message\n"); } - -void childdied() -{ +void childdied() { wait(NULL); } - -void sigint() -{ - if(verbose_flag) +void sigint() { + if (verbose_flag) PRINT_ERROR("received SIGINT\n") - if(server_socket != -1) { - if(verbose_flag) - PRINT_INFO("closing server socket\n") - close(server_socket); - } + if (server_socket != -1) { + if (verbose_flag) + PRINT_INFO("closing server socket\n") + close(server_socket); + } - if(raw_socket != -1) { - if(verbose_flag) + if (raw_socket != -1) { + if (verbose_flag) PRINT_INFO("closing can socket\n") - close(raw_socket); + close(raw_socket); } exit(0); diff --git a/state_bcm.c b/state_bcm.c index 05a5f7c..2c031c6 100644 --- a/state_bcm.c +++ b/state_bcm.c @@ -48,7 +48,7 @@ void state_bcm() { struct can_frame frame[257]; /* MAX_NFRAMES + MUX MASK */ } muxmsg; - if(previous_state != STATE_BCM) { + if (previous_state != STATE_BCM) { /* open BCM socket */ if ((sc = socket(PF_CAN, SOCK_DGRAM, CAN_BCM)) < 0) { PRINT_ERROR("Error while opening BCM socket %s\n", strerror(errno)); @@ -61,11 +61,11 @@ void state_bcm() { /* can_ifindex is set to 0 (any device) => need for sendto() */ PRINT_VERBOSE("connecting BCM socket...\n") - if (connect(sc, (struct sockaddr *)&caddr, sizeof(caddr)) < 0) { - PRINT_ERROR("Error while connecting BCM socket %s\n", strerror(errno)); - state = STATE_SHUTDOWN; - return; - } + if (connect(sc, (struct sockaddr*) &caddr, sizeof(caddr)) < 0) { + PRINT_ERROR("Error while connecting BCM socket %s\n", strerror(errno)); + state = STATE_SHUTDOWN; + return; + } previous_state = STATE_BCM; } @@ -77,14 +77,14 @@ void state_bcm() { * Check if there are more elements in the element buffer before calling select() and * blocking for new packets. */ - if(more_elements) { + if (more_elements) { FD_CLR(sc, &readfds); } else { - ret = select((sc > client_socket)?sc+1:client_socket+1, &readfds, NULL, NULL, NULL); + ret = select((sc > client_socket) ? sc + 1 : client_socket + 1, &readfds, NULL, NULL, NULL); - if(ret < 0) { + if (ret < 0) { PRINT_ERROR("Error in select()\n") - state = STATE_SHUTDOWN; + state = STATE_SHUTDOWN; return; } } @@ -92,40 +92,35 @@ void state_bcm() { if (FD_ISSET(sc, &readfds)) { struct timeval tv; - ret = recvfrom(sc, &msg, sizeof(msg), 0, - (struct sockaddr*)&caddr, &caddrlen); + ret = recvfrom(sc, &msg, sizeof(msg), 0, (struct sockaddr*) &caddr, &caddrlen); /* read timestamp data */ - if(ioctl(sc, SIOCGSTAMP, &tv) < 0) { + if (ioctl(sc, SIOCGSTAMP, &tv) < 0) { PRINT_ERROR("Could not receive timestamp\n"); } /* Check if this is an error frame */ - if(msg.msg_head.can_id & CAN_ERR_FLAG) { - if(msg.frame.can_dlc != CAN_ERR_DLC) { + if (msg.msg_head.can_id & CAN_ERR_FLAG) { + if (msg.frame.can_dlc != CAN_ERR_DLC) { PRINT_ERROR("Error frame has a wrong DLC!\n") - } else { + } else { snprintf(rxmsg, RXLEN, "< error %03X %ld.%06ld ", msg.msg_head.can_id, tv.tv_sec, tv.tv_usec); - for ( i = 0; i < msg.frame.can_dlc; i++) - snprintf(rxmsg + strlen(rxmsg), RXLEN - strlen(rxmsg), "%02X ", - msg.frame.data[i]); + for (i = 0; i < msg.frame.can_dlc; i++) + snprintf(rxmsg + strlen(rxmsg), RXLEN - strlen(rxmsg), "%02X ", msg.frame.data[i]); snprintf(rxmsg + strlen(rxmsg), RXLEN - strlen(rxmsg), " >"); send(client_socket, rxmsg, strlen(rxmsg), 0); } } else { - if(msg.msg_head.can_id & CAN_EFF_FLAG) { - snprintf(rxmsg, RXLEN, "< frame %08X %ld.%06ld ", - msg.msg_head.can_id & CAN_EFF_MASK, tv.tv_sec, tv.tv_usec); + if (msg.msg_head.can_id & CAN_EFF_FLAG) { + snprintf(rxmsg, RXLEN, "< frame %08X %ld.%06ld ", msg.msg_head.can_id & CAN_EFF_MASK, tv.tv_sec, tv.tv_usec); } else { - snprintf(rxmsg, RXLEN, "< frame %03X %ld.%06ld ", - msg.msg_head.can_id & CAN_SFF_MASK, tv.tv_sec, tv.tv_usec); + snprintf(rxmsg, RXLEN, "< frame %03X %ld.%06ld ", msg.msg_head.can_id & CAN_SFF_MASK, tv.tv_sec, tv.tv_usec); } - for ( i = 0; i < msg.frame.can_dlc; i++) - snprintf(rxmsg + strlen(rxmsg), RXLEN - strlen(rxmsg), "%02X ", - msg.frame.data[i]); + for (i = 0; i < msg.frame.can_dlc; i++) + snprintf(rxmsg + strlen(rxmsg), RXLEN - strlen(rxmsg), "%02X ", msg.frame.data[i]); snprintf(rxmsg + strlen(rxmsg), RXLEN - strlen(rxmsg), " >"); send(client_socket, rxmsg, strlen(rxmsg), 0); @@ -137,7 +132,7 @@ void state_bcm() { ret = receive_command(client_socket, buf); - if(ret != 0) { + if (ret != 0) { state = STATE_SHUTDOWN; return; } @@ -155,36 +150,25 @@ void state_bcm() { return; } - if(!strcmp("< echo >", buf)) { + if (!strcmp("< echo >", buf)) { send(client_socket, buf, strlen(buf), 0); return; } /* Send a single frame */ - if(!strncmp("< send ", buf, 7)) { + if (!strncmp("< send ", buf, 7)) { items = sscanf(buf, "< %*s %x %hhu " - "%hhx %hhx %hhx %hhx %hhx %hhx " - "%hhx %hhx >", - &msg.msg_head.can_id, - &msg.frame.can_dlc, - &msg.frame.data[0], - &msg.frame.data[1], - &msg.frame.data[2], - &msg.frame.data[3], - &msg.frame.data[4], - &msg.frame.data[5], - &msg.frame.data[6], - &msg.frame.data[7]); - - if ( (items < 2) || - (msg.frame.can_dlc > 8) || - (items != 2 + msg.frame.can_dlc)) { + "%hhx %hhx %hhx %hhx %hhx %hhx " + "%hhx %hhx >", &msg.msg_head.can_id, &msg.frame.can_dlc, &msg.frame.data[0], &msg.frame.data[1], &msg.frame.data[2], &msg.frame.data[3], + &msg.frame.data[4], &msg.frame.data[5], &msg.frame.data[6], &msg.frame.data[7]); + + if ((items < 2) || (msg.frame.can_dlc > 8) || (items != 2 + msg.frame.can_dlc)) { PRINT_ERROR("Syntax error in send command\n") - return; + return; } /* < send XXXXXXXX ... > check for extended identifier */ - if(element_length(buf, 2) == 8) + if (element_length(buf, 2) == 8) msg.msg_head.can_id |= CAN_EFF_FLAG; msg.msg_head.opcode = TX_SEND; @@ -192,36 +176,40 @@ void state_bcm() { if (!ioctl(sc, SIOCGIFINDEX, &ifr)) { caddr.can_ifindex = ifr.ifr_ifindex; - sendto(sc, &msg, sizeof(msg), 0, - (struct sockaddr*)&caddr, sizeof(caddr)); + sendto(sc, &msg, sizeof(msg), 0, (struct sockaddr*) &caddr, sizeof(caddr)); } /* Add a send job */ - } else if(!strncmp("< add ", buf, 6)) { + } else if (!strncmp("< sendrtr ", buf, 10)) { + //send RTR frame only + items = sscanf(buf, "< %*s %x >", &msg.msg_head.can_id); + if ((items < 1)) { + PRINT_ERROR("Syntax error in send command\n") + return; + } + //force DLC 0 since it is undocumented feature + msg.frame.can_dlc = 0; + msg.msg_head.can_id |= CAN_RTR_FLAG; + + if (element_length(buf, 2) == 8) + msg.msg_head.can_id |= CAN_EFF_FLAG; //extended + + if (!ioctl(sc, SIOCGIFINDEX, &ifr)) { + caddr.can_ifindex = ifr.ifr_ifindex; + sendto(sc, &msg, sizeof(msg), 0, (struct sockaddr*) &caddr, sizeof(caddr)); + } + } else if (!strncmp("< add ", buf, 6)) { items = sscanf(buf, "< %*s %lu %lu %x %hhu " - "%hhx %hhx %hhx %hhx %hhx %hhx " - "%hhx %hhx >", - &msg.msg_head.ival2.tv_sec, - &msg.msg_head.ival2.tv_usec, - &msg.msg_head.can_id, - &msg.frame.can_dlc, - &msg.frame.data[0], - &msg.frame.data[1], - &msg.frame.data[2], - &msg.frame.data[3], - &msg.frame.data[4], - &msg.frame.data[5], - &msg.frame.data[6], - &msg.frame.data[7]); - - if( (items < 4) || - (msg.frame.can_dlc > 8) || - (items != 4 + msg.frame.can_dlc) ) { + "%hhx %hhx %hhx %hhx %hhx %hhx " + "%hhx %hhx >", &msg.msg_head.ival2.tv_sec, &msg.msg_head.ival2.tv_usec, &msg.msg_head.can_id, &msg.frame.can_dlc, &msg.frame.data[0], + &msg.frame.data[1], &msg.frame.data[2], &msg.frame.data[3], &msg.frame.data[4], &msg.frame.data[5], &msg.frame.data[6], &msg.frame.data[7]); + + if ((items < 4) || (msg.frame.can_dlc > 8) || (items != 4 + msg.frame.can_dlc)) { PRINT_ERROR("Syntax error in add command.\n"); return; } /* < add sec usec XXXXXXXX ... > check for extended identifier */ - if(element_length(buf, 4) == 8) + if (element_length(buf, 4) == 8) msg.msg_head.can_id |= CAN_EFF_FLAG; msg.msg_head.opcode = TX_SETUP; @@ -230,57 +218,43 @@ void state_bcm() { if (!ioctl(sc, SIOCGIFINDEX, &ifr)) { caddr.can_ifindex = ifr.ifr_ifindex; - sendto(sc, &msg, sizeof(msg), 0, - (struct sockaddr*)&caddr, sizeof(caddr)); + sendto(sc, &msg, sizeof(msg), 0, (struct sockaddr*) &caddr, sizeof(caddr)); } /* Update send job */ - } else if(!strncmp("< update ", buf, 9)) { + } else if (!strncmp("< update ", buf, 9)) { items = sscanf(buf, "< %*s %x %hhu " - "%hhx %hhx %hhx %hhx %hhx %hhx " - "%hhx %hhx >", - &msg.msg_head.can_id, - &msg.frame.can_dlc, - &msg.frame.data[0], - &msg.frame.data[1], - &msg.frame.data[2], - &msg.frame.data[3], - &msg.frame.data[4], - &msg.frame.data[5], - &msg.frame.data[6], - &msg.frame.data[7]); - - if ( (items < 2) || - (msg.frame.can_dlc > 8) || - (items != 2 + msg.frame.can_dlc)) { + "%hhx %hhx %hhx %hhx %hhx %hhx " + "%hhx %hhx >", &msg.msg_head.can_id, &msg.frame.can_dlc, &msg.frame.data[0], &msg.frame.data[1], &msg.frame.data[2], &msg.frame.data[3], + &msg.frame.data[4], &msg.frame.data[5], &msg.frame.data[6], &msg.frame.data[7]); + + if ((items < 2) || (msg.frame.can_dlc > 8) || (items != 2 + msg.frame.can_dlc)) { PRINT_ERROR("Syntax error in update send job command\n") - return; + return; } /* < update XXXXXXXX ... > check for extended identifier */ - if(element_length(buf, 2) == 8) + if (element_length(buf, 2) == 8) msg.msg_head.can_id |= CAN_EFF_FLAG; msg.msg_head.opcode = TX_SETUP; - msg.msg_head.flags = 0; + msg.msg_head.flags = 0; msg.frame.can_id = msg.msg_head.can_id; if (!ioctl(sc, SIOCGIFINDEX, &ifr)) { caddr.can_ifindex = ifr.ifr_ifindex; - sendto(sc, &msg, sizeof(msg), 0, - (struct sockaddr*)&caddr, sizeof(caddr)); + sendto(sc, &msg, sizeof(msg), 0, (struct sockaddr*) &caddr, sizeof(caddr)); } /* Delete a send job */ - } else if(!strncmp("< delete ", buf, 9)) { - items = sscanf(buf, "< %*s %x >", - &msg.msg_head.can_id); + } else if (!strncmp("< delete ", buf, 9)) { + items = sscanf(buf, "< %*s %x >", &msg.msg_head.can_id); - if (items != 1) { + if (items != 1) { PRINT_ERROR("Syntax error in delete job command\n") - return; + return; } /* < delete XXXXXXXX ... > check for extended identifier */ - if(element_length(buf, 2) == 8) + if (element_length(buf, 2) == 8) msg.msg_head.can_id |= CAN_EFF_FLAG; msg.msg_head.opcode = TX_DELETE; @@ -288,84 +262,64 @@ void state_bcm() { if (!ioctl(sc, SIOCGIFINDEX, &ifr)) { caddr.can_ifindex = ifr.ifr_ifindex; - sendto(sc, &msg, sizeof(msg), 0, - (struct sockaddr*)&caddr, sizeof(caddr)); + sendto(sc, &msg, sizeof(msg), 0, (struct sockaddr*) &caddr, sizeof(caddr)); } /* Receive CAN ID with content matching */ - } else if(!strncmp("< filter ", buf, 9)) { + } else if (!strncmp("< filter ", buf, 9)) { items = sscanf(buf, "< %*s %lu %lu %x %hhu " - "%hhx %hhx %hhx %hhx %hhx %hhx " - "%hhx %hhx >", - &msg.msg_head.ival2.tv_sec, - &msg.msg_head.ival2.tv_usec, - &msg.msg_head.can_id, - &msg.frame.can_dlc, - &msg.frame.data[0], - &msg.frame.data[1], - &msg.frame.data[2], - &msg.frame.data[3], - &msg.frame.data[4], - &msg.frame.data[5], - &msg.frame.data[6], - &msg.frame.data[7]); - - if( (items < 4) || - (msg.frame.can_dlc > 8) || - (items != 4 + msg.frame.can_dlc) ) { + "%hhx %hhx %hhx %hhx %hhx %hhx " + "%hhx %hhx >", &msg.msg_head.ival2.tv_sec, &msg.msg_head.ival2.tv_usec, &msg.msg_head.can_id, &msg.frame.can_dlc, &msg.frame.data[0], + &msg.frame.data[1], &msg.frame.data[2], &msg.frame.data[3], &msg.frame.data[4], &msg.frame.data[5], &msg.frame.data[6], &msg.frame.data[7]); + + if ((items < 4) || (msg.frame.can_dlc > 8) || (items != 4 + msg.frame.can_dlc)) { PRINT_ERROR("syntax error in filter command.\n") - return; + return; } /* < filter sec usec XXXXXXXX ... > check for extended identifier */ - if(element_length(buf, 4) == 8) + if (element_length(buf, 4) == 8) msg.msg_head.can_id |= CAN_EFF_FLAG; msg.msg_head.opcode = RX_SETUP; - msg.msg_head.flags = SETTIMER; + msg.msg_head.flags = SETTIMER; msg.frame.can_id = msg.msg_head.can_id; if (!ioctl(sc, SIOCGIFINDEX, &ifr)) { caddr.can_ifindex = ifr.ifr_ifindex; - sendto(sc, &msg, sizeof(msg), 0, - (struct sockaddr*)&caddr, sizeof(caddr)); + sendto(sc, &msg, sizeof(msg), 0, (struct sockaddr*) &caddr, sizeof(caddr)); } /* Receive CAN ID with multiplex content matching */ - } else if(!strncmp("< muxfilter ", buf, 12)) { + } else if (!strncmp("< muxfilter ", buf, 12)) { char *cfptr; char tmp; memset(&muxmsg, 0, sizeof(muxmsg)); - items = sscanf(buf, "< %*s %lu %lu %x %u ", - &muxmsg.msg_head.ival2.tv_sec, - &muxmsg.msg_head.ival2.tv_usec, - &muxmsg.msg_head.can_id, - &muxmsg.msg_head.nframes); + items = sscanf(buf, "< %*s %lu %lu %x %u ", &muxmsg.msg_head.ival2.tv_sec, &muxmsg.msg_head.ival2.tv_usec, &muxmsg.msg_head.can_id, + &muxmsg.msg_head.nframes); - if( (items != 4) || - (muxmsg.msg_head.nframes < 2) || - (muxmsg.msg_head.nframes > 257) ) { + if ((items != 4) || (muxmsg.msg_head.nframes < 2) || (muxmsg.msg_head.nframes > 257)) { PRINT_ERROR("syntax error in muxfilter command.\n") - return; + return; } /* < muxfilter sec usec XXXXXXXX ... > check for extended identifier */ - if(element_length(buf, 4) == 8) + if (element_length(buf, 4) == 8) muxmsg.msg_head.can_id |= CAN_EFF_FLAG; muxmsg.msg_head.opcode = RX_SETUP; - muxmsg.msg_head.flags = SETTIMER; + muxmsg.msg_head.flags = SETTIMER; cfptr = element_start(buf, 6); if (cfptr == NULL) { PRINT_ERROR("failed to find filter data start in muxfilter.\n") - return; + return; } - if (strlen(cfptr) < muxmsg.msg_head.nframes * 24) { + if (strlen(cfptr) < muxmsg.msg_head.nframes * 24) { PRINT_ERROR("muxfilter data too short.\n") - return; + return; } /* copy filter data and mux mask in muxmsg.frame[0] */ @@ -373,18 +327,18 @@ void state_bcm() { for (j = 0; j < 8; j++) { - tmp = asc2nibble(cfptr[(24*i + 3*j)]); + tmp = asc2nibble(cfptr[(24 * i + 3 * j)]); if (tmp > 0x0F) { PRINT_ERROR("failed to process filter data in muxfilter.\n") - return; + return; } muxmsg.frame[i].data[j] = (tmp << 4); - tmp = asc2nibble(cfptr[(24*i + 3*j)+1]); + tmp = asc2nibble(cfptr[(24 * i + 3 * j) + 1]); if (tmp > 0x0F) { PRINT_ERROR("failed to process filter data in muxfilter.\n") - return; + return; } muxmsg.frame[i].data[j] |= tmp; @@ -393,60 +347,52 @@ void state_bcm() { if (!ioctl(sc, SIOCGIFINDEX, &ifr)) { caddr.can_ifindex = ifr.ifr_ifindex; - sendto(sc, &muxmsg, sizeof(struct bcm_msg_head) + - sizeof(struct can_frame) * muxmsg.msg_head.nframes, - 0, (struct sockaddr*)&caddr, sizeof(caddr)); + sendto(sc, &muxmsg, sizeof(struct bcm_msg_head) + sizeof(struct can_frame) * muxmsg.msg_head.nframes, 0, (struct sockaddr*) &caddr, sizeof(caddr)); } /* Add a filter */ - } else if(!strncmp("< subscribe ", buf, 12)) { - items = sscanf(buf, "< %*s %lu %lu %x >", - &msg.msg_head.ival2.tv_sec, - &msg.msg_head.ival2.tv_usec, - &msg.msg_head.can_id); + } else if (!strncmp("< subscribe ", buf, 12)) { + items = sscanf(buf, "< %*s %lu %lu %x >", &msg.msg_head.ival2.tv_sec, &msg.msg_head.ival2.tv_usec, &msg.msg_head.can_id); if (items != 3) { PRINT_ERROR("syntax error in subscribe command\n") - return; + return; } /* < subscribe sec usec XXXXXXXX ... > check for extended identifier */ - if(element_length(buf, 4) == 8) + if (element_length(buf, 4) == 8) msg.msg_head.can_id |= CAN_EFF_FLAG; msg.msg_head.opcode = RX_SETUP; - msg.msg_head.flags = RX_FILTER_ID | SETTIMER; + msg.msg_head.flags = RX_FILTER_ID | SETTIMER; msg.frame.can_id = msg.msg_head.can_id; if (!ioctl(sc, SIOCGIFINDEX, &ifr)) { caddr.can_ifindex = ifr.ifr_ifindex; - sendto(sc, &msg, sizeof(msg), 0, - (struct sockaddr*)&caddr, sizeof(caddr)); + sendto(sc, &msg, sizeof(msg), 0, (struct sockaddr*) &caddr, sizeof(caddr)); } /* Delete filter */ - } else if(!strncmp("< unsubscribe ", buf, 14)) { - items = sscanf(buf, "< %*s %x >", - &msg.msg_head.can_id); + } else if (!strncmp("< unsubscribe ", buf, 14)) { + items = sscanf(buf, "< %*s %x >", &msg.msg_head.can_id); if (items != 1) { PRINT_ERROR("syntax error in unsubscribe command\n") - return; + return; } /* < unsubscribe XXXXXXXX ... > check for extended identifier */ - if(element_length(buf, 2) == 8) + if (element_length(buf, 2) == 8) msg.msg_head.can_id |= CAN_EFF_FLAG; msg.msg_head.opcode = RX_DELETE; msg.frame.can_id = msg.msg_head.can_id; - + if (!ioctl(sc, SIOCGIFINDEX, &ifr)) { caddr.can_ifindex = ifr.ifr_ifindex; - sendto(sc, &msg, sizeof(msg), 0, - (struct sockaddr*)&caddr, sizeof(caddr)); + sendto(sc, &msg, sizeof(msg), 0, (struct sockaddr*) &caddr, sizeof(caddr)); } } else { PRINT_ERROR("unknown command '%s'.\n", buf) - strcpy(buf, "< error unknown command >"); + strcpy(buf, "< error unknown command >"); send(client_socket, buf, strlen(buf), 0); } } diff --git a/state_isotp.c b/state_isotp.c index e1b944b..ce8c7f4 100644 --- a/state_isotp.c +++ b/state_isotp.c @@ -36,14 +36,14 @@ void state_isotp() { char rxmsg[MAXLEN]; /* can to inet */ char buf[MAXLEN]; /* inet commands to can */ - unsigned char isobuf[ISOTPLEN+1]; /* binary buffer for isotp socket */ + unsigned char isobuf[ISOTPLEN + 1]; /* binary buffer for isotp socket */ unsigned char tmp; fd_set readfds; - - while(previous_state != STATE_ISOTP) { + + while (previous_state != STATE_ISOTP) { ret = receive_command(client_socket, buf); - if(ret != 0) { + if (ret != 0) { state = STATE_SHUTDOWN; return; } @@ -58,7 +58,7 @@ void state_isotp() { return; } - if(!strcmp("< echo >", buf)) { + if (!strcmp("< echo >", buf)) { send(client_socket, buf, strlen(buf), 0); continue; } @@ -68,33 +68,21 @@ void state_isotp() { memset(&addr, 0, sizeof(addr)); /* get configuration to open the socket */ - if(!strncmp("< isotpconf ", buf, 12)) { + if (!strncmp("< isotpconf ", buf, 12)) { items = sscanf(buf, "< %*s %x %x %x " - "%hhu %hhx %hhu " - "%hhx %hhx %hhx %hhx >", - &addr.can_addr.tp.tx_id, - &addr.can_addr.tp.rx_id, - &opts.flags, - &fcopts.bs, - &fcopts.stmin, - &fcopts.wftmax, - &opts.txpad_content, - &opts.rxpad_content, - &opts.ext_address, - &opts.rx_ext_address); + "%hhu %hhx %hhu " + "%hhx %hhx %hhx %hhx >", &addr.can_addr.tp.tx_id, &addr.can_addr.tp.rx_id, &opts.flags, &fcopts.bs, &fcopts.stmin, &fcopts.wftmax, + &opts.txpad_content, &opts.rxpad_content, &opts.ext_address, &opts.rx_ext_address); /* < isotpconf XXXXXXXX ... > check for extended identifier */ - if(element_length(buf, 2) == 8) + if (element_length(buf, 2) == 8) addr.can_addr.tp.tx_id |= CAN_EFF_FLAG; - if(element_length(buf, 3) == 8) + if (element_length(buf, 3) == 8) addr.can_addr.tp.rx_id |= CAN_EFF_FLAG; - if ((opts.flags & CAN_ISOTP_RX_EXT_ADDR && items < 10) || - (opts.flags & CAN_ISOTP_EXTEND_ADDR && items < 9) || - (opts.flags & CAN_ISOTP_RX_PADDING && items < 8) || - (opts.flags & CAN_ISOTP_TX_PADDING && items < 7) || - (items < 5)) { + if ((opts.flags & CAN_ISOTP_RX_EXT_ADDR && items < 10) || (opts.flags & CAN_ISOTP_EXTEND_ADDR && items < 9) + || (opts.flags & CAN_ISOTP_RX_PADDING && items < 8) || (opts.flags & CAN_ISOTP_TX_PADDING && items < 7) || (items < 5)) { PRINT_ERROR("Syntax error in isotpconf command\n"); /* try it once more */ continue; @@ -110,7 +98,7 @@ void state_isotp() { } strcpy(ifr.ifr_name, bus_name); - if(ioctl(si, SIOCGIFINDEX, &ifr) < 0) { + if (ioctl(si, SIOCGIFINDEX, &ifr) < 0) { PRINT_ERROR("Error while searching for bus %s\n", strerror(errno)); /* ensure proper handling in other states */ previous_state = STATE_ISOTP; @@ -128,14 +116,14 @@ void state_isotp() { setsockopt(si, SOL_CAN_ISOTP, CAN_ISOTP_RECV_FC, &fcopts, sizeof(fcopts)); PRINT_VERBOSE("binding ISOTP socket...\n") - if (bind(si, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + if (bind(si, (struct sockaddr*) &addr, sizeof(addr)) < 0) { PRINT_ERROR("Error while binding ISOTP socket %s\n", strerror(errno)); /* ensure proper handling in other states */ previous_state = STATE_ISOTP; state = STATE_SHUTDOWN; return; } - + /* ok we made it and have a proper isotp socket open */ previous_state = STATE_ISOTP; } @@ -149,25 +137,25 @@ void state_isotp() { * Check if there are more elements in the element buffer before calling select() and * blocking for new packets. */ - if(more_elements) { + if (more_elements) { FD_CLR(si, &readfds); } else { - ret = select((si > client_socket)?si+1:client_socket+1, &readfds, NULL, NULL, NULL); - if(ret < 0) { + ret = select((si > client_socket) ? si + 1 : client_socket + 1, &readfds, NULL, NULL, NULL); + if (ret < 0) { PRINT_ERROR("Error in select()\n") - state = STATE_SHUTDOWN; + state = STATE_SHUTDOWN; return; } } if (FD_ISSET(si, &readfds)) { - struct timeval tv = {0}; + struct timeval tv = { 0 }; items = read(si, isobuf, ISOTPLEN); /* read timestamp data */ - if(ioctl(si, SIOCGSTAMP, &tv) < 0) { + if (ioctl(si, SIOCGSTAMP, &tv) < 0) { PRINT_ERROR("Could not receive timestamp\n"); } @@ -178,8 +166,8 @@ void state_isotp() { sprintf(rxmsg, "< pdu %ld.%06ld ", tv.tv_sec, tv.tv_usec); startlen = strlen(rxmsg); - for (i=0; i < items; i++) - sprintf(rxmsg + startlen + 2*i, "%02X", isobuf[i]); + for (i = 0; i < items; i++) + sprintf(rxmsg + startlen + 2 * i, "%02X", isobuf[i]); sprintf(rxmsg + strlen(rxmsg), " >"); send(client_socket, rxmsg, strlen(rxmsg), 0); @@ -189,7 +177,7 @@ void state_isotp() { if (FD_ISSET(client_socket, &readfds)) { ret = receive_command(client_socket, buf); - if(ret != 0) { + if (ret != 0) { state = STATE_SHUTDOWN; return; } @@ -201,12 +189,12 @@ void state_isotp() { return; } - if(!strcmp("< echo >", buf)) { + if (!strcmp("< echo >", buf)) { send(client_socket, buf, strlen(buf), 0); return; } - if(!strncmp("< sendpdu ", buf, 10)) { + if (!strncmp("< sendpdu ", buf, 10)) { items = element_length(buf, 2); if (items & 1) { PRINT_ERROR("odd number of ASCII Hex values\n"); @@ -221,25 +209,25 @@ void state_isotp() { for (i = 0; i < items; i++) { - tmp = asc2nibble(buf[(2*i) + 10]); + tmp = asc2nibble(buf[(2 * i) + 10]); if (tmp > 0x0F) return; isobuf[i] = (tmp << 4); - tmp = asc2nibble(buf[(2*i) + 11]); + tmp = asc2nibble(buf[(2 * i) + 11]); if (tmp > 0x0F) return; isobuf[i] |= tmp; } ret = write(si, isobuf, items); - if(ret != items) { + if (ret != items) { PRINT_ERROR("Error in write()\n") - state = STATE_SHUTDOWN; + state = STATE_SHUTDOWN; return; } } else { PRINT_ERROR("unknown command '%s'.\n", buf) - strcpy(buf, "< error unknown command >"); + strcpy(buf, "< error unknown command >"); send(client_socket, buf, strlen(buf), 0); } } diff --git a/state_raw.c b/state_raw.c index 2c49975..1fa8c72 100644 --- a/state_raw.c +++ b/state_raw.c @@ -28,7 +28,7 @@ struct sockaddr_can addr; struct msghdr msg; struct can_frame frame; struct iovec iov; -char ctrlmsg[CMSG_SPACE(sizeof(struct timeval)) + CMSG_SPACE(sizeof(__u32))]; +char ctrlmsg[CMSG_SPACE(sizeof(struct timeval)) + CMSG_SPACE(sizeof(__u32 ))]; struct timeval tv; struct cmsghdr *cmsg; @@ -37,16 +37,16 @@ void state_raw() { int i, ret, items; fd_set readfds; - if(previous_state != STATE_RAW) { + if (previous_state != STATE_RAW) { - if((raw_socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { + if ((raw_socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { PRINT_ERROR("Error while creating RAW socket %s\n", strerror(errno)); state = STATE_SHUTDOWN; return; } strcpy(ifr.ifr_name, bus_name); - if(ioctl(raw_socket, SIOCGIFINDEX, &ifr) < 0) { + if (ioctl(raw_socket, SIOCGIFINDEX, &ifr) < 0) { PRINT_ERROR("Error while searching for bus %s\n", strerror(errno)); state = STATE_SHUTDOWN; return; @@ -56,13 +56,13 @@ void state_raw() { addr.can_ifindex = ifr.ifr_ifindex; const int timestamp_on = 1; - if(setsockopt( raw_socket, SOL_SOCKET, SO_TIMESTAMP, ×tamp_on, sizeof(timestamp_on)) < 0) { + if (setsockopt(raw_socket, SOL_SOCKET, SO_TIMESTAMP, ×tamp_on, sizeof(timestamp_on)) < 0) { PRINT_ERROR("Could not enable CAN timestamps\n"); state = STATE_SHUTDOWN; return; } - if(bind(raw_socket, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + if (bind(raw_socket, (struct sockaddr*) &addr, sizeof(addr)) < 0) { PRINT_ERROR("Error while binding RAW socket %s\n", strerror(errno)); state = STATE_SHUTDOWN; return; @@ -85,62 +85,65 @@ void state_raw() { * Check if there are more elements in the element buffer before calling select() and * blocking for new packets. */ - if(more_elements) { + if (more_elements) { FD_CLR(raw_socket, &readfds); } else { - ret = select((raw_socket > client_socket)?raw_socket+1:client_socket+1, &readfds, NULL, NULL, NULL); + ret = select((raw_socket > client_socket) ? raw_socket + 1 : client_socket + 1, &readfds, NULL, NULL, NULL); - if(ret < 0) { + if (ret < 0) { PRINT_ERROR("Error in select()\n") - state = STATE_SHUTDOWN; + state = STATE_SHUTDOWN; return; } } - if(FD_ISSET(raw_socket, &readfds)) { + if (FD_ISSET(raw_socket, &readfds)) { iov.iov_len = sizeof(frame); msg.msg_namelen = sizeof(addr); msg.msg_flags = 0; msg.msg_controllen = sizeof(ctrlmsg); ret = recvmsg(raw_socket, &msg, 0); - if(ret < sizeof(struct can_frame)) { + if (ret < sizeof(struct can_frame)) { PRINT_ERROR("Error reading frame from RAW socket\n") - } else { + } else { /* read timestamp data */ - for (cmsg = CMSG_FIRSTHDR(&msg); - cmsg && (cmsg->cmsg_level == SOL_SOCKET); - cmsg = CMSG_NXTHDR(&msg,cmsg)) { + for (cmsg = CMSG_FIRSTHDR(&msg); cmsg && (cmsg->cmsg_level == SOL_SOCKET); cmsg = CMSG_NXTHDR(&msg, cmsg)) { if (cmsg->cmsg_type == SO_TIMESTAMP) { - tv = *(struct timeval *)CMSG_DATA(cmsg); + tv = *(struct timeval*) CMSG_DATA(cmsg); } } - if(frame.can_id & CAN_ERR_FLAG) { - canid_t class = frame.can_id & CAN_EFF_MASK; + if (frame.can_id & CAN_ERR_FLAG) { + canid_t class = frame.can_id & CAN_EFF_MASK; ret = sprintf(buf, "< error %03X %ld.%06ld >", class, tv.tv_sec, tv.tv_usec); send(client_socket, buf, strlen(buf), 0); - } else if(frame.can_id & CAN_RTR_FLAG) { - /* TODO implement */ + } else if (frame.can_id & CAN_RTR_FLAG) { + if (frame.can_id & CAN_EFF_FLAG) { + ret = sprintf(buf, "< rtr %08X %ld.%06ld >", frame.can_id & CAN_EFF_MASK, tv.tv_sec, tv.tv_usec); + } else { + ret = sprintf(buf, "< rtr %03X %ld.%06ld >", frame.can_id & CAN_SFF_MASK, tv.tv_sec, tv.tv_usec); + } + send(client_socket, buf, strlen(buf), 0); } else { - if(frame.can_id & CAN_EFF_FLAG) { + if (frame.can_id & CAN_EFF_FLAG) { ret = sprintf(buf, "< frame %08X %ld.%06ld ", frame.can_id & CAN_EFF_MASK, tv.tv_sec, tv.tv_usec); } else { ret = sprintf(buf, "< frame %03X %ld.%06ld ", frame.can_id & CAN_SFF_MASK, tv.tv_sec, tv.tv_usec); } - for(i=0;i"); + sprintf(buf + ret, " >"); send(client_socket, buf, strlen(buf), 0); } } } - if(FD_ISSET(client_socket, &readfds)) { - ret = receive_command(client_socket, (char *) &buf); + if (FD_ISSET(client_socket, &readfds)) { + ret = receive_command(client_socket, (char*) &buf); - if(ret == 0) { + if (ret == 0) { if (state_changed(buf, state)) { close(raw_socket); @@ -149,44 +152,51 @@ void state_raw() { return; } - if(!strcmp("< echo >", buf)) { + if (!strcmp("< echo >", buf)) { send(client_socket, buf, strlen(buf), 0); return; } /* Send a single frame */ - if(!strncmp("< send ", buf, 7)) { + if (!strncmp("< send ", buf, 7)) { items = sscanf(buf, "< %*s %x %hhu " - "%hhx %hhx %hhx %hhx %hhx %hhx " - "%hhx %hhx >", - &frame.can_id, - &frame.can_dlc, - &frame.data[0], - &frame.data[1], - &frame.data[2], - &frame.data[3], - &frame.data[4], - &frame.data[5], - &frame.data[6], - &frame.data[7]); - - if ( (items < 2) || - (frame.can_dlc > 8) || - (items != 2 + frame.can_dlc)) { + "%hhx %hhx %hhx %hhx %hhx %hhx " + "%hhx %hhx >", &frame.can_id, &frame.can_dlc, &frame.data[0], &frame.data[1], &frame.data[2], &frame.data[3], &frame.data[4], &frame.data[5], + &frame.data[6], &frame.data[7]); + + if ((items < 2) || (frame.can_dlc > 8) || (items != 2 + frame.can_dlc)) { PRINT_ERROR("Syntax error in send command\n") - return; + return; } /* < send XXXXXXXX ... > check for extended identifier */ - if(element_length(buf, 2) == 8) + if (element_length(buf, 2) == 8) frame.can_id |= CAN_EFF_FLAG; ret = send(raw_socket, &frame, sizeof(struct can_frame), 0); - if(ret==-1) { + if (ret == -1) { state = STATE_SHUTDOWN; return; } + } else if (!strncmp("< sendrtr ", buf, 10)) { + //send RTR frame only + items = sscanf(buf, "< %*s %x >", &frame.can_id); + if ((items < 1)) { + PRINT_ERROR("Syntax error in send command\n") + return; + } + //force DLC 0 since it is undocumented feature + frame.can_dlc = 0; + frame.can_id |= CAN_RTR_FLAG; + + if (element_length(buf, 2) == 8) + frame.can_id |= CAN_EFF_FLAG; //extended + ret = send(raw_socket, &frame, sizeof(struct can_frame), 0); + if (ret == -1) { + state = STATE_SHUTDOWN; + return; + } } else { PRINT_ERROR("unknown command '%s'\n", buf); strcpy(buf, "< error unknown command >"); @@ -198,7 +208,7 @@ void state_raw() { } } else { ret = read(client_socket, &buf, 0); - if(ret==-1) { + if (ret == -1) { state = STATE_SHUTDOWN; return; } From d488f03380f5a87f66929e4e0b401fbd5711503c Mon Sep 17 00:00:00 2001 From: Vasili-Sk <1153192+Vasili-Sk@users.noreply.github.com> Date: Mon, 25 Oct 2021 20:45:11 +0300 Subject: [PATCH 2/5] Change length search to old style --- socketcandcl.c | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/socketcandcl.c b/socketcandcl.c index 3bcc29f..c71e896 100644 --- a/socketcandcl.c +++ b/socketcandcl.c @@ -302,7 +302,13 @@ inline void state_connected() { sscanf(buf, "< frame %x %*d.%*d %s >", &frame.can_id, data_str); - if (element_length(buf, 2) == 8) + char *s = buf + 7; + for (; ++s;) { + if (*s == ' ') { + break; + } + } + if ((s - buf - 7) > 4) frame.can_id |= CAN_EFF_FLAG; frame.can_dlc = strlen(data_str) / 2; @@ -322,8 +328,15 @@ inline void state_connected() { //force DLC 0 since it is undocumented feature frame.can_dlc = 0; frame.can_id |= CAN_RTR_FLAG; - if (element_length(buf, 2) == 8) - frame.can_id |= CAN_EFF_FLAG; //extended + + char *s = buf + 6; + for (; ++s;) { + if (*s == ' ') { + break; + } + } + if ((s - buf - 7) > 4) + frame.can_id |= CAN_EFF_FLAG; ret = write(raw_socket, &frame, sizeof(struct can_frame)); if (ret < sizeof(struct can_frame)) { From 6da4fd1cfe0524f2889422a07f4634702d141151 Mon Sep 17 00:00:00 2001 From: VasiliSk <1153192+Vasili-Sk@users.noreply.github.com> Date: Mon, 25 Oct 2021 22:37:46 +0300 Subject: [PATCH 3/5] Remove formatting from last commits --- socketcand.c | 2 +- socketcandcl.c | 213 +++++++++++++++++++++++----------------- state_bcm.c | 260 +++++++++++++++++++++++++++++++------------------ state_isotp.c | 76 +++++++++------ state_raw.c | 83 +++++++++------- 5 files changed, 380 insertions(+), 254 deletions(-) diff --git a/socketcand.c b/socketcand.c index b64ead1..6125be9 100644 --- a/socketcand.c +++ b/socketcand.c @@ -120,7 +120,7 @@ int state_changed(char *buf, int current_state) return (current_state != state); } -char* element_start(char *buf, int element) +char *element_start(char *buf, int element) { int len = strlen(buf); int elem, i; diff --git a/socketcandcl.c b/socketcandcl.c index c71e896..ac0a66a 100644 --- a/socketcandcl.c +++ b/socketcandcl.c @@ -86,22 +86,24 @@ void state_connected(); int server_socket; int raw_socket; int port; -int verbose_flag = 0; -int cmd_index = 0; -int more_elements = 0; +int verbose_flag=0; +int cmd_index=0; +int more_elements=0; int state, previous_state; char ldev[IFNAMSIZ]; char rdev[IFNAMSIZ]; char buf[MAXLEN]; char cmd_buffer[MAXLEN]; -int main(int argc, char **argv) { + +int main(int argc, char **argv) +{ int i; struct sockaddr_in serveraddr; struct hostent *server_ent; struct sigaction sigint_action; char buf[MAXLEN]; - char *server_string; + char* server_string; /* set default config settings */ port = PORT; @@ -109,22 +111,29 @@ int main(int argc, char **argv) { strcpy(rdev, "can0"); server_string = malloc(strlen("localhost")); + /* Parse commandline arguments */ - for (;;) { + for(;;) { /* getopt_long stores the option index here. */ int c, option_index = 0; - static struct option long_options[] = { { "verbose", no_argument, 0, 'v' }, { "interfaces", required_argument, 0, 'i' }, { "server", required_argument, 0, - 's' }, { "port", required_argument, 0, 'p' }, { "version", no_argument, 0, 'z' }, { 0, 0, 0, 0 } }; + static struct option long_options[] = { + {"verbose", no_argument, 0, 'v'}, + {"interfaces", required_argument, 0, 'i'}, + {"server", required_argument, 0, 's'}, + {"port", required_argument, 0, 'p'}, + {"version", no_argument, 0, 'z'}, + {0, 0, 0, 0} + }; c = getopt_long(argc, argv, "vhi:p:l:s:", long_options, &option_index); - if (c == -1) + if(c == -1) break; - switch (c) { + switch(c) { case 0: /* If this option set a flag, do nothing else now. */ - if (long_options[option_index].flag != 0) + if(long_options[option_index].flag != 0) break; break; @@ -138,7 +147,7 @@ int main(int argc, char **argv) { break; case 's': - server_string = realloc(server_string, strlen(optarg) + 1); + server_string = realloc(server_string, strlen(optarg)+1); strcpy(server_string, optarg); break; @@ -165,13 +174,14 @@ int main(int argc, char **argv) { } } + sigint_action.sa_handler = &sigint; sigemptyset(&sigint_action.sa_mask); sigint_action.sa_flags = 0; sigaction(SIGINT, &sigint_action, NULL); server_socket = socket(AF_INET, SOCK_STREAM, 0); - if (server_socket < 0) { + if(server_socket < 0) { perror("socket"); exit(1); } @@ -181,32 +191,35 @@ int main(int argc, char **argv) { serveraddr.sin_port = htons(port); server_ent = gethostbyname(server_string); - if (server_ent == 0) { + if(server_ent == 0) { perror(server_string); exit(1); } memcpy(&(serveraddr.sin_addr.s_addr), server_ent->h_addr, - server_ent->h_length); + server_ent->h_length); - if (connect(server_socket, (struct sockaddr*) &serveraddr, sizeof(serveraddr)) != 0) { + + if(connect(server_socket, (struct sockaddr*)&serveraddr, + sizeof(serveraddr)) != 0) { perror("connect"); exit(1); } - for (;;) { - switch (state) { + + for(;;) { + switch(state) { case STATE_INIT: /* has to start with a command */ - i = receive_command(server_socket, (char*) &buf); - if (i != 0) { + i = receive_command(server_socket, (char *) &buf); + if(i != 0) { PRINT_ERROR("Connection terminated while waiting for command.\n"); state = STATE_SHUTDOWN; previous_state = STATE_INIT; break; } - if (!strncmp("< hi", buf, 4)) { + if(!strncmp("< hi", buf, 4)) { /* send open and rawmode command */ sprintf(buf, "< open %s >", rdev); send(server_socket, buf, strlen(buf), 0); @@ -222,8 +235,7 @@ int main(int argc, char **argv) { state_connected(); break; case STATE_SHUTDOWN: - PRINT_VERBOSE("Closing client connection.\n") - ; + PRINT_VERBOSE("Closing client connection.\n"); close(server_socket); return 0; } @@ -231,7 +243,9 @@ int main(int argc, char **argv) { return 0; } -inline void state_connected() { + +inline void state_connected() +{ int ret; static struct can_frame frame; @@ -239,16 +253,16 @@ inline void state_connected() { static struct sockaddr_can addr; fd_set readfds; - if (previous_state != STATE_CONNECTED) { + if(previous_state != STATE_CONNECTED) { - if ((raw_socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { + if((raw_socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { PRINT_ERROR("Error while creating RAW socket %s\n", strerror(errno)); state = STATE_SHUTDOWN; return; } strcpy(ifr.ifr_name, ldev); - if (ioctl(raw_socket, SIOCGIFINDEX, &ifr) < 0) { + if(ioctl(raw_socket, SIOCGIFINDEX, &ifr) < 0) { PRINT_ERROR("Error while searching for bus %s\n", strerror(errno)); state = STATE_SHUTDOWN; return; @@ -259,13 +273,14 @@ inline void state_connected() { /* turn on timestamp */ const int timestamp_on = 0; - if (setsockopt(raw_socket, SOL_SOCKET, SO_TIMESTAMP, ×tamp_on, sizeof(timestamp_on)) < 0) { + if(setsockopt(raw_socket, SOL_SOCKET, SO_TIMESTAMP, + ×tamp_on, sizeof(timestamp_on)) < 0) { PRINT_ERROR("Could not enable CAN timestamps\n"); state = STATE_SHUTDOWN; return; } /* bind socket */ - if (bind(raw_socket, (struct sockaddr*) &addr, sizeof(addr)) < 0) { + if(bind(raw_socket, (struct sockaddr *) &addr, sizeof(addr)) < 0) { PRINT_ERROR("Error while binding RAW socket %s\n", strerror(errno)); state = STATE_SHUTDOWN; return; @@ -273,9 +288,10 @@ inline void state_connected() { previous_state = STATE_CONNECTED; } - if (fork()) { - for (;;) { + if(fork()) { + + for(;;) { FD_ZERO(&readfds); FD_SET(server_socket, &readfds); @@ -284,40 +300,44 @@ inline void state_connected() { * Check if there are more elements in the element buffer before * calling select() and blocking for new packets. */ - if (!more_elements) { - ret = select(server_socket + 1, &readfds, NULL, NULL, NULL); + if(!more_elements) { + ret = select(server_socket+1, &readfds, NULL, NULL, NULL); - if (ret < 0) { + if(ret < 0) { PRINT_ERROR("Error in select()\n") - state = STATE_SHUTDOWN; + state = STATE_SHUTDOWN; return; } } - if (FD_ISSET(server_socket, &readfds) || more_elements) { - ret = receive_command(server_socket, (char*) &buf); - if (ret == 0) { - if (!strncmp("< frame", buf, 7)) { - char data_str[2 * 8]; + if(FD_ISSET(server_socket, &readfds) || more_elements) { + ret = receive_command(server_socket, (char *) &buf); + if(ret == 0) { + if(!strncmp("< frame", buf, 7)) { + char data_str[2*8]; - sscanf(buf, "< frame %x %*d.%*d %s >", &frame.can_id, data_str); + sscanf(buf, "< frame %x %*d.%*d %s >", &frame.can_id, + data_str); - char *s = buf + 7; - for (; ++s;) { - if (*s == ' ') { + char* s = buf + 7; + for(; ++s;) { + if(*s== ' ') { break; } } - if ((s - buf - 7) > 4) + if((s - buf - 7) > 4) frame.can_id |= CAN_EFF_FLAG; frame.can_dlc = strlen(data_str) / 2; - sscanf(data_str, "%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx", &frame.data[0], &frame.data[1], &frame.data[2], &frame.data[3], - &frame.data[4], &frame.data[5], &frame.data[6], &frame.data[7]); + sscanf(data_str, "%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx%02hhx", + &frame.data[0], &frame.data[1], + &frame.data[2], &frame.data[3], + &frame.data[4], &frame.data[5], + &frame.data[6], &frame.data[7]); ret = write(raw_socket, &frame, sizeof(struct can_frame)); - if (ret < sizeof(struct can_frame)) { + if(ret') { + for(i=1; i') { stop = i; break; } } /* if no '>' is in the string we have to wait for more data */ - if (stop == -1) { + if(stop == -1) { return -1; } /* copy string to new destination and correct cmd_buffer */ - for (i = start; i <= stop; i++) { - buffer[i - start] = cmd_buffer[i]; + for(i=start; i<=stop; i++) { + buffer[i-start] = cmd_buffer[i]; } - buffer[i - start] = '\0'; + buffer[i-start] = '\0'; + /* if only this message was in the buffer we're done */ - if (stop == cmd_index - 1) { + if(stop == cmd_index-1) { cmd_index = 0; } else { /* check if there is a '<' after the stop */ start = -1; - for (i = stop; i < cmd_index; i++) { - if (cmd_buffer[i] == '<') { + for(i=stop; i') { + for(i=1; i') { stop = i; break; } } - if (stop != -1) { + if(stop != -1) { more_elements = 1; } } @@ -501,7 +524,9 @@ int receive_command(int socket, char *buffer) { return 0; } -void print_usage(void) { + +void print_usage(void) +{ printf("Usage: socketcandcl [-v | --verbose] [-i interfaces | --interfaces interfaces]\n\t\t[-s server | --server server ]\n\t\t[-p port | --port port]\n"); printf("Options:\n"); printf("\t-v activates verbose output to STDOUT\n"); @@ -511,24 +536,28 @@ void print_usage(void) { printf("\t-h prints this message\n"); } -void childdied() { + +void childdied() +{ wait(NULL); } -void sigint() { - if (verbose_flag) + +void sigint() +{ + if(verbose_flag) PRINT_ERROR("received SIGINT\n") - if (server_socket != -1) { - if (verbose_flag) - PRINT_INFO("closing server socket\n") - close(server_socket); - } + if(server_socket != -1) { + if(verbose_flag) + PRINT_INFO("closing server socket\n") + close(server_socket); + } - if (raw_socket != -1) { - if (verbose_flag) + if(raw_socket != -1) { + if(verbose_flag) PRINT_INFO("closing can socket\n") - close(raw_socket); + close(raw_socket); } exit(0); diff --git a/state_bcm.c b/state_bcm.c index 2c031c6..820d103 100644 --- a/state_bcm.c +++ b/state_bcm.c @@ -48,7 +48,7 @@ void state_bcm() { struct can_frame frame[257]; /* MAX_NFRAMES + MUX MASK */ } muxmsg; - if (previous_state != STATE_BCM) { + if(previous_state != STATE_BCM) { /* open BCM socket */ if ((sc = socket(PF_CAN, SOCK_DGRAM, CAN_BCM)) < 0) { PRINT_ERROR("Error while opening BCM socket %s\n", strerror(errno)); @@ -61,11 +61,11 @@ void state_bcm() { /* can_ifindex is set to 0 (any device) => need for sendto() */ PRINT_VERBOSE("connecting BCM socket...\n") - if (connect(sc, (struct sockaddr*) &caddr, sizeof(caddr)) < 0) { - PRINT_ERROR("Error while connecting BCM socket %s\n", strerror(errno)); - state = STATE_SHUTDOWN; - return; - } + if (connect(sc, (struct sockaddr *)&caddr, sizeof(caddr)) < 0) { + PRINT_ERROR("Error while connecting BCM socket %s\n", strerror(errno)); + state = STATE_SHUTDOWN; + return; + } previous_state = STATE_BCM; } @@ -77,14 +77,14 @@ void state_bcm() { * Check if there are more elements in the element buffer before calling select() and * blocking for new packets. */ - if (more_elements) { + if(more_elements) { FD_CLR(sc, &readfds); } else { - ret = select((sc > client_socket) ? sc + 1 : client_socket + 1, &readfds, NULL, NULL, NULL); + ret = select((sc > client_socket)?sc+1:client_socket+1, &readfds, NULL, NULL, NULL); - if (ret < 0) { + if(ret < 0) { PRINT_ERROR("Error in select()\n") - state = STATE_SHUTDOWN; + state = STATE_SHUTDOWN; return; } } @@ -92,35 +92,40 @@ void state_bcm() { if (FD_ISSET(sc, &readfds)) { struct timeval tv; - ret = recvfrom(sc, &msg, sizeof(msg), 0, (struct sockaddr*) &caddr, &caddrlen); + ret = recvfrom(sc, &msg, sizeof(msg), 0, + (struct sockaddr*)&caddr, &caddrlen); /* read timestamp data */ - if (ioctl(sc, SIOCGSTAMP, &tv) < 0) { + if(ioctl(sc, SIOCGSTAMP, &tv) < 0) { PRINT_ERROR("Could not receive timestamp\n"); } /* Check if this is an error frame */ - if (msg.msg_head.can_id & CAN_ERR_FLAG) { - if (msg.frame.can_dlc != CAN_ERR_DLC) { + if(msg.msg_head.can_id & CAN_ERR_FLAG) { + if(msg.frame.can_dlc != CAN_ERR_DLC) { PRINT_ERROR("Error frame has a wrong DLC!\n") - } else { + } else { snprintf(rxmsg, RXLEN, "< error %03X %ld.%06ld ", msg.msg_head.can_id, tv.tv_sec, tv.tv_usec); - for (i = 0; i < msg.frame.can_dlc; i++) - snprintf(rxmsg + strlen(rxmsg), RXLEN - strlen(rxmsg), "%02X ", msg.frame.data[i]); + for ( i = 0; i < msg.frame.can_dlc; i++) + snprintf(rxmsg + strlen(rxmsg), RXLEN - strlen(rxmsg), "%02X ", + msg.frame.data[i]); snprintf(rxmsg + strlen(rxmsg), RXLEN - strlen(rxmsg), " >"); send(client_socket, rxmsg, strlen(rxmsg), 0); } } else { - if (msg.msg_head.can_id & CAN_EFF_FLAG) { - snprintf(rxmsg, RXLEN, "< frame %08X %ld.%06ld ", msg.msg_head.can_id & CAN_EFF_MASK, tv.tv_sec, tv.tv_usec); + if(msg.msg_head.can_id & CAN_EFF_FLAG) { + snprintf(rxmsg, RXLEN, "< frame %08X %ld.%06ld ", + msg.msg_head.can_id & CAN_EFF_MASK, tv.tv_sec, tv.tv_usec); } else { - snprintf(rxmsg, RXLEN, "< frame %03X %ld.%06ld ", msg.msg_head.can_id & CAN_SFF_MASK, tv.tv_sec, tv.tv_usec); + snprintf(rxmsg, RXLEN, "< frame %03X %ld.%06ld ", + msg.msg_head.can_id & CAN_SFF_MASK, tv.tv_sec, tv.tv_usec); } - for (i = 0; i < msg.frame.can_dlc; i++) - snprintf(rxmsg + strlen(rxmsg), RXLEN - strlen(rxmsg), "%02X ", msg.frame.data[i]); + for ( i = 0; i < msg.frame.can_dlc; i++) + snprintf(rxmsg + strlen(rxmsg), RXLEN - strlen(rxmsg), "%02X ", + msg.frame.data[i]); snprintf(rxmsg + strlen(rxmsg), RXLEN - strlen(rxmsg), " >"); send(client_socket, rxmsg, strlen(rxmsg), 0); @@ -132,7 +137,7 @@ void state_bcm() { ret = receive_command(client_socket, buf); - if (ret != 0) { + if(ret != 0) { state = STATE_SHUTDOWN; return; } @@ -150,25 +155,36 @@ void state_bcm() { return; } - if (!strcmp("< echo >", buf)) { + if(!strcmp("< echo >", buf)) { send(client_socket, buf, strlen(buf), 0); return; } /* Send a single frame */ - if (!strncmp("< send ", buf, 7)) { + if(!strncmp("< send ", buf, 7)) { items = sscanf(buf, "< %*s %x %hhu " - "%hhx %hhx %hhx %hhx %hhx %hhx " - "%hhx %hhx >", &msg.msg_head.can_id, &msg.frame.can_dlc, &msg.frame.data[0], &msg.frame.data[1], &msg.frame.data[2], &msg.frame.data[3], - &msg.frame.data[4], &msg.frame.data[5], &msg.frame.data[6], &msg.frame.data[7]); - - if ((items < 2) || (msg.frame.can_dlc > 8) || (items != 2 + msg.frame.can_dlc)) { + "%hhx %hhx %hhx %hhx %hhx %hhx " + "%hhx %hhx >", + &msg.msg_head.can_id, + &msg.frame.can_dlc, + &msg.frame.data[0], + &msg.frame.data[1], + &msg.frame.data[2], + &msg.frame.data[3], + &msg.frame.data[4], + &msg.frame.data[5], + &msg.frame.data[6], + &msg.frame.data[7]); + + if ( (items < 2) || + (msg.frame.can_dlc > 8) || + (items != 2 + msg.frame.can_dlc)) { PRINT_ERROR("Syntax error in send command\n") - return; + return; } /* < send XXXXXXXX ... > check for extended identifier */ - if (element_length(buf, 2) == 8) + if(element_length(buf, 2) == 8) msg.msg_head.can_id |= CAN_EFF_FLAG; msg.msg_head.opcode = TX_SEND; @@ -176,7 +192,8 @@ void state_bcm() { if (!ioctl(sc, SIOCGIFINDEX, &ifr)) { caddr.can_ifindex = ifr.ifr_ifindex; - sendto(sc, &msg, sizeof(msg), 0, (struct sockaddr*) &caddr, sizeof(caddr)); + sendto(sc, &msg, sizeof(msg), 0, + (struct sockaddr*)&caddr, sizeof(caddr)); } /* Add a send job */ } else if (!strncmp("< sendrtr ", buf, 10)) { @@ -199,17 +216,30 @@ void state_bcm() { } } else if (!strncmp("< add ", buf, 6)) { items = sscanf(buf, "< %*s %lu %lu %x %hhu " - "%hhx %hhx %hhx %hhx %hhx %hhx " - "%hhx %hhx >", &msg.msg_head.ival2.tv_sec, &msg.msg_head.ival2.tv_usec, &msg.msg_head.can_id, &msg.frame.can_dlc, &msg.frame.data[0], - &msg.frame.data[1], &msg.frame.data[2], &msg.frame.data[3], &msg.frame.data[4], &msg.frame.data[5], &msg.frame.data[6], &msg.frame.data[7]); - - if ((items < 4) || (msg.frame.can_dlc > 8) || (items != 4 + msg.frame.can_dlc)) { + "%hhx %hhx %hhx %hhx %hhx %hhx " + "%hhx %hhx >", + &msg.msg_head.ival2.tv_sec, + &msg.msg_head.ival2.tv_usec, + &msg.msg_head.can_id, + &msg.frame.can_dlc, + &msg.frame.data[0], + &msg.frame.data[1], + &msg.frame.data[2], + &msg.frame.data[3], + &msg.frame.data[4], + &msg.frame.data[5], + &msg.frame.data[6], + &msg.frame.data[7]); + + if( (items < 4) || + (msg.frame.can_dlc > 8) || + (items != 4 + msg.frame.can_dlc) ) { PRINT_ERROR("Syntax error in add command.\n"); return; } /* < add sec usec XXXXXXXX ... > check for extended identifier */ - if (element_length(buf, 4) == 8) + if(element_length(buf, 4) == 8) msg.msg_head.can_id |= CAN_EFF_FLAG; msg.msg_head.opcode = TX_SETUP; @@ -218,43 +248,57 @@ void state_bcm() { if (!ioctl(sc, SIOCGIFINDEX, &ifr)) { caddr.can_ifindex = ifr.ifr_ifindex; - sendto(sc, &msg, sizeof(msg), 0, (struct sockaddr*) &caddr, sizeof(caddr)); + sendto(sc, &msg, sizeof(msg), 0, + (struct sockaddr*)&caddr, sizeof(caddr)); } /* Update send job */ - } else if (!strncmp("< update ", buf, 9)) { + } else if(!strncmp("< update ", buf, 9)) { items = sscanf(buf, "< %*s %x %hhu " - "%hhx %hhx %hhx %hhx %hhx %hhx " - "%hhx %hhx >", &msg.msg_head.can_id, &msg.frame.can_dlc, &msg.frame.data[0], &msg.frame.data[1], &msg.frame.data[2], &msg.frame.data[3], - &msg.frame.data[4], &msg.frame.data[5], &msg.frame.data[6], &msg.frame.data[7]); - - if ((items < 2) || (msg.frame.can_dlc > 8) || (items != 2 + msg.frame.can_dlc)) { + "%hhx %hhx %hhx %hhx %hhx %hhx " + "%hhx %hhx >", + &msg.msg_head.can_id, + &msg.frame.can_dlc, + &msg.frame.data[0], + &msg.frame.data[1], + &msg.frame.data[2], + &msg.frame.data[3], + &msg.frame.data[4], + &msg.frame.data[5], + &msg.frame.data[6], + &msg.frame.data[7]); + + if ( (items < 2) || + (msg.frame.can_dlc > 8) || + (items != 2 + msg.frame.can_dlc)) { PRINT_ERROR("Syntax error in update send job command\n") - return; + return; } /* < update XXXXXXXX ... > check for extended identifier */ - if (element_length(buf, 2) == 8) + if(element_length(buf, 2) == 8) msg.msg_head.can_id |= CAN_EFF_FLAG; msg.msg_head.opcode = TX_SETUP; - msg.msg_head.flags = 0; + msg.msg_head.flags = 0; msg.frame.can_id = msg.msg_head.can_id; if (!ioctl(sc, SIOCGIFINDEX, &ifr)) { caddr.can_ifindex = ifr.ifr_ifindex; - sendto(sc, &msg, sizeof(msg), 0, (struct sockaddr*) &caddr, sizeof(caddr)); + sendto(sc, &msg, sizeof(msg), 0, + (struct sockaddr*)&caddr, sizeof(caddr)); } /* Delete a send job */ - } else if (!strncmp("< delete ", buf, 9)) { - items = sscanf(buf, "< %*s %x >", &msg.msg_head.can_id); + } else if(!strncmp("< delete ", buf, 9)) { + items = sscanf(buf, "< %*s %x >", + &msg.msg_head.can_id); - if (items != 1) { + if (items != 1) { PRINT_ERROR("Syntax error in delete job command\n") - return; + return; } /* < delete XXXXXXXX ... > check for extended identifier */ - if (element_length(buf, 2) == 8) + if(element_length(buf, 2) == 8) msg.msg_head.can_id |= CAN_EFF_FLAG; msg.msg_head.opcode = TX_DELETE; @@ -262,64 +306,84 @@ void state_bcm() { if (!ioctl(sc, SIOCGIFINDEX, &ifr)) { caddr.can_ifindex = ifr.ifr_ifindex; - sendto(sc, &msg, sizeof(msg), 0, (struct sockaddr*) &caddr, sizeof(caddr)); + sendto(sc, &msg, sizeof(msg), 0, + (struct sockaddr*)&caddr, sizeof(caddr)); } /* Receive CAN ID with content matching */ - } else if (!strncmp("< filter ", buf, 9)) { + } else if(!strncmp("< filter ", buf, 9)) { items = sscanf(buf, "< %*s %lu %lu %x %hhu " - "%hhx %hhx %hhx %hhx %hhx %hhx " - "%hhx %hhx >", &msg.msg_head.ival2.tv_sec, &msg.msg_head.ival2.tv_usec, &msg.msg_head.can_id, &msg.frame.can_dlc, &msg.frame.data[0], - &msg.frame.data[1], &msg.frame.data[2], &msg.frame.data[3], &msg.frame.data[4], &msg.frame.data[5], &msg.frame.data[6], &msg.frame.data[7]); - - if ((items < 4) || (msg.frame.can_dlc > 8) || (items != 4 + msg.frame.can_dlc)) { + "%hhx %hhx %hhx %hhx %hhx %hhx " + "%hhx %hhx >", + &msg.msg_head.ival2.tv_sec, + &msg.msg_head.ival2.tv_usec, + &msg.msg_head.can_id, + &msg.frame.can_dlc, + &msg.frame.data[0], + &msg.frame.data[1], + &msg.frame.data[2], + &msg.frame.data[3], + &msg.frame.data[4], + &msg.frame.data[5], + &msg.frame.data[6], + &msg.frame.data[7]); + + if( (items < 4) || + (msg.frame.can_dlc > 8) || + (items != 4 + msg.frame.can_dlc) ) { PRINT_ERROR("syntax error in filter command.\n") - return; + return; } /* < filter sec usec XXXXXXXX ... > check for extended identifier */ - if (element_length(buf, 4) == 8) + if(element_length(buf, 4) == 8) msg.msg_head.can_id |= CAN_EFF_FLAG; msg.msg_head.opcode = RX_SETUP; - msg.msg_head.flags = SETTIMER; + msg.msg_head.flags = SETTIMER; msg.frame.can_id = msg.msg_head.can_id; if (!ioctl(sc, SIOCGIFINDEX, &ifr)) { caddr.can_ifindex = ifr.ifr_ifindex; - sendto(sc, &msg, sizeof(msg), 0, (struct sockaddr*) &caddr, sizeof(caddr)); + sendto(sc, &msg, sizeof(msg), 0, + (struct sockaddr*)&caddr, sizeof(caddr)); } /* Receive CAN ID with multiplex content matching */ - } else if (!strncmp("< muxfilter ", buf, 12)) { + } else if(!strncmp("< muxfilter ", buf, 12)) { char *cfptr; char tmp; memset(&muxmsg, 0, sizeof(muxmsg)); - items = sscanf(buf, "< %*s %lu %lu %x %u ", &muxmsg.msg_head.ival2.tv_sec, &muxmsg.msg_head.ival2.tv_usec, &muxmsg.msg_head.can_id, - &muxmsg.msg_head.nframes); + items = sscanf(buf, "< %*s %lu %lu %x %u ", + &muxmsg.msg_head.ival2.tv_sec, + &muxmsg.msg_head.ival2.tv_usec, + &muxmsg.msg_head.can_id, + &muxmsg.msg_head.nframes); - if ((items != 4) || (muxmsg.msg_head.nframes < 2) || (muxmsg.msg_head.nframes > 257)) { + if( (items != 4) || + (muxmsg.msg_head.nframes < 2) || + (muxmsg.msg_head.nframes > 257) ) { PRINT_ERROR("syntax error in muxfilter command.\n") - return; + return; } /* < muxfilter sec usec XXXXXXXX ... > check for extended identifier */ - if (element_length(buf, 4) == 8) + if(element_length(buf, 4) == 8) muxmsg.msg_head.can_id |= CAN_EFF_FLAG; muxmsg.msg_head.opcode = RX_SETUP; - muxmsg.msg_head.flags = SETTIMER; + muxmsg.msg_head.flags = SETTIMER; cfptr = element_start(buf, 6); if (cfptr == NULL) { PRINT_ERROR("failed to find filter data start in muxfilter.\n") - return; + return; } - if (strlen(cfptr) < muxmsg.msg_head.nframes * 24) { + if (strlen(cfptr) < muxmsg.msg_head.nframes * 24) { PRINT_ERROR("muxfilter data too short.\n") - return; + return; } /* copy filter data and mux mask in muxmsg.frame[0] */ @@ -327,18 +391,18 @@ void state_bcm() { for (j = 0; j < 8; j++) { - tmp = asc2nibble(cfptr[(24 * i + 3 * j)]); + tmp = asc2nibble(cfptr[(24*i + 3*j)]); if (tmp > 0x0F) { PRINT_ERROR("failed to process filter data in muxfilter.\n") - return; + return; } muxmsg.frame[i].data[j] = (tmp << 4); - tmp = asc2nibble(cfptr[(24 * i + 3 * j) + 1]); + tmp = asc2nibble(cfptr[(24*i + 3*j)+1]); if (tmp > 0x0F) { PRINT_ERROR("failed to process filter data in muxfilter.\n") - return; + return; } muxmsg.frame[i].data[j] |= tmp; @@ -347,52 +411,60 @@ void state_bcm() { if (!ioctl(sc, SIOCGIFINDEX, &ifr)) { caddr.can_ifindex = ifr.ifr_ifindex; - sendto(sc, &muxmsg, sizeof(struct bcm_msg_head) + sizeof(struct can_frame) * muxmsg.msg_head.nframes, 0, (struct sockaddr*) &caddr, sizeof(caddr)); + sendto(sc, &muxmsg, sizeof(struct bcm_msg_head) + + sizeof(struct can_frame) * muxmsg.msg_head.nframes, + 0, (struct sockaddr*)&caddr, sizeof(caddr)); } /* Add a filter */ - } else if (!strncmp("< subscribe ", buf, 12)) { - items = sscanf(buf, "< %*s %lu %lu %x >", &msg.msg_head.ival2.tv_sec, &msg.msg_head.ival2.tv_usec, &msg.msg_head.can_id); + } else if(!strncmp("< subscribe ", buf, 12)) { + items = sscanf(buf, "< %*s %lu %lu %x >", + &msg.msg_head.ival2.tv_sec, + &msg.msg_head.ival2.tv_usec, + &msg.msg_head.can_id); if (items != 3) { PRINT_ERROR("syntax error in subscribe command\n") - return; + return; } /* < subscribe sec usec XXXXXXXX ... > check for extended identifier */ - if (element_length(buf, 4) == 8) + if(element_length(buf, 4) == 8) msg.msg_head.can_id |= CAN_EFF_FLAG; msg.msg_head.opcode = RX_SETUP; - msg.msg_head.flags = RX_FILTER_ID | SETTIMER; + msg.msg_head.flags = RX_FILTER_ID | SETTIMER; msg.frame.can_id = msg.msg_head.can_id; if (!ioctl(sc, SIOCGIFINDEX, &ifr)) { caddr.can_ifindex = ifr.ifr_ifindex; - sendto(sc, &msg, sizeof(msg), 0, (struct sockaddr*) &caddr, sizeof(caddr)); + sendto(sc, &msg, sizeof(msg), 0, + (struct sockaddr*)&caddr, sizeof(caddr)); } /* Delete filter */ - } else if (!strncmp("< unsubscribe ", buf, 14)) { - items = sscanf(buf, "< %*s %x >", &msg.msg_head.can_id); + } else if(!strncmp("< unsubscribe ", buf, 14)) { + items = sscanf(buf, "< %*s %x >", + &msg.msg_head.can_id); if (items != 1) { PRINT_ERROR("syntax error in unsubscribe command\n") - return; + return; } /* < unsubscribe XXXXXXXX ... > check for extended identifier */ - if (element_length(buf, 2) == 8) + if(element_length(buf, 2) == 8) msg.msg_head.can_id |= CAN_EFF_FLAG; msg.msg_head.opcode = RX_DELETE; msg.frame.can_id = msg.msg_head.can_id; - + if (!ioctl(sc, SIOCGIFINDEX, &ifr)) { caddr.can_ifindex = ifr.ifr_ifindex; - sendto(sc, &msg, sizeof(msg), 0, (struct sockaddr*) &caddr, sizeof(caddr)); + sendto(sc, &msg, sizeof(msg), 0, + (struct sockaddr*)&caddr, sizeof(caddr)); } } else { PRINT_ERROR("unknown command '%s'.\n", buf) - strcpy(buf, "< error unknown command >"); + strcpy(buf, "< error unknown command >"); send(client_socket, buf, strlen(buf), 0); } } diff --git a/state_isotp.c b/state_isotp.c index ce8c7f4..e1b944b 100644 --- a/state_isotp.c +++ b/state_isotp.c @@ -36,14 +36,14 @@ void state_isotp() { char rxmsg[MAXLEN]; /* can to inet */ char buf[MAXLEN]; /* inet commands to can */ - unsigned char isobuf[ISOTPLEN + 1]; /* binary buffer for isotp socket */ + unsigned char isobuf[ISOTPLEN+1]; /* binary buffer for isotp socket */ unsigned char tmp; fd_set readfds; - - while (previous_state != STATE_ISOTP) { + + while(previous_state != STATE_ISOTP) { ret = receive_command(client_socket, buf); - if (ret != 0) { + if(ret != 0) { state = STATE_SHUTDOWN; return; } @@ -58,7 +58,7 @@ void state_isotp() { return; } - if (!strcmp("< echo >", buf)) { + if(!strcmp("< echo >", buf)) { send(client_socket, buf, strlen(buf), 0); continue; } @@ -68,21 +68,33 @@ void state_isotp() { memset(&addr, 0, sizeof(addr)); /* get configuration to open the socket */ - if (!strncmp("< isotpconf ", buf, 12)) { + if(!strncmp("< isotpconf ", buf, 12)) { items = sscanf(buf, "< %*s %x %x %x " - "%hhu %hhx %hhu " - "%hhx %hhx %hhx %hhx >", &addr.can_addr.tp.tx_id, &addr.can_addr.tp.rx_id, &opts.flags, &fcopts.bs, &fcopts.stmin, &fcopts.wftmax, - &opts.txpad_content, &opts.rxpad_content, &opts.ext_address, &opts.rx_ext_address); + "%hhu %hhx %hhu " + "%hhx %hhx %hhx %hhx >", + &addr.can_addr.tp.tx_id, + &addr.can_addr.tp.rx_id, + &opts.flags, + &fcopts.bs, + &fcopts.stmin, + &fcopts.wftmax, + &opts.txpad_content, + &opts.rxpad_content, + &opts.ext_address, + &opts.rx_ext_address); /* < isotpconf XXXXXXXX ... > check for extended identifier */ - if (element_length(buf, 2) == 8) + if(element_length(buf, 2) == 8) addr.can_addr.tp.tx_id |= CAN_EFF_FLAG; - if (element_length(buf, 3) == 8) + if(element_length(buf, 3) == 8) addr.can_addr.tp.rx_id |= CAN_EFF_FLAG; - if ((opts.flags & CAN_ISOTP_RX_EXT_ADDR && items < 10) || (opts.flags & CAN_ISOTP_EXTEND_ADDR && items < 9) - || (opts.flags & CAN_ISOTP_RX_PADDING && items < 8) || (opts.flags & CAN_ISOTP_TX_PADDING && items < 7) || (items < 5)) { + if ((opts.flags & CAN_ISOTP_RX_EXT_ADDR && items < 10) || + (opts.flags & CAN_ISOTP_EXTEND_ADDR && items < 9) || + (opts.flags & CAN_ISOTP_RX_PADDING && items < 8) || + (opts.flags & CAN_ISOTP_TX_PADDING && items < 7) || + (items < 5)) { PRINT_ERROR("Syntax error in isotpconf command\n"); /* try it once more */ continue; @@ -98,7 +110,7 @@ void state_isotp() { } strcpy(ifr.ifr_name, bus_name); - if (ioctl(si, SIOCGIFINDEX, &ifr) < 0) { + if(ioctl(si, SIOCGIFINDEX, &ifr) < 0) { PRINT_ERROR("Error while searching for bus %s\n", strerror(errno)); /* ensure proper handling in other states */ previous_state = STATE_ISOTP; @@ -116,14 +128,14 @@ void state_isotp() { setsockopt(si, SOL_CAN_ISOTP, CAN_ISOTP_RECV_FC, &fcopts, sizeof(fcopts)); PRINT_VERBOSE("binding ISOTP socket...\n") - if (bind(si, (struct sockaddr*) &addr, sizeof(addr)) < 0) { + if (bind(si, (struct sockaddr *)&addr, sizeof(addr)) < 0) { PRINT_ERROR("Error while binding ISOTP socket %s\n", strerror(errno)); /* ensure proper handling in other states */ previous_state = STATE_ISOTP; state = STATE_SHUTDOWN; return; } - + /* ok we made it and have a proper isotp socket open */ previous_state = STATE_ISOTP; } @@ -137,25 +149,25 @@ void state_isotp() { * Check if there are more elements in the element buffer before calling select() and * blocking for new packets. */ - if (more_elements) { + if(more_elements) { FD_CLR(si, &readfds); } else { - ret = select((si > client_socket) ? si + 1 : client_socket + 1, &readfds, NULL, NULL, NULL); - if (ret < 0) { + ret = select((si > client_socket)?si+1:client_socket+1, &readfds, NULL, NULL, NULL); + if(ret < 0) { PRINT_ERROR("Error in select()\n") - state = STATE_SHUTDOWN; + state = STATE_SHUTDOWN; return; } } if (FD_ISSET(si, &readfds)) { - struct timeval tv = { 0 }; + struct timeval tv = {0}; items = read(si, isobuf, ISOTPLEN); /* read timestamp data */ - if (ioctl(si, SIOCGSTAMP, &tv) < 0) { + if(ioctl(si, SIOCGSTAMP, &tv) < 0) { PRINT_ERROR("Could not receive timestamp\n"); } @@ -166,8 +178,8 @@ void state_isotp() { sprintf(rxmsg, "< pdu %ld.%06ld ", tv.tv_sec, tv.tv_usec); startlen = strlen(rxmsg); - for (i = 0; i < items; i++) - sprintf(rxmsg + startlen + 2 * i, "%02X", isobuf[i]); + for (i=0; i < items; i++) + sprintf(rxmsg + startlen + 2*i, "%02X", isobuf[i]); sprintf(rxmsg + strlen(rxmsg), " >"); send(client_socket, rxmsg, strlen(rxmsg), 0); @@ -177,7 +189,7 @@ void state_isotp() { if (FD_ISSET(client_socket, &readfds)) { ret = receive_command(client_socket, buf); - if (ret != 0) { + if(ret != 0) { state = STATE_SHUTDOWN; return; } @@ -189,12 +201,12 @@ void state_isotp() { return; } - if (!strcmp("< echo >", buf)) { + if(!strcmp("< echo >", buf)) { send(client_socket, buf, strlen(buf), 0); return; } - if (!strncmp("< sendpdu ", buf, 10)) { + if(!strncmp("< sendpdu ", buf, 10)) { items = element_length(buf, 2); if (items & 1) { PRINT_ERROR("odd number of ASCII Hex values\n"); @@ -209,25 +221,25 @@ void state_isotp() { for (i = 0; i < items; i++) { - tmp = asc2nibble(buf[(2 * i) + 10]); + tmp = asc2nibble(buf[(2*i) + 10]); if (tmp > 0x0F) return; isobuf[i] = (tmp << 4); - tmp = asc2nibble(buf[(2 * i) + 11]); + tmp = asc2nibble(buf[(2*i) + 11]); if (tmp > 0x0F) return; isobuf[i] |= tmp; } ret = write(si, isobuf, items); - if (ret != items) { + if(ret != items) { PRINT_ERROR("Error in write()\n") - state = STATE_SHUTDOWN; + state = STATE_SHUTDOWN; return; } } else { PRINT_ERROR("unknown command '%s'.\n", buf) - strcpy(buf, "< error unknown command >"); + strcpy(buf, "< error unknown command >"); send(client_socket, buf, strlen(buf), 0); } } diff --git a/state_raw.c b/state_raw.c index 1fa8c72..f01edec 100644 --- a/state_raw.c +++ b/state_raw.c @@ -28,7 +28,7 @@ struct sockaddr_can addr; struct msghdr msg; struct can_frame frame; struct iovec iov; -char ctrlmsg[CMSG_SPACE(sizeof(struct timeval)) + CMSG_SPACE(sizeof(__u32 ))]; +char ctrlmsg[CMSG_SPACE(sizeof(struct timeval)) + CMSG_SPACE(sizeof(__u32))]; struct timeval tv; struct cmsghdr *cmsg; @@ -37,16 +37,16 @@ void state_raw() { int i, ret, items; fd_set readfds; - if (previous_state != STATE_RAW) { + if(previous_state != STATE_RAW) { - if ((raw_socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { + if((raw_socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { PRINT_ERROR("Error while creating RAW socket %s\n", strerror(errno)); state = STATE_SHUTDOWN; return; } strcpy(ifr.ifr_name, bus_name); - if (ioctl(raw_socket, SIOCGIFINDEX, &ifr) < 0) { + if(ioctl(raw_socket, SIOCGIFINDEX, &ifr) < 0) { PRINT_ERROR("Error while searching for bus %s\n", strerror(errno)); state = STATE_SHUTDOWN; return; @@ -56,13 +56,13 @@ void state_raw() { addr.can_ifindex = ifr.ifr_ifindex; const int timestamp_on = 1; - if (setsockopt(raw_socket, SOL_SOCKET, SO_TIMESTAMP, ×tamp_on, sizeof(timestamp_on)) < 0) { + if(setsockopt( raw_socket, SOL_SOCKET, SO_TIMESTAMP, ×tamp_on, sizeof(timestamp_on)) < 0) { PRINT_ERROR("Could not enable CAN timestamps\n"); state = STATE_SHUTDOWN; return; } - if (bind(raw_socket, (struct sockaddr*) &addr, sizeof(addr)) < 0) { + if(bind(raw_socket, (struct sockaddr *) &addr, sizeof(addr)) < 0) { PRINT_ERROR("Error while binding RAW socket %s\n", strerror(errno)); state = STATE_SHUTDOWN; return; @@ -85,37 +85,39 @@ void state_raw() { * Check if there are more elements in the element buffer before calling select() and * blocking for new packets. */ - if (more_elements) { + if(more_elements) { FD_CLR(raw_socket, &readfds); } else { - ret = select((raw_socket > client_socket) ? raw_socket + 1 : client_socket + 1, &readfds, NULL, NULL, NULL); + ret = select((raw_socket > client_socket)?raw_socket+1:client_socket+1, &readfds, NULL, NULL, NULL); - if (ret < 0) { + if(ret < 0) { PRINT_ERROR("Error in select()\n") - state = STATE_SHUTDOWN; + state = STATE_SHUTDOWN; return; } } - if (FD_ISSET(raw_socket, &readfds)) { + if(FD_ISSET(raw_socket, &readfds)) { iov.iov_len = sizeof(frame); msg.msg_namelen = sizeof(addr); msg.msg_flags = 0; msg.msg_controllen = sizeof(ctrlmsg); ret = recvmsg(raw_socket, &msg, 0); - if (ret < sizeof(struct can_frame)) { + if(ret < sizeof(struct can_frame)) { PRINT_ERROR("Error reading frame from RAW socket\n") - } else { + } else { /* read timestamp data */ - for (cmsg = CMSG_FIRSTHDR(&msg); cmsg && (cmsg->cmsg_level == SOL_SOCKET); cmsg = CMSG_NXTHDR(&msg, cmsg)) { + for (cmsg = CMSG_FIRSTHDR(&msg); + cmsg && (cmsg->cmsg_level == SOL_SOCKET); + cmsg = CMSG_NXTHDR(&msg,cmsg)) { if (cmsg->cmsg_type == SO_TIMESTAMP) { - tv = *(struct timeval*) CMSG_DATA(cmsg); + tv = *(struct timeval *)CMSG_DATA(cmsg); } } - if (frame.can_id & CAN_ERR_FLAG) { - canid_t class = frame.can_id & CAN_EFF_MASK; + if(frame.can_id & CAN_ERR_FLAG) { + canid_t class = frame.can_id & CAN_EFF_MASK; ret = sprintf(buf, "< error %03X %ld.%06ld >", class, tv.tv_sec, tv.tv_usec); send(client_socket, buf, strlen(buf), 0); } else if (frame.can_id & CAN_RTR_FLAG) { @@ -126,24 +128,24 @@ void state_raw() { } send(client_socket, buf, strlen(buf), 0); } else { - if (frame.can_id & CAN_EFF_FLAG) { + if(frame.can_id & CAN_EFF_FLAG) { ret = sprintf(buf, "< frame %08X %ld.%06ld ", frame.can_id & CAN_EFF_MASK, tv.tv_sec, tv.tv_usec); } else { ret = sprintf(buf, "< frame %03X %ld.%06ld ", frame.can_id & CAN_SFF_MASK, tv.tv_sec, tv.tv_usec); } - for (i = 0; i < frame.can_dlc; i++) { - ret += sprintf(buf + ret, "%02X", frame.data[i]); + for(i=0;i"); + sprintf(buf+ret, " >"); send(client_socket, buf, strlen(buf), 0); } } } - if (FD_ISSET(client_socket, &readfds)) { - ret = receive_command(client_socket, (char*) &buf); + if(FD_ISSET(client_socket, &readfds)) { + ret = receive_command(client_socket, (char *) &buf); - if (ret == 0) { + if(ret == 0) { if (state_changed(buf, state)) { close(raw_socket); @@ -152,29 +154,40 @@ void state_raw() { return; } - if (!strcmp("< echo >", buf)) { + if(!strcmp("< echo >", buf)) { send(client_socket, buf, strlen(buf), 0); return; } /* Send a single frame */ - if (!strncmp("< send ", buf, 7)) { + if(!strncmp("< send ", buf, 7)) { items = sscanf(buf, "< %*s %x %hhu " - "%hhx %hhx %hhx %hhx %hhx %hhx " - "%hhx %hhx >", &frame.can_id, &frame.can_dlc, &frame.data[0], &frame.data[1], &frame.data[2], &frame.data[3], &frame.data[4], &frame.data[5], - &frame.data[6], &frame.data[7]); - - if ((items < 2) || (frame.can_dlc > 8) || (items != 2 + frame.can_dlc)) { + "%hhx %hhx %hhx %hhx %hhx %hhx " + "%hhx %hhx >", + &frame.can_id, + &frame.can_dlc, + &frame.data[0], + &frame.data[1], + &frame.data[2], + &frame.data[3], + &frame.data[4], + &frame.data[5], + &frame.data[6], + &frame.data[7]); + + if ( (items < 2) || + (frame.can_dlc > 8) || + (items != 2 + frame.can_dlc)) { PRINT_ERROR("Syntax error in send command\n") - return; + return; } /* < send XXXXXXXX ... > check for extended identifier */ - if (element_length(buf, 2) == 8) + if(element_length(buf, 2) == 8) frame.can_id |= CAN_EFF_FLAG; ret = send(raw_socket, &frame, sizeof(struct can_frame), 0); - if (ret == -1) { + if(ret==-1) { state = STATE_SHUTDOWN; return; } @@ -208,7 +221,7 @@ void state_raw() { } } else { ret = read(client_socket, &buf, 0); - if (ret == -1) { + if(ret==-1) { state = STATE_SHUTDOWN; return; } From dbf4276eb89c04edd3c5f2b2d73dae54d57ede4e Mon Sep 17 00:00:00 2001 From: Vasili-Sk <1153192+Vasili-Sk@users.noreply.github.com> Date: Tue, 26 Oct 2021 10:59:43 +0300 Subject: [PATCH 4/5] Added raspberrypi manual --- doc/raspberrypi.md | 77 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 77 insertions(+) create mode 100644 doc/raspberrypi.md diff --git a/doc/raspberrypi.md b/doc/raspberrypi.md new file mode 100644 index 0000000..7b0f62e --- /dev/null +++ b/doc/raspberrypi.md @@ -0,0 +1,77 @@ +Running on Raspberry PI +=================== + +For this example used adapter-board: https://www.waveshare.com/rs485-can-hat.htm you can find it on aliexpress too. +After flashing Raspberry image to SD card modify 'config.txt', uncomment this like: + + dtparam=spi=on + +Add this line: + + dtoverlay=mcp2515-can0,oscillator=12000000,interrupt=25,spimaxfrequency=5000000 + +Save file and create empty 'SSH' file if you want to remotely login over ethenet to pi. +For Windows (10+) use: + + ssh raspberrypi.local -l pi + +Password: raspberry + +You can check if CAN driver initialized successfully with: + + dmesg | grep -i '\(can\|spi\)' + +Prepare something needed for build + + sudo apt-get install libconfig-dev + +Now lets clone socketcand to sd card and build it + + cd boot + sudo git clone https://github.com/linux-can/socketcand.git + cd socketcand + sudo ./autogen.sh + sudo ./configure + sudo make + +Now you can install it into system + + sudo make install + +After that you may want to make it run on boot as service, run this command to edit it + + sudo systemctl edit --force --full socketcand.service + +Replace content with this rescription: + + [Unit] + Description=CAN ethernet + After=server.service multi-user.target + + [Service] + ExecStart= + ExecStart=-/usr/local/bin/socketcand -i can0 + Restart=always + TimeoutSec=10 + + [Install] + WantedBy=multi-user.target + +Try to start it with + + sudo systemctl daemon-reload + sudo systemctl start socketcand.service + +And listen on UDP default port 42000 for discovery message. + + + can://192.168.1.221:29536 + +Note that it should say 'can0' and not 'vcan0'. +If everything ok so far, what is quite suprisingly, then run this command to activate service on boot. + + sudo systemctl enable socketcand.service + sudo reboot + +Check for discovery again. +Now you can connect to IP: raspberrypi.local on TCP port 29536 (default) to start working with remote CAN. Check protocol file for commands and its description. \ No newline at end of file From 41cf7d651c1179beeb307b337f7d87761e10cadd Mon Sep 17 00:00:00 2001 From: Vasili-Sk <1153192+Vasili-Sk@users.noreply.github.com> Date: Tue, 26 Oct 2021 12:06:05 +0300 Subject: [PATCH 5/5] Added DLC to RTR message Updated guide --- doc/protocol.md | 25 +++++++++++++++++++++++++ doc/raspberrypi.md | 2 +- socketcandcl.c | 15 ++++++--------- state_bcm.c | 9 ++++----- state_raw.c | 13 ++++++------- 5 files changed, 42 insertions(+), 22 deletions(-) diff --git a/doc/protocol.md b/doc/protocol.md index 2f82195..50dc100 100644 --- a/doc/protocol.md +++ b/doc/protocol.md @@ -74,6 +74,22 @@ Send a single CAN frame without cyclic transmission // ID: 1AAAAAAA, Length: 2, Data: 0x01 0xF1 < send 1AAAAAAA 2 1 F1 > +#### Send a single Remote-transfer-request frame #### +This command is used to send a single RTR CAN frame. + + < sendrtr can_id can_dlc > + +Example: +Send a single CAN frame without cyclic transmission + + // ID: 123, DLC 0 + < sendrtr 123 0 > + + // ID: 1AAAAAAA, DLC 2 + < sendrtr 1AAAAAAA 2 > + +Note that DLC may cause collisions on CAN bus. Check CiA application note 802 + ### Commands for reception ### The commands for reception are 'subscribe' , 'unsubscribe' and 'filter'. @@ -169,7 +185,16 @@ Example: Reception of a CAN frame with CAN ID 0x123 , data length 4 and data 0x11, 0x22, 0x33 and 0x44 at time 23.424242> < frame 123 23.424242 11 22 33 44 > + +#### RTR frame transmission #### +CAN RTR messages received in raw mode: + < rtr can_id seconds.useconds can_dlc > +Example: +Reception of a RTR frame with CAN ID 0x123 , data length 4 at time 23.424242 + + < rtr 123 23.424242 4 > + #### Switch to BCM mode #### With '< bcmmode >' it is possible to switch back to BCM mode. diff --git a/doc/raspberrypi.md b/doc/raspberrypi.md index 7b0f62e..ef8931d 100644 --- a/doc/raspberrypi.md +++ b/doc/raspberrypi.md @@ -2,7 +2,7 @@ Running on Raspberry PI =================== For this example used adapter-board: https://www.waveshare.com/rs485-can-hat.htm you can find it on aliexpress too. -After flashing Raspberry image to SD card modify 'config.txt', uncomment this like: +After flashing Raspberry image to SD card modify 'config.txt', uncomment this line: dtparam=spi=on diff --git a/socketcandcl.c b/socketcandcl.c index ac0a66a..0a08172 100644 --- a/socketcandcl.c +++ b/socketcandcl.c @@ -344,9 +344,8 @@ inline void state_connected() } if (!strncmp("< rtr ", buf, 6)) { //send RTR frame only - sscanf(buf, "< %*s %x %*d.%*d >", &frame.can_id); - //force DLC 0 since it is undocumented feature - frame.can_dlc = 0; + sscanf(buf, "< %*s %x %*d.%*d %hhu >", &frame.can_id, &frame.can_dlc); + frame.can_id |= CAN_RTR_FLAG; char *s = buf + 6; for (; ++s;) { @@ -392,21 +391,19 @@ inline void state_connected() } else { if(frame.can_id & CAN_ERR_FLAG) { /* TODO implement */ - } else if(frame.can_id & CAN_RTR_FLAG) { - /* TODO implement */ } else { if (frame.can_id & CAN_RTR_FLAG) { if (frame.can_id & CAN_EFF_FLAG) { - ret = sprintf(buf, "< sendrtr %08X >", frame.can_id & CAN_EFF_MASK); + ret = sprintf(buf, "< sendrtr %08X %hhu >", frame.can_id & CAN_EFF_MASK, frame.can_dlc); } else { - ret = sprintf(buf, "< sendrtr %03X >", frame.can_id & CAN_SFF_MASK); + ret = sprintf(buf, "< sendrtr %03X %hhu >", frame.can_id & CAN_SFF_MASK, frame.can_dlc); } } else { int i; if (frame.can_id & CAN_EFF_FLAG) { - ret = sprintf(buf, "< send %08X %d ", frame.can_id & CAN_EFF_MASK, frame.can_dlc); + ret = sprintf(buf, "< send %08X %hhu ", frame.can_id & CAN_EFF_MASK, frame.can_dlc); } else { - ret = sprintf(buf, "< send %03X %d ", frame.can_id & CAN_SFF_MASK, frame.can_dlc); + ret = sprintf(buf, "< send %03X %hhu ", frame.can_id & CAN_SFF_MASK, frame.can_dlc); } for (i = 0; i < frame.can_dlc; i++) { ret += sprintf(buf + ret, "%02x ", frame.data[i]); diff --git a/state_bcm.c b/state_bcm.c index 820d103..89237a8 100644 --- a/state_bcm.c +++ b/state_bcm.c @@ -198,13 +198,12 @@ void state_bcm() { /* Add a send job */ } else if (!strncmp("< sendrtr ", buf, 10)) { //send RTR frame only - items = sscanf(buf, "< %*s %x >", &msg.msg_head.can_id); - if ((items < 1)) { - PRINT_ERROR("Syntax error in send command\n") + items = sscanf(buf, "< %*s %x %hhu >", &msg.msg_head.can_id, &msg.frame.can_dlc); + if ((items < 2) || (msg.frame.can_dlc > CAN_MAX_DLEN)) { + PRINT_ERROR("Syntax error in sendrtr command\n") return; } - //force DLC 0 since it is undocumented feature - msg.frame.can_dlc = 0; + msg.msg_head.can_id |= CAN_RTR_FLAG; if (element_length(buf, 2) == 8) diff --git a/state_raw.c b/state_raw.c index f01edec..c31e5a7 100644 --- a/state_raw.c +++ b/state_raw.c @@ -122,9 +122,9 @@ void state_raw() { send(client_socket, buf, strlen(buf), 0); } else if (frame.can_id & CAN_RTR_FLAG) { if (frame.can_id & CAN_EFF_FLAG) { - ret = sprintf(buf, "< rtr %08X %ld.%06ld >", frame.can_id & CAN_EFF_MASK, tv.tv_sec, tv.tv_usec); + ret = sprintf(buf, "< rtr %08X %ld.%06ld %hhu >", frame.can_id & CAN_EFF_MASK, tv.tv_sec, tv.tv_usec, frame.can_dlc); } else { - ret = sprintf(buf, "< rtr %03X %ld.%06ld >", frame.can_id & CAN_SFF_MASK, tv.tv_sec, tv.tv_usec); + ret = sprintf(buf, "< rtr %03X %ld.%06ld %hhu >", frame.can_id & CAN_SFF_MASK, tv.tv_sec, tv.tv_usec, frame.can_dlc); } send(client_socket, buf, strlen(buf), 0); } else { @@ -194,13 +194,12 @@ void state_raw() { } else if (!strncmp("< sendrtr ", buf, 10)) { //send RTR frame only - items = sscanf(buf, "< %*s %x >", &frame.can_id); - if ((items < 1)) { - PRINT_ERROR("Syntax error in send command\n") + items = sscanf(buf, "< %*s %x %hhu >", &frame.can_id, &frame.can_dlc); + if ((items < 2) || (frame.can_dlc > CAN_MAX_DLEN)) { + PRINT_ERROR("Syntax error in sendrtr command\n") return; } - //force DLC 0 since it is undocumented feature - frame.can_dlc = 0; + frame.can_id |= CAN_RTR_FLAG; if (element_length(buf, 2) == 8)