marvell/prestera/prestera_counter.c

0001 // SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
0002 /* Copyright (c) 2021 Marvell International Ltd. All rights reserved */
0003
0004 #include "prestera.h"
0005 #include "prestera_hw.h"
0006 #include "prestera_acl.h"
0007 #include "prestera_counter.h"
0008
0009 #define COUNTER_POLL_TIME   (msecs_to_jiffies(1000))
0010 #define COUNTER_RESCHED_TIME    (msecs_to_jiffies(50))
0011 #define COUNTER_BULK_SIZE   (256)
0012
0013 struct prestera_counter {
0014     struct prestera_switch *sw;
0015     struct delayed_work stats_dw;
0016     struct mutex mtx;  /* protect block_list */
0017     struct prestera_counter_block **block_list;
0018     u32 total_read;
0019     u32 block_list_len;
0020     u32 curr_idx;
0021     bool is_fetching;
0022 };
0023
0024 struct prestera_counter_block {
0025     struct list_head list;
0026     u32 id;
0027     u32 offset;
0028     u32 num_counters;
0029     u32 client;
0030     struct idr counter_idr;
0031     refcount_t refcnt;
0032     struct mutex mtx;  /* protect stats and counter_idr */
0033     struct prestera_counter_stats *stats;
0034     u8 *counter_flag;
0035     bool is_updating;
0036     bool full;
0037 };
0038
0039 enum {
0040     COUNTER_FLAG_READY = 0,
0041     COUNTER_FLAG_INVALID = 1
0042 };
0043
0044 static bool
0045 prestera_counter_is_ready(struct prestera_counter_block *block, u32 id)
0046 {
0047     return block->counter_flag[id - block->offset] == COUNTER_FLAG_READY;
0048 }
0049
0050 static void prestera_counter_lock(struct prestera_counter *counter)
0051 {
0052     mutex_lock(&counter->mtx);
0053 }
0054
0055 static void prestera_counter_unlock(struct prestera_counter *counter)
0056 {
0057     mutex_unlock(&counter->mtx);
0058 }
0059
0060 static void prestera_counter_block_lock(struct prestera_counter_block *block)
0061 {
0062     mutex_lock(&block->mtx);
0063 }
0064
0065 static void prestera_counter_block_unlock(struct prestera_counter_block *block)
0066 {
0067     mutex_unlock(&block->mtx);
0068 }
0069
0070 static bool prestera_counter_block_incref(struct prestera_counter_block *block)
0071 {
0072     return refcount_inc_not_zero(&block->refcnt);
0073 }
0074
0075 static bool prestera_counter_block_decref(struct prestera_counter_block *block)
0076 {
0077     return refcount_dec_and_test(&block->refcnt);
0078 }
0079
0080 /* must be called with prestera_counter_block_lock() */
0081 static void prestera_counter_stats_clear(struct prestera_counter_block *block,
0082                      u32 counter_id)
0083 {
0084     memset(&block->stats[counter_id - block->offset], 0,
0085            sizeof(*block->stats));
0086 }
0087
0088 static struct prestera_counter_block *
0089 prestera_counter_block_lookup_not_full(struct prestera_counter *counter,
0090                        u32 client)
0091 {
0092     u32 i;
0093
0094     prestera_counter_lock(counter);
0095     for (i = 0; i < counter->block_list_len; i++) {
0096         if (counter->block_list[i] &&
0097             counter->block_list[i]->client == client &&
0098             !counter->block_list[i]->full &&
0099             prestera_counter_block_incref(counter->block_list[i])) {
0100             prestera_counter_unlock(counter);
0101             return counter->block_list[i];
0102         }
0103     }
0104     prestera_counter_unlock(counter);
0105
0106     return NULL;
0107 }
0108
0109 static int prestera_counter_block_list_add(struct prestera_counter *counter,
0110                        struct prestera_counter_block *block)
0111 {
0112     struct prestera_counter_block **arr;
0113     u32 i;
0114
0115     prestera_counter_lock(counter);
0116
0117     for (i = 0; i < counter->block_list_len; i++) {
0118         if (counter->block_list[i])
0119             continue;
0120
0121         counter->block_list[i] = block;
0122         prestera_counter_unlock(counter);
0123         return 0;
0124     }
0125
0126     arr = krealloc(counter->block_list, (counter->block_list_len + 1) *
0127                sizeof(*counter->block_list), GFP_KERNEL);
0128     if (!arr) {
0129         prestera_counter_unlock(counter);
0130         return -ENOMEM;
0131     }
0132
0133     counter->block_list = arr;
0134     counter->block_list[counter->block_list_len] = block;
0135     counter->block_list_len++;
0136     prestera_counter_unlock(counter);
0137     return 0;
0138 }
0139
0140 static struct prestera_counter_block *
0141 prestera_counter_block_get(struct prestera_counter *counter, u32 client)
0142 {
0143     struct prestera_counter_block *block;
0144     int err;
0145
0146     block = prestera_counter_block_lookup_not_full(counter, client);
0147     if (block)
0148         return block;
0149
0150     block = kzalloc(sizeof(*block), GFP_KERNEL);
0151     if (!block)
0152         return ERR_PTR(-ENOMEM);
0153
0154     err = prestera_hw_counter_block_get(counter->sw, client,
0155                         &block->id, &block->offset,
0156                         &block->num_counters);
0157     if (err)
0158         goto err_block;
0159
0160     block->stats = kcalloc(block->num_counters,
0161                    sizeof(*block->stats), GFP_KERNEL);
0162     if (!block->stats) {
0163         err = -ENOMEM;
0164         goto err_stats;
0165     }
0166
0167     block->counter_flag = kcalloc(block->num_counters,
0168                       sizeof(*block->counter_flag),
0169                       GFP_KERNEL);
0170     if (!block->counter_flag) {
0171         err = -ENOMEM;
0172         goto err_flag;
0173     }
0174
0175     block->client = client;
0176     mutex_init(&block->mtx);
0177     refcount_set(&block->refcnt, 1);
0178     idr_init_base(&block->counter_idr, block->offset);
0179
0180     err = prestera_counter_block_list_add(counter, block);
0181     if (err)
0182         goto err_list_add;
0183
0184     return block;
0185
0186 err_list_add:
0187     idr_destroy(&block->counter_idr);
0188     mutex_destroy(&block->mtx);
0189     kfree(block->counter_flag);
0190 err_flag:
0191     kfree(block->stats);
0192 err_stats:
0193     prestera_hw_counter_block_release(counter->sw, block->id);
0194 err_block:
0195     kfree(block);
0196     return ERR_PTR(err);
0197 }
0198
0199 static void prestera_counter_block_put(struct prestera_counter *counter,
0200                        struct prestera_counter_block *block)
0201 {
0202     u32 i;
0203
0204     if (!prestera_counter_block_decref(block))
0205         return;
0206
0207     prestera_counter_lock(counter);
0208     for (i = 0; i < counter->block_list_len; i++) {
0209         if (counter->block_list[i] &&
0210             counter->block_list[i]->id == block->id) {
0211             counter->block_list[i] = NULL;
0212             break;
0213         }
0214     }
0215     prestera_counter_unlock(counter);
0216
0217     WARN_ON(!idr_is_empty(&block->counter_idr));
0218
0219     prestera_hw_counter_block_release(counter->sw, block->id);
0220     idr_destroy(&block->counter_idr);
0221     mutex_destroy(&block->mtx);
0222     kfree(block->stats);
0223     kfree(block);
0224 }
0225
0226 static int prestera_counter_get_vacant(struct prestera_counter_block *block,
0227                        u32 *id)
0228 {
0229     int free_id;
0230
0231     if (block->full)
0232         return -ENOSPC;
0233
0234     prestera_counter_block_lock(block);
0235     free_id = idr_alloc_cyclic(&block->counter_idr, NULL, block->offset,
0236                    block->offset + block->num_counters,
0237                    GFP_KERNEL);
0238     if (free_id < 0) {
0239         if (free_id == -ENOSPC)
0240             block->full = true;
0241
0242         prestera_counter_block_unlock(block);
0243         return free_id;
0244     }
0245     *id = free_id;
0246     prestera_counter_block_unlock(block);
0247
0248     return 0;
0249 }
0250
0251 int prestera_counter_get(struct prestera_counter *counter, u32 client,
0252              struct prestera_counter_block **bl, u32 *counter_id)
0253 {
0254     struct prestera_counter_block *block;
0255     int err;
0256     u32 id;
0257
0258 get_next_block:
0259     block = prestera_counter_block_get(counter, client);
0260     if (IS_ERR(block))
0261         return PTR_ERR(block);
0262
0263     err = prestera_counter_get_vacant(block, &id);
0264     if (err) {
0265         prestera_counter_block_put(counter, block);
0266
0267         if (err == -ENOSPC)
0268             goto get_next_block;
0269
0270         return err;
0271     }
0272
0273     prestera_counter_block_lock(block);
0274     if (block->is_updating)
0275         block->counter_flag[id - block->offset] = COUNTER_FLAG_INVALID;
0276     prestera_counter_block_unlock(block);
0277
0278     *counter_id = id;
0279     *bl = block;
0280
0281     return 0;
0282 }
0283
0284 void prestera_counter_put(struct prestera_counter *counter,
0285               struct prestera_counter_block *block, u32 counter_id)
0286 {
0287     if (!block)
0288         return;
0289
0290     prestera_counter_block_lock(block);
0291     idr_remove(&block->counter_idr, counter_id);
0292     block->full = false;
0293     prestera_counter_stats_clear(block, counter_id);
0294     prestera_counter_block_unlock(block);
0295
0296     prestera_hw_counter_clear(counter->sw, block->id, counter_id);
0297     prestera_counter_block_put(counter, block);
0298 }
0299
0300 static u32 prestera_counter_block_idx_next(struct prestera_counter *counter,
0301                        u32 curr_idx)
0302 {
0303     u32 idx, i, start = curr_idx + 1;
0304
0305     prestera_counter_lock(counter);
0306     for (i = 0; i < counter->block_list_len; i++) {
0307         idx = (start + i) % counter->block_list_len;
0308         if (!counter->block_list[idx])
0309             continue;
0310
0311         prestera_counter_unlock(counter);
0312         return idx;
0313     }
0314     prestera_counter_unlock(counter);
0315
0316     return 0;
0317 }
0318
0319 static struct prestera_counter_block *
0320 prestera_counter_block_get_by_idx(struct prestera_counter *counter, u32 idx)
0321 {
0322     if (idx >= counter->block_list_len)
0323         return NULL;
0324
0325     prestera_counter_lock(counter);
0326
0327     if (!counter->block_list[idx] ||
0328         !prestera_counter_block_incref(counter->block_list[idx])) {
0329         prestera_counter_unlock(counter);
0330         return NULL;
0331     }
0332
0333     prestera_counter_unlock(counter);
0334     return counter->block_list[idx];
0335 }
0336
0337 static void prestera_counter_stats_work(struct work_struct *work)
0338 {
0339     struct delayed_work *dl_work =
0340         container_of(work, struct delayed_work, work);
0341     struct prestera_counter *counter =
0342         container_of(dl_work, struct prestera_counter, stats_dw);
0343     struct prestera_counter_block *block;
0344     u32 resched_time = COUNTER_POLL_TIME;
0345     u32 count = COUNTER_BULK_SIZE;
0346     bool done = false;
0347     int err;
0348     u32 i;
0349
0350     block = prestera_counter_block_get_by_idx(counter, counter->curr_idx);
0351     if (!block) {
0352         if (counter->is_fetching)
0353             goto abort;
0354
0355         goto next;
0356     }
0357
0358     if (!counter->is_fetching) {
0359         err = prestera_hw_counter_trigger(counter->sw, block->id);
0360         if (err)
0361             goto abort;
0362
0363         prestera_counter_block_lock(block);
0364         block->is_updating = true;
0365         prestera_counter_block_unlock(block);
0366
0367         counter->is_fetching = true;
0368         counter->total_read = 0;
0369         resched_time = COUNTER_RESCHED_TIME;
0370         goto resched;
0371     }
0372
0373     prestera_counter_block_lock(block);
0374     err = prestera_hw_counters_get(counter->sw, counter->total_read,
0375                        &count, &done,
0376                        &block->stats[counter->total_read]);
0377     prestera_counter_block_unlock(block);
0378     if (err)
0379         goto abort;
0380
0381     counter->total_read += count;
0382     if (!done || counter->total_read < block->num_counters) {
0383         resched_time = COUNTER_RESCHED_TIME;
0384         goto resched;
0385     }
0386
0387     for (i = 0; i < block->num_counters; i++) {
0388         if (block->counter_flag[i] == COUNTER_FLAG_INVALID) {
0389             prestera_counter_block_lock(block);
0390             block->counter_flag[i] = COUNTER_FLAG_READY;
0391             memset(&block->stats[i], 0, sizeof(*block->stats));
0392             prestera_counter_block_unlock(block);
0393         }
0394     }
0395
0396     prestera_counter_block_lock(block);
0397     block->is_updating = false;
0398     prestera_counter_block_unlock(block);
0399
0400     goto next;
0401 abort:
0402     prestera_hw_counter_abort(counter->sw);
0403 next:
0404     counter->is_fetching = false;
0405     counter->curr_idx =
0406         prestera_counter_block_idx_next(counter, counter->curr_idx);
0407 resched:
0408     if (block)
0409         prestera_counter_block_put(counter, block);
0410
0411     schedule_delayed_work(&counter->stats_dw, resched_time);
0412 }
0413
0414 /* Can be executed without rtnl_lock().
0415  * So pay attention when something changing.
0416  */
0417 int prestera_counter_stats_get(struct prestera_counter *counter,
0418                    struct prestera_counter_block *block,
0419                    u32 counter_id, u64 *packets, u64 *bytes)
0420 {
0421     if (!block || !prestera_counter_is_ready(block, counter_id)) {
0422         *packets = 0;
0423         *bytes = 0;
0424         return 0;
0425     }
0426
0427     prestera_counter_block_lock(block);
0428     *packets = block->stats[counter_id - block->offset].packets;
0429     *bytes = block->stats[counter_id - block->offset].bytes;
0430
0431     prestera_counter_stats_clear(block, counter_id);
0432     prestera_counter_block_unlock(block);
0433
0434     return 0;
0435 }
0436
0437 int prestera_counter_init(struct prestera_switch *sw)
0438 {
0439     struct prestera_counter *counter;
0440
0441     counter = kzalloc(sizeof(*counter), GFP_KERNEL);
0442     if (!counter)
0443         return -ENOMEM;
0444
0445     counter->block_list = kzalloc(sizeof(*counter->block_list), GFP_KERNEL);
0446     if (!counter->block_list) {
0447         kfree(counter);
0448         return -ENOMEM;
0449     }
0450
0451     mutex_init(&counter->mtx);
0452     counter->block_list_len = 1;
0453     counter->sw = sw;
0454     sw->counter = counter;
0455
0456     INIT_DELAYED_WORK(&counter->stats_dw, prestera_counter_stats_work);
0457     schedule_delayed_work(&counter->stats_dw, COUNTER_POLL_TIME);
0458
0459     return 0;
0460 }
0461
0462 void prestera_counter_fini(struct prestera_switch *sw)
0463 {
0464     struct prestera_counter *counter = sw->counter;
0465     u32 i;
0466
0467     cancel_delayed_work_sync(&counter->stats_dw);
0468
0469     for (i = 0; i < counter->block_list_len; i++)
0470         WARN_ON(counter->block_list[i]);
0471
0472     mutex_destroy(&counter->mtx);
0473     kfree(counter->block_list);
0474     kfree(counter);
0475 }