Add support for data length code modifications for Classical CAN. The netlink configuration interface always allowed to pass any value that fits into a byte, therefore only the modification process had to be extended to handle the raw DLC represenation of Classical CAN frames. When a DLC value from 0 .. F is provided for Classical CAN frame modifications the 'len' value is modified as-is with the exception that potentially existing 9 .. F DLC values in the len8_dlc element are moved to the 'len' element for the modification operation by mod_retrieve_ccdlc(). After the modification the Classical CAN frame DLC information is brought back into the correct format by mod_store_ccdlc() which is filling 'len' and 'len8_dlc' accordingly.
Unified · Split
| @@ -98,8 +98,8 @@ enum { · 97 unmodified lines | ||
| 98 | 98 | |
| 99 | 99 | /* CAN frame elements that are affected by curr. 3 CAN frame modifications */ |
| 100 | 100 | #define CGW_MOD_ID 0x01 |
| 101 | −#define CGW_MOD_DLC 0x02 /* contains the data length in bytes */ | |
| 102 | −#define CGW_MOD_LEN CGW_MOD_DLC /* CAN FD length representation */ | |
| 101 | +#define CGW_MOD_DLC 0x02 /* Classical CAN data length code */ | |
| 102 | +#define CGW_MOD_LEN CGW_MOD_DLC /* CAN FD (plain) data length */ | |
| 103 | 103 | #define CGW_MOD_DATA 0x04 |
| 104 | 104 | #define CGW_MOD_FLAGS 0x08 /* CAN FD flags */ |
| 105 | 105 | |
| @@ -199,6 +199,68 @@ static void mod_set_fddata(struct canfd_frame *cf, struct cf_mod *mod) · 198 unmodified lines | ||
| 199 | 199 | memcpy(cf->data, mod->modframe.set.data, CANFD_MAX_DLEN); |
| 200 | 200 | } |
| 201 | 201 | |
| 202 | +/* retrieve valid CC DLC value and store it into 'len' */ | |
| 203 | +static void mod_retrieve_ccdlc(struct canfd_frame *cf) | |
| 204 | +{ | |
| 205 | + struct can_frame *ccf = (struct can_frame *)cf; | |
| 206 | + | |
| 207 | + /* len8_dlc is only valid if len == CAN_MAX_DLEN */ | |
| 208 | + if (ccf->len != CAN_MAX_DLEN) | |
| 209 | + return; | |
| 210 | + | |
| 211 | + /* do we have a valid len8_dlc value from 9 .. 15 ? */ | |
| 212 | + if (ccf->len8_dlc > CAN_MAX_DLEN && ccf->len8_dlc <= CAN_MAX_RAW_DLC) | |
| 213 | + ccf->len = ccf->len8_dlc; | |
| 214 | +} | |
| 215 | + | |
| 216 | +/* convert valid CC DLC value in 'len' into struct can_frame elements */ | |
| 217 | +static void mod_store_ccdlc(struct canfd_frame *cf) | |
| 218 | +{ | |
| 219 | + struct can_frame *ccf = (struct can_frame *)cf; | |
| 220 | + | |
| 221 | + /* clear potential leftovers */ | |
| 222 | + ccf->len8_dlc = 0; | |
| 223 | + | |
| 224 | + /* plain data length 0 .. 8 - that was easy */ | |
| 225 | + if (ccf->len <= CAN_MAX_DLEN) | |
| 226 | + return; | |
| 227 | + | |
| 228 | + /* potentially broken values are catched in can_can_gw_rcv() */ | |
| 229 | + if (ccf->len > CAN_MAX_RAW_DLC) | |
| 230 | + return; | |
| 231 | + | |
| 232 | + /* we have a valid dlc value from 9 .. 15 in ccf->len */ | |
| 233 | + ccf->len8_dlc = ccf->len; | |
| 234 | + ccf->len = CAN_MAX_DLEN; | |
| 235 | +} | |
| 236 | + | |
| 237 | +static void mod_and_ccdlc(struct canfd_frame *cf, struct cf_mod *mod) | |
| 238 | +{ | |
| 239 | + mod_retrieve_ccdlc(cf); | |
| 240 | + mod_and_len(cf, mod); | |
| 241 | + mod_store_ccdlc(cf); | |
| 242 | +} | |
| 243 | + | |
| 244 | +static void mod_or_ccdlc(struct canfd_frame *cf, struct cf_mod *mod) | |
| 245 | +{ | |
| 246 | + mod_retrieve_ccdlc(cf); | |
| 247 | + mod_or_len(cf, mod); | |
| 248 | + mod_store_ccdlc(cf); | |
| 249 | +} | |
| 250 | + | |
| 251 | +static void mod_xor_ccdlc(struct canfd_frame *cf, struct cf_mod *mod) | |
| 252 | +{ | |
| 253 | + mod_retrieve_ccdlc(cf); | |
| 254 | + mod_xor_len(cf, mod); | |
| 255 | + mod_store_ccdlc(cf); | |
| 256 | +} | |
| 257 | + | |
| 258 | +static void mod_set_ccdlc(struct canfd_frame *cf, struct cf_mod *mod) | |
| 259 | +{ | |
| 260 | + mod_set_len(cf, mod); | |
| 261 | + mod_store_ccdlc(cf); | |
| 262 | +} | |
| 263 | + | |
| 202 | 264 | static void canframecpy(struct canfd_frame *dst, struct can_frame *src) |
| 203 | 265 | { |
| 204 | 266 | /* Copy the struct members separately to ensure that no uninitialized |
| @@ -842,8 +904,8 @@ static int cgw_parse_attr(struct nlmsghdr *nlh, struct cf_mod *mod, · 637 unmodified lines | ||
| 842 | 904 | if (mb.modtype & CGW_MOD_ID) |
| 843 | 905 | mod->modfunc[modidx++] = mod_and_id; |
| 844 | 906 | |
| 845 | − if (mb.modtype & CGW_MOD_LEN) | |
| 846 | − mod->modfunc[modidx++] = mod_and_len; | |
| 907 | + if (mb.modtype & CGW_MOD_DLC) | |
| 908 | + mod->modfunc[modidx++] = mod_and_ccdlc; | |
| 847 | 909 | |
| 848 | 910 | if (mb.modtype & CGW_MOD_DATA) |
| 849 | 911 | mod->modfunc[modidx++] = mod_and_data; |
| @@ -858,8 +920,8 @@ static int cgw_parse_attr(struct nlmsghdr *nlh, struct cf_mod *mod, · 8 unmodified lines | ||
| 858 | 920 | if (mb.modtype & CGW_MOD_ID) |
| 859 | 921 | mod->modfunc[modidx++] = mod_or_id; |
| 860 | 922 | |
| 861 | − if (mb.modtype & CGW_MOD_LEN) | |
| 862 | − mod->modfunc[modidx++] = mod_or_len; | |
| 923 | + if (mb.modtype & CGW_MOD_DLC) | |
| 924 | + mod->modfunc[modidx++] = mod_or_ccdlc; | |
| 863 | 925 | |
| 864 | 926 | if (mb.modtype & CGW_MOD_DATA) |
| 865 | 927 | mod->modfunc[modidx++] = mod_or_data; |
| @@ -874,8 +936,8 @@ static int cgw_parse_attr(struct nlmsghdr *nlh, struct cf_mod *mod, · 8 unmodified lines | ||
| 874 | 936 | if (mb.modtype & CGW_MOD_ID) |
| 875 | 937 | mod->modfunc[modidx++] = mod_xor_id; |
| 876 | 938 | |
| 877 | − if (mb.modtype & CGW_MOD_LEN) | |
| 878 | − mod->modfunc[modidx++] = mod_xor_len; | |
| 939 | + if (mb.modtype & CGW_MOD_DLC) | |
| 940 | + mod->modfunc[modidx++] = mod_xor_ccdlc; | |
| 879 | 941 | |
| 880 | 942 | if (mb.modtype & CGW_MOD_DATA) |
| 881 | 943 | mod->modfunc[modidx++] = mod_xor_data; |
| @@ -890,8 +952,8 @@ static int cgw_parse_attr(struct nlmsghdr *nlh, struct cf_mod *mod, · 8 unmodified lines | ||
| 890 | 952 | if (mb.modtype & CGW_MOD_ID) |
| 891 | 953 | mod->modfunc[modidx++] = mod_set_id; |
| 892 | 954 | |
| 893 | − if (mb.modtype & CGW_MOD_LEN) | |
| 894 | − mod->modfunc[modidx++] = mod_set_len; | |
| 955 | + if (mb.modtype & CGW_MOD_DLC) | |
| 956 | + mod->modfunc[modidx++] = mod_set_ccdlc; | |
| 895 | 957 | |
| 896 | 958 | if (mb.modtype & CGW_MOD_DATA) |
| 897 | 959 | mod->modfunc[modidx++] = mod_set_data; |