mirror of
https://github.com/DCC-EX/CommandStation-EX.git
synced 2024-12-23 12:51:24 +01:00
Merge branch 'PORTX_HAL' of https://github.com/DCC-EX/CommandStation-EX into PORTX_HAL
This commit is contained in:
commit
6540ffee75
@ -85,7 +85,11 @@ private:
|
||||
static int freeMemory();
|
||||
static volatile int minimum_free_memory;
|
||||
static const int DCC_SIGNAL_TIME=58; // this is the 58uS DCC 1-bit waveform half-cycle
|
||||
#if defined(ARDUINO_ARCH_STM32) // TODO: PMA temporary hack - assumes 100Mhz F_CPU as STM32 can change frequency
|
||||
static const long CLOCK_CYCLES=(100000000L / 1000000 * DCC_SIGNAL_TIME) >>1;
|
||||
#else
|
||||
static const long CLOCK_CYCLES=(F_CPU / 1000000 * DCC_SIGNAL_TIME) >>1;
|
||||
#endif
|
||||
|
||||
};
|
||||
|
||||
|
118
DCCTimerSTM32.cpp
Normal file
118
DCCTimerSTM32.cpp
Normal file
@ -0,0 +1,118 @@
|
||||
/*
|
||||
* © 2022 Paul M Antoine
|
||||
* © 2021 Mike S
|
||||
* © 2021 Harald Barth
|
||||
* © 2021 Fred Decker
|
||||
* © 2021 Chris Harlow
|
||||
* © 2021 David Cutting
|
||||
* All rights reserved.
|
||||
*
|
||||
* This file is part of Asbelos DCC API
|
||||
*
|
||||
* This is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* It is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with CommandStation. If not, see <https://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
// ATTENTION: this file only compiles on a STM32 based boards
|
||||
// Please refer to DCCTimer.h for general comments about how this class works
|
||||
// This is to avoid repetition and duplication.
|
||||
#ifdef ARDUINO_ARCH_STM32
|
||||
|
||||
#include "FSH.h" //PMA temp debug
|
||||
#include "DIAG.h" //PMA temp debug
|
||||
#include "DCCTimer.h"
|
||||
|
||||
// STM32 doesn't have Serial1 defined by default
|
||||
HardwareSerial Serial1(PA10, PA15); // Rx=PA10, Tx=PA9
|
||||
|
||||
INTERRUPT_CALLBACK interruptHandler=0;
|
||||
// Let's use STM32's timer #1 until disabused of this notion
|
||||
HardwareTimer timer(TIM1);
|
||||
|
||||
// Timer IRQ handler
|
||||
void Timer1_Handler() {
|
||||
interruptHandler();
|
||||
}
|
||||
|
||||
void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
|
||||
interruptHandler=callback;
|
||||
noInterrupts();
|
||||
|
||||
timer.pause();
|
||||
timer.setPrescaleFactor(1);
|
||||
// timer.setOverflow(CLOCK_CYCLES * 2);
|
||||
timer.setOverflow(DCC_SIGNAL_TIME * 2, MICROSEC_FORMAT);
|
||||
timer.attachInterrupt(Timer1_Handler);
|
||||
timer.refresh();
|
||||
timer.resume();
|
||||
|
||||
interrupts();
|
||||
}
|
||||
|
||||
bool DCCTimer::isPWMPin(byte pin) {
|
||||
//TODO: SAMD whilst this call to digitalPinHasPWM will reveal which pins can do PWM,
|
||||
// there's no support yet for High Accuracy, so for now return false
|
||||
// return digitalPinHasPWM(pin);
|
||||
return false;
|
||||
}
|
||||
|
||||
void DCCTimer::setPWM(byte pin, bool high) {
|
||||
// TODO: High Accuracy mode is not supported as yet, and may never need to be
|
||||
(void) pin;
|
||||
(void) high;
|
||||
}
|
||||
|
||||
void DCCTimer::clearPWM() {
|
||||
return;
|
||||
}
|
||||
|
||||
void DCCTimer::getSimulatedMacAddress(byte mac[6]) {
|
||||
volatile uint32_t *serno1 = (volatile uint32_t *)0x0080A00C;
|
||||
volatile uint32_t *serno2 = (volatile uint32_t *)0x0080A040;
|
||||
// volatile uint32_t *serno3 = (volatile uint32_t *)0x0080A044;
|
||||
// volatile uint32_t *serno4 = (volatile uint32_t *)0x0080A048;
|
||||
|
||||
volatile uint32_t m1 = *serno1;
|
||||
volatile uint32_t m2 = *serno2;
|
||||
mac[0] = m1 >> 8;
|
||||
mac[1] = m1 >> 0;
|
||||
mac[2] = m2 >> 24;
|
||||
mac[3] = m2 >> 16;
|
||||
mac[4] = m2 >> 8;
|
||||
mac[5] = m2 >> 0;
|
||||
}
|
||||
|
||||
volatile int DCCTimer::minimum_free_memory=__INT_MAX__;
|
||||
|
||||
// Return low memory value...
|
||||
int DCCTimer::getMinimumFreeMemory() {
|
||||
noInterrupts(); // Disable interrupts to get volatile value
|
||||
int retval = freeMemory();
|
||||
interrupts();
|
||||
return retval;
|
||||
}
|
||||
|
||||
extern "C" char* sbrk(int incr);
|
||||
|
||||
int DCCTimer::freeMemory() {
|
||||
char top;
|
||||
return (int)(&top - reinterpret_cast<char *>(sbrk(0)));
|
||||
}
|
||||
|
||||
void DCCTimer::reset() {
|
||||
__disable_irq();
|
||||
NVIC_SystemReset();
|
||||
while(true) {};
|
||||
}
|
||||
|
||||
#endif
|
@ -88,8 +88,20 @@ void DCCTimer::getSimulatedMacAddress(byte mac[6]) {
|
||||
}
|
||||
#endif
|
||||
|
||||
volatile int DCCTimer::minimum_free_memory=__INT_MAX__;
|
||||
|
||||
// Return low memory value...
|
||||
int DCCTimer::getMinimumFreeMemory() {
|
||||
noInterrupts(); // Disable interrupts to get volatile value
|
||||
int retval = freeMemory();
|
||||
interrupts();
|
||||
return retval;
|
||||
}
|
||||
|
||||
extern "C" char* sbrk(int incr);
|
||||
|
||||
#if !defined(__IMXRT1062__)
|
||||
static inline int freeMemory() {
|
||||
int DCCTimer::freeMemory() {
|
||||
char top;
|
||||
return &top - reinterpret_cast<char*>(sbrk(0));
|
||||
}
|
||||
@ -110,7 +122,7 @@ static inline int freeMemory() {
|
||||
#endif
|
||||
#endif
|
||||
|
||||
static inline int freeMemory() {
|
||||
int DCCTimer::freeMemory() {
|
||||
extern unsigned long _ebss;
|
||||
extern unsigned long _sdata;
|
||||
extern unsigned long _estack;
|
||||
|
8
FSH.h
8
FSH.h
@ -47,6 +47,14 @@ typedef char FSH;
|
||||
#define FLASH
|
||||
#define strlen_P strlen
|
||||
#define strcpy_P strcpy
|
||||
#elif defined(ARDUINO_ARCH_STM32)
|
||||
typedef __FlashStringHelper FSH;
|
||||
#define GETFLASH(addr) pgm_read_byte_near(addr)
|
||||
#define GETFLASHW(addr) pgm_read_word_near(addr)
|
||||
#ifdef FLASH
|
||||
#undef FLASH
|
||||
#endif
|
||||
#define FLASH PROGMEM
|
||||
#else
|
||||
typedef __FlashStringHelper FSH;
|
||||
#define GETFLASH(addr) pgm_read_byte_near(addr)
|
||||
|
@ -1,5 +1,7 @@
|
||||
/*
|
||||
* © 2021, Neil McKechnie. All rights reserved.
|
||||
* © 2022 Paul M Antoine
|
||||
* © 2021, Neil McKechnie
|
||||
* All rights reserved.
|
||||
*
|
||||
* This file is part of CommandStation-EX
|
||||
*
|
||||
@ -30,6 +32,9 @@
|
||||
#elif defined(ARDUINO_ARCH_MEGAAVR)
|
||||
#include "I2CManager_NonBlocking.h"
|
||||
#include "I2CManager_Mega4809.h" // NanoEvery/UnoWifi
|
||||
#elif defined(ARDUINO_ARCH_SAMD)
|
||||
#include "I2CManager_NonBlocking.h"
|
||||
#include "I2CManager_SAMD.h" // SAMD21 for now... SAMD51 as well later
|
||||
#else
|
||||
#define I2C_USE_WIRE
|
||||
#include "I2CManager_Wire.h" // Other platforms
|
||||
|
@ -230,7 +230,11 @@ public:
|
||||
private:
|
||||
bool _beginCompleted = false;
|
||||
bool _clockSpeedFixed = false;
|
||||
#if defined(__arm__)
|
||||
uint32_t _clockSpeed = 32000000L; // 3.2MHz max on SAMD and STM32
|
||||
#else
|
||||
uint32_t _clockSpeed = 400000L; // 400kHz max on Arduino.
|
||||
#endif
|
||||
|
||||
// Finish off request block by waiting for completion and posting status.
|
||||
uint8_t finishRB(I2CRB *rb, uint8_t status);
|
||||
|
@ -1,5 +1,7 @@
|
||||
/*
|
||||
* © 2021, Neil McKechnie. All rights reserved.
|
||||
* © 2022 Paul M Antoine
|
||||
* © 2021, Neil McKechnie
|
||||
* All rights reserved.
|
||||
*
|
||||
* This file is part of CommandStation-EX
|
||||
*
|
||||
@ -23,7 +25,46 @@
|
||||
#include <Arduino.h>
|
||||
#include "I2CManager.h"
|
||||
#if defined(I2C_USE_INTERRUPTS)
|
||||
// atomic.h isn't available on SAMD, and likely others too...
|
||||
#if defined(__AVR__)
|
||||
#include <util/atomic.h>
|
||||
#elif defined(__arm__)
|
||||
// Helper assembly language functions
|
||||
static __inline__ uint8_t my_iSeiRetVal(void)
|
||||
{
|
||||
__asm__ __volatile__ ("cpsie i" ::);
|
||||
return 1;
|
||||
}
|
||||
|
||||
static __inline__ uint8_t my_iCliRetVal(void)
|
||||
{
|
||||
__asm__ __volatile__ ("cpsid i" ::);
|
||||
return 1;
|
||||
}
|
||||
|
||||
static __inline__ void my_iRestore(const uint32_t *__s)
|
||||
{
|
||||
uint32_t res = *__s;
|
||||
__asm__ __volatile__ ("MSR primask, %0" : : "r" (res) );
|
||||
}
|
||||
|
||||
static __inline__ uint32_t my_iGetIReg( void )
|
||||
{
|
||||
uint32_t reg;
|
||||
__asm__ __volatile__ ("MRS %0, primask" : "=r" (reg) );
|
||||
return reg;
|
||||
}
|
||||
// Macros for atomic isolation
|
||||
#define MY_ATOMIC_RESTORESTATE uint32_t _sa_saved \
|
||||
__attribute__((__cleanup__(my_iRestore))) = my_iGetIReg()
|
||||
|
||||
#define ATOMIC() \
|
||||
for ( MY_ATOMIC_RESTORESTATE, _done = my_iCliRetVal(); \
|
||||
_done; _done = 0 )
|
||||
|
||||
#define ATOMIC_BLOCK(x) ATOMIC()
|
||||
#define ATOMIC_RESTORESTATE
|
||||
#endif
|
||||
#else
|
||||
#define ATOMIC_BLOCK(x)
|
||||
#define ATOMIC_RESTORESTATE
|
||||
|
@ -1,5 +1,7 @@
|
||||
/*
|
||||
* © 2021, Neil McKechnie. All rights reserved.
|
||||
* © 2022 Paul M Antoine
|
||||
* © 2021, Neil McKechnie
|
||||
* All rights reserved.
|
||||
*
|
||||
* This file is part of CommandStation-EX
|
||||
*
|
||||
@ -25,31 +27,61 @@
|
||||
|
||||
//#include <avr/io.h>
|
||||
//#include <avr/interrupt.h>
|
||||
#include <wiring_private.h>
|
||||
|
||||
/***************************************************************************
|
||||
* Interrupt handler.
|
||||
* IRQ handler for SERCOM3 which is the default I2C definition for Arduino Zero
|
||||
* compatible variants such as the Sparkfun SAMD21 Dev Breakout etc.
|
||||
* Later we may wish to allow use of an alternate I2C bus, or more than one I2C
|
||||
* bus on the SAMD architecture
|
||||
***************************************************************************/
|
||||
#if defined(I2C_USE_INTERRUPTS) && defined(ARDUINO_SAMD_ZERO)
|
||||
// PMA - IRQ handler, based on SERCOM3 being used for I2C, as per Arduino Zero & Sparkfun SAMD21
|
||||
// TODO: test
|
||||
void SERCOM3_Handler() {
|
||||
I2CManagerClass::handleInterrupt();
|
||||
}
|
||||
#endif
|
||||
|
||||
// Assume SERCOM3 for now - default I2C bus on Arduino Zero and variants of same
|
||||
Sercom *s = SERCOM3;
|
||||
|
||||
/***************************************************************************
|
||||
* Set I2C clock speed register.
|
||||
***************************************************************************/
|
||||
void I2CManagerClass::I2C_setClock(unsigned long i2cClockSpeed) {
|
||||
unsigned long temp = ((F_CPU / i2cClockSpeed) - 16) / 2;
|
||||
for (uint8_t preScaler = 0; preScaler<=3; preScaler++) {
|
||||
if (temp <= 255) {
|
||||
TWBR = temp;
|
||||
TWSR = (TWSR & 0xfc) | preScaler;
|
||||
return;
|
||||
} else
|
||||
temp /= 4;
|
||||
void I2CManagerClass::I2C_setClock(uint32_t i2cClockSpeed) {
|
||||
|
||||
// Calculate a rise time appropriate to the requested bus speed
|
||||
int t_rise;
|
||||
if (i2cClockSpeed < 200000L) {
|
||||
i2cClockSpeed = 100000L;
|
||||
t_rise = 1000;
|
||||
} else if (i2cClockSpeed < 800000L) {
|
||||
i2cClockSpeed = 400000L;
|
||||
t_rise = 300;
|
||||
} else if (i2cClockSpeed < 1200000L) {
|
||||
i2cClockSpeed = 1000000L;
|
||||
t_rise = 120;
|
||||
} else {
|
||||
i2cClockSpeed = 100000L;
|
||||
t_rise = 1000;
|
||||
}
|
||||
// Set slowest speed ~= 500 bits/sec
|
||||
TWBR = 255;
|
||||
TWSR |= 0x03;
|
||||
|
||||
// Disable the I2C master mode and wait for sync
|
||||
s->I2CM.CTRLA.bit.ENABLE = 0 ;
|
||||
while (s->I2CM.SYNCBUSY.bit.ENABLE != 0);
|
||||
|
||||
// Calculate baudrate - using a rise time appropriate for the speed
|
||||
s->I2CM.BAUD.bit.BAUD = SystemCoreClock / (2 * i2cClockSpeed) - 5 - (((SystemCoreClock / 1000000) * t_rise) / (2 * 1000));
|
||||
|
||||
// Enable the I2C master mode and wait for sync
|
||||
s->I2CM.CTRLA.bit.ENABLE = 1 ;
|
||||
while (s->I2CM.SYNCBUSY.bit.ENABLE != 0);
|
||||
|
||||
// Setting bus idle mode and wait for sync
|
||||
s->I2CM.STATUS.bit.BUSSTATE = 1 ;
|
||||
while (s->I2CM.SYNCBUSY.bit.SYSOP != 0);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
/***************************************************************************
|
||||
@ -57,16 +89,58 @@ void I2CManagerClass::I2C_setClock(unsigned long i2cClockSpeed) {
|
||||
***************************************************************************/
|
||||
void I2CManagerClass::I2C_init()
|
||||
{
|
||||
// PMA - broadly we do the following
|
||||
initialise the clock
|
||||
initialise the NVIC
|
||||
software reset the I2C for the sercom
|
||||
set master mode
|
||||
do we need smart mode and quick command??
|
||||
configure interrupt handlers
|
||||
enable interrupts
|
||||
set default baud rate
|
||||
set SDA/SCL pins as outputs and enable pullups
|
||||
//Setting clock
|
||||
GCLK->CLKCTRL.reg = GCLK_CLKCTRL_ID(GCM_SERCOM3_CORE) | // Generic Clock 0 (SERCOM3)
|
||||
GCLK_CLKCTRL_GEN_GCLK0 | // Generic Clock Generator 0 is source
|
||||
GCLK_CLKCTRL_CLKEN ;
|
||||
|
||||
/* Wait for peripheral clock synchronization */
|
||||
while ( GCLK->STATUS.reg & GCLK_STATUS_SYNCBUSY );
|
||||
|
||||
// Software reset the SERCOM
|
||||
s->I2CM.CTRLA.bit.SWRST = 1;
|
||||
|
||||
//Wait both bits Software Reset from CTRLA and SYNCBUSY are equal to 0
|
||||
while(s->I2CM.CTRLA.bit.SWRST || s->I2CM.SYNCBUSY.bit.SWRST);
|
||||
|
||||
// Set master mode and enable SCL Clock Stretch mode (stretch after ACK bit)
|
||||
s->I2CM.CTRLA.reg = SERCOM_I2CM_CTRLA_MODE( I2C_MASTER_OPERATION )/* |
|
||||
SERCOM_I2CM_CTRLA_SCLSM*/ ;
|
||||
|
||||
// Enable Smart mode and Quick Command
|
||||
s->I2CM.CTRLB.reg = SERCOM_I2CM_CTRLB_SMEN | SERCOM_I2CM_CTRLB_QCEN;
|
||||
|
||||
#if defined(I2C_USE_INTERRUPTS)
|
||||
// Setting NVIC
|
||||
NVIC_EnableIRQ(SERCOM3_IRQn);
|
||||
NVIC_SetPriority (SERCOM3_IRQn, 0); /* set Priority */
|
||||
|
||||
// Enable all interrupts
|
||||
s->I2CM.INTENSET.reg = SERCOM_I2CM_INTENSET_MB | SERCOM_I2CM_INTENSET_SB | SERCOM_I2CM_INTENSET_ERROR;
|
||||
#endif
|
||||
|
||||
// Calculate baudrate and set default rate for now
|
||||
s->I2CM.BAUD.bit.BAUD = SystemCoreClock / ( 2 * I2C_FREQ) - 7 / (2 * 1000);
|
||||
|
||||
// Enable the I2C master mode and wait for sync
|
||||
s->I2CM.CTRLA.bit.ENABLE = 1 ;
|
||||
while (s->I2CM.SYNCBUSY.bit.ENABLE != 0);
|
||||
|
||||
// Setting bus idle mode and wait for sync
|
||||
s->I2CM.STATUS.bit.BUSSTATE = 1 ;
|
||||
while (s->I2CM.SYNCBUSY.bit.SYSOP != 0);
|
||||
|
||||
// Set SDA/SCL pins as outputs and enable pullups, at present we assume these are
|
||||
// the default ones for SERCOM3 (see assumption above)
|
||||
pinPeripheral(PIN_WIRE_SDA, g_APinDescription[PIN_WIRE_SDA].ulPinType);
|
||||
pinPeripheral(PIN_WIRE_SCL, g_APinDescription[PIN_WIRE_SCL].ulPinType);
|
||||
|
||||
// Enable the SCL and SDA pins on the sercom: includes increased driver strength,
|
||||
// pull-up resistors and pin multiplexer
|
||||
PORT->Group[g_APinDescription[PIN_WIRE_SCL].ulPort].PINCFG[g_APinDescription[PIN_WIRE_SCL].ulPin].reg =
|
||||
PORT_PINCFG_DRVSTR | PORT_PINCFG_PULLEN | PORT_PINCFG_PMUXEN;
|
||||
PORT->Group[g_APinDescription[PIN_WIRE_SDA].ulPort].PINCFG[g_APinDescription[PIN_WIRE_SDA].ulPin].reg =
|
||||
PORT_PINCFG_DRVSTR | PORT_PINCFG_PULLEN | PORT_PINCFG_PMUXEN;
|
||||
}
|
||||
|
||||
/***************************************************************************
|
||||
@ -75,28 +149,36 @@ void I2CManagerClass::I2C_init()
|
||||
void I2CManagerClass::I2C_sendStart() {
|
||||
bytesToSend = currentRequest->writeLen;
|
||||
bytesToReceive = currentRequest->readLen;
|
||||
|
||||
// We may have initiated a stop bit before this without waiting for it.
|
||||
// Wait for stop bit to be sent before sending start.
|
||||
while (TWCR & (1<<TWSTO)) {}
|
||||
TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT)|(1<<TWEA)|(1<<TWSTA); // Send Start
|
||||
while (s->I2CM.STATUS.bit.BUSSTATE == 0x2);
|
||||
|
||||
// If anything to send, initiate write. Otherwise initiate read.
|
||||
if (operation == OPERATION_READ || ((operation == OPERATION_REQUEST) && !bytesToSend))
|
||||
{
|
||||
// Send start and address with read/write flag or'd in
|
||||
s->I2CM.ADDR.bit.ADDR = (currentRequest->i2cAddress << 1) | 1;
|
||||
}
|
||||
else {
|
||||
// Wait while the I2C bus is BUSY
|
||||
while (s->I2CM.STATUS.bit.BUSSTATE != 0x1);
|
||||
s->I2CM.ADDR.bit.ADDR = (currentRequest->i2cAddress << 1ul) | 0;
|
||||
}
|
||||
}
|
||||
|
||||
/***************************************************************************
|
||||
* Initiate a stop bit for transmission (does not interrupt)
|
||||
***************************************************************************/
|
||||
void I2CManagerClass::I2C_sendStop() {
|
||||
TWDR = 0xff; // Default condition = SDA released
|
||||
TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA)|(1<<TWSTO); // Send Stop
|
||||
s->I2CM.CTRLB.bit.CMD = 3; // Stop condition
|
||||
}
|
||||
|
||||
/***************************************************************************
|
||||
* Close I2C down
|
||||
***************************************************************************/
|
||||
void I2CManagerClass::I2C_close() {
|
||||
// disable TWI
|
||||
I2C_sendStop();
|
||||
while (TWCR & (1<<TWSTO)) {}
|
||||
TWCR = (1<<TWINT); // clear any interrupt and stop twi.
|
||||
}
|
||||
|
||||
/***************************************************************************
|
||||
@ -105,84 +187,57 @@ void I2CManagerClass::I2C_close() {
|
||||
* (and therefore, indirectly, from I2CRB::wait() and I2CRB::isBusy()).
|
||||
***************************************************************************/
|
||||
void I2CManagerClass::I2C_handleInterrupt() {
|
||||
if (!(TWCR & (1<<TWINT))) return; // Nothing to do.
|
||||
|
||||
uint8_t twsr = TWSR & 0xF8;
|
||||
|
||||
// Cases are ordered so that the most frequently used ones are tested first.
|
||||
switch (twsr) {
|
||||
case TWI_MTX_DATA_ACK: // Data byte has been transmitted and ACK received
|
||||
case TWI_MTX_ADR_ACK: // SLA+W has been transmitted and ACK received
|
||||
if (bytesToSend) { // Send first.
|
||||
if (operation == OPERATION_SEND_P)
|
||||
TWDR = GETFLASH(currentRequest->writeBuffer + (txCount++));
|
||||
else
|
||||
TWDR = currentRequest->writeBuffer[txCount++];
|
||||
bytesToSend--;
|
||||
TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT)|(1<<TWEA);
|
||||
} else if (bytesToReceive) { // All sent, anything to receive?
|
||||
while (TWCR & (1<<TWSTO)) {} // Wait for stop to be sent
|
||||
TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT)|(1<<TWEA)|(1<<TWSTA); // Send Start
|
||||
} else { // Nothing left to send or receive
|
||||
TWDR = 0xff; // Default condition = SDA released
|
||||
TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA)|(1<<TWSTO); // Send Stop
|
||||
state = I2C_STATUS_OK;
|
||||
}
|
||||
break;
|
||||
case TWI_MRX_DATA_ACK: // Data byte has been received and ACK transmitted
|
||||
if (bytesToReceive > 0) {
|
||||
currentRequest->readBuffer[rxCount++] = TWDR;
|
||||
bytesToReceive--;
|
||||
}
|
||||
/* fallthrough */
|
||||
case TWI_MRX_ADR_ACK: // SLA+R has been sent and ACK received
|
||||
if (bytesToReceive <= 1) {
|
||||
TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT); // Send NACK after next reception
|
||||
} else {
|
||||
// send ack
|
||||
TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT)|(1<<TWEA);
|
||||
}
|
||||
break;
|
||||
case TWI_MRX_DATA_NACK: // Data byte has been received and NACK transmitted
|
||||
if (bytesToReceive > 0) {
|
||||
currentRequest->readBuffer[rxCount++] = TWDR;
|
||||
bytesToReceive--;
|
||||
}
|
||||
TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA)|(1<<TWSTO); // Send Stop
|
||||
state = I2C_STATUS_OK;
|
||||
break;
|
||||
case TWI_START: // START has been transmitted
|
||||
case TWI_REP_START: // Repeated START has been transmitted
|
||||
// Set up address and R/W
|
||||
if (operation == OPERATION_READ || (operation==OPERATION_REQUEST && !bytesToSend))
|
||||
TWDR = (currentRequest->i2cAddress << 1) | 1; // SLA+R
|
||||
else
|
||||
TWDR = (currentRequest->i2cAddress << 1) | 0; // SLA+W
|
||||
TWCR = (1<<TWEN)|ENABLE_TWI_INTERRUPT|(1<<TWINT)|(1<<TWEA);
|
||||
break;
|
||||
case TWI_MTX_ADR_NACK: // SLA+W has been transmitted and NACK received
|
||||
case TWI_MRX_ADR_NACK: // SLA+R has been transmitted and NACK received
|
||||
case TWI_MTX_DATA_NACK: // Data byte has been transmitted and NACK received
|
||||
TWDR = 0xff; // Default condition = SDA released
|
||||
TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA)|(1<<TWSTO); // Send Stop
|
||||
if (s->I2CM.STATUS.bit.ARBLOST) {
|
||||
// Arbitration lost, restart
|
||||
I2C_sendStart(); // Reinitiate request
|
||||
} else if (s->I2CM.STATUS.bit.BUSERR) {
|
||||
// Bus error
|
||||
state = I2C_STATUS_BUS_ERROR;
|
||||
} else if (s->I2CM.INTFLAG.bit.MB) {
|
||||
// Master write completed
|
||||
if (s->I2CM.STATUS.bit.RXNACK) {
|
||||
// Nacked, send stop.
|
||||
I2C_sendStop();
|
||||
state = I2C_STATUS_NEGATIVE_ACKNOWLEDGE;
|
||||
break;
|
||||
case TWI_ARB_LOST: // Arbitration lost
|
||||
// Restart transaction from start.
|
||||
I2C_sendStart();
|
||||
break;
|
||||
case TWI_BUS_ERROR: // Bus error due to an illegal START or STOP condition
|
||||
default:
|
||||
TWDR = 0xff; // Default condition = SDA released
|
||||
TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA)|(1<<TWSTO); // Send Stop
|
||||
state = I2C_STATUS_TRANSMIT_ERROR;
|
||||
} else if (bytesToSend) {
|
||||
// Acked, so send next byte
|
||||
if (currentRequest->operation == OPERATION_SEND_P)
|
||||
s->I2CM.DATA.bit.DATA = GETFLASH(currentRequest->writeBuffer + (txCount++));
|
||||
else
|
||||
s->I2CM.DATA.bit.DATA = currentRequest->writeBuffer[txCount++];
|
||||
bytesToSend--;
|
||||
} else if (bytesToReceive) {
|
||||
// Last sent byte acked and no more to send. Send repeated start, address and read bit.
|
||||
s->I2CM.ADDR.bit.ADDR = (currentRequest->i2cAddress << 1) | 1;
|
||||
} else {
|
||||
// No more data to send/receive. Initiate a STOP condition.
|
||||
I2C_sendStop();
|
||||
state = I2C_STATUS_OK; // Done
|
||||
}
|
||||
} else if (s->I2CM.INTFLAG.bit.SB) {
|
||||
// Master read completed without errors
|
||||
if (bytesToReceive) {
|
||||
currentRequest->readBuffer[rxCount++] = s->I2CM.DATA.bit.DATA; // Store received byte
|
||||
bytesToReceive--;
|
||||
} else {
|
||||
// Buffer full, issue nack/stop
|
||||
s->I2CM.CTRLB.bit.ACKACT = 1;
|
||||
I2C_sendStop();
|
||||
state = I2C_STATUS_OK;
|
||||
}
|
||||
if (bytesToReceive) {
|
||||
// PMA - I think Smart Mode means we have nothing to do...
|
||||
// More bytes to receive, issue ack and start another read
|
||||
}
|
||||
else
|
||||
{
|
||||
// Transaction finished, issue NACK and STOP.
|
||||
s->I2CM.CTRLB.bit.ACKACT = 1;
|
||||
I2C_sendStop();
|
||||
state = I2C_STATUS_OK;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(I2C_USE_INTERRUPTS)
|
||||
ISR(TWI_vect) {
|
||||
I2CManagerClass::handleInterrupt();
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* I2CMANAGER_AVR_H */
|
||||
#endif /* I2CMANAGER_SAMD_H */
|
||||
|
@ -31,7 +31,7 @@ public:
|
||||
private:
|
||||
// Constructor
|
||||
MCP23008(VPIN firstVpin, uint8_t nPins, uint8_t I2CAddress, int interruptPin=-1)
|
||||
: GPIOBase<uint8_t>((FSH *)F("MCP23008"), firstVpin, min(nPins, 8), I2CAddress, interruptPin) {
|
||||
: GPIOBase<uint8_t>((FSH *)F("MCP23008"), firstVpin, min(nPins, (uint8_t)8), I2CAddress, interruptPin) {
|
||||
|
||||
requestBlock.setRequestParams(_I2CAddress, inputBuffer, sizeof(inputBuffer),
|
||||
outputBuffer, sizeof(outputBuffer));
|
||||
|
@ -48,7 +48,7 @@ public:
|
||||
|
||||
private:
|
||||
PCF8574(VPIN firstVpin, uint8_t nPins, uint8_t I2CAddress, int interruptPin=-1)
|
||||
: GPIOBase<uint8_t>((FSH *)F("PCF8574"), firstVpin, min(nPins, 8), I2CAddress, interruptPin)
|
||||
: GPIOBase<uint8_t>((FSH *)F("PCF8574"), firstVpin, min(nPins, (uint8_t)8), I2CAddress, interruptPin)
|
||||
{
|
||||
requestBlock.setReadParams(_I2CAddress, inputBuffer, 1);
|
||||
}
|
||||
|
@ -268,6 +268,8 @@ void MotorDriver::getFastPin(const FSH* type,int pin, bool input, FASTPIN & res
|
||||
(void) type; // avoid compiler warning if diag not used above.
|
||||
#if defined(ARDUINO_ARCH_SAMD)
|
||||
PortGroup *port = digitalPinToPort(pin);
|
||||
#elif defined(ARDUINO_ARCH_STM32)
|
||||
GPIO_TypeDef *port = digitalPinToPort(pin);
|
||||
#else
|
||||
uint8_t port = digitalPinToPort(pin);
|
||||
#endif
|
||||
|
@ -43,6 +43,20 @@
|
||||
#if defined(ARDUINO_AVR_UNO)
|
||||
#define HAVE_PORTB(X) X
|
||||
#endif
|
||||
#if defined(ARDUINO_ARCH_SAMD)
|
||||
#define PORTA REG_PORT_OUT0
|
||||
#define HAVE_PORTA(X) X
|
||||
#define PORTB REG_PORT_OUT1
|
||||
#define HAVE_PORTB(X) X
|
||||
#endif
|
||||
#if defined(ARDUINO_ARCH_STM32)
|
||||
#define PORTA GPIOA->ODR
|
||||
#define HAVE_PORTA(X) X
|
||||
#define PORTB GPIOB->ODR
|
||||
#define HAVE_PORTB(X) X
|
||||
#define PORTC GPIOC->ODR
|
||||
#define HAVE_PORTC(X) X
|
||||
#endif
|
||||
|
||||
// if macros not defined as pass-through we define
|
||||
// them here as someting that is valid as a
|
||||
@ -63,7 +77,7 @@
|
||||
#define UNUSED_PIN 127 // inside int8_t
|
||||
#endif
|
||||
|
||||
#if defined(__IMXRT1062__) || defined(ARDUINO_ARCH_ESP8266) || defined(ARDUINO_ARCH_ESP32) || defined(ARDUINO_ARCH_SAMD)
|
||||
#if defined(__IMXRT1062__) || defined(ARDUINO_ARCH_ESP8266) || defined(ARDUINO_ARCH_ESP32) || defined(ARDUINO_ARCH_SAMD) || defined(ARDUINO_ARCH_STM32)
|
||||
typedef uint32_t portreg_t;
|
||||
#else
|
||||
typedef uint8_t portreg_t;
|
||||
|
60
defines.h
60
defines.h
@ -40,6 +40,7 @@
|
||||
// figure out if we have enough memory for advanced features
|
||||
// so define HAS_ENOUGH_MEMORY until proved otherwise.
|
||||
#define HAS_ENOUGH_MEMORY
|
||||
#undef USB_SERIAL // Teensy has this defined by default...
|
||||
#define USB_SERIAL Serial
|
||||
|
||||
#if defined(ARDUINO_AVR_UNO)
|
||||
@ -55,16 +56,62 @@
|
||||
#elif defined(ARDUINO_ARCH_MEGAAVR)
|
||||
#define ARDUINO_TYPE "MEGAAVR"
|
||||
#undef HAS_ENOUGH_MEMORY
|
||||
#elif defined(ARDUINO_TEENSY32)
|
||||
#define ARDUINO_TYPE "TEENSY32"
|
||||
#elif defined(ARDUINO_TEENSY31)
|
||||
#define ARDUINO_TYPE "TEENSY3132"
|
||||
#undef USB_SERIAL
|
||||
#define USB_SERIAL SerialUSB
|
||||
#ifndef DISABLE_EEPROM
|
||||
#define DISABLE_EEPROM
|
||||
#endif
|
||||
// Teensy support for native I2C is awaiting development
|
||||
#ifndef I2C_NO_INTERRUPTS
|
||||
#define I2C_NO_INTERRUPTS
|
||||
#endif
|
||||
#elif defined(ARDUINO_TEENSY35)
|
||||
#define ARDUINO_TYPE "TEENSY35"
|
||||
#undef USB_SERIAL
|
||||
#define USB_SERIAL SerialUSB
|
||||
// Teensy support for I2C is awaiting development
|
||||
#ifndef DISABLE_EEPROM
|
||||
#define DISABLE_EEPROM
|
||||
#endif
|
||||
// Teensy support for native I2C is awaiting development
|
||||
#ifndef I2C_NO_INTERRUPTS
|
||||
#define I2C_NO_INTERRUPTS
|
||||
#endif
|
||||
#elif defined(ARDUINO_TEENSY36)
|
||||
#define ARDUINO_TYPE "TEENSY36"
|
||||
#undef USB_SERIAL
|
||||
#define USB_SERIAL SerialUSB
|
||||
#ifndef DISABLE_EEPROM
|
||||
#define DISABLE_EEPROM
|
||||
#endif
|
||||
// Teensy support for native I2C is awaiting development
|
||||
#ifndef I2C_NO_INTERRUPTS
|
||||
#define I2C_NO_INTERRUPTS
|
||||
#endif
|
||||
#elif defined(ARDUINO_TEENSY40)
|
||||
#define ARDUINO_TYPE "TEENSY40"
|
||||
#undef USB_SERIAL
|
||||
#define USB_SERIAL SerialUSB
|
||||
#ifndef DISABLE_EEPROM
|
||||
#define DISABLE_EEPROM
|
||||
#endif
|
||||
// Teensy support for native I2C is awaiting development
|
||||
#ifndef I2C_NO_INTERRUPTS
|
||||
#define I2C_NO_INTERRUPTS
|
||||
#endif
|
||||
#elif defined(ARDUINO_TEENSY41)
|
||||
#define ARDUINO_TYPE "TEENSY41"
|
||||
#undef USB_SERIAL
|
||||
#define USB_SERIAL SerialUSB
|
||||
#ifndef DISABLE_EEPROM
|
||||
#define DISABLE_EEPROM
|
||||
#endif
|
||||
// Teensy support for native I2C is awaiting development
|
||||
#ifndef I2C_NO_INTERRUPTS
|
||||
#define I2C_NO_INTERRUPTS
|
||||
#endif
|
||||
#elif defined(ARDUINO_ARCH_ESP8266)
|
||||
#define ARDUINO_TYPE "ESP8266"
|
||||
#elif defined(ARDUINO_ARCH_ESP32)
|
||||
@ -73,10 +120,17 @@
|
||||
#define ARDUINO_TYPE "SAMD21"
|
||||
#undef USB_SERIAL
|
||||
#define USB_SERIAL SerialUSB
|
||||
// SAMD support for I2C is awaiting development
|
||||
// SAMD no EEPROM by default
|
||||
#ifndef DISABLE_EEPROM
|
||||
#define DISABLE_EEPROM
|
||||
#endif
|
||||
#elif defined(ARDUINO_ARCH_STM32)
|
||||
#define ARDUINO_TYPE "STM32"
|
||||
// STM32 no EEPROM by default
|
||||
#ifndef DISABLE_EEPROM
|
||||
#define DISABLE_EEPROM
|
||||
#endif
|
||||
// STM32 support for native I2C is awaiting development
|
||||
#ifndef I2C_NO_INTERRUPTS
|
||||
#define I2C_NO_INTERRUPTS
|
||||
#endif
|
||||
|
@ -18,6 +18,12 @@ default_envs =
|
||||
samd21-dev-usb
|
||||
samd21-zero-usb
|
||||
ESP32
|
||||
Nucleo-STM32F411RE
|
||||
Teensy3.2
|
||||
Teensy3.5
|
||||
Teensy3.6
|
||||
Teensy4.0
|
||||
Teensy4.1
|
||||
src_dir = .
|
||||
include_dir = .
|
||||
|
||||
@ -165,3 +171,52 @@ board = esp32dev
|
||||
framework = arduino
|
||||
lib_deps = ${env.lib_deps}
|
||||
build_flags = -std=c++17
|
||||
|
||||
[env:Nucleo-STM32F411RE]
|
||||
platform = ststm32
|
||||
board = nucleo_f411re
|
||||
framework = arduino
|
||||
lib_deps = ${env.lib_deps}
|
||||
build_flags = -std=c++17 -DDISABLE_EEPROM -Os -g2
|
||||
monitor_speed = 115200
|
||||
monitor_echo = yes
|
||||
|
||||
[env:Teensy3.2]
|
||||
platform = teensy
|
||||
board = teensy31
|
||||
framework = arduino
|
||||
build_flags = -std=c++17 -DDISABLE_EEPROM -Os -g2
|
||||
lib_deps = ${env.lib_deps}
|
||||
lib_ignore = NativeEthernet
|
||||
|
||||
[env:Teensy3.5]
|
||||
platform = teensy
|
||||
board = teensy35
|
||||
framework = arduino
|
||||
build_flags = -std=c++17 -DDISABLE_EEPROM -Os -g2
|
||||
lib_deps = ${env.lib_deps}
|
||||
lib_ignore = NativeEthernet
|
||||
|
||||
[env:Teensy3.6]
|
||||
platform = teensy
|
||||
board = teensy36
|
||||
framework = arduino
|
||||
build_flags = -std=c++17 -DDISABLE_EEPROM -Os -g2
|
||||
lib_deps = ${env.lib_deps}
|
||||
lib_ignore = NativeEthernet
|
||||
|
||||
[env:Teensy4.0]
|
||||
platform = teensy
|
||||
board = teensy40
|
||||
framework = arduino
|
||||
build_flags = -std=c++17 -DDISABLE_EEPROM -Os -g2
|
||||
lib_deps = ${env.lib_deps}
|
||||
lib_ignore = NativeEthernet
|
||||
|
||||
[env:Teensy4.1]
|
||||
platform = teensy
|
||||
board = teensy41
|
||||
framework = arduino
|
||||
build_flags = -std=c++17 -DDISABLE_EEPROM -Os -g2
|
||||
lib_deps = ${env.lib_deps}
|
||||
lib_ignore =
|
Loading…
Reference in New Issue
Block a user