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 = 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 */ 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 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 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