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 void     heap_up(cam_pinfo **queue_array, int new_index);
  50 static void     heap_down(cam_pinfo **queue_array, int index,
  51                           int last_index);
  52
  53 int
  54 camq_init(struct camq *camq, int size)
  55 {
  56         bzero(camq, sizeof(*camq));
  57         camq->array_size = size;
  58         if (camq->array_size != 0) {
  59                 /*
  60                  * Heap algorithms like everything numbered from 1, so
  61                  * allocate one more to account for 0 base.
  62                  */
  63                 camq->queue_array = malloc((size + 1) * sizeof(cam_pinfo*),
  64                     M_CAMQ, M_NOWAIT);
  65                 if (camq->queue_array == NULL) {
  66                         printf("camq_init: - cannot malloc array!\n");
  67                         return (1);
  68                 }
  69         }
  70         return (0);
  71 }
  72
  73 /*
  74  * Free a camq structure.  This should only be called if a controller
  75  * driver fails somehow during its attach routine or is unloaded and has
  76  * obtained a camq structure.  The XPT should ensure that the queue
  77  * is empty before calling this routine.
  78  */
  79 void
  80 camq_fini(struct camq *queue)
  81 {
  82         if (queue->queue_array != NULL) {
  83                 free(queue->queue_array, M_CAMQ);
  84         }
  85 }
  86
  87 u_int32_t
  88 camq_resize(struct camq *queue, int new_size)
  89 {
  90         cam_pinfo **new_array;
  91
  92         KASSERT(new_size >= queue->entries, ("camq_resize: "
  93             "New queue size can't accommodate queued entries (%d < %d).",
  94             new_size, queue->entries));
  95         new_array = malloc((new_size + 1) * sizeof(cam_pinfo *), M_CAMQ,
  96             M_NOWAIT);
  97         if (new_array == NULL) {
  98                 /* Couldn't satisfy request */
  99                 return (CAM_RESRC_UNAVAIL);
 100         }
 101         /*
 102          * Heap algorithms like everything numbered from 1, so
 103          * remember that our pointer into the heap array is offset
 104          * by one element.
 105          */
 106         if (queue->queue_array != NULL) {
 107                 bcopy(queue->queue_array, new_array,
 108                     (queue->entries + 1) * sizeof(cam_pinfo *));
 109                 free(queue->queue_array, M_CAMQ);
 110         }
 111         queue->queue_array = new_array;
 112         queue->array_size = new_size;
 113         return (CAM_REQ_CMP);
 114 }
 115
 116 /*
 117  * camq_insert: Given an array of cam_pinfo* elememnts with
 118  * the Heap(1, num_elements) property and array_size - num_elements >= 1,
 119  * output Heap(1, num_elements+1) including new_entry in the array.
 120  */
 121 void
 122 camq_insert(struct camq *queue, cam_pinfo *new_entry)
 123 {
 124
 125         KASSERT(queue->entries < queue->array_size,
 126             ("camq_insert: Attempt to insert into a full queue (%d >= %d)",
 127             queue->entries, queue->array_size));
 128         queue->entries++;
 129         queue->queue_array[queue->entries] = new_entry;
 130         new_entry->index = queue->entries;
 131         if (queue->entries != 0)
 132                 heap_up(queue->queue_array, queue->entries);
 133 }
 134
 135 /*
 136  * camq_remove:  Given an array of cam_pinfo* elevements with the
 137  * Heap(1, num_elements) property and an index such that 1 <= index <=
 138  * num_elements, remove that entry and restore the Heap(1, num_elements-1)
 139  * property.
 140  */
 141 cam_pinfo *
 142 camq_remove(struct camq *queue, int index)
 143 {
 144         cam_pinfo *removed_entry;
 145
 146         if (index <= 0 || index > queue->entries)
 147                 panic("%s: Attempt to remove out-of-bounds index %d "
 148                     "from queue %p of size %d", __func__, index, queue,
 149                     queue->entries);
 150
 151         removed_entry = queue->queue_array[index];
 152         if (queue->entries != index) {
 153                 queue->queue_array[index] = queue->queue_array[queue->entries];
 154                 queue->queue_array[index]->index = index;
 155                 heap_down(queue->queue_array, index, queue->entries - 1);
 156         }
 157         removed_entry->index = CAM_UNQUEUED_INDEX;
 158         queue->entries--;
 159         return (removed_entry);
 160 }
 161
 162 /*
 163  * camq_change_priority:  Given an array of cam_pinfo* elements with the
 164  * Heap(1, num_entries) property, an index such that 1 <= index <= num_elements,
 165  * and a new priority for the element at index, change the priority of
 166  * element index and restore the Heap(0, num_elements) property.
 167  */
 168 void
 169 camq_change_priority(struct camq *queue, int index, u_int32_t new_priority)
 170 {
 171         if (new_priority > queue->queue_array[index]->priority) {
 172                 queue->queue_array[index]->priority = new_priority;
 173                 heap_down(queue->queue_array, index, queue->entries);
 174         } else {
 175                 /* new_priority <= old_priority */
 176                 queue->queue_array[index]->priority = new_priority;
 177                 heap_up(queue->queue_array, index);
 178         }
 179 }
 180
 181 struct cam_devq *
 182 cam_devq_alloc(int devices, int openings)
 183 {
 184         struct cam_devq *devq;
 185
 186         devq = (struct cam_devq *)malloc(sizeof(*devq), M_CAMDEVQ, M_NOWAIT);
 187         if (devq == NULL) {
 188                 printf("cam_devq_alloc: - cannot malloc!\n");
 189                 return (NULL);
 190         }
 191         if (cam_devq_init(devq, devices, openings) != 0) {
 192                 free(devq, M_CAMDEVQ);
 193                 return (NULL);
 194         }
 195         return (devq);
 196 }
 197
 198 int
 199 cam_devq_init(struct cam_devq *devq, int devices, int openings)
 200 {
 201
 202         bzero(devq, sizeof(*devq));
 203         mtx_init(&devq->send_mtx, "CAM queue lock", NULL, MTX_DEF);
 204         if (camq_init(&devq->send_queue, devices) != 0)
 205                 return (1);
 206         devq->send_openings = openings;
 207         devq->send_active = 0;
 208         return (0);
 209 }
 210
 211 void
 212 cam_devq_free(struct cam_devq *devq)
 213 {
 214
 215         camq_fini(&devq->send_queue);
 216         mtx_destroy(&devq->send_mtx);
 217         free(devq, M_CAMDEVQ);
 218 }
 219
 220 u_int32_t
 221 cam_devq_resize(struct cam_devq *camq, int devices)
 222 {
 223         u_int32_t retval;
 224
 225         retval = camq_resize(&camq->send_queue, devices);
 226         return (retval);
 227 }
 228
 229 struct cam_ccbq *
 230 cam_ccbq_alloc(int openings)
 231 {
 232         struct cam_ccbq *ccbq;
 233
 234         ccbq = (struct cam_ccbq *)malloc(sizeof(*ccbq), M_CAMCCBQ, M_NOWAIT);
 235         if (ccbq == NULL) {
 236                 printf("cam_ccbq_alloc: - cannot malloc!\n");
 237                 return (NULL);
 238         }
 239         if (cam_ccbq_init(ccbq, openings) != 0) {
 240                 free(ccbq, M_CAMCCBQ);
 241                 return (NULL);
 242         }
 243
 244         return (ccbq);
 245 }
 246
 247 void
 248 cam_ccbq_free(struct cam_ccbq *ccbq)
 249 {
 250         if (ccbq) {
 251                 cam_ccbq_fini(ccbq);
 252                 free(ccbq, M_CAMCCBQ);
 253         }
 254 }
 255
 256 u_int32_t
 257 cam_ccbq_resize(struct cam_ccbq *ccbq, int new_size)
 258 {
 259         int delta;
 260
 261         delta = new_size - (ccbq->dev_active + ccbq->dev_openings);
 262         ccbq->total_openings += delta;
 263         ccbq->dev_openings += delta;
 264
 265         new_size = imax(64, 1 << fls(new_size + new_size / 2));
 266         if (new_size > ccbq->queue.array_size)
 267                 return (camq_resize(&ccbq->queue, new_size));
 268         else
 269                 return (CAM_REQ_CMP);
 270 }
 271
 272 int
 273 cam_ccbq_init(struct cam_ccbq *ccbq, int openings)
 274 {
 275         bzero(ccbq, sizeof(*ccbq));
 276         if (camq_init(&ccbq->queue,
 277             imax(64, 1 << fls(openings + openings / 2))) != 0)
 278                 return (1);
 279         ccbq->total_openings = openings;
 280         ccbq->dev_openings = openings;
 281         return (0);
 282 }
 283
 284 void
 285 cam_ccbq_fini(struct cam_ccbq *ccbq)
 286 {
 287
 288         camq_fini(&ccbq->queue);
 289 }
 290
 291 /*
 292  * Heap routines for manipulating CAM queues.
 293  */
 294 /*
 295  * queue_cmp: Given an array of cam_pinfo* elements and indexes i
 296  * and j, return less than 0, 0, or greater than 0 if i is less than,
 297  * equal too, or greater than j respectively.
 298  */
 299 static __inline int
 300 queue_cmp(cam_pinfo **queue_array, int i, int j)
 301 {
 302         if (queue_array[i]->priority == queue_array[j]->priority)
 303                 return (  queue_array[i]->generation
 304                         - queue_array[j]->generation );
 305         else
 306                 return (  queue_array[i]->priority
 307                         - queue_array[j]->priority );
 308 }
 309
 310 /*
 311  * swap: Given an array of cam_pinfo* elements and indexes i and j,
 312  * exchange elements i and j.
 313  */
 314 static __inline void
 315 swap(cam_pinfo **queue_array, int i, int j)
 316 {
 317         cam_pinfo *temp_qentry;
 318
 319         temp_qentry = queue_array[j];
 320         queue_array[j] = queue_array[i];
 321         queue_array[i] = temp_qentry;
 322         queue_array[j]->index = j;
 323         queue_array[i]->index = i;
 324 }
 325
 326 /*
 327  * heap_up:  Given an array of cam_pinfo* elements with the
 328  * Heap(1, new_index-1) property and a new element in location
 329  * new_index, output Heap(1, new_index).
 330  */
 331 static void
 332 heap_up(cam_pinfo **queue_array, int new_index)
 333 {
 334         int child;
 335         int parent;
 336
 337         child = new_index;
 338
 339         while (child != 1) {
 340                 parent = child >> 1;
 341                 if (queue_cmp(queue_array, parent, child) <= 0)
 342                         break;
 343                 swap(queue_array, parent, child);
 344                 child = parent;
 345         }
 346 }
 347
 348 /*
 349  * heap_down:  Given an array of cam_pinfo* elements with the
 350  * Heap(index + 1, num_entries) property with index containing
 351  * an unsorted entry, output Heap(index, num_entries).
 352  */
 353 static void
 354 heap_down(cam_pinfo **queue_array, int index, int num_entries)
 355 {
 356         int child;
 357         int parent;
 358
 359         parent = index;
 360         child = parent << 1;
 361         for (; child <= num_entries; child = parent << 1) {
 362                 if (child < num_entries) {
 363                         /* child+1 is the right child of parent */
 364                         if (queue_cmp(queue_array, child + 1, child) < 0)
 365                                 child++;
 366                 }
 367                 /* child is now the least child of parent */
 368                 if (queue_cmp(queue_array, parent, child) <= 0)
 369                         break;
 370                 swap(queue_array, child, parent);
 371                 parent = child;
 372         }
 373 }