int __cpuinit vmm_scheduler_init(void)
{
int rc;
char vcpu_name[VMM_FIELD_NAME_SIZE];
u32 cpu = vmm_smp_processor_id();
struct vmm_scheduler_ctrl *schedp = &this_cpu(sched);
/* Reset the scheduler control structure */
memset(schedp, 0, sizeof(struct vmm_scheduler_ctrl));
/* Create ready queue (Per Host CPU) */
schedp->rq = vmm_schedalgo_rq_create();
if (!schedp->rq) {
return VMM_EFAIL;
}
INIT_SPIN_LOCK(&schedp->rq_lock);
/* Initialize current VCPU. (Per Host CPU) */
schedp->current_vcpu = NULL;
/* Initialize IRQ state (Per Host CPU) */
schedp->irq_context = FALSE;
schedp->irq_regs = NULL;
/* Initialize yield on exit (Per Host CPU) */
schedp->yield_on_irq_exit = FALSE;
/* Create timer event and start it. (Per Host CPU) */
INIT_TIMER_EVENT(&schedp->ev, &vmm_scheduler_timer_event, schedp);
/* Create idle orphan vcpu with default time slice. (Per Host CPU) */
vmm_snprintf(vcpu_name, sizeof(vcpu_name), "idle/%d", cpu);
schedp->idle_vcpu = vmm_manager_vcpu_orphan_create(vcpu_name,
(virtual_addr_t)&idle_orphan,
IDLE_VCPU_STACK_SZ,
IDLE_VCPU_PRIORITY,
IDLE_VCPU_TIMESLICE);
if (!schedp->idle_vcpu) {
return VMM_EFAIL;
}
/* The idle vcpu need to stay on this cpu */
if ((rc = vmm_manager_vcpu_set_affinity(schedp->idle_vcpu,
vmm_cpumask_of(cpu)))) {
return rc;
}
/* Kick idle orphan vcpu */
if ((rc = vmm_manager_vcpu_kick(schedp->idle_vcpu))) {
return rc;
}
/* Start scheduler timer event */
vmm_timer_event_start(&schedp->ev, 0);
/* Mark this CPU online */
vmm_set_cpu_online(cpu, TRUE);
return VMM_OK;
}
int __cpuinit twd_clockchip_init(virtual_addr_t base,
virtual_addr_t ref_counter_addr,
u32 ref_counter_freq,
u32 ppi_hirq)
{
int rc;
u32 cpu = vmm_smp_processor_id();
struct twd_clockchip *cc = &this_cpu(twd_cc);
memset(cc, 0, sizeof(struct twd_clockchip));
twd_caliberate_freq(base, ref_counter_addr, ref_counter_freq);
vmm_sprintf(cc->name, "twd/%d", cpu);
cc->base = base;
cc->clkchip.name = cc->name;
cc->clkchip.hirq = ppi_hirq;
cc->clkchip.rating = 350;
cc->clkchip.cpumask = vmm_cpumask_of(cpu);
cc->clkchip.features =
VMM_CLOCKCHIP_FEAT_PERIODIC | VMM_CLOCKCHIP_FEAT_ONESHOT;
cc->clkchip.shift = 20;
cc->clkchip.mult = vmm_clockchip_hz2mult(twd_freq_hz, cc->clkchip.shift);
cc->clkchip.min_delta_ns = vmm_clockchip_delta2ns(0xF, &cc->clkchip);
cc->clkchip.max_delta_ns =
vmm_clockchip_delta2ns(0xFFFFFFFF, &cc->clkchip);
cc->clkchip.set_mode = &twd_clockchip_set_mode;
cc->clkchip.set_next_event = &twd_clockchip_set_next_event;
cc->clkchip.expire = &twd_clockchip_expire;
cc->clkchip.priv = cc;
if (!cpu) {
/* Register interrupt handler */
if ((rc = vmm_host_irq_register(ppi_hirq, "twd",
&twd_clockchip_irq_handler,
cc))) {
return rc;
}
/* Mark interrupt as per-cpu */
if ((rc = vmm_host_irq_mark_per_cpu(ppi_hirq))) {
return rc;
}
}
/* Explicitly enable local timer PPI in GIC
* Note: Local timer requires PPI support hence requires GIC
*/
gic_enable_ppi(ppi_hirq);
return vmm_clockchip_register(&cc->clkchip);
}
static int cmd_host_irq_set_affinity(struct vmm_chardev *cdev, u32 hirq, u32 hcpu)
{
if (CONFIG_CPU_COUNT <= hcpu) {
vmm_cprintf(cdev, "%s: invalid host CPU%d\n", __func__, hcpu);
return VMM_EINVALID;
}
if (!vmm_cpu_online(hcpu)) {
vmm_cprintf(cdev, "%s: host CPU%d not online\n", __func__, hcpu);
return VMM_EINVALID;
}
return vmm_host_irq_set_affinity(hirq, vmm_cpumask_of(hcpu), TRUE);
}
static int semaphore5_run(struct wboxtest *test, struct vmm_chardev *cdev,
u32 test_hcpu)
{
int i, ret = VMM_OK;
char wname[VMM_FIELD_NAME_SIZE];
u8 current_priority = vmm_scheduler_current_priority();
const struct vmm_cpumask *cpu_mask = vmm_cpumask_of(test_hcpu);
/* Initialise global data */
memset(workers, 0, sizeof(workers));
/* Create worker threads */
for (i = 0; i < NUM_THREADS; i++) {
vmm_snprintf(wname, VMM_FIELD_NAME_SIZE,
"semaphore5_worker%d", i);
workers[i] = vmm_threads_create(wname,
semaphore5_worker_thread_main,
(void *)(unsigned long)i,
current_priority,
VMM_THREAD_DEF_TIME_SLICE);
if (workers[i] == NULL) {
ret = VMM_EFAIL;
goto destroy_workers;
}
vmm_threads_set_affinity(workers[i], cpu_mask);
}
/* Do the test */
ret = semaphore5_do_test(cdev);
/* Destroy worker threads */
destroy_workers:
for (i = 0; i < NUM_THREADS; i++) {
if (workers[i]) {
vmm_threads_destroy(workers[i]);
workers[i] = NULL;
}
}
return ret;
}
static int __cpuinit twd_clockchip_init(struct vmm_devtree_node *node)
{
int rc;
u32 ref_cnt_freq;
virtual_addr_t ref_cnt_addr;
u32 cpu = vmm_smp_processor_id();
struct twd_clockchip *cc = &this_cpu(twd_cc);
if (!twd_base) {
rc = vmm_devtree_regmap(node, &twd_base, 0);
if (rc) {
goto fail;
}
}
if (!twd_ppi_irq) {
rc = vmm_devtree_irq_get(node, &twd_ppi_irq, 0);
if (rc) {
goto fail_regunmap;
}
}
if (!twd_freq_hz) {
/* First try to find TWD clock */
if (!twd_clk) {
twd_clk = of_clk_get(node, 0);
}
if (!twd_clk) {
twd_clk = clk_get_sys("smp_twd", NULL);
}
if (twd_clk) {
/* Use TWD clock to find frequency */
rc = clk_prepare_enable(twd_clk);
if (rc) {
clk_put(twd_clk);
goto fail_regunmap;
}
twd_freq_hz = clk_get_rate(twd_clk);
} else {
/* No TWD clock found hence caliberate */
rc = vmm_devtree_regmap(node, &ref_cnt_addr, 1);
if (rc) {
vmm_devtree_regunmap(node, ref_cnt_addr, 1);
goto fail_regunmap;
}
if (vmm_devtree_read_u32(node, "ref-counter-freq",
&ref_cnt_freq)) {
vmm_devtree_regunmap(node, ref_cnt_addr, 1);
goto fail_regunmap;
}
twd_caliberate_freq(twd_base,
ref_cnt_addr, ref_cnt_freq);
vmm_devtree_regunmap(node, ref_cnt_addr, 1);
}
}
memset(cc, 0, sizeof(struct twd_clockchip));
vmm_sprintf(cc->name, "twd/%d", cpu);
cc->clkchip.name = cc->name;
cc->clkchip.hirq = twd_ppi_irq;
cc->clkchip.rating = 350;
cc->clkchip.cpumask = vmm_cpumask_of(cpu);
cc->clkchip.features =
VMM_CLOCKCHIP_FEAT_PERIODIC | VMM_CLOCKCHIP_FEAT_ONESHOT;
vmm_clocks_calc_mult_shift(&cc->clkchip.mult, &cc->clkchip.shift,
VMM_NSEC_PER_SEC, twd_freq_hz, 10);
cc->clkchip.min_delta_ns = vmm_clockchip_delta2ns(0xF, &cc->clkchip);
cc->clkchip.max_delta_ns =
vmm_clockchip_delta2ns(0xFFFFFFFF, &cc->clkchip);
cc->clkchip.set_mode = &twd_clockchip_set_mode;
cc->clkchip.set_next_event = &twd_clockchip_set_next_event;
cc->clkchip.priv = cc;
if (vmm_smp_is_bootcpu()) {
/* Register interrupt handler */
if ((rc = vmm_host_irq_register(twd_ppi_irq, "twd",
&twd_clockchip_irq_handler,
cc))) {
goto fail_regunmap;
}
/* Mark interrupt as per-cpu */
if ((rc = vmm_host_irq_mark_per_cpu(twd_ppi_irq))) {
goto fail_unreg_irq;
}
}
/* Explicitly enable local timer PPI in GIC
* Note: Local timer requires PPI support hence requires GIC
*/
gic_enable_ppi(twd_ppi_irq);
rc = vmm_clockchip_register(&cc->clkchip);
if (rc) {
goto fail_unreg_irq;
}
return VMM_OK;
fail_unreg_irq:
if (vmm_smp_is_bootcpu()) {
vmm_host_irq_unregister(twd_ppi_irq, cc);
}
fail_regunmap:
vmm_devtree_regunmap(node, twd_base, 0);
fail:
return rc;
}
int vmm_scheduler_state_change(struct vmm_vcpu *vcpu, u32 new_state)
{
u64 tstamp;
int rc = VMM_OK;
irq_flags_t flags;
bool preempt = FALSE;
u32 chcpu = vmm_smp_processor_id(), vhcpu;
struct vmm_scheduler_ctrl *schedp;
u32 current_state;
if (!vcpu) {
return VMM_EFAIL;
}
vmm_write_lock_irqsave_lite(&vcpu->sched_lock, flags);
vhcpu = vcpu->hcpu;
schedp = &per_cpu(sched, vhcpu);
current_state = arch_atomic_read(&vcpu->state);
switch(new_state) {
case VMM_VCPU_STATE_UNKNOWN:
/* Existing VCPU being destroyed */
rc = vmm_schedalgo_vcpu_cleanup(vcpu);
break;
case VMM_VCPU_STATE_RESET:
if (current_state == VMM_VCPU_STATE_UNKNOWN) {
/* New VCPU */
rc = vmm_schedalgo_vcpu_setup(vcpu);
} else if (current_state != VMM_VCPU_STATE_RESET) {
/* Existing VCPU */
/* Make sure VCPU is not in a ready queue */
if ((schedp->current_vcpu != vcpu) &&
(current_state == VMM_VCPU_STATE_READY)) {
if ((rc = rq_detach(schedp, vcpu))) {
break;
}
}
/* Make sure current VCPU is preempted */
if ((schedp->current_vcpu == vcpu) &&
(current_state == VMM_VCPU_STATE_RUNNING)) {
preempt = TRUE;
}
vcpu->reset_count++;
if ((rc = arch_vcpu_init(vcpu))) {
break;
}
if ((rc = vmm_vcpu_irq_init(vcpu))) {
break;
}
} else {
rc = VMM_EFAIL;
}
break;
case VMM_VCPU_STATE_READY:
if ((current_state == VMM_VCPU_STATE_RESET) ||
(current_state == VMM_VCPU_STATE_PAUSED)) {
/* Enqueue VCPU to ready queue */
rc = rq_enqueue(schedp, vcpu);
if (!rc && (schedp->current_vcpu != vcpu)) {
preempt = rq_prempt_needed(schedp);
}
} else {
rc = VMM_EFAIL;
}
break;
case VMM_VCPU_STATE_PAUSED:
case VMM_VCPU_STATE_HALTED:
if ((current_state == VMM_VCPU_STATE_READY) ||
(current_state == VMM_VCPU_STATE_RUNNING)) {
/* Expire timer event if current VCPU
* is paused or halted
*/
if (schedp->current_vcpu == vcpu) {
preempt = TRUE;
} else if (current_state == VMM_VCPU_STATE_READY) {
/* Make sure VCPU is not in a ready queue */
rc = rq_detach(schedp, vcpu);
}
} else {
rc = VMM_EFAIL;
}
break;
}
if (rc == VMM_OK) {
tstamp = vmm_timer_timestamp();
switch (current_state) {
case VMM_VCPU_STATE_READY:
vcpu->state_ready_nsecs +=
tstamp - vcpu->state_tstamp;
break;
case VMM_VCPU_STATE_RUNNING:
vcpu->state_running_nsecs +=
tstamp - vcpu->state_tstamp;
break;
case VMM_VCPU_STATE_PAUSED:
vcpu->state_paused_nsecs +=
tstamp - vcpu->state_tstamp;
break;
case VMM_VCPU_STATE_HALTED:
vcpu->state_halted_nsecs +=
tstamp - vcpu->state_tstamp;
break;
default:
break;
}
if (new_state == VMM_VCPU_STATE_RESET) {
vcpu->state_ready_nsecs = 0;
vcpu->state_running_nsecs = 0;
vcpu->state_paused_nsecs = 0;
vcpu->state_halted_nsecs = 0;
vcpu->reset_tstamp = tstamp;
}
arch_atomic_write(&vcpu->state, new_state);
vcpu->state_tstamp = tstamp;
}
vmm_write_unlock_irqrestore_lite(&vcpu->sched_lock, flags);
if (preempt && schedp->current_vcpu) {
if (chcpu == vhcpu) {
if (schedp->current_vcpu->is_normal) {
schedp->yield_on_irq_exit = TRUE;
} else if (schedp->irq_context) {
vmm_scheduler_preempt_orphan(schedp->irq_regs);
} else {
arch_vcpu_preempt_orphan();
}
} else {
vmm_smp_ipi_async_call(vmm_cpumask_of(vhcpu),
scheduler_ipi_resched,
NULL, NULL, NULL);
}
}
return rc;
}
static int __cpuinit generic_timer_clockchip_init(struct vmm_devtree_node *node)
{
int rc;
u32 irq[4], num_irqs, val;
struct vmm_clockchip *cc;
/* Get and Check generic timer frequency */
generic_timer_get_freq(node);
if (generic_timer_hz == 0) {
return VMM_EFAIL;
}
/* Get hypervisor timer irq number */
irq[GENERIC_HYPERVISOR_TIMER] = vmm_devtree_irq_parse_map(node,
GENERIC_HYPERVISOR_TIMER);
if (!irq[GENERIC_HYPERVISOR_TIMER]) {
return VMM_ENODEV;
}
/* Get physical timer irq number */
irq[GENERIC_PHYSICAL_TIMER] = vmm_devtree_irq_parse_map(node,
GENERIC_PHYSICAL_TIMER);
if (!irq[GENERIC_PHYSICAL_TIMER]) {
return VMM_ENODEV;
}
/* Get virtual timer irq number */
irq[GENERIC_VIRTUAL_TIMER] = vmm_devtree_irq_parse_map(node,
GENERIC_VIRTUAL_TIMER);
if (!irq[GENERIC_VIRTUAL_TIMER]) {
return VMM_ENODEV;
}
/* Number of generic timer irqs */
num_irqs = vmm_devtree_irq_count(node);
if (!num_irqs) {
return VMM_EFAIL;
}
/* Ensure hypervisor timer is stopped */
generic_timer_stop();
/* Create generic hypervisor timer clockchip */
cc = vmm_zalloc(sizeof(struct vmm_clockchip));
if (!cc) {
return VMM_EFAIL;
}
cc->name = "gen-hyp-timer";
cc->hirq = irq[GENERIC_HYPERVISOR_TIMER];
cc->rating = 400;
cc->cpumask = vmm_cpumask_of(vmm_smp_processor_id());
cc->features = VMM_CLOCKCHIP_FEAT_ONESHOT;
vmm_clocks_calc_mult_shift(&cc->mult, &cc->shift,
VMM_NSEC_PER_SEC, generic_timer_hz, 10);
cc->min_delta_ns = vmm_clockchip_delta2ns(0xF, cc);
cc->max_delta_ns = vmm_clockchip_delta2ns(0x7FFFFFFF, cc);
cc->set_mode = &generic_timer_set_mode;
cc->set_next_event = &generic_timer_set_next_event;
cc->priv = NULL;
/* Register hypervisor timer clockchip */
rc = vmm_clockchip_register(cc);
if (rc) {
goto fail_free_cc;
}
/* Register irq handler for hypervisor timer */
rc = vmm_host_irq_register(irq[GENERIC_HYPERVISOR_TIMER],
"gen-hyp-timer",
&generic_hyp_timer_handler, cc);
if (rc) {
goto fail_unreg_cc;
}
if (num_irqs > 1) {
/* Register irq handler for physical timer */
rc = vmm_host_irq_register(irq[GENERIC_PHYSICAL_TIMER],
"gen-phys-timer",
&generic_phys_timer_handler,
NULL);
if (rc) {
goto fail_unreg_htimer;
}
}
if (num_irqs > 2) {
/* Register irq handler for virtual timer */
rc = vmm_host_irq_register(irq[GENERIC_VIRTUAL_TIMER],
"gen-virt-timer",
&generic_virt_timer_handler,
NULL);
if (rc) {
goto fail_unreg_ptimer;
}
}
if (num_irqs > 1) {
val = generic_timer_reg_read(GENERIC_TIMER_REG_HCTL);
val |= GENERIC_TIMER_HCTL_KERN_PCNT_EN;
val |= GENERIC_TIMER_HCTL_KERN_PTMR_EN;
generic_timer_reg_write(GENERIC_TIMER_REG_HCTL, val);
}
return VMM_OK;
fail_unreg_ptimer:
if (num_irqs > 1) {
vmm_host_irq_unregister(irq[GENERIC_PHYSICAL_TIMER],
&generic_phys_timer_handler);
}
fail_unreg_htimer:
vmm_host_irq_unregister(irq[GENERIC_HYPERVISOR_TIMER],
&generic_hyp_timer_handler);
fail_unreg_cc:
vmm_clockchip_unregister(cc);
fail_free_cc:
vmm_free(cc);
return rc;
}
static int __cpuinit bcm2835_clockchip_init(struct vmm_devtree_node *node)
{
int rc;
u32 clock, hirq;
struct bcm2835_clockchip *bcc;
/* Read clock frequency */
rc = vmm_devtree_clock_frequency(node, &clock);
if (rc) {
return rc;
}
/* Read irq attribute */
rc = vmm_devtree_irq_get(node, &hirq, DEFAULT_TIMER);
if (rc) {
return rc;
}
bcc = vmm_zalloc(sizeof(struct bcm2835_clockchip));
if (!bcc) {
return VMM_ENOMEM;
}
/* Map timer registers */
rc = vmm_devtree_regmap(node, &bcc->base, 0);
if (rc) {
vmm_free(bcc);
return rc;
}
bcc->system_clock = (void *)(bcc->base + REG_COUNTER_LO);
bcc->control = (void *)(bcc->base + REG_CONTROL);
bcc->compare = (void *)(bcc->base + REG_COMPARE(DEFAULT_TIMER));
bcc->match_mask = 1 << DEFAULT_TIMER;
/* Setup clockchip */
bcc->clkchip.name = "bcm2835-clkchip";
bcc->clkchip.hirq = hirq;
bcc->clkchip.rating = 300;
bcc->clkchip.cpumask = vmm_cpumask_of(0);
bcc->clkchip.features = VMM_CLOCKCHIP_FEAT_ONESHOT;
vmm_clocks_calc_mult_shift(&bcc->clkchip.mult,
&bcc->clkchip.shift,
VMM_NSEC_PER_SEC, clock, 10);
bcc->clkchip.min_delta_ns = vmm_clockchip_delta2ns(MIN_REG_COMPARE,
&bcc->clkchip);
bcc->clkchip.max_delta_ns = vmm_clockchip_delta2ns(MAX_REG_COMPARE,
&bcc->clkchip);
bcc->clkchip.set_mode = &bcm2835_clockchip_set_mode;
bcc->clkchip.set_next_event = &bcm2835_clockchip_set_next_event;
bcc->clkchip.priv = bcc;
/* Make sure compare register is set to zero */
vmm_writel(0x0, bcc->compare);
/* Make sure pending timer interrupts acknowledged */
if (vmm_readl(bcc->control) & bcc->match_mask) {
vmm_writel(bcc->match_mask, bcc->control);
}
/* Register interrupt handler */
rc = vmm_host_irq_register(hirq, "bcm2835_timer",
&bcm2835_clockchip_irq_handler, bcc);
if (rc) {
vmm_devtree_regunmap(node, bcc->base, 0);
vmm_free(bcc);
return rc;
}
/* Register clockchip */
rc = vmm_clockchip_register(&bcc->clkchip);
if (rc) {
vmm_host_irq_unregister(hirq, bcc);
vmm_devtree_regunmap(node, bcc->base, 0);
vmm_free(bcc);
return rc;
}
return VMM_OK;
}
int __cpuinit generic_timer_clockchip_init(void)
{
int rc;
u32 irq[3], num_irqs, val;
struct vmm_clockchip *cc;
struct vmm_devtree_node *node;
/* Find generic timer device tree node */
node = vmm_devtree_find_matching(NULL, generic_timer_match);
if (!node) {
return VMM_ENODEV;
}
/* Determine generic timer frequency */
if (generic_timer_hz == 0) {
rc = vmm_devtree_clock_frequency(node, &generic_timer_hz);
if (rc) {
/* Use preconfigured counter frequency
* in absence of dts node
*/
generic_timer_hz =
generic_timer_reg_read(GENERIC_TIMER_REG_FREQ);
} else if (generic_timer_freq_writeable()) {
/* Program the counter frequency as per the dts node */
generic_timer_reg_write(GENERIC_TIMER_REG_FREQ,
generic_timer_hz);
}
}
if (generic_timer_hz == 0) {
return VMM_EFAIL;
}
/* Get hypervisor timer irq number */
rc = vmm_devtree_irq_get(node,
&irq[GENERIC_HYPERVISOR_TIMER],
GENERIC_HYPERVISOR_TIMER);
if (rc) {
return rc;
}
/* Get physical timer irq number */
rc = vmm_devtree_irq_get(node,
&irq[GENERIC_PHYSICAL_TIMER],
GENERIC_PHYSICAL_TIMER);
if (rc) {
return rc;
}
/* Get virtual timer irq number */
rc = vmm_devtree_irq_get(node,
&irq[GENERIC_VIRTUAL_TIMER],
GENERIC_VIRTUAL_TIMER);
if (rc) {
return rc;
}
/* Number of generic timer irqs */
num_irqs = vmm_devtree_irq_count(node);
if (!num_irqs) {
return VMM_EFAIL;
}
/* Ensure hypervisor timer is stopped */
generic_timer_stop();
/* Create generic hypervisor timer clockchip */
cc = vmm_zalloc(sizeof(struct vmm_clockchip));
if (!cc) {
return VMM_EFAIL;
}
cc->name = "gen-hyp-timer";
cc->hirq = irq[GENERIC_HYPERVISOR_TIMER];
cc->rating = 400;
cc->cpumask = vmm_cpumask_of(vmm_smp_processor_id());
cc->features = VMM_CLOCKCHIP_FEAT_ONESHOT;
vmm_clocks_calc_mult_shift(&cc->mult, &cc->shift,
VMM_NSEC_PER_SEC, generic_timer_hz, 10);
cc->min_delta_ns = vmm_clockchip_delta2ns(0xF, cc);
cc->max_delta_ns = vmm_clockchip_delta2ns(0x7FFFFFFF, cc);
cc->set_mode = &generic_timer_set_mode;
cc->set_next_event = &generic_timer_set_next_event;
cc->priv = NULL;
/* Register hypervisor timer clockchip */
rc = vmm_clockchip_register(cc);
if (rc) {
goto fail_free_cc;
}
if (!vmm_smp_processor_id()) {
/* Register irq handler for hypervisor timer */
rc = vmm_host_irq_register(irq[GENERIC_HYPERVISOR_TIMER],
"gen-hyp-timer",
&generic_hyp_timer_handler, cc);
if (rc) {
goto fail_unreg_cc;
}
/* Mark hypervisor timer irq as per-CPU */
if ((rc = vmm_host_irq_mark_per_cpu(cc->hirq))) {
goto fail_unreg_htimer;
}
if (num_irqs > 1) {
/* Register irq handler for physical timer */
rc = vmm_host_irq_register(irq[GENERIC_PHYSICAL_TIMER],
"gen-phys-timer",
&generic_phys_timer_handler,
NULL);
if (rc) {
goto fail_unreg_htimer;
}
/* Mark physical timer irq as per-CPU */
rc = vmm_host_irq_mark_per_cpu(
irq[GENERIC_PHYSICAL_TIMER]);
if (rc) {
goto fail_unreg_ptimer;
}
}
if (num_irqs > 2) {
/* Register irq handler for virtual timer */
rc = vmm_host_irq_register(irq[GENERIC_VIRTUAL_TIMER],
"gen-virt-timer",
&generic_virt_timer_handler,
NULL);
if (rc) {
goto fail_unreg_ptimer;
}
/* Mark virtual timer irq as per-CPU */
rc = vmm_host_irq_mark_per_cpu(
irq[GENERIC_VIRTUAL_TIMER]);
if (rc) {
goto fail_unreg_vtimer;
}
}
}
if (num_irqs > 1) {
val = generic_timer_reg_read(GENERIC_TIMER_REG_HCTL);
val |= GENERIC_TIMER_HCTL_KERN_PCNT_EN;
val |= GENERIC_TIMER_HCTL_KERN_PTMR_EN;
generic_timer_reg_write(GENERIC_TIMER_REG_HCTL, val);
}
for (val = 0; val < num_irqs; val++) {
gic_enable_ppi(irq[val]);
}
return VMM_OK;
fail_unreg_vtimer:
if (!vmm_smp_processor_id() && num_irqs > 2) {
vmm_host_irq_unregister(irq[GENERIC_HYPERVISOR_TIMER],
&generic_virt_timer_handler);
}
fail_unreg_ptimer:
if (!vmm_smp_processor_id() && num_irqs > 1) {
vmm_host_irq_unregister(irq[GENERIC_PHYSICAL_TIMER],
&generic_phys_timer_handler);
}
fail_unreg_htimer:
if (!vmm_smp_processor_id()) {
vmm_host_irq_unregister(irq[GENERIC_HYPERVISOR_TIMER],
&generic_hyp_timer_handler);
}
fail_unreg_cc:
vmm_clockchip_register(cc);
fail_free_cc:
vmm_free(cc);
return rc;
}
int __cpuinit epit_clockchip_init(void)
{
int rc = VMM_ENODEV;
u32 clock, hirq, timer_num, *val;
struct vmm_devtree_node *node;
struct epit_clockchip *ecc;
/* find the first epit compatible node */
node = vmm_devtree_find_compatible(NULL, NULL, "freescale,epit-timer");
if (!node) {
goto fail;
}
/* Read clock frequency */
rc = vmm_devtree_clock_frequency(node, &clock);
if (rc) {
goto fail;
}
/* Read timer_num attribute */
val = vmm_devtree_attrval(node, "timer_num");
if (!val) {
rc = VMM_ENOTAVAIL;
goto fail;
}
timer_num = *val;
/* Read irq attribute */
rc = vmm_devtree_irq_get(node, &hirq, 0);
if (rc) {
goto fail;
}
/* allocate our struct */
ecc = vmm_zalloc(sizeof(struct epit_clockchip));
if (!ecc) {
rc = VMM_ENOMEM;
goto fail;
}
/* Map timer registers */
rc = vmm_devtree_regmap(node, &ecc->base, 0);
if (rc) {
goto regmap_fail;
}
ecc->match_mask = 1 << timer_num;
ecc->timer_num = timer_num;
/* Setup clockchip */
ecc->clkchip.name = node->name;
ecc->clkchip.hirq = hirq;
ecc->clkchip.rating = 300;
ecc->clkchip.cpumask = vmm_cpumask_of(0);
ecc->clkchip.features = VMM_CLOCKCHIP_FEAT_ONESHOT;
vmm_clocks_calc_mult_shift(&ecc->clkchip.mult,
&ecc->clkchip.shift,
VMM_NSEC_PER_SEC, clock, 10);
ecc->clkchip.min_delta_ns = vmm_clockchip_delta2ns(MIN_REG_COMPARE,
&ecc->clkchip);
ecc->clkchip.max_delta_ns = vmm_clockchip_delta2ns(MAX_REG_COMPARE,
&ecc->clkchip);
ecc->clkchip.set_mode = epit_set_mode;
ecc->clkchip.set_next_event = epit_set_next_event;
ecc->clkchip.priv = ecc;
/*
* Initialise to a known state (all timers off, and timing reset)
*/
vmm_writel(0x0, (void *)(ecc->base + EPITCR));
/*
* Initialize the load register to the max value to decrement.
*/
vmm_writel(0xffffffff, (void *)(ecc->base + EPITLR));
/*
* enable the timer, set it to the high reference clock,
* allow the timer to work in WAIT mode.
*/
vmm_writel(EPITCR_EN | EPITCR_CLKSRC_REF_HIGH | EPITCR_WAITEN,
(void *)(ecc->base + EPITCR));
/* Register interrupt handler */
rc = vmm_host_irq_register(hirq, ecc->clkchip.name,
&epit_timer_interrupt, ecc);
if (rc) {
goto irq_fail;
}
/* Register clockchip */
rc = vmm_clockchip_register(&ecc->clkchip);
if (rc) {
goto register_fail;
}
return VMM_OK;
register_fail:
vmm_host_irq_unregister(hirq, ecc);
irq_fail:
vmm_devtree_regunmap(node, ecc->base, 0);
regmap_fail:
vmm_free(ecc);
fail:
return rc;
}
int __cpuinit twd_clockchip_init(virtual_addr_t ref_counter_addr,
u32 ref_counter_freq)
{
int rc;
u32 cpu = vmm_smp_processor_id();
struct vmm_devtree_node *node;
struct twd_clockchip *cc = &this_cpu(twd_cc);
node = vmm_devtree_find_matching(NULL, twd_match);
if (!node) {
return VMM_ENODEV;
}
if (!twd_base) {
rc = vmm_devtree_regmap(node, &twd_base, 0);
if (rc) {
return rc;
}
}
if (!twd_ppi_irq) {
rc = vmm_devtree_irq_get(node, &twd_ppi_irq, 0);
if (rc) {
return rc;
}
}
twd_caliberate_freq(twd_base, ref_counter_addr, ref_counter_freq);
memset(cc, 0, sizeof(struct twd_clockchip));
vmm_sprintf(cc->name, "twd/%d", cpu);
cc->clkchip.name = cc->name;
cc->clkchip.hirq = twd_ppi_irq;
cc->clkchip.rating = 350;
cc->clkchip.cpumask = vmm_cpumask_of(cpu);
cc->clkchip.features =
VMM_CLOCKCHIP_FEAT_PERIODIC | VMM_CLOCKCHIP_FEAT_ONESHOT;
vmm_clocks_calc_mult_shift(&cc->clkchip.mult, &cc->clkchip.shift,
VMM_NSEC_PER_SEC, twd_freq_hz, 10);
cc->clkchip.min_delta_ns = vmm_clockchip_delta2ns(0xF, &cc->clkchip);
cc->clkchip.max_delta_ns =
vmm_clockchip_delta2ns(0xFFFFFFFF, &cc->clkchip);
cc->clkchip.set_mode = &twd_clockchip_set_mode;
cc->clkchip.set_next_event = &twd_clockchip_set_next_event;
cc->clkchip.priv = cc;
if (!cpu) {
/* Register interrupt handler */
if ((rc = vmm_host_irq_register(twd_ppi_irq, "twd",
&twd_clockchip_irq_handler,
cc))) {
return rc;
}
/* Mark interrupt as per-cpu */
if ((rc = vmm_host_irq_mark_per_cpu(twd_ppi_irq))) {
return rc;
}
}
/* Explicitly enable local timer PPI in GIC
* Note: Local timer requires PPI support hence requires GIC
*/
gic_enable_ppi(twd_ppi_irq);
return vmm_clockchip_register(&cc->clkchip);
}
