drivers/net/ethernet/marvell/prestera/prestera_counter.c

// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
/* Copyright (c) 2021 Marvell International Ltd. All rights reserved */

#include "prestera.h"
#include "prestera_hw.h"
#include "prestera_acl.h"
#include "prestera_counter.h"

#define COUNTER_POLL_TIME	(msecs_to_jiffies(1000))
#define COUNTER_RESCHED_TIME	(msecs_to_jiffies(50))
#define COUNTER_BULK_SIZE	(256)

struct prestera_counter {
	struct prestera_switch *sw;
	struct delayed_work stats_dw;
	struct mutex mtx;  /* protect block_list */
	struct prestera_counter_block **block_list;
	u32 total_read;
	u32 block_list_len;
	u32 curr_idx;
	bool is_fetching;
};

struct prestera_counter_block {
	struct list_head list;
	u32 id;
	u32 offset;
	u32 num_counters;
	u32 client;
	struct idr counter_idr;
	refcount_t refcnt;
	struct mutex mtx;  /* protect stats and counter_idr */
	struct prestera_counter_stats *stats;
	u8 *counter_flag;
	bool is_updating;
	bool full;
};

enum {
	COUNTER_FLAG_READY = 0,
	COUNTER_FLAG_INVALID = 1
};

static bool
prestera_counter_is_ready(struct prestera_counter_block *block, u32 id)
{
	return block->counter_flag[id - block->offset] == COUNTER_FLAG_READY;
}

static void prestera_counter_lock(struct prestera_counter *counter)
{
	mutex_lock(&counter->mtx);
}

static void prestera_counter_unlock(struct prestera_counter *counter)
{
	mutex_unlock(&counter->mtx);
}

static void prestera_counter_block_lock(struct prestera_counter_block *block)
{
	mutex_lock(&block->mtx);
}

static void prestera_counter_block_unlock(struct prestera_counter_block *block)
{
	mutex_unlock(&block->mtx);
}

static bool prestera_counter_block_incref(struct prestera_counter_block *block)
{
	return refcount_inc_not_zero(&block->refcnt);
}

static bool prestera_counter_block_decref(struct prestera_counter_block *block)
{
	return refcount_dec_and_test(&block->refcnt);
}

/* must be called with prestera_counter_block_lock() */
static void prestera_counter_stats_clear(struct prestera_counter_block *block,
					 u32 counter_id)
{
	memset(&block->stats[counter_id - block->offset], 0,
	       sizeof(*block->stats));
}

static struct prestera_counter_block *
prestera_counter_block_lookup_not_full(struct prestera_counter *counter,
				       u32 client)
{
	u32 i;

	prestera_counter_lock(counter);
	for (i = 0; i < counter->block_list_len; i++) {
		if (counter->block_list[i] &&
		    counter->block_list[i]->client == client &&
		    !counter->block_list[i]->full &&
		    prestera_counter_block_incref(counter->block_list[i])) {
			prestera_counter_unlock(counter);
			return counter->block_list[i];
		}
	}
	prestera_counter_unlock(counter);

	return NULL;
}

static int prestera_counter_block_list_add(struct prestera_counter *counter,
					   struct prestera_counter_block *block)
{
	struct prestera_counter_block **arr;
	u32 i;

	prestera_counter_lock(counter);

	for (i = 0; i < counter->block_list_len; i++) {
		if (counter->block_list[i])
			continue;

		counter->block_list[i] = block;
		prestera_counter_unlock(counter);
		return 0;
	}

	arr = krealloc(counter->block_list, (counter->block_list_len + 1) *
		       sizeof(*counter->block_list), GFP_KERNEL);
	if (!arr) {
		prestera_counter_unlock(counter);
		return -ENOMEM;
	}

	counter->block_list = arr;
	counter->block_list[counter->block_list_len] = block;
	counter->block_list_len++;
	prestera_counter_unlock(counter);
	return 0;
}

static struct prestera_counter_block *
prestera_counter_block_get(struct prestera_counter *counter, u32 client)
{
	struct prestera_counter_block *block;
	int err;

	block = prestera_counter_block_lookup_not_full(counter, client);
	if (block)
		return block;

	block = kzalloc(sizeof(*block), GFP_KERNEL);
	if (!block)
		return ERR_PTR(-ENOMEM);

	err = prestera_hw_counter_block_get(counter->sw, client,
					    &block->id, &block->offset,
					    &block->num_counters);
	if (err)
		goto err_block;

	block->stats = kcalloc(block->num_counters,
			       sizeof(*block->stats), GFP_KERNEL);
	if (!block->stats) {
		err = -ENOMEM;
		goto err_stats;
	}

	block->counter_flag = kcalloc(block->num_counters,
				      sizeof(*block->counter_flag),
				      GFP_KERNEL);
	if (!block->counter_flag) {
		err = -ENOMEM;
		goto err_flag;
	}

	block->client = client;
	mutex_init(&block->mtx);
	refcount_set(&block->refcnt, 1);
	idr_init_base(&block->counter_idr, block->offset);

	err = prestera_counter_block_list_add(counter, block);
	if (err)
		goto err_list_add;

	return block;

err_list_add:
	idr_destroy(&block->counter_idr);
	mutex_destroy(&block->mtx);
	kfree(block->counter_flag);
err_flag:
	kfree(block->stats);
err_stats:
	prestera_hw_counter_block_release(counter->sw, block->id);
err_block:
	kfree(block);
	return ERR_PTR(err);
}

static void prestera_counter_block_put(struct prestera_counter *counter,
				       struct prestera_counter_block *block)
{
	u32 i;

	if (!prestera_counter_block_decref(block))
		return;

	prestera_counter_lock(counter);
	for (i = 0; i < counter->block_list_len; i++) {
		if (counter->block_list[i] &&
		    counter->block_list[i]->id == block->id) {
			counter->block_list[i] = NULL;
			break;
		}
	}
	prestera_counter_unlock(counter);

	WARN_ON(!idr_is_empty(&block->counter_idr));

	prestera_hw_counter_block_release(counter->sw, block->id);
	idr_destroy(&block->counter_idr);
	mutex_destroy(&block->mtx);
	kfree(block->stats);
	kfree(block);
}

static int prestera_counter_get_vacant(struct prestera_counter_block *block,
				       u32 *id)
{
	int free_id;

	if (block->full)
		return -ENOSPC;

	prestera_counter_block_lock(block);
	free_id = idr_alloc_cyclic(&block->counter_idr, NULL, block->offset,
				   block->offset + block->num_counters,
				   GFP_KERNEL);
	if (free_id < 0) {
		if (free_id == -ENOSPC)
			block->full = true;

		prestera_counter_block_unlock(block);
		return free_id;
	}
	*id = free_id;
	prestera_counter_block_unlock(block);

	return 0;
}

int prestera_counter_get(struct prestera_counter *counter, u32 client,
			 struct prestera_counter_block **bl, u32 *counter_id)
{
	struct prestera_counter_block *block;
	int err;
	u32 id;

get_next_block:
	block = prestera_counter_block_get(counter, client);
	if (IS_ERR(block))
		return PTR_ERR(block);

	err = prestera_counter_get_vacant(block, &id);
	if (err) {
		prestera_counter_block_put(counter, block);

		if (err == -ENOSPC)
			goto get_next_block;

		return err;
	}

	prestera_counter_block_lock(block);
	if (block->is_updating)
		block->counter_flag[id - block->offset] = COUNTER_FLAG_INVALID;
	prestera_counter_block_unlock(block);

	*counter_id = id;
	*bl = block;

	return 0;
}

void prestera_counter_put(struct prestera_counter *counter,
			  struct prestera_counter_block *block, u32 counter_id)
{
	if (!block)
		return;

	prestera_counter_block_lock(block);
	idr_remove(&block->counter_idr, counter_id);
	block->full = false;
	prestera_counter_stats_clear(block, counter_id);
	prestera_counter_block_unlock(block);

	prestera_hw_counter_clear(counter->sw, block->id, counter_id);
	prestera_counter_block_put(counter, block);
}

static u32 prestera_counter_block_idx_next(struct prestera_counter *counter,
					   u32 curr_idx)
{
	u32 idx, i, start = curr_idx + 1;

	prestera_counter_lock(counter);
	for (i = 0; i < counter->block_list_len; i++) {
		idx = (start + i) % counter->block_list_len;
		if (!counter->block_list[idx])
			continue;

		prestera_counter_unlock(counter);
		return idx;
	}
	prestera_counter_unlock(counter);

	return 0;
}

static struct prestera_counter_block *
prestera_counter_block_get_by_idx(struct prestera_counter *counter, u32 idx)
{
	if (idx >= counter->block_list_len)
		return NULL;

	prestera_counter_lock(counter);

	if (!counter->block_list[idx] ||
	    !prestera_counter_block_incref(counter->block_list[idx])) {
		prestera_counter_unlock(counter);
		return NULL;
	}

	prestera_counter_unlock(counter);
	return counter->block_list[idx];
}

static void prestera_counter_stats_work(struct work_struct *work)
{
	struct delayed_work *dl_work =
		container_of(work, struct delayed_work, work);
	struct prestera_counter *counter =
		container_of(dl_work, struct prestera_counter, stats_dw);
	struct prestera_counter_block *block;
	u32 resched_time = COUNTER_POLL_TIME;
	u32 count = COUNTER_BULK_SIZE;
	bool done = false;
	int err;
	u32 i;

	block = prestera_counter_block_get_by_idx(counter, counter->curr_idx);
	if (!block) {
		if (counter->is_fetching)
			goto abort;

		goto next;
	}

	if (!counter->is_fetching) {
		err = prestera_hw_counter_trigger(counter->sw, block->id);
		if (err)
			goto abort;

		prestera_counter_block_lock(block);
		block->is_updating = true;
		prestera_counter_block_unlock(block);

		counter->is_fetching = true;
		counter->total_read = 0;
		resched_time = COUNTER_RESCHED_TIME;
		goto resched;
	}

	prestera_counter_block_lock(block);
	err = prestera_hw_counters_get(counter->sw, counter->total_read,
				       &count, &done,
				       &block->stats[counter->total_read]);
	prestera_counter_block_unlock(block);
	if (err)
		goto abort;

	counter->total_read += count;
	if (!done || counter->total_read < block->num_counters) {
		resched_time = COUNTER_RESCHED_TIME;
		goto resched;
	}

	for (i = 0; i < block->num_counters; i++) {
		if (block->counter_flag[i] == COUNTER_FLAG_INVALID) {
			prestera_counter_block_lock(block);
			block->counter_flag[i] = COUNTER_FLAG_READY;
			memset(&block->stats[i], 0, sizeof(*block->stats));
			prestera_counter_block_unlock(block);
		}
	}

	prestera_counter_block_lock(block);
	block->is_updating = false;
	prestera_counter_block_unlock(block);

	goto next;
abort:
	prestera_hw_counter_abort(counter->sw);
next:
	counter->is_fetching = false;
	counter->curr_idx =
		prestera_counter_block_idx_next(counter, counter->curr_idx);
resched:
	if (block)
		prestera_counter_block_put(counter, block);

	schedule_delayed_work(&counter->stats_dw, resched_time);
}

/* Can be executed without rtnl_lock().
 * So pay attention when something changing.
 */
int prestera_counter_stats_get(struct prestera_counter *counter,
			       struct prestera_counter_block *block,
			       u32 counter_id, u64 *packets, u64 *bytes)
{
	if (!block || !prestera_counter_is_ready(block, counter_id)) {
		*packets = 0;
		*bytes = 0;
		return 0;
	}

	prestera_counter_block_lock(block);
	*packets = block->stats[counter_id - block->offset].packets;
	*bytes = block->stats[counter_id - block->offset].bytes;

	prestera_counter_stats_clear(block, counter_id);
	prestera_counter_block_unlock(block);

	return 0;
}

int prestera_counter_init(struct prestera_switch *sw)
{
	struct prestera_counter *counter;

	counter = kzalloc(sizeof(*counter), GFP_KERNEL);
	if (!counter)
		return -ENOMEM;

	counter->block_list = kzalloc(sizeof(*counter->block_list), GFP_KERNEL);
	if (!counter->block_list) {
		kfree(counter);
		return -ENOMEM;
	}

	mutex_init(&counter->mtx);
	counter->block_list_len = 1;
	counter->sw = sw;
	sw->counter = counter;

	INIT_DELAYED_WORK(&counter->stats_dw, prestera_counter_stats_work);
	schedule_delayed_work(&counter->stats_dw, COUNTER_POLL_TIME);

	return 0;
}

void prestera_counter_fini(struct prestera_switch *sw)
{
	struct prestera_counter *counter = sw->counter;
	u32 i;

	cancel_delayed_work_sync(&counter->stats_dw);

	for (i = 0; i < counter->block_list_len; i++)
		WARN_ON(counter->block_list[i]);

	mutex_destroy(&counter->mtx);
	kfree(counter->block_list);
	kfree(counter);
}