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 | |
13 | struct 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 | |
24 | struct 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 | |
39 | enum { |
40 | COUNTER_FLAG_READY = 0, |
41 | COUNTER_FLAG_INVALID = 1 |
42 | }; |
43 | |
44 | static bool |
45 | prestera_counter_is_ready(struct prestera_counter_block *block, u32 id) |
46 | { |
47 | return block->counter_flag[id - block->offset] == COUNTER_FLAG_READY; |
48 | } |
49 | |
50 | static void prestera_counter_lock(struct prestera_counter *counter) |
51 | { |
52 | mutex_lock(&counter->mtx); |
53 | } |
54 | |
55 | static void prestera_counter_unlock(struct prestera_counter *counter) |
56 | { |
57 | mutex_unlock(lock: &counter->mtx); |
58 | } |
59 | |
60 | static void prestera_counter_block_lock(struct prestera_counter_block *block) |
61 | { |
62 | mutex_lock(&block->mtx); |
63 | } |
64 | |
65 | static void prestera_counter_block_unlock(struct prestera_counter_block *block) |
66 | { |
67 | mutex_unlock(lock: &block->mtx); |
68 | } |
69 | |
70 | static bool prestera_counter_block_incref(struct prestera_counter_block *block) |
71 | { |
72 | return refcount_inc_not_zero(r: &block->refcnt); |
73 | } |
74 | |
75 | static 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() */ |
81 | static 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 | |
88 | static struct prestera_counter_block * |
89 | prestera_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 | |
109 | static 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 | |
140 | static struct prestera_counter_block * |
141 | prestera_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 | |
186 | err_list_add: |
187 | idr_destroy(&block->counter_idr); |
188 | mutex_destroy(lock: &block->mtx); |
189 | kfree(objp: block->counter_flag); |
190 | err_flag: |
191 | kfree(objp: block->stats); |
192 | err_stats: |
193 | prestera_hw_counter_block_release(sw: counter->sw, block_id: block->id); |
194 | err_block: |
195 | kfree(objp: block); |
196 | return ERR_PTR(error: err); |
197 | } |
198 | |
199 | static 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 | |
226 | static 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 | |
251 | int 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 | |
258 | get_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 | |
284 | void 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 | |
300 | static 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 | |
319 | static struct prestera_counter_block * |
320 | prestera_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 | |
337 | static 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; |
401 | abort: |
402 | prestera_hw_counter_abort(sw: counter->sw); |
403 | next: |
404 | counter->is_fetching = false; |
405 | counter->curr_idx = |
406 | prestera_counter_block_idx_next(counter, curr_idx: counter->curr_idx); |
407 | resched: |
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 | */ |
417 | int 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 | |
437 | int 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 | |
462 | void 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 | |