476 lines · c
1// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.02/* 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 = 142};43 44static bool45prestera_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