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