/*-
 * Copyright (c) 1998 Robert Nordier
 * All rights reserved.
 * Copyright (c) 2001 Robert Drehmel
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms are freely
 * permitted provided that the above copyright notice and this
 * paragraph and the following disclaimer are duplicated in all
 * such forms.
 *
 * This software is provided "AS IS" and without any express or
 * implied warranties, including, without limitation, the implied
 * warranties of merchantability and fitness for a particular
 * purpose.
 */

#include <sys/param.h>
#include <sys/dirent.h>
#include <sys/endian.h>
#include <sys/stdarg.h>

#include <machine/elf.h>
#include <machine/md_var.h>

#include <ufs/ffs/fs.h>

#include "paths.h"

#define BSIZEMAX        16384

typedef int putc_func_t(char c, void *arg);
typedef int32_t ofwh_t;

struct sp_data {
        char    *sp_buf;
        u_int   sp_len;
        u_int   sp_size;
};

static const char digits[] = "0123456789abcdef";

static char bootpath[128];
static char bootargs[128];

static ofwh_t bootdev;

static struct fs fs;
static char blkbuf[BSIZEMAX];
static unsigned int fsblks;

static uint32_t fs_off;

int main(int ac, char **av);

static void exit(int) __dead2;
static void load(const char *);
static int dskread(void *, uint64_t, int);

static void usage(void) __dead2;

static void bcopy(const void *src, void *dst, size_t len);
static void bzero(void *b, size_t len);

static int domount(const char *device, int quiet);

static void panic(const char *fmt, ...) __dead2;
static int printf(const char *fmt, ...);
static int putchar(char c, void *arg);
static int vprintf(const char *fmt, va_list ap);
static int vsnprintf(char *str, size_t sz, const char *fmt, va_list ap);

static int __printf(const char *fmt, putc_func_t *putc, void *arg, va_list ap);
static int __putc(char c, void *arg);
static int __puts(const char *s, putc_func_t *putc, void *arg);
static int __sputc(char c, void *arg);
static char *__uitoa(char *buf, u_int val, int base);
static char *__ultoa(char *buf, u_long val, int base);

/*
 * Open Firmware interface functions
 */
typedef uint32_t        ofwcell_t;
typedef uint32_t        u_ofwh_t;
typedef int (*ofwfp_t)(ofwcell_t *);
ofwfp_t ofw;                    /* the prom Open Firmware entry */
ofwh_t chosenh;

void ofw_init(void *, int, ofwfp_t, char *, int);
static ofwh_t ofw_finddevice(const char *);
static ofwh_t ofw_open(const char *);
static int ofw_close(ofwh_t);
static int ofw_getprop(ofwh_t, const char *, void *, size_t);
static int ofw_setprop(ofwh_t, const char *, void *, size_t);
static int ofw_read(ofwh_t, void *, size_t);
static int ofw_write(ofwh_t, const void *, size_t);
static int ofw_claim(void *virt, size_t len, u_int align);
static int ofw_seek(ofwh_t, uint64_t);
static void ofw_exit(void) __dead2;

ofwh_t bootdevh;
ofwh_t stdinh, stdouth;

/*
 * Note about the entry point:
 *
 * For some odd reason, the first page of the load appears to have trouble
 * when entering in LE. The first five instructions decode weirdly.
 * I suspect it is some cache weirdness between the ELF headers and .text.
 *
 * Ensure we have a gap between the start of .text and the entry as a
 * workaround.
 */
__asm("                         \n\
        .data                   \n\
        .align 4                \n\
stack:                          \n\
        .space  16384           \n\
                                \n\
        .text                   \n\
        /* SLOF cache hack */   \n\
        .space 4096             \n\
        .globl  _start          \n\
_start:                         \n\
        lis     %r1,stack@ha    \n\
        addi    %r1,%r1,stack@l \n\
        addi    %r1,%r1,8192    \n\
                                \n\
        b       ofw_init        \n\
");

ofwfp_t realofw;

#if BYTE_ORDER == LITTLE_ENDIAN
/*
 * Minimal endianness-swap trampoline for LE.
 */
__attribute__((naked)) int
ofwtramp(void *buf, ofwfp_t cb)
{
__asm("                                                                 \n\
        mflr    %r0                                                     \n\
        stw     %r0, 4(%r1)                                             \n\
        stwu    %r1, -16(%r1)                                           \n\
        stw     %r30, 8(%r1)                                            \n\
        /* Save current MSR for restoration post-call. */               \n\
        mfmsr   %r30                                                    \n\
        mr      %r5, %r30                                               \n\
        /* Remove LE bit from MSR. */                                   \n\
        clrrwi  %r5, %r5, 1                                             \n\
        mtsrr0  %r4                                                     \n\
        mtsrr1  %r5                                                     \n\
        bcl     20, 31, .+4     /* LOAD_LR_NIA */                       \n\
1:                                                                      \n\
        mflr    %r4                                                     \n\
        addi    %r4, %r4, (2f - 1b)                                     \n\
        mtlr    %r4                                                     \n\
        /* Switch to BE and transfer control to OF entry */             \n\
        rfid                                                            \n\
2:                                                                      \n\
        /* Control is returned here, but in BE. */                      \n\
        .long   0x05009f42      /* LOAD_LR_NIA                        */\n\
                                /* 0:                                 */\n\
        .long   0xa603db7f      /* mtsrr1       %r30                  */\n\
        .long   0xa602c87f      /* mflr         %r30                  */\n\
        .long   0x1400de3b      /* addi         %r30, %r30, (1f - 0b) */\n\
        .long   0xa603da7f      /* mtsrr0       %r30                  */\n\
        .long   0x2400004c      /* rfid                               */\n\
                                /* 1:                                 */\n\
1:                                                                      \n\
        /* Back to normal. Tidy up for return. */                       \n\
        lwz     %r30, 8(%r1)                                            \n\
        lwz     %r0, 20(%r1)                                            \n\
        addi    %r1, %r1, 16                                            \n\
        mtlr    %r0                                                     \n\
        blr                                                             \n\
");
}

/*
 * Little-endian OFW entrypoint replacement.
 *
 * We are doing all the byteswapping in one place here to save space.
 * This means instance handles will be byteswapped as well.
 */
int
call_ofw(ofwcell_t* buf)
{
        int ret, i, ncells;

        ncells = 3 + buf[1] + buf[2];
        for (i = 0; i < ncells; i++)
                buf[i] = htobe32(buf[i]);

        ret = (ofwtramp(buf, realofw));
        for (i = 0; i < ncells; i++)
                buf[i] = be32toh(buf[i]);
        return (ret);
}
#endif

void
ofw_init(void *vpd, int res, ofwfp_t openfirm, char *arg, int argl)
{
        char *av[16];
        char *p;
        int ac;

#if BYTE_ORDER == LITTLE_ENDIAN
        realofw = openfirm;
        ofw = call_ofw;
#else
        realofw = ofw = openfirm;
#endif

        chosenh = ofw_finddevice("/chosen");
        ofw_getprop(chosenh, "stdin", &stdinh, sizeof(stdinh));
        stdinh = be32toh(stdinh);
        ofw_getprop(chosenh, "stdout", &stdouth, sizeof(stdouth));
        stdouth = be32toh(stdouth);
        ofw_getprop(chosenh, "bootargs", bootargs, sizeof(bootargs));
        ofw_getprop(chosenh, "bootpath", bootpath, sizeof(bootpath));

        bootargs[sizeof(bootargs) - 1] = '\0';
        bootpath[sizeof(bootpath) - 1] = '\0';

        p = bootpath;
        while (*p != '\0') {
                /* Truncate partition ID */
                if (*p == ':') {
                        ofw_close(bootdev);
                        *(++p) = '\0';
                        break;
                }
                p++;
        }

        ac = 0;
        p = bootargs;
        for (;;) {
                while (*p == ' ' && *p != '\0')
                        p++;
                if (*p == '\0' || ac >= 16)
                        break;
                av[ac++] = p;
                while (*p != ' ' && *p != '\0')
                        p++;
                if (*p != '\0')
                        *p++ = '\0';
        }

        exit(main(ac, av));
}

static ofwh_t
ofw_finddevice(const char *name)
{
        ofwcell_t args[] = {
                (ofwcell_t)"finddevice",
                1,
                1,
                (ofwcell_t)name,
                0
        };

        if ((*ofw)(args)) {
                printf("ofw_finddevice: name=\"%s\"\n", name);
                return (1);
        }
        return (args[4]);
}

static int
ofw_getprop(ofwh_t ofwh, const char *name, void *buf, size_t len)
{
        ofwcell_t args[] = {
                (ofwcell_t)"getprop",
                4,
                1,
                (u_ofwh_t)ofwh,
                (ofwcell_t)name,
                (ofwcell_t)buf,
                len,
        0
        };

        if ((*ofw)(args)) {
                printf("ofw_getprop: ofwh=0x%x buf=%p len=%u\n",
                        ofwh, buf, len);
                return (1);
        }
        return (0);
}

static int
ofw_setprop(ofwh_t ofwh, const char *name, void *buf, size_t len)
{
        ofwcell_t args[] = {
                (ofwcell_t)"setprop",
                4,
                1,
                (u_ofwh_t)ofwh,
                (ofwcell_t)name,
                (ofwcell_t)buf,
                len,
        0
        };

        if ((*ofw)(args)) {
                printf("ofw_setprop: ofwh=0x%x buf=%p len=%u\n",
                        ofwh, buf, len);
                return (1);
        }
        return (0);
}

static ofwh_t
ofw_open(const char *path)
{
        ofwcell_t args[] = {
                (ofwcell_t)"open",
                1,
                1,
                (ofwcell_t)path,
                0
        };

        if ((*ofw)(args)) {
                printf("ofw_open: path=\"%s\"\n", path);
                return (-1);
        }
        return (args[4]);
}

static int
ofw_close(ofwh_t devh)
{
        ofwcell_t args[] = {
                (ofwcell_t)"close",
                1,
                0,
                (u_ofwh_t)devh
        };

        if ((*ofw)(args)) {
                printf("ofw_close: devh=0x%x\n", devh);
                return (1);
        }
        return (0);
}

static int
ofw_claim(void *virt, size_t len, u_int align)
{
        ofwcell_t args[] = {
                (ofwcell_t)"claim",
                3,
                1,
                (ofwcell_t)virt,
                len,
                align,
                0,
                0
        };

        if ((*ofw)(args)) {
                printf("ofw_claim: virt=%p len=%u\n", virt, len);
                return (1);
        }

        return (0);
}

static int
ofw_read(ofwh_t devh, void *buf, size_t len)
{
        ofwcell_t args[] = {
                (ofwcell_t)"read",
                3,
                1,
                (u_ofwh_t)devh,
                (ofwcell_t)buf,
                len,
                0
        };

        if ((*ofw)(args)) {
                printf("ofw_read: devh=0x%x buf=%p len=%u\n", devh, buf, len);
                return (1);
        }
        return (0);
}

static int
ofw_write(ofwh_t devh, const void *buf, size_t len)
{
        ofwcell_t args[] = {
                (ofwcell_t)"write",
                3,
                1,
                (u_ofwh_t)devh,
                (ofwcell_t)buf,
                len,
                0
        };

        if ((*ofw)(args)) {
                printf("ofw_write: devh=0x%x buf=%p len=%u\n", devh, buf, len);
                return (1);
        }
        return (0);
}

static int
ofw_seek(ofwh_t devh, uint64_t off)
{
        ofwcell_t args[] = {
                (ofwcell_t)"seek",
                3,
                1,
                (u_ofwh_t)devh,
                off >> 32,
                off,
                0
        };

        if ((*ofw)(args)) {
                printf("ofw_seek: devh=0x%x off=0x%lx\n", devh, off);
                return (1);
        }
        return (0);
}

static void
ofw_exit(void)
{
        ofwcell_t args[3];

        args[0] = (ofwcell_t)"exit";
        args[1] = 0;
        args[2] = 0;

        for (;;)
                (*ofw)(args);
}

static void
bcopy(const void *src, void *dst, size_t len)
{
        const char *s = src;
        char *d = dst;

        while (len-- != 0)
                *d++ = *s++;
}

static void
memcpy(void *dst, const void *src, size_t len)
{
        bcopy(src, dst, len);
}

static void
bzero(void *b, size_t len)
{
        char *p = b;

        while (len-- != 0)
                *p++ = 0;
}

static int
strcmp(const char *s1, const char *s2)
{
        for (; *s1 == *s2 && *s1; s1++, s2++)
                ;
        return ((u_char)*s1 - (u_char)*s2);
}

#include "ufsread.c"

int
main(int ac, char **av)
{
        const char *path;
        char bootpath_full[255];
        int i, len;

        path = PATH_LOADER;
        for (i = 0; i < ac; i++) {
                switch (av[i][0]) {
                case '-':
                        switch (av[i][1]) {
                        default:
                                usage();
                        }
                        break;
                default:
                        path = av[i];
                        break;
                }
        }

        printf(" \n>> FreeBSD/powerpc Open Firmware boot block\n"
        "   Boot path:   %s\n"
        "   Boot loader: %s\n", bootpath, path);

        len = 0;
        while (bootpath[len] != '\0') len++;

        memcpy(bootpath_full,bootpath,len+1);

        if (bootpath_full[len-1] != ':') {
                /* First try full volume */
                if (domount(bootpath_full,1) == 0)
                        goto out;

                /* Add a : so that we try partitions if that fails */
                if (bootdev > 0)
                        ofw_close(bootdev);
                bootpath_full[len] = ':';
                len += 1;
        }

        /* Loop through first 16 partitions to find a UFS one */
        for (i = 0; i < 16; i++) {
                if (i < 10) {
                        bootpath_full[len] = i + '0';
                        bootpath_full[len+1] = '\0';
                } else {
                        bootpath_full[len] = '1';
                        bootpath_full[len+1] = i - 10 + '0';
                        bootpath_full[len+2] = '\0';
                }
                        
                if (domount(bootpath_full,1) >= 0)
                        break;

                if (bootdev > 0)
                        ofw_close(bootdev);
        }

        if (i >= 16)
                panic("domount");

out:
        printf("   Boot volume:   %s\n",bootpath_full);
        ofw_setprop(chosenh, "bootargs", bootpath_full, len+2);
        load(path);
        return (1);
}

static void
usage(void)
{

        printf("usage: boot device [/path/to/loader]\n");
        exit(1);
}

static void
exit(int code)
{

        ofw_exit();
}

static struct dmadat __dmadat;

static int
domount(const char *device, int quiet)
{

        dmadat = &__dmadat;
        if ((bootdev = ofw_open(device)) == -1) {
                printf("domount: can't open device\n");
                return (-1);
        }
        if (fsread(0, NULL, 0)) {
                if (!quiet)
                        printf("domount: can't read superblock\n");
                return (-1);
        }
        return (0);
}

static void
load(const char *fname)
{
        Elf32_Ehdr eh;
        Elf32_Phdr ph;
        caddr_t p;
        ufs_ino_t ino;
        int i;

        if ((ino = lookup(fname)) == 0) {
                printf("File %s not found\n", fname);
                return;
        }
        if (fsread(ino, &eh, sizeof(eh)) != sizeof(eh)) {
                printf("Can't read elf header\n");
                return;
        }
        if (!IS_ELF(eh)) {
                printf("Not an ELF file\n");
                return;
        }
        for (i = 0; i < eh.e_phnum; i++) {
                fs_off = eh.e_phoff + i * eh.e_phentsize;
                if (fsread(ino, &ph, sizeof(ph)) != sizeof(ph)) {
                        printf("Can't read program header %d\n", i);
                        return;
                }
                if (ph.p_type != PT_LOAD)
                        continue;
                fs_off = ph.p_offset;
                p = (caddr_t)ph.p_vaddr;
                ofw_claim(p,(ph.p_filesz > ph.p_memsz) ? 
                    ph.p_filesz : ph.p_memsz,0);
                if (fsread(ino, p, ph.p_filesz) != ph.p_filesz) {
                        printf("Can't read content of section %d\n", i);
                        return;
                }
                if (ph.p_filesz != ph.p_memsz)
                        bzero(p + ph.p_filesz, ph.p_memsz - ph.p_filesz);
                __syncicache(p, ph.p_memsz);
        }
        ofw_close(bootdev);
        (*(void (*)(void *, int, ofwfp_t, char *, int))eh.e_entry)(NULL, 0,
            realofw, NULL, 0);
}

static int
dskread(void *buf, uint64_t lba, int nblk)
{
        /*
         * The Open Firmware should open the correct partition for us.
         * That means, if we read from offset zero on an open instance handle,
         * we should read from offset zero of that partition.
         */
        ofw_seek(bootdev, lba * DEV_BSIZE);
        ofw_read(bootdev, buf, nblk * DEV_BSIZE);
        return (0);
}

static void
panic(const char *fmt, ...)
{
        char buf[128];
        va_list ap;

        va_start(ap, fmt);
        vsnprintf(buf, sizeof buf, fmt, ap);
        printf("panic: %s\n", buf);
        va_end(ap);

        exit(1);
}

static int
printf(const char *fmt, ...)
{
        va_list ap;
        int ret;

        va_start(ap, fmt);
        ret = vprintf(fmt, ap);
        va_end(ap);
        return (ret);
}

static int
putchar(char c, void *arg)
{
        char buf;

        if (c == '\n') {
                buf = '\r';
                ofw_write(stdouth, &buf, 1);
        }
        buf = c;
        ofw_write(stdouth, &buf, 1);
        return (1);
}

static int
vprintf(const char *fmt, va_list ap)
{
        int ret;

        ret = __printf(fmt, putchar, 0, ap);
        return (ret);
}

static int
vsnprintf(char *str, size_t sz, const char *fmt, va_list ap)
{
        struct sp_data sp;
        int ret;

        sp.sp_buf = str;
        sp.sp_len = 0;
        sp.sp_size = sz;
        ret = __printf(fmt, __sputc, &sp, ap);
        return (ret);
}

static int
__printf(const char *fmt, putc_func_t *putc, void *arg, va_list ap)
{
        char buf[(sizeof(long) * 8) + 1];
        char *nbuf;
        u_long ul;
        u_int ui;
        int lflag;
        int sflag;
        char *s;
        int pad;
        int ret;
        int c;

        nbuf = &buf[sizeof buf - 1];
        ret = 0;
        while ((c = *fmt++) != 0) {
                if (c != '%') {
                        ret += putc(c, arg);
                        continue;
                }
                lflag = 0;
                sflag = 0;
                pad = 0;
reswitch:       c = *fmt++;
                switch (c) {
                case '#':
                        sflag = 1;
                        goto reswitch;
                case '%':
                        ret += putc('%', arg);
                        break;
                case 'c':
                        c = va_arg(ap, int);
                        ret += putc(c, arg);
                        break;
                case 'd':
                        if (lflag == 0) {
                                ui = (u_int)va_arg(ap, int);
                                if (ui < (int)ui) {
                                        ui = -ui;
                                        ret += putc('-', arg);
                                }
                                s = __uitoa(nbuf, ui, 10);
                        } else {
                                ul = (u_long)va_arg(ap, long);
                                if (ul < (long)ul) {
                                        ul = -ul;
                                        ret += putc('-', arg);
                                }
                                s = __ultoa(nbuf, ul, 10);
                        }
                        ret += __puts(s, putc, arg);
                        break;
                case 'l':
                        lflag = 1;
                        goto reswitch;
                case 'o':
                        if (lflag == 0) {
                                ui = (u_int)va_arg(ap, u_int);
                                s = __uitoa(nbuf, ui, 8);
                        } else {
                                ul = (u_long)va_arg(ap, u_long);
                                s = __ultoa(nbuf, ul, 8);
                        }
                        ret += __puts(s, putc, arg);
                        break;
                case 'p':
                        ul = (u_long)va_arg(ap, void *);
                        s = __ultoa(nbuf, ul, 16);
                        ret += __puts("0x", putc, arg);
                        ret += __puts(s, putc, arg);
                        break;
                case 's':
                        s = va_arg(ap, char *);
                        ret += __puts(s, putc, arg);
                        break;
                case 'u':
                        if (lflag == 0) {
                                ui = va_arg(ap, u_int);
                                s = __uitoa(nbuf, ui, 10);
                        } else {
                                ul = va_arg(ap, u_long);
                                s = __ultoa(nbuf, ul, 10);
                        }
                        ret += __puts(s, putc, arg);
                        break;
                case 'x':
                        if (lflag == 0) {
                                ui = va_arg(ap, u_int);
                                s = __uitoa(nbuf, ui, 16);
                        } else {
                                ul = va_arg(ap, u_long);
                                s = __ultoa(nbuf, ul, 16);
                        }
                        if (sflag)
                                ret += __puts("0x", putc, arg);
                        ret += __puts(s, putc, arg);
                        break;
                case '0': case '1': case '2': case '3': case '4':
                case '5': case '6': case '7': case '8': case '9':
                        pad = pad * 10 + c - '0';
                        goto reswitch;
                default:
                        break;
                }
        }
        return (ret);
}

static int
__sputc(char c, void *arg)
{
        struct sp_data *sp;

        sp = arg;
        if (sp->sp_len < sp->sp_size)
                sp->sp_buf[sp->sp_len++] = c;
        sp->sp_buf[sp->sp_len] = '\0';
        return (1);
}

static int
__puts(const char *s, putc_func_t *putc, void *arg)
{
        const char *p;
        int ret;

        ret = 0;
        for (p = s; *p != '\0'; p++)
                ret += putc(*p, arg);
        return (ret);
}

static char *
__uitoa(char *buf, u_int ui, int base)
{
        char *p;

        p = buf;
        *p = '\0';
        do
                *--p = digits[ui % base];
        while ((ui /= base) != 0);
        return (p);
}

static char *
__ultoa(char *buf, u_long ul, int base)
{
        char *p;

        p = buf;
        *p = '\0';
        do
                *--p = digits[ul % base];
        while ((ul /= base) != 0);
        return (p);
}