1 /* SPDX-License-Identifier: GPL-2.0-only */ 2 /* 3 * Copyright (c) 2016 Qualcomm Atheros, Inc 4 * 5 * Based on net/sched/sch_fq_codel.c 6 */ 7 #ifndef __NET_SCHED_FQ_IMPL_H 8 #define __NET_SCHED_FQ_IMPL_H 9 10 #include <net/fq.h> 11 12 /* functions that are embedded into includer */ 13 14 15 static void 16 __fq_adjust_removal(struct fq *fq, struct fq_flow *flow, unsigned int packets, 17 unsigned int bytes, unsigned int truesize) 18 { 19 struct fq_tin *tin = flow->tin; 20 int idx; 21 22 tin->backlog_bytes -= bytes; 23 tin->backlog_packets -= packets; 24 flow->backlog -= bytes; 25 fq->backlog -= packets; 26 fq->memory_usage -= truesize; 27 28 if (flow->backlog) 29 return; 30 31 if (flow == &tin->default_flow) { 32 list_del_init(&tin->tin_list); 33 return; 34 } 35 36 idx = flow - fq->flows; 37 __clear_bit(idx, fq->flows_bitmap); 38 } 39 40 static void fq_adjust_removal(struct fq *fq, 41 struct fq_flow *flow, 42 struct sk_buff *skb) 43 { 44 __fq_adjust_removal(fq, flow, 1, skb->len, skb->truesize); 45 } 46 47 static struct sk_buff *fq_flow_dequeue(struct fq *fq, 48 struct fq_flow *flow) 49 { 50 struct sk_buff *skb; 51 52 lockdep_assert_held(&fq->lock); 53 54 skb = __skb_dequeue(&flow->queue); 55 if (!skb) 56 return NULL; 57 58 fq_adjust_removal(fq, flow, skb); 59 60 return skb; 61 } 62 63 static int fq_flow_drop(struct fq *fq, struct fq_flow *flow, 64 fq_skb_free_t free_func) 65 { 66 unsigned int packets = 0, bytes = 0, truesize = 0; 67 struct fq_tin *tin = flow->tin; 68 struct sk_buff *skb; 69 int pending; 70 71 lockdep_assert_held(&fq->lock); 72 73 pending = min_t(int, 32, skb_queue_len(&flow->queue) / 2); 74 do { 75 skb = __skb_dequeue(&flow->queue); 76 if (!skb) 77 break; 78 79 packets++; 80 bytes += skb->len; 81 truesize += skb->truesize; 82 free_func(fq, tin, flow, skb); 83 } while (packets < pending); 84 85 __fq_adjust_removal(fq, flow, packets, bytes, truesize); 86 87 return packets; 88 } 89 90 static struct sk_buff *fq_tin_dequeue(struct fq *fq, 91 struct fq_tin *tin, 92 fq_tin_dequeue_t dequeue_func) 93 { 94 struct fq_flow *flow; 95 struct list_head *head; 96 struct sk_buff *skb; 97 98 lockdep_assert_held(&fq->lock); 99 100 begin: 101 head = &tin->new_flows; 102 if (list_empty(head)) { 103 head = &tin->old_flows; 104 if (list_empty(head)) 105 return NULL; 106 } 107 108 flow = list_first_entry(head, struct fq_flow, flowchain); 109 110 if (flow->deficit <= 0) { 111 flow->deficit += fq->quantum; 112 list_move_tail(&flow->flowchain, 113 &tin->old_flows); 114 goto begin; 115 } 116 117 skb = dequeue_func(fq, tin, flow); 118 if (!skb) { 119 /* force a pass through old_flows to prevent starvation */ 120 if ((head == &tin->new_flows) && 121 !list_empty(&tin->old_flows)) { 122 list_move_tail(&flow->flowchain, &tin->old_flows); 123 } else { 124 list_del_init(&flow->flowchain); 125 flow->tin = NULL; 126 } 127 goto begin; 128 } 129 130 flow->deficit -= skb->len; 131 tin->tx_bytes += skb->len; 132 tin->tx_packets++; 133 134 return skb; 135 } 136 137 static u32 fq_flow_idx(struct fq *fq, struct sk_buff *skb) 138 { 139 u32 hash = skb_get_hash(skb); 140 141 return reciprocal_scale(hash, fq->flows_cnt); 142 } 143 144 static struct fq_flow *fq_flow_classify(struct fq *fq, 145 struct fq_tin *tin, u32 idx, 146 struct sk_buff *skb) 147 { 148 struct fq_flow *flow; 149 150 lockdep_assert_held(&fq->lock); 151 152 flow = &fq->flows[idx]; 153 if (flow->tin && flow->tin != tin) { 154 flow = &tin->default_flow; 155 tin->collisions++; 156 fq->collisions++; 157 } 158 159 if (!flow->tin) 160 tin->flows++; 161 162 return flow; 163 } 164 165 static struct fq_flow *fq_find_fattest_flow(struct fq *fq) 166 { 167 struct fq_tin *tin; 168 struct fq_flow *flow = NULL; 169 u32 len = 0; 170 int i; 171 172 for_each_set_bit(i, fq->flows_bitmap, fq->flows_cnt) { 173 struct fq_flow *cur = &fq->flows[i]; 174 unsigned int cur_len; 175 176 cur_len = cur->backlog; 177 if (cur_len <= len) 178 continue; 179 180 flow = cur; 181 len = cur_len; 182 } 183 184 list_for_each_entry(tin, &fq->tin_backlog, tin_list) { 185 unsigned int cur_len = tin->default_flow.backlog; 186 187 if (cur_len <= len) 188 continue; 189 190 flow = &tin->default_flow; 191 len = cur_len; 192 } 193 194 return flow; 195 } 196 197 static void fq_tin_enqueue(struct fq *fq, 198 struct fq_tin *tin, u32 idx, 199 struct sk_buff *skb, 200 fq_skb_free_t free_func) 201 { 202 struct fq_flow *flow; 203 bool oom; 204 205 lockdep_assert_held(&fq->lock); 206 207 flow = fq_flow_classify(fq, tin, idx, skb); 208 209 if (!flow->backlog) { 210 if (flow != &tin->default_flow) 211 __set_bit(idx, fq->flows_bitmap); 212 else if (list_empty(&tin->tin_list)) 213 list_add(&tin->tin_list, &fq->tin_backlog); 214 } 215 216 flow->tin = tin; 217 flow->backlog += skb->len; 218 tin->backlog_bytes += skb->len; 219 tin->backlog_packets++; 220 fq->memory_usage += skb->truesize; 221 fq->backlog++; 222 223 if (list_empty(&flow->flowchain)) { 224 flow->deficit = fq->quantum; 225 list_add_tail(&flow->flowchain, 226 &tin->new_flows); 227 } 228 229 __skb_queue_tail(&flow->queue, skb); 230 oom = (fq->memory_usage > fq->memory_limit); 231 while (fq->backlog > fq->limit || oom) { 232 flow = fq_find_fattest_flow(fq); 233 if (!flow) 234 return; 235 236 if (!fq_flow_drop(fq, flow, free_func)) 237 return; 238 239 flow->tin->overlimit++; 240 fq->overlimit++; 241 if (oom) { 242 fq->overmemory++; 243 oom = (fq->memory_usage > fq->memory_limit); 244 } 245 } 246 } 247 248 static void fq_flow_filter(struct fq *fq, 249 struct fq_flow *flow, 250 fq_skb_filter_t filter_func, 251 void *filter_data, 252 fq_skb_free_t free_func) 253 { 254 struct fq_tin *tin = flow->tin; 255 struct sk_buff *skb, *tmp; 256 257 lockdep_assert_held(&fq->lock); 258 259 skb_queue_walk_safe(&flow->queue, skb, tmp) { 260 if (!filter_func(fq, tin, flow, skb, filter_data)) 261 continue; 262 263 __skb_unlink(skb, &flow->queue); 264 fq_adjust_removal(fq, flow, skb); 265 free_func(fq, tin, flow, skb); 266 } 267 } 268 269 static void fq_tin_filter(struct fq *fq, 270 struct fq_tin *tin, 271 fq_skb_filter_t filter_func, 272 void *filter_data, 273 fq_skb_free_t free_func) 274 { 275 struct fq_flow *flow; 276 277 lockdep_assert_held(&fq->lock); 278 279 list_for_each_entry(flow, &tin->new_flows, flowchain) 280 fq_flow_filter(fq, flow, filter_func, filter_data, free_func); 281 list_for_each_entry(flow, &tin->old_flows, flowchain) 282 fq_flow_filter(fq, flow, filter_func, filter_data, free_func); 283 } 284 285 static void fq_flow_reset(struct fq *fq, 286 struct fq_flow *flow, 287 fq_skb_free_t free_func) 288 { 289 struct fq_tin *tin = flow->tin; 290 struct sk_buff *skb; 291 292 while ((skb = fq_flow_dequeue(fq, flow))) 293 free_func(fq, tin, flow, skb); 294 295 if (!list_empty(&flow->flowchain)) { 296 list_del_init(&flow->flowchain); 297 if (list_empty(&tin->new_flows) && 298 list_empty(&tin->old_flows)) 299 list_del_init(&tin->tin_list); 300 } 301 302 flow->tin = NULL; 303 304 WARN_ON_ONCE(flow->backlog); 305 } 306 307 static void fq_tin_reset(struct fq *fq, 308 struct fq_tin *tin, 309 fq_skb_free_t free_func) 310 { 311 struct list_head *head; 312 struct fq_flow *flow; 313 314 for (;;) { 315 head = &tin->new_flows; 316 if (list_empty(head)) { 317 head = &tin->old_flows; 318 if (list_empty(head)) 319 break; 320 } 321 322 flow = list_first_entry(head, struct fq_flow, flowchain); 323 fq_flow_reset(fq, flow, free_func); 324 } 325 326 WARN_ON_ONCE(!list_empty(&tin->tin_list)); 327 WARN_ON_ONCE(tin->backlog_bytes); 328 WARN_ON_ONCE(tin->backlog_packets); 329 } 330 331 static void fq_flow_init(struct fq_flow *flow) 332 { 333 INIT_LIST_HEAD(&flow->flowchain); 334 __skb_queue_head_init(&flow->queue); 335 } 336 337 static void fq_tin_init(struct fq_tin *tin) 338 { 339 INIT_LIST_HEAD(&tin->new_flows); 340 INIT_LIST_HEAD(&tin->old_flows); 341 INIT_LIST_HEAD(&tin->tin_list); 342 fq_flow_init(&tin->default_flow); 343 } 344 345 static int fq_init(struct fq *fq, int flows_cnt) 346 { 347 int i; 348 349 memset(fq, 0, sizeof(fq[0])); 350 spin_lock_init(&fq->lock); 351 INIT_LIST_HEAD(&fq->tin_backlog); 352 fq->flows_cnt = max_t(u32, flows_cnt, 1); 353 fq->quantum = 300; 354 fq->limit = 8192; 355 fq->memory_limit = 16 << 20; /* 16 MBytes */ 356 357 fq->flows = kvcalloc(fq->flows_cnt, sizeof(fq->flows[0]), GFP_KERNEL); 358 if (!fq->flows) 359 return -ENOMEM; 360 361 fq->flows_bitmap = bitmap_zalloc(fq->flows_cnt, GFP_KERNEL); 362 if (!fq->flows_bitmap) { 363 kvfree(fq->flows); 364 fq->flows = NULL; 365 return -ENOMEM; 366 } 367 368 for (i = 0; i < fq->flows_cnt; i++) 369 fq_flow_init(&fq->flows[i]); 370 371 return 0; 372 } 373 374 static void fq_reset(struct fq *fq, 375 fq_skb_free_t free_func) 376 { 377 int i; 378 379 for (i = 0; i < fq->flows_cnt; i++) 380 fq_flow_reset(fq, &fq->flows[i], free_func); 381 382 kvfree(fq->flows); 383 fq->flows = NULL; 384 385 bitmap_free(fq->flows_bitmap); 386 fq->flows_bitmap = NULL; 387 } 388 389 #endif 390