1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3 * UWB reservation management.
4 *
5 * Copyright (C) 2008 Cambridge Silicon Radio Ltd.
6 */
7 #include <linux/kernel.h>
8 #include <linux/slab.h>
9 #include "uwb.h"
10
11 #include "uwb-internal.h"
12
uwb_rsv_fill_column_alloc(struct uwb_rsv_alloc_info * ai)13 static void uwb_rsv_fill_column_alloc(struct uwb_rsv_alloc_info *ai)
14 {
15 int col, mas, safe_mas, unsafe_mas;
16 unsigned char *bm = ai->bm;
17 struct uwb_rsv_col_info *ci = ai->ci;
18 unsigned char c;
19
20 for (col = ci->csi.start_col; col < UWB_NUM_ZONES; col += ci->csi.interval) {
21
22 safe_mas = ci->csi.safe_mas_per_col;
23 unsafe_mas = ci->csi.unsafe_mas_per_col;
24
25 for (mas = 0; mas < UWB_MAS_PER_ZONE; mas++ ) {
26 if (bm[col * UWB_MAS_PER_ZONE + mas] == 0) {
27
28 if (safe_mas > 0) {
29 safe_mas--;
30 c = UWB_RSV_MAS_SAFE;
31 } else if (unsafe_mas > 0) {
32 unsafe_mas--;
33 c = UWB_RSV_MAS_UNSAFE;
34 } else {
35 break;
36 }
37 bm[col * UWB_MAS_PER_ZONE + mas] = c;
38 }
39 }
40 }
41 }
42
uwb_rsv_fill_row_alloc(struct uwb_rsv_alloc_info * ai)43 static void uwb_rsv_fill_row_alloc(struct uwb_rsv_alloc_info *ai)
44 {
45 int mas, col, rows;
46 unsigned char *bm = ai->bm;
47 struct uwb_rsv_row_info *ri = &ai->ri;
48 unsigned char c;
49
50 rows = 1;
51 c = UWB_RSV_MAS_SAFE;
52 for (mas = UWB_MAS_PER_ZONE - 1; mas >= 0; mas--) {
53 if (ri->avail[mas] == 1) {
54
55 if (rows > ri->used_rows) {
56 break;
57 } else if (rows > 7) {
58 c = UWB_RSV_MAS_UNSAFE;
59 }
60
61 for (col = 0; col < UWB_NUM_ZONES; col++) {
62 if (bm[col * UWB_NUM_ZONES + mas] != UWB_RSV_MAS_NOT_AVAIL) {
63 bm[col * UWB_NUM_ZONES + mas] = c;
64 if(c == UWB_RSV_MAS_SAFE)
65 ai->safe_allocated_mases++;
66 else
67 ai->unsafe_allocated_mases++;
68 }
69 }
70 rows++;
71 }
72 }
73 ai->total_allocated_mases = ai->safe_allocated_mases + ai->unsafe_allocated_mases;
74 }
75
76 /*
77 * Find the best column set for a given availability, interval, num safe mas and
78 * num unsafe mas.
79 *
80 * The different sets are tried in order as shown below, depending on the interval.
81 *
82 * interval = 16
83 * deep = 0
84 * set 1 -> { 8 }
85 * deep = 1
86 * set 1 -> { 4 }
87 * set 2 -> { 12 }
88 * deep = 2
89 * set 1 -> { 2 }
90 * set 2 -> { 6 }
91 * set 3 -> { 10 }
92 * set 4 -> { 14 }
93 * deep = 3
94 * set 1 -> { 1 }
95 * set 2 -> { 3 }
96 * set 3 -> { 5 }
97 * set 4 -> { 7 }
98 * set 5 -> { 9 }
99 * set 6 -> { 11 }
100 * set 7 -> { 13 }
101 * set 8 -> { 15 }
102 *
103 * interval = 8
104 * deep = 0
105 * set 1 -> { 4 12 }
106 * deep = 1
107 * set 1 -> { 2 10 }
108 * set 2 -> { 6 14 }
109 * deep = 2
110 * set 1 -> { 1 9 }
111 * set 2 -> { 3 11 }
112 * set 3 -> { 5 13 }
113 * set 4 -> { 7 15 }
114 *
115 * interval = 4
116 * deep = 0
117 * set 1 -> { 2 6 10 14 }
118 * deep = 1
119 * set 1 -> { 1 5 9 13 }
120 * set 2 -> { 3 7 11 15 }
121 *
122 * interval = 2
123 * deep = 0
124 * set 1 -> { 1 3 5 7 9 11 13 15 }
125 */
uwb_rsv_find_best_column_set(struct uwb_rsv_alloc_info * ai,int interval,int num_safe_mas,int num_unsafe_mas)126 static int uwb_rsv_find_best_column_set(struct uwb_rsv_alloc_info *ai, int interval,
127 int num_safe_mas, int num_unsafe_mas)
128 {
129 struct uwb_rsv_col_info *ci = ai->ci;
130 struct uwb_rsv_col_set_info *csi = &ci->csi;
131 struct uwb_rsv_col_set_info tmp_csi;
132 int deep, set, col, start_col_deep, col_start_set;
133 int start_col, max_mas_in_set, lowest_max_mas_in_deep;
134 int n_mas;
135 int found = UWB_RSV_ALLOC_NOT_FOUND;
136
137 tmp_csi.start_col = 0;
138 start_col_deep = interval;
139 n_mas = num_unsafe_mas + num_safe_mas;
140
141 for (deep = 0; ((interval >> deep) & 0x1) == 0; deep++) {
142 start_col_deep /= 2;
143 col_start_set = 0;
144 lowest_max_mas_in_deep = UWB_MAS_PER_ZONE;
145
146 for (set = 1; set <= (1 << deep); set++) {
147 max_mas_in_set = 0;
148 start_col = start_col_deep + col_start_set;
149 for (col = start_col; col < UWB_NUM_ZONES; col += interval) {
150
151 if (ci[col].max_avail_safe >= num_safe_mas &&
152 ci[col].max_avail_unsafe >= n_mas) {
153 if (ci[col].highest_mas[n_mas] > max_mas_in_set)
154 max_mas_in_set = ci[col].highest_mas[n_mas];
155 } else {
156 max_mas_in_set = 0;
157 break;
158 }
159 }
160 if ((lowest_max_mas_in_deep > max_mas_in_set) && max_mas_in_set) {
161 lowest_max_mas_in_deep = max_mas_in_set;
162
163 tmp_csi.start_col = start_col;
164 }
165 col_start_set += (interval >> deep);
166 }
167
168 if (lowest_max_mas_in_deep < 8) {
169 csi->start_col = tmp_csi.start_col;
170 found = UWB_RSV_ALLOC_FOUND;
171 break;
172 } else if ((lowest_max_mas_in_deep > 8) &&
173 (lowest_max_mas_in_deep != UWB_MAS_PER_ZONE) &&
174 (found == UWB_RSV_ALLOC_NOT_FOUND)) {
175 csi->start_col = tmp_csi.start_col;
176 found = UWB_RSV_ALLOC_FOUND;
177 }
178 }
179
180 if (found == UWB_RSV_ALLOC_FOUND) {
181 csi->interval = interval;
182 csi->safe_mas_per_col = num_safe_mas;
183 csi->unsafe_mas_per_col = num_unsafe_mas;
184
185 ai->safe_allocated_mases = (UWB_NUM_ZONES / interval) * num_safe_mas;
186 ai->unsafe_allocated_mases = (UWB_NUM_ZONES / interval) * num_unsafe_mas;
187 ai->total_allocated_mases = ai->safe_allocated_mases + ai->unsafe_allocated_mases;
188 ai->interval = interval;
189 }
190 return found;
191 }
192
get_row_descriptors(struct uwb_rsv_alloc_info * ai)193 static void get_row_descriptors(struct uwb_rsv_alloc_info *ai)
194 {
195 unsigned char *bm = ai->bm;
196 struct uwb_rsv_row_info *ri = &ai->ri;
197 int col, mas;
198
199 ri->free_rows = 16;
200 for (mas = 0; mas < UWB_MAS_PER_ZONE; mas ++) {
201 ri->avail[mas] = 1;
202 for (col = 1; col < UWB_NUM_ZONES; col++) {
203 if (bm[col * UWB_NUM_ZONES + mas] == UWB_RSV_MAS_NOT_AVAIL) {
204 ri->free_rows--;
205 ri->avail[mas]=0;
206 break;
207 }
208 }
209 }
210 }
211
uwb_rsv_fill_column_info(unsigned char * bm,int column,struct uwb_rsv_col_info * rci)212 static void uwb_rsv_fill_column_info(unsigned char *bm, int column, struct uwb_rsv_col_info *rci)
213 {
214 int mas;
215 int block_count = 0, start_block = 0;
216 int previous_avail = 0;
217 int available = 0;
218 int safe_mas_in_row[UWB_MAS_PER_ZONE] = {
219 8, 7, 6, 5, 4, 4, 4, 4, 4, 4, 4, 4, 4, 3, 2, 1,
220 };
221
222 rci->max_avail_safe = 0;
223
224 for (mas = 0; mas < UWB_MAS_PER_ZONE; mas ++) {
225 if (!bm[column * UWB_NUM_ZONES + mas]) {
226 available++;
227 rci->max_avail_unsafe = available;
228
229 rci->highest_mas[available] = mas;
230
231 if (previous_avail) {
232 block_count++;
233 if ((block_count > safe_mas_in_row[start_block]) &&
234 (!rci->max_avail_safe))
235 rci->max_avail_safe = available - 1;
236 } else {
237 previous_avail = 1;
238 start_block = mas;
239 block_count = 1;
240 }
241 } else {
242 previous_avail = 0;
243 }
244 }
245 if (!rci->max_avail_safe)
246 rci->max_avail_safe = rci->max_avail_unsafe;
247 }
248
get_column_descriptors(struct uwb_rsv_alloc_info * ai)249 static void get_column_descriptors(struct uwb_rsv_alloc_info *ai)
250 {
251 unsigned char *bm = ai->bm;
252 struct uwb_rsv_col_info *ci = ai->ci;
253 int col;
254
255 for (col = 1; col < UWB_NUM_ZONES; col++) {
256 uwb_rsv_fill_column_info(bm, col, &ci[col]);
257 }
258 }
259
uwb_rsv_find_best_row_alloc(struct uwb_rsv_alloc_info * ai)260 static int uwb_rsv_find_best_row_alloc(struct uwb_rsv_alloc_info *ai)
261 {
262 int n_rows;
263 int max_rows = ai->max_mas / UWB_USABLE_MAS_PER_ROW;
264 int min_rows = ai->min_mas / UWB_USABLE_MAS_PER_ROW;
265 if (ai->min_mas % UWB_USABLE_MAS_PER_ROW)
266 min_rows++;
267 for (n_rows = max_rows; n_rows >= min_rows; n_rows--) {
268 if (n_rows <= ai->ri.free_rows) {
269 ai->ri.used_rows = n_rows;
270 ai->interval = 1; /* row reservation */
271 uwb_rsv_fill_row_alloc(ai);
272 return UWB_RSV_ALLOC_FOUND;
273 }
274 }
275 return UWB_RSV_ALLOC_NOT_FOUND;
276 }
277
uwb_rsv_find_best_col_alloc(struct uwb_rsv_alloc_info * ai,int interval)278 static int uwb_rsv_find_best_col_alloc(struct uwb_rsv_alloc_info *ai, int interval)
279 {
280 int n_safe, n_unsafe, n_mas;
281 int n_column = UWB_NUM_ZONES / interval;
282 int max_per_zone = ai->max_mas / n_column;
283 int min_per_zone = ai->min_mas / n_column;
284
285 if (ai->min_mas % n_column)
286 min_per_zone++;
287
288 if (min_per_zone > UWB_MAS_PER_ZONE) {
289 return UWB_RSV_ALLOC_NOT_FOUND;
290 }
291
292 if (max_per_zone > UWB_MAS_PER_ZONE) {
293 max_per_zone = UWB_MAS_PER_ZONE;
294 }
295
296 for (n_mas = max_per_zone; n_mas >= min_per_zone; n_mas--) {
297 if (uwb_rsv_find_best_column_set(ai, interval, 0, n_mas) == UWB_RSV_ALLOC_NOT_FOUND)
298 continue;
299 for (n_safe = n_mas; n_safe >= 0; n_safe--) {
300 n_unsafe = n_mas - n_safe;
301 if (uwb_rsv_find_best_column_set(ai, interval, n_safe, n_unsafe) == UWB_RSV_ALLOC_FOUND) {
302 uwb_rsv_fill_column_alloc(ai);
303 return UWB_RSV_ALLOC_FOUND;
304 }
305 }
306 }
307 return UWB_RSV_ALLOC_NOT_FOUND;
308 }
309
uwb_rsv_find_best_allocation(struct uwb_rsv * rsv,struct uwb_mas_bm * available,struct uwb_mas_bm * result)310 int uwb_rsv_find_best_allocation(struct uwb_rsv *rsv, struct uwb_mas_bm *available,
311 struct uwb_mas_bm *result)
312 {
313 struct uwb_rsv_alloc_info *ai;
314 int interval;
315 int bit_index;
316
317 ai = kzalloc(sizeof(struct uwb_rsv_alloc_info), GFP_KERNEL);
318 if (!ai)
319 return UWB_RSV_ALLOC_NOT_FOUND;
320 ai->min_mas = rsv->min_mas;
321 ai->max_mas = rsv->max_mas;
322 ai->max_interval = rsv->max_interval;
323
324
325 /* fill the not available vector from the available bm */
326 for_each_clear_bit(bit_index, available->bm, UWB_NUM_MAS)
327 ai->bm[bit_index] = UWB_RSV_MAS_NOT_AVAIL;
328
329 if (ai->max_interval == 1) {
330 get_row_descriptors(ai);
331 if (uwb_rsv_find_best_row_alloc(ai) == UWB_RSV_ALLOC_FOUND)
332 goto alloc_found;
333 else
334 goto alloc_not_found;
335 }
336
337 get_column_descriptors(ai);
338
339 for (interval = 16; interval >= 2; interval>>=1) {
340 if (interval > ai->max_interval)
341 continue;
342 if (uwb_rsv_find_best_col_alloc(ai, interval) == UWB_RSV_ALLOC_FOUND)
343 goto alloc_found;
344 }
345
346 /* try row reservation if no column is found */
347 get_row_descriptors(ai);
348 if (uwb_rsv_find_best_row_alloc(ai) == UWB_RSV_ALLOC_FOUND)
349 goto alloc_found;
350 else
351 goto alloc_not_found;
352
353 alloc_found:
354 bitmap_zero(result->bm, UWB_NUM_MAS);
355 bitmap_zero(result->unsafe_bm, UWB_NUM_MAS);
356 /* fill the safe and unsafe bitmaps */
357 for (bit_index = 0; bit_index < UWB_NUM_MAS; bit_index++) {
358 if (ai->bm[bit_index] == UWB_RSV_MAS_SAFE)
359 set_bit(bit_index, result->bm);
360 else if (ai->bm[bit_index] == UWB_RSV_MAS_UNSAFE)
361 set_bit(bit_index, result->unsafe_bm);
362 }
363 bitmap_or(result->bm, result->bm, result->unsafe_bm, UWB_NUM_MAS);
364
365 result->safe = ai->safe_allocated_mases;
366 result->unsafe = ai->unsafe_allocated_mases;
367
368 kfree(ai);
369 return UWB_RSV_ALLOC_FOUND;
370
371 alloc_not_found:
372 kfree(ai);
373 return UWB_RSV_ALLOC_NOT_FOUND;
374 }
375