| lists.openwall.net | lists / announce owl-users owl-dev john-users john-dev passwdqc-users yescrypt popa3d-users / oss-security kernel-hardening musl sabotage tlsify passwords / crypt-dev xvendor / Bugtraq Full-Disclosure linux-kernel linux-netdev linux-ext4 linux-hardening linux-cve-announce PHC | |
|
Open Source and information security mailing list archives
| ||
|
Date: Tue, 6 Oct 2020 09:21:56 +0200
From: Oliver Hartkopp <socketcan@...tkopp.net>
To: Marc Kleine-Budde <mkl@...gutronix.de>
Cc: linux-can@...r.kernel.org, netdev@...r.kernel.org
Subject: Re: [PATCH can-next] can: add ISO 15765-2:2016 transport protocol
Wrong URL for the initial commit m(
https://github.com/hartkopp/can-isotp-modules/commit/ec109096fd72c0f286316d0084494582939626e5
Sorry
On 06.10.20 09:19, Oliver Hartkopp wrote:
> Hello Marc,
>
> this code had its initial commit on Nov 5th 2008:
>
> https://github.com/hartkopp/can-isotp/blob/master/net/can/isotp.c#L597
>
> and has been used (and tested) in many productive environments by many
> people since then.
>
> So many (happy) developers contacted my off-list after this post, that
> the mainline patch has finally been sent out.
>
> Too many projects are pointing to my personal GitHub repo today and
> people get more and more problems to make the out-of-tree code run on
> their machines with signed kernel modules. I did some cleanups and fixes
> several checkpatch issues after all.
>
> When there are no objections I would be happy to get the patch into
> linux-can-next for this merge window phase and continue the further
> development (if any) inside the kernel.
>
> Best,
> Oliver
>
> On 28.09.20 22:04, Oliver Hartkopp wrote:
>> CAN Transport Protocols offer support for segmented Point-to-Point
>> communication between CAN nodes via two defined CAN Identifiers.
>> As CAN frames can only transport a small amount of data bytes
>> (max. 8 bytes for 'classic' CAN and max. 64 bytes for CAN FD) this
>> segmentation is needed to transport longer PDUs as needed e.g. for
>> vehicle diagnosis (UDS, ISO 14229) or IP-over-CAN traffic.
>> This protocol driver implements data transfers according to
>> ISO 15765-2:2016 for 'classic' CAN and CAN FD frame types.
>>
>> Signed-off-by: Oliver Hartkopp <socketcan@...tkopp.net>
>> ---
>> MAINTAINERS | 1 +
>> include/uapi/linux/can/isotp.h | 166 ++++
>> net/can/Kconfig | 13 +
>> net/can/Makefile | 3 +
>> net/can/isotp.c | 1428 ++++++++++++++++++++++++++++++++
>> 5 files changed, 1611 insertions(+)
>> create mode 100644 include/uapi/linux/can/isotp.h
>> create mode 100644 net/can/isotp.c
>>
>> diff --git a/MAINTAINERS b/MAINTAINERS
>> index 190c7fa2ea01..92a0f6388f18 100644
>> --- a/MAINTAINERS
>> +++ b/MAINTAINERS
>> @@ -3912,6 +3912,7 @@ F: include/net/netns/can.h
>> F: include/uapi/linux/can.h
>> F: include/uapi/linux/can/bcm.h
>> F: include/uapi/linux/can/gw.h
>> +F: include/uapi/linux/can/isotp.h
>> F: include/uapi/linux/can/raw.h
>> F: net/can/
>> diff --git a/include/uapi/linux/can/isotp.h
>> b/include/uapi/linux/can/isotp.h
>> new file mode 100644
>> index 000000000000..553006509f4e
>> --- /dev/null
>> +++ b/include/uapi/linux/can/isotp.h
>> @@ -0,0 +1,166 @@
>> +/* SPDX-License-Identifier: ((GPL-2.0-only WITH Linux-syscall-note)
>> OR BSD-3-Clause) */
>> +/*
>> + * linux/can/isotp.h
>> + *
>> + * Definitions for isotp CAN sockets (ISO 15765-2:2016)
>> + *
>> + * Copyright (c) 2020 Volkswagen Group Electronic Research
>> + * All rights reserved.
>> + *
>> + * Redistribution and use in source and binary forms, with or without
>> + * modification, are permitted provided that the following conditions
>> + * are met:
>> + * 1. Redistributions of source code must retain the above copyright
>> + * notice, this list of conditions and the following disclaimer.
>> + * 2. Redistributions in binary form must reproduce the above copyright
>> + * notice, this list of conditions and the following disclaimer in
>> the
>> + * documentation and/or other materials provided with the
>> distribution.
>> + * 3. Neither the name of Volkswagen nor the names of its contributors
>> + * may be used to endorse or promote products derived from this
>> software
>> + * without specific prior written permission.
>> + *
>> + * Alternatively, provided that this notice is retained in full, this
>> + * software may be distributed under the terms of the GNU General
>> + * Public License ("GPL") version 2, in which case the provisions of the
>> + * GPL apply INSTEAD OF those given above.
>> + *
>> + * The provided data structures and external interfaces from this code
>> + * are not restricted to be used by modules with a GPL compatible
>> license.
>> + *
>> + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
>> + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
>> + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
>> + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
>> + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
>> + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
>> + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
>> + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
>> + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
>> + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
>> + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
>> + * DAMAGE.
>> + */
>> +
>> +#ifndef _UAPI_CAN_ISOTP_H
>> +#define _UAPI_CAN_ISOTP_H
>> +
>> +#include <linux/types.h>
>> +#include <linux/can.h>
>> +
>> +#define SOL_CAN_ISOTP (SOL_CAN_BASE + CAN_ISOTP)
>> +
>> +/* for socket options affecting the socket (not the global system) */
>> +
>> +#define CAN_ISOTP_OPTS 1 /* pass struct can_isotp_options */
>> +
>> +#define CAN_ISOTP_RECV_FC 2 /* pass struct can_isotp_fc_options */
>> +
>> +/* sockopts to force stmin timer values for protocol regression tests */
>> +
>> +#define CAN_ISOTP_TX_STMIN 3 /* pass __u32 value in nano
>> secs */
>> + /* use this time instead of value */
>> + /* provided in FC from the receiver */
>> +
>> +#define CAN_ISOTP_RX_STMIN 4 /* pass __u32 value in nano secs */
>> + /* ignore received CF frames which */
>> + /* timestamps differ less than val */
>> +
>> +#define CAN_ISOTP_LL_OPTS 5 /* pass struct can_isotp_ll_options */
>> +
>> +struct can_isotp_options {
>> +
>> + __u32 flags; /* set flags for isotp behaviour. */
>> + /* __u32 value : flags see below */
>> +
>> + __u32 frame_txtime; /* frame transmission time (N_As/N_Ar) */
>> + /* __u32 value : time in nano secs */
>> +
>> + __u8 ext_address; /* set address for extended addressing */
>> + /* __u8 value : extended address */
>> +
>> + __u8 txpad_content; /* set content of padding byte (tx) */
>> + /* __u8 value : content on tx path */
>> +
>> + __u8 rxpad_content; /* set content of padding byte (rx) */
>> + /* __u8 value : content on rx path */
>> +
>> + __u8 rx_ext_address; /* set address for extended
>> addressing */
>> + /* __u8 value : extended address (rx) */
>> +};
>> +
>> +struct can_isotp_fc_options {
>> +
>> + __u8 bs; /* blocksize provided in FC frame */
>> + /* __u8 value : blocksize. 0 = off */
>> +
>> + __u8 stmin; /* separation time provided in FC frame */
>> + /* __u8 value : */
>> + /* 0x00 - 0x7F : 0 - 127 ms */
>> + /* 0x80 - 0xF0 : reserved */
>> + /* 0xF1 - 0xF9 : 100 us - 900 us */
>> + /* 0xFA - 0xFF : reserved */
>> +
>> + __u8 wftmax; /* max. number of wait frame transmiss. */
>> + /* __u8 value : 0 = omit FC N_PDU WT */
>> +};
>> +
>> +struct can_isotp_ll_options {
>> +
>> + __u8 mtu; /* generated & accepted CAN frame type */
>> + /* __u8 value : */
>> + /* CAN_MTU (16) -> standard CAN 2.0 */
>> + /* CANFD_MTU (72) -> CAN FD frame */
>> +
>> + __u8 tx_dl; /* tx link layer data length in bytes */
>> + /* (configured maximum payload length) */
>> + /* __u8 value : 8,12,16,20,24,32,48,64 */
>> + /* => rx path supports all LL_DL values */
>> +
>> + __u8 tx_flags; /* set into struct canfd_frame.flags */
>> + /* at frame creation: e.g. CANFD_BRS */
>> + /* Obsolete when the BRS flag is fixed */
>> + /* by the CAN netdriver configuration */
>> +};
>> +
>> +/* flags for isotp behaviour */
>> +
>> +#define CAN_ISOTP_LISTEN_MODE 0x001 /* listen only (do not send
>> FC) */
>> +#define CAN_ISOTP_EXTEND_ADDR 0x002 /* enable extended
>> addressing */
>> +#define CAN_ISOTP_TX_PADDING 0x004 /* enable CAN frame padding
>> tx path */
>> +#define CAN_ISOTP_RX_PADDING 0x008 /* enable CAN frame padding
>> rx path */
>> +#define CAN_ISOTP_CHK_PAD_LEN 0x010 /* check received CAN frame
>> padding */
>> +#define CAN_ISOTP_CHK_PAD_DATA 0x020 /* check received CAN
>> frame padding */
>> +#define CAN_ISOTP_HALF_DUPLEX 0x040 /* half duplex error state
>> handling */
>> +#define CAN_ISOTP_FORCE_TXSTMIN 0x080 /* ignore stmin from
>> received FC */
>> +#define CAN_ISOTP_FORCE_RXSTMIN 0x100 /* ignore CFs depending
>> on rx stmin */
>> +#define CAN_ISOTP_RX_EXT_ADDR 0x200 /* different rx extended
>> addressing */
>> +#define CAN_ISOTP_WAIT_TX_DONE 0x400 /* wait for tx completion */
>> +
>> +
>> +/* default values */
>> +
>> +#define CAN_ISOTP_DEFAULT_FLAGS 0
>> +#define CAN_ISOTP_DEFAULT_EXT_ADDRESS 0x00
>> +#define CAN_ISOTP_DEFAULT_PAD_CONTENT 0xCC /* prevent bit-stuffing */
>> +#define CAN_ISOTP_DEFAULT_FRAME_TXTIME 0
>> +#define CAN_ISOTP_DEFAULT_RECV_BS 0
>> +#define CAN_ISOTP_DEFAULT_RECV_STMIN 0x00
>> +#define CAN_ISOTP_DEFAULT_RECV_WFTMAX 0
>> +
>> +#define CAN_ISOTP_DEFAULT_LL_MTU CAN_MTU
>> +#define CAN_ISOTP_DEFAULT_LL_TX_DL CAN_MAX_DLEN
>> +#define CAN_ISOTP_DEFAULT_LL_TX_FLAGS 0
>> +
>> +/*
>> + * Remark on CAN_ISOTP_DEFAULT_RECV_* values:
>> + *
>> + * We can strongly assume, that the Linux Kernel implementation of
>> + * CAN_ISOTP is capable to run with BS=0, STmin=0 and WFTmax=0.
>> + * But as we like to be able to behave as a commonly available ECU,
>> + * these default settings can be changed via sockopts.
>> + * For that reason the STmin value is intentionally _not_ checked for
>> + * consistency and copied directly into the flow control (FC) frame.
>> + *
>> + */
>> +
>> +#endif /* !_UAPI_CAN_ISOTP_H */
>> diff --git a/net/can/Kconfig b/net/can/Kconfig
>> index 25436a715db3..021fe03a8ed6 100644
>> --- a/net/can/Kconfig
>> +++ b/net/can/Kconfig
>> @@ -55,6 +55,19 @@ config CAN_GW
>> source "net/can/j1939/Kconfig"
>> +config CAN_ISOTP
>> + tristate "ISO 15765-2:2016 CAN transport protocol"
>> + default y
>> + help
>> + CAN Transport Protocols offer support for segmented Point-to-Point
>> + communication between CAN nodes via two defined CAN Identifiers.
>> + As CAN frames can only transport a small amount of data bytes
>> + (max. 8 bytes for 'classic' CAN and max. 64 bytes for CAN FD) this
>> + segmentation is needed to transport longer PDUs as needed e.g. for
>> + vehicle diagnosis (UDS, ISO 14229) or IP-over-CAN traffic.
>> + This protocol driver implements data transfers according to
>> + ISO 15765-2:2016 for 'classic' CAN and CAN FD frame types.
>> +
>> source "drivers/net/can/Kconfig"
>> endif
>> diff --git a/net/can/Makefile b/net/can/Makefile
>> index 08bd217fc051..58f2c31c1ef3 100644
>> --- a/net/can/Makefile
>> +++ b/net/can/Makefile
>> @@ -17,3 +17,6 @@ obj-$(CONFIG_CAN_GW) += can-gw.o
>> can-gw-y := gw.o
>> obj-$(CONFIG_CAN_J1939) += j1939/
>> +
>> +obj-$(CONFIG_CAN_ISOTP) += can-isotp.o
>> +can-isotp-y := isotp.o
>> diff --git a/net/can/isotp.c b/net/can/isotp.c
>> new file mode 100644
>> index 000000000000..efed3e47b6ee
>> --- /dev/null
>> +++ b/net/can/isotp.c
>> @@ -0,0 +1,1428 @@
>> +// SPDX-License-Identifier: ((GPL-2.0 WITH Linux-syscall-note) OR
>> BSD-3-Clause)
>> +/* isotp.c - ISO 15765-2 CAN transport protocol for protocol family CAN
>> + *
>> + * This implementation does not provide ISO-TP specific return values
>> to the
>> + * userspace.
>> + *
>> + * - RX path timeout of data reception leads to -ETIMEDOUT
>> + * - RX path SN mismatch leads to -EILSEQ
>> + * - RX path data reception with wrong padding leads to -EBADMSG
>> + * - TX path flowcontrol reception timeout leads to -ECOMM
>> + * - TX path flowcontrol reception overflow leads to -EMSGSIZE
>> + * - TX path flowcontrol reception with wrong layout/padding leads to
>> -EBADMSG
>> + * - when a transfer (tx) is on the run the next write() blocks until
>> it's done
>> + * - use CAN_ISOTP_WAIT_TX_DONE flag to block the caller until the
>> PDU is sent
>> + * - as we have static buffers the check whether the PDU fits into
>> the buffer
>> + * is done at FF reception time (no support for sending 'wait frames')
>> + * - take care of the tx-queue-len as traffic shaping is still on the
>> TODO list
>> + *
>> + * Copyright (c) 2020 Volkswagen Group Electronic Research
>> + * All rights reserved.
>> + *
>> + * Redistribution and use in source and binary forms, with or without
>> + * modification, are permitted provided that the following conditions
>> + * are met:
>> + * 1. Redistributions of source code must retain the above copyright
>> + * notice, this list of conditions and the following disclaimer.
>> + * 2. Redistributions in binary form must reproduce the above copyright
>> + * notice, this list of conditions and the following disclaimer in
>> the
>> + * documentation and/or other materials provided with the
>> distribution.
>> + * 3. Neither the name of Volkswagen nor the names of its contributors
>> + * may be used to endorse or promote products derived from this
>> software
>> + * without specific prior written permission.
>> + *
>> + * Alternatively, provided that this notice is retained in full, this
>> + * software may be distributed under the terms of the GNU General
>> + * Public License ("GPL") version 2, in which case the provisions of the
>> + * GPL apply INSTEAD OF those given above.
>> + *
>> + * The provided data structures and external interfaces from this code
>> + * are not restricted to be used by modules with a GPL compatible
>> license.
>> + *
>> + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
>> + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
>> + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
>> + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
>> + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
>> + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
>> + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
>> + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
>> + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
>> + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
>> + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
>> + * DAMAGE.
>> + */
>> +
>> +#include <linux/module.h>
>> +#include <linux/init.h>
>> +#include <linux/interrupt.h>
>> +#include <linux/hrtimer.h>
>> +#include <linux/wait.h>
>> +#include <linux/uio.h>
>> +#include <linux/net.h>
>> +#include <linux/netdevice.h>
>> +#include <linux/socket.h>
>> +#include <linux/if_arp.h>
>> +#include <linux/skbuff.h>
>> +#include <linux/can.h>
>> +#include <linux/can/core.h>
>> +#include <linux/can/skb.h>
>> +#include <linux/can/isotp.h>
>> +#include <linux/slab.h>
>> +#include <net/sock.h>
>> +#include <net/net_namespace.h>
>> +
>> +#define CAN_ISOTP_VERSION "20200928"
>> +
>> +MODULE_DESCRIPTION("PF_CAN isotp 15765-2:2016 protocol");
>> +MODULE_LICENSE("Dual BSD/GPL");
>> +MODULE_AUTHOR("Oliver Hartkopp <socketcan@...tkopp.net>");
>> +MODULE_ALIAS("can-proto-6");
>> +
>> +#define SINGLE_MASK(id) ((id & CAN_EFF_FLAG) ? \
>> + (CAN_EFF_MASK | CAN_EFF_FLAG | CAN_RTR_FLAG) : \
>> + (CAN_SFF_MASK | CAN_EFF_FLAG | CAN_RTR_FLAG))
>> +
>> +/* ISO 15765-2:2016 supports more than 4095 byte per ISO PDU as the
>> FF_DL can
>> + * take full 32 bit values (4 Gbyte). We would need some good concept
>> to handle
>> + * this between user space and kernel space. For now increase the
>> static buffer
>> + * to something about 8 kbyte to be able to test this new functionality.
>> + */
>> +#define MAX_MSG_LENGTH 8200
>> +
>> +/* N_PCI type values in bits 7-4 of N_PCI bytes */
>> +#define N_PCI_SF 0x00 /* single frame */
>> +#define N_PCI_FF 0x10 /* first frame */
>> +#define N_PCI_CF 0x20 /* consecutive frame */
>> +#define N_PCI_FC 0x30 /* flow control */
>> +
>> +#define N_PCI_SZ 1 /* size of the PCI byte #1 */
>> +#define SF_PCI_SZ4 1 /* size of SingleFrame PCI including 4 bit
>> SF_DL */
>> +#define SF_PCI_SZ8 2 /* size of SingleFrame PCI including 8 bit
>> SF_DL */
>> +#define FF_PCI_SZ12 2 /* size of FirstFrame PCI including 12 bit
>> FF_DL */
>> +#define FF_PCI_SZ32 6 /* size of FirstFrame PCI including 32 bit
>> FF_DL */
>> +#define FC_CONTENT_SZ 3 /* flow control content size in byte
>> (FS/BS/STmin) */
>> +
>> +#define ISOTP_CHECK_PADDING (CAN_ISOTP_CHK_PAD_LEN |
>> CAN_ISOTP_CHK_PAD_DATA)
>> +
>> +/* Flow Status given in FC frame */
>> +#define ISOTP_FC_CTS 0 /* clear to send */
>> +#define ISOTP_FC_WT 1 /* wait */
>> +#define ISOTP_FC_OVFLW 2 /* overflow */
>> +
>> +enum {
>> + ISOTP_IDLE = 0,
>> + ISOTP_WAIT_FIRST_FC,
>> + ISOTP_WAIT_FC,
>> + ISOTP_WAIT_DATA,
>> + ISOTP_SENDING
>> +};
>> +
>> +struct tpcon {
>> + int idx;
>> + int len;
>> + u8 state;
>> + u8 bs;
>> + u8 sn;
>> + u8 ll_dl;
>> + u8 buf[MAX_MSG_LENGTH + 1];
>> +};
>> +
>> +struct isotp_sock {
>> + struct sock sk;
>> + int bound;
>> + int ifindex;
>> + canid_t txid;
>> + canid_t rxid;
>> + ktime_t tx_gap;
>> + ktime_t lastrxcf_tstamp;
>> + struct hrtimer rxtimer, txtimer;
>> + struct can_isotp_options opt;
>> + struct can_isotp_fc_options rxfc, txfc;
>> + struct can_isotp_ll_options ll;
>> + __u32 force_tx_stmin;
>> + __u32 force_rx_stmin;
>> + struct tpcon rx, tx;
>> + struct notifier_block notifier;
>> + wait_queue_head_t wait;
>> +};
>> +
>> +static inline struct isotp_sock *isotp_sk(const struct sock *sk)
>> +{
>> + return (struct isotp_sock *)sk;
>> +}
>> +
>> +static enum hrtimer_restart isotp_rx_timer_handler(struct hrtimer
>> *hrtimer)
>> +{
>> + struct isotp_sock *so = container_of(hrtimer, struct isotp_sock,
>> + rxtimer);
>> + struct sock *sk = &so->sk;
>> +
>> + if (so->rx.state == ISOTP_WAIT_DATA) {
>> + /* we did not get new data frames in time */
>> +
>> + /* report 'connection timed out' */
>> + sk->sk_err = ETIMEDOUT;
>> + if (!sock_flag(sk, SOCK_DEAD))
>> + sk->sk_error_report(sk);
>> +
>> + /* reset rx state */
>> + so->rx.state = ISOTP_IDLE;
>> + }
>> +
>> + return HRTIMER_NORESTART;
>> +}
>> +
>> +static int isotp_send_fc(struct sock *sk, int ae, u8 flowstatus)
>> +{
>> + struct net_device *dev;
>> + struct sk_buff *nskb;
>> + struct canfd_frame *ncf;
>> + struct isotp_sock *so = isotp_sk(sk);
>> + int can_send_ret;
>> +
>> + nskb = alloc_skb(so->ll.mtu + sizeof(struct can_skb_priv),
>> gfp_any());
>> + if (!nskb)
>> + return 1;
>> +
>> + dev = dev_get_by_index(sock_net(sk), so->ifindex);
>> + if (!dev) {
>> + kfree_skb(nskb);
>> + return 1;
>> + }
>> +
>> + can_skb_reserve(nskb);
>> + can_skb_prv(nskb)->ifindex = dev->ifindex;
>> + can_skb_prv(nskb)->skbcnt = 0;
>> +
>> + nskb->dev = dev;
>> + can_skb_set_owner(nskb, sk);
>> + ncf = (struct canfd_frame *)nskb->data;
>> + skb_put(nskb, so->ll.mtu);
>> +
>> + /* create & send flow control reply */
>> + ncf->can_id = so->txid;
>> +
>> + if (so->opt.flags & CAN_ISOTP_TX_PADDING) {
>> + memset(ncf->data, so->opt.txpad_content, CAN_MAX_DLEN);
>> + ncf->len = CAN_MAX_DLEN;
>> + } else {
>> + ncf->len = ae + FC_CONTENT_SZ;
>> + }
>> +
>> + ncf->data[ae] = N_PCI_FC | flowstatus;
>> + ncf->data[ae + 1] = so->rxfc.bs;
>> + ncf->data[ae + 2] = so->rxfc.stmin;
>> +
>> + if (ae)
>> + ncf->data[0] = so->opt.ext_address;
>> +
>> + if (so->ll.mtu == CANFD_MTU)
>> + ncf->flags = so->ll.tx_flags;
>> +
>> + can_send_ret = can_send(nskb, 1);
>> + if (can_send_ret)
>> + printk_once(KERN_NOTICE "can-isotp: %s: can_send_ret %d\n",
>> + __func__, can_send_ret);
>> +
>> + dev_put(dev);
>> +
>> + /* reset blocksize counter */
>> + so->rx.bs = 0;
>> +
>> + /* reset last CF frame rx timestamp for rx stmin enforcement */
>> + so->lastrxcf_tstamp = ktime_set(0, 0);
>> +
>> + /* start rx timeout watchdog */
>> + hrtimer_start(&so->rxtimer, ktime_set(1, 0), HRTIMER_MODE_REL_SOFT);
>> + return 0;
>> +}
>> +
>> +static void isotp_rcv_skb(struct sk_buff *skb, struct sock *sk)
>> +{
>> + struct sockaddr_can *addr = (struct sockaddr_can *)skb->cb;
>> +
>> + BUILD_BUG_ON(sizeof(skb->cb) < sizeof(struct sockaddr_can));
>> +
>> + memset(addr, 0, sizeof(*addr));
>> + addr->can_family = AF_CAN;
>> + addr->can_ifindex = skb->dev->ifindex;
>> +
>> + if (sock_queue_rcv_skb(sk, skb) < 0)
>> + kfree_skb(skb);
>> +}
>> +
>> +static u8 padlen(u8 datalen)
>> +{
>> + const u8 plen[] = {8, 8, 8, 8, 8, 8, 8, 8, 8, /* 0 - 8 */
>> + 12, 12, 12, 12, /* 9 - 12 */
>> + 16, 16, 16, 16, /* 13 - 16 */
>> + 20, 20, 20, 20, /* 17 - 20 */
>> + 24, 24, 24, 24, /* 21 - 24 */
>> + 32, 32, 32, 32, 32, 32, 32, 32, /* 25 - 32 */
>> + 48, 48, 48, 48, 48, 48, 48, 48, /* 33 - 40 */
>> + 48, 48, 48, 48, 48, 48, 48, 48}; /* 41 - 48 */
>> +
>> + if (datalen > 48)
>> + return 64;
>> +
>> + return plen[datalen];
>> +}
>> +
>> +/* check for length optimization and return 1/true when the check
>> fails */
>> +static int check_optimized(struct canfd_frame *cf, int start_index)
>> +{
>> + /* for CAN_DL <= 8 the start_index is equal to the CAN_DL as the
>> + * padding would start at this point. E.g. if the padding would
>> + * start at cf.data[7] cf->len has to be 7 to be optimal.
>> + * Note: The data[] index starts with zero.
>> + */
>> + if (cf->len <= CAN_MAX_DLEN)
>> + return (cf->len != start_index);
>> +
>> + /* This relation is also valid in the non-linear DLC range, where
>> + * we need to take care of the minimal next possible CAN_DL.
>> + * The correct check would be (padlen(cf->len) !=
>> padlen(start_index)).
>> + * But as cf->len can only take discrete values from 12, .., 64
>> at this
>> + * point the padlen(cf->len) is always equal to cf->len.
>> + */
>> + return (cf->len != padlen(start_index));
>> +}
>> +
>> +/* check padding and return 1/true when the check fails */
>> +static int check_pad(struct isotp_sock *so, struct canfd_frame *cf,
>> + int start_index, __u8 content)
>> +{
>> + int i;
>> +
>> + /* no RX_PADDING value => check length of optimized frame length */
>> + if (!(so->opt.flags & CAN_ISOTP_RX_PADDING)) {
>> + if (so->opt.flags & CAN_ISOTP_CHK_PAD_LEN)
>> + return check_optimized(cf, start_index);
>> +
>> + /* no valid test against empty value => ignore frame */
>> + return 1;
>> + }
>> +
>> + /* check datalength of correctly padded CAN frame */
>> + if ((so->opt.flags & CAN_ISOTP_CHK_PAD_LEN) &&
>> + cf->len != padlen(cf->len))
>> + return 1;
>> +
>> + /* check padding content */
>> + if (so->opt.flags & CAN_ISOTP_CHK_PAD_DATA) {
>> + for (i = start_index; i < cf->len; i++)
>> + if (cf->data[i] != content)
>> + return 1;
>> + }
>> + return 0;
>> +}
>> +
>> +static int isotp_rcv_fc(struct isotp_sock *so, struct canfd_frame
>> *cf, int ae)
>> +{
>> + struct sock *sk = &so->sk;
>> +
>> + if (so->tx.state != ISOTP_WAIT_FC &&
>> + so->tx.state != ISOTP_WAIT_FIRST_FC)
>> + return 0;
>> +
>> + hrtimer_cancel(&so->txtimer);
>> +
>> + if ((cf->len < ae + FC_CONTENT_SZ) ||
>> + ((so->opt.flags & ISOTP_CHECK_PADDING) &&
>> + check_pad(so, cf, ae + FC_CONTENT_SZ,
>> so->opt.rxpad_content))) {
>> + /* malformed PDU - report 'not a data message' */
>> + sk->sk_err = EBADMSG;
>> + if (!sock_flag(sk, SOCK_DEAD))
>> + sk->sk_error_report(sk);
>> +
>> + so->tx.state = ISOTP_IDLE;
>> + wake_up_interruptible(&so->wait);
>> + return 1;
>> + }
>> +
>> + /* get communication parameters only from the first FC frame */
>> + if (so->tx.state == ISOTP_WAIT_FIRST_FC) {
>> + so->txfc.bs = cf->data[ae + 1];
>> + so->txfc.stmin = cf->data[ae + 2];
>> +
>> + /* fix wrong STmin values according spec */
>> + if (so->txfc.stmin > 0x7F &&
>> + (so->txfc.stmin < 0xF1 || so->txfc.stmin > 0xF9))
>> + so->txfc.stmin = 0x7F;
>> +
>> + so->tx_gap = ktime_set(0, 0);
>> + /* add transmission time for CAN frame N_As */
>> + so->tx_gap = ktime_add_ns(so->tx_gap, so->opt.frame_txtime);
>> + /* add waiting time for consecutive frames N_Cs */
>> + if (so->opt.flags & CAN_ISOTP_FORCE_TXSTMIN)
>> + so->tx_gap = ktime_add_ns(so->tx_gap,
>> + so->force_tx_stmin);
>> + else if (so->txfc.stmin < 0x80)
>> + so->tx_gap = ktime_add_ns(so->tx_gap,
>> + so->txfc.stmin * 1000000);
>> + else
>> + so->tx_gap = ktime_add_ns(so->tx_gap,
>> + (so->txfc.stmin - 0xF0)
>> + * 100000);
>> + so->tx.state = ISOTP_WAIT_FC;
>> + }
>> +
>> + switch (cf->data[ae] & 0x0F) {
>> + case ISOTP_FC_CTS:
>> + so->tx.bs = 0;
>> + so->tx.state = ISOTP_SENDING;
>> + /* start cyclic timer for sending CF frame */
>> + hrtimer_start(&so->txtimer, so->tx_gap,
>> + HRTIMER_MODE_REL_SOFT);
>> + break;
>> +
>> + case ISOTP_FC_WT:
>> + /* start timer to wait for next FC frame */
>> + hrtimer_start(&so->txtimer, ktime_set(1, 0),
>> + HRTIMER_MODE_REL_SOFT);
>> + break;
>> +
>> + case ISOTP_FC_OVFLW:
>> + /* overflow on receiver side - report 'message too long' */
>> + sk->sk_err = EMSGSIZE;
>> + if (!sock_flag(sk, SOCK_DEAD))
>> + sk->sk_error_report(sk);
>> + fallthrough;
>> +
>> + default:
>> + /* stop this tx job */
>> + so->tx.state = ISOTP_IDLE;
>> + wake_up_interruptible(&so->wait);
>> + }
>> + return 0;
>> +}
>> +
>> +static int isotp_rcv_sf(struct sock *sk, struct canfd_frame *cf, int
>> pcilen,
>> + struct sk_buff *skb, int len)
>> +{
>> + struct isotp_sock *so = isotp_sk(sk);
>> + struct sk_buff *nskb;
>> +
>> + hrtimer_cancel(&so->rxtimer);
>> + so->rx.state = ISOTP_IDLE;
>> +
>> + if (!len || len > cf->len - pcilen)
>> + return 1;
>> +
>> + if ((so->opt.flags & ISOTP_CHECK_PADDING) &&
>> + check_pad(so, cf, pcilen + len, so->opt.rxpad_content)) {
>> + /* malformed PDU - report 'not a data message' */
>> + sk->sk_err = EBADMSG;
>> + if (!sock_flag(sk, SOCK_DEAD))
>> + sk->sk_error_report(sk);
>> + return 1;
>> + }
>> +
>> + nskb = alloc_skb(len, gfp_any());
>> + if (!nskb)
>> + return 1;
>> +
>> + memcpy(skb_put(nskb, len), &cf->data[pcilen], len);
>> +
>> + nskb->tstamp = skb->tstamp;
>> + nskb->dev = skb->dev;
>> + isotp_rcv_skb(nskb, sk);
>> + return 0;
>> +}
>> +
>> +static int isotp_rcv_ff(struct sock *sk, struct canfd_frame *cf, int ae)
>> +{
>> + struct isotp_sock *so = isotp_sk(sk);
>> + int i;
>> + int off;
>> + int ff_pci_sz;
>> +
>> + hrtimer_cancel(&so->rxtimer);
>> + so->rx.state = ISOTP_IDLE;
>> +
>> + /* get the used sender LL_DL from the (first) CAN frame data
>> length */
>> + so->rx.ll_dl = padlen(cf->len);
>> +
>> + /* the first frame has to use the entire frame up to LL_DL length */
>> + if (cf->len != so->rx.ll_dl)
>> + return 1;
>> +
>> + /* get the FF_DL */
>> + so->rx.len = (cf->data[ae] & 0x0F) << 8;
>> + so->rx.len += cf->data[ae + 1];
>> +
>> + /* Check for FF_DL escape sequence supporting 32 bit PDU length */
>> + if (so->rx.len) {
>> + ff_pci_sz = FF_PCI_SZ12;
>> + } else {
>> + /* FF_DL = 0 => get real length from next 4 bytes */
>> + so->rx.len = cf->data[ae + 2] << 24;
>> + so->rx.len += cf->data[ae + 3] << 16;
>> + so->rx.len += cf->data[ae + 4] << 8;
>> + so->rx.len += cf->data[ae + 5];
>> + ff_pci_sz = FF_PCI_SZ32;
>> + }
>> +
>> + /* take care of a potential SF_DL ESC offset for TX_DL > 8 */
>> + off = (so->rx.ll_dl > CAN_MAX_DLEN) ? 1 : 0;
>> +
>> + if (so->rx.len + ae + off + ff_pci_sz < so->rx.ll_dl)
>> + return 1;
>> +
>> + if (so->rx.len > MAX_MSG_LENGTH) {
>> + /* send FC frame with overflow status */
>> + isotp_send_fc(sk, ae, ISOTP_FC_OVFLW);
>> + return 1;
>> + }
>> +
>> + /* copy the first received data bytes */
>> + so->rx.idx = 0;
>> + for (i = ae + ff_pci_sz; i < so->rx.ll_dl; i++)
>> + so->rx.buf[so->rx.idx++] = cf->data[i];
>> +
>> + /* initial setup for this pdu receiption */
>> + so->rx.sn = 1;
>> + so->rx.state = ISOTP_WAIT_DATA;
>> +
>> + /* no creation of flow control frames */
>> + if (so->opt.flags & CAN_ISOTP_LISTEN_MODE)
>> + return 0;
>> +
>> + /* send our first FC frame */
>> + isotp_send_fc(sk, ae, ISOTP_FC_CTS);
>> + return 0;
>> +}
>> +
>> +static int isotp_rcv_cf(struct sock *sk, struct canfd_frame *cf, int ae,
>> + struct sk_buff *skb)
>> +{
>> + struct isotp_sock *so = isotp_sk(sk);
>> + struct sk_buff *nskb;
>> + int i;
>> +
>> + if (so->rx.state != ISOTP_WAIT_DATA)
>> + return 0;
>> +
>> + /* drop if timestamp gap is less than force_rx_stmin nano secs */
>> + if (so->opt.flags & CAN_ISOTP_FORCE_RXSTMIN) {
>> + if (ktime_to_ns(ktime_sub(skb->tstamp, so->lastrxcf_tstamp)) <
>> + so->force_rx_stmin)
>> + return 0;
>> +
>> + so->lastrxcf_tstamp = skb->tstamp;
>> + }
>> +
>> + hrtimer_cancel(&so->rxtimer);
>> +
>> + /* CFs are never longer than the FF */
>> + if (cf->len > so->rx.ll_dl)
>> + return 1;
>> +
>> + /* CFs have usually the LL_DL length */
>> + if (cf->len < so->rx.ll_dl) {
>> + /* this is only allowed for the last CF */
>> + if (so->rx.len - so->rx.idx > so->rx.ll_dl - ae - N_PCI_SZ)
>> + return 1;
>> + }
>> +
>> + if ((cf->data[ae] & 0x0F) != so->rx.sn) {
>> + /* wrong sn detected - report 'illegal byte sequence' */
>> + sk->sk_err = EILSEQ;
>> + if (!sock_flag(sk, SOCK_DEAD))
>> + sk->sk_error_report(sk);
>> +
>> + /* reset rx state */
>> + so->rx.state = ISOTP_IDLE;
>> + return 1;
>> + }
>> + so->rx.sn++;
>> + so->rx.sn %= 16;
>> +
>> + for (i = ae + N_PCI_SZ; i < cf->len; i++) {
>> + so->rx.buf[so->rx.idx++] = cf->data[i];
>> + if (so->rx.idx >= so->rx.len)
>> + break;
>> + }
>> +
>> + if (so->rx.idx >= so->rx.len) {
>> + /* we are done */
>> + so->rx.state = ISOTP_IDLE;
>> +
>> + if ((so->opt.flags & ISOTP_CHECK_PADDING) &&
>> + check_pad(so, cf, i + 1, so->opt.rxpad_content)) {
>> + /* malformed PDU - report 'not a data message' */
>> + sk->sk_err = EBADMSG;
>> + if (!sock_flag(sk, SOCK_DEAD))
>> + sk->sk_error_report(sk);
>> + return 1;
>> + }
>> +
>> + nskb = alloc_skb(so->rx.len, gfp_any());
>> + if (!nskb)
>> + return 1;
>> +
>> + memcpy(skb_put(nskb, so->rx.len), so->rx.buf,
>> + so->rx.len);
>> +
>> + nskb->tstamp = skb->tstamp;
>> + nskb->dev = skb->dev;
>> + isotp_rcv_skb(nskb, sk);
>> + return 0;
>> + }
>> +
>> + /* no creation of flow control frames */
>> + if (so->opt.flags & CAN_ISOTP_LISTEN_MODE)
>> + return 0;
>> +
>> + /* perform blocksize handling, if enabled */
>> + if (!so->rxfc.bs || ++so->rx.bs < so->rxfc.bs) {
>> + /* start rx timeout watchdog */
>> + hrtimer_start(&so->rxtimer, ktime_set(1, 0),
>> + HRTIMER_MODE_REL_SOFT);
>> + return 0;
>> + }
>> +
>> + /* we reached the specified blocksize so->rxfc.bs */
>> + isotp_send_fc(sk, ae, ISOTP_FC_CTS);
>> + return 0;
>> +}
>> +
>> +static void isotp_rcv(struct sk_buff *skb, void *data)
>> +{
>> + struct sock *sk = (struct sock *)data;
>> + struct isotp_sock *so = isotp_sk(sk);
>> + struct canfd_frame *cf;
>> + int ae = (so->opt.flags & CAN_ISOTP_EXTEND_ADDR) ? 1 : 0;
>> + u8 n_pci_type, sf_dl;
>> +
>> + /* Strictly receive only frames with the configured MTU size
>> + * => clear separation of CAN2.0 / CAN FD transport channels
>> + */
>> + if (skb->len != so->ll.mtu)
>> + return;
>> +
>> + cf = (struct canfd_frame *)skb->data;
>> +
>> + /* if enabled: check receiption of my configured extended address */
>> + if (ae && cf->data[0] != so->opt.rx_ext_address)
>> + return;
>> +
>> + n_pci_type = cf->data[ae] & 0xF0;
>> +
>> + if (so->opt.flags & CAN_ISOTP_HALF_DUPLEX) {
>> + /* check rx/tx path half duplex expectations */
>> + if ((so->tx.state != ISOTP_IDLE && n_pci_type != N_PCI_FC) ||
>> + (so->rx.state != ISOTP_IDLE && n_pci_type == N_PCI_FC))
>> + return;
>> + }
>> +
>> + switch (n_pci_type) {
>> + case N_PCI_FC:
>> + /* tx path: flow control frame containing the FC parameters */
>> + isotp_rcv_fc(so, cf, ae);
>> + break;
>> +
>> + case N_PCI_SF:
>> + /* rx path: single frame
>> + *
>> + * As we do not have a rx.ll_dl configuration, we can only test
>> + * if the CAN frames payload length matches the LL_DL == 8
>> + * requirements - no matter if it's CAN 2.0 or CAN FD
>> + */
>> +
>> + /* get the SF_DL from the N_PCI byte */
>> + sf_dl = cf->data[ae] & 0x0F;
>> +
>> + if (cf->len <= CAN_MAX_DLEN) {
>> + isotp_rcv_sf(sk, cf, SF_PCI_SZ4 + ae, skb, sf_dl);
>> + } else {
>> + if (skb->len == CANFD_MTU) {
>> + /* We have a CAN FD frame and CAN_DL is greater than 8:
>> + * Only frames with the SF_DL == 0 ESC value are valid.
>> + *
>> + * If so take care of the increased SF PCI size
>> + * (SF_PCI_SZ8) to point to the message content behind
>> + * the extended SF PCI info and get the real SF_DL
>> + * length value from the formerly first data byte.
>> + */
>> + if (sf_dl == 0)
>> + isotp_rcv_sf(sk, cf, SF_PCI_SZ8 + ae, skb,
>> + cf->data[SF_PCI_SZ4 + ae]);
>> + }
>> + }
>> + break;
>> +
>> + case N_PCI_FF:
>> + /* rx path: first frame */
>> + isotp_rcv_ff(sk, cf, ae);
>> + break;
>> +
>> + case N_PCI_CF:
>> + /* rx path: consecutive frame */
>> + isotp_rcv_cf(sk, cf, ae, skb);
>> + break;
>> + }
>> +}
>> +
>> +static void isotp_fill_dataframe(struct canfd_frame *cf, struct
>> isotp_sock *so,
>> + int ae, int off)
>> +{
>> + int pcilen = N_PCI_SZ + ae + off;
>> + int space = so->tx.ll_dl - pcilen;
>> + int num = min_t(int, so->tx.len - so->tx.idx, space);
>> + int i;
>> +
>> + cf->can_id = so->txid;
>> + cf->len = num + pcilen;
>> +
>> + if (num < space) {
>> + if (so->opt.flags & CAN_ISOTP_TX_PADDING) {
>> + /* user requested padding */
>> + cf->len = padlen(cf->len);
>> + memset(cf->data, so->opt.txpad_content, cf->len);
>> + } else if (cf->len > CAN_MAX_DLEN) {
>> + /* mandatory padding for CAN FD frames */
>> + cf->len = padlen(cf->len);
>> + memset(cf->data, CAN_ISOTP_DEFAULT_PAD_CONTENT,
>> + cf->len);
>> + }
>> + }
>> +
>> + for (i = 0; i < num; i++)
>> + cf->data[pcilen + i] = so->tx.buf[so->tx.idx++];
>> +
>> + if (ae)
>> + cf->data[0] = so->opt.ext_address;
>> +}
>> +
>> +static void isotp_create_fframe(struct canfd_frame *cf, struct
>> isotp_sock *so,
>> + int ae)
>> +{
>> + int i;
>> + int ff_pci_sz;
>> +
>> + cf->can_id = so->txid;
>> + cf->len = so->tx.ll_dl;
>> + if (ae)
>> + cf->data[0] = so->opt.ext_address;
>> +
>> + /* create N_PCI bytes with 12/32 bit FF_DL data length */
>> + if (so->tx.len > 4095) {
>> + /* use 32 bit FF_DL notation */
>> + cf->data[ae] = N_PCI_FF;
>> + cf->data[ae + 1] = 0;
>> + cf->data[ae + 2] = (u8)(so->tx.len >> 24) & 0xFFU;
>> + cf->data[ae + 3] = (u8)(so->tx.len >> 16) & 0xFFU;
>> + cf->data[ae + 4] = (u8)(so->tx.len >> 8) & 0xFFU;
>> + cf->data[ae + 5] = (u8)so->tx.len & 0xFFU;
>> + ff_pci_sz = FF_PCI_SZ32;
>> + } else {
>> + /* use 12 bit FF_DL notation */
>> + cf->data[ae] = (u8)(so->tx.len >> 8) | N_PCI_FF;
>> + cf->data[ae + 1] = (u8)so->tx.len & 0xFFU;
>> + ff_pci_sz = FF_PCI_SZ12;
>> + }
>> +
>> + /* add first data bytes depending on ae */
>> + for (i = ae + ff_pci_sz; i < so->tx.ll_dl; i++)
>> + cf->data[i] = so->tx.buf[so->tx.idx++];
>> +
>> + so->tx.sn = 1;
>> + so->tx.state = ISOTP_WAIT_FIRST_FC;
>> +}
>> +
>> +static enum hrtimer_restart isotp_tx_timer_handler(struct hrtimer
>> *hrtimer)
>> +{
>> + struct isotp_sock *so = container_of(hrtimer, struct isotp_sock,
>> + txtimer);
>> + struct sock *sk = &so->sk;
>> + struct sk_buff *skb;
>> + struct net_device *dev;
>> + struct canfd_frame *cf;
>> + enum hrtimer_restart restart = HRTIMER_NORESTART;
>> + int can_send_ret;
>> + int ae = (so->opt.flags & CAN_ISOTP_EXTEND_ADDR) ? 1 : 0;
>> +
>> + switch (so->tx.state) {
>> + case ISOTP_WAIT_FC:
>> + case ISOTP_WAIT_FIRST_FC:
>> +
>> + /* we did not get any flow control frame in time */
>> +
>> + /* report 'communication error on send' */
>> + sk->sk_err = ECOMM;
>> + if (!sock_flag(sk, SOCK_DEAD))
>> + sk->sk_error_report(sk);
>> +
>> + /* reset tx state */
>> + so->tx.state = ISOTP_IDLE;
>> + wake_up_interruptible(&so->wait);
>> + break;
>> +
>> + case ISOTP_SENDING:
>> +
>> + /* push out the next segmented pdu */
>> + dev = dev_get_by_index(sock_net(sk), so->ifindex);
>> + if (!dev)
>> + break;
>> +
>> +isotp_tx_burst:
>> + skb = alloc_skb(so->ll.mtu + sizeof(struct can_skb_priv),
>> + gfp_any());
>> + if (!skb) {
>> + dev_put(dev);
>> + break;
>> + }
>> +
>> + can_skb_reserve(skb);
>> + can_skb_prv(skb)->ifindex = dev->ifindex;
>> + can_skb_prv(skb)->skbcnt = 0;
>> +
>> + cf = (struct canfd_frame *)skb->data;
>> + skb_put(skb, so->ll.mtu);
>> +
>> + /* create consecutive frame */
>> + isotp_fill_dataframe(cf, so, ae, 0);
>> +
>> + /* place consecutive frame N_PCI in appropriate index */
>> + cf->data[ae] = N_PCI_CF | so->tx.sn++;
>> + so->tx.sn %= 16;
>> + so->tx.bs++;
>> +
>> + if (so->ll.mtu == CANFD_MTU)
>> + cf->flags = so->ll.tx_flags;
>> +
>> + skb->dev = dev;
>> + can_skb_set_owner(skb, sk);
>> +
>> + can_send_ret = can_send(skb, 1);
>> + if (can_send_ret)
>> + printk_once(KERN_NOTICE "can-isotp: %s: can_send_ret %d\n",
>> + __func__, can_send_ret);
>> +
>> + if (so->tx.idx >= so->tx.len) {
>> + /* we are done */
>> + so->tx.state = ISOTP_IDLE;
>> + dev_put(dev);
>> + wake_up_interruptible(&so->wait);
>> + break;
>> + }
>> +
>> + if (so->txfc.bs && so->tx.bs >= so->txfc.bs) {
>> + /* stop and wait for FC */
>> + so->tx.state = ISOTP_WAIT_FC;
>> + dev_put(dev);
>> + hrtimer_set_expires(&so->txtimer,
>> + ktime_add(ktime_get(),
>> + ktime_set(1, 0)));
>> + restart = HRTIMER_RESTART;
>> + break;
>> + }
>> +
>> + /* no gap between data frames needed => use burst mode */
>> + if (!so->tx_gap)
>> + goto isotp_tx_burst;
>> +
>> + /* start timer to send next data frame with correct delay */
>> + dev_put(dev);
>> + hrtimer_set_expires(&so->txtimer,
>> + ktime_add(ktime_get(), so->tx_gap));
>> + restart = HRTIMER_RESTART;
>> + break;
>> +
>> + default:
>> + WARN_ON_ONCE(1);
>> + }
>> +
>> + return restart;
>> +}
>> +
>> +static int isotp_sendmsg(struct socket *sock, struct msghdr *msg,
>> size_t size)
>> +{
>> + struct sock *sk = sock->sk;
>> + struct isotp_sock *so = isotp_sk(sk);
>> + struct sk_buff *skb;
>> + struct net_device *dev;
>> + struct canfd_frame *cf;
>> + int ae = (so->opt.flags & CAN_ISOTP_EXTEND_ADDR) ? 1 : 0;
>> + int wait_tx_done = (so->opt.flags & CAN_ISOTP_WAIT_TX_DONE) ? 1 : 0;
>> + int off;
>> + int err;
>> +
>> + if (!so->bound)
>> + return -EADDRNOTAVAIL;
>> +
>> + /* we do not support multiple buffers - for now */
>> + if (so->tx.state != ISOTP_IDLE || wq_has_sleeper(&so->wait)) {
>> + if (msg->msg_flags & MSG_DONTWAIT)
>> + return -EAGAIN;
>> +
>> + /* wait for complete transmission of current pdu */
>> + wait_event_interruptible(so->wait, so->tx.state == ISOTP_IDLE);
>> + }
>> +
>> + if (!size || size > MAX_MSG_LENGTH)
>> + return -EINVAL;
>> +
>> + err = memcpy_from_msg(so->tx.buf, msg, size);
>> + if (err < 0)
>> + return err;
>> +
>> + dev = dev_get_by_index(sock_net(sk), so->ifindex);
>> + if (!dev)
>> + return -ENXIO;
>> +
>> + skb = sock_alloc_send_skb(sk, so->ll.mtu + sizeof(struct
>> can_skb_priv),
>> + msg->msg_flags & MSG_DONTWAIT, &err);
>> + if (!skb) {
>> + dev_put(dev);
>> + return err;
>> + }
>> +
>> + can_skb_reserve(skb);
>> + can_skb_prv(skb)->ifindex = dev->ifindex;
>> + can_skb_prv(skb)->skbcnt = 0;
>> +
>> + so->tx.state = ISOTP_SENDING;
>> + so->tx.len = size;
>> + so->tx.idx = 0;
>> +
>> + cf = (struct canfd_frame *)skb->data;
>> + skb_put(skb, so->ll.mtu);
>> +
>> + /* take care of a potential SF_DL ESC offset for TX_DL > 8 */
>> + off = (so->tx.ll_dl > CAN_MAX_DLEN) ? 1 : 0;
>> +
>> + /* check for single frame transmission depending on TX_DL */
>> + if (size <= so->tx.ll_dl - SF_PCI_SZ4 - ae - off) {
>> + /* The message size generally fits into a SingleFrame - good.
>> + *
>> + * SF_DL ESC offset optimization:
>> + *
>> + * When TX_DL is greater 8 but the message would still fit
>> + * into a 8 byte CAN frame, we can omit the offset.
>> + * This prevents a protocol caused length extension from
>> + * CAN_DL = 8 to CAN_DL = 12 due to the SF_SL ESC handling.
>> + */
>> + if (size <= CAN_MAX_DLEN - SF_PCI_SZ4 - ae)
>> + off = 0;
>> +
>> + isotp_fill_dataframe(cf, so, ae, off);
>> +
>> + /* place single frame N_PCI w/o length in appropriate index */
>> + cf->data[ae] = N_PCI_SF;
>> +
>> + /* place SF_DL size value depending on the SF_DL ESC offset */
>> + if (off)
>> + cf->data[SF_PCI_SZ4 + ae] = size;
>> + else
>> + cf->data[ae] |= size;
>> +
>> + so->tx.state = ISOTP_IDLE;
>> + wake_up_interruptible(&so->wait);
>> +
>> + /* don't enable wait queue for a single frame transmission */
>> + wait_tx_done = 0;
>> + } else {
>> + /* send first frame and wait for FC */
>> +
>> + isotp_create_fframe(cf, so, ae);
>> +
>> + /* start timeout for FC */
>> + hrtimer_start(&so->txtimer, ktime_set(1, 0),
>> HRTIMER_MODE_REL_SOFT);
>> + }
>> +
>> + /* send the first or only CAN frame */
>> + if (so->ll.mtu == CANFD_MTU)
>> + cf->flags = so->ll.tx_flags;
>> +
>> + skb->dev = dev;
>> + skb->sk = sk;
>> + err = can_send(skb, 1);
>> + dev_put(dev);
>> + if (err) {
>> + printk_once(KERN_NOTICE "can-isotp: %s: can_send_ret %d\n",
>> + __func__, err);
>> + return err;
>> + }
>> +
>> + if (wait_tx_done) {
>> + /* wait for complete transmission of current pdu */
>> + wait_event_interruptible(so->wait, so->tx.state == ISOTP_IDLE);
>> + }
>> +
>> + return size;
>> +}
>> +
>> +static int isotp_recvmsg(struct socket *sock, struct msghdr *msg,
>> size_t size,
>> + int flags)
>> +{
>> + struct sock *sk = sock->sk;
>> + struct sk_buff *skb;
>> + int err = 0;
>> + int noblock;
>> +
>> + noblock = flags & MSG_DONTWAIT;
>> + flags &= ~MSG_DONTWAIT;
>> +
>> + skb = skb_recv_datagram(sk, flags, noblock, &err);
>> + if (!skb)
>> + return err;
>> +
>> + if (size < skb->len)
>> + msg->msg_flags |= MSG_TRUNC;
>> + else
>> + size = skb->len;
>> +
>> + err = memcpy_to_msg(msg, skb->data, size);
>> + if (err < 0) {
>> + skb_free_datagram(sk, skb);
>> + return err;
>> + }
>> +
>> + sock_recv_timestamp(msg, sk, skb);
>> +
>> + if (msg->msg_name) {
>> + msg->msg_namelen = sizeof(struct sockaddr_can);
>> + memcpy(msg->msg_name, skb->cb, msg->msg_namelen);
>> + }
>> +
>> + skb_free_datagram(sk, skb);
>> +
>> + return size;
>> +}
>> +
>> +static int isotp_release(struct socket *sock)
>> +{
>> + struct sock *sk = sock->sk;
>> + struct isotp_sock *so;
>> + struct net *net;
>> +
>> + if (!sk)
>> + return 0;
>> +
>> + so = isotp_sk(sk);
>> + net = sock_net(sk);
>> +
>> + /* wait for complete transmission of current pdu */
>> + wait_event_interruptible(so->wait, so->tx.state == ISOTP_IDLE);
>> +
>> + unregister_netdevice_notifier(&so->notifier);
>> +
>> + lock_sock(sk);
>> +
>> + hrtimer_cancel(&so->txtimer);
>> + hrtimer_cancel(&so->rxtimer);
>> +
>> + /* remove current filters & unregister */
>> + if (so->bound) {
>> + if (so->ifindex) {
>> + struct net_device *dev;
>> +
>> + dev = dev_get_by_index(net, so->ifindex);
>> + if (dev) {
>> + can_rx_unregister(net, dev, so->rxid,
>> + SINGLE_MASK(so->rxid),
>> + isotp_rcv, sk);
>> + dev_put(dev);
>> + }
>> + }
>> + }
>> +
>> + so->ifindex = 0;
>> + so->bound = 0;
>> +
>> + sock_orphan(sk);
>> + sock->sk = NULL;
>> +
>> + release_sock(sk);
>> + sock_put(sk);
>> +
>> + return 0;
>> +}
>> +
>> +static int isotp_bind(struct socket *sock, struct sockaddr *uaddr,
>> int len)
>> +{
>> + struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
>> + struct sock *sk = sock->sk;
>> + struct isotp_sock *so = isotp_sk(sk);
>> + struct net *net = sock_net(sk);
>> + int ifindex;
>> + struct net_device *dev;
>> + int err = 0;
>> + int notify_enetdown = 0;
>> +
>> + if (len < CAN_REQUIRED_SIZE(struct sockaddr_can, can_addr.tp))
>> + return -EINVAL;
>> +
>> + if (addr->can_addr.tp.rx_id == addr->can_addr.tp.tx_id)
>> + return -EADDRNOTAVAIL;
>> +
>> + if ((addr->can_addr.tp.rx_id | addr->can_addr.tp.tx_id) &
>> + (CAN_ERR_FLAG | CAN_RTR_FLAG))
>> + return -EADDRNOTAVAIL;
>> +
>> + if (!addr->can_ifindex)
>> + return -ENODEV;
>> +
>> + lock_sock(sk);
>> +
>> + if (so->bound && addr->can_ifindex == so->ifindex &&
>> + addr->can_addr.tp.rx_id == so->rxid &&
>> + addr->can_addr.tp.tx_id == so->txid)
>> + goto out;
>> +
>> + dev = dev_get_by_index(net, addr->can_ifindex);
>> + if (!dev) {
>> + err = -ENODEV;
>> + goto out;
>> + }
>> + if (dev->type != ARPHRD_CAN) {
>> + dev_put(dev);
>> + err = -ENODEV;
>> + goto out;
>> + }
>> + if (dev->mtu < so->ll.mtu) {
>> + dev_put(dev);
>> + err = -EINVAL;
>> + goto out;
>> + }
>> + if (!(dev->flags & IFF_UP))
>> + notify_enetdown = 1;
>> +
>> + ifindex = dev->ifindex;
>> +
>> + can_rx_register(net, dev, addr->can_addr.tp.rx_id,
>> + SINGLE_MASK(addr->can_addr.tp.rx_id), isotp_rcv, sk,
>> + "isotp", sk);
>> +
>> + dev_put(dev);
>> +
>> + if (so->bound) {
>> + /* unregister old filter */
>> + if (so->ifindex) {
>> + dev = dev_get_by_index(net, so->ifindex);
>> + if (dev) {
>> + can_rx_unregister(net, dev, so->rxid,
>> + SINGLE_MASK(so->rxid),
>> + isotp_rcv, sk);
>> + dev_put(dev);
>> + }
>> + }
>> + }
>> +
>> + /* switch to new settings */
>> + so->ifindex = ifindex;
>> + so->rxid = addr->can_addr.tp.rx_id;
>> + so->txid = addr->can_addr.tp.tx_id;
>> + so->bound = 1;
>> +
>> +out:
>> + release_sock(sk);
>> +
>> + if (notify_enetdown) {
>> + sk->sk_err = ENETDOWN;
>> + if (!sock_flag(sk, SOCK_DEAD))
>> + sk->sk_error_report(sk);
>> + }
>> +
>> + return err;
>> +}
>> +
>> +static int isotp_getname(struct socket *sock, struct sockaddr *uaddr,
>> int peer)
>> +{
>> + struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
>> + struct sock *sk = sock->sk;
>> + struct isotp_sock *so = isotp_sk(sk);
>> +
>> + if (peer)
>> + return -EOPNOTSUPP;
>> +
>> + addr->can_family = AF_CAN;
>> + addr->can_ifindex = so->ifindex;
>> + addr->can_addr.tp.rx_id = so->rxid;
>> + addr->can_addr.tp.tx_id = so->txid;
>> +
>> + return sizeof(*addr);
>> +}
>> +
>> +static int isotp_setsockopt(struct socket *sock, int level, int optname,
>> + sockptr_t optval, unsigned int optlen)
>> +{
>> + struct sock *sk = sock->sk;
>> + struct isotp_sock *so = isotp_sk(sk);
>> + int ret = 0;
>> +
>> + if (level != SOL_CAN_ISOTP)
>> + return -EINVAL;
>> + if (optlen < 0)
>> + return -EINVAL;
>> +
>> + switch (optname) {
>> + case CAN_ISOTP_OPTS:
>> + if (optlen != sizeof(struct can_isotp_options))
>> + return -EINVAL;
>> +
>> + if (copy_from_sockptr(&so->opt, optval, optlen))
>> + return -EFAULT;
>> +
>> + /* no separate rx_ext_address is given => use ext_address */
>> + if (!(so->opt.flags & CAN_ISOTP_RX_EXT_ADDR))
>> + so->opt.rx_ext_address = so->opt.ext_address;
>> + break;
>> +
>> + case CAN_ISOTP_RECV_FC:
>> + if (optlen != sizeof(struct can_isotp_fc_options))
>> + return -EINVAL;
>> +
>> + if (copy_from_sockptr(&so->rxfc, optval, optlen))
>> + return -EFAULT;
>> + break;
>> +
>> + case CAN_ISOTP_TX_STMIN:
>> + if (optlen != sizeof(__u32))
>> + return -EINVAL;
>> +
>> + if (copy_from_sockptr(&so->force_tx_stmin, optval, optlen))
>> + return -EFAULT;
>> + break;
>> +
>> + case CAN_ISOTP_RX_STMIN:
>> + if (optlen != sizeof(__u32))
>> + return -EINVAL;
>> +
>> + if (copy_from_sockptr(&so->force_rx_stmin, optval, optlen))
>> + return -EFAULT;
>> + break;
>> +
>> + case CAN_ISOTP_LL_OPTS:
>> + if (optlen == sizeof(struct can_isotp_ll_options)) {
>> + struct can_isotp_ll_options ll;
>> +
>> + if (copy_from_sockptr(&ll, optval, optlen))
>> + return -EFAULT;
>> +
>> + /* check for correct ISO 11898-1 DLC data lentgh */
>> + if (ll.tx_dl != padlen(ll.tx_dl))
>> + return -EINVAL;
>> +
>> + if (ll.mtu != CAN_MTU && ll.mtu != CANFD_MTU)
>> + return -EINVAL;
>> +
>> + if (ll.mtu == CAN_MTU && ll.tx_dl > CAN_MAX_DLEN)
>> + return -EINVAL;
>> +
>> + memcpy(&so->ll, &ll, sizeof(ll));
>> +
>> + /* set ll_dl for tx path to similar place as for rx */
>> + so->tx.ll_dl = ll.tx_dl;
>> + } else {
>> + return -EINVAL;
>> + }
>> + break;
>> +
>> + default:
>> + ret = -ENOPROTOOPT;
>> + }
>> +
>> + return ret;
>> +}
>> +
>> +static int isotp_getsockopt(struct socket *sock, int level, int optname,
>> + char __user *optval, int __user *optlen)
>> +{
>> + struct sock *sk = sock->sk;
>> + struct isotp_sock *so = isotp_sk(sk);
>> + int len;
>> + void *val;
>> +
>> + if (level != SOL_CAN_ISOTP)
>> + return -EINVAL;
>> + if (get_user(len, optlen))
>> + return -EFAULT;
>> + if (len < 0)
>> + return -EINVAL;
>> +
>> + switch (optname) {
>> + case CAN_ISOTP_OPTS:
>> + len = min_t(int, len, sizeof(struct can_isotp_options));
>> + val = &so->opt;
>> + break;
>> +
>> + case CAN_ISOTP_RECV_FC:
>> + len = min_t(int, len, sizeof(struct can_isotp_fc_options));
>> + val = &so->rxfc;
>> + break;
>> +
>> + case CAN_ISOTP_TX_STMIN:
>> + len = min_t(int, len, sizeof(__u32));
>> + val = &so->force_tx_stmin;
>> + break;
>> +
>> + case CAN_ISOTP_RX_STMIN:
>> + len = min_t(int, len, sizeof(__u32));
>> + val = &so->force_rx_stmin;
>> + break;
>> +
>> + case CAN_ISOTP_LL_OPTS:
>> + len = min_t(int, len, sizeof(struct can_isotp_ll_options));
>> + val = &so->ll;
>> + break;
>> +
>> + default:
>> + return -ENOPROTOOPT;
>> + }
>> +
>> + if (put_user(len, optlen))
>> + return -EFAULT;
>> + if (copy_to_user(optval, val, len))
>> + return -EFAULT;
>> + return 0;
>> +}
>> +
>> +static int isotp_notifier(struct notifier_block *nb, unsigned long msg,
>> + void *ptr)
>> +{
>> + struct net_device *dev = netdev_notifier_info_to_dev(ptr);
>> + struct isotp_sock *so = container_of(nb, struct isotp_sock,
>> notifier);
>> + struct sock *sk = &so->sk;
>> +
>> + if (!net_eq(dev_net(dev), sock_net(sk)))
>> + return NOTIFY_DONE;
>> +
>> + if (dev->type != ARPHRD_CAN)
>> + return NOTIFY_DONE;
>> +
>> + if (so->ifindex != dev->ifindex)
>> + return NOTIFY_DONE;
>> +
>> + switch (msg) {
>> + case NETDEV_UNREGISTER:
>> + lock_sock(sk);
>> + /* remove current filters & unregister */
>> + if (so->bound)
>> + can_rx_unregister(dev_net(dev), dev, so->rxid,
>> + SINGLE_MASK(so->rxid),
>> + isotp_rcv, sk);
>> +
>> + so->ifindex = 0;
>> + so->bound = 0;
>> + release_sock(sk);
>> +
>> + sk->sk_err = ENODEV;
>> + if (!sock_flag(sk, SOCK_DEAD))
>> + sk->sk_error_report(sk);
>> + break;
>> +
>> + case NETDEV_DOWN:
>> + sk->sk_err = ENETDOWN;
>> + if (!sock_flag(sk, SOCK_DEAD))
>> + sk->sk_error_report(sk);
>> + break;
>> + }
>> +
>> + return NOTIFY_DONE;
>> +}
>> +
>> +static int isotp_init(struct sock *sk)
>> +{
>> + struct isotp_sock *so = isotp_sk(sk);
>> +
>> + so->ifindex = 0;
>> + so->bound = 0;
>> +
>> + so->opt.flags = CAN_ISOTP_DEFAULT_FLAGS;
>> + so->opt.ext_address = CAN_ISOTP_DEFAULT_EXT_ADDRESS;
>> + so->opt.rx_ext_address = CAN_ISOTP_DEFAULT_EXT_ADDRESS;
>> + so->opt.rxpad_content = CAN_ISOTP_DEFAULT_PAD_CONTENT;
>> + so->opt.txpad_content = CAN_ISOTP_DEFAULT_PAD_CONTENT;
>> + so->opt.frame_txtime = CAN_ISOTP_DEFAULT_FRAME_TXTIME;
>> + so->rxfc.bs = CAN_ISOTP_DEFAULT_RECV_BS;
>> + so->rxfc.stmin = CAN_ISOTP_DEFAULT_RECV_STMIN;
>> + so->rxfc.wftmax = CAN_ISOTP_DEFAULT_RECV_WFTMAX;
>> + so->ll.mtu = CAN_ISOTP_DEFAULT_LL_MTU;
>> + so->ll.tx_dl = CAN_ISOTP_DEFAULT_LL_TX_DL;
>> + so->ll.tx_flags = CAN_ISOTP_DEFAULT_LL_TX_FLAGS;
>> +
>> + /* set ll_dl for tx path to similar place as for rx */
>> + so->tx.ll_dl = so->ll.tx_dl;
>> +
>> + so->rx.state = ISOTP_IDLE;
>> + so->tx.state = ISOTP_IDLE;
>> +
>> + hrtimer_init(&so->rxtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL_SOFT);
>> + so->rxtimer.function = isotp_rx_timer_handler;
>> + hrtimer_init(&so->txtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL_SOFT);
>> + so->txtimer.function = isotp_tx_timer_handler;
>> +
>> + init_waitqueue_head(&so->wait);
>> +
>> + so->notifier.notifier_call = isotp_notifier;
>> + register_netdevice_notifier(&so->notifier);
>> +
>> + return 0;
>> +}
>> +
>> +static int isotp_sock_no_ioctlcmd(struct socket *sock, unsigned int cmd,
>> + unsigned long arg)
>> +{
>> + /* no ioctls for socket layer -> hand it down to NIC layer */
>> + return -ENOIOCTLCMD;
>> +}
>> +
>> +static const struct proto_ops isotp_ops = {
>> + .family = PF_CAN,
>> + .release = isotp_release,
>> + .bind = isotp_bind,
>> + .connect = sock_no_connect,
>> + .socketpair = sock_no_socketpair,
>> + .accept = sock_no_accept,
>> + .getname = isotp_getname,
>> + .poll = datagram_poll,
>> + .ioctl = isotp_sock_no_ioctlcmd,
>> + .gettstamp = sock_gettstamp,
>> + .listen = sock_no_listen,
>> + .shutdown = sock_no_shutdown,
>> + .setsockopt = isotp_setsockopt,
>> + .getsockopt = isotp_getsockopt,
>> + .sendmsg = isotp_sendmsg,
>> + .recvmsg = isotp_recvmsg,
>> + .mmap = sock_no_mmap,
>> + .sendpage = sock_no_sendpage,
>> +};
>> +
>> +static struct proto isotp_proto __read_mostly = {
>> + .name = "CAN_ISOTP",
>> + .owner = THIS_MODULE,
>> + .obj_size = sizeof(struct isotp_sock),
>> + .init = isotp_init,
>> +};
>> +
>> +static const struct can_proto isotp_can_proto = {
>> + .type = SOCK_DGRAM,
>> + .protocol = CAN_ISOTP,
>> + .ops = &isotp_ops,
>> + .prot = &isotp_proto,
>> +};
>> +
>> +static __init int isotp_module_init(void)
>> +{
>> + int err;
>> +
>> + pr_info("can: isotp protocol (rev " CAN_ISOTP_VERSION ")\n");
>> +
>> + err = can_proto_register(&isotp_can_proto);
>> + if (err < 0)
>> + pr_err("can: registration of isotp protocol failed\n");
>> +
>> + return err;
>> +}
>> +
>> +static __exit void isotp_module_exit(void)
>> +{
>> + can_proto_unregister(&isotp_can_proto);
>> +}
>> +
>> +module_init(isotp_module_init);
>> +module_exit(isotp_module_exit);
>>
Powered by blists - more mailing lists