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