void TwoWire::end()
{
if (!(hardware.clock_gate_register & hardware.clock_gate_mask)) return;
NVIC_DISABLE_IRQ(hardware.irq);
// TODO: should this try to create a stop condition??
port().C1 = 0;
volatile uint32_t *reg;
reg = portConfigRegister(hardware.scl_pin[scl_pin_index]);
*reg = 0;
reg = portConfigRegister(hardware.sda_pin[sda_pin_index]);
*reg = 0;
hardware.clock_gate_register &= ~hardware.clock_gate_mask;
}
bool PulsePositionInput::begin(uint8_t pin)
{
uint32_t channel;
volatile void *reg;
if (FTM0_MOD != 0xFFFF || FTM0_SC != (FTM_SC_CLKS(1) | FTM_SC_PS(0))) {
FTM0_SC = 0;
FTM0_CNT = 0;
FTM0_MOD = 0xFFFF;
FTM0_SC = FTM_SC_CLKS(1) | FTM_SC_PS(0);
FTM0_MODE = 0;
}
switch (pin) {
case 5:
channel = 7;
reg = &FTM0_C7SC;
break;
case 6:
channel = 4;
reg = &FTM0_C4SC;
break;
case 9:
channel = 2;
reg = &FTM0_C2SC;
break;
case 10:
channel = 3;
reg = &FTM0_C3SC;
break;
case 20:
channel = 5;
reg = &FTM0_C5SC;
break;
case 21:
channel = 6;
reg = &FTM0_C6SC;
break;
case 22:
channel = 0;
reg = &FTM0_C0SC;
break;
case 23:
channel = 1;
reg = &FTM0_C1SC;
break;
default:
return false;
}
prev = 0;
write_index = 255;
available_flag = false;
ftm = (struct ftm_channel_struct *)reg;
ftm->csc = cscEdge; // input capture & interrupt on rising edge
list[channel] = this;
channelmask |= (1<<channel);
*portConfigRegister(pin) = PORT_PCR_MUX(4) | PORT_PCR_DSE | PORT_PCR_SRE;
NVIC_SET_PRIORITY(IRQ_FTM0, 32);
NVIC_ENABLE_IRQ(IRQ_FTM0);
return true;
}
void attachInterrupt(uint8_t pin, void (*function)(void), int mode)
{
volatile uint32_t *config;
uint32_t cfg, mask;
if (pin >= CORE_NUM_DIGITAL) return;
switch (mode) {
case CHANGE: mask = 0x0B; break;
case RISING: mask = 0x09; break;
case FALLING: mask = 0x0A; break;
case LOW: mask = 0x08; break;
case HIGH: mask = 0x0C; break;
default: return;
}
mask = (mask << 16) | 0x01000000;
config = portConfigRegister(pin);
__disable_irq();
cfg = *config;
cfg &= ~0x000F0000; // disable any previous interrupt
*config = cfg;
intFunc[pin] = function; // set the function pointer
cfg |= mask;
*config = cfg; // enable the new interrupt
__enable_irq();
}
int touchRead(uint8_t pin)
{
uint32_t ch;
if (pin >= NUM_DIGITAL_PINS) return 0;
ch = pin2tsi[pin];
if (ch == 255) return 0;
*portConfigRegister(pin) = PORT_PCR_MUX(0);
SIM_SCGC5 |= SIM_SCGC5_TSI;
#if defined(HAS_KINETIS_TSI)
TSI0_GENCS = 0;
TSI0_PEN = (1 << ch);
TSI0_SCANC = TSI_SCANC_REFCHRG(3) | TSI_SCANC_EXTCHRG(CURRENT);
TSI0_GENCS = TSI_GENCS_NSCN(NSCAN) | TSI_GENCS_PS(PRESCALE) | TSI_GENCS_TSIEN | TSI_GENCS_SWTS;
delayMicroseconds(10);
while (TSI0_GENCS & TSI_GENCS_SCNIP) ; // wait
delayMicroseconds(1);
return *((volatile uint16_t *)(&TSI0_CNTR1) + ch);
#elif defined(HAS_KINETIS_TSI_LITE)
TSI0_GENCS = TSI_GENCS_REFCHRG(4) | TSI_GENCS_EXTCHRG(3) | TSI_GENCS_PS(PRESCALE)
| TSI_GENCS_NSCN(NSCAN) | TSI_GENCS_TSIEN | TSI_GENCS_EOSF;
TSI0_DATA = TSI_DATA_TSICH(ch) | TSI_DATA_SWTS;
delayMicroseconds(10);
while (TSI0_GENCS & TSI_GENCS_SCNIP) ; // wait
delayMicroseconds(1);
return TSI0_DATA & 0xFFFF;
#endif
}
void Snoozelc5vBuffer::disableDriver( void ) {
#if defined(KINETISL)
volatile uint32_t *config;
config = portConfigRegister( 17 );
return_core_pin_config = *config;
#endif
}
void attachInterrupt(uint8_t pin, void (* function)(void), int mode) {
volatile uint32_t* config;
uint32_t cfg, mask;
if (pin >= CORE_NUM_DIGITAL) {return; }
switch (mode) {
case CHANGE: mask = 0x0B; break;
case RISING: mask = 0x09; break;
case FALLING: mask = 0x0A; break;
case LOW: mask = 0x08; break;
case HIGH: mask = 0x0C; break;
default: return;
}
mask = (mask << 16) | 0x01000000;
config = portConfigRegister(pin);
#if defined(KINETISK)
attachInterruptVector(IRQ_PORTA, porta_interrupt);
attachInterruptVector(IRQ_PORTB, portb_interrupt);
attachInterruptVector(IRQ_PORTC, portc_interrupt);
attachInterruptVector(IRQ_PORTD, portd_interrupt);
attachInterruptVector(IRQ_PORTE, porte_interrupt);
#elif defined(KINETISL)
attachInterruptVector(IRQ_PORTA, porta_interrupt);
attachInterruptVector(IRQ_PORTCD, portcd_interrupt);
#endif
__disable_irq();
cfg = *config;
cfg &= ~0x000F0000; // disable any previous interrupt
*config = cfg;
intFunc[pin] = function; // set the function pointer
cfg |= mask;
*config = cfg; // enable the new interrupt
__enable_irq();
}
/*******************************************************************************
* remove interrupt from pin
*
* @param pin Teensy Pin
*******************************************************************************/
void SnoozeDigital::detachDigitalInterrupt( uint8_t pin ) {
volatile uint32_t *config;
__disable_irq( );
config = portConfigRegister( pin );
*config = ( ( *config & ~0x000F0000 ) | 0x01000000 );
__enable_irq( );
}
void tone(uint8_t pin, uint16_t frequency, uint32_t duration)
{
uint32_t count, load;
volatile uint32_t *config;
if (pin >= CORE_NUM_DIGITAL) return;
if (duration) {
count = (frequency * duration / 1000) * 2;
} else {
count = 0xFFFFFFFF;
}
load = (F_BUS / 2) / frequency;
config = portConfigRegister(pin);
__disable_irq();
if (pin != tone_pin) {
if (tone_pin < CORE_NUM_DIGITAL) {
tone_reg[0] = 1; // clear pin
}
tone_pin = pin;
tone_reg = portClearRegister(pin);
tone_reg[0] = 1; // clear pin
tone_reg[384] = 1; // output mode;
*config = PORT_PCR_SRE | PORT_PCR_DSE | PORT_PCR_MUX(1);
}
tone_toggle_count = count;
if (PIT_LDVAL3 != load) {
PIT_TCTRL3 = 0;
PIT_LDVAL3 = load;
PIT_TCTRL3 = 3;
}
__enable_irq();
}
bool TwoWire::wait_idle(void)
{
bool reset=false;
uint32_t wait_begin = millis();
//Serial.print("busy:");
while (i2c_status() & I2C_S_BUSY) {
//Serial.write('.') ;
uint32_t waited = millis() - wait_begin;
#if 1
if (waited > 15 && !reset) {
reset = true;
//Serial.println("attempt forced reset");
uint8_t sda_pin = hardware.sda_pin[sda_pin_index];
pinMode(sda_pin, INPUT_DISABLE);
uint8_t scl_pin = hardware.scl_pin[sda_pin_index];
pinMode(scl_pin, OUTPUT);
for (int i=0; i < 9; i++) {
digitalWrite(scl_pin, LOW);
delayMicroseconds(5);
digitalWrite(scl_pin, HIGH);
delayMicroseconds(5);
}
uint32_t mux;
volatile uint32_t *reg;
reg = portConfigRegister(hardware.sda_pin[sda_pin_index]);
mux = PORT_PCR_MUX(hardware.sda_mux[sda_pin_index]);
*reg = mux|PORT_PCR_ODE|PORT_PCR_SRE|PORT_PCR_DSE;
reg = portConfigRegister(hardware.scl_pin[scl_pin_index]);
mux = PORT_PCR_MUX(hardware.scl_mux[scl_pin_index]);
*reg = mux|PORT_PCR_ODE|PORT_PCR_SRE|PORT_PCR_DSE;
delayMicroseconds(10);
continue;
}
#endif
if (waited > 16) {
// bus stuck busy too long
port().C1 = 0;
port().C1 = I2C_C1_IICEN;
//Serial.println("abort");
//return 4; // timeout waiting for bus
return false;
}
}
return true;
}
void detachInterrupt(uint8_t pin) {
volatile uint32_t* config;
config = portConfigRegister(pin);
__disable_irq();
*config = ((*config & ~0x000F0000) | 0x01000000);
intFunc[pin] = NULL;
__enable_irq();
}
/*******************************************************************************
* Configure pin 17 on Teensy LC for OUTPUT LOW
*******************************************************************************/
void Snoozelc5vBuffer::enableDriver( void ) {
#if defined(KINETISL)
*portModeRegister( 17 ) &= ~digitalPinToBitMask( 17 );
volatile uint32_t *config;
config = portConfigRegister( 17 );
*config = return_core_pin_config;
pinMode( 17, OUTPUT );
digitalWriteFast( 17, LOW );
#endif
}
void TwoWire::setSCL(uint8_t pin)
{
if (pin == hardware.scl_pin[scl_pin_index]) return;
uint32_t newindex=0;
while (1) {
uint32_t scl_pin = hardware.scl_pin[newindex];
if (scl_pin == 255) return;
if (scl_pin == pin) break;
if (++newindex >= sizeof(hardware.scl_pin)) return;
}
if ((hardware.clock_gate_register & hardware.clock_gate_mask)) {
volatile uint32_t *reg;
reg = portConfigRegister(hardware.scl_pin[scl_pin_index]);
*reg = 0;
reg = portConfigRegister(hardware.scl_pin[newindex]);
uint32_t mux = PORT_PCR_MUX(hardware.scl_mux[newindex]);
*reg = mux|PORT_PCR_ODE|PORT_PCR_SRE|PORT_PCR_DSE;
}
scl_pin_index = newindex;
}
AS5134::AS5134(int cs, int clk, int dio, int quad_a, int quad_b, int minPeriod)
{
digitalWrite(cs, LOW);
digitalWrite(clk, LOW);
pinMode(cs, OUTPUT);
pinMode(clk, OUTPUT);
pinMode(dio, INPUT);
*portConfigRegister(dio) |= PORT_PCR_PE; //pull enable
*portConfigRegister(dio) &= ~PORT_PCR_PS; //pull down
pinMode(quad_a, INPUT);
pinMode(quad_b, INPUT);
this->cs = cs;
this->clk = clk;
this->dio = dio;
this->quad_a = quad_a;
this->quad_b = quad_b;
this->minPeriod = minPeriod;
attachInterrupt(quad_a, AS5134::CalculateSpeed, RISING);
Timer1.initialize(minPeriod);
Timer1.start();
Timer1.attachInterrupt(AS5134::ResetSpeed);
}
bool PulsePositionOutput::begin(uint8_t txPin, uint8_t framePin)
{
uint32_t channel;
volatile void *reg;
if (FTM0_MOD != 0xFFFF || (FTM0_SC & 0x7F) != FTM0_SC_VALUE) {
FTM0_SC = 0;
FTM0_CNT = 0;
FTM0_MOD = 0xFFFF;
FTM0_SC = FTM0_SC_VALUE;
#if defined(KINETISK)
FTM0_MODE = 0;
#endif
}
switch (txPin) {
case 6: channel = 4; reg = &FTM0_C4SC; break;
case 9: channel = 2; reg = &FTM0_C2SC; break;
case 10: channel = 3; reg = &FTM0_C3SC; break;
case 20: channel = 5; reg = &FTM0_C5SC; break;
case 22: channel = 0; reg = &FTM0_C0SC; break;
case 23: channel = 1; reg = &FTM0_C1SC; break;
#if defined(KINETISK)
case 5: channel = 7; reg = &FTM0_C7SC; break;
case 21: channel = 6; reg = &FTM0_C6SC; break;
#endif
default:
return false;
}
if (framePin < NUM_DIGITAL_PINS) {
framePinReg = portOutputRegister(framePin);
framePinMask = digitalPinToBitMask(framePin);
pinMode(framePin, OUTPUT);
FRAME_PIN_SET();
} else {
framePinReg = NULL;
}
state = 0;
current_channel = 0;
total_channels = 0;
ftm = (struct ftm_channel_struct *)reg;
ftm->cv = 200;
CSC_CHANGE(ftm, cscSet); // set on compare match & interrupt
list[channel] = this;
channelmask |= (1<<channel);
*portConfigRegister(txPin) = PORT_PCR_MUX(4) | PORT_PCR_DSE | PORT_PCR_SRE;
NVIC_SET_PRIORITY(IRQ_FTM0, 32);
NVIC_ENABLE_IRQ(IRQ_FTM0);
return true;
}
void TwoWire::begin(void)
{
//serial_begin(BAUD2DIV(115200));
//serial_print("\nWire Begin\n");
rxBufferIndex = 0;
rxBufferLength = 0;
txBufferIndex = 0;
txBufferLength = 0;
transmitting = 0;
user_onRequest = NULL;
user_onReceive = NULL;
slave_mode = 0;
hardware.clock_gate_register |= hardware.clock_gate_mask;
port().C1 = 0;
// On Teensy 3.0 external pullup resistors *MUST* be used
// the PORT_PCR_PE bit is ignored when in I2C mode
// I2C will not work at all without pullup resistors
// It might seem like setting PORT_PCR_PE & PORT_PCR_PS
// would enable pullup resistors. However, there seems
// to be a bug in chip while I2C is enabled, where setting
// those causes the port to be driven strongly high.
uint32_t mux;
volatile uint32_t *reg;
reg = portConfigRegister(hardware.sda_pin[sda_pin_index]);
mux = PORT_PCR_MUX(hardware.sda_mux[sda_pin_index]);
*reg = mux|PORT_PCR_ODE|PORT_PCR_SRE|PORT_PCR_DSE;
reg = portConfigRegister(hardware.scl_pin[scl_pin_index]);
mux = PORT_PCR_MUX(hardware.scl_mux[scl_pin_index]);
*reg = mux|PORT_PCR_ODE|PORT_PCR_SRE|PORT_PCR_DSE;
setClock(100000);
port().C2 = I2C_C2_HDRS;
port().C1 = I2C_C1_IICEN;
//pinMode(3, OUTPUT);
//pinMode(4, OUTPUT);
}
void tone(uint8_t pin, uint16_t frequency, uint32_t duration)
{
uint32_t count;
volatile uint32_t *config;
float usec;
if (pin >= CORE_NUM_DIGITAL) return;
if (duration) {
count = (frequency * duration / 1000) * 2;
} else {
count = 0xFFFFFFFF;
}
usec = (float)500000.0 / (float)frequency;
config = portConfigRegister(pin);
// TODO: IntervalTimer really needs an API to disable and enable
// the interrupt on a single timer.
__disable_irq();
if (pin == tone_pin) {
if (frequency == tone_frequency) {
// same pin, same frequency, so just update the
// duration. Users will call repetitively call
// tone() with the same setting, expecting a
// continuous output with no glitches or phase
// changes or jitter at each call.
tone_toggle_count = count;
} else {
// same pin, but a new frequency.
TONE_CLEAR_PIN; // clear pin
tone_timer.begin(tone_interrupt, usec);
}
} else {
if (tone_pin < CORE_NUM_DIGITAL) {
TONE_CLEAR_PIN; // clear pin
}
tone_pin = pin;
tone_reg = portClearRegister(pin);
#if defined(KINETISL)
tone_mask = digitalPinToBitMask(pin);
#endif
TONE_CLEAR_PIN; // clear pin
TONE_OUTPUT_PIN; // output mode;
*config = PORT_PCR_SRE | PORT_PCR_DSE | PORT_PCR_MUX(1);
tone_toggle_count = count;
tone_timer.begin(tone_interrupt, usec);
}
__enable_irq();
}
bool PulsePositionOutput::begin(uint8_t txPin, uint8_t framePin)
{
uint32_t channel;
volatile void *reg;
if (FTM0_MOD != 0xFFFF || FTM0_SC != (FTM_SC_CLKS(1) | FTM_SC_PS(0))) {
FTM0_SC = 0;
FTM0_CNT = 0;
FTM0_MOD = 0xFFFF;
FTM0_SC = FTM_SC_CLKS(1) | FTM_SC_PS(0);
FTM0_MODE = 0;
}
switch (txPin) {
case 5: channel = 7; reg = &FTM0_C7SC; break;
case 6: channel = 4; reg = &FTM0_C4SC; break;
case 9: channel = 2; reg = &FTM0_C2SC; break;
case 10: channel = 3; reg = &FTM0_C3SC; break;
case 20: channel = 5; reg = &FTM0_C5SC; break;
case 21: channel = 6; reg = &FTM0_C6SC; break;
case 22: channel = 0; reg = &FTM0_C0SC; break;
case 23: channel = 1; reg = &FTM0_C1SC; break;
default:
return false;
}
if (framePin < NUM_DIGITAL_PINS) {
framePinReg = portOutputRegister(framePin);
pinMode(framePin, OUTPUT);
*framePinReg = 1;
} else {
framePinReg = NULL;
}
state = 0;
current_channel = 0;
total_channels = 0;
ftm = (struct ftm_channel_struct *)reg;
ftm->cv = 200;
ftm->csc = cscSet; // set on compare match & interrupt
list[channel] = this;
channelmask |= (1<<channel);
*portConfigRegister(txPin) = PORT_PCR_MUX(4) | PORT_PCR_DSE | PORT_PCR_SRE;
NVIC_SET_PRIORITY(IRQ_FTM0, 32);
NVIC_ENABLE_IRQ(IRQ_FTM0);
return true;
}
void detachInterrupt(uint8_t pin)
{
volatile uint32_t *config;
config = portConfigRegister(pin);
#if defined(KINETISK)
voidFuncPtr* isr_table = getIsrTable(config);
if(!isr_table) return;
uint32_t pin_index = getPinIndex(config);
__disable_irq();
*config = ((*config & ~0x000F0000) | 0x01000000);
isr_table[pin_index] = dummy_isr;
__enable_irq();
#elif defined(KINETISL)
__disable_irq();
*config = ((*config & ~0x000F0000) | 0x01000000);
intFunc[pin] = dummy_isr;
__enable_irq();
#endif
}
bool PulsePositionInput::begin(uint8_t pin)
{
uint32_t channel;
volatile void *reg;
if (FTM0_MOD != 0xFFFF || (FTM0_SC & 0x7F) != FTM0_SC_VALUE) {
FTM0_SC = 0;
FTM0_CNT = 0;
FTM0_MOD = 0xFFFF;
FTM0_SC = FTM0_SC_VALUE;
#if defined(KINETISK)
FTM0_MODE = 0;
#endif
}
switch (pin) {
case 6: channel = 4; reg = &FTM0_C4SC; break;
case 9: channel = 2; reg = &FTM0_C2SC; break;
case 10: channel = 3; reg = &FTM0_C3SC; break;
case 20: channel = 5; reg = &FTM0_C5SC; break;
case 22: channel = 0; reg = &FTM0_C0SC; break;
case 23: channel = 1; reg = &FTM0_C1SC; break;
#if defined(KINETISK)
case 21: channel = 6; reg = &FTM0_C6SC; break;
case 5: channel = 7; reg = &FTM0_C7SC; break;
#endif
default:
return false;
}
prev = 0;
write_index = 255;
available_flag = false;
ftm = (struct ftm_channel_struct *)reg;
list[channel] = this;
channelmask |= (1<<channel);
*portConfigRegister(pin) = PORT_PCR_MUX(4);
CSC_CHANGE(ftm, cscEdge); // input capture & interrupt on rising edge
NVIC_SET_PRIORITY(IRQ_FTM0, 32);
NVIC_ENABLE_IRQ(IRQ_FTM0);
return true;
}
/*******************************************************************************
* sleep pin wakeups go through NVIC
*
* @param pin Teensy Pin
* @param mode CHANGE, FALLING or LOW, RISING or HIGH
*******************************************************************************/
void SnoozeDigital::attachDigitalInterrupt( uint8_t pin, int mode ) {
volatile uint32_t *config;
uint32_t cfg, mask;
if ( pin >= CORE_NUM_DIGITAL ) return;
switch ( mode ) {
case CHANGE: mask = 0x0B; break;
case RISING: mask = 0x09; break;
case FALLING: mask = 0x0A; break;
case LOW: mask = 0x08; break;
case HIGH: mask = 0x0C; break;
default: return;
}
__disable_irq( );
mask = ( mask << 16 ) | 0x01000000;
config = portConfigRegister( pin );
cfg = *config;
cfg &= ~0x000F0000; // disable any previous interrupt
*config = cfg;
cfg |= mask;
*config = cfg; // enable the new interrupt
__enable_irq( );
}
/*******************************************************************************
* Enable digital interrupt and configure the pin
*******************************************************************************/
void SnoozeDigital::enableDriver( void ) {
if ( mode == RUN_LP ) { return; }
#if defined(KINETISK)
uint64_t _pin = pin;
isr_pin = pin;
if ( mode == VLPW || mode == VLPS ) {
return_isr_a_enabled = NVIC_IS_ENABLED( IRQ_PORTA );
return_isr_b_enabled = NVIC_IS_ENABLED( IRQ_PORTB );
return_isr_c_enabled = NVIC_IS_ENABLED( IRQ_PORTC );
return_isr_d_enabled = NVIC_IS_ENABLED( IRQ_PORTD );
return_isr_e_enabled = NVIC_IS_ENABLED( IRQ_PORTE );
NVIC_DISABLE_IRQ(IRQ_PORTA);
NVIC_DISABLE_IRQ(IRQ_PORTB);
NVIC_DISABLE_IRQ(IRQ_PORTC);
NVIC_DISABLE_IRQ(IRQ_PORTD);
NVIC_DISABLE_IRQ(IRQ_PORTE);
NVIC_CLEAR_PENDING(IRQ_PORTA);
NVIC_CLEAR_PENDING(IRQ_PORTB);
NVIC_CLEAR_PENDING(IRQ_PORTC);
NVIC_CLEAR_PENDING(IRQ_PORTD);
NVIC_CLEAR_PENDING(IRQ_PORTE);
int priority = nvic_execution_priority( );// get current priority
// if running from interrupt set priority higher than current interrupt
priority = ( priority < 256 ) && ( ( priority - 16 ) > 0 ) ? priority - 16 : 128;
return_priority_a = NVIC_GET_PRIORITY( IRQ_PORTA );//get current priority
return_priority_b = NVIC_GET_PRIORITY( IRQ_PORTB );//get current priority
return_priority_c = NVIC_GET_PRIORITY( IRQ_PORTC );//get current priority
return_priority_d = NVIC_GET_PRIORITY( IRQ_PORTD );//get current priority
return_priority_e = NVIC_GET_PRIORITY( IRQ_PORTE );//get current priority
NVIC_SET_PRIORITY( IRQ_PORTA, priority );//set priority to new level
NVIC_SET_PRIORITY( IRQ_PORTB, priority );//set priority to new level
NVIC_SET_PRIORITY( IRQ_PORTC, priority );//set priority to new level
NVIC_SET_PRIORITY( IRQ_PORTD, priority );//set priority to new level
NVIC_SET_PRIORITY( IRQ_PORTE, priority );//set priority to new level
__disable_irq( );
return_porta_irq = _VectorsRam[IRQ_PORTA+16];// save prev isr handler
return_portb_irq = _VectorsRam[IRQ_PORTB+16];// save prev isr handler
return_portc_irq = _VectorsRam[IRQ_PORTC+16];// save prev isr handler
return_portd_irq = _VectorsRam[IRQ_PORTD+16];// save prev isr handler
return_porte_irq = _VectorsRam[IRQ_PORTE+16];// save prev isr handler
attachInterruptVector( IRQ_PORTA, isr );// set snooze digA isr
attachInterruptVector( IRQ_PORTB, isr );// set snooze digB isr
attachInterruptVector( IRQ_PORTC, isr );// set snooze digC isr
attachInterruptVector( IRQ_PORTD, isr );// set snooze digD isr
attachInterruptVector( IRQ_PORTE, isr );// set snooze digE isr
__enable_irq( );
NVIC_ENABLE_IRQ( IRQ_PORTA );
NVIC_ENABLE_IRQ( IRQ_PORTB );
NVIC_ENABLE_IRQ( IRQ_PORTC );
NVIC_ENABLE_IRQ( IRQ_PORTD );
NVIC_ENABLE_IRQ( IRQ_PORTE );
}
_pin = pin;
while ( __builtin_popcountll( _pin ) ) {
uint32_t pinNumber = 63 - __builtin_clzll( _pin );// get pin
if ( pinNumber > 33 ) break;
uint32_t pin_mode = irqType[pinNumber] >> 4;// get type
uint32_t pin_type = irqType[pinNumber] & 0x0F;// get mode
volatile uint32_t *config;
config = portConfigRegister( pinNumber );
return_core_pin_config[pinNumber] = *config;
if ( pin_mode == INPUT || pin_mode == INPUT_PULLUP ) {// setup pin mode/type/interrupt
*portModeRegister( pinNumber ) = 0;
if ( pin_mode == INPUT ) *config = PORT_PCR_MUX( 1 );
else *config = PORT_PCR_MUX( 1 ) | PORT_PCR_PE | PORT_PCR_PS;// pullup
if ( mode == VLPW || mode == VLPS ) {
attachDigitalInterrupt( pinNumber, pin_type );// set pin interrupt
}
else {
llwu_configure_pin_mask( pinNumber, mode );
}
} else {
//pinMode( pinNumber, pin_mode );
//digitalWriteFast( pinNumber, pin_type );
}
_pin &= ~( ( uint64_t )1 << pinNumber );// remove pin from list
}
#elif defined(KINETISL)
uint32_t _pin = pin;
isr_pin = pin;
if ( mode == VLPW || mode == VLPS ) {// if using sleep must setup pin interrupt to wake
return_isr_a_enabled = NVIC_IS_ENABLED( IRQ_PORTA );
return_isr_cd_enabled = NVIC_IS_ENABLED( IRQ_PORTCD );
NVIC_DISABLE_IRQ(IRQ_PORTA);
NVIC_DISABLE_IRQ(IRQ_PORTCD);
NVIC_CLEAR_PENDING(IRQ_PORTA);
NVIC_CLEAR_PENDING(IRQ_PORTCD);
int priority = nvic_execution_priority( );// get current priority
// if running from interrupt set priority higher than current interrupt
priority = ( priority < 256 ) && ( ( priority - 16 ) > 0 ) ? priority - 16 : 128;
return_priority_a = NVIC_GET_PRIORITY( IRQ_PORTA );//get current priority
return_priority_cd = NVIC_GET_PRIORITY( IRQ_PORTCD );//get current priority
NVIC_SET_PRIORITY( IRQ_PORTA, priority );//set priority to new level
NVIC_SET_PRIORITY( IRQ_PORTCD, priority );//set priority to new level
__disable_irq( );
return_porta_irq = _VectorsRam[IRQ_PORTA+16];// save prev isr handler
return_portcd_irq = _VectorsRam[IRQ_PORTCD+16];// save prev isr handler
attachInterruptVector( IRQ_PORTA, isr );// set snooze isr
attachInterruptVector( IRQ_PORTCD, isr );// set snooze isr
__enable_irq( );
NVIC_ENABLE_IRQ( IRQ_PORTA );
NVIC_ENABLE_IRQ( IRQ_PORTCD );
}
_pin = pin;
while ( __builtin_popcount( _pin ) ) {
uint32_t pinNumber = 31 - __builtin_clz( _pin );// get pin
if ( pinNumber > 33 ) break;
uint32_t pin_mode = irqType[pinNumber] >> 4;// get type
uint32_t pin_type = irqType[pinNumber] & 0x0F;// get mode
volatile uint32_t *config;
config = portConfigRegister( pinNumber );
return_core_pin_config[pinNumber] = *config;
if ( pin_mode == INPUT || pin_mode == INPUT_PULLUP ) {// setup pin mode/type/interrupt
*portModeRegister( pinNumber ) &= ~digitalPinToBitMask( pinNumber ); // TODO: atomic
if ( pin_mode == INPUT ) *config = PORT_PCR_MUX( 1 );
else *config = PORT_PCR_MUX( 1 ) | PORT_PCR_PE | PORT_PCR_PS;// pullup
if ( pin_mode == VLPW || pin_mode == VLPS ) {
attachDigitalInterrupt( pinNumber, pin_type );// set pin interrupt
}
else {
llwu_configure_pin_mask( pinNumber, pin_mode );
}
} else {
//pinMode( pinNumber, pin_mode );
//digitalWriteFast( pinNumber, pin_type );
}
_pin &= ~( ( uint32_t )1 << pinNumber );// remove pin from list
}
#endif
}
/*******************************************************************************
* Disable interrupt and configure pin to orignal state.
*******************************************************************************/
void SnoozeDigital::disableDriver( void ) {
if ( mode == RUN_LP ) { return; }
#if defined(KINETISK)
uint64_t _pin = pin;
while ( __builtin_popcountll( _pin ) ) {
uint32_t pinNumber = 63 - __builtin_clzll( _pin );
if ( pinNumber > 33 ) break;
*portModeRegister( pinNumber ) = 0;
volatile uint32_t *config;
config = portConfigRegister( pinNumber );
*config = return_core_pin_config[pinNumber];
_pin &= ~( ( uint64_t )1 << pinNumber );// remove pin from list
}
#elif defined(KINETISL)
uint32_t _pin = pin;
while ( __builtin_popcount( _pin ) ) {
uint32_t pinNumber = 31 - __builtin_clz( _pin );
if ( pinNumber > 33 ) break;
*portModeRegister( pinNumber ) &= ~digitalPinToBitMask( pinNumber );
volatile uint32_t *config;
config = portConfigRegister( pinNumber );
*config = return_core_pin_config[pinNumber];
_pin &= ~( ( uint32_t )1 << pinNumber );// remove pin from list
}
#endif
if ( mode == VLPW || mode == VLPS ) {
#if defined(KINETISK)
NVIC_SET_PRIORITY( IRQ_PORTA, return_priority_a );//set priority to new level
NVIC_SET_PRIORITY( IRQ_PORTB, return_priority_b );//set priority to new level
NVIC_SET_PRIORITY( IRQ_PORTC, return_priority_c );//set priority to new level
NVIC_SET_PRIORITY( IRQ_PORTD, return_priority_d );//set priority to new level
NVIC_SET_PRIORITY( IRQ_PORTE, return_priority_e );//set priority to new level
__disable_irq( );
attachInterruptVector( IRQ_PORTA, return_porta_irq );// set snooze isr
attachInterruptVector( IRQ_PORTB, return_portb_irq );// set snooze isr
attachInterruptVector( IRQ_PORTC, return_portc_irq );// set snooze isr
attachInterruptVector( IRQ_PORTD, return_portd_irq );// set snooze isr
attachInterruptVector( IRQ_PORTE, return_porte_irq );// set snooze isr
__enable_irq( );
if ( return_isr_a_enabled == 0 ) NVIC_DISABLE_IRQ( IRQ_PORTA );
if ( return_isr_b_enabled == 0 ) NVIC_DISABLE_IRQ( IRQ_PORTB );
if ( return_isr_c_enabled == 0 ) NVIC_DISABLE_IRQ( IRQ_PORTC );
if ( return_isr_d_enabled == 0 ) NVIC_DISABLE_IRQ( IRQ_PORTD );
if ( return_isr_e_enabled == 0 ) NVIC_DISABLE_IRQ( IRQ_PORTE );
#elif defined(KINETISL)
NVIC_SET_PRIORITY( IRQ_PORTA, return_priority_a );//set priority to new level
NVIC_SET_PRIORITY( IRQ_PORTCD, return_priority_cd );//set priority to new level
__disable_irq( );
attachInterruptVector( IRQ_PORTA, return_porta_irq );// set snooze isr
attachInterruptVector( IRQ_PORTCD, return_portcd_irq );// set snooze isr
__enable_irq( );
if ( return_isr_a_enabled == 0 ) NVIC_DISABLE_IRQ( IRQ_PORTA );
if ( return_isr_cd_enabled == 0 ) NVIC_DISABLE_IRQ( IRQ_PORTCD );
#endif
}
}
