io/prom/prommem.cc

/*
 *      PearPC
 *      prommem.cc
 *
 *      Copyright (C) 2003 Sebastian Biallas (sb@biallas.net)
 *
 *      This program is free software; you can redistribute it and/or modify
 *      it under the terms of the GNU General Public License version 2 as
 *      published by the Free Software Foundation.
 *
 *      This program 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 General Public License for more details.
 *
 *      You should have received a copy of the GNU General Public License
 *      along with this program; if not, write to the Free Software
 *      Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

#include <cstdlib>
#include <cstring>

#include "tools/snprintf.h"
#include "debug/tracers.h"
#include "cpu/mem.h"
#include "prom.h"
#include "promosi.h"
#include "prommem.h"

/*char *prom_ea_string(uint32 ea)
{
        byte *r;
        if (ppc_direct_effective_memory_handle(ea, r)) {
                IO_PROM_ERR("nciht gut\n");
                return NULL;
        }
        return (char*)r;
}*/

bool prom_get_string(String &result, uint32 ea)
{
        uint32 pa;
        result = "";
        if (!ppc_prom_effective_to_physical(pa, ea)) {
                IO_PROM_ERR("can't translate address in %s\n", __FUNCTION__);
                return false;
        }
        while (1) {
                byte mem[128];
                if (!ppc_dma_read(mem, pa, sizeof mem)) {
                        IO_PROM_ERR("read memory in %s\n", __FUNCTION__);
                        return false;
                }
                byte *end;
                if ((end = (byte *)memchr(mem, 0, sizeof mem))) {
                        if (end != mem) {
                                String s(mem, end-mem);
                                result += s;
                        }
                        return true;
                } else {
                        String s(mem, sizeof mem);
                        result += s;
                }
                pa += sizeof mem;
        }
}

static byte *gPhysMemoryUsed;
static int gPhysMemoryLastpage;

static int gPromMemStart, gPromMemEnd;

struct malloc_entry {
        uint32 prev PACKED;
        uint32 size PACKED;
};

#define MALLOC_BLOCK_FREE (1<<31)
#define MALLOC_BLOCK_GUARD (1)

static uint32 gPromMemFreeBlock;
static uint32 gPromMemLastBlock;

bool prom_claim_page(uint32 phys)
{
        uint32 page = phys / 4096;
        if (gPhysMemoryUsed[page / 8] & (1<<(page&0x7))) {
                IO_PROM_WARN("%08x in use!\n", phys);
                return false;
        }
        gPhysMemoryUsed[page / 8] |= (1<<(page&0x7));
        return true;
}

bool prom_claim_pages(uint32 phys, uint32 size)
{
        sint32 s=size;
        while (s > 0) {
                if (!prom_claim_page(phys)) return false;
                phys += 4096;
                s -= 4096;
        }
        return true;
}

extern uint32 gMemorySize; // GRRR

uint32 prom_get_free_page()
{
        int s = gMemorySize/4096/8+1;
        for (int i=gPhysMemoryLastpage; i<s; i++) {
                if (gPhysMemoryUsed[i]!=0xff) {
                        int x=1;
                        for (int j=0; j<8; j++) {
                                if (!(gPhysMemoryUsed[i] & x)) {
                                        uint32 pa = (i*8+j)*4096;
                                        prom_claim_page(pa);
                                        gPhysMemoryLastpage = i;
                                        return pa;
                                }
                                x<<=1;
                        }
                }
        }
        return 0;
}

uint32 prom_allocate_virt(uint32 size, uint32 align)
{
        return 0;
}

uint32 prom_allocate_mem(uint32 size, uint32 align, uint32 virt)
{
        uint32 ret;
        if (virt == 0) {
                ret = prom_mem_malloc(size+align-1);
                if (ret % align) {
                        ret += align - (ret % align);
                }
        } else {
                int pages = (size / 4096) + ((size % 4096)?1:0);
                ret = virt;
                for (int i=0; i<pages; i++) {
                        // test if phys==virtual mapping possible
                        if ((virt >= gMemorySize) ||
                            (gPhysMemoryUsed[virt/4096/8] & (1 << ((virt/4096) & 7)))) {
                                uint32 pa = prom_get_free_page();
                                if (!pa) return (uint32) -1;
                                ppc_prom_page_create(virt, pa);
                        } else {
                                prom_claim_page(virt);
                                ppc_prom_page_create(virt, virt);
                        }
                        virt+=4096;
                }
        }
        return ret;
}

bool prom_free_mem(uint32 virt)
{
        return false;
}

void prom_mem_set(uint32 pa, int c, int size)
{
        if (!ppc_dma_set(pa, c, size)) {
                IO_PROM_ERR("in mem_set\n");
        }
}


static uint32 prom_mem_entry_get_size(uint32 pa)
{
        uint32 r;
        ppc_dma_read(&r, pa, 4);
        return r;
}

static uint32 prom_mem_entry_get_prev(uint32 pa)
{
        uint32 r;
        ppc_dma_read(&r, pa+4, 4);
        return r;
}

static void prom_mem_entry_set_size(uint32 pa, uint32 v)
{
        ppc_dma_write(pa, &v, 4);
}

static void prom_mem_entry_set_prev(uint32 pa, uint32 v)
{
        ppc_dma_write(pa+4, &v, 4);
}

uint32 prom_mem_malloc(uint32 size)
{
        if (!size) {
                IO_PROM_ERR("zero byte allocation!\n");
        }
        size = (size+7) & ~7;
        size += sizeof(malloc_entry);

//      ht_printf(" --> %d\n", size);
        
        int i=0;
        bool ok=false;
        uint32 r = 0;
        while (!ok) {
                uint32 s = prom_mem_entry_get_size(gPromMemFreeBlock) & ~MALLOC_BLOCK_FREE;
//              ht_printf("s: %08x\n", s);
                if (s > size) {
                        // found block
                        uint32 blockp = gPromMemFreeBlock + s - size;
//                      ht_printf("blockp: %08x\n", blockp);
                        
                        uint32 block = blockp;
                        uint32 next = gPromMemFreeBlock+s;
                        
                        prom_mem_entry_set_size(gPromMemFreeBlock, (s - size) | MALLOC_BLOCK_FREE);
                        prom_mem_entry_set_prev(next, blockp);
                        prom_mem_entry_set_prev(blockp, gPromMemFreeBlock);
                        prom_mem_entry_set_size(blockp, size);                                          
                        
//                      ht_printf("malloced at %x\n", blockp+sizeof(malloc_entry));
                        return blockp+sizeof(malloc_entry);
                }
                if (s == size) {
                        // exact match
                        prom_mem_entry_set_size(gPromMemFreeBlock, s);
                        r = gPromMemFreeBlock + sizeof(malloc_entry);
                        ok = true;
                }
                do {
                        gPromMemFreeBlock = prom_mem_entry_get_prev(gPromMemFreeBlock);
                        if (prom_mem_entry_get_size(gPromMemFreeBlock) & MALLOC_BLOCK_GUARD) {
                                if (i) {
                                        IO_PROM_ERR("out of memory!\n");
                                }
                                i++;
                                gPromMemFreeBlock = prom_mem_entry_get_prev(gPromMemLastBlock);
                        }
                } while (!(prom_mem_entry_get_size(gPromMemFreeBlock) & MALLOC_BLOCK_FREE));
        }
        return r;
}

void prom_mem_free(uint32 p)
{
        uint32 block = p - sizeof(malloc_entry);
        if (prom_mem_entry_get_size(block) & MALLOC_BLOCK_FREE) {       
                IO_PROM_ERR("attempt to free unused block!\n");
        }
        if (prom_mem_entry_get_size(block) & MALLOC_BLOCK_GUARD) {
                IO_PROM_ERR("attempt to free guard block!\n");
        }
        uint32 prev = prom_mem_entry_get_prev(block);
        uint32 next = block + prom_mem_entry_get_size(block);
        if ((prom_mem_entry_get_size(next) & MALLOC_BLOCK_FREE) && !(prom_mem_entry_get_size(next) & MALLOC_BLOCK_GUARD)) {
                // merge with upper block
                uint32 nnext = block + prom_mem_entry_get_size(block) + (prom_mem_entry_get_size(next) & ~MALLOC_BLOCK_FREE);
                prom_mem_entry_set_prev(nnext, block);
                prom_mem_entry_set_size(block, prom_mem_entry_get_size(next) & ~MALLOC_BLOCK_FREE);
        }
        if ((prom_mem_entry_get_size(prev) & MALLOC_BLOCK_FREE) && !(prom_mem_entry_get_size(prev) & MALLOC_BLOCK_GUARD)) {
                // merge with lower block
                prom_mem_entry_set_size(prev, prom_mem_entry_get_size(prev) + prom_mem_entry_get_size(block));
                prom_mem_entry_set_prev(next, prom_mem_entry_get_prev(block));
                gPromMemFreeBlock = prev;
        } else {
                prom_mem_entry_set_size(block, prom_mem_entry_get_size(block) | MALLOC_BLOCK_FREE);
                gPromMemFreeBlock = block;
        }
}

uint32 prom_mem_virt_to_phys(uint32 v)
{
        return v+PROM_MEM_SIZE+gPromMemStart;
}

uint32 prom_mem_phys_to_virt(uint32 p)
{
        return p-gPromMemStart+(0-PROM_MEM_SIZE);
}

bool prom_mem_init()
{
        gPhysMemoryUsed = new byte[gMemorySize / 4096 / 8+1];
        memset(gPhysMemoryUsed, 0, gMemorySize / 4096 / 8+1);
        gPhysMemoryLastpage = 1;

        /*
         * The Prom-Memory (where the device-tree etc. resides) will be at the
         * end of the physical and virtual memory
         */

        gPromMemStart = gMemorySize - PROM_MEM_SIZE;
        gPromMemEnd = gMemorySize;
        uint32 v = 0-PROM_MEM_SIZE;
        // Allocate the physical pages
        for (int i=gPromMemStart; i<gPromMemEnd; i+=4096) {
                prom_claim_page(i);
                ppc_prom_page_create(v, i);
                v+=4096;
        }

//      ht_printf("gPromMemStart: %x\ngPromMemEnd: %x\n", gPromMemStart, gPromMemEnd);
        
        uint32 start = gPromMemStart;
        uint32 end = gPromMemEnd - sizeof(malloc_entry);
        uint32 mem = gPromMemStart + sizeof(malloc_entry);

        prom_mem_entry_set_size(start, sizeof(malloc_entry) | MALLOC_BLOCK_GUARD);
        prom_mem_entry_set_size(mem, (PROM_MEM_SIZE-2*sizeof(malloc_entry)) | MALLOC_BLOCK_FREE);
        prom_mem_entry_set_size(end, sizeof(malloc_entry) | MALLOC_BLOCK_GUARD);

        prom_mem_entry_set_prev(start, gPromMemEnd-sizeof(malloc_entry));
        prom_mem_entry_set_prev(end, gPromMemStart+sizeof(malloc_entry));
        prom_mem_entry_set_prev(mem, gPromMemStart);

        gPromMemLastBlock = end;
        gPromMemFreeBlock = mem;

        /*
         *      malloc works now
         */

#define DW(w) (w)>>24, (w)>>16, (w)>>8, (w)>>0, 
        uint8 magic_opcode[] = {
                DW(PROM_MAGIC_OPCODE)
                DW(0x4e800020)  // blr
        };
        gPromOSIEntry = prom_mem_malloc(sizeof magic_opcode);
        ppc_dma_write(gPromOSIEntry, &magic_opcode, sizeof magic_opcode);
        gPromOSIEntry = prom_mem_phys_to_virt(gPromOSIEntry);
        return true;
}

bool prom_mem_done()
{
        delete[] gPhysMemoryUsed;
        gPhysMemoryUsed = NULL;
        return true;
}
1	dpavlin	1	/*
2			* PearPC
3			* prommem.cc
4			*
5			* Copyright (C) 2003 Sebastian Biallas (sb@biallas.net)
6			*
7			* This program is free software; you can redistribute it and/or modify
8			* it under the terms of the GNU General Public License version 2 as
9			* published by the Free Software Foundation.
10			*
11			* This program is distributed in the hope that it will be useful,
12			* but WITHOUT ANY WARRANTY; without even the implied warranty of
13			* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14			* GNU General Public License for more details.
15			*
16			* You should have received a copy of the GNU General Public License
17			* along with this program; if not, write to the Free Software
18			* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
19			*/
20
21			#include <cstdlib>
22			#include <cstring>
23
24			#include "tools/snprintf.h"
25			#include "debug/tracers.h"
26			#include "cpu/mem.h"
27			#include "prom.h"
28			#include "promosi.h"
29			#include "prommem.h"
30
31			/char prom_ea_string(uint32 ea)
32			{
33			byte *r;
34			if (ppc_direct_effective_memory_handle(ea, r)) {
35			IO_PROM_ERR("nciht gut\n");
36			return NULL;
37			}
38			return (char*)r;
39			}*/
40
41			bool prom_get_string(String &result, uint32 ea)
42			{
43			uint32 pa;
44			result = "";
45			if (!ppc_prom_effective_to_physical(pa, ea)) {
46			IO_PROM_ERR("can't translate address in %s\n", __FUNCTION__);
47			return false;
48			}
49			while (1) {
50			byte mem[128];
51			if (!ppc_dma_read(mem, pa, sizeof mem)) {
52			IO_PROM_ERR("read memory in %s\n", __FUNCTION__);
53			return false;
54			}
55			byte *end;
56			if ((end = (byte *)memchr(mem, 0, sizeof mem))) {
57			if (end != mem) {
58			String s(mem, end-mem);
59			result += s;
60			}
61			return true;
62			} else {
63			String s(mem, sizeof mem);
64			result += s;
65			}
66			pa += sizeof mem;
67			}
68			}
69
70			static byte *gPhysMemoryUsed;
71			static int gPhysMemoryLastpage;
72
73			static int gPromMemStart, gPromMemEnd;
74
75			struct malloc_entry {
76			uint32 prev PACKED;
77			uint32 size PACKED;
78			};
79
80			#define MALLOC_BLOCK_FREE (1<<31)
81			#define MALLOC_BLOCK_GUARD (1)
82
83			static uint32 gPromMemFreeBlock;
84			static uint32 gPromMemLastBlock;
85
86			bool prom_claim_page(uint32 phys)
87			{
88			uint32 page = phys / 4096;
89			if (gPhysMemoryUsed[page / 8] & (1<<(page&0x7))) {
90			IO_PROM_WARN("%08x in use!\n", phys);
91			return false;
92			}
93			gPhysMemoryUsed[page / 8] \|= (1<<(page&0x7));
94			return true;
95			}
96
97			bool prom_claim_pages(uint32 phys, uint32 size)
98			{
99			sint32 s=size;
100			while (s > 0) {
101			if (!prom_claim_page(phys)) return false;
102			phys += 4096;
103			s -= 4096;
104			}
105			return true;
106			}
107
108			extern uint32 gMemorySize; // GRRR
109
110			uint32 prom_get_free_page()
111			{
112			int s = gMemorySize/4096/8+1;
113			for (int i=gPhysMemoryLastpage; i<s; i++) {
114			if (gPhysMemoryUsed[i]!=0xff) {
115			int x=1;
116			for (int j=0; j<8; j++) {
117			if (!(gPhysMemoryUsed[i] & x)) {
118			uint32 pa = (i8+j)4096;
119			prom_claim_page(pa);
120			gPhysMemoryLastpage = i;
121			return pa;
122			}
123			x<<=1;
124			}
125			}
126			}
127			return 0;
128			}
129
130			uint32 prom_allocate_virt(uint32 size, uint32 align)
131			{
132			return 0;
133			}
134
135			uint32 prom_allocate_mem(uint32 size, uint32 align, uint32 virt)
136			{
137			uint32 ret;
138			if (virt == 0) {
139			ret = prom_mem_malloc(size+align-1);
140			if (ret % align) {
141			ret += align - (ret % align);
142			}
143			} else {
144			int pages = (size / 4096) + ((size % 4096)?1:0);
145			ret = virt;
146			for (int i=0; i<pages; i++) {
147			// test if phys==virtual mapping possible
148			if ((virt >= gMemorySize) \|\|
149			(gPhysMemoryUsed[virt/4096/8] & (1 << ((virt/4096) & 7)))) {
150			uint32 pa = prom_get_free_page();
151			if (!pa) return (uint32) -1;
152			ppc_prom_page_create(virt, pa);
153			} else {
154			prom_claim_page(virt);
155			ppc_prom_page_create(virt, virt);
156			}
157			virt+=4096;
158			}
159			}
160			return ret;
161			}
162
163			bool prom_free_mem(uint32 virt)
164			{
165			return false;
166			}
167
168			void prom_mem_set(uint32 pa, int c, int size)
169			{
170			if (!ppc_dma_set(pa, c, size)) {
171			IO_PROM_ERR("in mem_set\n");
172			}
173			}
174
175
176			static uint32 prom_mem_entry_get_size(uint32 pa)
177			{
178			uint32 r;
179			ppc_dma_read(&r, pa, 4);
180			return r;
181			}
182
183			static uint32 prom_mem_entry_get_prev(uint32 pa)
184			{
185			uint32 r;
186			ppc_dma_read(&r, pa+4, 4);
187			return r;
188			}
189
190			static void prom_mem_entry_set_size(uint32 pa, uint32 v)
191			{
192			ppc_dma_write(pa, &v, 4);
193			}
194
195			static void prom_mem_entry_set_prev(uint32 pa, uint32 v)
196			{
197			ppc_dma_write(pa+4, &v, 4);
198			}
199
200			uint32 prom_mem_malloc(uint32 size)
201			{
202			if (!size) {
203			IO_PROM_ERR("zero byte allocation!\n");
204			}
205			size = (size+7) & ~7;
206			size += sizeof(malloc_entry);
207
208			// ht_printf(" --> %d\n", size);
209
210			int i=0;
211			bool ok=false;
212			uint32 r = 0;
213			while (!ok) {
214			uint32 s = prom_mem_entry_get_size(gPromMemFreeBlock) & ~MALLOC_BLOCK_FREE;
215			// ht_printf("s: %08x\n", s);
216			if (s > size) {
217			// found block
218			uint32 blockp = gPromMemFreeBlock + s - size;
219			// ht_printf("blockp: %08x\n", blockp);
220
221			uint32 block = blockp;
222			uint32 next = gPromMemFreeBlock+s;
223
224			prom_mem_entry_set_size(gPromMemFreeBlock, (s - size) \| MALLOC_BLOCK_FREE);
225			prom_mem_entry_set_prev(next, blockp);
226			prom_mem_entry_set_prev(blockp, gPromMemFreeBlock);
227			prom_mem_entry_set_size(blockp, size);
228
229			// ht_printf("malloced at %x\n", blockp+sizeof(malloc_entry));
230			return blockp+sizeof(malloc_entry);
231			}
232			if (s == size) {
233			// exact match
234			prom_mem_entry_set_size(gPromMemFreeBlock, s);
235			r = gPromMemFreeBlock + sizeof(malloc_entry);
236			ok = true;
237			}
238			do {
239			gPromMemFreeBlock = prom_mem_entry_get_prev(gPromMemFreeBlock);
240			if (prom_mem_entry_get_size(gPromMemFreeBlock) & MALLOC_BLOCK_GUARD) {
241			if (i) {
242			IO_PROM_ERR("out of memory!\n");
243			}
244			i++;
245			gPromMemFreeBlock = prom_mem_entry_get_prev(gPromMemLastBlock);
246			}
247			} while (!(prom_mem_entry_get_size(gPromMemFreeBlock) & MALLOC_BLOCK_FREE));
248			}
249			return r;
250			}
251
252			void prom_mem_free(uint32 p)
253			{
254			uint32 block = p - sizeof(malloc_entry);
255			if (prom_mem_entry_get_size(block) & MALLOC_BLOCK_FREE) {
256			IO_PROM_ERR("attempt to free unused block!\n");
257			}
258			if (prom_mem_entry_get_size(block) & MALLOC_BLOCK_GUARD) {
259			IO_PROM_ERR("attempt to free guard block!\n");
260			}
261			uint32 prev = prom_mem_entry_get_prev(block);
262			uint32 next = block + prom_mem_entry_get_size(block);
263			if ((prom_mem_entry_get_size(next) & MALLOC_BLOCK_FREE) && !(prom_mem_entry_get_size(next) & MALLOC_BLOCK_GUARD)) {
264			// merge with upper block
265			uint32 nnext = block + prom_mem_entry_get_size(block) + (prom_mem_entry_get_size(next) & ~MALLOC_BLOCK_FREE);
266			prom_mem_entry_set_prev(nnext, block);
267			prom_mem_entry_set_size(block, prom_mem_entry_get_size(next) & ~MALLOC_BLOCK_FREE);
268			}
269			if ((prom_mem_entry_get_size(prev) & MALLOC_BLOCK_FREE) && !(prom_mem_entry_get_size(prev) & MALLOC_BLOCK_GUARD)) {
270			// merge with lower block
271			prom_mem_entry_set_size(prev, prom_mem_entry_get_size(prev) + prom_mem_entry_get_size(block));
272			prom_mem_entry_set_prev(next, prom_mem_entry_get_prev(block));
273			gPromMemFreeBlock = prev;
274			} else {
275			prom_mem_entry_set_size(block, prom_mem_entry_get_size(block) \| MALLOC_BLOCK_FREE);
276			gPromMemFreeBlock = block;
277			}
278			}
279
280			uint32 prom_mem_virt_to_phys(uint32 v)
281			{
282			return v+PROM_MEM_SIZE+gPromMemStart;
283			}
284
285			uint32 prom_mem_phys_to_virt(uint32 p)
286			{
287			return p-gPromMemStart+(0-PROM_MEM_SIZE);
288			}
289
290			bool prom_mem_init()
291			{
292			gPhysMemoryUsed = new byte[gMemorySize / 4096 / 8+1];
293			memset(gPhysMemoryUsed, 0, gMemorySize / 4096 / 8+1);
294			gPhysMemoryLastpage = 1;
295
296			/*
297			* The Prom-Memory (where the device-tree etc. resides) will be at the
298			* end of the physical and virtual memory
299			*/
300
301			gPromMemStart = gMemorySize - PROM_MEM_SIZE;
302			gPromMemEnd = gMemorySize;
303			uint32 v = 0-PROM_MEM_SIZE;
304			// Allocate the physical pages
305			for (int i=gPromMemStart; i<gPromMemEnd; i+=4096) {
306			prom_claim_page(i);
307			ppc_prom_page_create(v, i);
308			v+=4096;
309			}
310
311			// ht_printf("gPromMemStart: %x\ngPromMemEnd: %x\n", gPromMemStart, gPromMemEnd);
312
313			uint32 start = gPromMemStart;
314			uint32 end = gPromMemEnd - sizeof(malloc_entry);
315			uint32 mem = gPromMemStart + sizeof(malloc_entry);
316
317			prom_mem_entry_set_size(start, sizeof(malloc_entry) \| MALLOC_BLOCK_GUARD);
318			prom_mem_entry_set_size(mem, (PROM_MEM_SIZE-2*sizeof(malloc_entry)) \| MALLOC_BLOCK_FREE);
319			prom_mem_entry_set_size(end, sizeof(malloc_entry) \| MALLOC_BLOCK_GUARD);
320
321			prom_mem_entry_set_prev(start, gPromMemEnd-sizeof(malloc_entry));
322			prom_mem_entry_set_prev(end, gPromMemStart+sizeof(malloc_entry));
323			prom_mem_entry_set_prev(mem, gPromMemStart);
324
325			gPromMemLastBlock = end;
326			gPromMemFreeBlock = mem;
327
328			/*
329			* malloc works now
330			*/
331
332			#define DW(w) (w)>>24, (w)>>16, (w)>>8, (w)>>0,
333			uint8 magic_opcode[] = {
334			DW(PROM_MAGIC_OPCODE)
335			DW(0x4e800020) // blr
336			};
337			gPromOSIEntry = prom_mem_malloc(sizeof magic_opcode);
338			ppc_dma_write(gPromOSIEntry, &magic_opcode, sizeof magic_opcode);
339			gPromOSIEntry = prom_mem_phys_to_virt(gPromOSIEntry);
340			return true;
341			}
342
343			bool prom_mem_done()
344			{
345			delete[] gPhysMemoryUsed;
346			gPhysMemoryUsed = NULL;
347			return true;
348			}