static word_t phasedetect(word_t qq, word_t ii, const int * angle_updates)
{
bool positive = !CTOP(qq) ^ CTOP(ii);
word_t angle = (CTOP(ii) << (OUT_BITS - 1)) + 1;
if (CTOP(qq ^ ii))
angle += MASK(OUT_BITS - 1) - angle_updates[ITERATIONS];
if (CTOP(qq))
qq = CMASK(~qq);
if (CTOP(ii))
ii = CMASK(~ii);
for (int i = 0; i < ITERATIONS; ++i) {
word_t ii_shift = ii >> (i * 2);
word_t qq_trial = CMASK(qq + ii_shift);
word_t ii_trial = IMASK(ii - qq);
ii = IMASK(ii << 1);
if (!BIT(ii_trial, IN_BITS)) {
if (positive)
angle = AMASK(angle + angle_updates[i]);
else
angle = AMASK(angle - angle_updates[i]);
if (i != 0) {
qq = qq_trial;
ii = IMASK(ii_trial << 1);
}
else {
word_t qq3 = qq;
qq = CMASK(ii >> 1);
ii = qq3 << 1;
positive = !positive;
}
}
}
if (ROUNDOFF_BIT >= 0) {
if (BIT(angle, ROUNDOFF_BIT & 31))
angle += 1 << (ROUNDOFF_BIT & 31);
angle &= ~MASK(ROUNDOFF_BIT & 31);
}
angle &= ~MASK(TRUNCATE_BIT);
return angle;
}
/*
* Recalculate the interrupt masks from scratch.
* We could code special registry and deregistry versions of this function that
* would be faster, but the code would be nastier, and we don't expect this to
* happen very much anyway.
*/
void
intr_calculatemasks(void)
{
int irq, level, unusedirqs;
struct intrhand *q;
/* First, figure out which levels each IRQ uses. */
unusedirqs = 0xffff;
for (irq = 0; irq < ICU_LEN; irq++) {
int levels = 0;
for (q = intrhand[irq]; q; q = q->ih_next)
levels |= 1 << IPL(q->ih_level);
intrlevel[irq] = levels;
if (levels)
unusedirqs &= ~(1 << irq);
}
/* Then figure out which IRQs use each level. */
for (level = 0; level < NIPL; level++) {
int irqs = 0;
for (irq = 0; irq < ICU_LEN; irq++)
if (intrlevel[irq] & (1 << level))
irqs |= 1 << irq;
imask[level] = irqs | unusedirqs;
}
/*
* Initialize soft interrupt masks to block themselves.
*/
IMASK(IPL_SOFTAST) |= 1 << SIR_AST;
IMASK(IPL_SOFTCLOCK) |= 1 << SIR_CLOCK;
IMASK(IPL_SOFTNET) |= 1 << SIR_NET;
IMASK(IPL_SOFTTTY) |= 1 << SIR_TTY;
/*
* Enforce a hierarchy that gives slow devices a better chance at not
* dropping data.
*/
for (level = 0; level < NIPL - 1; level++)
imask[level + 1] |= imask[level];
/* And eventually calculate the complete masks. */
for (irq = 0; irq < ICU_LEN; irq++) {
int irqs = 1 << irq;
int minlevel = IPL_NONE;
int maxlevel = IPL_NONE;
if (intrhand[irq] == NULL) {
maxlevel = IPL_HIGH;
irqs = IMASK(IPL_HIGH);
} else {
for (q = intrhand[irq]; q; q = q->ih_next) {
irqs |= IMASK(q->ih_level);
if (minlevel == IPL_NONE ||
q->ih_level < minlevel)
minlevel = q->ih_level;
if (q->ih_level > maxlevel)
maxlevel = q->ih_level;
}
}
if (irqs != IMASK(maxlevel))
panic("irq %d level %x mask mismatch: %x vs %x", irq,
maxlevel, irqs, IMASK(maxlevel));
intrmask[irq] = irqs;
iminlevel[irq] = minlevel;
imaxlevel[irq] = maxlevel;
#if 0
printf("irq %d: level %x, mask 0x%x (%x)\n", irq,
imaxlevel[irq], intrmask[irq], IMASK(imaxlevel[irq]));
#endif
}
/* Lastly, determine which IRQs are actually in use. */
{
int irqs = 0;
for (irq = 0; irq < ICU_LEN; irq++)
if (intrhand[irq])
irqs |= 1 << irq;
if (irqs >= 0x100) /* any IRQs >= 8 in use */
irqs |= 1 << IRQ_SLAVE;
imen = ~irqs;
SET_ICUS();
}
/* For speed of splx, provide the inverse of the interrupt masks. */
for (irq = 0; irq < ICU_LEN; irq++)
iunmask[irq] = ~imask[irq];
}
/*
* Determine i/o configuration for a machine.
*/
void
cpu_configure(void)
{
/*
* Note, on i386, configure is not running under splhigh unlike other
* architectures. This fact is used by the pcmcia irq line probing.
*/
gdt_init(); /* XXX - pcibios uses gdt stuff */
/* Set up proc0's TSS and LDT */
i386_proc0_tss_ldt_init();
#ifdef KVM86
kvm86_init();
#endif
if (config_rootfound("mainbus", NULL) == NULL)
panic("cpu_configure: mainbus not configured");
#if NIOAPIC > 0
if (nioapics > 0)
goto nomasks;
#endif
printf("biomask %x netmask %x ttymask %x\n", (u_short)IMASK(IPL_BIO),
(u_short)IMASK(IPL_NET), (u_short)IMASK(IPL_TTY));
#if NIOAPIC > 0
nomasks:
ioapic_enable();
#endif
proc0.p_addr->u_pcb.pcb_cr0 = rcr0();
#ifdef MULTIPROCESSOR
/* propagate TSS and LDT configuration to the idle pcb's. */
cpu_init_idle_pcbs();
#endif
spl0();
/*
* We can not know which is our root disk, defer
* until we can checksum blocks to figure it out.
*/
cold = 0;
/*
* At this point the RNG is running, and if FSXR is set we can
* use it. Here we setup a periodic timeout to collect the data.
*/
if (viac3_rnd_present) {
timeout_set(&viac3_rnd_tmo, viac3_rnd, &viac3_rnd_tmo);
viac3_rnd(&viac3_rnd_tmo);
}
#ifdef CRYPTO
/*
* Also, if the chip has crypto available, enable it.
*/
if (i386_has_xcrypt)
viac3_crypto_setup();
#endif
}
