Permalink
Browse files

Replace overlay with patching DTB directly

  • Loading branch information...
1 parent e7012e9 commit 0d65e734e5d03f91bc10451c4f510f60e5b90bc5 @gonzoua committed Nov 29, 2016
Showing with 274 additions and 8 deletions.
  1. +7 −8 Makefile
  2. +263 −0 fdtpatch.c
  3. +4 −0 pscimon.S
View
@@ -1,15 +1,11 @@
-.SUFFIXES: .S .o .elf .tmp .bin .dts .dtbo
+.SUFFIXES: .S .o .elf .tmp .bin .dtbo
CC8=aarch64-none-elf-gcc
LD8=aarch64-none-elf-ld
OBJCOPY8=aarch64-none-elf-objcopy
OBJDUMP8=aarch64-none-elf-objdump -maarch64
-# Requires dtc with overlay support
-DTC=dtc
-all: psci overlay
-
-overlay: psci.dtbo
+all: psci
psci: pscimon.bin
@@ -19,6 +15,9 @@ clean :
.S.o:
$(CC8) -c $< -o $@
+.c.o:
+ $(CC8) -fno-builtin -c $< -o $@
+
.o.elf:
$(LD8) --section-start=.text=0 $< -o $@
@@ -28,5 +27,5 @@ clean :
.tmp.bin:
dd if=$< ibs=256 of=$@ conv=sync
-.dts.dtbo:
- $(DTC) -O dtb -o $@ -b 0 -@ $<
+pscimon.elf: pscimon.o fdtpatch.o
+ $(LD8) --section-start=.text=0 ${.ALLSRC} -o $@
View
@@ -0,0 +1,263 @@
+/*-
+ * Copyright (c) 2016 Oleksandr Tymoshenko <gonzo@freebsd.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+/* FDT bits */
+#define FDT_MAGIC 0xd00dfeed /* 4: version, 4: total size */
+#define FDT_TAGSIZE sizeof(uint32_t)
+
+#define FDT_BEGIN_NODE 0x1 /* Start node: full name */
+#define FDT_END_NODE 0x2 /* End node */
+#define FDT_PROP 0x3 /* Property: name off,
+ size, content */
+#define FDT_NOP 0x4 /* nop */
+#define FDT_END 0x9
+
+#define PSCI_NODE_LEN 0x3C
+#define PSCI_NODE_COMAPTIBLE_OFF 0x14
+#define PSCI_NODE_METHOD_OFF 0x30
+const unsigned char psci_node[] = {
+ /* FDT_BEGIN_NODE, "psci", FDT_PROP */
+ 0, 0, 0, 1, 'p', 's', 'c', 'i', 0, 0, 0, 0, 0, 0, 0, 3,
+ /* len, @"compatible", "arm,psci-0.2 */
+ 0, 0, 0, 13, 0, 0, 0, 0, 'a', 'r', 'm', ',', 'p', 's', 'c', 'i',
+ /* FDT_PROP, len */
+ '-', '0', '.', '2', 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 4,
+ /* @"method", "smc", FDT_END_NODE */
+ 0, 0, 0, 0, 's', 'm', 'c', 0, 0, 0, 0, 2
+};
+
+#define STRINGS_SIZE 0x12
+#define STRINGS_COMPATIBLE_OFF 0
+#define STRINGS_METHOD_OFF 11
+const unsigned char strings[] = {
+ 'c', 'o', 'm', 'p', 'a', 't', 'i', 'b',
+ 'l', 'e', 0, 'm', 'e', 't', 'h', 'o',
+ 'd', 0
+};
+
+/* PL011 UART registers */
+#define UART_DR 0x3f201000
+#define UART_FR 0x3f201018
+
+typedef unsigned int uint32_t;
+typedef unsigned long long uint64_t;
+
+static inline void
+putc(char c)
+{
+ while (*(uint32_t*)UART_FR & (1 << 5))
+ ;
+ *(char*)UART_DR = c;
+}
+
+static inline void
+puts(const char *s)
+{
+ while (*s) {
+ putc(*s);
+ s++;
+ }
+}
+
+static inline void
+puthex(uint64_t v)
+{
+ const char *hexdigits = "0123456789absdef";
+ for (int i = 60; i >= 0; i -= 4)
+ putc(hexdigits[(v >> i) & 0xf]);
+}
+
+static inline uint32_t
+bswap32(uint32_t v)
+{
+ uint32_t ret;
+
+ __asm __volatile("rev32 %x0, %x1\n"
+ : "=&r" (ret), "+r" (v));
+
+ return (ret);
+}
+
+/*
+ * We always assume that dst is la
+ */
+void memmove(const void *src, void *dst, int len)
+{
+ const char *s;
+ char *d;
+
+ s = src;
+ d = dst;
+ for (int i = len - 1; i >= 0; i--)
+ d[i] = s[i];
+}
+
+void fixup_dt_blob(void *dtb)
+{
+ uint32_t magic;
+ uint32_t total_size;
+ uint32_t off_memrsrv;
+ uint32_t off_struct;
+ uint32_t off_strings;
+ uint32_t size_struct;
+ uint32_t size_strings;
+ uint32_t *dtb_hdr;
+ uint32_t *dtb_struct;
+ int tag_ptr, tag, done, len, level;
+ int psci_node_ptr;
+ int strings_end;
+ char *dtb_strings;
+ char *s;
+
+ dtb_hdr = dtb;
+
+ magic = bswap32(dtb_hdr[0]);
+ if (magic != FDT_MAGIC) {
+ puts("Invalid magic @");
+ puthex((uint64_t)dtb);
+ return;
+ }
+ total_size = bswap32(dtb_hdr[1]);
+ off_struct = bswap32(dtb_hdr[2]);
+ off_strings = bswap32(dtb_hdr[3]);
+ off_memrsrv = bswap32(dtb_hdr[4]);
+ size_strings = bswap32(dtb_hdr[8]);
+ size_struct = bswap32(dtb_hdr[9]);
+
+ dtb_struct = dtb_hdr + off_struct/sizeof(uint32_t);
+ dtb_strings = (char*)dtb + off_strings;
+
+ /*
+ * Find end first non-root node
+ */
+ done = 0;
+ tag_ptr = 0;
+ level = 0;
+ /* Find first sub-node of root node */
+ while (!done) {
+ tag = bswap32(dtb_struct[tag_ptr]);
+ switch (tag) {
+ case FDT_BEGIN_NODE:
+ level++;
+ if (level == 2) {
+ done = 1;
+ break;
+ }
+ tag_ptr++;
+ s = (char *)(dtb_struct + tag_ptr);
+ len = 0;
+ while (s[len] != 0)
+ len++;
+ len++; /* include zero byte */
+ len = (len + 3) & ~0x3;
+ tag_ptr += len/sizeof(uint32_t);
+ break;
+
+ case FDT_END_NODE:
+ tag_ptr++;
+ break;
+
+ case FDT_NOP:
+ tag_ptr++;
+ break;
+ case FDT_PROP:
+ tag_ptr++; /* skip tag */
+ len = bswap32(dtb_struct[tag_ptr]);
+ /* Align up to the next 32 bit */
+ len = (len + 3) & ~0x3;
+ tag_ptr += 2;
+ tag_ptr += len/sizeof(uint32_t);
+ break;
+ case FDT_END:
+ tag_ptr = -1;
+ done = 1;
+ break;
+ default:
+ puts("Invalid FDT tag ");
+ puthex(tag);
+ puts(" @");
+ puthex(tag_ptr * sizeof(uint32_t));
+ puts("\r\n");
+ tag_ptr = -1;
+ done = 1;
+ break;
+ }
+ }
+
+ /* Either invalid tag or reached end */
+ if (tag_ptr < 0)
+ return;
+
+ /*
+ * Insert free space for psci node before
+ * first non-root node
+ */
+ psci_node_ptr = off_struct + tag_ptr * sizeof(uint32_t);
+ memmove((char *)dtb + psci_node_ptr,
+ (char *)dtb + psci_node_ptr + PSCI_NODE_LEN,
+ total_size - psci_node_ptr);
+ memmove(psci_node, (char *)dtb + psci_node_ptr, PSCI_NODE_LEN);
+
+ /* Fixup lengths/offsets */
+ total_size += PSCI_NODE_LEN;
+ size_struct += PSCI_NODE_LEN;
+ if (off_strings > psci_node_ptr)
+ off_strings += PSCI_NODE_LEN;
+ if (off_memrsrv > psci_node_ptr)
+ off_memrsrv += PSCI_NODE_LEN;
+
+ /*
+ * Append some free space at the end of strings section
+ */
+ strings_end = off_strings + size_strings;
+ memmove((char *)dtb + strings_end,
+ (char *)dtb + strings_end + STRINGS_SIZE,
+ total_size - strings_end);
+ memmove(strings, (char *)dtb + strings_end, STRINGS_SIZE);
+
+ /* set property names offsets in psci node */
+ dtb_struct[tag_ptr + PSCI_NODE_COMAPTIBLE_OFF/sizeof(uint32_t)] =
+ bswap32(size_strings + STRINGS_COMPATIBLE_OFF);
+ dtb_struct[tag_ptr + PSCI_NODE_METHOD_OFF/sizeof(uint32_t)] =
+ bswap32(size_strings + STRINGS_METHOD_OFF);
+
+ /* Fixup lengths/offsets */
+ total_size += STRINGS_SIZE;
+ size_strings += STRINGS_SIZE;
+ if (off_struct > strings_end)
+ off_struct += STRINGS_SIZE;
+ if (off_memrsrv > psci_node_ptr)
+ off_memrsrv += STRINGS_SIZE;
+
+ /* Update header */
+ dtb_hdr[1] = bswap32(total_size);
+ dtb_hdr[2] = bswap32(off_struct);
+ dtb_hdr[3] = bswap32(off_strings);
+ dtb_hdr[4] = bswap32(off_memrsrv);
+ dtb_hdr[8] = bswap32(size_strings);
+ dtb_hdr[9] = bswap32(size_struct);
+
+ puts("\r\nRPi3 PSCI monitor installed\r\n");
+}
View
@@ -140,6 +140,10 @@ secondary_spin:
b boot_kernel
primary_cpu:
+ mov sp, #RESERVE_SIZE
+ sub sp, sp, #16
+ ldr w0, dtb_ptr32
+ bl fixup_dt_blob
ldr w4, kernel_entry32
ldr w0, dtb_ptr32
mov x1, #RESERVE_SIZE

0 comments on commit 0d65e73

Please sign in to comment.