^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1) // SPDX-License-Identifier: GPL-2.0
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 2) // Copyright (c) 2010-2011 EIA Electronics,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 3) // Pieter Beyens <pieter.beyens@eia.be>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 4) // Copyright (c) 2010-2011 EIA Electronics,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 5) // Kurt Van Dijck <kurt.van.dijck@eia.be>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 6) // Copyright (c) 2018 Protonic,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 7) // Robin van der Gracht <robin@protonic.nl>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 8) // Copyright (c) 2017-2019 Pengutronix,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 9) // Marc Kleine-Budde <kernel@pengutronix.de>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 10) // Copyright (c) 2017-2019 Pengutronix,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 11) // Oleksij Rempel <kernel@pengutronix.de>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 12)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 13) #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 14)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 15) #include <linux/can/can-ml.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 16) #include <linux/can/core.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 17) #include <linux/can/skb.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 18) #include <linux/errqueue.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 19) #include <linux/if_arp.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 20)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 21) #include "j1939-priv.h"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 22)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 23) #define J1939_MIN_NAMELEN CAN_REQUIRED_SIZE(struct sockaddr_can, can_addr.j1939)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 24)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 25) /* conversion function between struct sock::sk_priority from linux and
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 26) * j1939 priority field
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 27) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 28) static inline priority_t j1939_prio(u32 sk_priority)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 29) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 30) sk_priority = min(sk_priority, 7U);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 31)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 32) return 7 - sk_priority;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 33) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 34)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 35) static inline u32 j1939_to_sk_priority(priority_t prio)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 36) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 37) return 7 - prio;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 38) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 39)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 40) /* function to see if pgn is to be evaluated */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 41) static inline bool j1939_pgn_is_valid(pgn_t pgn)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 42) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 43) return pgn <= J1939_PGN_MAX;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 44) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 45)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 46) /* test function to avoid non-zero DA placeholder for pdu1 pgn's */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 47) static inline bool j1939_pgn_is_clean_pdu(pgn_t pgn)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 48) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 49) if (j1939_pgn_is_pdu1(pgn))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 50) return !(pgn & 0xff);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 51) else
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 52) return true;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 53) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 54)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 55) static inline void j1939_sock_pending_add(struct sock *sk)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 56) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 57) struct j1939_sock *jsk = j1939_sk(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 58)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 59) atomic_inc(&jsk->skb_pending);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 60) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 61)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 62) static int j1939_sock_pending_get(struct sock *sk)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 63) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 64) struct j1939_sock *jsk = j1939_sk(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 65)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 66) return atomic_read(&jsk->skb_pending);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 67) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 68)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 69) void j1939_sock_pending_del(struct sock *sk)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 70) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 71) struct j1939_sock *jsk = j1939_sk(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 72)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 73) /* atomic_dec_return returns the new value */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 74) if (!atomic_dec_return(&jsk->skb_pending))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 75) wake_up(&jsk->waitq); /* no pending SKB's */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 76) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 77)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 78) static void j1939_jsk_add(struct j1939_priv *priv, struct j1939_sock *jsk)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 79) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 80) jsk->state |= J1939_SOCK_BOUND;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 81) j1939_priv_get(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 82)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 83) spin_lock_bh(&priv->j1939_socks_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 84) list_add_tail(&jsk->list, &priv->j1939_socks);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 85) spin_unlock_bh(&priv->j1939_socks_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 86) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 87)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 88) static void j1939_jsk_del(struct j1939_priv *priv, struct j1939_sock *jsk)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 89) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 90) spin_lock_bh(&priv->j1939_socks_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 91) list_del_init(&jsk->list);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 92) spin_unlock_bh(&priv->j1939_socks_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 93)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 94) j1939_priv_put(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 95) jsk->state &= ~J1939_SOCK_BOUND;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 96) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 97)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 98) static bool j1939_sk_queue_session(struct j1939_session *session)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 99) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 100) struct j1939_sock *jsk = j1939_sk(session->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 101) bool empty;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 102)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 103) spin_lock_bh(&jsk->sk_session_queue_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 104) empty = list_empty(&jsk->sk_session_queue);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 105) j1939_session_get(session);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 106) list_add_tail(&session->sk_session_queue_entry, &jsk->sk_session_queue);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 107) spin_unlock_bh(&jsk->sk_session_queue_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 108) j1939_sock_pending_add(&jsk->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 109)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 110) return empty;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 111) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 112)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 113) static struct
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 114) j1939_session *j1939_sk_get_incomplete_session(struct j1939_sock *jsk)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 115) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 116) struct j1939_session *session = NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 117)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 118) spin_lock_bh(&jsk->sk_session_queue_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 119) if (!list_empty(&jsk->sk_session_queue)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 120) session = list_last_entry(&jsk->sk_session_queue,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 121) struct j1939_session,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 122) sk_session_queue_entry);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 123) if (session->total_queued_size == session->total_message_size)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 124) session = NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 125) else
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 126) j1939_session_get(session);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 127) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 128) spin_unlock_bh(&jsk->sk_session_queue_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 129)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 130) return session;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 131) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 132)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 133) static void j1939_sk_queue_drop_all(struct j1939_priv *priv,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 134) struct j1939_sock *jsk, int err)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 135) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 136) struct j1939_session *session, *tmp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 137)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 138) netdev_dbg(priv->ndev, "%s: err: %i\n", __func__, err);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 139) spin_lock_bh(&jsk->sk_session_queue_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 140) list_for_each_entry_safe(session, tmp, &jsk->sk_session_queue,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 141) sk_session_queue_entry) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 142) list_del_init(&session->sk_session_queue_entry);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 143) session->err = err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 144) j1939_session_put(session);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 145) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 146) spin_unlock_bh(&jsk->sk_session_queue_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 147) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 148)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 149) static void j1939_sk_queue_activate_next_locked(struct j1939_session *session)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 150) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 151) struct j1939_sock *jsk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 152) struct j1939_session *first;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 153) int err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 154)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 155) /* RX-Session don't have a socket (yet) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 156) if (!session->sk)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 157) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 158)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 159) jsk = j1939_sk(session->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 160) lockdep_assert_held(&jsk->sk_session_queue_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 161)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 162) err = session->err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 163)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 164) first = list_first_entry_or_null(&jsk->sk_session_queue,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 165) struct j1939_session,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 166) sk_session_queue_entry);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 167)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 168) /* Some else has already activated the next session */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 169) if (first != session)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 170) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 171)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 172) activate_next:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 173) list_del_init(&first->sk_session_queue_entry);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 174) j1939_session_put(first);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 175) first = list_first_entry_or_null(&jsk->sk_session_queue,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 176) struct j1939_session,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 177) sk_session_queue_entry);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 178) if (!first)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 179) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 180)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 181) if (WARN_ON_ONCE(j1939_session_activate(first))) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 182) first->err = -EBUSY;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 183) goto activate_next;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 184) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 185) /* Give receiver some time (arbitrary chosen) to recover */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 186) int time_ms = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 187)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 188) if (err)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 189) time_ms = 10 + prandom_u32_max(16);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 190)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 191) j1939_tp_schedule_txtimer(first, time_ms);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 192) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 193) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 194)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 195) void j1939_sk_queue_activate_next(struct j1939_session *session)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 196) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 197) struct j1939_sock *jsk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 198)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 199) if (!session->sk)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 200) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 201)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 202) jsk = j1939_sk(session->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 203)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 204) spin_lock_bh(&jsk->sk_session_queue_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 205) j1939_sk_queue_activate_next_locked(session);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 206) spin_unlock_bh(&jsk->sk_session_queue_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 207) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 208)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 209) static bool j1939_sk_match_dst(struct j1939_sock *jsk,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 210) const struct j1939_sk_buff_cb *skcb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 211) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 212) if ((jsk->state & J1939_SOCK_PROMISC))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 213) return true;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 214)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 215) /* Destination address filter */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 216) if (jsk->addr.src_name && skcb->addr.dst_name) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 217) if (jsk->addr.src_name != skcb->addr.dst_name)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 218) return false;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 219) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 220) /* receive (all sockets) if
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 221) * - all packages that match our bind() address
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 222) * - all broadcast on a socket if SO_BROADCAST
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 223) * is set
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 224) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 225) if (j1939_address_is_unicast(skcb->addr.da)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 226) if (jsk->addr.sa != skcb->addr.da)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 227) return false;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 228) } else if (!sock_flag(&jsk->sk, SOCK_BROADCAST)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 229) /* receiving broadcast without SO_BROADCAST
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 230) * flag is not allowed
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 231) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 232) return false;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 233) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 234) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 235)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 236) /* Source address filter */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 237) if (jsk->state & J1939_SOCK_CONNECTED) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 238) /* receive (all sockets) if
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 239) * - all packages that match our connect() name or address
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 240) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 241) if (jsk->addr.dst_name && skcb->addr.src_name) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 242) if (jsk->addr.dst_name != skcb->addr.src_name)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 243) return false;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 244) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 245) if (jsk->addr.da != skcb->addr.sa)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 246) return false;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 247) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 248) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 249)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 250) /* PGN filter */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 251) if (j1939_pgn_is_valid(jsk->pgn_rx_filter) &&
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 252) jsk->pgn_rx_filter != skcb->addr.pgn)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 253) return false;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 254)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 255) return true;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 256) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 257)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 258) /* matches skb control buffer (addr) with a j1939 filter */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 259) static bool j1939_sk_match_filter(struct j1939_sock *jsk,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 260) const struct j1939_sk_buff_cb *skcb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 261) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 262) const struct j1939_filter *f = jsk->filters;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 263) int nfilter = jsk->nfilters;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 264)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 265) if (!nfilter)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 266) /* receive all when no filters are assigned */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 267) return true;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 268)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 269) for (; nfilter; ++f, --nfilter) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 270) if ((skcb->addr.pgn & f->pgn_mask) != f->pgn)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 271) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 272) if ((skcb->addr.sa & f->addr_mask) != f->addr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 273) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 274) if ((skcb->addr.src_name & f->name_mask) != f->name)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 275) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 276) return true;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 277) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 278) return false;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 279) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 280)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 281) static bool j1939_sk_recv_match_one(struct j1939_sock *jsk,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 282) const struct j1939_sk_buff_cb *skcb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 283) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 284) if (!(jsk->state & J1939_SOCK_BOUND))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 285) return false;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 286)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 287) if (!j1939_sk_match_dst(jsk, skcb))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 288) return false;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 289)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 290) if (!j1939_sk_match_filter(jsk, skcb))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 291) return false;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 292)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 293) return true;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 294) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 295)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 296) static void j1939_sk_recv_one(struct j1939_sock *jsk, struct sk_buff *oskb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 297) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 298) const struct j1939_sk_buff_cb *oskcb = j1939_skb_to_cb(oskb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 299) struct j1939_sk_buff_cb *skcb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 300) struct sk_buff *skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 301)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 302) if (oskb->sk == &jsk->sk)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 303) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 304)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 305) if (!j1939_sk_recv_match_one(jsk, oskcb))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 306) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 307)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 308) skb = skb_clone(oskb, GFP_ATOMIC);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 309) if (!skb) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 310) pr_warn("skb clone failed\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 311) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 312) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 313) can_skb_set_owner(skb, oskb->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 314)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 315) skcb = j1939_skb_to_cb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 316) skcb->msg_flags &= ~(MSG_DONTROUTE);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 317) if (skb->sk)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 318) skcb->msg_flags |= MSG_DONTROUTE;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 319)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 320) if (sock_queue_rcv_skb(&jsk->sk, skb) < 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 321) kfree_skb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 322) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 323)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 324) bool j1939_sk_recv_match(struct j1939_priv *priv, struct j1939_sk_buff_cb *skcb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 325) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 326) struct j1939_sock *jsk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 327) bool match = false;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 328)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 329) spin_lock_bh(&priv->j1939_socks_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 330) list_for_each_entry(jsk, &priv->j1939_socks, list) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 331) match = j1939_sk_recv_match_one(jsk, skcb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 332) if (match)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 333) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 334) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 335) spin_unlock_bh(&priv->j1939_socks_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 336)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 337) return match;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 338) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 339)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 340) void j1939_sk_recv(struct j1939_priv *priv, struct sk_buff *skb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 341) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 342) struct j1939_sock *jsk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 343)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 344) spin_lock_bh(&priv->j1939_socks_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 345) list_for_each_entry(jsk, &priv->j1939_socks, list) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 346) j1939_sk_recv_one(jsk, skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 347) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 348) spin_unlock_bh(&priv->j1939_socks_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 349) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 350)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 351) static void j1939_sk_sock_destruct(struct sock *sk)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 352) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 353) struct j1939_sock *jsk = j1939_sk(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 354)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 355) /* This function will be call by the generic networking code, when then
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 356) * the socket is ultimately closed (sk->sk_destruct).
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 357) *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 358) * The race between
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 359) * - processing a received CAN frame
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 360) * (can_receive -> j1939_can_recv)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 361) * and accessing j1939_priv
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 362) * ... and ...
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 363) * - closing a socket
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 364) * (j1939_can_rx_unregister -> can_rx_unregister)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 365) * and calling the final j1939_priv_put()
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 366) *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 367) * is avoided by calling the final j1939_priv_put() from this
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 368) * RCU deferred cleanup call.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 369) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 370) if (jsk->priv) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 371) j1939_priv_put(jsk->priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 372) jsk->priv = NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 373) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 374)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 375) /* call generic CAN sock destruct */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 376) can_sock_destruct(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 377) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 378)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 379) static int j1939_sk_init(struct sock *sk)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 380) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 381) struct j1939_sock *jsk = j1939_sk(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 382)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 383) /* Ensure that "sk" is first member in "struct j1939_sock", so that we
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 384) * can skip it during memset().
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 385) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 386) BUILD_BUG_ON(offsetof(struct j1939_sock, sk) != 0);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 387) memset((void *)jsk + sizeof(jsk->sk), 0x0,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 388) sizeof(*jsk) - sizeof(jsk->sk));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 389)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 390) INIT_LIST_HEAD(&jsk->list);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 391) init_waitqueue_head(&jsk->waitq);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 392) jsk->sk.sk_priority = j1939_to_sk_priority(6);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 393) jsk->sk.sk_reuse = 1; /* per default */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 394) jsk->addr.sa = J1939_NO_ADDR;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 395) jsk->addr.da = J1939_NO_ADDR;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 396) jsk->addr.pgn = J1939_NO_PGN;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 397) jsk->pgn_rx_filter = J1939_NO_PGN;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 398) atomic_set(&jsk->skb_pending, 0);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 399) spin_lock_init(&jsk->sk_session_queue_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 400) INIT_LIST_HEAD(&jsk->sk_session_queue);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 401)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 402) /* j1939_sk_sock_destruct() depends on SOCK_RCU_FREE flag */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 403) sock_set_flag(sk, SOCK_RCU_FREE);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 404) sk->sk_destruct = j1939_sk_sock_destruct;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 405) sk->sk_protocol = CAN_J1939;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 406)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 407) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 408) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 409)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 410) static int j1939_sk_sanity_check(struct sockaddr_can *addr, int len)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 411) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 412) if (!addr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 413) return -EDESTADDRREQ;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 414) if (len < J1939_MIN_NAMELEN)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 415) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 416) if (addr->can_family != AF_CAN)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 417) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 418) if (!addr->can_ifindex)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 419) return -ENODEV;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 420) if (j1939_pgn_is_valid(addr->can_addr.j1939.pgn) &&
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 421) !j1939_pgn_is_clean_pdu(addr->can_addr.j1939.pgn))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 422) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 423)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 424) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 425) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 426)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 427) static int j1939_sk_bind(struct socket *sock, struct sockaddr *uaddr, int len)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 428) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 429) struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 430) struct j1939_sock *jsk = j1939_sk(sock->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 431) struct j1939_priv *priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 432) struct sock *sk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 433) struct net *net;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 434) int ret = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 435)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 436) ret = j1939_sk_sanity_check(addr, len);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 437) if (ret)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 438) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 439)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 440) lock_sock(sock->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 441)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 442) priv = jsk->priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 443) sk = sock->sk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 444) net = sock_net(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 445)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 446) /* Already bound to an interface? */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 447) if (jsk->state & J1939_SOCK_BOUND) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 448) /* A re-bind() to a different interface is not
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 449) * supported.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 450) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 451) if (jsk->ifindex != addr->can_ifindex) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 452) ret = -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 453) goto out_release_sock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 454) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 455)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 456) /* drop old references */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 457) j1939_jsk_del(priv, jsk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 458) j1939_local_ecu_put(priv, jsk->addr.src_name, jsk->addr.sa);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 459) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 460) struct can_ml_priv *can_ml;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 461) struct net_device *ndev;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 462)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 463) ndev = dev_get_by_index(net, addr->can_ifindex);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 464) if (!ndev) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 465) ret = -ENODEV;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 466) goto out_release_sock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 467) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 468)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 469) can_ml = can_get_ml_priv(ndev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 470) if (!can_ml) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 471) dev_put(ndev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 472) ret = -ENODEV;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 473) goto out_release_sock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 474) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 475)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 476) if (!(ndev->flags & IFF_UP)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 477) dev_put(ndev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 478) ret = -ENETDOWN;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 479) goto out_release_sock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 480) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 481)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 482) priv = j1939_netdev_start(ndev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 483) dev_put(ndev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 484) if (IS_ERR(priv)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 485) ret = PTR_ERR(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 486) goto out_release_sock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 487) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 488)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 489) jsk->ifindex = addr->can_ifindex;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 490)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 491) /* the corresponding j1939_priv_put() is called via
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 492) * sk->sk_destruct, which points to j1939_sk_sock_destruct()
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 493) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 494) j1939_priv_get(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 495) jsk->priv = priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 496) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 497)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 498) /* set default transmit pgn */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 499) if (j1939_pgn_is_valid(addr->can_addr.j1939.pgn))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 500) jsk->pgn_rx_filter = addr->can_addr.j1939.pgn;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 501) jsk->addr.src_name = addr->can_addr.j1939.name;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 502) jsk->addr.sa = addr->can_addr.j1939.addr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 503)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 504) /* get new references */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 505) ret = j1939_local_ecu_get(priv, jsk->addr.src_name, jsk->addr.sa);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 506) if (ret) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 507) j1939_netdev_stop(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 508) goto out_release_sock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 509) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 510)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 511) j1939_jsk_add(priv, jsk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 512)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 513) out_release_sock: /* fall through */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 514) release_sock(sock->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 515)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 516) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 517) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 518)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 519) static int j1939_sk_connect(struct socket *sock, struct sockaddr *uaddr,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 520) int len, int flags)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 521) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 522) struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 523) struct j1939_sock *jsk = j1939_sk(sock->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 524) int ret = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 525)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 526) ret = j1939_sk_sanity_check(addr, len);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 527) if (ret)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 528) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 529)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 530) lock_sock(sock->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 531)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 532) /* bind() before connect() is mandatory */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 533) if (!(jsk->state & J1939_SOCK_BOUND)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 534) ret = -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 535) goto out_release_sock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 536) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 537)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 538) /* A connect() to a different interface is not supported. */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 539) if (jsk->ifindex != addr->can_ifindex) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 540) ret = -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 541) goto out_release_sock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 542) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 543)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 544) if (!addr->can_addr.j1939.name &&
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 545) addr->can_addr.j1939.addr == J1939_NO_ADDR &&
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 546) !sock_flag(&jsk->sk, SOCK_BROADCAST)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 547) /* broadcast, but SO_BROADCAST not set */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 548) ret = -EACCES;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 549) goto out_release_sock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 550) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 551)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 552) jsk->addr.dst_name = addr->can_addr.j1939.name;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 553) jsk->addr.da = addr->can_addr.j1939.addr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 554)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 555) if (j1939_pgn_is_valid(addr->can_addr.j1939.pgn))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 556) jsk->addr.pgn = addr->can_addr.j1939.pgn;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 557)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 558) jsk->state |= J1939_SOCK_CONNECTED;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 559)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 560) out_release_sock: /* fall through */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 561) release_sock(sock->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 562)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 563) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 564) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 565)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 566) static void j1939_sk_sock2sockaddr_can(struct sockaddr_can *addr,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 567) const struct j1939_sock *jsk, int peer)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 568) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 569) /* There are two holes (2 bytes and 3 bytes) to clear to avoid
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 570) * leaking kernel information to user space.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 571) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 572) memset(addr, 0, J1939_MIN_NAMELEN);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 573)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 574) addr->can_family = AF_CAN;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 575) addr->can_ifindex = jsk->ifindex;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 576) addr->can_addr.j1939.pgn = jsk->addr.pgn;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 577) if (peer) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 578) addr->can_addr.j1939.name = jsk->addr.dst_name;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 579) addr->can_addr.j1939.addr = jsk->addr.da;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 580) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 581) addr->can_addr.j1939.name = jsk->addr.src_name;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 582) addr->can_addr.j1939.addr = jsk->addr.sa;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 583) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 584) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 585)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 586) static int j1939_sk_getname(struct socket *sock, struct sockaddr *uaddr,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 587) int peer)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 588) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 589) struct sockaddr_can *addr = (struct sockaddr_can *)uaddr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 590) struct sock *sk = sock->sk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 591) struct j1939_sock *jsk = j1939_sk(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 592) int ret = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 593)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 594) lock_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 595)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 596) if (peer && !(jsk->state & J1939_SOCK_CONNECTED)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 597) ret = -EADDRNOTAVAIL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 598) goto failure;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 599) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 600)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 601) j1939_sk_sock2sockaddr_can(addr, jsk, peer);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 602) ret = J1939_MIN_NAMELEN;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 603)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 604) failure:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 605) release_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 606)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 607) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 608) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 609)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 610) static int j1939_sk_release(struct socket *sock)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 611) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 612) struct sock *sk = sock->sk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 613) struct j1939_sock *jsk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 614)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 615) if (!sk)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 616) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 617)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 618) lock_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 619) jsk = j1939_sk(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 620)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 621) if (jsk->state & J1939_SOCK_BOUND) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 622) struct j1939_priv *priv = jsk->priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 623)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 624) if (wait_event_interruptible(jsk->waitq,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 625) !j1939_sock_pending_get(&jsk->sk))) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 626) j1939_cancel_active_session(priv, sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 627) j1939_sk_queue_drop_all(priv, jsk, ESHUTDOWN);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 628) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 629)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 630) j1939_jsk_del(priv, jsk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 631)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 632) j1939_local_ecu_put(priv, jsk->addr.src_name,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 633) jsk->addr.sa);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 634)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 635) j1939_netdev_stop(priv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 636) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 637)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 638) kfree(jsk->filters);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 639) sock_orphan(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 640) sock->sk = NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 641)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 642) release_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 643) sock_put(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 644)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 645) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 646) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 647)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 648) static int j1939_sk_setsockopt_flag(struct j1939_sock *jsk, sockptr_t optval,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 649) unsigned int optlen, int flag)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 650) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 651) int tmp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 652)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 653) if (optlen != sizeof(tmp))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 654) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 655) if (copy_from_sockptr(&tmp, optval, optlen))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 656) return -EFAULT;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 657) lock_sock(&jsk->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 658) if (tmp)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 659) jsk->state |= flag;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 660) else
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 661) jsk->state &= ~flag;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 662) release_sock(&jsk->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 663) return tmp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 664) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 665)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 666) static int j1939_sk_setsockopt(struct socket *sock, int level, int optname,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 667) sockptr_t optval, unsigned int optlen)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 668) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 669) struct sock *sk = sock->sk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 670) struct j1939_sock *jsk = j1939_sk(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 671) int tmp, count = 0, ret = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 672) struct j1939_filter *filters = NULL, *ofilters;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 673)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 674) if (level != SOL_CAN_J1939)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 675) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 676)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 677) switch (optname) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 678) case SO_J1939_FILTER:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 679) if (!sockptr_is_null(optval) && optlen != 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 680) struct j1939_filter *f;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 681) int c;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 682)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 683) if (optlen % sizeof(*filters) != 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 684) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 685)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 686) if (optlen > J1939_FILTER_MAX *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 687) sizeof(struct j1939_filter))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 688) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 689)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 690) count = optlen / sizeof(*filters);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 691) filters = memdup_sockptr(optval, optlen);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 692) if (IS_ERR(filters))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 693) return PTR_ERR(filters);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 694)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 695) for (f = filters, c = count; c; f++, c--) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 696) f->name &= f->name_mask;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 697) f->pgn &= f->pgn_mask;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 698) f->addr &= f->addr_mask;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 699) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 700) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 701)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 702) lock_sock(&jsk->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 703) ofilters = jsk->filters;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 704) jsk->filters = filters;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 705) jsk->nfilters = count;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 706) release_sock(&jsk->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 707) kfree(ofilters);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 708) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 709) case SO_J1939_PROMISC:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 710) return j1939_sk_setsockopt_flag(jsk, optval, optlen,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 711) J1939_SOCK_PROMISC);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 712) case SO_J1939_ERRQUEUE:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 713) ret = j1939_sk_setsockopt_flag(jsk, optval, optlen,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 714) J1939_SOCK_ERRQUEUE);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 715) if (ret < 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 716) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 717)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 718) if (!(jsk->state & J1939_SOCK_ERRQUEUE))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 719) skb_queue_purge(&sk->sk_error_queue);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 720) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 721) case SO_J1939_SEND_PRIO:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 722) if (optlen != sizeof(tmp))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 723) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 724) if (copy_from_sockptr(&tmp, optval, optlen))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 725) return -EFAULT;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 726) if (tmp < 0 || tmp > 7)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 727) return -EDOM;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 728) if (tmp < 2 && !capable(CAP_NET_ADMIN))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 729) return -EPERM;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 730) lock_sock(&jsk->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 731) jsk->sk.sk_priority = j1939_to_sk_priority(tmp);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 732) release_sock(&jsk->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 733) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 734) default:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 735) return -ENOPROTOOPT;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 736) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 737) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 738)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 739) static int j1939_sk_getsockopt(struct socket *sock, int level, int optname,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 740) char __user *optval, int __user *optlen)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 741) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 742) struct sock *sk = sock->sk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 743) struct j1939_sock *jsk = j1939_sk(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 744) int ret, ulen;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 745) /* set defaults for using 'int' properties */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 746) int tmp = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 747) int len = sizeof(tmp);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 748) void *val = &tmp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 749)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 750) if (level != SOL_CAN_J1939)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 751) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 752) if (get_user(ulen, optlen))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 753) return -EFAULT;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 754) if (ulen < 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 755) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 756)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 757) lock_sock(&jsk->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 758) switch (optname) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 759) case SO_J1939_PROMISC:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 760) tmp = (jsk->state & J1939_SOCK_PROMISC) ? 1 : 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 761) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 762) case SO_J1939_ERRQUEUE:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 763) tmp = (jsk->state & J1939_SOCK_ERRQUEUE) ? 1 : 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 764) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 765) case SO_J1939_SEND_PRIO:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 766) tmp = j1939_prio(jsk->sk.sk_priority);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 767) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 768) default:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 769) ret = -ENOPROTOOPT;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 770) goto no_copy;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 771) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 772)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 773) /* copy to user, based on 'len' & 'val'
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 774) * but most sockopt's are 'int' properties, and have 'len' & 'val'
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 775) * left unchanged, but instead modified 'tmp'
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 776) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 777) if (len > ulen)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 778) ret = -EFAULT;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 779) else if (put_user(len, optlen))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 780) ret = -EFAULT;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 781) else if (copy_to_user(optval, val, len))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 782) ret = -EFAULT;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 783) else
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 784) ret = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 785) no_copy:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 786) release_sock(&jsk->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 787) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 788) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 789)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 790) static int j1939_sk_recvmsg(struct socket *sock, struct msghdr *msg,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 791) size_t size, int flags)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 792) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 793) struct sock *sk = sock->sk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 794) struct sk_buff *skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 795) struct j1939_sk_buff_cb *skcb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 796) int ret = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 797)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 798) if (flags & ~(MSG_DONTWAIT | MSG_ERRQUEUE))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 799) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 800)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 801) if (flags & MSG_ERRQUEUE)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 802) return sock_recv_errqueue(sock->sk, msg, size, SOL_CAN_J1939,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 803) SCM_J1939_ERRQUEUE);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 804)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 805) skb = skb_recv_datagram(sk, flags, 0, &ret);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 806) if (!skb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 807) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 808)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 809) if (size < skb->len)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 810) msg->msg_flags |= MSG_TRUNC;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 811) else
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 812) size = skb->len;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 813)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 814) ret = memcpy_to_msg(msg, skb->data, size);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 815) if (ret < 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 816) skb_free_datagram(sk, skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 817) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 818) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 819)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 820) skcb = j1939_skb_to_cb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 821) if (j1939_address_is_valid(skcb->addr.da))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 822) put_cmsg(msg, SOL_CAN_J1939, SCM_J1939_DEST_ADDR,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 823) sizeof(skcb->addr.da), &skcb->addr.da);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 824)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 825) if (skcb->addr.dst_name)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 826) put_cmsg(msg, SOL_CAN_J1939, SCM_J1939_DEST_NAME,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 827) sizeof(skcb->addr.dst_name), &skcb->addr.dst_name);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 828)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 829) put_cmsg(msg, SOL_CAN_J1939, SCM_J1939_PRIO,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 830) sizeof(skcb->priority), &skcb->priority);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 831)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 832) if (msg->msg_name) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 833) struct sockaddr_can *paddr = msg->msg_name;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 834)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 835) msg->msg_namelen = J1939_MIN_NAMELEN;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 836) memset(msg->msg_name, 0, msg->msg_namelen);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 837) paddr->can_family = AF_CAN;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 838) paddr->can_ifindex = skb->skb_iif;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 839) paddr->can_addr.j1939.name = skcb->addr.src_name;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 840) paddr->can_addr.j1939.addr = skcb->addr.sa;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 841) paddr->can_addr.j1939.pgn = skcb->addr.pgn;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 842) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 843)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 844) sock_recv_ts_and_drops(msg, sk, skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 845) msg->msg_flags |= skcb->msg_flags;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 846) skb_free_datagram(sk, skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 847)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 848) return size;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 849) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 850)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 851) static struct sk_buff *j1939_sk_alloc_skb(struct net_device *ndev,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 852) struct sock *sk,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 853) struct msghdr *msg, size_t size,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 854) int *errcode)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 855) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 856) struct j1939_sock *jsk = j1939_sk(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 857) struct j1939_sk_buff_cb *skcb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 858) struct sk_buff *skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 859) int ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 860)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 861) skb = sock_alloc_send_skb(sk,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 862) size +
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 863) sizeof(struct can_frame) -
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 864) sizeof(((struct can_frame *)NULL)->data) +
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 865) sizeof(struct can_skb_priv),
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 866) msg->msg_flags & MSG_DONTWAIT, &ret);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 867) if (!skb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 868) goto failure;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 869)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 870) can_skb_reserve(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 871) can_skb_prv(skb)->ifindex = ndev->ifindex;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 872) can_skb_prv(skb)->skbcnt = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 873) skb_reserve(skb, offsetof(struct can_frame, data));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 874)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 875) ret = memcpy_from_msg(skb_put(skb, size), msg, size);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 876) if (ret < 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 877) goto free_skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 878)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 879) skb->dev = ndev;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 880)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 881) skcb = j1939_skb_to_cb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 882) memset(skcb, 0, sizeof(*skcb));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 883) skcb->addr = jsk->addr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 884) skcb->priority = j1939_prio(sk->sk_priority);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 885)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 886) if (msg->msg_name) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 887) struct sockaddr_can *addr = msg->msg_name;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 888)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 889) if (addr->can_addr.j1939.name ||
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 890) addr->can_addr.j1939.addr != J1939_NO_ADDR) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 891) skcb->addr.dst_name = addr->can_addr.j1939.name;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 892) skcb->addr.da = addr->can_addr.j1939.addr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 893) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 894) if (j1939_pgn_is_valid(addr->can_addr.j1939.pgn))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 895) skcb->addr.pgn = addr->can_addr.j1939.pgn;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 896) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 897)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 898) *errcode = ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 899) return skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 900)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 901) free_skb:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 902) kfree_skb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 903) failure:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 904) *errcode = ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 905) return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 906) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 907)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 908) static size_t j1939_sk_opt_stats_get_size(void)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 909) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 910) return
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 911) nla_total_size(sizeof(u32)) + /* J1939_NLA_BYTES_ACKED */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 912) 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 913) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 914)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 915) static struct sk_buff *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 916) j1939_sk_get_timestamping_opt_stats(struct j1939_session *session)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 917) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 918) struct sk_buff *stats;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 919) u32 size;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 920)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 921) stats = alloc_skb(j1939_sk_opt_stats_get_size(), GFP_ATOMIC);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 922) if (!stats)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 923) return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 924)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 925) if (session->skcb.addr.type == J1939_SIMPLE)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 926) size = session->total_message_size;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 927) else
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 928) size = min(session->pkt.tx_acked * 7,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 929) session->total_message_size);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 930)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 931) nla_put_u32(stats, J1939_NLA_BYTES_ACKED, size);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 932)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 933) return stats;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 934) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 935)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 936) void j1939_sk_errqueue(struct j1939_session *session,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 937) enum j1939_sk_errqueue_type type)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 938) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 939) struct j1939_priv *priv = session->priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 940) struct sock *sk = session->sk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 941) struct j1939_sock *jsk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 942) struct sock_exterr_skb *serr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 943) struct sk_buff *skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 944) char *state = "UNK";
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 945) int err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 946)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 947) /* currently we have no sk for the RX session */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 948) if (!sk)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 949) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 950)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 951) jsk = j1939_sk(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 952)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 953) if (!(jsk->state & J1939_SOCK_ERRQUEUE))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 954) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 955)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 956) skb = j1939_sk_get_timestamping_opt_stats(session);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 957) if (!skb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 958) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 959)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 960) skb->tstamp = ktime_get_real();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 961)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 962) BUILD_BUG_ON(sizeof(struct sock_exterr_skb) > sizeof(skb->cb));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 963)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 964) serr = SKB_EXT_ERR(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 965) memset(serr, 0, sizeof(*serr));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 966) switch (type) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 967) case J1939_ERRQUEUE_ACK:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 968) if (!(sk->sk_tsflags & SOF_TIMESTAMPING_TX_ACK)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 969) kfree_skb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 970) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 971) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 972)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 973) serr->ee.ee_errno = ENOMSG;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 974) serr->ee.ee_origin = SO_EE_ORIGIN_TIMESTAMPING;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 975) serr->ee.ee_info = SCM_TSTAMP_ACK;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 976) state = "ACK";
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 977) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 978) case J1939_ERRQUEUE_SCHED:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 979) if (!(sk->sk_tsflags & SOF_TIMESTAMPING_TX_SCHED)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 980) kfree_skb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 981) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 982) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 983)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 984) serr->ee.ee_errno = ENOMSG;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 985) serr->ee.ee_origin = SO_EE_ORIGIN_TIMESTAMPING;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 986) serr->ee.ee_info = SCM_TSTAMP_SCHED;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 987) state = "SCH";
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 988) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 989) case J1939_ERRQUEUE_ABORT:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 990) serr->ee.ee_errno = session->err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 991) serr->ee.ee_origin = SO_EE_ORIGIN_LOCAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 992) serr->ee.ee_info = J1939_EE_INFO_TX_ABORT;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 993) state = "ABT";
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 994) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 995) default:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 996) netdev_err(priv->ndev, "Unknown errqueue type %i\n", type);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 997) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 998)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 999) serr->opt_stats = true;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1000) if (sk->sk_tsflags & SOF_TIMESTAMPING_OPT_ID)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1001) serr->ee.ee_data = session->tskey;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1002)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1003) netdev_dbg(session->priv->ndev, "%s: 0x%p tskey: %i, state: %s\n",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1004) __func__, session, session->tskey, state);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1005) err = sock_queue_err_skb(sk, skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1006)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1007) if (err)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1008) kfree_skb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1009) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1010)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1011) void j1939_sk_send_loop_abort(struct sock *sk, int err)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1012) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1013) sk->sk_err = err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1014)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1015) sk->sk_error_report(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1016) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1017)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1018) static int j1939_sk_send_loop(struct j1939_priv *priv, struct sock *sk,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1019) struct msghdr *msg, size_t size)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1020)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1021) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1022) struct j1939_sock *jsk = j1939_sk(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1023) struct j1939_session *session = j1939_sk_get_incomplete_session(jsk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1024) struct sk_buff *skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1025) size_t segment_size, todo_size;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1026) int ret = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1027)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1028) if (session &&
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1029) session->total_message_size != session->total_queued_size + size) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1030) j1939_session_put(session);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1031) return -EIO;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1032) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1033)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1034) todo_size = size;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1035)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1036) while (todo_size) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1037) struct j1939_sk_buff_cb *skcb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1038)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1039) segment_size = min_t(size_t, J1939_MAX_TP_PACKET_SIZE,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1040) todo_size);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1041)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1042) /* Allocate skb for one segment */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1043) skb = j1939_sk_alloc_skb(priv->ndev, sk, msg, segment_size,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1044) &ret);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1045) if (ret)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1046) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1047)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1048) skcb = j1939_skb_to_cb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1049)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1050) if (!session) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1051) /* at this point the size should be full size
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1052) * of the session
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1053) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1054) skcb->offset = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1055) session = j1939_tp_send(priv, skb, size);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1056) if (IS_ERR(session)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1057) ret = PTR_ERR(session);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1058) goto kfree_skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1059) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1060) if (j1939_sk_queue_session(session)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1061) /* try to activate session if we a
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1062) * fist in the queue
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1063) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1064) if (!j1939_session_activate(session)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1065) j1939_tp_schedule_txtimer(session, 0);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1066) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1067) ret = -EBUSY;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1068) session->err = ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1069) j1939_sk_queue_drop_all(priv, jsk,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1070) EBUSY);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1071) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1072) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1073) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1074) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1075) skcb->offset = session->total_queued_size;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1076) j1939_session_skb_queue(session, skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1077) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1078)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1079) todo_size -= segment_size;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1080) session->total_queued_size += segment_size;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1081) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1082)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1083) switch (ret) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1084) case 0: /* OK */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1085) if (todo_size)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1086) netdev_warn(priv->ndev,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1087) "no error found and not completely queued?! %zu\n",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1088) todo_size);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1089) ret = size;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1090) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1091) case -ERESTARTSYS:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1092) ret = -EINTR;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1093) fallthrough;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1094) case -EAGAIN: /* OK */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1095) if (todo_size != size)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1096) ret = size - todo_size;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1097) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1098) default: /* ERROR */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1099) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1100) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1101)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1102) if (session)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1103) j1939_session_put(session);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1104)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1105) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1106)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1107) kfree_skb:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1108) kfree_skb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1109) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1110) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1111)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1112) static int j1939_sk_sendmsg(struct socket *sock, struct msghdr *msg,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1113) size_t size)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1114) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1115) struct sock *sk = sock->sk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1116) struct j1939_sock *jsk = j1939_sk(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1117) struct j1939_priv *priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1118) int ifindex;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1119) int ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1120)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1121) lock_sock(sock->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1122) /* various socket state tests */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1123) if (!(jsk->state & J1939_SOCK_BOUND)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1124) ret = -EBADFD;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1125) goto sendmsg_done;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1126) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1127)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1128) priv = jsk->priv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1129) ifindex = jsk->ifindex;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1130)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1131) if (!jsk->addr.src_name && jsk->addr.sa == J1939_NO_ADDR) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1132) /* no source address assigned yet */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1133) ret = -EBADFD;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1134) goto sendmsg_done;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1135) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1136)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1137) /* deal with provided destination address info */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1138) if (msg->msg_name) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1139) struct sockaddr_can *addr = msg->msg_name;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1140)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1141) if (msg->msg_namelen < J1939_MIN_NAMELEN) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1142) ret = -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1143) goto sendmsg_done;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1144) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1145)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1146) if (addr->can_family != AF_CAN) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1147) ret = -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1148) goto sendmsg_done;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1149) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1150)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1151) if (addr->can_ifindex && addr->can_ifindex != ifindex) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1152) ret = -EBADFD;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1153) goto sendmsg_done;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1154) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1155)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1156) if (j1939_pgn_is_valid(addr->can_addr.j1939.pgn) &&
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1157) !j1939_pgn_is_clean_pdu(addr->can_addr.j1939.pgn)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1158) ret = -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1159) goto sendmsg_done;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1160) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1161)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1162) if (!addr->can_addr.j1939.name &&
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1163) addr->can_addr.j1939.addr == J1939_NO_ADDR &&
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1164) !sock_flag(sk, SOCK_BROADCAST)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1165) /* broadcast, but SO_BROADCAST not set */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1166) ret = -EACCES;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1167) goto sendmsg_done;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1168) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1169) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1170) if (!jsk->addr.dst_name && jsk->addr.da == J1939_NO_ADDR &&
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1171) !sock_flag(sk, SOCK_BROADCAST)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1172) /* broadcast, but SO_BROADCAST not set */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1173) ret = -EACCES;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1174) goto sendmsg_done;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1175) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1176) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1177)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1178) ret = j1939_sk_send_loop(priv, sk, msg, size);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1179)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1180) sendmsg_done:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1181) release_sock(sock->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1182)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1183) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1184) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1185)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1186) void j1939_sk_netdev_event_netdown(struct j1939_priv *priv)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1187) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1188) struct j1939_sock *jsk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1189) int error_code = ENETDOWN;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1190)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1191) spin_lock_bh(&priv->j1939_socks_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1192) list_for_each_entry(jsk, &priv->j1939_socks, list) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1193) jsk->sk.sk_err = error_code;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1194) if (!sock_flag(&jsk->sk, SOCK_DEAD))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1195) jsk->sk.sk_error_report(&jsk->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1196)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1197) j1939_sk_queue_drop_all(priv, jsk, error_code);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1198) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1199) spin_unlock_bh(&priv->j1939_socks_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1200) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1201)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1202) static int j1939_sk_no_ioctlcmd(struct socket *sock, unsigned int cmd,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1203) unsigned long arg)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1204) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1205) /* no ioctls for socket layer -> hand it down to NIC layer */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1206) return -ENOIOCTLCMD;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1207) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1208)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1209) static const struct proto_ops j1939_ops = {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1210) .family = PF_CAN,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1211) .release = j1939_sk_release,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1212) .bind = j1939_sk_bind,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1213) .connect = j1939_sk_connect,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1214) .socketpair = sock_no_socketpair,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1215) .accept = sock_no_accept,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1216) .getname = j1939_sk_getname,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1217) .poll = datagram_poll,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1218) .ioctl = j1939_sk_no_ioctlcmd,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1219) .listen = sock_no_listen,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1220) .shutdown = sock_no_shutdown,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1221) .setsockopt = j1939_sk_setsockopt,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1222) .getsockopt = j1939_sk_getsockopt,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1223) .sendmsg = j1939_sk_sendmsg,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1224) .recvmsg = j1939_sk_recvmsg,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1225) .mmap = sock_no_mmap,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1226) .sendpage = sock_no_sendpage,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1227) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1228)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1229) static struct proto j1939_proto __read_mostly = {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1230) .name = "CAN_J1939",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1231) .owner = THIS_MODULE,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1232) .obj_size = sizeof(struct j1939_sock),
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1233) .init = j1939_sk_init,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1234) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1235)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1236) const struct can_proto j1939_can_proto = {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1237) .type = SOCK_DGRAM,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1238) .protocol = CAN_J1939,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1239) .ops = &j1939_ops,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1240) .prot = &j1939_proto,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1241) };
Orange Pi5 kernel
Deprecated Linux kernel 5.10.110 for OrangePi 5/5B/5+ boards
3 Commits
0 Branches
0 Tags