heapobj-heapmem.c 2.69 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
29
30
31
/*
 * Copyright (C) 2018 Philippe Gerum <rpm@xenomai.org>.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.

 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA.
 */
#include <stdlib.h>
#include "boilerplate/heapmem.h"
#include "copperplate/heapobj.h"
#include "copperplate/debug.h"
#include "copperplate/tunables.h"
#include "xenomai/init.h"

#define MIN_HEAPMEM_HEAPSZ  (64 * 1024)

struct heap_memory heapmem_main;

int __heapobj_init_private(struct heapobj *hobj, const char *name,
			   size_t size, void *mem)
{
32
	struct heap_memory *heap;
33
34
35
	void *_mem = mem;
	int ret;

36
37
38
39
	heap = __STD(malloc(sizeof(*heap)));
	if (heap == NULL)
		return -ENOMEM;

40
	if (mem == NULL) {
41
42
43
44
		size = HEAPMEM_ARENA_SIZE(size); /* Count meta-data in. */
		mem = __STD(malloc(size));
		if (mem == NULL) {
			__STD(free(heap));
45
			return -ENOMEM;
46
		}
47
48
49
50
51
52
53
	}
	
	if (name)
		snprintf(hobj->name, sizeof(hobj->name), "%s", name);
	else
		snprintf(hobj->name, sizeof(hobj->name), "%p", hobj);

54
55
56
57
	hobj->pool = heap;
	hobj->size = size;

	ret = heapmem_init(hobj->pool, mem, size);
58
	if (ret) {
59
60
61
		if (_mem == NULL)
			__STD(free(mem));
		__STD(free(heap));
62
63
64
65
66
67
68
69
70
		return ret;
	}

	return 0;
}

int heapobj_init_array_private(struct heapobj *hobj, const char *name,
			       size_t size, int elems)
{
71
72
	size_t log2 = sizeof(size) * CHAR_BIT - 1 -
		xenomai_count_leading_zeros(size);
73
74
75
76
77
78
79
80
81
82
83

	/*
	 * Heapmem aligns individual object sizes on the next ^2
	 * boundary, do likewise when determining the overall heap
	 * size, so that we can allocate as many as @elems items.
	 */
	if (size & (size - 1))
		log2++;

	size = 1 << log2;

84
	return __bt(__heapobj_init_private(hobj, name,
85
					   size * elems, NULL));
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
}

int heapobj_pkg_init_private(void)
{
	size_t size;
	void *mem;
	int ret;

#ifdef CONFIG_XENO_PSHARED
	size = MIN_HEAPMEM_HEAPSZ;
#else
	size = __copperplate_setup_data.mem_pool;
	if (size < MIN_HEAPMEM_HEAPSZ)
		size = MIN_HEAPMEM_HEAPSZ;
#endif
	size = HEAPMEM_ARENA_SIZE(size);
	mem = __STD(malloc(size));
	if (mem == NULL)
		return -ENOMEM;

	ret = heapmem_init(&heapmem_main, mem, size);
	if (ret) {
		__STD(free(mem));
		return ret;
	}

	return 0;
}