drivers/net/ethernet/marvell/prestera/prestera_counter.c

   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 }