^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1) // SPDX-License-Identifier: GPL-2.0
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 2) // Copyright (c) 2010-2011 EIA Electronics,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 3) // Pieter Beyens <pieter.beyens@eia.be>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 4) // Copyright (c) 2010-2011 EIA Electronics,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 5) // Kurt Van Dijck <kurt.van.dijck@eia.be>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 6) // Copyright (c) 2018 Protonic,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 7) // Robin van der Gracht <robin@protonic.nl>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 8) // Copyright (c) 2017-2019 Pengutronix,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 9) // Marc Kleine-Budde <kernel@pengutronix.de>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 10) // Copyright (c) 2017-2019 Pengutronix,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 11) // Oleksij Rempel <kernel@pengutronix.de>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 12)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 13) /* Core of can-j1939 that links j1939 to CAN. */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 14)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 15) #include <linux/can/can-ml.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 16) #include <linux/can/core.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 17) #include <linux/can/skb.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 18) #include <linux/if_arp.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 19) #include <linux/module.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 20)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 21) #include "j1939-priv.h"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 22)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 23) MODULE_DESCRIPTION("PF_CAN SAE J1939");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 24) MODULE_LICENSE("GPL v2");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 25) MODULE_AUTHOR("EIA Electronics (Kurt Van Dijck & Pieter Beyens)");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 26) MODULE_ALIAS("can-proto-" __stringify(CAN_J1939));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 27)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 28) /* LOWLEVEL CAN interface */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 29)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 30) /* CAN_HDR: #bytes before can_frame data part */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 31) #define J1939_CAN_HDR (offsetof(struct can_frame, data))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 32)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 33) /* CAN_FTR: #bytes beyond data part */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 34) #define J1939_CAN_FTR (sizeof(struct can_frame) - J1939_CAN_HDR - \
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 35) sizeof(((struct can_frame *)0)->data))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 36)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 37) /* lowest layer */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 38) static void j1939_can_recv(struct sk_buff *iskb, void *data)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 39) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 40) struct j1939_priv *priv = data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 41) struct sk_buff *skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 42) struct j1939_sk_buff_cb *skcb, *iskcb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 43) struct can_frame *cf;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 44)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 45) /* create a copy of the skb
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 46) * j1939 only delivers the real data bytes,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 47) * the header goes into sockaddr.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 48) * j1939 may not touch the incoming skb in such way
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 49) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 50) skb = skb_clone(iskb, GFP_ATOMIC);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 51) if (!skb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 52) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 53)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 54) j1939_priv_get(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 55) can_skb_set_owner(skb, iskb->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 56)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 57) /* get a pointer to the header of the skb
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 58) * the skb payload (pointer) is moved, so that the next skb_data
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 59) * returns the actual payload
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 60) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 61) cf = (void *)skb->data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 62) skb_pull(skb, J1939_CAN_HDR);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 63)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 64) /* fix length, set to dlc, with 8 maximum */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 65) skb_trim(skb, min_t(uint8_t, cf->can_dlc, 8));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 66)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 67) /* set addr */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 68) skcb = j1939_skb_to_cb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 69) memset(skcb, 0, sizeof(*skcb));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 70)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 71) iskcb = j1939_skb_to_cb(iskb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 72) skcb->tskey = iskcb->tskey;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 73) skcb->priority = (cf->can_id >> 26) & 0x7;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 74) skcb->addr.sa = cf->can_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 75) skcb->addr.pgn = (cf->can_id >> 8) & J1939_PGN_MAX;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 76) /* set default message type */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 77) skcb->addr.type = J1939_TP;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 78)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 79) if (!j1939_address_is_valid(skcb->addr.sa)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 80) netdev_err_once(priv->ndev, "%s: sa is broadcast address, ignoring!\n",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 81) __func__);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 82) goto done;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 83) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 84)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 85) if (j1939_pgn_is_pdu1(skcb->addr.pgn)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 86) /* Type 1: with destination address */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 87) skcb->addr.da = skcb->addr.pgn;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 88) /* normalize pgn: strip dst address */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 89) skcb->addr.pgn &= 0x3ff00;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 90) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 91) /* set broadcast address */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 92) skcb->addr.da = J1939_NO_ADDR;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 93) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 94)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 95) /* update localflags */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 96) read_lock_bh(&priv->lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 97) if (j1939_address_is_unicast(skcb->addr.sa) &&
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 98) priv->ents[skcb->addr.sa].nusers)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 99) skcb->flags |= J1939_ECU_LOCAL_SRC;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 100) if (j1939_address_is_unicast(skcb->addr.da) &&
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 101) priv->ents[skcb->addr.da].nusers)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 102) skcb->flags |= J1939_ECU_LOCAL_DST;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 103) read_unlock_bh(&priv->lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 104)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 105) /* deliver into the j1939 stack ... */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 106) j1939_ac_recv(priv, skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 107)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 108) if (j1939_tp_recv(priv, skb))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 109) /* this means the transport layer processed the message */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 110) goto done;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 111)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 112) j1939_simple_recv(priv, skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 113) j1939_sk_recv(priv, skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 114) done:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 115) j1939_priv_put(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 116) kfree_skb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 117) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 118)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 119) /* NETDEV MANAGEMENT */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 120)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 121) /* values for can_rx_(un)register */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 122) #define J1939_CAN_ID CAN_EFF_FLAG
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 123) #define J1939_CAN_MASK (CAN_EFF_FLAG | CAN_RTR_FLAG)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 124)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 125) static DEFINE_SPINLOCK(j1939_netdev_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 126)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 127) static struct j1939_priv *j1939_priv_create(struct net_device *ndev)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 128) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 129) struct j1939_priv *priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 130)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 131) priv = kzalloc(sizeof(*priv), GFP_KERNEL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 132) if (!priv)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 133) return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 134)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 135) rwlock_init(&priv->lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 136) INIT_LIST_HEAD(&priv->ecus);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 137) priv->ndev = ndev;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 138) kref_init(&priv->kref);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 139) kref_init(&priv->rx_kref);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 140) dev_hold(ndev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 141)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 142) netdev_dbg(priv->ndev, "%s : 0x%p\n", __func__, priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 143)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 144) return priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 145) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 146)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 147) static inline void j1939_priv_set(struct net_device *ndev,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 148) struct j1939_priv *priv)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 149) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 150) struct can_ml_priv *can_ml = can_get_ml_priv(ndev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 151)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 152) can_ml->j1939_priv = priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 153) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 154)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 155) static void __j1939_priv_release(struct kref *kref)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 156) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 157) struct j1939_priv *priv = container_of(kref, struct j1939_priv, kref);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 158) struct net_device *ndev = priv->ndev;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 159)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 160) netdev_dbg(priv->ndev, "%s: 0x%p\n", __func__, priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 161)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 162) WARN_ON_ONCE(!list_empty(&priv->active_session_list));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 163) WARN_ON_ONCE(!list_empty(&priv->ecus));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 164) WARN_ON_ONCE(!list_empty(&priv->j1939_socks));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 165)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 166) dev_put(ndev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 167) kfree(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 168) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 169)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 170) void j1939_priv_put(struct j1939_priv *priv)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 171) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 172) kref_put(&priv->kref, __j1939_priv_release);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 173) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 174)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 175) void j1939_priv_get(struct j1939_priv *priv)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 176) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 177) kref_get(&priv->kref);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 178) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 179)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 180) static int j1939_can_rx_register(struct j1939_priv *priv)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 181) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 182) struct net_device *ndev = priv->ndev;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 183) int ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 184)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 185) j1939_priv_get(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 186) ret = can_rx_register(dev_net(ndev), ndev, J1939_CAN_ID, J1939_CAN_MASK,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 187) j1939_can_recv, priv, "j1939", NULL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 188) if (ret < 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 189) j1939_priv_put(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 190) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 191) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 192)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 193) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 194) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 195)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 196) static void j1939_can_rx_unregister(struct j1939_priv *priv)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 197) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 198) struct net_device *ndev = priv->ndev;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 199)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 200) can_rx_unregister(dev_net(ndev), ndev, J1939_CAN_ID, J1939_CAN_MASK,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 201) j1939_can_recv, priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 202)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 203) /* The last reference of priv is dropped by the RCU deferred
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 204) * j1939_sk_sock_destruct() of the last socket, so we can
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 205) * safely drop this reference here.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 206) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 207) j1939_priv_put(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 208) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 209)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 210) static void __j1939_rx_release(struct kref *kref)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 211) __releases(&j1939_netdev_lock)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 212) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 213) struct j1939_priv *priv = container_of(kref, struct j1939_priv,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 214) rx_kref);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 215)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 216) j1939_can_rx_unregister(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 217) j1939_ecu_unmap_all(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 218) j1939_priv_set(priv->ndev, NULL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 219) spin_unlock(&j1939_netdev_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 220) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 221)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 222) /* get pointer to priv without increasing ref counter */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 223) static inline struct j1939_priv *j1939_ndev_to_priv(struct net_device *ndev)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 224) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 225) struct can_ml_priv *can_ml = can_get_ml_priv(ndev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 226)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 227) return can_ml->j1939_priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 228) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 229)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 230) static struct j1939_priv *j1939_priv_get_by_ndev_locked(struct net_device *ndev)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 231) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 232) struct j1939_priv *priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 233)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 234) lockdep_assert_held(&j1939_netdev_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 235)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 236) priv = j1939_ndev_to_priv(ndev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 237) if (priv)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 238) j1939_priv_get(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 239)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 240) return priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 241) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 242)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 243) static struct j1939_priv *j1939_priv_get_by_ndev(struct net_device *ndev)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 244) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 245) struct j1939_priv *priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 246)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 247) spin_lock(&j1939_netdev_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 248) priv = j1939_priv_get_by_ndev_locked(ndev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 249) spin_unlock(&j1939_netdev_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 250)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 251) return priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 252) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 253)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 254) struct j1939_priv *j1939_netdev_start(struct net_device *ndev)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 255) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 256) struct j1939_priv *priv, *priv_new;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 257) int ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 258)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 259) spin_lock(&j1939_netdev_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 260) priv = j1939_priv_get_by_ndev_locked(ndev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 261) if (priv) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 262) kref_get(&priv->rx_kref);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 263) spin_unlock(&j1939_netdev_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 264) return priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 265) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 266) spin_unlock(&j1939_netdev_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 267)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 268) priv = j1939_priv_create(ndev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 269) if (!priv)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 270) return ERR_PTR(-ENOMEM);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 271)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 272) j1939_tp_init(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 273) spin_lock_init(&priv->j1939_socks_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 274) INIT_LIST_HEAD(&priv->j1939_socks);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 275)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 276) spin_lock(&j1939_netdev_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 277) priv_new = j1939_priv_get_by_ndev_locked(ndev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 278) if (priv_new) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 279) /* Someone was faster than us, use their priv and roll
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 280) * back our's.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 281) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 282) kref_get(&priv_new->rx_kref);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 283) spin_unlock(&j1939_netdev_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 284) dev_put(ndev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 285) kfree(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 286) return priv_new;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 287) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 288) j1939_priv_set(ndev, priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 289) spin_unlock(&j1939_netdev_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 290)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 291) ret = j1939_can_rx_register(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 292) if (ret < 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 293) goto out_priv_put;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 294)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 295) return priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 296)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 297) out_priv_put:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 298) j1939_priv_set(ndev, NULL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 299) dev_put(ndev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 300) kfree(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 301)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 302) return ERR_PTR(ret);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 303) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 304)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 305) void j1939_netdev_stop(struct j1939_priv *priv)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 306) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 307) kref_put_lock(&priv->rx_kref, __j1939_rx_release, &j1939_netdev_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 308) j1939_priv_put(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 309) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 310)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 311) int j1939_send_one(struct j1939_priv *priv, struct sk_buff *skb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 312) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 313) int ret, dlc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 314) canid_t canid;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 315) struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 316) struct can_frame *cf;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 317)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 318) /* apply sanity checks */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 319) if (j1939_pgn_is_pdu1(skcb->addr.pgn))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 320) skcb->addr.pgn &= J1939_PGN_PDU1_MAX;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 321) else
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 322) skcb->addr.pgn &= J1939_PGN_MAX;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 323)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 324) if (skcb->priority > 7)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 325) skcb->priority = 6;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 326)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 327) ret = j1939_ac_fixup(priv, skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 328) if (unlikely(ret))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 329) goto failed;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 330) dlc = skb->len;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 331)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 332) /* re-claim the CAN_HDR from the SKB */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 333) cf = skb_push(skb, J1939_CAN_HDR);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 334)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 335) /* make it a full can frame again */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 336) skb_put(skb, J1939_CAN_FTR + (8 - dlc));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 337)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 338) canid = CAN_EFF_FLAG |
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 339) (skcb->priority << 26) |
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 340) (skcb->addr.pgn << 8) |
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 341) skcb->addr.sa;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 342) if (j1939_pgn_is_pdu1(skcb->addr.pgn))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 343) canid |= skcb->addr.da << 8;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 344)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 345) cf->can_id = canid;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 346) cf->can_dlc = dlc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 347)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 348) return can_send(skb, 1);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 349)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 350) failed:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 351) kfree_skb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 352) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 353) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 354)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 355) static int j1939_netdev_notify(struct notifier_block *nb,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 356) unsigned long msg, void *data)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 357) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 358) struct net_device *ndev = netdev_notifier_info_to_dev(data);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 359) struct can_ml_priv *can_ml = can_get_ml_priv(ndev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 360) struct j1939_priv *priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 361)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 362) if (!can_ml)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 363) goto notify_done;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 364)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 365) priv = j1939_priv_get_by_ndev(ndev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 366) if (!priv)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 367) goto notify_done;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 368)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 369) switch (msg) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 370) case NETDEV_DOWN:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 371) j1939_cancel_active_session(priv, NULL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 372) j1939_sk_netdev_event_netdown(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 373) j1939_ecu_unmap_all(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 374) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 375) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 376)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 377) j1939_priv_put(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 378)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 379) notify_done:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 380) return NOTIFY_DONE;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 381) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 382)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 383) static struct notifier_block j1939_netdev_notifier = {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 384) .notifier_call = j1939_netdev_notify,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 385) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 386)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 387) /* MODULE interface */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 388) static __init int j1939_module_init(void)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 389) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 390) int ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 391)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 392) pr_info("can: SAE J1939\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 393)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 394) ret = register_netdevice_notifier(&j1939_netdev_notifier);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 395) if (ret)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 396) goto fail_notifier;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 397)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 398) ret = can_proto_register(&j1939_can_proto);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 399) if (ret < 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 400) pr_err("can: registration of j1939 protocol failed\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 401) goto fail_sk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 402) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 403)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 404) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 405)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 406) fail_sk:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 407) unregister_netdevice_notifier(&j1939_netdev_notifier);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 408) fail_notifier:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 409) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 410) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 411)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 412) static __exit void j1939_module_exit(void)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 413) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 414) can_proto_unregister(&j1939_can_proto);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 415)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 416) unregister_netdevice_notifier(&j1939_netdev_notifier);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 417) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 418)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 419) module_init(j1939_module_init);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 420) module_exit(j1939_module_exit);
Orange Pi5 kernel
Deprecated Linux kernel 5.10.110 for OrangePi 5/5B/5+ boards
3 Commits
0 Branches
0 Tags