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 }