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