int hexstring2candata(char *arg, struct can_frame *cf) {
int len = strlen(arg);
int i;
unsigned char tmp;
if (!len || len%2 || len > 16)
return 1;
for (i=0; i < len/2; i++) {
tmp = asc2nibble(*(arg+(2*i)));
if (tmp > 0x0F)
return 1;
cf->data[i] = (tmp << 4);
tmp = asc2nibble(*(arg+(2*i)+1));
if (tmp > 0x0F)
return 1;
cf->data[i] |= tmp;
}
return 0;
}
/* Send one completely decapsulated can_frame to the network layer */
static void slc_bump(struct slcan *sl)
{
struct net_device_stats *stats = sl->dev->get_stats(sl->dev);
struct sk_buff *skb;
struct can_frame cf;
int i, dlc_pos, tmp;
char cmd = sl->rbuff[0];
if ((cmd != 't') && (cmd != 'T') && (cmd != 'r') && (cmd != 'R'))
return;
if (cmd & 0x20) /* tiny chars 'r' 't' => standard frame format */
dlc_pos = 4; /* dlc position tiiid */
else
dlc_pos = 9; /* dlc position Tiiiiiiiid */
if (!((sl->rbuff[dlc_pos] >= '0') && (sl->rbuff[dlc_pos] < '9')))
return;
cf.can_dlc = sl->rbuff[dlc_pos] & 0x0F; /* get can_dlc */
sl->rbuff[dlc_pos] = 0; /* terminate can_id string */
cf.can_id = simple_strtoul(sl->rbuff+1, NULL, 16);
if (!(cmd & 0x20)) /* NO tiny chars => extended frame format */
cf.can_id |= CAN_EFF_FLAG;
if ((cmd | 0x20) == 'r') /* RTR frame */
cf.can_id |= CAN_RTR_FLAG;
*(u64 *) (&cf.data) = 0; /* clear payload */
for (i = 0, dlc_pos++; i < cf.can_dlc; i++){
if ((tmp = asc2nibble(sl->rbuff[dlc_pos++])) > 0x0F)
return;
cf.data[i] = (tmp << 4);
if ((tmp = asc2nibble(sl->rbuff[dlc_pos++])) > 0x0F)
return;
cf.data[i] |= tmp;
}
skb = dev_alloc_skb(sizeof(struct can_frame));
if (!skb)
return;
skb->dev = sl->dev;
skb->protocol = htons(ETH_P_CAN);
skb->pkt_type = PACKET_BROADCAST;
skb->ip_summed = CHECKSUM_UNNECESSARY;
memcpy(skb_put(skb, sizeof(struct can_frame)),
&cf, sizeof(struct can_frame));
netif_rx(skb);
sl->dev->last_rx = jiffies;
stats->rx_packets++;
stats->rx_bytes += cf.can_dlc;
}
/* read data from pty, send CAN frames to CAN socket and answer commands */
int pty2can(int pty, int socket, struct can_filter *fi,
int *is_open, int *tstamp)
{
int nbytes;
char cmd;
char buf[200];
char replybuf[10]; /* for answers to received commands */
int ptr;
struct can_frame frame;
int tmp, i;
nbytes = read(pty, &buf, sizeof(buf)-1);
if (nbytes < 0) {
perror("read pty");
return 1;
}
rx_restart:
/* remove trailing '\r' characters to be robust against some apps */
while (buf[0] == '\r' && nbytes > 0) {
for (tmp = 0; tmp < nbytes; tmp++)
buf[tmp] = buf[tmp+1];
nbytes--;
}
if (!nbytes)
return 0;
cmd = buf[0];
buf[nbytes] = 0;
#ifdef DEBUG
for (tmp = 0; tmp < nbytes; tmp++)
if (buf[tmp] == '\r')
putchar('@');
else
putchar(buf[tmp]);
printf("\n");
#endif
/* check for filter configuration commands */
if (cmd == 'm' || cmd == 'M') {
buf[9] = 0; /* terminate filter string */
ptr = 9;
#if 0
/* the filter is no SocketCAN filter :-( */
/* TODO: behave like a SJA1000 controller specific filter */
if (cmd == 'm') {
fi->can_id = strtoul(buf+1,NULL,16);
fi->can_id &= CAN_EFF_MASK;
} else {
fi->can_mask = strtoul(buf+1,NULL,16);
fi->can_mask &= CAN_EFF_MASK;
}
if (*is_open)
setsockopt(socket, SOL_CAN_RAW,
CAN_RAW_FILTER, fi,
sizeof(struct can_filter));
#endif
goto rx_out_ack;
}
/* check for timestamp on/off command */
if (cmd == 'Z') {
*tstamp = buf[1] & 0x01;
ptr = 2;
goto rx_out_ack;
}
/* check for 'O'pen command */
if (cmd == 'O') {
setsockopt(socket, SOL_CAN_RAW,
CAN_RAW_FILTER, fi,
sizeof(struct can_filter));
ptr = 1;
*is_open = 1;
goto rx_out_ack;
}
/* check for 'C'lose command */
if (cmd == 'C') {
setsockopt(socket, SOL_CAN_RAW, CAN_RAW_FILTER,
NULL, 0);
ptr = 1;
*is_open = 0;
goto rx_out_ack;
}
/* check for 'V'ersion command */
if (cmd == 'V') {
sprintf(replybuf, "V1013\r");
tmp = strlen(replybuf);
ptr = 1;
goto rx_out;
}
/* check for serial 'N'umber command */
if (cmd == 'N') {
sprintf(replybuf, "N4242\r");
tmp = strlen(replybuf);
ptr = 1;
goto rx_out;
}
/* check for read status 'F'lags */
if (cmd == 'F') {
sprintf(replybuf, "F00\r");
tmp = strlen(replybuf);
ptr = 1;
goto rx_out;
}
/* correctly answer unsupported commands */
if (cmd == 'U') {
ptr = 2;
goto rx_out_ack;
}
if (cmd == 'S') {
ptr = 2;
goto rx_out_ack;
}
if (cmd == 's') {
ptr = 5;
goto rx_out_ack;
}
if (cmd == 'P' || cmd == 'A') {
ptr = 1;
goto rx_out_nack;
}
if (cmd == 'X') {
ptr = 2;
if (buf[1] & 0x01)
goto rx_out_ack;
else
goto rx_out_nack;
}
/* catch unknown commands */
if ((cmd != 't') && (cmd != 'T') &&
(cmd != 'r') && (cmd != 'R')) {
ptr = nbytes-1;
goto rx_out_nack;
}
if (cmd & 0x20) /* tiny chars 'r' 't' => SFF */
ptr = 4; /* dlc position tiiid */
else
ptr = 9; /* dlc position Tiiiiiiiid */
*(unsigned long long *) (&frame.data) = 0ULL; /* clear data[] */
if ((cmd | 0x20) == 'r' && buf[ptr] != '0') {
/*
* RTR frame without dlc information!
* This is against the SLCAN spec but sent
* by a commercial CAN tool ... so we are
* robust against this protocol violation.
*/
frame.can_dlc = buf[ptr]; /* save following byte */
buf[ptr] = 0; /* terminate can_id string */
frame.can_id = strtoul(buf+1, NULL, 16);
frame.can_id |= CAN_RTR_FLAG;
if (!(cmd & 0x20)) /* NO tiny chars => EFF */
frame.can_id |= CAN_EFF_FLAG;
buf[ptr] = frame.can_dlc; /* restore following byte */
frame.can_dlc = 0;
ptr--; /* we have no dlc component in the violation case */
} else {
if (!(buf[ptr] >= '0' && buf[ptr] < '9'))
goto rx_out_nack;
frame.can_dlc = buf[ptr] - '0'; /* get dlc from ASCII val */
buf[ptr] = 0; /* terminate can_id string */
frame.can_id = strtoul(buf+1, NULL, 16);
if (!(cmd & 0x20)) /* NO tiny chars => EFF */
frame.can_id |= CAN_EFF_FLAG;
if ((cmd | 0x20) == 'r') /* RTR frame */
frame.can_id |= CAN_RTR_FLAG;
for (i = 0, ptr++; i < frame.can_dlc; i++) {
tmp = asc2nibble(buf[ptr++]);
if (tmp > 0x0F)
goto rx_out_nack;
frame.data[i] = (tmp << 4);
tmp = asc2nibble(buf[ptr++]);
if (tmp > 0x0F)
goto rx_out_nack;
frame.data[i] |= tmp;
}
/* point to last real data */
if (frame.can_dlc)
ptr--;
}
tmp = write(socket, &frame, sizeof(frame));
if (tmp != sizeof(frame)) {
perror("write socket");
return 1;
}
rx_out_ack:
replybuf[0] = '\r';
tmp = 1;
goto rx_out;
rx_out_nack:
replybuf[0] = '\a';
tmp = 1;
rx_out:
tmp = write(pty, replybuf, tmp);
if (tmp < 0) {
perror("write pty replybuf");
return 1;
}
/* check if there is another command in this buffer */
if (nbytes > ptr+1) {
for (tmp = 0, ptr++; ptr+tmp < nbytes; tmp++)
buf[tmp] = buf[ptr+tmp];
nbytes = tmp;
goto rx_restart;
}
return 0;
}
int parse_canframe(char *cs, struct can_frame *cf) {
/* documentation see lib.h */
int i, idx, dlc, len;
unsigned char tmp;
len = strlen(cs);
//printf("'%s' len %d\n", cs, len);
memset(cf, 0, sizeof(*cf)); /* init CAN frame, e.g. DLC = 0 */
if (len < 4)
return 1;
if (!((cs[3] == CANID_DELIM) || (cs[8] == CANID_DELIM)))
return 1;
if (cs[8] == CANID_DELIM) { /* 8 digits */
idx = 9;
for (i=0; i<8; i++){
if ((tmp = asc2nibble(cs[i])) > 0x0F)
return 1;
cf->can_id |= (tmp << (7-i)*4);
}
if (!(cf->can_id & CAN_ERR_FLAG)) /* 8 digits but no errorframe? */
cf->can_id |= CAN_EFF_FLAG; /* then it is an extended frame */
} else { /* 3 digits */
idx = 4;
for (i=0; i<3; i++){
if ((tmp = asc2nibble(cs[i])) > 0x0F)
return 1;
cf->can_id |= (tmp << (2-i)*4);
}
}
if((cs[idx] == 'R') || (cs[idx] == 'r')){ /* RTR frame */
cf->can_id |= CAN_RTR_FLAG;
return 0;
}
for (i=0, dlc=0; i<8; i++){
if(cs[idx] == DATA_SEPERATOR) /* skip (optional) seperator */
idx++;
if(idx >= len) /* end of string => end of data */
break;
if ((tmp = asc2nibble(cs[idx++])) > 0x0F)
return 1;
cf->data[i] = (tmp << 4);
if ((tmp = asc2nibble(cs[idx++])) > 0x0F)
return 1;
cf->data[i] |= tmp;
dlc++;
}
cf->can_dlc = dlc;
return 0;
}
void state_isotp() {
int i, items, ret;
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 tmp;
if(previous_state == STATE_ISOTP) {
state_isotp_init();
previous_state = STATE_ISOTP;
}
FD_ZERO(&readfds);
FD_SET(si, &readfds);
FD_SET(client_socket, &readfds);
/*
* Check if there are more elements in the element buffer before calling select() and
* blocking for new packets.
*/
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) {
PRINT_ERROR("Error in select()\n")
change_state(STATE_SHUTDOWN);
return;
}
}
if (FD_ISSET(si, &readfds)) {
struct timeval tv = {0};
items = read(si, isobuf, ISOTPLEN);
/* read timestamp data */
if(ioctl(si, SIOCGSTAMP, &tv) < 0) {
PRINT_ERROR("Could not receive timestamp\n");
}
if (items > 0 && items <= ISOTPLEN) {
int startlen;
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]);
sprintf(rxmsg + strlen(rxmsg), " >");
send(client_socket, rxmsg, strlen(rxmsg), 0);
}
}
if (FD_ISSET(client_socket, &readfds)) {
ret = receive_command(client_socket, buf);
if(ret != 0) {
change_state(STATE_SHUTDOWN);
return;
}
if ( (ret = state_changed(buf, state)) ) {
if(ret == CONTROL_SWITCH_STATE) state_isotp_deinit();
strcpy(buf, "< ok >");
send(client_socket, buf, strlen(buf), 0);
return;
}
#if 0
if(!strcmp("< echo >", buf)) {
send(client_socket, buf, strlen(buf), 0);
return;
}
if(!strncmp("< sendpdu ", buf, 10)) {
items = element_length(buf, 2);
if (items & 1) {
PRINT_ERROR("odd number of ASCII Hex values\n");
return;
}
items /= 2;
if (items > ISOTPLEN) {
PRINT_ERROR("PDU too long\n");
return;
}
for (i = 0; i < items; i++) {
tmp = asc2nibble(buf[(2*i) + 10]);
if (tmp > 0x0F)
return;
isobuf[i] = (tmp << 4);
tmp = asc2nibble(buf[(2*i) + 11]);
if (tmp > 0x0F)
return;
isobuf[i] |= tmp;
}
ret = write(si, isobuf, items);
if(ret != items) {
PRINT_ERROR("Error in write()\n")
change_state(STATE_SHUTDOWN);
return;
}
} else {
PRINT_ERROR("unknown command '%s'.\n", buf)
strcpy(buf, "< error unknown command >");
send(client_socket, buf, strlen(buf), 0);
}
#else
ret = decode_command(buf);
switch(ret) {
case COMMAND_ECHO:
send(client_socket, buf, strlen(buf), 0);
break;
case COMMAND_SENDPDU:
items = element_length(buf, 2);
if (items & 1) {
PRINT_ERROR("odd number of ASCII Hex values\n");
return;
}
items /= 2;
if (items > ISOTPLEN) {
PRINT_ERROR("PDU too long\n");
return;
}
for (i = 0; i < items; i++) {
tmp = asc2nibble(buf[(2*i) + 10]);
if (tmp > 0x0F)
return;
isobuf[i] = (tmp << 4);
tmp = asc2nibble(buf[(2*i) + 11]);
if (tmp > 0x0F)
return;
isobuf[i] |= tmp;
}
ret = write(si, isobuf, items);
if(ret != items) {
PRINT_ERROR("Error in write()\n")
change_state(STATE_SHUTDOWN);
return;
}
break; /* COMMAND_SENDPDU */
case COMMAND_UNKNOWN:
default:
PRINT_ERROR("unknown command '%s'.\n", buf)
strcpy(buf, "< error unknown command >");
send(client_socket, buf, strlen(buf), 0);
break; /* COMMAND_UNKNOWN */
}
#endif
}
}
int ihex_parse(char input[], ihex_record_t *record) {
//given a NULL-TERMINATED input string (use gets()) in I16HEX format, write the binary record in record. return 0 on success.
uint8_t inputlen;
uint8_t t, i, checksum_calc=0, checksum_read;
//first check for ":" leading character
if(input[0] != ':') return -1;
//then check the string for only valid ASCII ['0'-'F']
inputlen=1;
while(input[inputlen]) {
if( !isxdigit(input[inputlen++]) ) return -2;
}
//then read the length.
record->length = (asc2nibble(input[1]) << 4) + asc2nibble(input[2]);
if(input[(record->length<<1) + 11] != 0) return -3; //if we're missing a null terminator in the right place
//then read the address.
record->addr = (asc2nibble(input[3]) << 12) + (asc2nibble(input[4]) << 8) + (asc2nibble(input[5]) << 4) + asc2nibble(input[6]);
//then read the record type.
record->type = (asc2nibble(input[7]) << 4) + asc2nibble(input[8]);
// if(record->type > 4) return -4;
//then read the data, which goes from input[9] to input[9+length*2].
for(i=0; i < record->length; i++) {
t = 9 + (i<<1);
record->data[i] = (asc2nibble(input[t]) << 4) + (asc2nibble(input[t + 1]));
checksum_calc += record->data[i]; //might as well keep a running checksum as we read
}
checksum_calc += record->length + record->type + (record->addr >> 8) + (record->addr & 0xFF); //get the rest of the data into that checksum
checksum_calc = ~checksum_calc + 1; //checksum is 2's complement
//now read the checksum of the record
checksum_read = (asc2nibble(input[9 + (record->length<<1)]) << 4) + asc2nibble(input[10 + (record->length<<1)]);
if(checksum_calc != checksum_read) return -5; //compare 'em
return 0;
}
