|
| 1 | +/* SPDX-License-Identifier: GPL-3.0-only */ |
| 2 | +#include <errno.h> |
| 3 | + |
| 4 | +#include <bwlimit.h> |
| 5 | +#include <platform.h> |
| 6 | + |
| 7 | +#define timespeczerorize(ts) \ |
| 8 | + do { \ |
| 9 | + ts.tv_sec = 0; \ |
| 10 | + ts.tv_nsec = 0; \ |
| 11 | + } while (0) |
| 12 | + |
| 13 | +int bwlimit_init(struct bwlimit *bw, uint64_t bps, uint64_t win) |
| 14 | +{ |
| 15 | + if (!(bw->sem = sem_create(1))) |
| 16 | + return -1; |
| 17 | + |
| 18 | + bw->bps = bps; |
| 19 | + bw->win = win; /* msec window */ |
| 20 | + bw->amt = (double)bps / 8 / 1000 * win; /* bytes in a window (msec) */ |
| 21 | + bw->credit = bw->amt; |
| 22 | + timespeczerorize(bw->wstart); |
| 23 | + timespeczerorize(bw->wend); |
| 24 | + |
| 25 | + return 0; |
| 26 | +} |
| 27 | + |
| 28 | +#define timespecisset(ts) ((ts).tv_sec || (ts).tv_nsec) |
| 29 | + |
| 30 | +#define timespecmsadd(a, msec, r) \ |
| 31 | + do { \ |
| 32 | + (r).tv_sec = (a).tv_sec; \ |
| 33 | + (r).tv_nsec = (a).tv_nsec + (msec * 1000000); \ |
| 34 | + if ((r).tv_nsec > 1000000000) { \ |
| 35 | + (r).tv_sec += (r.tv_nsec) / 1000000000L; \ |
| 36 | + (r).tv_nsec = (r.tv_nsec) % 1000000000L; \ |
| 37 | + } \ |
| 38 | + } while (0) |
| 39 | + |
| 40 | +#define timespecsub(a, b, r) \ |
| 41 | + do { \ |
| 42 | + (r).tv_sec = (a).tv_sec - (b).tv_sec; \ |
| 43 | + (r).tv_nsec = (a).tv_nsec - (b).tv_nsec; \ |
| 44 | + if ((r).tv_nsec < 0) { \ |
| 45 | + (r).tv_sec -= 1; \ |
| 46 | + (r).tv_nsec += 1000000000; \ |
| 47 | + } \ |
| 48 | + } while (0) |
| 49 | + |
| 50 | +#define timespeccmp(a, b, expr) \ |
| 51 | + ((a.tv_sec * 1000000000 + a.tv_nsec) expr(b.tv_sec * 1000000000 + b.tv_nsec)) |
| 52 | + |
| 53 | +#include <stdio.h> |
| 54 | + |
| 55 | +int bwlimit_wait(struct bwlimit *bw, size_t nr_bytes) |
| 56 | +{ |
| 57 | + struct timespec now, end, rq, rm; |
| 58 | + |
| 59 | + if (bw->bps == 0) |
| 60 | + return 0; /* no bandwidth limit */ |
| 61 | + |
| 62 | + if (sem_wait(bw->sem) < 0) |
| 63 | + return -1; |
| 64 | + |
| 65 | + clock_gettime(CLOCK_MONOTONIC, &now); |
| 66 | + |
| 67 | + if (!timespecisset(bw->wstart)) { |
| 68 | + bw->wstart = now; |
| 69 | + timespecmsadd(bw->wstart, bw->win, bw->wend); |
| 70 | + } |
| 71 | + |
| 72 | + bw->credit -= nr_bytes; |
| 73 | + |
| 74 | + if (bw->credit < 0) { |
| 75 | + /* no more credit on this window. sleep until the end |
| 76 | + * of this windown and additional time for the |
| 77 | + * remaining bytes. */ |
| 78 | + uint64_t addition = (double)(bw->credit * -1) / (bw->bps / 8); |
| 79 | + timespecmsadd(bw->wend, addition * 1000, end); |
| 80 | + if (timespeccmp(end, now, >)) { |
| 81 | + timespecsub(end, now, rq); |
| 82 | + while (nanosleep(&rq, &rm) == -1) { |
| 83 | + if (errno != EINTR) |
| 84 | + break; |
| 85 | + rq = rm; |
| 86 | + } |
| 87 | + } |
| 88 | + bw->credit = bw->amt; |
| 89 | + timespeczerorize(bw->wstart); |
| 90 | + } |
| 91 | + |
| 92 | + sem_post(bw->sem); |
| 93 | + return 0; |
| 94 | +} |
0 commit comments