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
prestera_counter_is_ready(struct prestera_counter_block * block,u32 id)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 
prestera_counter_lock(struct prestera_counter * counter)50 static void prestera_counter_lock(struct prestera_counter *counter)
51 {
52 	mutex_lock(&counter->mtx);
53 }
54 
prestera_counter_unlock(struct prestera_counter * counter)55 static void prestera_counter_unlock(struct prestera_counter *counter)
56 {
57 	mutex_unlock(&counter->mtx);
58 }
59 
prestera_counter_block_lock(struct prestera_counter_block * block)60 static void prestera_counter_block_lock(struct prestera_counter_block *block)
61 {
62 	mutex_lock(&block->mtx);
63 }
64 
prestera_counter_block_unlock(struct prestera_counter_block * block)65 static void prestera_counter_block_unlock(struct prestera_counter_block *block)
66 {
67 	mutex_unlock(&block->mtx);
68 }
69 
prestera_counter_block_incref(struct prestera_counter_block * block)70 static bool prestera_counter_block_incref(struct prestera_counter_block *block)
71 {
72 	return refcount_inc_not_zero(&block->refcnt);
73 }
74 
prestera_counter_block_decref(struct prestera_counter_block * block)75 static 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() */
prestera_counter_stats_clear(struct prestera_counter_block * block,u32 counter_id)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 *
prestera_counter_block_lookup_not_full(struct prestera_counter * counter,u32 client)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(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 
prestera_counter_block_list_add(struct prestera_counter * counter,struct prestera_counter_block * block)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(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 
140 static struct prestera_counter_block *
prestera_counter_block_get(struct prestera_counter * counter,u32 client)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(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 
186 err_list_add:
187 	idr_destroy(&block->counter_idr);
188 	mutex_destroy(&block->mtx);
189 	kfree(block->counter_flag);
190 err_flag:
191 	kfree(block->stats);
192 err_stats:
193 	prestera_hw_counter_block_release(counter->sw, block->id);
194 err_block:
195 	kfree(block);
196 	return ERR_PTR(err);
197 }
198 
prestera_counter_block_put(struct prestera_counter * counter,struct prestera_counter_block * block)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(counter->sw, block->id);
220 	idr_destroy(&block->counter_idr);
221 	mutex_destroy(&block->mtx);
222 	kfree(block->stats);
223 	kfree(block);
224 }
225 
prestera_counter_get_vacant(struct prestera_counter_block * block,u32 * id)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, 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 
prestera_counter_get(struct prestera_counter * counter,u32 client,struct prestera_counter_block ** bl,u32 * counter_id)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(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 
prestera_counter_put(struct prestera_counter * counter,struct prestera_counter_block * block,u32 counter_id)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, 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 
prestera_counter_block_idx_next(struct prestera_counter * counter,u32 curr_idx)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 *
prestera_counter_block_get_by_idx(struct prestera_counter * counter,u32 idx)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(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 
prestera_counter_stats_work(struct work_struct * work)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, 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;
401 abort:
402 	prestera_hw_counter_abort(counter->sw);
403 next:
404 	counter->is_fetching = false;
405 	counter->curr_idx =
406 		prestera_counter_block_idx_next(counter, counter->curr_idx);
407 resched:
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  */
prestera_counter_stats_get(struct prestera_counter * counter,struct prestera_counter_block * block,u32 counter_id,u64 * packets,u64 * bytes)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, 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 
prestera_counter_init(struct prestera_switch * sw)437 int 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 
prestera_counter_fini(struct prestera_switch * sw)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(&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