drivers/uwb/allocator.c

   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 <linux/uwb.h>
  10
  11 #include "uwb-internal.h"
  12
  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
  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  */
 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
 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
 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
 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
 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
 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
 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 }