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