memory_pools.c 24.7 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
/*
 * Copyright (c) 2015, EURECOM (www.eurecom.fr)
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * The views and conclusions contained in the software and documentation are those
 * of the authors and should not be interpreted as representing official policies,
 * either expressed or implied, of the FreeBSD Project.
 */
29

winckel's avatar
winckel committed
30
31
32
33
#ifdef RTAI
# include <rtai_shm.h>
#endif

34
35
#include "assertions.h"
#include "memory_pools.h"
36
37
38
#if defined(OAI_EMU) || defined(RTAI)
# include "vcd_signal_dumper.h"
#endif
39

40
41
42
43
44
45
46
47
48
49
50
/*------------------------------------------------------------------------------*/
const static int mp_debug = 0;

#ifdef RTAI
# define MP_DEBUG(x, args...) do { if (mp_debug) rt_printk("[MP][D]"x, ##args); } \
    while(0)
#else
# define MP_DEBUG(x, args...) do { if (mp_debug) fprintf(stdout, "[MP][D]"x, ##args); fflush (stdout); } \
    while(0)
#endif

51
52
53
54
55
#if defined(OAI_EMU) || defined(RTAI)
uint64_t vcd_mp_alloc;
uint64_t vcd_mp_free;
#endif

56
/*------------------------------------------------------------------------------*/
Cedric Roux's avatar
Cedric Roux committed
57
#ifndef CHARS_TO_UINT32
58
#define CHARS_TO_UINT32(c1, c2, c3, c4) (((c1) << 24) | ((c2) << 16) | ((c3) << 8) | (c4))
Cedric Roux's avatar
Cedric Roux committed
59
#endif
60

winckel's avatar
winckel committed
61
62
#define MEMORY_POOL_ITEM_INFO_NUMBER    2

63
/*------------------------------------------------------------------------------*/
64
65
typedef int32_t     items_group_position_t;
typedef int32_t     items_group_index_t;
66

winckel's avatar
winckel committed
67
typedef union items_group_positions_u {
68
69
70
71
72
  uint64_t                    all;
  struct {
    items_group_position_t  put;
    items_group_position_t  get;
  } ind;
winckel's avatar
winckel committed
73
74
} items_group_positions_t;

75
typedef struct items_group_s {
76
77
78
79
  items_group_position_t              number_plus_one;
  volatile uint32_t                   minimum;
  volatile items_group_positions_t    positions;
  volatile items_group_index_t       *indexes;
80
} items_group_t;
81

82
83
84
/*------------------------------------------------------------------------------*/
static const items_group_position_t ITEMS_GROUP_POSITION_INVALID    = -1;
static const items_group_index_t    ITEMS_GROUP_INDEX_INVALID       = -1;
85

86
87
88
89
90
91
92
93
94
95
96
/*------------------------------------------------------------------------------*/
typedef uint32_t    pool_item_start_mark_t;
typedef uint32_t    pool_item_end_mark_t;

typedef uint32_t    memory_pool_data_t;

typedef uint32_t    pool_start_mark_t;

typedef uint32_t    pools_start_mark_t;

typedef uint8_t     pool_id_t;
97
typedef uint8_t     item_status_t;
98
99

typedef struct memory_pool_item_start_s {
100
  pool_item_start_mark_t      start_mark;
101

102
103
104
  pool_id_t                   pool_id;
  item_status_t               item_status;
  uint16_t                    info[MEMORY_POOL_ITEM_INFO_NUMBER];
105
106
107
} memory_pool_item_start_t;

typedef struct memory_pool_item_end_s {
108
  pool_item_end_mark_t        end_mark;
109
110
111
} memory_pool_item_end_t;

typedef struct memory_pool_item_s {
112
113
114
  memory_pool_item_start_t    start;
  memory_pool_data_t          data[0];
  memory_pool_item_end_t      end;
115
116
117
} memory_pool_item_t;

typedef struct memory_pool_s {
118
  pool_start_mark_t           start_mark;
119

120
121
122
123
124
  pool_id_t                   pool_id;
  uint32_t                    item_data_number;
  uint32_t                    pool_item_size;
  items_group_t               items_group_free;
  memory_pool_item_t         *items;
125
126
} memory_pool_t;

127

128
typedef struct memory_pools_s {
129
  pools_start_mark_t          start_mark;
130

131
132
133
  uint32_t                    pools_number;
  uint32_t                    pools_defined;
  memory_pool_t              *pools;
134
135
136
137
} memory_pools_t;

/*------------------------------------------------------------------------------*/
static const uint32_t               MAX_POOLS_NUMBER =      20;
138
static const uint32_t               MAX_POOL_ITEMS_NUMBER = 200 * 1000;
139
140
141
142
143
static const uint32_t               MAX_POOL_ITEM_SIZE =    100 * 1000;

static const pool_item_start_mark_t POOL_ITEM_START_MARK =  CHARS_TO_UINT32 ('P', 'I', 's', 't');
static const pool_item_end_mark_t   POOL_ITEM_END_MARK =    CHARS_TO_UINT32 ('p', 'i', 'E', 'N');

144
145
146
static const item_status_t          ITEM_STATUS_FREE =      'F';
static const item_status_t          ITEM_STATUS_ALLOCATED = 'a';

147
148
149
150
151
static const pool_start_mark_t      POOL_START_MARK =       CHARS_TO_UINT32 ('P', '_', 's', 't');

static const pools_start_mark_t     POOLS_START_MARK =      CHARS_TO_UINT32 ('P', 'S', 's', 't');

/*------------------------------------------------------------------------------*/
winckel's avatar
winckel committed
152
static inline uint32_t items_group_number_items (items_group_t *items_group)
153
{
154
  return items_group->number_plus_one - 1;
winckel's avatar
winckel committed
155
156
157
158
}

static inline uint32_t items_group_free_items (items_group_t *items_group)
{
159
160
  items_group_positions_t positions;
  uint32_t                free_items;
winckel's avatar
winckel committed
161

162
  positions.all = items_group->positions.all;
163

164
165
  free_items = items_group->number_plus_one + positions.ind.put - positions.ind.get;
  free_items %= items_group->number_plus_one;
winckel's avatar
winckel committed
166

167
  return free_items;
winckel's avatar
winckel committed
168
169
170
171
}

static inline items_group_index_t items_group_get_free_item (items_group_t *items_group)
{
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
  items_group_position_t  get_raw;
  items_group_position_t  put;
  items_group_position_t  get;
  items_group_position_t  free_items;

  items_group_index_t     index = ITEMS_GROUP_INDEX_INVALID;

  /* Get current put position */
  put = items_group->positions.ind.put % items_group->number_plus_one;
  /* Get current get position and increase it */
  get_raw = __sync_fetch_and_add (&items_group->positions.ind.get, 1);
  get = get_raw % items_group->number_plus_one;

  if(put == get) {
    /* No more item free, restore previous position */
    __sync_fetch_and_sub (&items_group->positions.ind.get, 1);
  } else {
    /* Get index at current get position */
    index = items_group->indexes[get];

    if (index <= ITEMS_GROUP_INDEX_INVALID) {
      /* Index has not yet been completely freed, restore previous get position */
      __sync_fetch_and_sub (&items_group->positions.ind.get, 1);
    } else {
      if (get_raw == items_group->number_plus_one) {
        /* Wrap get position */
        __sync_fetch_and_sub (&items_group->positions.ind.get, items_group->number_plus_one);
      }

      free_items = items_group_free_items(items_group);

      /* Updates minimum free items if needed */
      while (items_group->minimum > free_items) {
        items_group->minimum = free_items;
      }

      /* Clear index at current get position to indicate that item is free */
      items_group->indexes[get] = ITEMS_GROUP_INDEX_INVALID;
210
    }
211
  }
212

213
  return (index);
214
215
}

216
static inline int items_group_put_free_item (items_group_t *items_group, items_group_index_t index)
217
{
218
219
  items_group_position_t  put_raw;
  items_group_position_t  put;
220

221
222
223
  /* Get current put position and increase it */
  put_raw = __sync_fetch_and_add (&items_group->positions.ind.put, 1);
  put = put_raw % items_group->number_plus_one;
224

225
226
227
228
  if (put_raw == items_group->number_plus_one) {
    /* Wrap position */
    __sync_fetch_and_sub (&items_group->positions.ind.put, items_group->number_plus_one);
  }
229

230
231
  AssertError (items_group->indexes[put] <= ITEMS_GROUP_INDEX_INVALID, return (EXIT_FAILURE),
               "Index at current put position (%d) is not marked as free (%d)!\n", put, items_group->number_plus_one);
232

233
234
235
  /* Save freed item index at current put position */
  items_group->indexes[put] = index;
  return (EXIT_SUCCESS);
236
}
237
238

/*------------------------------------------------------------------------------*/
239
static inline memory_pools_t *memory_pools_from_handler (memory_pools_handle_t memory_pools_handle)
winckel's avatar
winckel committed
240
{
241
  memory_pools_t *memory_pools;
winckel's avatar
winckel committed
242

243
244
245
246
247
  /* Recover memory_pools */
  memory_pools = (memory_pools_t *) memory_pools_handle;
  /* Sanity check on passed handle */
  AssertError (memory_pools->start_mark == POOLS_START_MARK, memory_pools = NULL,
               "Handle %p is not a valid memory pools handle, start mark is missing!\n", memory_pools_handle);
winckel's avatar
winckel committed
248

249
  return (memory_pools);
winckel's avatar
winckel committed
250
251
}

252
static inline memory_pool_item_t *memory_pool_item_from_handler (memory_pool_item_handle_t memory_pool_item_handle)
winckel's avatar
winckel committed
253
{
254
255
  void               *address;
  memory_pool_item_t *memory_pool_item;
winckel's avatar
winckel committed
256

257
258
259
  /* Recover memory_pools */
  address = memory_pool_item_handle - sizeof(memory_pool_item_start_t);
  memory_pool_item = (memory_pool_item_t *) address;
260

261
262
263
  /* Sanity check on passed handle */
  AssertError (memory_pool_item->start.start_mark == POOL_ITEM_START_MARK, memory_pool_item = NULL,
               "Handle %p is not a valid memory pool item handle, start mark is missing!\n", memory_pool_item);
winckel's avatar
winckel committed
264

265
  return (memory_pool_item);
winckel's avatar
winckel committed
266
267
}

268
static inline memory_pool_item_t *memory_pool_item_from_index (memory_pool_t *memory_pool, items_group_index_t index)
winckel's avatar
winckel committed
269
{
270
  void               *address;
winckel's avatar
winckel committed
271

272
273
  address = (void *) memory_pool->items;
  address += index * memory_pool->pool_item_size;
winckel's avatar
winckel committed
274

275
  return (address);
winckel's avatar
winckel committed
276
}
277
278

/*------------------------------------------------------------------------------*/
279
memory_pools_handle_t memory_pools_create (uint32_t pools_number)
280
{
281
282
  memory_pools_t *memory_pools;
  pool_id_t       pool;
283

284
  AssertFatal (pools_number <= MAX_POOLS_NUMBER, "Too many memory pools requested (%d/%d)!\n", pools_number, MAX_POOLS_NUMBER); /* Limit to a reasonable number of pools */
285

286
287
288
  /* Allocate memory_pools */
  memory_pools = malloc (sizeof(memory_pools_t));
  AssertFatal (memory_pools != NULL, "Memory pools structure allocation failed!\n");
289

290
291
292
293
294
295
296
297
298
  /* Initialize memory_pools */
  {
    memory_pools->start_mark    = POOLS_START_MARK;
    memory_pools->pools_number  = pools_number;
    memory_pools->pools_defined = 0;

    /* Allocate pools */
    memory_pools->pools         = calloc (pools_number, sizeof(memory_pool_t));
    AssertFatal (memory_pools->pools != NULL, "Memory pools allocation failed!\n");
299

300
301
302
    /* Initialize pools */
    for (pool = 0; pool < pools_number; pool++) {
      memory_pools->pools[pool].start_mark = POOL_START_MARK;
303
    }
304
  }
305

306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
  return ((memory_pools_handle_t) memory_pools);
}

char *memory_pools_statistics(memory_pools_handle_t memory_pools_handle)
{
  memory_pools_t     *memory_pools;
  pool_id_t           pool;
  char               *statistics;
  int                 printed_chars;
  uint32_t            allocated_pool_memory;
  uint32_t            allocated_pools_memory = 0;
  items_group_t      *items_group;
  uint32_t            pool_items_size;

  /* Recover memory_pools */
  memory_pools = memory_pools_from_handler (memory_pools_handle);
  AssertFatal (memory_pools != NULL, "Failed to retrieve memory pool for handle %p!\n", memory_pools_handle);

  statistics = malloc(memory_pools->pools_defined * 200);

  printed_chars = sprintf (&statistics[0], "Pool:   size, number, minimum,   free, address space and memory used in Kbytes\n");

  for (pool = 0; pool < memory_pools->pools_defined; pool++) {
    items_group = &memory_pools->pools[pool].items_group_free;
    allocated_pool_memory = items_group_number_items (items_group) * memory_pools->pools[pool].pool_item_size;
    allocated_pools_memory += allocated_pool_memory;
    pool_items_size = memory_pools->pools[pool].item_data_number * sizeof(memory_pool_data_t);
    printed_chars += sprintf (&statistics[printed_chars], "  %2u: %6u, %6u,  %6u, %6u, [%p-%p] %6u\n",
                              pool, pool_items_size,
                              items_group_number_items (items_group),
                              items_group->minimum,
                              items_group_free_items (items_group),
                              memory_pools->pools[pool].items,
                              ((void *) memory_pools->pools[pool].items) + allocated_pool_memory,
                              allocated_pool_memory / (1024));
  }

  printed_chars = sprintf (&statistics[printed_chars], "Pools memory %u Kbytes\n", allocated_pools_memory / (1024));

  return (statistics);
346
347
348
349
}

int memory_pools_add_pool (memory_pools_handle_t memory_pools_handle, uint32_t pool_items_number, uint32_t pool_item_size)
{
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
  memory_pools_t     *memory_pools;
  memory_pool_t      *memory_pool;
  pool_id_t           pool;
  items_group_index_t item_index;
  memory_pool_item_t *memory_pool_item;

  AssertFatal (pool_items_number <= MAX_POOL_ITEMS_NUMBER, "Too many items for a memory pool (%u/%d)!\n", pool_items_number, MAX_POOL_ITEMS_NUMBER); /* Limit to a reasonable number of items */
  AssertFatal (pool_item_size <= MAX_POOL_ITEM_SIZE, "Item size is too big for memory pool items (%u/%d)!\n", pool_item_size, MAX_POOL_ITEM_SIZE);   /* Limit to a reasonable item size */

  /* Recover memory_pools */
  memory_pools    = memory_pools_from_handler (memory_pools_handle);
  AssertFatal (memory_pools != NULL, "Failed to retrieve memory pool for handle %p!\n", memory_pools_handle);

  /* Check number of already created pools */
  AssertFatal (memory_pools->pools_defined < memory_pools->pools_number, "Can not allocate more memory pool (%d)!\n", memory_pools->pools_number);

  /* Select pool */
  pool            = memory_pools->pools_defined;
  memory_pool     = &memory_pools->pools[pool];

  /* Initialize pool */
  {
    memory_pool->pool_id                            = pool;
    /* Item size in memory_pool_data_t items by excess */
    memory_pool->item_data_number                   = (pool_item_size + sizeof(memory_pool_data_t) - 1) / sizeof(memory_pool_data_t);
    memory_pool->pool_item_size                     = (memory_pool->item_data_number * sizeof(memory_pool_data_t)) + sizeof(memory_pool_item_t);
    memory_pool->items_group_free.number_plus_one   = pool_items_number + 1;
    memory_pool->items_group_free.minimum           = pool_items_number;
    memory_pool->items_group_free.positions.ind.put = pool_items_number;
    memory_pool->items_group_free.positions.ind.get = 0;

    /* Allocate free indexes */
    memory_pool->items_group_free.indexes = malloc(memory_pool->items_group_free.number_plus_one * sizeof(items_group_index_t));
    AssertFatal (memory_pool->items_group_free.indexes != NULL, "Memory pool indexes allocation failed!\n");

    /* Initialize free indexes */
    for (item_index = 0; item_index < pool_items_number; item_index++) {
      memory_pool->items_group_free.indexes[item_index] = item_index;
    }
389

390
391
    /* Last index is not allocated */
    memory_pool->items_group_free.indexes[item_index] = ITEMS_GROUP_INDEX_INVALID;
392

393
394
395
    /* Allocate items */
    memory_pool->items = calloc (pool_items_number, memory_pool->pool_item_size);
    AssertFatal (memory_pool->items != NULL, "Memory pool items allocation failed!\n");
winckel's avatar
winckel committed
396

397
398
399
400
401
402
403
    /* Initialize items */
    for (item_index = 0; item_index < pool_items_number; item_index++) {
      memory_pool_item                                      = memory_pool_item_from_index (memory_pool, item_index);
      memory_pool_item->start.start_mark                    = POOL_ITEM_START_MARK;
      memory_pool_item->start.pool_id                       = pool;
      memory_pool_item->start.item_status                   = ITEM_STATUS_FREE;
      memory_pool_item->data[memory_pool->item_data_number] = POOL_ITEM_END_MARK;
404
    }
405
  }
406

407
  memory_pools->pools_defined ++;
408

409
  return (0);
410
411
}

412
memory_pool_item_handle_t memory_pools_allocate (memory_pools_handle_t memory_pools_handle, uint32_t item_size, uint16_t info_0, uint16_t info_1)
winckel's avatar
winckel committed
413
{
414
415
416
417
418
  memory_pools_t             *memory_pools;
  memory_pool_item_t         *memory_pool_item;
  memory_pool_item_handle_t   memory_pool_item_handle = NULL;
  pool_id_t                   pool;
  items_group_index_t         item_index = ITEMS_GROUP_INDEX_INVALID;
winckel's avatar
winckel committed
419

420
#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
421
  VCD_SIGNAL_DUMPER_DUMP_VARIABLE_BY_NAME(VCD_SIGNAL_DUMPER_VARIABLE_MP_ALLOC,
422
                                          __sync_or_and_fetch (&vcd_mp_alloc, 1L << info_0));
423
424
#endif

425
426
427
  /* Recover memory_pools */
  memory_pools = memory_pools_from_handler (memory_pools_handle);
  AssertError (memory_pools != NULL, {}, "Failed to retrieve memory pool for handle %p!\n", memory_pools_handle);
winckel's avatar
winckel committed
428

429
430
431
432
  for (pool = 0; pool < memory_pools->pools_defined; pool++) {
    if ((memory_pools->pools[pool].item_data_number * sizeof(memory_pool_data_t)) < item_size) {
      /* This memory pool has too small items, skip it */
      continue;
433
    }
434
435
436
437
438
439
440
441
442

    item_index = items_group_get_free_item(&memory_pools->pools[pool].items_group_free);

    if (item_index <= ITEMS_GROUP_INDEX_INVALID) {
      /* Allocation failed, skip this pool */
      continue;
    } else {
      /* Allocation succeed, exit searching loop */
      break;
winckel's avatar
winckel committed
443
    }
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
  }

  if (item_index > ITEMS_GROUP_INDEX_INVALID) {
    /* Convert item index into memory_pool_item address */
    memory_pool_item                    = memory_pool_item_from_index (&memory_pools->pools[pool], item_index);
    /* Sanity check on item status, must be free */
    AssertFatal (memory_pool_item->start.item_status == ITEM_STATUS_FREE, "Item status is not set to free (%d) in pool %u, item %d!\n",
                 memory_pool_item->start.item_status, pool, item_index);

    memory_pool_item->start.item_status = ITEM_STATUS_ALLOCATED;
    memory_pool_item->start.info[0]     = info_0;
    memory_pool_item->start.info[1]     = info_1;
    memory_pool_item_handle             = memory_pool_item->data;

    MP_DEBUG(" Alloc [%2u][%6d]{%6d}, %3u %3u, %6u, %p, %p, %p\n",
             pool, item_index,
             items_group_free_items (&memory_pools->pools[pool].items_group_free),
             info_0, info_1,
             item_size,
             memory_pools->pools[pool].items,
             memory_pool_item,
             memory_pool_item_handle);
  } else {
    MP_DEBUG(" Alloc [--][------]{------}, %3u %3u, %6u, failed!\n", info_0, info_1, item_size);
  }
winckel's avatar
winckel committed
469

470
#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
471
  VCD_SIGNAL_DUMPER_DUMP_VARIABLE_BY_NAME(VCD_SIGNAL_DUMPER_VARIABLE_MP_ALLOC,
472
                                          __sync_and_and_fetch (&vcd_mp_alloc, ~(1L << info_0)));
473
474
#endif

475
  return memory_pool_item_handle;
winckel's avatar
winckel committed
476
477
}

478
int memory_pools_free (memory_pools_handle_t memory_pools_handle, memory_pool_item_handle_t memory_pool_item_handle, uint16_t info_0)
winckel's avatar
winckel committed
479
{
480
481
482
483
484
485
486
487
  memory_pools_t     *memory_pools;
  memory_pool_item_t *memory_pool_item;
  pool_id_t           pool;
  items_group_index_t item_index;
  uint32_t            item_size;
  uint32_t            pool_item_size;
  uint16_t            info_1;
  int                 result;
winckel's avatar
winckel committed
488

489
490
491
492
493
494
495
496
497
498
499
  /* Recover memory_pools */
  memory_pools = memory_pools_from_handler (memory_pools_handle);
  AssertError (memory_pools != NULL, return (EXIT_FAILURE), "Failed to retrieve memory pools for handle %p!\n", memory_pools_handle);

  /* Recover memory pool item */
  memory_pool_item = memory_pool_item_from_handler (memory_pool_item_handle);
  AssertError (memory_pool_item != NULL, return (EXIT_FAILURE), "Failed to retrieve memory pool item for handle %p!\n", memory_pool_item_handle);

  info_1 = memory_pool_item->start.info[1];

#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
500
  VCD_SIGNAL_DUMPER_DUMP_VARIABLE_BY_NAME(VCD_SIGNAL_DUMPER_VARIABLE_MP_FREE,
501
502
503
504
505
506
507
508
509
510
                                          __sync_or_and_fetch (&vcd_mp_free, 1L << info_1));
#endif

  /* Recover pool index */
  pool = memory_pool_item->start.pool_id;
  AssertFatal (pool < memory_pools->pools_defined, "Pool index is invalid (%u/%u)!\n", pool, memory_pools->pools_defined);

  item_size = memory_pools->pools[pool].item_data_number;
  pool_item_size = memory_pools->pools[pool].pool_item_size;
  item_index = (((void *) memory_pool_item) - ((void *) memory_pools->pools[pool].items)) / pool_item_size;
511

512
513
514
515
516
517
  MP_DEBUG(" Free  [%2u][%6d]{%6d}, %3u %3u,         %p, %p, %p, %u\n",
           pool, item_index,
           items_group_free_items (&memory_pools->pools[pool].items_group_free),
           memory_pool_item->start.info[0], info_1,
           memory_pool_item_handle, memory_pool_item,
           memory_pools->pools[pool].items, ((uint32_t) (item_size * sizeof(memory_pool_data_t))));
518

519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
  /* Sanity check on calculated item index */
  AssertFatal (memory_pool_item == memory_pool_item_from_index(&memory_pools->pools[pool], item_index),
               "Incorrect memory pool item address (%p, %p) for pool %u, item %d!\n",
               memory_pool_item, memory_pool_item_from_index(&memory_pools->pools[pool], item_index), pool, item_index);
  /* Sanity check on end marker, must still be present (no write overflow) */
  AssertFatal (memory_pool_item->data[item_size] == POOL_ITEM_END_MARK,
               "Memory pool item is corrupted, end mark is not present for pool %u, item %d!\n", pool, item_index);
  /* Sanity check on item status, must be allocated */
  AssertFatal (memory_pool_item->start.item_status == ITEM_STATUS_ALLOCATED,
               "Trying to free a non allocated (%x) memory pool item (pool %u, item %d)!\n",
               memory_pool_item->start.item_status, pool, item_index);

  memory_pool_item->start.item_status = ITEM_STATUS_FREE;

  result = items_group_put_free_item(&memory_pools->pools[pool].items_group_free, item_index);

  AssertError (result == EXIT_SUCCESS, {}, "Failed to free memory pool item (pool %u, item %d)!\n", pool, item_index);
536
537

#if defined(OAI_EMU) || defined(RTAI)
gauthier's avatar
gauthier committed
538
  VCD_SIGNAL_DUMPER_DUMP_VARIABLE_BY_NAME(VCD_SIGNAL_DUMPER_VARIABLE_MP_FREE,
539
                                          __sync_and_and_fetch (&vcd_mp_free, ~(1L << info_1)));
540
541
#endif

542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
  return (result);
}

void memory_pools_set_info (memory_pools_handle_t memory_pools_handle, memory_pool_item_handle_t memory_pool_item_handle, int index, uint16_t info)
{
  memory_pools_t     *memory_pools;
  memory_pool_item_t *memory_pool_item;
  pool_id_t           pool;
  items_group_index_t item_index;
  uint32_t            item_size;
  uint32_t            pool_item_size;

  AssertFatal (index < MEMORY_POOL_ITEM_INFO_NUMBER, "Incorrect info index (%d/%d)!\n", index, MEMORY_POOL_ITEM_INFO_NUMBER);

  /* Recover memory pool item */
  memory_pool_item = memory_pool_item_from_handler (memory_pool_item_handle);
  AssertFatal (memory_pool_item != NULL, "Failed to retrieve memory pool item for handle %p!\n", memory_pool_item_handle);

  /* Set info[1] */
  memory_pool_item->start.info[index] = info;

  /* Check item validity and log (not mandatory) */
  if (1) {
    /* Recover memory_pools */
    memory_pools = memory_pools_from_handler (memory_pools_handle);
    AssertFatal (memory_pools != NULL, "Failed to retrieve memory pool for handle %p!\n", memory_pools_handle);

winckel's avatar
winckel committed
569
570
    /* Recover pool index */
    pool = memory_pool_item->start.pool_id;
571
    AssertFatal (pool < memory_pools->pools_defined, "Pool index is invalid (%u/%u)!\n", pool, memory_pools->pools_defined);
winckel's avatar
winckel committed
572

573
574
575
576
    item_size = memory_pools->pools[pool].item_data_number;
    pool_item_size = memory_pools->pools[pool].pool_item_size;
    item_index = (((void *) memory_pool_item) - ((void *) memory_pools->pools[pool].items)) / pool_item_size;

577
    MP_DEBUG(" Info  [%2u][%6d]{%6d}, %3u %3u,         %p, %p, %p, %u\n",
winckel's avatar
winckel committed
578
579
             pool, item_index,
             items_group_free_items (&memory_pools->pools[pool].items_group_free),
580
             memory_pool_item->start.info[0], memory_pool_item->start.info[1],
581
             memory_pool_item_handle, memory_pool_item,
winckel's avatar
winckel committed
582
             memory_pools->pools[pool].items, ((uint32_t) (item_size * sizeof(memory_pool_data_t))));
583

winckel's avatar
winckel committed
584
    /* Sanity check on calculated item index */
585
586
    AssertFatal (memory_pool_item == memory_pool_item_from_index(&memory_pools->pools[pool], item_index),
                 "Incorrect memory pool item address (%p, %p) for pool %u, item %d!\n",
winckel's avatar
winckel committed
587
                 memory_pool_item, memory_pool_item_from_index(&memory_pools->pools[pool], item_index), pool, item_index);
588
    /* Sanity check on end marker, must still be present (no write overflow) */
589
590
    AssertFatal (memory_pool_item->data[item_size] == POOL_ITEM_END_MARK,
                 "Memory pool item is corrupted, end mark is not present for pool %u, item %d!\n", pool, item_index);
591
    /* Sanity check on item status, must be allocated */
592
    AssertFatal (memory_pool_item->start.item_status == ITEM_STATUS_ALLOCATED,
593
                 "Trying to free a non allocated (%x) memory pool item (pool %u, item %d)\n",
winckel's avatar
winckel committed
594
                 memory_pool_item->start.item_status, pool, item_index);
595
  }
winckel's avatar
winckel committed
596
}