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(lock: &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(lock: &block->mtx);
68}
69
70static bool prestera_counter_block_incref(struct prestera_counter_block *block)
71{
72 return refcount_inc_not_zero(r: &block->refcnt);
73}
74
75static bool prestera_counter_block_decref(struct prestera_counter_block *block)
76{
77 return refcount_dec_and_test(r: &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(block: 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(objp: counter->block_list, new_size: (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(size: sizeof(*block), GFP_KERNEL);
151 if (!block)
152 return ERR_PTR(error: -ENOMEM);
153
154 err = prestera_hw_counter_block_get(sw: counter->sw, client,
155 block_id: &block->id, offset: &block->offset,
156 num_counters: &block->num_counters);
157 if (err)
158 goto err_block;
159
160 block->stats = kcalloc(n: block->num_counters,
161 size: sizeof(*block->stats), GFP_KERNEL);
162 if (!block->stats) {
163 err = -ENOMEM;
164 goto err_stats;
165 }
166
167 block->counter_flag = kcalloc(n: block->num_counters,
168 size: 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(r: &block->refcnt, n: 1);
178 idr_init_base(idr: &block->counter_idr, base: 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(lock: &block->mtx);
189 kfree(objp: block->counter_flag);
190err_flag:
191 kfree(objp: block->stats);
192err_stats:
193 prestera_hw_counter_block_release(sw: counter->sw, block_id: block->id);
194err_block:
195 kfree(objp: block);
196 return ERR_PTR(error: 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(sw: counter->sw, block_id: block->id);
220 idr_destroy(&block->counter_idr);
221 mutex_destroy(lock: &block->mtx);
222 kfree(objp: block->stats);
223 kfree(objp: 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, start: block->offset,
236 end: 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(ptr: block))
261 return PTR_ERR(ptr: block);
262
263 err = prestera_counter_get_vacant(block, id: &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, id: 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(sw: counter->sw, block_id: 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(block: 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, idx: 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(sw: counter->sw, block_id: 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(sw: counter->sw, idx: counter->total_read,
375 len: &count, done: &done,
376 stats: &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(sw: counter->sw);
403next:
404 counter->is_fetching = false;
405 counter->curr_idx =
406 prestera_counter_block_idx_next(counter, curr_idx: counter->curr_idx);
407resched:
408 if (block)
409 prestera_counter_block_put(counter, block);
410
411 schedule_delayed_work(dwork: &counter->stats_dw, delay: 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, id: 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(size: sizeof(*counter), GFP_KERNEL);
442 if (!counter)
443 return -ENOMEM;
444
445 counter->block_list = kzalloc(size: sizeof(*counter->block_list), GFP_KERNEL);
446 if (!counter->block_list) {
447 kfree(objp: 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(dwork: &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(dwork: &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(lock: &counter->mtx);
473 kfree(objp: counter->block_list);
474 kfree(objp: counter);
475}
476

source code of linux/drivers/net/ethernet/marvell/prestera/prestera_counter.c