kernel_optimize_test/net/can/j1939/main.c

// SPDX-License-Identifier: GPL-2.0
// Copyright (c) 2010-2011 EIA Electronics,
//                         Pieter Beyens <pieter.beyens@eia.be>
// Copyright (c) 2010-2011 EIA Electronics,
//                         Kurt Van Dijck <kurt.van.dijck@eia.be>
// Copyright (c) 2018 Protonic,
//                         Robin van der Gracht <robin@protonic.nl>
// Copyright (c) 2017-2019 Pengutronix,
//                         Marc Kleine-Budde <kernel@pengutronix.de>
// Copyright (c) 2017-2019 Pengutronix,
//                         Oleksij Rempel <kernel@pengutronix.de>

/* 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->can_dlc, 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_priv = ndev->ml_priv;

	can_ml_priv->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);

	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_priv = ndev->ml_priv;

	if (!can_ml_priv)
		return NULL;

	return can_ml_priv->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);

	if (ndev->type != ARPHRD_CAN)
		return NULL;

	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->can_dlc = 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 j1939_priv *priv;

	priv = j1939_priv_get_by_ndev(ndev);
	if (!priv)
		goto notify_done;

	if (ndev->type != ARPHRD_CAN)
		goto notify_put;

	switch (msg) {
	case NETDEV_DOWN:
		j1939_cancel_active_session(priv, NULL);
		j1939_sk_netdev_event_netdown(priv);
		j1939_ecu_unmap_all(priv);
		break;
	}

notify_put:
	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);