source: rtems/cpukit/sapi/src/rbheap.c

Last change on this file was 059529e, checked in by Sebastian Huber <sebastian.huber@…>, on Jul 21, 2016 at 8:15:02 AM

score: Add debug support to chains

This helps to detect

  • double insert, append, prepend errors, and
  • get from empty chain errors.
  • Property mode set to 100644
File size: 7.1 KB
Line 
1/**
2 * @file
3 *
4 * @ingroup RBHeap
5 *
6 * @brief Red-Black Tree Heap implementation.
7 */
8
9/*
10 * Copyright (c) 2012-2015 embedded brains GmbH.  All rights reserved.
11 *
12 *  embedded brains GmbH
13 *  Dornierstr. 4
14 *  82178 Puchheim
15 *  Germany
16 *  <rtems@embedded-brains.de>
17 *
18 * The license and distribution terms for this file may be
19 * found in the file LICENSE in this distribution or at
20 * http://www.rtems.org/license/LICENSE.
21 */
22
23#if HAVE_CONFIG_H
24  #include "config.h"
25#endif
26
27#include <rtems/rbheap.h>
28
29#include <stdlib.h>
30
31static uintptr_t align_up(uintptr_t alignment, uintptr_t value)
32{
33  uintptr_t excess = value % alignment;
34
35  if (excess > 0) {
36    value += alignment - excess;
37  }
38
39  return value;
40}
41
42static uintptr_t align_down(uintptr_t alignment, uintptr_t value)
43{
44  uintptr_t excess = value % alignment;
45
46  return value - excess;
47}
48
49static rtems_rbtree_compare_result chunk_compare(
50  const rtems_rbtree_node *a,
51  const rtems_rbtree_node *b
52)
53{
54  const rtems_rbheap_chunk *left = rtems_rbheap_chunk_of_node(a);
55  const rtems_rbheap_chunk *right = rtems_rbheap_chunk_of_node(b);
56
57  return (rtems_rbtree_compare_result)
58    ((left->begin >> 1) - (right->begin >> 1));
59}
60
61static rtems_rbheap_chunk *get_chunk(rtems_rbheap_control *control)
62{
63  rtems_chain_control *chain = &control->spare_descriptor_chain;
64  rtems_chain_node *chunk = rtems_chain_get_unprotected(chain);
65
66  if (chunk == NULL) {
67    (*control->extend_descriptors)(control);
68    chunk = rtems_chain_get_unprotected(chain);
69  }
70
71  return (rtems_rbheap_chunk *) chunk;
72}
73
74static void add_to_chain(
75  rtems_chain_control *chain,
76  rtems_rbheap_chunk *chunk
77)
78{
79  rtems_chain_initialize_node(&chunk->chain_node);
80  rtems_chain_prepend_unprotected(chain, &chunk->chain_node);
81}
82
83static void insert_into_tree(
84  rtems_rbtree_control *tree,
85  rtems_rbheap_chunk *chunk
86)
87{
88  rtems_rbtree_insert(tree, &chunk->tree_node, chunk_compare, true);
89}
90
91rtems_status_code rtems_rbheap_initialize(
92  rtems_rbheap_control *control,
93  void *area_begin,
94  uintptr_t area_size,
95  uintptr_t alignment,
96  rtems_rbheap_extend_descriptors extend_descriptors,
97  void *handler_arg
98)
99{
100  rtems_status_code sc = RTEMS_SUCCESSFUL;
101  uintptr_t begin = (uintptr_t) area_begin;
102  uintptr_t end = begin + area_size;
103  uintptr_t aligned_begin;
104  uintptr_t aligned_end;
105
106  /*
107   * Ensure that the alignment is at least two, so that we can keep
108   * chunk_compare() that simple.
109   */
110  alignment = alignment < 2 ? 2 : alignment;
111
112  aligned_begin = align_up(alignment, begin);
113  aligned_end = align_down(alignment, end);
114
115  if (begin < end && begin <= aligned_begin && aligned_begin < aligned_end) {
116    rtems_chain_control *free_chain = &control->free_chunk_chain;
117    rtems_rbtree_control *chunk_tree = &control->chunk_tree;
118    rtems_rbheap_chunk *first = NULL;
119
120    rtems_chain_initialize_empty(free_chain);
121    rtems_chain_initialize_empty(&control->spare_descriptor_chain);
122    rtems_rbtree_initialize_empty(chunk_tree);
123    control->alignment = alignment;
124    control->handler_arg = handler_arg;
125    control->extend_descriptors = extend_descriptors;
126
127    first = get_chunk(control);
128    if (first != NULL) {
129      first->begin = aligned_begin;
130      first->size = aligned_end - aligned_begin;
131      add_to_chain(free_chain, first);
132      insert_into_tree(chunk_tree, first);
133    } else {
134      sc = RTEMS_NO_MEMORY;
135    }
136  } else {
137    sc = RTEMS_INVALID_ADDRESS;
138  }
139
140  return sc;
141}
142
143static rtems_rbheap_chunk *search_free_chunk(
144  rtems_chain_control *free_chain,
145  size_t size
146)
147{
148  rtems_chain_node *current = rtems_chain_first(free_chain);
149  const rtems_chain_node *tail = rtems_chain_tail(free_chain);
150  rtems_rbheap_chunk *big_enough = NULL;
151
152  while (current != tail && big_enough == NULL) {
153    rtems_rbheap_chunk *free_chunk = (rtems_rbheap_chunk *) current;
154
155    if (free_chunk->size >= size) {
156      big_enough = free_chunk;
157    }
158
159    current = rtems_chain_next(current);
160  }
161
162  return big_enough;
163}
164
165void *rtems_rbheap_allocate(rtems_rbheap_control *control, size_t size)
166{
167  void *ptr = NULL;
168  rtems_chain_control *free_chain = &control->free_chunk_chain;
169  rtems_rbtree_control *chunk_tree = &control->chunk_tree;
170  uintptr_t alignment = control->alignment;
171  uintptr_t aligned_size = align_up(alignment, size);
172
173  if (size > 0 && size <= aligned_size) {
174    rtems_rbheap_chunk *free_chunk = search_free_chunk(free_chain, aligned_size);
175
176    if (free_chunk != NULL) {
177      uintptr_t free_size = free_chunk->size;
178
179      if (free_size > aligned_size) {
180        rtems_rbheap_chunk *new_chunk = get_chunk(control);
181
182        if (new_chunk != NULL) {
183          uintptr_t new_free_size = free_size - aligned_size;
184
185          free_chunk->size = new_free_size;
186          new_chunk->begin = free_chunk->begin + new_free_size;
187          new_chunk->size = aligned_size;
188          rtems_chain_set_off_chain(&new_chunk->chain_node);
189          insert_into_tree(chunk_tree, new_chunk);
190          ptr = (void *) new_chunk->begin;
191        }
192      } else {
193        rtems_chain_extract_unprotected(&free_chunk->chain_node);
194        rtems_chain_set_off_chain(&free_chunk->chain_node);
195        ptr = (void *) free_chunk->begin;
196      }
197    }
198  }
199
200  return ptr;
201}
202
203#define NULL_PAGE rtems_rbheap_chunk_of_node(NULL)
204
205static rtems_rbheap_chunk *find(rtems_rbtree_control *chunk_tree, uintptr_t key)
206{
207  rtems_rbheap_chunk chunk = { .begin = key };
208
209  return rtems_rbheap_chunk_of_node(
210    rtems_rbtree_find(chunk_tree, &chunk.tree_node, chunk_compare, true)
211  );
212}
213
214static rtems_rbheap_chunk *pred(const rtems_rbheap_chunk *chunk)
215{
216  return rtems_rbheap_chunk_of_node(
217    rtems_rbtree_predecessor(&chunk->tree_node)
218  );
219}
220
221static rtems_rbheap_chunk *succ(const rtems_rbheap_chunk *chunk)
222{
223  return rtems_rbheap_chunk_of_node(
224    rtems_rbtree_successor(&chunk->tree_node)
225  );
226}
227
228static void check_and_merge(
229  rtems_rbheap_control *control,
230  rtems_rbheap_chunk *a,
231  rtems_rbheap_chunk *b,
232  rtems_rbheap_chunk *c
233)
234{
235  if (c != NULL_PAGE && rtems_rbheap_is_chunk_free(c)) {
236    a->size += b->size;
237    rtems_chain_extract_unprotected(&b->chain_node);
238    rtems_rbheap_add_to_spare_descriptor_chain(control, b);
239    rtems_rbtree_extract(&control->chunk_tree, &b->tree_node);
240  }
241}
242
243rtems_status_code rtems_rbheap_free(rtems_rbheap_control *control, void *ptr)
244{
245  rtems_status_code sc = RTEMS_SUCCESSFUL;
246
247  if (ptr != NULL) {
248    rtems_rbheap_chunk *chunk = find(&control->chunk_tree, (uintptr_t) ptr);
249
250    if (chunk != NULL_PAGE) {
251      if (!rtems_rbheap_is_chunk_free(chunk)) {
252        rtems_rbheap_chunk *other;
253
254        add_to_chain(&control->free_chunk_chain, chunk);
255        other = succ(chunk);
256        check_and_merge(control, chunk, other, other);
257        other = pred(chunk);
258        check_and_merge(control, other, chunk, other);
259      } else {
260        sc = RTEMS_INCORRECT_STATE;
261      }
262    } else {
263      sc = RTEMS_INVALID_ID;
264    }
265  }
266
267  return sc;
268}
269
270void rtems_rbheap_extend_descriptors_never(rtems_rbheap_control *control)
271{
272  /* Do nothing */
273}
274
275void rtems_rbheap_extend_descriptors_with_malloc(rtems_rbheap_control *control)
276{
277  rtems_rbheap_chunk *chunk = malloc(sizeof(*chunk));
278
279  if (chunk != NULL) {
280    rtems_rbheap_add_to_spare_descriptor_chain(control, chunk);
281  }
282}
Note: See TracBrowser for help on using the repository browser.