Jacob Erlbeck | 4f666bc | 2015-07-20 12:40:42 +0200 | [diff] [blame] | 1 | /* gprs_codel.cpp |
| 2 | * |
| 3 | * Copyright (C) 2015 by Sysmocom s.f.m.c. GmbH |
| 4 | * Author: Jacob Erlbeck <jerlbeck@sysmocom.de> |
| 5 | * |
| 6 | * This program is free software; you can redistribute it and/or |
| 7 | * modify it under the terms of the GNU General Public License |
| 8 | * as published by the Free Software Foundation; either version 2 |
| 9 | * of the License, or (at your option) any later version. |
| 10 | * |
| 11 | * This program is distributed in the hope that it will be useful, |
| 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 14 | * GNU General Public License for more details. |
Jacob Erlbeck | 4f666bc | 2015-07-20 12:40:42 +0200 | [diff] [blame] | 15 | */ |
| 16 | |
| 17 | #include "gprs_codel.h" |
| 18 | #include "gprs_debug.h" |
| 19 | |
| 20 | #include <osmocom/core/utils.h> |
Pau Espin Pedrol | 1de6873 | 2020-03-11 14:04:52 +0100 | [diff] [blame] | 21 | #include <osmocom/core/timer_compat.h> |
Jacob Erlbeck | 4f666bc | 2015-07-20 12:40:42 +0200 | [diff] [blame] | 22 | |
| 23 | #include <stdint.h> |
| 24 | #include <stdlib.h> |
| 25 | #include <math.h> |
| 26 | |
Pau Espin Pedrol | 1de6873 | 2020-03-11 14:04:52 +0100 | [diff] [blame] | 27 | static void control_law(struct gprs_codel *state, struct timespec *delta) |
Jacob Erlbeck | 4f666bc | 2015-07-20 12:40:42 +0200 | [diff] [blame] | 28 | { |
| 29 | /* 256 / sqrt(x), limited to 255 */ |
| 30 | static uint8_t inv_sqrt_tab[] = {255, |
| 31 | 255, 181, 147, 128, 114, 104, 96, 90, 85, 80, 77, 73, 71, 68, |
| 32 | 66, 64, 62, 60, 58, 57, 55, 54, 53, 52, 51, 50, 49, 48, 47, 46, |
| 33 | 45, 45, 44, 43, 43, 42, 42, 41, 40, 40, 39, 39, 39, 38, 38, 37, |
| 34 | 37, 36, 36, 36, 35, 35, 35, 34, 34, 34, 33, 33, 33, 33, 32, 32, |
| 35 | 32, 32, 31, 31, 31, 31, 30, 30, 30, 30, 29, 29, 29, 29, 29, 28, |
| 36 | 28, 28, 28, 28, 28, 27, 27, 27, 27, 27, 27, 26, 26, 26, 26, 26, |
| 37 | 26, 26, 25, 25, 25, 25, 25, 25, 25, 25, 24, 24, 24, 24, 24, 24, |
| 38 | 24, 24, 24, 23, 23, 23, 23, 23, 23, 23, 23, 23, 23, 22, 22, 22, |
| 39 | 22, 22, 22, 22, 22, 22, 22, 22, 22, 21, 21, 21, 21, 21, 21, 21, |
| 40 | 21, 21, 21, 21, 21, 21, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, |
| 41 | 20, 20, 20, 20, 20, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, |
| 42 | 19, 19, 19, 19, 19, 19, 19, 18, 18, 18, 18, 18, 18, 18, 18, 18, |
| 43 | 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18, 17, 17, 17, 17, |
| 44 | 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, |
| 45 | 17, 17, 17, 17 |
| 46 | }; |
| 47 | uint_fast32_t delta_usecs; |
| 48 | uint_fast32_t inv_sqrt; |
| 49 | div_t q; |
| 50 | |
| 51 | if (state->count >= ARRAY_SIZE(inv_sqrt_tab)) |
| 52 | inv_sqrt = 16; |
| 53 | else |
| 54 | inv_sqrt = inv_sqrt_tab[state->count]; |
| 55 | |
| 56 | /* delta = state->interval / sqrt(count) */ |
Pau Espin Pedrol | 1de6873 | 2020-03-11 14:04:52 +0100 | [diff] [blame] | 57 | delta_usecs = state->interval.tv_sec * 1000000 + state->interval.tv_nsec/1000; |
Jacob Erlbeck | 4f666bc | 2015-07-20 12:40:42 +0200 | [diff] [blame] | 58 | delta_usecs = delta_usecs * inv_sqrt / 256; |
| 59 | |
| 60 | q = div(delta_usecs, 1000000); |
| 61 | delta->tv_sec = q.quot; |
Pau Espin Pedrol | 1de6873 | 2020-03-11 14:04:52 +0100 | [diff] [blame] | 62 | delta->tv_nsec = q.rem * 1000; |
Jacob Erlbeck | 4f666bc | 2015-07-20 12:40:42 +0200 | [diff] [blame] | 63 | } |
| 64 | |
| 65 | void gprs_codel_init(struct gprs_codel *state) |
| 66 | { |
| 67 | static const struct gprs_codel init_state = {0}; |
| 68 | |
| 69 | *state = init_state; |
| 70 | gprs_codel_set_interval(state, -1); |
| 71 | gprs_codel_set_maxpacket(state, -1); |
| 72 | } |
| 73 | |
| 74 | void gprs_codel_set_interval(struct gprs_codel *state, int interval_ms) |
| 75 | { |
| 76 | div_t q; |
| 77 | |
| 78 | if (interval_ms <= 0) |
| 79 | interval_ms = GPRS_CODEL_DEFAULT_INTERVAL_MS; |
| 80 | |
| 81 | q = div(interval_ms, 1000); |
| 82 | state->interval.tv_sec = q.quot; |
Pau Espin Pedrol | 1de6873 | 2020-03-11 14:04:52 +0100 | [diff] [blame] | 83 | state->interval.tv_nsec = q.rem * 1000000; |
Jacob Erlbeck | 4f666bc | 2015-07-20 12:40:42 +0200 | [diff] [blame] | 84 | |
| 85 | /* target ~ 5% of interval */ |
| 86 | q = div(interval_ms * 13 / 256, 1000); |
| 87 | state->target.tv_sec = q.quot; |
Pau Espin Pedrol | 1de6873 | 2020-03-11 14:04:52 +0100 | [diff] [blame] | 88 | state->target.tv_nsec = q.rem * 1000000; |
Jacob Erlbeck | 4f666bc | 2015-07-20 12:40:42 +0200 | [diff] [blame] | 89 | } |
| 90 | |
| 91 | void gprs_codel_set_maxpacket(struct gprs_codel *state, int maxpacket) |
| 92 | { |
| 93 | |
| 94 | if (maxpacket < 0) |
| 95 | maxpacket = GPRS_CODEL_DEFAULT_MAXPACKET; |
| 96 | |
| 97 | state->maxpacket = maxpacket; |
| 98 | } |
| 99 | |
| 100 | /* |
| 101 | * This is an broken up variant of the algorithm being described in |
| 102 | * http://queue.acm.org/appendices/codel.html |
| 103 | */ |
Pau Espin Pedrol | 1de6873 | 2020-03-11 14:04:52 +0100 | [diff] [blame] | 104 | int gprs_codel_control(struct gprs_codel *state, const struct timespec *recv, |
| 105 | const struct timespec *now, int bytes) |
Jacob Erlbeck | 4f666bc | 2015-07-20 12:40:42 +0200 | [diff] [blame] | 106 | { |
Pau Espin Pedrol | 1de6873 | 2020-03-11 14:04:52 +0100 | [diff] [blame] | 107 | struct timespec sojourn_time; |
| 108 | struct timespec delta; |
Jacob Erlbeck | 4f666bc | 2015-07-20 12:40:42 +0200 | [diff] [blame] | 109 | |
| 110 | if (recv == NULL) |
| 111 | goto stop_dropping; |
| 112 | |
Pau Espin Pedrol | 1de6873 | 2020-03-11 14:04:52 +0100 | [diff] [blame] | 113 | timespecsub(now, recv, &sojourn_time); |
Jacob Erlbeck | 4f666bc | 2015-07-20 12:40:42 +0200 | [diff] [blame] | 114 | |
Pau Espin Pedrol | 1de6873 | 2020-03-11 14:04:52 +0100 | [diff] [blame] | 115 | if (timespeccmp(&sojourn_time, &state->target, <)) |
Jacob Erlbeck | 4f666bc | 2015-07-20 12:40:42 +0200 | [diff] [blame] | 116 | goto stop_dropping; |
| 117 | |
| 118 | if (bytes >= 0 && (unsigned)bytes <= state->maxpacket) |
| 119 | goto stop_dropping; |
| 120 | |
Pau Espin Pedrol | 1de6873 | 2020-03-11 14:04:52 +0100 | [diff] [blame] | 121 | if (!timespecisset(&state->first_above_time)) { |
| 122 | timespecadd(now, &state->interval, &state->first_above_time); |
Jacob Erlbeck | 4f666bc | 2015-07-20 12:40:42 +0200 | [diff] [blame] | 123 | goto not_ok_to_drop; |
| 124 | } |
| 125 | |
Pau Espin Pedrol | 1de6873 | 2020-03-11 14:04:52 +0100 | [diff] [blame] | 126 | if (timespeccmp(now, &state->first_above_time, <)) |
Jacob Erlbeck | 4f666bc | 2015-07-20 12:40:42 +0200 | [diff] [blame] | 127 | goto not_ok_to_drop; |
| 128 | |
| 129 | /* Ok to drop */ |
| 130 | |
| 131 | if (!state->dropping) { |
| 132 | int recently = 0; |
| 133 | int in_drop_cycle = 0; |
Pau Espin Pedrol | 1de6873 | 2020-03-11 14:04:52 +0100 | [diff] [blame] | 134 | if (timespecisset(&state->drop_next)) { |
| 135 | timespecsub(now, &state->drop_next, &delta); |
| 136 | in_drop_cycle = timespeccmp(&delta, &state->interval, <); |
Jacob Erlbeck | 4f666bc | 2015-07-20 12:40:42 +0200 | [diff] [blame] | 137 | recently = in_drop_cycle; |
| 138 | } |
| 139 | if (!recently) { |
Pau Espin Pedrol | 1de6873 | 2020-03-11 14:04:52 +0100 | [diff] [blame] | 140 | timespecsub(now, &state->first_above_time, &delta); |
| 141 | recently = !timespeccmp(&delta, &state->interval, <); |
Jacob Erlbeck | 4f666bc | 2015-07-20 12:40:42 +0200 | [diff] [blame] | 142 | }; |
| 143 | if (!recently) |
| 144 | return 0; |
| 145 | |
| 146 | state->dropping = 1; |
| 147 | |
| 148 | if (in_drop_cycle && state->count > 2) |
| 149 | state->count -= 2; |
| 150 | else |
| 151 | state->count = 1; |
| 152 | |
| 153 | state->drop_next = *now; |
| 154 | } else { |
Pau Espin Pedrol | 1de6873 | 2020-03-11 14:04:52 +0100 | [diff] [blame] | 155 | if (timespeccmp(now, &state->drop_next, <)) |
Jacob Erlbeck | 4f666bc | 2015-07-20 12:40:42 +0200 | [diff] [blame] | 156 | return 0; |
| 157 | |
| 158 | state->count += 1; |
| 159 | } |
| 160 | |
| 161 | control_law(state, &delta); |
Pau Espin Pedrol | 1de6873 | 2020-03-11 14:04:52 +0100 | [diff] [blame] | 162 | timespecadd(&state->drop_next, &delta, &state->drop_next); |
Jacob Erlbeck | 4f666bc | 2015-07-20 12:40:42 +0200 | [diff] [blame] | 163 | |
| 164 | #if 1 |
| 165 | LOGP(DRLCMAC, LOGL_INFO, |
| 166 | "CoDel decided to drop packet, window = %d.%03dms, count = %d\n", |
Pau Espin Pedrol | 1de6873 | 2020-03-11 14:04:52 +0100 | [diff] [blame] | 167 | (int)delta.tv_sec, (int)(delta.tv_nsec / 1000000), state->count); |
Jacob Erlbeck | 4f666bc | 2015-07-20 12:40:42 +0200 | [diff] [blame] | 168 | #endif |
| 169 | return 1; |
| 170 | |
| 171 | stop_dropping: |
Pau Espin Pedrol | 1de6873 | 2020-03-11 14:04:52 +0100 | [diff] [blame] | 172 | timespecclear(&state->first_above_time); |
Jacob Erlbeck | 4f666bc | 2015-07-20 12:40:42 +0200 | [diff] [blame] | 173 | not_ok_to_drop: |
| 174 | state->dropping = 0; |
| 175 | return 0; |
| 176 | } |