Permalink
Browse files

Replace overlay with patching DTB directly

  • Loading branch information...
gonzoua committed Nov 29, 2016
1 parent e7012e9 commit 0d65e734e5d03f91bc10451c4f510f60e5b90bc5
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.