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