334 lines
7.0 KiB
C
334 lines
7.0 KiB
C
// SPDX-License-Identifier: GPL-2.0
|
|
// Copyright (c) 2010-2011 EIA Electronics,
|
|
// Kurt Van Dijck <kurt.van.dijck@eia.be>
|
|
// Copyright (c) 2017-2019 Pengutronix,
|
|
// Marc Kleine-Budde <kernel@pengutronix.de>
|
|
// Copyright (c) 2017-2019 Pengutronix,
|
|
// Oleksij Rempel <kernel@pengutronix.de>
|
|
|
|
/* 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);
|
|
}
|