// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
/* Copyright (c) 2019 Mellanox Technologies. All rights reserved */
#include <linux/ptp_clock_kernel.h>
#include <linux/clocksource.h>
#include <linux/timecounter.h>
#include <linux/spinlock.h>
#include <linux/device.h>
#include <linux/rhashtable.h>
#include <linux/ptp_classify.h>
#include <linux/if_ether.h>
#include <linux/if_vlan.h>
#include <linux/net_tstamp.h>
#include "spectrum.h"
#include "spectrum_ptp.h"
#include "core.h"
#define MLXSW_SP1_PTP_CLOCK_CYCLES_SHIFT 29
#define MLXSW_SP1_PTP_CLOCK_FREQ_KHZ 156257 /* 6.4nSec */
#define MLXSW_SP1_PTP_CLOCK_MASK 64
#define MLXSW_SP1_PTP_HT_GC_INTERVAL 500 /* ms */
/* How long, approximately, should the unmatched entries stay in the hash table
* before they are collected. Should be evenly divisible by the GC interval.
*/
#define MLXSW_SP1_PTP_HT_GC_TIMEOUT 1000 /* ms */
struct mlxsw_sp_ptp_state {
struct mlxsw_sp *mlxsw_sp;
struct rhashtable unmatched_ht;
spinlock_t unmatched_lock; /* protects the HT */
struct delayed_work ht_gc_dw;
u32 gc_cycle;
};
struct mlxsw_sp1_ptp_key {
u8 local_port;
u8 message_type;
u16 sequence_id;
u8 domain_number;
bool ingress;
};
struct mlxsw_sp1_ptp_unmatched {
struct mlxsw_sp1_ptp_key key;
struct rhash_head ht_node;
struct rcu_head rcu;
struct sk_buff *skb;
u64 timestamp;
u32 gc_cycle;
};
static const struct rhashtable_params mlxsw_sp1_ptp_unmatched_ht_params = {
.key_len = sizeof_field(struct mlxsw_sp1_ptp_unmatched, key),
.key_offset = offsetof(struct mlxsw_sp1_ptp_unmatched, key),
.head_offset = offsetof(struct mlxsw_sp1_ptp_unmatched, ht_node),
};
struct mlxsw_sp_ptp_clock {
struct mlxsw_core *core;
spinlock_t lock; /* protect this structure */
struct cyclecounter cycles;
struct timecounter tc;
u32 nominal_c_mult;
struct ptp_clock *ptp;
struct ptp_clock_info ptp_info;
unsigned long overflow_period;
struct delayed_work overflow_work;
};
static u64 __mlxsw_sp1_ptp_read_frc(struct mlxsw_sp_ptp_clock *clock,
struct ptp_system_timestamp *sts)
{
struct mlxsw_core *mlxsw_core = clock->core;
u32 frc_h1, frc_h2, frc_l;
frc_h1 = mlxsw_core_read_frc_h(mlxsw_core);
ptp_read_system_prets(sts);
frc_l = mlxsw_core_read_frc_l(mlxsw_core);
ptp_read_system_postts(sts);
frc_h2 = mlxsw_core_read_frc_h(mlxsw_core);
if (frc_h1 != frc_h2) {
/* wrap around */
ptp_read_system_prets(sts);
frc_l = mlxsw_core_read_frc_l(mlxsw_core);
ptp_read_system_postts(sts);
}
return (u64) frc_l | (u64) frc_h2 << 32;
}
static u64 mlxsw_sp1_ptp_read_frc(const struct cyclecounter *cc)
{
struct mlxsw_sp_ptp_clock *clock =
container_of(cc, struct mlxsw_sp_ptp_clock, cycles);
return __mlxsw_sp1_ptp_read_frc(clock, NULL) & cc->mask;
}
static int
mlxsw_sp1_ptp_phc_adjfreq(struct mlxsw_sp_ptp_clock *clock, int freq_adj)
{
struct mlxsw_core *mlxsw_core = clock->core;
char mtutc_pl[MLXSW_REG_MTUTC_LEN];
mlxsw_reg_mtutc_pack(mtutc_pl, MLXSW_REG_MTUTC_OPERATION_ADJUST_FREQ,
freq_adj, 0);
return mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtutc), mtutc_pl);
}
static u64 mlxsw_sp1_ptp_ns2cycles(const struct timecounter *tc, u64 nsec)
{
u64 cycles = (u64) nsec;
cycles <<= tc->cc->shift;
cycles = div_u64(cycles, tc->cc->mult);
return cycles;
}
static int
mlxsw_sp1_ptp_phc_settime(struct mlxsw_sp_ptp_clock *clock, u64 nsec)
{
struct mlxsw_core *mlxsw_core = clock->core;
u64 next_sec, next_sec_in_nsec, cycles;
char mtutc_pl[MLXSW_REG_MTUTC_LEN];
char mtpps_pl[MLXSW_REG_MTPPS_LEN];
int err;
next_sec = div_u64(nsec, NSEC_PER_SEC) + 1;
next_sec_in_nsec = next_sec * NSEC_PER_SEC;
spin_lock_bh(&clock->lock);
cycles = mlxsw_sp1_ptp_ns2cycles(&clock->tc, next_sec_in_nsec);
spin_unlock_bh(&clock->lock);
mlxsw_reg_mtpps_vpin_pack(mtpps_pl, cycles);
err = mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtpps), mtpps_pl);
if (err)
return err;
mlxsw_reg_mtutc_pack(mtutc_pl,
MLXSW_REG_MTUTC_OPERATION_SET_TIME_AT_NEXT_SEC,