1/*  *********************************************************************
2    *  Broadcom Common Firmware Environment (CFE)
3    *
4    *  Local memory manager			File: cfe_malloc.c
5    *
6    *  This routine is used to manage memory allocated within the
7    *  firmware.  You give it a chunk of memory to manage, and then
8    *  these routines manage suballocations from there.
9    *
10    *  Author:  Mitch Lichtenberg
11    *
12    *********************************************************************
13    *
14    *  Copyright 2000,2001,2002,2003
15    *  Broadcom Corporation. All rights reserved.
16    *
17    *  This software is furnished under license and may be used and
18    *  copied only in accordance with the following terms and
19    *  conditions.  Subject to these conditions, you may download,
20    *  copy, install, use, modify and distribute modified or unmodified
21    *  copies of this software in source and/or binary form.  No title
22    *  or ownership is transferred hereby.
23    *
24    *  1) Any source code used, modified or distributed must reproduce
25    *     and retain this copyright notice and list of conditions
26    *     as they appear in the source file.
27    *
28    *  2) No right is granted to use any trade name, trademark, or
29    *     logo of Broadcom Corporation.  The "Broadcom Corporation"
30    *     name may not be used to endorse or promote products derived
31    *     from this software without the prior written permission of
32    *     Broadcom Corporation.
33    *
34    *  3) THIS SOFTWARE IS PROVIDED "AS-IS" AND ANY EXPRESS OR
35    *     IMPLIED WARRANTIES, INCLUDING BUT NOT LIMITED TO, ANY IMPLIED
36    *     WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
37    *     PURPOSE, OR NON-INFRINGEMENT ARE DISCLAIMED. IN NO EVENT
38    *     SHALL BROADCOM BE LIABLE FOR ANY DAMAGES WHATSOEVER, AND IN
39    *     PARTICULAR, BROADCOM SHALL NOT BE LIABLE FOR DIRECT, INDIRECT,
40    *     INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
41    *     (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
42    *     GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
43    *     BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
44    *     OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
45    *     TORT (INCLUDING NEGLIGENCE OR OTHERWISE), EVEN IF ADVISED OF
46    *     THE POSSIBILITY OF SUCH DAMAGE.
47    ********************************************************************* */
48
49#ifdef TESTPROG
50#include <stdio.h>
51#include <malloc.h>
52#include <stdlib.h>
53#endif
54
55#include "lib_types.h"
56#include "lib_printf.h"
57#include "lib_malloc.h"
58
59
60/*  *********************************************************************
61    *  Constants
62    ********************************************************************* */
63
64#define MEMNODE_SEAL 0xFAAFA123		/* just some random constant */
65#define MINBLKSIZE 64
66
67/*  *********************************************************************
68    *  Types
69    ********************************************************************* */
70
71typedef enum { memnode_free = 0, memnode_alloc } memnode_status_t;
72
73typedef struct memnode_s {
74    unsigned int seal;
75    struct memnode_s *next;		/* pointer to next node */
76    unsigned int length;		/* length of the entire data section */
77    memnode_status_t status;		/* alloc/free status */
78    unsigned char *data;		/* points to actual user data */
79    void *memnodeptr;			/* memnode back pointer (see comments) */
80} memnode_t;
81
82struct mempool_s {
83    memnode_t *root;			/* pointer to root node */
84    unsigned char *base;		/* base of memory region */
85    unsigned int length;		/* size of memory region */
86};
87
88#define memnode_data(t,m) (t) (((memnode_t *) (m))+1)
89
90/*  *********************************************************************
91    *  Globals
92    ********************************************************************* */
93
94mempool_t kmempool;			/* default pool */
95
96/*  *********************************************************************
97    *  kmeminit(pool,buffer,length)
98    *
99    *  Initialize the memory manager, given a pointer to an area
100    *  of memory and a size.  This routine simply initializes the
101    *  root node to be a single block of empty space.
102    *
103    *  Input parameters:
104    *      pool - pool pointer
105    *  	   buffer - beginning of buffer area, must be pointer-aligned
106    *  	   length - length of buffer area
107    *
108    *  Return value:
109    *  	   nothing
110    ********************************************************************* */
111
112
113void kmeminit(mempool_t *pool,unsigned char *buffer,int length)
114{
115    pool->root = (memnode_t *) buffer;
116    pool->root->seal = MEMNODE_SEAL;
117    pool->root->length = length - sizeof(memnode_t);
118    pool->root->data = memnode_data(unsigned char *,pool->root);
119    pool->root->status = memnode_free;
120    pool->root->next = NULL;
121
122    pool->base = buffer;
123    pool->length = length;
124}
125
126
127/*  *********************************************************************
128    *  kmempoolbase(pool)
129    *
130    *  Returns the base address of the specified memory pool
131    *
132    *  Input parameters:
133    *  	   pool - pool pointer
134    *
135    *  Return value:
136    *  	   pointer to beginning of pool's memory
137    ********************************************************************* */
138void *kmempoolbase(mempool_t *pool)
139{
140    return pool->base;
141}
142
143/*  *********************************************************************
144    *  kmempoolsize(pool)
145    *
146    *  Returns the total size of the specified memory pool
147    *
148    *  Input parameters:
149    *  	   pool - pool pointer
150    *
151    *  Return value:
152    *  	   size of pool in bytes
153    ********************************************************************* */
154
155int kmempoolsize(mempool_t *pool)
156{
157    return pool->length;
158}
159
160/*  *********************************************************************
161    *  kmemcompact(pool)
162    *
163    *  Compact the memory blocks, coalescing consectutive free blocks
164    *  on the list.
165    *
166    *  Input parameters:
167    *  	   pool - pool descriptor
168    *
169    *  Return value:
170    *  	   nothing
171    ********************************************************************* */
172
173static void kmemcompact(mempool_t *pool)
174{
175    memnode_t *m;
176    int compacted;
177
178    do {
179	compacted = 0;
180
181	for (m = pool->root; m; m = m->next) {
182
183	    /* Check seal to be sure that we're doing ok */
184
185	    if (m->seal != MEMNODE_SEAL) {
186#ifdef TESTPROG
187		printf("Memory list corrupted!\n");
188#endif
189		return;
190		}
191
192	    /*
193	     * If we're not on the last block and both this
194	     * block and the next one are free, combine them
195	     */
196
197	    if (m->next &&
198		(m->status == memnode_free) &&
199		(m->next->status == memnode_free)) {
200		m->length += sizeof(memnode_t) + m->next->length;
201		m->next->seal = 0;
202		m->next = m->next->next;
203		compacted++;
204		}
205
206	    /* Keep going till we make a pass without doing anything. */
207	    }
208	} while (compacted > 0);
209}
210
211
212/*  *********************************************************************
213    *  kfree(ptr)
214    *
215    *  Return some memory to the pool.
216    *
217    *  Input parameters:
218    *  	   ptr - pointer to something allocated via kmalloc()
219    *
220    *  Return value:
221    *  	   nothing
222    ********************************************************************* */
223
224void kfree(mempool_t *pool,void *ptr)
225{
226    memnode_t **backptr;
227    memnode_t *m;
228
229    if (((unsigned char *) ptr < pool->base) ||
230	((unsigned char *) ptr >= (pool->base+pool->length))) {
231#ifdef TESTPROG
232	printf("Pointer %08X does not belong to pool %08X\n",ptr,pool);
233#endif
234	return;
235	}
236
237    backptr = (memnode_t **) (((unsigned char *) ptr) - sizeof(memnode_t *));
238    m = *backptr;
239
240    if (m->seal != MEMNODE_SEAL) {
241#ifdef TESTPROG
242	printf("Invalid node freed: %08X\n",m);
243#endif
244	return;
245	}
246
247    m->status = memnode_free;
248
249    kmemcompact(pool);
250}
251
252/*  *********************************************************************
253    *  lib_outofmemory()
254    *
255    *  Called when we run out of memory.
256    *  XXX replace with something real someday
257    *
258    *  Input parameters:
259    *  	   nothing
260    *
261    *  Return value:
262    *  	   nothing
263    ********************************************************************* */
264
265void lib_outofmemory(void);
266void lib_outofmemory(void)
267{
268    xprintf("PANIC: out of memory!\n");
269}
270
271/*  *********************************************************************
272    *  kmalloc(pool,size,align)
273    *
274    *  Allocate some memory from the pool.
275    *
276    *  Input parameters:
277    *      pool - pool structure
278    *  	   size - size of item to allocate
279    *  	   align - alignment (must be zero or a power of 2)
280    *
281    *  Return value:
282    *  	   pointer to data, or NULL if no memory left
283    ********************************************************************* */
284
285void *kmalloc(mempool_t *pool,unsigned int size,unsigned int align)
286{
287    memnode_t *m;
288    memnode_t *newm;
289    memnode_t **backptr;
290    uintptr_t daddr = 0;
291    uintptr_t realsize = 0;
292    uintptr_t extra;
293    uintptr_t blkend;
294    uintptr_t ptralign;
295
296    /*
297     * Everything should be aligned by at least the
298     * size of an int64
299     */
300
301    ptralign = (uintptr_t) align;
302    if (ptralign < sizeof(void *)) ptralign = sizeof(uint64_t);
303
304    /*
305     * Everything should be at least a multiple of the
306     * size of a pointer.
307     */
308
309    if (size == 0) size = sizeof(void *);
310    if (size & (sizeof(void *)-1)) {
311	size += sizeof(void *);
312	size &= ~(sizeof(void *)-1);
313	}
314
315    /*
316     * Find a memnode at least big enough to hold the storage we
317     * want.
318     */
319
320    for (m = pool->root; m; m = m->next) {
321
322	if (m->status == memnode_alloc) continue;
323
324	/*
325	 * If we wanted a particular alignment, we will
326	 * need to adjust the size.
327	 */
328
329	daddr = memnode_data(uintptr_t,m);
330	extra = 0;
331	if (daddr & (ptralign-1)) {
332	    extra = size + (ptralign - (daddr & (ptralign-1)));
333	    }
334	realsize = size + extra;
335
336	if (m->length < realsize) continue;
337	break;
338	}
339
340    /*
341     * If m is null, there's no memory left.
342     */
343
344    if (m == NULL) {
345	lib_outofmemory();
346	return NULL;
347	}
348
349    /*
350     * Otherwise, use this block.  Calculate the address of the data
351     * to preserve the alignment.
352     */
353
354    if (daddr & (ptralign-1)) {
355	daddr += ptralign;
356	daddr &= ~(ptralign-1);
357	}
358
359    /* Mark this node as allocated. */
360
361    m->data   = (unsigned char *) daddr;
362    m->status = memnode_alloc;
363
364    /*
365     * Okay, this is ugly.  Store a pointer to the original
366     * memnode just before what we've allocated.  It's guaranteed
367     * to be aligned at least well enough for this pointer.
368     * If for some reason the memnode was already exactly
369     * aligned, backing up will put us inside the memnode
370     * structure itself... that's why the memnodeptr field
371     * is there, as a placeholder for this eventuality.
372     */
373
374    backptr   = (memnode_t **) (m->data - sizeof(memnode_t *));
375    *backptr  = m;
376
377    /*
378     * See if we need to split it.
379     * Don't bother to split if the resulting size will be
380     * less than MINBLKSIZE bytes
381     */
382
383    if (m->length - realsize < MINBLKSIZE) {
384	return m->data;
385	}
386
387    /*
388     * Split this block.  Align the address on a pointer-size
389     * boundary.
390     */
391
392    daddr += size;
393    if (daddr & (uintptr_t)(sizeof(void *)-1)) {
394	daddr += (uintptr_t)sizeof(void *);
395	daddr &= ~(uintptr_t)(sizeof(void *)-1);
396	}
397
398    blkend = memnode_data(uintptr_t,m) + (uintptr_t)(m->length);
399
400    newm = (memnode_t *) daddr;
401
402    newm->next   = m->next;
403    m->length    = (unsigned int) (daddr - memnode_data(uintptr_t,m));
404    m->next      = newm;
405    m->status    = memnode_alloc;
406    newm->seal   = MEMNODE_SEAL;
407    newm->data    = memnode_data(unsigned char *,newm);
408    newm->length = (unsigned int) (blkend - memnode_data(uintptr_t,newm));
409    newm->status = memnode_free;
410
411    return m->data;
412}
413
414
415int kmemstats(mempool_t *pool,memstats_t *stats)
416{
417    memnode_t *m;
418    memnode_t **backptr;
419    uintptr_t daddr;
420
421    stats->mem_totalbytes = pool->length;
422    stats->mem_allocbytes = 0;
423    stats->mem_freebytes = 0;
424    stats->mem_allocnodes = 0;
425    stats->mem_freenodes = 0;
426    stats->mem_largest = 0;
427
428    for (m = pool->root; m; m = m->next) {
429	if (m->status) {
430	    stats->mem_allocnodes++;
431	    stats->mem_allocbytes += m->length;
432	    }
433	else {
434	    stats->mem_freenodes++;
435	    stats->mem_freebytes += m->length;
436	    if (m->length > stats->mem_largest) {
437		stats->mem_largest = m->length;
438		}
439	    }
440
441	daddr = memnode_data(uintptr_t,m);
442	if (m->seal != MEMNODE_SEAL) {
443	    return -1;
444	    }
445	if (m->next && ((daddr + m->length) != (uintptr_t) m->next)) {
446	    return -1;
447	    }
448	if (m->next && (m->next < m)) {
449	    return -1;
450	    }
451	if (m->data < (unsigned char *) m) {
452	    return -1;
453	    }
454	if (m->status == memnode_alloc) {
455	    backptr = (memnode_t **) (m->data - sizeof(void *));
456	    if (*backptr != m) {
457		return -1;
458		}
459	    }
460	}
461
462    return 0;
463}
464
465
466/*  *********************************************************************
467    *  kmemchk()
468    *
469    *  Check the consistency of the memory pool.
470    *
471    *  Input parameters:
472    *      pool - pool pointer
473    *
474    *  Return value:
475    *  	   0 - pool is consistent
476    *  	   -1 - pool is corrupt
477    ********************************************************************* */
478
479#ifdef TESTPROG
480int kmemchk(mempool_t *pool,int verbose)
481{
482    memnode_t *m;
483    memnode_t **backptr;
484    unsigned int daddr;
485
486    for (m = pool->root; m; m = m->next) {
487	if (verbose) {
488	    printf("%08X: Next=%08X  Len=%5u  %s  Data=%08X ",
489	       m,m->next,m->length,
490	       m->status ? "alloc" : "free ",
491	       m->data);
492	    }
493	daddr = memnode_data(uintptr_t,m);
494	if (m->seal != MEMNODE_SEAL) {
495	    if (verbose) printf("BadSeal ");
496	    else return -1;
497	    }
498	if (m->next && (daddr + m->length != (unsigned int) m->next)) {
499	    if (verbose) printf("BadLength ");
500	    else return -1;
501	    }
502	if (m->next && (m->next < m)) {
503	    if (verbose) printf("BadOrder ");
504	    else return -1;
505	    }
506	if (m->data < (unsigned char *) m) {
507	    if (verbose) printf("BadData ");
508	    else return -1;
509	    }
510	if (m->status == memnode_alloc) {
511	    backptr = (memnode_t **) (m->data - sizeof(void *));
512	    if (*backptr != m) {
513		if (verbose) printf("BadBackPtr ");
514		else return -1;
515		}
516	    }
517	if (verbose) printf("\n");
518	}
519
520    return 0;
521}
522
523
524#define MEMSIZE 1024*1024
525
526unsigned char *ptrs[4096];
527unsigned int sizes[4096];
528
529/*  *********************************************************************
530    *  main(argc,argv)
531    *
532    *  Test program for the memory allocator
533    *
534    *  Input parameters:
535    *  	   argc,argv
536    *
537    *  Return value:
538    *  	   nothing
539    ********************************************************************* */
540
541
542void main(int argc,char *argv[])
543{
544    unsigned char *mem;
545    int items = 0;
546    int idx;
547    int size;
548    int totalsize = 0;
549    int nfree,freecnt;
550    mempool_t *pool = &kmempool;
551
552    mem = malloc(MEMSIZE);
553    kmeminit(pool,mem,MEMSIZE);
554
555    items = 0;
556
557    for (;;) {
558
559	for (;;) {
560	    if (items == 4096) break;
561	    size = rand() % 1024;
562	    ptrs[items] = kmalloc(pool,size,1<<(rand() & 7));
563	    if (!ptrs[items]) break;
564	    sizes[items] = size;
565	    items++;
566	    totalsize += size;
567	    }
568
569	printf("%d items allocated, %d total bytes\n",items,totalsize);
570
571	if (kmemchk(pool,0) < 0) {
572	    kmemchk(pool,1);
573	    exit(1);
574	    }
575
576	/* Scramble the pointers */
577	idx = items - 1;
578
579	while (idx) {
580	    if (rand() & 2) {
581		mem = ptrs[0];
582		ptrs[0] = ptrs[idx];
583		ptrs[idx] = mem;
584
585		nfree = sizes[0];
586		sizes[0] = sizes[idx];
587		sizes[idx] = nfree;
588		}
589	    idx--;
590	    }
591
592	/* now free a random number of elements */
593
594	nfree = rand() % items;
595	freecnt = 0;
596
597	for (idx = nfree; idx < items; idx++) {
598	    kfree(pool,ptrs[idx]);
599	    totalsize -= sizes[idx];
600	    freecnt++;
601	    ptrs[idx] = NULL;
602	    sizes[idx] = 0;
603	    if (kmemchk(pool,0) < 0) {
604		kmemchk(pool,1);
605		exit(1);
606		}
607	    }
608
609	items -= freecnt;
610
611	printf(".");
612
613	}
614
615    kmemchk(pool,1);
616
617    exit(0);
618}
619
620#endif	 /* TESTPROG */
621