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