of-devtree.c

xen 3.2.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 (C) IBM Corp. 2005
 *
 * Authors: Jimi Xenidis <[e-mail]>
 */

/* WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING
 * This code is intended to be used but relocatable routines So PLEASE
 * do not place any global data here including const integrals or
 * literals.
 * The local assert() is ok for string literal usage.. but thats it.
 */


#include <xen/config.h>
#include <xen/init.h>
#include <xen/lib.h>
#include "of-devtree.h"

static int (*ofd_write)(const char *, size_t len) = NULL;

void ofd_init(int (*write)(const char *, size_t len))
{
    ofd_write = write;
}
                  

static void ofd_stop(void)
{
    for ( ; ; ) ;
}

/* this is so it can be called from anywhere */
static void ofd_assprint(int line)
{
    char a[13];
    char num[20];
    int i;

    a[0]  = '\n';
    a[1]  = '\n';
    a[2]  = 'O';
    a[3]  = 'F';
    a[4]  = 'D';
    a[5]  = ':';
    a[6]  = 'A';
    a[7]  = 'S';
    a[8]  = 'S';
    a[9]  = 'E';
    a[10] = 'R';
    a[11] = 'T';
    a[12] = ':';


    ofd_write(a, sizeof (a) - 1);
    
    /* put the number in backwards */
    i = 0;
    while ( line > 0 ) {
        num[i++] = '0' + (line % 10);
        line /= 10;
    }
    /* print it */
    /* number */
    while (i-- > 0) {
        ofd_write(&num[i], 1);
    }
    ofd_write("\n", 1);

    ofd_stop();
}

#ifdef assert
#undef assert
#endif

#define assert(EX)                                              \
    do {                                                        \
        if ( !(EX) ) {                                          \
            ofd_assprint(__LINE__);                             \
        }                                                       \
    } while (0)

/*
 * We treat memory like an array of u64.  For the sake of
 * compactness we assume that a short is big enough as an index.
 */
struct ofd_node {
    ofdn_t on_ima;
    ofdn_t on_parent;
    ofdn_t on_child;
    ofdn_t on_peer;
    ofdn_t on_io;
    ofdn_t on_next;     /* for search lists */
    ofdn_t on_prev;
    ofdn_t on_prop;
    u32 on_pathlen;
    u32 on_last;
    char on_path[0];
};

struct ofd_prop {
    ofdn_t op_ima;
    ofdn_t op_next;
    u32 op_objsz;
    u32 op_namesz;
    /* must have 64bit alignment */
    char op_data[0]  __attribute__ ((aligned(8)));
};

struct ofd_io {
    ofdn_t oi_ima;
    ofdn_t oi_node;
    u64 oi_open   __attribute__ ((aligned(8)));
};

struct ofd_free {
    ofdn_t of_cells;
    ofdn_t of_next;
};

struct ofd_mem {
    ofdn_t om_num;
    ofdn_t om_next;
    ofdn_t om_free;     /* Future site of a free list */
    ofdn_t _om_pad;
    u64 om_mem[0] __attribute__((aligned(8)));
};

#define NODE_PAT    0x0f01
#define PROP_PAT    0x0f03
#define IO_PAT      0x0f05


size_t ofd_size(void *mem)
{
    struct ofd_mem *m = (struct ofd_mem *)mem;
    size_t sz;

    sz = m->om_next * sizeof (u64) + sizeof(*m);
    return sz;
}

size_t ofd_space(void *mem)
{
    struct ofd_mem *m = (struct ofd_mem *)mem;
    size_t sz;

    sz = m->om_num * sizeof (u64);
    return sz;
}


static int ofd_pathsplit_right(const char *s, int c, size_t max)
{
    int i = 0;

    if ( max == 0 ) {
        --max;
    }
    
    while ( *s != '\0' && *s != c && max != 0 ) {
        ++i;
        ++s;
        --max;
    }
    return i;
}

static int ofd_pathsplit_left(const char *p, int c, size_t len)
{
    const char *s;

    if ( len > 0 ) {
        /* move s to the end */
        s = p + len - 1;

        /* len could include a null */
        if ( *s == '\0' ) {
            --s;
        }
        while ( s >= p ) {
            if ( *s == c ) {
                ++s;
                break;
            }
            --s;
        }
        if ( s < p ) {
            return 0;
        }
        return (s - p);
    }
    return 0;
}

void *ofd_create(void *mem, size_t sz)
{
    struct ofd_mem *m = (struct ofd_mem *)mem;
    struct ofd_node *n;
    size_t sum;
    ofdn_t cells;

    if ( sz < (sizeof (*n) * 4) ) {
        return NULL;
    }

    memset(mem, 0, sz);

    m->om_num = (sz - sizeof(*m)) / sizeof (u64);

    /* skip the first cell */
    m->om_next = OFD_ROOT;
    n = (struct ofd_node *)&m->om_mem[m->om_next];
    n->on_ima = NODE_PAT;
    n->on_pathlen = 2;
    n->on_last = 1;
    n->on_path[0] = '/';
    n->on_path[1] = '\0';

    sum = sizeof (*n) + 2; /* Don't forget the path */
    cells = (sum + sizeof (m->om_mem[0]) - 1) / sizeof (m->om_mem[0]);
    m->om_next += cells;

    return m;
}

static struct ofd_node *ofd_node_get(struct ofd_mem *m, ofdn_t n)
{
    if ( n < m->om_next ) {
        struct ofd_node *r;

        r = (struct ofd_node *)&m->om_mem[n];
        if ( r->on_ima == NODE_PAT ) {
            return r;
        }
    }
    return NULL;
}

ofdn_t ofd_node_parent(void *mem, ofdn_t n)
{
    struct ofd_mem *m = (struct ofd_mem *)mem;
    struct ofd_node *r = ofd_node_get(m, n);

    if ( r == NULL) return 0;
    return r->on_parent;
}

ofdn_t ofd_node_peer(void *mem, ofdn_t n)
{
    struct ofd_mem *m = (struct ofd_mem *)mem;
    struct ofd_node *r;

    if ( n == 0 ) {
        return OFD_ROOT;
    }

    r = ofd_node_get(m, n);
    if ( r == NULL) return 0;
    return r->on_peer;
}

const char *ofd_node_path(void *mem, ofdn_t n)
{
    struct ofd_mem *m = (struct ofd_mem *)mem;
    struct ofd_node *r = ofd_node_get(m, n);

    if ( r == NULL) return NULL;
    return r->on_path;
}

static ofdn_t ofd_node_prop(void *mem, ofdn_t n)
{
    struct ofd_mem *m = (struct ofd_mem *)mem;
    struct ofd_node *r = ofd_node_get(m, n);

    if ( r == NULL) return 0;
    return r->on_prop;
}

ofdn_t ofd_node_child(void *mem, ofdn_t p)
{
    struct ofd_mem *m = (struct ofd_mem *)mem;
    struct ofd_node *r = ofd_node_get(m, p);

    if ( r == NULL) return 0;
    return r->on_child;
}

int ofd_node_to_path(void *mem, ofdn_t p, void *buf, size_t sz)
{
    struct ofd_mem *m = (struct ofd_mem *)mem;
    struct ofd_node *r = ofd_node_get(m, p);

    if ( sz > r->on_pathlen ) {
        sz = r->on_pathlen;
    }

    memcpy(buf, r->on_path, sz);

    if ( r == NULL) return -1;
    return r->on_pathlen;
}

static int ofd_check(void *p, size_t l)
{
    int i;
    u64 *v = (u64 *)p;

    for ( i = 0; i < l; i++ ) {
        if ( v[i] != 0ULL ) {
            return 0;
        }
    }
    return 1;
}



static ofdn_t ofd_node_create(
    struct ofd_mem *m, const char *path, size_t pathlen)
{
    struct ofd_node *n;
    ofdn_t pos;
    size_t sum = pathlen + 1 + sizeof (*n); /* add trailing zero to path */
    ofdn_t cells = (sum + sizeof(m->om_mem[0]) - 1) / sizeof(m->om_mem[0]);

    if ( m->om_next + cells >= m->om_num ) {
        return 0;
    }

    pos = m->om_next;
        
    assert(ofd_check(&m->om_mem[pos], cells)); /* non-zero */
    m->om_next += cells;

    n = (struct ofd_node *)&m->om_mem[pos];
    assert(n->on_ima == 0); /* new node not empty */

    n->on_ima = NODE_PAT;
    n->on_peer = 0;
    n->on_child = 0;
    n->on_io = 0;
    n->on_pathlen = pathlen;
    n->on_last = ofd_pathsplit_left(path, '/', pathlen);
    strlcpy(n->on_path, path, pathlen + 1);

    return pos;
}

/* prunes a node and all its children simply by wasting memory and
 * unlinking it from the tree */
int ofd_node_prune(void *mem, ofdn_t node)
{
    struct ofd_mem *m = (struct ofd_mem *)mem;
    struct ofd_node *n;
    struct ofd_node *p;

    n = ofd_node_get(m, node);
    if (n == NULL) return -1;

    p = ofd_node_get(m, n->on_parent);
    assert(p != NULL);

    if ( p->on_child == node ) {
        /* easy case */
        p->on_child = n->on_peer;
    } else {
        struct ofd_node *s;

        s = ofd_node_get(m, p->on_child);
        assert(s != NULL);
        while ( s->on_peer != node ) {
            s = ofd_node_get(m, s->on_peer);
            assert(s != NULL);
        }
        s->on_peer = n->on_peer;
    }
    return 1;
}

ofdn_t ofd_prune_path(void *m, const char *path)
{
    ofdn_t n;
    int rc = -1;
    while ((n = ofd_node_find(m, path)) > 0) {
        rc = ofd_node_prune(m, n);
    }

    return rc;
}

ofdn_t ofd_node_child_create(
    void *mem, ofdn_t parent, const char *path, size_t pathlen)
{
    struct ofd_mem *m = (struct ofd_mem *)mem;
    struct ofd_node *p;
    struct ofd_node *n;
    ofdn_t pos;

    p = ofd_node_get(m, parent);
    if (p == NULL) return  0;

    pos = ofd_node_create(m, path, pathlen);
    n = ofd_node_get(m, pos);
    assert(n != NULL);

    assert(p->on_child == 0); /* child exists */
    if ( p->on_child == 0 ) {
        p->on_child = pos;
        n->on_parent = parent;
    } else {
        pos = 0;
    }

    return pos;
}

ofdn_t ofd_node_peer_create(
    void *mem, ofdn_t sibling, const char *path, size_t pathlen)
{
    struct ofd_mem *m = (struct ofd_mem *)mem;
    struct ofd_node *s;
    struct ofd_node *n;
    ofdn_t pos;

    s = ofd_node_get(m, sibling);
    if (s == NULL) return 0;

    pos = ofd_node_create(m, path, pathlen);
    n = ofd_node_get(m, pos);
    assert(n != NULL);

    if ( s->on_peer == 0 ) {
        s->on_peer = pos;
        n->on_parent = s->on_parent;
    } else {
        assert(0); /* peer exists */
        pos = 0;
    }
    return pos;
}

static ofdn_t ofd_node_peer_last(void *mem, ofdn_t c)
{
    struct ofd_mem *m = (struct ofd_mem *)mem;
    struct ofd_node *n;

    n = ofd_node_get(m, c);
    if (n == NULL) return 0;

    while ( n->on_peer > 0 ) {
        c = n->on_peer;
        n = ofd_node_get(m, c);
        assert(n != NULL);
    }

    return c;
}

static ofdn_t ofd_node_walk(struct ofd_mem *m, ofdn_t p, const char *s)
{
    struct ofd_node *np;
    ofdn_t n;
    ofdn_t r;

    if ( *s == '/' ) {
        ++s;
        if ( *s == '\0' ) {
            assert(0); /* ends in / */
            return 0;
        }
    }

    np = ofd_node_get(m, p);
    if (np == NULL) return 0;

    r = p;
    do {
        int last = np->on_last;
        size_t lsz = np->on_pathlen - last;
        size_t sz;

        sz = ofd_pathsplit_right(s, '/', 0);
        
        if ( lsz > 0 && strncmp(s, &np->on_path[last], sz) == 0 ) {
            if ( s[sz] == '\0' ) {
                return r;
            }
            /* there is more to the path */
            n = ofd_node_child(m, p);
            if ( n != 0 ) {
                r = ofd_node_walk(m, n, &s[sz]);
                return r;
            }
            /* there are no children */
            return 0;
        }
    } while ( 0 );

    /*
     * we know that usually we are only serching for top level peers
     * so we do peers first peer
     */
    n = ofd_node_peer(m, p);
    if ( n > 0 ) {
        r = ofd_node_walk(m, n, s);
    } else {
        r = 0;
    }

    return r;
}


ofdn_t ofd_node_find(void *mem, const char *devspec)
{
    struct ofd_mem *m = (struct ofd_mem *)mem;
    ofdn_t n = OFD_ROOT;
    const char *s = devspec;
    size_t sz;

    if ( s == NULL || s[0] == '\0' ) {
        return OFD_ROOT;
    }

    if ( s[0] != '/' ) {
        size_t asz;