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_SPINLOCK(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 spin_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 spin_lock(&j1939_netdev_lock); 252 priv = j1939_priv_get_by_ndev_locked(ndev); 253 spin_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 spin_lock(&j1939_netdev_lock); 264 priv = j1939_priv_get_by_ndev_locked(ndev); 265 if (priv) { 266 kref_get(&priv->rx_kref); 267 spin_unlock(&j1939_netdev_lock); 268 return priv; 269 } 270 spin_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 spin_lock_init(&priv->j1939_socks_lock); 278 INIT_LIST_HEAD(&priv->j1939_socks); 279 280 spin_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 spin_unlock(&j1939_netdev_lock); 288 dev_put(ndev); 289 kfree(priv); 290 return priv_new; 291 } 292 j1939_priv_set(ndev, priv); 293 spin_unlock(&j1939_netdev_lock); 294 295 ret = j1939_can_rx_register(priv); 296 if (ret < 0) 297 goto out_priv_put; 298 299 return priv; 300 301 out_priv_put: 302 j1939_priv_set(ndev, NULL); 303 dev_put(ndev); 304 kfree(priv); 305 306 return ERR_PTR(ret); 307 } 308 309 void j1939_netdev_stop(struct j1939_priv *priv) 310 { 311 kref_put_lock(&priv->rx_kref, __j1939_rx_release, &j1939_netdev_lock); 312 j1939_priv_put(priv); 313 } 314 315 int j1939_send_one(struct j1939_priv *priv, struct sk_buff *skb) 316 { 317 int ret, dlc; 318 canid_t canid; 319 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb); 320 struct can_frame *cf; 321 322 /* apply sanity checks */ 323 if (j1939_pgn_is_pdu1(skcb->addr.pgn)) 324 skcb->addr.pgn &= J1939_PGN_PDU1_MAX; 325 else 326 skcb->addr.pgn &= J1939_PGN_MAX; 327 328 if (skcb->priority > 7) 329 skcb->priority = 6; 330 331 ret = j1939_ac_fixup(priv, skb); 332 if (unlikely(ret)) 333 goto failed; 334 dlc = skb->len; 335 336 /* re-claim the CAN_HDR from the SKB */ 337 cf = skb_push(skb, J1939_CAN_HDR); 338 339 /* initialize header structure */ 340 memset(cf, 0, J1939_CAN_HDR); 341 342 /* make it a full can frame again */ 343 skb_put(skb, J1939_CAN_FTR + (8 - dlc)); 344 345 canid = CAN_EFF_FLAG | 346 (skcb->priority << 26) | 347 (skcb->addr.pgn << 8) | 348 skcb->addr.sa; 349 if (j1939_pgn_is_pdu1(skcb->addr.pgn)) 350 canid |= skcb->addr.da << 8; 351 352 cf->can_id = canid; 353 cf->len = dlc; 354 355 return can_send(skb, 1); 356 357 failed: 358 kfree_skb(skb); 359 return ret; 360 } 361 362 static int j1939_netdev_notify(struct notifier_block *nb, 363 unsigned long msg, void *data) 364 { 365 struct net_device *ndev = netdev_notifier_info_to_dev(data); 366 struct can_ml_priv *can_ml = can_get_ml_priv(ndev); 367 struct j1939_priv *priv; 368 369 if (!can_ml) 370 goto notify_done; 371 372 priv = j1939_priv_get_by_ndev(ndev); 373 if (!priv) 374 goto notify_done; 375 376 switch (msg) { 377 case NETDEV_DOWN: 378 j1939_cancel_active_session(priv, NULL); 379 j1939_sk_netdev_event_netdown(priv); 380 j1939_ecu_unmap_all(priv); 381 break; 382 } 383 384 j1939_priv_put(priv); 385 386 notify_done: 387 return NOTIFY_DONE; 388 } 389 390 static struct notifier_block j1939_netdev_notifier = { 391 .notifier_call = j1939_netdev_notify, 392 }; 393 394 /* MODULE interface */ 395 static __init int j1939_module_init(void) 396 { 397 int ret; 398 399 pr_info("can: SAE J1939\n"); 400 401 ret = register_netdevice_notifier(&j1939_netdev_notifier); 402 if (ret) 403 goto fail_notifier; 404 405 ret = can_proto_register(&j1939_can_proto); 406 if (ret < 0) { 407 pr_err("can: registration of j1939 protocol failed\n"); 408 goto fail_sk; 409 } 410 411 return 0; 412 413 fail_sk: 414 unregister_netdevice_notifier(&j1939_netdev_notifier); 415 fail_notifier: 416 return ret; 417 } 418 419 static __exit void j1939_module_exit(void) 420 { 421 can_proto_unregister(&j1939_can_proto); 422 423 unregister_netdevice_notifier(&j1939_netdev_notifier); 424 } 425 426 module_init(j1939_module_init); 427 module_exit(j1939_module_exit); 428