sys/cam/cam_queue.c

   1 /*-
   2  * CAM request queue management functions.
   3  *
   4  * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
   5  *
   6  * Copyright (c) 1997 Justin T. Gibbs.
   7  * All rights reserved.
   8  *
   9  * Redistribution and use in source and binary forms, with or without
  10  * modification, are permitted provided that the following conditions
  11  * are met:
  12  * 1. Redistributions of source code must retain the above copyright
  13  *    notice, this list of conditions, and the following disclaimer,
  14  *    without modification, immediately at the beginning of the file.
  15  * 2. The name of the author may not be used to endorse or promote products
  16  *    derived from this software without specific prior written permission.
  17  *
  18  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  19  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  20  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  21  * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE FOR
  22  * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  23  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  24  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  25  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  26  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  27  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  28  * SUCH DAMAGE.
  29  */
  30
  31 #include <sys/cdefs.h>
  32 __FBSDID("$FreeBSD$");
  33
  34 #include <sys/param.h>
  35 #include <sys/systm.h>
  36 #include <sys/types.h>
  37 #include <sys/malloc.h>
  38 #include <sys/kernel.h>
  39
  40 #include <cam/cam.h>
  41 #include <cam/cam_ccb.h>
  42 #include <cam/cam_queue.h>
  43 #include <cam/cam_debug.h>
  44
  45 static MALLOC_DEFINE(M_CAMQ, "CAM queue", "CAM queue buffers");
  46 static MALLOC_DEFINE(M_CAMDEVQ, "CAM dev queue", "CAM dev queue buffers");
  47 static MALLOC_DEFINE(M_CAMCCBQ, "CAM ccb queue", "CAM ccb queue buffers");
  48
  49 static __inline int
  50                 queue_cmp(cam_pinfo **queue_array, int i, int j);
  51 static __inline void
  52                 swap(cam_pinfo **queue_array, int i, int j);
  53 static void     heap_up(cam_pinfo **queue_array, int new_index);
  54 static void     heap_down(cam_pinfo **queue_array, int index,
  55                           int last_index);
  56
  57 int
  58 camq_init(struct camq *camq, int size)
  59 {
  60         bzero(camq, sizeof(*camq));
  61         camq->array_size = size;
  62         if (camq->array_size != 0) {
  63                 /*
  64                  * Heap algorithms like everything numbered from 1, so
  65                  * allocate one more to account for 0 base.
  66                  */
  67                 camq->queue_array = malloc((size + 1) * sizeof(cam_pinfo*),
  68                     M_CAMQ, M_NOWAIT);
  69                 if (camq->queue_array == NULL) {
  70                         printf("camq_init: - cannot malloc array!\n");
  71                         return (1);
  72                 }
  73         }
  74         return (0);
  75 }
  76
  77 /*
  78  * Free a camq structure.  This should only be called if a controller
  79  * driver failes somehow during its attach routine or is unloaded and has
  80  * obtained a camq structure.  The XPT should ensure that the queue
  81  * is empty before calling this routine.
  82  */
  83 void
  84 camq_fini(struct camq *queue)
  85 {
  86         if (queue->queue_array != NULL) {
  87                 free(queue->queue_array, M_CAMQ);
  88         }
  89 }
  90
  91 u_int32_t
  92 camq_resize(struct camq *queue, int new_size)
  93 {
  94         cam_pinfo **new_array;
  95
  96         KASSERT(new_size >= queue->entries, ("camq_resize: "
  97             "New queue size can't accommodate queued entries (%d < %d).",
  98             new_size, queue->entries));
  99         new_array = malloc((new_size + 1) * sizeof(cam_pinfo *), M_CAMQ,
 100             M_NOWAIT);
 101         if (new_array == NULL) {
 102                 /* Couldn't satisfy request */
 103                 return (CAM_RESRC_UNAVAIL);
 104         }
 105         /*
 106          * Heap algorithms like everything numbered from 1, so
 107          * remember that our pointer into the heap array is offset
 108          * by one element.
 109          */
 110         if (queue->queue_array != NULL) {
 111                 bcopy(queue->queue_array, new_array,
 112                     (queue->entries + 1) * sizeof(cam_pinfo *));
 113                 free(queue->queue_array, M_CAMQ);
 114         }
 115         queue->queue_array = new_array;
 116         queue->array_size = new_size;
 117         return (CAM_REQ_CMP);
 118 }
 119
 120 /*
 121  * camq_insert: Given an array of cam_pinfo* elememnts with
 122  * the Heap(1, num_elements) property and array_size - num_elements >= 1,
 123  * output Heap(1, num_elements+1) including new_entry in the array.
 124  */
 125 void
 126 camq_insert(struct camq *queue, cam_pinfo *new_entry)
 127 {
 128
 129         KASSERT(queue->entries < queue->array_size,
 130             ("camq_insert: Attempt to insert into a full queue (%d >= %d)",
 131             queue->entries, queue->array_size));
 132         queue->entries++;
 133         queue->queue_array[queue->entries] = new_entry;
 134         new_entry->index = queue->entries;
 135         if (queue->entries != 0)
 136                 heap_up(queue->queue_array, queue->entries);
 137 }
 138
 139 /*
 140  * camq_remove:  Given an array of cam_pinfo* elevements with the
 141  * Heap(1, num_elements) property and an index such that 1 <= index <=
 142  * num_elements, remove that entry and restore the Heap(1, num_elements-1)
 143  * property.
 144  */
 145 cam_pinfo *
 146 camq_remove(struct camq *queue, int index)
 147 {
 148         cam_pinfo *removed_entry;
 149
 150         if (index <= 0 || index > queue->entries)
 151                 panic("%s: Attempt to remove out-of-bounds index %d "
 152                     "from queue %p of size %d", __func__, index, queue,
 153                     queue->entries);
 154
 155         removed_entry = queue->queue_array[index];
 156         if (queue->entries != index) {
 157                 queue->queue_array[index] = queue->queue_array[queue->entries];
 158                 queue->queue_array[index]->index = index;
 159                 heap_down(queue->queue_array, index, queue->entries - 1);
 160         }
 161         removed_entry->index = CAM_UNQUEUED_INDEX;
 162         queue->entries--;
 163         return (removed_entry);
 164 }
 165
 166 /*
 167  * camq_change_priority:  Given an array of cam_pinfo* elements with the
 168  * Heap(1, num_entries) property, an index such that 1 <= index <= num_elements,
 169  * and a new priority for the element at index, change the priority of
 170  * element index and restore the Heap(0, num_elements) property.
 171  */
 172 void
 173 camq_change_priority(struct camq *queue, int index, u_int32_t new_priority)
 174 {
 175         if (new_priority > queue->queue_array[index]->priority) {
 176                 queue->queue_array[index]->priority = new_priority;
 177                 heap_down(queue->queue_array, index, queue->entries);
 178         } else {
 179                 /* new_priority <= old_priority */
 180                 queue->queue_array[index]->priority = new_priority;
 181                 heap_up(queue->queue_array, index);
 182         }
 183 }
 184
 185 struct cam_devq *
 186 cam_devq_alloc(int devices, int openings)
 187 {
 188         struct cam_devq *devq;
 189
 190         devq = (struct cam_devq *)malloc(sizeof(*devq), M_CAMDEVQ, M_NOWAIT);
 191         if (devq == NULL) {
 192                 printf("cam_devq_alloc: - cannot malloc!\n");
 193                 return (NULL);
 194         }
 195         if (cam_devq_init(devq, devices, openings) != 0) {
 196                 free(devq, M_CAMDEVQ);
 197                 return (NULL);
 198         }
 199         return (devq);
 200 }
 201
 202 int
 203 cam_devq_init(struct cam_devq *devq, int devices, int openings)
 204 {
 205
 206         bzero(devq, sizeof(*devq));
 207         mtx_init(&devq->send_mtx, "CAM queue lock", NULL, MTX_DEF);
 208         if (camq_init(&devq->send_queue, devices) != 0)
 209                 return (1);
 210         devq->send_openings = openings;
 211         devq->send_active = 0;
 212         return (0);
 213 }
 214
 215 void
 216 cam_devq_free(struct cam_devq *devq)
 217 {
 218
 219         camq_fini(&devq->send_queue);
 220         mtx_destroy(&devq->send_mtx);
 221         free(devq, M_CAMDEVQ);
 222 }
 223
 224 u_int32_t
 225 cam_devq_resize(struct cam_devq *camq, int devices)
 226 {
 227         u_int32_t retval;
 228
 229         retval = camq_resize(&camq->send_queue, devices);
 230         return (retval);
 231 }
 232
 233 struct cam_ccbq *
 234 cam_ccbq_alloc(int openings)
 235 {
 236         struct cam_ccbq *ccbq;
 237
 238         ccbq = (struct cam_ccbq *)malloc(sizeof(*ccbq), M_CAMCCBQ, M_NOWAIT);
 239         if (ccbq == NULL) {
 240                 printf("cam_ccbq_alloc: - cannot malloc!\n");
 241                 return (NULL);
 242         }
 243         if (cam_ccbq_init(ccbq, openings) != 0) {
 244                 free(ccbq, M_CAMCCBQ);
 245                 return (NULL);
 246         }
 247
 248         return (ccbq);
 249 }
 250
 251 void
 252 cam_ccbq_free(struct cam_ccbq *ccbq)
 253 {
 254         if (ccbq) {
 255                 cam_ccbq_fini(ccbq);
 256                 free(ccbq, M_CAMCCBQ);
 257         }
 258 }
 259
 260 u_int32_t
 261 cam_ccbq_resize(struct cam_ccbq *ccbq, int new_size)
 262 {
 263         int delta;
 264
 265         delta = new_size - (ccbq->dev_active + ccbq->dev_openings);
 266         ccbq->total_openings += delta;
 267         ccbq->dev_openings += delta;
 268
 269         new_size = imax(64, 1 << fls(new_size + new_size / 2));
 270         if (new_size > ccbq->queue.array_size)
 271                 return (camq_resize(&ccbq->queue, new_size));
 272         else
 273                 return (CAM_REQ_CMP);
 274 }
 275
 276 int
 277 cam_ccbq_init(struct cam_ccbq *ccbq, int openings)
 278 {
 279         bzero(ccbq, sizeof(*ccbq));
 280         if (camq_init(&ccbq->queue,
 281             imax(64, 1 << fls(openings + openings / 2))) != 0)
 282                 return (1);
 283         ccbq->total_openings = openings;
 284         ccbq->dev_openings = openings;
 285         return (0);
 286 }
 287
 288 void
 289 cam_ccbq_fini(struct cam_ccbq *ccbq)
 290 {
 291
 292         camq_fini(&ccbq->queue);
 293 }
 294
 295 /*
 296  * Heap routines for manipulating CAM queues.
 297  */
 298 /*
 299  * queue_cmp: Given an array of cam_pinfo* elements and indexes i
 300  * and j, return less than 0, 0, or greater than 0 if i is less than,
 301  * equal too, or greater than j respectively.
 302  */
 303 static __inline int
 304 queue_cmp(cam_pinfo **queue_array, int i, int j)
 305 {
 306         if (queue_array[i]->priority == queue_array[j]->priority)
 307                 return (  queue_array[i]->generation
 308                         - queue_array[j]->generation );
 309         else
 310                 return (  queue_array[i]->priority
 311                         - queue_array[j]->priority );
 312 }
 313
 314 /*
 315  * swap: Given an array of cam_pinfo* elements and indexes i and j,
 316  * exchange elements i and j.
 317  */
 318 static __inline void
 319 swap(cam_pinfo **queue_array, int i, int j)
 320 {
 321         cam_pinfo *temp_qentry;
 322
 323         temp_qentry = queue_array[j];
 324         queue_array[j] = queue_array[i];
 325         queue_array[i] = temp_qentry;
 326         queue_array[j]->index = j;
 327         queue_array[i]->index = i;
 328 }
 329
 330 /*
 331  * heap_up:  Given an array of cam_pinfo* elements with the
 332  * Heap(1, new_index-1) property and a new element in location
 333  * new_index, output Heap(1, new_index).
 334  */
 335 static void
 336 heap_up(cam_pinfo **queue_array, int new_index)
 337 {
 338         int child;
 339         int parent;
 340
 341         child = new_index;
 342
 343         while (child != 1) {
 344                 parent = child >> 1;
 345                 if (queue_cmp(queue_array, parent, child) <= 0)
 346                         break;
 347                 swap(queue_array, parent, child);
 348                 child = parent;
 349         }
 350 }
 351
 352 /*
 353  * heap_down:  Given an array of cam_pinfo* elements with the
 354  * Heap(index + 1, num_entries) property with index containing
 355  * an unsorted entry, output Heap(index, num_entries).
 356  */
 357 static void
 358 heap_down(cam_pinfo **queue_array, int index, int num_entries)
 359 {
 360         int child;
 361         int parent;
 362
 363         parent = index;
 364         child = parent << 1;
 365         for (; child <= num_entries; child = parent << 1) {
 366                 if (child < num_entries) {
 367                         /* child+1 is the right child of parent */
 368                         if (queue_cmp(queue_array, child + 1, child) < 0)
 369                                 child++;
 370                 }
 371                 /* child is now the least child of parent */
 372                 if (queue_cmp(queue_array, parent, child) <= 0)
 373                         break;
 374                 swap(queue_array, child, parent);
 375                 parent = child;
 376         }
 377 }