3ds printk mod

#include <linux/kernel.h>
#include <linux/init.h>
#include <linux/io.h>
#include <linux/irqchip/arm-gic.h>
#include <linux/platform_device.h>

#include <linux/clk-provider.h>
#include <linux/dma-mapping.h>
#include <linux/init.h>
#include <linux/of.h>
#include <linux/of_platform.h>
#include <linux/platform_device.h>

#include <linux/tty.h>
#include <linux/tty_flip.h>

#include <linux/vt_kern.h>
#include <linux/console_struct.h>

#include <asm/mach-types.h>
#include <asm/mach/arch.h>
#include <asm/mach/map.h>
#include <asm/smp_twd.h>

#include <mach/hardware.h>
#include <mach/platform.h>

/*
static struct map_desc nintendo3ds_ctr_io_desc[] __initdata = {
    {
        .virtual    = IO_ADDRESS(NINTENDO3DS_CTR_PRIV_MEM_BASE),
        .pfn        = __phys_to_pfn(NINTENDO3DS_CTR_PRIV_MEM_BASE),
        .length     = NINTENDO3DS_CTR_PRIV_MEM_SIZE,
        .type       = MT_DEVICE,
    }
};*/


/*
static void __init nintendo3ds_ctr_init_irq(void)
{
    int i;

    printk("nintendo3ds_ctr_init_irq\n");

    // Enable clear all the GID interrupts
    // Pending clear all the GID interrupts
    for (i = 0; i < 8; i++) {
        writel(0xFFFFFFFF, __io_address(NINTENDO3DS_CTR_GIC_INT_PEND_CLR_REG(i)));
        writel(0xFFFFFFFF, __io_address(NINTENDO3DS_CTR_GIC_INT_EN_CLR_REG(i)));
    }

    gic_init(0, 29, __io_address(NINTENDO3DS_CTR_GIC_BASE),
        __io_address(NINTENDO3DS_CTR_CPU_INT_INTERFACE_BASE));

    //printk("nintendo3ds_ctr_init_irq DONE\n");

    //gic_init(unsigned int nr, int start, void __iomem *dist , void __iomem *cpu)

    // Enable CPU Interrupt Interface
    //writel(1, __io_address(NINTENDO3DS_CTR_CPU_INT_INTERFACE_CONTROL));

    // Enable Global Interrupt Distributor
    //writel(1, __io_address(NINTENDO3DS_CTR_GID_IDCR));
}*/

static void __init nintendo3ds_ctr_map_io(void)
{
    printk("nintendo3ds_ctr_map_io\n");
    //iotable_init(nintendo3ds_ctr_io_desc, ARRAY_SIZE(nintendo3ds_ctr_io_desc));
}

static void __init nintendo3ds_ctr_init_early(void)
{
}

#define HID_PAD_PA   (0x10146000)
#define HID_PAD_SIZE (4)

#define BUTTON_A      (1 << 0)
#define BUTTON_B      (1 << 1)
#define BUTTON_SELECT (1 << 2)
#define BUTTON_START  (1 << 3)
#define BUTTON_RIGHT  (1 << 4)
#define BUTTON_LEFT   (1 << 5)
#define BUTTON_UP     (1 << 6)
#define BUTTON_DOWN   (1 << 7)
#define BUTTON_R1     (1 << 8)
#define BUTTON_L1     (1 << 9)
#define BUTTON_X      (1 << 10)
#define BUTTON_Y      (1 << 11)

#define BUTTON_PRESS(b) (~ioread32(hid_pad) & b)

static void *hid_pad = NULL;

static void __init nintendo3ds_ctr_dt_init_machine(void)
{
    printk("nintendo3ds_ctr_dt_init_machine\n");
    of_platform_populate(NULL, of_default_bus_match_table, NULL, NULL);


    /* Try to map VRAM */

    /*vram_va = ioremap(NINTENDO3DS_CTR_VRAM_BASE,
        NINTENDO3DS_CTR_VRAM_SIZE);

    printk("VRAM mapped to: %p - %p\n", vram_va,
        vram_va + NINTENDO3DS_CTR_VRAM_SIZE);

    if (vram_va) {
        for (i = 0; i < 420*340*3*2; i+=4)
            iowrite32(0xFFFFFFFF, vram_va + i);
    }*/

    /* Try to map HID_PAD */
    if (!request_mem_region(HID_PAD_PA, HID_PAD_SIZE, "N3DS_HID_PAD")) {
        printk("HID_PAD region not available.\n");
    } else {
        hid_pad = ioremap_nocache(HID_PAD_PA, HID_PAD_SIZE);

        printk("HID_PAD mapped to: %p - %p\n", hid_pad,
            hid_pad + HID_PAD_SIZE);
    }
}

int n3ds_can_continue(void)
{
    volatile int i, sum = 0;
    if (hid_pad) {
        while (!BUTTON_PRESS(BUTTON_A)) {
            if (BUTTON_PRESS(BUTTON_UP)) {
                for (i = 0; i < MAX_NR_CONSOLES; i++) {
                    if (vc_cons_allocated(i)) {
                        scrollback(vc_cons[i].d, 0);
                        //tty_schedule_flip(&vc_cons[i].d->port);
                        //tty_insert_flip_char(&vc_cons[i].d->port, 65, 0);
                    }
                }
            } else if (BUTTON_PRESS(BUTTON_DOWN)) {
                for (i = 0; i < MAX_NR_CONSOLES; i++) {
                    if (vc_cons_allocated(i)) {
                        scrollfront(vc_cons[i].d, 0);
                        //tty_schedule_flip(&vc_cons[i].d->port);
                    }
                }
            }
        }
        for (i = 0; i < 1000000; i++)
            sum += i;
        return 1;
    }
    return 1;
}

static void __init nintendo3ds_ctr_restart(enum reboot_mode mode, const char *cmd)
{
    printk("nintendo3ds_ctr_restart\n");
}


static const char *nintendo3ds_ctr_dt_platform_compat[] __initconst = {
    "nintendo3ds,ctr",
    NULL,
};

DT_MACHINE_START(NINTENDO3DS_DT, "Nintendo 3DS (CTR) (Device Tree)")
    .map_io     = nintendo3ds_ctr_map_io,
    .init_early = nintendo3ds_ctr_init_early,
    .init_machine   = nintendo3ds_ctr_dt_init_machine,
    .dt_compat  = nintendo3ds_ctr_dt_platform_compat,
    .restart    = nintendo3ds_ctr_restart,
MACHINE_END