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_priv = ndev->ml_priv; 144 145 can_ml_priv->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 j1939_priv_put(priv); 197 } 198 199 static void __j1939_rx_release(struct kref *kref) 200 __releases(&j1939_netdev_lock) 201 { 202 struct j1939_priv *priv = container_of(kref, struct j1939_priv, 203 rx_kref); 204 205 j1939_can_rx_unregister(priv); 206 j1939_ecu_unmap_all(priv); 207 j1939_priv_set(priv->ndev, NULL); 208 spin_unlock(&j1939_netdev_lock); 209 } 210 211 /* get pointer to priv without increasing ref counter */ 212 static inline struct j1939_priv *j1939_ndev_to_priv(struct net_device *ndev) 213 { 214 struct can_ml_priv *can_ml_priv = ndev->ml_priv; 215 216 if (!can_ml_priv) 217 return NULL; 218 219 return can_ml_priv->j1939_priv; 220 } 221 222 static struct j1939_priv *j1939_priv_get_by_ndev_locked(struct net_device *ndev) 223 { 224 struct j1939_priv *priv; 225 226 lockdep_assert_held(&j1939_netdev_lock); 227 228 if (ndev->type != ARPHRD_CAN) 229 return NULL; 230 231 priv = j1939_ndev_to_priv(ndev); 232 if (priv) 233 j1939_priv_get(priv); 234 235 return priv; 236 } 237 238 static struct j1939_priv *j1939_priv_get_by_ndev(struct net_device *ndev) 239 { 240 struct j1939_priv *priv; 241 242 spin_lock(&j1939_netdev_lock); 243 priv = j1939_priv_get_by_ndev_locked(ndev); 244 spin_unlock(&j1939_netdev_lock); 245 246 return priv; 247 } 248 249 struct j1939_priv *j1939_netdev_start(struct net_device *ndev) 250 { 251 struct j1939_priv *priv, *priv_new; 252 int ret; 253 254 priv = j1939_priv_get_by_ndev(ndev); 255 if (priv) { 256 kref_get(&priv->rx_kref); 257 return priv; 258 } 259 260 priv = j1939_priv_create(ndev); 261 if (!priv) 262 return ERR_PTR(-ENOMEM); 263 264 j1939_tp_init(priv); 265 spin_lock_init(&priv->j1939_socks_lock); 266 INIT_LIST_HEAD(&priv->j1939_socks); 267 268 spin_lock(&j1939_netdev_lock); 269 priv_new = j1939_priv_get_by_ndev_locked(ndev); 270 if (priv_new) { 271 /* Someone was faster than us, use their priv and roll 272 * back our's. 273 */ 274 spin_unlock(&j1939_netdev_lock); 275 dev_put(ndev); 276 kfree(priv); 277 kref_get(&priv_new->rx_kref); 278 return priv_new; 279 } 280 j1939_priv_set(ndev, priv); 281 spin_unlock(&j1939_netdev_lock); 282 283 ret = j1939_can_rx_register(priv); 284 if (ret < 0) 285 goto out_priv_put; 286 287 return priv; 288 289 out_priv_put: 290 j1939_priv_set(ndev, NULL); 291 dev_put(ndev); 292 kfree(priv); 293 294 return ERR_PTR(ret); 295 } 296 297 void j1939_netdev_stop(struct j1939_priv *priv) 298 { 299 kref_put_lock(&priv->rx_kref, __j1939_rx_release, &j1939_netdev_lock); 300 j1939_priv_put(priv); 301 } 302 303 int j1939_send_one(struct j1939_priv *priv, struct sk_buff *skb) 304 { 305 int ret, dlc; 306 canid_t canid; 307 struct j1939_sk_buff_cb *skcb = j1939_skb_to_cb(skb); 308 struct can_frame *cf; 309 310 /* apply sanity checks */ 311 if (j1939_pgn_is_pdu1(skcb->addr.pgn)) 312 skcb->addr.pgn &= J1939_PGN_PDU1_MAX; 313 else 314 skcb->addr.pgn &= J1939_PGN_MAX; 315 316 if (skcb->priority > 7) 317 skcb->priority = 6; 318 319 ret = j1939_ac_fixup(priv, skb); 320 if (unlikely(ret)) 321 goto failed; 322 dlc = skb->len; 323 324 /* re-claim the CAN_HDR from the SKB */ 325 cf = skb_push(skb, J1939_CAN_HDR); 326 327 /* make it a full can frame again */ 328 skb_put(skb, J1939_CAN_FTR + (8 - dlc)); 329 330 canid = CAN_EFF_FLAG | 331 (skcb->priority << 26) | 332 (skcb->addr.pgn << 8) | 333 skcb->addr.sa; 334 if (j1939_pgn_is_pdu1(skcb->addr.pgn)) 335 canid |= skcb->addr.da << 8; 336 337 cf->can_id = canid; 338 cf->len = dlc; 339 340 return can_send(skb, 1); 341 342 failed: 343 kfree_skb(skb); 344 return ret; 345 } 346 347 static int j1939_netdev_notify(struct notifier_block *nb, 348 unsigned long msg, void *data) 349 { 350 struct net_device *ndev = netdev_notifier_info_to_dev(data); 351 struct j1939_priv *priv; 352 353 priv = j1939_priv_get_by_ndev(ndev); 354 if (!priv) 355 goto notify_done; 356 357 if (ndev->type != ARPHRD_CAN) 358 goto notify_put; 359 360 switch (msg) { 361 case NETDEV_DOWN: 362 j1939_cancel_active_session(priv, NULL); 363 j1939_sk_netdev_event_netdown(priv); 364 j1939_ecu_unmap_all(priv); 365 break; 366 } 367 368 notify_put: 369 j1939_priv_put(priv); 370 371 notify_done: 372 return NOTIFY_DONE; 373 } 374 375 static struct notifier_block j1939_netdev_notifier = { 376 .notifier_call = j1939_netdev_notify, 377 }; 378 379 /* MODULE interface */ 380 static __init int j1939_module_init(void) 381 { 382 int ret; 383 384 pr_info("can: SAE J1939\n"); 385 386 ret = register_netdevice_notifier(&j1939_netdev_notifier); 387 if (ret) 388 goto fail_notifier; 389 390 ret = can_proto_register(&j1939_can_proto); 391 if (ret < 0) { 392 pr_err("can: registration of j1939 protocol failed\n"); 393 goto fail_sk; 394 } 395 396 return 0; 397 398 fail_sk: 399 unregister_netdevice_notifier(&j1939_netdev_notifier); 400 fail_notifier: 401 return ret; 402 } 403 404 static __exit void j1939_module_exit(void) 405 { 406 can_proto_unregister(&j1939_can_proto); 407 408 unregister_netdevice_notifier(&j1939_netdev_notifier); 409 } 410 411 module_init(j1939_module_init); 412 module_exit(j1939_module_exit); 413