Message-ID: <20110427085847.GD757@kurt.e-circ.dyndns.org>
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