flatdevtree.c

来自「linux 内核源代码」· C语言 代码 · 共 1,037 行 · 第 1/2 页

/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * 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, 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 *
 * Copyright Pantelis Antoniou 2006
 * Copyright (C) IBM Corporation 2006
 *
 * Authors: Pantelis Antoniou <[e-mail]>
 *	    Hollis Blanchard <[e-mail]>
 *	    Mark A. Greer <[e-mail]>
 *	    Paul Mackerras <[e-mail]>
 */

#include <string.h>
#include <stddef.h>
#include "flatdevtree.h"
#include "flatdevtree_env.h"

#define _ALIGN(x, al)	(((x) + (al) - 1) & ~((al) - 1))

static char *ft_root_node(struct ft_cxt *cxt)
{
	return cxt->rgn[FT_STRUCT].start;
}

/* Routines for keeping node ptrs returned by ft_find_device current */
/* First entry not used b/c it would return 0 and be taken as NULL/error */
static void *ft_get_phandle(struct ft_cxt *cxt, char *node)
{
	unsigned int i;

	if (!node)
		return NULL;

	for (i = 1; i < cxt->nodes_used; i++)	/* already there? */
		if (cxt->node_tbl[i] == node)
			return (void *)i;

	if (cxt->nodes_used < cxt->node_max) {
		cxt->node_tbl[cxt->nodes_used] = node;
		return (void *)cxt->nodes_used++;
	}

	return NULL;
}

static char *ft_node_ph2node(struct ft_cxt *cxt, const void *phandle)
{
	unsigned int i = (unsigned int)phandle;

	if (i < cxt->nodes_used)
		return cxt->node_tbl[i];
	return NULL;
}

static void ft_node_update_before(struct ft_cxt *cxt, char *addr, int shift)
{
	unsigned int i;

	if (shift == 0)
		return;

	for (i = 1; i < cxt->nodes_used; i++)
		if (cxt->node_tbl[i] < addr)
			cxt->node_tbl[i] += shift;
}

static void ft_node_update_after(struct ft_cxt *cxt, char *addr, int shift)
{
	unsigned int i;

	if (shift == 0)
		return;

	for (i = 1; i < cxt->nodes_used; i++)
		if (cxt->node_tbl[i] >= addr)
			cxt->node_tbl[i] += shift;
}

/* Struct used to return info from ft_next() */
struct ft_atom {
	u32 tag;
	const char *name;
	void *data;
	u32 size;
};

/* Set ptrs to current one's info; return addr of next one */
static char *ft_next(struct ft_cxt *cxt, char *p, struct ft_atom *ret)
{
	u32 sz;

	if (p >= cxt->rgn[FT_STRUCT].start + cxt->rgn[FT_STRUCT].size)
		return NULL;

	ret->tag = be32_to_cpu(*(u32 *) p);
	p += 4;

	switch (ret->tag) {	/* Tag */
	case OF_DT_BEGIN_NODE:
		ret->name = p;
		ret->data = (void *)(p - 4);	/* start of node */
		p += _ALIGN(strlen(p) + 1, 4);
		break;
	case OF_DT_PROP:
		ret->size = sz = be32_to_cpu(*(u32 *) p);
		ret->name = cxt->str_anchor + be32_to_cpu(*(u32 *) (p + 4));
		ret->data = (void *)(p + 8);
		p += 8 + _ALIGN(sz, 4);
		break;
	case OF_DT_END_NODE:
	case OF_DT_NOP:
		break;
	case OF_DT_END:
	default:
		p = NULL;
		break;
	}

	return p;
}

#define HDR_SIZE	_ALIGN(sizeof(struct boot_param_header), 8)
#define EXPAND_INCR	1024	/* alloc this much extra when expanding */

/* Copy the tree to a newly-allocated region and put things in order */
static int ft_reorder(struct ft_cxt *cxt, int nextra)
{
	unsigned long tot;
	enum ft_rgn_id r;
	char *p, *pend;
	int stroff;

	tot = HDR_SIZE + EXPAND_INCR;
	for (r = FT_RSVMAP; r <= FT_STRINGS; ++r)
		tot += cxt->rgn[r].size;
	if (nextra > 0)
		tot += nextra;
	tot = _ALIGN(tot, 8);

	if (!cxt->realloc)
		return 0;
	p = cxt->realloc(NULL, tot);
	if (!p)
		return 0;

	memcpy(p, cxt->bph, sizeof(struct boot_param_header));
	/* offsets get fixed up later */

	cxt->bph = (struct boot_param_header *)p;
	cxt->max_size = tot;
	pend = p + tot;
	p += HDR_SIZE;

	memcpy(p, cxt->rgn[FT_RSVMAP].start, cxt->rgn[FT_RSVMAP].size);
	cxt->rgn[FT_RSVMAP].start = p;
	p += cxt->rgn[FT_RSVMAP].size;

	memcpy(p, cxt->rgn[FT_STRUCT].start, cxt->rgn[FT_STRUCT].size);
	ft_node_update_after(cxt, cxt->rgn[FT_STRUCT].start,
			p - cxt->rgn[FT_STRUCT].start);
	cxt->p += p - cxt->rgn[FT_STRUCT].start;
	cxt->rgn[FT_STRUCT].start = p;

	p = pend - cxt->rgn[FT_STRINGS].size;
	memcpy(p, cxt->rgn[FT_STRINGS].start, cxt->rgn[FT_STRINGS].size);
	stroff = cxt->str_anchor - cxt->rgn[FT_STRINGS].start;
	cxt->rgn[FT_STRINGS].start = p;
	cxt->str_anchor = p + stroff;

	cxt->isordered = 1;
	return 1;
}

static inline char *prev_end(struct ft_cxt *cxt, enum ft_rgn_id r)
{
	if (r > FT_RSVMAP)
		return cxt->rgn[r - 1].start + cxt->rgn[r - 1].size;
	return (char *)cxt->bph + HDR_SIZE;
}

static inline char *next_start(struct ft_cxt *cxt, enum ft_rgn_id r)
{
	if (r < FT_STRINGS)
		return cxt->rgn[r + 1].start;
	return (char *)cxt->bph + cxt->max_size;
}

/*
 * See if we can expand region rgn by nextra bytes by using up
 * free space after or before the region.
 */
static int ft_shuffle(struct ft_cxt *cxt, char **pp, enum ft_rgn_id rgn,
		int nextra)
{
	char *p = *pp;
	char *rgn_start, *rgn_end;

	rgn_start = cxt->rgn[rgn].start;
	rgn_end = rgn_start + cxt->rgn[rgn].size;
	if (nextra <= 0 || rgn_end + nextra <= next_start(cxt, rgn)) {
		/* move following stuff */
		if (p < rgn_end) {
			if (nextra < 0)
				memmove(p, p - nextra, rgn_end - p + nextra);
			else
				memmove(p + nextra, p, rgn_end - p);
			if (rgn == FT_STRUCT)
				ft_node_update_after(cxt, p, nextra);
		}
		cxt->rgn[rgn].size += nextra;
		if (rgn == FT_STRINGS)
			/* assumes strings only added at beginning */
			cxt->str_anchor += nextra;
		return 1;
	}
	if (prev_end(cxt, rgn) <= rgn_start - nextra) {
		/* move preceding stuff */
		if (p > rgn_start) {
			memmove(rgn_start - nextra, rgn_start, p - rgn_start);
			if (rgn == FT_STRUCT)
				ft_node_update_before(cxt, p, -nextra);
		}
		*pp -= nextra;
		cxt->rgn[rgn].start -= nextra;
		cxt->rgn[rgn].size += nextra;
		return 1;
	}
	return 0;
}

static int ft_make_space(struct ft_cxt *cxt, char **pp, enum ft_rgn_id rgn,
			 int nextra)
{
	unsigned long size, ssize, tot;
	char *str, *next;
	enum ft_rgn_id r;

	if (!cxt->isordered) {
		unsigned long rgn_off = *pp - cxt->rgn[rgn].start;

		if (!ft_reorder(cxt, nextra))
			return 0;

		*pp = cxt->rgn[rgn].start + rgn_off;
	}
	if (ft_shuffle(cxt, pp, rgn, nextra))
		return 1;

	/* See if there is space after the strings section */
	ssize = cxt->rgn[FT_STRINGS].size;
	if (cxt->rgn[FT_STRINGS].start + ssize
			< (char *)cxt->bph + cxt->max_size) {
		/* move strings up as far as possible */
		str = (char *)cxt->bph + cxt->max_size - ssize;
		cxt->str_anchor += str - cxt->rgn[FT_STRINGS].start;
		memmove(str, cxt->rgn[FT_STRINGS].start, ssize);
		cxt->rgn[FT_STRINGS].start = str;
		/* enough space now? */
		if (rgn >= FT_STRUCT && ft_shuffle(cxt, pp, rgn, nextra))
			return 1;
	}

	/* how much total free space is there following this region? */
	tot = 0;
	for (r = rgn; r < FT_STRINGS; ++r) {
		char *r_end = cxt->rgn[r].start + cxt->rgn[r].size;
		tot += next_start(cxt, rgn) - r_end;
	}

	/* cast is to shut gcc up; we know nextra >= 0 */
	if (tot < (unsigned int)nextra) {
		/* have to reallocate */
		char *newp, *new_start;
		int shift;

		if (!cxt->realloc)
			return 0;
		size = _ALIGN(cxt->max_size + (nextra - tot) + EXPAND_INCR, 8);
		newp = cxt->realloc(cxt->bph, size);
		if (!newp)
			return 0;
		cxt->max_size = size;
		shift = newp - (char *)cxt->bph;

		if (shift) { /* realloc can return same addr */
			cxt->bph = (struct boot_param_header *)newp;
			ft_node_update_after(cxt, cxt->rgn[FT_STRUCT].start,
					shift);
			for (r = FT_RSVMAP; r <= FT_STRINGS; ++r) {
				new_start = cxt->rgn[r].start + shift;
				cxt->rgn[r].start = new_start;
			}
			*pp += shift;
			cxt->str_anchor += shift;
		}

		/* move strings up to the end */
		str = newp + size - ssize;
		cxt->str_anchor += str - cxt->rgn[FT_STRINGS].start;
		memmove(str, cxt->rgn[FT_STRINGS].start, ssize);
		cxt->rgn[FT_STRINGS].start = str;

		if (ft_shuffle(cxt, pp, rgn, nextra))
			return 1;
	}

	/* must be FT_RSVMAP and we need to move FT_STRUCT up */
	if (rgn == FT_RSVMAP) {
		next = cxt->rgn[FT_RSVMAP].start + cxt->rgn[FT_RSVMAP].size
			+ nextra;
		ssize = cxt->rgn[FT_STRUCT].size;
		if (next + ssize >= cxt->rgn[FT_STRINGS].start)
			return 0;	/* "can't happen" */
		memmove(next, cxt->rgn[FT_STRUCT].start, ssize);
		ft_node_update_after(cxt, cxt->rgn[FT_STRUCT].start, nextra);
		cxt->rgn[FT_STRUCT].start = next;

		if (ft_shuffle(cxt, pp, rgn, nextra))
			return 1;
	}

	return 0;		/* "can't happen" */
}

static void ft_put_word(struct ft_cxt *cxt, u32 v)
{
	*(u32 *) cxt->p = cpu_to_be32(v);
	cxt->p += 4;
}

static void ft_put_bin(struct ft_cxt *cxt, const void *data, unsigned int sz)
{
	unsigned long sza = _ALIGN(sz, 4);

	/* zero out the alignment gap if necessary */
	if (sz < sza)
		*(u32 *) (cxt->p + sza - 4) = 0;

	/* copy in the data */
	memcpy(cxt->p, data, sz);

	cxt->p += sza;
}

char *ft_begin_node(struct ft_cxt *cxt, const char *name)
{
	unsigned long nlen = strlen(name) + 1;
	unsigned long len = 8 + _ALIGN(nlen, 4);
	char *ret;

	if (!ft_make_space(cxt, &cxt->p, FT_STRUCT, len))
		return NULL;

	ret = cxt->p;

	ft_put_word(cxt, OF_DT_BEGIN_NODE);
	ft_put_bin(cxt, name, strlen(name) + 1);

	return ret;
}

void ft_end_node(struct ft_cxt *cxt)
{
	ft_put_word(cxt, OF_DT_END_NODE);
}

void ft_nop(struct ft_cxt *cxt)
{
	if (ft_make_space(cxt, &cxt->p, FT_STRUCT, 4))
		ft_put_word(cxt, OF_DT_NOP);
}

#define NO_STRING	0x7fffffff

static int lookup_string(struct ft_cxt *cxt, const char *name)
{
	char *p, *end;

	p = cxt->rgn[FT_STRINGS].start;
	end = p + cxt->rgn[FT_STRINGS].size;
	while (p < end) {
		if (strcmp(p, (char *)name) == 0)
			return p - cxt->str_anchor;
		p += strlen(p) + 1;
	}

	return NO_STRING;
}

/* lookup string and insert if not found */
static int map_string(struct ft_cxt *cxt, const char *name)
{
	int off;
	char *p;

	off = lookup_string(cxt, name);
	if (off != NO_STRING)
		return off;
	p = cxt->rgn[FT_STRINGS].start;
	if (!ft_make_space(cxt, &p, FT_STRINGS, strlen(name) + 1))
		return NO_STRING;
	strcpy(p, name);
	return p - cxt->str_anchor;
}

int ft_prop(struct ft_cxt *cxt, const char *name, const void *data,
		unsigned int sz)
{
	int off, len;

	off = map_string(cxt, name);
	if (off == NO_STRING)
		return -1;

	len = 12 + _ALIGN(sz, 4);
	if (!ft_make_space(cxt, &cxt->p, FT_STRUCT, len))
		return -1;

	ft_put_word(cxt, OF_DT_PROP);
	ft_put_word(cxt, sz);
	ft_put_word(cxt, off);
	ft_put_bin(cxt, data, sz);
	return 0;
}

int ft_prop_str(struct ft_cxt *cxt, const char *name, const char *str)
{
	return ft_prop(cxt, name, str, strlen(str) + 1);
}

int ft_prop_int(struct ft_cxt *cxt, const char *name, unsigned int val)
{
	u32 v = cpu_to_be32((u32) val);

	return ft_prop(cxt, name, &v, 4);
}

/* Calculate the size of the reserved map */
static unsigned long rsvmap_size(struct ft_cxt *cxt)
{
	struct ft_reserve *res;

	res = (struct ft_reserve *)cxt->rgn[FT_RSVMAP].start;
	while (res->start || res->len)
		++res;
	return (char *)(res + 1) - cxt->rgn[FT_RSVMAP].start;
}

/* Calculate the size of the struct region by stepping through it */
static unsigned long struct_size(struct ft_cxt *cxt)
{
	char *p = cxt->rgn[FT_STRUCT].start;
	char *next;
	struct ft_atom atom;

	/* make check in ft_next happy */
	if (cxt->rgn[FT_STRUCT].size == 0)
		cxt->rgn[FT_STRUCT].size = 0xfffffffful - (unsigned long)p;

	while ((next = ft_next(cxt, p, &atom)) != NULL)
		p = next;
	return p + 4 - cxt->rgn[FT_STRUCT].start;
}

/* add `adj' on to all string offset values in the struct area */
static void adjust_string_offsets(struct ft_cxt *cxt, int adj)
{
	char *p = cxt->rgn[FT_STRUCT].start;
	char *next;
	struct ft_atom atom;
	int off;

	while ((next = ft_next(cxt, p, &atom)) != NULL) {
		if (atom.tag == OF_DT_PROP) {
			off = be32_to_cpu(*(u32 *) (p + 8));
			*(u32 *) (p + 8) = cpu_to_be32(off + adj);
		}
		p = next;
	}
}

/* start construction of the flat OF tree from scratch */
void ft_begin(struct ft_cxt *cxt, void *blob, unsigned int max_size,
		void *(*realloc_fn) (void *, unsigned long))
{
	struct boot_param_header *bph = blob;
	char *p;
	struct ft_reserve *pres;

	/* clear the cxt */
	memset(cxt, 0, sizeof(*cxt));

	cxt->bph = bph;
	cxt->max_size = max_size;
	cxt->realloc = realloc_fn;
	cxt->isordered = 1;

	/* zero everything in the header area */
	memset(bph, 0, sizeof(*bph));

	bph->magic = cpu_to_be32(OF_DT_HEADER);
	bph->version = cpu_to_be32(0x10);
	bph->last_comp_version = cpu_to_be32(0x10);

	/* start pointers */
	cxt->rgn[FT_RSVMAP].start = p = blob + HDR_SIZE;
	cxt->rgn[FT_RSVMAP].size = sizeof(struct ft_reserve);