1
/*
2
 * Copyright (c) 2018 naehrwert
3
 * Copyright (c) 2018-2024 CTCaer
4
 *
5
 * This program is free software; you can redistribute it and/or modify it
6
 * under the terms and conditions of the GNU General Public License,
7
 * version 2, as published by the Free Software Foundation.
8
 *
9
 * This program is distributed in the hope it will be useful, but WITHOUT
10
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
11
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
12
 * more details.
13
 *
14
 * You should have received a copy of the GNU General Public License
15
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
16
 */
17

18
#include <string.h>
19
#include "heap.h"
20
#include <gfx_utils.h>
21

22
heap_t _heap;
23

24
static void _heap_create(void *start)
25
{
26
	_heap.start = start;
27
	_heap.first = NULL;
28
	_heap.last = NULL;
29
}
30

31
// Node info is before node address.
32
static void *_heap_alloc(u32 size)
33
{
34
	hnode_t *node, *new_node;
35

36
	// Align to cache line size.
37
	size = ALIGN(size, sizeof(hnode_t));
38

39
	// First allocation.
40
	if (!_heap.first)
41
	{
42
		node = (hnode_t *)_heap.start;
43
		node->used = 1;
44
		node->size = size;
45
		node->prev = NULL;
46
		node->next = NULL;
47

48
		_heap.first = node;
49
		_heap.last = node;
50

51
		return (void *)node + sizeof(hnode_t);
52
	}
53

54
#ifdef BDK_MALLOC_NO_DEFRAG
55
	// Get the last allocated block.
56
	node = _heap.last;
57
#else
58
	// Get first block and find the first available one.
59
	node = _heap.first;
60
	while (true)
61
	{
62
		// Check if there's available unused node.
63
		if (!node->used && (size <= node->size))
64
		{
65
			// Size and offset of the new unused node.
66
			u32 new_size = node->size - size;
67
			new_node = (hnode_t *)((void *)node + sizeof(hnode_t) + size);
68

69
			// If there's aligned unused space from the old node,
70
			// create a new one and set the leftover size.
71
			if (new_size >= (sizeof(hnode_t) << 2))
72
			{
73
				new_node->size = new_size - sizeof(hnode_t);
74
				new_node->used = 0;
75
				new_node->next = node->next;
76

77
				// Check that we are not on first node.
78
				if (new_node->next)
79
					new_node->next->prev = new_node;
80

81
				new_node->prev = node;
82
				node->next = new_node;
83
			}
84
			else // Unused node size is just enough.
85
				size += new_size;
86

87
			node->size = size;
88
			node->used = 1;
89

90
			return (void *)node + sizeof(hnode_t);
91
		}
92

93
		// No unused node found, try the next one.
94
		if (node->next)
95
			node = node->next;
96
		else
97
			break;
98
	}
99
#endif
100

101
	// No unused node found, create a new one.
102
	new_node = (hnode_t *)((void *)node + sizeof(hnode_t) + node->size);
103
	new_node->used = 1;
104
	new_node->size = size;
105
	new_node->prev = node;
106
	new_node->next = NULL;
107

108
	node->next = new_node;
109
	_heap.last = new_node;
110

111
	return (void *)new_node + sizeof(hnode_t);
112
}
113

114
static void _heap_free(void *addr)
115
{
116
	hnode_t *node = (hnode_t *)(addr - sizeof(hnode_t));
117
	node->used = 0;
118
	node = _heap.first;
119

120
#ifndef BDK_MALLOC_NO_DEFRAG
121
	// Do simple defragmentation on next blocks.
122
	while (node)
123
	{
124
		if (!node->used)
125
		{
126
			if (node->prev && !node->prev->used)
127
			{
128
				node->prev->size += node->size + sizeof(hnode_t);
129
				node->prev->next = node->next;
130

131
				if (node->next)
132
					node->next->prev = node->prev;
133
			}
134
		}
135
		node = node->next;
136
	}
137
#endif
138
}
139

140
void heap_init(void *base)
141
{
142
	_heap_create(base);
143
}
144

145
void heap_set(heap_t *heap)
146
{
147
	memcpy(&_heap, heap, sizeof(heap_t));
148
}
149

150
void *malloc(u32 size)
151
{
152
	return _heap_alloc(size);
153
}
154

155
void *calloc(u32 num, u32 size)
156
{
157
	void *res = (void *)_heap_alloc(num * size);
158
	memset(res, 0, ALIGN(num * size, sizeof(hnode_t))); // Clear the aligned size.
159
	return res;
160
}
161

162
void *zalloc(u32 size)
163
{
164
	void *res = (void *)_heap_alloc(size);
165
	memset(res, 0, ALIGN(size, sizeof(hnode_t))); // Clear the aligned size.
166
	return res;
167
}
168

169
void free(void *buf)
170
{
171
	if (buf >= _heap.start)
172
		_heap_free(buf);
173
}
174

175
void heap_monitor(heap_monitor_t *mon, bool print_node_stats)
176
{
177
	u32 count = 0;
178
	memset(mon, 0, sizeof(heap_monitor_t));
179

180
	hnode_t *node = _heap.first;
181
	while (true)
182
	{
183
		if (node->used)
184
		{
185
			mon->nodes_used++;
186
			mon->used += node->size + sizeof(hnode_t);
187
		}
188
		else
189
			mon->total += node->size + sizeof(hnode_t);
190

191
		if (print_node_stats)
192
			gfx_printf("%3d - %d, addr: 0x%08X, size: 0x%X\n",
193
				count, node->used, (u32)node + sizeof(hnode_t), node->size);
194

195
		count++;
196

197
		if (node->next)
198
			node = node->next;
199
		else
200
			break;
201
	}
202
	mon->total += mon->used;
203
	mon->nodes_total = count;
204
}
205

206