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); 426