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