From 17bdd2d724c63cf83ef56d3152751dce411dfd91 Mon Sep 17 00:00:00 2001 From: pmantoine Date: Sat, 6 Aug 2022 16:53:14 +0800 Subject: [PATCH 1/3] Teensy build support, STM32F411RE first beta --- DCCTimer.h | 4 ++ DCCTimerSTM32.cpp | 118 +++++++++++++++++++++++++++++++++++++++++++++ DCCTimerTEENSY.cpp | 16 +++++- FSH.h | 8 +++ IO_MCP23008.h | 2 +- IO_PCF8574.h | 2 +- MotorDriver.cpp | 2 + MotorDriver.h | 16 +++++- defines.h | 64 ++++++++++++++++++++++-- platformio.ini | 57 +++++++++++++++++++++- 10 files changed, 280 insertions(+), 9 deletions(-) create mode 100644 DCCTimerSTM32.cpp diff --git a/DCCTimer.h b/DCCTimer.h index fa2b854..34b2d91 100644 --- a/DCCTimer.h +++ b/DCCTimer.h @@ -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 }; diff --git a/DCCTimerSTM32.cpp b/DCCTimerSTM32.cpp new file mode 100644 index 0000000..4ca20c0 --- /dev/null +++ b/DCCTimerSTM32.cpp @@ -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 . + */ + +// 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(sbrk(0))); +} + +void DCCTimer::reset() { + __disable_irq(); + NVIC_SystemReset(); + while(true) {}; +} + +#endif \ No newline at end of file diff --git a/DCCTimerTEENSY.cpp b/DCCTimerTEENSY.cpp index dc682df..a29f000 100644 --- a/DCCTimerTEENSY.cpp +++ b/DCCTimerTEENSY.cpp @@ -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(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; diff --git a/FSH.h b/FSH.h index c901fb9..f13e597 100644 --- a/FSH.h +++ b/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) diff --git a/IO_MCP23008.h b/IO_MCP23008.h index aa4679b..a411117 100644 --- a/IO_MCP23008.h +++ b/IO_MCP23008.h @@ -31,7 +31,7 @@ public: private: // Constructor MCP23008(VPIN firstVpin, uint8_t nPins, uint8_t I2CAddress, int interruptPin=-1) - : GPIOBase((FSH *)F("MCP23008"), firstVpin, min(nPins, 8), I2CAddress, interruptPin) { + : GPIOBase((FSH *)F("MCP23008"), firstVpin, min(nPins, (uint8_t)8), I2CAddress, interruptPin) { requestBlock.setRequestParams(_I2CAddress, inputBuffer, sizeof(inputBuffer), outputBuffer, sizeof(outputBuffer)); diff --git a/IO_PCF8574.h b/IO_PCF8574.h index 44eccad..f862984 100644 --- a/IO_PCF8574.h +++ b/IO_PCF8574.h @@ -48,7 +48,7 @@ public: private: PCF8574(VPIN firstVpin, uint8_t nPins, uint8_t I2CAddress, int interruptPin=-1) - : GPIOBase((FSH *)F("PCF8574"), firstVpin, min(nPins, 8), I2CAddress, interruptPin) + : GPIOBase((FSH *)F("PCF8574"), firstVpin, min(nPins, (uint8_t)8), I2CAddress, interruptPin) { requestBlock.setReadParams(_I2CAddress, inputBuffer, 1); } diff --git a/MotorDriver.cpp b/MotorDriver.cpp index 0239d1c..620d996 100644 --- a/MotorDriver.cpp +++ b/MotorDriver.cpp @@ -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 diff --git a/MotorDriver.h b/MotorDriver.h index 9e32ea0..9e2ff4f 100644 --- a/MotorDriver.h +++ b/MotorDriver.h @@ -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; diff --git a/defines.h b/defines.h index 23a7993..fa8dd17 100644 --- a/defines.h +++ b/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,21 @@ #define ARDUINO_TYPE "SAMD21" #undef USB_SERIAL #define USB_SERIAL SerialUSB -// SAMD support for I2C is awaiting development +// STM32 no EEPROM by default #ifndef DISABLE_EEPROM #define DISABLE_EEPROM #endif +// SAMD support for native I2C is awaiting development +#ifndef I2C_NO_INTERRUPTS + #define I2C_NO_INTERRUPTS +#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 diff --git a/platformio.ini b/platformio.ini index 0a38cf8..43a4092 100644 --- a/platformio.ini +++ b/platformio.ini @@ -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 = . @@ -164,4 +170,53 @@ platform = espressif32 board = esp32dev framework = arduino lib_deps = ${env.lib_deps} -build_flags = -std=c++17 \ No newline at end of file +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 = \ No newline at end of file From e7d8d320bdbb7cb933ce853124d6c5466f4d1fd7 Mon Sep 17 00:00:00 2001 From: pmantoine Date: Sat, 6 Aug 2022 17:51:13 +0800 Subject: [PATCH 2/3] SAMD21 I2C native interrupt capable driver --- I2CManager.cpp | 7 +- I2CManager_NonBlocking.h | 43 ++++++- I2CManager_SAMD.h | 271 +++++++++++++++++++++++---------------- defines.h | 6 +- 4 files changed, 212 insertions(+), 115 deletions(-) diff --git a/I2CManager.cpp b/I2CManager.cpp index a3ea611..a951a87 100644 --- a/I2CManager.cpp +++ b/I2CManager.cpp @@ -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 diff --git a/I2CManager_NonBlocking.h b/I2CManager_NonBlocking.h index 112e51e..fbcb98a 100644 --- a/I2CManager_NonBlocking.h +++ b/I2CManager_NonBlocking.h @@ -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 #include "I2CManager.h" #if defined(I2C_USE_INTERRUPTS) +// atomic.h isn't available on SAMD, and likely others too... +#if defined(__AVR__) #include +#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 diff --git a/I2CManager_SAMD.h b/I2CManager_SAMD.h index aeabd6c..4dd2790 100644 --- a/I2CManager_SAMD.h +++ b/I2CManager_SAMD.h @@ -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 //#include +#include +/*************************************************************************** + * 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<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<I2CM.CTRLB.bit.CMD = 3; // Stop condition } /*************************************************************************** * Close I2C down ***************************************************************************/ void I2CManagerClass::I2C_close() { - // disable TWI I2C_sendStop(); - while (TWCR & (1<writeBuffer + (txCount++)); - else - TWDR = currentRequest->writeBuffer[txCount++]; - bytesToSend--; - TWCR = (1< 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< 0) { - currentRequest->readBuffer[rxCount++] = TWDR; - bytesToReceive--; - } - TWCR = (1<i2cAddress << 1) | 1; // SLA+R - else - TWDR = (currentRequest->i2cAddress << 1) | 0; // SLA+W - TWCR = (1<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<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 */ diff --git a/defines.h b/defines.h index fa8dd17..ba96b4e 100644 --- a/defines.h +++ b/defines.h @@ -120,14 +120,10 @@ #define ARDUINO_TYPE "SAMD21" #undef USB_SERIAL #define USB_SERIAL SerialUSB -// STM32 no EEPROM by default +// SAMD no EEPROM by default #ifndef DISABLE_EEPROM #define DISABLE_EEPROM #endif -// SAMD support for native I2C is awaiting development -#ifndef I2C_NO_INTERRUPTS - #define I2C_NO_INTERRUPTS -#endif #elif defined(ARDUINO_ARCH_STM32) #define ARDUINO_TYPE "STM32" // STM32 no EEPROM by default From af75297a2324e240032c327540b5cbd17e7d0e28 Mon Sep 17 00:00:00 2001 From: pmantoine Date: Sat, 6 Aug 2022 18:06:00 +0800 Subject: [PATCH 3/3] I2CManager support for 1Mhz+ I2C speeds --- I2CManager.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/I2CManager.h b/I2CManager.h index 49f3e8f..e99b4f3 100644 --- a/I2CManager.h +++ b/I2CManager.h @@ -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);