net/can/j1939/main.c

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