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