/* FIXME: Optimize. */
int32_t dt_nr_cells(void *fdt, int node, const char *name)
{
const int32_t *nr_cells;
node = fdt_parent_offset(fdt, node);
do {
int lenp;
nr_cells = dt_get_property(fdt, node, name, &lenp);
if (nr_cells) {
return fdt32_to_cpu(*nr_cells);
}
node = fdt_parent_offset(fdt, node);
} while (node >= 0);
return -1;
}
int __init arch_cpu_init(int cpu, struct dt_device_node *dn)
{
const char *enable_method;
enable_method = dt_get_property(dn, "enable-method", NULL);
if (!enable_method)
{
printk("CPU%d has no enable method\n", cpu);
return -EINVAL;
}
if ( !strcmp(enable_method, "spin-table") )
smp_spin_table_init(cpu, dn);
else if ( !strcmp(enable_method, "psci") )
return smp_psci_init(cpu);
else
{
printk("CPU%d has unknown enable method \"%s\"\n", cpu, enable_method);
return -EINVAL;
}
return 0;
}
/* Parse the device tree and build the logical map array containing
* MPIDR values related to logical cpus
* Code base on Linux arch/arm/kernel/devtree.c
*/
void __init smp_init_cpus(void)
{
register_t mpidr;
struct dt_device_node *cpus = dt_find_node_by_path("/cpus");
struct dt_device_node *cpu;
unsigned int i, j;
unsigned int cpuidx = 1;
static u32 tmp_map[NR_CPUS] __initdata =
{
[0 ... NR_CPUS - 1] = MPIDR_INVALID
};
bool_t bootcpu_valid = 0;
int rc;
/* scan the DTB for a PSCI node and set a global variable */
psci_init();
if ( (rc = arch_smp_init()) < 0 )
{
printk(XENLOG_WARNING "SMP init failed (%d)\n"
"Using only 1 CPU\n", rc);
return;
}
mpidr = boot_cpu_data.mpidr.bits & MPIDR_HWID_MASK;
if ( !cpus )
{
printk(XENLOG_WARNING "WARNING: Can't find /cpus in the device tree.\n"
"Using only 1 CPU\n");
return;
}
dt_for_each_child_node( cpus, cpu )
{
const __be32 *prop;
u64 addr;
u32 reg_len, hwid;
if ( !dt_device_type_is_equal(cpu, "cpu") )
continue;
if ( dt_n_size_cells(cpu) != 0 )
printk(XENLOG_WARNING "cpu node `%s`: #size-cells %d\n",
dt_node_full_name(cpu), dt_n_size_cells(cpu));
prop = dt_get_property(cpu, "reg", ®_len);
if ( !prop )
{
printk(XENLOG_WARNING "cpu node `%s`: has no reg property\n",
dt_node_full_name(cpu));
continue;
}
if ( reg_len < dt_cells_to_size(dt_n_addr_cells(cpu)) )
{
printk(XENLOG_WARNING "cpu node `%s`: reg property too short\n",
dt_node_full_name(cpu));
continue;
}
addr = dt_read_number(prop, dt_n_addr_cells(cpu));
hwid = addr;
if ( hwid != addr )
{
printk(XENLOG_WARNING "cpu node `%s`: hwid overflow %"PRIx64"\n",
dt_node_full_name(cpu), addr);
continue;
}
/*
* 8 MSBs must be set to 0 in the DT since the reg property
* defines the MPIDR[23:0]
*/
if ( hwid & ~MPIDR_HWID_MASK )
{
printk(XENLOG_WARNING "cpu node `%s`: invalid hwid value (0x%x)\n",
dt_node_full_name(cpu), hwid);
continue;
}
/*
* Duplicate MPIDRs are a recipe for disaster. Scan all initialized
* entries and check for duplicates. If any found just skip the node.
* temp values values are initialized to MPIDR_INVALID to avoid
* matching valid MPIDR[23:0] values.
*/
for ( j = 0; j < cpuidx; j++ )
{
if ( tmp_map[j] == hwid )
{
printk(XENLOG_WARNING
"cpu node `%s`: duplicate /cpu reg properties %"PRIx32" in the DT\n",
dt_node_full_name(cpu), hwid);
break;
}
}
if ( j != cpuidx )
continue;
/*
* Build a stashed array of MPIDR values. Numbering scheme requires
* that if detected the boot CPU must be assigned logical id 0. Other
* CPUs get sequential indexes starting from 1. If a CPU node
* with a reg property matching the boot CPU MPIDR is detected,
* this is recorded and so that the logical map build from DT is
* validated and can be used to set the map.
*/
if ( hwid == mpidr )
{
i = 0;
bootcpu_valid = 1;
}
else
i = cpuidx++;
if ( cpuidx > NR_CPUS )
{
printk(XENLOG_WARNING
"DT /cpu %u node greater than max cores %u, capping them\n",
cpuidx, NR_CPUS);
cpuidx = NR_CPUS;
break;
}
if ( (rc = arch_cpu_init(i, cpu)) < 0 )
{
printk("cpu%d init failed (hwid %x): %d\n", i, hwid, rc);
tmp_map[i] = MPIDR_INVALID;
}
else
tmp_map[i] = hwid;
}
if ( !bootcpu_valid )
{
printk(XENLOG_WARNING "DT missing boot CPU MPIDR[23:0]\n"
"Using only 1 CPU\n");
return;
}
for ( i = 0; i < cpuidx; i++ )
{
if ( tmp_map[i] == MPIDR_INVALID )
continue;
cpumask_set_cpu(i, &cpu_possible_map);
cpu_logical_map(i) = tmp_map[i];
}
}
void __init video_init(void)
{
struct lfb_prop lfbp;
unsigned char *lfb;
paddr_t hdlcd_start, hdlcd_size;
paddr_t framebuffer_start, framebuffer_size;
const char *mode_string;
char _mode_string[16];
int bytes_per_pixel = 4;
struct color_masks *c = NULL;
struct modeline *videomode = NULL;
int i;
const struct dt_device_node *dev;
const __be32 *cells;
u32 lenp;
int res;
dev = dt_find_compatible_node(NULL, NULL, "arm,hdlcd");
if ( !dev )
{
early_printk("HDLCD: Cannot find node compatible with \"arm,hdcld\"\n");
return;
}
res = dt_device_get_address(dev, 0, &hdlcd_start, &hdlcd_size);
if ( !res )
{
early_printk("HDLCD: Unable to retrieve MMIO base address\n");
return;
}
cells = dt_get_property(dev, "framebuffer", &lenp);
if ( !cells )
{
early_printk("HDLCD: Unable to retrieve framebuffer property\n");
return;
}
framebuffer_start = dt_next_cell(dt_n_addr_cells(dev), &cells);
framebuffer_size = dt_next_cell(dt_n_size_cells(dev), &cells);
if ( !hdlcd_start )
{
early_printk(KERN_ERR "HDLCD: address missing from device tree, disabling driver\n");
return;
}
if ( !framebuffer_start )
{
early_printk(KERN_ERR "HDLCD: framebuffer address missing from device tree, disabling driver\n");
return;
}
res = dt_property_read_string(dev, "mode", &mode_string);
if ( res )
{
get_color_masks("32", &c);
memcpy(_mode_string, "1280x1024@60", strlen("1280x1024@60") + 1);
bytes_per_pixel = 4;
}
else if ( strlen(mode_string) < strlen("800x600@60") ||
strlen(mode_string) > sizeof(_mode_string) - 1 )
{
early_printk(KERN_ERR "HDLCD: invalid modeline=%s\n", mode_string);
return;
} else {
char *s = strchr(mode_string, '-');
if ( !s )
{
early_printk(KERN_INFO "HDLCD: bpp not found in modeline %s, assume 32 bpp\n",
mode_string);
get_color_masks("32", &c);
memcpy(_mode_string, mode_string, strlen(mode_string) + 1);
bytes_per_pixel = 4;
} else {
if ( strlen(s) < 6 )
{
early_printk(KERN_ERR "HDLCD: invalid mode %s\n", mode_string);
return;
}
s++;
if ( get_color_masks(s, &c) < 0 )
{
early_printk(KERN_WARNING "HDLCD: unsupported bpp %s\n", s);
return;
}
bytes_per_pixel = simple_strtoll(s, NULL, 10) / 8;
}
i = s - mode_string - 1;
memcpy(_mode_string, mode_string, i);
memcpy(_mode_string + i, mode_string + i + 3, 4);
}
for ( i = 0; i < ARRAY_SIZE(videomodes); i++ ) {
if ( !strcmp(_mode_string, videomodes[i].mode) )
{
videomode = &videomodes[i];
break;
}
}
if ( !videomode )
{
early_printk(KERN_WARNING "HDLCD: unsupported videomode %s\n",
_mode_string);
return;
}
if ( framebuffer_size < bytes_per_pixel * videomode->xres * videomode->yres )
{
early_printk(KERN_ERR "HDLCD: the framebuffer is too small, disabling the HDLCD driver\n");
return;
}
early_printk(KERN_INFO "Initializing HDLCD driver\n");
lfb = ioremap_wc(framebuffer_start, framebuffer_size);
if ( !lfb )
{
early_printk(KERN_ERR "Couldn't map the framebuffer\n");
return;
}
memset(lfb, 0x00, bytes_per_pixel * videomode->xres * videomode->yres);
/* uses FIXMAP_MISC */
set_pixclock(videomode->pixclock);
set_fixmap(FIXMAP_MISC, hdlcd_start >> PAGE_SHIFT, DEV_SHARED);
HDLCD[HDLCD_COMMAND] = 0;
HDLCD[HDLCD_LINELENGTH] = videomode->xres * bytes_per_pixel;
HDLCD[HDLCD_LINECOUNT] = videomode->yres - 1;
HDLCD[HDLCD_LINEPITCH] = videomode->xres * bytes_per_pixel;
HDLCD[HDLCD_PF] = ((bytes_per_pixel - 1) << 3);
HDLCD[HDLCD_INTMASK] = 0;
HDLCD[HDLCD_FBBASE] = framebuffer_start;
HDLCD[HDLCD_BUS] = 0xf00 | (1 << 4);
HDLCD[HDLCD_VBACK] = videomode->vback - 1;
HDLCD[HDLCD_VSYNC] = videomode->vsync - 1;
HDLCD[HDLCD_VDATA] = videomode->yres - 1;
HDLCD[HDLCD_VFRONT] = videomode->vfront - 1;
HDLCD[HDLCD_HBACK] = videomode->hback - 1;
HDLCD[HDLCD_HSYNC] = videomode->hsync - 1;
HDLCD[HDLCD_HDATA] = videomode->xres - 1;
HDLCD[HDLCD_HFRONT] = videomode->hfront - 1;
HDLCD[HDLCD_POLARITIES] = (1 << 2) | (1 << 3);
HDLCD[HDLCD_RED] = (c->red_size << 8) | c->red_shift;
HDLCD[HDLCD_GREEN] = (c->green_size << 8) | c->green_shift;
HDLCD[HDLCD_BLUE] = (c->blue_size << 8) | c->blue_shift;
HDLCD[HDLCD_COMMAND] = 1;
clear_fixmap(FIXMAP_MISC);
lfbp.pixel_on = (((1 << c->red_size) - 1) << c->red_shift) |
(((1 << c->green_size) - 1) << c->green_shift) |
(((1 << c->blue_size) - 1) << c->blue_shift);
lfbp.lfb = lfb;
lfbp.font = &font_vga_8x16;
lfbp.bits_per_pixel = bytes_per_pixel*8;
lfbp.bytes_per_line = bytes_per_pixel*videomode->xres;
lfbp.width = videomode->xres;
lfbp.height = videomode->yres;
lfbp.flush = hdlcd_flush;
lfbp.text_columns = videomode->xres / 8;
lfbp.text_rows = videomode->yres / 16;
if ( lfb_init(&lfbp) < 0 )
return;
video_puts = lfb_scroll_puts;
}