tjh.dev
Linux kernel mirror (for testing)
git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git
kernel
os
linux
can: add ISO 15765-2:2016 transport protocol
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@hartkopp.net>
Link: https://lore.kernel.org/r/20200928200404.82229-1-socketcan@hartkopp.net
[mkl: Removed "WITH Linux-syscall-note" from isotp.c.
Fixed indention, a checkpatch warning and typos.
Replaced __u{8,32} by u{8,32}.
Removed always false (optlen < 0) check in isotp_setsockopt().]
Signed-off-by: Marc Kleine-Budde <mkl@pengutronix.de>
authored by
Oliver Hartkopp
and committed by
Marc Kleine-Budde
e057dd3f
1c47fa6b
+1609
+1
MAINTAINERS
+1
MAINTAINERS
+166
include/uapi/linux/can/isotp.h
+166
include/uapi/linux/can/isotp.h
···
1
+
/* SPDX-License-Identifier: ((GPL-2.0-only WITH Linux-syscall-note) OR BSD-3-Clause) */
2
+
/*
3
+
* linux/can/isotp.h
4
+
*
5
+
* Definitions for isotp CAN sockets (ISO 15765-2:2016)
6
+
*
7
+
* Copyright (c) 2020 Volkswagen Group Electronic Research
8
+
* All rights reserved.
9
+
*
10
+
* Redistribution and use in source and binary forms, with or without
11
+
* modification, are permitted provided that the following conditions
12
+
* are met:
13
+
* 1. Redistributions of source code must retain the above copyright
14
+
* notice, this list of conditions and the following disclaimer.
15
+
* 2. Redistributions in binary form must reproduce the above copyright
16
+
* notice, this list of conditions and the following disclaimer in the
17
+
* documentation and/or other materials provided with the distribution.
18
+
* 3. Neither the name of Volkswagen nor the names of its contributors
19
+
* may be used to endorse or promote products derived from this software
20
+
* without specific prior written permission.
21
+
*
22
+
* Alternatively, provided that this notice is retained in full, this
23
+
* software may be distributed under the terms of the GNU General
24
+
* Public License ("GPL") version 2, in which case the provisions of the
25
+
* GPL apply INSTEAD OF those given above.
26
+
*
27
+
* The provided data structures and external interfaces from this code
28
+
* are not restricted to be used by modules with a GPL compatible license.
29
+
*
30
+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
31
+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
32
+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
33
+
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
34
+
* OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
35
+
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
36
+
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
37
+
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
38
+
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
39
+
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
40
+
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
41
+
* DAMAGE.
42
+
*/
43
+
44
+
#ifndef _UAPI_CAN_ISOTP_H
45
+
#define _UAPI_CAN_ISOTP_H
46
+
47
+
#include <linux/types.h>
48
+
#include <linux/can.h>
49
+
50
+
#define SOL_CAN_ISOTP (SOL_CAN_BASE + CAN_ISOTP)
51
+
52
+
/* for socket options affecting the socket (not the global system) */
53
+
54
+
#define CAN_ISOTP_OPTS 1 /* pass struct can_isotp_options */
55
+
56
+
#define CAN_ISOTP_RECV_FC 2 /* pass struct can_isotp_fc_options */
57
+
58
+
/* sockopts to force stmin timer values for protocol regression tests */
59
+
60
+
#define CAN_ISOTP_TX_STMIN 3 /* pass __u32 value in nano secs */
61
+
/* use this time instead of value */
62
+
/* provided in FC from the receiver */
63
+
64
+
#define CAN_ISOTP_RX_STMIN 4 /* pass __u32 value in nano secs */
65
+
/* ignore received CF frames which */
66
+
/* timestamps differ less than val */
67
+
68
+
#define CAN_ISOTP_LL_OPTS 5 /* pass struct can_isotp_ll_options */
69
+
70
+
struct can_isotp_options {
71
+
72
+
__u32 flags; /* set flags for isotp behaviour. */
73
+
/* __u32 value : flags see below */
74
+
75
+
__u32 frame_txtime; /* frame transmission time (N_As/N_Ar) */
76
+
/* __u32 value : time in nano secs */
77
+
78
+
__u8 ext_address; /* set address for extended addressing */
79
+
/* __u8 value : extended address */
80
+
81
+
__u8 txpad_content; /* set content of padding byte (tx) */
82
+
/* __u8 value : content on tx path */
83
+
84
+
__u8 rxpad_content; /* set content of padding byte (rx) */
85
+
/* __u8 value : content on rx path */
86
+
87
+
__u8 rx_ext_address; /* set address for extended addressing */
88
+
/* __u8 value : extended address (rx) */
89
+
};
90
+
91
+
struct can_isotp_fc_options {
92
+
93
+
__u8 bs; /* blocksize provided in FC frame */
94
+
/* __u8 value : blocksize. 0 = off */
95
+
96
+
__u8 stmin; /* separation time provided in FC frame */
97
+
/* __u8 value : */
98
+
/* 0x00 - 0x7F : 0 - 127 ms */
99
+
/* 0x80 - 0xF0 : reserved */
100
+
/* 0xF1 - 0xF9 : 100 us - 900 us */
101
+
/* 0xFA - 0xFF : reserved */
102
+
103
+
__u8 wftmax; /* max. number of wait frame transmiss. */
104
+
/* __u8 value : 0 = omit FC N_PDU WT */
105
+
};
106
+
107
+
struct can_isotp_ll_options {
108
+
109
+
__u8 mtu; /* generated & accepted CAN frame type */
110
+
/* __u8 value : */
111
+
/* CAN_MTU (16) -> standard CAN 2.0 */
112
+
/* CANFD_MTU (72) -> CAN FD frame */
113
+
114
+
__u8 tx_dl; /* tx link layer data length in bytes */
115
+
/* (configured maximum payload length) */
116
+
/* __u8 value : 8,12,16,20,24,32,48,64 */
117
+
/* => rx path supports all LL_DL values */
118
+
119
+
__u8 tx_flags; /* set into struct canfd_frame.flags */
120
+
/* at frame creation: e.g. CANFD_BRS */
121
+
/* Obsolete when the BRS flag is fixed */
122
+
/* by the CAN netdriver configuration */
123
+
};
124
+
125
+
/* flags for isotp behaviour */
126
+
127
+
#define CAN_ISOTP_LISTEN_MODE 0x001 /* listen only (do not send FC) */
128
+
#define CAN_ISOTP_EXTEND_ADDR 0x002 /* enable extended addressing */
129
+
#define CAN_ISOTP_TX_PADDING 0x004 /* enable CAN frame padding tx path */
130
+
#define CAN_ISOTP_RX_PADDING 0x008 /* enable CAN frame padding rx path */
131
+
#define CAN_ISOTP_CHK_PAD_LEN 0x010 /* check received CAN frame padding */
132
+
#define CAN_ISOTP_CHK_PAD_DATA 0x020 /* check received CAN frame padding */
133
+
#define CAN_ISOTP_HALF_DUPLEX 0x040 /* half duplex error state handling */
134
+
#define CAN_ISOTP_FORCE_TXSTMIN 0x080 /* ignore stmin from received FC */
135
+
#define CAN_ISOTP_FORCE_RXSTMIN 0x100 /* ignore CFs depending on rx stmin */
136
+
#define CAN_ISOTP_RX_EXT_ADDR 0x200 /* different rx extended addressing */
137
+
#define CAN_ISOTP_WAIT_TX_DONE 0x400 /* wait for tx completion */
138
+
139
+
140
+
/* default values */
141
+
142
+
#define CAN_ISOTP_DEFAULT_FLAGS 0
143
+
#define CAN_ISOTP_DEFAULT_EXT_ADDRESS 0x00
144
+
#define CAN_ISOTP_DEFAULT_PAD_CONTENT 0xCC /* prevent bit-stuffing */
145
+
#define CAN_ISOTP_DEFAULT_FRAME_TXTIME 0
146
+
#define CAN_ISOTP_DEFAULT_RECV_BS 0
147
+
#define CAN_ISOTP_DEFAULT_RECV_STMIN 0x00
148
+
#define CAN_ISOTP_DEFAULT_RECV_WFTMAX 0
149
+
150
+
#define CAN_ISOTP_DEFAULT_LL_MTU CAN_MTU
151
+
#define CAN_ISOTP_DEFAULT_LL_TX_DL CAN_MAX_DLEN
152
+
#define CAN_ISOTP_DEFAULT_LL_TX_FLAGS 0
153
+
154
+
/*
155
+
* Remark on CAN_ISOTP_DEFAULT_RECV_* values:
156
+
*
157
+
* We can strongly assume, that the Linux Kernel implementation of
158
+
* CAN_ISOTP is capable to run with BS=0, STmin=0 and WFTmax=0.
159
+
* But as we like to be able to behave as a commonly available ECU,
160
+
* these default settings can be changed via sockopts.
161
+
* For that reason the STmin value is intentionally _not_ checked for
162
+
* consistency and copied directly into the flow control (FC) frame.
163
+
*
164
+
*/
165
+
166
+
#endif /* !_UAPI_CAN_ISOTP_H */
+13
net/can/Kconfig
+13
net/can/Kconfig
···
55
55
56
56
source "net/can/j1939/Kconfig"
57
57
58
+
config CAN_ISOTP
59
+
tristate "ISO 15765-2:2016 CAN transport protocol"
60
+
default y
61
+
help
62
+
CAN Transport Protocols offer support for segmented Point-to-Point
63
+
communication between CAN nodes via two defined CAN Identifiers.
64
+
As CAN frames can only transport a small amount of data bytes
65
+
(max. 8 bytes for 'classic' CAN and max. 64 bytes for CAN FD) this
66
+
segmentation is needed to transport longer PDUs as needed e.g. for
67
+
vehicle diagnosis (UDS, ISO 14229) or IP-over-CAN traffic.
68
+
This protocol driver implements data transfers according to
69
+
ISO 15765-2:2016 for 'classic' CAN and CAN FD frame types.
70
+
58
71
source "drivers/net/can/Kconfig"
59
72
60
73
endif
+3
net/can/Makefile
+3
net/can/Makefile
+1426
net/can/isotp.c
+1426
net/can/isotp.c
···
1
+
// SPDX-License-Identifier: (GPL-2.0 OR BSD-3-Clause)
2
+
/* isotp.c - ISO 15765-2 CAN transport protocol for protocol family CAN
3
+
*
4
+
* This implementation does not provide ISO-TP specific return values to the
5
+
* userspace.
6
+
*
7
+
* - RX path timeout of data reception leads to -ETIMEDOUT
8
+
* - RX path SN mismatch leads to -EILSEQ
9
+
* - RX path data reception with wrong padding leads to -EBADMSG
10
+
* - TX path flowcontrol reception timeout leads to -ECOMM
11
+
* - TX path flowcontrol reception overflow leads to -EMSGSIZE
12
+
* - TX path flowcontrol reception with wrong layout/padding leads to -EBADMSG
13
+
* - when a transfer (tx) is on the run the next write() blocks until it's done
14
+
* - use CAN_ISOTP_WAIT_TX_DONE flag to block the caller until the PDU is sent
15
+
* - as we have static buffers the check whether the PDU fits into the buffer
16
+
* is done at FF reception time (no support for sending 'wait frames')
17
+
* - take care of the tx-queue-len as traffic shaping is still on the TODO list
18
+
*
19
+
* Copyright (c) 2020 Volkswagen Group Electronic Research
20
+
* All rights reserved.
21
+
*
22
+
* Redistribution and use in source and binary forms, with or without
23
+
* modification, are permitted provided that the following conditions
24
+
* are met:
25
+
* 1. Redistributions of source code must retain the above copyright
26
+
* notice, this list of conditions and the following disclaimer.
27
+
* 2. Redistributions in binary form must reproduce the above copyright
28
+
* notice, this list of conditions and the following disclaimer in the
29
+
* documentation and/or other materials provided with the distribution.
30
+
* 3. Neither the name of Volkswagen nor the names of its contributors
31
+
* may be used to endorse or promote products derived from this software
32
+
* without specific prior written permission.
33
+
*
34
+
* Alternatively, provided that this notice is retained in full, this
35
+
* software may be distributed under the terms of the GNU General
36
+
* Public License ("GPL") version 2, in which case the provisions of the
37
+
* GPL apply INSTEAD OF those given above.
38
+
*
39
+
* The provided data structures and external interfaces from this code
40
+
* are not restricted to be used by modules with a GPL compatible license.
41
+
*
42
+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
43
+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
44
+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
45
+
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
46
+
* OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
47
+
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
48
+
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
49
+
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
50
+
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
51
+
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
52
+
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
53
+
* DAMAGE.
54
+
*/
55
+
56
+
#include <linux/module.h>
57
+
#include <linux/init.h>
58
+
#include <linux/interrupt.h>
59
+
#include <linux/hrtimer.h>
60
+
#include <linux/wait.h>
61
+
#include <linux/uio.h>
62
+
#include <linux/net.h>
63
+
#include <linux/netdevice.h>
64
+
#include <linux/socket.h>
65
+
#include <linux/if_arp.h>
66
+
#include <linux/skbuff.h>
67
+
#include <linux/can.h>
68
+
#include <linux/can/core.h>
69
+
#include <linux/can/skb.h>
70
+
#include <linux/can/isotp.h>
71
+
#include <linux/slab.h>
72
+
#include <net/sock.h>
73
+
#include <net/net_namespace.h>
74
+
75
+
#define CAN_ISOTP_VERSION "20200928"
76
+
77
+
MODULE_DESCRIPTION("PF_CAN isotp 15765-2:2016 protocol");
78
+
MODULE_LICENSE("Dual BSD/GPL");
79
+
MODULE_AUTHOR("Oliver Hartkopp <socketcan@hartkopp.net>");
80
+
MODULE_ALIAS("can-proto-6");
81
+
82
+
#define SINGLE_MASK(id) (((id) & CAN_EFF_FLAG) ? \
83
+
(CAN_EFF_MASK | CAN_EFF_FLAG | CAN_RTR_FLAG) : \
84
+
(CAN_SFF_MASK | CAN_EFF_FLAG | CAN_RTR_FLAG))
85
+
86
+
/* ISO 15765-2:2016 supports more than 4095 byte per ISO PDU as the FF_DL can
87
+
* take full 32 bit values (4 Gbyte). We would need some good concept to handle
88
+
* this between user space and kernel space. For now increase the static buffer
89
+
* to something about 8 kbyte to be able to test this new functionality.
90
+
*/
91
+
#define MAX_MSG_LENGTH 8200
92
+
93
+
/* N_PCI type values in bits 7-4 of N_PCI bytes */
94
+
#define N_PCI_SF 0x00 /* single frame */
95
+
#define N_PCI_FF 0x10 /* first frame */
96
+
#define N_PCI_CF 0x20 /* consecutive frame */
97
+
#define N_PCI_FC 0x30 /* flow control */
98
+
99
+
#define N_PCI_SZ 1 /* size of the PCI byte #1 */
100
+
#define SF_PCI_SZ4 1 /* size of SingleFrame PCI including 4 bit SF_DL */
101
+
#define SF_PCI_SZ8 2 /* size of SingleFrame PCI including 8 bit SF_DL */
102
+
#define FF_PCI_SZ12 2 /* size of FirstFrame PCI including 12 bit FF_DL */
103
+
#define FF_PCI_SZ32 6 /* size of FirstFrame PCI including 32 bit FF_DL */
104
+
#define FC_CONTENT_SZ 3 /* flow control content size in byte (FS/BS/STmin) */
105
+
106
+
#define ISOTP_CHECK_PADDING (CAN_ISOTP_CHK_PAD_LEN | CAN_ISOTP_CHK_PAD_DATA)
107
+
108
+
/* Flow Status given in FC frame */
109
+
#define ISOTP_FC_CTS 0 /* clear to send */
110
+
#define ISOTP_FC_WT 1 /* wait */
111
+
#define ISOTP_FC_OVFLW 2 /* overflow */
112
+
113
+
enum {
114
+
ISOTP_IDLE = 0,
115
+
ISOTP_WAIT_FIRST_FC,
116
+
ISOTP_WAIT_FC,
117
+
ISOTP_WAIT_DATA,
118
+
ISOTP_SENDING
119
+
};
120
+
121
+
struct tpcon {
122
+
int idx;
123
+
int len;
124
+
u8 state;
125
+
u8 bs;
126
+
u8 sn;
127
+
u8 ll_dl;
128
+
u8 buf[MAX_MSG_LENGTH + 1];
129
+
};
130
+
131
+
struct isotp_sock {
132
+
struct sock sk;
133
+
int bound;
134
+
int ifindex;
135
+
canid_t txid;
136
+
canid_t rxid;
137
+
ktime_t tx_gap;
138
+
ktime_t lastrxcf_tstamp;
139
+
struct hrtimer rxtimer, txtimer;
140
+
struct can_isotp_options opt;
141
+
struct can_isotp_fc_options rxfc, txfc;
142
+
struct can_isotp_ll_options ll;
143
+
u32 force_tx_stmin;
144
+
u32 force_rx_stmin;
145
+
struct tpcon rx, tx;
146
+
struct notifier_block notifier;
147
+
wait_queue_head_t wait;
148
+
};
149
+
150
+
static inline struct isotp_sock *isotp_sk(const struct sock *sk)
151
+
{
152
+
return (struct isotp_sock *)sk;
153
+
}
154
+
155
+
static enum hrtimer_restart isotp_rx_timer_handler(struct hrtimer *hrtimer)
156
+
{
157
+
struct isotp_sock *so = container_of(hrtimer, struct isotp_sock,
158
+
rxtimer);
159
+
struct sock *sk = &so->sk;
160
+
161
+
if (so->rx.state == ISOTP_WAIT_DATA) {
162
+
/* we did not get new data frames in time */
163
+
164
+
/* report 'connection timed out' */
165
+
sk->sk_err = ETIMEDOUT;
166
+
if (!sock_flag(sk, SOCK_DEAD))
167
+
sk->sk_error_report(sk);
168
+
169
+
/* reset rx state */
170
+
so->rx.state = ISOTP_IDLE;
171
+
}
172
+
173
+
return HRTIMER_NORESTART;
174
+
}
175
+
176
+
static int isotp_send_fc(struct sock *sk, int ae, u8 flowstatus)
177
+
{
178
+
struct net_device *dev;
179
+
struct sk_buff *nskb;
180
+
struct canfd_frame *ncf;
181
+
struct isotp_sock *so = isotp_sk(sk);
182
+
int can_send_ret;
183
+
184
+
nskb = alloc_skb(so->ll.mtu + sizeof(struct can_skb_priv), gfp_any());
185
+
if (!nskb)
186
+
return 1;
187
+
188
+
dev = dev_get_by_index(sock_net(sk), so->ifindex);
189
+
if (!dev) {
190
+
kfree_skb(nskb);
191
+
return 1;
192
+
}
193
+
194
+
can_skb_reserve(nskb);
195
+
can_skb_prv(nskb)->ifindex = dev->ifindex;
196
+
can_skb_prv(nskb)->skbcnt = 0;
197
+
198
+
nskb->dev = dev;
199
+
can_skb_set_owner(nskb, sk);
200
+
ncf = (struct canfd_frame *)nskb->data;
201
+
skb_put(nskb, so->ll.mtu);
202
+
203
+
/* create & send flow control reply */
204
+
ncf->can_id = so->txid;
205
+
206
+
if (so->opt.flags & CAN_ISOTP_TX_PADDING) {
207
+
memset(ncf->data, so->opt.txpad_content, CAN_MAX_DLEN);
208
+
ncf->len = CAN_MAX_DLEN;
209
+
} else {
210
+
ncf->len = ae + FC_CONTENT_SZ;
211
+
}
212
+
213
+
ncf->data[ae] = N_PCI_FC | flowstatus;
214
+
ncf->data[ae + 1] = so->rxfc.bs;
215
+
ncf->data[ae + 2] = so->rxfc.stmin;
216
+
217
+
if (ae)
218
+
ncf->data[0] = so->opt.ext_address;
219
+
220
+
if (so->ll.mtu == CANFD_MTU)
221
+
ncf->flags = so->ll.tx_flags;
222
+
223
+
can_send_ret = can_send(nskb, 1);
224
+
if (can_send_ret)
225
+
printk_once(KERN_NOTICE "can-isotp: %s: can_send_ret %d\n",
226
+
__func__, can_send_ret);
227
+
228
+
dev_put(dev);
229
+
230
+
/* reset blocksize counter */
231
+
so->rx.bs = 0;
232
+
233
+
/* reset last CF frame rx timestamp for rx stmin enforcement */
234
+
so->lastrxcf_tstamp = ktime_set(0, 0);
235
+
236
+
/* start rx timeout watchdog */
237
+
hrtimer_start(&so->rxtimer, ktime_set(1, 0), HRTIMER_MODE_REL_SOFT);
238
+
return 0;
239
+
}
240
+
241
+
static void isotp_rcv_skb(struct sk_buff *skb, struct sock *sk)
242
+
{
243
+
struct sockaddr_can *addr = (struct sockaddr_can *)skb->cb;
244
+
245
+
BUILD_BUG_ON(sizeof(skb->cb) < sizeof(struct sockaddr_can));
246
+
247
+
memset(addr, 0, sizeof(*addr));
248
+
addr->can_family = AF_CAN;
249
+
addr->can_ifindex = skb->dev->ifindex;
250
+
251
+
if (sock_queue_rcv_skb(sk, skb) < 0)
252
+
kfree_skb(skb);
253
+
}
254
+
255
+
static u8 padlen(u8 datalen)
256
+
{
257
+
const u8 plen[] = {8, 8, 8, 8, 8, 8, 8, 8, 8, /* 0 - 8 */
258
+
12, 12, 12, 12, /* 9 - 12 */
259
+
16, 16, 16, 16, /* 13 - 16 */
260
+
20, 20, 20, 20, /* 17 - 20 */
261
+
24, 24, 24, 24, /* 21 - 24 */
262
+
32, 32, 32, 32, 32, 32, 32, 32, /* 25 - 32 */
263
+
48, 48, 48, 48, 48, 48, 48, 48, /* 33 - 40 */
264
+
48, 48, 48, 48, 48, 48, 48, 48}; /* 41 - 48 */
265
+
266
+
if (datalen > 48)
267
+
return 64;
268
+
269
+
return plen[datalen];
270
+
}
271
+
272
+
/* check for length optimization and return 1/true when the check fails */
273
+
static int check_optimized(struct canfd_frame *cf, int start_index)
274
+
{
275
+
/* for CAN_DL <= 8 the start_index is equal to the CAN_DL as the
276
+
* padding would start at this point. E.g. if the padding would
277
+
* start at cf.data[7] cf->len has to be 7 to be optimal.
278
+
* Note: The data[] index starts with zero.
279
+
*/
280
+
if (cf->len <= CAN_MAX_DLEN)
281
+
return (cf->len != start_index);
282
+
283
+
/* This relation is also valid in the non-linear DLC range, where
284
+
* we need to take care of the minimal next possible CAN_DL.
285
+
* The correct check would be (padlen(cf->len) != padlen(start_index)).
286
+
* But as cf->len can only take discrete values from 12, .., 64 at this
287
+
* point the padlen(cf->len) is always equal to cf->len.
288
+
*/
289
+
return (cf->len != padlen(start_index));
290
+
}
291
+
292
+
/* check padding and return 1/true when the check fails */
293
+
static int check_pad(struct isotp_sock *so, struct canfd_frame *cf,
294
+
int start_index, u8 content)
295
+
{
296
+
int i;
297
+
298
+
/* no RX_PADDING value => check length of optimized frame length */
299
+
if (!(so->opt.flags & CAN_ISOTP_RX_PADDING)) {
300
+
if (so->opt.flags & CAN_ISOTP_CHK_PAD_LEN)
301
+
return check_optimized(cf, start_index);
302
+
303
+
/* no valid test against empty value => ignore frame */
304
+
return 1;
305
+
}
306
+
307
+
/* check datalength of correctly padded CAN frame */
308
+
if ((so->opt.flags & CAN_ISOTP_CHK_PAD_LEN) &&
309
+
cf->len != padlen(cf->len))
310
+
return 1;
311
+
312
+
/* check padding content */
313
+
if (so->opt.flags & CAN_ISOTP_CHK_PAD_DATA) {
314
+
for (i = start_index; i < cf->len; i++)
315
+
if (cf->data[i] != content)
316
+
return 1;
317
+
}
318
+
return 0;
319
+
}
320
+
321
+
static int isotp_rcv_fc(struct isotp_sock *so, struct canfd_frame *cf, int ae)
322
+
{
323
+
struct sock *sk = &so->sk;
324
+
325
+
if (so->tx.state != ISOTP_WAIT_FC &&
326
+
so->tx.state != ISOTP_WAIT_FIRST_FC)
327
+
return 0;
328
+
329
+
hrtimer_cancel(&so->txtimer);
330
+
331
+
if ((cf->len < ae + FC_CONTENT_SZ) ||
332
+
((so->opt.flags & ISOTP_CHECK_PADDING) &&
333
+
check_pad(so, cf, ae + FC_CONTENT_SZ, so->opt.rxpad_content))) {
334
+
/* malformed PDU - report 'not a data message' */
335
+
sk->sk_err = EBADMSG;
336
+
if (!sock_flag(sk, SOCK_DEAD))
337
+
sk->sk_error_report(sk);
338
+
339
+
so->tx.state = ISOTP_IDLE;
340
+
wake_up_interruptible(&so->wait);
341
+
return 1;
342
+
}
343
+
344
+
/* get communication parameters only from the first FC frame */
345
+
if (so->tx.state == ISOTP_WAIT_FIRST_FC) {
346
+
so->txfc.bs = cf->data[ae + 1];
347
+
so->txfc.stmin = cf->data[ae + 2];
348
+
349
+
/* fix wrong STmin values according spec */
350
+
if (so->txfc.stmin > 0x7F &&
351
+
(so->txfc.stmin < 0xF1 || so->txfc.stmin > 0xF9))
352
+
so->txfc.stmin = 0x7F;
353
+
354
+
so->tx_gap = ktime_set(0, 0);
355
+
/* add transmission time for CAN frame N_As */
356
+
so->tx_gap = ktime_add_ns(so->tx_gap, so->opt.frame_txtime);
357
+
/* add waiting time for consecutive frames N_Cs */
358
+
if (so->opt.flags & CAN_ISOTP_FORCE_TXSTMIN)
359
+
so->tx_gap = ktime_add_ns(so->tx_gap,
360
+
so->force_tx_stmin);
361
+
else if (so->txfc.stmin < 0x80)
362
+
so->tx_gap = ktime_add_ns(so->tx_gap,
363
+
so->txfc.stmin * 1000000);
364
+
else
365
+
so->tx_gap = ktime_add_ns(so->tx_gap,
366
+
(so->txfc.stmin - 0xF0)
367
+
* 100000);
368
+
so->tx.state = ISOTP_WAIT_FC;
369
+
}
370
+
371
+
switch (cf->data[ae] & 0x0F) {
372
+
case ISOTP_FC_CTS:
373
+
so->tx.bs = 0;
374
+
so->tx.state = ISOTP_SENDING;
375
+
/* start cyclic timer for sending CF frame */
376
+
hrtimer_start(&so->txtimer, so->tx_gap,
377
+
HRTIMER_MODE_REL_SOFT);
378
+
break;
379
+
380
+
case ISOTP_FC_WT:
381
+
/* start timer to wait for next FC frame */
382
+
hrtimer_start(&so->txtimer, ktime_set(1, 0),
383
+
HRTIMER_MODE_REL_SOFT);
384
+
break;
385
+
386
+
case ISOTP_FC_OVFLW:
387
+
/* overflow on receiver side - report 'message too long' */
388
+
sk->sk_err = EMSGSIZE;
389
+
if (!sock_flag(sk, SOCK_DEAD))
390
+
sk->sk_error_report(sk);
391
+
fallthrough;
392
+
393
+
default:
394
+
/* stop this tx job */
395
+
so->tx.state = ISOTP_IDLE;
396
+
wake_up_interruptible(&so->wait);
397
+
}
398
+
return 0;
399
+
}
400
+
401
+
static int isotp_rcv_sf(struct sock *sk, struct canfd_frame *cf, int pcilen,
402
+
struct sk_buff *skb, int len)
403
+
{
404
+
struct isotp_sock *so = isotp_sk(sk);
405
+
struct sk_buff *nskb;
406
+
407
+
hrtimer_cancel(&so->rxtimer);
408
+
so->rx.state = ISOTP_IDLE;
409
+
410
+
if (!len || len > cf->len - pcilen)
411
+
return 1;
412
+
413
+
if ((so->opt.flags & ISOTP_CHECK_PADDING) &&
414
+
check_pad(so, cf, pcilen + len, so->opt.rxpad_content)) {
415
+
/* malformed PDU - report 'not a data message' */
416
+
sk->sk_err = EBADMSG;
417
+
if (!sock_flag(sk, SOCK_DEAD))
418
+
sk->sk_error_report(sk);
419
+
return 1;
420
+
}
421
+
422
+
nskb = alloc_skb(len, gfp_any());
423
+
if (!nskb)
424
+
return 1;
425
+
426
+
memcpy(skb_put(nskb, len), &cf->data[pcilen], len);
427
+
428
+
nskb->tstamp = skb->tstamp;
429
+
nskb->dev = skb->dev;
430
+
isotp_rcv_skb(nskb, sk);
431
+
return 0;
432
+
}
433
+
434
+
static int isotp_rcv_ff(struct sock *sk, struct canfd_frame *cf, int ae)
435
+
{
436
+
struct isotp_sock *so = isotp_sk(sk);
437
+
int i;
438
+
int off;
439
+
int ff_pci_sz;
440
+
441
+
hrtimer_cancel(&so->rxtimer);
442
+
so->rx.state = ISOTP_IDLE;
443
+
444
+
/* get the used sender LL_DL from the (first) CAN frame data length */
445
+
so->rx.ll_dl = padlen(cf->len);
446
+
447
+
/* the first frame has to use the entire frame up to LL_DL length */
448
+
if (cf->len != so->rx.ll_dl)
449
+
return 1;
450
+
451
+
/* get the FF_DL */
452
+
so->rx.len = (cf->data[ae] & 0x0F) << 8;
453
+
so->rx.len += cf->data[ae + 1];
454
+
455
+
/* Check for FF_DL escape sequence supporting 32 bit PDU length */
456
+
if (so->rx.len) {
457
+
ff_pci_sz = FF_PCI_SZ12;
458
+
} else {
459
+
/* FF_DL = 0 => get real length from next 4 bytes */
460
+
so->rx.len = cf->data[ae + 2] << 24;
461
+
so->rx.len += cf->data[ae + 3] << 16;
462
+
so->rx.len += cf->data[ae + 4] << 8;
463
+
so->rx.len += cf->data[ae + 5];
464
+
ff_pci_sz = FF_PCI_SZ32;
465
+
}
466
+
467
+
/* take care of a potential SF_DL ESC offset for TX_DL > 8 */
468
+
off = (so->rx.ll_dl > CAN_MAX_DLEN) ? 1 : 0;
469
+
470
+
if (so->rx.len + ae + off + ff_pci_sz < so->rx.ll_dl)
471
+
return 1;
472
+
473
+
if (so->rx.len > MAX_MSG_LENGTH) {
474
+
/* send FC frame with overflow status */
475
+
isotp_send_fc(sk, ae, ISOTP_FC_OVFLW);
476
+
return 1;
477
+
}
478
+
479
+
/* copy the first received data bytes */
480
+
so->rx.idx = 0;
481
+
for (i = ae + ff_pci_sz; i < so->rx.ll_dl; i++)
482
+
so->rx.buf[so->rx.idx++] = cf->data[i];
483
+
484
+
/* initial setup for this pdu reception */
485
+
so->rx.sn = 1;
486
+
so->rx.state = ISOTP_WAIT_DATA;
487
+
488
+
/* no creation of flow control frames */
489
+
if (so->opt.flags & CAN_ISOTP_LISTEN_MODE)
490
+
return 0;
491
+
492
+
/* send our first FC frame */
493
+
isotp_send_fc(sk, ae, ISOTP_FC_CTS);
494
+
return 0;
495
+
}
496
+
497
+
static int isotp_rcv_cf(struct sock *sk, struct canfd_frame *cf, int ae,
498
+
struct sk_buff *skb)
499
+
{
500
+
struct isotp_sock *so = isotp_sk(sk);
501
+
struct sk_buff *nskb;
502
+
int i;
503
+
504
+
if (so->rx.state != ISOTP_WAIT_DATA)
505
+
return 0;
506
+
507
+
/* drop if timestamp gap is less than force_rx_stmin nano secs */
508
+
if (so->opt.flags & CAN_ISOTP_FORCE_RXSTMIN) {
509
+
if (ktime_to_ns(ktime_sub(skb->tstamp, so->lastrxcf_tstamp)) <
510
+
so->force_rx_stmin)
511
+
return 0;
512
+
513
+
so->lastrxcf_tstamp = skb->tstamp;
514
+
}
515
+
516
+
hrtimer_cancel(&so->rxtimer);
517
+
518
+
/* CFs are never longer than the FF */
519
+
if (cf->len > so->rx.ll_dl)
520
+
return 1;
521
+
522
+
/* CFs have usually the LL_DL length */
523
+
if (cf->len < so->rx.ll_dl) {
524
+
/* this is only allowed for the last CF */
525
+
if (so->rx.len - so->rx.idx > so->rx.ll_dl - ae - N_PCI_SZ)
526
+
return 1;
527
+
}
528
+
529
+
if ((cf->data[ae] & 0x0F) != so->rx.sn) {
530
+
/* wrong sn detected - report 'illegal byte sequence' */
531
+
sk->sk_err = EILSEQ;
532
+
if (!sock_flag(sk, SOCK_DEAD))
533
+
sk->sk_error_report(sk);
534
+
535
+
/* reset rx state */
536
+
so->rx.state = ISOTP_IDLE;
537
+
return 1;
538
+
}
539
+
so->rx.sn++;
540
+
so->rx.sn %= 16;
541
+
542
+
for (i = ae + N_PCI_SZ; i < cf->len; i++) {
543
+
so->rx.buf[so->rx.idx++] = cf->data[i];
544
+
if (so->rx.idx >= so->rx.len)
545
+
break;
546
+
}
547
+
548
+
if (so->rx.idx >= so->rx.len) {
549
+
/* we are done */
550
+
so->rx.state = ISOTP_IDLE;
551
+
552
+
if ((so->opt.flags & ISOTP_CHECK_PADDING) &&
553
+
check_pad(so, cf, i + 1, so->opt.rxpad_content)) {
554
+
/* malformed PDU - report 'not a data message' */
555
+
sk->sk_err = EBADMSG;
556
+
if (!sock_flag(sk, SOCK_DEAD))
557
+
sk->sk_error_report(sk);
558
+
return 1;
559
+
}
560
+
561
+
nskb = alloc_skb(so->rx.len, gfp_any());
562
+
if (!nskb)
563
+
return 1;
564
+
565
+
memcpy(skb_put(nskb, so->rx.len), so->rx.buf,
566
+
so->rx.len);
567
+
568
+
nskb->tstamp = skb->tstamp;
569
+
nskb->dev = skb->dev;
570
+
isotp_rcv_skb(nskb, sk);
571
+
return 0;
572
+
}
573
+
574
+
/* no creation of flow control frames */
575
+
if (so->opt.flags & CAN_ISOTP_LISTEN_MODE)
576
+
return 0;
577
+
578
+
/* perform blocksize handling, if enabled */
579
+
if (!so->rxfc.bs || ++so->rx.bs < so->rxfc.bs) {
580
+
/* start rx timeout watchdog */
581
+
hrtimer_start(&so->rxtimer, ktime_set(1, 0),
582
+
HRTIMER_MODE_REL_SOFT);
583
+
return 0;
584
+
}
585
+
586
+
/* we reached the specified blocksize so->rxfc.bs */
587
+
isotp_send_fc(sk, ae, ISOTP_FC_CTS);
588
+
return 0;
589
+
}
590
+
591
+
static void isotp_rcv(struct sk_buff *skb, void *data)
592
+
{
593
+
struct sock *sk = (struct sock *)data;
594
+
struct isotp_sock *so = isotp_sk(sk);
595
+
struct canfd_frame *cf;
596
+
int ae = (so->opt.flags & CAN_ISOTP_EXTEND_ADDR) ? 1 : 0;
597
+
u8 n_pci_type, sf_dl;
598
+
599
+
/* Strictly receive only frames with the configured MTU size
600
+
* => clear separation of CAN2.0 / CAN FD transport channels
601
+
*/
602
+
if (skb->len != so->ll.mtu)
603
+
return;
604
+
605
+
cf = (struct canfd_frame *)skb->data;
606
+
607
+
/* if enabled: check reception of my configured extended address */
608
+
if (ae && cf->data[0] != so->opt.rx_ext_address)
609
+
return;
610
+
611
+
n_pci_type = cf->data[ae] & 0xF0;
612
+
613
+
if (so->opt.flags & CAN_ISOTP_HALF_DUPLEX) {
614
+
/* check rx/tx path half duplex expectations */
615
+
if ((so->tx.state != ISOTP_IDLE && n_pci_type != N_PCI_FC) ||
616
+
(so->rx.state != ISOTP_IDLE && n_pci_type == N_PCI_FC))
617
+
return;
618
+
}
619
+
620
+
switch (n_pci_type) {
621
+
case N_PCI_FC:
622
+
/* tx path: flow control frame containing the FC parameters */
623
+
isotp_rcv_fc(so, cf, ae);
624
+
break;
625
+
626
+
case N_PCI_SF:
627
+
/* rx path: single frame
628
+
*
629
+
* As we do not have a rx.ll_dl configuration, we can only test
630
+
* if the CAN frames payload length matches the LL_DL == 8
631
+
* requirements - no matter if it's CAN 2.0 or CAN FD
632
+
*/
633
+
634
+
/* get the SF_DL from the N_PCI byte */
635
+
sf_dl = cf->data[ae] & 0x0F;
636
+
637
+
if (cf->len <= CAN_MAX_DLEN) {
638
+
isotp_rcv_sf(sk, cf, SF_PCI_SZ4 + ae, skb, sf_dl);
639
+
} else {
640
+
if (skb->len == CANFD_MTU) {
641
+
/* We have a CAN FD frame and CAN_DL is greater than 8:
642
+
* Only frames with the SF_DL == 0 ESC value are valid.
643
+
*
644
+
* If so take care of the increased SF PCI size
645
+
* (SF_PCI_SZ8) to point to the message content behind
646
+
* the extended SF PCI info and get the real SF_DL
647
+
* length value from the formerly first data byte.
648
+
*/
649
+
if (sf_dl == 0)
650
+
isotp_rcv_sf(sk, cf, SF_PCI_SZ8 + ae, skb,
651
+
cf->data[SF_PCI_SZ4 + ae]);
652
+
}
653
+
}
654
+
break;
655
+
656
+
case N_PCI_FF:
657
+
/* rx path: first frame */
658
+
isotp_rcv_ff(sk, cf, ae);
659
+
break;
660
+
661
+
case N_PCI_CF:
662
+
/* rx path: consecutive frame */
663
+
isotp_rcv_cf(sk, cf, ae, skb);
664
+
break;
665
+
}
666
+
}
667
+
668
+
static void isotp_fill_dataframe(struct canfd_frame *cf, struct isotp_sock *so,
669
+
int ae, int off)
670
+
{
671
+
int pcilen = N_PCI_SZ + ae + off;
672
+
int space = so->tx.ll_dl - pcilen;
673
+
int num = min_t(int, so->tx.len - so->tx.idx, space);
674
+
int i;
675
+
676
+
cf->can_id = so->txid;
677
+
cf->len = num + pcilen;
678
+
679
+
if (num < space) {
680
+
if (so->opt.flags & CAN_ISOTP_TX_PADDING) {
681
+
/* user requested padding */
682
+
cf->len = padlen(cf->len);
683
+
memset(cf->data, so->opt.txpad_content, cf->len);
684
+
} else if (cf->len > CAN_MAX_DLEN) {
685
+
/* mandatory padding for CAN FD frames */
686
+
cf->len = padlen(cf->len);
687
+
memset(cf->data, CAN_ISOTP_DEFAULT_PAD_CONTENT,
688
+
cf->len);
689
+
}
690
+
}
691
+
692
+
for (i = 0; i < num; i++)
693
+
cf->data[pcilen + i] = so->tx.buf[so->tx.idx++];
694
+
695
+
if (ae)
696
+
cf->data[0] = so->opt.ext_address;
697
+
}
698
+
699
+
static void isotp_create_fframe(struct canfd_frame *cf, struct isotp_sock *so,
700
+
int ae)
701
+
{
702
+
int i;
703
+
int ff_pci_sz;
704
+
705
+
cf->can_id = so->txid;
706
+
cf->len = so->tx.ll_dl;
707
+
if (ae)
708
+
cf->data[0] = so->opt.ext_address;
709
+
710
+
/* create N_PCI bytes with 12/32 bit FF_DL data length */
711
+
if (so->tx.len > 4095) {
712
+
/* use 32 bit FF_DL notation */
713
+
cf->data[ae] = N_PCI_FF;
714
+
cf->data[ae + 1] = 0;
715
+
cf->data[ae + 2] = (u8)(so->tx.len >> 24) & 0xFFU;
716
+
cf->data[ae + 3] = (u8)(so->tx.len >> 16) & 0xFFU;
717
+
cf->data[ae + 4] = (u8)(so->tx.len >> 8) & 0xFFU;
718
+
cf->data[ae + 5] = (u8)so->tx.len & 0xFFU;
719
+
ff_pci_sz = FF_PCI_SZ32;
720
+
} else {
721
+
/* use 12 bit FF_DL notation */
722
+
cf->data[ae] = (u8)(so->tx.len >> 8) | N_PCI_FF;
723
+
cf->data[ae + 1] = (u8)so->tx.len & 0xFFU;
724
+
ff_pci_sz = FF_PCI_SZ12;
725
+
}
726
+
727
+
/* add first data bytes depending on ae */
728
+
for (i = ae + ff_pci_sz; i < so->tx.ll_dl; i++)
729
+
cf->data[i] = so->tx.buf[so->tx.idx++];
730
+
731
+
so->tx.sn = 1;
732
+
so->tx.state = ISOTP_WAIT_FIRST_FC;
733
+
}
734
+
735
+
static enum hrtimer_restart isotp_tx_timer_handler(struct hrtimer *hrtimer)
736
+
{
737
+
struct isotp_sock *so = container_of(hrtimer, struct isotp_sock,
738
+
txtimer);
739
+
struct sock *sk = &so->sk;
740
+
struct sk_buff *skb;
741
+
struct net_device *dev;
742
+
struct canfd_frame *cf;
743
+
enum hrtimer_restart restart = HRTIMER_NORESTART;
744
+
int can_send_ret;
745
+
int ae = (so->opt.flags & CAN_ISOTP_EXTEND_ADDR) ? 1 : 0;
746
+
747
+
switch (so->tx.state) {
748
+
case ISOTP_WAIT_FC:
749
+
case ISOTP_WAIT_FIRST_FC:
750
+
751
+
/* we did not get any flow control frame in time */
752
+
753
+
/* report 'communication error on send' */
754
+
sk->sk_err = ECOMM;
755
+
if (!sock_flag(sk, SOCK_DEAD))
756
+
sk->sk_error_report(sk);
757
+
758
+
/* reset tx state */
759
+
so->tx.state = ISOTP_IDLE;
760
+
wake_up_interruptible(&so->wait);
761
+
break;
762
+
763
+
case ISOTP_SENDING:
764
+
765
+
/* push out the next segmented pdu */
766
+
dev = dev_get_by_index(sock_net(sk), so->ifindex);
767
+
if (!dev)
768
+
break;
769
+
770
+
isotp_tx_burst:
771
+
skb = alloc_skb(so->ll.mtu + sizeof(struct can_skb_priv),
772
+
gfp_any());
773
+
if (!skb) {
774
+
dev_put(dev);
775
+
break;
776
+
}
777
+
778
+
can_skb_reserve(skb);
779
+
can_skb_prv(skb)->ifindex = dev->ifindex;
780
+
can_skb_prv(skb)->skbcnt = 0;
781
+
782
+
cf = (struct canfd_frame *)skb->data;
783
+
skb_put(skb, so->ll.mtu);
784
+
785
+
/* create consecutive frame */
786
+
isotp_fill_dataframe(cf, so, ae, 0);
787
+
788
+
/* place consecutive frame N_PCI in appropriate index */
789
+
cf->data[ae] = N_PCI_CF | so->tx.sn++;
790
+
so->tx.sn %= 16;
791
+
so->tx.bs++;
792
+
793
+
if (so->ll.mtu == CANFD_MTU)
794
+
cf->flags = so->ll.tx_flags;
795
+
796
+
skb->dev = dev;
797
+
can_skb_set_owner(skb, sk);
798
+
799
+
can_send_ret = can_send(skb, 1);
800
+
if (can_send_ret)
801
+
printk_once(KERN_NOTICE "can-isotp: %s: can_send_ret %d\n",
802
+
__func__, can_send_ret);
803
+
804
+
if (so->tx.idx >= so->tx.len) {
805
+
/* we are done */
806
+
so->tx.state = ISOTP_IDLE;
807
+
dev_put(dev);
808
+
wake_up_interruptible(&so->wait);
809
+
break;
810
+
}
811
+
812
+
if (so->txfc.bs && so->tx.bs >= so->txfc.bs) {
813
+
/* stop and wait for FC */
814
+
so->tx.state = ISOTP_WAIT_FC;
815
+
dev_put(dev);
816
+
hrtimer_set_expires(&so->txtimer,
817
+
ktime_add(ktime_get(),
818
+
ktime_set(1, 0)));
819
+
restart = HRTIMER_RESTART;
820
+
break;
821
+
}
822
+
823
+
/* no gap between data frames needed => use burst mode */
824
+
if (!so->tx_gap)
825
+
goto isotp_tx_burst;
826
+
827
+
/* start timer to send next data frame with correct delay */
828
+
dev_put(dev);
829
+
hrtimer_set_expires(&so->txtimer,
830
+
ktime_add(ktime_get(), so->tx_gap));
831
+
restart = HRTIMER_RESTART;
832
+
break;
833
+
834
+
default:
835
+
WARN_ON_ONCE(1);
836
+
}
837
+
838
+
return restart;
839
+
}
840
+
841
+
static int isotp_sendmsg(struct socket *sock, struct msghdr *msg, size_t size)
842
+
{
843
+
struct sock *sk = sock->sk;
844
+
struct isotp_sock *so = isotp_sk(sk);
845
+
struct sk_buff *skb;
846
+
struct net_device *dev;
847
+
struct canfd_frame *cf;
848
+
int ae = (so->opt.flags & CAN_ISOTP_EXTEND_ADDR) ? 1 : 0;
849
+
int wait_tx_done = (so->opt.flags & CAN_ISOTP_WAIT_TX_DONE) ? 1 : 0;
850
+
int off;
851
+
int err;
852
+
853
+
if (!so->bound)
854
+
return -EADDRNOTAVAIL;
855
+
856
+
/* we do not support multiple buffers - for now */
857
+
if (so->tx.state != ISOTP_IDLE || wq_has_sleeper(&so->wait)) {
858
+
if (msg->msg_flags & MSG_DONTWAIT)
859
+
return -EAGAIN;
860
+
861
+
/* wait for complete transmission of current pdu */
862
+
wait_event_interruptible(so->wait, so->tx.state == ISOTP_IDLE);
863
+
}
864
+
865
+
if (!size || size > MAX_MSG_LENGTH)
866
+
return -EINVAL;
867
+
868
+
err = memcpy_from_msg(so->tx.buf, msg, size);
869
+
if (err < 0)
870
+
return err;
871
+
872
+
dev = dev_get_by_index(sock_net(sk), so->ifindex);
873
+
if (!dev)
874
+
return -ENXIO;
875
+
876
+
skb = sock_alloc_send_skb(sk, so->ll.mtu + sizeof(struct can_skb_priv),
877
+
msg->msg_flags & MSG_DONTWAIT, &err);
878
+
if (!skb) {
879
+
dev_put(dev);
880
+
return err;
881
+
}
882
+
883
+
can_skb_reserve(skb);
884
+
can_skb_prv(skb)->ifindex = dev->ifindex;
885
+
can_skb_prv(skb)->skbcnt = 0;
886
+
887
+
so->tx.state = ISOTP_SENDING;
888
+
so->tx.len = size;
889
+
so->tx.idx = 0;
890
+
891
+
cf = (struct canfd_frame *)skb->data;
892
+
skb_put(skb, so->ll.mtu);
893
+
894
+
/* take care of a potential SF_DL ESC offset for TX_DL > 8 */
895
+
off = (so->tx.ll_dl > CAN_MAX_DLEN) ? 1 : 0;
896
+
897
+
/* check for single frame transmission depending on TX_DL */
898
+
if (size <= so->tx.ll_dl - SF_PCI_SZ4 - ae - off) {
899
+
/* The message size generally fits into a SingleFrame - good.
900
+
*
901
+
* SF_DL ESC offset optimization:
902
+
*
903
+
* When TX_DL is greater 8 but the message would still fit
904
+
* into a 8 byte CAN frame, we can omit the offset.
905
+
* This prevents a protocol caused length extension from
906
+
* CAN_DL = 8 to CAN_DL = 12 due to the SF_SL ESC handling.
907
+
*/
908
+
if (size <= CAN_MAX_DLEN - SF_PCI_SZ4 - ae)
909
+
off = 0;
910
+
911
+
isotp_fill_dataframe(cf, so, ae, off);
912
+
913
+
/* place single frame N_PCI w/o length in appropriate index */
914
+
cf->data[ae] = N_PCI_SF;
915
+
916
+
/* place SF_DL size value depending on the SF_DL ESC offset */
917
+
if (off)
918
+
cf->data[SF_PCI_SZ4 + ae] = size;
919
+
else
920
+
cf->data[ae] |= size;
921
+
922
+
so->tx.state = ISOTP_IDLE;
923
+
wake_up_interruptible(&so->wait);
924
+
925
+
/* don't enable wait queue for a single frame transmission */
926
+
wait_tx_done = 0;
927
+
} else {
928
+
/* send first frame and wait for FC */
929
+
930
+
isotp_create_fframe(cf, so, ae);
931
+
932
+
/* start timeout for FC */
933
+
hrtimer_start(&so->txtimer, ktime_set(1, 0), HRTIMER_MODE_REL_SOFT);
934
+
}
935
+
936
+
/* send the first or only CAN frame */
937
+
if (so->ll.mtu == CANFD_MTU)
938
+
cf->flags = so->ll.tx_flags;
939
+
940
+
skb->dev = dev;
941
+
skb->sk = sk;
942
+
err = can_send(skb, 1);
943
+
dev_put(dev);
944
+
if (err) {
945
+
printk_once(KERN_NOTICE "can-isotp: %s: can_send_ret %d\n",
946
+
__func__, err);
947
+
return err;
948
+
}
949
+
950
+
if (wait_tx_done) {
951
+
/* wait for complete transmission of current pdu */
952
+
wait_event_interruptible(so->wait, so->tx.state == ISOTP_IDLE);
953
+
}
954
+
955
+
return size;
956
+
}
957
+
958
+
static int isotp_recvmsg(struct socket *sock, struct msghdr *msg, size_t size,
959
+
int flags)
960
+
{
961
+
struct sock *sk = sock->sk;
962
+
struct sk_buff *skb;
963
+
int err = 0;
964
+
int noblock;
965
+
966
+
noblock = flags & MSG_DONTWAIT;
967
+
flags &= ~MSG_DONTWAIT;
968
+
969
+
skb = skb_recv_datagram(sk, flags, noblock, &err);
970
+
if (!skb)
971
+
return err;
972
+
973
+
if (size < skb->len)
974
+
msg->msg_flags |= MSG_TRUNC;
975
+
else
976
+
size = skb->len;
977
+
978
+
err = memcpy_to_msg(msg, skb->data, size);
979
+
if (err < 0) {
980
+
skb_free_datagram(sk, skb);
981
+
return err;
982
+
}
983
+
984
+
sock_recv_timestamp(msg, sk, skb);
985
+
986
+
if (msg->msg_name) {
987
+
msg->msg_namelen = sizeof(struct sockaddr_can);
988
+
memcpy(msg->msg_name, skb->cb, msg->msg_namelen);
989
+
}
990
+
991
+
skb_free_datagram(sk, skb);
992
+
993
+
return size;
994
+
}
995
+
996
+
static int isotp_release(struct socket *sock)
997
+
{
998
+
struct sock *sk = sock->sk;
999
+
struct isotp_sock *so;
1000
+
struct net *net;
1001
+
1002
+
if (!sk)
1003
+
return 0;
1004
+
1005
+
so = isotp_sk(sk);
1006
+
net = sock_net(sk);
1007
+
1008
+
/* wait for complete transmission of current pdu */
1009
+
wait_event_interruptible(so->wait, so->tx.state == ISOTP_IDLE);
1010
+
1011
+
unregister_netdevice_notifier(&so->notifier);
1012
+
1013
+
lock_sock(sk);
1014
+
1015
+
hrtimer_cancel(&so->txtimer);
1016
+
hrtimer_cancel(&so->rxtimer);
1017
+
1018
+
/* remove current filters & unregister */
1019
+
if (so->bound) {
1020
+
if (so->ifindex) {
1021
+
struct net_device *dev;
1022
+
1023
+
dev = dev_get_by_index(net, so->ifindex);
1024
+
if (dev) {
1025
+
can_rx_unregister(net, dev, so->rxid,
1026
+
SINGLE_MASK(so->rxid),
1027
+
isotp_rcv, sk);
1028
+
dev_put(dev);
1029
+
}
1030
+
}
1031
+
}
1032
+
1033
+
so->ifindex = 0;
1034
+
so->bound = 0;
1035
+
1036
+
sock_orphan(sk);
1037
+
sock->sk = NULL;
1038
+
1039
+
release_sock(sk);
1040
+
sock_put(sk);
1041
+
1042
+
return 0;
1043
+
}
1044
+
1045
+
static int isotp_bind(struct socket *sock, struct sockaddr *uaddr, int len)
1046
+
{
1047
+
struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
1048
+
struct sock *sk = sock->sk;
1049
+
struct isotp_sock *so = isotp_sk(sk);
1050
+
struct net *net = sock_net(sk);
1051
+
int ifindex;
1052
+
struct net_device *dev;
1053
+
int err = 0;
1054
+
int notify_enetdown = 0;
1055
+
1056
+
if (len < CAN_REQUIRED_SIZE(struct sockaddr_can, can_addr.tp))
1057
+
return -EINVAL;
1058
+
1059
+
if (addr->can_addr.tp.rx_id == addr->can_addr.tp.tx_id)
1060
+
return -EADDRNOTAVAIL;
1061
+
1062
+
if ((addr->can_addr.tp.rx_id | addr->can_addr.tp.tx_id) &
1063
+
(CAN_ERR_FLAG | CAN_RTR_FLAG))
1064
+
return -EADDRNOTAVAIL;
1065
+
1066
+
if (!addr->can_ifindex)
1067
+
return -ENODEV;
1068
+
1069
+
lock_sock(sk);
1070
+
1071
+
if (so->bound && addr->can_ifindex == so->ifindex &&
1072
+
addr->can_addr.tp.rx_id == so->rxid &&
1073
+
addr->can_addr.tp.tx_id == so->txid)
1074
+
goto out;
1075
+
1076
+
dev = dev_get_by_index(net, addr->can_ifindex);
1077
+
if (!dev) {
1078
+
err = -ENODEV;
1079
+
goto out;
1080
+
}
1081
+
if (dev->type != ARPHRD_CAN) {
1082
+
dev_put(dev);
1083
+
err = -ENODEV;
1084
+
goto out;
1085
+
}
1086
+
if (dev->mtu < so->ll.mtu) {
1087
+
dev_put(dev);
1088
+
err = -EINVAL;
1089
+
goto out;
1090
+
}
1091
+
if (!(dev->flags & IFF_UP))
1092
+
notify_enetdown = 1;
1093
+
1094
+
ifindex = dev->ifindex;
1095
+
1096
+
can_rx_register(net, dev, addr->can_addr.tp.rx_id,
1097
+
SINGLE_MASK(addr->can_addr.tp.rx_id), isotp_rcv, sk,
1098
+
"isotp", sk);
1099
+
1100
+
dev_put(dev);
1101
+
1102
+
if (so->bound) {
1103
+
/* unregister old filter */
1104
+
if (so->ifindex) {
1105
+
dev = dev_get_by_index(net, so->ifindex);
1106
+
if (dev) {
1107
+
can_rx_unregister(net, dev, so->rxid,
1108
+
SINGLE_MASK(so->rxid),
1109
+
isotp_rcv, sk);
1110
+
dev_put(dev);
1111
+
}
1112
+
}
1113
+
}
1114
+
1115
+
/* switch to new settings */
1116
+
so->ifindex = ifindex;
1117
+
so->rxid = addr->can_addr.tp.rx_id;
1118
+
so->txid = addr->can_addr.tp.tx_id;
1119
+
so->bound = 1;
1120
+
1121
+
out:
1122
+
release_sock(sk);
1123
+
1124
+
if (notify_enetdown) {
1125
+
sk->sk_err = ENETDOWN;
1126
+
if (!sock_flag(sk, SOCK_DEAD))
1127
+
sk->sk_error_report(sk);
1128
+
}
1129
+
1130
+
return err;
1131
+
}
1132
+
1133
+
static int isotp_getname(struct socket *sock, struct sockaddr *uaddr, int peer)
1134
+
{
1135
+
struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
1136
+
struct sock *sk = sock->sk;
1137
+
struct isotp_sock *so = isotp_sk(sk);
1138
+
1139
+
if (peer)
1140
+
return -EOPNOTSUPP;
1141
+
1142
+
addr->can_family = AF_CAN;
1143
+
addr->can_ifindex = so->ifindex;
1144
+
addr->can_addr.tp.rx_id = so->rxid;
1145
+
addr->can_addr.tp.tx_id = so->txid;
1146
+
1147
+
return sizeof(*addr);
1148
+
}
1149
+
1150
+
static int isotp_setsockopt(struct socket *sock, int level, int optname,
1151
+
sockptr_t optval, unsigned int optlen)
1152
+
{
1153
+
struct sock *sk = sock->sk;
1154
+
struct isotp_sock *so = isotp_sk(sk);
1155
+
int ret = 0;
1156
+
1157
+
if (level != SOL_CAN_ISOTP)
1158
+
return -EINVAL;
1159
+
1160
+
switch (optname) {
1161
+
case CAN_ISOTP_OPTS:
1162
+
if (optlen != sizeof(struct can_isotp_options))
1163
+
return -EINVAL;
1164
+
1165
+
if (copy_from_sockptr(&so->opt, optval, optlen))
1166
+
return -EFAULT;
1167
+
1168
+
/* no separate rx_ext_address is given => use ext_address */
1169
+
if (!(so->opt.flags & CAN_ISOTP_RX_EXT_ADDR))
1170
+
so->opt.rx_ext_address = so->opt.ext_address;
1171
+
break;
1172
+
1173
+
case CAN_ISOTP_RECV_FC:
1174
+
if (optlen != sizeof(struct can_isotp_fc_options))
1175
+
return -EINVAL;
1176
+
1177
+
if (copy_from_sockptr(&so->rxfc, optval, optlen))
1178
+
return -EFAULT;
1179
+
break;
1180
+
1181
+
case CAN_ISOTP_TX_STMIN:
1182
+
if (optlen != sizeof(u32))
1183
+
return -EINVAL;
1184
+
1185
+
if (copy_from_sockptr(&so->force_tx_stmin, optval, optlen))
1186
+
return -EFAULT;
1187
+
break;
1188
+
1189
+
case CAN_ISOTP_RX_STMIN:
1190
+
if (optlen != sizeof(u32))
1191
+
return -EINVAL;
1192
+
1193
+
if (copy_from_sockptr(&so->force_rx_stmin, optval, optlen))
1194
+
return -EFAULT;
1195
+
break;
1196
+
1197
+
case CAN_ISOTP_LL_OPTS:
1198
+
if (optlen == sizeof(struct can_isotp_ll_options)) {
1199
+
struct can_isotp_ll_options ll;
1200
+
1201
+
if (copy_from_sockptr(&ll, optval, optlen))
1202
+
return -EFAULT;
1203
+
1204
+
/* check for correct ISO 11898-1 DLC data length */
1205
+
if (ll.tx_dl != padlen(ll.tx_dl))
1206
+
return -EINVAL;
1207
+
1208
+
if (ll.mtu != CAN_MTU && ll.mtu != CANFD_MTU)
1209
+
return -EINVAL;
1210
+
1211
+
if (ll.mtu == CAN_MTU && ll.tx_dl > CAN_MAX_DLEN)
1212
+
return -EINVAL;
1213
+
1214
+
memcpy(&so->ll, &ll, sizeof(ll));
1215
+
1216
+
/* set ll_dl for tx path to similar place as for rx */
1217
+
so->tx.ll_dl = ll.tx_dl;
1218
+
} else {
1219
+
return -EINVAL;
1220
+
}
1221
+
break;
1222
+
1223
+
default:
1224
+
ret = -ENOPROTOOPT;
1225
+
}
1226
+
1227
+
return ret;
1228
+
}
1229
+
1230
+
static int isotp_getsockopt(struct socket *sock, int level, int optname,
1231
+
char __user *optval, int __user *optlen)
1232
+
{
1233
+
struct sock *sk = sock->sk;
1234
+
struct isotp_sock *so = isotp_sk(sk);
1235
+
int len;
1236
+
void *val;
1237
+
1238
+
if (level != SOL_CAN_ISOTP)
1239
+
return -EINVAL;
1240
+
if (get_user(len, optlen))
1241
+
return -EFAULT;
1242
+
if (len < 0)
1243
+
return -EINVAL;
1244
+
1245
+
switch (optname) {
1246
+
case CAN_ISOTP_OPTS:
1247
+
len = min_t(int, len, sizeof(struct can_isotp_options));
1248
+
val = &so->opt;
1249
+
break;
1250
+
1251
+
case CAN_ISOTP_RECV_FC:
1252
+
len = min_t(int, len, sizeof(struct can_isotp_fc_options));
1253
+
val = &so->rxfc;
1254
+
break;
1255
+
1256
+
case CAN_ISOTP_TX_STMIN:
1257
+
len = min_t(int, len, sizeof(u32));
1258
+
val = &so->force_tx_stmin;
1259
+
break;
1260
+
1261
+
case CAN_ISOTP_RX_STMIN:
1262
+
len = min_t(int, len, sizeof(u32));
1263
+
val = &so->force_rx_stmin;
1264
+
break;
1265
+
1266
+
case CAN_ISOTP_LL_OPTS:
1267
+
len = min_t(int, len, sizeof(struct can_isotp_ll_options));
1268
+
val = &so->ll;
1269
+
break;
1270
+
1271
+
default:
1272
+
return -ENOPROTOOPT;
1273
+
}
1274
+
1275
+
if (put_user(len, optlen))
1276
+
return -EFAULT;
1277
+
if (copy_to_user(optval, val, len))
1278
+
return -EFAULT;
1279
+
return 0;
1280
+
}
1281
+
1282
+
static int isotp_notifier(struct notifier_block *nb, unsigned long msg,
1283
+
void *ptr)
1284
+
{
1285
+
struct net_device *dev = netdev_notifier_info_to_dev(ptr);
1286
+
struct isotp_sock *so = container_of(nb, struct isotp_sock, notifier);
1287
+
struct sock *sk = &so->sk;
1288
+
1289
+
if (!net_eq(dev_net(dev), sock_net(sk)))
1290
+
return NOTIFY_DONE;
1291
+
1292
+
if (dev->type != ARPHRD_CAN)
1293
+
return NOTIFY_DONE;
1294
+
1295
+
if (so->ifindex != dev->ifindex)
1296
+
return NOTIFY_DONE;
1297
+
1298
+
switch (msg) {
1299
+
case NETDEV_UNREGISTER:
1300
+
lock_sock(sk);
1301
+
/* remove current filters & unregister */
1302
+
if (so->bound)
1303
+
can_rx_unregister(dev_net(dev), dev, so->rxid,
1304
+
SINGLE_MASK(so->rxid),
1305
+
isotp_rcv, sk);
1306
+
1307
+
so->ifindex = 0;
1308
+
so->bound = 0;
1309
+
release_sock(sk);
1310
+
1311
+
sk->sk_err = ENODEV;
1312
+
if (!sock_flag(sk, SOCK_DEAD))
1313
+
sk->sk_error_report(sk);
1314
+
break;
1315
+
1316
+
case NETDEV_DOWN:
1317
+
sk->sk_err = ENETDOWN;
1318
+
if (!sock_flag(sk, SOCK_DEAD))
1319
+
sk->sk_error_report(sk);
1320
+
break;
1321
+
}
1322
+
1323
+
return NOTIFY_DONE;
1324
+
}
1325
+
1326
+
static int isotp_init(struct sock *sk)
1327
+
{
1328
+
struct isotp_sock *so = isotp_sk(sk);
1329
+
1330
+
so->ifindex = 0;
1331
+
so->bound = 0;
1332
+
1333
+
so->opt.flags = CAN_ISOTP_DEFAULT_FLAGS;
1334
+
so->opt.ext_address = CAN_ISOTP_DEFAULT_EXT_ADDRESS;
1335
+
so->opt.rx_ext_address = CAN_ISOTP_DEFAULT_EXT_ADDRESS;
1336
+
so->opt.rxpad_content = CAN_ISOTP_DEFAULT_PAD_CONTENT;
1337
+
so->opt.txpad_content = CAN_ISOTP_DEFAULT_PAD_CONTENT;
1338
+
so->opt.frame_txtime = CAN_ISOTP_DEFAULT_FRAME_TXTIME;
1339
+
so->rxfc.bs = CAN_ISOTP_DEFAULT_RECV_BS;
1340
+
so->rxfc.stmin = CAN_ISOTP_DEFAULT_RECV_STMIN;
1341
+
so->rxfc.wftmax = CAN_ISOTP_DEFAULT_RECV_WFTMAX;
1342
+
so->ll.mtu = CAN_ISOTP_DEFAULT_LL_MTU;
1343
+
so->ll.tx_dl = CAN_ISOTP_DEFAULT_LL_TX_DL;
1344
+
so->ll.tx_flags = CAN_ISOTP_DEFAULT_LL_TX_FLAGS;
1345
+
1346
+
/* set ll_dl for tx path to similar place as for rx */
1347
+
so->tx.ll_dl = so->ll.tx_dl;
1348
+
1349
+
so->rx.state = ISOTP_IDLE;
1350
+
so->tx.state = ISOTP_IDLE;
1351
+
1352
+
hrtimer_init(&so->rxtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL_SOFT);
1353
+
so->rxtimer.function = isotp_rx_timer_handler;
1354
+
hrtimer_init(&so->txtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL_SOFT);
1355
+
so->txtimer.function = isotp_tx_timer_handler;
1356
+
1357
+
init_waitqueue_head(&so->wait);
1358
+
1359
+
so->notifier.notifier_call = isotp_notifier;
1360
+
register_netdevice_notifier(&so->notifier);
1361
+
1362
+
return 0;
1363
+
}
1364
+
1365
+
static int isotp_sock_no_ioctlcmd(struct socket *sock, unsigned int cmd,
1366
+
unsigned long arg)
1367
+
{
1368
+
/* no ioctls for socket layer -> hand it down to NIC layer */
1369
+
return -ENOIOCTLCMD;
1370
+
}
1371
+
1372
+
static const struct proto_ops isotp_ops = {
1373
+
.family = PF_CAN,
1374
+
.release = isotp_release,
1375
+
.bind = isotp_bind,
1376
+
.connect = sock_no_connect,
1377
+
.socketpair = sock_no_socketpair,
1378
+
.accept = sock_no_accept,
1379
+
.getname = isotp_getname,
1380
+
.poll = datagram_poll,
1381
+
.ioctl = isotp_sock_no_ioctlcmd,
1382
+
.gettstamp = sock_gettstamp,
1383
+
.listen = sock_no_listen,
1384
+
.shutdown = sock_no_shutdown,
1385
+
.setsockopt = isotp_setsockopt,
1386
+
.getsockopt = isotp_getsockopt,
1387
+
.sendmsg = isotp_sendmsg,
1388
+
.recvmsg = isotp_recvmsg,
1389
+
.mmap = sock_no_mmap,
1390
+
.sendpage = sock_no_sendpage,
1391
+
};
1392
+
1393
+
static struct proto isotp_proto __read_mostly = {
1394
+
.name = "CAN_ISOTP",
1395
+
.owner = THIS_MODULE,
1396
+
.obj_size = sizeof(struct isotp_sock),
1397
+
.init = isotp_init,
1398
+
};
1399
+
1400
+
static const struct can_proto isotp_can_proto = {
1401
+
.type = SOCK_DGRAM,
1402
+
.protocol = CAN_ISOTP,
1403
+
.ops = &isotp_ops,
1404
+
.prot = &isotp_proto,
1405
+
};
1406
+
1407
+
static __init int isotp_module_init(void)
1408
+
{
1409
+
int err;
1410
+
1411
+
pr_info("can: isotp protocol (rev " CAN_ISOTP_VERSION ")\n");
1412
+
1413
+
err = can_proto_register(&isotp_can_proto);
1414
+
if (err < 0)
1415
+
pr_err("can: registration of isotp protocol failed\n");
1416
+
1417
+
return err;
1418
+
}
1419
+
1420
+
static __exit void isotp_module_exit(void)
1421
+
{
1422
+
can_proto_unregister(&isotp_can_proto);
1423
+
}
1424
+
1425
+
module_init(isotp_module_init);
1426
+
module_exit(isotp_module_exit);