From patchwork Wed Apr 27 08:58:47 2011 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: Kurt Van Dijck X-Patchwork-Id: 93007 X-Patchwork-Delegate: davem@davemloft.net Return-Path: X-Original-To: patchwork-incoming@ozlabs.org Delivered-To: patchwork-incoming@ozlabs.org Received: from vger.kernel.org (vger.kernel.org [209.132.180.67]) by ozlabs.org (Postfix) with ESMTP id 6B154B6F06 for ; Wed, 27 Apr 2011 18:59:16 +1000 (EST) Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S1755409Ab1D0I7I (ORCPT ); Wed, 27 Apr 2011 04:59:08 -0400 Received: from gate.eia.be ([194.78.71.18]:17958 "EHLO mail.eia.be" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S1754831Ab1D0I7C (ORCPT ); Wed, 27 Apr 2011 04:59:02 -0400 Received: from kurt.e-circ.dyndns.org ([172.17.16.81]) by mail.eia.be over TLS secured channel with Microsoft SMTPSVC(6.0.3790.4675); Wed, 27 Apr 2011 10:58:49 +0200 Date: Wed, 27 Apr 2011 10:58:47 +0200 From: Kurt Van Dijck To: socketcan-core@lists.berlios.de, netdev@vger.kernel.org Subject: [PATCH v4 3/5] can-j1939: Import SAE J1939 stack Message-ID: <20110427085847.GD757@kurt.e-circ.dyndns.org> Mail-Followup-To: socketcan-core@lists.berlios.de, netdev@vger.kernel.org References: <20110427085330.GA757@kurt.e-circ.dyndns.org> MIME-Version: 1.0 Content-Disposition: inline In-Reply-To: <20110427085330.GA757@kurt.e-circ.dyndns.org> User-Agent: Mutt/1.5.20 (2009-06-14) X-OriginalArrivalTime: 27 Apr 2011 08:58:49.0242 (UTC) FILETIME=[52DA97A0:01CC04B9] Sender: netdev-owner@vger.kernel.org Precedence: bulk List-ID: X-Mailing-List: netdev@vger.kernel.org Import SAE J1939 Signed-off-by: Kurt Van Dijck --- include/linux/can/Kbuild | 1 + include/linux/can/j1939.h | 99 +++ net/can/Kconfig | 1 + net/can/Makefile | 2 + net/can/j1939/Kconfig | 22 + net/can/j1939/Makefile | 14 + net/can/j1939/address-claim.c | 227 +++++++ net/can/j1939/bus.c | 523 +++++++++++++++ net/can/j1939/filter.c | 76 +++ net/can/j1939/j1939-priv.h | 314 +++++++++ net/can/j1939/main.c | 458 +++++++++++++ net/can/j1939/proc.c | 104 +++ net/can/j1939/promisc.c | 43 ++ net/can/j1939/rtnl.c | 308 +++++++++ net/can/j1939/socket.c | 969 +++++++++++++++++++++++++++ net/can/j1939/transport.c | 1449 +++++++++++++++++++++++++++++++++++++++++ 16 files changed, 4610 insertions(+), 0 deletions(-) create mode 100644 include/linux/can/j1939.h create mode 100644 net/can/j1939/Kconfig create mode 100644 net/can/j1939/Makefile create mode 100644 net/can/j1939/address-claim.c create mode 100644 net/can/j1939/bus.c create mode 100644 net/can/j1939/filter.c create mode 100644 net/can/j1939/j1939-priv.h create mode 100644 net/can/j1939/main.c create mode 100644 net/can/j1939/proc.c create mode 100644 net/can/j1939/promisc.c create mode 100644 net/can/j1939/rtnl.c create mode 100644 net/can/j1939/socket.c create mode 100644 net/can/j1939/transport.c diff --git a/include/linux/can/Kbuild b/include/linux/can/Kbuild index 8cb05aa..0364eef 100644 --- a/include/linux/can/Kbuild +++ b/include/linux/can/Kbuild @@ -2,3 +2,4 @@ header-y += raw.h header-y += bcm.h header-y += error.h header-y += netlink.h +header-y += j1939.h diff --git a/include/linux/can/j1939.h b/include/linux/can/j1939.h new file mode 100644 index 0000000..7ff419e --- /dev/null +++ b/include/linux/can/j1939.h @@ -0,0 +1,99 @@ +/* + * j1939.h + * + * Copyright (c) 2010-2011 EIA Electronics + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + */ + +#ifndef _J1939_H_ +#define _J1939_H_ + +#include +#include +#include + +#define J1939_IDLE_ADDR 0xfe +#define J1939_NO_ADDR 0xff +#define J1939_NO_NAME 0 +#define J1939_NO_PGN 0x40000 +/* + * J1939 Parameter Group Number + * + * bit 0-7 : PDU Specific (PS) + * bit 8-15 : PDU Format (PF) + * bit 16 : Data Page (DP) + * bit 17 : Reserved (R) + * bit 19-31 : set to zero + */ +typedef __u32 pgn_t; + +/* + * J1939 Priority + * + * bit 0-2 : Priority (P) + * bit 3-7 : set to zero + */ +typedef __u8 priority_t; + +/* + * J1939 NAME + * + * bit 0-20 : Identity Number + * bit 21-31 : Manufacturer Code + * bit 32-34 : ECU Instance + * bit 35-39 : Function Instance + * bit 40-47 : Function + * bit 48 : Reserved + * bit 49-55 : Vehicle System + * bit 56-59 : Vehicle System Instance + * bit 60-62 : Industry Group + * bit 63 : Arbitrary Address Capable + */ +typedef __u64 name_t; + +/* + * J1939 socket options + */ +#define SOL_CAN_J1939 (SOL_CAN_BASE + CAN_J1939) +enum { + SO_J1939_FILTER = 1, /* set filters */ + SO_J1939_PROMISC = 2, /* set/clr promiscuous mode */ + SO_J1939_RECV_OWN = 3, + SO_J1939_SEND_PRIO = 4, +}; + +enum { + SCM_J1939_DEST_ADDR = 1, + SCM_J1939_DEST_NAME = 2, + SCM_J1939_PRIO = 3, +}; + +struct j1939_filter { + name_t name; + name_t name_mask; + __u8 addr; + __u8 addr_mask; + pgn_t pgn; + pgn_t pgn_mask; +}; + +/* + * RTNETLINK + */ +enum { + IFLA_J1939_UNSPEC, + IFLA_J1939_ENABLE, + IFLA_J1939_MAX, +}; + +enum { + IFA_J1939_UNSPEC, + IFA_J1939_ADDR, + IFA_J1939_NAME, + IFA_J1939_MAX, +}; + +#endif /* _J1939_H_ */ diff --git a/net/can/Kconfig b/net/can/Kconfig index 89395b2..7feb58c 100644 --- a/net/can/Kconfig +++ b/net/can/Kconfig @@ -40,5 +40,6 @@ config CAN_BCM CAN messages are used on the bus (e.g. in automotive environments). To use the Broadcast Manager, use AF_CAN with protocol CAN_BCM. +source "net/can/j1939/Kconfig" source "drivers/net/can/Kconfig" diff --git a/net/can/Makefile b/net/can/Makefile index 2d3894b..953d851 100644 --- a/net/can/Makefile +++ b/net/can/Makefile @@ -10,3 +10,5 @@ can-raw-y := raw.o obj-$(CONFIG_CAN_BCM) += can-bcm.o can-bcm-y := bcm.o + +obj-$(CONFIG_CAN_J1939) += j1939/ diff --git a/net/can/j1939/Kconfig b/net/can/j1939/Kconfig new file mode 100644 index 0000000..74d2a86 --- /dev/null +++ b/net/can/j1939/Kconfig @@ -0,0 +1,22 @@ +# +# SAE J1939 network layer core configuration +# + +config CAN_J1939 + tristate "SAE J1939" + depends on CAN + ---help--- + SAE J1939 + Say Y to have in-kernel support for j1939 socket type. This + allows communication according to SAE j1939. + The relevant parts in kernel are + SAE j1939-21 (datalink & transport protocol) + & SAE j1939-81 (network management). + +config CAN_J1939_DEBUG + bool "debug SAE J1939" + depends on CAN_J1939 + default n + ---help--- + Say Y to add extra debug code (via printk) in the j1939 stack + diff --git a/net/can/j1939/Makefile b/net/can/j1939/Makefile new file mode 100644 index 0000000..7ca2fc9 --- /dev/null +++ b/net/can/j1939/Makefile @@ -0,0 +1,14 @@ + +obj-$(CONFIG_CAN_J1939) += can-j1939.o + +can-j1939-objs := main.o \ + proc.o bus.o \ + rtnl.o \ + socket.o \ + address-claim.o transport.o \ + promisc.o filter.o + +ifeq ($(CONFIG_CAN_J1939_DEBUG),y) + EXTRA_CFLAGS += -DDEBUG +endif + diff --git a/net/can/j1939/address-claim.c b/net/can/j1939/address-claim.c new file mode 100644 index 0000000..826c7e9 --- /dev/null +++ b/net/can/j1939/address-claim.c @@ -0,0 +1,227 @@ +/* + * Copyright (c) 2010-2011 EIA Electronics + * + * Authors: + * Pieter Beyens + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation + */ + +/* + * J1939 Address Claiming. + * Address Claiming in the kernel + * - keeps track of the AC states of ECU's, + * - resolves NAME<=>SA taking into account the AC states of ECU's. + * + * All Address Claim msgs (including host-originated msg) are processed + * at the receive path (a sent msg is always received again via CAN echo). + * As such, the processing of AC msgs is done in the order on which msgs + * are sent on the bus. + * + * This module doesn't send msgs itself (e.g. replies on Address Claims), + * this is the responsibility of a user space application or daemon. + */ + +#include +#include + +#include "j1939-priv.h" + +#define CANDATA2NAME(data) le64_to_cpup((uint64_t *)data) + +static inline int ac_msg_is_request_for_ac(struct sk_buff *skb) +{ + struct j1939_sk_buff_cb *sk_addr = (void *)skb->cb; + int req_pgn; + + if ((skb->len < 3) || (sk_addr->pgn != PGN_REQUEST)) + return 0; + req_pgn = skb->data[0] | (skb->data[1] << 8) | (skb->data[2] << 16); + return req_pgn = PGN_ADDRESS_CLAIMED; +} + +static int j1939_verify_outgoing_address_claim(struct sk_buff *skb) +{ + struct j1939_sk_buff_cb *sk_addr = (void *)skb->cb; + + if (skb->len != 8) { + j1939_notice("tx address claim with dlc %i\n", skb->len); + return -EPROTO; + } + + if (sk_addr->src.name != CANDATA2NAME(skb->data)) { + j1939_notice("tx address claim with different name\n"); + return -EPROTO; + } + + if (sk_addr->src.addr == J1939_NO_ADDR) { + j1939_notice("tx address claim with broadcast sa\n"); + return -EPROTO; + } + + /* ac must always be a broadcast */ + if (sk_addr->dst.name || (sk_addr->dst.addr != J1939_NO_ADDR)) { + j1939_notice("tx address claim with dest, not broadcast\n"); + return -EPROTO; + } + return 0; +} + +int j1939_send_address_claim(struct sk_buff *skb) +{ + int ret, sa; + struct j1939_sk_buff_cb *sk_addr = (void *)skb->cb; + + /* network mgmt: address claiming msgs */ + if (sk_addr->pgn == PGN_ADDRESS_CLAIMED) { + struct j1939_ecu *ecu; + + ret = j1939_verify_outgoing_address_claim(skb); + /* return both when failure & when successfull */ + if (ret < 0) + return ret; + ecu = j1939_ecu_find_by_name(sk_addr->src.name, + sk_addr->ifindex); + if (!ecu) + return -ENODEV; + if (!(ecu->flags & ECUFLAG_LOCAL)) { + put_j1939_ecu(ecu); + return -EREMOTE; + } + + if (ecu->sa != sk_addr->src.addr) + /* hold further traffic for ecu, remove from parent */ + j1939_ecu_remove_sa(ecu); + put_j1939_ecu(ecu); + } else if (sk_addr->src.name) { + /* assign source address */ + sa = j1939_name_to_sa(sk_addr->src.name, sk_addr->ifindex); + if (!j1939_address_is_unicast(sa) && + !ac_msg_is_request_for_ac(skb)) { + j1939_notice("tx drop: invalid sa for name " + "0x%016llx\n", sk_addr->src.name); + return -EADDRNOTAVAIL; + } + sk_addr->src.addr = sa; + } + + /* assign destination address */ + if (sk_addr->dst.name) { + sa = j1939_name_to_sa(sk_addr->dst.name, sk_addr->ifindex); + if (!j1939_address_is_unicast(sa)) { + j1939_notice("tx drop: invalid da for name " + "0x%016llx\n", sk_addr->dst.name); + return -EADDRNOTAVAIL; + } + sk_addr->dst.addr = sa; + } + return 0; +} + +static struct j1939_ecu *j1939_process_address_claim(struct sk_buff *skb) +{ + struct j1939_sk_buff_cb *sk_addr = (void *)skb->cb; + struct j1939_ecu *ecu, *dut, **pref; + name_t name; + + if (skb->len < 8) { + j1939_notice("rx address claim with wrong dlc %i\n", skb->len); + return ERR_PTR(-EPROTO); + } + + name = CANDATA2NAME(skb->data); + if (!name) { + j1939_notice("rx address claim without name\n"); + return ERR_PTR(-EPROTO); + } + + if (!j1939_address_is_valid(sk_addr->src.addr)) { + j1939_notice("rx address claim with broadcast sa\n"); + return ERR_PTR(-EPROTO); + } + + ecu = j1939_ecu_get_register(name, sk_addr->ifindex, ECUFLAG_REMOTE, 1); + if (IS_ERR(ecu)) + return ecu; + if ((ecu->flags & ECUFLAG_LOCAL) && !skb->sk) + j1939_warning("duplicate name on the bus %016llx!\n", + (long long)name); + + if (sk_addr->src.addr >= J1939_IDLE_ADDR) { + j1939_ecu_remove_sa(ecu); + if (ecu->flags & ECUFLAG_REMOTE) + /* extra put => schedule removal */ + j1939_ecu_unregister(ecu); + return ecu; + } + + write_lock_bh(&ecu->parent->lock); + /* save new SA */ + if (sk_addr->src.addr != ecu->sa) + j1939_ecu_remove_sa_locked(ecu); + ecu->sa = sk_addr->src.addr; + /* iterate this segment */ + list_for_each_entry(dut, &ecu->parent->ecus, list) { + /* cancel pending claims for this SA */ + /* this includes myself ! */ + if (ecu->sa == dut->sa) + /* + * cancel pending claims for our new SA + * this includes 'ecu', since we will + * schedule a timer soon now + */ + hrtimer_try_to_cancel(&dut->ac_timer); + if (dut->name > ecu->name) + dut->sa = J1939_IDLE_ADDR; + } + + pref = &ecu->parent->ents[sk_addr->src.addr].ecu; + if (*pref && ((*pref)->name > ecu->name)) + *pref = NULL; + + /* schedule timer in 250 msec to commit address change */ + hrtimer_start(&ecu->ac_timer, ktime_set(0, 250000000), + HRTIMER_MODE_REL); + write_unlock_bh(&ecu->parent->lock); + + return ecu; +} + +int j1939_recv_address_claim(struct sk_buff *skb) +{ + struct j1939_sk_buff_cb *sk_addr = (void *)skb->cb; + struct j1939_ecu *ecu; + + /* + * network mgmt + */ + if (sk_addr->pgn == PGN_ADDRESS_CLAIMED) { + ecu = j1939_process_address_claim(skb); + if (IS_ERR(ecu)) + return PTR_ERR(ecu); + } else if (j1939_address_is_unicast(sk_addr->src.addr)) { + ecu = j1939_ecu_find_by_addr(sk_addr->src.addr, + sk_addr->ifindex); + } else { + ecu = NULL; + } + + /* assign source stuff */ + if (ecu) { + ecu->rxtime = ktime_get(); + sk_addr->src.flags = ecu->flags; + sk_addr->src.name = ecu->name; + put_j1939_ecu(ecu); + } + /* assign destination stuff */ + ecu = j1939_ecu_find_by_addr(sk_addr->dst.addr, sk_addr->ifindex); + if (ecu) { + sk_addr->dst.flags = ecu->flags; + sk_addr->dst.name = ecu->name; + put_j1939_ecu(ecu); + } + return 0; +} + diff --git a/net/can/j1939/bus.c b/net/can/j1939/bus.c new file mode 100644 index 0000000..c51245d --- /dev/null +++ b/net/can/j1939/bus.c @@ -0,0 +1,523 @@ +/* + * Copyright (c) 2010-2011 EIA Electronics + * + * Authors: + * Kurt Van Dijck + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation + */ + +/* + * j1939-bus.c - bus for j1939 remote devices + * Since rtnetlink, no real bus is used. + */ + +#include +#include +#include +#include +#include + +#include "j1939-priv.h" + +#define jseg_dbg(jseg, fmt, ...) \ + pr_debug("j1939-%i: " fmt, (jseg)->ifindex, ##__VA_ARGS__) + +#define ecu_dbg(ecu, fmt, ...) \ + pr_debug("j1939-%i,%016llx,%02x: " fmt, (ecu)->parent->ifindex, \ + (ecu)->name, (ecu)->sa, ##__VA_ARGS__) +#define ecu_alert(ecu, fmt, ...) \ + pr_alert("j1939-%i,%016llx,%02x: " fmt, (ecu)->parent->ifindex, \ + (ecu)->name, (ecu)->sa, ##__VA_ARGS__) + +static struct { + struct list_head list; + spinlock_t lock; +} segments; + +struct j1939_segment *j1939_segment_find(int ifindex) +{ + struct j1939_segment *jseg; + + spin_lock_bh(&segments.lock); + list_for_each_entry(jseg, &segments.list, flist) { + if (jseg->ifindex == ifindex) { + get_j1939_segment(jseg); + goto found; + } + } + jseg = NULL; +found: + spin_unlock_bh(&segments.lock); + return jseg; +} + +/* + * iterate over ECU's, + * and register flagged ecu's on their claimed SA + */ +static void j1939_segment_ac_task(unsigned long val) +{ + struct j1939_segment *jseg = (void *)val; + struct j1939_ecu *ecu; + + write_lock_bh(&jseg->lock); + list_for_each_entry(ecu, &jseg->ecus, list) { + /* next 2 (read & set) could be merged into xxx? */ + if (!atomic_read(&ecu->ac_delay_expired)) + continue; + atomic_set(&ecu->ac_delay_expired, 0); + if (j1939_address_is_unicast(ecu->sa)) + ecu->parent->ents[ecu->sa].ecu = ecu; + } + write_unlock_bh(&jseg->lock); +} +/* + * segment device interface + */ +static void cb_put_j1939_segment(struct kref *kref) +{ + struct j1939_segment *jseg = + container_of(kref, struct j1939_segment, kref); + + tasklet_disable_nosync(&jseg->ac_task); + kfree(jseg); +} + +void put_j1939_segment(struct j1939_segment *segment) +{ + kref_put(&segment->kref, cb_put_j1939_segment); +} + +int j1939_segment_register(struct net_device *netdev) +{ + int ret; + struct j1939_segment *jseg; + + jseg = j1939_segment_find(netdev->ifindex); + if (jseg) { + put_j1939_segment(jseg); + ret = -EALREADY; + goto fail_exist; + } + jseg = kzalloc(sizeof(*jseg), GFP_KERNEL); + if (!jseg) { + ret = -ENOMEM; + goto fail_malloc; + } + tasklet_init(&jseg->ac_task, j1939_segment_ac_task, + (unsigned long)jseg); + rwlock_init(&jseg->lock); + INIT_LIST_HEAD(&jseg->ecus); + INIT_LIST_HEAD(&jseg->flist); + jseg->ifindex = netdev->ifindex; + + kref_init(&jseg->kref); + + spin_lock_bh(&segments.lock); + list_add_tail(&jseg->flist, &segments.list); + spin_unlock_bh(&segments.lock); + + jseg_dbg(jseg, "register\n"); + return 0; + +fail_malloc: +fail_exist: + return ret; +} + +void j1939_segment_unregister(struct j1939_segment *jseg) +{ + struct j1939_ecu *ecu; + + if (!jseg) + return; + + spin_lock_bh(&segments.lock); + list_del_init(&jseg->flist); + spin_unlock_bh(&segments.lock); + + write_lock_bh(&jseg->lock); + while (!list_empty(&jseg->ecus)) { + ecu = list_first_entry(&jseg->ecus, struct j1939_ecu, list); + write_unlock_bh(&jseg->lock); + j1939_ecu_unregister(ecu); + write_lock_bh(&jseg->lock); + } + write_unlock_bh(&jseg->lock); + jseg_dbg(jseg, "unregister\n"); + put_j1939_segment(jseg); +} + +/* + * ECU device interface + */ +static enum hrtimer_restart j1939_ecu_timer_handler(struct hrtimer *hrtimer) +{ + struct j1939_ecu *ecu = + container_of(hrtimer, struct j1939_ecu, ac_timer); + + atomic_set(&ecu->ac_delay_expired, 1); + tasklet_schedule(&ecu->parent->ac_task); + return HRTIMER_NORESTART; +} + +static void cb_put_j1939_ecu(struct kref *kref) +{ + struct j1939_ecu *ecu =container_of(kref, struct j1939_ecu, kref); + + kfree(ecu); +} +void put_j1939_ecu(struct j1939_ecu *ecu) +{ + kref_put(&ecu->kref, cb_put_j1939_ecu); +} + +struct j1939_ecu *j1939_ecu_get_register(name_t name, int ifindex, int flags, + int return_existing) +{ + struct j1939_segment *parent; + struct j1939_ecu *ecu, *dut; + + if (!ifindex || !name) { + pr_alert("%s(%i, %016llx) invalid\n", + __func__, ifindex, (long long)name); + return ERR_PTR(-EINVAL); + } + + parent = j1939_segment_find(ifindex); + if (!parent) { + pr_alert("%s %i: segment not found\n", __func__, ifindex); + return ERR_PTR(-EINVAL); + } + if (return_existing) { + read_lock_bh(&parent->lock); + /* test for existing name */ + list_for_each_entry(dut, &parent->ecus, list) { + if (dut->name == name) { + get_j1939_ecu(dut); + read_unlock_bh(&parent->lock); + return dut; + } + } + read_unlock_bh(&parent->lock); + } + /* alloc */ + ecu = kzalloc(sizeof(*ecu), gfp_any()); + if (!ecu) + /* should we look for an existing ecu */ + return ERR_PTR(-ENOMEM); + kref_init(&ecu->kref); + ecu->sa = J1939_IDLE_ADDR; + ecu->name = name; + ecu->flags = flags; + + hrtimer_init(&ecu->ac_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); + ecu->ac_timer.function = j1939_ecu_timer_handler; + INIT_LIST_HEAD(&ecu->list); + + /* first add to internal list */ + write_lock_bh(&parent->lock); + /* test for duplicate name */ + list_for_each_entry(dut, &parent->ecus, list) { + if (dut->name == ecu->name) + goto duplicate; + } + get_j1939_ecu(ecu); + /* a ref to parent is held */ + ecu->parent = parent; + list_add_tail(&ecu->list, &parent->ecus); + write_unlock_bh(&parent->lock); + ecu_dbg(ecu, "register\n"); + return ecu; + +duplicate: + get_j1939_ecu(dut); + write_unlock_bh(&parent->lock); + put_j1939_segment(parent); + if (return_existing) + return dut; + ecu_alert(ecu, "duplicate name\n"); + put_j1939_ecu(ecu); + return ERR_PTR(-EEXIST); +} + +void j1939_ecu_unregister(struct j1939_ecu *ecu) +{ + BUG_ON(!ecu); + ecu_dbg(ecu, "unregister\n"); + hrtimer_try_to_cancel(&ecu->ac_timer); + + write_lock_bh(&ecu->parent->lock); + j1939_ecu_remove_sa_locked(ecu); + list_del_init(&ecu->list); + write_unlock_bh(&ecu->parent->lock); + /* put segment, reverting the effect done by ..._register() */ + put_j1939_segment(ecu->parent); + put_j1939_ecu(ecu); +} + +struct j1939_ecu *j1939_ecu_find_by_addr(int sa, int ifindex) +{ + struct j1939_ecu *ecu; + struct j1939_segment *parent; + + if (!j1939_address_is_unicast(sa)) + return NULL; + parent = j1939_segment_find(ifindex); + if (!parent) + return NULL; + read_lock_bh(&parent->lock); + ecu = parent->ents[sa].ecu; + if (ecu) + get_j1939_ecu(ecu); + read_unlock_bh(&parent->lock); + put_j1939_segment(parent); + return ecu; +} + +int j1939_name_to_sa(uint64_t name, int ifindex) +{ + struct j1939_ecu *ecu; + struct j1939_segment *parent; + int sa; + + if (!name) + return J1939_IDLE_ADDR; + parent = j1939_segment_find(ifindex); + if (!parent) + return J1939_IDLE_ADDR; + + sa = J1939_IDLE_ADDR; + read_lock_bh(&parent->lock); + list_for_each_entry(ecu, &parent->ecus, list) { + if (ecu->name == name) { + if ((sa == J1939_IDLE_ADDR) && + (parent->ents[ecu->sa].ecu == ecu)) + /* ecu's SA is registered */ + sa = ecu->sa; + break; + } + } + read_unlock_bh(&parent->lock); + put_j1939_segment(parent); + return sa; +} + +struct j1939_ecu *j1939_ecu_find_segment_default_tx(int ifindex, + name_t *name, uint8_t *addr) +{ + struct j1939_ecu *ecu; + struct j1939_segment *parent; + struct addr_ent *paddr; + int j; + + if (ifindex <= 0) + return ERR_PTR(-EINVAL); + parent = j1939_segment_find(ifindex); + if (!parent) + return ERR_PTR(-ENETUNREACH); + read_lock_bh(&parent->lock); + list_for_each_entry(ecu, &parent->ecus, list) { + if (ecu->flags & ECUFLAG_LOCAL) { + get_j1939_ecu(ecu); + if (name) + *name = ecu->name; + if (addr) + *addr = ecu->sa; + goto found; + } + } + ecu = NULL; + for (j = 0, paddr = parent->ents; j < J1939_IDLE_ADDR; ++j, ++paddr) { + if (paddr->ecu) + continue; + if (paddr->flags & ECUFLAG_LOCAL) { + if (name) + *name = 0; + if (addr) + *addr = j; + goto found; + } + } + ecu = ERR_PTR(-EHOSTDOWN); +found: + read_unlock_bh(&parent->lock); + put_j1939_segment(parent); + return ecu; +} + +/* ecu lookup helper */ +static struct j1939_ecu *_j1939_ecu_find_by_name(name_t name, + struct j1939_segment *jseg) +{ + struct j1939_ecu *ecu; + + read_lock_bh(&jseg->lock); + list_for_each_entry(ecu, &jseg->ecus, list) { + if (ecu->name == name) { + get_j1939_ecu(ecu); + goto found_on_intf; + } + } + ecu = NULL; +found_on_intf: + read_unlock_bh(&jseg->lock); + return ecu; +} + +/* ecu lookup by name */ +struct j1939_ecu *j1939_ecu_find_by_name(name_t name, int ifindex) +{ + struct j1939_ecu *ecu; + struct j1939_segment *jseg; + + if (!name) + return NULL; + if (ifindex) { + jseg = j1939_segment_find(ifindex); + if (!jseg) + return NULL; + ecu = _j1939_ecu_find_by_name(name, jseg); + put_j1939_segment(jseg); + return ecu; + } + /* iterate segments */ + spin_lock_bh(&segments.lock); + list_for_each_entry(jseg, &segments.list, flist) { + get_j1939_segment(jseg); + ecu = _j1939_ecu_find_by_name(name, jseg); + put_j1939_segment(jseg); + if (ecu) + goto found; + } + ecu = NULL; +found: + spin_unlock_bh(&segments.lock); + return ecu; +} + +/* PROC */ +static int j1939_proc_addr(struct seq_file *sqf, void *v) +{ + struct j1939_segment *jseg; + struct net_device *netdev; + struct addr_ent *paddr; + int j, flags; + ktime_t now; + struct timeval tv; + + now = ktime_get(); + seq_printf(sqf, "iface\tSA\tflags\trxtime\n"); + spin_lock_bh(&segments.lock); + list_for_each_entry(jseg, &segments.list, flist) { + get_j1939_segment(jseg); + netdev = dev_get_by_index(&init_net, jseg->ifindex); + if (!netdev) { + pr_alert("j1939 proc: ifindex %i not found\n", + jseg->ifindex); + put_j1939_segment(jseg); + continue; + } + read_lock_bh(&jseg->lock); + for (j = 0, paddr = jseg->ents; j < J1939_IDLE_ADDR; + ++j, ++paddr) { + flags = paddr->flags; + if (paddr->ecu) + flags |= paddr->ecu->flags; + tv = ktime_to_timeval(ktime_sub(now, paddr->rxtime)); + if (!paddr->flags && !paddr->ecu) + continue; + seq_printf(sqf, "%s\t%02x\t%c%c%c%c\t-%lu.%06lu\n", + netdev->name, j, + (flags & ECUFLAG_LOCAL) ? 'L' : '-', + (flags & ECUFLAG_REMOTE) ? 'R' : '-', + (paddr->flags) ? 'S' : '-', + paddr->ecu ? 'E' : '-', + tv.tv_sec, tv.tv_usec); + } + read_unlock_bh(&jseg->lock); + dev_put(netdev); + put_j1939_segment(jseg); + } + spin_unlock_bh(&segments.lock); + return 0; +} + +static int j1939_proc_ecu(struct seq_file *sqf, void *v) +{ + struct j1939_segment *jseg; + struct j1939_ecu *ecu; + struct net_device *netdev; + ktime_t now; + struct timeval tv; + char sa[4]; + + now = ktime_get(); + seq_printf(sqf, "iface\taddr\tname\tflags\trxtime\n"); + spin_lock_bh(&segments.lock); + list_for_each_entry(jseg, &segments.list, flist) { + get_j1939_segment(jseg); + netdev = dev_get_by_index(&init_net, jseg->ifindex); + if (!netdev) { + pr_alert("j1939 proc: ifindex %i not found\n", + jseg->ifindex); + put_j1939_segment(jseg); + continue; + } + read_lock_bh(&jseg->lock); + list_for_each_entry(ecu, &jseg->ecus, list) { + tv = ktime_to_timeval(ktime_sub(now, ecu->rxtime)); + if (j1939_address_is_unicast(ecu->sa) && + (ecu->parent->ents[ecu->sa].ecu == ecu)) + snprintf(sa, sizeof(sa), "%02x", ecu->sa); + else + strcpy(sa, "-"); + seq_printf(sqf, "%s\t%s\t%016llx\t%c\t-%lu.%06lu\n", + netdev->name, sa, + (unsigned long long)ecu->name, + (ecu->flags & ECUFLAG_LOCAL) ? 'L' : 'R', + tv.tv_sec, tv.tv_usec); + } + read_unlock_bh(&jseg->lock); + dev_put(netdev); + put_j1939_segment(jseg); + } + spin_unlock_bh(&segments.lock); + return 0; +} + +/* exported init */ +int __init j1939bus_module_init(void) +{ + INIT_LIST_HEAD(&segments.list); + spin_lock_init(&segments.lock); + j1939_proc_add("addr", j1939_proc_addr, NULL); + j1939_proc_add("ecu", j1939_proc_ecu, NULL); + return 0; +} + +void j1939bus_module_exit(void) +{ + struct j1939_segment *jseg; + struct net_device *netdev; + + spin_lock_bh(&segments.lock); + while (!list_empty(&segments.list)) { + jseg = list_first_entry(&segments.list, + struct j1939_segment, flist); + netdev = dev_get_by_index(&init_net, jseg->ifindex); + spin_unlock_bh(&segments.lock); + j1939_segment_detach(netdev); + dev_put(netdev); + spin_lock_bh(&segments.lock); + } + spin_unlock_bh(&segments.lock); + + j1939_proc_remove("ecu"); + j1939_proc_remove("addr"); +} + + diff --git a/net/can/j1939/filter.c b/net/can/j1939/filter.c new file mode 100644 index 0000000..c10f7b9 --- /dev/null +++ b/net/can/j1939/filter.c @@ -0,0 +1,76 @@ +/* + * Copyright (c) 2010-2011 EIA Electronics + * + * Authors: + * Pieter Beyens + * Kurt Van Dijck + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation + */ + +#include +#include +#include +#include +#include + +#include "j1939-priv.h" + +static LIST_HEAD(filters); +DEFINE_RWLOCK(j1939_receiver_rwlock); /* protects the filter list */ + +struct filter { + struct list_head list; + void *vp; + void (*fn)(struct sk_buff *, void *); +}; + +int j1939_recv_distribute(struct sk_buff *skb) +{ + struct filter *filter; + + read_lock_bh(&j1939_receiver_rwlock); + list_for_each_entry(filter, &filters, list) + filter->fn(skb, filter->vp); + read_unlock_bh(&j1939_receiver_rwlock); + + return 0; +} + +int j1939_recv_add(void *vp, void (*fn)(struct sk_buff *, void *)) +{ + struct filter *f; + + f = kzalloc(sizeof(*f), GFP_KERNEL); + if (!f) + return -ENOMEM; + + f->vp = vp; + f->fn = fn; + + j1939_recv_suspend(); + list_add(&f->list, &filters); + j1939_recv_resume(); + return 0; +} + +int j1939_recv_remove(void *vp, void (*fn)(struct sk_buff *, void *)) +{ + struct filter *filter; + int found = 0; + + j1939_recv_suspend(); + list_for_each_entry(filter, &filters, list) { + if ((filter->vp == vp) && (filter->fn == fn)) { + list_del_init(&filter->list); + kfree(filter); + found = 1; + break; + } + } + j1939_recv_resume(); + return found ? 0 : -ENOENT; +} + diff --git a/net/can/j1939/j1939-priv.h b/net/can/j1939/j1939-priv.h new file mode 100644 index 0000000..3531ee5 --- /dev/null +++ b/net/can/j1939/j1939-priv.h @@ -0,0 +1,314 @@ +/* + * j1939-priv.h + * + * Copyright (c) 2010-2011 EIA Electronics + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + */ + +#ifndef _J1939_PRIV_H_ +#define _J1939_PRIV_H_ + +#include +#include +#include + +#include +#include +#include +#include +#include + +/* TODO: return ENETRESET on busoff. */ + +#define ECUFLAG_LOCAL 0x01 +#define ECUFLAG_REMOTE 0x02 + +#define PGN_REQUEST 0x0ea00 +#define PGN_ADDRESS_CLAIMED 0x0ee00 +#define PGN_MAX 0x3ffff + +#define SA_MAX_UNICAST 0xfd +/* + * j1939 devices + */ +struct j1939_ecu { + struct list_head list; + ktime_t rxtime; + name_t name; + int flags; + uint8_t sa; + /* + * atomic flag, set by ac_timer + * cleared/processed by segment's tasklet + * indicates that this ecu successfully claimed @sa as its address + * By communicating this from the ac_timer event to segments tasklet, + * a context locking problem is solved. All other 'ecu readers' + * must only lock with _bh, not with _irq. + */ + atomic_t ac_delay_expired; + struct hrtimer ac_timer; + struct kref kref; + struct j1939_segment *parent; +}; +#define to_j1939_ecu(x) container_of((x), struct j1939_ecu, dev) + +struct j1939_segment { + struct list_head ecus; /* + * local list entry in parent + * These allow irq (& softirq) context lookups on j1939 devices + * This approach (seperate lists) is done as the other 2 alternatives + * are not easier or even wrong + * 1) using the pure kobject methods involves mutexes, which are not + * allowed in irq context. + * 2) duplicating data structures would require a lot of synchronization + * code + * usage: + */ + rwlock_t lock; /* + * segments need a lock to protect the above list + */ + struct list_head flist; /* + * list entry for use by interrupt lookup routines + */ + int ifindex; + struct addr_ent { + ktime_t rxtime; + struct j1939_ecu *ecu; + int flags; + } ents[256]; + + /* + * tasklet to process ecu address claimed events. + * These events raise in hardirq context. Signalling the event + * and scheduling this tasklet successfully moves the + * event to softirq context + */ + struct tasklet_struct ac_task; + /* + * list of 256 ecu ptrs, that cache the claimed addresses. + * also protected by the above lock + * don't use directly, use j1939_ecu_set_address() instead + */ + struct kref kref; +}; +#define to_j1939_segment(x) container_of((x), struct j1939_segment, dev) + +extern void put_j1939_ecu(struct j1939_ecu *ecu); +extern void put_j1939_segment(struct j1939_segment *segment); +static inline struct j1939_ecu *get_j1939_ecu(struct j1939_ecu *dut) +{ + kref_get(&dut->kref); + return dut; +} +static inline struct j1939_segment *get_j1939_segment(struct j1939_segment *dut) +{ + kref_get(&dut->kref); + return dut; +} + +/* + * conversion function between (struct sock | struct sk_buff)->sk_priority + * from linux and j1939 priority field + */ +static inline int j1939_prio(int sk_priority) +{ + if (sk_priority < 0) + return 6; /* default */ + else if (sk_priority > 7) + return 0; + else + return 7 - sk_priority; +} +static inline int j1939_to_sk_priority(int j1939_prio) +{ + return 7 - j1939_prio; +} + +static inline int j1939_address_is_valid(uint8_t sa) +{ + return sa != J1939_NO_ADDR; +} + +static inline int j1939_address_is_unicast(uint8_t sa) +{ + return sa <= SA_MAX_UNICAST; +} + +static inline int pgn_is_pdu1(pgn_t pgn) +{ + /* ignore dp & res bits for this */ + return (pgn & 0xff00) < 0xf000; +} + +static inline int pgn_is_valid(pgn_t pgn) +{ + return pgn <= PGN_MAX; +} + +/* utility to correctly unregister a SA */ +static inline void j1939_ecu_remove_sa_locked(struct j1939_ecu *ecu) +{ + if (!j1939_address_is_unicast(ecu->sa)) + return; + if (ecu->parent->ents[ecu->sa].ecu == ecu) + ecu->parent->ents[ecu->sa].ecu = NULL; +} + +static inline void j1939_ecu_remove_sa(struct j1939_ecu *ecu) +{ + if (!j1939_address_is_unicast(ecu->sa)) + return; + write_lock_bh(&ecu->parent->lock); + j1939_ecu_remove_sa_locked(ecu); + write_unlock_bh(&ecu->parent->lock); +} + +extern int j1939_name_to_sa(uint64_t name, int ifindex); +extern struct j1939_ecu *j1939_ecu_find_by_addr(int sa, int ifindex); +extern struct j1939_ecu *j1939_ecu_find_by_name(name_t name, int ifindex); +/* find_by_name, with kref & read_lock taken */ +extern struct j1939_ecu *j1939_ecu_find_segment_default_tx( + int ifindex, name_t *pname, uint8_t *paddr); + +extern void j1939_put_promisc_receiver(int ifindex); +extern void j1939_get_promisc_receiver(int ifindex); + +extern int j1939_proc_add(const char *file, + int (*seq_show)(struct seq_file *sqf, void *v), + write_proc_t write); +extern void j1939_proc_remove(const char *file); + +extern const char j1939_procname[]; +/* j1939 printk */ +#define j1939_printk(level, ...) printk(level "J1939 " __VA_ARGS__) + +#define j1939_err(...) j1939_printk(KERN_ERR , __VA_ARGS__) +#define j1939_warning(...) j1939_printk(KERN_WARNING , __VA_ARGS__) +#define j1939_notice(...) j1939_printk(KERN_NOTICE , __VA_ARGS__) +#define j1939_info(...) j1939_printk(KERN_INFO , __VA_ARGS__) +#ifdef DEBUG +#define j1939_debug(...) j1939_printk(KERN_DEBUG , __VA_ARGS__) +#else +#define j1939_debug(...) +#endif + +struct sk_buff; + +/* control buffer of the sk_buff */ +struct j1939_sk_buff_cb { + int ifindex; + priority_t priority; + struct { + name_t name; + uint8_t addr; + int flags; + } src, dst; + pgn_t pgn; + int msg_flags; + /* for tx, MSG_SYN will be used to sync on sockets */ +}; +#define J1939_MSG_RESERVED MSG_SYN +#define J1939_MSG_SYNC MSG_SYN + +static inline int j1939cb_is_broadcast(const struct j1939_sk_buff_cb *cb) +{ + return (!cb->dst.name && (cb->dst.addr >= 0xff)); +} + +/* J1939 stack */ +enum { + j1939_level_can, + j1939_level_transport, + j1939_level_sky, +}; + +#define RESULT_STOP 1 +/* + * return RESULT_STOP when stack processing may stop. + * it is up to the stack entry itself to kfree_skb() the sk_buff + */ + +extern int j1939_send(struct sk_buff *, int level); +extern int j1939_recv(struct sk_buff *, int level); + +/* stack entries */ +extern int j1939_recv_promisc(struct sk_buff *); +extern int j1939_send_transport(struct sk_buff *); +extern int j1939_recv_transport(struct sk_buff *); +extern int j1939_send_address_claim(struct sk_buff *); +extern int j1939_recv_address_claim(struct sk_buff *); + +extern int j1939_recv_distribute(struct sk_buff *); + +/* network management */ +/* + * j1939_ecu_get_register + * 'create' & 'register' & 'get' new ecu + * when a matching ecu already exists, the behaviour depends + * on @return_existing. + * when @return_existing is 0, -EEXISTS is returned + * when @return_exsiting is 1, that ecu is 'get' & returned. + * @flags is only used when creating new ecu. + */ +extern struct j1939_ecu *j1939_ecu_get_register(name_t name, int ifindex, + int flags, int return_existing); +extern void j1939_ecu_unregister(struct j1939_ecu *); + +extern int j1939_segment_attach(struct net_device *); +extern int j1939_segment_detach(struct net_device *); + +extern int j1939_segment_register(struct net_device *); +extern void j1939_segment_unregister(struct j1939_segment *); +extern struct j1939_segment *j1939_segment_find(int ifindex); + +extern void j1939sk_netdev_event(int ifindex, int error_code); + +/* add/remove receiver */ +extern int j1939_recv_add(void *vp, void (*fn)(struct sk_buff *, void *)); +extern int j1939_recv_remove(void *vp, void (*fn)(struct sk_buff *, void *)); + +/* + * provide public access to this lock + * so sparse can verify the context balance + */ +extern rwlock_t j1939_receiver_rwlock; +static inline void j1939_recv_suspend(void) +{ + write_lock_bh(&j1939_receiver_rwlock); +} + +static inline void j1939_recv_resume(void) +{ + write_unlock_bh(&j1939_receiver_rwlock); +} + +/* locks the recv module */ +extern void j1939_recv_suspend(void); +extern void j1939_recv_resume(void); + +/* + * decrement pending skb for a j1939 socket + */ +extern void j1939_sock_pending_del(struct sock *sk); + +/* seperate module-init/modules-exit's */ +extern __init int j1939_proc_module_init(void); +extern __init int j1939bus_module_init(void); +extern __init int j1939sk_module_init(void); +extern __init int j1939tp_module_init(void); + +extern void j1939_proc_module_exit(void); +extern void j1939bus_module_exit(void); +extern void j1939sk_module_exit(void); +extern void j1939tp_module_exit(void); + +/* rtnetlink */ +extern const struct rtnl_af_ops j1939_rtnl_af_ops; +extern int j1939rtnl_new_addr(struct sk_buff *, struct nlmsghdr *, void *arg); +extern int j1939rtnl_del_addr(struct sk_buff *, struct nlmsghdr *, void *arg); +extern int j1939rtnl_dump_addr(struct sk_buff *, struct netlink_callback *); + +#endif /* _J1939_PRIV_H_ */ diff --git a/net/can/j1939/main.c b/net/can/j1939/main.c new file mode 100644 index 0000000..7edf843 --- /dev/null +++ b/net/can/j1939/main.c @@ -0,0 +1,458 @@ +/* + * Copyright (c) 2010-2011 EIA Electronics + * + * Authors: + * Kurt Van Dijck + * Pieter Beyens + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation + */ + +/* + * Core of can-j1939 that links j1939 to CAN. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include "j1939-priv.h" + +MODULE_DESCRIPTION("PF_CAN SAE J1939"); +MODULE_LICENSE("GPL v2"); +MODULE_AUTHOR("EIA Electronics (Kurt Van Dijck & Pieter Beyens)"); + +static struct { + struct notifier_block notifier; +} s; + +/* LOWLEVEL CAN interface */ + +/* CAN_HDR: #bytes before can_frame data part */ +#define CAN_HDR (offsetof(struct can_frame, data)) +/* CAN_FTR: #bytes beyond data part */ +#define CAN_FTR (sizeof(struct can_frame)-CAN_HDR-\ + sizeof(((struct can_frame *)0)->data)) + +static void j1939_recv_ecu_flags(struct sk_buff *skb, void *data) +{ + struct j1939_segment *jseg = data; + struct j1939_sk_buff_cb *cb = (void *)skb->cb; + struct addr_ent *paddr; + + if (!jseg) + return; + write_lock_bh(&jseg->lock); + if (j1939_address_is_unicast(cb->src.addr)) { + paddr = &jseg->ents[cb->src.addr]; + paddr->rxtime = ktime_get(); + if (0x0ee00 == cb->pgn) { + /* do not touch many things for Address claims */ + } else if (paddr->ecu) { + paddr->ecu->rxtime = paddr->rxtime; + cb->src.flags = paddr->ecu->flags; + } else { + if (!paddr->flags) + paddr->flags |= ECUFLAG_REMOTE; + cb->src.flags = paddr->flags; + } + } + + if (j1939_address_is_unicast(cb->dst.addr)) { + paddr = &jseg->ents[cb->dst.addr]; + if (paddr->ecu) + cb->dst.flags = paddr->ecu->flags; + else + cb->dst.flags = paddr->flags ?: ECUFLAG_REMOTE; + } + write_unlock_bh(&jseg->lock); +} + +/* lowest layer */ +static void j1939_can_recv(struct sk_buff *skb, void *data) +{ + int orig_len; + struct j1939_sk_buff_cb *sk_addr; + struct can_frame *msg; + uint8_t saved_cb[sizeof(skb->cb)]; + + BUILD_BUG_ON(sizeof(*sk_addr) > sizeof(skb->cb)); + /* + * get a pointer to the header of the skb + * the skb payload (pointer) is moved, so that the next skb_data + * returns the actual payload + */ + msg = (void *)skb->data; + orig_len = skb->len; + skb_pull(skb, CAN_HDR); + /* fix length, set to dlc, with 8 maximum */ + skb_trim(skb, min_t(uint8_t, msg->can_dlc, 8)); + + /* set addr */ + sk_addr = (struct j1939_sk_buff_cb *)skb->cb; + memcpy(saved_cb, sk_addr, sizeof(saved_cb)); + memset(sk_addr, 0, sizeof(*sk_addr)); + if (skb->dev) + sk_addr->ifindex = skb->dev->ifindex; + sk_addr->priority = (msg->can_id & 0x1c000000) >> 26; + sk_addr->src.addr = msg->can_id & 0xff; + sk_addr->pgn = (msg->can_id & 0x3ffff00) >> 8; + if (pgn_is_pdu1(sk_addr->pgn)) { + /* Type 1: with destination address */ + sk_addr->dst.addr = sk_addr->pgn & 0xff; + /* normalize pgn: strip dst address */ + sk_addr->pgn &= 0x3ff00; + } else { + /* set broadcast address */ + sk_addr->dst.addr = J1939_NO_ADDR; + } + j1939_recv_ecu_flags(skb, data); + j1939_recv(skb, j1939_level_can); + + /* restore the original skb, should always work */ + skb_push(skb, CAN_HDR); + /* no safety check, it just restores the skbuf's contents */ + __skb_trim(skb, orig_len); + memcpy(sk_addr, saved_cb, sizeof(saved_cb)); +} + +static int j1939_send_can(struct sk_buff *skb) +{ + int ret, dlc; + canid_t canid; + struct j1939_sk_buff_cb *sk_addr; + struct net_device *netdev = NULL; + struct can_frame *msg; + + dlc = skb->len; + if (dlc > 8) + return -EMSGSIZE; + ret = pskb_expand_head(skb, SKB_DATA_ALIGN(CAN_HDR), + CAN_FTR + (8-dlc), GFP_ATOMIC); + if (ret < 0) + return ret; + + msg = (void *)skb_push(skb, CAN_HDR); + BUG_ON(!msg); + /* make it a full can frame */ + skb_put(skb, CAN_FTR + (8 - dlc)); + + sk_addr = (struct j1939_sk_buff_cb *)skb->cb; + canid = CAN_EFF_FLAG | + (sk_addr->src.addr & 0xff) | + ((sk_addr->priority & 0x7) << 26); + if (pgn_is_pdu1(sk_addr->pgn)) + canid |= ((sk_addr->pgn & 0x3ff00) << 8) | + ((sk_addr->dst.addr & 0xff) << 8); + else + canid |= ((sk_addr->pgn & 0x3ffff) << 8); + + msg->can_id = canid; + msg->can_dlc = dlc; + + /* set net_device */ + ret = -ENODEV; + if (!skb->dev) { + if (!sk_addr->ifindex) + goto failed; + netdev = dev_get_by_index(&init_net, sk_addr->ifindex); + if (!netdev) + goto failed; + skb->dev = netdev; + } + + /* fix the 'always free' policy of can_send */ + skb = skb_get(skb); + ret = can_send(skb, 1); + if (!ret) { + /* free when can_send succeeded */ + kfree_skb(skb); + /* is this necessary ? */ + ret = RESULT_STOP; + } +failed: + if (netdev) + dev_put(netdev); + return ret; +} + +static int j1939_send_normalize(struct sk_buff *skb) +{ + struct j1939_sk_buff_cb *cb = (void *)skb->cb; + struct j1939_segment *jseg; + struct addr_ent *paddr; + struct j1939_ecu *ecu; + int ret = 0; + + /* apply sanity checks */ + cb->pgn &= (pgn_is_pdu1(cb->pgn)) ? 0x3ff00 : 0x3ffff; + if (cb->priority > 7) + cb->priority = 6; + + /* verify source */ + if (!cb->ifindex) + return -ENETUNREACH; + jseg = j1939_segment_find(cb->ifindex); + if (!jseg) + return -ENETUNREACH; + read_lock_bh(&jseg->lock); + /* verify source */ + if (cb->src.name) { + ecu = j1939_ecu_find_by_name(cb->src.name, cb->ifindex); + cb->src.flags = ecu ? ecu->flags : 0; + if (ecu) + put_j1939_ecu(ecu); + } else if (j1939_address_is_unicast(cb->src.addr)) { + paddr = &jseg->ents[cb->src.addr]; + cb->src.flags = paddr->flags; + } else if (cb->src.addr == J1939_IDLE_ADDR) { + /* allow always */ + cb->src.flags = ECUFLAG_LOCAL; + } else { + /* J1939_NO_ADDR */ + cb->src.flags = 0; + } + if (cb->src.flags & ECUFLAG_REMOTE) { + ret = -EREMOTE; + goto failed; + } else if (!(cb->src.flags & ECUFLAG_LOCAL)) { + ret = -EADDRNOTAVAIL; + goto failed; + } + + /* verify destination */ + if (cb->dst.name) { + ecu = j1939_ecu_find_by_name(cb->dst.name, cb->ifindex); + if (!ecu) { + ret = -EADDRNOTAVAIL; + goto failed; + } + cb->dst.flags = ecu->flags; + put_j1939_ecu(ecu); + } else if (cb->dst.addr == J1939_IDLE_ADDR) { + /* not a valid destination */ + ret = -EADDRNOTAVAIL; + goto failed; + } else if (j1939_address_is_unicast(cb->dst.addr)) { + paddr = &jseg->ents[cb->dst.addr]; + cb->dst.flags = paddr->flags; + } else { + cb->dst.flags = 0; + } + + ret = 0; +failed: + read_unlock_bh(&jseg->lock); + put_j1939_segment(jseg); + return ret; +} + +/* TOPLEVEL interface */ +int j1939_recv(struct sk_buff *skb, int level) +{ + int ret; + + /* this stack operates with fallthrough switch statement */ + switch (level) { + default: + WARN_ONCE(1, "%s: unsupported level %i\n", __func__, level); + return 0; + case j1939_level_can: + ret = j1939_recv_address_claim(skb); + if (unlikely(ret)) + break; + ret = j1939_recv_promisc(skb); + if (unlikely(ret)) + break; + ret = j1939_recv_transport(skb); + if (unlikely(ret)) + break; + case j1939_level_transport: + case j1939_level_sky: + ret = j1939_recv_distribute(skb); + break; + } + if (ret == RESULT_STOP) + return 0; + return ret; + +} +EXPORT_SYMBOL_GPL(j1939_recv); + +int j1939_send(struct sk_buff *skb, int level) +{ + int ret; + struct sock *sk = NULL; + + /* this stack operates with fallthrough switch statement */ + switch (level) { + default: + WARN_ONCE(1, "%s: unsupported level %i\n", __func__, level); + case j1939_level_sky: + sk = skb->sk; + if (sk) + sock_hold(sk); + ret = j1939_send_normalize(skb); + if (unlikely(ret)) + break; + ret = j1939_send_transport(skb); + if (unlikely(ret)) + break; + case j1939_level_transport: + ret = j1939_send_address_claim(skb); + if (unlikely(ret)) + break; + case j1939_level_can: + ret = j1939_send_can(skb); + if (RESULT_STOP == ret) + /* don't mark as stopped, it can't be better */ + ret = 0; + break; + } + if (ret == RESULT_STOP) + ret = 0; + else if (!ret && sk) + j1939_sock_pending_del(sk); + if (sk) + sock_put(sk); + return ret; + +} +EXPORT_SYMBOL_GPL(j1939_send); + +/* NETDEV MANAGEMENT */ + +#define J1939_CAN_ID CAN_EFF_FLAG +#define J1939_CAN_MASK (CAN_EFF_FLAG | CAN_RTR_FLAG) +int j1939_segment_attach(struct net_device *netdev) +{ + int ret; + struct j1939_segment *jseg; + + if (!netdev) + return -ENODEV; + if (netdev->type != ARPHRD_CAN) + return -EAFNOSUPPORT; + + ret = j1939_segment_register(netdev); + if (ret < 0) + goto fail_register; + jseg = j1939_segment_find(netdev->ifindex); + ret = can_rx_register(netdev, J1939_CAN_ID, J1939_CAN_MASK, + j1939_can_recv, jseg, "j1939"); + if (ret < 0) + goto fail_can_rx; + return 0; + +fail_can_rx: + j1939_segment_unregister(jseg); + put_j1939_segment(jseg); +fail_register: + return ret; +} + +int j1939_segment_detach(struct net_device *netdev) +{ + struct j1939_segment *jseg; + + BUG_ON(!netdev); + jseg = j1939_segment_find(netdev->ifindex); + if (!jseg) + return -EHOSTDOWN; + can_rx_unregister(netdev, J1939_CAN_ID, J1939_CAN_MASK, + j1939_can_recv, jseg); + j1939_segment_unregister(jseg); + put_j1939_segment(jseg); + j1939sk_netdev_event(netdev->ifindex, EHOSTDOWN); + return 0; +} + +static int j1939_notifier(struct notifier_block *nb, + unsigned long msg, void *data) +{ + struct net_device *netdev = (struct net_device *)data; + struct j1939_segment *jseg; + + if (!net_eq(dev_net(netdev), &init_net)) + return NOTIFY_DONE; + + if (netdev->type != ARPHRD_CAN) + return NOTIFY_DONE; + + switch (msg) { + case NETDEV_UNREGISTER: + jseg = j1939_segment_find(netdev->ifindex); + if (!jseg) + break; + j1939_segment_unregister(jseg); + j1939sk_netdev_event(netdev->ifindex, ENODEV); + break; + + case NETDEV_DOWN: + j1939sk_netdev_event(netdev->ifindex, ENETDOWN); + break; + } + + return NOTIFY_DONE; +} + +/* MODULE interface */ + +static __init int j1939_module_init(void) +{ + int ret; + + pr_info("can: SAE J1939\n"); + + ret = j1939_proc_module_init(); + if (ret < 0) + goto fail_proc; + + s.notifier.notifier_call = j1939_notifier; + register_netdevice_notifier(&s.notifier); + + ret = j1939bus_module_init(); + if (ret < 0) + goto fail_bus; + ret = j1939sk_module_init(); + if (ret < 0) + goto fail_sk; + ret = j1939tp_module_init(); + if (ret < 0) + goto fail_tp; + return 0; + + j1939tp_module_exit(); +fail_tp: + j1939sk_module_exit(); +fail_sk: + j1939bus_module_exit(); +fail_bus: + unregister_netdevice_notifier(&s.notifier); + + j1939_proc_module_exit(); +fail_proc: + return ret; +} + +static __exit void j1939_module_exit(void) +{ + j1939tp_module_exit(); + j1939sk_module_exit(); + j1939bus_module_exit(); + + unregister_netdevice_notifier(&s.notifier); + + j1939_proc_module_exit(); +} + +module_init(j1939_module_init); +module_exit(j1939_module_exit); diff --git a/net/can/j1939/proc.c b/net/can/j1939/proc.c new file mode 100644 index 0000000..76acfa0 --- /dev/null +++ b/net/can/j1939/proc.c @@ -0,0 +1,104 @@ +/* + * Copyright (c) 2010-2011 EIA Electronics + * + * Authors: + * Kurt Van Dijck + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation + */ + +#include +#include +#include +#include +#include + +#include "j1939-priv.h" + +const char j1939_procname[] = "can-j1939"; + +static struct proc_dir_entry *rootdir; + +static int j1939_proc_open(struct inode *inode, struct file *file) +{ + struct proc_dir_entry *pde = PDE(inode); + int (*fn)(struct seq_file *sqf, void *v) = pde->data; + + return single_open(file, fn, pde); +} + +/* copied from fs/proc/generic.c */ +static ssize_t +proc_file_write(struct file *file, const char __user *buffer, + size_t count, loff_t *ppos) +{ + struct inode *inode = file->f_path.dentry->d_inode; + struct proc_dir_entry *dp; + + dp = PDE(inode); + + if (!dp->write_proc) + return -EIO; + + /* FIXME: does this routine need ppos? probably... */ + return dp->write_proc(file, buffer, count, dp->data); +} + +static const struct file_operations j1939_proc_ops = { + .owner = THIS_MODULE, + .open = j1939_proc_open, + .read = seq_read, + .write = proc_file_write, + .llseek = seq_lseek, + .release = single_release, +}; + +int j1939_proc_add(const char *file, + int (*seq_show)(struct seq_file *sqf, void *v), + write_proc_t write) +{ + struct proc_dir_entry *pde; + int mode = 0; + + if (seq_show) + mode |= 0444; + if (write) + mode |= 0200; + + if (!rootdir) + return -ENODEV; + pde = proc_create(file, mode, rootdir, &j1939_proc_ops); + if (!pde) + goto fail_create; + pde->data = seq_show; + pde->write_proc = write; + return 0; + +fail_create: + return -ENOENT; +} +EXPORT_SYMBOL(j1939_proc_add); + +void j1939_proc_remove(const char *file) +{ + remove_proc_entry(file, rootdir); +} +EXPORT_SYMBOL(j1939_proc_remove); + +__init int j1939_proc_module_init(void) +{ + /* create /proc/net/can directory */ + rootdir = proc_mkdir(j1939_procname, init_net.proc_net); + if (!rootdir) + return -EINVAL; + return 0; +} + +void j1939_proc_module_exit(void) +{ + if (rootdir) + proc_net_remove(&init_net, j1939_procname); +} + diff --git a/net/can/j1939/promisc.c b/net/can/j1939/promisc.c new file mode 100644 index 0000000..14be755 --- /dev/null +++ b/net/can/j1939/promisc.c @@ -0,0 +1,43 @@ +/* + * Copyright (c) 2010-2011 EIA Electronics + * + * Authors: + * Kurt Van Dijck + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation + */ + +#include +#include +#include +#include "j1939-priv.h" + +static atomic_t n_promisc = ATOMIC_INIT(0); + +void j1939_get_promisc_receiver(int ifindex) +{ + atomic_inc(&n_promisc); +} +EXPORT_SYMBOL_GPL(j1939_get_promisc_receiver); + +void j1939_put_promisc_receiver(int ifindex) +{ + atomic_dec(&n_promisc); +} +EXPORT_SYMBOL_GPL(j1939_put_promisc_receiver); + +int j1939_recv_promisc(struct sk_buff *skb) +{ + struct j1939_sk_buff_cb *cb = (void *)skb->cb; + + if ((cb->src.flags & ECUFLAG_REMOTE) && + (cb->dst.flags & ECUFLAG_REMOTE)) { + if (!atomic_read(&n_promisc)) + /* stop receive path */ + return RESULT_STOP; + } + return 0; +} + diff --git a/net/can/j1939/rtnl.c b/net/can/j1939/rtnl.c new file mode 100644 index 0000000..851060f --- /dev/null +++ b/net/can/j1939/rtnl.c @@ -0,0 +1,308 @@ +/* + * Copyright (c) 2011 EIA Electronics + * + * Authors: + * Kurt Van Dijck + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation + */ + +/* + * j1939-rtnl.c - netlink addressing interface + */ + +#include +#include +#include +#include +#include + +#include "j1939-priv.h" + +static const struct nla_policy j1939_ifa_policy[IFA_J1939_MAX] = { + [IFA_J1939_ADDR] = { .type = NLA_U8, }, + [IFA_J1939_NAME] = { .type = NLA_U64, }, +}; + +int j1939rtnl_del_addr(struct sk_buff *skb, struct nlmsghdr *nlh, void *arg) +{ + int ret; + struct ifaddrmsg *ifm; + struct j1939_segment *jseg; + uint8_t jaddr = J1939_NO_ADDR; + uint64_t jname = J1939_NO_NAME; + + struct nlattr *nla, *tb[IFA_J1939_MAX]; + + if (!net_eq(sock_net(skb->sk), &init_net)) + return -EINVAL; + + nla = nlmsg_find_attr(nlh, sizeof(*ifm), IFA_LOCAL); + if (!nla) + return -EINVAL; + + nla_parse_nested(tb, IFA_J1939_MAX-1, nla, j1939_ifa_policy); + if (tb[IFA_J1939_ADDR]) + jaddr = nla_get_u8(tb[IFA_J1939_ADDR]); + if (tb[IFA_J1939_NAME]) + jname = be64_to_cpu(nla_get_u64(tb[IFA_J1939_NAME])); + + ifm = nlmsg_data(nlh); + jseg = j1939_segment_find(ifm->ifa_index); + if (!jseg) + return -EHOSTDOWN; + + ret = 0; + if (j1939_address_is_unicast(jaddr)) { + struct addr_ent *ent; + + ent = &jseg->ents[jaddr]; + write_lock_bh(&jseg->lock); + if (!ent->flags) + ret = -EADDRNOTAVAIL; + else if (!(ent->flags & ECUFLAG_LOCAL)) + ret = -EREMOTE; + else + ent->flags = 0; + write_unlock_bh(&jseg->lock); + } else if (jname) { + struct j1939_ecu *ecu; + + ecu = j1939_ecu_find_by_name(jname, ifm->ifa_index); + if (ecu) { + if (ecu->flags & ECUFLAG_LOCAL) { + j1939_ecu_unregister(ecu); + put_j1939_ecu(ecu); + } else { + ret = -EREMOTE; + } + } else { + ret = -ENODEV; + } + } + put_j1939_segment(jseg); + return ret; +} + +int j1939rtnl_new_addr(struct sk_buff *skb, struct nlmsghdr *nlh, void *arg) +{ + struct ifaddrmsg *ifm; + struct j1939_segment *jseg; + uint8_t jaddr = J1939_NO_ADDR; + uint64_t jname = J1939_NO_NAME; + struct addr_ent *ent; + int ret; + struct nlattr *nla, *tb[IFA_J1939_MAX]; + + if (!net_eq(sock_net(skb->sk), &init_net)) + return -EINVAL; + + nla = nlmsg_find_attr(nlh, sizeof(*ifm), IFA_LOCAL); + if (!nla) + return -EINVAL; + + ifm = nlmsg_data(nlh); + jseg = j1939_segment_find(ifm->ifa_index); + if (!jseg) + return -EHOSTDOWN; + + nla_parse_nested(tb, IFA_J1939_MAX-1, nla, j1939_ifa_policy); + if (tb[IFA_J1939_ADDR]) + jaddr = nla_get_u8(tb[IFA_J1939_ADDR]); + if (tb[IFA_J1939_NAME]) + jname = be64_to_cpu(nla_get_u64(tb[IFA_J1939_NAME])); + + + ret = 0; + if (j1939_address_is_unicast(jaddr)) { + ent = &jseg->ents[jaddr]; + write_lock_bh(&jseg->lock); + if ((ent->ecu && (ent->ecu->flags & ECUFLAG_REMOTE)) || + (ent->flags & ECUFLAG_REMOTE)) + ret = -EREMOTE; + else + ent->flags |= ECUFLAG_LOCAL; + write_unlock_bh(&jseg->lock); + } else if (jname) { + struct j1939_ecu *ecu; + + ecu = j1939_ecu_get_register(jname, ifm->ifa_index, + ECUFLAG_LOCAL, 0); + if (IS_ERR(ecu)) + ret = PTR_ERR(ecu); + else + put_j1939_ecu(ecu); + } + put_j1939_segment(jseg); + return ret; +} + +static int j1939rtnl_fill_ifaddr(struct sk_buff *skb, int ifindex, + uint8_t addr, uint64_t name, int j1939_flags, + u32 pid, u32 seq, int event, unsigned int flags) +{ + struct ifaddrmsg *ifm; + struct nlmsghdr *nlh; + struct nlattr *nla; + + nlh = nlmsg_put(skb, pid, seq, event, sizeof(*ifm), flags); + if (nlh == NULL) + return -EMSGSIZE; + + ifm = nlmsg_data(nlh); + ifm->ifa_family = AF_CAN; + ifm->ifa_prefixlen = CAN_J1939; + ifm->ifa_flags = name ? 0 : IFA_F_PERMANENT; + ifm->ifa_scope = RT_SCOPE_LINK; + ifm->ifa_index = ifindex; + + nla = nla_nest_start(skb, IFA_LOCAL); + if (j1939_address_is_unicast(addr)) + NLA_PUT_U8(skb, IFA_J1939_ADDR, addr); + if (name) + NLA_PUT_U64(skb, IFA_J1939_NAME, cpu_to_be64(name)); + nla_nest_end(skb, nla); + + return nlmsg_end(skb, nlh); + +nla_put_failure: + nlmsg_cancel(skb, nlh); + return -EMSGSIZE; +} + +int j1939rtnl_dump_addr(struct sk_buff *skb, struct netlink_callback *cb) +{ + int ndev, addr, ret, sa; + struct net_device *netdev; + struct j1939_segment *jseg; + struct j1939_ecu *ecu; + struct addr_ent *ent; + + if (!net_eq(sock_net(skb->sk), &init_net)) + return 0; + + ndev = 0; + for_each_netdev(&init_net, netdev) { + ++ndev; + if (ndev < cb->args[1]) + continue; + if (netdev->type != ARPHRD_CAN) + continue; + + jseg = j1939_segment_find(netdev->ifindex); + if (!jseg) + continue; + + read_lock_bh(&jseg->lock); + for (addr = cb->args[2]; addr < J1939_IDLE_ADDR; ++addr) { + ent = &jseg->ents[addr]; + if (!ent->flags) + continue; + ret = j1939rtnl_fill_ifaddr(skb, netdev->ifindex, addr, + 0, ent->flags, NETLINK_CB(cb->skb).pid, + cb->nlh->nlmsg_seq, RTM_NEWADDR, + NLM_F_MULTI); + if (ret < 0) { + read_unlock_bh(&jseg->lock); + goto done; + } + cb->args[2] = addr + 1; + } + + if (addr > J1939_IDLE_ADDR) + addr = J1939_IDLE_ADDR; + list_for_each_entry(ecu, &jseg->ecus, list) { + if (addr++ < cb->args[2]) + continue; + if (!(ecu->flags & ECUFLAG_LOCAL)) + continue; + sa = ecu->sa; + if (ecu->parent->ents[sa].ecu != ecu) + sa = J1939_IDLE_ADDR; + ret = j1939rtnl_fill_ifaddr(skb, netdev->ifindex, + sa, ecu->name, ecu->flags, + NETLINK_CB(cb->skb).pid, + cb->nlh->nlmsg_seq, RTM_NEWADDR, + NLM_F_MULTI); + if (ret < 0) { + read_unlock_bh(&jseg->lock); + goto done; + } + cb->args[2] = addr; + } + read_unlock_bh(&jseg->lock); + /* reset first address for device */ + cb->args[2] = 0; + } + ++ndev; +done: + cb->args[1] = ndev; + + return skb->len; +} + +/* + * rtnl_link_ops + */ + +static const struct nla_policy j1939_ifla_policy[IFLA_J1939_MAX] = { + [IFLA_J1939_ENABLE] = { .type = NLA_U8, }, +}; + +static size_t j1939_get_link_af_size(const struct net_device *dev) +{ + return nla_policy_len(j1939_ifla_policy, IFLA_J1939_MAX-1); +} + +static int j1939_validate_link_af(const struct net_device *dev, + const struct nlattr *nla) +{ + return nla_validate_nested(nla, IFLA_J1939_MAX-1, j1939_ifla_policy); +} + +static int j1939_fill_link_af(struct sk_buff *skb, const struct net_device *dev) +{ + struct j1939_segment *jseg; + + if (!dev) + return -ENODEV; + jseg = j1939_segment_find(dev->ifindex); + if (jseg) + put_j1939_segment(jseg); + NLA_PUT_U8(skb, IFLA_J1939_ENABLE, jseg ? 1 : 0); + return 0; + +nla_put_failure: + return -EMSGSIZE; +} + +static int j1939_set_link_af(struct net_device *dev, const struct nlattr *nla) +{ + int ret; + struct nlattr *tb[IFLA_J1939_MAX]; + + ret = nla_parse_nested(tb, IFLA_J1939_MAX-1, nla, j1939_ifla_policy); + if (ret < 0) + return ret; + + if (tb[IFLA_J1939_ENABLE]) { + if (nla_get_u8(tb[IFLA_J1939_ENABLE])) + ret = j1939_segment_attach(dev); + else + ret = j1939_segment_detach(dev); + if (ret < 0) + return ret; + } + return 0; +} + +const struct rtnl_af_ops j1939_rtnl_af_ops = { + .family = AF_CAN, + .fill_link_af = j1939_fill_link_af, + .get_link_af_size = j1939_get_link_af_size, + .validate_link_af = j1939_validate_link_af, + .set_link_af = j1939_set_link_af, +}; + diff --git a/net/can/j1939/socket.c b/net/can/j1939/socket.c new file mode 100644 index 0000000..e41cb31 --- /dev/null +++ b/net/can/j1939/socket.c @@ -0,0 +1,969 @@ +/* + * Copyright (c) 2010-2011 EIA Electronics + * + * Authors: + * Kurt Van Dijck + * Pieter Beyens + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include "j1939-priv.h" + +struct j1939_sock { + struct sock sk; /* must be first to skip with memset */ + struct list_head list; + + int state; + #define JSK_BOUND BIT(0) + #define JSK_CONNECTED BIT(1) + #define PROMISC BIT(2) + #define RECV_OWN BIT(3) + + struct { + name_t src, dst; + pgn_t pgn; + + uint8_t sa, da; + } addr; + + struct j1939_filter *filters; + int nfilters; + + int skb_pending; + spinlock_t lock; + wait_queue_head_t waitq; +}; + +static inline struct j1939_sock *j1939_sk(const struct sock *sk) +{ + return container_of(sk, struct j1939_sock, sk); +} + +/* skb_pending issues */ +static inline int j1939_sock_pending_add_first(struct sock *sk) +{ + int saved; + struct j1939_sock *jsk = j1939_sk(sk); + + spin_lock_bh(&jsk->lock); + if (!jsk->skb_pending) { + ++jsk->skb_pending; + saved = 1; + } else + saved = 0; + spin_unlock_bh(&jsk->lock); + return saved; +} + +static inline void j1939_sock_pending_add(struct sock *sk) +{ + struct j1939_sock *jsk = j1939_sk(sk); + + spin_lock_bh(&jsk->lock); + ++jsk->skb_pending; + spin_unlock_bh(&jsk->lock); +} + +void j1939_sock_pending_del(struct sock *sk) +{ + struct j1939_sock *jsk = j1939_sk(sk); + int saved; + + spin_lock_bh(&jsk->lock); + --jsk->skb_pending; + saved = jsk->skb_pending; + spin_unlock_bh(&jsk->lock); + if (!saved) + wake_up(&jsk->waitq); +} + + +static inline int j1939_no_address(const struct sock *sk) +{ + const struct j1939_sock *jsk = j1939_sk(sk); + return (jsk->addr.sa == J1939_NO_ADDR) && !jsk->addr.src; +} + +/* + * list of sockets + */ +static struct { + struct mutex lock; + struct list_head socks; +} s; + +/* matches skb control buffer (addr) with a j1939 filter */ +static inline int packet_match(const struct j1939_sk_buff_cb *cb, + const struct j1939_filter *f, int nfilter) +{ + if (!nfilter) + /* receive all when no filters are assigned */ + return 1; + /* + * Filters relying on the addr for static addressing _should_ get + * packets from dynamic addressed ECU's too if they match their SA. + * Sockets using dynamic addressing in their filters should not set it. + */ + for (; nfilter; ++f, --nfilter) { + if ((cb->pgn & f->pgn_mask) != (f->pgn & f->pgn_mask)) + continue; + if ((cb->src.addr & f->addr_mask) != (f->addr & f->addr_mask)) + continue; + if ((cb->src.name & f->name_mask) != (f->name & f->name_mask)) + continue; + return 1; + } + return 0; +} + +/* + * callback per socket, called from filter infrastructure + */ +static void j1939sk_recv_skb(struct sk_buff *oskb, void *data) +{ + struct sk_buff *skb; + struct j1939_sock *jsk = (struct j1939_sock *)data; + struct j1939_sk_buff_cb *cb = (void *)oskb->cb; + + if (jsk->sk.sk_bound_dev_if && (jsk->sk.sk_bound_dev_if != cb->ifindex)) + /* this socket does not take packets from this iface */ + return; + if (!(jsk->state & PROMISC)) { + if (cb->dst.flags & ECUFLAG_REMOTE) + /* + * this msg was destined for an ECU associated + * with this socket + */ + return; + if (jsk->addr.src) { + if (cb->dst.name && + (cb->dst.name != jsk->addr.src)) + /* + * the msg is not destined for the name + * that the socket is bound to + */ + return; + } else if (j1939_address_is_unicast(jsk->addr.sa)) { + if (j1939_address_is_unicast(cb->dst.addr) && + (cb->dst.addr != jsk->addr.sa)) + /* + * the msg is not destined for the name + * that the socket is bound to + */ + return; + } + } + + if ((oskb->sk == &jsk->sk) && !(jsk->state & RECV_OWN)) + /* own message */ + return; + + if (!packet_match(cb, jsk->filters, jsk->nfilters)) + return; + + skb = skb_clone(oskb, GFP_ATOMIC); + if (!skb) { + j1939_warning("skb clone failed\n"); + return; + } + cb = (void *)skb->cb; + cb->msg_flags &= ~(MSG_DONTROUTE | MSG_CONFIRM); + if (oskb->sk) + cb->msg_flags |= MSG_DONTROUTE; + if (oskb->sk == &jsk->sk) + cb->msg_flags |= MSG_CONFIRM; + + skb->sk = &jsk->sk; + if (sock_queue_rcv_skb(&jsk->sk, skb) < 0) + kfree_skb(skb); +} + +static int j1939sk_init(struct sock *sk) +{ + struct j1939_sock *jsk = j1939_sk(sk); + + INIT_LIST_HEAD(&jsk->list); + spin_lock_init(&jsk->lock); + init_waitqueue_head(&jsk->waitq); + jsk->sk.sk_priority = j1939_to_sk_priority(6); + jsk->sk.sk_reuse = 1; /* per default */ + jsk->addr.sa = J1939_NO_ADDR; + jsk->addr.da = J1939_NO_ADDR; + return 0; +} + +/* + * helper: return <0 for error, >0 for error to notify + */ +static int j1939sk_bind_netdev_helper(struct socket *sock) +{ + struct j1939_sock *jsk = j1939_sk(sock->sk); + int ret; + struct net_device *netdev; + struct j1939_segment *jseg; + + if (!jsk->sk.sk_bound_dev_if) + return 0; + ret = 0; + + netdev = dev_get_by_index(&init_net, jsk->sk.sk_bound_dev_if); + if (!netdev) { + ret = -ENODEV; + goto fail_netdev; + } + + /* no need to test for CAN device, + * implicitely done by j1939_segment + */ + jseg = j1939_segment_find(netdev->ifindex); + if (!jseg) { + ret = -EHOSTDOWN; + goto fail_segment; + } + + if (!(netdev->flags & IFF_UP)) { + sock->sk->sk_err = ENETDOWN; + sock->sk->sk_error_report(sock->sk); + } + put_j1939_segment(jseg); +fail_segment: + dev_put(netdev); +fail_netdev: + return ret; +} + +static int j1939sk_bind(struct socket *sock, struct sockaddr *uaddr, int len) +{ + struct sockaddr_can *addr = (struct sockaddr_can *)uaddr; + struct j1939_sock *jsk = j1939_sk(sock->sk); + struct j1939_ecu *ecu = NULL; + int ret, old_state; + + if (len < required_size(can_addr.j1939, *addr)) + return -EINVAL; + if (addr->can_family != AF_CAN) + return -EINVAL; + + /* lock s.lock first, to avoid circular lock dependancy */ + mutex_lock(&s.lock); + lock_sock(sock->sk); + if (jsk->state & JSK_BOUND) { + ret = -EBUSY; + if (addr->can_ifindex && + (addr->can_ifindex != jsk->sk.sk_bound_dev_if)) + goto fail_locked; + /* + * do not allow to change addres after first bind(), + * (it would require updating the j1939_ecu list) + * but allow the change SA when using dynaddr, + * and allow to change PGN + */ + if (!jsk->addr.src || + (jsk->addr.src != addr->can_addr.j1939.name) || + (jsk->addr.pgn != addr->can_addr.j1939.pgn)) + goto fail_locked; + /* set to be able to send address claims */ + jsk->addr.sa = addr->can_addr.j1939.addr; + /* since this socket is bound already, we can skip a lot */ + release_sock(sock->sk); + mutex_unlock(&s.lock); + return 0; + } + + /* do netdev */ + if (jsk->sk.sk_bound_dev_if && addr->can_ifindex && + (jsk->sk.sk_bound_dev_if != addr->can_ifindex)) { + ret = -EBADR; + goto fail_locked; + } + if (!jsk->sk.sk_bound_dev_if) + jsk->sk.sk_bound_dev_if = addr->can_ifindex; + + ret = j1939sk_bind_netdev_helper(sock); + if (ret < 0) + goto fail_locked; + + /* bind name/addr */ + if (addr->can_addr.j1939.name) { + ecu = j1939_ecu_find_by_name(addr->can_addr.j1939.name, + jsk->sk.sk_bound_dev_if); + if (!ecu) { + ret = -EADDRNOTAVAIL; + goto fail_locked; + } else if (ecu->flags & ECUFLAG_REMOTE) { + ret = -EREMOTE; + goto fail_with_ecu; + } else if (jsk->sk.sk_bound_dev_if != ecu->parent->ifindex) { + ret = -EHOSTUNREACH; + goto fail_with_ecu; + } + jsk->addr.src = ecu->name; + jsk->addr.sa = addr->can_addr.j1939.addr; + } else if (j1939_address_is_unicast(addr->can_addr.j1939.addr)) { + struct j1939_segment *jseg; + struct addr_ent *paddr; + int flags; + + /* static addressing, netdev is required */ + if (!jsk->sk.sk_bound_dev_if) { + ret = -EINVAL; + goto fail_locked; + } + jseg = j1939_segment_find(jsk->sk.sk_bound_dev_if); + if (!jseg) { + ret = -ENETUNREACH; + goto fail_locked; + } + paddr = &jseg->ents[addr->can_addr.j1939.addr]; + ret = 0; + read_lock_bh(&jseg->lock); + flags = paddr->flags; + read_unlock_bh(&jseg->lock); + put_j1939_segment(jseg); + if (!(flags & ECUFLAG_LOCAL)) { + ret = -EADDRNOTAVAIL; + goto fail_locked; + } + jsk->addr.sa = addr->can_addr.j1939.addr; + } else if (addr->can_addr.j1939.addr == J1939_IDLE_ADDR) { + /* static addressing, netdev is required */ + if (!jsk->sk.sk_bound_dev_if) { + ret = -EINVAL; + goto fail_locked; + } + jsk->addr.sa = addr->can_addr.j1939.addr; + } else { + /* no name, no addr */ + } + + /* set default transmit pgn/priority */ + jsk->addr.pgn = addr->can_addr.j1939.pgn; + + old_state = jsk->state; + jsk->state |= JSK_BOUND; + + if (!(old_state & (JSK_BOUND | JSK_CONNECTED))) { + list_add_tail(&jsk->list, &s.socks); + j1939_recv_add(jsk, j1939sk_recv_skb); + } + + ret = 0; + +fail_with_ecu: + if (ecu && !IS_ERR(ecu)) + put_j1939_ecu(ecu); +fail_locked: + release_sock(sock->sk); + mutex_unlock(&s.lock); + return ret; +} + +static int j1939sk_connect(struct socket *sock, struct sockaddr *uaddr, + int len, int flags) +{ + int ret, old_state; + struct sockaddr_can *addr = (struct sockaddr_can *)uaddr; + struct j1939_sock *jsk = j1939_sk(sock->sk); + struct j1939_ecu *ecu; + int ifindex; + + if (!uaddr) + return -EDESTADDRREQ; + + if (len < required_size(can_addr.j1939, *addr)) + return -EINVAL; + if (addr->can_family != AF_CAN) + return -EINVAL; + + mutex_lock(&s.lock); + lock_sock(sock->sk); + if (jsk->state & JSK_CONNECTED) { + ret = -EISCONN; + goto fail_locked; + } + + ifindex = jsk->sk.sk_bound_dev_if; + if (ifindex && addr->can_ifindex && (ifindex != addr->can_ifindex)) { + ret = -ECONNREFUSED; + goto fail_locked; + } + if (!ifindex) + ifindex = addr->can_ifindex; + + /* lookup destination */ + if (addr->can_addr.j1939.name) { + ecu = j1939_ecu_find_by_name(addr->can_addr.j1939.name, + ifindex); + if (!ecu) { + ret = -EADDRNOTAVAIL; + goto fail_locked; + } + if (ifindex && (ifindex != ecu->parent->ifindex)) { + ret = -EHOSTUNREACH; + goto fail_locked; + } + ifindex = ecu->parent->ifindex; + jsk->addr.dst = ecu->name; + jsk->addr.da = ecu->sa; + put_j1939_ecu(ecu); + } else { + /* broadcast */ + jsk->addr.dst = 0; + jsk->addr.da = addr->can_addr.j1939.addr; + } + /* + * take a default source when not present, so connected sockets + * will stick to the same source ECU + */ + if (!jsk->addr.src && !j1939_address_is_valid(jsk->addr.sa)) { + ecu = j1939_ecu_find_segment_default_tx(ifindex, + &jsk->addr.src, &jsk->addr.sa); + if (IS_ERR(ecu)) { + ret = PTR_ERR(ecu); + goto fail_locked; + } + put_j1939_ecu(ecu); + } + + /* start assigning, no problem can occur at this point anymore */ + jsk->sk.sk_bound_dev_if = ifindex; + + if (!(jsk->state & JSK_BOUND) || !pgn_is_valid(jsk->addr.pgn)) { + /* + * bind() takes precedence over connect() for the + * pgn to use ourselve + */ + jsk->addr.pgn = addr->can_addr.j1939.pgn; + } + + old_state = jsk->state; + jsk->state |= JSK_CONNECTED; + + if (!(old_state & (JSK_BOUND | JSK_CONNECTED))) { + list_add_tail(&jsk->list, &s.socks); + j1939_recv_add(jsk, j1939sk_recv_skb); + } + release_sock(sock->sk); + mutex_unlock(&s.lock); + return 0; + +fail_locked: + release_sock(sock->sk); + mutex_unlock(&s.lock); + return ret; +} + +static void j1939sk_sock2sockaddr_can(struct sockaddr_can *addr, + const struct j1939_sock *jsk, int peer) +{ + addr->can_family = AF_CAN; + addr->can_ifindex = jsk->sk.sk_bound_dev_if; + addr->can_addr.j1939.name = peer ? jsk->addr.dst : jsk->addr.src; + addr->can_addr.j1939.pgn = jsk->addr.pgn; + addr->can_addr.j1939.addr = peer ? jsk->addr.da : jsk->addr.sa; +} + +static int j1939sk_getname(struct socket *sock, struct sockaddr *uaddr, + int *len, int peer) +{ + struct sockaddr_can *addr = (struct sockaddr_can *)uaddr; + struct sock *sk = sock->sk; + struct j1939_sock *jsk = j1939_sk(sk); + int ret = 0; + + lock_sock(sk); + + if (peer && !(jsk->state & JSK_CONNECTED)) { + ret = -EADDRNOTAVAIL; + goto failure; + } + + j1939sk_sock2sockaddr_can(addr, jsk, peer); + *len = sizeof(*addr); + +failure: + release_sock(sk); + + return ret; +} + +static int j1939sk_release(struct socket *sock) +{ + struct sock *sk = sock->sk; + struct j1939_sock *jsk; + + if (!sk) + return 0; + jsk = j1939_sk(sk); + j1939_recv_remove(jsk, j1939sk_recv_skb); + mutex_lock(&s.lock); + list_del_init(&jsk->list); + mutex_unlock(&s.lock); + + lock_sock(sk); + if (jsk->state & PROMISC) + j1939_put_promisc_receiver(jsk->sk.sk_bound_dev_if); + + sock_orphan(sk); + sock->sk = NULL; + + release_sock(sk); + sock_put(sk); + + return 0; +} + +static int j1939sk_setsockopt_flag(struct j1939_sock *jsk, + char __user *optval, unsigned int optlen, int flag) +{ + int tmp; + + if (optlen != sizeof(tmp)) + return -EINVAL; + if (copy_from_user(&tmp, optval, optlen)) + return -EFAULT; + lock_sock(&jsk->sk); + if (tmp) + jsk->state |= flag; + else + jsk->state &= ~flag; + release_sock(&jsk->sk); + return tmp; +} + +static int j1939sk_setsockopt(struct socket *sock, int level, int optname, + char __user *optval, unsigned int optlen) +{ + struct sock *sk = sock->sk; + struct j1939_sock *jsk = j1939_sk(sk); + int ret = 0, tmp, count; + struct j1939_filter *filters, *ofilters; + + if (level != SOL_CAN_J1939) + return -EINVAL; + + switch (optname) { + case SO_J1939_FILTER: + if (optval) { + if (optlen % sizeof(*filters) != 0) + return -EINVAL; + count = optlen / sizeof(*filters); + filters = kmalloc(optlen, GFP_KERNEL); + if (!filters) + return -ENOMEM; + if (copy_from_user(filters, optval, optlen)) { + kfree(filters); + return -EFAULT; + } + } else { + filters = NULL; + count = 0; + } + + j1939_recv_suspend(); + ofilters = jsk->filters; + jsk->filters = filters; + jsk->nfilters = count; + j1939_recv_resume(); + if (ofilters) + kfree(ofilters); + break; + case SO_J1939_PROMISC: + tmp = jsk->state & PROMISC; + ret = j1939sk_setsockopt_flag(jsk, optval, optlen, PROMISC); + if (ret && !tmp) + j1939_get_promisc_receiver(jsk->sk.sk_bound_dev_if); + else if (!ret && tmp) + j1939_put_promisc_receiver(jsk->sk.sk_bound_dev_if); + ret = 0; + break; + case SO_J1939_RECV_OWN: + j1939sk_setsockopt_flag(jsk, optval, optlen, RECV_OWN); + break; + case SO_J1939_SEND_PRIO: + if (optlen != sizeof(tmp)) + return -EINVAL; + if (copy_from_user(&tmp, optval, optlen)) + return -EFAULT; + if ((tmp < 0) || (tmp > 7)) + return -EDOM; + if ((tmp < 2) && !capable(CAP_NET_ADMIN)) + return -EPERM; + lock_sock(&jsk->sk); + jsk->sk.sk_priority = j1939_to_sk_priority(tmp); + release_sock(&jsk->sk); + break; + default: + return -ENOPROTOOPT; + } + + return ret; +} + +static int j1939sk_getsockopt(struct socket *sock, int level, int optname, + char __user *optval, int __user *optlen) +{ + struct sock *sk = sock->sk; + struct j1939_sock *jsk = j1939_sk(sk); + int ret, ulen; + /* set defaults for using 'int' properties */ + int tmp = 0; + int len = sizeof(tmp); + void *val = &tmp; + + if (level != SOL_CAN_J1939) + return -EINVAL; + if (get_user(ulen, optlen)) + return -EFAULT; + if (ulen < 0) + return -EINVAL; + + lock_sock(&jsk->sk); + switch (optname) { + case SO_J1939_PROMISC: + tmp = (jsk->state & PROMISC) ? 1 : 0; + break; + case SO_J1939_RECV_OWN: + tmp = (jsk->state & RECV_OWN) ? 1 : 0; + break; + case SO_J1939_SEND_PRIO: + tmp = j1939_prio(jsk->sk.sk_priority); + break; + default: + ret = -ENOPROTOOPT; + goto no_copy; + } + + /* + * copy to user, based on 'len' & 'val' + * but most sockopt's are 'int' properties, and have 'len' & 'val' + * left unchanged, but instead modified 'tmp' + */ + if (len > ulen) + ret = -EFAULT; + else if (put_user(len, optlen)) + ret = -EFAULT; + else if (copy_to_user(optval, val, len)) + ret = -EFAULT; + else + ret = 0; +no_copy: + release_sock(&jsk->sk); + return ret; +} + +static int j1939sk_recvmsg(struct kiocb *iocb, struct socket *sock, + struct msghdr *msg, size_t size, int flags) +{ + struct sock *sk = sock->sk; + struct sk_buff *skb; + struct j1939_sk_buff_cb *sk_addr; + int ret = 0; + + skb = skb_recv_datagram(sk, flags, 0, &ret); + if (!skb) + return ret; + + if (size < skb->len) + msg->msg_flags |= MSG_TRUNC; + else + size = skb->len; + + ret = memcpy_toiovec(msg->msg_iov, skb->data, size); + if (ret < 0) + goto failed_with_skb; + + sock_recv_timestamp(msg, sk, skb); + sk_addr = (void *)skb->cb; + + if (j1939_address_is_valid(sk_addr->dst.addr)) + put_cmsg(msg, SOL_CAN_J1939, SCM_J1939_DEST_ADDR, + sizeof(sk_addr->dst.addr), &sk_addr->dst.addr); + + if (sk_addr->dst.name) + put_cmsg(msg, SOL_CAN_J1939, SCM_J1939_DEST_NAME, + sizeof(sk_addr->dst.name), &sk_addr->dst.name); + + put_cmsg(msg, SOL_CAN_J1939, SCM_J1939_PRIO, + sizeof(sk_addr->priority), &sk_addr->priority); + + if (msg->msg_name) { + struct sockaddr_can *paddr = msg->msg_name; + + msg->msg_namelen = required_size(can_addr.j1939, *paddr); + memset(msg->msg_name, 0, msg->msg_namelen); + paddr->can_family = AF_CAN; + paddr->can_ifindex = sk_addr->ifindex; + paddr->can_addr.j1939.name = sk_addr->src.name; + paddr->can_addr.j1939.addr = sk_addr->src.addr; + paddr->can_addr.j1939.pgn = sk_addr->pgn; + } + + skb_free_datagram(sk, skb); + + return size; + +failed_with_skb: + skb_kill_datagram(sk, skb, flags); + return ret; +} + +static int j1939sk_sendmsg(struct kiocb *iocb, struct socket *sock, + struct msghdr *msg, size_t size) +{ + struct sock *sk = sock->sk; + struct j1939_sock *jsk = j1939_sk(sk); + struct j1939_sk_buff_cb *skb_cb; + struct sk_buff *skb; + struct net_device *dev; + struct j1939_ecu *ecu; + int ifindex; + int ret; + + if (!(jsk->state | JSK_BOUND)) + return -ENOTCONN; + + if (msg->msg_name && (msg->msg_namelen < + required_size(can_addr.j1939, struct sockaddr_can))) + return -EINVAL; + + ifindex = jsk->sk.sk_bound_dev_if; + if (msg->msg_name) { + struct sockaddr_can *addr = msg->msg_name; + if (msg->msg_namelen < required_size(can_addr.j1939, *addr)) + return -EFAULT; + if (addr->can_family != AF_CAN) + return -EINVAL; + if (ifindex && addr->can_ifindex && + (ifindex != addr->can_ifindex)) + return -ENONET; + if (!ifindex) + /* take destination intf when intf not yet set */ + ifindex = addr->can_ifindex; + } + + if (!ifindex) + return -EDESTADDRREQ; + if (j1939_no_address(&jsk->sk)) { + lock_sock(&jsk->sk); + ecu = j1939_ecu_find_segment_default_tx( + jsk->sk.sk_bound_dev_if, + &jsk->addr.src, &jsk->addr.sa); + release_sock(&jsk->sk); + if (IS_ERR(ecu)) + return PTR_ERR(ecu); + } + + dev = dev_get_by_index(&init_net, ifindex); + if (!dev) + return -ENXIO; + + skb = sock_alloc_send_skb(sk, size, + msg->msg_flags & MSG_DONTWAIT, &ret); + if (!skb) + goto put_dev; + + ret = memcpy_fromiovec(skb_put(skb, size), msg->msg_iov, size); + if (ret < 0) + goto free_skb; + skb->dev = dev; + skb->sk = sk; + + BUILD_BUG_ON(sizeof(skb->cb) < sizeof(*skb_cb)); + + skb_cb = (void *) skb->cb; + memset(skb_cb, 0, sizeof(*skb_cb)); + skb_cb->msg_flags = msg->msg_flags; + skb_cb->ifindex = ifindex; + skb_cb->src.name = jsk->addr.src; + skb_cb->dst.name = jsk->addr.dst; + skb_cb->pgn = jsk->addr.pgn; + skb_cb->priority = j1939_prio(jsk->sk.sk_priority); + skb_cb->src.addr = jsk->addr.sa; + skb_cb->dst.addr = jsk->addr.da; + + if (msg->msg_name) { + struct sockaddr_can *addr = msg->msg_name; + if (addr->can_addr.j1939.name) { + ecu = j1939_ecu_find_by_name(addr->can_addr.j1939.name, + ifindex); + if (!ecu) + return -EADDRNOTAVAIL; + skb_cb->dst.name = ecu->name; + skb_cb->dst.addr = ecu->sa; + put_j1939_ecu(ecu); + } else { + skb_cb->dst.name = 0; + skb_cb->dst.addr = addr->can_addr.j1939.addr; + } + if (pgn_is_valid(addr->can_addr.j1939.pgn)) + skb_cb->pgn = addr->can_addr.j1939.pgn; + } + + if (skb_cb->msg_flags & J1939_MSG_SYNC) { + if (skb_cb->msg_flags & MSG_DONTWAIT) { + ret = j1939_sock_pending_add_first(&jsk->sk); + if (ret > 0) + ret = -EAGAIN; + } else { + ret = wait_event_interruptible(jsk->waitq, + j1939_sock_pending_add_first(&jsk->sk)); + } + if (ret < 0) + goto free_skb; + } else { + j1939_sock_pending_add(&jsk->sk); + } + + ret = j1939_send(skb, j1939_level_sky); + if (ret < 0) + goto decrement_pending; + + dev_put(dev); + return size; + +decrement_pending: + j1939_sock_pending_del(&jsk->sk); +free_skb: + kfree_skb(skb); +put_dev: + dev_put(dev); + return ret; +} + +/* PROC */ +static int j1939sk_proc_show(struct seq_file *sqf, void *v) +{ + struct j1939_sock *jsk; + struct net_device *netdev; + + seq_printf(sqf, "iface\tflags\tlocal\tremote\tpgn\tprio\tpending\n"); + mutex_lock(&s.lock); + list_for_each_entry(jsk, &s.socks, list) { + lock_sock(&jsk->sk); + netdev = NULL; + if (jsk->sk.sk_bound_dev_if) + netdev = dev_get_by_index(&init_net, + jsk->sk.sk_bound_dev_if); + seq_printf(sqf, "%s\t", netdev ? netdev->name : "-"); + if (netdev) + dev_put(netdev); + seq_printf(sqf, "%c%c%c%c\t", + (jsk->state & JSK_BOUND) ? 'b' : '-', + (jsk->state & JSK_CONNECTED) ? 'c' : '-', + (jsk->state & PROMISC) ? 'P' : '-', + (jsk->state & RECV_OWN) ? 'o' : '-'); + if (jsk->addr.src) + seq_printf(sqf, "%016llx", (long long)jsk->addr.src); + else if (j1939_address_is_unicast(jsk->addr.sa)) + seq_printf(sqf, "%02x", jsk->addr.sa); + else + seq_printf(sqf, "-"); + seq_printf(sqf, "\t"); + if (jsk->addr.dst) + seq_printf(sqf, "%016llx", (long long)jsk->addr.dst); + else if (j1939_address_is_unicast(jsk->addr.da)) + seq_printf(sqf, "%02x", jsk->addr.da); + else + seq_printf(sqf, "-"); + seq_printf(sqf, "\t%05x", jsk->addr.pgn); + seq_printf(sqf, "\t%u", j1939_prio(jsk->sk.sk_priority)); + seq_printf(sqf, "\t%u", jsk->skb_pending); + release_sock(&jsk->sk); + seq_printf(sqf, "\n"); + } + mutex_unlock(&s.lock); + return 0; +} + +void j1939sk_netdev_event(int ifindex, int error_code) +{ + struct j1939_sock *jsk; + + mutex_lock(&s.lock); + list_for_each_entry(jsk, &s.socks, list) { + if (jsk->sk.sk_bound_dev_if != ifindex) + continue; + jsk->sk.sk_err = error_code; + if (!sock_flag(&jsk->sk, SOCK_DEAD)) + jsk->sk.sk_error_report(&jsk->sk); + /* do not remove filters here */ + } + mutex_unlock(&s.lock); +} + +static const struct proto_ops j1939_ops = { + .family = PF_CAN, + .release = j1939sk_release, + .bind = j1939sk_bind, + .connect = j1939sk_connect, + .socketpair = sock_no_socketpair, + .accept = sock_no_accept, + .getname = j1939sk_getname, + .poll = datagram_poll, + .ioctl = can_ioctl, + .listen = sock_no_listen, + .shutdown = sock_no_shutdown, + .setsockopt = j1939sk_setsockopt, + .getsockopt = j1939sk_getsockopt, + .sendmsg = j1939sk_sendmsg, + .recvmsg = j1939sk_recvmsg, + .mmap = sock_no_mmap, + .sendpage = sock_no_sendpage, +}; + +static struct proto j1939_proto __read_mostly = { + .name = "CAN_J1939", + .owner = THIS_MODULE, + .obj_size = sizeof(struct j1939_sock), + .init = j1939sk_init, +}; + +static const struct can_proto j1939_can_proto = { + .type = SOCK_DGRAM, + .protocol = CAN_J1939, + .ops = &j1939_ops, + .prot = &j1939_proto, + + .rtnl_link_ops = &j1939_rtnl_af_ops, + .rtnl_new_addr = j1939rtnl_new_addr, + .rtnl_del_addr = j1939rtnl_del_addr, + .rtnl_dump_addr = j1939rtnl_dump_addr, +}; + +__init int j1939sk_module_init(void) +{ + int ret; + + INIT_LIST_HEAD(&s.socks); + mutex_init(&s.lock); + + ret = can_proto_register(&j1939_can_proto); + if (ret < 0) + pr_err("can: registration of j1939 protocol failed\n"); + else + j1939_proc_add("sock", j1939sk_proc_show, NULL); + return ret; +} + +void j1939sk_module_exit(void) +{ + j1939_proc_remove("sock"); + can_proto_unregister(&j1939_can_proto); +} + +MODULE_ALIAS("can-proto-" __stringify(CAN_J1939)); + diff --git a/net/can/j1939/transport.c b/net/can/j1939/transport.c new file mode 100644 index 0000000..9f723c6 --- /dev/null +++ b/net/can/j1939/transport.c @@ -0,0 +1,1449 @@ +/* + * Copyright (c) 2010-2011 EIA Electronics + * + * Authors: + * Kurt Van Dijck + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the version 2 of the GNU General Public License + * as published by the Free Software Foundation + */ + +#include +#include +#include +#include +#include +#include "j1939-priv.h" + +#define REGULAR 0 +#define EXTENDED 1 + +#define etp_pgn_ctl 0xc800 +#define etp_pgn_dat 0xc700 +#define tp_pgn_ctl 0xec00 +#define tp_pgn_dat 0xeb00 + +#define tp_cmd_bam 0x20 +#define tp_cmd_rts 0x10 +#define tp_cmd_cts 0x11 +#define tp_cmd_eof 0x13 +#define tp_cmd_abort 0xff + +#define etp_cmd_rts 0x14 +#define etp_cmd_cts 0x15 +#define etp_cmd_dpo 0x16 +#define etp_cmd_eof 0x17 +#define etp_cmd_abort 0xff + +#define ABORT_BUSY 1 +#define ABORT_RESOURCE 2 +#define ABORT_TIMEOUT 3 +#define ABORT_GENERIC 4 +#define ABORT_FAULT 5 + +#define MAX_TP_PACKET_SIZE (7*255) +#define MAX_ETP_PACKET_SIZE (7*0xffffff) + +static int block = 255; +static int max_packet_size = 1024*100; +static int retry_ms = 20; + +struct session { + struct list_head list; + atomic_t refs; + spinlock_t lock; + + struct j1939_sk_buff_cb *cb; /* + * ifindex, src, dst, pgn define the session block + * the are _never_ modified after insertion in the list + * this decreases locking problems a _lot_ + */ + struct sk_buff *skb; + + /* + * all tx related stuff (last_txcmd, pkt.tx) + * is protected (modified only) with the txtask tasklet + * 'total' & 'block' are never changed, + * last_cmd, last & block are protected by ->lock + * this means that the tx may run after cts is received that should + * have stopped tx, but this time discrepancy is never avoided anyhow + */ + uint8_t last_cmd, last_txcmd; + uint8_t transmission; + uint8_t extd; + struct { + /* + * these do not require 16 bit, they should fit in uint8_t + * but putting in int makes it easier to deal with + */ + unsigned int total, done, last, tx; + unsigned int block; /* for TP */ + unsigned int dpo; /* for ETP */ + } pkt; + struct hrtimer txtimer, rxtimer; + /* tasklets for execution of tx/rx timer hander in softirq */ + struct tasklet_struct txtask, rxtask; +}; + +static struct j1939tp { + spinlock_t lock; + struct list_head sessionq; + struct list_head extsessionq; + struct { + struct list_head sessionq; + spinlock_t lock; + struct work_struct work; + } del; + wait_queue_head_t wait; + struct notifier_block notifier; +} s; + +static struct session *j1939session_new(struct sk_buff *skb); +static struct session *j1939session_fresh_new(int size, + struct j1939_sk_buff_cb *rel_cb, pgn_t pgn); + +static inline void fix_cb(struct j1939_sk_buff_cb *cb) +{ + cb->msg_flags &= ~J1939_MSG_RESERVED; +} + +static inline struct list_head *sessionq(int extd) +{ + return extd ? &s.extsessionq : &s.sessionq; +} + +static inline void j1939session_destroy(struct session *session) +{ + if (session->skb) + kfree_skb(session->skb); + hrtimer_cancel(&session->rxtimer); + hrtimer_cancel(&session->txtimer); + tasklet_disable(&session->rxtask); + tasklet_disable(&session->txtask); + kfree(session); +} + +/* clean up work queue */ +static void j1939tp_del_work(struct work_struct *work) +{ + struct session *session; + int cnt = 0; + + do { + session = NULL; + spin_lock_bh(&s.del.lock); + if (list_empty(&s.del.sessionq)) { + spin_unlock_bh(&s.del.lock); + break; + } + session = list_first_entry(&s.del.sessionq, + struct session, list); + list_del_init(&session->list); + spin_unlock_bh(&s.del.lock); + j1939session_destroy(session); + ++cnt; + } while (1); +} +/* reference counter */ +static inline void get_session(struct session *session) +{ + atomic_inc(&session->refs); +} + +static void put_session(struct session *session) +{ + BUG_ON(!session); + if (atomic_add_return(-1, &session->refs) >= 0) + /* not the last one */ + return; + /* it should have been removed from any list long time ago */ + BUG_ON(!list_empty(&session->list)); + + hrtimer_try_to_cancel(&session->rxtimer); + hrtimer_try_to_cancel(&session->txtimer); + tasklet_disable_nosync(&session->rxtask); + tasklet_disable_nosync(&session->txtask); + + if (in_interrupt()) { + spin_lock_bh(&s.del.lock); + list_add_tail(&session->list, &s.del.sessionq); + spin_unlock_bh(&s.del.lock); + schedule_work(&s.del.work); + } else { + /* destroy session right here */ + j1939session_destroy(session); + } +} + +/* transport status locking */ +static inline void session_lock(struct session *session) +{ + get_session(session); /* safety measure */ + spin_lock_bh(&session->lock); +} + +static inline void session_unlock(struct session *session) +{ + spin_unlock_bh(&session->lock); + put_session(session); +} + +static inline void sessionlist_lock(void) +{ + spin_lock_bh(&s.lock); +} + +static inline void sessionlist_unlock(void) +{ + spin_unlock_bh(&s.lock); +} + +/* + * see if we are receiver + * returns 0 for broadcasts, although we will receive them + */ +static inline int j1939tp_im_receiver(const struct j1939_sk_buff_cb *cb) +{ + return (cb->dst.flags & ECUFLAG_LOCAL) ? 1 : 0; +} + +/* see if we are sender */ +static inline int j1939tp_im_transmitter(const struct j1939_sk_buff_cb *cb) +{ + return (cb->src.flags & ECUFLAG_LOCAL) ? 1 : 0; +} + +/* see if we are involved as either receiver or transmitter */ +/* reverse = -1 means : any direction */ +static int j1939tp_im_involved(const struct j1939_sk_buff_cb *cb, int reverse) +{ + if (reverse < 0) { + return ((cb->src.flags | cb->dst.flags) & ECUFLAG_LOCAL) + ? 1 : 0; + } else if (reverse) { + return j1939tp_im_receiver(cb); + } else { + return j1939tp_im_transmitter(cb); + } +} + +/* extract pgn from flow-ctl message */ +static inline pgn_t j1939xtp_ctl_to_pgn(const uint8_t *dat) +{ + pgn_t pgn; + + pgn = (dat[7] << 16) | (dat[6] << 8) | (dat[5] << 0); + if (pgn_is_pdu1(pgn)) + pgn &= 0xffff00; + return pgn; +} + +static inline unsigned int j1939tp_ctl_to_size(const uint8_t *dat) +{ + return (dat[2] << 8) + (dat[1] << 0); +} +static inline unsigned int j1939etp_ctl_to_packet(const uint8_t *dat) +{ + return (dat[4] << 16) | (dat[3] << 8) | (dat[2] << 0); +} +static inline unsigned int j1939etp_ctl_to_size(const uint8_t *dat) +{ + return (dat[4] << 24) | (dat[3] << 16) | + (dat[2] << 8) | (dat[1] << 0); +} + +/* + * find existing session: + * reverse: swap cb's src & dst + * there is no problem with matching broadcasts, since + * broadcasts (no dst, no da) would never call this + * with reverse==1 + */ +static int j1939tp_match(const struct j1939_sk_buff_cb *a, + const struct j1939_sk_buff_cb *b, int reverse) +{ + if (a->ifindex != b->ifindex) + return 0; + if (!reverse) { + if (a->src.name) { + if (a->src.name != b->src.name) + return 0; + } else if (a->src.addr != b->src.addr) + return 0; + if (a->dst.name) { + if (a->dst.name != b->dst.name) + return 0; + } else if (a->dst.addr != b->dst.addr) + return 0; + } else { + if (a->src.name) { + if (a->src.name != b->dst.name) + return 0; + } else if (a->src.addr != b->dst.addr) + return 0; + if (a->dst.name) { + if (a->dst.name != b->src.name) + return 0; + } else if (a->dst.addr != b->src.addr) + return 0; + } + return 1; +} + +static struct session *_j1939tp_find(struct list_head *root, + const struct j1939_sk_buff_cb *cb, int reverse) +{ + struct session *session; + + list_for_each_entry(session, root, list) { + get_session(session); + if (j1939tp_match(session->cb, cb, reverse)) + return session; + put_session(session); + } + return NULL; +} + +static struct session *j1939tp_find(struct list_head *root, + const struct j1939_sk_buff_cb *cb, int reverse) +{ + struct session *session; + sessionlist_lock(); + session = _j1939tp_find(root, cb, reverse); + sessionlist_unlock(); + return session; +} + +static void j1939_skbcb_swap(struct j1939_sk_buff_cb *cb) +{ + name_t name; + uint8_t addr; + int flags; + + name = cb->dst.name; + cb->dst.name = cb->src.name; + cb->src.name = name; + + addr = cb->dst.addr; + cb->dst.addr = cb->src.addr; + cb->src.addr = addr; + + flags = cb->dst.flags; + cb->dst.flags = cb->src.flags; + cb->src.flags = flags; +} +/* TP transmit packet functions */ +static int j1939tp_tx_dat(struct session *related, + const uint8_t *dat, int len) +{ + int ret; + struct sk_buff *skb; + struct j1939_sk_buff_cb *skb_cb; + uint8_t *skdat; + + skb = dev_alloc_skb(8); + if (unlikely(!skb)) { + pr_alert("%s: out of memory?\n", __func__); + return -ENOMEM; + } + skb->protocol = related->skb->protocol; + skb->pkt_type = related->skb->pkt_type; + skb->ip_summed = related->skb->ip_summed; + skb->sk = related->skb->sk; + + skb_cb = (void *)skb->cb; + *skb_cb = *(related->cb); + fix_cb(skb_cb); + /* fix pgn */ + skb_cb->pgn = related->extd ? etp_pgn_dat : tp_pgn_dat; + + skdat = skb_put(skb, len); + memcpy(skdat, dat, len); + ret = j1939_send(skb, j1939_level_transport); + if (ret < 0) + kfree_skb(skb); + return ret; +} + +static int j1939xtp_do_tx_ctl(struct sk_buff *related, int extd, + int swap_src_dst, pgn_t pgn, const uint8_t dat[5]) +{ + int ret; + struct sk_buff *skb; + struct j1939_sk_buff_cb *skb_cb, *rel_cb; + uint8_t *skdat; + + rel_cb = (void *)related->cb; + if (!j1939tp_im_involved(rel_cb, swap_src_dst)) + return 0; + + skb = dev_alloc_skb(8); + if (unlikely(!skb)) { + pr_alert("%s: out of memory?\n", __func__); + return -ENOMEM; + } + skb->protocol = related->protocol; + skb->pkt_type = related->pkt_type; + skb->ip_summed = related->ip_summed; + skb->sk = related->sk; + + skb_cb = (void *)skb->cb; + *skb_cb = *rel_cb; + fix_cb(skb_cb); + if (swap_src_dst) + j1939_skbcb_swap(skb_cb); + skb_cb->pgn = extd ? etp_pgn_ctl : tp_pgn_ctl; + + skdat = skb_put(skb, 8); + memcpy(skdat, dat, 5); + skdat[7] = (pgn >> 16) & 0xff; + skdat[6] = (pgn >> 8) & 0xff; + skdat[5] = (pgn >> 0) & 0xff; + + ret = j1939_send(skb, j1939_level_transport); + if (ret) + kfree_skb(skb); + return ret; +} + +static inline int j1939tp_tx_ctl(struct session *session, + int swap_src_dst, const uint8_t dat[8]) +{ + return j1939xtp_do_tx_ctl(session->skb, session->extd, swap_src_dst, + session->cb->pgn, dat); +} + +static int j1939xtp_tx_abort(struct sk_buff *related, int extd, + int swap_src_dst, int err, pgn_t pgn) +{ + struct j1939_sk_buff_cb *cb = (void *)related->cb; + uint8_t dat[5]; + + if (!j1939tp_im_involved(cb, swap_src_dst)) + return 0; + + memset(dat, 0xff, sizeof(dat)); + dat[0] = tp_cmd_abort; + if (!extd) + dat[1] = err ?: ABORT_GENERIC; + return j1939xtp_do_tx_ctl(related, extd, swap_src_dst, pgn, dat); +} + +/* timer & scheduler functions */ +static inline void j1939session_schedule_txnow(struct session *session) +{ + tasklet_schedule(&session->txtask); +} +static enum hrtimer_restart j1939tp_txtimer(struct hrtimer *hrtimer) +{ + struct session *session = + container_of(hrtimer, struct session, txtimer); + j1939session_schedule_txnow(session); + return HRTIMER_NORESTART; +} +static inline void j1939tp_schedule_txtimer(struct session *session, int msec) +{ + hrtimer_start(&session->txtimer, + ktime_set(msec / 1000, (msec % 1000)*1000000UL), + HRTIMER_MODE_REL); +} +static inline void j1939tp_set_rxtimeout(struct session *session, int msec) +{ + hrtimer_start(&session->rxtimer, + ktime_set(msec / 1000, (msec % 1000)*1000000UL), + HRTIMER_MODE_REL); +} + +/* + * session completion functions + */ +/* + * j1939session_drop + * removes a session from open session list + */ +static inline void j1939session_drop(struct session *session) +{ + sessionlist_lock(); + list_del_init(&session->list); + sessionlist_unlock(); + + if (session->transmission) { + if (session->skb && session->skb->sk) + j1939_sock_pending_del(session->skb->sk); + wake_up_all(&s.wait); + } + put_session(session); +} + +static inline void j1939session_completed(struct session *session) +{ + j1939_recv(session->skb, j1939_level_transport); + j1939session_drop(session); +} + +static void j1939session_cancel(struct session *session, int err) +{ + if ((err >= 0) && j1939tp_im_involved(session->cb, -1)) { + if (!j1939cb_is_broadcast(session->cb)) { + /* do not send aborts on incoming broadcasts */ + j1939xtp_tx_abort(session->skb, session->extd, + !j1939tp_im_transmitter(session->cb), + err, session->cb->pgn); + } + } + j1939session_drop(session); +} + +static enum hrtimer_restart j1939tp_rxtimer(struct hrtimer *hrtimer) +{ + struct session *session = + container_of(hrtimer, struct session, rxtimer); + tasklet_schedule(&session->rxtask); + return HRTIMER_NORESTART; +} + +static void j1939tp_rxtask(unsigned long val) +{ + struct session *session = (void *)val; + + get_session(session); + pr_alert("%s: timeout on %i\n", __func__, session->cb->ifindex); + j1939session_cancel(session, ABORT_TIMEOUT); + put_session(session); +} + +/* + * receive packet functions + */ +static void _j1939xtp_rx_bad_message(struct sk_buff *skb, int extd) +{ + struct j1939_sk_buff_cb *cb = (void *)skb->cb; + struct session *session; + pgn_t pgn; + + pgn = j1939xtp_ctl_to_pgn(skb->data); + session = j1939tp_find(sessionq(extd), cb, 0); + if (session /*&& (session->cb->pgn == pgn)*/) { + /* do not allow TP control messages on 2 pgn's */ + j1939session_cancel(session, ABORT_FAULT); + put_session(session); /* ~j1939tp_find */ + return; + } + j1939xtp_tx_abort(skb, extd, 0, ABORT_FAULT, pgn); + if (!session) + return; + put_session(session); /* ~j1939tp_find */ +} + +/* abort packets may come in 2 directions */ +static void j1939xtp_rx_bad_message(struct sk_buff *skb, int extd) +{ + struct j1939_sk_buff_cb *cb = (void *)skb->cb; + + pr_info("%s, pgn %05x\n", __func__, j1939xtp_ctl_to_pgn(skb->data)); + _j1939xtp_rx_bad_message(skb, extd); + j1939_skbcb_swap(cb); + _j1939xtp_rx_bad_message(skb, extd); + /* restore skb */ + j1939_skbcb_swap(cb); + return; +} + +static void _j1939xtp_rx_abort(struct sk_buff *skb, int extd) +{ + struct j1939_sk_buff_cb *cb = (void *)skb->cb; + struct session *session; + pgn_t pgn; + + pgn = j1939xtp_ctl_to_pgn(skb->data); + session = j1939tp_find(sessionq(extd), cb, 0); + if (!session) + return; + if (session->transmission && !session->last_txcmd) { + /* + * empty block: + * do not drop session when a transmit session did not + * start yet + */ + } else if (session->cb->pgn == pgn) + j1939session_drop(session); + /* another PGN had a bad message */ + /* + * TODO: maybe cancel current connection + * as another pgn was communicated + */ + put_session(session); /* ~j1939tp_find */ +} +/* abort packets may come in 2 directions */ +static inline void j1939xtp_rx_abort(struct sk_buff *skb, int extd) +{ + struct j1939_sk_buff_cb *cb = (void *)skb->cb; + + pr_info("%s %i, %05x\n", __func__, cb->ifindex, + j1939xtp_ctl_to_pgn(skb->data)); + _j1939xtp_rx_abort(skb, extd); + j1939_skbcb_swap(cb); + _j1939xtp_rx_abort(skb, extd); + /* restore skb */ + j1939_skbcb_swap(cb); + return; +} + +static void j1939xtp_rx_eof(struct sk_buff *skb, int extd) +{ + struct j1939_sk_buff_cb *cb = (void *)skb->cb; + struct session *session; + pgn_t pgn; + + /* end of tx cycle */ + pgn = j1939xtp_ctl_to_pgn(skb->data); + session = j1939tp_find(sessionq(extd), cb, 1); + if (!session) + /* + * strange, we had EOF on closed connection + * do nothing, as EOF closes the connection anyway + */ + return; + + if (session->cb->pgn != pgn) { + j1939xtp_tx_abort(skb, extd, 1, ABORT_BUSY, pgn); + j1939session_cancel(session, ABORT_BUSY); + } else { + /* transmitted without problems */ + j1939session_completed(session); + } + put_session(session); /* ~j1939tp_find */ +} + +static void j1939xtp_rx_cts(struct sk_buff *skb, int extd) +{ + struct j1939_sk_buff_cb *cb = (void *)skb->cb; + struct session *session; + pgn_t pgn; + unsigned int pkt; + const uint8_t *dat; + + dat = skb->data; + pgn = j1939xtp_ctl_to_pgn(skb->data); + session = j1939tp_find(sessionq(extd), cb, 1); + if (!session) { + /* 'CTS shall be ignored' */ + return; + } + if (session->cb->pgn != pgn) { + /* what to do? */ + j1939xtp_tx_abort(skb, extd, 1, ABORT_BUSY, pgn); + j1939session_cancel(session, ABORT_BUSY); + put_session(session); /* ~j1939tp_find */ + return; + } + session_lock(session); + pkt = extd ? j1939etp_ctl_to_packet(dat) : dat[2]; + if (!dat[0]) + hrtimer_cancel(&session->txtimer); + else if (!pkt) + goto bad_fmt; + else if (dat[1] > session->pkt.block /* 0xff for etp */) + goto bad_fmt; + else { + /* set packet counters only when not CTS(0) */ + session->pkt.done = pkt - 1; + session->pkt.last = session->pkt.done + dat[1]; + if (session->pkt.last > session->pkt.total) + /* safety measure */ + session->pkt.last = session->pkt.total; + /* TODO: do not set tx here, do it in txtask */ + session->pkt.tx = session->pkt.done; + } + session->last_cmd = dat[0]; + session_unlock(session); + if (dat[1]) { + j1939tp_set_rxtimeout(session, 1250); + if (j1939tp_im_transmitter(session->cb)) + j1939session_schedule_txnow(session); + } else { + /* CTS(0) */ + j1939tp_set_rxtimeout(session, 550); + } + put_session(session); /* ~j1939tp_find */ + return; +bad_fmt: + session_unlock(session); + j1939session_cancel(session, ABORT_FAULT); + put_session(session); /* ~j1939tp_find */ +} + +static void j1939xtp_rx_rts(struct sk_buff *skb, int extd) +{ + struct j1939_sk_buff_cb *cb = (void *)skb->cb; + struct session *session; + int len; + const uint8_t *dat; + pgn_t pgn; + + dat = skb->data; + pgn = j1939xtp_ctl_to_pgn(dat); + + if ((tp_cmd_rts == dat[0]) && j1939cb_is_broadcast(cb)) { + pr_alert("%s: rts without destination (%i %02x)\n", __func__, + cb->ifindex, cb->src.addr); + return; + } + /* + * TODO: abort RTS when a similar + * TP is pending in the other direction + */ + session = j1939tp_find(sessionq(extd), cb, 0); + if (session && !j1939tp_im_transmitter(cb)) { + /* RTS on pending connection */ + j1939session_cancel(session, ABORT_BUSY); + if ((pgn != session->cb->pgn) && (tp_cmd_bam != dat[0])) + j1939xtp_tx_abort(skb, extd, 1, ABORT_BUSY, pgn); + put_session(session); /* ~j1939tp_find */ + return; + } else if (!session && j1939tp_im_transmitter(cb)) { + pr_alert("%s: I should tx (%i %02x %02x)\n", __func__, + cb->ifindex, cb->src.addr, cb->dst.addr); + return; + } + if (session && (0 != session->last_cmd)) { + /* we received a second rts on the same connection */ + pr_alert("%s: connection exists (%i %02x %02x)\n", __func__, + cb->ifindex, cb->src.addr, cb->dst.addr); + j1939session_cancel(session, ABORT_BUSY); + put_session(session); /* ~j1939tp_find */ + return; + } + if (session) { + /* + * make sure 'sa' & 'da' are correct ! + * They may be 'not filled in yet' for sending + * skb's, since they did not pass the Address Claim ever. + */ + session->cb->src.addr = cb->src.addr; + session->cb->dst.addr = cb->dst.addr; + } else { + int abort = 0; + if (extd) { + len = j1939etp_ctl_to_size(dat); + if (len > (max_packet_size ?: MAX_ETP_PACKET_SIZE)) + abort = ABORT_RESOURCE; + else if (len <= MAX_TP_PACKET_SIZE) + abort = ABORT_FAULT; + } else { + len = j1939tp_ctl_to_size(dat); + if (len > MAX_TP_PACKET_SIZE) + abort = ABORT_FAULT; + else if (max_packet_size && (len > max_packet_size)) + abort = ABORT_RESOURCE; + } + if (abort) { + j1939xtp_tx_abort(skb, extd, 1, abort, pgn); + return; + } + session = j1939session_fresh_new(len, cb, pgn); + if (!session) { + j1939xtp_tx_abort(skb, extd, 1, ABORT_RESOURCE, pgn); + return; + } + session->extd = extd; + /* initialize the control buffer: plain copy */ + session->pkt.total = (len+6)/7; + session->pkt.block = 0xff; + if (!extd) { + if (dat[3] != session->pkt.total) + pr_alert("%s: strange total," + " %u != %u\n", __func__, + session->pkt.total, dat[3]); + session->pkt.total = dat[3]; + session->pkt.block = dat[4]; + } + session->pkt.done = session->pkt.tx = 0; + get_session(session); /* equivalent to j1939tp_find() */ + sessionlist_lock(); + list_add_tail(&session->list, sessionq(extd)); + sessionlist_unlock(); + } + session->last_cmd = dat[0]; + + j1939tp_set_rxtimeout(session, 1250); + + if (j1939tp_im_receiver(session->cb)) { + if (extd || (tp_cmd_bam != dat[0])) + j1939session_schedule_txnow(session); + } + /* + * as soon as it's inserted, things can go fast + * protect against a long delay + * between spin_unlock & next statement + * so, only release here, at the end + */ + put_session(session); /* ~j1939tp_find */ + return; +} + +static void j1939xtp_rx_dpo(struct sk_buff *skb, int extd) +{ + struct j1939_sk_buff_cb *cb = (void *)skb->cb; + struct session *session; + pgn_t pgn; + const uint8_t *dat = skb->data; + + pgn = j1939xtp_ctl_to_pgn(dat); + session = j1939tp_find(sessionq(extd), cb, 0); + if (!session) { + pr_info("%s: %s\n", __func__, "no connection found"); + return; + } + + if (session->cb->pgn != pgn) { + pr_info("%s: different pgn\n", __func__); + j1939xtp_tx_abort(skb, 1, 1, ABORT_BUSY, pgn); + j1939session_cancel(session, ABORT_BUSY); + put_session(session); /* ~j1939tp_find */ + return; + } + /* transmitted without problems */ + session->pkt.dpo = j1939etp_ctl_to_packet(skb->data); + session->last_cmd = dat[0]; + j1939tp_set_rxtimeout(session, 750); + put_session(session); /* ~j1939tp_find */ +} + +static void j1939xtp_rx_dat(struct sk_buff *skb, int extd) +{ + struct j1939_sk_buff_cb *cb = (void *)skb->cb; + struct session *session; + const uint8_t *dat; + uint8_t *tpdat; + int offset; + int nbytes; + int final; + int do_cts_eof; + int packet; + + session = j1939tp_find(sessionq(extd), cb, 0); + if (!session) { + pr_info("%s:%s\n", __func__, "no connection found"); + return; + } + dat = skb->data; + if (skb->len <= 1) + /* makes no sense */ + goto strange_packet_unlocked; + + session_lock(session); + + switch (session->last_cmd) { + case 0xff: + break; + case etp_cmd_dpo: + if (extd) + break; + case tp_cmd_bam: + case tp_cmd_cts: + if (!extd) + break; + default: + pr_info("%s: last %02x\n", __func__, + session->last_cmd); + goto strange_packet; + } + + packet = (dat[0]-1+session->pkt.dpo); + offset = packet * 7; + if ((packet > session->pkt.total) || + (session->pkt.done+1) > session->pkt.total) { + pr_info("%s: should have been completed\n", __func__); + goto strange_packet; + } + nbytes = session->skb->len - offset; + if (nbytes > 7) + nbytes = 7; + if ((nbytes <= 0) || ((nbytes + 1) > skb->len)) { + pr_info("%s: nbytes %i, len %i\n", __func__, nbytes, + skb->len); + goto strange_packet; + } + tpdat = session->skb->data; + memcpy(&tpdat[offset], &dat[1], nbytes); + if (packet == session->pkt.done) + ++session->pkt.done; + + if (!extd && j1939cb_is_broadcast(session->cb)) { + final = session->pkt.done >= session->pkt.total; + do_cts_eof = 0; + } else { + final = 0; /* never final, an EOF must follow */ + do_cts_eof = (session->pkt.done >= session->pkt.last); + } + session_unlock(session); + if (final) { + j1939session_completed(session); + } else if (do_cts_eof) { + j1939tp_set_rxtimeout(session, 1250); + if (j1939tp_im_receiver(session->cb)) + j1939session_schedule_txnow(session); + } else { + j1939tp_set_rxtimeout(session, 250); + } + session->last_cmd = 0xff; + put_session(session); /* ~j1939tp_find */ + return; + +strange_packet: + /* unlock session (spinlock) before trying to send */ + session_unlock(session); +strange_packet_unlocked: + j1939session_cancel(session, ABORT_FAULT); + put_session(session); /* ~j1939tp_find */ +} + +/* + * transmit function + */ +static int j1939tp_txnext(struct session *session) +{ + uint8_t dat[8]; + const uint8_t *tpdat; + int ret, offset, len, pkt_done, pkt_end; + unsigned int pkt; + + memset(dat, 0xff, sizeof(dat)); + get_session(session); /* do not loose it */ + + switch (session->last_cmd) { + case 0: + if (!j1939tp_im_transmitter(session->cb)) + break; + dat[1] = (session->skb->len >> 0) & 0xff; + dat[2] = (session->skb->len >> 8) & 0xff; + dat[3] = session->pkt.total; + if (session->extd) { + dat[0] = etp_cmd_rts; + dat[1] = (session->skb->len >> 0) & 0xff; + dat[2] = (session->skb->len >> 8) & 0xff; + dat[3] = (session->skb->len >> 16) & 0xff; + dat[4] = (session->skb->len >> 24) & 0xff; + } else if (j1939cb_is_broadcast(session->cb)) { + dat[0] = tp_cmd_bam; + /* fake cts for broadcast */ + session->pkt.tx = 0; + } else { + dat[0] = tp_cmd_rts; + dat[4] = dat[3]; + } + if (dat[0] == session->last_txcmd) + /* done already */ + break; + ret = j1939tp_tx_ctl(session, 0, dat); + if (ret < 0) + goto failed; + session->last_txcmd = dat[0]; + /* must lock? */ + if (tp_cmd_bam == dat[0]) + j1939tp_schedule_txtimer(session, 50); + j1939tp_set_rxtimeout(session, 1250); + break; + case tp_cmd_rts: + case etp_cmd_rts: + if (!j1939tp_im_receiver(session->cb)) + break; +tx_cts: + ret = 0; + len = session->pkt.total - session->pkt.done; + if (len > 255) + len = 255; + if (len > session->pkt.block) + len = session->pkt.block; + if (block && (len > block)) + len = block; + + if (session->extd) { + pkt = session->pkt.done+1; + dat[0] = etp_cmd_cts; + dat[1] = len; + dat[2] = (pkt >> 0) & 0xff; + dat[3] = (pkt >> 8) & 0xff; + dat[4] = (pkt >> 16) & 0xff; + } else { + dat[0] = tp_cmd_cts; + dat[1] = len; + dat[2] = session->pkt.done+1; + } + if (dat[0] == session->last_txcmd) + /* done already */ + break; + ret = j1939tp_tx_ctl(session, 1, dat); + if (ret < 0) + goto failed; + if (len) + /* only mark cts done when len is set */ + session->last_txcmd = dat[0]; + j1939tp_set_rxtimeout(session, 1250); + break; + case etp_cmd_cts: + if (j1939tp_im_transmitter(session->cb) && session->extd && + (etp_cmd_dpo != session->last_txcmd)) { + /* do dpo */ + dat[0] = etp_cmd_dpo; + session->pkt.dpo = session->pkt.done; + pkt = session->pkt.dpo; + dat[1] = session->pkt.last - session->pkt.done; + dat[2] = (pkt >> 0) & 0xff; + dat[3] = (pkt >> 8) & 0xff; + dat[4] = (pkt >> 16) & 0xff; + ret = j1939tp_tx_ctl(session, 0, dat); + if (ret < 0) + goto failed; + session->last_txcmd = dat[0]; + j1939tp_set_rxtimeout(session, 1250); + session->pkt.tx = session->pkt.done; + } + case tp_cmd_cts: + case 0xff: /* did some data */ + case etp_cmd_dpo: + if ((session->extd || !j1939cb_is_broadcast(session->cb)) && + j1939tp_im_receiver(session->cb)) { + if (session->pkt.done >= session->pkt.total) { + if (session->extd) { + dat[0] = etp_cmd_eof; + dat[1] = session->skb->len >> 0; + dat[2] = session->skb->len >> 8; + dat[3] = session->skb->len >> 16; + dat[4] = session->skb->len >> 24; + } else { + dat[0] = tp_cmd_eof; + dat[1] = session->skb->len; + dat[2] = session->skb->len >> 8; + dat[3] = session->pkt.total; + } + if (dat[0] == session->last_txcmd) + /* done already */ + break; + ret = j1939tp_tx_ctl(session, 1, dat); + if (ret < 0) + goto failed; + session->last_txcmd = dat[0]; + j1939tp_set_rxtimeout(session, 1250); + /* wait for the EOF packet to come in */ + break; + } else if (session->pkt.done >= session->pkt.last) { + session->last_txcmd = 0; + goto tx_cts; + } + } + case tp_cmd_bam: + if (!j1939tp_im_transmitter(session->cb)) + break; + tpdat = session->skb->data; + ret = 0; + pkt_done = 0; + pkt_end = (!session->extd && j1939cb_is_broadcast(session->cb)) + ? session->pkt.total : session->pkt.last; + + while (session->pkt.tx < pkt_end) { + dat[0] = session->pkt.tx - session->pkt.dpo+1; + offset = session->pkt.tx * 7; + len = session->skb->len - offset; + if (len > 7) + len = 7; + memcpy(&dat[1], &tpdat[offset], len); + ret = j1939tp_tx_dat(session, dat, len+1); + if (ret < 0) + break; + session->last_txcmd = 0xff; + ++pkt_done; + ++session->pkt.tx; + if (j1939cb_is_broadcast(session->cb)) { + if (session->pkt.tx < session->pkt.total) + j1939tp_schedule_txtimer(session, 50); + break; + } + } + if (pkt_done) + j1939tp_set_rxtimeout(session, 250); + if (ret) + goto failed; + break; + } + put_session(session); + return 0; +failed: + put_session(session); + return ret; +} + +static void j1939tp_txtask(unsigned long val) +{ + struct session *session = (void *)val; + int ret; + + get_session(session); + ret = j1939tp_txnext(session); + if (ret < 0) + j1939tp_schedule_txtimer(session, retry_ms); + put_session(session); +} + +static inline int j1939tp_tx_initial(struct session *session) +{ + int ret; + + get_session(session); + ret = j1939tp_txnext(session); + /* set nonblocking for further packets */ + session->cb->msg_flags |= MSG_DONTWAIT; + put_session(session); + return ret; +} + +/* this call is to be used as probe within wait_event_xxx() */ +static int j1939session_insert(struct session *session) +{ + struct session *pending; + + sessionlist_lock(); + pending = _j1939tp_find(sessionq(session->extd), session->cb, 0); + if (pending) + /* revert the effect of find() */ + put_session(pending); + else + list_add_tail(&session->list, sessionq(session->extd)); + sessionlist_unlock(); + return pending ? 0 : 1; +} +/* + * j1939 main intf + */ +int j1939_send_transport(struct sk_buff *skb) +{ + struct j1939_sk_buff_cb *cb = (void *)skb->cb; + struct session *session; + int ret; + + if ((tp_pgn_dat == cb->pgn) || (tp_pgn_ctl == cb->pgn) || + (etp_pgn_dat == cb->pgn) || (etp_pgn_ctl == cb->pgn)) + /* avoid conflict */ + return -EDOM; + if (skb->len <= 8) + return 0; + else if (skb->len > (max_packet_size ?: MAX_ETP_PACKET_SIZE)) + return -EMSGSIZE; + + if (skb->len > MAX_TP_PACKET_SIZE) { + if (j1939cb_is_broadcast(cb)) + return -EDESTADDRREQ; + } + + /* prepare new session */ + session = j1939session_new(skb); + if (!session) + return -ENOMEM; + + session->extd = (skb->len > MAX_TP_PACKET_SIZE) ? EXTENDED : REGULAR; + session->transmission = 1; + session->pkt.total = (skb->len + 6)/7; + session->pkt.block = session->extd ? 255 : + (block ?: session->pkt.total); + if (j1939cb_is_broadcast(session->cb)) + /* set the end-packet for broadcast */ + session->pkt.last = session->pkt.total; + + /* insert into queue, but avoid collision with pending session */ + if (session->cb->msg_flags & MSG_DONTWAIT) + ret = j1939session_insert(session) ? 0 : -EAGAIN; + else + ret = wait_event_interruptible(s.wait, + j1939session_insert(session)); + if (ret < 0) + goto failed; + + ret = j1939tp_tx_initial(session); + if (!ret) + /* transmission started */ + return RESULT_STOP; + sessionlist_lock(); + list_del_init(&session->list); + sessionlist_unlock(); +failed: + /* + * hide the skb from j1939session_drop, as it would + * kfree_skb, but our caller will kfree_skb(skb) too. + */ + session->skb = NULL; + j1939session_drop(session); + return ret; +} + +int j1939_recv_transport(struct sk_buff *skb) +{ + struct j1939_sk_buff_cb *cb = (void *)skb->cb; + const uint8_t *dat; + + switch (cb->pgn) { + case etp_pgn_dat: + j1939xtp_rx_dat(skb, EXTENDED); + break; + case etp_pgn_ctl: + if (skb->len < 8) { + j1939xtp_rx_bad_message(skb, EXTENDED); + break; + } + dat = skb->data; + switch (*dat) { + case etp_cmd_rts: + j1939xtp_rx_rts(skb, EXTENDED); + break; + case etp_cmd_cts: + j1939xtp_rx_cts(skb, EXTENDED); + break; + case etp_cmd_dpo: + j1939xtp_rx_dpo(skb, EXTENDED); + break; + case etp_cmd_eof: + j1939xtp_rx_eof(skb, EXTENDED); + break; + case etp_cmd_abort: + j1939xtp_rx_abort(skb, EXTENDED); + break; + default: + j1939xtp_rx_bad_message(skb, EXTENDED); + break; + } + break; + case tp_pgn_dat: + j1939xtp_rx_dat(skb, REGULAR); + break; + case tp_pgn_ctl: + if (skb->len < 8) { + j1939xtp_rx_bad_message(skb, REGULAR); + break; + } + dat = skb->data; + switch (*dat) { + case tp_cmd_bam: + case tp_cmd_rts: + j1939xtp_rx_rts(skb, REGULAR); + break; + case tp_cmd_cts: + j1939xtp_rx_cts(skb, REGULAR); + break; + case tp_cmd_eof: + j1939xtp_rx_eof(skb, REGULAR); + break; + case tp_cmd_abort: + j1939xtp_rx_abort(skb, REGULAR); + break; + default: + j1939xtp_rx_bad_message(skb, REGULAR); + break; + } + break; + default: + return 0; + } + return RESULT_STOP; +} + +static struct session *j1939session_fresh_new(int size, + struct j1939_sk_buff_cb *rel_cb, pgn_t pgn) +{ + struct sk_buff *skb; + struct j1939_sk_buff_cb *cb; + struct session *session; + + skb = dev_alloc_skb(size); + if (!skb) + return NULL; + cb = (void *)skb->cb; + *cb = *rel_cb; + fix_cb(cb); + cb->pgn = pgn; + + session = j1939session_new(skb); + if (!session) { + kfree(skb); + return NULL; + } + /* alloc data area */ + skb_put(skb, size); + return session; +} +static struct session *j1939session_new(struct sk_buff *skb) +{ + struct session *session; + + session = kzalloc(sizeof(*session), gfp_any()); + if (!session) + return NULL; + INIT_LIST_HEAD(&session->list); + spin_lock_init(&session->lock); + session->skb = skb; + + session->cb = (void *)session->skb->cb; + hrtimer_init(&session->txtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); + session->txtimer.function = j1939tp_txtimer; + hrtimer_init(&session->rxtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); + session->rxtimer.function = j1939tp_rxtimer; + tasklet_init(&session->txtask, j1939tp_txtask, (unsigned long)session); + tasklet_init(&session->rxtask, j1939tp_rxtask, (unsigned long)session); + return session; +} + +static int j1939tp_notifier(struct notifier_block *nb, + unsigned long msg, void *data) +{ + struct net_device *netdev = (struct net_device *)data; + struct session *session, *saved; + + if (!net_eq(dev_net(netdev), &init_net)) + return NOTIFY_DONE; + + if (netdev->type != ARPHRD_CAN) + return NOTIFY_DONE; + + if (msg != NETDEV_UNREGISTER) + return NOTIFY_DONE; + + sessionlist_lock(); + list_for_each_entry_safe(session, saved, &s.sessionq, list) { + if (session->cb->ifindex != netdev->ifindex) + continue; + list_del_init(&session->list); + put_session(session); + } + list_for_each_entry_safe(session, saved, &s.extsessionq, list) { + if (session->cb->ifindex != netdev->ifindex) + continue; + list_del_init(&session->list); + put_session(session); + } + sessionlist_unlock(); + return NOTIFY_DONE; +} + +/* SYSCTL */ +static struct ctl_table_header *j1939tp_table_header; + +static int min_block = 1; +static int max_block = 255; +static int min_packet = 8; +static int max_packet = ((2 << 24)-1)*7; + +static int min_retry = 5; +static int max_retry = 5000; + +static ctl_table j1939tp_table[] = { + { + .procname = "transport_cts_nr_of_frames", + .data = &block, + .maxlen = sizeof(block), + .mode = 0644, + .proc_handler = &proc_dointvec_minmax, + .extra1 = &min_block, + .extra2 = &max_block, + }, + { + .procname = "transport_max_payload_in_bytes", + .data = &max_packet_size, + .maxlen = sizeof(max_packet_size), + .mode = 0644, + .proc_handler = &proc_dointvec_minmax, + .extra1 = &min_packet, + .extra2 = &max_packet, + }, + { + .procname = "transport_tx_retry_ms", + .data = &retry_ms, + .maxlen = sizeof(retry_ms), + .mode = 0644, + .proc_handler = &proc_dointvec_minmax, + .extra1 = &min_retry, + .extra2 = &max_retry, + }, + { }, +}; + +static struct ctl_path j1939tp_path[] = { + { .procname = "net", }, + { .procname = j1939_procname, }, + { } +}; + +/* PROC */ +static int j1939tp_proc_show_session(struct seq_file *sqf, + struct session *session) +{ + seq_printf(sqf, "%i", session->cb->ifindex); + if (session->cb->src.name) + seq_printf(sqf, "\t%016llx", session->cb->src.name); + else + seq_printf(sqf, "\t%02x", session->cb->src.addr); + if (session->cb->dst.name) + seq_printf(sqf, "\t%016llx", session->cb->dst.name); + else if (j1939_address_is_unicast(session->cb->dst.addr)) + seq_printf(sqf, "\t%02x", session->cb->dst.addr); + else + seq_printf(sqf, "\t-"); + seq_printf(sqf, "\t%05x\t%u/%u\n", session->cb->pgn, + session->pkt.done*7, session->skb->len); + return 0; +} + +static int j1939tp_proc_show(struct seq_file *sqf, void *v) +{ + struct session *session; + + seq_printf(sqf, "iface\tsrc\tdst\tpgn\tdone/total\n"); + sessionlist_lock(); + list_for_each_entry(session, &s.sessionq, list) + j1939tp_proc_show_session(sqf, session); + list_for_each_entry(session, &s.extsessionq, list) + j1939tp_proc_show_session(sqf, session); + sessionlist_unlock(); + return 0; +} + +int __init j1939tp_module_init(void) +{ + spin_lock_init(&s.lock); + INIT_LIST_HEAD(&s.sessionq); + INIT_LIST_HEAD(&s.extsessionq); + spin_lock_init(&s.del.lock); + INIT_LIST_HEAD(&s.del.sessionq); + INIT_WORK(&s.del.work, j1939tp_del_work); + + s.notifier.notifier_call = j1939tp_notifier; + register_netdevice_notifier(&s.notifier); + + j1939_proc_add("transport", j1939tp_proc_show, NULL); + j1939tp_table_header = + register_sysctl_paths(j1939tp_path, j1939tp_table); + init_waitqueue_head(&s.wait); + return 0; +} + +void j1939tp_module_exit(void) +{ + struct session *session, *saved; + + wake_up_all(&s.wait); + + unregister_sysctl_table(j1939tp_table_header); + unregister_netdevice_notifier(&s.notifier); + j1939_proc_remove("transport"); + sessionlist_lock(); + list_for_each_entry_safe(session, saved, &s.extsessionq, list) { + list_del_init(&session->list); + put_session(session); + } + list_for_each_entry_safe(session, saved, &s.sessionq, list) { + list_del_init(&session->list); + put_session(session); + } + sessionlist_unlock(); + flush_scheduled_work(); +} +