prestera_counter.c

  1// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
  2/* Copyright (c) 2021 Marvell International Ltd. All rights reserved */
  3
  4#include "prestera.h"
  5#include "prestera_hw.h"
  6#include "prestera_acl.h"
  7#include "prestera_counter.h"
  8
  9#define COUNTER_POLL_TIME	(msecs_to_jiffies(1000))
 10#define COUNTER_RESCHED_TIME	(msecs_to_jiffies(50))
 11#define COUNTER_BULK_SIZE	(256)
 12
 13struct prestera_counter {
 14	struct prestera_switch *sw;
 15	struct delayed_work stats_dw;
 16	struct mutex mtx;  /* protect block_list */
 17	struct prestera_counter_block **block_list;
 18	u32 total_read;
 19	u32 block_list_len;
 20	u32 curr_idx;
 21	bool is_fetching;
 22};
 23
 24struct prestera_counter_block {
 25	struct list_head list;
 26	u32 id;
 27	u32 offset;
 28	u32 num_counters;
 29	u32 client;
 30	struct idr counter_idr;
 31	refcount_t refcnt;
 32	struct mutex mtx;  /* protect stats and counter_idr */
 33	struct prestera_counter_stats *stats;
 34	u8 *counter_flag;
 35	bool is_updating;
 36	bool full;
 37};
 38
 39enum {
 40	COUNTER_FLAG_READY = 0,
 41	COUNTER_FLAG_INVALID = 1
 42};
 43
 44static bool
 45prestera_counter_is_ready(struct prestera_counter_block *block, u32 id)
 46{
 47	return block->counter_flag[id - block->offset] == COUNTER_FLAG_READY;
 48}
 49
 50static void prestera_counter_lock(struct prestera_counter *counter)
 51{
 52	mutex_lock(&counter->mtx);
 53}
 54
 55static void prestera_counter_unlock(struct prestera_counter *counter)
 56{
 57	mutex_unlock(&counter->mtx);
 58}
 59
 60static void prestera_counter_block_lock(struct prestera_counter_block *block)
 61{
 62	mutex_lock(&block->mtx);
 63}
 64
 65static void prestera_counter_block_unlock(struct prestera_counter_block *block)
 66{
 67	mutex_unlock(&block->mtx);
 68}
 69
 70static bool prestera_counter_block_incref(struct prestera_counter_block *block)
 71{
 72	return refcount_inc_not_zero(&block->refcnt);
 73}
 74
 75static bool prestera_counter_block_decref(struct prestera_counter_block *block)
 76{
 77	return refcount_dec_and_test(&block->refcnt);
 78}
 79
 80/* must be called with prestera_counter_block_lock() */
 81static void prestera_counter_stats_clear(struct prestera_counter_block *block,
 82					 u32 counter_id)
 83{
 84	memset(&block->stats[counter_id - block->offset], 0,
 85	       sizeof(*block->stats));
 86}
 87
 88static struct prestera_counter_block *
 89prestera_counter_block_lookup_not_full(struct prestera_counter *counter,
 90				       u32 client)
 91{
 92	u32 i;
 93
 94	prestera_counter_lock(counter);
 95	for (i = 0; i < counter->block_list_len; i++) {
 96		if (counter->block_list[i] &&
 97		    counter->block_list[i]->client == client &&
 98		    !counter->block_list[i]->full &&
 99		    prestera_counter_block_incref(counter->block_list[i])) {
100			prestera_counter_unlock(counter);
101			return counter->block_list[i];
102		}
103	}
104	prestera_counter_unlock(counter);
105
106	return NULL;
107}
108
109static int prestera_counter_block_list_add(struct prestera_counter *counter,
110					   struct prestera_counter_block *block)
111{
112	struct prestera_counter_block **arr;
113	u32 i;
114
115	prestera_counter_lock(counter);
116
117	for (i = 0; i < counter->block_list_len; i++) {
118		if (counter->block_list[i])
119			continue;
120
121		counter->block_list[i] = block;
122		prestera_counter_unlock(counter);
123		return 0;
124	}
125
126	arr = krealloc(counter->block_list, (counter->block_list_len + 1) *
127		       sizeof(*counter->block_list), GFP_KERNEL);
128	if (!arr) {
129		prestera_counter_unlock(counter);
130		return -ENOMEM;
131	}
132
133	counter->block_list = arr;
134	counter->block_list[counter->block_list_len] = block;
135	counter->block_list_len++;
136	prestera_counter_unlock(counter);
137	return 0;
138}
139
140static struct prestera_counter_block *
141prestera_counter_block_get(struct prestera_counter *counter, u32 client)
142{
143	struct prestera_counter_block *block;
144	int err;
145
146	block = prestera_counter_block_lookup_not_full(counter, client);
147	if (block)
148		return block;
149
150	block = kzalloc(sizeof(*block), GFP_KERNEL);
151	if (!block)
152		return ERR_PTR(-ENOMEM);
153
154	err = prestera_hw_counter_block_get(counter->sw, client,
155					    &block->id, &block->offset,
156					    &block->num_counters);
157	if (err)
158		goto err_block;
159
160	block->stats = kcalloc(block->num_counters,
161			       sizeof(*block->stats), GFP_KERNEL);
162	if (!block->stats) {
163		err = -ENOMEM;
164		goto err_stats;
165	}
166
167	block->counter_flag = kcalloc(block->num_counters,
168				      sizeof(*block->counter_flag),
169				      GFP_KERNEL);
170	if (!block->counter_flag) {
171		err = -ENOMEM;
172		goto err_flag;
173	}
174
175	block->client = client;
176	mutex_init(&block->mtx);
177	refcount_set(&block->refcnt, 1);
178	idr_init_base(&block->counter_idr, block->offset);
179
180	err = prestera_counter_block_list_add(counter, block);
181	if (err)
182		goto err_list_add;
183
184	return block;
185
186err_list_add:
187	idr_destroy(&block->counter_idr);
188	mutex_destroy(&block->mtx);
189	kfree(block->counter_flag);
190err_flag:
191	kfree(block->stats);
192err_stats:
193	prestera_hw_counter_block_release(counter->sw, block->id);
194err_block:
195	kfree(block);
196	return ERR_PTR(err);
197}
198
199static void prestera_counter_block_put(struct prestera_counter *counter,
200				       struct prestera_counter_block *block)
201{
202	u32 i;
203
204	if (!prestera_counter_block_decref(block))
205		return;
206
207	prestera_counter_lock(counter);
208	for (i = 0; i < counter->block_list_len; i++) {
209		if (counter->block_list[i] &&
210		    counter->block_list[i]->id == block->id) {
211			counter->block_list[i] = NULL;
212			break;
213		}
214	}
215	prestera_counter_unlock(counter);
216
217	WARN_ON(!idr_is_empty(&block->counter_idr));
218
219	prestera_hw_counter_block_release(counter->sw, block->id);
220	idr_destroy(&block->counter_idr);
221	mutex_destroy(&block->mtx);
222	kfree(block->stats);
223	kfree(block);
224}
225
226static int prestera_counter_get_vacant(struct prestera_counter_block *block,
227				       u32 *id)
228{
229	int free_id;
230
231	if (block->full)
232		return -ENOSPC;
233
234	prestera_counter_block_lock(block);
235	free_id = idr_alloc_cyclic(&block->counter_idr, NULL, block->offset,
236				   block->offset + block->num_counters,
237				   GFP_KERNEL);
238	if (free_id < 0) {
239		if (free_id == -ENOSPC)
240			block->full = true;
241
242		prestera_counter_block_unlock(block);
243		return free_id;
244	}
245	*id = free_id;
246	prestera_counter_block_unlock(block);
247
248	return 0;
249}
250
251int prestera_counter_get(struct prestera_counter *counter, u32 client,
252			 struct prestera_counter_block **bl, u32 *counter_id)
253{
254	struct prestera_counter_block *block;
255	int err;
256	u32 id;
257
258get_next_block:
259	block = prestera_counter_block_get(counter, client);
260	if (IS_ERR(block))
261		return PTR_ERR(block);
262
263	err = prestera_counter_get_vacant(block, &id);
264	if (err) {
265		prestera_counter_block_put(counter, block);
266
267		if (err == -ENOSPC)
268			goto get_next_block;
269
270		return err;
271	}
272
273	prestera_counter_block_lock(block);
274	if (block->is_updating)
275		block->counter_flag[id - block->offset] = COUNTER_FLAG_INVALID;
276	prestera_counter_block_unlock(block);
277
278	*counter_id = id;
279	*bl = block;
280
281	return 0;
282}
283
284void prestera_counter_put(struct prestera_counter *counter,
285			  struct prestera_counter_block *block, u32 counter_id)
286{
287	if (!block)
288		return;
289
290	prestera_counter_block_lock(block);
291	idr_remove(&block->counter_idr, counter_id);
292	block->full = false;
293	prestera_counter_stats_clear(block, counter_id);
294	prestera_counter_block_unlock(block);
295
296	prestera_hw_counter_clear(counter->sw, block->id, counter_id);
297	prestera_counter_block_put(counter, block);
298}
299
300static u32 prestera_counter_block_idx_next(struct prestera_counter *counter,
301					   u32 curr_idx)
302{
303	u32 idx, i, start = curr_idx + 1;
304
305	prestera_counter_lock(counter);
306	for (i = 0; i < counter->block_list_len; i++) {
307		idx = (start + i) % counter->block_list_len;
308		if (!counter->block_list[idx])
309			continue;
310
311		prestera_counter_unlock(counter);
312		return idx;
313	}
314	prestera_counter_unlock(counter);
315
316	return 0;
317}
318
319static struct prestera_counter_block *
320prestera_counter_block_get_by_idx(struct prestera_counter *counter, u32 idx)
321{
322	if (idx >= counter->block_list_len)
323		return NULL;
324
325	prestera_counter_lock(counter);
326
327	if (!counter->block_list[idx] ||
328	    !prestera_counter_block_incref(counter->block_list[idx])) {
329		prestera_counter_unlock(counter);
330		return NULL;
331	}
332
333	prestera_counter_unlock(counter);
334	return counter->block_list[idx];
335}
336
337static void prestera_counter_stats_work(struct work_struct *work)
338{
339	struct delayed_work *dl_work = to_delayed_work(work);
340	struct prestera_counter *counter =
341		container_of(dl_work, struct prestera_counter, stats_dw);
342	struct prestera_counter_block *block;
343	u32 resched_time = COUNTER_POLL_TIME;
344	u32 count = COUNTER_BULK_SIZE;
345	bool done = false;
346	int err;
347	u32 i;
348
349	block = prestera_counter_block_get_by_idx(counter, counter->curr_idx);
350	if (!block) {
351		if (counter->is_fetching)
352			goto abort;
353
354		goto next;
355	}
356
357	if (!counter->is_fetching) {
358		err = prestera_hw_counter_trigger(counter->sw, block->id);
359		if (err)
360			goto abort;
361
362		prestera_counter_block_lock(block);
363		block->is_updating = true;
364		prestera_counter_block_unlock(block);
365
366		counter->is_fetching = true;
367		counter->total_read = 0;
368		resched_time = COUNTER_RESCHED_TIME;
369		goto resched;
370	}
371
372	prestera_counter_block_lock(block);
373	err = prestera_hw_counters_get(counter->sw, counter->total_read,
374				       &count, &done,
375				       &block->stats[counter->total_read]);
376	prestera_counter_block_unlock(block);
377	if (err)
378		goto abort;
379
380	counter->total_read += count;
381	if (!done || counter->total_read < block->num_counters) {
382		resched_time = COUNTER_RESCHED_TIME;
383		goto resched;
384	}
385
386	for (i = 0; i < block->num_counters; i++) {
387		if (block->counter_flag[i] == COUNTER_FLAG_INVALID) {
388			prestera_counter_block_lock(block);
389			block->counter_flag[i] = COUNTER_FLAG_READY;
390			memset(&block->stats[i], 0, sizeof(*block->stats));
391			prestera_counter_block_unlock(block);
392		}
393	}
394
395	prestera_counter_block_lock(block);
396	block->is_updating = false;
397	prestera_counter_block_unlock(block);
398
399	goto next;
400abort:
401	prestera_hw_counter_abort(counter->sw);
402next:
403	counter->is_fetching = false;
404	counter->curr_idx =
405		prestera_counter_block_idx_next(counter, counter->curr_idx);
406resched:
407	if (block)
408		prestera_counter_block_put(counter, block);
409
410	schedule_delayed_work(&counter->stats_dw, resched_time);
411}
412
413/* Can be executed without rtnl_lock().
414 * So pay attention when something changing.
415 */
416int prestera_counter_stats_get(struct prestera_counter *counter,
417			       struct prestera_counter_block *block,
418			       u32 counter_id, u64 *packets, u64 *bytes)
419{
420	if (!block || !prestera_counter_is_ready(block, counter_id)) {
421		*packets = 0;
422		*bytes = 0;
423		return 0;
424	}
425
426	prestera_counter_block_lock(block);
427	*packets = block->stats[counter_id - block->offset].packets;
428	*bytes = block->stats[counter_id - block->offset].bytes;
429
430	prestera_counter_stats_clear(block, counter_id);
431	prestera_counter_block_unlock(block);
432
433	return 0;
434}
435
436int prestera_counter_init(struct prestera_switch *sw)
437{
438	struct prestera_counter *counter;
439
440	counter = kzalloc(sizeof(*counter), GFP_KERNEL);
441	if (!counter)
442		return -ENOMEM;
443
444	counter->block_list = kzalloc(sizeof(*counter->block_list), GFP_KERNEL);
445	if (!counter->block_list) {
446		kfree(counter);
447		return -ENOMEM;
448	}
449
450	mutex_init(&counter->mtx);
451	counter->block_list_len = 1;
452	counter->sw = sw;
453	sw->counter = counter;
454
455	INIT_DELAYED_WORK(&counter->stats_dw, prestera_counter_stats_work);
456	schedule_delayed_work(&counter->stats_dw, COUNTER_POLL_TIME);
457
458	return 0;
459}
460
461void prestera_counter_fini(struct prestera_switch *sw)
462{
463	struct prestera_counter *counter = sw->counter;
464	u32 i;
465
466	cancel_delayed_work_sync(&counter->stats_dw);
467
468	for (i = 0; i < counter->block_list_len; i++)
469		WARN_ON(counter->block_list[i]);
470
471	mutex_destroy(&counter->mtx);
472	kfree(counter->block_list);
473	kfree(counter);
474}