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 (mpl@broadcom.com)
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
253void lib_outofmemory(void);
254void lib_outofmemory(void)
255{
256    xprintf("PANIC: out of memory!\n");
257}
258
259/*  *********************************************************************
260    *  kmalloc(pool,size,align)
261    *
262    *  Allocate some memory from the pool.
263    *
264    *  Input parameters:
265    *      pool - pool structure
266    *  	   size - size of item to allocate
267    *  	   align - alignment (must be zero or a power of 2)
268    *
269    *  Return value:
270    *  	   pointer to data, or NULL if no memory left
271    ********************************************************************* */
272
273void *kmalloc(mempool_t *pool,unsigned int size,unsigned int align)
274{
275    memnode_t *m;
276    memnode_t *newm;
277    memnode_t **backptr;
278    uintptr_t daddr = 0;
279    uintptr_t realsize = 0;
280    uintptr_t extra;
281    uintptr_t blkend;
282    uintptr_t ptralign;
283
284    /*
285     * Everything should be aligned by at least the
286     * size of an int64
287     */
288
289    ptralign = (uintptr_t) align;
290    if (ptralign < sizeof(void *)) ptralign = sizeof(uint64_t);
291
292    /*
293     * Everything should be at least a multiple of the
294     * size of a pointer.
295     */
296
297    if (size == 0) size = sizeof(void *);
298    if (size & (sizeof(void *)-1)) {
299	size += sizeof(void *);
300	size &= ~(sizeof(void *)-1);
301	}
302
303    /*
304     * Find a memnode at least big enough to hold the storage we
305     * want.
306     */
307
308    for (m = pool->root; m; m = m->next) {
309
310	if (m->status == memnode_alloc) continue;
311
312	/*
313	 * If we wanted a particular alignment, we will
314	 * need to adjust the size.
315	 */
316
317	daddr = memnode_data(uintptr_t,m);
318	extra = 0;
319	if (daddr & (ptralign-1)) {
320	    extra = size + (ptralign - (daddr & (ptralign-1)));
321	    }
322	realsize = size + extra;
323
324	if (m->length < realsize) continue;
325	break;
326	}
327
328    /*
329     * If m is null, there's no memory left.
330     */
331
332    if (m == NULL) {
333	lib_outofmemory();
334	return NULL;
335	}
336
337    /*
338     * Otherwise, use this block.  Calculate the address of the data
339     * to preserve the alignment.
340     */
341
342    if (daddr & (ptralign-1)) {
343	daddr += ptralign;
344	daddr &= ~(ptralign-1);
345	}
346
347    /* Mark this node as allocated. */
348
349    m->data   = (unsigned char *) daddr;
350    m->status = memnode_alloc;
351
352    /*
353     * Okay, this is ugly.  Store a pointer to the original
354     * memnode just before what we've allocated.  It's guaranteed
355     * to be aligned at least well enough for this pointer.
356     * If for some reason the memnode was already exactly
357     * aligned, backing up will put us inside the memnode
358     * structure itself... that's why the memnodeptr field
359     * is there, as a placeholder for this eventuality.
360     */
361
362    backptr   = (memnode_t **) (m->data - sizeof(memnode_t *));
363    *backptr  = m;
364
365    /*
366     * See if we need to split it.
367     * Don't bother to split if the resulting size will be
368     * less than MINBLKSIZE bytes
369     */
370
371    if (m->length - realsize < MINBLKSIZE) {
372	return m->data;
373	}
374
375    /*
376     * Split this block.  Align the address on a pointer-size
377     * boundary.
378     */
379
380    daddr += size;
381    if (daddr & (uintptr_t)(sizeof(void *)-1)) {
382	daddr += (uintptr_t)sizeof(void *);
383	daddr &= ~(uintptr_t)(sizeof(void *)-1);
384	}
385
386    blkend = memnode_data(uintptr_t,m) + (uintptr_t)(m->length);
387
388    newm = (memnode_t *) daddr;
389
390    newm->next   = m->next;
391    m->length    = (unsigned int) (daddr - memnode_data(uintptr_t,m));
392    m->next      = newm;
393    m->status    = memnode_alloc;
394    newm->seal   = MEMNODE_SEAL;
395    newm->data    = memnode_data(unsigned char *,newm);
396    newm->length = (unsigned int) (blkend - memnode_data(uintptr_t,newm));
397    newm->status = memnode_free;
398
399    return m->data;
400}
401
402
403int kmemstats(mempool_t *pool,memstats_t *stats)
404{
405    memnode_t *m;
406    memnode_t **backptr;
407    uintptr_t daddr;
408
409    stats->mem_totalbytes = pool->length;
410    stats->mem_allocbytes = 0;
411    stats->mem_freebytes = 0;
412    stats->mem_allocnodes = 0;
413    stats->mem_freenodes = 0;
414    stats->mem_largest = 0;
415
416    for (m = pool->root; m; m = m->next) {
417	if (m->status) {
418	    stats->mem_allocnodes++;
419	    stats->mem_allocbytes += m->length;
420	    }
421	else {
422	    stats->mem_freenodes++;
423	    stats->mem_freebytes += m->length;
424	    if (m->length > stats->mem_largest) {
425		stats->mem_largest = m->length;
426		}
427	    }
428
429	daddr = memnode_data(uintptr_t,m);
430	if (m->seal != MEMNODE_SEAL) {
431	    return -1;
432	    }
433	if (m->next && ((daddr + m->length) != (uintptr_t) m->next)) {
434	    return -1;
435	    }
436	if (m->next && (m->next < m)) {
437	    return -1;
438	    }
439	if (m->data < (unsigned char *) m) {
440	    return -1;
441	    }
442	if (m->status == memnode_alloc) {
443	    backptr = (memnode_t **) (m->data - sizeof(void *));
444	    if (*backptr != m) {
445		return -1;
446		}
447	    }
448	}
449
450    return 0;
451}
452
453
454/*  *********************************************************************
455    *  kmemchk()
456    *
457    *  Check the consistency of the memory pool.
458    *
459    *  Input parameters:
460    *      pool - pool pointer
461    *
462    *  Return value:
463    *  	   0 - pool is consistent
464    *  	   -1 - pool is corrupt
465    ********************************************************************* */
466
467#ifdef TESTPROG
468int kmemchk(mempool_t *pool,int verbose)
469{
470    memnode_t *m;
471    memnode_t **backptr;
472    unsigned int daddr;
473
474    for (m = pool->root; m; m = m->next) {
475	if (verbose) {
476	    printf("%08X: Next=%08X  Len=%5u  %s  Data=%08X ",
477	       m,m->next,m->length,
478	       m->status ? "alloc" : "free ",
479	       m->data);
480	    }
481	daddr = memnode_data(uintptr_t,m);
482	if (m->seal != MEMNODE_SEAL) {
483	    if (verbose) printf("BadSeal ");
484	    else return -1;
485	    }
486	if (m->next && (daddr + m->length != (unsigned int) m->next)) {
487	    if (verbose) printf("BadLength ");
488	    else return -1;
489	    }
490	if (m->next && (m->next < m)) {
491	    if (verbose) printf("BadOrder ");
492	    else return -1;
493	    }
494	if (m->data < (unsigned char *) m) {
495	    if (verbose) printf("BadData ");
496	    else return -1;
497	    }
498	if (m->status == memnode_alloc) {
499	    backptr = (memnode_t **) (m->data - sizeof(void *));
500	    if (*backptr != m) {
501		if (verbose) printf("BadBackPtr ");
502		else return -1;
503		}
504	    }
505	if (verbose) printf("\n");
506	}
507
508    return 0;
509}
510
511
512#define MEMSIZE 1024*1024
513
514unsigned char *ptrs[4096];
515unsigned int sizes[4096];
516
517/*  *********************************************************************
518    *  main(argc,argv)
519    *
520    *  Test program for the memory allocator
521    *
522    *  Input parameters:
523    *  	   argc,argv
524    *
525    *  Return value:
526    *  	   nothing
527    ********************************************************************* */
528
529
530void main(int argc,char *argv[])
531{
532    unsigned char *mem;
533    int items = 0;
534    int idx;
535    int size;
536    int totalsize = 0;
537    int nfree,freecnt;
538    mempool_t *pool = &kmempool;
539
540    mem = malloc(MEMSIZE);
541    kmeminit(pool,mem,MEMSIZE);
542
543    items = 0;
544
545    for (;;) {
546
547	for (;;) {
548	    if (items == 4096) break;
549	    size = rand() % 1024;
550	    ptrs[items] = kmalloc(pool,size,1<<(rand() & 7));
551	    if (!ptrs[items]) break;
552	    sizes[items] = size;
553	    items++;
554	    totalsize += size;
555	    }
556
557	printf("%d items allocated, %d total bytes\n",items,totalsize);
558
559	if (kmemchk(pool,0) < 0) {
560	    kmemchk(pool,1);
561	    exit(1);
562	    }
563
564	/* Scramble the pointers */
565	idx = items - 1;
566
567	while (idx) {
568	    if (rand() & 2) {
569		mem = ptrs[0];
570		ptrs[0] = ptrs[idx];
571		ptrs[idx] = mem;
572
573		nfree = sizes[0];
574		sizes[0] = sizes[idx];
575		sizes[idx] = nfree;
576		}
577	    idx--;
578	    }
579
580	/* now free a random number of elements */
581
582	nfree = rand() % items;
583	freecnt = 0;
584
585	for (idx = nfree; idx < items; idx++) {
586	    kfree(pool,ptrs[idx]);
587	    totalsize -= sizes[idx];
588	    freecnt++;
589	    ptrs[idx] = NULL;
590	    sizes[idx] = 0;
591	    if (kmemchk(pool,0) < 0) {
592		kmemchk(pool,1);
593		exit(1);
594		}
595	    }
596
597	items -= freecnt;
598
599	printf(".");
600
601	}
602
603    kmemchk(pool,1);
604
605    exit(0);
606}
607
608#endif	 /* TESTPROG */
609