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