mirror of https://github.com/Qortal/Brooklyn
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
410 lines
9.4 KiB
410 lines
9.4 KiB
// SPDX-License-Identifier: GPL-2.0 |
|
// Copyright (c) 2010-2011 EIA Electronics, |
|
// Pieter Beyens <[email protected]> |
|
// Copyright (c) 2010-2011 EIA Electronics, |
|
// Kurt Van Dijck <[email protected]> |
|
// Copyright (c) 2018 Protonic, |
|
// Robin van der Gracht <[email protected]> |
|
// Copyright (c) 2017-2019 Pengutronix, |
|
// Marc Kleine-Budde <[email protected]> |
|
// Copyright (c) 2017-2019 Pengutronix, |
|
// Oleksij Rempel <[email protected]> |
|
|
|
/* Core of can-j1939 that links j1939 to CAN. */ |
|
|
|
#include <linux/can/can-ml.h> |
|
#include <linux/can/core.h> |
|
#include <linux/can/skb.h> |
|
#include <linux/if_arp.h> |
|
#include <linux/module.h> |
|
|
|
#include "j1939-priv.h" |
|
|
|
MODULE_DESCRIPTION("PF_CAN SAE J1939"); |
|
MODULE_LICENSE("GPL v2"); |
|
MODULE_AUTHOR("EIA Electronics (Kurt Van Dijck & Pieter Beyens)"); |
|
MODULE_ALIAS("can-proto-" __stringify(CAN_J1939)); |
|
|
|
/* LOWLEVEL CAN interface */ |
|
|
|
/* CAN_HDR: #bytes before can_frame data part */ |
|
#define J1939_CAN_HDR (offsetof(struct can_frame, data)) |
|
|
|
/* CAN_FTR: #bytes beyond data part */ |
|
#define J1939_CAN_FTR (sizeof(struct can_frame) - J1939_CAN_HDR - \ |
|
sizeof(((struct can_frame *)0)->data)) |
|
|
|
/* lowest layer */ |
|
static void j1939_can_recv(struct sk_buff *iskb, void *data) |
|
{ |
|
struct j1939_priv *priv = data; |
|
struct sk_buff *skb; |
|
struct j1939_sk_buff_cb *skcb, *iskcb; |
|
struct can_frame *cf; |
|
|
|
/* create a copy of the skb |
|
* j1939 only delivers the real data bytes, |
|
* the header goes into sockaddr. |
|
* j1939 may not touch the incoming skb in such way |
|
*/ |
|
skb = skb_clone(iskb, GFP_ATOMIC); |
|
if (!skb) |
|
return; |
|
|
|
j1939_priv_get(priv); |
|
can_skb_set_owner(skb, iskb->sk); |
|
|
|
/* 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 |
|
*/ |
|
cf = (void *)skb->data; |
|
skb_pull(skb, J1939_CAN_HDR); |
|
|
|
/* fix length, set to dlc, with 8 maximum */ |
|
skb_trim(skb, min_t(uint8_t, cf->len, 8)); |
|
|
|
/* set addr */ |
|
skcb = j1939_skb_to_cb(skb); |
|
memset(skcb, 0, sizeof(*skcb)); |
|
|
|
iskcb = j1939_skb_to_cb(iskb); |
|
skcb->tskey = iskcb->tskey; |
|
skcb->priority = (cf->can_id >> 26) & 0x7; |
|
skcb->addr.sa = cf->can_id; |
|
skcb->addr.pgn = (cf->can_id >> 8) & J1939_PGN_MAX; |
|
/* set default message type */ |
|
skcb->addr.type = J1939_TP; |
|
if (j1939_pgn_is_pdu1(skcb->addr.pgn)) { |
|
/* Type 1: with destination address */ |
|
skcb->addr.da = skcb->addr.pgn; |
|
/* normalize pgn: strip dst address */ |
|
skcb->addr.pgn &= 0x3ff00; |
|
} else { |
|
/* set broadcast address */ |
|
skcb->addr.da = J1939_NO_ADDR; |
|
} |
|
|
|
/* update localflags */ |
|
read_lock_bh(&priv->lock); |
|
if (j1939_address_is_unicast(skcb->addr.sa) && |
|
priv->ents[skcb->addr.sa].nusers) |
|
skcb->flags |= J1939_ECU_LOCAL_SRC; |
|
if (j1939_address_is_unicast(skcb->addr.da) && |
|
priv->ents[skcb->addr.da].nusers) |
|
skcb->flags |= J1939_ECU_LOCAL_DST; |
|
read_unlock_bh(&priv->lock); |
|
|
|
/* deliver into the j1939 stack ... */ |
|
j1939_ac_recv(priv, skb); |
|
|
|
if (j1939_tp_recv(priv, skb)) |
|
/* this means the transport layer processed the message */ |
|
goto done; |
|
|
|
j1939_simple_recv(priv, skb); |
|
j1939_sk_recv(priv, skb); |
|
done: |
|
j1939_priv_put(priv); |
|
kfree_skb(skb); |
|
} |
|
|
|
/* NETDEV MANAGEMENT */ |
|
|
|
/* values for can_rx_(un)register */ |
|
#define J1939_CAN_ID CAN_EFF_FLAG |
|
#define J1939_CAN_MASK (CAN_EFF_FLAG | CAN_RTR_FLAG) |
|
|
|
static DEFINE_SPINLOCK(j1939_netdev_lock); |
|
|
|
static struct j1939_priv *j1939_priv_create(struct net_device *ndev) |
|
{ |
|
struct j1939_priv *priv; |
|
|
|
priv = kzalloc(sizeof(*priv), GFP_KERNEL); |
|
if (!priv) |
|
return NULL; |
|
|
|
rwlock_init(&priv->lock); |
|
INIT_LIST_HEAD(&priv->ecus); |
|
priv->ndev = ndev; |
|
kref_init(&priv->kref); |
|
kref_init(&priv->rx_kref); |
|
dev_hold(ndev); |
|
|
|
netdev_dbg(priv->ndev, "%s : 0x%p\n", __func__, priv); |
|
|
|
return priv; |
|
} |
|
|
|
static inline void j1939_priv_set(struct net_device *ndev, |
|
struct j1939_priv *priv) |
|
{ |
|
struct can_ml_priv *can_ml = can_get_ml_priv(ndev); |
|
|
|
can_ml->j1939_priv = priv; |
|
} |
|
|
|
static void __j1939_priv_release(struct kref *kref) |
|
{ |
|
struct j1939_priv *priv = container_of(kref, struct j1939_priv, kref); |
|
struct net_device *ndev = priv->ndev; |
|
|
|
netdev_dbg(priv->ndev, "%s: 0x%p\n", __func__, priv); |
|
|
|
WARN_ON_ONCE(!list_empty(&priv->active_session_list)); |
|
WARN_ON_ONCE(!list_empty(&priv->ecus)); |
|
WARN_ON_ONCE(!list_empty(&priv->j1939_socks)); |
|
|
|
dev_put(ndev); |
|
kfree(priv); |
|
} |
|
|
|
void j1939_priv_put(struct j1939_priv *priv) |
|
{ |
|
kref_put(&priv->kref, __j1939_priv_release); |
|
} |
|
|
|
void j1939_priv_get(struct j1939_priv *priv) |
|
{ |
|
kref_get(&priv->kref); |
|
} |
|
|
|
static int j1939_can_rx_register(struct j1939_priv *priv) |
|
{ |
|
struct net_device *ndev = priv->ndev; |
|
int ret; |
|
|
|
j1939_priv_get(priv); |
|
ret = can_rx_register(dev_net(ndev), ndev, J1939_CAN_ID, J1939_CAN_MASK, |
|
j1939_can_recv, priv, "j1939", NULL); |
|
if (ret < 0) { |
|
j1939_priv_put(priv); |
|
return ret; |
|
} |
|
|
|
return 0; |
|
} |
|
|
|
static void j1939_can_rx_unregister(struct j1939_priv *priv) |
|
{ |
|
struct net_device *ndev = priv->ndev; |
|
|
|
can_rx_unregister(dev_net(ndev), ndev, J1939_CAN_ID, J1939_CAN_MASK, |
|
j1939_can_recv, priv); |
|
|
|
/* The last reference of priv is dropped by the RCU deferred |
|
* j1939_sk_sock_destruct() of the last socket, so we can |
|
* safely drop this reference here. |
|
*/ |
|
j1939_priv_put(priv); |
|
} |
|
|
|
static void __j1939_rx_release(struct kref *kref) |
|
__releases(&j1939_netdev_lock) |
|
{ |
|
struct j1939_priv *priv = container_of(kref, struct j1939_priv, |
|
rx_kref); |
|
|
|
j1939_can_rx_unregister(priv); |
|
j1939_ecu_unmap_all(priv); |
|
j1939_priv_set(priv->ndev, NULL); |
|
spin_unlock(&j1939_netdev_lock); |
|
} |
|
|
|
/* get pointer to priv without increasing ref counter */ |
|
static inline struct j1939_priv *j1939_ndev_to_priv(struct net_device *ndev) |
|
{ |
|
struct can_ml_priv *can_ml = can_get_ml_priv(ndev); |
|
|
|
return can_ml->j1939_priv; |
|
} |
|
|
|
static struct j1939_priv *j1939_priv_get_by_ndev_locked(struct net_device *ndev) |
|
{ |
|
struct j1939_priv *priv; |
|
|
|
lockdep_assert_held(&j1939_netdev_lock); |
|
|
|
priv = j1939_ndev_to_priv(ndev); |
|
if (priv) |
|
j1939_priv_get(priv); |
|
|
|
return priv; |
|
} |
|
|
|
static struct j1939_priv *j1939_priv_get_by_ndev(struct net_device *ndev) |
|
{ |
|
struct j1939_priv *priv; |
|
|
|
spin_lock(&j1939_netdev_lock); |
|
priv = j1939_priv_get_by_ndev_locked(ndev); |
|
spin_unlock(&j1939_netdev_lock); |
|
|
|
return priv; |
|
} |
|
|
|
struct j1939_priv *j1939_netdev_start(struct net_device *ndev) |
|
{ |
|
struct j1939_priv *priv, *priv_new; |
|
int ret; |
|
|
|
priv = j1939_priv_get_by_ndev(ndev); |
|
if (priv) { |
|
kref_get(&priv->rx_kref); |
|
return priv; |
|
} |
|
|
|
priv = j1939_priv_create(ndev); |
|
if (!priv) |
|
return ERR_PTR(-ENOMEM); |
|
|
|
j1939_tp_init(priv); |
|
spin_lock_init(&priv->j1939_socks_lock); |
|
INIT_LIST_HEAD(&priv->j1939_socks); |
|
|
|
spin_lock(&j1939_netdev_lock); |
|
priv_new = j1939_priv_get_by_ndev_locked(ndev); |
|
if (priv_new) { |
|
/* Someone was faster than us, use their priv and roll |
|
* back our's. |
|
*/ |
|
spin_unlock(&j1939_netdev_lock); |
|
dev_put(ndev); |
|
kfree(priv); |
|
kref_get(&priv_new->rx_kref); |
|
return priv_new; |
|
} |
|
j1939_priv_set(ndev, priv); |
|
spin_unlock(&j1939_netdev_lock); |
|
|
|
ret = j1939_can_rx_register(priv); |
|
if (ret < 0) |
|
goto out_priv_put; |
|
|
|
return priv; |
|
|
|
out_priv_put: |
|
j1939_priv_set(ndev, NULL); |
|
dev_put(ndev); |
|
kfree(priv); |
|
|
|
return ERR_PTR(ret); |
|
} |
|
|
|
void j1939_netdev_stop(struct j1939_priv *priv) |
|
{ |
|
kref_put_lock(&priv->rx_kref, __j1939_rx_release, &j1939_netdev_lock); |
|
j1939_priv_put(priv); |
|
} |
|
|
|
int j1939_send_one(struct j1939_priv *priv, struct sk_buff *skb) |
|
{ |
|
int ret, dlc; |
|
canid_t canid; |
|
struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb); |
|
struct can_frame *cf; |
|
|
|
/* apply sanity checks */ |
|
if (j1939_pgn_is_pdu1(skcb->addr.pgn)) |
|
skcb->addr.pgn &= J1939_PGN_PDU1_MAX; |
|
else |
|
skcb->addr.pgn &= J1939_PGN_MAX; |
|
|
|
if (skcb->priority > 7) |
|
skcb->priority = 6; |
|
|
|
ret = j1939_ac_fixup(priv, skb); |
|
if (unlikely(ret)) |
|
goto failed; |
|
dlc = skb->len; |
|
|
|
/* re-claim the CAN_HDR from the SKB */ |
|
cf = skb_push(skb, J1939_CAN_HDR); |
|
|
|
/* make it a full can frame again */ |
|
skb_put(skb, J1939_CAN_FTR + (8 - dlc)); |
|
|
|
canid = CAN_EFF_FLAG | |
|
(skcb->priority << 26) | |
|
(skcb->addr.pgn << 8) | |
|
skcb->addr.sa; |
|
if (j1939_pgn_is_pdu1(skcb->addr.pgn)) |
|
canid |= skcb->addr.da << 8; |
|
|
|
cf->can_id = canid; |
|
cf->len = dlc; |
|
|
|
return can_send(skb, 1); |
|
|
|
failed: |
|
kfree_skb(skb); |
|
return ret; |
|
} |
|
|
|
static int j1939_netdev_notify(struct notifier_block *nb, |
|
unsigned long msg, void *data) |
|
{ |
|
struct net_device *ndev = netdev_notifier_info_to_dev(data); |
|
struct can_ml_priv *can_ml = can_get_ml_priv(ndev); |
|
struct j1939_priv *priv; |
|
|
|
if (!can_ml) |
|
goto notify_done; |
|
|
|
priv = j1939_priv_get_by_ndev(ndev); |
|
if (!priv) |
|
goto notify_done; |
|
|
|
switch (msg) { |
|
case NETDEV_DOWN: |
|
j1939_cancel_active_session(priv, NULL); |
|
j1939_sk_netdev_event_netdown(priv); |
|
j1939_ecu_unmap_all(priv); |
|
break; |
|
} |
|
|
|
j1939_priv_put(priv); |
|
|
|
notify_done: |
|
return NOTIFY_DONE; |
|
} |
|
|
|
static struct notifier_block j1939_netdev_notifier = { |
|
.notifier_call = j1939_netdev_notify, |
|
}; |
|
|
|
/* MODULE interface */ |
|
static __init int j1939_module_init(void) |
|
{ |
|
int ret; |
|
|
|
pr_info("can: SAE J1939\n"); |
|
|
|
ret = register_netdevice_notifier(&j1939_netdev_notifier); |
|
if (ret) |
|
goto fail_notifier; |
|
|
|
ret = can_proto_register(&j1939_can_proto); |
|
if (ret < 0) { |
|
pr_err("can: registration of j1939 protocol failed\n"); |
|
goto fail_sk; |
|
} |
|
|
|
return 0; |
|
|
|
fail_sk: |
|
unregister_netdevice_notifier(&j1939_netdev_notifier); |
|
fail_notifier: |
|
return ret; |
|
} |
|
|
|
static __exit void j1939_module_exit(void) |
|
{ |
|
can_proto_unregister(&j1939_can_proto); |
|
|
|
unregister_netdevice_notifier(&j1939_netdev_notifier); |
|
} |
|
|
|
module_init(j1939_module_init); |
|
module_exit(j1939_module_exit);
|
|
|