void up_sigdeliver(void)
{
#ifndef CONFIG_DISABLE_SIGNALS
FAR _TCB *rtcb = (_TCB*)g_readytorun.head;
chipreg_t regs[XCPTCONTEXT_REGS];
sig_deliver_t sigdeliver;
/* Save the errno. This must be preserved throughout the signal handling
* so that the user code final gets the correct errno value (probably
* EINTR).
*/
int saved_errno = rtcb->pterrno;
up_ledon(LED_SIGNAL);
sdbg("rtcb=%p sigdeliver=%p sigpendactionq.head=%p\n",
rtcb, rtcb->xcp.sigdeliver, rtcb->sigpendactionq.head);
ASSERT(rtcb->xcp.sigdeliver != NULL);
/* Save the real return state on the stack. */
ez80_copystate(regs, rtcb->xcp.regs);
regs[XCPT_PC] = rtcb->xcp.saved_pc;
regs[XCPT_I] = rtcb->xcp.saved_i;
/* Get a local copy of the sigdeliver function pointer. We do this so
* that we can nullify the sigdeliver function pointer in the TCB and
* accept more signal deliveries while processing the current pending
* signals.
*/
sigdeliver = rtcb->xcp.sigdeliver;
rtcb->xcp.sigdeliver = NULL;
/* Then restore the task interrupt state. */
irqrestore(regs[XCPT_I]);
/* Deliver the signals */
sigdeliver(rtcb);
/* Output any debug messages BEFORE restoring errno (because they may
* alter errno), then disable interrupts again and restore the original
* errno that is needed by the user logic (it is probably EINTR).
*/
sdbg("Resuming\n");
(void)irqsave();
rtcb->pterrno = saved_errno;
/* Then restore the correct state for this thread of
* execution.
*/
up_ledoff(LED_SIGNAL);
ez80_restorecontext(regs);
#endif
}
void up_decodeirq(uint32_t *regs)
{
#ifdef CONFIG_SUPPRESS_INTERRUPTS
up_ledon(LED_INIRQ);
lib_lowprintf("Unexpected IRQ\n");
current_regs = regs;
PANIC(OSERR_ERREXCEPTION);
#else
unsigned int irq;
/* Read the IRQ number from the IVR register (Could probably get the same
* info from CIC register without the setup).
*/
up_ledon(LED_INIRQ);
irq = getreg32(STR71X_EIC_IVR);
/* Verify that the resulting IRQ number is valid */
if (irq < NR_IRQS)
{
uint32_t* savestate;
/* Current regs non-zero indicates that we are processing an interrupt;
* current_regs is also used to manage interrupt level context switches.
*/
savestate = (uint32_t*)current_regs;
current_regs = regs;
/* Mask and acknowledge the interrupt */
up_maskack_irq(irq);
/* Deliver the IRQ */
irq_dispatch(irq, regs);
/* Restore the previous value of current_regs. NULL would indicate that
* we are no longer in an interrupt handler. It will be non-NULL if we
* are returning from a nested interrupt.
*/
current_regs = savestate;
/* Unmask the last interrupt (global interrupts are still disabled) */
up_enable_irq(irq);
}
#if CONFIG_DEBUG
else
{
PANIC(OSERR_ERREXCEPTION); /* Normally never happens */
}
#endif
up_ledoff(LED_INIRQ);
#endif
}
uint32_t *up_doirq(int irq, uint32_t *regs)
{
up_ledon(LED_INIRQ);
#ifdef CONFIG_SUPPRESS_INTERRUPTS
PANIC(OSERR_ERREXCEPTION);
#else
uint32_t *savestate;
/* Nested interrupts are not supported in this implementation. If you want
* to implement nested interrupts, you would have to (1) change the way that
* current_regs is handled and (2) the design associated with
* CONFIG_ARCH_INTERRUPTSTACK. The savestate variable will not work for
* that purpose as implemented here because only the outermost nested
* interrupt can result in a context switch (it can probably be deleted).
*/
/* Current regs non-zero indicates that we are processing an interrupt;
* current_regs is also used to manage interrupt level context switches.
*/
savestate = (uint32_t*)current_regs;
current_regs = regs;
/* Disable further occurences of this interrupt (until the interrupt sources
* have been clear by the driver.
*/
up_disable_irq(irq);
/* Deliver the IRQ */
irq_dispatch(irq, regs);
/* If a context switch occurred while processing the interrupt then
* current_regs may have change value. If we return any value different
* from the input regs, then the lower level will know that a context
* switch occurred during interrupt processing.
*/
regs = (uint32_t*)current_regs;
/* Restore the previous value of current_regs. NULL would indicate that
* we are no longer in an interrupt handler. It will be non-NULL if we
* are returning from a nested interrupt.
*/
current_regs = savestate;
/* Unmask the last interrupt (global interrupts are still disabled) */
up_enable_irq(irq);
#endif
up_ledoff(LED_INIRQ);
return regs;
}
uint32_t *up_doirq(int irq, uint32_t* regs)
{
up_ledon(LED_INIRQ);
#ifdef CONFIG_SUPPRESS_INTERRUPTS
PANIC(OSERR_ERREXCEPTION);
#else
if ((unsigned)irq < NR_IRQS)
{
uint32_t *savestate;
/* Current regs non-zero indicates that we are processing
* an interrupt; current_regs is also used to manage
* interrupt level context switches.
*/
savestate = (uint32_t*)current_regs;
current_regs = regs;
/* Mask and acknowledge the interrupt (if supported by the chip) */
#ifndef CONFIG_ARCH_NOINTC
up_maskack_irq(irq);
#endif
/* Deliver the IRQ */
irq_dispatch(irq, regs);
/* Get the current value of regs... it may have changed because
* of a context switch performed during interrupt processing.
*/
regs = current_regs;
/* Restore the previous value of current_regs. NULL would indicate that
* we are no longer in an interrupt handler. It will be non-NULL if we
* are returning from a nested interrupt.
*/
current_regs = savestate;
/* Unmask the last interrupt (global interrupts are still
* disabled.
*/
#ifndef CONFIG_ARCH_NOINTC
up_enable_irq(irq);
#endif
}
up_ledoff(LED_INIRQ);
#endif
return regs;
}
uint32_t *isr_handler(uint32_t *regs)
{
#ifdef CONFIG_SUPPRESS_INTERRUPTS
up_ledon(LED_INIRQ);
PANIC(OSERR_ERREXCEPTION); /* Doesn't return */
return regs; /* To keep the compiler happy */
#else
uint32_t *ret;
/* Dispatch the interrupt */
up_ledon(LED_INIRQ);
ret = common_handler((int)regs[REG_IRQNO], regs);
up_ledoff(LED_INIRQ);
return ret;
#endif
}
uint8_t *up_doirq(int irq, uint8_t *regs)
{
up_ledon(LED_INIRQ);
#ifdef CONFIG_SUPPRESS_INTERRUPTS
PANIC(OSERR_ERREXCEPTION);
#else
uint8_t *savestate;
/* Nested interrupts are not supported in this implementation. If you want
* implemented nested interrupts, you would have to (1) change the way that
* current regs is handled and (2) the design associated with
* CONFIG_ARCH_INTERRUPTSTACK.
*/
/* Current regs non-zero indicates that we are processing an interrupt;
* current_regs is also used to manage interrupt level context switches.
*/
savestate = (uint8_t*)current_regs;
current_regs = regs;
/* Deliver the IRQ */
irq_dispatch(irq, regs);
/* If a context switch occurred while processing the interrupt then
* current_regs may have change value. If we return any value different
* from the input regs, then the lower level will know that a context
* switch occurred during interrupt processing.
*/
regs = (uint8_t*)current_regs;
/* Restore the previous value of current_regs. NULL would indicate that
* we are no longer in an interrupt handler. It will be non-NULL if we
* are returning from a nested interrupt.
*/
current_regs = savestate;
#endif
up_ledoff(LED_INIRQ);
return regs;
}
static void _up_assert(int errorcode)
{
/* Are we in an interrupt handler or the idle task? */
if (current_regs || ((struct tcb_s*)g_readytorun.head)->pid == 0)
{
(void)irqsave();
for(;;)
{
#ifdef CONFIG_ARCH_LEDS
up_ledon(LED_PANIC);
up_mdelay(250);
up_ledoff(LED_PANIC);
up_mdelay(250);
#endif
}
}
else
{
exit(errorcode);
}
}
static void _up_assert(int errorcode) /* __attribute__ ((noreturn)) */
{
/* Are we in an interrupt handler or the idle task? */
if (up_interrupt_context() || ((FAR _TCB*)g_readytorun.head)->pid == 0)
{
(void)irqsave();
for(;;)
{
#ifdef CONFIG_ARCH_LEDS
up_ledon(LED_PANIC);
up_mdelay(250);
up_ledoff(LED_PANIC);
up_mdelay(250);
#endif
}
}
else
{
exit(errorcode);
}
}
uint32_t *irq_handler(uint32_t *regs)
{
#ifdef CONFIG_SUPPRESS_INTERRUPTS
up_ledon(LED_INIRQ);
PANIC(OSERR_ERREXCEPTION); /* Doesn't return */
return regs; /* To keep the compiler happy */
#else
uint32_t *ret;
int irq;
up_ledon(LED_INIRQ);
/* Get the IRQ number */
irq = (int)regs[REG_IRQNO];
/* Send an EOI (end of interrupt) signal to the PICs if this interrupt
* involved the slave.
*/
if (irq >= IRQ8)
{
/* Send reset signal to slave */
idt_outb(PIC_OCW2_EOI_NONSPEC, PIC2_OCW2);
}
/* Send reset signal to master */
idt_outb(PIC_OCW2_EOI_NONSPEC, PIC1_OCW2);
/* Dispatch the interrupt */
ret = common_handler(irq, regs);
up_ledoff(LED_INIRQ);
return ret;
#endif
}
void up_idle(void)
{
#if defined(CONFIG_ARCH_LEDS) && defined(CONFIG_ARCH_BRINGUP)
g_ledtoggle++;
if (g_ledtoggle == 0x80)
{
up_ledon(LED_IDLE);
}
else if (g_ledtoggle == 0x00)
{
up_ledoff(LED_IDLE);
}
#endif
#if defined(CONFIG_SUPPRESS_INTERRUPTS) || defined(CONFIG_SUPPRESS_TIMER_INTS)
/* If the system is idle and there are no timer interrupts,
* then process "fake" timer interrupts. Hopefully, something
* will wake up.
*/
sched_process_timer();
#endif
}
FAR chipreg_t *up_doirq(uint8_t irq, FAR chipreg_t *regs)
{
up_ledon(LED_INIRQ);
#ifdef CONFIG_SUPPRESS_INTERRUPTS
lowsyslog("Unexpected IRQ\n");
IRQ_ENTER(regs);
PANIC();
return NULL; /* Won't get here */
#else
if (irq < NR_IRQS)
{
DECL_SAVESTATE();
/* Indicate that we have entered IRQ processing logic */
IRQ_ENTER(irq, regs);
/* Deliver the IRQ */
irq_dispatch(irq, regs);
/* If a context switch occurred, 'regs' will hold the new context */
regs = IRQ_STATE();
/* Indicate that we are no longer in interrupt processing logic */
IRQ_LEAVE(irq);
}
up_ledoff(LED_INIRQ);
return regs;
#endif
}
void up_sigdeliver(void)
{
struct tcb_s *rtcb = (struct tcb_s*)g_readytorun.head;
uint32_t regs[XCPTCONTEXT_REGS];
sig_deliver_t sigdeliver;
/* Save the errno. This must be preserved throughout the signal handling
* so that the user code final gets the correct errno value (probably
* EINTR).
*/
int saved_errno = rtcb->pterrno;
up_ledon(LED_SIGNAL);
sdbg("rtcb=%p sigdeliver=%p sigpendactionq.head=%p\n",
rtcb, rtcb->xcp.sigdeliver, rtcb->sigpendactionq.head);
ASSERT(rtcb->xcp.sigdeliver != NULL);
/* Save the real return state on the stack. */
up_copystate(regs, rtcb->xcp.regs);
regs[REG_EPC] = rtcb->xcp.saved_epc;
regs[REG_STATUS] = rtcb->xcp.saved_status;
/* Get a local copy of the sigdeliver function pointer. We do this so that
* we can nullify the sigdeliver function pointer in the TCB and accept
* more signal deliveries while processing the current pending signals.
*/
sigdeliver = rtcb->xcp.sigdeliver;
rtcb->xcp.sigdeliver = NULL;
/* Then restore the task interrupt state */
irqrestore((irqstate_t)regs[REG_STATUS]);
/* Deliver the signals */
sigdeliver(rtcb);
/* Output any debug messages BEFORE restoring errno (because they may
* alter errno), then disable interrupts again and restore the original
* errno that is needed by the user logic (it is probably EINTR).
*/
svdbg("Resuming EPC: %08x STATUS: %08x\n", regs[REG_EPC], regs[REG_STATUS]);
(void)irqsave();
rtcb->pterrno = saved_errno;
/* Then restore the correct state for this thread of
* execution.
*/
up_ledoff(LED_SIGNAL);
up_fullcontextrestore(regs);
/* up_fullcontextrestore() should not return but could if the software
* interrupts are disabled.
*/
PANIC();
}
void up_sigdeliver(void)
{
_TCB *rtcb = (_TCB*)g_readytorun.head;
#if 0
uint32_t regs[XCPTCONTEXT_REGS+3]; /* Why +3? See below */
#else
uint32_t regs[XCPTCONTEXT_REGS];
#endif
sig_deliver_t sigdeliver;
/* Save the errno. This must be preserved throughout the signal handling
* so that the user code final gets the correct errno value (probably EINTR).
*/
int saved_errno = rtcb->pterrno;
up_ledon(LED_SIGNAL);
sdbg("rtcb=%p sigdeliver=%p sigpendactionq.head=%p\n",
rtcb, rtcb->xcp.sigdeliver, rtcb->sigpendactionq.head);
ASSERT(rtcb->xcp.sigdeliver != NULL);
/* Save the real return state on the stack. */
up_copystate(regs, rtcb->xcp.regs);
regs[REG_PC] = rtcb->xcp.saved_pc;
regs[REG_SR] = rtcb->xcp.saved_sr;
/* Get a local copy of the sigdeliver function pointer. We do this so that
* we can nullify the sigdeliver function pointer in the TCB and accept
* more signal deliveries while processing the current pending signals.
*/
sigdeliver = rtcb->xcp.sigdeliver;
rtcb->xcp.sigdeliver = NULL;
/* Then restore the task interrupt state */
irqrestore(regs[REG_SR]);
/* Deliver the signals */
sigdeliver(rtcb);
/* Output any debug messages BEFORE restoring errno (because they may
* alter errno), then disable interrupts again and restore the original
* errno that is needed by the user logic (it is probably EINTR).
*/
sdbg("Resuming\n");
(void)irqsave();
rtcb->pterrno = saved_errno;
/* Then restore the correct state for this thread of execution. This is an
* unusual case that must be handled by up_fullcontextresore. This case is
* unusal in two ways:
*
* 1. It is not a context switch between threads. Rather, up_fullcontextrestore
* must behave more it more like a longjmp within the same task, using
* he same stack.
* 2. In this case, up_fullcontextrestore is called with r12 pointing to
* a register save area on the stack to be destroyed. This is
* dangerous because there is the very real possibility that the new
* stack pointer might overlap with the register save area and hat stack
* usage in up_fullcontextrestore might corrupt the register save data
* before the state is restored. At present, there does not appear to
* be any stack overlap problems. If there were, then adding 3 words
* to the size of register save structure size will protect its contents.
*/
up_ledoff(LED_SIGNAL);
up_fullcontextrestore(regs);
}
int nsh_archinitialize(void)
{
int result;
/* INIT 1 Lowest level NuttX initialization has been done at this point, LEDs and UARTs are configured */
/* INIT 2 Configuring PX4 low-level peripherals, these will be always needed */
/* configure the high-resolution time/callout interface */
#ifdef CONFIG_HRT_TIMER
hrt_init();
#endif
/* configure CPU load estimation */
#ifdef CONFIG_SCHED_INSTRUMENTATION
cpuload_initialize_once();
#endif
/* set up the serial DMA polling */
#ifdef SERIAL_HAVE_DMA
{
static struct hrt_call serial_dma_call;
struct timespec ts;
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
ts.tv_sec = 0;
ts.tv_nsec = 1000000;
hrt_call_every(&serial_dma_call,
ts_to_abstime(&ts),
ts_to_abstime(&ts),
(hrt_callout)stm32_serial_dma_poll,
NULL);
}
#endif
message("\r\n");
up_ledoff(LED_BLUE);
up_ledoff(LED_AMBER);
up_ledon(LED_BLUE);
/* Configure user-space led driver */
px4fmu_led_init();
/* Configure SPI-based devices */
spi1 = up_spiinitialize(1);
if (!spi1)
{
message("[boot] FAILED to initialize SPI port 1\r\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
// Setup 10 MHz clock (maximum rate the BMA180 can sustain)
SPI_SETFREQUENCY(spi1, 10000000);
SPI_SETBITS(spi1, 8);
SPI_SETMODE(spi1, SPIDEV_MODE3);
SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false);
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
up_udelay(20);
message("[boot] Successfully initialized SPI port 1\r\n");
/* initialize SPI peripherals redundantly */
int gyro_attempts = 0;
int gyro_fail = 0;
while (gyro_attempts < 5)
{
gyro_fail = l3gd20_attach(spi1, PX4_SPIDEV_GYRO);
gyro_attempts++;
if (gyro_fail == 0) break;
up_udelay(1000);
}
if (gyro_fail) message("[boot] FAILED to attach L3GD20 gyro\r\n");
int acc_attempts = 0;
int acc_fail = 0;
while (acc_attempts < 5)
{
acc_fail = bma180_attach(spi1, PX4_SPIDEV_ACCEL);
acc_attempts++;
if (acc_fail == 0) break;
up_udelay(1000);
}
if (acc_fail) message("[boot] FAILED to attach BMA180 accelerometer\r\n");
int mpu_attempts = 0;
int mpu_fail = 0;
while (mpu_attempts < 1)
{
mpu_fail = mpu6000_attach(spi1, PX4_SPIDEV_MPU);
mpu_attempts++;
if (mpu_fail == 0) break;
up_udelay(200);
}
if (mpu_fail) message("[boot] FAILED to attach MPU 6000 gyro/acc\r\n");
/* initialize I2C2 bus */
i2c2 = up_i2cinitialize(2);
if (!i2c2) {
message("[boot] FAILED to initialize I2C bus 2\r\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
/* set I2C2 speed */
I2C_SETFREQUENCY(i2c2, 400000);
i2c3 = up_i2cinitialize(3);
if (!i2c3) {
message("[boot] FAILED to initialize I2C bus 3\r\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
/* set I2C3 speed */
I2C_SETFREQUENCY(i2c3, 400000);
int mag_attempts = 0;
int mag_fail = 0;
while (mag_attempts < 5)
{
mag_fail = hmc5883l_attach(i2c2);
mag_attempts++;
if (mag_fail == 0) break;
up_udelay(1000);
}
if (mag_fail) message("[boot] FAILED to attach HMC5883L magnetometer\r\n");
int baro_attempts = 0;
int baro_fail = 0;
while (baro_attempts < 5)
{
baro_fail = ms5611_attach(i2c2);
baro_attempts++;
if (baro_fail == 0) break;
up_udelay(1000);
}
if (baro_fail) message("[boot] FAILED to attach MS5611 baro at addr #1 or #2 (0x76 or 0x77)\r\n");
/* try to attach, don't fail if device is not responding */
(void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS,
FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES,
FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES,
FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1);
int eeprom_attempts = 0;
int eeprom_fail;
while (eeprom_attempts < 5)
{
/* try to attach, fail if device does not respond */
eeprom_fail = eeprom_attach(i2c2, FMU_ONBOARD_EEPROM_ADDRESS,
FMU_ONBOARD_EEPROM_TOTAL_SIZE_BYTES,
FMU_ONBOARD_EEPROM_PAGE_SIZE_BYTES,
FMU_ONBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/eeprom", 1);
eeprom_attempts++;
if (eeprom_fail == OK) break;
up_udelay(1000);
}
if (eeprom_fail) message("[boot] FAILED to attach FMU EEPROM\r\n");
/* Report back sensor status */
if (acc_fail || gyro_fail || mag_fail || baro_fail || eeprom_fail)
{
up_ledon(LED_AMBER);
}
#if defined(CONFIG_STM32_SPI3)
/* Get the SPI port */
message("[boot] Initializing SPI port 3\r\n");
spi3 = up_spiinitialize(3);
if (!spi3)
{
message("[boot] FAILED to initialize SPI port 3\r\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
message("[boot] Successfully initialized SPI port 3\r\n");
/* Now bind the SPI interface to the MMCSD driver */
result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
if (result != OK)
{
message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\r\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\r\n");
#endif /* SPI3 */
/* initialize I2C1 bus */
i2c1 = up_i2cinitialize(1);
if (!i2c1) {
message("[boot] FAILED to initialize I2C bus 1\r\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
/* set I2C1 speed */
I2C_SETFREQUENCY(i2c1, 400000);
/* INIT 3: MULTIPORT-DEPENDENT INITIALIZATION */
/* Get board information if available */
/* Initialize the user GPIOs */
px4fmu_gpio_init();
#ifdef CONFIG_ADC
int adc_state = adc_devinit();
if (adc_state != OK)
{
/* Try again */
adc_state = adc_devinit();
if (adc_state != OK)
{
/* Give up */
message("[boot] FAILED adc_devinit: %d\r\n", adc_state);
return -ENODEV;
}
}
#endif
/* configure the tone generator */
#ifdef CONFIG_TONE_ALARM
tone_alarm_init();
#endif
return OK;
}
uint32_t *pic32mx_decodeirq(uint32_t *regs)
{
#ifdef CONFIG_SUPPRESS_INTERRUPTS
up_ledon(LED_INIRQ);
PANIC(OSERR_ERREXCEPTION);
up_ledoff(LED_INIRQ); /* Won't get here */
return regs;
#else
uint32_t *savestate;
uint32_t regval;
int irq;
/* If the board supports LEDs, turn on an LED now to indicate that we are
* processing an interrupt.
*/
up_ledon(LED_INIRQ);
/* Save the current value of current_regs (to support nested interrupt
* handling). Then set current_regs to regs, indicating that this is
* the interrupted context that is being processed now.
*/
savestate = (uint32_t*)current_regs;
current_regs = regs;
/* Loop while there are pending interrupts with priority greater than zero */
for (;;)
{
/* Read the INTSTAT register. This register contains both the priority
* and the interrupt vector number.
*/
regval = getreg32(PIC32MX_INT_INTSTAT);
if ((regval & INT_INTSTAT_RIPL_MASK) == 0)
{
/* Break out of the loop when the priority is zero meaning that
* there are no further pending interrupts.
*/
break;
}
/* Get the vector number. The IRQ numbers have been arranged so that
* vector numbers and NuttX IRQ numbers are the same value.
*/
irq = ((regval) & INT_INTSTAT_VEC_MASK) >> INT_INTSTAT_VEC_SHIFT;
/* Disable further interrupts from this source until the driver has
* cleared the pending interrupt sources.
*/
up_disable_irq(irq);
/* Deliver the IRQ */
irq_dispatch(irq, regs);
/* Unmask the last interrupt (global interrupt below the current interrupt
* level are are still disabled)
*/
up_enable_irq(irq);
}
/* If a context switch occurred while processing the interrupt then
* current_regs may have change value. If we return any value different
* from the input regs, then the lower level will know that a context
* switch occurred during interrupt processing.
*/
regs = (uint32_t*)current_regs;
/* Restore the previous value of current_regs. NULL would indicate that
* we are no longer in an interrupt handler. It will be non-NULL if we
* are returning from a nested interrupt.
*/
current_regs = savestate;
if (current_regs == NULL)
{
up_ledoff(LED_INIRQ);
}
return regs;
#endif
}
uint32_t *pic32mx_decodeirq(uint32_t *regs)
{
#ifdef CONFIG_PIC32MX_NESTED_INTERRUPTS
uint32_t *savestate;
#endif
uint32_t regval;
int irq;
/* If the board supports LEDs, turn on an LED now to indicate that we are
* processing an interrupt.
*/
up_ledon(LED_INIRQ);
/* Save the current value of current_regs (to support nested interrupt
* handling). Then set current_regs to regs, indicating that this is
* the interrupted context that is being processed now.
*/
#ifdef CONFIG_PIC32MX_NESTED_INTERRUPTS
savestate = (uint32_t*)current_regs;
#else
DEBUGASSERT(current_regs == NULL);
#endif
current_regs = regs;
/* Loop while there are pending interrupts with priority greater than zero */
for (;;)
{
/* Read the INTSTAT register. This register contains both the priority
* and the interrupt vector number.
*/
regval = getreg32(PIC32MX_INT_INTSTAT);
if ((regval & INT_INTSTAT_RIPL_MASK) == 0)
{
/* Break out of the loop when the priority is zero meaning that
* there are no further pending interrupts.
*/
break;
}
/* Get the vector number. The IRQ numbers have been arranged so that
* vector numbers and NuttX IRQ numbers are the same value.
*/
irq = ((regval) & INT_INTSTAT_VEC_MASK) >> INT_INTSTAT_VEC_SHIFT;
/* Deliver the IRQ */
irq_dispatch(irq, regs);
}
/* If a context switch occurred while processing the interrupt then
* current_regs may have change value. If we return any value different
* from the input regs, then the lower level will know that a context
* switch occurred during interrupt processing.
*/
regs = (uint32_t*)current_regs;
/* Restore the previous value of current_regs. NULL would indicate that
* we are no longer in an interrupt handler. It will be non-NULL if we
* are returning from a nested interrupt.
*/
#ifdef CONFIG_PIC32MX_NESTED_INTERRUPTS
/* I think there are some task switching issues here. You should not
* enable nested interrupts unless you are ready to deal with the
* complexities of nested context switching. The logic here is probably
* insufficient.
*/
current_regs = savestate;
if (current_regs == NULL)
{
up_ledoff(LED_INIRQ);
}
#else
current_regs = NULL;
up_ledoff(LED_INIRQ);
#endif
return regs;
}
__EXPORT int nsh_archinitialize(void)
{
int result;
/* configure the high-resolution time/callout interface */
#ifdef CONFIG_HRT_TIMER
hrt_init();
#endif
/* configure CPU load estimation */
#ifdef CONFIG_SCHED_INSTRUMENTATION
cpuload_initialize_once();
#endif
/* set up the serial DMA polling */
#ifdef SERIAL_HAVE_DMA
{
static struct hrt_call serial_dma_call;
struct timespec ts;
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
ts.tv_sec = 0;
ts.tv_nsec = 1000000;
hrt_call_every(&serial_dma_call,
ts_to_abstime(&ts),
ts_to_abstime(&ts),
(hrt_callout)stm32_serial_dma_poll,
NULL);
}
#endif
message("\r\n");
// initial LED state
drv_led_start();
up_ledoff(LED_BLUE);
up_ledoff(LED_AMBER);
up_ledon(LED_BLUE);
/* Configure SPI-based devices */
spi1 = up_spiinitialize(1);
if (!spi1) {
message("[boot] FAILED to initialize SPI port 1\r\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
// Default SPI1 to 1MHz and de-assert the known chip selects.
SPI_SETFREQUENCY(spi1, 10000000);
SPI_SETBITS(spi1, 8);
SPI_SETMODE(spi1, SPIDEV_MODE3);
SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false);
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
up_udelay(20);
message("[boot] Successfully initialized SPI port 1\r\n");
/* Get the SPI port for the microSD slot */
message("[boot] Initializing SPI port 3\n");
spi3 = up_spiinitialize(3);
if (!spi3) {
message("[boot] FAILED to initialize SPI port 3\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
message("[boot] Successfully initialized SPI port 3\n");
/* Now bind the SPI interface to the MMCSD driver */
result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
if (result != OK) {
message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
stm32_configgpio(GPIO_ADC1_IN10);
stm32_configgpio(GPIO_ADC1_IN11);
//stm32_configgpio(GPIO_ADC1_IN12); // XXX is this available?
//stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards
return OK;
}
int nsh_archinitialize(void)
{
int result;
/* INIT 1 Lowest level NuttX initialization has been done at this point, LEDs and UARTs are configured */
/* INIT 2 Configuring PX4 low-level peripherals, these will be always needed */
/* configure the high-resolution time/callout interface */
#ifdef CONFIG_HRT_TIMER
hrt_init();
#endif
/* configure CPU load estimation */
#ifdef CONFIG_SCHED_INSTRUMENTATION
cpuload_initialize_once();
#endif
/* set up the serial DMA polling */
#ifdef SERIAL_HAVE_DMA
{
static struct hrt_call serial_dma_call;
struct timespec ts;
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
ts.tv_sec = 0;
ts.tv_nsec = 1000000;
hrt_call_every(&serial_dma_call,
ts_to_abstime(&ts),
ts_to_abstime(&ts),
(hrt_callout)stm32_serial_dma_poll,
NULL);
}
#endif
message("\r\n");
up_ledoff(LED_BLUE);
up_ledoff(LED_AMBER);
up_ledon(LED_BLUE);
/* Configure user-space led driver */
px4fmu_led_init();
i2c2 = up_i2cinitialize(2);
if (!i2c2) {
message("[boot] FAILED to initialize I2C bus 3\r\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
/* set I2C3 speed */
I2C_SETFREQUENCY(i2c2, 400000);
/* try to attach, don't fail if device is not responding */
(void)eeprom_attach(i2c2, FMU_BASEBOARD_EEPROM_ADDRESS,
FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES,
FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES,
FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1);
int eeprom_attempts = 0;
int eeprom_fail;
while (eeprom_attempts < 5)
{
/* try to attach, fail if device does not respond */
eeprom_fail = eeprom_attach(i2c2, FMU_ONBOARD_EEPROM_ADDRESS,
FMU_ONBOARD_EEPROM_TOTAL_SIZE_BYTES,
FMU_ONBOARD_EEPROM_PAGE_SIZE_BYTES,
FMU_ONBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/eeprom", 1);
eeprom_attempts++;
if (eeprom_fail == OK) break;
up_udelay(1000);
}
if (eeprom_fail) message("[boot] FAILED to attach FMU EEPROM\r\n");
/* Report back sensor status */
if (eeprom_fail)
{
up_ledon(LED_AMBER);
}
#if defined(CONFIG_STM32_SPI3)
/* Get the SPI port */
message("[boot] Initializing SPI port 3\r\n");
spi3 = up_spiinitialize(3);
if (!spi3)
{
message("[boot] FAILED to initialize SPI port 3\r\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
message("[boot] Successfully initialized SPI port 3\r\n");
/* Now bind the SPI interface to the MMCSD driver */
result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
if (result != OK)
{
message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\r\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\r\n");
#endif /* SPI3 */
/* INIT 3: MULTIPORT-DEPENDENT INITIALIZATION */
/* Get board information if available */
/* Initialize the user GPIOs */
px4fmu_gpio_init();
#ifdef CONFIG_ADC
int adc_state = adc_devinit();
if (adc_state != OK)
{
/* Try again */
adc_state = adc_devinit();
if (adc_state != OK)
{
/* Give up */
message("[boot] FAILED adc_devinit: %d\r\n", adc_state);
return -ENODEV;
}
}
#endif
/* configure the tone generator */
#ifdef CONFIG_TONE_ALARM
tone_alarm_init();
#endif
return OK;
}
uint32_t *up_doirq(int irq, uint32_t* regs)
{
up_ledon(LED_INIRQ);
#ifdef CONFIG_SUPPRESS_INTERRUPTS
PANIC();
#else
if ((unsigned)irq < NR_IRQS)
{
uint32_t *savestate;
/* Nested interrupts are not supported in this implementation. If
* you want to implement nested interrupts, you would have to (1)
* change the way that current_regs is handled and (2) the design
* associated with CONFIG_ARCH_INTERRUPTSTACK. The savestate
* variable will not work for that purpose as implemented here
* because only the outermost nested interrupt can result in a
* context switch (it can probably be deleted).
*/
/* Current regs non-zero indicates that we are processing
* an interrupt; current_regs is also used to manage
* interrupt level context switches.
*/
savestate = (uint32_t*)current_regs;
current_regs = regs;
/* Mask and acknowledge the interrupt (if supported by the chip) */
#ifndef CONFIG_ARCH_NOINTC
up_maskack_irq(irq);
#endif
/* Deliver the IRQ */
irq_dispatch(irq, regs);
/* Get the current value of regs... it may have changed because
* of a context switch performed during interrupt processing.
*/
regs = current_regs;
/* Restore the previous value of current_regs. NULL would indicate that
* we are no longer in an interrupt handler. It will be non-NULL if we
* are returning from a nested interrupt.
*/
current_regs = savestate;
/* Unmask the last interrupt (global interrupts are still
* disabled.
*/
#ifndef CONFIG_ARCH_NOINTC
up_enable_irq(irq);
#endif
}
up_ledoff(LED_INIRQ);
#endif
return regs;
}
