forked from Qortal/Brooklyn
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
333 lines
7.0 KiB
333 lines
7.0 KiB
// SPDX-License-Identifier: GPL-2.0 |
|
// Copyright (c) 2010-2011 EIA Electronics, |
|
// Kurt Van Dijck <[email protected]> |
|
// Copyright (c) 2017-2019 Pengutronix, |
|
// Marc Kleine-Budde <[email protected]> |
|
// Copyright (c) 2017-2019 Pengutronix, |
|
// Oleksij Rempel <[email protected]> |
|
|
|
/* bus for j1939 remote devices |
|
* Since rtnetlink, no real bus is used. |
|
*/ |
|
|
|
#include <net/sock.h> |
|
|
|
#include "j1939-priv.h" |
|
|
|
static void __j1939_ecu_release(struct kref *kref) |
|
{ |
|
struct j1939_ecu *ecu = container_of(kref, struct j1939_ecu, kref); |
|
struct j1939_priv *priv = ecu->priv; |
|
|
|
list_del(&ecu->list); |
|
kfree(ecu); |
|
j1939_priv_put(priv); |
|
} |
|
|
|
void j1939_ecu_put(struct j1939_ecu *ecu) |
|
{ |
|
kref_put(&ecu->kref, __j1939_ecu_release); |
|
} |
|
|
|
static void j1939_ecu_get(struct j1939_ecu *ecu) |
|
{ |
|
kref_get(&ecu->kref); |
|
} |
|
|
|
static bool j1939_ecu_is_mapped_locked(struct j1939_ecu *ecu) |
|
{ |
|
struct j1939_priv *priv = ecu->priv; |
|
|
|
lockdep_assert_held(&priv->lock); |
|
|
|
return j1939_ecu_find_by_addr_locked(priv, ecu->addr) == ecu; |
|
} |
|
|
|
/* ECU device interface */ |
|
/* map ECU to a bus address space */ |
|
static void j1939_ecu_map_locked(struct j1939_ecu *ecu) |
|
{ |
|
struct j1939_priv *priv = ecu->priv; |
|
struct j1939_addr_ent *ent; |
|
|
|
lockdep_assert_held(&priv->lock); |
|
|
|
if (!j1939_address_is_unicast(ecu->addr)) |
|
return; |
|
|
|
ent = &priv->ents[ecu->addr]; |
|
|
|
if (ent->ecu) { |
|
netdev_warn(priv->ndev, "Trying to map already mapped ECU, addr: 0x%02x, name: 0x%016llx. Skip it.\n", |
|
ecu->addr, ecu->name); |
|
return; |
|
} |
|
|
|
j1939_ecu_get(ecu); |
|
ent->ecu = ecu; |
|
ent->nusers += ecu->nusers; |
|
} |
|
|
|
/* unmap ECU from a bus address space */ |
|
void j1939_ecu_unmap_locked(struct j1939_ecu *ecu) |
|
{ |
|
struct j1939_priv *priv = ecu->priv; |
|
struct j1939_addr_ent *ent; |
|
|
|
lockdep_assert_held(&priv->lock); |
|
|
|
if (!j1939_address_is_unicast(ecu->addr)) |
|
return; |
|
|
|
if (!j1939_ecu_is_mapped_locked(ecu)) |
|
return; |
|
|
|
ent = &priv->ents[ecu->addr]; |
|
ent->ecu = NULL; |
|
ent->nusers -= ecu->nusers; |
|
j1939_ecu_put(ecu); |
|
} |
|
|
|
void j1939_ecu_unmap(struct j1939_ecu *ecu) |
|
{ |
|
write_lock_bh(&ecu->priv->lock); |
|
j1939_ecu_unmap_locked(ecu); |
|
write_unlock_bh(&ecu->priv->lock); |
|
} |
|
|
|
void j1939_ecu_unmap_all(struct j1939_priv *priv) |
|
{ |
|
int i; |
|
|
|
write_lock_bh(&priv->lock); |
|
for (i = 0; i < ARRAY_SIZE(priv->ents); i++) |
|
if (priv->ents[i].ecu) |
|
j1939_ecu_unmap_locked(priv->ents[i].ecu); |
|
write_unlock_bh(&priv->lock); |
|
} |
|
|
|
void j1939_ecu_timer_start(struct j1939_ecu *ecu) |
|
{ |
|
/* The ECU is held here and released in the |
|
* j1939_ecu_timer_handler() or j1939_ecu_timer_cancel(). |
|
*/ |
|
j1939_ecu_get(ecu); |
|
|
|
/* Schedule timer in 250 msec to commit address change. */ |
|
hrtimer_start(&ecu->ac_timer, ms_to_ktime(250), |
|
HRTIMER_MODE_REL_SOFT); |
|
} |
|
|
|
void j1939_ecu_timer_cancel(struct j1939_ecu *ecu) |
|
{ |
|
if (hrtimer_cancel(&ecu->ac_timer)) |
|
j1939_ecu_put(ecu); |
|
} |
|
|
|
static enum hrtimer_restart j1939_ecu_timer_handler(struct hrtimer *hrtimer) |
|
{ |
|
struct j1939_ecu *ecu = |
|
container_of(hrtimer, struct j1939_ecu, ac_timer); |
|
struct j1939_priv *priv = ecu->priv; |
|
|
|
write_lock_bh(&priv->lock); |
|
/* TODO: can we test if ecu->addr is unicast before starting |
|
* the timer? |
|
*/ |
|
j1939_ecu_map_locked(ecu); |
|
|
|
/* The corresponding j1939_ecu_get() is in |
|
* j1939_ecu_timer_start(). |
|
*/ |
|
j1939_ecu_put(ecu); |
|
write_unlock_bh(&priv->lock); |
|
|
|
return HRTIMER_NORESTART; |
|
} |
|
|
|
struct j1939_ecu *j1939_ecu_create_locked(struct j1939_priv *priv, name_t name) |
|
{ |
|
struct j1939_ecu *ecu; |
|
|
|
lockdep_assert_held(&priv->lock); |
|
|
|
ecu = kzalloc(sizeof(*ecu), gfp_any()); |
|
if (!ecu) |
|
return ERR_PTR(-ENOMEM); |
|
kref_init(&ecu->kref); |
|
ecu->addr = J1939_IDLE_ADDR; |
|
ecu->name = name; |
|
|
|
hrtimer_init(&ecu->ac_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL_SOFT); |
|
ecu->ac_timer.function = j1939_ecu_timer_handler; |
|
INIT_LIST_HEAD(&ecu->list); |
|
|
|
j1939_priv_get(priv); |
|
ecu->priv = priv; |
|
list_add_tail(&ecu->list, &priv->ecus); |
|
|
|
return ecu; |
|
} |
|
|
|
struct j1939_ecu *j1939_ecu_find_by_addr_locked(struct j1939_priv *priv, |
|
u8 addr) |
|
{ |
|
lockdep_assert_held(&priv->lock); |
|
|
|
return priv->ents[addr].ecu; |
|
} |
|
|
|
struct j1939_ecu *j1939_ecu_get_by_addr_locked(struct j1939_priv *priv, u8 addr) |
|
{ |
|
struct j1939_ecu *ecu; |
|
|
|
lockdep_assert_held(&priv->lock); |
|
|
|
if (!j1939_address_is_unicast(addr)) |
|
return NULL; |
|
|
|
ecu = j1939_ecu_find_by_addr_locked(priv, addr); |
|
if (ecu) |
|
j1939_ecu_get(ecu); |
|
|
|
return ecu; |
|
} |
|
|
|
struct j1939_ecu *j1939_ecu_get_by_addr(struct j1939_priv *priv, u8 addr) |
|
{ |
|
struct j1939_ecu *ecu; |
|
|
|
read_lock_bh(&priv->lock); |
|
ecu = j1939_ecu_get_by_addr_locked(priv, addr); |
|
read_unlock_bh(&priv->lock); |
|
|
|
return ecu; |
|
} |
|
|
|
/* get pointer to ecu without increasing ref counter */ |
|
static struct j1939_ecu *j1939_ecu_find_by_name_locked(struct j1939_priv *priv, |
|
name_t name) |
|
{ |
|
struct j1939_ecu *ecu; |
|
|
|
lockdep_assert_held(&priv->lock); |
|
|
|
list_for_each_entry(ecu, &priv->ecus, list) { |
|
if (ecu->name == name) |
|
return ecu; |
|
} |
|
|
|
return NULL; |
|
} |
|
|
|
struct j1939_ecu *j1939_ecu_get_by_name_locked(struct j1939_priv *priv, |
|
name_t name) |
|
{ |
|
struct j1939_ecu *ecu; |
|
|
|
lockdep_assert_held(&priv->lock); |
|
|
|
if (!name) |
|
return NULL; |
|
|
|
ecu = j1939_ecu_find_by_name_locked(priv, name); |
|
if (ecu) |
|
j1939_ecu_get(ecu); |
|
|
|
return ecu; |
|
} |
|
|
|
struct j1939_ecu *j1939_ecu_get_by_name(struct j1939_priv *priv, name_t name) |
|
{ |
|
struct j1939_ecu *ecu; |
|
|
|
read_lock_bh(&priv->lock); |
|
ecu = j1939_ecu_get_by_name_locked(priv, name); |
|
read_unlock_bh(&priv->lock); |
|
|
|
return ecu; |
|
} |
|
|
|
u8 j1939_name_to_addr(struct j1939_priv *priv, name_t name) |
|
{ |
|
struct j1939_ecu *ecu; |
|
int addr = J1939_IDLE_ADDR; |
|
|
|
if (!name) |
|
return J1939_NO_ADDR; |
|
|
|
read_lock_bh(&priv->lock); |
|
ecu = j1939_ecu_find_by_name_locked(priv, name); |
|
if (ecu && j1939_ecu_is_mapped_locked(ecu)) |
|
/* ecu's SA is registered */ |
|
addr = ecu->addr; |
|
|
|
read_unlock_bh(&priv->lock); |
|
|
|
return addr; |
|
} |
|
|
|
/* TX addr/name accounting |
|
* Transport protocol needs to know if a SA is local or not |
|
* These functions originate from userspace manipulating sockets, |
|
* so locking is straigforward |
|
*/ |
|
|
|
int j1939_local_ecu_get(struct j1939_priv *priv, name_t name, u8 sa) |
|
{ |
|
struct j1939_ecu *ecu; |
|
int err = 0; |
|
|
|
write_lock_bh(&priv->lock); |
|
|
|
if (j1939_address_is_unicast(sa)) |
|
priv->ents[sa].nusers++; |
|
|
|
if (!name) |
|
goto done; |
|
|
|
ecu = j1939_ecu_get_by_name_locked(priv, name); |
|
if (!ecu) |
|
ecu = j1939_ecu_create_locked(priv, name); |
|
err = PTR_ERR_OR_ZERO(ecu); |
|
if (err) |
|
goto done; |
|
|
|
ecu->nusers++; |
|
/* TODO: do we care if ecu->addr != sa? */ |
|
if (j1939_ecu_is_mapped_locked(ecu)) |
|
/* ecu's sa is active already */ |
|
priv->ents[ecu->addr].nusers++; |
|
|
|
done: |
|
write_unlock_bh(&priv->lock); |
|
|
|
return err; |
|
} |
|
|
|
void j1939_local_ecu_put(struct j1939_priv *priv, name_t name, u8 sa) |
|
{ |
|
struct j1939_ecu *ecu; |
|
|
|
write_lock_bh(&priv->lock); |
|
|
|
if (j1939_address_is_unicast(sa)) |
|
priv->ents[sa].nusers--; |
|
|
|
if (!name) |
|
goto done; |
|
|
|
ecu = j1939_ecu_find_by_name_locked(priv, name); |
|
if (WARN_ON_ONCE(!ecu)) |
|
goto done; |
|
|
|
ecu->nusers--; |
|
/* TODO: do we care if ecu->addr != sa? */ |
|
if (j1939_ecu_is_mapped_locked(ecu)) |
|
/* ecu's sa is active already */ |
|
priv->ents[ecu->addr].nusers--; |
|
j1939_ecu_put(ecu); |
|
|
|
done: |
|
write_unlock_bh(&priv->lock); |
|
}
|
|
|