X-Git-Url: http://rtime.felk.cvut.cz/gitweb/sojka/can-utils.git/blobdiff_plain/3a0612c14add62285523c81f28cae6e4110e3555..HEAD:/cangw.c diff --git a/cangw.c b/cangw.c index e4312dc..802aa86 100644 --- a/cangw.c +++ b/cangw.c @@ -1,7 +1,3 @@ -/* - * $Id$ - */ - /* * cangw.c - manage PF_CAN netlink gateway * @@ -41,7 +37,7 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH * DAMAGE. * - * Send feedback to + * Send feedback to * */ @@ -72,6 +68,9 @@ struct modattr { } __attribute__((packed)); +#define RTCAN_RTA(r) ((struct rtattr*)(((char*)(r)) + NLMSG_ALIGN(sizeof(struct rtcanmsg)))) +#define RTCAN_PAYLOAD(n) NLMSG_PAYLOAD(n,sizeof(struct rtcanmsg)) + /* some netlink helpers stolen from iproute2 package */ #define NLMSG_TAIL(nmsg) \ ((struct rtattr *)(((void *) (nmsg)) + NLMSG_ALIGN((nmsg)->nlmsg_len))) @@ -99,7 +98,10 @@ void printfilter(const void *data) { struct can_filter *filter = (struct can_filter *)data; - printf("-f %03X:%X ", filter->can_id, filter->can_mask); + if (filter->can_id & CAN_INV_FILTER) + printf("-f %03X~%X ", (filter->can_id & ~CAN_INV_FILTER), filter->can_mask); + else + printf("-f %03X:%X ", filter->can_id, filter->can_mask); } void printmod(const char *type, const void *data) @@ -128,23 +130,82 @@ void printmod(const char *type, const void *data) printf(" "); } +void print_cs_xor(struct cgw_csum_xor *cs_xor) +{ + printf("-x %d:%d:%d:%02X ", + cs_xor->from_idx, cs_xor->to_idx, + cs_xor->result_idx, cs_xor->init_xor_val); +} + +void print_cs_crc8_profile(struct cgw_csum_crc8 *cs_crc8) +{ + int i; + + printf("-p %d:", cs_crc8->profile); + + switch (cs_crc8->profile) { + + case CGW_CRC8PRF_1U8: + + printf("%02X", cs_crc8->profile_data[0]); + break; + + case CGW_CRC8PRF_16U8: + + for (i = 0; i < 16; i++) + printf("%02X", cs_crc8->profile_data[i]); + break; + + case CGW_CRC8PRF_SFFID_XOR: + break; + + default: + printf("", cs_crc8->profile); + } + + printf(" "); +} + +void print_cs_crc8(struct cgw_csum_crc8 *cs_crc8) +{ + int i; + + printf("-c %d:%d:%d:%02X:%02X:", + cs_crc8->from_idx, cs_crc8->to_idx, + cs_crc8->result_idx, cs_crc8->init_crc_val, + cs_crc8->final_xor_val); + + for (i = 0; i < 256; i++) + printf("%02X", cs_crc8->crctab[i]); + + printf(" "); + + if (cs_crc8->profile != CGW_CRC8PRF_UNSPEC) + print_cs_crc8_profile(cs_crc8); +} void print_usage(char *prg) { fprintf(stderr, "\nUsage: %s [options]\n\n", prg); fprintf(stderr, "Commands: -A (add a new rule)\n"); fprintf(stderr, " -D (delete a rule)\n"); - fprintf(stderr, " -F (flush - delete all rules)\n"); + fprintf(stderr, " -F (flush / delete all rules)\n"); fprintf(stderr, " -L (list all rules)\n"); fprintf(stderr, "Mandatory: -s (source netdevice)\n"); fprintf(stderr, " -d (destination netdevice)\n"); fprintf(stderr, "Options: -t (preserve src_dev rx timestamp)\n"); fprintf(stderr, " -e (echo sent frames - recommended on vcanx)\n"); + fprintf(stderr, " -i (allow to route to incoming interface)\n"); fprintf(stderr, " -f (set CAN filter)\n"); fprintf(stderr, " -m (set frame modifications)\n"); + fprintf(stderr, " -x ::: (XOR checksum)\n"); + fprintf(stderr, " -c ::::: (CRC8 cs)\n"); + fprintf(stderr, " -p :[] (CRC8 checksum profile & parameters)\n"); fprintf(stderr, "\nValues are given and expected in hexadecimal values. Leading 0s can be omitted.\n"); fprintf(stderr, "\n"); - fprintf(stderr, " is a : CAN identifier filter\n"); + fprintf(stderr, " is a CAN identifier filter\n"); + fprintf(stderr, " : (matches when & mask == can_id & mask)\n"); + fprintf(stderr, " ~ (matches when & mask != can_id & mask)\n"); fprintf(stderr, "\n"); fprintf(stderr, " is a CAN frame modification instruction consisting of\n"); fprintf(stderr, "::..\n"); @@ -153,18 +214,71 @@ void print_usage(char *prg) fprintf(stderr, " - is an u32 value containing the CAN Identifier\n"); fprintf(stderr, " - is an u8 value containing the data length code (0 .. 8)\n"); fprintf(stderr, " - is always eight(!) u8 values containing the CAN frames data\n"); - fprintf(stderr, "The instructions are performed in the order 'AND' -> 'OR' -> 'XOR' -> 'SET'\n"); + fprintf(stderr, "The max. four modifications are performed in the order AND -> OR -> XOR -> SET\n"); fprintf(stderr, "\n"); fprintf(stderr, "Example:\n"); - fprintf(stderr, "%s -A -s can0 -d vcan3 -f 123:C00007FF -m SET:IL:80000333.4.1122334455667788\n", prg); + fprintf(stderr, "%s -A -s can0 -d vcan3 -e -f 123:C00007FF -m SET:IL:333.4.1122334455667788\n", prg); + fprintf(stderr, "\n"); + fprintf(stderr, "Supported CRC 8 profiles:\n"); + fprintf(stderr, "Profile '%d' (1U8) - add one additional u8 value\n", CGW_CRC8PRF_1U8); + fprintf(stderr, "Profile '%d' (16U8) - add u8 value from table[16] indexed by (data[1] & 0xF)\n", CGW_CRC8PRF_16U8); + fprintf(stderr, "Profile '%d' (SFFID_XOR) - add u8 value (can_id & 0xFF) ^ (can_id >> 8 & 0xFF)\n", CGW_CRC8PRF_SFFID_XOR); fprintf(stderr, "\n"); } -int parse_mod(char *optarg, struct modattr *modmsg) +int b64hex(char *asc, unsigned char *bin, int len) { - char *ptr, *nptr; int i; + for (i = 0; i < len; i++) { + if (!sscanf(asc+(i*2), "%2hhx", bin+i)) + return 1; + } + return 0; +} + +int parse_crc8_profile(char *optarg, struct cgw_csum_crc8 *crc8) +{ + int ret = 1; + char *ptr; + + if (sscanf(optarg, "%hhd:", &crc8->profile) != 1) + return ret; + + switch (crc8->profile) { + + case CGW_CRC8PRF_1U8: + + if (sscanf(optarg, "%hhd:%2hhx", &crc8->profile, &crc8->profile_data[0]) == 2) + ret = 0; + + break; + + case CGW_CRC8PRF_16U8: + + ptr = strchr(optarg, ':'); + + /* check if length contains 16 base64 hex values */ + if (ptr != NULL && + strlen(ptr) == strlen(":00112233445566778899AABBCCDDEEFF") && + b64hex(ptr+1, (unsigned char *)&crc8->profile_data[0], 16) == 0) + ret = 0; + + break; + + case CGW_CRC8PRF_SFFID_XOR: + + /* no additional parameters needed */ + ret = 0; + break; + } + + return ret; +} + +int parse_mod(char *optarg, struct modattr *modmsg) +{ + char *ptr, *nptr; char hexdata[17] = {0}; ptr = optarg; @@ -214,23 +328,198 @@ int parse_mod(char *optarg, struct modattr *modmsg) ptr++; } - if ((sscanf(++ptr, "%lx.%hhd.%16s", - (long unsigned int *)&modmsg->cf.can_id, - (unsigned char *)&modmsg->cf.can_dlc, hexdata) != 3) || - (modmsg->cf.can_dlc > 8)) + if (sscanf(++ptr, "%x.%hhx.%16s", &modmsg->cf.can_id, + (unsigned char *)&modmsg->cf.can_dlc, hexdata) != 3) return 5; - if (strlen(hexdata) != 16) + /* 4-bit masks can have values from 0 to 0xF */ + if (modmsg->cf.can_dlc > 0xF) return 6; - for (i = 0; i < 8; i++) { - if (!sscanf(&hexdata[i*2], "%2hhx", &modmsg->cf.data[i])) - return 7; - } + /* but when setting CAN_DLC the value has to be limited to 8 */ + if (modmsg->instruction == CGW_MOD_SET && modmsg->cf.can_dlc > 8) + return 7; + + if (strlen(hexdata) != 16) + return 8; + + if (b64hex(hexdata, &modmsg->cf.data[0], 8)) + return 9; return 0; /* ok */ } +int parse_rtlist(char *prgname, unsigned char *rxbuf, int len) +{ + char ifname[IF_NAMESIZE]; /* internface name for if_indextoname() */ + struct rtcanmsg *rtc; + struct rtattr *rta; + struct nlmsghdr *nlh; + unsigned int src_ifindex = 0; + unsigned int dst_ifindex = 0; + __u32 handled, dropped, deleted; + int rtlen; + + + nlh = (struct nlmsghdr *)rxbuf; + + while (1) { + if (!NLMSG_OK(nlh, len)) + return 0; + + if (nlh->nlmsg_type == NLMSG_ERROR) { + printf("NLMSG_ERROR\n"); + return 1; + } + + if (nlh->nlmsg_type == NLMSG_DONE) { + //printf("NLMSG_DONE\n"); + return 1; + } + + rtc = (struct rtcanmsg *)NLMSG_DATA(nlh); + if (rtc->can_family != AF_CAN) { + printf("received msg from unknown family %d\n", rtc->can_family); + return -EINVAL; + } + + if (rtc->gwtype != CGW_TYPE_CAN_CAN) { + printf("received msg with unknown gwtype %d\n", rtc->gwtype); + return -EINVAL; + } + + /* + * print list in a representation that + * can be used directly for start scripts. + * + * To order the mandatory and optional parameters in the + * output string, the NLMSG is parsed twice. + */ + + handled = 0; + dropped = 0; + deleted = 0; + src_ifindex = 0; + dst_ifindex = 0; + + printf("%s -A ", basename(prgname)); + + /* first parse for mandatory options */ + rta = (struct rtattr *) RTCAN_RTA(rtc); + rtlen = RTCAN_PAYLOAD(nlh); + for(;RTA_OK(rta, rtlen);rta=RTA_NEXT(rta,rtlen)) + { + //printf("(A-%d)", rta->rta_type); + switch(rta->rta_type) { + + case CGW_FILTER: + case CGW_MOD_AND: + case CGW_MOD_OR: + case CGW_MOD_XOR: + case CGW_MOD_SET: + case CGW_CS_XOR: + case CGW_CS_CRC8: + break; + + case CGW_SRC_IF: + src_ifindex = *(__u32 *)RTA_DATA(rta); + break; + + case CGW_DST_IF: + dst_ifindex = *(__u32 *)RTA_DATA(rta); + break; + + case CGW_HANDLED: + handled = *(__u32 *)RTA_DATA(rta); + break; + + case CGW_DROPPED: + dropped = *(__u32 *)RTA_DATA(rta); + break; + + case CGW_DELETED: + deleted = *(__u32 *)RTA_DATA(rta); + break; + + default: + printf("Unknown attribute %d!", rta->rta_type); + return -EINVAL; + break; + } + } + + + printf("-s %s ", if_indextoname(src_ifindex, ifname)); + printf("-d %s ", if_indextoname(dst_ifindex, ifname)); + + if (rtc->flags & CGW_FLAGS_CAN_ECHO) + printf("-e "); + + if (rtc->flags & CGW_FLAGS_CAN_SRC_TSTAMP) + printf("-t "); + + if (rtc->flags & CGW_FLAGS_CAN_IIF_TX_OK) + printf("-i "); + + /* second parse for mod attributes */ + rta = (struct rtattr *) RTCAN_RTA(rtc); + rtlen = RTCAN_PAYLOAD(nlh); + for(;RTA_OK(rta, rtlen);rta=RTA_NEXT(rta,rtlen)) + { + //printf("(B-%d)", rta->rta_type); + switch(rta->rta_type) { + + case CGW_FILTER: + printfilter(RTA_DATA(rta)); + break; + + case CGW_MOD_AND: + printmod("AND", RTA_DATA(rta)); + break; + + case CGW_MOD_OR: + printmod("OR", RTA_DATA(rta)); + break; + + case CGW_MOD_XOR: + printmod("XOR", RTA_DATA(rta)); + break; + + case CGW_MOD_SET: + printmod("SET", RTA_DATA(rta)); + break; + + case CGW_CS_XOR: + print_cs_xor((struct cgw_csum_xor *)RTA_DATA(rta)); + break; + + case CGW_CS_CRC8: + print_cs_crc8((struct cgw_csum_crc8 *)RTA_DATA(rta)); + break; + + case CGW_SRC_IF: + case CGW_DST_IF: + case CGW_HANDLED: + case CGW_DROPPED: + case CGW_DELETED: + break; + + default: + printf("Unknown attribute %d!", rta->rta_type); + return -EINVAL; + break; + } + } + + /* end of entry */ + printf("# %d handled %d dropped %d deleted\n", + handled, dropped, deleted); + + /* jump to next NLMSG in the given buffer */ + nlh = NLMSG_NEXT(nlh, len); + } +} + int main(int argc, char **argv) { int s; @@ -241,35 +530,40 @@ int main(int argc, char **argv) int cmd = UNSPEC; int have_filter = 0; + int have_cs_xor = 0; + int have_cs_crc8 = 0; struct { struct nlmsghdr nh; struct rtcanmsg rtcan; - char buf[200]; + char buf[600]; } req; - char rxbuf[8192]; /* netlink receive buffer */ - char ifname[IF_NAMESIZE]; /* internface name for if_indextoname() */ + unsigned char rxbuf[8192]; /* netlink receive buffer */ struct nlmsghdr *nlh; struct nlmsgerr *rte; - struct rtcanmsg *rtc; - struct rtattr *rta; - __u32 handled = 0; - __u32 dropped = 0; - int rtlen; + unsigned int src_ifindex = 0; + unsigned int dst_ifindex = 0; + __u16 flags = 0; int len; struct can_filter filter; struct sockaddr_nl nladdr; + struct cgw_csum_xor cs_xor; + struct cgw_csum_crc8 cs_crc8; + char crc8tab[513] = {0}; + struct modattr modmsg[CGW_MOD_FUNCS]; int modidx = 0; int i; memset(&req, 0, sizeof(req)); + memset(&cs_xor, 0, sizeof(cs_xor)); + memset(&cs_crc8, 0, sizeof(cs_crc8)); - while ((opt = getopt(argc, argv, "ADFLs:d:tef:m:?")) != -1) { + while ((opt = getopt(argc, argv, "ADFLs:d:teif:c:p:x:m:?")) != -1) { switch (opt) { case 'A': @@ -293,25 +587,32 @@ int main(int argc, char **argv) break; case 's': - req.rtcan.src_ifindex = if_nametoindex(optarg); + src_ifindex = if_nametoindex(optarg); break; case 'd': - req.rtcan.dst_ifindex = if_nametoindex(optarg); + dst_ifindex = if_nametoindex(optarg); break; case 't': - req.rtcan.can_txflags |= CAN_GW_TXFLAGS_SRC_TSTAMP; + flags |= CGW_FLAGS_CAN_SRC_TSTAMP; break; case 'e': - req.rtcan.can_txflags |= CAN_GW_TXFLAGS_ECHO; + flags |= CGW_FLAGS_CAN_ECHO; + break; + + case 'i': + flags |= CGW_FLAGS_CAN_IIF_TX_OK; break; case 'f': - if (sscanf(optarg, "%lx:%lx", - (long unsigned int *)&filter.can_id, - (long unsigned int *)&filter.can_mask) == 2) { + if (sscanf(optarg, "%x:%x", &filter.can_id, + &filter.can_mask) == 2) { + have_filter = 1; + } else if (sscanf(optarg, "%x~%x", &filter.can_id, + &filter.can_mask) == 2) { + filter.can_id |= CAN_INV_FILTER; have_filter = 1; } else { printf("Bad filter definition '%s'.\n", optarg); @@ -319,6 +620,38 @@ int main(int argc, char **argv) } break; + case 'x': + if (sscanf(optarg, "%hhd:%hhd:%hhd:%hhx", + &cs_xor.from_idx, &cs_xor.to_idx, + &cs_xor.result_idx, &cs_xor.init_xor_val) == 4) { + have_cs_xor = 1; + } else { + printf("Bad XOR checksum definition '%s'.\n", optarg); + exit(1); + } + break; + + case 'c': + if ((sscanf(optarg, "%hhd:%hhd:%hhd:%hhx:%hhx:%512s", + &cs_crc8.from_idx, &cs_crc8.to_idx, + &cs_crc8.result_idx, &cs_crc8.init_crc_val, + &cs_crc8.final_xor_val, crc8tab) == 6) && + (strlen(crc8tab) == 512) && + (b64hex(crc8tab, (unsigned char *)&cs_crc8.crctab, 256) == 0)) { + have_cs_crc8 = 1; + } else { + printf("Bad CRC8 checksum definition '%s'.\n", optarg); + exit(1); + } + break; + + case 'p': + if (parse_crc8_profile(optarg, &cs_crc8)) { + printf("Bad CRC8 profile definition '%s'.\n", optarg); + exit(1); + } + break; + case 'm': /* may be triggered by each of the CGW_MOD_FUNCS functions */ if ((modidx < CGW_MOD_FUNCS) && (err = parse_mod(optarg, &modmsg[modidx++]))) { @@ -346,11 +679,16 @@ int main(int argc, char **argv) } if ((cmd == ADD || cmd == DEL) && - ((!req.rtcan.src_ifindex) || (!req.rtcan.dst_ifindex))) { + ((!src_ifindex) || (!dst_ifindex))) { print_usage(basename(argv[0])); exit(1); } + if (!modidx && (have_cs_crc8 || have_cs_xor)) { + printf("-c or -x can only be used in conjunction with -m\n"); + exit(1); + } + s = socket(PF_NETLINK, SOCK_RAW, NETLINK_ROUTE); switch (cmd) { @@ -369,8 +707,8 @@ int main(int argc, char **argv) req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK; req.nh.nlmsg_type = RTM_DELROUTE; /* if_index set to 0 => remove all entries */ - req.rtcan.src_ifindex = 0; - req.rtcan.dst_ifindex = 0; + src_ifindex = 0; + dst_ifindex = 0; break; case LIST: @@ -388,12 +726,23 @@ int main(int argc, char **argv) req.nh.nlmsg_seq = 0; req.rtcan.can_family = AF_CAN; + req.rtcan.gwtype = CGW_TYPE_CAN_CAN; + req.rtcan.flags = flags; + + addattr_l(&req.nh, sizeof(req), CGW_SRC_IF, &src_ifindex, sizeof(src_ifindex)); + addattr_l(&req.nh, sizeof(req), CGW_DST_IF, &dst_ifindex, sizeof(dst_ifindex)); /* add new attributes here */ if (have_filter) addattr_l(&req.nh, sizeof(req), CGW_FILTER, &filter, sizeof(filter)); + if (have_cs_crc8) + addattr_l(&req.nh, sizeof(req), CGW_CS_CRC8, &cs_crc8, sizeof(cs_crc8)); + + if (have_cs_xor) + addattr_l(&req.nh, sizeof(req), CGW_CS_XOR, &cs_xor, sizeof(cs_xor)); + /* * a better example code * modmsg.modtype = CGW_MOD_ID; @@ -417,7 +766,7 @@ int main(int argc, char **argv) } /* clean netlink receive buffer */ - bzero(rxbuf, sizeof(rxbuf)); + memset(rxbuf, 0x0, sizeof(rxbuf)); if (cmd != LIST) { @@ -452,74 +801,17 @@ int main(int argc, char **argv) perror("netlink recv"); return len; } - nlh = (struct nlmsghdr *)rxbuf; - if (nlh->nlmsg_type == NLMSG_DONE) - break; - - rtc = (struct rtcanmsg *)NLMSG_DATA(nlh); - if (rtc->can_family != AF_CAN) { - printf("received msg from unknown family %d\n", rtc->can_family); - return -EINVAL; - } +#if 0 + printf("received msg len %d\n", len); - /* - * print list in a representation that - * can be used directly for start scripts - */ - - printf("%s -A ", basename(argv[0])); - printf("-s %s ", if_indextoname(rtc->src_ifindex, ifname)); - printf("-d %s ", if_indextoname(rtc->dst_ifindex, ifname)); - - if (rtc->can_txflags & CAN_GW_TXFLAGS_ECHO) - printf("-e "); - - if (rtc->can_txflags & CAN_GW_TXFLAGS_SRC_TSTAMP) - printf("-t "); - - /* check for attributes */ - rta = (struct rtattr *) RTM_RTA(rtc); - rtlen = RTM_PAYLOAD(nlh); - for(;RTA_OK(rta, rtlen);rta=RTA_NEXT(rta,rtlen)) - { - switch(rta->rta_type) { - - case CGW_FILTER: - printfilter(RTA_DATA(rta)); - break; - - case CGW_MOD_AND: - printmod("AND", RTA_DATA(rta)); - break; - - case CGW_MOD_OR: - printmod("OR", RTA_DATA(rta)); - break; - - case CGW_MOD_XOR: - printmod("XOR", RTA_DATA(rta)); - break; - - case CGW_MOD_SET: - printmod("SET", RTA_DATA(rta)); - break; - - case CGW_HANDLED: - handled = *(__u32 *)RTA_DATA(rta); - break; - - case CGW_DROPPED: - dropped = *(__u32 *)RTA_DATA(rta); - break; - - default: - printf("Unknown attribute %d!", rta->rta_type); - return -EINVAL; - break; - } - } + for (i = 0; i < len; i++) + printf("%02X ", rxbuf[i]); - printf("# %d handled %d dropped\n", handled, dropped); /* end of entry */ + printf("\n"); +#endif + /* leave on errors or NLMSG_DONE */ + if (parse_rtlist(argv[0], rxbuf, len)) + break; } }