net/can/isotp.c

313 static int check_optimized(struct canfd_frame *cf, int start_index)
317 	 * start at cf.data[7] cf->len has to be 7 to be optimal.
320 	if (cf->len <= CAN_MAX_DLEN)
321 		return (cf->len != start_index);
325 	 * The correct check would be (padlen(cf->len) != padlen(start_index)).
326 	 * But as cf->len can only take discrete values from 12, .., 64 at this
327 	 * point the padlen(cf->len) is always equal to cf->len.
329 	return (cf->len != padlen(start_index));
333 static int check_pad(struct isotp_sock *so, struct canfd_frame *cf,
341 			return check_optimized(cf, start_index);
349 	    cf->len != padlen(cf->len))
354 		for (i = start_index; i < cf->len; i++)
355 			if (cf->data[i] != content)
363 static int isotp_rcv_fc(struct isotp_sock *so, struct canfd_frame *cf, int ae)
373 	if ((cf->len < ae + FC_CONTENT_SZ) ||
375 	     check_pad(so, cf, ae + FC_CONTENT_SZ, so->opt.rxpad_content))) {
389 		so->txfc.bs = cf->data[ae + 1];
390 		so->txfc.stmin = cf->data[ae + 2];
414 	switch (cf->data[ae] & 0x0F) {
445 static int isotp_rcv_sf(struct sock *sk, struct canfd_frame *cf, int pcilen,
454 	if (!len || len > cf->len - pcilen)
458 	    check_pad(so, cf, pcilen + len, so->opt.rxpad_content)) {
470 	memcpy(skb_put(nskb, len), &cf->data[pcilen], len);
478 static int isotp_rcv_ff(struct sock *sk, struct canfd_frame *cf, int ae)
489 	so->rx.ll_dl = padlen(cf->len);
492 	if (cf->len != so->rx.ll_dl)
496 	so->rx.len = (cf->data[ae] & 0x0F) << 8;
497 	so->rx.len += cf->data[ae + 1];
504 		so->rx.len = cf->data[ae + 2] << 24;
505 		so->rx.len += cf->data[ae + 3] << 16;
506 		so->rx.len += cf->data[ae + 4] << 8;
507 		so->rx.len += cf->data[ae + 5];
536 		so->rx.buf[so->rx.idx++] = cf->data[i];
551 static int isotp_rcv_cf(struct sock *sk, struct canfd_frame *cf, int ae,
573 	if (cf->len > so->rx.ll_dl)
577 	if (cf->len < so->rx.ll_dl) {
583 	if ((cf->data[ae] & 0x0F) != so->rx.sn) {
596 	for (i = ae + N_PCI_SZ; i < cf->len; i++) {
597 		so->rx.buf[so->rx.idx++] = cf->data[i];
607 		    check_pad(so, cf, i + 1, so->opt.rxpad_content)) {
649 	struct canfd_frame *cf;
659 	cf = (struct canfd_frame *)skb->data;
662 	if (ae && cf->data[0] != so->opt.rx_ext_address)
665 	n_pci_type = cf->data[ae] & 0xF0;
683 		isotp_rcv_fc(so, cf, ae);
695 		sf_dl = cf->data[ae] & 0x0F;
697 		if (cf->len <= CAN_MAX_DLEN) {
698 			isotp_rcv_sf(sk, cf, SF_PCI_SZ4 + ae, skb, sf_dl);
710 					isotp_rcv_sf(sk, cf, SF_PCI_SZ8 + ae, skb,
711 						     cf->data[SF_PCI_SZ4 + ae]);
718 		isotp_rcv_ff(sk, cf, ae);
723 		isotp_rcv_cf(sk, cf, ae, skb);
731 static void isotp_fill_dataframe(struct canfd_frame *cf, struct isotp_sock *so,
739 	cf->can_id = so->txid;
740 	cf->len = num + pcilen;
745 			cf->len = padlen(cf->len);
746 			memset(cf->data, so->opt.txpad_content, cf->len);
747 		} else if (cf->len > CAN_MAX_DLEN) {
749 			cf->len = padlen(cf->len);
750 			memset(cf->data, CAN_ISOTP_DEFAULT_PAD_CONTENT,
751 			       cf->len);
756 		cf->data[pcilen + i] = so->tx.buf[so->tx.idx++];
759 		cf->data[0] = so->opt.ext_address;
767 	struct canfd_frame *cf;
785 	cf = (struct canfd_frame *)skb->data;
789 	isotp_fill_dataframe(cf, so, ae, 0);
792 	cf->data[ae] = N_PCI_CF | so->tx.sn++;
796 	cf->flags = so->ll.tx_flags;
806 	so->cfecho = *(u32 *)cf->data;
819 static void isotp_create_fframe(struct canfd_frame *cf, struct isotp_sock *so,
825 	cf->can_id = so->txid;
826 	cf->len = so->tx.ll_dl;
828 		cf->data[0] = so->opt.ext_address;
833 		cf->data[ae] = N_PCI_FF;
834 		cf->data[ae + 1] = 0;
835 		cf->data[ae + 2] = (u8)(so->tx.len >> 24) & 0xFFU;
836 		cf->data[ae + 3] = (u8)(so->tx.len >> 16) & 0xFFU;
837 		cf->data[ae + 4] = (u8)(so->tx.len >> 8) & 0xFFU;
838 		cf->data[ae + 5] = (u8)so->tx.len & 0xFFU;
842 		cf->data[ae] = (u8)(so->tx.len >> 8) | N_PCI_FF;
843 		cf->data[ae + 1] = (u8)so->tx.len & 0xFFU;
849 		cf->data[i] = so->tx.buf[so->tx.idx++];
858 	struct canfd_frame *cf = (struct canfd_frame *)skb->data;
861 	if (skb->sk != sk || so->cfecho != *(u32 *)cf->data)
944 	struct canfd_frame *cf;
1017 	cf = (struct canfd_frame *)skb->data;
1038 		isotp_fill_dataframe(cf, so, ae, off);
1041 		cf->data[ae] = N_PCI_SF;
1045 			cf->data[SF_PCI_SZ4 + ae] = size;
1047 			cf->data[ae] |= size;
1050 		so->cfecho = *(u32 *)cf->data;
1054 		isotp_create_fframe(cf, so, ae);
1067 			so->cfecho = *(u32 *)cf->data;
1084 	cf->flags = so->ll.tx_flags;