/*!
* \brief Toggle external hardware reset line.
*/
static void eNet_sam7X_Reset(void)
{
uint32_t rstcr_tmp = inr(RSTC_MR) & 0x00FFFFFF;
/* Set reset pulse length to 250us, disable user reset. */
outr(RSTC_MR, RSTC_KEY | (2 << RSTC_ERSTL_LSB));
/* Invoke external reset. */
outr(RSTC_CR, RSTC_KEY | RSTC_EXTRST);
while ((inr(RSTC_SR) & RSTC_NRSTL) == 0);
/* If we have 10k/100n RC, we need to wait 25us (1200 cycles)
** for NRST becoming low. */
eNet_sam7X_Delay(250);
/* Wait until reset pin is released. */
while ((inr(RSTC_SR) & RSTC_NRSTL) == 0);
/* Due to the RC filter, the pin is rising very slowly. */
eNet_sam7X_Delay(25000);
outr(RSTC_MR, RSTC_KEY | rstcr_tmp);
}
/* nrf24l01_set_prx:
* set pipe to send data;
* the radio will be the Primary Receiver (PRX).
* See page 33 of nRF24L01_Product_Specification_v2_0.pdf
* Receive the value of pipe 0 addr
*/
int8_t nrf24l01_set_prx(int8_t spi_fd, uint8_t *pipe0_addr)
{
/* The addr of pipe 0 is necessary to avoid
* save this value internally. If there are more than
* one radio using this lib, the value of pipe 0 addr
* can be different.
*/
set_standby1();
set_address_pipe(spi_fd, NRF24_RX_ADDR_P0, pipe0_addr);
/* RX Settings */
outr(spi_fd, NRF24_STATUS, NRF24_ST_RX_DR);
outr(spi_fd, NRF24_CONFIG, nrf24reg_read(spi_fd, NRF24_CONFIG)
| NRF24_CFG_PRIM_RX);
/* Enable and delay time to TSTBY2A timing */
enable();
delay_us(TSTBY2A);
return 0;
}
void ADCEnableChannel(TADCChannel channel)
{
uint32_t adc_chsr;
register int idx;
register int max = channel;
outr(ADC_CHER, _BV(channel));
adc_chsr = inr(ADC_CHSR);
/* Search the highest numbered channel which is enabled */
for (idx = 0; idx < 8; idx ++) {
if (adc_chsr & _BV(idx)) max = idx;
}
/* Disable EOC for all channels */
outr(ADC_IDR, 0x000000FF);
/* Enable EOC for highest numbered channel */
outr(ADC_IER, _BV(max));
}
/* For an unknown reason the system hangs in NutTimer(!) processing
when trying to read the version. Something is somewhere
mysteriously broken. Stack? CPU initialization? Keep this code
for reference. */
static int PmmReadReg(unsigned int reg, unsigned char *val)
{
unsigned long sr;
volatile unsigned int tmo;
outr(TWI_IADRR, reg);
outr(TWI_MMR, 0x22 << TWI_DADR_LSB | TWI_IADRSZ_1BYTE | TWI_MREAD);
outr(TWI_CR, TWI_START | TWI_STOP);
for (tmo = 0; ((sr = inr(TWI_SR)) & TWI_RXRDY) == 0; tmo++) {
if (tmo > 100000) {
return -1;
}
}
if (sr & TWI_NACK) {
return -1;
}
*val = inb(TWI_RHR);
return 0;
}
/* nrf24l01_set_ptx:
* set pipe to send data;
* the radio will be the Primary Transmitter (PTX).
* See page 31 of nRF24L01_Product_Specification_v2_0.pdf
*/
int8_t nrf24l01_set_ptx(int8_t spi_fd, uint8_t pipe)
{
/*check if pipe exists*/
if (pipe > NRF24_PIPE_MAX)
return -1;
/* put the radio in mode standby-1 */
set_standby1();
/* TX Settling */
/*
* If the ack is enable in this pipe is necessary enable
* the ack in pipe0 too. Because ack always arrive in pipe 0.
*/
if (nrf24reg_read(spi_fd, NRF24_EN_AA) & pipe_reg[pipe].enaa
&& pipe != NRF24_PIPE0_ADDR)
outr(spi_fd, NRF24_EN_AA, nrf24reg_read(spi_fd, NRF24_EN_AA)
| NRF24_AA_P0);
else
outr(spi_fd, NRF24_EN_AA, nrf24reg_read(spi_fd, NRF24_EN_AA)
& ~NRF24_AA_P0);
set_address_pipe(spi_fd, NRF24_RX_ADDR_P0,
get_address_pipe(spi_fd, pipe));
set_address_pipe(spi_fd, NRF24_TX_ADDR, get_address_pipe(spi_fd, pipe));
#if (NRF24_ARC != NRF24_ARC_DISABLE)
/*
* Set ARC and ARD by pipe index to different
* retry periods to reduce data collisions
* compute ARD range: 1500us <= ARD[pipe] <= 4000us
*/
outr(spi_fd, NRF24_SETUP_RETR,
NRF24_RETR_ARD(((pipe * 2) + 5))
| NRF24_RETR_ARC(NRF24_ARC));
#endif
outr(spi_fd, NRF24_STATUS, NRF24_ST_TX_DS | NRF24_ST_MAX_RT);
outr(spi_fd, NRF24_CONFIG, nrf24reg_read(spi_fd, NRF24_CONFIG)
& ~NRF24_CFG_PRIM_RX);
/* Enable and delay time to TSTBY2A timing */
enable();
delay_us(TSTBY2A);
return 0;
}
int At91SpiSetTxDelay(unsigned int base, unsigned int cs, unsigned int dly)
{
unsigned int csr = base + SPI_CSR0_OFF + cs * 4;
outr(csr, (inr(csr) & ~SPI_DLYBCT) | ((dly << SPI_DLYBCT_LSB) & SPI_DLYBCT));
if (At91SpiGetTxDelay(base, cs) != dly) {
return -1;
}
return 0;
}
/*!
* \brief Toggle external hardware reset line.
*/
static void MorphoqReset(void)
{
unsigned int mr;
/* Save initial configuration. */
mr = inr(RSTC_MR) & ~RSTC_KEY_MSK;
/* Set reset pulse length to 250us, disable user reset. */
outr(RSTC_MR, RSTC_KEY | (2 << RSTC_ERSTL_LSB));
/* Invoke external reset. */
outr(RSTC_CR, RSTC_KEY | RSTC_EXTRST);
/* If we have 10k/100n RC, we need to wait 25us (1200 cycles)
** for NRST becoming low. */
MorphoqDelay(250);
/* Wait until reset pin is released. */
while ((inr(RSTC_SR) & RSTC_NRSTL) == 0);
/* Due to the RC filter, the pin is rising very slowly. */
MorphoqDelay(25000);
/* Restore initial configuration. */
outr(RSTC_MR, RSTC_KEY | mr);
}
/*!
* \brief Configure the SPI operation mode.
*
* \param base SPI register base.
* \param cs Chip select line.
* \param mode Any of the following
* - SPIMF_MASTER Master mode.
* - SPIMF_PCSDEC Decoded chip selects.
* - SPIMF_MFDETECT Mode fault detection.
* - SPIMF_LOOPBACK Loopback mode.
* - SPIMF_SCKIAHI Clock is high when inactive.
* - SPIMF_CAPRISE Data captured on rising edge.
* - SPIMF_KEEPCS Chip select remains active after transfer.
*/
int At91SpiSetModeFlags(unsigned int base, unsigned int cs, uint32_t mode)
{
unsigned int mv;
mv = inr(base + SPI_MR_OFF) & ~(SPI_MSTR | SPI_PCSDEC | SPI_MODFDIS | SPI_LLB);
if (mode & SPIMF_MASTER) {
mv |= SPI_MSTR;
}
if (mode & SPIMF_PCSDEC) {
mv |= SPI_PCSDEC;
}
if (mode & SPIMF_MFDETECT) {
mv &= ~SPI_MODFDIS;
}
if (mode & SPIMF_LOOPBACK) {
mv |= SPI_LLB;
}
outr(base + SPI_MR_OFF, mv);
mv = inr(base + SPI_CSR0_OFF + cs * 4) & ~(SPI_CPOL | SPI_NCPHA | SPI_CSAAT);
if (mode & SPIMF_SCKIAHI) {
if (mode & SPIMF_CAPRISE) {
mv |= SPI_CPOL;
} else {
mv |= SPI_CPOL | SPI_NCPHA;
}
} else {
if (mode & SPIMF_CAPRISE) {
mv |= SPI_NCPHA;
}
}
if (mode & SPIMF_KEEPCS) {
mv |= SPI_CSAAT;
}
outr(base + SPI_CSR0_OFF + cs * 4, mv);
if (At91SpiGetModeFlags(base, cs) != mode) {
return -1;
}
return 0;
}
/*!
* \brief Write value to an 8-bit power management register.
*
* \param reg PMM register to write to.
* \param val Value to write.
*
* \return 0 on success, -1 otherwise.
*/
static int PmmWriteReg(unsigned int reg, unsigned int val)
{
volatile int tmo;
outr(TWI_MMR, 0x22 << TWI_DADR_LSB);
outr(TWI_CR, TWI_START);
outr(TWI_THR, reg);
for (tmo = 0; (inr(TWI_SR) & TWI_TXRDY) == 0; tmo++) {
if (tmo > 100000) {
return -1;
}
}
outr(TWI_CR, TWI_STOP);
outr(TWI_THR, val);
for (tmo = 0; (inr(TWI_SR) & TWI_TXCOMP) == 0; tmo++) {
if (tmo > 100000) {
return -1;
}
}
return 0;
}
void __init2(void)
{
/*
* The watchdog is enabled after processor reset.
*/
#if defined(NUT_WDT_START)
#if NUT_WDT_START
/* Configure the watchdog. */
outr(WDT_MR, NUT_WDT_START);
#else
/* Disable the watchdog. */
outr(WDT_MR, WDT_WDDIS);
#endif
#endif
/*
* Enable external reset key.
*/
outr(RSTC_MR, RSTC_KEY | RSTC_URSTEN);
/* Continue with runtime initialization. */
__set_stacks();
}
/*!
* \brief Initialize debug device 2.
*
* \return Always 0.
*/
static int DebugInit(NUTDEVICE * dev)
{
#if NUT_DEV_DEBUG_PINS
/* Disable GPIO on UART tx/rx pins. */
outr(NUT_DEV_DEBUG_PDR, NUT_DEV_DEBUG_PINS);
#endif
/* Reset UART. */
outr(DBGU_CR, US_RSTRX | US_RSTTX | US_RXDIS | US_TXDIS);
/* Disable all UART interrupts. */
outr(DBGU_IDR, 0xFFFFFFFF);
#if NUT_DEV_DEBUG_SPEED
/* Set UART baud rate generator register. */
outr(DBGU_BRGR, At91BaudRateDiv(NUT_DEV_DEBUG_SPEED));
#endif
/* Set UART mode to 8 data bits, no parity and 1 stop bit. */
outr(DBGU_MR, US_CHMODE_NORMAL | US_CHRL_8 | US_PAR_NO | US_NBSTOP_1);
/* Enable UART transmitter and optionally the receiver. */
outr(DBGU_CR, NUT_DEV_DEBUG_ENA);
return 0;
}
/*!
* \brief Initialize the second serial peripheral interface on the AT91 MCU.
*/
int At91Spi1Init(void)
{
/* Enable SPI peripherals. */
At91Spi1Enable();
/* Enable SPI clock. */
outr(PMC_PCER, _BV(SPI1_ID));
/* Register and enable SPI1 interrupt handler. */
NutRegisterIrqHandler(&sig_SPI1, At91Spi1Interrupt, 0);
NutIrqEnable(&sig_SPI1);
return At91SpiReset(SPI1_BASE);
}
int8_t nrf24l01_deinit(int8_t spi_fd)
{
disable();
/* Power down the radio */
outr(spi_fd, NRF24_CONFIG, nrf24reg_read(spi_fd, NRF24_CONFIG) &
~NRF24_CFG_PWR_UP);
/* Deinit SPI and GPIO */
io_reset(spi_fd);
return 0;
}
/*!
* \brief Initialize the cable driver.
*
* \return Pointer to a cable specific information structure, which must
* be passed to all other routines of this driver.
*/
static void *JtagCable0Open(void)
{
CABLE_INFO *cib;
cib = calloc(1, sizeof(CABLE_INFO));
if (cib) {
#if defined(MCU_AT91) && defined(JTAG0_TDO_PIO_ID)
#if defined(PS_PCER)
outr(PS_PCER, _BV(JTAG0_TDO_PIO_ID));
#elif defined(PMC_PCER)
outr(PMC_PCER, _BV(JTAG0_TDO_PIO_ID));
#endif
#endif
JTAG0_TDO_SI();
JTAG0_TDI_SO();
JTAG0_TMS_SO();
JTAG0_TCK_SO();
JTAG0_NSRST_SO();
JTAG0_NTRST_SO();
}
return (void *)cib;
}
/*
* ATIRGB514Save --
*
* This function saves IBM RGB514 related data into an ATIHWRec.
*/
void
ATIRGB514Save
(
ATIPtr pATI,
ATIHWPtr pATIHW
)
{
CARD32 crtc_gen_cntl, dac_cntl;
CARD8 index_lo, index_hi, index_ctl;
int Index;
/* Temporarily switch to Mach64 CRTC */
crtc_gen_cntl = inr(CRTC_GEN_CNTL);
if (!(crtc_gen_cntl & CRTC_EXT_DISP_EN))
outr(CRTC_GEN_CNTL, crtc_gen_cntl | CRTC_EXT_DISP_EN);
/* Temporarily switch to IBM RGB 514 registers */
dac_cntl = inr(DAC_CNTL) & ~(DAC_EXT_SEL_RS2 | DAC_EXT_SEL_RS3);
outr(DAC_CNTL, dac_cntl | DAC_EXT_SEL_RS2);
index_lo = in8(M64_DAC_WRITE);
index_hi = in8(M64_DAC_DATA);
index_ctl = in8(M64_DAC_READ);
out8(M64_DAC_WRITE, 0x00U);
out8(M64_DAC_DATA, 0x00U);
out8(M64_DAC_READ, 0x01U); /* Auto-increment */
/* Save IBM RGB 514 registers */
for (Index = 0; Index < NumberOf(pATIHW->ibmrgb514); Index++)
{
/* Need to rewrite the index every so often... */
if ((Index == 0x0100) || (Index == 0x0500))
{
out8(M64_DAC_WRITE, 0);
out8(M64_DAC_DATA, Index >> 8);
}
pATIHW->ibmrgb514[Index] = in8(M64_DAC_MASK);
}
/*
* nrf24l01_set_channel:
* Bandwidth < 1MHz at 250kbps
* Bandwidth < 1MHZ at 1Mbps
* Bandwidth < 2MHz at 2Mbps
* 2Mbps -> channel max is 54
* 1Mbps -> channel max is 116
*/
int8_t nrf24l01_set_channel(int8_t spi_fd, uint8_t ch)
{
uint8_t max;
max = NRF24_RF_DR(nrf24reg_read(spi_fd, NRF24_RF_SETUP)) ==
NRF24_DR_2MBPS?NRF24_CH_MAX_2MBPS:NRF24_CH_MAX_1MBPS;
if (ch != _CONSTRAIN(ch, NRF24_CH_MIN, max))
return -1;
if (ch != NRF24_CH(nrf24reg_read(spi_fd, NRF24_RF_CH))) {
set_standby1();
outr(spi_fd, NRF24_STATUS, NRF24_ST_RX_DR
| NRF24_ST_TX_DS | NRF24_ST_MAX_RT);
command(spi_fd, NRF24_FLUSH_TX);
command(spi_fd, NRF24_FLUSH_RX);
/* Set the device channel */
outr(spi_fd, NRF24_RF_CH,
NRF24_CH(_CONSTRAIN(ch, NRF24_CH_MIN, max)));
}
return 0;
}
/*!
* \brief Software interrupt control.
*
* \param cmd Control command.
* - NUT_IRQCTL_INIT Initialize and disable interrupt.
* - NUT_IRQCTL_STATUS Query interrupt status.
* - NUT_IRQCTL_ENABLE Enable interrupt.
* - NUT_IRQCTL_DISABLE Disable interrupt.
* - NUT_IRQCTL_GETPRIO Query interrupt priority.
* - NUT_IRQCTL_SETPRIO Set interrupt priority.
* - NUT_IRQCTL_GETCOUNT Query and clear interrupt counter.
* \param param Pointer to optional parameter.
*
* \return 0 on success, -1 otherwise.
*/
static int SoftwareIrqCtl(int cmd, void *param)
{
int rc = 0;
unsigned int *ival = (unsigned int *)param;
int_fast8_t enabled = inr(AIC_IMR) & _BV(SWIRQ_ID);
/* Disable interrupt. */
if (enabled) {
outr(AIC_IDCR, _BV(SWIRQ_ID));
}
switch(cmd) {
case NUT_IRQCTL_INIT:
/* Set the vector. */
outr(AIC_SVR(SWIRQ_ID), (unsigned int)SoftwareIrqEntry);
/* Initialize to edge triggered with defined priority. */
outr(AIC_SMR(SWIRQ_ID), AIC_SRCTYPE_INT_EDGE_TRIGGERED | NUT_IRQPRI_SWIRQ);
/* Clear interrupt */
outr(AIC_ICCR, _BV(SWIRQ_ID));
break;
case NUT_IRQCTL_STATUS:
if (enabled) {
*ival |= 1;
}
else {
*ival &= ~1;
}
break;
case NUT_IRQCTL_ENABLE:
enabled = 1;
break;
case NUT_IRQCTL_DISABLE:
enabled = 0;
break;
case NUT_IRQCTL_GETPRIO:
*ival = inr(AIC_SMR(SWIRQ_ID)) & AIC_PRIOR;
break;
case NUT_IRQCTL_SETPRIO:
outr(AIC_SMR(SWIRQ_ID), (inr(AIC_SMR(SWIRQ_ID)) & ~AIC_PRIOR) | *ival);
break;
#ifdef NUT_PERFMON
case NUT_IRQCTL_GETCOUNT:
*ival = (unsigned int)sig_SWIRQ.ir_count;
sig_SWIRQ.ir_count = 0;
break;
#endif
default:
rc = -1;
break;
}
/* Enable interrupt. */
if (enabled) {
outr(AIC_IECR, _BV(SWIRQ_ID));
}
return rc;
}
/*!
* \brief Configure the SPI rate.
*
* \param base SPI register base.
* \param cs Chip select line.
* \param rate Baudrate. The maximum is MCK/1 and the minimum is MCK/255.
* If the specified rate is above the maximum or below the
* minimum, the maximum or minimum value resp. will be set.
*/
int At91SpiSetRate(unsigned int base, unsigned int cs, uint32_t rate)
{
int rc = 0;
unsigned int divider;
/* The SPI clock is driven by the master clock. */
divider = (unsigned int) At91GetMasterClock();
/* Calculate the SPI clock divider. Avoid rounding errors. */
divider += (unsigned int) (rate / 2);
divider /= rate;
/* A divider value of 0 is not allowed. */
if (divider < 1) {
divider = 1;
}
/* The divider value maximum is 255. */
else if (divider > 255) {
divider = 255;
}
switch (cs) {
case 0:
outr(base + SPI_CSR0_OFF, (inr(base + SPI_CSR0_OFF) & ~SPI_SCBR) | (divider << SPI_SCBR_LSB));
break;
case 1:
outr(base + SPI_CSR1_OFF, (inr(base + SPI_CSR1_OFF) & ~SPI_SCBR) | (divider << SPI_SCBR_LSB));
break;
case 2:
outr(base + SPI_CSR2_OFF, (inr(base + SPI_CSR2_OFF) & ~SPI_SCBR) | (divider << SPI_SCBR_LSB));
break;
case 3:
outr(base + SPI_CSR3_OFF, (inr(base + SPI_CSR3_OFF) & ~SPI_SCBR) | (divider << SPI_SCBR_LSB));
break;
default:
rc = -1;
break;
}
return 0;
}
int8_t nrf24l01_close_pipe(int8_t spi_fd, int8_t pipe)
{
pipe_reg_t rpipe;
/*check if pipe exists*/
if (pipe < NRF24_PIPE_MIN || pipe > NRF24_PIPE_MAX)
return -1;
memcpy(&rpipe, &pipe_reg[pipe], sizeof(rpipe));
if (nrf24reg_read(spi_fd, NRF24_EN_RXADDR) & rpipe.en_rxaddr) {
/*
* The data pipes are enabled with the bits in the EN_RXADDR
* Disable the EN_RXADDR for this pipe
*/
outr(spi_fd, NRF24_EN_RXADDR, nrf24reg_read(spi_fd, NRF24_EN_RXADDR)
& ~rpipe.en_rxaddr);
/* Disable auto ack in this pipe */
outr(spi_fd, NRF24_EN_AA, nrf24reg_read(spi_fd, NRF24_EN_AA)
& ~rpipe.enaa);
}
return 0;
}
/*nrf24l01_ptx_wait_datasent:
* wait while data is being sent
* check Data Sent TX FIFO interrupt
*/
int8_t nrf24l01_ptx_wait_datasent(int8_t spi_fd)
{
uint16_t value;
while (!((value = nrf24reg_read(spi_fd, NRF24_STATUS)) & NRF24_ST_TX_DS)) {
/* if arrived in Maximum number of TX retransmits
* the send failed
*/
if (value & NRF24_ST_MAX_RT) {
outr(spi_fd, NRF24_STATUS, NRF24_ST_MAX_RT);
command(spi_fd, NRF24_FLUSH_TX);
return -1;
}
}
return 0;
}
/*nrf24l01_prx_data:
* return the len of data received
* Send command to read data width for the RX FIFO
*/
int8_t nrf24l01_prx_data(int8_t spi_fd, void *pdata, uint16_t len)
{
uint16_t rxlen = 0;
outr(spi_fd, NRF24_STATUS, NRF24_ST_RX_DR);
command_data(spi_fd, NRF24_R_RX_PL_WID, &rxlen, DATA_SIZE);
/* Note: flush RX FIFO if the value read is larger than 32 bytes.*/
if (rxlen > NRF24_PAYLOAD_SIZE) {
command(spi_fd, NRF24_FLUSH_RX);
return 0;
}
if (rxlen != 0) {
rxlen = _MIN(len, rxlen);
command_data(spi_fd, NRF24_R_RX_PAYLOAD, pdata, rxlen);
}
return (int8_t)rxlen;
}
int At91SpiSetBits(unsigned int base, unsigned int cs, unsigned int bits)
{
unsigned int mv;
mv = inr(base + SPI_CSR0_OFF + cs * 4) & ~SPI_BITS;
switch (bits) {
case 9:
mv |= SPI_BITS_9;
break;
case 10:
mv |= SPI_BITS_10;
break;
case 11:
mv |= SPI_BITS_11;
break;
case 12:
mv |= SPI_BITS_12;
break;
case 13:
mv |= SPI_BITS_13;
break;
case 14:
mv |= SPI_BITS_14;
break;
case 15:
mv |= SPI_BITS_15;
break;
case 16:
mv |= SPI_BITS_16;
break;
default:
mv |= SPI_BITS_8;
break;
}
outr(base + SPI_CSR0_OFF + cs * 4, mv);
if (At91SpiGetBits(base, cs) != bits) {
return -1;
}
return 0;
}
/*
* \return 0 on success, -1 in case of any errors.
*/
static int SendRawByte(AHDLCDCB * dcb, uint8_t ch, uint8_t flush)
{
/*
* If transmit buffer is full, wait until interrupt routine
* signals an empty buffer or until a timeout occurs.
*/
while ((uint8_t) (dcb->dcb_wr_idx + 1) == dcb->dcb_tx_idx) {
if (NutEventWait(&dcb->dcb_tx_rdy, dcb->dcb_wtimeout))
break;
}
/*
* If transmit buffer is still full, we have a write timeout.
*/
if ((uint8_t) (dcb->dcb_wr_idx + 1) == dcb->dcb_tx_idx) {
return -1;
}
/*
* Buffer has room for more data. Put the byte in the buffer
* and increment the write index.
*/
dcb->dcb_tx_buf[dcb->dcb_wr_idx] = ch;
dcb->dcb_wr_idx++;
/*
* If transmit buffer has become full and the transmitter
* is not active, then activate it.
*/
if (flush || (uint8_t) (dcb->dcb_wr_idx + 1) == dcb->dcb_tx_idx) {
NutEnterCritical();
outr(US1_IER, US_TXRDY);
NutExitCritical();
}
return 0;
}
/*!
* \brief Ethernet PHY hardware reset.
*/
static void PmmPhyReset(void)
{
/* Enable PIO pull-ups at PHY mode strap pins. */
outr(PIOA_ODR, _BV(14) | _BV(15) | _BV(17));
outr(PIOA_PUER, _BV(14) | _BV(15) | _BV(17));
outr(PIOA_PER, _BV(14) | _BV(15) | _BV(17));
/* Enable PIO at PHY address 0 strap pin. */
outr(PIOA_ODR, _BV(18));
outr(PIOA_PUDR, _BV(18));
outr(PIOA_PER, _BV(18));
BootMilliDelay(10);
PmmWriteReg(PWRMAN_REG_ENA, PWRMAN_ETHRST | PWRMAN_ETHCLK);
BootMilliDelay(1);
PmmWriteReg(PWRMAN_REG_DIS, PWRMAN_ETHRST);
BootMilliDelay(10);
}
/*!
* \brief USART interrupt handler.
*
* \param arg Pointer to the device specific control block.
*/
static void At91UsartInterrupt(void *arg)
{
AHDLCDCB *dcb = arg;
uint16_t count, i;
ureg_t csr = inr(US1_CSR);
if (csr & (US_ENDRX | US_RXBUFF | US_TIMEOUT)) {
// Todo, handle error flags
if (csr & US_TIMEOUT) {
count = NUT_AHDLC_RECV_DMA_SIZE - inr(USART1_BASE + PERIPH_RCR_OFF);
} else {
count = NUT_AHDLC_RECV_DMA_SIZE;
}
for (i = 0; i < count; i++) {
dcb->dcb_rx_buf[dcb->dcb_rx_idx] = DMA_RxBuf0[i];
dcb->dcb_rx_idx++;
}
outr(USART1_BASE + PERIPH_RPR_OFF, (unsigned int) DMA_RxBuf0);
outr(USART1_BASE + PERIPH_RCR_OFF, NUT_AHDLC_RECV_DMA_SIZE);
outr(USART1_BASE + PERIPH_PTCR_OFF, PDC_RXTEN);
outr(US1_CR, inr(US1_CR) | US_STTTO);
NutEventPostFromIrq(&dcb->dcb_rx_rdy);
}
if (csr & US_TXRDY) {
if (dcb->dcb_tx_idx != dcb->dcb_wr_idx) {
outr(US1_THR, dcb->dcb_tx_buf[dcb->dcb_tx_idx]);
dcb->dcb_tx_idx++;
} else {
outr(US1_IDR, US_TXRDY);
NutEventPostFromIrq(&dcb->dcb_tx_rdy);
}
}
}
/*!
* \brief Enable peripheral clocks.
*/
static void MorphoqClockInit(void)
{
outr(PMC_PCER, _BV(PIOA_ID));
outr(PMC_PCER, _BV(PIOB_ID));
outr(PMC_PCER, _BV(EMAC_ID));
}
Sivia::Sivia(repere& R, struct sivia_struct *par) : R(R) {
par->area = 0;
// Create the function we want to apply SIVIA on.
Variable x,y;
double ei = par->ei;
double xb=par->xb1,yb=par->yb1;
Interval xbi=Interval(par->xb1-ei,par->xb1+ei),ybi=Interval(par->yb1-ei,par->yb1+ei);
double arc = par->sonar_arc;
double r = pow(par->sonar_radius,2);
double th1 = par->th[0];
double th2=th1+arc;
double th21= par->th[1];
double th22=th21 + arc;
double th31= par->th[2];
double th32=th31 + arc;
double e=1;
double epsilon = par->epsilon;
double xin,yin;
// First SONAR
Function f(x,y,sqr(x-xbi)+sqr(y-ybi));
NumConstraint c1(x,y,f(x,y)<=r+e);
NumConstraint c2(x,y,f(x,y)>=e);
NumConstraint c3(x,y,f(x,y)>r+e);
NumConstraint c4(x,y,f(x,y)<e);
double sign1,sign2;
if(cos(th1)>0) sign1=1;
else sign1=-1;
if(cos(th2)<0) sign2=1;
else sign2=-1;
NumConstraint cth11(x,y,sign1*(y-ybi-((sin(th1))/(cos(th1)))*(x-xbi))<0);
NumConstraint cth12(x,y,sign1*(y-ybi-((sin(th1))/(cos(th1)))*(x-xbi))>0);
NumConstraint cth21(x,y,sign2*(y-ybi-((sin(th2))/(cos(th2)))*(x-xbi))<0);
NumConstraint cth22(x,y,sign2*(y-ybi-((sin(th2))/(cos(th2)))*(x-xbi))>0);
// Create contractors with respect to each
// of the previous constraints.
CtcFwdBwd out1(c1);
CtcFwdBwd out2(c2);
CtcFwdBwd in1(c3);
CtcFwdBwd in2(c4);
CtcFwdBwd outth1(cth12);
CtcFwdBwd inth1(cth11);
CtcFwdBwd inth2(cth21);
CtcFwdBwd outth2(cth22);
// CtcIn inside(f,Interval(-1,1));
// CtcNotIn outside(f,Interval(-1,1));
// Create a contractor that removes all the points
// that do not satisfy either f(x,y)<=2 or f(x,y)>=0.
// These points are "outside" of the solution set.
CtcCompo outside1(out1,out2,outth1,outth2);
// Create a contractor that removes all the points
// that do not satisfy both f(x,y)>2 or f(x,y)<0.
// These points are "inside" the solution set.
CtcUnion inside11(in1,in2,inth1);
CtcUnion inside1(inside11,inth2);
// Second SONAR
double xb2=par->xb2,yb2=par->yb2;
Interval xb2i=Interval(par->xb2-ei,par->xb2+ei),yb2i=Interval(par->yb2-ei,par->yb2+ei);
Function f2(x,y,sqr(x-xb2i)+sqr(y-yb2i));
NumConstraint c21(x,y,f2(x,y)<=r+e);
NumConstraint c22(x,y,f2(x,y)>=e);
NumConstraint c23(x,y,f2(x,y)>r+e);
NumConstraint c24(x,y,f2(x,y)<e);
double sign21,sign22;
if(cos(th21)>0) sign21=-1;
else sign21=1;
if(cos(th22)<0) sign22=1;
else sign22=-1;
NumConstraint cth211(x,y,sign21*(y-yb2i-((sin(th21))/(cos(th21)))*(x-xb2i))<0);
NumConstraint cth212(x,y,sign21*(y-yb2i-((sin(th21))/(cos(th21)))*(x-xb2i))>0);
NumConstraint cth221(x,y,sign22*(y-yb2i-((sin(th22))/(cos(th22)))*(x-xb2i))<0);
NumConstraint cth222(x,y,sign22*(y-yb2i-((sin(th22))/(cos(th22)))*(x-xb2i))>0);
// Create contractors with respect to each
// of the previous constraints.
CtcFwdBwd out21(c21);
CtcFwdBwd out22(c22);
CtcFwdBwd in21(c23);
CtcFwdBwd in22(c24);
CtcFwdBwd outth21(cth211);
CtcFwdBwd inth21(cth212);
CtcFwdBwd inth22(cth221);
CtcFwdBwd outth22(cth222);
// CtcIn inside(f,Interval(-1,1));
// CtcNotIn outside(f,Interval(-1,1));
// Create a contractor that removes all the points
// that do not satisfy either f(x,y)<=2 or f(x,y)>=0.
// These points are "outside" of the solution set.
CtcCompo outside2(out21,out22,outth21,outth22);
// Create a contractor that removes all the points
// that do not satisfy both f(x,y)>2 or f(x,y)<0.
// These points are "inside" the solution set.
CtcUnion inside21(in21,in22,inth21);
CtcUnion inside2(inside21,inth22);
//Third SONAR
double xb3=par->xb3,yb3=par->yb3;
Interval xb3i=Interval(par->xb3-ei,par->xb3+ei),yb3i=Interval(par->yb3-ei,par->yb3+ei);
Function f3(x,y,sqr(x-xb3i)+sqr(y-yb3i));
NumConstraint c31(x,y,f3(x,y)<=r+e);
NumConstraint c32(x,y,f3(x,y)>=e);
NumConstraint c33(x,y,f3(x,y)>r+e);
NumConstraint c34(x,y,f3(x,y)<e);
double sign31,sign32;
if(cos(th31)>0) sign31=-1;
else sign31=1;
if(cos(th32)<0) sign32=1;
else sign32=-1;
NumConstraint cth311(x,y,sign31*(y-yb3i-((sin(th31))/(cos(th31)))*(x-xb3i))<0);
NumConstraint cth312(x,y,sign31*(y-yb3i-((sin(th31))/(cos(th31)))*(x-xb3i))>0);
NumConstraint cth321(x,y,sign32*(y-yb3i-((sin(th32))/(cos(th32)))*(x-xb3i))<0);
NumConstraint cth322(x,y,sign32*(y-yb3i-((sin(th32))/(cos(th32)))*(x-xb3i))>0);
// Create contractors with respect to each
// of the previous constraints.
CtcFwdBwd out31(c31);
CtcFwdBwd out32(c32);
CtcFwdBwd in31(c33);
CtcFwdBwd in32(c34);
CtcFwdBwd outth31(cth311);
CtcFwdBwd inth31(cth312);
CtcFwdBwd inth32(cth321);
CtcFwdBwd outth32(cth322);
// CtcIn inside(f,Interval(-1,1));
// CtcNotIn outside(f,Interval(-1,1));
// Create a contractor that removes all the points
// that do not satisfy either f(x,y)<=2 or f(x,y)>=0.
// These points are "outside" of the solution set.
CtcCompo outside3(out31,out32,outth31,outth32);
// Create a contractor that removes all the points
// that do not satisfy both f(x,y)>2 or f(x,y)<0.
// These points are "inside" the solution set.
CtcUnion inside31(in31,in32,inth31);
CtcUnion inside3(inside31,inth32);
//CtcQInter inter(inside,1);
//Artifact MODELISATION
double xa = par->xa;
double ya = par->ya;
double ra = par->ra;
Function f_a(x,y,sqr(x-xa)+sqr(y-ya));
NumConstraint ca1(x,y,f_a(x,y)<=sqr(ra));
NumConstraint ca2(x,y,f_a(x,y)>=sqr(ra)-par->thick);
NumConstraint ca3(x,y,f_a(x,y)>sqr(ra));
NumConstraint ca4(x,y,f_a(x,y)<sqr(ra)-par->thick);
CtcFwdBwd aout1(ca1);
CtcFwdBwd aout2(ca2);
CtcFwdBwd ain1(ca3);
CtcFwdBwd ain2(ca4);
CtcUnion ain(ain1,ain2);
CtcCompo aout(aout1,aout2);
//Robot MODELISATION
double xr = par->xr; //robot position x
double yr = par->yr; //robot position y
double wr = par->wr; //robot width
double lr = par->lr; //robot length
double ep = par->thick;
xr = par->xr - wr/2;
NumConstraint inrx1(x,y,x>xr+ep);
NumConstraint outrx1(x,y,x<xr+ep);
NumConstraint inrx2(x,y,x<xr-ep);
NumConstraint outrx2(x,y,x>xr-ep);
NumConstraint inry1(x,y,y<yr-lr/2);
NumConstraint outry1(x,y,y>yr-lr/2);
NumConstraint inry2(x,y,y>yr+lr/2);
NumConstraint outry2(x,y,y<yr+lr/2);
CtcFwdBwd incrx1(inrx1);
CtcFwdBwd incrx2(inrx2);
CtcFwdBwd incry1(inry1);
CtcFwdBwd incry2(inry2);
CtcFwdBwd outcrx1(outrx1);
CtcFwdBwd outcrx2(outrx2);
CtcFwdBwd outcry1(outry1);
CtcFwdBwd outcry2(outry2);
CtcUnion inrtemp(incrx1,incrx2,incry1);
CtcUnion inr1(inrtemp,incry2);
CtcCompo outrtemp(outcrx1,outcrx2,outcry1);
CtcCompo outr1(outrtemp,outcry2);
//2nd rectangle
xr = par->xr + wr/2;
NumConstraint inrx21(x,y,x>xr+ep);
NumConstraint outrx21(x,y,x<xr+ep);
NumConstraint inrx22(x,y,x<xr-ep);
NumConstraint outrx22(x,y,x>xr-ep);
NumConstraint inry21(x,y,y<yr-lr/2);
NumConstraint outry21(x,y,y>yr-lr/2);
NumConstraint inry22(x,y,y>yr+lr/2);
NumConstraint outry22(x,y,y<yr+lr/2);
CtcFwdBwd incrx21(inrx21);
CtcFwdBwd incrx22(inrx22);
CtcFwdBwd incry21(inry21);
CtcFwdBwd incry22(inry22);
CtcFwdBwd outcrx21(outrx21);
CtcFwdBwd outcrx22(outrx22);
CtcFwdBwd outcry21(outry21);
CtcFwdBwd outcry22(outry22);
CtcUnion inrtemp2(incrx21,incrx22,incry21);
CtcUnion inr2(inrtemp2,incry22);
CtcCompo outrtemp2(outcrx21,outcrx22,outcry21);
CtcCompo outr2(outrtemp2,outcry22);
//3nd rectangle top rectangle
yr=par->yr+par->lr/2;
xr=par->xr;
NumConstraint inrx31(x,y,x>xr+wr/2+ep);
NumConstraint outrx31(x,y,x<xr+wr/2+ep);
NumConstraint inrx32(x,y,x<xr-wr/2-ep);
NumConstraint outrx32(x,y,x>xr-wr/2-ep);
NumConstraint inry31(x,y,y<yr-ep);
NumConstraint outry31(x,y,y>yr-ep);
NumConstraint inry32(x,y,y>yr+ep);
NumConstraint outry32(x,y,y<yr+ep);
CtcFwdBwd incrx31(inrx31);
CtcFwdBwd incrx32(inrx32);
CtcFwdBwd incry31(inry31);
CtcFwdBwd incry32(inry32);
CtcFwdBwd outcrx31(outrx31);
CtcFwdBwd outcrx32(outrx32);
CtcFwdBwd outcry31(outry31);
CtcFwdBwd outcry32(outry32);
CtcUnion inrtemp3(incrx31,incrx32,incry31);
CtcUnion inr3(inrtemp3,incry32);
CtcCompo outrtemp3(outcrx31,outcrx32,outcry31);
CtcCompo outr3(outrtemp3,outcry32);
//4 rectangle bot
yr=par->yr-par->lr/2;
xr=par->xr;
NumConstraint inrx41(x,y,x>xr+wr/2+ep);
NumConstraint outrx41(x,y,x<xr+wr/2+ep);
NumConstraint inrx42(x,y,x<xr-wr/2-ep);
NumConstraint outrx42(x,y,x>xr-wr/2-ep);
NumConstraint inry41(x,y,y<yr-ep);
NumConstraint outry41(x,y,y>yr-ep);
NumConstraint inry42(x,y,y>yr+ep);
NumConstraint outry42(x,y,y<yr+ep);
CtcFwdBwd incrx41(inrx41);
CtcFwdBwd incrx42(inrx42);
CtcFwdBwd incry41(inry41);
CtcFwdBwd incry42(inry42);
CtcFwdBwd outcrx41(outrx41);
CtcFwdBwd outcrx42(outrx42);
CtcFwdBwd outcry41(outry41);
CtcFwdBwd outcry42(outry42);
CtcUnion inrtemp4(incrx41,incrx42,incry41);
CtcUnion inr4(inrtemp4,incry42);
CtcCompo outrtemp4(outcrx41,outcrx42,outcry41);
CtcCompo outr4(outrtemp4,outcry42);
CtcCompo inrtp(inr1,inr2,inr3);
CtcUnion outrtp(outr1,outr2,outr3);
CtcCompo inr(inrtp,inr4);
CtcUnion outr(outrtp,outr4);
yr = par->yr;
int maxq = 3; //nb of contractors
int Qinter = 2;
int ctcq = maxq - Qinter + 1; //nb for q-relaxed function of Ibex
Array<Ctc> inside1r1(inside1,inr,ain);
Array<Ctc> outside1r1(outside1,outr,aout);
Array<Ctc> inside2r1(inside2,inr,ain);
Array<Ctc> outside2r1(outside2,outr,aout);
Array<Ctc> inside3r1(inside3,inr,ain);
Array<Ctc> outside3r1(outside3,outr,aout);
CtcQInter outside1r(outside1r1,Qinter);
CtcQInter inside1r(inside1r1,ctcq);
CtcQInter outside2r(outside2r1,Qinter);
CtcQInter inside2r(inside2r1,ctcq);
CtcQInter outside3r(outside3r1,Qinter);
CtcQInter inside3r(inside3r1,ctcq);
// Build the initial box.
IntervalVector box(2);
box[0]=Interval(-10,10);
box[1]=Interval(-10,10);
par->vin.clear();
// Build the way boxes will be bisected.
// "LargestFirst" means that the dimension bisected
// is always the largest one.
int nbox1=0;
LargestFirst lf;
IntervalVector viinside1(2);
stack<IntervalVector> s;
s.push(box);
while (!s.empty()) {
IntervalVector box=s.top();
s.pop();
contract_and_draw(inside1r,box,viinside1,1,par,nbox1,Qt::magenta,Qt::red);
if (box.is_empty()) { continue; }
contract_and_draw(outside1r,box,viinside1,0,par,nbox1,Qt::darkBlue,Qt::cyan);
if (box.is_empty()) { continue; }
if (box.max_diam()<epsilon) {
R.DrawBox(box[0].lb(),box[0].ub(),box[1].lb(),box[1].ub(),QPen(Qt::yellow),QBrush(Qt::NoBrush));
} else {
pair<IntervalVector,IntervalVector> boxes=lf.bisect(box);
s.push(boxes.first);
s.push(boxes.second);
}
}
if(par->isinside==1){
robot_position_estimator(nbox1,par);
par->isinside1=1;
par->isinside=0;
//cout<<"area1: "<<par->area<<endl;
}
IntervalVector box2(2);
box2[0]=Interval(-10,10);
box2[1]=Interval(-10,10);
// Build the way boxes will be bisected.
// "LargestFirst" means that the dimension bisected
// is always the largest one.
int nbox2=0;
LargestFirst lf2;
IntervalVector viinside2(2);
stack<IntervalVector> s2;
s2.push(box2);
while (!s2.empty()) {
IntervalVector box2=s2.top();
s2.pop();
contract_and_draw(inside2r,box2,viinside2,2,par,nbox2,Qt::magenta,Qt::red);
if (box2.is_empty()) { continue; }
contract_and_draw(outside2r,box2,viinside2,0,par,nbox2,Qt::darkBlue,Qt::cyan);
if (box2.is_empty()) { continue; }
if (box2.max_diam()<epsilon) {
R.DrawBox(box2[0].lb(),box2[0].ub(),box2[1].lb(),box2[1].ub(),QPen(Qt::yellow),QBrush(Qt::NoBrush));
} else {
pair<IntervalVector,IntervalVector> boxes2=lf2.bisect(box2);
s2.push(boxes2.first);
s2.push(boxes2.second);
}
}
if(par->isinside==1){
robot_position_estimator(nbox2,par);
par->isinside2=1;
par->isinside=0;
//cout<<"area2: "<<par->area<<endl;
}
IntervalVector box3(2);
box3[0]=Interval(-10,10);
box3[1]=Interval(-10,10);
// Build the way boxes will be bisected.
// "LargestFirst" means that the dimension bisected
// is always the largest one.
int nbox3=0;
LargestFirst lf3;
IntervalVector viinside3(2);
stack<IntervalVector> s3;
s3.push(box3);
while (!s3.empty()) {
IntervalVector box3=s3.top();
s3.pop();
contract_and_draw(inside3r,box3,viinside3,3,par,nbox3,Qt::magenta,Qt::red);
if (box3.is_empty()) { continue; }
contract_and_draw(outside3r,box3,viinside3,0,par,nbox3,Qt::darkBlue,Qt::cyan);
if (box3.is_empty()) { continue; }
if (box3.max_diam()<epsilon) {
R.DrawBox(box3[0].lb(),box3[0].ub(),box3[1].lb(),box3[1].ub(),QPen(Qt::yellow),QBrush(Qt::NoBrush));
} else {
pair<IntervalVector,IntervalVector> boxes3=lf3.bisect(box3);
s3.push(boxes3.first);
s3.push(boxes3.second);
}
}
if(par->isinside==1){
robot_position_estimator(nbox3,par);
par->isinside3=1;
par->isinside=0;
//cout<<"area3: "<<par->area<<endl;
}
par->state.clear();
if (par->isinside1 ==1 || par->isinside2 ==1 || par->isinside3 ==1){
double *aimth = new double[3];
aimth[0] = get_angle(xb,yb,par->xin,par->yin)+M_PI ;
aimth[1] = get_angle(xb2,yb2,par->xin,par->yin)+M_PI;
aimth[2] = get_angle(xb3,yb3,par->xin,par->yin)+M_PI;
R.DrawLine(xb,yb,xb+r*cos(aimth[0]),yb+r*sin(aimth[0]),QPen(Qt::red));
R.DrawLine(xb2,yb2,xb2+r*cos(aimth[1]),yb2+r*sin(aimth[1]),QPen(Qt::red));
R.DrawLine(xb3,yb3,xb3+r*cos(aimth[2]),yb3+r*sin(aimth[2]),QPen(Qt::red));
par->state = std::string("found");
double kp = par->kp;
double u[3];
for (int i=0;i<3;i++){
u[i] = -kp*atan(tan((par->th[i] - (aimth[i] - arc/2.0 ))/2));
if(u[i]>par->sonar_speed) par->th[i] += par->sonar_speed;
if(u[i]<-par->sonar_speed) par->th[i] += -par->sonar_speed;
else par->th[i] += u[i];
}
// for (int i=0;i<3;i++){
// u[i] = atan(tan((par->th[i] - (aimth[i] - arc/2.0 ))/2));
// par->th[i] -=u[i];
// }
}
r = sqrt(r);
//cout<<"th1"<<th1<<endl;
R.DrawEllipse(xb,yb,par->ei,QPen(Qt::black),QBrush(Qt::NoBrush));
R.DrawEllipse(xb2,yb2,par->ei,QPen(Qt::black),QBrush(Qt::NoBrush));
R.DrawEllipse(xb3,yb3,par->ei,QPen(Qt::black),QBrush(Qt::NoBrush));
R.DrawLine(xb,yb,xb+r*cos(th2),yb+r*sin(th2),QPen(Qt::green));
R.DrawLine(xb2,yb2,xb2+r*cos(th22),yb2+r*sin(th22),QPen(Qt::green));
R.DrawLine(xb3,yb3,xb3+r*cos(th32),yb3+r*sin(th32),QPen(Qt::green));
R.DrawLine(xb,yb,xb+r*cos(th1),yb+r*sin(th1),QPen(Qt::green));
R.DrawLine(xb2,yb2,xb2+r*cos(th21),yb2+r*sin(th21),QPen(Qt::green));
R.DrawLine(xb3,yb3,xb3+r*cos(th31),yb3+r*sin(th31),QPen(Qt::green));
R.DrawEllipse(par->xa,par->ya,par->ra,QPen(Qt::black),QBrush(Qt::NoBrush));
R.DrawRobot(xr-wr/2,yr+lr/2,-3.14/2,wr,lr);
R.Save("paving");
par->vin.clear();
}
/*!
* \brief Timer/Counter 0 interrupt control.
*
* \param cmd Control command.
* - NUT_IRQCTL_INIT Initialize and disable interrupt.
* - NUT_IRQCTL_STATUS Query interrupt status.
* - NUT_IRQCTL_ENABLE Enable interrupt.
* - NUT_IRQCTL_DISABLE Disable interrupt.
* - NUT_IRQCTL_GETMODE Query interrupt mode.
* - NUT_IRQCTL_SETMODE Set interrupt mode (NUT_IRQMODE_LEVEL or NUT_IRQMODE_EDGE).
* - NUT_IRQCTL_GETPRIO Query interrupt priority.
* - NUT_IRQCTL_SETPRIO Set interrupt priority.
* - NUT_IRQCTL_GETCOUNT Query and clear interrupt counter.
* \param param Pointer to optional parameter.
*
* \return 0 on success, -1 otherwise.
*/
static int TimerCounter0IrqCtl(int cmd, void *param)
{
int rc = 0;
unsigned int *ival = (unsigned int *)param;
int_fast8_t enabled = inr(AIC_IMR) & _BV(TC0_ID);
/* Disable interrupt. */
if (enabled) {
outr(AIC_IDCR, _BV(TC0_ID));
}
switch(cmd) {
case NUT_IRQCTL_INIT:
/* Set the vector. */
outr(AIC_SVR(TC0_ID), (unsigned int)TimerCounter0IrqEntry);
/* Initialize to edge triggered with defined priority. */
outr(AIC_SMR(TC0_ID), AIC_SRCTYPE_INT_EDGE_TRIGGERED | NUT_IRQPRI_TC0);
/* Clear interrupt */
outr(AIC_ICCR, _BV(TC0_ID));
break;
case NUT_IRQCTL_STATUS:
if (enabled) {
*ival |= 1;
}
else {
*ival &= ~1;
}
break;
case NUT_IRQCTL_ENABLE:
enabled = 1;
break;
case NUT_IRQCTL_DISABLE:
enabled = 0;
break;
case NUT_IRQCTL_GETMODE:
{
unsigned int val = inr(AIC_SMR(TC0_ID)) & AIC_SRCTYPE;
if (val == AIC_SRCTYPE_INT_LEVEL_SENSITIVE || val == AIC_SRCTYPE_EXT_HIGH_LEVEL) {
*ival = NUT_IRQMODE_LEVEL;
} else {
*ival = NUT_IRQMODE_EDGE;
}
}
break;
case NUT_IRQCTL_SETMODE:
if (*ival == NUT_IRQMODE_LEVEL) {
outr(AIC_SMR(TC0_ID), (inr(AIC_SMR(TC0_ID)) & ~AIC_SRCTYPE) | AIC_SRCTYPE_INT_LEVEL_SENSITIVE);
} else if (*ival == NUT_IRQMODE_EDGE) {
outr(AIC_SMR(TC0_ID), (inr(AIC_SMR(TC0_ID)) & ~AIC_SRCTYPE) | AIC_SRCTYPE_INT_EDGE_TRIGGERED);
} else {
rc = -1;
}
break;
case NUT_IRQCTL_GETPRIO:
*ival = inr(AIC_SMR(TC0_ID)) & AIC_PRIOR;
break;
case NUT_IRQCTL_SETPRIO:
outr(AIC_SMR(TC0_ID), (inr(AIC_SMR(TC0_ID)) & ~AIC_PRIOR) | *ival);
break;
#ifdef NUT_PERFMON
case NUT_IRQCTL_GETCOUNT:
*ival = (unsigned int)sig_TC0.ir_count;
sig_TC0.ir_count = 0;
break;
#endif
default:
rc = -1;
break;
}
/* Enable interrupt. */
if (enabled) {
outr(AIC_IECR, _BV(TC0_ID));
}
return rc;
}
void ADCStartConversion(void)
{
outr(ADC_CR, ADC_START);
}
void ADCDisableChannel(TADCChannel channel)
{
outr(ADC_CHDR, _BV(channel));
outr(ADC_IDR, _BV(channel));
}