| lists.openwall.net | lists / announce owl-users owl-dev john-users john-dev passwdqc-users yescrypt popa3d-users / oss-security kernel-hardening musl sabotage tlsify passwords / crypt-dev xvendor / Bugtraq Full-Disclosure linux-kernel linux-netdev linux-ext4 linux-hardening linux-cve-announce PHC | |
|
Open Source and information security mailing list archives
| ||
|
Date: Wed, 27 Apr 2011 10:58:47 +0200
From: Kurt Van Dijck <kurt.van.dijck@....be>
To: socketcan-core@...ts.berlios.de, netdev@...r.kernel.org
Subject: [PATCH v4 3/5] can-j1939: Import SAE J1939 stack
Import SAE J1939
Signed-off-by: Kurt Van Dijck <kurt.van.dijck@....be>
---
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 <linux/types.h>
+#include <linux/socket.h>
+#include <linux/can.h>
+
+#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 <pieter.beyens@....be>
+ *
+ * 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 <linux/skbuff.h>
+#include <linux/byteorder/generic.h>
+
+#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 <kurt.van.dijck@....be>
+ *
+ * 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 <linux/version.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/workqueue.h>
+
+#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 <pieter.beyens@....be>
+ * Kurt Van Dijck <kurt.van.dijck@....be>
+ *
+ * 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 <linux/list.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/skbuff.h>
+#include <linux/spinlock.h>
+
+#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 <linux/kref.h>
+#include <linux/list.h>
+#include <net/sock.h>
+
+#include <linux/seq_file.h>
+#include <linux/proc_fs.h>
+#include <linux/module.h>
+#include <linux/can/j1939.h>
+#include <linux/atomic.h>
+
+/* 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 <kurt.van.dijck@....be>
+ * Pieter Beyens <pieter.beyens@....be>
+ *
+ * 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 <linux/version.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/socket.h>
+#include <linux/list.h>
+#include <linux/if_arp.h>
+#include <net/tcp_states.h>
+
+#include <linux/can.h>
+#include <linux/can/core.h>
+#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 <kurt.van.dijck@....be>
+ *
+ * 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 <linux/version.h>
+#include <linux/string.h>
+#include <linux/seq_file.h>
+#include <linux/proc_fs.h>
+#include <linux/uaccess.h>
+
+#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 <kurt.van.dijck@....be>
+ *
+ * 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 <linux/version.h>
+#include <linux/skbuff.h>
+#include <linux/sysctl.h>
+#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 <kurt.van.dijck@....be>
+ *
+ * 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 <linux/version.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/if_arp.h>
+
+#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 <kurt.van.dijck@....be>
+ * Pieter Beyens <pieter.beyens@....be>
+ *
+ * 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 <linux/version.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/socket.h>
+#include <linux/list.h>
+#include <linux/if_arp.h>
+#include <net/tcp_states.h>
+
+#include <linux/can/core.h>
+#include <linux/can/j1939.h>
+#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 <kurt.van.dijck@....be>
+ *
+ * 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 <linux/skbuff.h>
+#include <linux/hrtimer.h>
+#include <linux/version.h>
+#include <linux/if_arp.h>
+#include <linux/wait.h>
+#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();
+}
+
--
1.7.2.5
--
To unsubscribe from this list: send the line "unsubscribe netdev" in
the body of a message to majordomo@...r.kernel.org
More majordomo info at http://vger.kernel.org/majordomo-info.html
Powered by blists - more mailing lists