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 =
340		container_of(work, struct delayed_work, work);
341	struct prestera_counter *counter =
342		container_of(dl_work, struct prestera_counter, stats_dw);
343	struct prestera_counter_block *block;
344	u32 resched_time = COUNTER_POLL_TIME;
345	u32 count = COUNTER_BULK_SIZE;
346	bool done = false;
347	int err;
348	u32 i;
349
350	block = prestera_counter_block_get_by_idx(counter, counter->curr_idx);
351	if (!block) {
352		if (counter->is_fetching)
353			goto abort;
354
355		goto next;
356	}
357
358	if (!counter->is_fetching) {
359		err = prestera_hw_counter_trigger(counter->sw, block->id);
360		if (err)
361			goto abort;
362
363		prestera_counter_block_lock(block);
364		block->is_updating = true;
365		prestera_counter_block_unlock(block);
366
367		counter->is_fetching = true;
368		counter->total_read = 0;
369		resched_time = COUNTER_RESCHED_TIME;
370		goto resched;
371	}
372
373	prestera_counter_block_lock(block);
374	err = prestera_hw_counters_get(counter->sw, counter->total_read,
375				       &count, &done,
376				       &block->stats[counter->total_read]);
377	prestera_counter_block_unlock(block);
378	if (err)
379		goto abort;
380
381	counter->total_read += count;
382	if (!done || counter->total_read < block->num_counters) {
383		resched_time = COUNTER_RESCHED_TIME;
384		goto resched;
385	}
386
387	for (i = 0; i < block->num_counters; i++) {
388		if (block->counter_flag[i] == COUNTER_FLAG_INVALID) {
389			prestera_counter_block_lock(block);
390			block->counter_flag[i] = COUNTER_FLAG_READY;
391			memset(&block->stats[i], 0, sizeof(*block->stats));
392			prestera_counter_block_unlock(block);
393		}
394	}
395
396	prestera_counter_block_lock(block);
397	block->is_updating = false;
398	prestera_counter_block_unlock(block);
399
400	goto next;
401abort:
402	prestera_hw_counter_abort(counter->sw);
403next:
404	counter->is_fetching = false;
405	counter->curr_idx =
406		prestera_counter_block_idx_next(counter, counter->curr_idx);
407resched:
408	if (block)
409		prestera_counter_block_put(counter, block);
410
411	schedule_delayed_work(&counter->stats_dw, resched_time);
412}
413
414/* Can be executed without rtnl_lock().
415 * So pay attention when something changing.
416 */
417int prestera_counter_stats_get(struct prestera_counter *counter,
418			       struct prestera_counter_block *block,
419			       u32 counter_id, u64 *packets, u64 *bytes)
420{
421	if (!block || !prestera_counter_is_ready(block, counter_id)) {
422		*packets = 0;
423		*bytes = 0;
424		return 0;
425	}
426
427	prestera_counter_block_lock(block);
428	*packets = block->stats[counter_id - block->offset].packets;
429	*bytes = block->stats[counter_id - block->offset].bytes;
430
431	prestera_counter_stats_clear(block, counter_id);
432	prestera_counter_block_unlock(block);
433
434	return 0;
435}
436
437int prestera_counter_init(struct prestera_switch *sw)
438{
439	struct prestera_counter *counter;
440
441	counter = kzalloc(sizeof(*counter), GFP_KERNEL);
442	if (!counter)
443		return -ENOMEM;
444
445	counter->block_list = kzalloc(sizeof(*counter->block_list), GFP_KERNEL);
446	if (!counter->block_list) {
447		kfree(counter);
448		return -ENOMEM;
449	}
450
451	mutex_init(&counter->mtx);
452	counter->block_list_len = 1;
453	counter->sw = sw;
454	sw->counter = counter;
455
456	INIT_DELAYED_WORK(&counter->stats_dw, prestera_counter_stats_work);
457	schedule_delayed_work(&counter->stats_dw, COUNTER_POLL_TIME);
458
459	return 0;
460}
461
462void prestera_counter_fini(struct prestera_switch *sw)
463{
464	struct prestera_counter *counter = sw->counter;
465	u32 i;
466
467	cancel_delayed_work_sync(&counter->stats_dw);
468
469	for (i = 0; i < counter->block_list_len; i++)
470		WARN_ON(counter->block_list[i]);
471
472	mutex_destroy(&counter->mtx);
473	kfree(counter->block_list);
474	kfree(counter);
475}
476