memory_pool.c

Go to the documentation of this file.

/*
 * Copyright (C) 2014 Swift Navigation Inc.
 * Contact: Fergus Noble <fergus@swift-nav.com>
 *
 * This source is subject to the license found in the file 'LICENSE' which must
 * be be distributed together with this source. All other rights reserved.
 *
 * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
 * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
 */

#include <stdlib.h>
#include <string.h>

#include "memory_pool.h"

inline static size_t calc_node_size(size_t element_size)
{
  return element_size + sizeof(memory_pool_node_hdr_t);
}

inline static node_t *get_node_n(memory_pool_t *pool, node_t *head, u32 n)
{
  return (node_t *)((u8 *)head + calc_node_size(pool->element_size) * n);
}

memory_pool_t *memory_pool_new(u32 n_elements, size_t element_size)
{
  memory_pool_t *new_pool = malloc(sizeof(memory_pool_t));
  if (!new_pool) {
    return NULL;
  }

  /* Allocate memory pool */
  size_t node_size = calc_node_size(element_size);
  node_t *buff = (node_t *)malloc(node_size * n_elements);
  if (!buff) {
    free(new_pool);
    return NULL;
  }

  memory_pool_init(new_pool, n_elements, element_size, buff);

  return new_pool;
}

s8 memory_pool_init(memory_pool_t *new_pool, u32 n_elements,
                    size_t element_size, void *buff)
{
  if (!new_pool) {
    return -1;
  }

  new_pool->n_elements = n_elements;
  new_pool->element_size = element_size;

  /* Setup memory pool buffer area */
  new_pool->pool = (node_t *)buff;
  if (!new_pool->pool) {
    return -2;
  }

  /* Create linked list out of all the nodes in the pool, adding them to the
   * list of free nodes so they are ready to be allocated. */
  new_pool->free_nodes_head = new_pool->pool;
  node_t *current = NULL;
  node_t *next_free = NULL;
  /* Make linked list from last element to first,
   * very last element points to NULL. */
  for (s32 i=n_elements-1; i>=0; i--) {
    current = get_node_n(new_pool, new_pool->free_nodes_head, i);
    current->hdr.next = next_free;
    next_free = current;
  }

  /* No nodes currently allocated. */
  new_pool->allocated_nodes_head = NULL;

  return 0;
}

void memory_pool_destroy(memory_pool_t *pool)
{
  free(pool->pool);
  free(pool);
}

s32 memory_pool_n_free(memory_pool_t *pool)
{
  u32 count = 0;

  node_t *p = pool->free_nodes_head;
  while (p && count <= pool->n_elements) {
    p = p->hdr.next;
    count++;
  }

  if (count == pool->n_elements && p)
    /* The list of free elements is larger than the pool,
     * something has gone horribly wrong. */
    return -1;

  return count;
}

s32 memory_pool_n_allocated(memory_pool_t *pool)
{
  u32 count = 0;

  node_t *p = pool->allocated_nodes_head;
  while (p && count <= pool->n_elements) {
    p = p->hdr.next;
    count++;
  }

  if (count == pool->n_elements && p)
    /* The list of free elements is larger than the pool,
     * something has gone horribly wrong. */
    return -1;

  return count;
}

u8 memory_pool_empty(memory_pool_t *pool)
{
  return (pool->allocated_nodes_head == NULL);
}

u32 memory_pool_n_elements(memory_pool_t *pool)
{
  return pool->n_elements;
}
s32 memory_pool_to_array(memory_pool_t *pool, void *array)
{
  u32 count = 0;

  node_t *p = pool->allocated_nodes_head;
  while (p && count <= pool->n_elements) {
    memcpy((u8 *)array + count*pool->element_size, p->elem, pool->element_size);
    p = p->hdr.next;
    count++;
  }

  if (count == pool->n_elements && p)
    /* The list of free elements is larger than the pool,
     * something has gone horribly wrong. */
    return -1;

  return count;
}

element_t *memory_pool_add(memory_pool_t *pool)
{
  /* Take the head of the list of free nodes, insert it as the head of the
   * allocated nodes and return a pointer to the node's element. */

  if (!pool->free_nodes_head) {
    /* free_nodes_head is NULL, no free nodes available, pool is full. */
    return NULL;
  }

  node_t *new_node = pool->free_nodes_head;
  pool->free_nodes_head = new_node->hdr.next;

  new_node->hdr.next = pool->allocated_nodes_head;
  pool->allocated_nodes_head = new_node;

  return new_node->elem;
}

s32 memory_pool_map(memory_pool_t *pool, void *arg, void (*f)(void *arg, element_t *elem))
{
  u32 count = 0;

  node_t *p = pool->allocated_nodes_head;
  while (p && count <= pool->n_elements) {
    (*f)(arg, p->elem);
    p = p->hdr.next;
    count++;
  }

  if (count == pool->n_elements && p)
    /* The list of elements is larger than the pool,
     * something has gone horribly wrong. */
    return -1;

  return count;
}

s32 memory_pool_fold(memory_pool_t *pool, void *x0,
                     void (*f)(void *x, element_t *elem))
{
  u32 count = 0;

  node_t *p = pool->allocated_nodes_head;
  while (p && count <= pool->n_elements) {
    (*f)(x0, p->elem);
    p = p->hdr.next;
    count++;
  }

  if (count == pool->n_elements && p)
    /* The list of elements is larger than the pool,
     * something has gone horribly wrong. */
    return -1;

  return count;
}

double memory_pool_dfold(memory_pool_t *pool, double x0,
                         double (*f)(double x, element_t *elem))
{
  u32 count = 0;
  double x = x0;

  node_t *p = pool->allocated_nodes_head;
  while (p && count <= pool->n_elements) {
    x = (*f)(x, p->elem);
    p = p->hdr.next;
    count++;
  }

  return x;
}

float memory_pool_ffold(memory_pool_t *pool, float x0,
                        float (*f)(float x, element_t *elem))
{
  u32 count = 0;
  float x = x0;

  node_t *p = pool->allocated_nodes_head;
  while (p && count <= pool->n_elements) {
    x = (*f)(x, p->elem);
    p = p->hdr.next;
    count++;
  }

  return x;
}

s32 memory_pool_ifold(memory_pool_t *pool, s32 x0,
                      s32 (*f)(s32 x, element_t *elem))
{
  u32 count = 0;
  s32 x = x0;

  node_t *p = pool->allocated_nodes_head;
  while (p && count <= pool->n_elements) {
    x = (*f)(x, p->elem);
    p = p->hdr.next;
    count++;
  }

  return x;
}

s32 memory_pool_filter(memory_pool_t *pool, void *arg, s8 (*f)(void *arg, element_t *elem))
{
  u32 count = 0;

  /* Construct a fake 'previous' node for the head of the list, this eliminates
   * special cases for the beginning of the list. */
  node_t fake_head_node_prev = {
    .hdr = {
      .next = pool->allocated_nodes_head,
    },
  };

  node_t *p_prev = &fake_head_node_prev;
  node_t *p = pool->allocated_nodes_head;

  while (p && count <= pool->n_elements) {
    if ((*f)(arg, p->elem)) {
      /* Keep element, move along.. */
      p_prev = p;
      p = p->hdr.next;
      count++;
    } else {
      /* Drop this element from the list */
      p_prev->hdr.next = p->hdr.next;
      /* and return its node to the pool. */
      p->hdr.next = pool->free_nodes_head;
      pool->free_nodes_head = p;
      p = p_prev->hdr.next;
    }
  }

  /* Use our fake previous node to update the head pointer. */
  pool->allocated_nodes_head = fake_head_node_prev.hdr.next;

  if (count == pool->n_elements && p)
    /* The list of elements is larger than the pool,
     * something has gone horribly wrong. */
    return -1;

  return count;
}

s32 memory_pool_clear(memory_pool_t *pool)
{
  u32 count = 0;
  node_t *p = pool->allocated_nodes_head;

  if (!p) {
    /* If allocated_nodes_head is NULL then the pool was already empty. */
    return 0;
  }

  while (p->hdr.next && count <= pool->n_elements) {
    p = p->hdr.next;
    count++;
  }

  if (p->hdr.next) {
    /* Failed to find the end of the allocaed nodes list. */
    return -1;
  }

  /* p now points to the last node in the list. We can now insert the old
   * allocated nodes list in front of the current free nodes list. */
  p->hdr.next = pool->free_nodes_head;
  pool->free_nodes_head = pool->allocated_nodes_head;
  pool->allocated_nodes_head = NULL;

  return 0;
}

void memory_pool_sort(memory_pool_t *pool, void *arg,
                      s32 (*cmp)(void *arg, element_t *a, element_t *b))
{
  /* If collection is empty, return immediately. */
  if (!pool->allocated_nodes_head)
    return;

  u32 insize = 1;

  while (1) {
    node_t *p = pool->allocated_nodes_head;
    pool->allocated_nodes_head = NULL;
    node_t *tail = NULL;

    u32 nmerges = 0;  /* count number of merges we do in this pass */

    while (p) {
      nmerges++;  /* there exists a merge to be done */
      /* step `insize' places along from p */
      node_t *q = p;
      u32 psize = 0;
      for (u32 i = 0; i < insize; i++) {
        psize++;
        q = q->hdr.next;
        if (!q) break;
      }

      /* if q hasn't fallen off end, we have two lists to merge */
      u32 qsize = insize;

      /* now we have two lists; merge them */
      while (psize > 0 || (qsize > 0 && q)) {

        node_t *e;

        /* decide whether next element of merge comes from p or q */
        if (psize == 0) {
          /* p is empty; e must come from q. */
          e = q; q = q->hdr.next; qsize--;
        } else if (qsize == 0 || !q) {
          /* q is empty; e must come from p. */
          e = p; p = p->hdr.next; psize--;
        } else if (cmp(arg, p->elem, q->elem) <= 0) {
          /* First element of p is lower (or same);
           * e must come from p. */
          e = p; p = p->hdr.next; psize--;
        } else {
          /* First element of q is lower; e must come from q. */
          e = q; q = q->hdr.next; qsize--;
        }

        /* add the next element to the merged list */
        if (tail) {
          tail->hdr.next = e;
        } else {
          pool->allocated_nodes_head = e;
        }
        tail = e;
      }

      /* now p has stepped `insize' places along, and q has too */
      p = q;
    }
    tail->hdr.next = NULL;

    /* If we have done only one merge, we're finished. */
    if (nmerges <= 1)   /* allow for nmerges==0, the empty list case */
      return;

    /* Otherwise repeat, merging lists twice the size */
    insize *= 2;
  }
}

void memory_pool_group_by(memory_pool_t *pool, void *arg,
                          s32 (*cmp)(void *arg, element_t *a, element_t *b),
                          void *x0, size_t x_size,
                          void (*agg)(element_t *new, void *x, u32 n, element_t *elem))
{
  /* If collection is empty, return immediately. */
  if (!pool->allocated_nodes_head)
    return;

  /* First sort the existing list using the compare function. */
  memory_pool_sort(pool, arg, cmp);

  /* Save the head of the unaggregated list and reset the pool head where the
   * aggregated data will be added. */
  node_t *old_head = pool->allocated_nodes_head;
  pool->allocated_nodes_head = NULL;

  /* Allocate working area for the fold function. */
  u8 x_work[x_size];

  u32 count = 0;

  node_t *p = old_head;

  while (p && count <= pool->n_elements) {
    u32 group_count = 0;

    /* Keep a pointer to the head of the group. */
    node_t *group_head = p;

    /* Re-initialize the working area. */
    if (x_size)
      memcpy(x_work, x0, x_size);

    /* Create a new element to hold the aggregate of this group. */
    element_t *new_elem = memory_pool_add(pool);

    /* Initialize the new element to the first element in the group. */
    memcpy(new_elem, p->elem, pool->element_size);

    /* Aggregate this group. */
    do {
      agg(new_elem, (void *)x_work, group_count, p->elem);
      group_count++;

      /* Store pointer to next node to process. */
      node_t *next_p = p->hdr.next;

      /* Return current node to the pool. */
      p->hdr.next = pool->free_nodes_head;
      pool->free_nodes_head = p;

      p = next_p;
    } while(p && cmp(arg, group_head->elem, p->elem) == 0);

    count++;
  }
}

s32 memory_pool_product(memory_pool_t *pool, void *xs, u32 n_xs, size_t x_size,
                        void (*prod)(element_t *new, void *x, u32 n_xs, u32 n, element_t *elem))
{
  /* Save the head of the original list and reset the pool head where the
   * product data will be added. */
  node_t *old_head = pool->allocated_nodes_head;
  pool->allocated_nodes_head = NULL;

  u32 count = 0;

  node_t *p = old_head;
  while (p && count <= pool->n_elements) {
    /* Add one element to the new list for each pair of
     * the current element and an item in xs. */
    for (u32 i=0; i<n_xs; i++) {
      element_t *new = memory_pool_add(pool);
      if (!new) {
        /* Pool is full. */
        return -2;
      }
      /* Initialize the element to the same as the original element. */
      memcpy(new, p->elem, pool->element_size);
      prod(new, ((u8 *)xs + i*x_size), n_xs, i, p->elem);
      count++;
    }

    /* Store pointer to next node to process. */
    node_t *next_p = p->hdr.next;

    /* Return current node to the pool. */
    p->hdr.next = pool->free_nodes_head;
    pool->free_nodes_head = p;

    p = next_p;
  }

  if (count == pool->n_elements && p)
    /* The list of elements is larger than the pool,
     * something has gone horribly wrong. */
    return -1;

  return count;
}

s32 memory_pool_product_generator(memory_pool_t *pool, void *x0, u32 max_xs, size_t x_size,
                                  s8 (*next)(void *x, u32 n),
                                  void (*prod)(element_t *new, void *x, u32 n, element_t *elem))
{
  /* Save the head of the original list and reset the pool head where the
   * product data will be added. */
  node_t *old_head = pool->allocated_nodes_head;
  pool->allocated_nodes_head = NULL;

  u32 count = 0;

  node_t *p = old_head;
  while (p && count <= pool->n_elements) {
    /* Iterate through our generator adding a new element for each pair
     * of a generated element and the current element from the old list. */
    u8 x_work[x_size];
    memcpy(x_work, x0, x_size);
    u32 x_count = 0;
    do {
      if (x_count > max_xs) {
        /* Exceded maximum number of generator iterations. */
        return -3;
      }
      element_t *new = memory_pool_add(pool);
      if (!new) {
        /* Pool is full. */
        return -2;
      }
      /* Initialize the element to the same as the original element. */
      memcpy(new, p->elem, pool->element_size);
      prod(new, x_work, x_count, p->elem);
      x_count++;
      count++;
    } while (next(x_work, x_count));

    /* Store pointer to next node to process. */
    node_t *next_p = p->hdr.next;

    /* Return current node to the pool. */
    p->hdr.next = pool->free_nodes_head;
    pool->free_nodes_head = p;

    p = next_p;
  }

  if (count == pool->n_elements && p)
    /* The list of elements is larger than the pool,
     * something has gone horribly wrong. */
    return -1;

  return count;
}