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