Skip to content

Commit

Permalink
can: isotp: add local echo tx processing for consecutive frames
Browse files Browse the repository at this point in the history 
[ Upstream commit 4b7fe92 ]

Instead of dumping the CAN frames into the netdevice queue the process to
transmit consecutive frames (CF) now waits for the frame to be transmitted
and therefore echo'ed from the CAN interface.

Link: https://lore.kernel.org/all/[email protected]
Signed-off-by: Oliver Hartkopp <[email protected]>
Signed-off-by: Marc Kleine-Budde <[email protected]>
Signed-off-by: Sasha Levin <[email protected]>
  • Loading branch information
@hartkopp
hartkopp authored and Sasha Levin committed Apr 2, 2022
1 parent 7943884 commit 790670a
Showing 1 changed file with 139 additions and 80 deletions.
219 changes: 139 additions & 80 deletions net/can/isotp.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,6 @@
* - 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.
Expand Down Expand Up @@ -143,6 +142,7 @@ struct isotp_sock {
struct can_isotp_ll_options ll;
u32 force_tx_stmin;
u32 force_rx_stmin;
u32 cfecho; /* consecutive frame echo tag */
struct tpcon rx, tx;
struct list_head notifier;
wait_queue_head_t wait;
Expand Down Expand Up @@ -712,6 +712,63 @@ static void isotp_fill_dataframe(struct canfd_frame *cf, struct isotp_sock *so,
cf->data[0] = so->opt.ext_address;
}

static void isotp_send_cframe(struct isotp_sock *so)
{
struct sock *sk = &so->sk;
struct sk_buff *skb;
struct net_device *dev;
struct canfd_frame *cf;
int can_send_ret;
int ae = (so->opt.flags & CAN_ISOTP_EXTEND_ADDR) ? 1 : 0;

dev = dev_get_by_index(sock_net(sk), so->ifindex);
if (!dev)
return;

skb = alloc_skb(so->ll.mtu + sizeof(struct can_skb_priv), GFP_ATOMIC);
if (!skb) {
dev_put(dev);
return;
}

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_zero(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++;

cf->flags = so->ll.tx_flags;

skb->dev = dev;
can_skb_set_owner(skb, sk);

/* cfecho should have been zero'ed by init/isotp_rcv_echo() */
if (so->cfecho)
pr_notice_once("can-isotp: cfecho is %08X != 0\n", so->cfecho);

/* set consecutive frame echo tag */
so->cfecho = *(u32 *)cf->data;

/* send frame with local echo enabled */
can_send_ret = can_send(skb, 1);
if (can_send_ret) {
pr_notice_once("can-isotp: %s: can_send_ret %pe\n",
__func__, ERR_PTR(can_send_ret));
if (can_send_ret == -ENOBUFS)
pr_notice_once("can-isotp: tx queue is full\n");
}
dev_put(dev);
}

static void isotp_create_fframe(struct canfd_frame *cf, struct isotp_sock *so,
int ae)
{
Expand Down Expand Up @@ -748,19 +805,74 @@ static void isotp_create_fframe(struct canfd_frame *cf, struct isotp_sock *so,
so->tx.state = ISOTP_WAIT_FIRST_FC;
}

static void isotp_rcv_echo(struct sk_buff *skb, void *data)
{
struct sock *sk = (struct sock *)data;
struct isotp_sock *so = isotp_sk(sk);
struct canfd_frame *cf = (struct canfd_frame *)skb->data;

/* only handle my own local echo skb's */
if (skb->sk != sk || so->cfecho != *(u32 *)cf->data)
return;

/* cancel local echo timeout */
hrtimer_cancel(&so->txtimer);

/* local echo skb with consecutive frame has been consumed */
so->cfecho = 0;

if (so->tx.idx >= so->tx.len) {
/* we are done */
so->tx.state = ISOTP_IDLE;
wake_up_interruptible(&so->wait);
return;
}

if (so->txfc.bs && so->tx.bs >= so->txfc.bs) {
/* stop and wait for FC with timeout */
so->tx.state = ISOTP_WAIT_FC;
hrtimer_start(&so->txtimer, ktime_set(1, 0),
HRTIMER_MODE_REL_SOFT);
return;
}

/* no gap between data frames needed => use burst mode */
if (!so->tx_gap) {
isotp_send_cframe(so);
return;
}

/* start timer to send next consecutive frame with correct delay */
hrtimer_start(&so->txtimer, so->tx_gap, HRTIMER_MODE_REL_SOFT);
}

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_SENDING:

/* cfecho should be consumed by isotp_rcv_echo() here */
if (!so->cfecho) {
/* start timeout for unlikely lost echo skb */
hrtimer_set_expires(&so->txtimer,
ktime_add(ktime_get(),
ktime_set(2, 0)));
restart = HRTIMER_RESTART;

/* push out the next consecutive frame */
isotp_send_cframe(so);
break;
}

/* cfecho has not been cleared in isotp_rcv_echo() */
pr_notice_once("can-isotp: cfecho %08X timeout\n", so->cfecho);
fallthrough;

case ISOTP_WAIT_FC:
case ISOTP_WAIT_FIRST_FC:

Expand All @@ -776,78 +888,6 @@ static enum hrtimer_restart isotp_tx_timer_handler(struct hrtimer *hrtimer)
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_ATOMIC);
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_zero(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++;

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) {
pr_notice_once("can-isotp: %s: can_send_ret %pe\n",
__func__, ERR_PTR(can_send_ret));
if (can_send_ret == -ENOBUFS)
pr_notice_once("can-isotp: tx queue is full, increasing txqueuelen may prevent this error\n");
}
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);
}
Expand Down Expand Up @@ -1075,6 +1115,9 @@ static int isotp_release(struct socket *sock)
can_rx_unregister(net, dev, so->rxid,
SINGLE_MASK(so->rxid),
isotp_rcv, sk);
can_rx_unregister(net, dev, so->txid,
SINGLE_MASK(so->txid),
isotp_rcv_echo, sk);
dev_put(dev);
synchronize_rcu();
}
Expand Down Expand Up @@ -1161,11 +1204,20 @@ static int isotp_bind(struct socket *sock, struct sockaddr *uaddr, int len)

ifindex = dev->ifindex;

if (do_rx_reg)
if (do_rx_reg) {
can_rx_register(net, dev, addr->can_addr.tp.rx_id,
SINGLE_MASK(addr->can_addr.tp.rx_id),
isotp_rcv, sk, "isotp", sk);

/* no consecutive frame echo skb in flight */
so->cfecho = 0;

/* register for echo skb's */
can_rx_register(net, dev, addr->can_addr.tp.tx_id,
SINGLE_MASK(addr->can_addr.tp.tx_id),
isotp_rcv_echo, sk, "isotpe", sk);
}

dev_put(dev);

if (so->bound && do_rx_reg) {
Expand All @@ -1176,6 +1228,9 @@ static int isotp_bind(struct socket *sock, struct sockaddr *uaddr, int len)
can_rx_unregister(net, dev, so->rxid,
SINGLE_MASK(so->rxid),
isotp_rcv, sk);
can_rx_unregister(net, dev, so->txid,
SINGLE_MASK(so->txid),
isotp_rcv_echo, sk);
dev_put(dev);
}
}
Expand Down Expand Up @@ -1381,10 +1436,14 @@ static void isotp_notify(struct isotp_sock *so, unsigned long msg,
case NETDEV_UNREGISTER:
lock_sock(sk);
/* remove current filters & unregister */
if (so->bound && (!(so->opt.flags & CAN_ISOTP_SF_BROADCAST)))
if (so->bound && (!(so->opt.flags & CAN_ISOTP_SF_BROADCAST))) {
can_rx_unregister(dev_net(dev), dev, so->rxid,
SINGLE_MASK(so->rxid),
isotp_rcv, sk);
can_rx_unregister(dev_net(dev), dev, so->txid,
SINGLE_MASK(so->txid),
isotp_rcv_echo, sk);
}

so->ifindex = 0;
so->bound = 0;
Expand Down

0 comments on commit 790670a

Please sign in to comment.