Skip to content

Commit

Permalink
cangw: add CAN FD support
Browse files Browse the repository at this point in the history 
Signed-off-by: Oliver Hartkopp <[email protected]>
  • Loading branch information
@hartkopp
hartkopp committed Aug 16, 2019
1 parent 07054c0 commit d45f3f4
Showing 1 changed file with 173 additions and 10 deletions.
183 changes: 173 additions & 10 deletions cangw.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,12 @@ struct modattr {
__u8 instruction;
} __attribute__((packed));

struct fdmodattr {
struct canfd_frame cf;
__u8 modtype;
__u8 instruction;
} __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))
Expand Down Expand Up @@ -125,7 +131,36 @@ void printmod(const char *type, const void *data)

printf(":%03X.%X.", mod.cf.can_id, mod.cf.can_dlc);

for (i = 0; i < 8; i++)
for (i = 0; i < CAN_MAX_DLEN; i++)
printf("%02X", mod.cf.data[i]);

printf(" ");
}

void printfdmod(const char *type, const void *data)
{
struct fdmodattr mod;
int i;

memcpy (&mod, data, CGW_FDMODATTR_LEN);

printf("-M %s:", type);

if (mod.modtype & CGW_MOD_ID)
printf("I");

if (mod.modtype & CGW_MOD_FLAGS)
printf("F");

if (mod.modtype & CGW_MOD_LEN)
printf("L");

if (mod.modtype & CGW_MOD_DATA)
printf("D");

printf(":%03X.%X.%X.", mod.cf.can_id, mod.cf.flags, mod.cf.len);

for (i = 0; i < CANFD_MAX_DLEN; i++)
printf("%02X", mod.cf.data[i]);

printf(" ");
Expand Down Expand Up @@ -201,7 +236,8 @@ void print_usage(char *prg)
fprintf(stderr, " -u <uid> (user defined modification identifier)\n");
fprintf(stderr, " -l <hops> (limit the number of frame hops / routings)\n");
fprintf(stderr, " -f <filter> (set CAN filter)\n");
fprintf(stderr, " -m <mod> (set frame modifications)\n");
fprintf(stderr, " -m <mod> (set Classic CAN frame modifications)\n");
fprintf(stderr, " -M <MOD> (set CAN FD frame modifications)\n");
fprintf(stderr, " -x <from_idx>:<to_idx>:<result_idx>:<init_xor_val> (XOR checksum)\n");
fprintf(stderr, " -c <from>:<to>:<result>:<init_val>:<xor_val>:<crctab[256]> (CRC8 cs)\n");
fprintf(stderr, " -p <profile>:[<profile_data>] (CRC8 checksum profile & parameters)\n");
Expand All @@ -218,6 +254,14 @@ void print_usage(char *prg)
fprintf(stderr, " - <can_id> is an u32 value containing the CAN Identifier\n");
fprintf(stderr, " - <can_dlc> is an u8 value containing the data length code (0 .. 8)\n");
fprintf(stderr, " - <can_data> is always eight(!) u8 values containing the CAN frames data\n");
fprintf(stderr, "<MOD> is a CAN FD frame modification instruction consisting of\n");
fprintf(stderr, "<instruction>:<canfd_frame-elements>:<can_id>.<flags>.<len>.<can_data>\n");
fprintf(stderr, " - <instruction> is one of 'AND' 'OR' 'XOR' 'SET'\n");
fprintf(stderr, " - <canfd_frame-elements> is _one_ or _more_ of 'I'd 'F'lags 'L'ength 'D'ata\n");
fprintf(stderr, " - <can_id> is an u32 value containing the CAN FD Identifier\n");
fprintf(stderr, " - <flags> is an u8 value containing CAN FD flags (CANFD_BRS, CANFD_ESI)\n");
fprintf(stderr, " - <len> is an u8 value containing the data length (0 .. 64)\n");
fprintf(stderr, " - <can_data> is always 64(!) u8 values containing the CAN FD frames data\n");
fprintf(stderr, "The max. four modifications are performed in the order AND -> OR -> XOR -> SET\n");
fprintf(stderr, "\n");
fprintf(stderr, "Example:\n");
Expand Down Expand Up @@ -283,7 +327,7 @@ int parse_crc8_profile(char *optarg, struct cgw_csum_crc8 *crc8)
int parse_mod(char *optarg, struct modattr *modmsg)
{
char *ptr, *nptr;
char hexdata[17] = {0};
char hexdata[(CAN_MAX_DLEN * 2) + 1] = {0};
canid_t can_id;

ptr = optarg;
Expand Down Expand Up @@ -336,12 +380,86 @@ int parse_mod(char *optarg, struct modattr *modmsg)
if (sscanf(++ptr, "%x.%hhx.%16s", &can_id,
(unsigned char *)&modmsg->cf.can_dlc, hexdata) != 3)
return 5;

modmsg->cf.can_id = can_id;

if (strlen(hexdata) != 16)
if (strlen(hexdata) != (CAN_MAX_DLEN * 2))
return 6;

if (b64hex(hexdata, &modmsg->cf.data[0], 8))
if (b64hex(hexdata, &modmsg->cf.data[0], CAN_MAX_DLEN))
return 7;

return 0; /* ok */
}

int parse_fdmod(char *optarg, struct fdmodattr *modmsg)
{
char *ptr, *nptr;
char hexdata[(CANFD_MAX_DLEN * 2) + 1] = {0};
canid_t can_id;

ptr = optarg;
nptr = strchr(ptr, ':');

if ((nptr - ptr > 3) || (nptr - ptr == 0))
return 1;

if (!strncmp(ptr, "AND", 3))
modmsg->instruction = CGW_FDMOD_AND;
else if (!strncmp(ptr, "OR", 2))
modmsg->instruction = CGW_FDMOD_OR;
else if (!strncmp(ptr, "XOR", 3))
modmsg->instruction = CGW_FDMOD_XOR;
else if (!strncmp(ptr, "SET", 3))
modmsg->instruction = CGW_FDMOD_SET;
else
return 2;

ptr = nptr+1;
nptr = strchr(ptr, ':');

if ((nptr - ptr > 4) || (nptr - ptr == 0))
return 3;

modmsg->modtype = 0;

while (*ptr != ':') {

switch (*ptr) {

case 'I':
modmsg->modtype |= CGW_MOD_ID;
break;

case 'F':
modmsg->modtype |= CGW_MOD_FLAGS;
break;

case 'L':
modmsg->modtype |= CGW_MOD_LEN;
break;

case 'D':
modmsg->modtype |= CGW_MOD_DATA;
break;

default:
return 4;
}
ptr++;
}

if (sscanf(++ptr, "%x.%hhx.%hhx.%128s", &can_id,
(unsigned char *)&modmsg->cf.flags,
(unsigned char *)&modmsg->cf.len, hexdata) != 4)
return 5;

modmsg->cf.can_id = can_id;

if (strlen(hexdata) != (CANFD_MAX_DLEN * 2))
return 6;

if (b64hex(hexdata, &modmsg->cf.data[0], CANFD_MAX_DLEN))
return 7;

return 0; /* ok */
Expand Down Expand Up @@ -415,6 +533,10 @@ int parse_rtlist(char *prgname, unsigned char *rxbuf, int len)
case CGW_MOD_OR:
case CGW_MOD_XOR:
case CGW_MOD_SET:
case CGW_FDMOD_AND:
case CGW_FDMOD_OR:
case CGW_FDMOD_XOR:
case CGW_FDMOD_SET:
case CGW_MOD_UID:
case CGW_LIM_HOPS:
case CGW_CS_XOR:
Expand Down Expand Up @@ -492,6 +614,22 @@ int parse_rtlist(char *prgname, unsigned char *rxbuf, int len)
printmod("SET", RTA_DATA(rta));
break;

case CGW_FDMOD_AND:
printfdmod("AND", RTA_DATA(rta));
break;

case CGW_FDMOD_OR:
printfdmod("OR", RTA_DATA(rta));
break;

case CGW_FDMOD_XOR:
printfdmod("XOR", RTA_DATA(rta));
break;

case CGW_FDMOD_SET:
printfdmod("SET", RTA_DATA(rta));
break;

case CGW_MOD_UID:
printf("-u %X ", *(__u32 *)RTA_DATA(rta));
break;
Expand Down Expand Up @@ -547,8 +685,7 @@ int main(int argc, char **argv)
struct {
struct nlmsghdr nh;
struct rtcanmsg rtcan;
char buf[600];

char buf[1500];
} req;

unsigned char rxbuf[8192]; /* netlink receive buffer */
Expand All @@ -569,14 +706,16 @@ int main(int argc, char **argv)
char crc8tab[513] = {0};

struct modattr modmsg[CGW_MOD_FUNCS];
struct fdmodattr fdmodmsg[CGW_MOD_FUNCS];
int modidx = 0;
int fdmodidx = 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:Xteiu:l:f:c:p:x:m:?")) != -1) {
while ((opt = getopt(argc, argv, "ADFLs:d:Xteiu:l:f:c:p:x:m:M:?")) != -1) {
switch (opt) {

case 'A':
Expand Down Expand Up @@ -688,6 +827,14 @@ int main(int argc, char **argv)
}
break;

case 'M':
/* may be triggered by each of the CGW_FDMOD_FUNCS functions */
if ((fdmodidx < CGW_MOD_FUNCS) && (err = parse_fdmod(optarg, &fdmodmsg[fdmodidx++]))) {
printf("Problem %d with modification definition '%s'.\n", err, optarg);
exit(1);
}
break;

case '?':
print_usage(basename(argv[0]));
exit(0);
Expand All @@ -712,8 +859,20 @@ int main(int argc, char **argv)
exit(1);
}

if (!modidx && (have_cs_crc8 || have_cs_xor)) {
printf("-c or -x can only be used in conjunction with -m\n");
if (flags & CGW_FLAGS_CAN_FD) {
if (modidx) {
printf("No -m modifications allowed in CAN FD mode!\n");
exit(1);
}
} else {
if (fdmodidx) {
printf("No -M modifications allowed in Classic CAN mode!\n");
exit(1);
}
}

if ((!modidx && !fdmodidx) && (have_cs_crc8 || have_cs_xor)) {
printf("-c or -x can only be used in conjunction with -m/-M\n");
exit(1);
}

Expand Down Expand Up @@ -787,6 +946,10 @@ int main(int argc, char **argv)
for (i = 0; i < modidx; i++)
addattr_l(&req.nh, sizeof(req), modmsg[i].instruction, &modmsg[i], CGW_MODATTR_LEN);

/* add up to CGW_FDMOD_FUNCS modification definitions */
for (i = 0; i < fdmodidx; i++)
addattr_l(&req.nh, sizeof(req), fdmodmsg[i].instruction, &fdmodmsg[i], CGW_FDMODATTR_LEN);

memset(&nladdr, 0, sizeof(nladdr));
nladdr.nl_family = AF_NETLINK;
nladdr.nl_pid = 0;
Expand Down

0 comments on commit d45f3f4

Please sign in to comment.