[lib][devicetree] first pass at library for dealing with devicetree

Basic validation and walking of flattened devicetree blobs.

lib/devicetree/devicetree.c

@@ -0,0 +1,209 @@
+/*
+ * Copyright (c) 2014 Brian Swetland
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include "devicetree.h"
+
+#define DT_MAGIC	0xD00DFEED
+#define DT_NODE_BEGIN	1
+#define DT_NODE_END	2
+#define DT_PROP		3
+#define DT_END		9
+
+typedef struct dt_slice slice_t;
+
+u32 dt_rd32(u8 *data) {
+	return (data[0] << 24) | (data[1] << 16) | (data[2] << 8) | data[3];
+}
+
+void dt_wr32(u32 n, u8 *data) {
+	*data++ = n >> 24;
+	*data++ = n >> 16;
+	*data++ = n >> 8;
+	*data = n;
+}
+
+/* init subslice from slice, returning 0 if successful */
+static int sslice(slice_t *src, slice_t *dst, u32 off, u32 len) {
+	if (off >= src->size)
+		return -1;
+	if (len >= src->size)
+		return -1;
+	if ((off + len) > src->size)
+		return -1;
+	dst->data = src->data + off;
+	dst->size = len;
+	return 0;
+}
+
+/* return nonzero if slice is empty */
+static inline int sempty(slice_t *s) {
+	return s->size == 0;
+}
+
+/* read be32 from slice,
+ * or 0 (and make slice empty) if slice is too small
+ */
+static u32 su32(slice_t *s) {
+	if (s->size < 4) {
+		s->size = 0;
+		return 0;
+	} else {
+		u32 n = (s->data[0] << 24) | (s->data[1] << 16) | (s->data[2] << 8) | s->data[3];
+		s->size -= 4;
+		s->data += 4;
+		return n;
+	}
+}
+
+/* return pointer to data in slice,
+ * or 0 (and make slice empty) if slice is too small
+ */
+static void *sdata(slice_t *s, u32 len) {
+	if (len > s->size) {
+		s->size = 0;
+		return 0;
+	} else {
+		void *data = s->data;
+		s->size -= len;
+		s->data += len;
+		while (len & 3) {
+			if (s->size) {
+				s->size++;
+				s->data++;
+			}
+			len++;
+		}
+		return data;
+	}
+}
+
+/* return pointer to string in slice,
+ * or "" (and make slice empty) if slice is too small
+ */
+static const char *sstring(slice_t *s) {
+	u32 sz = s->size;
+	u8 *end = s->data;
+	const char *data;
+	while (sz-- > 0) {
+		if (*end++ == 0) {
+			while (((end - s->data) & 3) && (sz > 0)) {
+				end++;
+				sz--;
+			}
+			data = (const char*) s->data;
+			s->size = sz;
+			s->data = end;
+			return data;
+		}
+	}
+	s->size = 0;
+	return "";
+}
+
+static int oops(devicetree_t *dt, const char *msg) {
+	if (dt->error)
+		dt->error(msg);
+	return -1;
+}
+
+int dt_init(devicetree_t *dt, void *data, u32 len) {
+	slice_t s;
+
+	dt->top.data = data;
+	dt->top.size = len;
+
+	s = dt->top;
+
+	dt->hdr.magic = su32(&s);
+	dt->hdr.size = su32(&s);
+	dt->hdr.off_struct = su32(&s);
+	dt->hdr.off_strings = su32(&s);
+	dt->hdr.off_reserve = su32(&s);
+	dt->hdr.version = su32(&s);
+	dt->hdr.version_compat = su32(&s);
+	dt->hdr.boot_cpuid = su32(&s);
+	dt->hdr.sz_strings = su32(&s);
+	dt->hdr.sz_struct = su32(&s);
+
+	if (dt->hdr.magic != DT_MAGIC)
+		return oops(dt, "bad magic");
+	if (dt->hdr.size > dt->top.size)
+		return oops(dt, "bogus size field");
+	if (dt->hdr.version != 17)
+		return oops(dt, "version != 17");
+	if (sslice(&dt->top, &dt->dt, dt->hdr.off_struct, dt->hdr.sz_struct))
+		return oops(dt, "invalid structure off/len");
+	if (sslice(&dt->top, &dt->ds, dt->hdr.off_strings, dt->hdr.sz_strings))
+		return oops(dt, "invalid strings off/len");
+
+	return 0;
+}
+
+int dt_walk(devicetree_t *dtree, dt_node_cb ncb, dt_prop_cb pcb, void *cookie) {
+	const char *p;
+	void *data;
+	u32 depth = 0;
+	slice_t dt, ds;
+	u32 sz, str;
+
+	dt = dtree->dt;
+	ds = dtree->ds;
+
+	while (!sempty(&dt)) {
+		u32 type = su32(&dt);
+		switch (type) {
+		case DT_END:
+			if (depth)
+				return oops(dtree, "unexpected DT_END");
+			return 0;
+		case DT_NODE_BEGIN:
+			depth++;
+			p = sstring(&dt);
+			if (ncb(depth, p, cookie))
+				return 0;
+			break;
+		case DT_NODE_END:
+			if (depth == 0)
+				return oops(dtree, "unexpected NODE_END");
+			depth--;
+			break;
+		case DT_PROP:
+			if (depth == 0)
+				return oops(dtree, "PROP outside of NODE");
+			sz = su32(&dt);
+			str = su32(&dt);
+			data = sdata(&dt, sz);
+			if (pcb((const char*) (ds.data + str), data, sz, cookie))
+				return 0;
+			break;
+		default:
+			return oops(dtree, "invalid node type");
+
+		}
+	}
+
+	if (depth != 0)
+		return oops(dtree, "incomplete tree");
+
+	return 0;
+}

lib/devicetree/devicetreedump.c

@@ -0,0 +1,111 @@
+/*
+ * Copyright (c) 2014 Brian Swetland
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include "devicetree.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+
+void error(const char *msg) {
+	printf("error: %s\n", msg);
+};
+
+void indent(u32 n) {
+	while (n-- > 0)
+		putchar(' ');
+}
+
+void hexdump(u8 *data, u32 count) {
+	while (count-- > 0)
+		printf("%02x ", *data++);
+}
+
+int loadfile(const char *fn, dt_slice_t *out) {
+	void *data = NULL;
+	off_t end;	
+	int fd;
+	if ((fd = open(fn, O_RDONLY)) < 0)
+		return -1;
+	if ((end = lseek(fd, 0, SEEK_END)) == -1)
+		goto oops;
+	if (lseek(fd, 0, SEEK_SET))
+		goto oops;
+	if ((data = malloc(end + 1)) == NULL)
+		goto oops;
+	if ((read(fd, data, end) != end))
+		goto oops;
+	out->data = data;
+	out->size = end;
+	close(fd);
+	return 0;
+oops:
+	free(data);
+	close(fd);
+	return -1;
+}
+
+static int _depth = 0;
+
+int node_cb(int depth, const char *name, void *cookie) {
+	_depth = depth;
+	indent(depth*2);
+	printf("node: '%s'\n", name);
+	return 0;
+}
+
+int prop_cb(const char *name, u8 *data, u32 size, void *cookie) {
+	indent(_depth * 2 + 2);
+	printf("prop '%s' sz=%d\n", name, size);
+	indent(_depth * 2 + 2);
+	printf("data ");
+	hexdump(data, size);
+	printf("\n");
+	return 0;
+}
+
+int main(int argc, char **argv) {
+	devicetree_t dt;
+	dt_slice_t s;
+
+	dt.error = error;
+
+	if (argc != 2)
+		return -1;
+	if (loadfile(argv[1], &s))
+		return -1;
+	if (dt_init(&dt, s.data, s.size))
+		return -1;
+
+	printf("magic %x\n", dt.hdr.magic);
+	printf("size %d\n", dt.hdr.size);
+	printf("off_struct %d (%d)\n", dt.hdr.off_struct, dt.hdr.sz_struct);
+	printf("off_strings %d (%d)\n", dt.hdr.off_strings, dt.hdr.sz_strings);
+	printf("version %d (min %d)\n", dt.hdr.version, dt.hdr.version_compat);
+
+	dt_walk(&dt, node_cb, prop_cb, NULL);
+	return 0;
+}
+

lib/devicetree/include/lib/devicetree.h

@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2014 Brian Swetland
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef _DEVICETREE_H_
+#define _DEVICETREE_H_
+
+#include <sys/types.h>
+
+typedef struct dt_slice {
+	u8 *data;
+	u32 size;
+} dt_slice_t;
+
+struct devicetree_header {
+	u32 magic;
+	u32 size;
+	u32 off_struct;		// offset from start to DT 'structure'
+	u32 off_strings;	// offset from start to stringdata
+	u32 off_reserve;	// offset from start to reserve memory map
+	u32 version;
+	u32 version_compat;	// last compatible version
+	u32 boot_cpuid;
+	u32 sz_strings;		// size of stringdata
+	u32 sz_struct;		// size of DT 'structure'
+};
+
+typedef struct devicetree {
+	dt_slice_t top;
+	dt_slice_t dt;
+	dt_slice_t ds;
+	struct devicetree_header hdr;
+	void (*error)(const char *msg);
+} devicetree_t;
+
+typedef int (*dt_node_cb)(int depth, const char *name, void *cookie);
+typedef int (*dt_prop_cb)(const char *name, u8 *data, u32 size, void *cookie);
+
+int dt_init(devicetree_t *dt, void *data, u32 len);
+int dt_walk(devicetree_t *dt, dt_node_cb ncb, dt_prop_cb pcb, void *cookie);
+
+u32 dt_rd32(u8 *data);
+void dt_wr32(u32 n, u8 *data);
+
+#endif
+

lib/devicetree/rules.mk

@@ -0,0 +1,8 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+MODULE := $(LOCAL_DIR)
+
+MODULE_SRCS += \
+	$(LOCAL_DIR)/devicetree.c
+
+include make/module.mk